韓勇強(qiáng)，王新健，謝 玲，陳家斌

（北京理工大學(xué)，北京 100081）

初始對(duì)準(zhǔn)一直是慣性導(dǎo)航系統(tǒng)極為重要的問(wèn)題，傳統(tǒng)的靜基座對(duì)準(zhǔn)多使用解析式粗對(duì)準(zhǔn)方法[1]，精度滿(mǎn)足使用要求，但對(duì)準(zhǔn)過(guò)程中載體處于靜止?fàn)顟B(tài)，缺乏機(jī)動(dòng)性。針對(duì)這一問(wèn)題，文獻(xiàn)[2-4]基于姿態(tài)矩陣分解的思想，將載體姿態(tài)分為三個(gè)獨(dú)立的部分，提出了計(jì)算初始姿態(tài)的關(guān)系式，但是未對(duì)如何計(jì)算姿態(tài)角進(jìn)行有效說(shuō)明。文獻(xiàn)[5,6]在粗對(duì)準(zhǔn)計(jì)算過(guò)程中使用雙矢量叉乘計(jì)算方法，沒(méi)有充分利用兩個(gè)積分時(shí)間段信息之間的關(guān)系，對(duì)信噪比較低的地球旋轉(zhuǎn)角速度依賴(lài)較高，對(duì)重力矢量信息的利用不足。文獻(xiàn)[7,8]分別使用卡爾曼濾波與航位推算方法，但簡(jiǎn)化了粗對(duì)準(zhǔn)過(guò)程，導(dǎo)致航向角收斂速度慢，精度受到限制。目前運(yùn)動(dòng)基座粗對(duì)準(zhǔn)多使用QUEST算法，該算法能夠克服上述缺點(diǎn)，具有適中的計(jì)算復(fù)雜程度。文獻(xiàn)[9,10]使用QUEST算法，實(shí)現(xiàn)了晃動(dòng)基座下的粗對(duì)準(zhǔn)，但其采用批處理方法，并未利用里程計(jì)對(duì)載體姿態(tài)進(jìn)行實(shí)時(shí)反饋，不利于嵌入式實(shí)現(xiàn)。同時(shí)，由于慣導(dǎo)及里程計(jì)均存在一定誤差，上述文獻(xiàn)均未對(duì)誤差進(jìn)行預(yù)處理，精度較差。

里程計(jì)自主性好，信號(hào)不易受干擾，標(biāo)定后與車(chē)體坐標(biāo)系重合，可以更直接地得到車(chē)體坐標(biāo)系下的速度，結(jié)合地理坐標(biāo)系下的重力矢量，可以方便地得到車(chē)體與地理坐標(biāo)系的轉(zhuǎn)換關(guān)系?；谝陨蠁?wèn)題，本文提出了一種基于里程計(jì)輔助的QUEST優(yōu)化算法，并經(jīng)過(guò)實(shí)驗(yàn)驗(yàn)證，實(shí)現(xiàn)了車(chē)載捷聯(lián)慣導(dǎo)行進(jìn)間粗對(duì)準(zhǔn)，減小了系統(tǒng)粗對(duì)準(zhǔn)誤差。

1 坐標(biāo)系定義

(1)e系為地心坐標(biāo)系，原點(diǎn)位于地心，z軸沿地球自轉(zhuǎn)方向，x軸位于赤道平面內(nèi)，從地心指向車(chē)體初始位置子午線(xiàn)，x軸、z軸與y軸構(gòu)成右手坐標(biāo)系。e系與地球固聯(lián)。

(2)i系為慣性系，與初始時(shí)刻e系指向相同，且不與地球固聯(lián)。

(3)b系為車(chē)體坐標(biāo)系，x、y、z軸等分別指向車(chē)體右、前、上方，且與車(chē)體固聯(lián)。

(4)n系為車(chē)體所在位置的當(dāng)?shù)氐乩碜鴺?biāo)系，x軸指向正東，y軸指向正北，z軸指向天。

2 粗對(duì)準(zhǔn)算法研究

