王 龍,章 政,王 立

武漢科技大學 信息科學與工程學院,武漢 430081)(*通信作者電子郵箱124296774@qq.com)

改進擴展卡爾曼濾波的四旋翼姿態(tài)估計算法

王 龍*,章 政,王 立

武漢科技大學 信息科學與工程學院,武漢 430081)(*通信作者電子郵箱124296774@qq.com)

為了提高標準擴展卡爾曼姿態(tài)估計算法的精確度和快速性,將運動加速度抑制的動態(tài)步長梯度下降算法融入擴展卡爾曼中,提出一種改進擴展卡爾曼的四旋翼姿態(tài)估計算法。該算法在卡爾曼測量更新中采用梯度下降法進行非線性觀測,消除標準擴展卡爾曼算法在線性化時帶來的線性化誤差,提高算法的準確性和快速性;對梯度下降法的梯度步長進行動態(tài)處理,使算法步長與四旋翼飛行器的運動合角速度成正比,增強微型四旋翼飛行器姿態(tài)解算的動態(tài)性能;對強機動運動過程中機體產生的運動加速度進行抑制處理,消除運動加速度對姿態(tài)解算的不利影響,提高了微型四旋翼飛行器姿態(tài)解算的跟蹤精度。為了驗證所設計算法的可行性和有效性,基于STM32單片機搭建四旋翼實驗平臺系統進行實時在線性能驗證。結果表明,所設計算法能提高四旋翼飛行器在強機動、高速運動情況下的姿態(tài)跟蹤精度、動態(tài)性能,增強姿態(tài)融合算法的抗干擾性,保證微型四旋翼飛行器的穩(wěn)定飛行。

四旋翼飛行器;擴展卡爾曼濾波器;運動加速度抑制;動態(tài)步長;梯度下降法

0 引言

四旋翼飛行器(Quadrotor) 具有體積小、重量輕、易于隱蔽等優(yōu)點,被廣泛應用在農業(yè)、軍工業(yè)維護和偵查探測上[1-2],成為了目前無人飛行機器人(Unmanned Aerial Vehicle, UAV)研究領域的熱點問題。微機電系統(Micro-Electro-Mechanical System, MEMS)作為四旋翼飛行器慣性測量的主要傳感器,其中的低成本陀螺儀存在漂移積分累計誤差[3],影響了高精度姿態(tài)測量的效果,必須要求多種傳感器組合使用,才能獲得較為準確的姿態(tài)信息。因而,多傳感器之間的融合濾波算法以及四旋翼飛行器的姿態(tài)解算速度和精度直接決定了四旋翼飛行器的飛行質量,研究精確、快速的姿態(tài)解算[4-7]算法成為當前研究熱點之一。

針對以上問題,國內外學者們提出了多種姿態(tài)算法。文獻[8]提出了一種互補濾波姿態(tài)算法,利用加速度計對陀螺儀漂移誤差進行補償;但是沒有考慮到機體在強機動運動過程中產生的運動加速度和機體振動對加速度計的干擾造成姿態(tài)誤差變大的問題。相對于文獻[8]算法,文獻[9]提出的一種自適應混合濾波算法考慮了機體振動對加速度計的高頻噪聲,利用最優(yōu)重力估計來對參數優(yōu)化,消除了加速度的噪聲影響;但是仍然沒有對運動加速度進行處理。文獻[10]提出了一種定步長的梯度下降姿態(tài)估計算法,通過插值計算出誤差函數的極值,提高了姿態(tài)解算的精度。文獻[11]則在文獻[10]的基礎上增加了磁力計,提出了一種基于梯度下降法的9軸姿態(tài)融合算法。這兩種梯度下降算法中梯度步長為定值,當四旋翼飛行器處于高速運動時,梯度步長過小導致算法動態(tài)性能差。但是以上互補和梯度算法均未考慮到系統的噪聲和傳感器的測量噪聲問題。文獻[12]提出了一種擴展卡爾曼濾波(Extended Kalman Filter, EKF)方程測量模型,將系統噪聲和測量噪聲引入狀態(tài)方程,進一步提高了姿態(tài)解算的精度,但是在觀測方程線性化時引入了線性誤差;對此,文獻[13]提出一種EKF算法,將梯度下降算法引入EKF算法中,解決了這種線性化誤差,但是卻沒有解決運動加速度造成的錯誤姿態(tài)估計影響;文獻[14]提出一種無損卡爾曼濾波器(Unscented Kalman Filter, UKF),解決了這種線性化帶來的誤差;文獻[15]提出了一種基于EKF的算法通過高斯-牛頓法對觀測方程非線性觀測,簡化了觀測方程的計算,但是沒有考慮飛行器運動加速度的影響;文獻[16]提出一種粒子濾波器(Particle Filter,PF)算法針對系統噪聲為分高斯分布的情況。但是上述UKF和PF的計算量過大,在低成本的微控制器中不易實現。

