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

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

0 引言

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

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

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

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

(1)

其中：

(2)

(3)

1.2 SINS速度誤差方程

理想情況下：

(4)

實(shí)際系統(tǒng)中：

(5)

式中

(6)

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

(7)

1.3 SINS位置誤差方程

緯度誤差方程為：

(8)

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

δLvetanLsecL/(RN+h)

(9)

1.4 桿臂誤差方程

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

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

(10)

由圖1可知，當(dāng)載體發(fā)生角運(yùn)動(dòng)時(shí)，2個(gè)測(cè)量點(diǎn)速度的關(guān)系為：

(11)

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

(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)計(jì)量的轉(zhuǎn)移，而UKF不行，后者必須借助UT計(jì)算采樣點(diǎn)來演化狀態(tài)分布特性的轉(zhuǎn)移[13-14]。

2.1 UT

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

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

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

yi=f(χi)

(14)

(15)

(16)

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

為了減少計(jì)算量和保證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)方程為非線性的，而觀測(cè)方程為線性的，所以可以對(duì)觀測(cè)方程的更新進(jìn)行化簡(jiǎn)。按對(duì)稱采樣方法選取2n+1個(gè)Sigma點(diǎn)近似系統(tǒng)狀態(tài)的先驗(yàn)統(tǒng)計(jì)特性[15-16]。

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

(19)

2)時(shí)間更新

(20)

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

(21)

(22)

(23)

(24)

3)量測(cè)更新

Pzz=HPk+1|kHT+R

(25)

Pxz=Pk+1|kHT

(26)

(27)

(28)

(29)

(30)

λ=α2(n+κ)-n

(31)

(32)

(33)

(34)

(35)

3 試驗(yàn)與分析

3.1 仿真分析

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

圖2 仿真運(yùn)動(dòng)軌跡Fig.2 Simulation trajectory

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

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

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

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

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

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

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

圖5 桿臂估計(jì)值Fig.5 Estimated value of lever arm

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

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

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

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

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

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

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

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

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

4 結(jié)論

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