里程儀輔助捷聯(lián)慣導(dǎo)系統(tǒng)的旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)方法

江一夫，師為建

（中國航天科技集團(tuán)第十六研究所,西安710100）

0 引言

行進(jìn)間對準(zhǔn)能夠有效提高捷聯(lián)慣導(dǎo)武器系統(tǒng)平臺的快速反應(yīng)能力和機(jī)動(dòng)性能，具有很高的軍事應(yīng)用價(jià)值。根據(jù)計(jì)算方法的不同，行進(jìn)間對準(zhǔn)方法可分為直接計(jì)算姿態(tài)的解析對準(zhǔn)法、在經(jīng)典控制方法框架下把對準(zhǔn)過程近似解耦為調(diào)平回路和尋北回路的羅經(jīng)對準(zhǔn)法[1]、使用狀態(tài)估計(jì)方法求解姿態(tài)的Kalman濾波對準(zhǔn)法[2]。羅經(jīng)法和Kalman濾波法采用Euler角描述姿態(tài)，并選擇迭代的局部線性化近似方法，因此會(huì)受到小誤差角的制約。文獻(xiàn)[3]～文獻(xiàn)[6]應(yīng)用慣性凝固思想解析對準(zhǔn)，較好地解決了在動(dòng)態(tài)條件下快速獲取粗略姿態(tài)信息的問題，使行進(jìn)間對準(zhǔn)技術(shù)的研究不再集中于非線性誤差模型的優(yōu)化。

無論應(yīng)用哪種計(jì)算方法，行進(jìn)間對準(zhǔn)的極限精度仍然受限于慣性儀表誤差和輔助傳感器誤差。而另一方面，旋轉(zhuǎn)調(diào)制技術(shù)開始在準(zhǔn)靜態(tài)對準(zhǔn)領(lǐng)域獲得廣泛的應(yīng)用，其本質(zhì)仍然是整周旋轉(zhuǎn)積分對慣性儀表零偏的平均對消作用。這種原理對運(yùn)動(dòng)狀態(tài)中的慣導(dǎo)系統(tǒng)依然適用，因此本文將旋轉(zhuǎn)調(diào)制技術(shù)延伸到了行進(jìn)間對準(zhǔn)，對消慣性儀表隨機(jī)常值零偏，反饋校正輔助傳感器誤差，從根本原理角度提升了行進(jìn)間對準(zhǔn)的極限精度。

本文提出了 “基于慣性凝固思想的q-method優(yōu)化對準(zhǔn)[6]+回溯Kalman濾波[7]”方法，解決了從非零速開始的動(dòng)態(tài)對準(zhǔn)問題?；趹T性凝固思想的q-method優(yōu)化對準(zhǔn)方法將慣導(dǎo)系統(tǒng)對準(zhǔn)問題等價(jià)變換為無窮觀測向量下的姿態(tài)優(yōu)化問題，很好地解決了誤差模型非線性的問題。從原理上而言，無論姿態(tài)運(yùn)動(dòng)如何，慣性系優(yōu)化對準(zhǔn)方法均能夠在任意運(yùn)動(dòng)狀態(tài)下快速實(shí)現(xiàn)自主對準(zhǔn)。但是，慣性系優(yōu)化對準(zhǔn)方法存在不適用于頻繁轉(zhuǎn)彎軌跡[7]、導(dǎo)航參數(shù)不全等弊端。在小角度誤差條件下，Kalman濾波仍然是一種可優(yōu)先選擇的最小方差意義的估計(jì)算法。因此，利用慣性系優(yōu)化對準(zhǔn)期間存儲的原始數(shù)據(jù)和獲取的初始姿態(tài)值回溯到對準(zhǔn)起始時(shí)刻重新開始Kalman濾波精對準(zhǔn)，等效延長對準(zhǔn)時(shí)間，提高對準(zhǔn)精度。

1 旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)方法

