基于仿射因子補償?shù)母倪M地形匹配ICCP算法

程向紅，王依能，丁 鵬

（1. 東南大學 微慣性儀表與先進導航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

水下航行器在軍用和民用領(lǐng)域具有重要戰(zhàn)略意義，隨著地球物理場輔助慣性導航技術(shù)的不斷發(fā)展，應用地磁、重力、地形等信息來輔助慣性導航系統(tǒng)為水下航行器導航技術(shù)研究提供了一種新思路[1]。由于地形高程信息測量較為方便，且陸地上的地形匹配技術(shù)已應用于實際，可以為水下地形匹配導航技術(shù)提供借鑒[2]，因而應用地形高度信息來輔助水下航行器的導航技術(shù)具有高度可行性。

目前，地球物理場輔助導航方法主要有地形輪廓匹配算法（Terrain Contour Matching, TERCOM）、桑迪亞慣性地形輔助導航算法（Sandia Inertia Terrain-aided Navigation, SITAN）和迭代最近等值線算法（Iterated Closest Contour Point, ICCP）。TERCOM實時性差[3]，且對于航向誤差較為敏感；SITAN系統(tǒng)則對初始位置精度要求高[4]，且要求對地形做線性化估計，當?shù)匦涡甭实恼撎栕兓瘯r則會導致濾波發(fā)散。

ICCP算法由ICP（Iterated Corresponding Point）算法發(fā)展而來，最早由Besl和McKay在圖像對準領(lǐng)域使用，隨后被Behzad K. P.[5]首次應用于重力匹配輔助導航，Bishop通過大量仿真試驗，系統(tǒng)地分析了該算法的可行性及誤差影響[6]，劉承香進一步詳細分析了ICCP算法在地形匹配輔助導航領(lǐng)域的適用性[7]?？紤]到匹配過程中因為速度誤差會導致慣導指示航跡與真實航跡的形狀存在不同[8]，羅詩圖等采用最小失配點準則估計仿射變換參數(shù)，但精度不高[9]，此外模擬退火[10]、最小二乘等最優(yōu)估計算法[11]精度優(yōu)于最小失配點準則，但需要較長的計算時間。因此，為了盡可能得到精度高的仿射因子，同時很好地保證算法的實時性，本文首先對ICCP算法的原理與缺陷進行分析，接著對仿射模型進行數(shù)學分析，推導得出仿射因子的解析解，用仿射因子補償匹配過程中的仿射誤差，提出改進算法模型，最后進行試驗驗證并給出結(jié)論，驗證改進算法提升匹配精度的效果。

1 ICCP算法

1.1 ICCP算法原理

ICCP的基本實現(xiàn)原理可以理解為，從數(shù)據(jù)形態(tài)中抽取出若干點構(gòu)成點集1，從模型形態(tài)中抽取同等數(shù)目的若干點構(gòu)成點集2，并設置初始的迭代參數(shù)，不斷循環(huán)進行計算點集1與點集2中各對應點的最近點、計算對準變換、應用對準、判定精度的步驟，當?shù)Y(jié)果變化低于預先設定的門限值時停止迭代、完成匹配[12]。

算法實現(xiàn)示意圖如圖1所示，當水下航行器進入適配區(qū)后，通過航行器攜帶的測深傳感器測量并存儲航行軌跡點對應的水深值，根據(jù)慣性導航系統(tǒng)提供的位置提取所在區(qū)域的水深數(shù)據(jù)庫，再根據(jù)實測水深值hi，在水深數(shù)據(jù)庫中提取慣導指示航跡點pi附近的等值線ci，并在等值線ci上搜索到pi的最鄰近參考點yi。其中，i = 1, 2… N表示第i個慣導指示航跡點序號，hi表示第i個航跡點所對應的實測水深值，pi表示第i個慣導指示航跡點所對應的二維位置坐標，ci表示第i個指示航跡點附近對應實測水深值hi的等值線，yi表示第i個指示航跡點到等值線ci上的最鄰近參考點的二維位置坐標。

圖1 ICCP算法實現(xiàn)示意圖Fig.1 ICCP algorithm implementation

根據(jù)從參考數(shù)據(jù)庫中搜索的最鄰近參考點與慣導指示航跡點確定旋轉(zhuǎn)矩陣R和平移向量t，使得最鄰近參考點集合Y與慣導指示航跡點集合P之間的歐幾里得距離平方和最小，公式如下：