初始對(duì)準(zhǔn)其實(shí)就是利用里程計(jì)和慣導(dǎo)的輸出，求解在初始狀態(tài)下b系與n系的轉(zhuǎn)換關(guān)系，即確定姿態(tài)矩陣Cbn。然后再將計(jì)算得到的姿態(tài)、速度、位置，反饋回算法當(dāng)中，從而實(shí)現(xiàn)車(chē)載慣導(dǎo)行進(jìn)間的粗對(duì)準(zhǔn)。算法原理圖如圖1所示。

圖1 粗對(duì)準(zhǔn)算法原理圖Fig.1 Diagram of the coarse alignment algorithm

2.1 重力矢量信息的計(jì)算

基于時(shí)間更新過(guò)程，我們可以對(duì)姿態(tài)矩陣進(jìn)行分解：

這樣姿態(tài)矩陣便被分成三部分，導(dǎo)航系的更新，車(chē)體坐標(biāo)系的更新以及初始姿態(tài)矩陣。這為我們后續(xù)的計(jì)算帶來(lái)方便。

其次,寫(xiě)出n系下捷聯(lián)慣導(dǎo)的速度比力方程：

其中，Vn為車(chē)體在導(dǎo)航坐標(biāo)系下的速度，Cbn為t時(shí)刻的姿態(tài)矩陣，fb為捷聯(lián)慣導(dǎo)測(cè)量到的比力，為n系下的地球自轉(zhuǎn)角速度，為n系下n系相對(duì)于e系的角速度，gn為n系下當(dāng)?shù)氐闹亓κ噶俊?/p>

式（2）經(jīng)過(guò)變換后可以寫(xiě)成

將式(1)代入式(3)并在方程左右兩側(cè)同時(shí)乘以，于是得到：

對(duì)等號(hào)兩邊同時(shí)進(jìn)行積分

和分別是車(chē)體坐標(biāo)系和導(dǎo)航坐標(biāo)系的更新，它們可以通過(guò)式(6)的方法來(lái)進(jìn)行確定。

其中，

其中，T為采樣時(shí)間間隔，tk=kT，k=0,1,2,…,N-1；ΔV1、ΔV2與 Δθ1、Δθ2分別為慣導(dǎo)在T時(shí)間間隔內(nèi)分兩次采到的速度與角度增量，的值可以通過(guò)里程計(jì)的輸出獲得。

2.2 優(yōu)化的QUEST算法

使用2.1所描述的方法之后，可以得到多個(gè)時(shí)刻的Gb（tN）與Gn（tN），計(jì)算初始姿態(tài)矩陣的過(guò)程其實(shí)就是尋找一個(gè)矩陣，使其可以盡量滿(mǎn)足式（7）的關(guān)系。常用的方法有正交向量計(jì)算法和最小二乘法。但經(jīng)過(guò)實(shí)驗(yàn)證明，正交向量計(jì)算法只利用最終時(shí)刻t以及t/2時(shí)刻的向量，信息利用不足，難以得到正確的姿態(tài)結(jié)果，最小二乘法得到的結(jié)果為非正交矩陣，需要多次正交化處理，且整個(gè)計(jì)算過(guò)程受隨機(jī)誤差的影響較大，有較大概率出現(xiàn)錯(cuò)誤的航向角結(jié)果，穩(wěn)定性較差。本文提出一種基于QUEST的優(yōu)化算法，利用重力矢量模值誤差與實(shí)時(shí)反饋的方法，實(shí)現(xiàn)了更加穩(wěn)定快速的粗對(duì)準(zhǔn)過(guò)程。

（1）QUEST算法

QUEST算法具有廣泛的應(yīng)用，其中一種便是應(yīng)用于姿態(tài)估計(jì)。假設(shè)矩陣A為我們要求取的矩陣，可以定義如下的最優(yōu)估計(jì)模型：

其中，與分別為同一變量在兩個(gè)不同觀(guān)測(cè)系下得到的值。ai為不同時(shí)刻信息的權(quán)重，視不同的應(yīng)用情況而定。那么增益函數(shù)g(A)可以定義為

當(dāng)L(A)達(dá)到極小，即g(A)達(dá)到極大時(shí)，矩陣A即為求得的最優(yōu)結(jié)果。把g(A)寫(xiě)成如式（12）形式。

