CN111929699B - 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 - Google Patents

Abstract

本发明提供一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统，其中方法包括数据预处理，还包括以下步骤：顾及动态障碍物的特征提取；进行激光点云帧间匹配和ESKF数据融合；利用帧与局部地图进行精细点云匹配，优化激光里程计的位姿，建立全局的3D点云地图。本发明提出的顾及动态障碍物的激光雷达惯导里程计与建图方法及系统，采用深度图像的方式去除道路环境中的动态障碍物，方案不同，采用三种特征点云分类别分步骤的进行点云匹配和位姿求解，充分的问题，利用ESKF框架融合惯导的预积分信息和激光雷达帧间匹配信息，提高位姿估计的精度。

Description

一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统

技术领域

本发明涉及AB测试的技术领域，特别是一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统。

背景技术

准确的位姿估计是移动机器人定位与导航的关键技术，它能够计算出机器人的位置和姿态信息，为机器人的实时控制、决策和路径规划提供重要保障。

SLAM技术能够实现移动机器人在未知环境中的自我运动估计即“定位”，同时建立周围环境地图。按照搭载传感器的不同，SLAM技术可大致分为激光SLAM和视觉SLAM两大类[1]。基于3D激光雷达的SLAM技术无需GNSS信号，可直接获取空间三维信息，不受光照变化影响、不存在尺度漂移现象，同时无需预先布置场景，可融合多传感器生成便于导航的环境地图，因此成为目前定位方案中不可或缺的新技术[2]。另外，仅依靠3D激光SLAM进行位姿估计存在诸多局限，输出位姿的频率过低，且随着行驶距离增加，会产生较大的累计误差，不能满足机器人快速定位与导航的需求。而微机电(MicroElectroMechanicalSystem，MEMS)惯导成本低廉，能够输出高频率的三轴加速度和角速度信息，具有较高的角速度测量精度，可以在激光雷达扫描之间提供稳健的状态估计。通过多传感器融合，能够弥补单一传感器进行状态估计的不足，可以提供更可靠的位置和姿态信息。

Zhang等人提出的激光雷达里程计与地图构建方案(LidarOdometryandMapping，LOAM)，它仅是个激光里程计算法，无回环检测和后端优化，LOAM方案将复杂的SLAM问题划分为两个算法的结合，即高频的粗略配准和低频的精细配准。Shan等人提出轻量级和地面优化的LOAM(LightweightandGroundOptimizedLOAM，Lego-LOAM)，在LOAM基础上加入数据预处理、回环检测、后端优化，后端采用基于增量平滑的非线性优化库，是较完整的3D激光SLAM方案。Zhang等人提出在LOAM基础上利用视觉里程计高频的估计位姿，激光里程计低频的优化位姿，并校准漂移；Ye等人提出一种基于图优化的激光雷达惯导紧耦合里程计方案，经过实际测试，该算法过于耗时，为了达到实时输出位姿，被动采用跳帧处理，室内跳一帧计算，室外跳两帧计算。Qin等人在Lego-LOAM的基础上加入迭代误差状态卡尔曼滤波数据融合框架；李等人在Lego-LOAM中加入惯导因子进行后端因子图优化。Behley等人提出一种稠密的基于Surfel的3D激光SLAM方案。Zuo等人提出一种基于MSCKF融合激光惯导视觉的里程计方法。Lin等人[11]提出一种基于固态激光雷达的的里程计方法。Li等人提出一种扫描到扫描的实时激光里程计估计网络。Zhang等人提出激光雷达视觉惯导耦合的SLAM方案，能够实现较高精度的里程计。然而，由于上述方法均假设周围环境是静态的，没有顾及到动态障碍物对位姿估计和建图的影响，因此这些方法很难在室外动态道路环境中实现低漂移、鲁棒的位姿估计。

发明内容

为了解决上述的技术问题，本发明提出的顾及动态障碍物的激光雷达惯导里程计与建图方法及系统，采用深度图像的方式去除道路环境中的动态障碍物，方案不同，采用三种特征点云分类别分步骤的进行点云匹配和位姿求解，充分的问题，利用ESKF框架融合惯导的预积分信息和激光雷达帧间匹配信息，提高位姿估计的精度。

本发明的第一目的是提供一种顾及动态障碍物的激光雷达惯导里程计与建图方法，数据预处理，还包括以下步骤：

步骤1：顾及动态障碍物的特征提取；

步骤2：进行激光点云帧间匹配和ESKF数据融合；

步骤3：利用帧与局部地图进行精细点云匹配，优化激光里程计的位姿，建立全局的3D点云地图。

优选的是，所述数据预处理是指通过高频的MEMS惯导辅助激光雷达，对每一帧内的所有激光点云进行畸变补偿校正。

在上述任一方案中优选的是，所述畸变补偿校正的方法包括以下子步骤：

步骤01：首先找到与当前激光点的时间戳ｔ_ｃurr最接近的时间戳ｔ_ｋ和ｔ_k+1的连续惯导测量值；

步骤02：通过惯导积分过程，分别得到惯导在时间k和时间k+1的世界坐标系W中的状态，公式为：

其中，p表示位置，v表示速度，θ表示角向量，x表示x轴朝向，y表示y轴朝向，z表示z轴朝向；

步骤03：将世界坐标系W定义为起始惯导坐标系，可利用时间因子线性插值来确定惯导在时刻ｔ_curr在世界坐标系中的状态

公式为

其中，ratio₁和ratio₂为时间插值因子，t_k表示k时刻的时间戳，t_k+1表示k+1时刻的时间戳；

步骤04：利用

和

能够得到当前激光点和起始激光点所对应惯导的位置

