基于卡爾曼濾波的地形輔助導航＊

周亦軍 鄭 彤

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

1 引言

海底地形輔助導航系統［1～7］是近幾十年出現的一種新型的導航系統，是水下運動載體導航技術的一個發展方向。利用海底地形進行導航的就是將水下載體實際測量的水深值與已存在的地圖進行匹配，對水下載體進行定位，基本原理［1～3］如圖1所示。在海底地形輔助導航系統中，精確的數字地圖和合適的匹配算法是核心技術，這也是當前相關領域的重點和熱點研究問題。

圖1 水下地形輔助導航基本原理

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

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

2 匹配算法

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

圖2中由Pi（i＝1，2，…，N）點構成的是“指示航跡”，N為航跡點序號。彎曲實線稱為“實際航跡”，由Xi（i＝1，2，…，N）構成，Ci（i＝1，2，…，N）表示的是當地實測水深等值線。由于慣導誤差的存在，｛Pi：i＝1，…，N｝和｛Xi：i＝1，…，N｝之間必定存在誤差。Xi必定位于Ci的等值線上，那么按照一定的準則，使Pi靠攏到Ci上，從而找到最優估計點Yi和對應的“匹配航跡”，實現對真實航跡的最優估計。

圖2 ICCP算法原理示意圖

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

式中，T表示變換，包括旋轉和平移。

3 慣導誤差方程與卡爾曼濾波

以平臺式慣導為例，系統誤差狀態方程［23～24］為

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

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

4 獲得真實地形數據

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

圖3 多波束測深系統示意圖

5 仿真分析

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

圖4 地形匹配輔助導航

圖5 東向水平誤差角估計

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

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

圖6 北向水平誤差角估計

圖7 水平誤差角估計

表1 平臺誤差角穩態值

6 結語

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

［1］王英鈞.地形輔助導航綜述［J］.航空電子技術，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］劉光軍，袁書明，黃詠梅.海底地形匹配技術研究［J］.中國慣性學報，1999（1）：19－22.

［4］吳鳳德.美國戰略潛艇導航的現行技術和新技術綜述［M］.吳祖銓譯.艦船導航，1999，5.

［5］劉光軍，陳晶.海底地形輔助導航系統仿真技術研究［J］.計算機仿真，2000，17（2）：21－24.

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

［7］劉準，佀文芳，陳哲.海底地形匹配技術研究［J］.系統仿真學報，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］.國防科技大學學報，2000，22（4）：98－101.

［14］徐克虎，沈春林.地形輔助慣性導航系統研究［J］.南京航空航天大學學報，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］.宇航學報，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］.海洋測繪，2002，22（5）：3－6.

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

［24］許麗佳，陳陽舟，崔平遠.GPS／INS組合導航系統中的信息融合算法研究［J］.計算機仿真，2004，21（5）：20－23.