基于矩陣分解的捷聯(lián)慣導(dǎo)多位置系統(tǒng)級(jí)標(biāo)定方法*

趙桂玲，譚茂林，吳 偲

(遼寧工程技術(shù)大學(xué)測(cè)繪與地理科學(xué)學(xué)院，遼寧 阜新 123000)

捷聯(lián)慣導(dǎo)系統(tǒng)級(jí)標(biāo)定的實(shí)質(zhì)是根據(jù)系統(tǒng)的導(dǎo)航輸出進(jìn)行慣性測(cè)量單元(Inertial Measurement Unit，IMU)誤差參數(shù)的估計(jì)[1-3]。IMU誤差參數(shù)標(biāo)定結(jié)果直接影響系統(tǒng)的導(dǎo)航精度，IMU估計(jì)精度和標(biāo)定時(shí)間也成為反映系統(tǒng)級(jí)標(biāo)定技術(shù)的關(guān)鍵指標(biāo)[4-5]。常用的系統(tǒng)級(jí)標(biāo)定方法分為擬合法和濾波法[6-7]。擬合法的研究集中在標(biāo)定路徑編排，通過(guò)設(shè)計(jì)合適的標(biāo)定路徑建立起觀測(cè)量與誤差參數(shù)之間的關(guān)系，再利用最小二乘估計(jì)對(duì)誤差參數(shù)進(jìn)行標(biāo)定[8]。擬合法路徑編排與待辨識(shí)誤差參數(shù)密切相關(guān)，計(jì)算簡(jiǎn)便，但標(biāo)定模型推導(dǎo)復(fù)雜[9]，標(biāo)定環(huán)境和條件要求嚴(yán)格[10]。

濾波法研究集中在設(shè)計(jì)濾波方案[11-12]，通過(guò)建立系統(tǒng)狀態(tài)方程和量測(cè)方程，利用Kalman濾波算法對(duì)誤差參數(shù)進(jìn)行估計(jì)[13]，鮮有文獻(xiàn)對(duì)路徑編排進(jìn)行深入的理論推導(dǎo)和分析，導(dǎo)致標(biāo)定位置多、標(biāo)定時(shí)間長(zhǎng)。文獻(xiàn)[14]建立了36維Kalman濾波模型，標(biāo)定路徑為25位置，標(biāo)定時(shí)間約為4 h。文獻(xiàn)[15]設(shè)計(jì)了一組13位置轉(zhuǎn)動(dòng)編排方案，標(biāo)定21項(xiàng)誤差參數(shù)用時(shí)約1 h。文獻(xiàn)[16]以導(dǎo)航速度誤差為觀測(cè)量建立了27維Kalman濾波器，設(shè)計(jì)了一組8位置標(biāo)定路徑來(lái)標(biāo)定21個(gè)誤差參數(shù)，標(biāo)定時(shí)間為40 min。上述文獻(xiàn)未對(duì)誤差參數(shù)辨識(shí)與路徑設(shè)計(jì)進(jìn)行深入的理論分析和探討。文獻(xiàn)[17]對(duì)IMU誤差參數(shù)激發(fā)過(guò)程進(jìn)行分析，建立了27維濾波模型，設(shè)計(jì)了35次序標(biāo)定路徑，標(biāo)定時(shí)間為2 h，標(biāo)定路徑過(guò)于復(fù)雜。上述研究均存在標(biāo)定時(shí)間長(zhǎng)，標(biāo)定路徑復(fù)雜等問(wèn)題。

針對(duì)上述問(wèn)題，本文將安裝誤差角分解為不正交角和正交的失準(zhǔn)角，建立非正交和失準(zhǔn)的標(biāo)定模型。將擬合法中路徑編排與誤差參數(shù)辨識(shí)緊密聯(lián)系的特點(diǎn)引入濾波法，設(shè)計(jì)了7位置連續(xù)標(biāo)定路徑，對(duì)誤差參數(shù)的可辨識(shí)性、可觀測(cè)強(qiáng)度和標(biāo)定時(shí)間進(jìn)行分析，完成21項(xiàng)誤差參數(shù)的完全標(biāo)定，標(biāo)定時(shí)間小于30 min。

1 系統(tǒng)模型的建立

1.1 IMU輸出誤差模型

捷聯(lián)慣導(dǎo)系統(tǒng)通常由三個(gè)加速度計(jì)和三個(gè)陀螺組成，加速度計(jì)的比力誤差和陀螺的角速率誤差為:

式中:δfb和分別表示加速度計(jì)和陀螺的輸出誤差；δMa和δMg是三階方陣，兩個(gè)矩陣中主對(duì)角線元素分別表示加速度計(jì)組件和陀螺組件標(biāo)度因數(shù)誤差，非主對(duì)角線元素表示加速度計(jì)組件和陀螺組件安裝誤差；fb和表示IMU理想輸出；Ba和Bg分別表示加速度計(jì)和陀螺的零偏誤差。

