基于IEKF的SINS/USBL組合導(dǎo)航系統(tǒng)安裝偏差標(biāo)定算法

徐 博，趙曉偉，王連釗

（1.哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001；2.近地面探測技術(shù)重點實驗室，無錫 214035）

海洋中蘊(yùn)藏著豐富的戰(zhàn)略資源，水下自主潛航器（Autonomous Underwater Vehicle,AUV）具有高度自主性、便攜性和隱蔽性，目前廣泛應(yīng)用于海洋資源開發(fā)和水下目標(biāo)探測等工作中[1]。捷聯(lián)慣性導(dǎo)航和超短基線組合導(dǎo)航系統(tǒng)可為水下潛航器在水下工作提供姿態(tài)、速度和位置信息，但由于捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System,SINS）和超短基線（Ultra-short Baseline,USBL）設(shè)備存在安裝偏差，如果不加以補(bǔ)償將會影響組合導(dǎo)航精度，1 °的航向安裝偏差角將會造成約1.7%斜距的位置誤差，如果航向安裝偏差大將會危及水下潛航器的航行安全[2]。安裝偏差標(biāo)定技術(shù)是SINS/USBL組合導(dǎo)航系統(tǒng)關(guān)鍵技術(shù)之一，標(biāo)定精度的好壞直接影響到組合導(dǎo)航解算的精度。近年來，國內(nèi)外很多機(jī)構(gòu)學(xué)者圍繞著SINS/USBL組合導(dǎo)航系統(tǒng)的標(biāo)定開展了大量的研究。法國iXBlue、挪威Kongsberg、英國Sonardyne、美國LinkQuest等公司均有商用的深水超短基線定位系統(tǒng)，其中英國Sonardyne公司的Ranger2 Pro定位性能指標(biāo)已達(dá)到0.1%斜距，這些公司均有針對自己設(shè)備的標(biāo)定方案，一般是采用“8”字形標(biāo)定軌跡，但有關(guān)標(biāo)定算法的文獻(xiàn)資料卻很少。國內(nèi)張濤等提出了一種基于小角度近似的安裝偏差標(biāo)定方法，首先對安裝偏差角對應(yīng)的姿態(tài)矩陣進(jìn)行小角度近似，然后利用最小二乘算法對安裝偏差進(jìn)行標(biāo)定。該方法簡單易行，計算量小，但由于對矩陣進(jìn)行了近似，所以會帶來原理性誤差，特別是當(dāng)安裝偏差角較大時會帶來較大的標(biāo)定誤差[3]。在此基礎(chǔ)上，趙俊波等對最小二乘標(biāo)定算法進(jìn)行了改進(jìn)，同時提出了基于帶約束的最小二乘標(biāo)定算法，該算法具有一定的抗聲速測量誤差的能力[4]。童金武等提出了一種基于單應(yīng)答器雙矢量重構(gòu)的安裝偏差標(biāo)定算法，該算法可以有效避免安裝偏差的標(biāo)定值收斂于偽點，為確定SINS/USBL組合系統(tǒng)安裝偏差提供了新的理論依據(jù)[5]。王健等提出了一種基于相對量測信息濾波估計的安裝偏差標(biāo)定算法，將Huber M估計與變分貝葉斯相結(jié)合，有效解決了野值噪聲對標(biāo)定的影響[6]。徐曉蘇等為了進(jìn)一步提高SINS/USBL組合系統(tǒng)安裝偏差標(biāo)定的效率，同時增強(qiáng)標(biāo)定方法的適應(yīng)性，提出了一種交互多模型和無跡卡爾曼濾波器（IMM-UKF）輔助INS/USBL的標(biāo)定方案，克服了水下復(fù)雜環(huán)境下導(dǎo)致標(biāo)定精度下降的缺陷[7]。

為了對標(biāo)定算法進(jìn)行深入探索，本文針對姿態(tài)矩陣小角度近似時產(chǎn)生誤差的問題，提出了一種基于迭代擴(kuò)展卡爾曼濾波（Iterative Extended Kalman Filter,IEKF）的安裝偏差標(biāo)定算法，推導(dǎo)了非線性量測模型，并進(jìn)行了車載試驗驗證，試驗結(jié)果表明了所提算法的有效性。