其中，

將式（12）改為姿態(tài)四元數(shù)進(jìn)行求解，則增益函數(shù)可以重新表示為

其中，K為4×4矩陣

因此，想要取得極值需要滿(mǎn)足g'()=0，即是K的特征向量，且λ取得最大時(shí)，對(duì)應(yīng)的特征向量即為所求。因此，對(duì)于任何一個(gè)特征值λ，均有

其中Y的定義為

對(duì)應(yīng)特征向量為

當(dāng)λ取得最大值λmax時(shí)，Y與便可取得最佳姿態(tài)解Yopt與。由式(19)-(21)可以得到λ的表達(dá)式

其中a=σ2- tr(adjS)，b=σ2+ZTZ，c= detS+ZTSZ，d=ZTS2Z。然后利用牛頓-拉夫遜算法對(duì)λ進(jìn)行多次迭代求解，并將迭代的最終結(jié)果λmax代入到式（19）中，可以得到

其中X=（αI +βS+S2）Z。進(jìn)而可以得到最優(yōu)的姿態(tài)四元數(shù)結(jié)果

此時(shí)得到的四元數(shù)已無(wú)需進(jìn)行歸一化。再將結(jié)果轉(zhuǎn)換為姿態(tài)矩陣代入式（1），即可得到實(shí)時(shí)的姿態(tài)角。

（2）算法優(yōu)化

由（1）中的推導(dǎo)可知，只要觀(guān)察記錄同一個(gè)量在不同坐標(biāo)系下的投影，即可得到兩個(gè)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。因此我們用2.1節(jié)中的Gn（tN）與Gb（tN）分別代替QUEST算法中的與即可。

粗對(duì)準(zhǔn)過(guò)程中，不同時(shí)刻計(jì)算得到的Gn（tN）或Gb（tN）均具有不同程度的誤差，由式（10）可知，此時(shí)它們?cè)谒惴ㄖ械臋?quán)重ai應(yīng)該是不同的。比較可靠的方法便是依靠“誤差”的大小來(lái)對(duì)ai進(jìn)行取值。

通過(guò)觀(guān)察式（8）可以發(fā)現(xiàn)，式（8）的前半部分為t時(shí)間內(nèi)里程計(jì)輸出速度增量 ΔVod，理想情況下沒(méi)有重力矢量信息的參與。后半部分為慣導(dǎo)輸出的速度增量 ΔVimu，其中在豎直方向上的輸出是有重力矢量參與引起的。二者相減再進(jìn)行投影得到的便是重力矢量在b(0)坐標(biāo)系下引起的速度變化 ΔVb。其原理如圖2所示。

圖2 b系天向速度增量的計(jì)算Fig.2 Calculation of Speed increment in vertical direction in body Frame

而式（9）相對(duì)簡(jiǎn)單，即T時(shí)間內(nèi)重力矢量在n(0)坐標(biāo)系下引起的速度變化。雖然重力矢量在不同坐標(biāo)系下的投影不同，若忽略重力矢量的變化，其投影模值大小應(yīng)等于重力矢量的模值。

基于以上分析，選擇Gn（tN）、Gb（tN）與當(dāng)?shù)刂亓κ噶吭诓蓸娱g隔時(shí)間T內(nèi)積分模值的誤差作為參數(shù)，定義誤差函數(shù)

誤差函數(shù)F可以根據(jù)實(shí)際效果調(diào)整。根據(jù)以上分析，首先提出一種誤差計(jì)算方法：

遵循ei與誤差成正比的原則。然后對(duì)該計(jì)算方法進(jìn)行分析驗(yàn)證。

圖3、4為一次實(shí)驗(yàn)的姿態(tài)、速度變化與ei值隨運(yùn)動(dòng)變化的情況。

圖3 姿態(tài)、速度變化曲線(xiàn)Fig.3 Curves of attitude and speed changing

