一種用于仿生導(dǎo)航無人機航姿求解的混合濾波方法

金仁成，謝林達，魏 巍，蔚彥昭

(大連理工大學(xué)機械工程學(xué)院，大連 116024)

0 引言

近年來小型無人飛行器(Unmanned Aerial Vehicle, UAV)發(fā)展迅速，尤其在軍事偵察、農(nóng)業(yè)監(jiān)測和救災(zāi)巡檢等領(lǐng)域應(yīng)用廣泛[1-3]。事實上小型無人飛行器成本有限，在一定程度上限制了激光陀螺和光纖陀螺等高精度姿態(tài)測量裝置的應(yīng)用，于是體積小、質(zhì)量小、集成度高、功耗低的微機電系統(tǒng)(Micro-Electro-Mechanical Systems, MEMS)成為執(zhí)行導(dǎo)航任務(wù)的最佳選擇[4-6]。由MEMS傳感器組成的微型慣性測量單元(Micro Inertial Measurement Unit, MIMU)是MEMS的重要分支，通常包括角速度陀螺儀和加速度計。雖然慣性器件和導(dǎo)航技術(shù)得到了飛速發(fā)展,但是單純依靠提高慣性儀表精度來改善導(dǎo)航系統(tǒng)性能不僅需要較高的技術(shù)水平，同時研發(fā)成本也相應(yīng)提升。因此，現(xiàn)有方案大多以慣性導(dǎo)航為主體，結(jié)合其他導(dǎo)航設(shè)備形成組合導(dǎo)航系統(tǒng)，并利用無誤差積累效應(yīng)的外部參考信息源對慣性器件誤差項實現(xiàn)補償修正。現(xiàn)階段普遍采取的組合導(dǎo)航方案是慣性/衛(wèi)星組合導(dǎo)航技術(shù)[7-8]，但是衛(wèi)星信號易被干擾甚至欺騙，導(dǎo)致無法執(zhí)行導(dǎo)航任務(wù)以及存在無人飛行器被誘捕的危險。同時慣性/地磁組合導(dǎo)航技術(shù)也是常用方法[9-10]，該方式通過感知地球磁場強度分布提供導(dǎo)航所需信息。然而由于地表附近存在周期變化的磁場異常現(xiàn)象，影響了地磁場數(shù)據(jù)庫構(gòu)建精度，導(dǎo)致地磁匹配效果不佳。而且通過感應(yīng)地球磁場方向提供航向信息的電子羅盤，極易受到周圍電磁環(huán)境影響，無法給出準(zhǔn)確測量結(jié)果。

針對上述問題，不僅需要準(zhǔn)確性高、性能優(yōu)良的姿態(tài)融合算法，同時設(shè)計一種自主性好、誤差不累積、抗電磁干擾能力強的導(dǎo)航系統(tǒng)更是起到關(guān)鍵性作用。近年來仿生學(xué)逐漸成為熱點研究內(nèi)容，自然界中蟋蟀、蜻蜓、蝴蝶等生物巧妙利用天空中大氣偏振模式實現(xiàn)精確導(dǎo)航的現(xiàn)象被人們發(fā)現(xiàn)[11-13]。國外學(xué)者Rayleigh等通過對太陽光偏振現(xiàn)象的分析論證，建立了著名的Rayleigh散射模型。隨后Lambrinos等仿照沙蟻復(fù)眼結(jié)構(gòu)，將偏振光敏感策略應(yīng)用于機器人平臺以提取偏振方向信息，實驗結(jié)果表明輸出的角度值不存在累積誤差[14]。通過研究偏振光導(dǎo)航機理，國內(nèi)的大連理工大學(xué)褚金奎教授團隊率先研制出新型偏振光傳感器，并利用地面移動機器人成功完成導(dǎo)航尋跡任務(wù)，驗證了偏振光信息用于導(dǎo)航定向的可行性[15]。針對偏振光組合導(dǎo)航算法，哈爾濱工業(yè)大學(xué)通過仿真實驗證明了偏振光信息能夠修正慣性導(dǎo)航器件的姿態(tài)誤差[16]。同時國防科技大學(xué)基于標(biāo)準(zhǔn)大氣偏振模型推導(dǎo)了三維空間中航向角的計算方法，并仿真分析了各項誤差源對航向角估計誤差的影響[17]。

