QuadrotorFly-四旋翼無人機動力學仿真環境介紹

QuadrotorFly四旋翼無人機動力學模型

主要目的是開發一個用於無人機動力學仿真的簡單易用、功能相對齊全的仿真環境（也許是水論文環境）。這個仿真是基於python編寫的，GPL開源。git的地址在：
https://github.com/linxiaobo110/QuadrotorFly

主要功能（已實現）

模型功能

• 四旋翼基本動力學模型，即電機推力到角速度、速度的動力學模型。
• 電機動力學模型，簡化成一階慣性系統，控制輸入爲百分比（即0-1的小數）。
• 系統干擾，作用在基本動力學模型上的均值可設定的隨機噪聲
• 機型選擇，‘x’ 型或 ‘+’ 型。見常見機型
• 提供了常見控制器以供參考，現有PD控制器
• 提供了獎勵函數以供學習算法使用
• 狀態邊界檢查，超出最大值後finish標誌變成True

仿真功能

• 支持隨機初始位置、固定初始位置兩種啓動模式
• 採樣時間可設定，狀態更新方式採用4階龍格庫塔方法
• GUI動畫，目前基於matplotlib搭建，支持多機

環境與依賴

環境

python3 + 你喜歡的編輯器
新手推薦anconda+spyder簡單粗暴，入門建議anaconda+jupyter快捷穩定逼格高，想長期學習的建議anaconda+pycharm門檻高功能強大到難以想象。

需要的庫

• numpy
• matplotlib

我使用的環境

• win10 + Anaconda （python 3.6）+ Pycharm

使用教程

這些程序在pycharm、spyder下運行通過。jupyter的測試程序是工程目錄下的TestInJupyter.ipynb，模型可以運行不過gui動畫不會動，暫時還沒有修復。使用過程中有什麼建議或者問題可以聯繫我[email protected].

測試

下載解壓（建議使用git克隆）後運行其中的 QuadrotorFlyTest.py文件。
git克隆指令

git clone https://github.com/linxiaobo110/QuadrotorFly.git

成功運行可以看到以下效果：

運行結束後在畫面單擊可以看整個過程的飛行曲線

最簡實現

不畫圖和記錄數據，就是了解整個調用流程。可以在當前目錄裏新疆一個Test_simple.py（這個代碼在Test文件夾裏有），然後放入以下代碼:

# 包含頭文件
import numpy as np
import QuadrotorFlyModel as Qfm
# 定義無人機參數
uavPara = Qfm.QuadParas()
# 定義仿真的參數
simPara = Qfm.QuadSimOpt()
# 使用參數新建一個無人機
quad1 = Qfm.QuadModel(uavPara, simPara)

# 仿真循環開始，總共1000步
print("Simplest simulation begin!")
for i in range(1000):
    # 設定目標，分別是x,y,z的位置，和偏航角的角度
    ref = np.array([0., 0., 1., 0.])
    # 獲取無人機的狀態，
    #   共12個維度分別是，xyz位置，xyz速度，roll pitch yaw姿態角，roll pitch yaw 角速度
    stateTemp = quad1.observe()
    # 通過無人機狀態計算控制量，這也是自行設計控制器應修改的地方
    #    這是一個內置的控制器，返回值是給各個電機的控制量即油門的大小
    action2, oil = quad1.get_controller_pid(stateTemp, ref)
    # 使用控制量更新無人機，控制量是4維向量，範圍0-1
    quad1.step(action2)
    
print("Simulation finish!")

遇到“No module name ‘QuadrotorFlyGui’ ”或者 “No module name ‘QuadrotorFlyModel’”，見工程環境設置錯誤。

自定義仿真參數

在工程下新建Test_full.py(這個代碼在Test文件夾裏有)，然後複製以下代碼：

# 包含文件
import numpy as np
import QuadrotorFlyModel as Qfm
import QuadrotorFlyGui as Qfg
import MemoryStore
import matplotlib.pyplot as plt
# 角度到弧度的轉換
D2R = Qfm.D2R
# 仿真參數設置
simPara = Qfm.QuadSimOpt(
        # 初值重置方式（隨機或者固定）；姿態初值參數（隨機就是上限，固定就是設定值）；位置初值參數（同上）
        init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]),
        # 仿真運行的最大位置，最大速度，最大姿態角（角度，不是弧度注意），最大角速度（角度每秒）
        max_position=8, max_velocity=8, max_attitude=180, max_angular=200,
        # 系統噪聲，分別在位置環和速度環
        sysnoise_bound_pos=0, sysnoise_bound_att=0,
        #執行器模式，簡單模式沒有電機動力學，
        actuator_mode=Qfm.ActuatorMode.dynamic
        )
# 無人機參數設置，可以爲不同的無人機設置不同的參數
uavPara = Qfm.QuadParas(
        # 重力加速度；採樣時間；機型plus或者x
        g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus,
        # 無人機臂長（米）；質量（千克）；飛機繞三個軸的轉動慣量（千克·平方米）
        uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2,
        # 螺旋槳推力系數（牛每平方（弧度每秒）），螺旋槳扭力系數（牛·米每平方（弧度每秒）），旋翼轉動慣量，
        rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5,
        # 電機轉速比例參數（度每秒），電機轉速偏置參數（度每秒），電機相應時間（秒）
        rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2
        )
# 使用參數新建無人機
quad1 = Qfm.QuadModel(uavPara, simPara)

# 初始化GUI，並添加無人機
gui = Qfg.QuadrotorFlyGui([quad1])

# 初始化記錄器，用於記錄飛行數據
record = MemoryStore.DataRecord()

