基于改進(jìn)四元數(shù)阻尼誤差模型的SINS初始對(duì)準(zhǔn)算法

趙仁杰, 李開龍, 胡柏青, 田佳玉

(海軍工程大學(xué)電氣工程學(xué)院, 湖北 武漢 430033)

0 引 言

初始對(duì)準(zhǔn)是捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system, SINS)的一項(xiàng)關(guān)鍵性技術(shù),主要為SINS提供準(zhǔn)確的初始姿態(tài)等信息,其中,對(duì)準(zhǔn)的精度和速度是影響SINS后續(xù)工作的兩項(xiàng)重要指標(biāo)[1]。

經(jīng)過不斷研究,SINS初始對(duì)準(zhǔn)方法逐漸豐富和完善。目前,這些方法主要可以歸納為以下兩類:第一類是兩階段對(duì)準(zhǔn),先通過粗對(duì)準(zhǔn)獲取粗略的姿態(tài)信息,再進(jìn)行精對(duì)準(zhǔn)。其中,傳統(tǒng)粗對(duì)準(zhǔn)方法為解析法[2-3],即利用慣性器件敏感重力加速度和地球自轉(zhuǎn)角速度,通過矢量定姿求解姿態(tài),一般僅適用于靜止或微幅擾動(dòng)條件下,并對(duì)器件精度有一定要求。為克服解析法的局限性,文獻(xiàn)[4]提出了一種基于坐標(biāo)系分解的慣性系對(duì)準(zhǔn)方法,能夠有效隔離角運(yùn)動(dòng),實(shí)現(xiàn)晃動(dòng)基座粗對(duì)準(zhǔn);在此基礎(chǔ)上,Wu等[5]又將對(duì)準(zhǔn)問題轉(zhuǎn)化為姿態(tài)確定問題(Wahba問題[6]),提出了優(yōu)化對(duì)準(zhǔn)(optimization-based alignment, OBA)方法,提高了信息利用率,具有較強(qiáng)的魯棒性,后續(xù)得到了廣泛關(guān)注和研究[7-10];精對(duì)準(zhǔn)方法主要有羅經(jīng)法、參數(shù)辨識(shí)法和卡爾曼濾波法。第二類是大失準(zhǔn)角非線性對(duì)準(zhǔn)[11-12],研究主要集中在非線性誤差模型和非線性濾波算法兩方面,其中,非線性誤差模型的研究重點(diǎn)在于姿態(tài)[13-15];非線性濾波算法主要有拓展卡爾曼濾波、無跡卡爾曼濾波、容積卡爾曼濾波、粒子濾波等,其中,無跡卡爾曼濾波(unscented Kalman filter, UKF)憑借較高精度以及適中的計(jì)算量,在解決非線性問題中被廣泛應(yīng)用。

非線性對(duì)準(zhǔn)可以簡(jiǎn)化對(duì)準(zhǔn)過程,縮短對(duì)準(zhǔn)時(shí)間,但實(shí)際對(duì)準(zhǔn)效果往往卻達(dá)不到最優(yōu),對(duì)此許多學(xué)者對(duì)濾波算法進(jìn)行了改進(jìn),有效提高了濾波性能[16-19];文獻(xiàn)[20]提出了一種基于坐標(biāo)系一致性的矢量幾何誤差構(gòu)建思想,在此基礎(chǔ)上,王茂松等[21-22]指出傳統(tǒng)速度誤差只考慮了向量的大小差異,而忽視了方向差異,重新對(duì)其進(jìn)行了定義,提出一種狀態(tài)變換卡爾曼濾波方法,有效提高了組合導(dǎo)航的航向估計(jì)精度和穩(wěn)定性。如果真實(shí)導(dǎo)航系未知,實(shí)際SINS解算應(yīng)在計(jì)算坐標(biāo)系下進(jìn)行,上述文獻(xiàn)將速度誤差矢量統(tǒng)一在真實(shí)導(dǎo)航系下的做法仍有待討論。Chang等[23]將速度誤差統(tǒng)一定義在計(jì)算坐標(biāo)系下,推導(dǎo)了改進(jìn)歐拉角非線性對(duì)準(zhǔn)模型,并通過試驗(yàn)驗(yàn)證了模型的有效性。

基于矢量誤差坐標(biāo)系一致性思想,本文對(duì)傳統(tǒng)四元數(shù)非線性誤差模型中的姿態(tài)誤差模型和速度誤差模型進(jìn)行改進(jìn),將誤差矢量統(tǒng)一投影在計(jì)算導(dǎo)航系下,同時(shí)引入外界阻尼信息,提出一種改進(jìn)四元數(shù)阻尼誤差模型對(duì)準(zhǔn)算法,并應(yīng)用于系泊狀態(tài)下的SINS初始對(duì)準(zhǔn),通過仿真和車載試驗(yàn)對(duì)比了該改進(jìn)算法與傳統(tǒng)四元數(shù)阻尼誤差模型對(duì)準(zhǔn)算法以及文獻(xiàn)[23]中歐拉角阻尼誤差模型對(duì)準(zhǔn)算法的對(duì)準(zhǔn)效果,驗(yàn)證了本文所提算法的有效性。