目前，國內(nèi)外偏振光導(dǎo)航方面的研究主要集中在地面導(dǎo)航領(lǐng)域，無人機導(dǎo)航方面研究較少。現(xiàn)階段偏振光導(dǎo)航策略主要是基于卡爾曼濾波的偏振光輔助定姿方法。然而利用卡爾曼濾波模型實現(xiàn)數(shù)據(jù)融合的每次迭代過程中其計算量相對較大，實時性難以保證，同時偏振光傳感器的模型誤差受多種因素影響不易確定。因此，通常利用卡爾曼濾波進行仿真分析[18-19]，同時由于仿真條件比較理想化，在實際導(dǎo)航環(huán)境中的應(yīng)用效果不佳。

本文針對現(xiàn)有技術(shù)不足，設(shè)計了一種用于仿生導(dǎo)航無人機航姿求解的混合濾波方法。通過Mahony濾波對存在低頻噪聲的陀螺儀與易受高頻噪聲干擾的偏振光傳感器及加速度計實現(xiàn)初步數(shù)據(jù)融合，再將獲得的姿態(tài)信息作為系統(tǒng)觀測量，并結(jié)合擴展卡爾曼濾波(Extended Kalman Filter，EKF)算法構(gòu)建混合濾波模型，以實現(xiàn)傳感器數(shù)據(jù)的深度融合，最終輸出精度更高的航姿求解結(jié)果。

1 偏振光分布模式及傳感器測量模型

1.1 偏振光分布模式

在晴朗天氣條件下，太陽光穿過大氣層遇到大氣中的空氣分子和氣溶膠粒子時，其光波振動方向會發(fā)生改變，從而在整個天空范圍內(nèi)形成相對穩(wěn)定的大氣偏振模式[20]。根據(jù)Rayleigh散射理論模型，該種模式與太陽空間位置和觀測者所在位置密切相關(guān)，并在日地相對轉(zhuǎn)動過程中保持穩(wěn)定分布。

近年來國內(nèi)外學(xué)者研究分析了生物的偏振視覺結(jié)構(gòu)，建立了理想大氣環(huán)境下的天空光偏振分布模型，這里采用天球模型[21]進行相應(yīng)描述，天空光偏振分布模式如圖1所示。圖1中，O點代表偏振光傳感器所在位置，S點代表太陽在天球模型上投影位置，Z點表示天頂位置(偏振光傳感器所在位置正上方與天球交點)，SM為太陽子午線(通過S點和Z點的弧線)，ASM為逆太陽子午線。模型中的短線表示偏振矢量，短線的粗細表示偏振矢量強度，短線的切線方向表示偏振矢量方向。

圖1 天空光偏振分布模式Fig.1 Polarization distribution mode of skylight

1.2 偏振光傳感器測量模型

本文偏振光傳感器輸出的偏振方位角φ是偏振矢量方向與傳感器參考軸方向之間的夾角。這里定義偏振光模塊坐標(biāo)系為m系，并根據(jù)偏振光傳感器量測輸出值獲得偏振矢量在m系中的投影為

(1)

圖1中，Xm軸表示偏振光傳感器參考軸方向，同時該方向也是傳感器內(nèi)偏振片安裝基準(zhǔn)，Zm軸表示偏振光傳感器觀測方向，這里O-XmYmZm構(gòu)成笛卡爾右手坐標(biāo)系。O點與S點連線為太陽矢量方向，這里用?s與δs分別表示太陽高度角和太陽方位角，則有太陽矢量在導(dǎo)航坐標(biāo)系下的投影為

(2)

根據(jù)Rayleigh散射模型原理，偏振矢量方向總是垂直于太陽入射方向與觀測方向所確定的平面，由此計算偏振矢量在m系中的解析式為

(3)

2 仿生導(dǎo)航混合濾波方法設(shè)計

