基于無跡卡爾曼濾波的四旋翼無人飛行器姿態(tài)估計(jì)算法

朱 巖，付 巍

（1.北京長城計(jì)量測(cè)試技術(shù)研究所，北京 100095；2.中北大學(xué)計(jì)算機(jī)與控制工程學(xué)院，山西太原 030051）

0 引 言

姿態(tài)解算的精度是實(shí)現(xiàn)四旋翼無人飛行器穩(wěn)定飛行的前提［1，2］.目前，通常采用陀螺儀獲得飛行器的姿態(tài)角.但陀螺儀存在漂移的問題，長期測(cè)量精度較差，因此需要對(duì)陀螺儀輸出的數(shù)據(jù)進(jìn)行有效的數(shù)據(jù)處理才能獲得可靠和穩(wěn)定的姿態(tài)角信息.陀螺儀輸出的數(shù)據(jù)通過運(yùn)動(dòng)學(xué)微分方程，可以推導(dǎo)出載體的姿態(tài)角，其觀測(cè)方程具有非線性的特點(diǎn).擴(kuò)展卡爾曼濾波算法是常用的解決觀測(cè)方程非線性問題的算法.擴(kuò)展卡爾曼濾波算法將非線性方程進(jìn)行泰勒級(jí)數(shù)展開，去掉二階以上的高階項(xiàng)，使非線性方程線性化，其優(yōu)點(diǎn)是運(yùn)算速度快，實(shí)時(shí)性強(qiáng)，但估計(jì)精度較低［3］.為了提高飛行器的姿態(tài)估計(jì)精度，本文利用無跡卡爾曼濾波算法實(shí)現(xiàn)飛行器的姿態(tài)估計(jì).無跡卡爾曼濾波算法在姿態(tài)角的估計(jì)值附近進(jìn)行確定采樣，得到一些采樣點(diǎn)，這些采樣點(diǎn)被稱為Sigma點(diǎn).在無跡卡爾曼濾波算法中，狀態(tài)分布采用高斯隨機(jī)變量表示，通過采用一些確定的Sigma點(diǎn)來描述高斯隨機(jī)變量的特征.Sigma點(diǎn)集合具有和高斯隨機(jī)變量相同的均值和方差.這些采樣點(diǎn)通過非線性函數(shù)傳播后，后驗(yàn)均值和方差可以達(dá)到2階精度，而擴(kuò)展卡爾曼濾波算法只能達(dá)到1階精度［4］.無跡卡爾曼濾波算法不同于擴(kuò)展卡爾曼濾波算法之處在于無跡卡爾曼濾波算法不是對(duì)非線性函數(shù)的近似，而是對(duì)狀態(tài)隨機(jī)變量分布的近似，它使用真實(shí)的非線性模型，因此，無跡卡爾曼濾波算法的精度和魯棒性都強(qiáng)于擴(kuò)展卡爾曼濾波算法.

1 四旋翼無人飛行器姿態(tài)解算模型的建立

四旋翼無人飛行器姿態(tài)解算是將飛行器上慣性單元的輸出，實(shí)時(shí)轉(zhuǎn)換成飛行器的姿態(tài)，即飛行器的載體坐標(biāo)系（b系）相對(duì)于導(dǎo)航坐標(biāo)系（n系）的角位置.

載體坐標(biāo)系（b系），是固定在載體上面的坐標(biāo)系，其原點(diǎn)在載體的質(zhì)心處，OX b沿載體橫向指向右，OY b沿載體縱向指向前，OZ b垂直于OX b Y b指向上.導(dǎo)航坐標(biāo)系（n系），是用來表示載體所處位置的坐標(biāo)系，其原點(diǎn)選在載體的質(zhì)心處，OX n指向東，OY n指向北，OZ n沿垂直方向指向天，通常稱為東北天坐標(biāo)系［5］.導(dǎo)航坐標(biāo)系與載體坐標(biāo)系如圖1所示.根據(jù)載體坐標(biāo)系和導(dǎo)航坐標(biāo)系之間的相對(duì)角位置關(guān)系，可以定義飛行器的3個(gè)姿態(tài)角.這3個(gè)姿態(tài)角分別為：俯仰角φ、橫滾角ψ、偏航角γ，如圖2所示.

圖2 導(dǎo)航坐標(biāo)系與載體坐標(biāo)系之間轉(zhuǎn)換角的關(guān)系示意圖Fig.2 Schematic diagram of the angle conversion relations between carrier coordinates and navigation coordinates

