基于UKF的大失準角船舶行進間對準技術研究

裴新凱 ，張吉先 ，郭玉勝 ，丁海旭 ，莊廣琛 ，鄧 亮

(1.北京自動化控制設備研究所，北京 100074；2.昆明海威機電技術研究所，昆明 650200)

0 引言

捷聯慣導系統(Strapdown inertial navigation system，SINS)具有完全自主性的優點，但誤差隨時間積累,而全球定位系統(Global Positioning System，GPS)誤差不隨時間積累,但不具備自主性，二者優勢互補。因此，常構造SINS/GPS作為組合導航系統，其中初始對準是保證導航精度的關鍵技術。目前，誤差傳播模型和濾波算法是研究捷聯慣導系統初始對準的2個重要問題，而描述SINS的誤差模型本質上是非線性的，標準卡爾曼濾波(Kalman Filter，KF)只能有效解決小失準角下的初始對準問題，工程上通常先用雙矢量定姿原理進行粗對準[1-2]，在小角度下應用標準卡爾曼濾波。本文提出了應用無跡卡爾曼濾波(Unscented Kalman Filter，UKF)的方法，在未知角度的前提下直接進行濾波，估計出滿足導航精度的初始姿態信息。

UKF和標準卡爾曼濾波都屬于線性最小方差估計，算法都是基于模型的[3-5]；區別在于標準卡爾曼濾波的均值和方差具有線性傳遞性，可以通過遞推直接進行計算，但只適用于系統方程和量測方程均為線性的情況。盡管擴展卡爾曼濾波(Extended Kalman Filter，EKF)可解決系統和量測為非線性時的估計問題，但必須對原系統和量測作泰勒級數展開，并保留線性項，本質上與KF沒有區別，因此，EKF只適用于弱非線性對象的估計[6]。對于捷聯慣導系統擁有維數較高的狀態變量，通過線性化求解雅格比矩陣比較復雜，而UKF通過無損變換(Unscented Transform，UT)，舍棄近似非線性函數轉而近似其函數的概率密度分布，可以有效解決大失準角條件下的估計問題。本文通過建立捷聯慣導系統的非線性誤差方程[4,7-8]，設計UKF濾波器，以GPS的速度信息為觀測量，實現大失準角條件下的行進間初始對準。同時由于GPS天線的安裝位置與慣導位置間存在桿臂誤差，桿臂越長、轉彎角速度越大，速度測量誤差就越大，進而影響對準精度。本文對載車的桿臂效應作了簡要分析后，建立了包含桿臂參數的UKF模型，以實時地對桿臂大小進行估計[9-11],最后通過實驗驗證了算法的有效性。

1 SINS/GPS組合導航系統非線性誤差模型

1.1 SINS姿態誤差方程

(1)

其中：

(2)

(3)

1.2 SINS速度誤差方程

理想情況下：

(4)

實際系統中：

(5)

式中

(6)

式(4)和式(5)作差得：

(7)

1.3 SINS位置誤差方程

緯度誤差方程為：

(8)

經度誤差方程為：

δLvetanLsecL/(RN+h)

(9)

1.4 桿臂誤差方程

假設GPS天線相對慣導位置在載體系下可以表示為：

δl=[δlx,δly,δlz]T

(10)

由圖1可知，當載體發生角運動時，2個測量點速度的關系為：

(11)

式中，右邊第二項就是由桿臂引起的速度誤差，其中：

(12)

圖1 IMU安裝位置與天線安裝位置關系Fig.1 Relative position of IMU and antenna installation

2 UKF算法

一般濾波模型描述如下

(13)

UKF采用與KF一樣的算法結構，所不同的是，在KF中可直接通過線性方程確定狀態統計量的轉移，而UKF不行，后者必須借助UT計算采樣點來演化狀態分布特性的轉移[13-14]。

2.1 UT

UT以兩條準則為基礎：1)對單一點執行非線性變換是容易的(相比與整個概率分布函數)；2)在狀態空間中不難找到一組點，利用這些點可以近似狀態向量的真實概率分布函數。

一般意義下的UT算法框架如下：

2)對采樣的輸入變量Sigma點集{χi}中的每個Sigma點進行f(·)非線性變換，得到變換后的Sigma點集{yi}。

yi=f(χi)

(14)

(15)

(16)

2.2 適用于SINS/GPS組合導航系統的UKF算法

為了減少計算量和保證UKF的穩定性，在大失準角的條件下并未將慣性器件誤差納入濾波方程，因此，濾波狀態為

x=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δlx,δly,δlz]

則UKF的狀態方程可寫為如下的離散形式：

(17)

z(k)=Hx(k)

(18)

