水下慣性導(dǎo)航系統(tǒng)姿態(tài)算法的適用性分析

王傲明 李?yuàn)檴?范 雕 張金輝 黃 炎 黃志勇

1 信息工程大學(xué)地理空間信息學(xué)院，鄭州市科學(xué)大道62號(hào)，450001

重力輔助慣性導(dǎo)航具有自主性強(qiáng)、隱蔽性好、抗干擾性強(qiáng)、不受地域和時(shí)域限制以及定位精度高等諸多優(yōu)點(diǎn)，是目前水下潛器實(shí)現(xiàn)長時(shí)間安全航行的重要手段之一[1-2]。慣性導(dǎo)航(下文簡稱慣導(dǎo))的解算精度會(huì)極大地影響重力輔助導(dǎo)航的精度，其解算核心是姿態(tài)更新，姿態(tài)的精度又會(huì)直接影響速度、位置以及重力匹配的精度[3-5]，因此需要對(duì)各種姿態(tài)算法的精度和適用性進(jìn)行研究。最常用的姿態(tài)求解方法為經(jīng)典多子樣算法和多子樣優(yōu)化算法，但經(jīng)典多子樣算法只考慮了Bortz方程[6]的二階項(xiàng)，存在原理誤差，會(huì)出現(xiàn)高子樣算法精度低于低子樣算法精度的現(xiàn)象[3-4]；多子樣優(yōu)化算法只適用于小角度錐角和低動(dòng)態(tài)載體的姿態(tài)求解[7-8]。宋敏[9]提出適用于大機(jī)動(dòng)環(huán)境下載體姿態(tài)求解的擴(kuò)展圓錐誤差補(bǔ)償算法，但只考慮了Bortz方程的二階項(xiàng)，仍存在原理誤差；嚴(yán)恭敏等[4]基于四元數(shù)微分方程給出了姿態(tài)更新的迭代算法，算法在推導(dǎo)過程中未作任何假設(shè)，有效避免了原理誤差。本文以圓錐運(yùn)動(dòng)作為姿態(tài)算法的測試輸入，通過仿真實(shí)驗(yàn)分析經(jīng)典多子樣算法、多子樣優(yōu)化算法、擴(kuò)展圓錐誤差補(bǔ)償算法以及基于迭代的姿態(tài)更新算法的精度、計(jì)算量和適用范圍，給出適用于水下工作環(huán)境的慣導(dǎo)姿態(tài)求解算法。

1 姿態(tài)更新算法與測試

1.1 經(jīng)典多子樣算法與多子樣優(yōu)化算法

假設(shè)陀螺在姿態(tài)更新周期[0,T]內(nèi)進(jìn)行了N次等間隔角增量采樣(N子樣)，則等效旋轉(zhuǎn)矢量φ(T)的經(jīng)典多子樣算法計(jì)算公式為[10]：

(1)

式中，Δθi為姿態(tài)更新周期內(nèi)的N個(gè)角增量，kij為各子樣對(duì)應(yīng)的不可交換的誤差補(bǔ)償系數(shù)，具體數(shù)值見文獻(xiàn)[10]。

等效旋轉(zhuǎn)矢量φ(T)的多子樣優(yōu)化算法為[8]：

(2)

式中，kN-i為各子樣對(duì)應(yīng)的圓錐誤差補(bǔ)償系數(shù)，具體數(shù)值見文獻(xiàn)[8]。

1.2 擴(kuò)展圓錐誤差補(bǔ)償算法與基于四元數(shù)微分方程迭代的姿態(tài)更新算法

為使大機(jī)動(dòng)環(huán)境下的載體姿態(tài)求解具有較高精度，宋敏[9]提出擴(kuò)展的圓錐誤差補(bǔ)償算法及其優(yōu)化方法，具體步驟見文獻(xiàn)[9]。嚴(yán)恭敏等[4]提出基于四元數(shù)微分方程迭代的姿態(tài)更新算法，通過迭代計(jì)算給出姿態(tài)更新的精確數(shù)值解，推導(dǎo)過程見文獻(xiàn)[4]。由于迭代算法在原理上未作任何假設(shè)，因此可獲得等效旋轉(zhuǎn)矢量的精確數(shù)值解，具有良好的環(huán)境適應(yīng)性，但缺點(diǎn)是計(jì)算量較大[4]。

