一種車載捷聯(lián)慣導行進間QUEST優(yōu)化粗對準算法

韓勇強，王新健，謝 玲，陳家斌

（北京理工大學，北京 100081）

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

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

1 坐標系定義

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

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

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

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

2 粗對準算法研究

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

圖1 粗對準算法原理圖Fig.1 Diagram of the coarse alignment algorithm

2.1 重力矢量信息的計算

基于時間更新過程，我們可以對姿態(tài)矩陣進行分解：

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

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

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

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

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

對等號兩邊同時進行積分

和分別是車體坐標系和導航坐標系的更新，它們可以通過式(6)的方法來進行確定。

其中，

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

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

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

（1）QUEST算法

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

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

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

其中，

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

其中，K為4×4矩陣

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

其中Y的定義為

對應特征向量為

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

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

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

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

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

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

粗對準過程中，不同時刻計算得到的Gn（tN）或Gb（tN）均具有不同程度的誤差，由式（10）可知，此時它們在算法中的權(quán)重ai應該是不同的。比較可靠的方法便是依靠“誤差”的大小來對ai進行取值。

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

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

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

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

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

遵循ei與誤差成正比的原則。然后對該計算方法進行分析驗證。

圖3、4為一次實驗的姿態(tài)、速度變化與ei值隨運動變化的情況。

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

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

圖4 值變化曲線Fig.4 Changing of

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

位置增量的計算遵循式（29）

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

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

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

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

3 實驗結(jié)果分析

3.1 仿真實驗

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

表1 線路方案Tab.1 Route plans

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

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

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

從線路1實驗結(jié)果可以看出，本文提出的動基座粗對準算法在靜基座條件下，粗對準航向角小于0.06 °，同樣保持了較高的精度。從線路2的實驗結(jié)果可以看出，當車載體運動時，粗對準結(jié)果略有下降，但仍然保持較高的精度水平。

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

圖5 航向角粗對準精度對比Fig.5 Comparison of yaw angle accuracy

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

3.2 車載實驗

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

圖6 車載實驗設備構(gòu)成Fig.6 Equipment arrangement in on-vehicleroad tests

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

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

圖7 實驗路線Fig.7 Road testroute

圖8 實際測試中航向角變化曲線Fig.8 Changing of yaw angle in road tests

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

表4 車載實驗粗對準算法對比（單位：°）Tab.4 Comparison of the coarse alignment algorithm in road tests(°)

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

4 總 結(jié)

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