基于共軛梯度的EKF姿態(tài)估計算法

曾 聰，章 政,2，王 龍

(1.武漢科技大學(xué) 信息科學(xué)與工程學(xué)院， 湖北 武漢 430081；2.武漢科技大學(xué) 冶金自動化與檢測技術(shù)教育部工程研究中心， 湖北 武漢 430081； 3.湖北汽車工業(yè)學(xué)院 電氣與信息工程學(xué)院， 湖北 十堰 442000)

0 引 言

姿態(tài)解算是四旋翼飛行器[1-3]設(shè)計的核心環(huán)節(jié)，由于微機(jī)電系統(tǒng)(micro-electro mechanical systems，MEMS)具有成本低、體積小、功耗低等特點(diǎn)，它已成為了四旋翼姿態(tài)測量的首選器件[4]。但是實(shí)際的導(dǎo)航問題中，低成本的陀螺儀存在漂移，長時間運(yùn)行漂移嚴(yán)重會產(chǎn)生累積誤差，易導(dǎo)致解算姿態(tài)不穩(wěn)定[5]，因此實(shí)現(xiàn)多傳感器融合及設(shè)計穩(wěn)定快速的濾波算法是獲取飛行器精準(zhǔn)的姿態(tài)信息的有效途徑之一[6]。

文獻(xiàn)[7]和文獻(xiàn)[8]提出了一種基于互補(bǔ)濾波的姿態(tài)解算算法，該算法利用加速度計和磁力計來補(bǔ)償陀螺儀漂移所引起的姿態(tài)偏差。文獻(xiàn)[9]和文獻(xiàn)[10]提出了一種基于梯度下降法的姿態(tài)融合算法，利用梯度下降法求解出目標(biāo)函數(shù)誤差方程姿態(tài)四元數(shù)與微分方程求解出的姿態(tài)四元數(shù)進(jìn)行融合，得到最優(yōu)姿態(tài)。由于互補(bǔ)濾波和梯度算法均沒有對系統(tǒng)過程噪聲和傳感器的觀測噪聲進(jìn)行分析，從理論上已證明Kalman濾波算法是不同導(dǎo)航器件的測量信息和系統(tǒng)狀態(tài)信息融合的有效方法。文獻(xiàn)[11]采用了基于四元數(shù)的卡爾曼(Kalman filter，KF)濾波器姿態(tài)估計算法，將過程噪聲和觀測噪聲引入姿態(tài)估計系統(tǒng)，通過KF濾波器進(jìn)行姿態(tài)融合，但是KF濾波器針對的是線性系統(tǒng)，而飛行器姿態(tài)估計系統(tǒng)一般是非線性系統(tǒng)。文獻(xiàn)[12]提出了一種擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)算法,該算法把非線性系統(tǒng)進(jìn)行線性化，提高了姿態(tài)估計的精度，但是在觀測方程線性化的同時引入了線性誤差。文獻(xiàn)[13]將EKF姿態(tài)估計算法進(jìn)行改進(jìn)，該改進(jìn)算法將梯度下降法所得到的姿態(tài)四元數(shù)引入觀測量，很好地解決了觀測模型線性化的誤差。文獻(xiàn)[14]提出了一種無跡卡爾曼濾波(unscented Kalman filter，UKF)姿態(tài)估計算法，該算法有效解決了上述觀測模型的線性誤差，同時也提高了姿態(tài)估計的精度。文獻(xiàn)[15]提出了一種基于粒子濾波(particle filter，PF)的飛行器姿態(tài)估計算法，該算法將狀態(tài)向量分為線性部分和非線性部分分別進(jìn)行處理，有效地提高了姿態(tài)估計的精度和穩(wěn)定性。然而，在實(shí)際應(yīng)用過程中，UKF算法和PF算法計算量過大，實(shí)現(xiàn)過程相對比較復(fù)雜，不易在MEMS中實(shí)現(xiàn)。