式中，Y={yi, i = 1, 2… N}表示最鄰近參考點集合，P ={pi, i = 1, 2… N}表示慣導指示航跡點集合，d表示集合Y和集合P的歐幾里得距離平方和，表示向量X的2范數(shù)。

利用矩陣的特征值特征向量分解方法得到旋轉(zhuǎn)矩陣R，首先對慣導指示航跡點pi和最鄰近參考點yi作一個變換，即令：

然后根據(jù)相對二維位置坐標Δpi和Δyi構(gòu)造一個2×2矩陣M，即：

式中，M是由坐標變換后的慣導指示航跡點Δpi和最鄰近參考點Δyi構(gòu)成的2階方陣，右上角標T表示矩陣的轉(zhuǎn)置。

接著構(gòu)造一個4×4矩陣H：

M11、M12、M21、M22是矩陣M的元素，矩陣H的四個特征值易證是實數(shù)，如下所示：

記最大的特征值為λm，由特征向量與旋轉(zhuǎn)角、旋轉(zhuǎn)矩陣的單位四元數(shù)表示法之間的關(guān)系，可得旋轉(zhuǎn)角θ與旋轉(zhuǎn)矩陣R為：

平移向量t為：

基于上述原理表述可以看出，ICCP算法遵循“初始對準—尋找對應關(guān)系（最近點）—求解使得目標函數(shù)最小的變換—應用變換”的循環(huán)過程[13]，在等值線上尋找最小度量意義下的全局最優(yōu)值，該全局最優(yōu)值就是最終的匹配對準結(jié)果。

1.2 ICCP算法的局限性

水下航行器進入匹配區(qū)域后，由于慣導誤差隨時間不斷積累，導航定位結(jié)果本身會攜帶位置誤差Δp0、速度誤差δv與姿態(tài)誤差ψ，速度方程可表示為：

鑒于短時間內(nèi)姿態(tài)誤差的累積量較小，ICCP算法每一次迭代過程中慣導航向誤差的變化可以忽略，因而航向誤差可看作常值；ICCP算法僅對水下航行器的水平方向位置進行對準匹配，姿態(tài)誤差ψ對匹配結(jié)果幾乎沒有影響。在以上兩點的假設基礎上，速度誤差的變化εv可以看作隨時間t線性變化，進一步可以用其數(shù)學期望εm來表示，因此速度誤差與速度方程可以表示為：

假設配準開始是為0時刻且采樣間隔為ΔT，那么水下航行器的位置更新方程離散形式可以表示為：

Δp0為位置誤差，將式(11)(13)代入式(12)可得：

式中，Δp0+Δv(k+1)ΔT兩項為最終的位置匹配誤差，Δp0是匹配前的系統(tǒng)位置誤差，Δv(k+1)ΔT是由速度誤差造成的累積誤差。在實際的軌跡匹配中，Δp0表現(xiàn)為平移誤差，Δv(k+1)ΔT表現(xiàn)為旋轉(zhuǎn)誤差和仿射誤差，軌跡匹配過程中的旋轉(zhuǎn)誤差在上述分析中被認為是常值。

根據(jù)1.1中旋轉(zhuǎn)矩陣R和平移向量t的定義，平移向量t的離散形式即為位置誤差Δp0，同時令仿射因子為s，那么(k+1)時刻和k時刻的慣導系統(tǒng)指示位置可改寫為：

對比式(14)和式(15)，可以得出慣導系統(tǒng)速度誤差造成的累計誤差Δv(k+1)ΔT與仿射因子s之間的關(guān)系為：

但由于ICCP算法本身只考慮了匹配變換過程中的平移誤差與旋轉(zhuǎn)誤差，并未修正由速度誤差引起的仿射誤差，因此傳統(tǒng)的ICCP匹配算法只能對慣導指示航跡做剛性軌跡匹配，精度有限[14]。針對這一局限性，可以在現(xiàn)有ICCP算法的基礎上引入仿射模型與仿射因子，對于由慣導軌跡到真實軌跡的變換進行優(yōu)化，修正仿射誤差，提高匹配精度。

2 仿射因子解析解

鑒于傳統(tǒng)ICCP算法不能修正速度誤差帶來的仿射誤差，因此需要借助仿射變換模型對仿射誤差進行補償。而引入仿射變換模型后，改進ICCP算法的核心即為求解旋轉(zhuǎn)矩陣R、仿射因子s和平移向量t，使得最鄰近參考點集合Y與慣導指示航跡點集合P之間的歐幾里得距離平方和最小，公式如下：