# 重置系統
# 當需要跑多次重複實驗時注意重置系統，
quad1.reset_states()
record.clear()

# 仿真過程
print("Simplest simulation begin!")
for i in range(1000):
    # 模型更新
    # 設置目標
    ref = np.array([0., 0., 1., 0.])
    # 獲取無人機的狀態，共12維的向量
    #   分別是，xyz位置，xyz速度，roll pitch yaw姿態角，roll pitch yaw 角速度
    stateTemp = quad1.observe()
    # 通過無人機狀態計算控制量，這也是自行設計控制器應修改的地方
    #    這是一個內置的控制器，返回值是給各個電機的控制量即油門的大小
    action2, oil = quad1.get_controller_pid(stateTemp, ref)
    # 使用控制量更新無人機，控制量是4維向量，範圍0-1
    quad1.step(action2)
    
    # 記錄數據
    record.buffer_append((stateTemp, action2))
    
    # 渲染GUI
    gui.render()
    
# 輸出結果
## 獲取記錄中的狀態
bs = np.array([_[0] for _ in record.buffer])
## 獲取記錄中的控制量
ba = np.array([_[1] for _ in record.buffer])
## 生成時間序列
t = range(0, record.count)
## 畫圖
fig1 = plt.figure(2)
plt.clf()
## 姿態圖
plt.subplot(3, 1, 1)
plt.plot(t, bs[t, 6] / D2R, label='roll')
plt.plot(t, bs[t, 7] / D2R, label='pitch')
plt.plot(t, bs[t, 8] / D2R, label='yaw')
plt.ylabel('Attitude $(\circ)$', fontsize=15)
plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05))
## 位置圖(xy)
plt.subplot(3, 1, 2)
plt.plot(t, bs[t, 0], label='x')
plt.plot(t, bs[t, 1], label='y')
plt.ylabel('Position (m)', fontsize=15)
plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05))
## 高度圖
plt.subplot(3, 1, 3)
plt.plot(t, bs[t, 2], label='z')
plt.ylabel('Altitude (m)', fontsize=15)
plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 
print("Simulation finish!")

在spyder下的運行效果：

如果遇到spyder運行時，圖片在終端裏顯示（就是沒有彈出新窗口），見Spyder圖形渲染設置。

基礎知識

常見四旋翼無人機機型

• + 型四旋翼無人機
  

• X 型四旋翼無人機

四旋翼基本動力學模型

$\begin{aligned} \ddot{p}_x&=[\cos{\varphi}\sin{\theta}\cos{\psi}+\sin{\varphi}\sin{\psi}]\frac{\tau_0}{m} + d_1\\ \ddot{p}_y&=[\cos{\varphi}\sin{\theta}\sin{\psi}-\sin{\varphi}\cos{\psi}]\frac{\tau_0}{m} + d_2\\ \ddot{p}_z&=\cos{\theta}cos{\varphi}\frac{\tau_0}{m}-g + d_3\\ \ddot{\varphi}&=\dot{\varphi}\dot{\psi}(\frac{J_{zz}-J_{xx}}{J_{yy}}) + \frac{J_R}{J_{yy}}\dot{\varphi}\Omega_R + \frac{L}{J_{yy}}\tau_1 + d_4\\ \ddot{\theta}&=\dot{\theta}\dot{\psi} (\frac{J_{yy}-J_{zz}}{J_{xx}}) - \frac{J_R}{J_{xx}}\dot{\theta}\Omega_R +\frac{L}{J_{xx}}\tau_2 + d_5\\ \ddot{\psi}&=\dot{\theta}\dot{\varphi}(\frac{J_{xx}-J_{yy}}{J_{zz}}) + \frac{1}{J_{zz}}\tau_3 + d_6, \end{aligned}$
其中$p_x,p_y,p_z$ 位置，$\varphi,\theta,\psi$是姿態，$\tau_{0，1,2,3}$分別是總體推力，繞x軸、y軸，z軸的扭力。

電機動力學模型

$\begin{aligned} \dot{\omega} &=\frac{1}{T}(-\omega+C_Ru+w_b)\\ T &= C_T\omega^2\\ M &= C_M \omega^2 \end{aligned}$
其中$\omega$是電機的轉速；$u$是輸入給電機的控制信號；$T,M$分別是電機產生的推力和扭力。

動力學中的力與螺旋槳產生的力關係

以十型舉例
$\begin{aligned} \tau_0 &= T_0 + T_1 + T_2 + T_3\\ \tau_1 &= T_1 - T_0\\ \tau_2 &= T_3 - T_2\\ \tau_3 &= -M_1 - M_2 + M_3 + M_4 \end{aligned}$

FAQ

工程環境設置錯誤

• 現象：
  ModuleNotFoundError: No module named ‘QuadrotorFlyGui’
• 原因：
  這是因爲工程運行的目錄不是QuadrotorFly目錄，比如在Test裏，或者是在一個上級的目錄。
• 解決辦法

如果上級目錄，修改import語句成

# 其他類似
import QuadrotorFly.QuadrotorFlyGui

如果是在Test裏，或者不確定在哪裏，直接修改程序運行目錄

1. spyder編輯器
   選擇爲每個文件單獨指定配置文件
   
   選擇指定文件夾
   ![在這裏插入圖片描述](https://img-blog.csdnimg.cn/2019050620353539.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xpbnhpYW9ibzExMA==,size_16,color_FFFFFF,t_70

選擇QuadrotorFly目錄

Spyder圖形渲染設置

選擇tool->preferences

設置合適的渲染工具，不是inline