針對MEMS中單個慣性測量器件可信度低和復(fù)雜算法對硬件要求高等問題，本文提出一種基于共軛梯度的擴(kuò)展卡爾曼姿態(tài)估計算法。該算法通過將共軛梯度引入觀測模型，對噪聲進(jìn)行動態(tài)的估計并采用傾角補(bǔ)償?shù)姆椒▎为?dú)計算偏航角從而提高了飛行器姿態(tài)解算的精度和抗干擾性能。本文設(shè)計4組對比實(shí)驗(yàn)，實(shí)驗(yàn)數(shù)據(jù)表明改進(jìn)EFK算法能夠快速可靠跟蹤飛行器的姿態(tài)，并實(shí)現(xiàn)自主懸停。

1 四元數(shù)姿態(tài)系統(tǒng)

1.1 坐標(biāo)系介紹

在姿態(tài)解算中，通常定義兩個坐標(biāo)系表達(dá)姿態(tài)角的關(guān)系，分別為導(dǎo)航坐標(biāo)系(n系)和機(jī)體坐標(biāo)系(b系)，如圖1所示。導(dǎo)航坐標(biāo)系使用東北天(ENU)坐標(biāo)系，機(jī)體坐標(biāo)系使用標(biāo)準(zhǔn)右前上坐標(biāo)系，導(dǎo)航坐標(biāo)系與機(jī)體坐標(biāo)系原點(diǎn)重合且位于機(jī)體質(zhì)心。通常定義繞x軸旋轉(zhuǎn)的姿態(tài)角為傾仰角(pitch)、繞y軸旋轉(zhuǎn)的姿態(tài)角為橫滾角(roll)、繞z軸旋轉(zhuǎn)的姿態(tài)角為偏航角(yaw)，分別對應(yīng)圖中的Φ、θ、Ψ。

圖1 坐標(biāo)系旋轉(zhuǎn)

1.2 四元數(shù)與姿態(tài)的轉(zhuǎn)換

通常使用四元數(shù)法、歐拉角法、方向余弦法[7]描述導(dǎo)航坐標(biāo)系與機(jī)體坐標(biāo)系的轉(zhuǎn)換關(guān)系。其中歐拉角法會遇到Gimbal Lock問題，方向余弦法計算量大、效率低無法滿足四旋翼飛行器的姿態(tài)解算，本文采用簡單易行、效率高的四元數(shù)法進(jìn)行姿態(tài)解算。

(1)

根據(jù)zyx順規(guī)Φ、θ、Ψ，有如下轉(zhuǎn)換公式

(2)

1.3 四元數(shù)更新算法

姿態(tài)解算使用四元數(shù)微分方程完成姿態(tài)四元數(shù)更新。四元數(shù)微分方程表述的是角速度與四元數(shù)的關(guān)系，如式(3)所示

(3)

采用離散迭代的方法更新四元數(shù)，由式(4)可得

(4)

2 改進(jìn)EKF姿態(tài)估計算法

由于常規(guī)的EKF算法在精度和抗干擾性方面還存在不足，本文提出一種基于共軛梯度的擴(kuò)展卡爾曼姿態(tài)估計算法，該算法結(jié)構(gòu)如圖2所示。

圖2 基于共軛梯度的擴(kuò)展卡爾曼姿態(tài)估計算法結(jié)構(gòu)

2.1 卡爾曼濾波器的設(shè)計

2.1.1 狀態(tài)模型

本文所設(shè)計的狀態(tài)模型選取姿態(tài)四元數(shù)作為狀態(tài)量，根據(jù)式(3)中的四元數(shù)微分方程可以得到如下系統(tǒng)狀態(tài)方程

(5)

式中：Ω[ωb]是一個4*4的反對稱矩陣，如式(6)

(6)

對式(5)進(jìn)行離散化處理，假設(shè)陀螺儀的采樣周期為Ts(至少20 ms)，經(jīng)過離散化的狀態(tài)方程如式(7)

