魚少少，裴 軍，胡 超

(1. 中國科學(xué)院大學(xué)，北京 100049；2. 中國科學(xué)院國家天文臺(tái)，北京 100012)

隨著微小型飛行器和機(jī)器人等的快速發(fā)展，對(duì)其姿態(tài)研究顯得越來越重要。一般載體的姿態(tài)常常采用濾波器對(duì)來自微機(jī)械(Micro-Electro-Mechanical System, MEMS)慣性測量單元(IMU)的多傳感器數(shù)據(jù)進(jìn)行融合得到，常用的微機(jī)械慣性測量器件包括三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)。文[1]建立了四元數(shù)卡爾曼濾波方程并進(jìn)行了數(shù)據(jù)仿真，文[2]對(duì)文[1]提出的四元數(shù)卡爾曼濾波進(jìn)行性能分析。同時(shí)很多人也進(jìn)行了粒子濾波在姿態(tài)估計(jì)中的研究[3]。文[4]和文[5]通過對(duì)四元數(shù)的分析，設(shè)計(jì)了四元數(shù)粒子濾波算法求取姿態(tài)。本文通過對(duì)微機(jī)械器件數(shù)據(jù)采集和處理，設(shè)計(jì)非線性四元數(shù)卡爾曼濾波和粒子濾波，并對(duì)其姿態(tài)解算結(jié)果進(jìn)行進(jìn)一步分析。

1 四元數(shù)簡介

載體上的微機(jī)械慣性測量器件的輸出轉(zhuǎn)換成載體的姿態(tài)，即載體自身坐標(biāo)系(xb,yb,zb)相對(duì)于導(dǎo)航坐標(biāo)系(xn,yn,zn)的角位置，寫成矩陣形式如下:

(1)

歐拉角是載體的3個(gè)姿態(tài)角，即俯仰角(Pitch)、橫滾角(Roll)和偏航角(Yaw)。根據(jù)歐拉旋轉(zhuǎn)定律，可以通過3次旋轉(zhuǎn)使得載體坐標(biāo)系與導(dǎo)航坐標(biāo)系重合，每一次旋轉(zhuǎn)都繞導(dǎo)航坐標(biāo)系的一個(gè)坐標(biāo)軸轉(zhuǎn)動(dòng)，轉(zhuǎn)過的角度就是歐拉角，每次旋轉(zhuǎn)后坐標(biāo)關(guān)系可由一個(gè)旋轉(zhuǎn)矩陣表示，即方向余弦矩陣，(1)式采用z-y-x的旋轉(zhuǎn)順序依次旋轉(zhuǎn)：

Az-y-x(ψ,θ,φ)=Az(φ)Ay(θ)Ax(ψ)=

(2)

其中，ψ,φ,θ分別代表偏航角、橫滾角、俯仰角。為了避免歐拉角在表示姿態(tài)時(shí)可能出現(xiàn)的奇異問題，四元數(shù)在載體的姿態(tài)表示方面有廣泛的應(yīng)用。設(shè)計(jì)載體姿態(tài)四元數(shù)為

q=q0,q1,q2,q3=q0+q1i+q2j+q3k,

(3)

導(dǎo)航坐標(biāo)系與載體坐標(biāo)系之間的坐標(biāo)變換可以用方向余弦矩陣表示，其四元數(shù)形式為

(4)

簡記為：

(5)

(6)

四元數(shù)的微分方程如下：

(7)

其中，q為描述載體轉(zhuǎn)動(dòng)的四元數(shù)；ω為載體相對(duì)導(dǎo)航坐標(biāo)系的角速度，也表示為四元數(shù)的形式：

ω=0+ωxi+ωyj+ωzk，

(8)

(9)

(10)

通過三軸陀螺儀測量3個(gè)軸的角速度就可以實(shí)現(xiàn)實(shí)時(shí)更新四元數(shù)的值，進(jìn)而更新姿態(tài)角獲得姿態(tài)信息。

2 四元數(shù)卡爾曼濾波

在四元數(shù)和非線性濾波算法的結(jié)合中，最常用的算法就是基于四元數(shù)卡爾曼濾波，本文使用文[1]的四元數(shù)卡爾曼濾波，濾波過程簡介如下：

(1)濾波器初始化

為Q和R選取初始值，可以通過對(duì)靜態(tài)下三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)進(jìn)行測量，計(jì)算出各個(gè)軸的方差作為濾波器數(shù)據(jù)誤差；通過初始加速度計(jì)測量出俯仰角和橫滾角，用磁力計(jì)測量出方位角作為初始姿態(tài)，并把初始姿態(tài)角轉(zhuǎn)變?yōu)槌跏妓脑獢?shù)q0/0，選擇P0/0=I4作為系統(tǒng)初始噪聲。