本文提出一種改進EKF的四旋翼姿態(tài)估計算法。首先,由陀螺儀輸出的角速度通過四元數微分方程得到估計姿態(tài)四元數;然后,對全球定位系統(Global Positioning System, GPS)和加速度計融合后的加速度進行運動加速度抑制處理,消除運動加速度對姿態(tài)估計的影響,再通過動態(tài)步長梯度下降算法得到觀測姿態(tài)四元數;然后建立基于四元數的姿態(tài)估計濾波方程,從而完成了微型四旋翼飛行器的水平姿態(tài)精確、快速解算;最后通過磁力計校正并且傾角補償后單獨計算偏航角,實現了四旋翼飛行器的精確穩(wěn)定飛行。

1 姿態(tài)參考系統

1.1 姿態(tài)坐標系

在姿態(tài)解算中,定義兩個坐標系——導航坐標系n系和載體坐標系b系來表示姿態(tài)角,其中導航坐標系采用東北天建立坐標系,導航坐標系的坐標原點O位于運載體的質心。載體坐標系采用右前上建立坐標系,載體坐標系原點O′與導航坐標系原點O重合。姿態(tài)角的定義如圖1所示,繞X軸旋轉對應俯仰角pitch、繞Y軸旋轉對應橫滾角roll、繞Z軸旋轉對應偏航角yaw,分別為三軸歐拉角φ、θ、ψ。

圖1 載體坐標系和導航坐標系

1.2 姿態(tài)角與四元數的關系

描述導航坐標系和載體坐標系的轉換關系通常有三種方法:歐拉角法[17]、方向余弦法[18]、四元數法[19]。但是歐拉角法會在90 °時出現奇點問題,而余弦矩陣的計算量過大,因此選用計算量小、算法簡單的四元數法描述導航坐標系和載體坐標系的轉換關系。捷聯慣導系統理論[20]中定義四元數q:

q=q0+q1i+q2j+q3k=[q0q1q2q3]T

(1)

(2)

根據z-y-x旋轉順規(guī),可以求解出三個姿態(tài)角:

(3)

1.3 改進EKF姿態(tài)估計算法結構

改進EKF姿態(tài)估計算法結構原理如圖2所示,利用加速度計輸出值fba和GPS的速度微分輸出值fbg對加速度進行補償得到補償后的加速度fa-g,通過運動加速度抑制處理后得到加速度fb,由動態(tài)步長梯度下降算法得出姿態(tài)四元數q▽作為卡爾曼濾波器的量測值。由四元數的微分方程對陀螺儀的輸出角速率ωb更新得到姿態(tài)四元數的估計值qω。通過磁力計單獨估計偏航角,考慮到環(huán)境的復雜性,在使用磁力計之前對其作了橢球校正和傾角補償,即對磁力計輸出m作橢圓校正得到mb,通過傾角補償后求解偏航角,最終更新出姿態(tài)角pitch、roll、yaw。

圖2 改進EKF姿態(tài)估計算法原理

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

