李世強(qiáng), 易文俊

(南京理工大學(xué) 瞬態(tài)物理國(guó)家重點(diǎn)實(shí)驗(yàn)室,江蘇 南京 210094)

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

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

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

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

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

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

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

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

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

(1)

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

(2)

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

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

(3)

在SO(3)中運(yùn)動(dòng)的連續(xù)軌跡R(t)有

(4)

(5)

對(duì)fw(R)有

fw(R1R2)=R1R2w∧

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

(6)

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

=0

(7)

=ηlw∧-w∧ηl

(8)

(9)

2 建立模型

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

2.1 參考坐標(biāo)系

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

2.2 運(yùn)動(dòng)模型和測(cè)量模型

四旋翼無(wú)人機(jī)由旋轉(zhuǎn)矩陣表示的連續(xù)時(shí)間姿態(tài)運(yùn)動(dòng)方程為

(10)

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

(11)

(12)

(13)

(14)

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

(15)

3 不變擴(kuò)展卡爾曼濾波

3.1 預(yù)測(cè)

考慮完整的系統(tǒng)運(yùn)動(dòng)方程和測(cè)量方程:

(16)

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

(17)

(18)

(19)

(20)

3.2 測(cè)量更新

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

(21)

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

(22)

η+=Exp(Kkzk)η

(23)

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

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

(24)

因此

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

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

=ξ+Kk(Hξ+Vk)

(25)

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

(26)

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

(27)

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

(28)

3.3 引入陀螺儀偏差

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

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

(29)

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

(30)

式中:ζ為偏差誤差。

(31)

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

(32)

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

(33)

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

(34)

測(cè)量方程不依賴(lài)于IMU的偏差。因此,對(duì)增強(qiáng)的右不變誤差,H矩陣可以簡(jiǎn)單地附加零以滿(mǎn)足維度要求。線(xiàn)性更新方程成為

(35)

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

(36)

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

(37)

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

(38)

(39)

3.4 離散化

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

(40)

(41)

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

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

(42)

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

(43)

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

(44)

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

(45)

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

4 數(shù)值模擬

使用 MATLAB 對(duì)前述不變擴(kuò)展卡爾曼濾波算法進(jìn)行仿真實(shí)驗(yàn),證明了所提出的不變擴(kuò)展卡爾曼濾波方案的有效性。為了進(jìn)行較為真實(shí)的仿真,首先從一個(gè)實(shí)際的陀螺儀上采集一組真實(shí)的角速率測(cè)量值。采樣速率為60 Hz。從R0=I開(kāi)始,真實(shí)姿態(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的過(guò)程噪聲方差為Σb=0.0012Irad2/s4,而磁力計(jì)和加速度計(jì)噪聲Σa=Σm=0.01I。

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

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

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

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

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

圖2 陀螺儀偏差估計(jì)誤差的變化圖

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

圖3 濾波過(guò)程結(jié)束時(shí)刻陀螺儀偏差誤差的箱線(xiàn)圖

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

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

圖4 某次仿真開(kāi)始階段陀螺儀偏差的變化圖

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

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

5 結(jié)束語(yǔ)

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