1 四元數(shù)阻尼SINS誤差模型

關(guān)于SINS的大失準(zhǔn)角非線性誤差模型已有完整推導(dǎo)[13-14],考慮形式比較復(fù)雜,本節(jié)引入全球定位系統(tǒng)(global positioning system,GPS)速度和位置阻尼信息進(jìn)行模型簡(jiǎn)化,推導(dǎo)了四元數(shù)阻尼非線性誤差模型。

為方便建模,將由GPS阻尼信息獲取的參量統(tǒng)一用下標(biāo)g表示,忽略小量誤差,認(rèn)為帶下標(biāo)g的計(jì)算參量值等效為真實(shí)值。此外,定義坐標(biāo)系如下:記地心慣性坐標(biāo)系為i系,地球坐標(biāo)系為e系,選擇“右前上”載體坐標(biāo)系為b系,“東北天”地理坐標(biāo)系為導(dǎo)航坐標(biāo)系,記為n系,計(jì)算導(dǎo)航坐標(biāo)系為n′系。

1.1 SINS狀態(tài)微分方程

給出由四元數(shù)表示的SINS姿態(tài)、速度和位置微分方程,具體如下:

(1)

(2)

(3)

(4)

(5)

式中:I3×3為3維單位矩陣;RP為當(dāng)?shù)厍示仃?表達(dá)式為

(6)

式中:RM和RN分別表示子午圈與卯酉圈主曲率半徑。

1.2 傳統(tǒng)阻尼誤差方程

引入GPS阻尼速度和位置,實(shí)際阻尼狀態(tài)微分方程可表示為

(7)

(8)

(9)

(10)

(11)

(12)

(13)

式中:q*代表四元數(shù)共軛。

對(duì)式(13)左右兩邊同時(shí)微分可得

(14)

將式(1)和式(7)代入式(14),參照文獻(xiàn)[14]推導(dǎo)得到傳統(tǒng)的阻尼姿態(tài)誤差方程,具體如下:

(15)

傳統(tǒng)的速度誤差定義為

δvn′=vn′-vn

(16)

對(duì)式(16)兩邊同時(shí)微分,推導(dǎo)可得傳統(tǒng)的阻尼速度誤差方程,具體如下:

(17)

式中:δfb表示加速度計(jì)測(cè)量誤差。

直接給出傳統(tǒng)的阻尼位置誤差方程如下:

(18)

1.3 改進(jìn)阻尼誤差方程

基于實(shí)際SINS解算在n′系下進(jìn)行這一認(rèn)識(shí),通過觀察第1.2節(jié)傳統(tǒng)阻尼非線性誤差方程的推導(dǎo)過程,不難發(fā)現(xiàn)有以下兩處不夠嚴(yán)格:

(2)式(16)速度誤差定義并未考慮速度矢量的方向性,只是標(biāo)量意義上的減法,存在坐標(biāo)系不一致問題。

針對(duì)上述問題,本節(jié)將誤差統(tǒng)一至n′系,重新推導(dǎo)了改進(jìn)阻尼非線性誤差方程,其中,改進(jìn)阻尼姿態(tài)誤差方程如下所示:

(19)

考慮矢量方向性,重新定義速度誤差[23]為

(20)

對(duì)式(20)左右兩邊同時(shí)微分可得:

(21)

將式(2)和式(8)代入式(21),推導(dǎo)可得改進(jìn)阻尼速度誤差方程,具體如下:

(22)

改進(jìn)阻尼位置誤差方程如下所示:

(23)

2 系泊條件下的非線性對(duì)準(zhǔn)算法

本文研究系泊狀態(tài)下的SINS初始對(duì)準(zhǔn),在系泊狀態(tài)下,載體位置相對(duì)固定,可以通過GPS準(zhǔn)確獲取,線速度近似為零,是有效的零速約束條件。SINS誤差模型通常包括姿態(tài)、速度、位置和器件誤差,由于對(duì)準(zhǔn)的目的是在短時(shí)間內(nèi)快速獲取準(zhǔn)確的姿態(tài),考慮到器件誤差估計(jì)不僅受器件精度的影響,還和載體運(yùn)動(dòng)狀態(tài)有關(guān),且靜態(tài)下器件誤差的可觀性不強(qiáng),因此,本節(jié)僅將姿態(tài)誤差和速度誤差列為狀態(tài),推導(dǎo)系泊條件下的非線性對(duì)準(zhǔn)模型,同時(shí)結(jié)合UKF算法給出具體的對(duì)準(zhǔn)步驟。

