GPS輔助的SINS系統快速動基座初始對準

王 偉，劉 萌，薛 冰

(哈爾濱工程大學 自動化學院,哈爾濱 150001)

捷聯式慣性導航系統(strapdown inertial navigation system, SINS)根據陀螺儀輸出的角速率和加速度計輸出的比力可以計算出載體的姿態、速度和位置，被廣泛應用于各種軍用和民用行業.SINS系統在進行導航前需要進行初始對準，初始對準的精度直接影響SINS系統的導航精度.因此，精確的初始對準對SINS系統尤為重要.

對于高精度SINS系統，系統可以在靜基座環境下通過感應地球自轉和重力加速度確定出初始姿態，系統的位置和速度可以通過GPS系統得到.但是在一些緊急情況下，SINS系統需要在運動過程中進行初始對準，傳統的靜基座對準算法就不再適用.對于低精度的SINS系統，例如MEMS-IMU，系統本身的噪聲大于地球自轉角速率，同樣無法進行靜基座對準，需要在外部傳感器輔助的情況下通過載體的運動進行初始對準.

近年來，研究人員對動基座初始對準進行了研究，提出了許多動基座對準算法.文獻[1]對空中對準算法進行了研究，算法利用加速度計的輸出計算初始水平姿態，利用GPS航跡向作為初始航向角，進行粗對準，然后利用自適應擴展卡爾曼濾波進行精對準，文章中只考慮了大方位失準角下的誤差模型，但是利用加速度計得到的水平姿態可能會存在較大誤差.文獻[2]中算法則不用考慮初始姿態信息，使用OBA算法實現空中對準.通過引入初始導航慣性坐標系和初始載體慣性坐標系，將姿態矩陣拆分成3部分，分別為導航系與初始導航慣性系之間的方向余弦矩陣、載體系與初始載體慣性系之間的方向余弦矩陣和初始導航系與初始載體系之間的姿態矩陣.其中，前兩個矩陣是時變矩陣，可以通過GPS數據和慣導數據對其進行計算，第3個矩陣為常值矩陣，利用基于優化的四元數法可以實現對常值矩陣的計算.該算法雖然能夠實現快速對準，但算法沒有考慮慣性器件誤差帶來的影響.文獻[3]對文獻[2]中的方法進行了進一步的研究和詳細的推導，分別對速度積分和位置積分的效果進行了比較，但是文獻[3]指出，該算法只適用于高精度的慣導系統.文獻[3]中的算法只是單純的對姿態進行估計，沒有對慣性器件的誤差進行補償，文獻[4]針對這一問題進行研究，使用迭代牛頓拉格朗日算法進行解算，對GPS桿臂誤差和慣性器件的誤差進行估計，然而算法依然只適用于高精度的慣導系統.文獻[5-6]在假設水平姿態失準角為小角度的情況下，將方位失準角的三角函數作為狀態變量，這樣系統模型便轉化成線性，減小計算量，但是在對準速度上并沒有明顯的提高.文獻[7]對所有姿態角均為大失準角的情況進行了非線性誤差方程的推導，并根據量測方程為線性的特性，將UKF算法進行簡化，并證明了算法簡化的可行性，減小了UKF算法的計算量.文獻[8]用四元數定義姿態誤差，對四元數形式的非線性誤差方程進行推導，用UKF算法對系統進行濾波，可以實現大失準角的動基座對準，即使是90°的大方位失準角算法也可以收斂，但是算法中沒有考慮UKF采樣對四元數帶來的影響.文獻[9]將OBA算法與卡爾曼濾波結合，利用里程計作為輔助傳感器對陀螺儀誤差引起的失準角進行估計，并將估計結果反饋給OBA算法，從而提高對準精度，算法中雖然考慮了慣性器件誤差的影響，但是沒有對其進行估計，需要根據經驗手動的對陀螺儀誤差進行設置.文獻[10]針對OBA算法會累積慣性器件誤差的問題提出滑動窗積分計算的方法，防止誤差的累積，但是該方法需要對窗口內的數據進行存儲和積分運算，計算量會隨著窗口的增大而增大.文獻[11]中，根據飛機起飛前的運動特點建立了飛機的運動約束模型，從而達到在不增加其他傳感器的情況下增加量測信息的效果，進一步提高動基座對準的速度和精度，但是這種方法只適用于飛機在跑道上的動基座對準，有一定的局限性.文獻[12]使用IIR濾波器去除加速度計中的高頻噪聲，對提高對準精度有一定的幫助，但是沒能從根本上解決慣性器件誤差帶來的問題.

本文在OBA算法的基礎上，推導了陀螺儀誤差與載體系和計算載體系的失準角之間的關系，并根據此關系建立非線性誤差方程，然后利用UKF算法對失準角和陀螺儀常值漂移進行估計，將估計結果反饋給OBA算法，從而提高對準的精度，使算法可應用于低精度的慣導系統.考慮到量測噪聲的不確定性，引入自適應濾波算法對量測噪聲進行估計，提高系統的穩定性.