1 超短基線系統(tǒng)定位原理

超短基線系統(tǒng)由水聽器和應(yīng)答器構(gòu)成，通過聲波信號往返于水聽器和應(yīng)答器間的傳輸時間和聲波速度獲得載體和水下應(yīng)答器間的斜距信息，并且根據(jù)各個水聽器接收聲波信號的相位差確定載體的姿態(tài)。超短基線系統(tǒng)具有體積小、精度高的優(yōu)點，被廣泛應(yīng)用在水下潛航器中[8]，圖1為超短基線系統(tǒng)的定位原理圖。兩條基線上安裝有四個水聽器，同一條基線上的兩個水聽器間的距離為d，水下應(yīng)答器的位置坐標(biāo)為T(xu,yu,zu)。

由超短基線系統(tǒng)定位原理圖和幾何原理可得[9]：

其中，r為超短基線基座到水下應(yīng)答器之間直線的距離，m為距離r在水平面oxy上的投影，r與x軸和y軸的夾角分別為xθ和yθ，m與x軸的夾角為θ。

設(shè)x軸兩個水聽器接收聲波的相位差為xφ，y軸兩個水聽器接收聲波的相位差為yφ，λ為聲波的波長，則有：

由幾何關(guān)系可得：

由式(1)-(3)可得：

其中，超短基線基座到水下應(yīng)答器之間直線的距離r可由聲波大小和往返時間確定，相位差xφ和yφ由超短基線量測系統(tǒng)內(nèi)部測量及計算得到。

2 SINS/USBL系統(tǒng)安裝偏差角標(biāo)定算法

2.1 坐標(biāo)系定義

（1）地球坐標(biāo)系（e系）：坐標(biāo)原點取在地球中心，oxe軸在赤道平面與載體當(dāng)?shù)刈游缑娴慕唤缇€上，oze沿極軸方向，oxe軸、oye軸和oze軸構(gòu)成右手坐標(biāo)系[10]。

（2）選取“東-北-天”坐標(biāo)系為導(dǎo)航坐標(biāo)系（n系）。

（3）定義“右-前-上”坐標(biāo)系為載體坐標(biāo)系（b系）。

（4）超短基線基座坐標(biāo)系（u系）：定義取短基線基座的中心為原點，xu軸、yu軸、zu軸分別指向載體的右前上，由于安裝偏差的存在，u系與載體坐標(biāo)系b系不重合。

2.2 安裝偏差角定義

載體坐標(biāo)系（b系）和超短基線基座坐標(biāo)系（u系）之間的轉(zhuǎn)換可以用三次旋轉(zhuǎn)得到的姿態(tài)旋轉(zhuǎn)矩陣表示[11]。

圖2所示為坐標(biāo)系變換圖，載體坐標(biāo)系首先繞zb軸逆時針旋轉(zhuǎn)ψ角，再繞xb

圖2 坐標(biāo)系變換圖Fig.2 Coordinate system transformation diagram

'軸逆時針旋轉(zhuǎn)θ角，最后繞軸逆時針旋轉(zhuǎn)γ角得到超短基線基座坐標(biāo)系。通過三次坐標(biāo)軸旋轉(zhuǎn)得到從b系到u系的姿態(tài)旋轉(zhuǎn)矩陣，表示為：

其中，θ為俯仰安裝偏差角，γ為橫滾安裝偏差角，ψ為航向安裝偏差角。

2.3 IEKF標(biāo)定算法模型

迭代擴(kuò)展卡爾曼濾波（Iterative Extended Kalman Filter,IEKF）是在擴(kuò)展卡爾曼濾波（Extended Kalman Filter,EKF）上發(fā)展而來的，適用于對非線性系統(tǒng)進(jìn)行濾波估計，并且IEKF對狀態(tài)估計值進(jìn)行了多次迭代，濾波的精度更高[12]。