圖1 導(dǎo)航坐標(biāo)系與載體坐標(biāo)系示意圖Fig.1 Schematic diagram of carrier coordinates and navigation coordinates

由四旋翼無人飛行器上的三軸陀螺儀可以得到飛行器相對(duì)于導(dǎo)航坐標(biāo)系的角速度ωnb＝［ωnbx，ωnby，ωnbz］T，利用歐拉角法可得到飛行器的姿態(tài)運(yùn)動(dòng)學(xué)方程［6］.

四旋翼無人飛行器上的三軸陀螺儀在每一個(gè)采樣時(shí)刻可以同時(shí)得到當(dāng)前時(shí)刻飛行器的姿態(tài)角和姿態(tài)角變化率的信息.利用三軸陀螺儀測(cè)得的被噪聲污染的角速度信息，結(jié)合數(shù)據(jù)處理算法就能夠估算出飛行器當(dāng)前的姿態(tài)角，實(shí)現(xiàn)對(duì)飛行器的姿態(tài)估計(jì).

2 四旋翼無人飛行器姿態(tài)估計(jì)算法

2.1 系統(tǒng)狀態(tài)方程

根據(jù)飛行器姿態(tài)解算模型，可建立系統(tǒng)狀態(tài)方程和觀測(cè)方程.

W（k）為第k個(gè)采樣時(shí)刻的系統(tǒng)噪聲向量；V（k）為第k個(gè)采樣時(shí)刻的觀測(cè)噪聲向量.

2.2 無跡卡爾曼濾波算法

為了提高飛行器姿態(tài)估計(jì)精度，提出利用無跡卡爾曼濾波算法對(duì)安裝于飛行器上的三軸陀螺儀輸出的數(shù)據(jù)進(jìn)行處理，解算出飛行器的姿態(tài)角.

無跡卡爾曼濾波算法單步流程如下：

假設(shè)在第k個(gè)采樣時(shí)刻，系統(tǒng)狀態(tài)的估計(jì)值表示為）；通過預(yù)測(cè)得到下一采樣時(shí)刻（即第k＋1次）的系統(tǒng)狀態(tài)的估計(jì)值表示為（k＋1｜k）；經(jīng)過一次無跡卡爾曼濾波算法更新后，最終得到的在第k＋1個(gè)采樣時(shí)刻，系統(tǒng)狀態(tài)的估計(jì)值表示為X（k＋1）.

在第k個(gè)采樣時(shí)刻，系統(tǒng)狀態(tài)的估計(jì)誤差協(xié)方差矩陣表示為P（k）；通過P（k）預(yù)測(cè)得到第k＋1個(gè)采樣時(shí)刻系統(tǒng)狀態(tài)的估計(jì)誤差協(xié)方差矩陣表示為P（k＋1｜k）.經(jīng)過一次無跡卡爾曼濾波算法更新后，得到的在第k＋1個(gè)采樣時(shí)刻，系統(tǒng)的估計(jì)誤差協(xié)方差矩陣表示為P（k＋1）.第k＋1個(gè)采樣時(shí)刻系統(tǒng)狀態(tài)的估計(jì)誤差協(xié)方差矩陣P（k＋1）反映了在第k個(gè)采樣時(shí)刻系統(tǒng)狀態(tài)的估計(jì)值與系統(tǒng)狀態(tài)的真實(shí)值的偏差.

確定Sigma點(diǎn)和各個(gè)Sigma點(diǎn)的權(quán)重

式中：n x表示系統(tǒng)狀態(tài)向量的維數(shù)；λ＝ξ2（n x＋κ）－nx.

式中：ξ，κ，η為待選參數(shù).ξ控制采樣點(diǎn)的分布狀態(tài)，調(diào)節(jié)ξ可以使高階項(xiàng)的影響達(dá)到最小.ξ應(yīng)該是一個(gè)較小的值，以避免狀態(tài)方程非線性嚴(yán)重時(shí)采樣點(diǎn)的非局域性影響［5］.κ的具體取值沒有界限，對(duì)高斯分布的情況，當(dāng)狀態(tài)向量為單變量時(shí)，選擇κ＝2；狀態(tài)向量為多變量時(shí)，一般選擇κ＝3－n x.η可以合并方程中高階項(xiàng)的動(dòng)差，調(diào)節(jié)η可以提高方差的精度［6］.

