基于測速測距激光雷達的飛行器地形匹配導航方法

桑 洋，紀新春，魏東巖，尤逸軒，張文超，袁 洪

（1.中國科學院 空天信息創新研究院，北京 100094；2.中國科學院大學 電子電氣與通信工程學院，北京 100049；3.西北工業大學 電子信息學院，西安 710129）

地形匹配導航[1-4]通過測量飛行器下方地形獲得實時高程圖，與預先存儲的基準高程圖進行匹配后校正慣導，適用于山地、丘陵等地形起伏較大的區域。憑借自主性強、隱蔽性好、抗干擾性強和無累積誤差等優勢，地形匹配導航已成為中低空飛行器的一種重要導航手段。按照測量的高程信息維度，地形匹配導航可分為基于一維線狀高程的導航方式和基于二維面狀高程的導航方式，二維高程相比于一維高程信息更加豐富，有利于提高地形匹配導航的精度、魯棒性和適用性，但也增加了地形匹配的運算量，不利于導航系統的實時性。

基于二維面狀高程的地形匹配導航中，地形測量手段包括干涉合成孔徑雷達[5]（Interferometric Synthetic Aperture Radar,InSAR）、攝影測量[6]和激光雷達[7]（Light Detection and Ranging,LiDAR）。與其他兩種測量方式相比，激光雷達具有數據處理簡單、穿透力強、抗干擾能力強、精度高等優勢。眾多學者專家開展了基于激光雷達的飛行器地形匹配導航研究。孟海東等[8]利用激光雷達采集的多條路徑地形數據進行匹配導航，可實現比單一路徑地形匹配更快速、精準和魯棒的定位。Leines[9]提出一種基于激光雷達測距的尺度不變特征變換[10]（Scale-invariant Feature Transform,SIFT）的地形匹配導航算法，對利用慣導位姿恢復的點云插值構建實時高程圖，通過提取匹配實時高程圖與基準高程圖的SIFT 特征來估計飛行器位置。Salles 等[11]利用地表高度、樹冠高度和激光反射強度的聯合模板實現熱帶雨林區域的地形匹配導航，實驗結果表明多類型數據融合能夠提高匹配的可靠性和精度。激光雷達對地形的測量依靠載體運動，傳統激光雷達地形匹配導航根據慣導位姿恢復點云并由此構建實時高程圖，然而，慣導誤差累積將導致高程圖變形從而限制匹配精度。并且，根據慣導位姿構建的高程圖會導致地形匹配定位結果與慣導存在相關性，不符合組合導航源相互獨立的假設。此外，傳統激光雷達地形匹配導航只利用匹配結果校正慣導，若在地形特征匱乏區域出現匹配失效，慣導將得不到校正從而導致定位精度變差。

隨著激光測量理論和技術的發展，出現了具備測距和測速功能的“調頻連續波（Frequency Modulated Continuous Wave,FMCW）激光雷達”，也稱“測速測距激光雷達”。除三維點云外，其由多個激光束方向的多普勒頻移可反演獲得載體三維速度[12,13]。美國國家航空航天局（National Aeronautics and Space Administration,NASA）已研制出用于導航的FMCW激光雷達并搭載于直升機上，成功開展了對其進行測速功能的飛行測試[14]。FMCW 激光雷達對載體速度的測量具有高頻率、高精度且誤差不發散的特點，且與慣導相互獨立，解決了傳統激光雷達地形匹配導航中實時高程圖變形限制匹配精度、地形匹配失效期間定位可用性差和導航源不獨立的問題，具有重要應用價值。然而，已有的激光雷達地形匹配導航架構并不適用于FMCW 激光雷達，需要研究基于該傳感器的地形匹配導航方法。