導(dǎo)航坐標(biāo)系下觀測(cè)量對(duì)載體坐標(biāo)系參數(shù)約束不足，加速度計(jì)和陀螺12項(xiàng)安裝誤差系數(shù)存在3組耦合關(guān)系[18]。將加速度計(jì)或陀螺敏感軸進(jìn)行重新定義，對(duì)載體系參數(shù)形成約束，安裝誤差參數(shù)降為9項(xiàng)。引入矩陣分解原理[19]，將δMa和δMg分解為對(duì)稱(chēng)矩陣(Symmetric matrix)與斜對(duì)稱(chēng)矩陣(Skewsymmetric matrix)之和。和表示對(duì)稱(chēng)矩陣，和表示斜對(duì)稱(chēng)矩陣。

加速度計(jì)組件和陀螺組件的安裝誤差特性由非正交特性和正交失準(zhǔn)特性構(gòu)成。非正交特性表現(xiàn)為組件的3個(gè)輸入軸不正交，正交失準(zhǔn)特性表現(xiàn)為正交組件相對(duì)載體系失準(zhǔn)。采用最佳約束方式使陀螺斜對(duì)稱(chēng)矩陣為零[19]，對(duì)應(yīng)安裝誤差標(biāo)定流程如圖1所示。

圖1 安裝誤差標(biāo)定流程

從式(2)和圖1可知，本文對(duì)IMU安裝誤差進(jìn)行分解，安裝誤差角被分解為非正交角和正交的失準(zhǔn)角。安裝誤差標(biāo)定流程為先完成安裝誤差正交化，正交化完成后的組件相對(duì)載體系b系失準(zhǔn)，需要轉(zhuǎn)動(dòng)與b系重合，使IMU完成真正意義上的安裝誤差建模。由于約束關(guān)系的存在，本文設(shè)計(jì)的約束方式是將正交化后的陀螺組件構(gòu)成的正交坐標(biāo)系視為與b系重合。由此，系統(tǒng)輸出誤差模型設(shè)定為:

1.2 系統(tǒng)誤差方程

忽略δg影響，略去二階小量，姿態(tài)誤差角φ＝[φxφyφz]T為小角度，靜基座下IMU誤差參數(shù)引起捷聯(lián)慣導(dǎo)系統(tǒng)速度和姿態(tài)誤差方程為:

式中:導(dǎo)航坐標(biāo)系n系選用東-北-天坐標(biāo)系，載體坐標(biāo)系b系選用右-前-上坐標(biāo)系。表示n系下速度誤差，fn表示比力在n系下的投影，表示地球自轉(zhuǎn)角速度在n系下的投影，δfb和為系統(tǒng)輸出誤差模型中b系下比力誤差和旋轉(zhuǎn)角速度誤差。表示b系到n系的捷聯(lián)矩陣，靜基座條件下fn＝(說(shuō)明:ωc、ωs分別為ωiecosL、ωiesinL的簡(jiǎn)寫(xiě)，L為當(dāng)?shù)氐乩砭暥?。

1.3 Kalman濾波模型

根據(jù)IMU系統(tǒng)輸出誤差模型和系統(tǒng)誤差方程，建立的Kalman濾波狀態(tài)變量為27維:

根據(jù)式(4)和式(5)構(gòu)建濾波器狀態(tài)方程和量測(cè)方程:

式中:F為狀態(tài)轉(zhuǎn)移矩陣，G為系統(tǒng)噪聲分配矩陣，W為IMU輸出中的白噪聲向量，觀測(cè)矩陣H＝[I6×606×21]。

2 標(biāo)定路徑的設(shè)計(jì)

2.1 路徑編排

在靜基座條件下，IMU通過(guò)敏感重力加速度g和地球自轉(zhuǎn)角速率ωie完成誤差參數(shù)的標(biāo)定。由于ωie是一個(gè)小量，為更好地激勵(lì)陀螺各項(xiàng)誤差參數(shù)，同時(shí)保證標(biāo)定過(guò)程的連續(xù)性，通過(guò)轉(zhuǎn)臺(tái)施加轉(zhuǎn)動(dòng)角速率ωr完成位置變換。

1.2.1 對(duì)照組 行單一肛瘺切除術(shù)治療:患者手術(shù)前進(jìn)行常規(guī)的灌腸和導(dǎo)瀉,所有患者進(jìn)行腰-硬聯(lián)合麻醉[3],麻醉后進(jìn)行常規(guī)消毒,然后鋪巾,等待肛門(mén)括約肌松弛舒張后進(jìn)行肛內(nèi)消毒。行單一肛瘺切除術(shù)治療時(shí)首先探針由外口向內(nèi)口插入,然后將瘺管和內(nèi)口沿探針切除,沿著切開(kāi)的瘺管清除所有瘢痕和壞死組織。對(duì)患者術(shù)后情況觀察并記錄。