如圖1所示，定義g系為由三軸陀螺敏感軸構(gòu)成的非正交系，定義g0系（Oxg0yg0zg0）為陀螺正交參考坐標(biāo)系，xg0軸與陀螺敏感軸xg重合，yg0軸在xgyg平面內(nèi)，zg0與xg0、yg0構(gòu)成正交系。在實(shí)際情況中，非正交系g與g0系之間存在小角度安裝誤差。定義p系為轉(zhuǎn)位機(jī)構(gòu)的臺面坐標(biāo)系，zp與轉(zhuǎn)位機(jī)構(gòu)的旋轉(zhuǎn)軸重合，xp與xgo在轉(zhuǎn)臺臺面上的投影重合，yp與zp、xp構(gòu)成正交系。對于機(jī)抖偏頻激光陀螺慣導(dǎo)系統(tǒng)而言，p系與g0系之間僅存在小角度安裝誤差，可以通過轉(zhuǎn)位機(jī)構(gòu)的正反整圈旋轉(zhuǎn)標(biāo)定得到兩者之間的轉(zhuǎn)換關(guān)系。

圖1 旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)中的坐標(biāo)系關(guān)系Fig.1 Coordinates relationship in rotator inertial navigator

圖1中，三個(gè)坐標(biāo)系與轉(zhuǎn)位機(jī)構(gòu)固聯(lián)，隨轉(zhuǎn)位機(jī)構(gòu)旋轉(zhuǎn)。設(shè)轉(zhuǎn)位機(jī)構(gòu)在電氣零位時(shí)的p系為捷聯(lián)慣導(dǎo)載體右前上坐標(biāo)系b系，在尋北過程中轉(zhuǎn)位機(jī)構(gòu)繞zp的旋轉(zhuǎn)角度為?（t），則有

進(jìn)一步，設(shè)當(dāng)?shù)貣|北天地理坐標(biāo)系為n系，里程儀輸出坐標(biāo)系為車體右前上坐標(biāo)系m系，其與慣組載體坐標(biāo)系b系之間存在小角度安裝誤差。如圖2所示，兩個(gè)坐標(biāo)系之間的俯仰安裝偏差角為αθ，方位安裝偏差角為αψ。

圖2 慣組載體坐標(biāo)系與車體坐標(biāo)系的相互關(guān)系Fig.2 Relationship between the IMU body frame and the vehicle frame

旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)采用 “慣性系優(yōu)化對準(zhǔn)+回溯Kalman濾波精對準(zhǔn)”方式，如圖3所示。其中，ts為對準(zhǔn)開始時(shí)間，t1為慣性系解析對準(zhǔn)結(jié)束時(shí)間，te為行進(jìn)間對準(zhǔn)結(jié)束時(shí)間。從ts時(shí)刻啟動(dòng)旋轉(zhuǎn)開始，慣性系對準(zhǔn)并保存慣性儀表、轉(zhuǎn)位機(jī)構(gòu)的數(shù)據(jù)和里程儀脈沖直至t1時(shí)刻，然后回溯到ts時(shí)刻重新開始進(jìn)行Kalman濾波對準(zhǔn)直至對準(zhǔn)結(jié)束，以達(dá)到等效延長對準(zhǔn)時(shí)間的目的。

圖3 旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)方案Fig.3 Scheme of rotatory in-motion alignment