2.1 陀螺儀姿態(tài)更新算法

(4)

(7)

2.2 加速度補償算法

(8)

得到由GPS補償后的加速度信息為：

(9)

2.3 動態(tài)步長梯度下降算法

標準EKF算法是為了解決實際系統中的非線形環(huán)節(jié)而提出的,通過對系統方程和量測方程的非線性函數作泰勒級數展開來線性化。這種線性化處理的方式會帶來線性化誤差[21],而梯度下降算法的特點之一是可以進行非線性觀測,消除了標準EKF算法中的線性誤差,提高了姿態(tài)估計的精度。

梯度下降法是利用目標函數對應的負梯度方向來更新每次迭代的新的方向,使得每次迭代能使待優(yōu)化的目標函數逐步減小,通常用來求解函數的最小值。因此,將導航坐標系n中重力加速度g通過四元數法旋轉到載體坐標系b中的值,然后減去當前加速度計的測量值作差,這就是通過加速度計表征的旋轉矩陣的誤差函數。

(10)

(11)

(12)

通過雅可比矩陣得到誤差函數的梯度值。

(13)

迭代式(13)即從初始時刻的姿態(tài)沿負梯度方向更新到誤差函數的極值點,即為所求的姿態(tài)四元數。

梯度步長直接描述了算法的性能:當且僅當算法的收斂速度比運載體實際運行速度快時,解算結果才正確。而固定的梯度步長只能滿足一定速率下的算法精度,所以當物體處于高速轉體運動時,算法步長過小,導致姿態(tài)估計結果錯誤。

因此提出了動態(tài)步長的梯度下降算法,該步長的大小正比于實際物理的運動合角速度模長。當物體慢速運動時,梯度步長較小,滿足算法靜態(tài)精度;而當物體快速運動時,梯度步長增加。步長μ與載體運動的角速度、系統的采樣時間正相關:

μ～α‖ω‖Ts

(14)

其中：‖ω‖為載體的運動角速度模長;Ts為系統的采樣時間;α>1。式(14)表明載體的運動角速度越快,采樣時間越長,那么梯度的步長也就越長。

載體的運動角速度即為陀螺儀的測量角速度。假設陀螺儀的測量角速度為:

ω=[ωxωyωz]

則載體的運動角速度模長為:

(15)

為避免靜態(tài)或者低速運動時ω=0而導致動態(tài)步長為0的結果,這里給動態(tài)步長補一個初始值μ0,該初始值μ0通過靜態(tài)測試實驗得出,因此梯度下降法中的動態(tài)步長為:

(16)

2.4 運動加速度抑制處理算法

在動態(tài)梯度下降算法中,通過加速度計表征的旋轉矩陣誤差為導航坐標系n中的重力加速度gn旋轉到載體坐標系b中的向量gb與加速度計測量值的差。因此載體坐標系中加速度計的測量值只能存在重力加速度,但是飛行器在強機動情況下會產生運動加速度,會降低姿態(tài)解算的精度。所以要把飛行器的重力加速度和運動加速度分離開來處理,如果運動加速度的值過大,式(13)中本次迭代的加速度計更新出來的四元數應該減小。所以運動加速度越大,算法對加速度計測量值的不信任程度越深。

定義δk為加速度計測量值的信任程度因子:

δk=ε/(ε+‖asbk‖)

(17)

其中:‖asbk‖為運動加速度的模長,ε為信任因子的可調參數。式(17)表明運動加速度的值越大,信任因子越低。運動加速度向量為asbk為:

(18)

其中：abk為當前加速度計的測量值;g為重力加速度。

(19)

3 一種改進EKF姿態(tài)估計建模

標準EKF算法是為了解決實際系統中的許多非線性環(huán)節(jié)而出現的算法,通過對系統方程和量測方程的非線性函數作泰勒級數展開并保留其中的線性項,從而獲得線性模型。

建立如下模型:

(20)

