周亦軍 鄭 彤

（海軍駐四三八廠軍事代表室 武漢 430064）

1 引言

海底地形輔助導(dǎo)航系統(tǒng)［1～7］是近幾十年出現(xiàn)的一種新型的導(dǎo)航系統(tǒng)，是水下運(yùn)動(dòng)載體導(dǎo)航技術(shù)的一個(gè)發(fā)展方向。利用海底地形進(jìn)行導(dǎo)航的就是將水下載體實(shí)際測量的水深值與已存在的地圖進(jìn)行匹配，對水下載體進(jìn)行定位，基本原理［1～3］如圖1所示。在海底地形輔助導(dǎo)航系統(tǒng)中，精確的數(shù)字地圖和合適的匹配算法是核心技術(shù)，這也是當(dāng)前相關(guān)領(lǐng)域的重點(diǎn)和熱點(diǎn)研究問題。

圖1 水下地形輔助導(dǎo)航基本原理

文獻(xiàn)［8～14］研究了SITAN（Sandia Intertial Terrain－Aided Navigation）算法和TERCOM算法（Terrain Contour Matching）在空中載體中的應(yīng)用，其在各種地形和不同覆蓋層上空均顯出了自主、可靠的隱蔽特性。但受INS精度、數(shù)字地形圖的精度、雷達(dá)高度表測量精度和匹配算法的影響較大。近年來使用比較多的是ICCP算法（Iterative Closest Contour Point）［15～21］，它 是 ICP 算 法 （Iterative Closest Point）的一個(gè)特例。

多波束測深系統(tǒng)［22］，是在回聲測深儀的基礎(chǔ)上發(fā)展起來的一項(xiàng)海底地形精密勘測技術(shù)，是近幾十年來海洋測繪界進(jìn)行海底地形條帶式探測的熱點(diǎn)。本文利用的海洋測繪多波束測量試驗(yàn)測得的真實(shí)地形數(shù)據(jù)來建立的真實(shí)地形數(shù)據(jù)庫，并在此基礎(chǔ)上，通過仿真研究水下載體在航行過程中，采用ICCP算法修正慣性導(dǎo)航系統(tǒng)累計(jì)誤差，提高水下載體的導(dǎo)航精度，為水下載體實(shí)現(xiàn)水下、長期、精確導(dǎo)航提供技術(shù)支撐。

2 匹配算法

ICCP算法最初來源于圖像配準(zhǔn)的ICP算法，不需事前確定對應(yīng)估計(jì)，只是不斷重復(fù)（初始）運(yùn)動(dòng)變換—確定最近點(diǎn)—求運(yùn)動(dòng)變換的過程，逐步改進(jìn)運(yùn)動(dòng)的估計(jì)。算法的主要思路是：將測得的沿著航跡的水深值連接起來構(gòu)成曲線，與已存在的水深等值線圖進(jìn)行匹配。主要是通過最近點(diǎn)的迭代實(shí)現(xiàn)測量圖像與模型之間的對準(zhǔn)和匹配。算法采用歐氏距離平方最小為目標(biāo)函數(shù)，求得測量航跡與真實(shí)航跡之間的最優(yōu)變換，通過該變換校正航跡，實(shí)現(xiàn)對測量航跡的校正，如圖2所示。

圖2中由Pi（i＝1，2，…，N）點(diǎn)構(gòu)成的是“指示航跡”，N為航跡點(diǎn)序號(hào)。彎曲實(shí)線稱為“實(shí)際航跡”，由Xi（i＝1，2，…，N）構(gòu)成，Ci（i＝1，2，…，N）表示的是當(dāng)?shù)貙?shí)測水深等值線。由于慣導(dǎo)誤差的存在，｛Pi：i＝1，…，N｝和｛Xi：i＝1，…，N｝之間必定存在誤差。Xi必定位于Ci的等值線上，那么按照一定的準(zhǔn)則，使Pi靠攏到Ci上，從而找到最優(yōu)估計(jì)點(diǎn)Yi和對應(yīng)的“匹配航跡”，實(shí)現(xiàn)對真實(shí)航跡的最優(yōu)估計(jì)。