1 OBA動基座對準算法

1.1 姿態矩陣分解

(1)

(2)

(3)

1.2 速度積分計算

在不考慮誤差的情況下，比力方程可表示為

(4)

(5)

(6)

對式(6)兩邊同時積分并整理可得：

(7)

其中：

(8)

(9)

OBA算法在計算方式上完全解析，不需要像傳統的卡爾曼濾波一樣將部分變量用上一時刻的代替，但是OBA算法沒有考慮慣性器件誤差帶來的影響，因此OBA算法更適合用在高精度SINS系統中，當用在低精度SINS系統中時，該算法的可靠性就無法得到保障.本文接下來將對SINS/GPS組合系統的誤差進行分析，對SINS系統的誤差方程做一個新的推導，使其能夠與OBA算法結合，然后建立一種新的非線性系統模型，利用自適應無跡卡爾曼濾波算法對失準角和陀螺儀誤差進行估計，從而在保證算法對準速度的同時提高對準的精度.因為算法中考慮了陀螺儀的誤差影響，所以算法可以用在低精度的SINS系統中.

2 系統的誤差分析及建模

2.1 系統誤差分析

(10)

(11)

式中：式(10)中的φn可由式(11)計算得到，矢量φn為導航系由tk-1時刻到tk時刻的等效旋轉矢量；‖φn‖為矢量φn的模.

(12)

(13)

(14)

式中：式(13)中的φb可由式(14)計算得到，矢量φb為載體系由tk-1時刻到tk時刻的等效旋轉矢量，其計算方式采用雙子樣計算；Δθ1、Δθ2為相鄰兩時刻的陀螺儀角增量.

(15)

(16)

將式(16)帶入式(15)可得：

(17)

對式(17)兩端求導可得：

(18)

(19)

(20)

將式(3)、(15)、(16)、(17)、(19)、(20)帶入式(18)可得：

(21)

(22)

整理式(22)可得：

(23)

(A1×)(A2×)-(A2×)(A1×)=(A1×A2)×.

(24)

則式(23)可根據式(24)簡化為

(25)

式(25)進一步化簡有

(26)

(27)

式(27)為載體系與計算載體系之間的誤差方程，該誤差方程反映了陀螺儀誤差對SINS系統姿態估計精度的影響，通過式(27)可以對SINS系統進行建模，從而估計出SINS系統的失準角和陀螺儀誤差，提高算法的對準精度和適用范圍.

2.2 SINS/GPS系統建模

2.2.1 系統狀態方程

根據式(27)，選取等效旋轉矢量φ和陀螺儀常值漂移εb作為狀態變量X，即

(28)

(29)

式中Wk是均值為零、協方差為Qk的系統白噪聲.

2.2.2 系統量測方程

在tk時刻，式(8)可表示為

(30)

式(30)繼續變換可得：

(31)

(32)

根據式(31)、(32)，式(7)可表示為

(33)

(34)

(35)

對式(35)化簡并整理得

(36)

將系統的量測方程表示為：

Zk=HkXk+Vk,

(37)

(38)

(39)

式(36)、(37)相對應，其對應關系如式(38)、(39)所示.量測方程中的Vk是均值為零的量測白噪聲，其協方差記為Rk.

3 自適應無跡卡爾曼濾波算法

由于系統的狀態方程是非線性的，所以普通卡爾曼濾波不適合本文中的系統.無跡卡爾曼濾波在UT變換的基礎上，根據KF濾波算法的原理，使用一系列的采樣點進行采樣計算來逼近系統的非線性分布，從而減小系統線性化帶來的誤差，提高濾波的精度[13].所以本文中選擇UKF算法對狀態變量進行濾波估計.

濾波過程中使用式(28)作為濾波的狀態變量，使用式(29)和式(38)作為UKF濾波時的狀態方程和量測方程.根據式(29)和式(37)可以看出，系統的狀態方程為非線性，量測方程為線性，所以在使用UKF算法時可以對量測更新部分做進一步簡化，使用線性卡爾曼濾波的方式進行量測更新，從而減少采樣次數和計算量.簡化后的量測更新如下：

在進行UKF算法濾波時，量測噪聲方差矩陣R設置的不準確會導致系統濾波的不穩定.在本文中，量測方程是由速度、比力等的積分構成的，很難通過手動調試將量測噪聲方差矩陣設置準確.針對這一問題，本文引入Sage-Husa自適應濾波算法對量測噪聲方差矩陣R進行實時估計，從而提高系統的濾波精度和穩定性.對量測噪聲方差矩陣R的估計公式如下：

4 算法的計算機仿真

4.1 仿真軌跡設置

