基于测速测距激光雷达的飞行器地形匹配导航方法

2024-02-16纪新春魏东岩尤逸轩张文超

中国惯性技术学报 2024年1期

桑洋，纪新春，魏东岩，尤逸轩，张文超，袁洪

（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%。