設(shè)計(jì)IMU敏感軸在兩個(gè)標(biāo)定位置分別指向天向和地向快速消除誤差參數(shù)中的部分耦合關(guān)系。以“北-東-地”和“北-西-天”兩個(gè)位置為例。在“北-東-地”完成靜態(tài)導(dǎo)航后，繞x軸轉(zhuǎn)動(dòng)180°到達(dá)“北-西-天”。對(duì)于IMU靜止在標(biāo)定位置時(shí)，設(shè)靜止時(shí)間從t0~t1，則在靜止位置下有:

將兩個(gè)位置對(duì)應(yīng)的參數(shù)代入式(11)，得兩個(gè)位置下的速度誤差和姿態(tài)誤差，如下

從式(12)和式(13)可看出，兩個(gè)位置下速度誤差和姿態(tài)誤差對(duì)應(yīng)被激勵(lì)的IMU誤差參數(shù)是相同的，參數(shù)前的正負(fù)號(hào)不同，聯(lián)立公式即可消除參數(shù)間的耦合關(guān)系。通過(guò)這種標(biāo)定位置設(shè)計(jì)方式可以快速辨識(shí)加速度計(jì)和陀螺誤差參數(shù)，如下:

上述兩個(gè)標(biāo)定位置可以直接辨識(shí)出7項(xiàng)誤差參數(shù)，另外有9項(xiàng)誤差參數(shù)雖被激勵(lì)，但由于存在耦合關(guān)系不能被辨識(shí)出，剩下的5項(xiàng)參數(shù)則不能被激勵(lì)。

IMU從“北-東-地”繞x軸旋轉(zhuǎn)180°到“北-西-天”，轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)角速率ωr設(shè)計(jì)為6°/s，標(biāo)定中由于ωc?ωr、ωs?ωr，Bgi(i＝x，y，z)?ωr，因此在轉(zhuǎn)動(dòng)過(guò)程中可忽略地球自轉(zhuǎn)角速率和陀螺零偏。在t1~t2轉(zhuǎn)動(dòng)過(guò)程中由陀螺誤差引起的姿態(tài)誤差方程如下:

式中:以“北-東-地”為初始位置繞x軸轉(zhuǎn)動(dòng)，轉(zhuǎn)動(dòng)過(guò)程中的捷聯(lián)矩陣為:

轉(zhuǎn)動(dòng)完成后，

從式(17)可看出，本次轉(zhuǎn)動(dòng)可使陀螺3項(xiàng)誤差參數(shù)被辨識(shí)出。由于Bgi(i＝x，y，z)?ωr，在轉(zhuǎn)動(dòng)過(guò)程中難以辨識(shí)，在轉(zhuǎn)臺(tái)靜止時(shí)進(jìn)行IMU零偏標(biāo)定。零偏仿真結(jié)果如圖2所示。每個(gè)位置靜止600 s，轉(zhuǎn)動(dòng)時(shí)間為30 s，標(biāo)定時(shí)間共計(jì)20.5 min。

圖2 IMU零偏誤差估計(jì)曲線

2.2 標(biāo)定方案

同一標(biāo)定路徑下，不同誤差參數(shù)的可觀測(cè)度不同。圖2中，加速度計(jì)零偏誤差可觀測(cè)度高，陀螺可觀測(cè)度低。當(dāng)從“北-東-地”轉(zhuǎn)換到“北-西-天”時(shí)，加速度計(jì)3項(xiàng)零偏誤差在很短的時(shí)間內(nèi)達(dá)到收斂。陀螺3項(xiàng)零偏誤差的可觀測(cè)度差別明顯，Bgx在該路徑下可觀測(cè)度強(qiáng)，短時(shí)間即達(dá)到收斂，Bgy在轉(zhuǎn)動(dòng)完成后需要約6min才能收斂，Bgz需要約8 min才能收斂。分析原因，陀螺敏感地球自轉(zhuǎn)角速率是一個(gè)小量，導(dǎo)致陀螺零偏的可觀測(cè)度低，需要的標(biāo)定時(shí)間長(zhǎng)。綜合考慮加速計(jì)和陀螺零偏收斂時(shí)間，本文設(shè)計(jì)標(biāo)定路徑如圖3所示，前2個(gè)位置靜止時(shí)間分別為500 s，后5個(gè)位置靜止時(shí)間分別為120 s。

圖3 7位置標(biāo)定路徑

2.3 可觀測(cè)性分析