里程儀在已被粗標(biāo)定的基礎(chǔ)上可以提供車體系m的等效前向速度，其與載體約束信息相結(jié)合可以生成三維速度矢量，通過安裝關(guān)系和轉(zhuǎn)位機(jī)構(gòu)測角信息可以將該速度信息轉(zhuǎn)換到轉(zhuǎn)位機(jī)構(gòu)臺面坐標(biāo)系p系中。事實(shí)上，由于機(jī)抖激光陀螺的數(shù)字濾波存在延時(shí)，慣性儀表信息與里程儀信息和轉(zhuǎn)位測角信息是不同步的，但這個(gè)延時(shí)是固定的，具體延時(shí)由數(shù)字濾波器的階數(shù)和采樣頻率確定。為了簡單可靠地對延時(shí)進(jìn)行補(bǔ)償，本文通過保存后兩者一段時(shí)長的數(shù)據(jù)，實(shí)現(xiàn)等效 “延后”組合，待轉(zhuǎn)位機(jī)構(gòu)停止轉(zhuǎn)動(dòng)時(shí)輸出對準(zhǔn)結(jié)果。

慣性儀表的旋轉(zhuǎn)調(diào)制相當(dāng)于一種誤差自補(bǔ)償措施，可以消除轉(zhuǎn)軸垂直方向慣性儀表的常值零偏誤差，其短時(shí)高精度又可以反饋校正里程儀誤差，最終兩者的誤差可相互校正，實(shí)現(xiàn)對準(zhǔn)精度的提升?；厮轂V波等效延長了對準(zhǔn)時(shí)間，減小了慣性儀表的隨機(jī)游走誤差和里程儀噪聲誤差的影響，確保了行進(jìn)間對準(zhǔn)在不同軌跡下可達(dá)到較高的精度。

2 慣性系優(yōu)化對準(zhǔn)

Kalman回溯最優(yōu)濾波的前提是姿態(tài)誤差角已是小角度。為了獲取初始角度，采用基于慣性凝固思想的q-method優(yōu)化對準(zhǔn)方法將慣導(dǎo)系統(tǒng)對準(zhǔn)問題等價(jià)變換為無窮觀測向量下的姿態(tài)優(yōu)化問題，很好地解決了最優(yōu)濾波精對準(zhǔn)誤差模型非線性的問題。

設(shè)慣性空間固聯(lián)轉(zhuǎn)位機(jī)構(gòu)臺面坐標(biāo)系為p0，慣性空間固聯(lián)初始時(shí)刻地理坐標(biāo)系為n0，可將慣性系的比力方程改寫為

由于里程儀輸出的是里程增量信息，常規(guī)動(dòng)基座算法均對式（2）采取了雙重積分[3，8]。但是，這種方法淹沒了載體部分運(yùn)動(dòng)信息，只能適應(yīng)起始速度為零的動(dòng)基座對準(zhǔn)。若從非零速開始對準(zhǔn)，其收斂速度極慢，很多時(shí)候甚至不能滿足粗對準(zhǔn)的精度要求。因此，對式（2）兩邊只取一次積分，得到兩個(gè)矢量的表示形式為

式（3）中，vp為速度信息通過固定安裝關(guān)系轉(zhuǎn)換得到的捷聯(lián)慣組坐標(biāo)系的速度。vp（0）和vp（t）均采用滑動(dòng)平滑方式進(jìn)行求取，這種方式既避免了對里程增量求微分而導(dǎo)致的速度噪聲放大，又能實(shí)現(xiàn)從任意起始速度開始的行進(jìn)間對準(zhǔn)。載體速度信息輔助行進(jìn)間對準(zhǔn)的計(jì)算流程如圖4所示。

圖4 由載體速度信息輔助的旋轉(zhuǎn)式慣性系對準(zhǔn)算法Fig.4 Inertial freezing algorithm of vehicle-velocity aided rotatory IMA

利用多對觀測矢量，對準(zhǔn)問題實(shí)質(zhì)上可以轉(zhuǎn)換為利用導(dǎo)航參考系和載體系的觀測矢量來確定姿態(tài)的問題。解決這一類問題的算法在航天器飛行控制中有著很長的發(fā)展歷史，其主要目的是找出滿足如式（5）的正交姿態(tài)方向余弦矩陣A。