圖2 ICCP算法原理示意圖

對于ICCP算法的匹配過程可解釋為：從已知點(diǎn)p0出發(fā)，沿真實(shí)航跡航行，并不斷測量水深。在實(shí)時(shí)測深傳感器沒有測量誤差的情況下，這些測量點(diǎn)一定落在對應(yīng)的等值線上。由于有測量誤差，測量航跡給出的位置一定偏離對應(yīng)等值線。分別設(shè)指示航跡位置點(diǎn)集合為｛Pn｝，對應(yīng)的實(shí)際航跡點(diǎn)集合為｛Xn｝，為了求得真實(shí)位置，將測量航跡與真實(shí)航跡匹配，需要確定剛性變換T（包括一個(gè)旋轉(zhuǎn)分量和二個(gè)平移分量），通過剛性變換T使集合｛Pn｝與集合｛Xn｝的距離最小。由于集合｛Xn｝一定在等值線上，只是無法確定在等值線上的確切位置，因而在最優(yōu)化目標(biāo)函數(shù)的基礎(chǔ)上采用迭代方法尋找剛性變換T，使式（1）表示的距離最小。

式中，T表示變換，包括旋轉(zhuǎn)和平移。

3 慣導(dǎo)誤差方程與卡爾曼濾波

以平臺(tái)式慣導(dǎo)為例，系統(tǒng)誤差狀態(tài)方程［23～24］為

式中 X（t）＝［φx，φy，φz，δVx，δVy，δφ，δλ，ΔAx，ΔAy，εx，εy，εz］為慣性導(dǎo)航系統(tǒng)的12維誤差狀態(tài)矢量，F(xiàn)（t）為狀態(tài)矩陣；W（t）為12維系統(tǒng)噪聲向量；φx，φy，φz，δVx，δVy，δφ，δλ分別為平臺(tái)誤差角、速度和位置誤差；ΔAx，ΔAy，εx，εy，εz分別為加速度計(jì)誤差和陀螺漂移。

在重力輔助慣性導(dǎo)航系統(tǒng)中，由匹配算法得到最佳匹配位置后，利用其與慣導(dǎo)指示位置的誤差作為觀測量，其量測方程可表示為

4 獲得真實(shí)地形數(shù)據(jù)

進(jìn)行船測試驗(yàn)，利用多波束測深系統(tǒng)測量真實(shí)的地形數(shù)據(jù)。多波束測深系統(tǒng)是采用發(fā)射、接收指向性正交的兩組換能器陣獲得一系列垂直航向分布的窄波束。如圖3所示波束數(shù)為16，波束角為2°×2°的單方面換能器多波束系統(tǒng)。圖3中系統(tǒng)聲信號(hào)的發(fā)射和接收由方向垂直的發(fā)射陣和接收陣組成。發(fā)射陣平行船縱向排列，呈兩側(cè)堆成向下方發(fā)射2°（沿船縱向）×44°（沿船橫向）的扇形脈沖聲波。接收陣沿船橫向排列，但在束控方向上接收方式與發(fā)射方式正好相反，以20°（沿船縱向）×2°（沿船橫向）的16個(gè)接收波束角接收來自海底照射面積為發(fā)射2°（沿船縱向）×44°（沿船橫向）的扇區(qū)回波。接收的回波經(jīng)過原始的編輯后，使用專業(yè)軟件CARIS HIPS and SIPS進(jìn)行處理（如圖4所示），就可以得到真實(shí)的地形數(shù)據(jù)了，但為保密起見，本文中涉及的地形圖都將經(jīng)緯度隱去。

圖3 多波束測深系統(tǒng)示意圖

5 仿真分析

