李世強, 易文俊

(南京理工大學(xué) 瞬態(tài)物理國家重點實驗室,江蘇 南京 210094)

近年來,無人機迎來了大發(fā)展,被廣泛用于軍事偵察、安防巡檢、災(zāi)后搜救、農(nóng)業(yè)生產(chǎn)以及攝影航拍等不同領(lǐng)域,其中尤以四旋翼飛行器(以下簡稱“四旋翼”)最為流行。許多研究人員也對四旋翼產(chǎn)生了濃厚的興趣,搭建了多種飛行平臺,實驗了不同的控制算法。飛行器要實現(xiàn)穩(wěn)定可靠飛行,必須準(zhǔn)確高效地獲取自身的各種狀態(tài)信息。而四旋翼本質(zhì)上是一個欠驅(qū)動系統(tǒng),只能產(chǎn)生垂直于機體平面的升力,姿態(tài)估計信息被用于最底層控制環(huán)路以穩(wěn)定飛行器,因此快速準(zhǔn)確的姿態(tài)估計算法尤為重要,飛行控制算法的穩(wěn)定性和可靠性直接依賴于姿態(tài)解算的精度和速度[1]。

由于受到成本、載荷和尺寸的限制,四旋翼普遍依賴慣性測量單元 (Inertial Measurement Unit,IMU)估算姿態(tài)。IMU是一組傳感器,由測量加速度的加速度計和測量角速度的陀螺儀組成,通常還包括一個磁力計來測量地球的磁場。這3個傳感器中的每一個都產(chǎn)生一個3軸測量值,總共構(gòu)成一個9軸測量值。但I(xiàn)MU不提供直接的姿態(tài)估計,為了提高性能和魯棒性,必須對原始信號進(jìn)行非線性濾波和數(shù)據(jù)融合處理,方能重建平滑的姿態(tài)估計和校正角速度測量的偏差。此外,從不同的傳感器測量結(jié)果中同時估計姿態(tài)和陀螺儀偏差大為有益[2]。

剛體姿態(tài)估計問題極具挑戰(zhàn)性,幾十年來一直是導(dǎo)航研究的焦點。這是因為剛體旋轉(zhuǎn)的基本位形空間,即特殊正交群SO(3)不是向量空間而是彎曲空間,所以姿態(tài)估計本質(zhì)上是非線性問題[3]。簡單直接地用局部坐標(biāo)(例如歐拉角、Rodrigues參數(shù))來表示旋轉(zhuǎn)存在諸多問題。局部坐標(biāo)包含需要進(jìn)行特殊處理的奇異點(例如當(dāng)俯仰角為90°時),并且估計既取決于局部坐標(biāo)的選擇,也取決于固定和移動參考坐標(biāo)系。如果將標(biāo)準(zhǔn)向量空間中的濾波方法不加改造地用到姿態(tài)的局部坐標(biāo)表示上,則不僅狀態(tài)空間動力學(xué)方程和測量方程高度非線性,依賴于參考坐標(biāo)的選擇,而且濾波性能在位形空間的不同區(qū)域高度不均勻[4]。

許多濾波方法都致力于處理非最小姿態(tài)表示所固有的約束。乘性擴展卡爾曼濾波器(Multiplicative Extended Kalman Filter,MEKF)因其靈活性和計算效率高而得到廣泛應(yīng)用。它利用旋轉(zhuǎn)矩陣(或四元數(shù))的幾何結(jié)構(gòu),對在原點處線性化的本征狀態(tài)誤差建立了誤差狀態(tài)卡爾曼濾波器。然而,當(dāng)動態(tài)、測量模型高度非線性或沒有良好的先驗狀態(tài)估計時,MEKF容易出現(xiàn)發(fā)散現(xiàn)象[5]。

不變觀測器理論基于估計誤差在矩陣?yán)钊旱淖饔孟虏蛔僛6],該性質(zhì)被稱為系統(tǒng)的對稱性[7],推動了不變擴展卡爾曼濾波(Invariant Entended Kalman Filter,InEKF)的發(fā)展[8-9],在即時定位與地圖構(gòu)建(Simultaneous Localization and Mapping,SLAM)[10]和輔助慣性導(dǎo)航系統(tǒng)[11]中得到成功應(yīng)用并取得良好效果。由InEKF算法可知,如果狀態(tài)定義在一個李群上,并且動力學(xué)方程滿足“群仿射”特性,那么對稱性使得估計誤差滿足李代數(shù)上的“對數(shù)線性”自治微分方程[8]。在確定性情況下,該線性系統(tǒng)可以用來精確地復(fù)原在群上演化的非線性系統(tǒng)的狀態(tài)估計。因此,對數(shù)線性特性允許設(shè)計一個非線性觀測器或具有強收斂特性的狀態(tài)估計器[12]。

