第4期 宋锐，等：基于LiDAR/NS的野外移动机器人组合导航方法 ·807· 取流程中，根据点云各点深度，计算在其同行邻 域S中的粗糙度c: min ∑d.+∑d, (4) T41 pueF c BF-vi 进一步得到x:与x1之间的旋转变化量 △T+lo 其中，粗糙度值较大的非地面点定义为边缘点 2.4多传感耦合算法 F,较小的为平面点F。但在实际应用中，由于 本文采用基于迭代误差状态卡尔曼滤波(er- 数量过大的点云数据特征会对系统实时性有一定 ror state Kalman filter,.ESKF)方法对最小化非线性 影响，因此在数据处理时仅选取若干关键帧作为 约束求解，进而更新状态，根据得到的全局位姿 优化估计的状态量。提取LDAR第i个关键帧的 信息进行地图构建。迭代ESKF在量测更新中增 特征F:={F,F},当相邻时刻机器人位姿变化超 加一步迭代优化，进而得到状态收敛时的最优 出预设阈值，则保存与机器人更新状态节点x+1 解，可以有效降低传统EKF舍弃高阶误差项带来 相关的关键帧F+1。根据实验条件，位移和旋转 的算法误差。 阈值可设置为1m和10°。当接收到新的关键帧 在基于LIO紧耦合模型，将状态估计误差定 时，利用前n+1时刻关键帧的特征集合{F-,F-+1,…, 义为δx=[80,8p,8m,δbl,δ0为角度误差，假设前一 F}和位姿估计{T-,T-+1,…,T}构建局部地图M 时刻状态为，则x,可表示为 M:={M5,Mg\ 4+:⑧exp(δ0 式中：M和Mg分别由世界坐标系下边缘特征 F-n'Fn1…,'F与平面特征Fn'Fn1…,'F} 4=h(x41,δ)= P41+δp -吹1+δv 集合构成。 -b+8b 2.3地图匹配与回环检测 其中⑧表示将角度向量变换到旋转矩阵。当 帧间匹配的算法主要有基于迭代临近点和基 MU相邻时间间隔为△：，则在迭代ESKF中，状态 于正态分布变换，针对野外大范围空旷场景特征 更新可转换为式（⑤）的优化问题： 稀少等引起的LOAM出现退化现象，参考CPFG minf( (5) (closet probability and feature grid)SLAM方法，将 基于ICP平面特征提取和基于NDT的点特征结 ll表示Mahalanobis范数，f)为基于ICP的 合，进而从雷达单帧和拼接的多帧点云数据中提 残差函数，J:为残差函数的雅克比矩阵，M为量 取得到环境特征。利用基于距离阈值判断关键帧 测噪声的协方差矩阵。求解式(6)得到ESKF的 增益方程和状态更新方程： 的方法对地图进行保存，从而保证了所建立地图 的稀疏性。在后端加入基于ICP算法的闭环检 K,=PHE(HPHE+JMJE）- 测，可以在检测到闭环时降低累积漂移误差，使 δx+1=δx,+K(Hδx,-fr) (6) 所建地图达到全局一致性的效果。 在每次迭代过程中，根据更新的匹配边缘和 针对车体快速转弯等引起点云变形等问题， 平面，进一步最小化度量误差，进而计算更新的 本文采用IMU的预积分因子与激光的scan-to- 雅克比矩阵和增益。 map两项构成优化函数，更新计算每帧激光的位 当迭代n步后终止，更新的协方差方程表示为 姿，并将MU的预测量作为优化的初值。对于边 P =(I-KHin)P(I-KHin)+KnMKi 缘特征'F中的更新点pk,以及构成边缘线的 经过ESKF迭代，根据得到6x1可以对x4 p和p,平面特征F,中的k和构成平面的 进行更新，并将其应用于下一时刻的信息更新。 p、p和p。点到边缘线的距离d。表示为 由于位姿信息是在以机器人为中心的坐标系下计 d-l-pE)xu-吃) 算得到的，经过状态合成可将其转换到全局坐标系。 (P-p, 点到平面的距离d。表示为 3实验与结果分析 d,-k-)--)x-pk 3.1实验平台 (pip)x (p-p 基于上述对移动机器人环境建模与定位研究 计算式(4)最小值并根据Gauss-.Newton方法 的分析，搭建如图2所示的野外移动机器人实验 得到最优的转换矩阵： 平台，搭载有Xsens MTi-G-710IMU和Velodyneri S c 取流程中，根据点云各点深度 计算在其同行邻 域 中的粗糙度 : c = 1 |S | ·∥ri∥ ∑ j∈S, j,i (rj −ri) F e i F p i i Fi = { F e i ,F p i } xi+1 Fi+1 m n+1 {Fi−n,Fi−n+1,··· , Fi} {Ti−n,Ti−n+1,··· ,Ti} Mi 其中，粗糙度值较大的非地面点定义为边缘点 ，较小的为平面点 。但在实际应用中，由于 数量过大的点云数据特征会对系统实时性有一定 影响，因此在数据处理时仅选取若干关键帧作为 优化估计的状态量。提取 LiDAR 第 个关键帧的 特征 ，当相邻时刻机器人位姿变化超 出预设阈值，则保存与机器人更新状态节点 相关的关键帧 。根据实验条件，位移和旋转 阈值可设置为 1 和 10°。当接收到新的关键帧 时，利用前 时刻关键帧的特征集合 和位姿估计 构建局部地图 Mi = { Me i , M p i } Me i M p i { ′F e i−n , ′ F p i−n+1 ,··· , ′ F e i } { ′F p i−n , ′ F p i−n+1 ,··· , ′ F p i } 式中： 和 分别由世界坐标系下边缘特征 与平面特征 集合构成。 2.3 地图匹配与回环检测 帧间匹配的算法主要有基于迭代临近点和基 于正态分布变换，针对野外大范围空旷场景特征 稀少等引起的 LOAM 出现退化现象，参考 CPFG (closet probability and feature grid) SLAM 方法，将 基于 ICP 平面特征提取和基于 NDT 的点特征结 合，进而从雷达单帧和拼接的多帧点云数据中提 取得到环境特征。利用基于距离阈值判断关键帧 的方法对地图进行保存，从而保证了所建立地图 的稀疏性。在后端加入基于 ICP 算法的闭环检 测，可以在检测到闭环时降低累积漂移误差，使 所建地图达到全局一致性的效果。 ′F e i p e i+1,k p e i,u p e i,v ′F p i+1 p p i+1,k p p i,u p p i,v p p i,w de 针对车体快速转弯等引起点云变形等问题， 本文采用 IMU 的预积分因子与激光的 scan-to￾map 两项构成优化函数，更新计算每帧激光的位 姿，并将 IMU 的预测量作为优化的初值。对于边 缘特征 中的更新点 ，以及构成边缘线的 和 ，平面特征 中的 和构成平面的 、 和 。点到边缘线的距离 表示为 de =   (p e i+1,k − p e i,u )×(p e i+1,k − p e i,v )    (p e i,u − p e i,v ) 点到平面的距离 dp 表示为 dp =   (p p i+1,k − p p i,u )·(p p i,u − p p i,v )×(p p i,u − p p i,w )      (p p i,u − p p i,v )×(p p i,u − p p i,w )    计算式 (4) 最小值并根据 Gauss-Newton 方法 得到最优的转换矩阵： min Ti+1    ∑ p e i+1,k ∈ ′F e i+1 de + ∑ p p i+1,k ∈ ′F p i+1 dp    (4) xi xi+1 ∆Ti,i+1 进一步得到 与 之间的旋转变化量 。 2.4 多传感耦合算法 本文采用基于迭代误差状态卡尔曼滤波 (er￾ror state Kalman filter, ESKF) 方法对最小化非线性 约束求解，进而更新状态，根据得到的全局位姿 信息进行地图构建。迭代 ESKF 在量测更新中增 加一步迭代优化，进而得到状态收敛时的最优 解，可以有效降低传统 EKF 舍弃高阶误差项带来 的算法误差。 δx = [δθ,δp,δv,δb] δθ − x k k+1 x k k+1 在基于 LIO 紧耦合模型，将状态估计误差定 义为 ， 为角度误差，假设前一 时刻状态为 ，则 可表示为 x k k+1= h( − x k k+1 ,δx) =   −q k k+1 ⊗exp(δθ) − p k k+1 +δp − v k k+1 +δv −b+δb   ⊗ ∆t 其中 表示将角度向量变换到旋转矩阵。当 IMU 相邻时间间隔为 ,则在迭代 ESKF 中，状态 更新可转换为式 (5) 的优化问题： min δx ∥δx∥(Pk ) −1 + f(x k k+1 ) (Jk Mk J T k ) −1 (5) ∥·∥ f(·) Jk Mk 表示 Mahalanobis 范数， 为基于 ICP 的 残差函数， 为残差函数的雅克比矩阵， 为量 测噪声的协方差矩阵。求解式 (6) 得到 ESKF 的 增益方程和状态更新方程： Kk, j = PkH T k, j (Hk, jPkH T k, j + Jk, jMk J T k, j ) −1 δxj+1 = δxj + Kk, j(Hk, j δxj − f(x k k+1 )) (6) 在每次迭代过程中，根据更新的匹配边缘和 平面，进一步最小化度量误差，进而计算更新的 雅克比矩阵和增益。 当迭代 n 步后终止，更新的协方差方程表示为 Pk+1 = (I− Kk,nHk,n)Pk(I− Kk,nHk,n) T + Kk,nMkK T k,n δxj+1 x k 经过 ESKF 迭代，根据得到 可以对 k+1 进行更新，并将其应用于下一时刻的信息更新。 由于位姿信息是在以机器人为中心的坐标系下计 算得到的，经过状态合成可将其转换到全局坐标系。 3 实验与结果分析 3.1 实验平台 基于上述对移动机器人环境建模与定位研究 的分析，搭建如图 2 所示的野外移动机器人实验 平台，搭载有 Xsens MTi-G-710 IMU 和 Velodyne 第 4 期 宋锐，等：基于 LiDAR/INS 的野外移动机器人组合导航方法 ·807·