曾 聰，章 政,2，王 龍

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

0 引 言

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

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

針對(duì)MEMS中單個(gè)慣性測(cè)量器件可信度低和復(fù)雜算法對(duì)硬件要求高等問題，本文提出一種基于共軛梯度的擴(kuò)展卡爾曼姿態(tài)估計(jì)算法。該算法通過將共軛梯度引入觀測(cè)模型，對(duì)噪聲進(jìn)行動(dòng)態(tài)的估計(jì)并采用傾角補(bǔ)償?shù)姆椒▎为?dú)計(jì)算偏航角從而提高了飛行器姿態(tài)解算的精度和抗干擾性能。本文設(shè)計(jì)4組對(duì)比實(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)解算中，通常定義兩個(gè)坐標(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)，分別對(duì)應(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)系。其中歐拉角法會(huì)遇到Gimbal Lock問題，方向余弦法計(jì)算量大、效率低無法滿足四旋翼飛行器的姿態(tài)解算，本文采用簡(jiǎn)單易行、效率高的四元數(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)估計(jì)算法

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

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

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

2.1.1 狀態(tài)模型

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

(5)

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

(6)

對(duì)式(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 觀測(cè)模型

構(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)步長(zhǎng)，dk為第k次姿態(tài)更新的搜索方向，qcg,k為第k次更新的姿態(tài)四元數(shù)。

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

(14)

(15)

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

(16)

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

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

(17)

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

(18)

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

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

2.2 變?cè)肼曁幚?/h3>

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

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

Rk=αR0

(19)

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

(20)

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

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

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

(21)

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

(22)

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

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

本實(shí)驗(yàn)平臺(tái)采用自行設(shè)計(jì)的飛控板實(shí)現(xiàn)，選用STM32F405(Cortex-M4內(nèi)核)為主控處理器，最高工作頻率可達(dá)168 MHz。IMU采用MPU6050和MS5611，MPU6050集成一個(gè)三軸加速度計(jì)和一個(gè)陀螺儀，MS5611用于測(cè)量磁場(chǎng)。系統(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è)計(jì)算法的可行性和有效性，本文設(shè)計(jì)了4組對(duì)比實(shí)驗(yàn)：第一組實(shí)驗(yàn)為靜態(tài)對(duì)比實(shí)驗(yàn)，驗(yàn)證算法的靜態(tài)性能。第二組實(shí)驗(yàn)為磁干擾對(duì)比實(shí)驗(yàn)，驗(yàn)證算法的抗磁性能。第三組實(shí)驗(yàn)為水平滑動(dòng)對(duì)比實(shí)驗(yàn)，驗(yàn)證算法在非重力運(yùn)動(dòng)加速度干擾下，姿態(tài)解算的準(zhǔn)確性。第四組實(shí)驗(yàn)為繞軸轉(zhuǎn)動(dòng)對(duì)比實(shí)驗(yàn)，驗(yàn)證算法的動(dòng)態(tài)性能。

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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

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

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

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

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

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