·806· 智能系统学报 第15卷 构成的小回环为转角提供更多约束，可用于提升 2 LiDAR/INS数据融合方法 激光里程计的鲁棒性，而大回环则可以通过建立 闭环约束并修正全局累计误差；5)基于紧耦合的 2.1数据预处理 数据融合，包含LiDAR里程计、IMU预积分和地 MU在t时刻的角速度和加速度观测模型可 图配准等，进而得到机器人全局一致的位姿。本 表示为 文所提算法的流程图如图1所示，其中标号为文 ù，=+b+n à，=C(a,-g)+b+n 中对应公式。 式中：o,和a,分别为MU原始数据；b、b和、 IMU +预积分 运动估计 分别表示对应输出信号中的漂移和高斯白噪 特征点云 位姿 声；C为坐标转换矩阵。移动机器人状态定义为 畸变校正 →点云配准 估计紧耦合 算法 x;=R:p.v.bi LDAR+特征分割 地图配淮→局部地图 式中：R∈SO(3)为旋转矩阵；p,∈R3表示位置向 量；，和b:分别为载体速度和IMU漂移；T∈SE(3) 图1算法流程图 表示从载体系到导航系的转换矩阵，即T=[RP。 Fig.1 The flowchart of the algorithm 基于上述分析，移动机器人在t+△：时刻的速 通过融合LiDAR点云描述和机器人运动学 度、位置和旋转可以分别表示为 信息，进一步分析移动平台颠簸对LiDAR点云畸 v+=y,+g△1+C(a,-b-n)△ 变和视觉图像模糊的影响。具体方案如下： 1)针对机器人的姿态变化引起的点云图像扰 Aw=R+w,A+58Ar+C宁a-所-g9ar 动，采用组合导航系统信息对坐标转换中的旋转 R+=Cexp(o-b-n)△1 矩阵R和平移T进行初始化，当导航坐标系、载 利用IMU预积分理论计算相邻i和j时刻 体坐标系和第i个传感器坐标系分别表示为{}、 机器人相对位姿变化量，△、△P、△R;分别表 {b}、{S},则环境传感器测量点P,转换到导航系 示为 可以表示为式(1)： △v=R(yj--g△) np=f(Ψ，Φ，p)=Rg(RP+T)+T6 (1) △P=R(p-p.-△t-8A) △R=RR 式中：平，Φ分别为机器人在导航坐标{m,b1,{S 针对车体快速转弯等运动情形，考虑在一个 系下的位姿和第i传感器相对于b系的位姿。 优化任务里执行点云畸变补偿和点云匹配，计算 2)对三维点云数据的不确定性和量测点误差 激光点采集时刻和相对于该帧点云初始时刻的姿 进行研究并建立对应误差模型，将量测点投影到 态变化量，在点云配准时将畸变补偿的残差作为 二维平面，则其分布在栅格的概率可用黎曼分 误差函数的一部分，对LiDAR位姿之间的IMU 布近似表示为式(2)： 运动信息进行积分，并将积分得到的相对位姿于 P(np.,nn,)eg)≈ 因子图中的匹配位姿实施图优化，实验结果表 --0:-)P54+,4+4） (2) 2 2 明，该方法可以进一步提高定位和建图的精度。 利用式(3)对多层环境栅格地图的每个栅格 2.2点云分割与特征提取 平均高程值进行更新： 本文在点云分割和特征提取阶段，考虑加入 aen 基于区域生长的分割方法和地面提取算法，根据 N二1 m=1 (3) 被分割物体的点云个数判断其是否为稳定特征， ∑P. 进而对地面提取之后剩余的点云进行聚类分析， 式中：P.为第n高程值在栅格内的概率；en为高 可以在一定程度上减小动态物体对建图的影响， 程的均值。 同时可以有效降低Z轴方向的漂移误差。在激 3)针对环境特征点稀少引起的近似误差增大 光里程计模块，基于提取的特征点构建scan-to- 和目标函数陷入极小值点的问题，可以采用闭环 scan的约束关系，经过两次Levenberg-Marquardt 检测等方式抑制漂移的影响。在此基础上，利用 优化得到姿态变换矩阵。 高斯过程模型对LiDAR运动速度进行估计并利 假设在1时刻获取的激光点云为P,={p1,P2,…, 用惯性信息修正位姿，进而对环境模型更新进行 P,VLP-l6线激光雷达的水平和垂直方向角分辨 优化，实现机器人环境特征的完整映射。 率为0.2°和2°。将地面和目标分割后，在特征提构成的小回环为转角提供更多约束，可用于提升 激光里程计的鲁棒性，而大回环则可以通过建立 闭环约束并修正全局累计误差；5) 基于紧耦合的 数据融合，包含 LiDAR 里程计、IMU 预积分和地 图配准等，进而得到机器人全局一致的位姿。本 文所提算法的流程图如图 1 所示，其中标号为文 中对应公式。 IMU LiDAR 预积分 畸变校正 特征分割 地图配准 局部地图 点云配准 紧耦合 算法 运动估计 特征点云 位姿 估计 图 1 算法流程图 Fig. 1 The flowchart of the algorithm 通过融合 LiDAR 点云描述和机器人运动学 信息，进一步分析移动平台颠簸对 LiDAR 点云畸 变和视觉图像模糊的影响。具体方案如下： R T i P S i 1) 针对机器人的姿态变化引起的点云图像扰 动，采用组合导航系统信息对坐标转换中的旋转 矩阵 和平移 进行初始化，当导航坐标系、载 体坐标系和第 个传感器坐标系分别表示为{n}、 {b}、{Si}，则环境传感器测量点 转换到导航系 可以表示为式 (1)： np = f(Ψ,Φ,P S i ) = R n b (R b si P S i +T b si )+T n b (1) Ψ,Φ {n},{b},{S i} i b 式中： 分别为机器人在导航坐标 系下的位姿和第 传感器相对于 系的位姿。 gi j 2) 对三维点云数据的不确定性和量测点误差 进行研究并建立对应误差模型，将量测点投影到 二维平面，则其分布在栅格 的概率可用黎曼分 布近似表示为式 (2)： P ( (npx ,npy ) ∈ gi j) ≈ (xi+ − xi−)(yi+ −yi−)· P ( xi+ + xi− 2 , yi+ +yi− 2 ) (2) 利用式 (3) 对多层环境栅格地图的每个栅格 平均高程值进行更新： ξˆ N = ∑N n=1 Pneˆn ∑N n=1 Pn (3) 式中： Pn 为第 n 高程值在栅格内的概率；eˆn 为高 程的均值。 3) 针对环境特征点稀少引起的近似误差增大 和目标函数陷入极小值点的问题，可以采用闭环 检测等方式抑制漂移的影响。在此基础上，利用 高斯过程模型对 LiDAR 运动速度进行估计并利 用惯性信息修正位姿，进而对环境模型更新进行 优化，实现机器人环境特征的完整映射。 2 LiDAR/INS 数据融合方法 2.1 数据预处理 IMU 在 t 时刻的角速度和加速度观测模型可 表示为 ωˆ t = ωt +b ω t +n ω t aˆt = C bn t (at −g)+b a t +n a t ωˆ t aˆt b ω t b a t n ω t n a t C bn t 式中： 和 分别为 IMU 原始数据； 、 和 、 分别表示对应输出信号中的漂移和高斯白噪 声； 为坐标转换矩阵。移动机器人状态定义为 xi = [ Ri pi vi bi ] Ri ∈ S O(3) pi ∈ R 3 vi bi T ∈ S E(3) T = [ Ri pi ] 式中： 为旋转矩阵； 表示位置向 量； 和 分别为载体速度和 IMU 漂移； 表示从载体系到导航系的转换矩阵，即 。 基于上述分析，移动机器人在 t+ ∆t 时刻的速 度、位置和旋转可以分别表示为 vt+∆t = vt+g∆t+C bn t (ˆat −b a t −n a t )∆t pt+∆t = pt+vt∆t+ 1 2 g∆t 2+ 1 2 C bn t (ˆat −b a t −n a t )∆t 2 Rt+∆t = C bn t exp( ˆωt −b ω t −n ω t )∆t i j ∆vi j、∆pi j、∆Ri j 利用 IMU 预积分理论计算相邻 和 时刻 机器人相对位姿变化量， 分别表 示为 ∆vi j = R T i (vj −vi −g∆ti j) ∆pi j = R T i (pj − pi −vi∆ti j − 1 2 g∆t 2 i j) ∆Ri j = R T i Rj 针对车体快速转弯等运动情形，考虑在一个 优化任务里执行点云畸变补偿和点云匹配，计算 激光点采集时刻和相对于该帧点云初始时刻的姿 态变化量，在点云配准时将畸变补偿的残差作为 误差函数的一部分，对 LiDAR 位姿之间的 IMU 运动信息进行积分，并将积分得到的相对位姿于 因子图中的匹配位姿实施图优化，实验结果表 明，该方法可以进一步提高定位和建图的精度。 2.2 点云分割与特征提取 Z 本文在点云分割和特征提取阶段，考虑加入 基于区域生长的分割方法和地面提取算法，根据 被分割物体的点云个数判断其是否为稳定特征， 进而对地面提取之后剩余的点云进行聚类分析， 可以在一定程度上减小动态物体对建图的影响， 同时可以有效降低 轴方向的漂移误差。在激 光里程计模块，基于提取的特征点构建 scan-to￾scan 的约束关系，经过两次 Levenberg-Marquardt 优化得到姿态变换矩阵。 t Pt = {p1, p2,··· , pn} 假设在 时刻获取的激光点云为 ，VLP-16 线激光雷达的水平和垂直方向角分辨 率为 0.2°和 2°。将地面和目标分割后，在特征提 ·806· 智 能 系 统 学 报 第 15 卷