和惯导的速度

在起始激光点坐标中得到每个激光点的运动畸变量，公式为

其中，

表示激光雷达当前帧起始点坐标系与世界坐标系之间的变换矩阵，i表示一帧点云数据中每个点占据一帧扫描周期的时间，

表示当前激光点所对应的惯导在世界坐标系下的状态，

表示起始激光点所对应的惯导在世界坐标系下的状态。

在上述任一方案中优选的是，所述步骤1包括剔除道路环境中的动态障碍物，并从静态点云中提取特征点，所述特征点包括地面特征点、非地面线特征点和非地面表面特征点。

在上述任一方案中优选的是，所述动态障碍物的剔除方法为提取地面点云，对剩余的非地面点云进行目标分割并聚类，将小于设定尺寸的目标剔除。

在上述任一方案中优选的是，所述地面点云的提取方法为采用基于深度图像的角度分割方式提取地面点，将每一帧内的激光点云投影为深度图像，将无序杂乱的非结构化点云数据转化成有组织的点云数据；深度图像中的行数r由激光雷达垂直方向的激光线束数量决定，列数c由激光雷达的水平分辨率决定；

点A和点B是激光雷达在同一水平分辨率下发射，在竖直方向的相邻激光点，角度和对应在深度图像中r-1和r行激光束的垂直角度；

定义一个竖直角度α，将深度图像R的每列c转换成角度

的形式，已知两个相邻垂直激光束的深度值，角度α的计算公式为：

其中，R_r-1,c表示第r-1行第c列激光点的深度值，R_r,c表示第r行第c列激光点的深度值，θ₁和θ₂表示不同激光束在竖直方向上的角度，

每一个角度α计算需要两个距离读数，将这些角度

的所有集合视为矩阵M_α，得到夹角图像，将Savitsky-Golay平滑算法应用在矩阵M_α的每一列，在夹角图像上进行地面标记，从期望属于地面的行开始，使用广度优先搜索将相似的组件标记在一起。

在上述任一方案中优选的是，所述目标分割的方法为对剩余的3D激光点云进行目标聚类，即将3D激光点云圆柱投影到2D深度图像，基于点云深度图的实时目标分割，然后从每一帧激光雷达数据中剔除动态障碍物，包括以下子步骤：

步骤11：计算在深度图像中行方向或列方向上相邻点A和B的角度β；

步骤12：当所述角度β小于阈值θ，则认为两条激光束的测量深度值变化过大，将扫描得到的两点分割为不同的类别，它们之间的夹角β大于阈值θ，则将它们视为来自同一对象；

步骤13：处理动态障碍物，将目标点云的包围盒在X方向、Y方向、Z方向小于设定阈值的剔除；

步骤14：把地面点云重新加载进来。

在上述任一方案中优选的是，将每一帧的地面点云使用体素栅格滤波的方法进行不同程度的下采样，标记为地面特征点P^G(more)和点P^G(less)。

在上述任一方案中优选的是，设置平滑度阈值，将平滑度大于所述平滑度阈值的视为的点记为非地面线特征点P^E，将平滑度小于所述平滑度阈值的点记为非地面表面特征点P^S，从P^E中挑选n_E个u值最大的点构成P^E(max)，并满足P^E(max)∈P^E，从P^S中挑选个u值最小的点构成P^S(min)，并满足P^S(min)∈P^S。

在上述任一方案中优选的是，所述平滑度的计算方法包括以下子步骤：

步骤120：在同一扫描内，对于任意点A，选取左右各五个点；

步骤121：每个点的X坐标与点A的X坐标求差，将所有的差求和，得到

步骤122：每个点的Y坐标与点A的Y坐标求差，将所有的差求和，得到

步骤123：每个点的Z坐标与点A的Z坐标求差，将所有的差求和，得到

步骤124：计算点A的平滑度，公式为u＝(u_x)²+(u_y)²+(u_z)²。

在上述任一方案中优选的是，在所述特征点提取时，使用过滤策略对所述特征点进行过滤，移除实时帧中不稳定的噪声点。

在上述任一方案中优选的是，所述过滤策略包括：

1)当飞行时间小于阈值，返回的激光点会带有误差，需要去除深度值小的点；

2)去掉视场边缘的点即深度值大的点；

3)去除激光束和平面夹角过小的点，定义点A的深度值为P_A，点D的深度值为P_D，点E的深度值为P_E，计算激光束和局部平面的入射夹角，如果所述入射夹角接近180°或者0°，则删除该激光点，所述入射夹角的计算公式为

4)去除部分被遮挡的激光点，定义点B的深度值为P_B，点A的深度值为P_A，遮挡点的判别公式为|P_B|<|P_A|，|P_B-P_A|≥0.1|P_A|。

在上述任一方案中优选的是，所述激光点云帧间匹配包括基于三种特征点云进行分类别的帧间匹配与分步骤的位姿解算。

在上述任一方案中优选的是，所述分类别的帧间匹配的方法包括：

1)对于地面特征点，在

中索引

的对应匹配点，构建

的点到平面的距离约束，其中，

为上一帧中数量较多的地面特征点，

为当前帧中数量较少的地面特征点；

2)对于非地面线特征点，在

中索引

的对应匹配点，构建

的点到直线的距离约束，其中，

为上一帧中数量较多的非地面线特征点，

为当前帧中数量较少的非地面线特征点；

3)对于非地面表面特征点，在

中索引

的对应匹配点，构建

的点到平面的距离约束，其中，

为上一帧中数量较多的非地面表面特征点，

为当前帧中数量较少的非地面表面特征点。