由此可見，系統的狀態方程為非線性的，而觀測方程為線性的，所以可以對觀測方程的更新進行化簡。按對稱采樣方法選取2n+1個Sigma點近似系統狀態的先驗統計特性[15-16]。

1)初始化狀態變量及其均方差

(19)

2)時間更新

(20)

χi,k+1|k=f(χi,k)

(21)

(22)

(23)

(24)

3)量測更新

Pzz=HPk+1|kHT+R

(25)

Pxz=Pk+1|kHT

(26)

(27)

(28)

(29)

(30)

λ=α2(n+κ)-n

(31)

(32)

(33)

(34)

(35)

3 試驗與分析

3.1 仿真分析

首先設計軌跡發生器，運行軌跡如圖2所示，初始速度為2m/s，仿真時間773s。初始失準角為俯仰角10°，滾轉角10°，航向角80°。采樣周期為10ms，陀螺漂移0.01(°)/h，加表零位50μg。

圖2 仿真運動軌跡Fig.2 Simulation trajectory

桿臂設置為[2m,3m,5m]，在50s以后船體向左轉彎90°，250s后再向右轉彎90°，轉速均為10(°)/s，在500s之后船體有3次滾轉的機動，轉速為10(°)/s。

分別進行沒有桿臂補償和有桿臂補償的UKF對準仿真，對二者進行比較，由圖3和圖4可以看出，利用UKF進行行進間初始對準，可以有效地估計出誤差失準角。通過對桿臂進行建模，在UKF過程中對參考速度誤差進行分解，可以有效地解決桿臂誤差造成的影響。

圖3 有無桿臂模型的天向失準角對比Fig.3 Comparison of the upside misalignment angles with or without the lever arm model

圖4 有無桿臂模型的水平失準角對比Fig.4 Comparison of the horizontal misalignment angles with or without the lever arm model

如表1所示，在對準結束時刻，對桿臂誤差進行建模后的初始對準航向精度要比沒有對桿臂建模的航向精度高出1個數量級。

表1 補償桿臂前后的失準角對比Tab.1 Comparison of misalignment angle with or without lever arm compensation

從圖5可以看出，該方法能夠估計出3個方向上的桿臂誤差，在2次轉彎過程中，水平方向的桿臂誤差均有明顯修正效果，通過P陣的曲線變化也可以得到同樣的結論，如圖6所示。該仿真結果表明，利用UKF可以解決導航初始時刻的大失準角問題，同時補償了桿臂誤差導致的對準精度低的問題。

圖5 桿臂估計值Fig.5 Estimated value of lever arm

圖6 桿臂狀態的P陣曲線Fig.6 P-array curve of the lever arm state

3.2 海試試驗數據驗證

利用在青島某海域的SINS/GPS組合導航試驗數據對算法進行進一步的驗證。圖7展示了實驗用船與調試現場。

圖7 船載試驗用船及各設備安裝圖Fig.7 Shipboard test ship and equipment installation

海試過程為粗對準5min，然后船舶進行轉彎運動精對準30min，最后轉組合導航。為了便于進行基于慣性系的粗對準和UKF對準算法的比較，截取精對準之后時刻的IMU和GPS數據，分別用UKF和基于慣性系算法進行動基座初始對準。因為此時彈上數據已經經過濾波修正，因此可以當作基準。從圖8、圖9可以看出，二者的水平姿態角對準結果基本相同，根據表2可以看出，補償了桿臂的UKF算法航向角收斂速度較快，航向精度也較高，200s時的航向對準精度優于傳統對準算法，可達到0.03°。圖10展示了當粗對準結束后轉KF經過幾次航向機動后的濾波效果，UKF的主要目的是降低失準角誤差，可以看出當失準角比較小以后，采用經典卡爾曼濾波更加有效。

表2 航向角誤差值對比Tab.2 Comparison of heading angle error

圖8 UKF與粗對準+KF精對準水平姿態角對比Fig.8 Horizontal attitude comparison of UKF and KF with coarse alignment

圖9 UKF與粗對準+KF精對準航向角對比Fig.9 Heading angle comparison of UKF and KF with coarse alignment

圖10 UKF與KF精對準航向角對比Fig.10 Heading angle comparison of UKF and KF alignment

4 結論

針對船舶動基座過程中的大失準角初始對準問題，提出了一種簡化的UKF算法，通過對GPS桿臂進行建模，相當于對不準確的參考速度信息進行補償。通過仿真分析和海試試驗結果表明，基于UKF的行進間對準算法可以有效地解決組合導航過程中初始大失準角的問題，并且精度優于傳統粗對準方法。后續將對UT作進一步分析，簡化狀態方程中線性部分的計算方法，提高計算效率，進而對狀態方程進行擴展，提高計算精度。