雖然單獨利用陀螺儀解算姿態(tài)角短時間內(nèi)精度較高，但長期工作受漂移誤差影響嚴重，同時考慮到傳統(tǒng)卡爾曼濾波數(shù)據(jù)融合算法的局限性，本文設(shè)計了基于偏振光的混合濾波模型，并采用級聯(lián)式框架結(jié)構(gòu)。前一階段利用偏振光傳感器和加速度計低頻特性良好的優(yōu)勢，與不易受高頻噪聲干擾的陀螺儀組建Mahony濾波模型，再結(jié)合EKF算法將獲得信息作為后一階段的系統(tǒng)觀測量，構(gòu)成偏振光混合濾波模型(Polarization Hybrid Filter, PHF) ，實現(xiàn)偏振光與慣性量測信息的深層融合，以獲得高精度姿態(tài)解算結(jié)果。根據(jù)上述分析，本文設(shè)計的仿生導(dǎo)航混合濾波姿態(tài)估計方法原理圖如圖2所示。

圖2 混合濾波姿態(tài)估計方法原理圖Fig.2 Schematic diagram of hybrid filter attitude estimation method

2.1 基于Mahony濾波的航姿求解方法

澳大利亞學(xué)者Robert Mahony提出的濾波算法是一種基于傳感器頻率特性的數(shù)據(jù)融合方法[22]。本文利用低頻信號穩(wěn)定的偏振光傳感器與加速度計校正陀螺儀漂移誤差，獲得初步的姿態(tài)估計值。

本文的Mahony濾波模型將偏振光傳感器和加速度計量測值作為前饋輸入量，通過PI(比例-積分)反饋控制器實現(xiàn)對陀螺儀積分誤差的補償修正。基于大氣偏振模式的Mahony濾波算法流程圖如圖3所示。

圖3 Mahony濾波算法流程圖Fig.3 Schematic diagram of the Mahony filter for attitude determination

根據(jù)圖3可知，經(jīng)過Mahony濾波后的姿態(tài)角估計值可表示為

(4)

(5)

(6)

(7)

(8)

將總偏差矢量e輸入PI反饋控制器得到誤差補償項，再對陀螺儀量測角速度進行校正，獲得修正后的角速度值如式(9)所示。

(9)

(10)

式中，γ、θ、ψ分別代表航姿參考系統(tǒng)(Attitude and Heading Reference System, AHRS)中橫滾角、俯仰角與航向角。

2.2 融合EKF的混合濾波姿態(tài)解算方法

小型無人機姿態(tài)變化是非線性的，因此采用EKF對其狀態(tài)進行線性最小方差估計，通過預(yù)測及狀態(tài)更新過程獲得機體姿態(tài)最優(yōu)估計值。

(1)狀態(tài)量與狀態(tài)方程的選取及建立

本文選取3個姿態(tài)值作為非線性系統(tǒng)狀態(tài)量，并建立系統(tǒng)狀態(tài)方程。設(shè)系統(tǒng)狀態(tài)量為

(11)

式中，γ(k)、θ(k)、ψ(k)為k時刻橫滾角、俯仰角與航向角的姿態(tài)值，并利用狀態(tài)量建立如下系統(tǒng)狀態(tài)方程

X(k)=f(X(k-1),k-1)+G(k-1)W(k-1)

(12)

式中，G(k-1)為系統(tǒng)噪聲驅(qū)動矩陣，W(k-1)為系統(tǒng)過程噪聲矩陣。對f(X(k-1),k-1)求雅可比矩陣可得系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣為

(13)

(2)觀測量與觀測方程的選取及建立

考慮到觀測量的誤差精度直接影響卡爾曼濾波整體效果，因此觀測量的選取成為卡爾曼濾波器設(shè)計的關(guān)鍵。本文考慮到偏振光傳感器和加速度計易受機體振動等高頻噪聲干擾，而經(jīng)過Mahony濾波后的傳感器融合數(shù)據(jù)精度相對較高，因此本文將前一階段降噪后的姿態(tài)估計值作為非線性系統(tǒng)觀測量，設(shè)系統(tǒng)觀測量為

(14)

式中，γap(k)、θap(k)、ψap(k)為k時刻Mahony濾波求解的橫滾角、俯仰角與航向角數(shù)值，并利用觀測量建立如下系統(tǒng)觀測方程