在上述任一方案中优选的是，所述分步骤的位姿解算方法为将位姿中的六个变量分成三组，分步骤的求解位姿{t_x，t_y，t_z，θ_roll，θ_pitch，θ_yaw}，使用地面特征点、非地面线特征点和非地面表面特征点依次进行L-M算法位姿解算。

在上述任一方案中优选的是，所述ESKF数据融合的方法包括以下子步骤：

步骤201：进行状态定义，真实状态s、预测状态

和误差状态δs的关系公式为：

其中，b_a为加速度偏差，b_g为角速度偏差其中，b_a为加速度偏差，b_g为角速度偏差；

步骤:202：通过惯导数据预积分实现离散传播，传播预测状态、误差状态以及误差状态的协方差矩阵；

步骤203：进行状态更新，将状态更新转化为误差状态与点线距离或者点面距离的最小化，随着激光雷达帧间匹配估计的位姿测量值，计算卡尔曼增益矩阵，并更新全误差状态和误差状态的协方差矩阵。

本发明的第二目的是提供一种顾及动态障碍物的激光雷达惯导里程计与建图系统，包括预处理模块，还包括以下模块：

特征提取模块：用于顾及动态障碍物的特征提取；

激光里程计模块：用于进行激光点云帧间匹配和ESKF数据融合；

激光建图模块：用于利用帧与局部地图进行精细点云匹配，优化激光里程计的位姿，建立全局的3D点云地图；

所述系统按照如第一目的所述的方法自动生成3D点云地图。

本发明提出了一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统，能够模块，避免在动态物体上提取不稳定的特征点，提高计算效率和位姿估计的精度。

附图说明

图1为按照本发明的顾及动态障碍物的激光雷达惯导里程计与建图方法的一优选实施例的流程图。

图2为按照本发明的顾及动态障碍物的激光雷达惯导里程计与建图系统的一优选实施例的模块图。

图3为按照本发明的顾及动态障碍物的激光雷达惯导里程计与建图方法的一优选实施例的从点云自动重建室内语义模型流程图。

图4为按照本发明的顾及动态障碍物的激光雷达惯导里程计与建图方法的一优选实施例的道路场景示意图。

图5为按照本发明的顾及动态障碍物的激光雷达惯导里程计与建图方法的一优选实施例的激光雷达场景示意图。

图6为按照本发明的顾及动态障碍物的激光雷达惯导里程计与建图方法的一优选实施例的筛选点示意图。

图7为按照本发明的顾及动态障碍物的激光雷达惯导里程计与建图方法的一优选实施例的分步骤优化求解示意图。

具体实施方式

下面结合附图和具体的实施例对本发明做进一步的阐述。

实施例一

如图1、2所示，执行步骤100，预处理模块200进行数据预处理，通过高频的MEMS惯导辅助激光雷达，对每一帧内的所有激光点云进行畸变补偿校正。

畸变补偿校正的方法包括以下子步骤：

步骤01：首先找到与当前激光点的时间戳t_curr最接近的时间戳t_k和t_k+1的连续惯导测量值；

步骤02：通过惯导积分过程，分别得到惯导在时间k和时间k+1的世界坐标系W中的状态，公式为：

其中，p表示位置，v表示速度，θ表示角向量，x表示x轴朝向，y表示y轴朝向，z表示z轴朝向；

步骤03：将世界坐标系W定义为起始惯导坐标系，可利用时间因子线性插值来确定惯导在时刻t_curr在世界坐标系中的状态

公式为

其中，ratio₁和ratio₂为时间插值因子，t_k表示k时刻的时间戳，t_k+1表示k+1时刻的时间戳；

步骤04：利用

和

能够得到当前激光点和起始激光点所对应惯导的位置

和惯导的速度

在起始激光点坐标中得到每个激光点的运动畸变量，公式为

其中，

表示激光雷达当前帧起始点坐标系与世界坐标系之间的变换矩阵，i表示一帧点云数据中每个点占据一帧扫描周期的时间，

表示当前激光点所对应的惯导在世界坐标系下的状态，

表示起始激光点所对应的惯导在世界坐标系下的状态。

执行步骤110，特征提取模块210进行顾及动态障碍物的特征提取，剔除道路环境中的动态障碍物，并从静态点云中提取特征点，所述特征点包括地面特征点、非地面线特征点和非地面表面特征点。

动态障碍物的剔除方法为提取地面点云，对剩余的非地面点云进行目标分割并聚类，将小于设定尺寸的目标剔除。

地面点云的提取方法为采用基于深度图像的角度分割方式提取地面点，将每一帧内的激光点云投影为深度图像，将无序杂乱的非结构化点云数据转化成有组织的点云数据；深度图像中的行数r由激光雷达垂直方向的激光线束数量决定，列数c由激光雷达的水平分辨率决定；

点A和点B是激光雷达在同一水平分辨率下发射，在竖直方向的相邻激光点，角度和对应在深度图像中r-1和r行激光束的垂直角度；

定义一个竖直角度α，将深度图像R的每列c转换成角度

的形式，已知两个相邻垂直激光束的深度值，角度α的计算公式为：

其中，R_r-1,c表示第r-1行第c列激光点的深度值，R_r,c表示第r行第c列激光点的深度值，θ₁和θ₂表示不同激光束在竖直方向上的角度，

每一个角度α计算需要两个距离读数，将这些角度

的所有集合视为矩阵M_α，得到夹角图像，将Savitsky-Golay平滑算法应用在矩阵M_α的每一列，在夹角图像上进行地面标记，从期望属于地面的行开始，使用广度优先搜索将相似的组件标记在一起。