基于此，提出了一種基于測速測距激光雷達的飛行器地形匹配導航方法。針對傳統激光雷達地形匹配導航架構中實時高程圖變形限制匹配精度以及匹配結果與慣導相關的問題，設計激光測速輔助的實時高程圖精準構建方法，減小實時高程圖的變形進而提高匹配定位精度，同時降低匹配定位結果與慣導的相關性。利用SIFT 特征匹配實時高程圖與基準高程圖，針對二維地形匹配運算量大的問題，提出結合高程差一致性與剛體變換一致性的匹配點對分步核驗方法，在維持匹配精度的同時減小運算量進而提高算法的實時性。針對地形匹配失效期間無法校正慣導導致定位可用性變差的問題，利用激光測速信息對慣導進行實時校正，提高了弱地形特征區域的定位可用性。基于真實地形數據開展半物理仿真實驗，對提出的地形匹配導航方法進行驗證。實驗結果表明，所提方法相比于常規方法在定位精度、實時性和可用性方面更具優勢。

1 系統整體框架

本文地形匹配導航方法框架如圖1 所示。飛行器搭載測速測距激光雷達、慣性測量單元（Inertial Measurement Unit,IMU）和氣壓高度表，并存儲定位區域地形特征數據庫。數據處理流程分為激光測速輔助的實時高程圖構建、集成SIFT 特征與分步核驗的快速地形匹配、慣導/激光測速/地形匹配組合導航三個階段。

圖1 基于測速測距激光雷達的地形匹配導航方法框架Fig.1 Framework of terrain matching navigation method based on FMCW LiDAR

在實時高程圖構建階段，基于航位推算的載體位姿和氣壓測高校正的慣導高度關聯多幀點云數據并計算點云高程，插值生成實時高程圖，減小了其在水平方向的變形，提高地形匹配精度，并且降低匹配結果與慣導的相關性。在地形匹配階段，利用SIFT 特征匹配實時高程圖與基準高程圖，然后基于特征點對高程差一致與剛體變換一致的特性在隨機抽樣一致性[15]（Random Sample Consensus,RANSAC）誤匹配檢測方法中引入卡方分布預核驗機制，維持匹配精度，減小運算量，提高算法的實時性。在組合導航階段，采用序貫濾波融合慣導、氣壓高度表、激光測速和地形匹配信息，激光測速信息的引入有效提高了弱地形特征區域的定位可用性。

2 激光測速輔助的實時高程圖構建

實時高程圖構建涉及的坐標系有激光雷達坐標系（l系）、載體坐標系（b系）、地心地固坐標系（e系）、“東-北-天”導航坐標系（n系）、世界坐標系（w系），如圖2 所示。安裝誤差標定后，可實現l系與b系之間的轉換，因此可假設l系與b系重合。

當前時刻j掃描的點云載體系坐標可根據激光測距值d、橫向掃描角θ和縱向掃描角α計算得到：

通過j-1時刻載體的姿態矩陣將位移增量投影至j-1時刻的導航系：

載體的水平位置更新算法為：

其中，λ、L和h分別表示經度、緯度和高度，h由氣壓高度計校正慣導獲得分別為的東向和北向分量；RM、RN分別為相應位置的子午圈曲率半徑和卯酉圈曲率半徑。

載體的姿態更新算法為：

利用載體姿態矩陣將點云投影至當前時刻導航系：

通過式(8)將點云投影至地心地固坐標系：

將點云投影至匹配窗口起始時刻的導航系（n0系）：

以n0系作為w系，通過以上計算，可將匹配窗口內的點云統一到w系中，并獲得點云高程。

根據水平坐標將空間區域分成規則的等距離單元，格網中心點高程由周圍點云的高程內插求得，采用精度較高的反距離加權插值法[16,17]。

其中，Z0為估計的格網中心點高程；Zi為控制點i的高程；di為控制點i到中心點的距離；s為用于插值的控制點數量；K為指定的冪。

點云插值構建的實時高程圖如圖3 所示。

圖3 點云插值構建的實時高程圖Fig.3 Real-time elevation model constructed by point cloud interpolation

3 基于SIFT 特征與分步核驗的快速地形匹配