1.3 圓錐運(yùn)動(dòng)

假設(shè)圓錐運(yùn)動(dòng)的錐軸為x軸，半錐角為φ，圓錐運(yùn)動(dòng)頻率為f，振動(dòng)角頻率為Ω(Ω=2πf)。當(dāng)t>0時(shí)，動(dòng)坐標(biāo)系(b系)相對(duì)于參考坐標(biāo)系(i系)的變換四元數(shù)Q(t)、角速度ω(t)、等效旋轉(zhuǎn)矢量φ(t)分別為[11]：

(3)

根據(jù)式(3)可計(jì)算出任意時(shí)刻圓錐運(yùn)動(dòng)的姿態(tài)，并作為參考姿態(tài)與姿態(tài)更新算法解算出的姿態(tài)作差獲得失準(zhǔn)角誤差，從而實(shí)現(xiàn)對(duì)姿態(tài)算法的精度評(píng)定。

2 仿真實(shí)驗(yàn)與分析

水下潛器上搭載的慣導(dǎo)會(huì)受到周期性機(jī)械振動(dòng)噪聲、螺旋槳噪聲和水動(dòng)力噪聲的影響[12]，容易激發(fā)出圓錐角運(yùn)動(dòng)。但由于水下潛器質(zhì)量巨大且航行過程較為平穩(wěn)，潛器不會(huì)出現(xiàn)大角度的機(jī)動(dòng)，且圓錐運(yùn)動(dòng)的半錐角也不會(huì)過大。因此，半錐角為5°的圓錐運(yùn)動(dòng)與潛器的實(shí)際運(yùn)動(dòng)情況較為相符。

以圓錐運(yùn)動(dòng)作為測試輸入對(duì)經(jīng)典多子樣算法、多子樣優(yōu)化算法、擴(kuò)展圓錐補(bǔ)償算法、迭代更新算法進(jìn)行仿真實(shí)驗(yàn)。設(shè)置圓錐運(yùn)動(dòng)頻率f=1 Hz，角增量采樣間隔為0.01 s，半錐角φ分別取1′和5°，子樣數(shù)分別取2～6(擴(kuò)展圓錐算法最高為5子樣)，仿真時(shí)長為1 h。以錐軸方向姿態(tài)每小時(shí)的漂移角度作為衡量算法精度的指標(biāo)(通常要求算法漂移的角度誤差小于1″/h)，以各種算法的運(yùn)行時(shí)間作為算法的計(jì)算量。算法運(yùn)行環(huán)境為Windows 10 64 bit，MATLAB R2018a。

2.1 仿真實(shí)驗(yàn)1

為考察載體在小角度半錐角低動(dòng)態(tài)環(huán)境下各算法的求解精度和計(jì)算量，取半錐角φ為1′，子樣數(shù)為2～6。不同子樣數(shù)下各算法錐軸方向的姿態(tài)漂移角度如圖1所示，各方向誤差最大值的絕對(duì)值如表1、表2和表3所示(單位″/h)。不同子樣數(shù)下各姿態(tài)更新算法的運(yùn)行時(shí)間如表4所示(單位s)，其中clas為經(jīng)典多子樣算法，opti為多子樣優(yōu)化算法，unco為擴(kuò)展圓錐誤差補(bǔ)償算法，iter 為姿態(tài)更新迭代算法。

圖1 2～6子樣數(shù)下錐軸方向姿態(tài)漂移角度Fig.1 Attitude drift angle of cone axis under two to six sub-samples

表1 X軸(錐軸)誤差最大值

表2 Y軸誤差最大值

表3 Z軸誤差最大值

表4 運(yùn)行時(shí)間

由表1可見，當(dāng)半錐角為小角度時(shí)，各算法各子樣的姿態(tài)誤差量級(jí)最大為10-4″/h，均滿足姿態(tài)求解的精度要求。隨著子樣數(shù)的增加，各算法姿態(tài)求解的精度逐漸提高，在相同子樣數(shù)下，unco精度最高、opti次之、clas最低，這與理論上3種算法在同一子樣數(shù)下的精度大小是一致的。當(dāng)子樣數(shù)為2時(shí)，各算法精度相當(dāng)且均滿足解算精度要求，因此為提高解算頻率，建議采用2子樣進(jìn)行解算。由表2和表3可見，除錐軸方向外，各算法在其他兩個(gè)軸向上姿態(tài)漂移角度的量級(jí)均為10-9″/h，可忽略不計(jì)。由表4可見，unco、opti和clas運(yùn)行時(shí)間大致相同，而iter的運(yùn)行時(shí)間較長。

