QuadrotorFly 0.2 - 添加了傳感器系統

v0.2 更新的內容

1. 加入了傳感器子系統（默認不使能），包含IMU（陀螺儀+加速度計），GPS，compass（磁力計），
2. 加入了一個簡單的卡爾曼濾波估計器。
3. 加入了時間系統，即無人機有一個自己的內部時間，每次迭代會累計採樣週期，reset回導致時間歸零。
4. 參考例子在StateEstimator.py和Test_fullWithSensor.py
5. 可以通過繼承SensorBase來完成自定義傳感器, 通過繼承StateEstimatorBase來實現自定義的位姿估計器

github連接：
https://github.com/linxiaobo110/QuadrotorFly

每個傳感器考慮不同的特性，主要內容如下：

• IMU考慮噪聲和零飄，100Hz的更新頻率（與系統默認相同）
• GPS考慮低採樣率（10Hz）、噪聲、啓動延時（1s）和時延（100ms，即latency）
• 磁力計考慮噪聲和偏置，座標系假設爲東北天

使能傳感器系統後的注意事項

下面假設quad1是QuadrotorFlyModel類的一個實例，並且這個類在實例化時使能了傳感器系統，即

q1 = Qfm.QuadModel(Qfm.QuadParas(), Qfm.QuadSimOpt(enable_sensor_sys=True))

及時獲取時間戳

不同的傳感器採樣時刻和啓動的時長是不一樣的，因此需要啓動時要注意延時一定的時間。
使用quad1.ts來獲取當前時間戳

模型調用接口的返回值發生變化

使能傳感器系統後，quad1.step函數和observe函數返回的是傳感器信息，傳感器信息採用字典類型（dict）儲存，格式如下：

print(quad1.observe())
{'imu': (True, array([ 5.78443156e-02, -3.81653176e-01, -9.50556019e+00,  1.75481697e-03,
       -2.82903321e-02,  6.36374403e-02])), 
 'gps': (False, array([ 1.00240709, -0.24003651,  0.64546363])), 
 'compass': (True, array([  7.89924613,  32.83536768, -93.90494583]))}

其中，字典的鍵名是傳感器的名稱（imu, gps, compass)，值的第一個參數是傳感器這一刻是否更新（True代表更新，False反之），第二個值是傳感器數據，定義分別如下：

名稱	imu	gps	compass
參數1-3	加速度(m/(s^2))	位置(m)	磁場強度(T)
參數4-6	角速度（rad/s)	x	x

注意，這些傳感器的值是帶有噪聲和零飄的，並且零飄是傳感器初始化的時候隨機的， 可以通過quad1.imu0.gyroBias和quad1.imu0.accBias獲取imu的零飄。假設磁力計的北邊和正北大概差16度。

真實狀態和估計狀態存在差異

控制的目的是穩定系統的真實狀態，但是隻能觀測到傳感器返回的信息，通過估計器（觀測器、濾波器之類的名稱）解算得到系統狀態與真實的狀態會有一定的差異（受估計器性能影響）。但是可以通過quad1.state（這是一個屬性化的函數）獲得無人機的真實狀態，保存數據的時候注意兩組數據都需要保存，方便對比。

帶傳感器的完整實驗

# 弧度到角度的轉換參數
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)
# 新建卡爾曼濾波器（已實現的，作爲參考） 
filter1 = StateEstimator.KalmanFilterSimple()
# 新建GUI
gui = Qfg.QuadrotorFlyGui([quad1])
# 新建存儲對象，用於儲存過程數據
record = MemoryStore.DataRecord()

# 復位
quad1.reset_states()
record.clear()
filter1.reset(quad1.state)

# 設置imu零飄，這個實際是拿不到的，這裏簡化了
filter1.gyroBias = quad1.imu0.gyroBias
filter1.accBias = quad1.imu0.accBias
print(filter1.gyroBias, filter1.accBias)

# 設置仿真時間
t = np.arange(0, 30, 0.01)
ii_len = len(t)