(7)

陀螺儀的輸出量作為狀態(tài)轉(zhuǎn)移的變量，陀螺儀的模型如式(8)所示[16]

ω=ωr+gb+gω

(8)

(9)

(10)

(11)

2.1.2 觀測模型

構(gòu)造目標(biāo)誤差方程，如式(12)所示

(12)

根據(jù)共軛梯度法[17]，可按照式(13)更新機(jī)體姿態(tài)四元數(shù)

qcg,k+1=qcg,k+λkdk

(13)

式中：λk為第k次姿態(tài)跟新的最優(yōu)步長，dk為第k次姿態(tài)更新的搜索方向，qcg,k為第k次更新的姿態(tài)四元數(shù)。

根據(jù)文獻(xiàn)[17]中共軛梯度法，姿態(tài)計算過程如式(14)所示

(14)

(15)

J(q)為式(15)的雅克比矩陣

(16)

通過以上公式計算過程，不停的進(jìn)行姿態(tài)四元數(shù)的迭代更新，并作為卡爾曼濾波器的觀測量。使得觀測模型可以簡化為

Zk=qcg,k-1=xk+vk,k=1,2,3,…

(17)

式中：qcg,k為共軛梯度法計算出來的卡爾曼的觀測量，觀測模型的噪聲與加速度的測量噪聲vk相關(guān)。所以過程噪聲協(xié)方差矩陣如式(18)所示

(18)

由此可得，觀測模型的觀測矩陣Hk=I4*4。根據(jù)卡爾曼濾波器的黃金五步法[13]，基于共軛梯度的卡爾曼姿態(tài)估計算法如圖3所示。

圖3 基于共軛梯度的擴(kuò)展卡爾曼姿態(tài)估計算法步驟

2.2 變噪聲處理

在實(shí)際飛行運(yùn)動過程中，四旋翼飛行器一般都是非勻速飛行，飛行器在高速飛行時會產(chǎn)生一定的運(yùn)動加速度，這種運(yùn)動加速度會降低姿態(tài)估計的精度，對姿態(tài)的控制產(chǎn)生不利的影響。通過常規(guī)物理減震方法和卡爾曼濾波器對高頻震動具有良好的濾波效果，但是無法消除非重力運(yùn)動加速度的影響。這種非重力運(yùn)動加速度是一種非零隨機(jī)噪聲，在觀測模型中，一般假設(shè)式(18)中的觀測噪聲Rk為常量，這樣會降低姿態(tài)估計的精度。針對上述非重力運(yùn)動加速度對姿態(tài)估計造成的影響，本文根據(jù)環(huán)境的變化設(shè)計一種自適應(yīng)處理方法，該方法將觀測噪聲視為一種隨非重力運(yùn)動加速度實(shí)時變化的動態(tài)隨機(jī)噪聲，從而實(shí)現(xiàn)噪聲的動態(tài)跟蹤估計。

假設(shè)在非重力運(yùn)動加速度可忽略的情況下，測得過程噪聲的協(xié)方差矩陣最優(yōu)值為R0，設(shè)計k時刻的觀測噪聲協(xié)方差矩陣如式(19)所示

Rk=αR0

(19)

式中：α為觀測噪聲的自適應(yīng)調(diào)整因子，當(dāng)非重力運(yùn)動加速度越大時，Rk也越大

(20)