基于矢量觀測的定姿算法主要可分為兩類：確定性算法（一般是指TRIAD算法）和最優(yōu)定姿算法。其中，最優(yōu)定姿算法主要為基于Wahba問題的各類最小二乘意義的最優(yōu)算法，如奇異值分解算法（SVD）、 四元數(shù)算法（q-method）及其變形、 快速最優(yōu)姿態(tài)矩陣算法（FOAM）[9]等。由于各類算法種類繁多，綜合考慮精確度、魯棒性、實(shí)時(shí)遞推性和計(jì)算量等性能后，行進(jìn)間對準(zhǔn)選用了Davenport提出的q-method四元數(shù)方法。四元數(shù)方法可被簡單表述如下：令為各對矢量觀測對應(yīng)的非負(fù)權(quán)重，則可認(rèn)為式（6）的最大正特征值對應(yīng)的特征向量即為所求的最優(yōu)姿態(tài)四元數(shù)。

式（6）中，

3 回溯Kalman濾波精對準(zhǔn)

慣性系優(yōu)化對準(zhǔn)能解決快速粗略姿態(tài)的獲取問題，但其存在軌跡適應(yīng)性差、導(dǎo)航參數(shù)不全等弊端。同時(shí)，由于里程儀參數(shù)不能準(zhǔn)確已知且對準(zhǔn)算法未對其進(jìn)行估計(jì)，慣性系優(yōu)化對準(zhǔn)算法難以達(dá)到高精度水平。在已經(jīng)獲得粗略姿態(tài)角的條件下，Kalman濾波仍不失為一種優(yōu)先選擇的對準(zhǔn)算法。本文利用慣性系優(yōu)化對準(zhǔn)期間存儲的原始數(shù)據(jù)和獲取的初始姿態(tài)值回溯到對準(zhǔn)起始時(shí)刻重新開始Kalman濾波精對準(zhǔn)，同時(shí)對里程儀參數(shù)進(jìn)行估計(jì)。

綜合里程儀的俯仰、航向安裝偏角估計(jì)誤差δαθ、δαψ及標(biāo)度因數(shù)誤差Δfv，慣性自主導(dǎo)航系統(tǒng)的多信息融合濾波器可建立如下18維狀態(tài)量

式（8）中，δn=[δrEδrNδrU]為在n系中的計(jì)算位置誤差，δvn為地理系的地速誤差，φn為慣組系相對地理系的姿態(tài)角誤差，εp和Δp分別為p系的等效陀螺零偏和加速度計(jì)零偏，ξm=[δαθδαψΔfv]，δαθ為安裝俯仰偏差角的估計(jì)誤差，δαψ為安裝方位偏差角的估計(jì)誤差。

結(jié)合慣性導(dǎo)航系統(tǒng)通用的擾動(dòng)誤差模型，可將Kalman濾波系統(tǒng)方程表示為

為了避免因長時(shí)間導(dǎo)航慣性儀表零偏和里程儀參數(shù)估計(jì)精度出現(xiàn) “虛高”的現(xiàn)象，式（9）將慣性儀表零偏和里程儀參數(shù)誤差建模為一階Markov過程。其中，β為反相關(guān)時(shí)間常數(shù)，w為Markov過程的激勵(lì)噪聲，激勵(lì)噪聲均方差q=2βRx（0），Rx（0）為 Markov過程的自相關(guān)函數(shù)。分別為地理系相對ECEF系的旋轉(zhuǎn)速率、地理系相對慣性系的旋轉(zhuǎn)速率和地球自轉(zhuǎn)速率在地理系上的投影。