其中:Ak為狀態(tài)轉移矩陣；Zk為觀測量；Hk為觀測陣；Wk為系統噪聲矩陣；Vk為測量噪聲矩陣。Wk和Vk滿足:

(21)

其中:Qk為系統噪聲方差矩陣；Rk為測量噪聲矩陣。

3.1 過程模型

選用4D四元數作為狀態(tài)變量,根據式(4)四元數的微分方程得到系統的狀態(tài)方程:

(22)

式(22)用四元數表示即為:

(23)

對式(23)的求解,此處采用離散化處理求解,設系統的采樣周期為Ts,離散化后的狀態(tài)方程為:

(24)

(25)

由于陀螺儀的輸出角速度作為狀態(tài)轉移矩陣的變量,因此對陀螺儀進行如下建模:

ωb=ωreal+gb+gw

(26)

(27)

(28)

其中：ek-1=[q1,k，q2,k，q3,k]T；[ek-1×]為ek-1的反對稱矩陣。由于陀螺儀的測量噪聲gw和采樣時間Ts足夠小,使得式(28)近似成立。所以可以求出系統過程噪聲方差陣:

(29)

3.2 觀測模型

標準EKF算法在建立觀測模型時需要對非線性函數在先驗密度處求偏導來將非線性化方程化為線性化方程；但是這種線性化方式會引入線性誤差,造成姿態(tài)估計的精度降低。因此將可以進行非線性觀測的梯度下降算法引入卡爾曼濾波器,避免了線性化,提高了姿態(tài)估計的精度。

為了滿足梯度下降算法的實時性和快速性,將動態(tài)步長引入梯度下降算法中;為了提高加速度的輸出測量精度,采用兩步優(yōu)化法:1)引入了GPS的速度微分信息來補償加速度計的測量值,從而提高了加速度測量值的精確度;2)對強機動情況下產生的運動加速度進行抑制處理,提高了加速度輸出測量值的準確性。

綜上所述,本文將GPS、加速度計融合的運動加速度抑制的動態(tài)步長梯度下降算法引入卡爾曼濾波器中,使得觀測模型簡化為:

zi=q▽,i-1=xi+vi；i=1,2,3,4

(30)

Hk=I4×4

(31)

由卡爾曼濾波器的5個基本算法方程,本文提出的一種改進EKF姿態(tài)估計算法的原理框圖如圖3所示。

圖3 改進EKF姿態(tài)估計算法原理框圖

3.3 組建改進EKF四旋翼姿態(tài)估計算法

由于磁力計容易受到磁場干擾源干擾,為了避免磁力計對水平姿態(tài)造成錯誤干擾,前面的算法中并沒有將磁力計的數據加入到卡爾曼濾波器中，而是對磁力計進行橢球校正補償后,再進行傾角補償,最終單獨計算偏航角，增強了姿態(tài)的抗磁干擾性能。

校正磁力計的方法：假設空間中地磁場的磁感線是垂直指向地面的,讓飛行器的X、Y、Z軸的正反6個方向依次指向這條垂線的方向,緩慢勻速地轉動飛行器,采集磁力計的輸出數據；根據6次的采集數據在Matlab上擬合出球體,并求出球心坐標,從而校正偏差。

坐標原點即為圖中的“*”所在的位置,從圖4可以看出球心并不在坐標原點,所以進行硬磁校正。由圖4得到的磁力計X、Y、Z三軸偏差硬磁校正數據為-5.546、214.661、-81.502。

假設校正后的磁力計輸出mb=[0，mbx,mby,mbz]T,通過傾角補償后的結果為:

(32)

其中:θ為改進EKF估計出來的姿態(tài)橫滾角roll;φ為改進EKF濾波估計出來的姿態(tài)俯仰角pitch。

最終的姿態(tài)偏航角即可以由傾角補償后的磁力計輸出數據計算得到:

(33)

綜上所述,本文提出的改進EKF四旋翼姿態(tài)估計完整算法組建完畢。

圖4 磁力計球形擬合

4 實驗測試與結果分析

4.1 實驗測試平臺