(2)狀態(tài)方程傳播

文[1]中的Hamilton算子定義如下：

(11)

其中，s表示四元數(shù)的標(biāo)量；v表示四元數(shù)的矢量。

(12)

(13)

qk+1/k=Φk+1/kqk/k,

(14)

(15)

trMk/kI4-Mk/kQk.

(16)

(3)量測方程更新

首先通過預(yù)測得到的qk+1/k應(yīng)用(4)式計(jì)算出觀測方程中的系數(shù)矩陣H(qk+1/k).

(17)

bk+1,

(18)

(19)

(20)

(21)

qk+1/k+1=qk+1/k+Kk+1bk+1-Hqk+1/knk+1,

(22)

(23)

3 粒子濾波

粒子濾波是通過尋找一組在狀態(tài)空間中傳播的隨機(jī)樣本近似表示狀態(tài)變量，用樣本均值代替積分運(yùn)算，進(jìn)而獲得系統(tǒng)最小方差的過程，這些樣本被稱為粒子。采用數(shù)學(xué)語言描述如下：對(duì)于平穩(wěn)的隨機(jī)過程，假設(shè)k-1時(shí)刻系統(tǒng)的后驗(yàn)概率為p(xk-1zk-1),依據(jù)一定原則選取n個(gè)隨機(jī)樣本，k時(shí)刻獲得測量更新后，經(jīng)過狀態(tài)和時(shí)間過程，n個(gè)粒子的后驗(yàn)概率密度可近似為p(xkzk)，隨著粒子數(shù)目的增加，粒子的概率密度函數(shù)逐漸逼近狀態(tài)的概率密度函數(shù)，從而粒子濾波達(dá)到最優(yōu)貝葉斯估計(jì)的效果[3]。

假設(shè)非線性動(dòng)態(tài)離散系統(tǒng)為

(24)

其中，xk∈Rn為k時(shí)刻的n維狀態(tài)向量；zk∈Rm為m維觀測向量；wk和vk分別為過程噪聲和量測噪聲。

粒子濾波算法步驟如下：

(2)通過重要性采樣更新樣本粒子狀態(tài)，

(25)

(26)

(3)計(jì)算更新后粒子的權(quán)值

(27)

(4)歸一化粒子權(quán)值

(28)

(29)

(30)

(7)令k=k+1,重復(fù)以上步驟。

4 數(shù)據(jù)實(shí)驗(yàn)

實(shí)驗(yàn)通過選擇傳感器模塊mpu9250，它包含三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)，能夠通過自身所有的低通濾波器和A/D變換模塊直接輸出九軸傳感器數(shù)據(jù)。實(shí)驗(yàn)通過將mpu6050固定在轉(zhuǎn)臺(tái)上測量，在安裝過程中遠(yuǎn)離環(huán)境磁場的干擾。采用單片機(jī)讀取mpu9250測量數(shù)據(jù)后通過I2C串口傳輸給電腦進(jìn)行處理，以檢驗(yàn)算法的可用性和精度。

(1)首先將傳感器模塊mpu9250在靜止?fàn)顟B(tài)下測量輸出數(shù)據(jù)，采樣速率為100 Hz，采樣1 min，分析靜態(tài)情況下的九軸輸出數(shù)據(jù)。陀螺儀三軸靜態(tài)數(shù)據(jù)如圖1。

圖1 陀螺儀三軸靜態(tài)輸出數(shù)據(jù)
Fig.1 Three-axis gyroscope static output data

圖1是1 min采集的陀螺儀三軸數(shù)據(jù)，自上而下依次是x軸、y軸和z軸，運(yùn)用MATLAB軟件分析x軸的均值和方差分別為：0.0205(°)/s和1.238e(-4)(°)/s；y軸的均值和方差分別為：-0.004 8(°)/s和1.57e(-4)(°)/s；z軸的均值和方差分別為：0.015 4(°)/s和2.369 9e(-4)(°)/s。用同樣的方法處理三軸加速度計(jì)和三軸陀螺儀的原始數(shù)據(jù)，結(jié)果見表1。

通過以上測量，加速度計(jì)在水平位置x軸和y軸均值不為0，所以在做姿態(tài)解算時(shí)引入均值誤差提高測量精度。

(2)通過上面靜態(tài)數(shù)據(jù)的讀取，將各個(gè)傳感器的方差作為四元數(shù)卡爾曼濾波和粒子濾波程序的方差參考，靜止?fàn)顟B(tài)下四元數(shù)卡爾曼濾波和粒子濾波的姿態(tài)角結(jié)算誤差如圖2、圖3。其中橫坐標(biāo)表示時(shí)間，單位是秒；縱坐標(biāo)表示輸出的角度誤差，單位是(°)