由圖3可以看出，在40 s左右時(shí)，水平姿態(tài)角變化較為劇烈，航向角稍有波動(dòng)，反映到圖4中即為ei值的增大，在110 s附近時(shí)，航向角變化劇烈，同樣使得ei值變大。55 s到75 s以及85 s到105 s期間，盡管水平速度大小不同，但ei值總體保持平穩(wěn)，僅在90 s處突然減速的瞬間產(chǎn)生了較高凸起。

圖4 值變化曲線(xiàn)Fig.4 Changing of

基于上述現(xiàn)象，再對(duì)式（8）（9）進(jìn)行分析。式（9）中，的更新僅基于，與慣導(dǎo)里程計(jì)均無(wú)關(guān)，只要得到當(dāng)前時(shí)刻的粗略姿態(tài)角結(jié)果，便可以結(jié)合里程計(jì)輸出的速度計(jì)算得到，再將其補(bǔ)償?shù)街小５挠?jì)算遵循公式(27)、(28)。

位置增量的計(jì)算遵循式（29）

其中,L為當(dāng)?shù)鼐暥?，RM與RN分別為子午圈和卯酉圈的主曲率半徑，h為車(chē)載體的高程。

而式（8）中，--車(chē)體坐標(biāo)系的更新依賴(lài)于陀螺儀的原始輸出值，未經(jīng)旋轉(zhuǎn)、劃槳效應(yīng)等補(bǔ)償，在姿態(tài)變化劇烈時(shí)會(huì)存在較大誤差，--車(chē)體速度由里程計(jì)測(cè)得，路況以及突然啟動(dòng)或停止時(shí)都會(huì)對(duì)測(cè)量值造成一定的影響。

由此可見(jiàn)，對(duì)于QUEST算法來(lái)說(shuō)，水平速度的大小相較于姿態(tài)角波動(dòng)而言對(duì)算法的穩(wěn)定性影響較小，但當(dāng)加速度較大，如突然啟動(dòng)或停止時(shí)同樣會(huì)造成較大影響。這與圖3，4的實(shí)際測(cè)試結(jié)果相吻合，證明ei值可以很好地反映各時(shí)刻信息的誤差大小。

根據(jù)ei大小可對(duì)信息的權(quán)重進(jìn)行取值。當(dāng)誤差較小時(shí)，ai值可按比例取為較大值或1，使得當(dāng)前時(shí)刻信息得到較完整保留。而當(dāng)某一時(shí)刻誤差很大時(shí)，ai值可以根據(jù)誤差大小取較小值或取為0，減弱當(dāng)前時(shí)刻信息對(duì)整個(gè)對(duì)準(zhǔn)過(guò)程的影響。經(jīng)實(shí)驗(yàn)驗(yàn)證，可有效減弱誤差較大信息的影響。

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

3.1 仿真實(shí)驗(yàn)

基于MATLAB平臺(tái)設(shè)計(jì)了兩條仿真線(xiàn)路，初始姿態(tài)為[4；0；98]（俯仰，橫滾，航向角/°），每條線(xiàn)路均進(jìn)行5組測(cè)試。線(xiàn)路方案以及陀螺、加表參數(shù)分別列在表1與表2中。

表1 線(xiàn)路方案Tab.1 Route plans

表2 陀螺儀和加速度計(jì)基本參數(shù)Tab.2 Basic parameters for gyroscopes and accelerometers

表3中分別給出了兩條線(xiàn)路的粗對(duì)準(zhǔn)姿態(tài)角誤差。

表3 粗對(duì)準(zhǔn)誤差（單位：°）Tab.3 Coarse alignment errors(°)

從線(xiàn)路1實(shí)驗(yàn)結(jié)果可以看出，本文提出的動(dòng)基座粗對(duì)準(zhǔn)算法在靜基座條件下，粗對(duì)準(zhǔn)航向角小于0.06 °，同樣保持了較高的精度。從線(xiàn)路2的實(shí)驗(yàn)結(jié)果可以看出，當(dāng)車(chē)載體運(yùn)動(dòng)時(shí)，粗對(duì)準(zhǔn)結(jié)果略有下降，但仍然保持較高的精度水平。

基于線(xiàn)路2，給出了使用未經(jīng)優(yōu)化的算法與優(yōu)化后算法的粗對(duì)準(zhǔn)效果對(duì)比,如圖5所示。