2.1 非線性對(duì)準(zhǔn)模型

(24)

式中:

(25)

(26)

以速度誤差dvn′=vn′作為量測(cè),相應(yīng)量測(cè)模型如下:

y=Hx+v=[03×3,I3×3]x+v

(27)

式中:v表示速度噪聲。

式(24)和式(27)共同構(gòu)成了非線性對(duì)準(zhǔn)模型。

2.2 非線性對(duì)準(zhǔn)算法

本文選取UKF算法用于非線性對(duì)準(zhǔn),為避免四元數(shù)在UKF中存在的歸一化約束以及方差匹配問題,采用四元數(shù)無跡估計(jì)器(unscented quaternion estimator, USQUE)框架[24]。

下文給出對(duì)準(zhǔn)算法流程。

步驟 1初始化

定義四元數(shù)誤差δq,參照下式將其轉(zhuǎn)換為誤差修正羅德里格斯參數(shù)δσ,設(shè)置局部狀態(tài)為x=[δσT,xeT]T,其中,xe表示非姿態(tài)誤差狀態(tài)。

σ=ρ/(1+q0)

(28)

(29)

式中:n為狀態(tài)向量維數(shù);α為調(diào)節(jié)因子,一般取1e-4≤α≤1;κ、β均為比例因子,通常取κ=0,β≥0;

步驟 2導(dǎo)航解算

(30)

(31)

步驟3濾波

(1)時(shí)間更新

產(chǎn)生狀態(tài)sigma點(diǎn):

(32)

(33)

(34)

(35)

(36)

(37)

(38)

(2)量測(cè)更新

由于量測(cè)模型為線性,量測(cè)更新如下:

(39)

(40)

Pk=(In×n-KkHk)Pk/k-1

(41)

(3)姿態(tài)更新

(42)

3 試驗(yàn)驗(yàn)證

3.1 仿真試驗(yàn)

為驗(yàn)證本文所提改進(jìn)四元數(shù)阻尼誤差模型對(duì)準(zhǔn)算法在系泊條件下對(duì)準(zhǔn)的有效性,將改進(jìn)算法與傳統(tǒng)四元數(shù)阻尼誤差模型對(duì)準(zhǔn)算法以及文獻(xiàn)[23]中改進(jìn)和傳統(tǒng)歐拉角阻尼誤差模型對(duì)準(zhǔn)算法進(jìn)行對(duì)比,為簡(jiǎn)便表述,將上述4種算法分別記為Quaternion-I、Quaternion-T、Euler-I和Euler-T。

觀察圖1可以看出,相比其他3種算法,本文提出的Quaternion-I算法在對(duì)準(zhǔn)速度和精度上均具有優(yōu)勢(shì);此外,相同姿態(tài)表示下,改進(jìn)模型對(duì)準(zhǔn)算法的姿態(tài)估計(jì)效果整體上優(yōu)于傳統(tǒng)模型;在改進(jìn)算法中,基于四元數(shù)模型的Quaternion-I算法對(duì)準(zhǔn)效果要優(yōu)于基于歐拉角模型的Euler-I算法。

圖1 200次蒙特卡羅仿真姿態(tài)角估計(jì)誤差

為更加直觀的對(duì)比4種算法對(duì)準(zhǔn)效果,將200次蒙特卡羅仿真試驗(yàn)姿態(tài)估計(jì)誤差取絕對(duì)值后求平均,得到姿態(tài)平均估計(jì)誤差,如圖2所示。

圖2 200次蒙特卡羅仿真姿態(tài)角平均估計(jì)誤差

觀察圖2,在靜基座大失準(zhǔn)角仿真條件下,4種對(duì)準(zhǔn)算法均能將姿態(tài)失準(zhǔn)角收斂至小范圍,由于水平姿態(tài)角的可觀性強(qiáng)于航向角,其估計(jì)效果也明顯優(yōu)于航向角。在航向估計(jì)上,本文提出的Quaternion-I算法收斂速度最快,其次為Euler-I算法,而Quaternion-T算法和Euler-T算法則相對(duì)較慢,雖然也可以將航向失準(zhǔn)角拉回至小角度,但對(duì)準(zhǔn)結(jié)束時(shí)刻仍未穩(wěn)定收斂。為便于比較,統(tǒng)計(jì)對(duì)準(zhǔn)結(jié)束時(shí)刻的姿態(tài)角平均誤差作為算法的對(duì)準(zhǔn)精度,如表1所示。

表1 4種算法姿態(tài)對(duì)準(zhǔn)精度

由表1可知,Quaternion-I算法的俯仰角估計(jì)精度為0.399′,橫滾角精度為0.375′,航向角精度為0.097°,整體估計(jì)精度最優(yōu)。