3.1 SIFT 特征提取與粗匹配

提取實時高程圖中的SIFT 特征，根據慣導指示位置及其標準差確定搜索區域，將實時圖特征與基準地形特征庫在搜索區域中的特征進行粗匹配，如圖4所示，左側為搜索區域的基準高程圖，右側為實時高程圖，綠色線條表示特征對應關系。

圖4 地形特征粗匹配效果圖Fig.4 Rough matching diagram of terrain features

3.2 特征點對二維剛體變換與高程差一致性

定義實時高程圖坐標系（m系）和基準高程圖坐標系（d系）均為平面直角坐標系。假設有N對特征點，實時高程圖特征點集其中表示實時高程圖第i個特征點的坐標。基準高程圖特征點集其中表示基準高程圖第i個特征點的坐標。令待估的二維剛體變換參數包括旋轉角θ和兩個方向的平移量Δx、Δy。易知正確匹配的特征點對具有一致的二維剛體變換。

特征點對的重投影誤差為：

通過最小化重投影誤差平方和實現二維剛體變換參數的最小二乘估計：

正確匹配的實時圖與基準圖特征點的真實高程相等，結合式(14)可知高程差ei由式(15)所示的各項測量誤差組成。

根據誤差特性，假設式(15)的各項測量誤差由常值零偏和高斯噪聲組成，且每對特征點的測量誤差獨立同分布，則每對特征點高程差的期望均為同一個常值零偏。因此，正確匹配的特征點對具有一致高程差。

3.3 TCI-RANSAC 誤匹配檢測算法

基于正確匹配的地形特征點對二維剛體變換和高程差均一致的特性，參考視覺定位領域奇偶校驗初始化隨機抽樣一致性算法[18]（Parity Initialized Random Sample Consensus,PI-RANSAC）對RANSAC 隨機抽樣的特征點對進行卡方預檢驗的思想，提出卡方初始化隨機抽樣一致性的地形特征誤匹配檢測算法（Terrain Cardinality Initialized Random Sample Consensus,TCI-RANSAC）。首先，隨機抽取g對特征點，對其進行二維剛體變換一致和高程差一致的雙準則卡方檢驗；通過檢驗后，執行后續的RANSAC 步驟，否則繼續進行抽樣，算法流程詳見表 1。TCI-RANSAC 算法能夠維持地形匹配的定位精度并且減小特征誤匹配檢測的運算量，提高匹配的實時性。

利用TCI-RANSAC 算法完成核驗后，根據最優特征點對集合Ubest通過式(13)計算二維剛體變換參數，利用變換參數將載體在實時高程圖坐標系的坐標轉換到基準高程圖坐標系中，然后根據地理基準確定匹配的定位結果。

表1 TCI-RANSAC 算法流程Tab.1 TCI-RANSAC algorithm flow

下面介紹雙準則卡方檢驗原理。將測量模型h(r)在二維剛體變換參數近似值r0處線性化，構造線性觀測方程：

定義正交矩陣V∈R(2N-3)×2N，且V與HQ正交，定義奇偶矢量構造檢測統計量λQ：

其中，σQ為量測誤差εQ中各元素的標準差。

無誤匹配時，量測誤差εQ服從均值為0 的聯合高斯分布，可知λQ服從自由度為2N-3的卡方分布：

特征點對的高程差通過式(19)求得。

EN可看作常值零偏誤差bE的觀測值，構造觀測方程：

其中，HE=[1 …1]T∈RN×1為觀測矩陣；εE為量測誤差。

則最小二乘殘差矢量ωE表示為：

構造檢測統計量λE：

其中，σE為量測誤差εE中各元素的標準差。

無誤匹配時，量測誤差εE服從均值為0 的聯合高斯分布，可知λE服從自由度為N-1的卡方分布：

當λQ與λE均小于設置的相應故障檢測門限時，認為特征點對中不存在誤匹配，否則存在誤匹配。