本文實驗平臺選用以ARM-CortexM3為內核的STM32F405、32位微處理器來搭建的微型四旋翼飛行器。慣性導航傳感器選用美國體感技術公司InvenSense的MPU6050,該模塊為集成三軸加速度計、三軸陀螺儀、三軸磁力計的EMES傳感器。系統的結構示意圖如圖5所示。

圖5 四旋翼飛行器實驗平臺系統結構

圖5中四旋翼的飛行器實驗平臺有遙控器、數傳模塊、GPS模塊、慣性導航傳感器MPU6050、電機驅動模塊、無刷電機、OLED顯示模塊、電源及穩(wěn)壓模塊組成。

四旋翼飛行器的各項參數如表1所示。表1中:電機尺寸2208代表電機定子內徑為22mm,定子厚度為8mm；電機KV值的大小代表轉速和力矩的信息。

表1 四旋翼飛行器的各項參數

表2 靜態(tài)測試下兩種算法的姿態(tài)誤差表

4.2 實驗結果及分析

為了驗證改進EKF姿態(tài)估計算法的性能，進行了以下五組對比實驗:第一組為靜態(tài)實驗,用來測試算法的靜態(tài)性能；第二組實驗為抗磁干擾實驗,用來驗證算法在有磁場干擾時,水平姿態(tài)解算的精度；第三組實驗為水平滑動實驗,用來測試四旋翼在強機動情況下有了運動加速度后,改進EKF中加速度抑制處理算法對姿態(tài)解算的影響；第四組實驗為繞軸旋轉實驗,用來測試飛行器在高速運動情況下動態(tài)步長對姿態(tài)解算的動態(tài)性能影響；第五組實驗為四旋翼不水平時測試偏航角實驗,用來驗證算法在四旋翼不水平時偏航角的精度。

在實驗平臺靜止時,同時用兩種姿態(tài)算法解算出飛行器的姿態(tài)并統計俯仰角和橫滾角的數據進行對比，結果如圖6所示。由圖6中的姿態(tài)波形對比得出兩種算法得到的姿態(tài)角波動范圍均為0°～0.1°,但是改進EKF算法得到的姿態(tài)波動性較小。表2給出了兩種算法姿態(tài)角的均方根誤差、最大值以及最小值,得到靜態(tài)時改進EKF算法的姿態(tài)解算精度高于標準EKF算法。通過靜態(tài)測試實驗可知，由于加速度融合了加速度計和GPS,因此姿態(tài)更加穩(wěn)定。

圖6 靜態(tài)測試下兩種姿態(tài)算法的結果

在四旋翼飛行器靜止時用一根通電導線模擬磁場干擾源在四旋翼實驗平臺飛控板上方從左往右劃過,保持飛行器靜止。同時用兩種算法測得這期間的水平姿態(tài)角如圖7所示。圖7中可以明顯觀察出,在有了磁場干擾后,標準EKF算法受到了較大的影響,而改進EKF算法基本上不受磁場干擾的影響。通過磁場干擾實驗可以得出,改進EKF算法水平姿態(tài)分離磁力計后,明顯增強了而抗干擾能力,提高了水平姿態(tài)的解算精度。

圖7 抗磁干擾實驗結果

四旋翼實驗平臺沿Y軸水平來回快速運動4次,然后沿X軸水平來回快速運動4次,同時用兩種姿態(tài)算法解算出飛行器的姿態(tài)并統計俯仰角和橫滾角的數據進行對比，結果如圖8所示。從圖8中標準EKF算法和改進EKF下測得的roll和pitch的數據對比波形可以得出在飛行器有了運動加速度后,會對標準EKF算法姿態(tài)解算造成較大誤差,而改進的EKF算法姿態(tài)解算幾乎不受運動加速度影響。表3給出水平滑動實驗測試下兩種算法得到的姿態(tài)角均方根誤差、最大值和最小值。由表中數據得到,在水平滑動實驗測試下,改進EKF算法得到的姿態(tài)角誤差明顯小于標準EKF算法得到的姿態(tài)角;并且姿態(tài)的波動范圍小。