在生成仿真軌跡時主要是對SINS系統中陀螺儀和加速度計的參數、GPS系統的參數、SINS系統的初始條件和SINS系統的運動狀態進行設置.

1)仿真參數設置為：陀螺儀的常值漂移和隨機噪聲分別為1.0°/h和0.1°/h，加速度計的常值零偏和隨機噪聲分別為100 ug和50 ug，陀螺儀和加速度計的輸出頻率均為100 Hz；GPS系統的速度誤差為0.05 m/s，位置誤差為5 m，GPS系統的輸出頻率為10 Hz.

2)SINS系統的初始狀態設置為：SINS系統的初始位置為北緯45°和東經126°，初始姿態為航向角45°、俯仰角0°和橫滾角0°，初始速度為東向速度5 m/s、北向速度5 m/s和天向速度0 m/s.

3)SINS系統的狀態變化為：載體勻速運動50 s→左轉彎90°，用時5 s→勻速運動50 s→右轉彎90°，用時5 s→勻速運動50 s→左轉彎90°，用時5 s→勻速運動50 s→右轉彎90°，用時5 s→勻速運動50 s，總的運動時長為270 s.圖1為載體運動軌跡變化，圖2為載體的航向角變化.

圖1 仿真運動軌跡

圖2 航向角變化

4.2 仿真結果

使用設計好的運動軌跡對所提出的快速動態對準算法(FIMA)以及現有的算法進行仿真對比.圖3為仿真過程中OBA算法與所提出的FIMA算法的姿態誤差對比圖，由圖2中效果可以看出，使用所提出的FIMA算法航向角誤差δYaw在80 s可以收斂到0.2°，并且航向角誤差能夠始終保持在0.2°以內，而OBA算法雖然也可以快速收斂，但是其航向角誤差會隨著對準時間的延長而變大.圖中兩算法的俯仰角誤差δPitch和橫滾角誤差δRoll均能快速收斂，且收斂的精度相差不大，但是FIMA算法的水平姿態誤差更穩定.圖中黑色箭頭所指部分紅色和藍色凸起是由于參考姿態與算法解算姿態頻率不一致引起的.

圖3 與OBA算法的姿態誤差對比圖

圖4為FIMA算法與文獻[1]中的EKF算法的姿態誤差對比圖，因為FIMA算法針對的是任意初始姿態，所以為了對比算法的效果，在仿真時EKF算法的姿態初始誤差設置為航向角誤差50°、俯仰角誤差15°和橫滾角誤差15°的大失準角誤差.由圖中效果可以看出，與非線性EKF動態對準算法相比，當姿態初始值存在大失準角時，FIMA算法的航向角對準速度更快，水平姿態的對準效果更穩定.

圖4 與EKF算法的姿態誤差對比圖

圖5是FIMA算法與EKF算法關于陀螺儀零偏估計的對比圖，EKF算法的仿真條件與姿態誤差對比的仿真條件相同.圖5中，黑色虛線是仿真軌跡中加入的陀螺儀常值漂移，大小為1 °/h，圖中的零偏估計效果顯示，FIMA算法對x軸和y軸的陀螺儀零偏估計精度高于EKF算法，而兩種算法對z軸陀螺儀零偏的估計都不太準確，這一現象與仿真軌跡運動方式的設計過于單一有關.圖5的仿真效果說明，FIMA算法可以實現對陀螺儀零偏的估計.

圖5 與EKF算法的陀螺儀零偏估計對比

在本文的算法中，由于量測方程均有積分項構成，所以量測噪聲難以確定，因此本文中FIMA算法引入自適應濾波，對量測噪聲進行實時估計.圖6為FIMA算法在加入自適應算法之前與正常的FIMA算法的仿真對比.對比結果表明，加入自適應的FIMA算法姿態精度更高，估計結果也更穩定.

圖6 與非自適應FIMA算法的姿態誤差對比

5 跑車實驗驗證

5.1 跑車實驗設備

本文中算法的計算機仿真說明了算法的有效性，為了進一步驗證算法的可行性，本文使用實際的實驗數據進行算法驗證.圖7為本次跑車實驗所用設備.圖中，序號1是UPS電源，為整套實驗設備供電；序號2是穩壓直流電源，為SINS系統供電；序號3是MEMS-IMU，其型號為ADIS16488；序號4為自制的高精度光纖慣導系統，可提供高精度的姿態參考；序號5為GPS天線，GPS接收機使用的是Trimble公司的BD982.實驗中采集MEMS-IMU的陀螺儀和加速度計的數據進行算法驗證，其中陀螺儀的偏置穩定度為5.1 °/h，加速度計的偏置穩定度為70 ug.

圖7 跑車實驗設備

5.2 結果及分析