目标分割的方法为对剩余的3D激光点云进行目标聚类，即将3D激光点云圆柱投影到2D深度图像，基于点云深度图的实时目标分割，然后从每一帧激光雷达数据中剔除动态障碍物，包括以下子步骤：

步骤11：计算在深度图像中行方向或列方向上相邻点A和B的角度β；

步骤12：当所述角度β小于阈值θ，则认为两条激光束的测量深度值变化过大，将扫描得到的两点分割为不同的类别，它们之间的夹角β大于阈值θ，则将它们视为来自同一对象；

步骤13：处理动态障碍物，将目标点云的包围盒在X方向、Y方向、Z方向小于设定阈值的剔除；

步骤14：把地面点云重新加载进来。将每一帧的地面点云使用体素栅格滤波的方法进行不同程度的下采样，标记为地面特征点P^G(more)和点P^G(less)。设置平滑度阈值，将平滑度大于所述平滑度阈值的视为的点记为非地面线特征点P^E，将平滑度小于所述平滑度阈值的点记为非地面表面特征点P^S，从P^E中挑选n_E个u值最大的点构成P^E(max)，并满足P^E(max)∈P^E，从P^S中挑选个u值最小的点构成P^S(min)，并满足P^S(min)∈P^S所述平滑度的计算方法包括以下子步骤：

步骤120：在同一扫描内，对于任意点A，选取左右各五个点；

步骤121：每个点的X坐标与点A的X坐标求差，将所有的差求和，得到

步骤122：每个点的Y坐标与点A的Y坐标求差，将所有的差求和，得到

步骤123：每个点的Z坐标与点A的Z坐标求差，将所有的差求和，得到

步骤124：计算点A的平滑度，公式为u＝(u_x)²+(u_y)²+(u_z)²。

在所述特征点提取时，使用过滤策略对所述特征点进行过滤，移除实时帧中不稳定的噪声点。所述过滤策略包括：

1)当飞行时间小于阈值，返回的激光点会带有误差，需要去除深度值小的点；

2)去掉视场边缘的点即深度值大的点；

3)去除激光束和平面夹角过小的点，定义点A的深度值为P_A，点D的深度值为P_D，点E的深度值为P_E，计算激光束和局部平面的入射夹角，如果所述入射夹角接近180°或者0°，则删除该激光点(为入射夹角过小的激光点即激光束和局部平面近似平行的激光点)，所述入射夹角的计算公式为

4)去除部分被遮挡的激光点，定义点B的深度值为P_B，点A的深度值为P_A，遮挡点的判别公式为|P_B|<|P_A|，|P_B-P_A|≥0.1|P_A|。

执行步骤120，激光里程计模块220进行激光点云帧间匹配和ESKF数据融合。激光点云帧间匹配包括基于三种特征点云进行分类别的帧间匹配与分步骤的位姿解算。分类别的帧间匹配的方法包括：

1)对于地面特征点，在

中索引

的对应匹配点，构建

的点到平面的距离约束，其中，

为上一帧中数量较多的地面特征点，

为当前帧中数量较少的地面特征点；

2)对于非地面线特征点，在

中索引

的对应匹配点，构建

的点到直线的距离约束，其中，

为上一帧中数量较多的非地面线特征点，

为当前帧中数量较少的非地面线特征点；

3)对于非地面表面特征点，在

中索引

的对应匹配点，构建

的点到平面的距离约束，其中，

为上一帧中数量较多的非地面表面特征点，

为当前帧中数量较少的非地面表面特征点。

分步骤的位姿解算方法为将位姿中的六个变量分成三组，分步骤的求解位姿{t_x，t_y，t_z，θ_roll，θ_pitch，θ_yaw}，使用地面特征点、非地面线特征点和非地面表面特征点依次进行L-M算法位姿解算。

ESKF数据融合的方法包括以下子步骤：

步骤201：进行状态定义，真实状态s、预测状态

和误差状态δs的关系公式为：

其中，b_a为加速度偏差，b_g为角速度偏差其中，b_a为加速度偏差，b_g为角速度偏差；

步骤:202：通过惯导数据预积分实现离散传播，传播预测状态、误差状态以及误差状态的协方差矩阵；

步骤203：进行状态更新，将状态更新转化为误差状态与点线距离或者点面距离的最小化，随着激光雷达帧间匹配估计的位姿测量值，计算卡尔曼增益矩阵，并更新全误差状态和误差状态的协方差矩阵。

执行步骤130，激光建图模块230利用帧与局部地图进行精细点云匹配，优化激光里程计的位姿，建立全局的3D点云地图。

实施例二

本发明在LOAM、Lego-LOAM方案基础上进行改进，重点改进它们的特征提取和激光里程计部分，主要包括以下三个方面：

1)在特征点提取之前增加动态障碍物剔除模块，避免在动态物体上提取不稳定的特征点，从而影响正确的数据关联。

2)与LOAM方案和Lego-LOAM方案不同，采用三种特征点云分类别分步骤的进行点云匹配和位姿求解，提高计算效率。

3)针对LOAM方案和Lego-LOAM方案融合惯导数据不充分的问题，利用ESKF框架融合惯导的预积分信息和激光雷达帧间匹配信息，提高位姿估计的精度。

本申请的改进方案的框架如图3所示。

1.1数据预处理

由于在动态的环境中，搭载激光雷达的载体不可避免的产生非线性运动，导致扫描的激光点云存在非线性运动畸变，因此通过高频的MEMS惯导辅助激光雷达，对每一帧内的所有激光点云进行补偿校正。畸变补偿方法如下：

首先找到与当前激光点的时间戳t_curr最接近的时间戳t_k和t_k+1的连续惯导测量值。通过惯导积分过程，分别得到惯导在时间k和时间k+1的世界坐标系W中的状态，如公式(1)所示：

