徐 博，趙曉偉，王連釗

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

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

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

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

超短基線系統(tǒng)由水聽(tīng)器和應(yīng)答器構(gòu)成，通過(guò)聲波信號(hào)往返于水聽(tīng)器和應(yīng)答器間的傳輸時(shí)間和聲波速度獲得載體和水下應(yīng)答器間的斜距信息，并且根據(jù)各個(gè)水聽(tīng)器接收聲波信號(hào)的相位差確定載體的姿態(tài)。超短基線系統(tǒng)具有體積小、精度高的優(yōu)點(diǎn)，被廣泛應(yīng)用在水下潛航器中[8]，圖1為超短基線系統(tǒng)的定位原理圖。兩條基線上安裝有四個(gè)水聽(tīng)器，同一條基線上的兩個(gè)水聽(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軸兩個(gè)水聽(tīng)器接收聲波的相位差為xφ，y軸兩個(gè)水聽(tīng)器接收聲波的相位差為yφ，λ為聲波的波長(zhǎng)，則有：

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

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

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

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

2.1 坐標(biāo)系定義

（1）地球坐標(biāo)系（e系）：坐標(biāo)原點(diǎn)取在地球中心，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系）：定義取短基線基座的中心為原點(diǎn)，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軸逆時(shí)針旋轉(zhuǎn)ψ角，再繞xb

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

'軸逆時(shí)針旋轉(zhuǎn)θ角，最后繞軸逆時(shí)針旋轉(zhuǎn)γ角得到超短基線基座坐標(biāo)系。通過(guò)三次坐標(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ā)展而來(lái)的，適用于對(duì)非線性系統(tǒng)進(jìn)行濾波估計(jì)，并且IEKF對(duì)狀態(tài)估計(jì)值進(jìn)行了多次迭代，濾波的精度更高[12]。

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

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

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

3 車載半實(shí)物試驗(yàn)與分析

3.1 半實(shí)物仿真條件

圖3為車載試驗(yàn)組合導(dǎo)航設(shè)備圖，車載軌跡如圖4所示，繞著建筑樓宇進(jìn)行1500 s的車載試驗(yàn)，全程進(jìn)行SINS/GPS組合導(dǎo)航，起始位置緯度為45.7703 °，經(jīng)度為126.6816 °，車載試驗(yàn)高度視為0 m，終點(diǎn)位置緯度為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í)物仿真試驗(yàn)設(shè)備圖Fig.3 Equipment diagram of on-board hardware in the loop simulation experiment

圖4 車載半實(shí)物仿真試驗(yàn)軌跡圖Fig.4 Trajectory diagram of on-board hardware in the loop simulation experiment

3.2 單次仿真試驗(yàn)

進(jìn)行單次離線仿真試驗(yàn)，設(shè)置安裝偏差角θ、γ、ψ分別為5 °、5 °、40 °。為了驗(yàn)證基于IEKF標(biāo)定算法的有效性和優(yōu)越性，同時(shí)進(jìn)行了基于小角度近似原理的遞推最小二乘（Recursive Least Squares,RLS）標(biāo)定算法和擴(kuò)展卡爾曼濾波（EKF）標(biāo)定算法，設(shè)置IEKF的最大迭代次數(shù)為10次，設(shè)置閾值為10-10rad，當(dāng)前后兩次狀態(tài)估計(jì)值之差的模長(zhǎng)小于閾值或者達(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)定算法對(duì)矩陣進(jìn)行了小角度近似，故其標(biāo)定精度最低，標(biāo)定結(jié)果統(tǒng)計(jì)數(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)定算法對(duì)安裝偏差角的標(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)計(jì)數(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 蒙特卡羅仿真試驗(yàn)

為了進(jìn)一步證明所提標(biāo)定算法的有效性和優(yōu)越性，利用車載組合導(dǎo)航數(shù)據(jù)進(jìn)行50次蒙特卡羅半實(shí)物仿真試驗(yàn)，每次仿真設(shè)置的安裝偏差角在單次仿真試驗(yàn)設(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)計(jì)參數(shù)如表3所示，誤差標(biāo)準(zhǔn)差統(tǒng)計(jì)值如表4所示。圖14為三種算法的安裝偏差角標(biāo)定誤差均值曲線，可以看出IEKF標(biāo)定算法可以對(duì)三個(gè)安裝偏差角進(jìn)行有效標(biāo)定，并且對(duì)于三個(gè)安裝偏差角的標(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)定算法對(duì)大安裝偏差角也具有較好的估計(jì)效果。

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