基于擴(kuò)展卡爾曼濾波算法的機(jī)器人姿態(tài)解算研究*

黃龍亮,胡鵬飛,曹 祥,王海軍

(貴州裝備制造職業(yè)學(xué)院,貴州 貴陽(yáng) 551400)

0 引言

無(wú)人機(jī)能被廣泛用于軍用中的偵察、追蹤等任務(wù)和民用中的航拍、噴灑農(nóng)藥、配送快遞等任務(wù)[1]。在飛行過(guò)程中,無(wú)人機(jī)需要利用慣性導(dǎo)航系統(tǒng)不斷地檢測(cè)其飛行姿態(tài),為控制算法提供反饋數(shù)據(jù),實(shí)現(xiàn)良好的飛行效果,但由于發(fā)動(dòng)機(jī)振動(dòng)和氣流的影響,會(huì)導(dǎo)致慣性導(dǎo)航系統(tǒng)測(cè)量的數(shù)據(jù)存在大量噪聲和干擾,使得飛行變得不穩(wěn)定,甚至發(fā)生墜機(jī)的危險(xiǎn)。

慣性導(dǎo)航系統(tǒng)由陀螺儀、加速度計(jì)和電子羅盤組成。陀螺儀容易受溫度影響,在積分后會(huì)產(chǎn)生嚴(yán)重的累積誤差;加速度計(jì)易受載體震動(dòng)和運(yùn)動(dòng)加速度的影響;電子羅盤容易受外部磁場(chǎng)的影響[2]。這三種傳感器測(cè)量的數(shù)據(jù)不能直接使用,需要進(jìn)行傳感器數(shù)據(jù)濾波和姿態(tài)解算。

目前傳感器數(shù)據(jù)濾波和姿態(tài)解算方法,包括互補(bǔ)濾波、擴(kuò)展卡爾曼濾波(EKF)和無(wú)跡卡爾曼濾波(UKF)等[3-5],但多數(shù)研究停留在仿真,未進(jìn)行實(shí)際應(yīng)用。本文利用擴(kuò)展卡爾曼濾波(EKF)進(jìn)行對(duì)傳感器數(shù)據(jù)濾波和姿態(tài)解算,并通過(guò)靜態(tài)測(cè)試和動(dòng)態(tài)測(cè)試,對(duì)比分析EKF算法的實(shí)際效果。

1 EKF濾波器設(shè)計(jì)

慣性導(dǎo)航系統(tǒng)中三軸角速度計(jì)輸出的角速度,有低頻噪聲,通過(guò)角速度積分獲得的姿態(tài)會(huì)漂移,而三軸加速度計(jì)、三軸磁傳感器輸出靜態(tài)數(shù)據(jù)有高頻噪聲,動(dòng)態(tài)數(shù)據(jù)比較差。因此,EKF濾波器設(shè)計(jì)中,在獲取到角速度、加速度和磁向量數(shù)據(jù)后,先使用EKF算法得到濾波后的數(shù)據(jù),最后解算出俯仰、滾轉(zhuǎn)、偏航角度,流程如圖1所示。

圖1 基于擴(kuò)展卡爾曼濾波算法的姿態(tài)解算流程圖

根據(jù)哥氏定理和向量在不同坐標(biāo)系間的變換關(guān)系,得到方向余弦矩陣的微分方程:

(1)

在機(jī)體坐標(biāo)系下的重力場(chǎng)和磁場(chǎng)微分向量:

(2)

其中,在機(jī)體坐標(biāo)系下,重力場(chǎng)向量gb=[gbxgbygbz]T,在地面坐標(biāo)系下重力場(chǎng)向量gg=[0 0g]T;在機(jī)體坐標(biāo)系下,磁場(chǎng)向量Mb=[MbxMbyMbz]T,地面坐標(biāo)系下的磁場(chǎng)向量Mg=[MgxMgyMgz]T。將公式(1)帶入公式(2)得到:

(3)

加速度計(jì)和磁航向計(jì)的輸出表示為:

(4)

fb=-gb+wa

(5)

(6)

根據(jù)公式(1)-(6),得到線性化的狀態(tài)方程和觀測(cè)方程:

(7)

式中,Φk,k-1為狀態(tài)轉(zhuǎn)移陣,Hk為觀測(cè)陣。

(8)

(9)

當(dāng)磁地理坐標(biāo)系(m系)與b系重合時(shí),磁航向計(jì)的輸出為Mm=[MN0MD]T,可得到地磁矢量X軸和Y軸分量:

(10)

(11)

2 實(shí)驗(yàn)與分析

在編程軟件Keil中,將上述擴(kuò)展卡爾曼濾波算法編寫為程序,并下載到實(shí)驗(yàn)平臺(tái)數(shù)據(jù)處理芯片中。其中系統(tǒng)噪聲陣Q和測(cè)量噪聲陣R如表1所示。實(shí)驗(yàn)平臺(tái)如圖2所示:

表1 擴(kuò)展卡爾曼濾波參數(shù)表

圖2 擴(kuò)展卡爾曼濾波算法實(shí)驗(yàn)測(cè)試平臺(tái)

1)自行搭建450四旋翼;

2)數(shù)據(jù)濾波解算芯片STM32f405RGT;

3)數(shù)據(jù)采集使用Labview地面站完成;

4)在MATLAB中對(duì)采集數(shù)據(jù)顯示。

靜態(tài)時(shí)測(cè)量濾波前后數(shù)據(jù)如圖3所示,可以發(fā)現(xiàn)偏航、俯仰、滾轉(zhuǎn)三通道的尖峰高頻噪聲明顯減少,噪聲能夠得到較好的抑制。

圖3 靜態(tài)測(cè)試濾波前后的姿態(tài)解算角度 圖4 動(dòng)態(tài)測(cè)試濾波前后的姿態(tài)解算角度

動(dòng)態(tài)時(shí)測(cè)量濾波前后數(shù)據(jù)結(jié)果如圖4所示,可以發(fā)現(xiàn)偏航、俯仰、滾轉(zhuǎn)三通道,動(dòng)態(tài)性能號(hào),同時(shí)尖峰高頻噪聲也明顯減少,性能表現(xiàn)較好。

3 結(jié)論

本文使用擴(kuò)展卡爾曼濾波(EKF)對(duì)傳感器測(cè)量的數(shù)據(jù)濾波,并進(jìn)行姿態(tài)解算,通過(guò)靜態(tài)、動(dòng)態(tài)測(cè)試完成濾波前后數(shù)據(jù)對(duì)比,實(shí)驗(yàn)結(jié)果表明擴(kuò)展卡爾曼濾波(EKF)不僅可以濾除測(cè)量數(shù)據(jù)中的大部分噪聲,同時(shí)可以提高姿態(tài)解算精度。