其中，p表示位置，v表示速度，θ表示角向量，x表示x轴朝向，y表示y轴朝向，z表示z轴朝向，将世界坐标系W定义为起始惯导坐标系，可利用时间因子线性插值来确定惯导在时刻t_curr在世界坐标系中的状态

如公式(2)所示：

其中ratio₁和ratio₂为时间插值因子。

需要解决的是由于非线性运动引起的当前激光点相对于起始激光点的运动畸变。用

和

能够得到当前激光点和起始激光点所对应惯导的位置

和惯导的速度

在起始激光点坐标中得到每个激光点的运动畸变量，如公式(3)所示：

其中，

表示激光雷达当前帧起始点坐标系与世界坐标系之间的变换矩阵，i表示一帧点云数据中每个点占据一帧扫描周期的时间。将每一帧激光点云转换到每帧激光雷达起点坐标系中，将每个激光点的原始坐标减去每个激光点的点云运动畸变量。

1.2顾及动态障碍物的特征提取

由于每帧原始激光点云包含数据量较大，使用全部点匹配来估计位姿，对计算资源的消耗巨大，可利用特征点云代替全部点云进行点云配准恢复位姿。此外，道路场景中存在许多动态障碍物，例如行人、车辆等，这些动态物体在点云匹配过程中是不稳定的噪声点，会影响点云匹配精度，从而导致激光里程计精度低。因此本模块首先剔除道路环境中的动态障碍物，然后从静态点云中提取特征点。

1.2.1动态障碍物处理

去除动态障碍物的手段是基于几何的方式，包括地面提取和目标分割两部分，首先提取地面点云，然后对剩余的非地面点云进行目标分割并聚类，将小于设定尺寸的目标剔除。

(1)地面提取

考虑到系统的实时性以及地面提取的准确性，采用基于深度图像的角度分割方式提取地面点。将每一帧内的激光点云投影为深度图像，将无序杂乱的非结构化点云数据转化成有组织的点云数据。深度图像中的行数r由激光雷达垂直方向的激光线束数量决定，列数c由激光雷达的水平分辨率决定。

道路场景如图4所示，点点A和点B是激光雷达在同一水平分辨率下发射，在竖直方向的相邻激光点，角度和对应在深度图像中r-1和r行激光束的垂直角度。

定义一个竖直角度，将深度图像R的每列c转换成角度的形式。已知两个相邻垂直激光束的深度值，角度的计算方法如公式(4)所示：

每一个角度α计算需要两个距离读数，将这些角度

的所有集合视为矩阵M_α，得到夹角图像，将Savitsky-Golay平滑算法应用在矩阵M_α的每一列，在夹角图像上进行地面标记，从期望属于地面的行开始，使用广度优先搜索(Breadth-FirstSearch,BFS)将相似的组件标记在一起。通常将角度α<2°且同一列相邻两点的高

差，视为地面点所对应的区域。

(2)目标分割

接下来对剔除地面点云后的深度图像进行实时目标分割，从每一帧激光雷达数据中剔除动态障碍物。

如图5所示，图中表示激光雷达的XOY平面，从位于O处的激光雷达发射激光束OA和OB，得到点A和点B，点C为点B在OA上的垂直投影点，假设Y轴沿着两条激光束中较长的方向，此处参数μ为两条激光线束的夹角。角度β是深度值较大的扫描线与相邻扫描线构成的AB之间的夹角，定义如公式(5)所示：

其中‖OB‖为第一条激光线束的深度值，对应d₁，‖OA‖为第二条激光线束的深度值，对应d₂，μ为角分辨率，计算在深度图像中行方向或列方向上相邻点A和B的角度β。角度β背后的含义是它对于大多数单个物体保持相对较大，此处需要设置一个分界值参数θ，该分界值用于判断将深度图像中的相邻两激光点分割为两个类别还是合并为一个类别。因此，可以将目标分割看作成利用深度图像结构和β的约束来寻找连通的二维分量的问题，直接对深度图像进行分割。

剔除地面点云后，接下来对剩余的3D激光点云进行目标聚类，此处采用的方法是将3D激光点云圆柱投影到2D深度图像，基于点云深度图的实时目标分割，然后从每一帧激光雷达数据中剔除动态障碍物。定义一个参数β，计算在深度图像中行方向或列方向上相邻点A和B的角度β。在第一种情况下，角度β对应行方向上的角度增量，而另一种情况对应列方向上的增量。角度β背后的含义是它对于大多数单个物体保持相对较大，此处需要设置一个阈值参数θ，该分界值用于判断将深度图像中的相邻两激光点分割为两个类别还是合并为一个类别。如果角度β小于阈值θ，则认为两条激光束的测量深度值变化过大，将扫描得到的两点分割为不同的类别，如果深度图像中相邻的两个激光点，它们之间的夹角β大于阈值θ，则将它们视为来自同一对象。因此，可以将目标分割看作成利用深度图像结构和β的约束来寻找连通的二维分量的问题，直接对深度图像进行分割。连接同一物体的过程采用BFS算法。当目标分割全部完毕即目标点云聚类后，开始处理动态障碍物，由于道路上的车辆和行人的包围盒是有阈值限制的将目标点云的包围盒在X方向、Y方向、Z方向小于设定阈值的剔除，认为是道路上的动态障碍物，例如行人、车辆等。障碍物剔除完毕后，将地面点云重新加载进来。