圖8 水平滑動測試兩種算法的結果

算法姿態(tài)角均方根誤差/(°)最大值/(°)最小值/(°)標準EKF改進EKFpitch0.42300.7-1.2roll0.42631.1-0.7pitch0.2972-0.1-0.5roll0.20640.4-0.3

通過第二組水平滑動測試實驗得到,水平滑動測試下改進EKF算法優(yōu)于標準EKF算法。說明了對運動加速度抑制處理會在強機動情況下提高姿態(tài)解算的精度,并且增強了姿態(tài)的抗干擾性。

將四旋翼實驗平臺繞X軸旋轉70°，即增大pitch到70°然后減小pitch到0°靜止,重復以上運動3次；再將四旋翼實驗平臺繞Y軸旋轉-60°，即減小roll到-60°然后增大roll到0°靜止,重復以上運動3次。同時用固定步長的梯度下降算法(Gradient Descent Algorithm,GDA)和標準EKF姿態(tài)算法解算出飛行器的姿態(tài)并統計俯仰角和橫滾角的數據進行對比,結果如圖9所示。從GDA和改進EKF算法形成2組對比實驗中得出,在旋轉過程中GDA下姿態(tài)角度跟隨相較改進EKF算法不僅存在明顯滯后,并且精度上也存在較大誤差。在繞軸旋轉測試下,GDA姿態(tài)跟隨的平均滯后時間為106 ms,改進EKF算法為48 ms。這說明改進EKF算法中動態(tài)步長梯度下降算法姿態(tài)跟蹤性能優(yōu)于定步長姿態(tài)下降算法，即動態(tài)步長的梯度下降算法動態(tài)性能和精度比定步長梯度下降姿態(tài)算法好。

圖9 動態(tài)繞軸測試下兩種算法結果

在四旋翼實驗平臺不水平并且保持偏航角為180°時,即快速旋轉將pitch增大至70°后再減小至0°,完成偏航角傾角補償實驗,同時用兩種算法測得這期間的偏航角yaw的數據,結果如圖10所示。從圖10中的兩種算法對比波形可以觀測出,標準EKF算法沒有對磁力計進行傾角補償,所以在四旋翼不水平時,偏航角的數據出現了較大誤差；而改進EKF在四旋翼不水平時對偏航角進行了補償,所以偏航角基本上無偏差。

圖10 兩種算法偏航角輸出結果

5 結語

本文提出了一種改進EKF四旋翼姿態(tài)算法,將運動加速度抑制的動態(tài)步長梯度下降算法融入擴展卡爾曼中,提高了四旋翼姿態(tài)解算的精確度和快速性,并且增強了姿態(tài)的抗干擾性。該算法與標準EKF算法和GDA在四旋翼實驗平臺上,分別完成靜態(tài)實驗、抗磁干擾實驗、水平運動實驗、繞軸運動實驗和偏航角傾角補償實驗。實驗結果表明:改進EKF算法在靜態(tài)時提高了加速度計的輸出精度;在強機動情況下。抑制了運動加速度,提高了姿態(tài)解算的精度;在高速運動情況下的動態(tài)步長增強了算法的動態(tài)性能,實現了對四旋翼飛行器姿態(tài)的實時跟蹤;增強了水平姿態(tài)的抗干擾性,提高了偏航角的準確性。

References)

[1] PRIMICERIO J, GENNARO S F D, FIORILLO E, GENESIO L, et al. A flexible unmanned aerial vehicle for precision agriculture[J]. Precision Agriculture, 2012, 13(4):517-523.

[2] 張靜, 霍建文, 劉星, 等. 微型四旋翼飛行偵察機器人控制系統設計[J]. 測控技術, 2015, 34(7):67-69.(ZHANG J, HUO J W, LIU X, et al. Design of a micro-quadrotor reconnaissance robot control system[J]. Measurement and Control Technology, 2015, 34(7): 67-69.)