大失準(zhǔn)角對(duì)準(zhǔn)條件下,非線性濾波采用反饋校正方式,將濾波器輸出結(jié)果反饋至導(dǎo)航結(jié)算過程,每一步都會(huì)對(duì)狀態(tài)誤差進(jìn)行修正,進(jìn)而使得誤差能夠快速有效收斂;同一姿態(tài)表示下,改進(jìn)模型修正了由坐標(biāo)系投影不一致引起的模型誤差,相比傳統(tǒng)模型具有更好的對(duì)準(zhǔn)效果;此外,對(duì)于改進(jìn)模型對(duì)準(zhǔn)算法,由于大失準(zhǔn)角下歐拉角表示的姿態(tài)誤差矩陣會(huì)因轉(zhuǎn)動(dòng)次序不同而不同,模型并不嚴(yán)格,相比之下,四元數(shù)是作為整體被使用,不存在轉(zhuǎn)動(dòng)次序問題且無奇異,故四元數(shù)模型對(duì)準(zhǔn)算法的姿態(tài)對(duì)準(zhǔn)效果要優(yōu)于歐拉角模型。仿真結(jié)果表明,本文提出的Quaternion-I算法不僅在對(duì)準(zhǔn)速度和精度上具有優(yōu)勢(shì),同時(shí)還具有較好的穩(wěn)定性。

3.2 車載試驗(yàn)

利用慣性級(jí)激光捷聯(lián)慣組靜態(tài)數(shù)據(jù)進(jìn)行算法驗(yàn)證。捷聯(lián)慣組器件精度如下:陀螺隨機(jī)漂移穩(wěn)定性為0.01°/h,加速度計(jì)零偏穩(wěn)定性為50 μg;數(shù)據(jù)采集位置為北緯34.246°。試驗(yàn)將捷聯(lián)慣組安裝在載車上,靜止采集約半小時(shí)數(shù)據(jù),實(shí)驗(yàn)過程存在開關(guān)車門、上下車走動(dòng)等干擾活動(dòng)。

本文選取600 s數(shù)據(jù)用于算法驗(yàn)證,在姿態(tài)基準(zhǔn)上加入大失準(zhǔn)角構(gòu)造初始姿態(tài),為充分驗(yàn)證算法有效性,設(shè)置5組不同的大失準(zhǔn)角,具體如下:Ⅰ[-10° 10° 20°]、Ⅱ[-20° 20° 40°]、Ⅲ[-30° 30° 60°]、Ⅳ[-40° 40° 80°]、Ⅳ[-50° 50° 100°],針對(duì)不同失準(zhǔn)角條件,執(zhí)行4種算法,對(duì)比姿態(tài)對(duì)準(zhǔn)結(jié)果,限于篇幅僅給出第Ⅲ組條件下的姿態(tài)誤差對(duì)比,如圖3所示。

統(tǒng)計(jì)5組失準(zhǔn)角下4種算法的姿態(tài)估計(jì)誤差,如表2所示。分析圖3和表2可得,在不同條件下,4種算法的水平失準(zhǔn)角均能夠很快收斂,且精度較高,但在航向估計(jì)上,600 s對(duì)準(zhǔn)時(shí)間內(nèi),隨著初始失準(zhǔn)角的增大,Euler-T算法和Quaternion-T算法的對(duì)準(zhǔn)誤差有增加趨勢(shì),而Euler-I算法和 Quaternion-I算法對(duì)準(zhǔn)精度較好且相對(duì)穩(wěn)定,反映出改進(jìn)算法的收斂速度較傳統(tǒng)算法要快,其中Quaternion-I算法對(duì)準(zhǔn)效果最優(yōu);此外,失準(zhǔn)角范圍設(shè)置較廣,Quaternion-I算法均能夠快速完成對(duì)準(zhǔn),表明該算法具有較好的穩(wěn)定性,車載試驗(yàn)與仿真試驗(yàn)結(jié)果基本一致。

圖3 第Ⅲ組失準(zhǔn)角下姿態(tài)估計(jì)誤差

表2 5組失準(zhǔn)角下4種算法姿態(tài)估計(jì)誤差

4 結(jié) 論

本文針對(duì)傳統(tǒng)SINS四元數(shù)非線性誤差模型存在的坐標(biāo)系不一致問題進(jìn)行了研究,提出了一種改進(jìn)四元數(shù)阻尼誤差模型對(duì)準(zhǔn)算法,并應(yīng)用于系泊狀態(tài)下的SINS初始對(duì)準(zhǔn)。通過仿真和車載試驗(yàn)驗(yàn)證了算法的有效性,不同大失準(zhǔn)角條件下,改進(jìn)算法在對(duì)準(zhǔn)速度、精度以及穩(wěn)定性方面都具有較好的優(yōu)勢(shì),具有一定的工程應(yīng)用價(jià)值。