由于三軸慣性儀表在旋轉(zhuǎn)過程中與車體系的里程儀信息進(jìn)行了組合，因此其必須通過轉(zhuǎn)換才能進(jìn)行最優(yōu)融合。假設(shè)車體系m與捷聯(lián)慣組系b之間固定安裝偏角的估計(jì)誤差矢量為η=[δαθδαγδαψ]，而旋轉(zhuǎn)的轉(zhuǎn)位機(jī)構(gòu)臺面坐標(biāo)系p相對捷聯(lián)慣組系b的轉(zhuǎn)位角度可以通過光電編碼器實(shí)時(shí)讀取。為了確保測角信息與慣性儀信息的同步，測角信息也經(jīng)過了同等階數(shù)數(shù)字濾波器的轉(zhuǎn)換，具體轉(zhuǎn)換關(guān)系如式（1）所示。量測模型可以表示為

4 仿真及試驗(yàn)驗(yàn)證

4.1 仿真分析

針對實(shí)際作戰(zhàn)要求進(jìn)行仿真分析，設(shè)置陀螺的等效零偏為 0.005（°）/h（1σ），角隨機(jī)游走誤差為0.0008（°）/h1/2，加速度計(jì)的等效常值零偏為100μg（1σ），噪聲強(qiáng)度為 10μg，初始地理緯度、 經(jīng)度和高度分別為34°、108°、380m。里程儀標(biāo)度因數(shù)存在0.1%的初始誤差和隨機(jī)誤差，真實(shí)俯仰相對安裝角和航向相對安裝角分別為0.7°和0.2°。對 “捷聯(lián)式行進(jìn)間對準(zhǔn)”和 “正反旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)”兩種模式進(jìn)行仿真，其三軸慣性儀表的真實(shí)姿態(tài)如圖5和圖6所示。其中，θ、γ和ψ分別為轉(zhuǎn)位機(jī)構(gòu)臺面坐標(biāo)系p相對地理系n的俯仰角、橫滾角和航向角。從圖6可以看出，旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)姿態(tài)變化曲線相當(dāng)于在圖5捷聯(lián)式行進(jìn)間對準(zhǔn)姿態(tài)變化曲線的基礎(chǔ)上疊加了持續(xù)正反旋轉(zhuǎn)。

圖5 捷聯(lián)式行進(jìn)間對準(zhǔn)的三軸真實(shí)姿態(tài)曲線Fig.5 Attitude curves during strapdown IMA procedure

圖6 旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)的三軸真實(shí)姿態(tài)曲線Fig.6 Attitude curves during rotatory IMA procedure

捷聯(lián)式行進(jìn)間對準(zhǔn)和旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)的三軸姿態(tài)誤差收斂曲線如圖7和圖8所示。其中，δφx、δφy和δφz分別為俯仰角誤差、 橫滾角誤差和航向角誤差。為了對回溯Kalman濾波的優(yōu)勢進(jìn)行說明，圖中還比較了慣性系解析算法和回溯Kalman濾波算法的優(yōu)劣。

從圖7、圖8的仿真結(jié)果可以看出，回溯濾波算法明顯優(yōu)于慣性系解析算法，因?yàn)榛厮轂V波算法大幅加快了對準(zhǔn)的收斂速度。無論是捷聯(lián)式方案或是旋轉(zhuǎn)式方案，回溯濾波算法在900s時(shí)的對準(zhǔn)精度要優(yōu)于慣性系解析算法。

圖7 捷聯(lián)式行進(jìn)間對準(zhǔn)姿態(tài)誤差收斂曲線Fig.7 Attitude error curves during strapdown IMA procedure