在本文中,基于InEKF理論解決李群SO(3)上的姿態(tài)估計問題,建立連續(xù)時間運動模型和離散時間觀測模型。定義的確定性系統(tǒng)滿足“群仿射”特性,因此,誤差動態(tài)是對數(shù)線性的。在實踐中,隨著傳感器噪聲和IMU偏差的引入,該對數(shù)線性誤差系統(tǒng)只是近似成立,在許多情況下,由于優(yōu)越的收斂性和一致性,InEKF仍然表現(xiàn)良好?？梢酝ㄟ^MATLAB仿真實驗來證明所開發(fā)濾波方法的效用和準(zhǔn)確性。

1 理論背景及預(yù)備知識

首先回顧一些有關(guān)特殊正交群SO(3)及其李代數(shù)so(3)的基本知識和有用的數(shù)學(xué)公式,這將在后面的章節(jié)中用于推導(dǎo)InEKF算法。

SO(3)中的元素由滿足RTR=I,det(R)=1的3×3實矩陣R組成,這里的I表示3×3單位矩陣。SO(3)是一個矩陣?yán)钊?與其相關(guān)聯(lián)的李代數(shù)記為so(3),是3×3斜對稱矩陣。為方便計算,令(·)∧表示R3到so(3)的線性映射。

(1)

so(3)和SO(3)之間由指數(shù)映射exp建立聯(lián)系:

(2)

式中:‖·‖為標(biāo)準(zhǔn)向量范數(shù)。為方便,記Exp(w)=exp(w∧)。

對于任意R∈SO(3),w∧∈so(3),有伴隨映射Rw∧RT=(Rw)∧。另外,定義J為SO(3)的左雅可比,有

(3)

在SO(3)中運動的連續(xù)軌跡R(t)有

(4)

(5)

對fw(R)有

fw(R1R2)=R1R2w∧

=fw(R1)R2+R1fw(R2)-R1fw(I)R2

(6)

式(6)即為“群仿射”特性[8]。由于“群仿射”特性,右和左不變誤差是軌跡獨立的,即滿足

=0

(7)

=ηlw∧-w∧ηl

(8)

(9)

2 建立模型

在描述InEKF算法之前,首先需要建立坐標(biāo)系,定義符號系統(tǒng),建立四旋翼無人機姿態(tài)運動模型,描述傳感器測量模型及其基本假設(shè)。

2.1 參考坐標(biāo)系

為了描述四旋翼無人機的姿態(tài),以及討論加速度計和陀螺儀的測量值,必須引入一些參考坐標(biāo)系。通常定義兩個坐標(biāo)系表達(dá)姿態(tài)角的關(guān)系,一般使用導(dǎo)航坐標(biāo)系(N系)和機體坐標(biāo)系(B系)。導(dǎo)航坐標(biāo)系,即當(dāng)?shù)氐牡乩碜鴺?biāo)系,相對大地水平面定義,坐標(biāo)軸Xn,Yn,Zn分別指向地理上的北、東和當(dāng)?shù)卮咕€方向(向下),即北東地(NED)坐標(biāo)系。機體坐標(biāo)系則使用標(biāo)準(zhǔn)右前上坐標(biāo)系,坐標(biāo)軸Xb,Yb,Zb分別指向無人機的右側(cè)、前方以及上方。機體坐標(biāo)系到導(dǎo)航坐標(biāo)系的坐標(biāo)變換矩陣用R表示,即無人機的姿態(tài)。

2.2 運動模型和測量模型

四旋翼無人機由旋轉(zhuǎn)矩陣表示的連續(xù)時間姿態(tài)運動方程為

(10)

式中:Ω∈R3為無人機在機體系內(nèi)的瞬時角速度,由陀螺儀測量。然而測量結(jié)果被加性高斯白噪聲過程干擾,即

(11)

(12)

(13)

(14)

式中:Vk∈R6是由wa和wm組成的高斯噪聲。則有

(15)

3 不變擴展卡爾曼濾波

3.1 預(yù)測

考慮完整的系統(tǒng)運動方程和測量方程:

(16)

噪聲wΩ的引入,使得右不變誤差η變?yōu)?/p>