# 仿真過程
for ii in range(ii_len):
    # 等待傳感器啓動
    if ii < 100:
        # 獲取傳感器數據
        sensor_data1 = quad1.observe()
        # 在傳感器啓動前使用系統狀態直接獲取控制器值
        _, oil = quad1.get_controller_pid(quad1.state)
        # 其實只需要油門維持穩定
        action = np.ones(4) * oil
        # 執行一步
        quad1.step(action)
        # 把獲取到的傳感器數據給估計器
        state_est = filter1.update(sensor_data1, quad1.ts)
        # 儲存數據
        record.buffer_append((quad1.state, state_est))
   else:
        # 獲取傳感器數據
        sensor_data1 = quad1.observe()
        # 使用估計器估計的狀態來計算控制量而非真實的狀態
        action, oil = quad1.get_controller_pid(filter1.state, np.array([0, 0, 2, 0]))
        # 執行一步
        quad1.step(action)
        # 把獲取到的傳感器數據給估計器
        state_est = filter1.update(sensor_data1, quad1.ts)
        # 儲存數據
        record.buffer_append((quad1.state, state_est))
# 這個不能省去
record.episode_append()

# 輸出結果
# 1. 獲取最新的數據
data = record.get_episode_buffer()
# 1.1 真實的狀態
bs_r = data[0]
# 1.2 估計的狀態
bs_e = data[1]
# 2. 產生時間序列
t = range(0, record.count)
# 3. 畫圖
# 3.1 畫姿態和角速度
fig1 = plt.figure(2)
yLabelList = ['roll', 'pitch', 'yaw', 'rate_roll', 'rate_pit', 'rate_yaw']
for ii in range(6):
    plt.subplot(6, 1, ii + 1)
    plt.plot(t, bs_r[:, 6 + ii] / D2R, '-b', label='real')
    plt.plot(t, bs_e[:, 6 + ii] / D2R, '-g', label='est')
    plt.legend()
    plt.ylabel(yLabelList[ii])

# 3.2 畫位置和速度
yLabelList = ['p_x', 'p_y', 'p_z', 'vel_x', 'vel_y', 'vel_z']
plt.figure(3)
for ii in range(6):
    plt.subplot(6, 1, ii + 1)
    plt.plot(t, bs_r[:, ii], '-b', label='real')
    plt.plot(t, bs_e[:, ii], '-g', label='est')
    plt.legend()
    plt.ylabel(yLabelList[ii])
plt.show()
print("Simulation finish!")

結果

1. 從3d的效果圖可以看出有一點抖動
   

2. 位置的真實和估計
   

3. 姿態的真實和估計
   

基本原理

傳感器仿真的原理

IMU考慮噪聲和零飄，100Hz的更新頻率（與系統默認相同）

陀螺儀參考的參數來自傳感器MPU6500

$\begin{aligned} \omega_o &= \omega+b_g+w_g\\ a_s &= a_F+g\\ a_o &= R_{i2b}a_s + b_a+w_a\\ \end{aligned}$

其中下標帶o的是傳感器的輸出，$\omega$是真實的角速度，$a_F$是真實的合外力造成的加速度，g是重力加速度，$R_{i2b}$是慣性系到機體系的轉換矩陣， $b_g$和$b_a$分別是陀螺儀和加速度計的零飄，$w_g$和$w_a$分別是陀螺儀和加速度計的噪聲，這裏假設成白噪聲。

GPS考慮低採樣率、噪聲、啓動延時和時延

GPS默認的參數來自M8N，採樣率爲10H、精度爲1米、啓動延時爲1s，時延（即latency)是100ms。時延和啓動延時是不一樣的，時延指由於採樣過程或者傳輸過程導致採集的數據表徵的是之前時刻的狀態，啓動延時就是啓動的前1秒內沒有響應，之後就有響應。

其中t代表連續的時間，$t_k$是採樣的序列，$T_s$和$T_l$分別代表啓動延時和時延，$w_p$是傳感器噪聲。

磁力計主要考慮低採樣率

與imu類似，採樣率只有imu的1/10，假設的慣性座標系爲東北天。

最簡卡爾曼濾波器的原理

懶，下次再說吧，詳細見代碼。簡單的說就是首先判斷各個傳感器是否更新，更新的話就運行相應的更新程序。在姿態估計上，陀螺儀當作過程更新，加速度計（就是測重力方向）和磁力計當作測量更新；在位置估計上，加速度計做過程更新，gps做測量更新。