基于UKF的大失準(zhǔn)角船舶行進間對準(zhǔn)技術(shù)研究

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

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

0 引言

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

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

1 SINS/GPS組合導(dǎo)航系統(tǒng)非線性誤差模型

1.1 SINS姿態(tài)誤差方程

(1)

其中：

(2)

(3)

1.2 SINS速度誤差方程

理想情況下：

(4)

實際系統(tǒng)中：

(5)

式中

(6)

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

(7)

1.3 SINS位置誤差方程

緯度誤差方程為：

(8)

經(jīng)度誤差方程為：

δLvetanLsecL/(RN+h)

(9)

1.4 桿臂誤差方程

假設(shè)GPS天線相對慣導(dǎo)位置在載體系下可以表示為：

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

(10)

由圖1可知，當(dāng)載體發(fā)生角運動時，2個測量點速度的關(guān)系為：

(11)

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

(12)

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

2 UKF算法

一般濾波模型描述如下

(13)

UKF采用與KF一樣的算法結(jié)構(gòu)，所不同的是，在KF中可直接通過線性方程確定狀態(tài)統(tǒng)計量的轉(zhuǎn)移，而UKF不行，后者必須借助UT計算采樣點來演化狀態(tài)分布特性的轉(zhuǎn)移[13-14]。

2.1 UT

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

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

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

yi=f(χi)

(14)

(15)

(16)

2.2 適用于SINS/GPS組合導(dǎo)航系統(tǒng)的UKF算法

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

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

則UKF的狀態(tài)方程可寫為如下的離散形式：

(17)

z(k)=Hx(k)

(18)

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

1)初始化狀態(tài)變量及其均方差

(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 仿真分析

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

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

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

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

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

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

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

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

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

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

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

3.2 海試試驗數(shù)據(jù)驗證

利用在青島某海域的SINS/GPS組合導(dǎo)航試驗數(shù)據(jù)對算法進行進一步的驗證。圖7展示了實驗用船與調(diào)試現(xiàn)場。

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

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

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

圖8 UKF與粗對準(zhǔn)+KF精對準(zhǔn)水平姿態(tài)角對比Fig.8 Horizontal attitude comparison of UKF and KF with coarse alignment

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

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

4 結(jié)論

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