(17)

(18)

(19)

(20)

3.2 測量更新

為了將新的測量值納入考量,記zk∈R6為更新量,取為

(21)

狀態(tài)校正方程為

(22)

η+=Exp(Kkzk)η

(23)

式中:Kk為時刻tk的卡爾曼增益矩陣,同樣用η=Exp(ξ)≈I+ξ∧進(jìn)行一階近似將式(23)線性化,忽略高階項,有

η+≈I+ξ+∧≈I+ξ∧+(Kkzk)∧

(24)

因此

ξ+=ξ+Kk((I-ξ∧)r-r+Vk)

=ξ+Kk(-ξ∧r+Vk)

=ξ+Kk(Hξ+Vk)

(25)

最后,可以利用導(dǎo)出的線性更新方程和卡爾曼濾波理論將InEKF的狀態(tài)和協(xié)方差更新方程寫成

(26)

式中:卡爾曼增益矩陣Kk由式(27)計算

(27)

由式(25)可得靈敏度矩陣H和測量噪聲協(xié)方差N為

(28)

3.3 引入陀螺儀偏差

在硬件上實現(xiàn)基于IMU的狀態(tài)估計算法通常需要對額外的狀態(tài)加以考慮,如陀螺儀和加速度計的偏差?？梢允褂脙煞N不同的方式來進(jìn)行處理。一種是將偏差作為一個恒定的參數(shù),假設(shè)它通常在比實驗時間更長的時間段內(nèi)變化,然后可以在一個單獨的實驗中對偏差進(jìn)行預(yù)校準(zhǔn)。另一種是視偏差為所估計的狀態(tài)的一部分,則可以在估計姿態(tài)時同步估計偏差。本文采用后一種方式,因為在線實時補償偏差的影響能提高姿態(tài)估計的精度。但是,一旦將偏差納入考慮,則沒有李群可以滿足動力學(xué)方程的群仿射特性。盡管InEKF的許多理論性質(zhì)不再成立,但仍能設(shè)計一個“增廣InEKF”,其性能仍然優(yōu)于標(biāo)準(zhǔn)EKF[14]。由于兩種偏差的處理過程相似,為節(jié)省篇幅,本文只考慮陀螺儀偏差。

陀螺儀的偏差b是緩慢時變信號,以加性的方式破壞測量結(jié)果

(29)

模型的狀態(tài)現(xiàn)在變?yōu)樽鴺?biāo)變換矩陣R和偏差b,χ=(R,b)∈SO(3)×R3。現(xiàn)在定義增廣的右不變量誤差為

(30)

式中:ζ為偏差誤差。

(31)

偏差b動態(tài)使用典型的“布朗運動”模型建模,即導(dǎo)數(shù)是高斯白噪聲,以捕捉參數(shù)緩慢時變性質(zhì):

(32)

為了計算線性化的誤差動態(tài),首先對增強的右不變誤差進(jìn)行微分。在執(zhí)行鏈?zhǔn)揭?guī)則并進(jìn)行一階近似后得

(33)

誤差動態(tài)只通過噪聲wΩ和偏差誤差ζ依賴于估計軌跡。當(dāng)沒有噪聲wΩ和偏差誤差ζ時,誤差動態(tài)對估計軌跡沒有依賴。現(xiàn)在可以從中構(gòu)建一個線性系統(tǒng),得到

(34)

測量方程不依賴于IMU的偏差。因此,對增強的右不變誤差,H矩陣可以簡單地附加零以滿足維度要求。線性更新方程成為

(35)

最終,包括陀螺儀偏差b的“增廣InEKF”方程,可以完整寫出。估計狀態(tài)使用以下一組微分方程進(jìn)行預(yù)測傳播。

(36)

增廣的右不變誤差動態(tài)的協(xié)方差通過解Riccati方程計算

(37)

式中:矩陣A,B由式(34)給出。估計狀態(tài)更新方程為

(38)

(39)

3.4 離散化

在前面的章節(jié)中,濾波狀態(tài)傳播方程是連續(xù)時間的。然而,為了實現(xiàn)濾波過程,這些方程需要離散化。為此,對慣性測量量(即輸入)做零階保持,并進(jìn)行積分。偏差動態(tài)是簡單的高斯噪聲,因此,離散形式確定性動態(tài)方程為

(40)

(41)

式中:Δt=tk+1-tk。為了更新協(xié)方差,需要解一個連續(xù)時間的Riccati方程(37),它的解析解為[13]