圖5 航向角粗對(duì)準(zhǔn)精度對(duì)比Fig.5 Comparison of yaw angle accuracy

圖5結(jié)果表明，仿真條件下，使用經(jīng)過(guò)優(yōu)化改進(jìn)后的算法，可以有效地提高粗對(duì)準(zhǔn)的精度。

3.2 車(chē)載實(shí)驗(yàn)

采用某激光陀螺雙軸轉(zhuǎn)位慣性導(dǎo)航系統(tǒng)，固定于車(chē)載體之上，配合已標(biāo)定好的里程計(jì)，進(jìn)行了四組車(chē)載實(shí)驗(yàn)。實(shí)驗(yàn)設(shè)備布局如圖6所示。

圖6 車(chē)載實(shí)驗(yàn)設(shè)備構(gòu)成Fig.6 Equipment arrangement in on-vehicleroad tests

實(shí)驗(yàn)路線(xiàn)如圖7所示。實(shí)驗(yàn)路線(xiàn)全長(zhǎng)約1.5 km，包含急轉(zhuǎn)彎，長(zhǎng)直路段。出發(fā)前首先靜止1分鐘，前半程位于校內(nèi)，速度較慢，60 s左右進(jìn)入公路（（-250，-60）坐標(biāo)處），開(kāi)始加速行駛。

跑車(chē)結(jié)束后進(jìn)行了靜基座對(duì)準(zhǔn)，得到一組實(shí)驗(yàn)車(chē)載體的姿態(tài)角[3.864;1.013;8.385]（單位：°）。圖6給出了一組實(shí)際測(cè)試中航向角收斂過(guò)程。

圖7 實(shí)驗(yàn)路線(xiàn)Fig.7 Road testroute

圖8 實(shí)際測(cè)試中航向角變化曲線(xiàn)Fig.8 Changing of yaw angle in road tests

從圖8中可以看出，在40 s到50 s轉(zhuǎn)彎處，85 s及105 s轉(zhuǎn)彎處，以及60 s進(jìn)入長(zhǎng)直路段加速處，本文中的優(yōu)化算法相對(duì)于傳統(tǒng)的QUEST算法，適應(yīng)性更高，能夠有效減少因誤差較大的信息造成的波動(dòng)，且粗對(duì)準(zhǔn)精度較高，表4中給出了全部四組車(chē)載實(shí)驗(yàn)的粗對(duì)準(zhǔn)算法對(duì)比情況。

表4 車(chē)載實(shí)驗(yàn)粗對(duì)準(zhǔn)算法對(duì)比（單位：°）Tab.4 Comparison of the coarse alignment algorithm in road tests(°)

從圖7，8和表4可以看出，在實(shí)際應(yīng)用中，該粗對(duì)準(zhǔn)算法對(duì)路況以及速度的變化有較強(qiáng)的適應(yīng)性，并且有效減弱了誤差較大信息的影響，保持較高的對(duì)準(zhǔn)精度，在150 s內(nèi)航向角誤差已經(jīng)縮小到 ± 2°范圍內(nèi)，完全滿(mǎn)足精對(duì)準(zhǔn)需求。證明該算法具有很好的實(shí)用性。

4 總 結(jié)

本文提出了一種基于里程計(jì)輔助的QUEST優(yōu)化算法。由于小量的忽略和誤差的累積，每個(gè)時(shí)刻的信息權(quán)重并不相同，因此采用了與重力矢量之間的模值誤差為權(quán)重參考，并進(jìn)行實(shí)時(shí)反饋修正，減小誤差影響。最后設(shè)計(jì)多次試驗(yàn)，證明了該算法的準(zhǔn)確性以及對(duì)于實(shí)際應(yīng)用中車(chē)載體運(yùn)動(dòng)的適應(yīng)性。從實(shí)驗(yàn)結(jié)果可以看出，該算法可以實(shí)現(xiàn)車(chē)載體行進(jìn)間的粗對(duì)準(zhǔn)，適應(yīng)性較強(qiáng)，能夠得到較高精度的姿態(tài)估計(jì)。