對于狀態(tài)方程為線性，量測方程為非線性離散系統(tǒng)的狀態(tài)空間模型，如式(6)所示[13]：

IEKF濾波器的時間更新公式如式(8)所示：

IEKF濾波器的量測更新公式如式(9)所示：

3 車載半實物試驗與分析

3.1 半實物仿真條件

圖3為車載試驗組合導(dǎo)航設(shè)備圖，車載軌跡如圖4所示，繞著建筑樓宇進(jìn)行1500 s的車載試驗，全程進(jìn)行SINS/GPS組合導(dǎo)航，起始位置緯度為45.7703 °，經(jīng)度為126.6816 °，車載試驗高度視為0 m，終點位置緯度為45.7693 °，經(jīng)度為126.6828 °。USBL系統(tǒng)輸出的數(shù)據(jù)均由仿真生成，水下應(yīng)答器的位置坐標(biāo)設(shè)定為（45.7703 °，126.6816 °，-500 m），超短基線系統(tǒng)輸出的位置坐標(biāo)矢量由PHINS/GPS組合導(dǎo)航輸出位置坐標(biāo)與水下應(yīng)答器的位置坐標(biāo)分別投影到u系并相減構(gòu)成。

圖3 車載半實物仿真試驗設(shè)備圖Fig.3 Equipment diagram of on-board hardware in the loop simulation experiment

圖4 車載半實物仿真試驗軌跡圖Fig.4 Trajectory diagram of on-board hardware in the loop simulation experiment

3.2 單次仿真試驗

進(jìn)行單次離線仿真試驗，設(shè)置安裝偏差角θ、γ、ψ分別為5 °、5 °、40 °。為了驗證基于IEKF標(biāo)定算法的有效性和優(yōu)越性，同時進(jìn)行了基于小角度近似原理的遞推最小二乘（Recursive Least Squares,RLS）標(biāo)定算法和擴(kuò)展卡爾曼濾波（EKF）標(biāo)定算法，設(shè)置IEKF的最大迭代次數(shù)為10次，設(shè)置閾值為10-10rad，當(dāng)前后兩次狀態(tài)估計值之差的模長小于閾值或者達(dá)到最大迭代次數(shù)就結(jié)束當(dāng)次迭代。三種算法的安裝偏差角標(biāo)定結(jié)果如圖5-7所示。

圖5為基于小角度近似原理的RLS標(biāo)定算法，θ、γ、ψ標(biāo)定結(jié)果分別為5.4479 °、1.8140 °、37.0705 °。圖6為基于EKF的標(biāo)定算法，θ、γ、ψ標(biāo)定結(jié)果分別為4.8021 °、4.9113 °、39.7024 °。圖7為基于本文所提的IEKF標(biāo)定算法，θ、γ、ψ標(biāo)定結(jié)果分別為4.8961 °、4.9635 °、39.8601 °。從標(biāo)定結(jié)果可以看出基于IEKF的標(biāo)定算法精度高于其他兩種標(biāo)定方法，由于RLS標(biāo)定算法對矩陣進(jìn)行了小角度近似，故其標(biāo)定精度最低，標(biāo)定結(jié)果統(tǒng)計數(shù)據(jù)如表1所示。

圖5 RLS方法標(biāo)定結(jié)果Fig.5 RLS method calibration result

圖6 EKF方法標(biāo)定結(jié)果Fig.6 EKF method calibration result

圖7 IEKF方法標(biāo)定結(jié)果Fig.7 IEKF method calibration result

表1 三種算法標(biāo)定結(jié)果Tab.1 Calibration results of three algorithms

三種標(biāo)定算法對安裝偏差角的標(biāo)定誤差曲線如圖8-10所示，從標(biāo)定誤差曲線可以看出，IEKF標(biāo)定算法精度高于EKF算法和RLS算法。

圖8 三種算法的俯仰安裝偏差角標(biāo)定誤差曲線Fig.8 Calibration error curve of pitch installation deviation angle based on three algorithms