Pk+1=Φ(tk+1,tk)PkΦ(tk+1,tk)T+Qd

(42)

其中,離散噪聲協(xié)方差矩陣為

(43)

狀態(tài)轉(zhuǎn)移矩陣Φ(tk+1,tk)滿足:

(44)

求解式(44)得到[12]:

(45)

與狀態(tài)轉(zhuǎn)移矩陣類似,離散噪聲協(xié)方差矩陣也有一個解析解。不過實踐中,這個矩陣通常被近似為Qd≈ΦQΦTΔt。

4 數(shù)值模擬

使用 MATLAB 對前述不變擴展卡爾曼濾波算法進(jìn)行仿真實驗,證明了所提出的不變擴展卡爾曼濾波方案的有效性。為了進(jìn)行較為真實的仿真,首先從一個實際的陀螺儀上采集一組真實的角速率測量值。采樣速率為60 Hz。從R0=I開始,真實姿態(tài)矩陣由下式迭代生成:

Rk+1=Rk(ΩΔt)

初始陀螺儀偏差的真值設(shè)置為b0=[-0.1 0.1 0.1]Trad/s。然后生成一組數(shù)據(jù)為

(46)

陀螺儀噪聲wΩ有0.001 rad/s的標(biāo)準(zhǔn)差,陀螺儀偏差b的過程噪聲方差為Σb=0.0012Irad2/s4,而磁力計和加速度計噪聲Σa=Σm=0.01I。

將所提方案與經(jīng)典的MEKF相比較,MEKF同樣采用旋轉(zhuǎn)矩陣作為姿態(tài)表示。進(jìn)行50次Monte Carlo實驗,得到兩種方案的姿態(tài)估計誤差的變化如圖1所示。

圖1 姿態(tài)估計誤差的變化圖

從圖1中可以看出,由于兩種方案都采用旋轉(zhuǎn)矩陣表示,在濾波預(yù)測階段過程方程也相同,從而姿態(tài)估計誤差相差極小。由于姿態(tài)誤差呈周期性波動,故采用最后15 s的誤差平均值進(jìn)行比較。50次仿真結(jié)果的均值與標(biāo)準(zhǔn)差如表1所示。

表1 50次仿真最后15 s姿態(tài)估計誤差

可以看出在姿態(tài)估計誤差方面InEKF比MEKF略有優(yōu)勢。而對陀螺儀偏差的估計如圖2所示。

圖2 陀螺儀偏差估計誤差的變化圖

由圖2可以看出在姿態(tài)估計誤差相對穩(wěn)定之后,InEKF估計的陀螺儀偏差要更加接近真值。圖3是濾波過程結(jié)束時刻陀螺儀偏差誤差的箱線圖。

圖3 濾波過程結(jié)束時刻陀螺儀偏差誤差的箱線圖

由圖3可以看出InEKF估計的陀螺儀偏差整體更優(yōu),InEKF誤差的平均值為0.008 947,而MEKF的平均值為0.010 850,InEKF比MEKF的精度提高了約18%,具體數(shù)據(jù)如表2所示。某次濾波過程開始階段和中間一段時間陀螺儀偏差的變化如圖4和圖5所示。

表2 仿真中陀螺儀偏差誤差統(tǒng)計量

圖4 某次仿真開始階段陀螺儀偏差的變化圖

圖5 某次仿真中間階段陀螺儀偏差的變化圖

從變化軌跡可以看出,InEKF的估計比MEKF收斂到真值附近的時間更快,證明了該方法具有優(yōu)越性。

5 結(jié)束語

本文提出了一種InEKF算法,用于同時估計無人機姿態(tài)和陀螺儀偏差。借助特殊正交群SO(3)的李群特性,推導(dǎo)出一種在SO(3)×R3中運動的連續(xù)時間隨機非線性濾波算法,該算法的主要特點是將姿態(tài)估計結(jié)果和估計誤差表示為SO(3)的元素,通過指數(shù)映射推導(dǎo)誤差動態(tài)的近似一階線性微分方程,從而減小誤差動態(tài)對當(dāng)前估計值的依賴,增強了濾波算法對初值不確定的魯棒性。通過仿真將該算法與經(jīng)典的MEKF算法進(jìn)行比較,仿真結(jié)果表明所提出的濾波算法在保持姿態(tài)估計精度的同時,能提供更優(yōu)的陀螺儀偏差估計,驗證了該算法在精度和濾波收斂性等方面具有優(yōu)越性。