4 慣導/地形匹配/激光測速組合導航

4.1 狀態方程

組合導航狀態方程基于慣導誤差方程建立：

其中，Xk表示k時刻的狀態向量；Φk,k-1為狀態轉移矩陣；Γk為噪聲驅動矩陣；Wk為系統噪聲。

狀態向量Xk由姿態誤差δΨ、速度誤差δvn、位置誤差δp、陀螺儀零偏ε和加速度計零偏?組成：

4.2 量測方程

當氣壓高度計信息有效時，可利用氣壓測高結果校正慣導高度通道誤差，量測方程為：

當地形匹配信息有效時，可利用地形匹配定位結果校正慣導水平通道誤差，量測方程為：

FMCW 激光雷達提供了載體速度這一新的量測信息，當該信息有效時，可用于校正慣導誤差。量測方程為：

4.3 多源信息融合

地形特征分布影響地形匹配信息的有效性，因此，本文采用序貫濾波進行信息融合解算以實現即插即用，算法結構如圖5 所示。

圖5 序貫濾波信息融合算法結構圖Fig.5 The diagram of sequential-filtering fusion algorithm

狀態更新方程為：

5 仿真驗證與分析

5.1 實驗數據

使用的真實地形數據為分辨率12.5 m的數字高程模型（Digital Elevation Model,DEM），來源于某衛星搭載合成孔徑雷達干涉測量的結果，如圖6 所示。區域大小約為69 km×78 km，地形以丘陵和山地為主，地形特征較為豐富。在真實地形數據上添加強度為5 m 的高斯白噪聲，作為地形匹配的基準高程圖。

圖6 基準高程圖與飛行軌跡Fig.6 Reference elevation model and flight path

測試的飛行軌跡數據時長3600 s，飛行器經歷了加速、勻速、轉彎、減速等運動狀態，軌跡長度約215 km，如圖6 所示。基于仿真飛行軌跡，通過捷聯慣導反演算法生成IMU 數據，并參考巡航飛行器典型IMU 配置設置主要誤差參數，如表2 所示。

表2 IMU 仿真參數Tab.2 IMU simulation parameters

氣壓高度計數據在理論高度的基礎上添加5 m 的常值零偏和強度為2 m 的高斯白噪聲進行仿真。FMCW 激光雷達數據在理論測速測距值的基礎上添加高斯白噪聲進行仿真，理論測速值為真實載體系速度，理論測距值基于仿真地形數據通過射線追蹤獲得，主要參數參照某款國產測速測距激光雷達性能指標設置，如表3 所示。

表3 FMCW 激光雷達仿真參數Tab.3 FMCW LiDAR simulation parameters

匹配間隔設為1 km，正確匹配特征的重投影誤差閾值設為10 m，判定匹配是否成功的特征數量閾值設為10。更長的地形匹配窗口意味著更豐富的地形信息，有利于提高匹配成功率和魯棒性，但高程圖的變形隨載體位姿誤差發散會更嚴重，測試過程中通過平衡以上兩個因素將匹配窗口設為典型值5 km。

5.2 激光測速輔助的實時高程圖構建

根據實時高程圖構建原理可知，其變形程度取決于匹配窗口內恢復點云水平方向的變形程度。在典型飛行軌跡下，兩種方法恢復出的窗口內單幀點云的水平坐標均方根誤差（Root Mean Squared Error,RMSE）隨里程的變化趨勢如圖7 所示。從圖7 可以看出，激光測速輔助的方法恢復出的單幀點云水平坐標RMSE增長速度和RMSE 最大值小于基于慣導的方法的RMSE 增長速度和RMSE 最大值，激光測速輔助的方法與基于慣導的方法恢復出的窗口內所有點云水平坐標RMSE 分別為2.33 m 與5.78 m。綜上，激光測速輔助的方法構建的實時高程圖變形程度更小。