连接组件时使用具有复杂度O(N)的直通滤波器的变体，其中N是像素的数量，即每次扫描的距离读数的数量，保证最多访问深度图像中的每个点两次。此时，标记为地面的深度图像的所有像素都设置为零，并且不参与以下过程：从深度图像的左上角开始，从上到下、从左到右遍历每个像素，每当遇到未被标记的像素，从该像素开始BFS，搜索的目标是标记此组件的每个像素。BFS使用队列和N4邻域，根据相邻点和当前点生成的角度β，决定是否将N4邻域中的点添加到BFS队列中，此过程保证整个连接的组件将获得相同的标签。若BFS的队列为空，将继续按顺序遍历深度图像，直至到达一个新的未标记点。

当目标分割全部完毕即目标点云聚类后，开始处理动态障碍物，将目标点云的包围盒在X方向、Y方向、Z方向小于设定阈值的剔除，认为是道路上的动态障碍物，例如行人、车辆等。当动态障碍物剔除完毕后，将提取的地面点云重新加载进来。

1.2.1特征点提取

(1)三类特征点提取

对于地面特征点提取方法见1.2.1节地面提取，基本原理是在深度图像基础上加入几何约束，利用竖直角度α和高程差

联合约束地面点，由于激光雷达是水平安置在载体上，这种地面提取方法的优势是并不严格要求地面处于完全水平状态。将每一帧的地面点云使用体素栅格滤波的方法进行不同程度的下采样，标记为地面特征点P^G(more)和点P^G(less)。

在剔除动态障碍物后，将剩余的非地面点云进行线特征点和表面特征点的提取，定义平滑度u，步骤如下：

1)在同一扫描内，对于任意点A，选取左右各五个点；

2)每个点的X坐标与点A的X坐标求差，将所有的差求和，得到u_x；

3)每个点的Y坐标与点A的Y坐标求差，将所有的差求和，得到u_y；

4)每个点的Z坐标与点A的Z坐标求差，将所有的差求和，得到u_z；

5)计算平滑度：u＝(u_x)²+(u_y)²+(u_z)²。

需要说明的是，每个扫描的边缘5个点不参与特征点选取，因为不满足前后五个点计算平滑度的条件。为了防止一帧数据中的特征点过于聚集，将每一帧点云中每个扫描从起始扫描到终止扫描均分为六份。设置平滑度阈值。线特征点选择条件：将平滑度大于阈值的点视为线特征点，若该点前后五个点中已存在线特征点，则跳过该点，从平滑度更小的点中选择。表面特征点选择条件：将平滑度小于阈值的点视为表面特征点，若该点前后五个点中已存在表面特征点，则跳过该点，从平滑度更大的点中选择。

对于每一帧点云，将u值较大的点标记为线特征点P^E，将u值较小的点标记为表面特征点P^S，从P^E中挑选n_E个u值最大的点构成P^E(max)，并满足P^E(max)∈P^E，从P^S中挑选个u值最小的点构成P^S(min)，并满足P^S(min)∈P^S。对每一帧激光点云进行三种特征点提取，包括地面特征点、非地面线特征点、非地面表面特征点，并对每一种特征点云又分为两份进行存储，即P^{G (more)}和点P^G(less)、P^E和P^E(max)、P^S和P^S(min)。

(2)筛选点策略

在特征点提取时设计了四种过滤特征点的策略，移除实时帧中不稳定的噪声点。

1)由于TOF型机械激光雷达的工作原理，当飞行时间小于阈值，返回的激光点会带有误差，需要去除深度值小的点；

2)由于激光雷达视场边缘的点存在变形，会产生大曲率现象，影响特征提取的质量，因此去掉视场边缘的点即深度值大的点；

3)去除激光束和平面夹角过小的点，即激光束和平面接近平行的情况。如图6所示，定义点A的深度值为P_A，点D的深度值为P_D，点E的深度值为P_E，计算激光束和局部平面的入射夹角，如公式(6)所示，若入射夹角接近180°或者0°，该特征点会被拉长导致量测不准确，不利于点云匹配：

4)去除部分被遮挡的激光点，由于激光雷达在移动的时，被遮挡的点所处的面可能在下一帧中消失，如图6中点B的情况。定义点B的深度值为P_B，点A的深度值为P_A，遮挡点的判别标准如公式(7)所示：

|P_B|<|P_A|，|P_B-P_A|≥0.1|P_A|   (7)

1.3激光里程计

该模块是恢复位姿的过程，包括激光点云帧间匹配和ESKF数据融合。

1.3.1点云匹配

本模块借鉴LOAM方案[3]以及Lego-LOAM方案[4]的点云匹配思想即利用点与直线匹配和点与平面匹配策略。与LOAM、Lego-LOAM方案不同，本模块提出基于三种特征点云进行分类别的帧间匹配与分步骤的位姿解算。

(1)分类别的帧间匹配

由于提取的三类特征点是分开维护的，因此在进行点云匹配时，充分利用类别信息，仅从同一类别的特征点云中寻找对应点，例如对于地面特征点，在

中索引

的对应匹配点，构建

的点到平面的距离约束；对于非地面线特征点，在

中索引

的对应匹配点，构建

的点到直线的距离约束；对于非地面表面特征点，在

中索引

的对应匹配点，构建

的点到平面的距离约束。

考虑提取特征点的几何特性，例如提取地面特征点方向与激光雷达XOY平面平行，基于该先验信息，对于地面特征点，在连续帧匹配时只需要索引一个最临近邻点，法向量直接设置为d(0,0,1)。在帧间匹配过程中，对于不同的特征点类型、匹配方式、所需的最近点数以及方向向量和法向量的计算方法如表1所示：

表1不同类型特征点的帧间匹配

(2)分步骤的位姿解算