[3] 楊克虎, 史英桂, 姬靖, 等. 基于低成本磁場和MEMS慣性傳感器的姿態(tài)測定系統[J]. 中國慣性技術學報, 2008, 16(5):556-562.(YANG K H, SHI Y G, JI J, et al. Attitude determination system with low cost magnetic and MEMS inertial sensors[J]. Journal of Chinese Inertial Technology, 2008, 16(5):556-562.)

[4] DI L, FROMM T, CHEN Y. A data fusion system for attitude estimation of low-cost miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012, 65(1/2/3/4):621-635.

[5] MADGWICK S O H. An efficient orientation filter for inertial and inertial /magnetic sensor arrays [EB/OL]. [2016- 03- 10]. https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf.

[6] MADGWICK S O H, HARRISON A J L, VAIDYANATHAN R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]// Proceedings of the 2011 IEEE International Conference on Rehabilitation Robotics. Piscataway, NJ: IEEE, 2011:1-7.

[7] 張榮輝, 賈宏光, 陳濤, 等. 基于四元數法的捷聯式慣性導航系統的姿態(tài)解算[J]. 光學精密工程, 2008, 16(10):1963-1970.(ZHANG R H, JIA H G, CHEN T, et al. Attitude solution for strapdown inertial navigation system based on quaternion algorithm[J]. Optics and Precision Engineering, 2008, 16(10):1963-1970.)

[8] 王立, 章政, 孫平. 一種自適應互補濾波姿態(tài)估計算法[J]. 控制工程, 2015,22(5): 881-886.(WANG L, ZHANG Z, SUN P. An adaptive complementary filter for attitude estimation[J]. Control Engineering of China, 2015,22(5): 881-886.)

[9] 傅忠云, 劉文波, 孫金秋, 等. 自適應混合濾波算法在微型飛行器姿態(tài)估計中的應用[J]. 傳感技術學報, 2014,27(5):698-703.(FU Z Y, LIU W B, SUN J Q, et al. Application of adaptive hybrid filter algorithm in the estimation of the micro air vehicle attitude[J]. Chinese Journal of Sensors and Actuators, 2014,27(5):698-703.)

[10] 冀亮, 錢正洪, 白茹. 基于四元數的四軸無人機姿態(tài)的估計和控制[J]. 現代電子技術, 2015(11):112-116.(JI L, QIAN Z H, BAI R. Attitude estimation and control of quaternion based quad-axis UAV[J]. Modern Electronics Technique, 2015(11):112-116.)

[11] 彭孝東, 張鐵民, 李繼宇, 等. 基于傳感器校正與融合的農用小型無人機姿態(tài)估計算法[J]. 自動化學報, 2015, 41(4):854-860.(PENG X D, ZHANG T M, LI J Y, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica, 2015, 41(4):854-860.)

[12] SABATINI A M. Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing [J]. IEEE Transactions on Biomedical Engineering, 2006, 53(7):1346-1356.

[13] WANG L, ZHANG Z, SUN P. Quaternion-based Kalman filter for AHRS using an adaptive-step gradient descent algorithm[J/OL]. [2016- 05- 10].International Journal of Advanced Robotic Systems, 2015.http://journals.sagepub.com/doi/pdf/10.5772/61313.

[14] ENAYATI N, DE MOMI E, FERRIGNO G. A quaternion-based unscented Kalman filter for robust optical/inertial motion tracking in computer-assisted surgery[J]. IEEE Transactions on Instrumentation & Measurement, 2015, 64(8): 2291-2301.

[15] YUN X, LIZARRAGA M, BACHMANN E R, et al. An improved quaternion-based Kalman filter for real-time tracking of rigid body orientation[C]// Proceedings of the 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2003:1074-1079.