根據(jù)文獻(xiàn)[10]中的可觀性分析，捷聯(lián)式慣導(dǎo)和里程計(jì)組合導(dǎo)航在直線行駛條件下的對準(zhǔn)極限精度近似為靜基座對準(zhǔn)精度，在轉(zhuǎn)彎機(jī)動(dòng)下定向極限精度近似為靜基座多位置對準(zhǔn)方位精度。因此，根據(jù)仿真條件，捷聯(lián)式行進(jìn)間對準(zhǔn)在車輛直線行駛時(shí)的航向?qū)?zhǔn)極限精度約為1.3′，仿真軌跡存在90°的轉(zhuǎn)彎機(jī)動(dòng)，航向精度略高于此。在圖7中，最終航向?qū)?zhǔn)精度約為1′。旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)近似對消了旋轉(zhuǎn)軸垂直方向的陀螺零偏，只殘留了旋轉(zhuǎn)軸陀螺零偏投影的等效東向陀螺零偏和隨機(jī)誤差，因此可以達(dá)到較高精度。在圖8中，最終航向?qū)?zhǔn)精度達(dá)到了0.5′。對兩種方案各進(jìn)行了10次仿真，其俯仰角、橫滾角和航向角的對準(zhǔn)結(jié)果如表1所示。表1中的航向精度統(tǒng)計(jì)結(jié)果是相對仿真俯仰角、橫滾角和航向角真值而言的，三者分別為-0.7°、 0.03°和 0.2°。 從表 1中的結(jié)果可以看出，捷聯(lián)式行進(jìn)間對準(zhǔn)航向精度為1.08′，旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)航向精度達(dá)到了0.54′（RMS），可明顯提高行進(jìn)間對準(zhǔn)精度。

圖8 旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)姿態(tài)誤差收斂曲線Fig.8 Attitude error curves during rotatory IMA procedure

表1 捷聯(lián)式和旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)仿真結(jié)果統(tǒng)計(jì)Table 1 Statistics of strapdown and rotatory IMA simulation

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

采用陀螺零偏穩(wěn)定性為 0.005（°）/h（1σ）、 加速度計(jì)零偏穩(wěn)定性為30μg（1σ）的樣機(jī)系統(tǒng)進(jìn)行了旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)試驗(yàn)，圖9為駐車對準(zhǔn)后純慣性導(dǎo)航姿態(tài)結(jié)果經(jīng)轉(zhuǎn)位測角轉(zhuǎn)換后的載體航向角變化曲線。旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)過程待轉(zhuǎn)位機(jī)構(gòu)停止轉(zhuǎn)動(dòng)后，停車記錄相應(yīng)的三軸姿態(tài)結(jié)果，并再次進(jìn)行駐車對準(zhǔn)，以之作為參考基準(zhǔn)，參考基準(zhǔn)的精度可以達(dá)到0.4′（RMS）。表2為相應(yīng)行進(jìn)間對準(zhǔn)結(jié)果。由表2可以看出，行進(jìn)間對準(zhǔn)精度能夠達(dá)到0.99′（RMS）。由于實(shí)際過程中載體水平姿態(tài)的變化，等效東向陀螺零偏并不能被抵消完全，同時(shí)由行駛路況等因素引起的里程儀標(biāo)度因數(shù)誤差同樣會(huì)使量測產(chǎn)生較大誤差，因此實(shí)際試驗(yàn)精度并沒有達(dá)到仿真精度。

圖9 旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)試驗(yàn)慣組載體航向變化曲線Fig.9 Heading curves of IMU during rotatory IMA test

表2 旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)試驗(yàn)統(tǒng)計(jì)結(jié)果Table 2 Statistics of rotatory IMA vehicular test

5 結(jié)論

為提高行進(jìn)間對準(zhǔn)的精度和軌跡適應(yīng)性，將旋轉(zhuǎn)調(diào)制原理引入了行進(jìn)間對準(zhǔn)，提出了 “正反旋轉(zhuǎn)+慣性系粗對準(zhǔn)+回溯Kalman濾波精對準(zhǔn)”的對準(zhǔn)方案，改進(jìn)常規(guī)慣性系行進(jìn)間對準(zhǔn)算法為基于平滑里程計(jì)微分速度的一次積分算法，同時(shí)給出了多源自主信息的時(shí)間同步措施。仿真和試驗(yàn)驗(yàn)證了提出的行進(jìn)間對準(zhǔn)方案，表明旋轉(zhuǎn)式行進(jìn)間對準(zhǔn)能較大幅度地提升對準(zhǔn)精度。