表1 加速度計(jì)和陀螺儀輸出數(shù)據(jù)的均值和方差Table 1 Mean and variance of accelerometer and gyro output data

圖2 四元數(shù)卡爾曼濾波靜態(tài)三軸姿態(tài)角誤差Fig.2 Quaternion Kalman filter static triaxial attitude angle error

圖3 粒子濾波靜態(tài)三軸姿態(tài)角誤差Fig.3 Particle filter static triaxial attitude angle error

靜止?fàn)顟B(tài)下四元數(shù)卡爾曼濾波和粒子濾波三軸姿態(tài)角均值和誤差如表2。通過對(duì)靜態(tài)數(shù)據(jù)的分析可以看出，兩種算法對(duì)均值改善相差不多，但是粒子濾波比四元數(shù)卡爾曼濾波在方差上有所改善，即粒子濾波靜態(tài)測量相對(duì)穩(wěn)定。

(3)由于沒有轉(zhuǎn)臺(tái)等實(shí)驗(yàn)設(shè)備，通過對(duì)靜態(tài)數(shù)據(jù)添加一個(gè)穩(wěn)定的姿態(tài)仿真軌跡計(jì)算四元數(shù)卡爾曼濾波和粒子濾波在動(dòng)態(tài)情況下的結(jié)算結(jié)果。通過對(duì)三軸姿態(tài)角分別添加角速率為2，3，5(°)/s，幅度為5仿真，得到如圖4的仿真結(jié)果。其中橫坐標(biāo)表示時(shí)間，單位是秒；縱坐標(biāo)表示輸出的角度誤差，單位是(°)

表2 靜止?fàn)顟B(tài)兩種算法的均值和方差Table 2 Mean and variance of the two algorithms for the quiescent state

圖4 兩種算法姿態(tài)角誤差比較
Fig.4 Comparison of Attitude Angle Errors of Two Algorithms

表3 仿真狀態(tài)兩種算法的均值和方差Table 3 The mean and variance of the two algorithms for Simulation states

從表3可以看出，通過對(duì)動(dòng)態(tài)的仿真實(shí)驗(yàn)分析，兩種算法在均值改善和靜態(tài)數(shù)據(jù)基本相同，但是粒子濾波比四元數(shù)卡爾曼濾波在方差上有所改善，即動(dòng)態(tài)情況下粒子濾波相對(duì)穩(wěn)定。

5 結(jié) 論

本文通過對(duì)四元數(shù)卡爾曼濾波和粒子濾波進(jìn)行簡單介紹，并通過對(duì)mpu9250進(jìn)行數(shù)據(jù)采集和仿真實(shí)驗(yàn)，驗(yàn)證粒子濾波和四元數(shù)卡爾曼濾波的可行性，以誤差均值和標(biāo)準(zhǔn)差為檢驗(yàn)標(biāo)準(zhǔn)，驗(yàn)證了粒子濾波相對(duì)四元數(shù)卡爾曼濾波提高了標(biāo)準(zhǔn)差。

[1] Choukroun D, Bar-Itzhack I Y, Oshman Y. Novel quaternion Kalman filter[J]. IEEE Transactions on Aerospace and Electronics Systems, 2006, 42(1): 174-190.

[2] 高顯忠, 侯中喜, 王波, 等. 四元數(shù)卡爾曼濾波組合導(dǎo)航算法性能分析[J]. 控制理論與應(yīng)用, 2013, 30(2): 171-177.

Gao Xianzhong, Hou Zhongxi, Wang Bo, et al. Quaternion-based Kalman filter and its performance analysis in integrated navigation[J]. Control Theory & Applications, 2013, 30(2): 171-177.

[3] 梁軍. 粒子濾波算法及其應(yīng)用研究[D]. 哈爾濱: 哈爾濱工業(yè)大學(xué), 2009.

[4] 喬相偉, 周衛(wèi)東, 吉宇人. 基于四元數(shù)粒子濾波的飛行器姿態(tài)估計(jì)算法研究[J]. 兵工學(xué)報(bào), 2012, 33(9): 1070-1075.

Qiao Xiangwei, Zhou Weidong, Ji Yuren. Study on aerial vehicle attitude estimation based on quaternion particle filter algorithm[J]. Acta Armamentarii, 2012, 33(9): 1070-1075.

[5] 吳海亮, 王惠南, 陳志明, 等. 基于粒子濾波的微小衛(wèi)星姿態(tài)確定算法[J]. 中國慣性技術(shù)學(xué)報(bào), 2007, 15(4): 427-430.

Wu Hailiang, Wang Huinan, Chen Zhiming, et al. Particle filtering-based algorithm for micro-satelliteattitude determination[J]. Journal of Chinese Inertial Technology,2007,15(4):427-430.