[16] 曲仕茹, 馬志強. 改進的粒子濾波在四旋翼姿態(tài)估計中的應用[J]. 飛行力學, 2013, 31(5):458-461.(QU S R, MA Z Q. Application of the improved particle filter to quad-rotor aircraft’s attitude estimation[J]. Flight Dynamics, 2013, 31(5):458-461.)

[17] 周紹磊, 叢源材, 李娟, 等. 方向余弦矩陣中四元數提取算法比較[J]. 中國慣性技術學報, 2008, 16(4):415-418.(ZHOU S L, CONG Y C, LI J, et al. Comparison of algorithms for extracting quaternion from DCM[J]. Journal of Chinese Inertial Technology, 2008, 16(4):415-418.)

[18] 蔣鈺, 諶海云, 岑汝平. 基于四元數的四旋翼飛行器姿態(tài)解算算法[J]. 制造業(yè)自動化, 2015, 37(23):77-80.(JIANG Y, CHEN H Y, CEN R P. Attitude solution algorithm for four rotor aircraft based on four element number[J]. Manufacturing Automation, 2015, 37(23):77-80.)

[19] 陳哲. 捷聯慣導系統原理[M]. 北京:中國宇航出版社, 1986.(CHEN Z. Strapdown Inertial Navigation System[M]. Beijing: China Astronautic Publishing House, 1986.)

[20] 秦永元. 慣性導航[M]. 北京:科學出版社, 2014: 288-299.(QIN Y Y. Inertial Navigation[M]. Beijing: Science Press, 2014: 288-299.)

[21] 王立. 基于北斗/GPS導航的視覺著陸四旋翼飛行器設計[D]. 武漢:武漢科技大學, 2016.(WANG L. Design of a Beidou/GPS quadrotor with vision-based landing[D]. Wuhan: Wuhan University of Science and Technology, 2016.)

WANG Long, born in 1991, M. S. candidate. His research interests include quadrotor’s attitude estimation, navigation control.

ZHANG Zheng, born in 1974, Ph. D., professor. His research interests include artificial intelligence, mobile robot.

WANG Li, born in 1990, M. S. His research interests include quadrotor’s attitude estimation, navigation control.

Improved extended Kalman filter for attitude estimation of quadrotor

WANG Long*, ZHANG Zheng, WANG Li

(School of Information Science and Engineering, Wuhan University of Science and Technology, Wuhan Hubei 430081, China)

In order to improve the rapidity and tracking accuracy of Extended Kalman Filter (EKF), an improved EKF for attitude estimation of quadrotor was proposed by introducing a dynamic step gradient descent algorithm with acceleration restraint. Gradient descent algorithm was used to carry out nonlinear observation in the Kalman measurement update, eliminate the linearity error caused by the linearization of the standard extended Kalman algorithm and improve the accuracy and rapidity of the algorithm. The gradient step of gradient descent algorithm was dynamically processed to be proportional to the angular velocity of the quadrotor, thus enhancing the dynamic performance of the quadrotor. The motion acceleration generated during strong maneuverability was restrained to remove the adverse effect to attitude calculation and improve tracking accuracy of quadrotor’s attitude estimation. To verify the feasibility and effectiveness of proposed algorithm, a quadrotor experimental platform was set up based on STM32 microcontroller. The experimental results show that the proposed algorithm has higher estimation accuracy, better dynamic performance and anti-interference characteristics under strong maneuverability and high-speed motion, and can ensure the stable flight of the quadrotor.

quadrotor; Extended Kalman Filter (EKF); motion acceleration restraint; dynamic step; gradient descent algorithm

2016- 08- 15;

2016- 12- 26。

王龍(1991—),男,湖北十堰人,碩士研究生,主要研究方向:四旋翼姿態(tài)解算、導航控制; 章政(1974—),男,湖北武漢人,教授,博士,主要研究方向:人工智能、移動機器人; 王立(1990—),男,湖北宜昌人,碩士,主要研究方向:四旋翼姿態(tài)解算、導航控制。

1001- 9081(2017)04- 1122- 07

10.11772/j.issn.1001- 9081.2017.04.1122

TP273.2

A