Z(k)=h(X(k),k)+V(k)

(15)

(3)卡爾曼濾波算法更新過程

1)基于狀態(tài)轉(zhuǎn)移矩陣的狀態(tài)預(yù)測方程為

(16)

2)狀態(tài)預(yù)測協(xié)方差矩陣為

P(k,k-1)=Φ(k,k-1)P(k-1)·

ΦT(k,k-1)+Q(k)

(17)

式中，P(k,k-1)為狀態(tài)協(xié)方差矩陣的估計，Q(k)為系統(tǒng)噪聲協(xié)方差矩陣。

3)卡爾曼濾波增益更新方程為

K(k)=P(k,k-1)HT(k)[H(k)P(k,k-1)·

HT(k)+R(k)]-1

(18)

式中，K(k)為濾波增益矩陣，H(k)為系統(tǒng)量測矩陣，R(k)為觀測噪聲協(xié)方差矩陣。

4)系統(tǒng)狀態(tài)更新方程為

(19)

5)狀態(tài)協(xié)方差矩陣更新方程為

P(k)=[I-K(k)H(k)]P(k,k-1)

(20)

將Mahony濾波降噪后的航姿數(shù)據(jù)作為后一階段的系統(tǒng)觀測量，采用與EKF模型相結(jié)合的級聯(lián)式濾波算法，實現(xiàn)對偏振光傳感器與IMU輸出信息的深層融合，從而獲得高精度的姿態(tài)估計結(jié)果。

3 算法驗證與測試分析

基于偏振光的AHRS由IMU與偏振光傳感器組成。這里采用開源Lisa/M飛行控制系統(tǒng)輸出的姿態(tài)信息作為算法驗證的參考值，其中主要包含IMU MPU-6050與電子羅盤 HMC5883L等電子器件。為了評估本文提出的混合濾波方法的性能，在靜態(tài)與動態(tài)不同實驗環(huán)境下進行測試，并與基于電子羅盤的EKF航姿參考值進行比較。本文搭建的偏振光導(dǎo)航實驗平臺如圖4所示。

圖4 偏振光導(dǎo)航實驗平臺Fig.4 Experiment platform of polarized-light navigation

3.1 靜態(tài)環(huán)境驗證分析

由于偏振光分布模式與開展實驗的時間及地點密切相關(guān)，因此這里給出詳細信息。本次靜態(tài)實驗從2018年6月20日17點50分開始，持續(xù)約250s，且當(dāng)時當(dāng)?shù)氐奶柛叨冉菫?6.31°，太陽方位角為-107.24°。將本文提出的航姿求解混合濾波PHF方法與飛控系統(tǒng)標(biāo)準(zhǔn)EKF方法以及單獨采用Mahony濾波融合偏振光數(shù)據(jù)的最小噪聲分離(Minimum Noise Fraction，MNF)方法進行比較分析，姿態(tài)角對比曲線如圖5所示。同時為表明不同方法之間的差異，靜態(tài)環(huán)境姿態(tài)角均方差結(jié)果如表1所示。

(a) Roll-γ

(b) Pitch-θ

(c) Yaw-ψ圖5 靜態(tài)環(huán)境姿態(tài)角測試結(jié)果Fig.5 Static test results of attitude determination

航姿求解方法橫滾角/(°)俯仰角/(°)航向角/(°)EKF0.03230.04080.0856MNF0.02290.02260.0180PHF0.01510.01450.0154

圖5中，基于偏振光的Mahony濾波方法與本文設(shè)計的混合濾波方法均能有效抑制陀螺儀漂移誤差，但是MNF方法不能有效消除偏振光傳感器內(nèi)部噪聲，導(dǎo)致姿態(tài)角曲線抖動明顯。相比之下,本文提出的PHF方法能有效抑制內(nèi)部噪聲干擾。表1中，基于偏振光的兩種方法較基于電子羅盤的EKF算法的姿態(tài)角均方差更小，表明新型慣性/偏振光組合導(dǎo)航系統(tǒng)的抗隨機噪聲干擾能力更強。同時相較于EKF算法，本文提出的PHF方法表現(xiàn)出更高的靜態(tài)精度，其中橫滾角提高53.3%，俯仰角提高64.5%，航向角提高82.0%，可見PHF方法的靜態(tài)穩(wěn)定性更高，有利于無人機導(dǎo)航系統(tǒng)的自主調(diào)整與控制。