考慮水下載體以某一航跡航行，水下載體實(shí)際航跡為以某一角度自西向東航行。圖4所示載體的真實(shí)航跡，指示航跡以及匹配航跡。如圖所示匹配航跡跟真實(shí)航跡形狀是一致的，說明將ICCP算法用于水下地形匹配能取得比較滿意的匹配結(jié)果。然后利用最佳匹配位置對慣導(dǎo)的誤差進(jìn)行修正，以位置誤差作為觀測量，用卡爾曼濾波對導(dǎo)航系統(tǒng)的誤差進(jìn)行最優(yōu)估計(jì)，將最優(yōu)誤差估計(jì)值去校正導(dǎo)航系統(tǒng)。

圖4 地形匹配輔助導(dǎo)航

圖5 東向水平誤差角估計(jì)

設(shè)系統(tǒng)狀態(tài)X的初始值X（0）為0，等效陀螺漂移為0.11°／h，加速度計(jì)的初始偏差均取為1×10－4g，慣導(dǎo)平臺(tái)的初始誤差角為φx0＝300″、φy0＝300″、φx0＝1500″，慣導(dǎo)初始定位誤差在北向及東向均為500m，初始速度誤差為1.8m／s，水下潛器航速為8節(jié)，考慮到潛器航速、地圖分辨率以及航跡采樣點(diǎn)數(shù)，濾波周期選為250s。在以上仿真條件下得到的慣導(dǎo)各誤差估計(jì)值如圖5～圖7所示。

圖5、圖6為平臺(tái)水平誤差角估計(jì)，圖7為方位誤差角估計(jì)，三種誤差的最終收斂值如表1所示。由圖和表可以看出：用ICCP算法計(jì)算出的匹配位置誤差作為量測值進(jìn)行卡爾曼濾波，可有效估計(jì)慣性導(dǎo)航系統(tǒng)的誤差，進(jìn)而慣導(dǎo)系統(tǒng)的速度、位置、平臺(tái)誤差角都能很好地被修正。

圖6 北向水平誤差角估計(jì)

圖7 水平誤差角估計(jì)

表1 平臺(tái)誤差角穩(wěn)態(tài)值

6 結(jié)語

地形輔助導(dǎo)航是導(dǎo)航技術(shù)研究的一個(gè)新方向，該技術(shù)具有諸多優(yōu)點(diǎn)。本文采用的是ICCP算法作為數(shù)據(jù)處理的主要方法。并利用多波束測深系統(tǒng)測得的真實(shí)地形數(shù)據(jù)，建立的地形數(shù)據(jù)庫，在此基礎(chǔ)上，用該算法進(jìn)行了仿真計(jì)算，并得到了比較滿意的匹配結(jié)果。該算法的主要優(yōu)點(diǎn)在于它不需要對數(shù)字地圖的數(shù)據(jù)做預(yù)處理或進(jìn)行特征提取等操作，算法原理簡單。為了進(jìn)一步驗(yàn)證所得匹配位置是否適合用來修正慣導(dǎo)的積累誤差，將匹配位置誤差作為觀測量，用卡爾曼濾波對慣導(dǎo)系統(tǒng)誤差進(jìn)行最優(yōu)估計(jì)，由最后的仿真結(jié)果可以看出，以匹配位置誤差作為觀測量可較好的估計(jì)慣導(dǎo)系統(tǒng)各項(xiàng)誤差。

［1］王英鈞.地形輔助導(dǎo)航綜述［J］.航空電子技術(shù)，1998（1）：24－29.

［2］L.Lucido，B.P.Popesque，J.Opderbecke，et al.Segmentation of Bathymetric Profiles and Terrian Matching for Underwater Vehicle Navigation［J］.Internation Journal of Systems Science，1998，29（10）：1157－1176.

［3］劉光軍，袁書明，黃詠梅.海底地形匹配技術(shù)研究［J］.中國慣性學(xué)報(bào)，1999（1）：19－22.

［4］吳鳳德.美國戰(zhàn)略潛艇導(dǎo)航的現(xiàn)行技術(shù)和新技術(shù)綜述［M］.吳祖銓譯.艦船導(dǎo)航，1999，5.