式中：μ為可調(diào)參數(shù)，為常量，根據(jù)實(shí)際情況和不同的系統(tǒng)做適當(dāng)?shù)恼{(diào)整即可。

2.3 改進(jìn)偏航角估計

在實(shí)際飛行的環(huán)境中，存在一些磁干擾源的影響，造成磁力計的測量值會產(chǎn)生很大的誤差，導(dǎo)致水平姿態(tài)解算錯誤。為了避免磁干擾造成的水平姿態(tài)解算誤差，在改進(jìn)的EKF算法中，未將磁力計的測量值融合到卡爾曼濾波器中，而是單獨(dú)的計算偏航角，由此增強(qiáng)了姿態(tài)的抗磁干擾能力。

(21)

式中：pitch(Φ)和roll(θ)由式(2)中給出，偏航角yaw(Ψ)由傾角補(bǔ)償后磁力計計算可得

(22)

3 實(shí)驗(yàn)研究

3.1 實(shí)驗(yàn)平臺

本實(shí)驗(yàn)平臺采用自行設(shè)計的飛控板實(shí)現(xiàn)，選用STM32F405(Cortex-M4內(nèi)核)為主控處理器，最高工作頻率可達(dá)168 MHz。IMU采用MPU6050和MS5611，MPU6050集成一個三軸加速度計和一個陀螺儀，MS5611用于測量磁場。系統(tǒng)的控制頻率為250 Hz，即4 ms進(jìn)行一次姿態(tài)解算。

四旋翼飛行器的各項(xiàng)數(shù)據(jù)見表1。

表1 四旋翼飛行器各項(xiàng)數(shù)據(jù)

3.2 實(shí)驗(yàn)與結(jié)果分析

為了驗(yàn)證本文所設(shè)計算法的可行性和有效性，本文設(shè)計了4組對比實(shí)驗(yàn)：第一組實(shí)驗(yàn)為靜態(tài)對比實(shí)驗(yàn)，驗(yàn)證算法的靜態(tài)性能。第二組實(shí)驗(yàn)為磁干擾對比實(shí)驗(yàn)，驗(yàn)證算法的抗磁性能。第三組實(shí)驗(yàn)為水平滑動對比實(shí)驗(yàn)，驗(yàn)證算法在非重力運(yùn)動加速度干擾下，姿態(tài)解算的準(zhǔn)確性。第四組實(shí)驗(yàn)為繞軸轉(zhuǎn)動對比實(shí)驗(yàn)，驗(yàn)證算法的動態(tài)性能。

3.2.1 靜態(tài)實(shí)驗(yàn)

靜態(tài)實(shí)驗(yàn)將四旋翼飛行器放置在無磁干擾水平的平臺上，分別采集兩組算法的姿態(tài)角進(jìn)行對比。根據(jù)圖4可以得到各算法姿態(tài)角的標(biāo)準(zhǔn)差和波動范圍見表2。兩種姿態(tài)估計算法基本滿足四旋翼飛行器飛行要求，在靜態(tài)時改進(jìn)的EKF姿態(tài)估計算法的波動范圍和標(biāo)準(zhǔn)差小于常規(guī)的EKF姿態(tài)估計算法。由該組靜態(tài)實(shí)驗(yàn)對比可知，改進(jìn)的EKF姿態(tài)估計算法比常規(guī)的EKF姿態(tài)估計算法精度更高，解算的姿態(tài)更加穩(wěn)定。

3.2.2 磁干擾實(shí)驗(yàn)

磁實(shí)驗(yàn)將四旋翼飛行器靜止放置在一個無磁干擾源的水平平臺上，利用一根有電流流過的導(dǎo)線模擬磁場干擾源，在四旋翼飛行器的上方來回劃動。兩種算法的磁干擾實(shí)驗(yàn)結(jié)果如圖5所示。在干擾磁場加入后，常規(guī)EKF姿態(tài)估計算法的水平姿態(tài)角pitch和roll角受到磁干擾影響嚴(yán)重，而改進(jìn)EKF姿態(tài)估計算法幾乎不受磁干擾的影響。由抗磁實(shí)驗(yàn)對比可知，從卡爾曼濾波器中分離出偏航角的改進(jìn)EKF姿態(tài)估計算法的抗磁能力明顯增強(qiáng)。

圖4 靜態(tài)實(shí)驗(yàn)兩種算法的姿態(tài)角

表2 靜態(tài)實(shí)驗(yàn)兩種算法的誤差

3.2.3 水平滑動實(shí)驗(yàn)

水平滑動實(shí)驗(yàn)將四旋翼飛行器放置在水平的平臺上，沿著任意的方向來回的水平滑動。兩種算法水平滑動實(shí)驗(yàn)姿態(tài)角數(shù)據(jù)如圖6所示， 根據(jù)圖6姿態(tài)角的數(shù)據(jù)得到兩種算法水平姿態(tài)角的標(biāo)準(zhǔn)差和波動范圍見表3。當(dāng)存在非重力運(yùn)動加速度時，兩種算法的水平姿態(tài)角都會受到一定的影響，但是改進(jìn)EKF姿態(tài)估計算法波動范圍比較小、而且更加穩(wěn)定。從該組水平滑動實(shí)驗(yàn)對比可得，在非重力運(yùn)動加速度下，改進(jìn)EKF姿態(tài)估計算法的精度明顯優(yōu)于常規(guī)EKF姿態(tài)估計算法，而且有效抑制非重力運(yùn)動加速度對姿態(tài)解算的影響。

表3 水平滑動實(shí)驗(yàn)兩種算法的誤差

3.2.4 繞軸轉(zhuǎn)動實(shí)驗(yàn)

繞軸轉(zhuǎn)動實(shí)驗(yàn)將四旋翼飛行器分別繞著x軸和y軸從-70°到70°來回轉(zhuǎn)動3次，分別采集兩種姿態(tài)估計算法的pitch角和roll角。兩種姿態(tài)估計算法的姿態(tài)角結(jié)果如圖7所示。兩種姿態(tài)估計算法的隨動性都很好，都能夠跟隨著機(jī)體的快速轉(zhuǎn)動實(shí)時精準(zhǔn)的解算出姿態(tài)。圖7中局部區(qū)域放大如圖8所示，改進(jìn)EKF姿態(tài)估計算法比常規(guī)的EKF姿態(tài)估計算法的曲線更加平滑。由該組對比實(shí)驗(yàn)可得，改進(jìn)EKF姿態(tài)估計算法在快速轉(zhuǎn)動過程中姿態(tài)更加平滑，動態(tài)性能優(yōu)于常規(guī)EKF姿態(tài)估計算法。

4 結(jié)束語

本文提出一種基于共軛梯度的擴(kuò)展卡爾曼姿態(tài)估計算法。該改進(jìn)EKF姿態(tài)估計算法把基于共軛梯度算法估計出來的姿態(tài)四元數(shù)引入到觀測模型，避免了觀測模型線性化帶來的誤差，提高了估計精度。然后將非重力運(yùn)動加速度引入到噪聲協(xié)方差矩陣，實(shí)現(xiàn)噪聲的自適應(yīng)動態(tài)估計，抑制非重力運(yùn)動加速度對姿態(tài)估計的影響，采用傾角補(bǔ)償?shù)姆椒▎为?dú)計算偏航角，提高了四旋翼飛行器抗干擾能力。

圖5 抗磁干擾實(shí)驗(yàn)姿態(tài)角

圖6 水平滑動實(shí)驗(yàn)姿態(tài)角

圖7 繞軸轉(zhuǎn)動實(shí)驗(yàn)姿態(tài)角

圖8 圖7局部區(qū)域放大圖

實(shí)驗(yàn)結(jié)果表明：改進(jìn)EKF姿態(tài)估計算法提高了姿態(tài)估計的精度，在機(jī)體快速運(yùn)動的情況下，能夠?qū)崟r精準(zhǔn)的解算出姿態(tài)角并且有效抑制非重力運(yùn)動加速度對姿態(tài)估計造成的影響，在磁干擾下能夠確保四旋翼飛行器的水平姿態(tài)不受影響，保證四旋翼飛行器的穩(wěn)定飛行。