旋轉(zhuǎn)矩陣R的求解方式與1.1中保持一致，最鄰近參考點集合Y與慣導指示航跡點集合P之間的歐幾里得距離平方和d(Y,sRP+t)，也即集合Y與集合P之間誤差ei的平方和，可以改寫為如下形式：

其中平移向量的誤差Δt為：

由此可以得到：

根據(jù)剛性變換不改變長度模值的特性：

可將式(20)改寫為：

3 改進ICCP算法

在旋轉(zhuǎn)矩陣R、仿射因子s和平移向量t的基礎上，基于仿射因子補償解的改進ICCP算法流程圖如圖2所示。改進ICCP算法以組合導航系統(tǒng)為平臺，借助水深參考數(shù)據(jù)庫對慣導航跡進行匹配對準：

圖2 改進ICCP算法流程圖Fig.2 Flow chart of the improved ICCP algorithm

第一步：水下航行器進入適配區(qū)后，通過水深傳感器測量并存儲航行軌跡點的水深值，根據(jù)慣導系統(tǒng)提供的位置信息提取所在區(qū)域的水深參考數(shù)據(jù)庫；

第二步：根據(jù)實測水深值提取慣導指示航跡點附近的等值線，并搜索指示航跡點到附近等值線上的對應最鄰近參考點；

第三步：根據(jù)從參考數(shù)據(jù)庫中搜索的最鄰近參考點與慣導指示航跡點確定旋轉(zhuǎn)矩陣R、仿射因子s與平移向量t；

第四步：利用第三步得到的旋轉(zhuǎn)矩陣R、仿射因子s與平移向量t更新指示航跡點的位置，判斷迭代次數(shù)是否超限或滿足局部最優(yōu)條件，迭代允許的最大次數(shù)由匹配過程的實時性要求以及實際操作經(jīng)驗確定，一般可取10~20次，局部最優(yōu)條件是當前匹配航跡相較于慣導指示航跡點組成的航跡的歐幾里得距離平方和的變化率小于特定值εsop，εsop的具體數(shù)值根據(jù)地形圖網(wǎng)格精度、慣導器件誤差、初始位置誤差等實際情況確定，試驗驗證中取εsop為5%，如果沒有達到迭代次數(shù)或沒有滿足收斂條件，則返回第二步進行循環(huán)操作，否則進入下一步；

第五步：對最后得到的航跡進行精度評估，判斷結(jié)果是否可靠，如果最終的匹配航跡相較于上一次迭代得到的匹配航跡（若為第一次迭代，則與慣導指示航跡點組成的航跡比對）的歐幾里得距離平方和小于特定值εop則認為結(jié)果可靠，εop的具體數(shù)值根據(jù)地形圖網(wǎng)格精度、慣導器件誤差、初始位置誤差等實際情況確定，試驗驗證中取εop為五分之一個網(wǎng)格精度，最后得到的匹配航跡可用以校正慣導指示航跡，否則認為匹配失敗，需要放棄當前航跡，使用其他航跡重新進行匹配。

4 試驗驗證

4.1 仿真試驗

為了驗證本文所提方法的有效性，選取某海域的水深數(shù)據(jù)庫進行仿真實驗，設置INS待匹配軌跡的采樣點數(shù)為3200個，兩點之間的采樣間隔為0.01 s，即仿真軌跡持續(xù)32 s。仿真實驗參數(shù)如表1所示。

表1 仿真試驗參數(shù)Tab.1 Simulation experiment parameters

規(guī)劃不同的水下航行器運動軌跡，進行5次相同仿真參數(shù)、不同運動軌跡的仿真試驗，利用傳統(tǒng)ICCP算法與基于仿射因子補償?shù)母倪MICCP算法進行軌跡匹配，驗證改進算法的匹配效果，不同軌跡下的匹配精度數(shù)據(jù)如表2所示。5條水下航行器軌跡匹配仿真圖具有類似的情況，選其中軌跡1的軌跡匹配結(jié)果繪圖展示，如圖3-4所示。

表2 不同軌跡的匹配結(jié)果Tab.2 Matching results of different trajectories

圖3 水下航行器軌跡匹配仿真圖Fig.3 Simulation of underwater vehicle trajectory matching