3.2 動態(tài)環(huán)境驗證分析

本次實驗主要考慮傾斜運動環(huán)境下算法的適應(yīng)性。實驗地點不變，實驗時間為2018年6月20日18點35分，其中太陽高度角為8.10°，太陽方位角為-113.65°。將本文設(shè)計的PHF方法與飛控系統(tǒng)參考EKF方法和MNF方法進行比較分析，姿態(tài)角對比曲線如圖6所示。

(a) Roll-γ

(b) Pitch-θ

為表明PHF方法與MNF方法求解姿態(tài)信息的準(zhǔn)確性，兩種方法的姿態(tài)值估計誤差如圖7所示，其估計誤差范圍如表2所示。

(a) Roll-γ

(b) Pitch-θ

(c) Yaw-ψ圖7 動態(tài)環(huán)境姿態(tài)值估計誤差Fig.7 Attitude estimation error in the dynamic test

姿態(tài)角MNF-Error(Max)MNF-Error(Min)PHF-Error(Max)PHF-Error(Min)橫滾角/(°)3.69-3.952.86-3.67俯仰角/(°)5.13-4.903.89-3.83航向角/(°)5.02-7.462.43-2.74

圖6中，利用MNF方法求解航姿時存在超調(diào)現(xiàn)象，主要是由于該方法響應(yīng)速度較慢，導(dǎo)致對姿態(tài)變化的敏感性較差。相比之下，本文提出的PHF方法的姿態(tài)跟蹤效果更好、響應(yīng)更及時、可靠性更高。同時，從圖7的誤差曲線可以看出，采用PHF方法所求解的姿態(tài)角誤差值較MNF方法更小。根據(jù)表2數(shù)據(jù)可知，在動態(tài)環(huán)境中利用PHF方法求解的航姿精度更高，其姿態(tài)值估計誤差控制在±4°范圍內(nèi)，與MNF方法±8°的誤差范圍相比，混合濾波方法顯著提高了偏振光組合導(dǎo)航系統(tǒng)姿態(tài)估計的動態(tài)精度，保證了傾斜運動條件下姿態(tài)解算的準(zhǔn)確性。

4 結(jié)論

考慮到傳統(tǒng)組合導(dǎo)航系統(tǒng)的局限性，本文引入了無誤差累積效應(yīng)、自主性好、抗電磁干擾能力強的偏振光傳感器與慣導(dǎo)構(gòu)成新型仿生組合導(dǎo)航系統(tǒng)。同時針對現(xiàn)有偏振光導(dǎo)航姿態(tài)求解方法精度不高的問題，設(shè)計了一種用于仿生導(dǎo)航無人機航姿求解的混合濾波方法，并在不同環(huán)境中進行了測試分析。

經(jīng)過與Mahony濾波方法以及融合電子羅盤數(shù)據(jù)的EKF算法姿態(tài)解算結(jié)果比較可知，本文的混合濾波方法能夠更大限度地消除偏振光傳感器內(nèi)部噪聲干擾，靜態(tài)定姿精度普遍提高了50%以上，且航向角定姿精度提升尤為顯著，達到了82%。同時在動態(tài)情況下，混合濾波方法克服了Mahony濾波的姿態(tài)估計超調(diào)問題，表現(xiàn)出更好的跟蹤效果和更快的響應(yīng)速度，提高了偏振光組合導(dǎo)航系統(tǒng)的動態(tài)定姿精度，保證了無人機系統(tǒng)控制輸出的準(zhǔn)確性和可靠性。但本文實驗大多選擇晴朗無云的天氣條件開展，以避免多次散射的不利影響，下一步工作將研究不同天氣條件下天空光的偏振模型，以提高偏振光組合導(dǎo)航系統(tǒng)對復(fù)雜天氣環(huán)境的適應(yīng)能力。