3 仿真實(shí)例

圖3 俯仰角估計(jì)誤差對(duì)比曲線Fig.3 The estimation error contrast curve of pitch angle

為了驗(yàn)證UKF算法的有效性和實(shí)用性，設(shè)置了飛行器和陀螺儀的仿真數(shù)值，通過仿真實(shí)驗(yàn)的方式比較EKF和UKF在相同條件下對(duì)飛行器姿態(tài)信息解算的精度.飛行器參數(shù)設(shè)置如下：飛行器的飛行傾角為10°，飛行器的三軸初始姿態(tài)角分別為1°，2°，5°，飛行器的轉(zhuǎn)動(dòng)慣量矩陣為J＝［3，0，0；0，9，0；0，0，9］.慣性單元參數(shù)設(shè)置如下：陀螺儀測(cè)量白噪聲為0.6°／h，陀螺儀輸出的初始姿態(tài)角速度分別為：0.01°／s，0.02°／s，0.03°／s，陀螺漂移在三軸上的初始值分別為1°／h，陀螺儀輸出采樣頻率為100 Hz.依據(jù)上述初始條件，分別采用EKF和UKF對(duì)飛行器姿態(tài)角估計(jì)，估計(jì)結(jié)果如圖3～圖5所示.

由圖3～圖5可以看出，無跡卡爾曼濾波算法和擴(kuò)展卡爾曼濾波算法都能實(shí)現(xiàn)對(duì)飛行器姿態(tài)角的估計(jì)，但在相同條件下，無跡卡爾曼濾波算法的估計(jì)精度和收斂速度都明顯好于擴(kuò)展卡爾曼濾波算法.

圖4 橫滾角估計(jì)誤差對(duì)比曲線Fig.4 The estimation error contrast curve of roll angle

圖5 偏航角估計(jì)誤差對(duì)比曲線Fig.5 The estimation error contrast curve of yaw angle

4 結(jié) 論

研究了基于陀螺儀獲取四旋翼無傷飛行器姿態(tài)解算模型的方法，建立了飛行器的系統(tǒng)狀態(tài)方程和測(cè)量方程.為了提高飛行器姿態(tài)解算精度，提出了一種基于無跡卡爾曼濾波的姿態(tài)估計(jì)算法.理論分析和仿真表明，在相同條件下，基于無跡卡爾曼濾波的飛行器姿態(tài)估計(jì)算法的估計(jì)精度和收斂速度都明顯好于擴(kuò)展卡爾曼濾波算法.

［1］ 單海燕.四旋翼無人直升機(jī)飛行控制技術(shù)研究［D］.南京：南京航空航天大學(xué)，2008.

［2］ 聶博文，馬宏緒，王劍，等.微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù)［J］.電光與控制，2007，14（6）：113－117.

Nie Bowen，Ma Hongxu，Wang Jian，et al.Study on actualities and critical technologies of micro／mini quadrotor［J］.Electronics Optics ＆Control，2007，14（6）：113－117.（in Chinese）

［3］ Julier S J，Uhlmann J K，Durrant－Whyte H.A new approach for filtering nonliear systems［J］.In：Proceedings of the American Control Conference，2005，11（2）：1628－1632.

［4］ Julier S J，Uhlmann J K.A new extension of the Kalman filter to nonlinear systems［J］.Proc.Of Aerosense：the 11th International Sumposium on Aerospace／Defence Sensing，Simulation and Controls，Orlando，F(xiàn)lorida，2007，3（6）：118－124.

［5］ 馬敏，吳海超.基于四元數(shù)自補(bǔ)償四旋翼飛行器姿態(tài)解算［J］.制造業(yè)自動(dòng)化，2013，35（12）：18－21.

Ma Min，Wu Haichao.Quaternion－based self－compensating attitude estimation for a quad－rotor aircraft［J］.Manufacturing Automation，2013，35（12）：18－21.（in Chinese）

［6］ 周曉堯，范大鵬，張智永，等.基于模糊加權(quán)EKF的車載光電桅桿姿態(tài)估計(jì)算法［J］.紅外與激光工程，2011，40（8）：1569－1575.

Zhou Xiaoyao，F(xiàn)an Dapeng，Zhang Zhiyong，et al.Attitude estimation algorithm of vehicle－bone optronics mast systems based on fuzzy weighted EKF［J］.Infrared and Laser Engineering，2011，40（8）：1569－1575.（in Chinese）