圖4 匹配軌跡點位置誤差比較圖Fig.4 Comparison of matching points' positioning errors

根據(jù)軌跡1的軌跡匹配仿真圖，可以看出ICCP匹配軌跡、基于仿射因子補償?shù)母倪MICCP匹配軌跡與理論航跡都具有類似的形狀，都能一定程度上修正INS待匹配航跡，但改進ICCP匹配航跡更加接近水下航行器的仿真理論真實航跡。根據(jù)匹配軌跡點位置誤差比較圖，則更明顯地看出改進ICCP算法的匹配結(jié)果遠好于傳統(tǒng)ICCP算法，傳統(tǒng)算法的軌跡點位置誤差在80 m到120 m波動，而改進算法位置誤差小于40 m。

結(jié)合表2中不同軌跡匹配結(jié)果的均方根位置誤差具體數(shù)據(jù)，可以驗證本文提出的基于仿射因子補償?shù)母倪MICCP方法很好地修正了傳統(tǒng)ICCP算法中的仿射誤差，相比于傳統(tǒng)算法的結(jié)果，改進ICCP算法的匹配精度有80%以上的提升；并且以數(shù)學推導求解仿射因子的方式并未帶來過多的運算量，傳統(tǒng)算法與改進算法的時間復雜度差別不大，利用蒙特卡羅法進行多次模擬可以得到改進算法平均耗時約為1.7391 s，相對于傳統(tǒng)算法1.7235 s的平均耗時，僅增加不到1%。

4.2 半物理仿真試驗

在純仿真實驗驗證本算法的可靠性之后，基于浙江省千島湖新安江水庫部分區(qū)域的實際水下地貌與船載實驗數(shù)據(jù)，進一步驗證本算法在實際系統(tǒng)中的效果。

表3 中記錄了船載實驗中慣性導航系統(tǒng)的設備參數(shù)及地形圖網(wǎng)格間距，這里選用一段曲折程度較大的軌跡進行半物理仿真試驗，意在從圖中直觀體現(xiàn)出仿射因子對于匹配前后軌跡形態(tài)的影響。軌跡采樣點設置類似4.1中的仿真試驗，總采樣點4800個，兩點間采樣間隔為0.01 s，即所選航行軌跡持續(xù)48 s。

表3 船載試驗參數(shù)Tab.3 Shipboard experiment parameters

圖5 為基于船載實驗數(shù)據(jù)的半物理仿真圖，相較于INS待匹配航跡與ICCP匹配航跡，可以明顯看出本文提出的改進ICCP算法得到的匹配航跡匹配誤差小、更貼合理論航跡。圖6為軌跡點位置誤差比較圖，同樣可以看出：傳統(tǒng)ICCP算法的位置誤差波動幅度大，誤差值基本保持在50 m以上；改進ICCP算法的位置誤差則較為穩(wěn)定，始終保持在50 m以內(nèi)。

圖5 軌跡匹配半物理試驗圖Fig.5 Semi-physical experiment of trajectory matching

圖6 匹配軌跡點位置誤差比較圖Fig.6 Comparison of matching points' positioning errors

經(jīng)過計算，傳統(tǒng)ICCP算法匹配后均方根位置誤差為127.8 m，耗時約為2.5366 s；改進ICCP算法匹配后均方根位置誤差為27.4 m，耗時約為2.5518 s。改進算法的匹配精度比傳統(tǒng)算法提升了78.6%，而算法耗時僅增加0.6%。由此可以表明：改進ICCP算法在實際系統(tǒng)中仍然能夠以很小的時間代價，帶來較大的匹配精度提升。

5 結(jié) 論

本文介紹了ICCP算法的實現(xiàn)原理，對配準誤差進行理論分析，得出其剛性變換的局限性，并研究仿射變換模型，引入仿射因子的解析解，修正剛性變換未能解決的仿射誤差。通過仿真試驗與半物理試驗驗證了本算法的有效性，半實物仿真試驗結(jié)果表明：基于仿射因子補償?shù)母倪MICCP算法可以在算法耗時增加不到1%的前提下，修正航跡匹配過程中的仿射誤差，匹配精度較傳統(tǒng)的ICCP算法平均提高78%以上，在10 m網(wǎng)格的水深圖中，傳統(tǒng)算法匹配精度在10個網(wǎng)格精度左右，改進算法的匹配精度可達到1~2個網(wǎng)格精度。