圖9 三種算法的橫滾安裝偏差角標(biāo)定誤差曲線Fig.9 Calibration error curve of roll installation deviation angle based on three algorithms

圖10 三種算法的航向安裝偏差角標(biāo)定誤差曲線Fig.10 Calibration error curve of heading installation deviation angle based on three algorithms

標(biāo)定誤差結(jié)果統(tǒng)計數(shù)據(jù)如表2所示，IEKF標(biāo)定算法相比RLS算法和EKF算法俯仰安裝偏差角標(biāo)定精度分別提高7.56%和1.96%；橫滾安裝偏差角標(biāo)定精度分別提高173.62%和1.06%；航向安裝偏差角標(biāo)定精度分別提高7.52%和0.39%。

表2 三種算法標(biāo)定結(jié)果誤差Tab.2 Error of calibration results of three algorithms

3.3 蒙特卡羅仿真試驗

為了進(jìn)一步證明所提標(biāo)定算法的有效性和優(yōu)越性，利用車載組合導(dǎo)航數(shù)據(jù)進(jìn)行50次蒙特卡羅半實物仿真試驗，每次仿真設(shè)置的安裝偏差角在單次仿真試驗設(shè)置的基礎(chǔ)上加上方差大小為3 °的隨機(jī)值，50次蒙特卡羅仿真誤差結(jié)果如圖11-14所示。

圖11 RLS方法標(biāo)定誤差曲線Fig.11 RLS method calibration error curve

圖12 EKF方法標(biāo)定誤差曲線Fig.12 EKF method calibration error curve

圖13 IEKF方法標(biāo)定誤差曲線Fig.13 IEKF method calibration error curve

圖11-13為三種標(biāo)定算法的50次蒙特卡羅標(biāo)定誤差曲線，其中紅色的粗虛線為50次標(biāo)定的誤差均值，誤差均值統(tǒng)計參數(shù)如表3所示，誤差標(biāo)準(zhǔn)差統(tǒng)計值如表4所示。圖14為三種算法的安裝偏差角標(biāo)定誤差均值曲線，可以看出IEKF標(biāo)定算法可以對三個安裝偏差角進(jìn)行有效標(biāo)定，并且對于三個安裝偏差角的標(biāo)定誤差和標(biāo)定誤差標(biāo)準(zhǔn)差均小于其他兩種方法，IEKF標(biāo)定算法相比RLS算法和EKF算法俯仰安裝偏差角標(biāo)定精度分別提高16.44%和2.60%；橫滾安裝偏差角標(biāo)定精度分別提高157.69%和1.17%；航向安裝偏差角標(biāo)定精度分別提高7.50%和0.41%，從標(biāo)定精度看IEKF標(biāo)定算法對大安裝偏差角也具有較好的估計效果。

圖14 三種算法標(biāo)定誤差均值曲線Fig.14 Calibration error mean curve of three algorithms

表3 三種算法標(biāo)定結(jié)果誤差均值Tab.3 Error mean of calibration results of three algorithms

表4 三種算法標(biāo)定結(jié)果誤差標(biāo)準(zhǔn)差Tab.4 Standard deviation of calibration results of three algorithms

4 結(jié)論

SINS/USBL組合導(dǎo)航系統(tǒng)的安裝偏差角的標(biāo)定結(jié)果對于水下潛航器的導(dǎo)航定位至關(guān)重要，為了更加準(zhǔn)確地得到標(biāo)定結(jié)果，本文所提方法首先根據(jù)安裝偏差角的定義和超短基線定位原理建立非線性量測模型，然后基于IEKF濾波算法對安裝偏差角進(jìn)行濾波估計，最后進(jìn)行了車載試驗，并與其他兩種標(biāo)定算法進(jìn)行對比，對比試驗結(jié)果表明：基于IEKF的標(biāo)定算法可以有效地對SINS/USBL組合系統(tǒng)的安裝偏差進(jìn)行估計，并且具有較高的標(biāo)定精度和穩(wěn)定性。