2.2 仿真實(shí)驗(yàn)2

為考察潛器在實(shí)際運(yùn)動(dòng)情況下各算法的求解精度和計(jì)算量，取半錐角φ為5°，子樣數(shù)為2～6。

不同子樣數(shù)下各算法錐軸方向的姿態(tài)漂移角度如圖2所示，各方向誤差最大值的絕對(duì)值如表5、表6和表7所示(單位″/h)，不同子樣數(shù)下各算法的運(yùn)行時(shí)間如表8所示(單位s)。

圖2 2～6子樣數(shù)下錐軸方向姿態(tài)漂移角度Fig.2 Attitude drift angle of cone axis under two to six sub-sample

表5 X軸(錐軸)誤差最大值

表6 Y軸誤差最大值

表7 Z軸誤差最大值

表8 運(yùn)行時(shí)間

由表5可見，當(dāng)半錐角為5°、子樣數(shù)大于3時(shí)clas、opti、unco的精度會(huì)隨子樣數(shù)的增加而降低，這與嚴(yán)恭敏等[4]的結(jié)論相同。當(dāng)姿態(tài)角不滿足小角度條件時(shí)，clas會(huì)有部分子樣數(shù)精度低于1″/h，難以滿足姿態(tài)解算的精度要求。而iter的精度會(huì)隨子樣數(shù)的增加而逐漸升高，當(dāng)子樣數(shù)大于3時(shí)，角度漂移量小于0.03″/h，能夠滿足姿態(tài)求解精度的要求。由表6和表7可見，除錐軸方向外各算法在其他兩個(gè)軸向上的姿態(tài)誤差可以忽略不計(jì)，clas、opti、unco的漂移角度均隨子樣數(shù)的增加而逐漸增大，而iter的漂移角度卻隨子樣數(shù)的增加而逐漸減小。對(duì)比同一子樣數(shù)下各算法在其他兩個(gè)軸向上的姿態(tài)漂移角度可知，iter的漂移角度最小，clas與unco大致相當(dāng)，opti漂移最為嚴(yán)重(最大值接近0.11″/h)，這是由于推導(dǎo)圓錐誤差補(bǔ)償系數(shù)時(shí)未對(duì)其他兩個(gè)方向上的分量進(jìn)行補(bǔ)償所致。由表8可見，unco、opti和clas運(yùn)行時(shí)間大致相同，而iter的運(yùn)行時(shí)間較長。

3 結(jié) 語

本文以圓錐運(yùn)動(dòng)作為姿態(tài)更新算法的測試輸入，對(duì)現(xiàn)有姿態(tài)更新算法進(jìn)行仿真研究，對(duì)比分析各種算法的精度、計(jì)算量以及適用范圍，得出如下結(jié)論：

1)當(dāng)載體處于小角度低動(dòng)態(tài)環(huán)境時(shí)，多子樣優(yōu)化算法、經(jīng)典多子樣算法、擴(kuò)展圓錐誤差補(bǔ)償算法和迭代算法均能滿足姿態(tài)解算的精度要求。為獲得更高的姿態(tài)解算頻率，建議采用2子樣解算。

2)當(dāng)載體處于大角度低動(dòng)態(tài)或高動(dòng)態(tài)環(huán)境時(shí)，迭代算法具有明顯的優(yōu)勢，能夠更好地適應(yīng)環(huán)境，在錐軸或非錐軸方向上均具有較高的補(bǔ)償精度，可以精確地進(jìn)行姿態(tài)求解。其缺點(diǎn)在于計(jì)算量較大，但現(xiàn)代導(dǎo)航計(jì)算機(jī)的高性能處理能力能夠滿足導(dǎo)航實(shí)時(shí)計(jì)算的要求。

3)綜合考慮水下潛器實(shí)際所處環(huán)境、姿態(tài)求解精度以及計(jì)算量后，建議選擇子樣數(shù)大于3的迭代算法進(jìn)行水下重力輔助慣性導(dǎo)航姿態(tài)的求解。