圖7 點云水平坐標RMSE 對比Fig.7 Comparison of point cloud horizontal coordinate RMSE

分別利用兩種方法構建的實時高程圖進行地形匹配，匹配定位誤差及其概率分布函數如圖8 和圖9 所示。

圖8 匹配定位誤差對比Fig.8 Matching positioning error comparison

圖9 匹配定位誤差概率分布函數對比Fig.9 Comparison of probability distribution functions for matching positioning error

由圖8 和圖9 可以看出，基于慣導的方法匹配定位RMSE 為8.28 m，激光測速輔助的方法匹配定位RMSE 為5.25 m，定位精度提高了36.6%，驗證了本文方法可有效減小高程圖變形進而提高匹配定位的精度。此外，本文采用航位推算位姿替代慣導位姿構建高程圖，能夠降低匹配定位結果與慣導的相關性。

5.3 TCI-RANSAC 地形特征誤匹配檢測

為了評估本文TCI-RANSAC 的性能，從匹配定位的精度（RMSE）和平均運行時間兩個方面將其與傳統的RANSAC 方法進行對比，結果如表4 所示。所有程序均在MATLAB R2015b 上實現，處理器為Intel酷睿i7 12700H。

表4 定位精度與運行時間對比Tab.4 Comparison of positioning accuracy and running time

從表4 可以看出，TCI-RANSAC 方法迭代20 次與傳統RANSAC 方法迭代100 次的匹配定位精度相當，但運行時間縮短了55.6%，驗證了本文方法在維持定位精度的同時也提高了匹配的實時性。

5.4 慣導/激光測速/地形匹配組合導航

為了測試載體平臺測速信息對慣導實時校正的效果，將慣導/激光測速/地形匹配組合導航與慣導/地形匹配組合導航結果進行對比，兩種組合方式采用相同的高程圖構建和地形匹配方法，水平定位誤差如圖10所示。

圖10 組合導航水平定位誤差對比Fig.10 Comparison of horizontal positioning error of integrated navigation systems

從圖10 可以看出，慣導/激光測速/地形匹配組合的定位誤差明顯小于慣導/地形匹配組合的定位誤差。在地形可用性較差區域出現了兩次匹配中斷，在第二次匹配中斷期間，慣導/地形匹配組合系統中慣導未得到校正導致定位誤差快速發散，而激光測速信息的引入有效抑制了定位誤差發散，增強了弱地形特征區域的定位可用性。慣導/地形匹配組合定位RMSE 為10.93 m，慣導/激光測速/地形匹配組合定位RMSE 為3.73 m，測速信息的引入使得定位精度提高了65.9%。

兩種組合方式的航向誤差如圖11 所示。從圖11可以看出，慣導/激光測速/地形匹配組合的航向誤差小于慣導/地形匹配組合的航向誤差，匹配中斷期間其航向未發散。慣導/地形匹配組合的航向RMSE 為0.063 °，慣導/激光測速/地形匹配組合的航向RMSE為0.020 °，測速信息的引入使得航向精度提高了68.3%。

圖11 組合導航航向誤差對比Fig.11 Comparison of heading error of integrated navigation systems

6 結論

面向中低空飛行器自主導航的應用需求，本文研究了一種基于測速測距激光雷達的飛行器地形匹配導航方法，主要貢獻在于：

1）提出一種激光測速輔助的實時高程圖精準構建方法，能夠減小高程圖變形從而提高匹配精度，同時降低匹配結果與慣導的相關性，并以激光測速信息實時校正慣導，增強地形匹配失效期間的定位可用性。

2）提出結合特征點對高程差與剛體變換一致性的地形特征匹配分步核驗方法，能夠在維持匹配定位精度的同時減小匹配運算量。

3）實驗結果表明，相比于傳統方法，本文方法在定位精度、可用性和實時性方面更具優勢，典型場景中組合定位精度3.73 m，航向精度0.020 °，滿足中低空飛行器自主導航應用需求，具備較高的工程應用價值。