考虑到每种类型的特征点具有不同的空间几何特征，它们约束不同位姿变量的能力存在显著差异。例如非地面线特征点，对位姿中{t_x，t_y，θ_yaw}具有很强的约束，但是对{t_z}不构成约束，而地面特征点能够很好的约束{t_z，θ_roll，θ_pitch}，但是对{t_x，t_y，θ_yaw}无任何约束。因此，将位姿中的六个变量分成三组，分步骤的求解位姿{t_x，t_y，t_z，θ_roll，θ_pitch，θ_yaw}，使用地面特征点、非地面线特征点和非地面表面特征点依次进行L-M算法位姿解算，分步骤优化求解路线如图7所示。

上述过程将上一步优化求解的结果作为下一步优化的初值，更好的初值可以降低陷入局部最优的可能。需要说明的是，优化是有顺序的，具有最多约束的表面特征点用于最后的优化。

1.3.2ESKF数据融合

LOAM、Lego-LOAM方案中激光里程计的输出没有融合惯导信息，仅依靠激光雷达点云匹配进行位姿估计，而惯导的作用只是在数据预处理阶段辅助激光雷达进行点云的运动畸变补偿。为了对惯导预积分信息和激光雷达帧间匹配信息有效的融合，本文采用误差状态卡尔曼滤波来实现。滤波器状态为15维的误差状态，包括位置、速度、姿态、加速度偏差和角速度偏差。相比于KF或EKF，ESKF的优势如下：

(1)误差状态是小量，可以省略二阶小项，减少计算量；

(2)姿态的误差状态是小量，避免过参数化，万向锁，三维的旋转误差比旋转矩阵和四元数更直观；

(3)相比于预测阶段，能够以更低的频率进行卡尔曼滤波校正。

状态定义：真实状态s、预测状态

和误差状态δs的关系如公式(8)所示

离散传播阶段：该阶段传播预测状态、误差状态以及误差状态的协方差矩阵，该阶段是通过惯导数据预积分实现。

状态更新阶段：将状态更新问题转化为误差状态与点线距离或者点面距离的最小化，随着激光雷达帧间匹配估计的位姿测量值，计算卡尔曼增益矩阵，并更新全误差状态和误差状态的协方差矩阵。将误差状态加入到预测状态得到融合后的位姿，并重置预测状态。

1.4激光建图

通过激光里程计模块，获取了高频的激光里程计，但是恢复的激光里程计是低精度的。激光建图模块利用帧与前面的局部地图进行精细点云匹配，进一步优化激光里程计的位姿，并且建立全局的3D点云地图。帧与局部地图的精配准，与1.3.1节连续帧之间的点云匹配过程类似，仍然采用三种特征点的分类别匹配和分步骤L-M的位姿解算，位姿解算路线如图7所示。LOAM方案中帧与局部地图进行匹配时，方向向量和法向量通过搜索查找5个最近邻点进行主成分分析(PrincipalComponentsAnalysis,PCA)，通过特征值分解拟合所需要的模型参数，与LOAM方案线面两类特征点均采用PCA方式计算方向向量和法向量不同，本方案对于不同的特征点类型，匹配方法、所需的最近点数以及方向向量和法向量的计算方法进行区分，如表2所示。

表2不同类型特征点的帧间匹配

本发明提出的顾及动态障碍物的激光雷达惯导里程计与建图方法及系统，采用深度图像的方式去除道路环境中的动态障碍物，方案不同，采用三种特征点云分类别分步骤的进行点云匹配和位姿求解，充分的问题，利用ESKF框架融合惯导的预积分信息和激光雷达帧间匹配信息，提高位姿估计的精度。

在激光里程计中，首先进行连续帧间匹配，对提取的三类特征点云按照一定顺序，分类别的进行数据关联，即当前帧的地面特征、非地面线特征和非地面表面特征点云分别与上一帧对应的地面特征、非地面线特征和非地面表面特征点云进行配准，经帧间匹配后获得粗略的位姿信息(三个平移量，三个旋转量)。接下来利用误差状态卡尔曼滤波框架融合惯导的预积分信息与激光雷达帧间匹配的信息，进一步提高位姿估计的精度，同时估计出惯导中加速度计与陀螺仪的零偏误差，而校正后的惯导能够对下一帧激光点云产生的非线性运动畸变进行补偿。

为了更好地理解本发明，以上结合本发明的具体实施例做了详细描述，但并非是对本发明的限制。凡是依据本发明的技术实质对以上实施例所做的任何简单修改，均仍属于本发明技术方案的范围。本说明书中每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似的部分相互参见即可。对于系统实施例而言，由于其与方法实施例基本对应，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

Claims (8)

1.一种顾及动态障碍物的激光雷达惯导里程计与建图方法，包括数据预处理，其特征在于，还包括以下步骤：

步骤1：顾及动态障碍物的特征提取；

步骤2：进行激光点云帧间匹配和ESKF数据融合；

步骤3：利用帧与局部地图进行精细点云匹配，优化激光里程计的位姿，建立全局的3D点云地图；

所述数据预处理是指通过高频的MEMS惯导辅助激光雷达，对每一帧内的所有激光点云进行畸变补偿校正，所述畸变补偿校正的方法包括以下子步骤：

步骤01：首先找到与当前激光点的时间戳t_curr最接近的时间戳t_k和t_k+1的连续惯导测量值；

步骤02：通过惯导积分过程，分别得到惯导在时间k和时间k+1的世界坐标系W中的状态，公式为：

其中，p表示位置，v表示速度，θ表示角向量，x表示x轴朝向，y表示y轴朝向，z表示z轴朝向；