本次跑車實驗在哈爾濱工程大學校內進行，汽車的運動軌跡如圖8中紅線所示.汽車在180 s之前靜止不動，180 s之后開始運動，運動過程中的姿態變化如圖9所示.

圖8 汽車運動軌跡

圖9 SINS系統姿態變化

為了驗證算法對陀螺儀誤差引起的姿態誤差的補償效果，本文先對文獻[3]中的OBA算法和本文中的FIMA算法進行跑車實驗比較，兩種算法均是在MATLAB軟件中實現的.在FIMA算法中，狀態變量和相應的誤差方差矩陣初始值分別設置為X0=06×1和P0=diag([(0.1)11×3(10°/h)11×3])2，其中11×3為元素全為1的一行三列矢量矩陣，算法中的系統噪聲和量測噪聲根據所用傳感器分別設置為Qk=Ts×diag([(5.1°/h)11×3(0)11×3])2×2和R0=diag([(10×π/180)11×3])2，Ts為IMU的采樣頻率，因為算法中用的是雙子樣進行的解算，所以離散化得到的Qk需要乘2，量測噪聲Rk只需要對初始值R0進行設置，之后則根據自適應算法進行實時估計.

圖10是本次跑車實驗中FIMA算法與OBA算法解算的姿態誤差的對比，汽車是在180 s之后開始運動，兩種算法均是在193 s即汽車運動13 s后開始運行.由圖中姿態誤差可以看出，對于低精度的SINS系統，OBA算法雖然能夠快速的確定出姿態角，但是姿態角并沒有隨著時間的延長而趨于穩定，且航向角誤差會越來越大；本文中的FIMA則克服了OBA算法的這一缺點，能夠保證算法在低精度的SINS系統中依然能夠穩定的收斂.

圖10 與OBA算法實測姿態誤差對比

圖11為FIMA算法姿態誤差放大圖，由圖中航向角誤差可以知道，在沒有任何初始姿態信息的條件下，算法用15 s的時間便可將航向角誤差收斂到3°以內，俯仰角誤差和橫滾角誤差收斂到0.4°以內.在FIMA對準算法運行190 s以后，航向角誤差可收斂到1°以內.圖12為FIMA算法對陀螺儀零偏的估計結果，估計結果表明，FIMA算法能夠實現對陀螺儀零偏的估計.為證明零偏估計的有效性，將FIMA算法估計的陀螺儀零偏寫入純慣導算法中進行補償，補償后的效果如圖13中所示.經過陀螺儀誤差補償后的純慣導算法的航向角誤差明顯好于補償前的效果，陀螺儀誤差的補償對水平姿態誤差影響不大，由此可以看出，FIMA算法對陀螺儀零偏的估計是有效的.

圖11 FIMA算法姿態誤差

圖12 FIMA算法陀螺儀零偏估計

圖13 純慣導算法姿態誤差對比

為了體現本文中算法在任意失準角條件下快速對準的優越性，本文對FIMA算法與文獻[1]中EKF對準算法用跑車實測數據進行比較.在EKF對準算法運行前，設置航向角誤差70°、俯仰角誤差和橫滾角誤差20°的大失準角誤差.兩種算法的姿態誤差對比如圖14所示，由于大失準角的原因，EKF算法的航向誤差收斂速度明顯比FIMA算法慢，EKF算法的水平姿態誤差在對準的前期也不如FIMA算法的穩定.

圖14 與EKF算法實測姿態誤差對比

圖15 與其他動基座對準算法的實測姿態誤差對比

圖16 陀螺儀零偏估計對比

分別為本文中的FIMA算法與文獻[14-15]中動基座對準算法的姿態誤差和陀螺儀零偏估計的對比圖.由姿態誤差圖可以看出，本文中的FIMA算法的姿態對準精度優于文獻[14-15]中的算法.在陀螺儀零偏估計對比圖中，FIMA算法對陀螺儀零偏的估計更加穩定和接近真實值，所以FIMA算法的水平姿態誤差相對于其他算法更加穩定.

6 結 論

1)本文在推導了載體系與計算載體系之間失準角更新方程的基礎上對SINS系統重新建立非線性系統方程，使用自適應UKF濾波算法對失準角和陀螺儀誤差進行估計，同時使用OBA算法中的最小二乘原理對初始姿態矩陣進行估計，從而實現快速動基座初始對準.

2)計算機仿真和跑車實驗表明: 對于低精度SINS系統，FIMA算法可在15 s左右將航向角誤差收斂至3°以內，俯仰角和橫滾角誤差收斂至0.4°以內；當算法運行至3 min后，航向角誤差可收斂至1°.

3)FIMA對準算法可以在無任何初始姿態信息的條件下實現快速動基座對準，并且能夠有效的估計出陀螺儀誤差，可適用于低精度的SINS系統，對準精度優于現有的GPS輔助的SINS系統OBA對準算法，對準速度比非線性EKF動態對準算法更快.