為驗(yàn)證標(biāo)定路徑的有效性，采用分段線性定長(zhǎng)系統(tǒng)(Piece-Wise Constant System，PWCS)對(duì)路徑進(jìn)行可觀測(cè)性分析。利用提取的可觀測(cè)性矩陣(Stripped Observability Matrix，SOM)代替總的可觀測(cè)性矩陣，使得模型的可觀測(cè)分析變得更簡(jiǎn)單[20]。

根據(jù)Kalman濾波器的狀態(tài)方程和量測(cè)方程，可觀測(cè)性矩陣SOM為:

式中:Q代表矩陣SOM，n為Kalman濾波器狀態(tài)維數(shù)，為27維。計(jì)算SOM在各位置下的秩，結(jié)果如圖4。

圖4 各位置下可觀測(cè)矩陣的秩

從圖4可以看出，系統(tǒng)在第六個(gè)位置時(shí)，SOM的秩達(dá)到27，等于濾波器狀態(tài)維數(shù)?？梢耘卸ㄏ到y(tǒng)狀態(tài)在該路徑下完全可觀測(cè)[21]，驗(yàn)證了論文設(shè)計(jì)路徑的可行性。

3 實(shí)驗(yàn)結(jié)果與分析

對(duì)7位置系統(tǒng)級(jí)標(biāo)定算法進(jìn)行仿真實(shí)驗(yàn)。初始速度設(shè)置為V0＝03×1，3個(gè)初始姿態(tài)角分別設(shè)置為:航向角270°，俯仰角0°，橫滾角180°，采樣頻率設(shè)置為100 Hz。各誤差參數(shù)的預(yù)設(shè)值見(jiàn)表2。濾波估計(jì)曲線如圖5~圖10所示。

從圖5~圖10中可以看出，各誤差參數(shù)都能完全收斂，估計(jì)結(jié)果見(jiàn)表1。

圖5 加速度計(jì)零偏誤差估計(jì)曲線

圖10 陀螺安裝誤差估計(jì)曲線

表1 標(biāo)定誤差估計(jì)結(jié)果

圖6 加速度計(jì)標(biāo)度因數(shù)誤差濾波估計(jì)曲線

圖7 加速度計(jì)安裝誤差估計(jì)曲線

圖8 陀螺零偏誤差估計(jì)曲線

圖9 陀螺標(biāo)度因數(shù)誤差估計(jì)曲線

計(jì)算IMU待標(biāo)定參數(shù)估計(jì)結(jié)果，陀螺零偏誤差優(yōu)于0.000 15°/h，加速度計(jì)的零偏誤差優(yōu)于3μgn，陀螺標(biāo)度因數(shù)誤差優(yōu)于2×10-6，加速度計(jì)的標(biāo)度因數(shù)誤差優(yōu)于4.5×10-6，安裝誤差角優(yōu)于1″。

為驗(yàn)證基于矩陣分解的捷聯(lián)慣導(dǎo)多位置系統(tǒng)級(jí)標(biāo)定方法的可靠性，另設(shè)置3組不同的安裝誤差參數(shù)，分別為表1中安裝誤差參數(shù)預(yù)設(shè)值的40%、60%、80%(組1~組3)，進(jìn)行仿真對(duì)比實(shí)驗(yàn)。

表2以各組中加速度計(jì)和陀螺安裝誤差參數(shù)估計(jì)精度最低的誤差項(xiàng)作為安裝誤差的估計(jì)誤差，IMU各項(xiàng)安裝誤差標(biāo)定精度均優(yōu)于1″。

表2 對(duì)比實(shí)驗(yàn)安裝誤差估計(jì)誤差

4 結(jié)論

本文基于矩陣分解原理對(duì)安裝誤差矩陣進(jìn)行分解，將安裝誤差矩陣分解為對(duì)稱(chēng)矩陣和斜對(duì)稱(chēng)矩陣之和，對(duì)應(yīng)地將安裝誤差角分解為不正交角和正交的失準(zhǔn)角，建立了包含21項(xiàng)誤差參數(shù)的標(biāo)定誤差模型。以導(dǎo)航速度誤差和姿態(tài)誤差為觀測(cè)量，建立27維Kalman濾波模型。深入分析標(biāo)定路徑與誤差參數(shù)辨識(shí)間的關(guān)系，根據(jù)陀螺和加速度計(jì)零偏可觀測(cè)度的不同設(shè)置標(biāo)定時(shí)間，設(shè)計(jì)了一種7位置標(biāo)定編排方案，誤差參數(shù)在標(biāo)定結(jié)束后全部收斂，標(biāo)定時(shí)間小于30 min。仿真實(shí)驗(yàn)驗(yàn)證了所提方法和標(biāo)定路徑的可行性。本文為系統(tǒng)級(jí)標(biāo)定濾波法的路徑編排提供了一些新思路，具有良好的工程參考價(jià)值。