步骤03：将世界坐标系W定义为起始惯导坐标系，可利用时间因子线性插值来确定惯导在时刻t_curr在世界坐标系中的状态

公式为

其中，ratio₁和ratio₂为时间插值因子，t_k表示k时刻的时间戳，t_k+1表示k+1时刻的时间戳；

步骤04：利用

和

能够得到当前激光点和起始激光点所对应惯导的位置

和惯导的速度

在起始激光点坐标中得到每个激光点的运动畸变量，公式为

其中，

表示激光雷达当前帧起始点坐标系与世界坐标系之间的变换矩阵，i表示一帧点云数据中每个点占据一帧扫描周期的时间，

表示当前激光点所对应的惯导在世界坐标系下的状态，

表示起始激光点所对应的惯导在世界坐标系下的状态。

2.如权利要求1所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，所述步骤1包括剔除道路环境中的动态障碍物，并从静态点云中提取特征点，所述特征点包括地面特征点、非地面线特征点和非地面表面特征点。

3.如权利要求2所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，所述动态障碍物的剔除方法为提取地面点云，对剩余的非地面点云进行目标分割并聚类，将小于设定尺寸的目标剔除。

4.如权利要求3所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，所述地面点云的提取方法为采用基于深度图像的角度分割方式提取地面点，将每一帧内的激光点云投影为深度图像，将无序杂乱的非结构化点云数据转化成有组织的点云数据；深度图像中的行数r由激光雷达垂直方向的激光线束数量决定，列数c由激光雷达的水平分辨率决定；

点A和点B是激光雷达在同一水平分辨率下发射，在竖直方向的相邻激光点，角度和对应在深度图像中r-1和r行激光束的垂直角度；定义一个竖直角度α，将深度图像R的每列c转换成角度

的形式，已知两个相邻垂直激光束的深度值，角度α的计算公式为：

其中，R_r-1,c表示第r-1行第c列激光点的深度值，R_r,c表示第r行第c列激光点的深度值，θ₁和θ₂表示不同激光束在竖直方向上的角度，每一个角度α计算需要两个距离读数，将这些角度

的所有集合视为矩阵M_α，得到夹角图像，将Savitsky-Golay平滑算法应用在矩阵M_α的每一列，在夹角图像上进行地面标记，从期望属于地面的行开始，使用广度优先搜索将相似的组件标记在一起。

5.如权利要求4所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，所述目标分割的方法为对剩余的3D激光点云进行目标聚类，即将3D激光点云圆柱投影到2D深度图像，基于点云深度图的实时目标分割，然后从每一帧激光雷达数据中剔除动态障碍物，包括以下子步骤：

步骤11：计算在深度图像中行方向或列方向上相邻点A和B的角度β；

步骤12：当所述角度β小于阈值θ，则认为两条激光束的测量深度值变化过大，将扫描得到的两点分割为不同的类别，它们之间的夹角β大于阈值θ，则将它们视为来自同一对象；

步骤13：处理动态障碍物，将目标点云的包围盒在X方向、Y方向、Z方向小于设定阈值的剔除；

步骤14：把地面点云重新加载进来。

6.如权利要求5所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，将每一帧的地面点云使用体素栅格滤波的方法进行不同程度的下采样，标记为地面特征点P^G(more)和点P^G(less)。

7.如权利要求6所述的顾及动态障碍物的激光雷达惯导里程计与建图方法，其特征在于，设置平滑度阈值，将平滑度大于所述平滑度阈值的视为的点记为非地面线特征点P^E，将平滑度小于所述平滑度阈值的点记为非地面表面特征点P^S，从P^E中挑选n_E个u值最大的点构成P^E(max)，并满足P^E(max)∈P^E，从P^S中挑选个u值最小的点构成P^S(min)，并满足P^S(min)∈P^S。

8.一种顾及动态障碍物的激光雷达惯导里程计与建图系统，包括

预处理模块，其特征在于，还包括以下模块：

特征提取模块：用于顾及动态障碍物的特征提取；

激光里程计模块：用于进行激光点云帧间匹配和ESKF数据融合；

激光建图模块：用于利用帧与局部地图进行精细点云匹配，优化激光里程计的位姿，建立全局的3D点云地图；

所述预处理模块用于通过高频的MEMS惯导辅助激光雷达，对每一帧内的所有激光点云进行畸变补偿校正，所述畸变补偿校正的方法包括以下子步骤：

步骤01：首先找到与当前激光点的时间戳t_curr最接近的时间戳t_k和t_k+1的连续惯导测量值；

步骤02：通过惯导积分过程，分别得到惯导在时间k和时间k+1的世界坐标系W中的状态，公式为：

其中，p表示位置，v表示速度，θ表示角向量，x表示x轴朝向，y表示y轴朝向，z表示z轴朝向；

步骤03：将世界坐标系W定义为起始惯导坐标系，可利用时间因子线性插值来确定惯导在时刻t_curr在世界坐标系中的状态

公式为

其中，ratio₁和ratio₂为时间插值因子，t_k表示k时刻的时间戳，t_k+1表示k+1时刻的时间戳；

步骤04：利用

和

能够得到当前激光点和起始激光点所对应惯导的位置

和惯导的速度

在起始激光点坐标中得到每个激光点的运动畸变量，公式为

其中，

表示激光雷达当前帧起始点坐标系与世界坐标系之间的变换矩阵，i表示一帧点云数据中每个点占据一帧扫描周期的时间，

表示当前激光点所对应的惯导在世界坐标系下的状态，

表示起始激光点所对应的惯导在世界坐标系下的状态；

所述系统按照如权利要求1所述的方法自动生成3D点云地图。

Images