［5］劉光軍，陳晶.海底地形輔助導(dǎo)航系統(tǒng)仿真技術(shù)研究［J］.計(jì)算機(jī)仿真，2000，17（2）：21－24.

［6］張飛舟，侶文芳，晏磊，等.水下無源導(dǎo)航系統(tǒng)仿真匹配算法研究［J］.武漢大學(xué)學(xué)報(bào)信息科學(xué)版，2003，28（2）：153－571.

［7］劉準(zhǔn)，佀文芳，陳哲.海底地形匹配技術(shù)研究［J］.系統(tǒng)仿真學(xué)報(bào)，2004，16（4）：700－703.

［8］Hollowell J.Heli／SITAN：a terrain referenced navigation algorithm for helicopters［C］／／IEEE Position，Location，and NAVIGATION Symposium 1990（PLANS’90），Las Vegas，NV，USA，March 20－23，1990：616－625.

［9］Pei Y.，Che Z.，Hung J.C.BITAN－II：an improved terrain aided navigation algorithm［J］.IECON Proceeding，1996（3）：1675－1680.

［10］Russell E.，Darryl M.Terrain－aided navigation using the Viterbi algorithm［J］.Joural of Guidance，Control and Dynamics，1995，43（9）：1444－1449.

［11］Feng Q.T.，Shen L.C.，Chang W.S.，et al.An HMM based terrain elevation matching algorithm［J］.Acta Automatica Sinica，2005，31（6）：960－964.

［12］Golden J.P.Terrain contour matching（TERCOM）：a cruise missile guidance aid［C］／／Proceedings of the Society of Photo－Optical Instrumentation Engineers，San Diego，CA，USA，July 20－August 1，1980，238：10－18.

［13］朱華勇，沈林成，常文森.基于地形差分矩的TERCOM地圖性能估計(jì)［J］.國防科技大學(xué)學(xué)報(bào)，2000，22（4）：98－101.

［14］徐克虎，沈春林.地形輔助慣性導(dǎo)航系統(tǒng)研究［J］.南京航空航天大學(xué)學(xué)報(bào)，1997，29（3）：289－294.

［15］Behzad K.P.，Behrooz K.P.Vehicle localization on gravity maps［J］.Proceedings of SPIE－The International Society for Optical Engineering，1999，3693：182－191.

［16］Maurer C.，Aboutanos G.，Dawant B.，et al.Registration of 3－D image using weighted geometrical features［J］.IEEE Tansactions on Medical Imageing，1996，15（6）：836－849.

［17］Cordon O.，Damas S.Image restration with iterated local search［J］.Journal of Heuristics，2006，12（1－2）：73－94.

［18］Madhavan R.，Hong T.，Messina E.Temporal range registration for unmanned ground and aerial vehicles［C］／／IEEE International Conference on Robotics and Automation，new Orleans，LA，USA，April 26－May 1，2004：3180－3187.

［19］于秋則，程輝，田金文，等.基于等高線圖與小波變換的3D地形匹配算法研究［J］.宇航學(xué)報(bào)，2004，25（3）：262－268.

［20］Madhavan R.，Durrant－Whyte H.F.2Dmap－building and localization in outdoor environments［J］.Journal of Robotic Systems，2005，22（1）：45－63.

［21］Bishop G.C.Gravatational field maps and navigational erros［J］.IEEE Journal of Oceanic Engineering，2002，27（3）：726－737.

［22］劉經(jīng)南，趙建虎.多波束測深系統(tǒng)的現(xiàn)狀和發(fā)展趨勢［J］.海洋測繪，2002，22（5）：3－6.

［23］董緒榮，張守信，華中春.GPS／INS組合導(dǎo)航定位及其應(yīng)用［M］.長沙：國防科技大學(xué)出版社，1998.

［24］許麗佳，陳陽舟，崔平遠(yuǎn).GPS／INS組合導(dǎo)航系統(tǒng)中的信息融合算法研究［J］.計(jì)算機(jī)仿真，2004，21（5）：20－23.