[发明专利]基于高度信息的激光SLAM方法和系统

权利要求书

1.一种基于高度信息的激光SLAM方法，包括以下步骤：

S1、获取三维激光雷达数据，进行运动畸变和动态点滤除处理，得到第一点云数据；

S2、获取机器人IMU数据，进行预积分处理，得到第一位姿数据；

S3、对所述第一点云数据提取特征，得到特征点云，并记录所述特征点云的高度；

S4、根据所述特征点云和所述特征点云的高度，得到激光雷达里程计数据；

S5、基于因子图，对所述第一位姿数据和所述激光雷达里程计数据进行融合，得到第二位姿数据作为机器人位姿；

S6、根据所述第二位姿数据确定对应的点云数据，拼接生成三维点云地图。

2.根据权利要求1所述的方法，其中，所述步骤S1包括：

S11、对于所述三维激光雷达数据，通过对机器人进行运动补偿，获得第二点云数据；

S12、获取当前帧的所述第二点云数据与局部地图的深度图像数据的差值，根据所述差值确定当前帧的所述第二点云数据是否为动态点；所述局部地图为机器人行进过程中根据当前帧之前的第一时间内所述第一点云数据生成的地图；

S13、删除确定为动态点的所述第二点云数据，得到所述第一点云数据。

3.根据权利要求1所述的方法，其中，所述特征点云包括角点特征点云；所述步骤S3包括：

计算当前帧中的所述第一点云数据中每个的点云的曲率；

将曲率大于第一曲率阈值的点云作为所述角点特征点云。

4.根据权利要求1或3所述的方法，所述步骤S3包括：

从当前帧中的所述第一点云数据中获取地面点云数据；

对所述地面点云数据进行平面拟合，得到当前帧的地面平面方程。

5.根据权利要求3所述的方法，其中，所述步骤S4包括：

检测出当前帧的所述第一点云数据中的角点特征点云；并获取所述角点特征点云对应的地图特征点；

获取所述地图特征点在局部地图中对应的边线特征的两个坐标点，得到通过这两个坐标点的第一直线；所述局部地图为机器人行进过程中根据当前帧之前的第一时间内的所述第一点云数据生成的地图；

将所述地图特征点与所述第一直线的距离作为优化函数，以缩短距离获取机器人的真实位姿，

其中第一直线为通过以下两个坐标点的直线，这两个点为角点特征点在地图中对应的边线特征的两个坐标点：

式中x_i表示当前帧的第一点云数据中检测出的第i个角点特征点，x_j和x_l分别表示第i个角点特征点在地图中对应的边线特征的两个坐标点。

6.根据权利要求4所述的方法，其中，所述步骤S4包括：

以当前帧的地面平面方程为特征，与局部地图的地面平面方程匹配；所述局部地图为机器人行进过程中根据当前帧之前的第一时间内的所述第一点云数据生成的地图；

获取当前帧的地面平面方程与匹配的地面平面方程的法向量角度差，作为优化函数。

7.根据权利要求1所述的方法，其中，所述步骤S4包括：获取当前帧的所述第一点云数据的特征点云与局部地图的高度匹配差，作为优化函数；所述局部地图为机器人行进过程中根据当前帧之前的所述第一点云数据生成的地图。

8.根据权利要求1所述的方法，其中，所述步骤S5包括：

将所述第一位姿作为当前的机器人位姿；

基于因子图，通过所述激光雷达里程计数据对当前的机器人位姿进行更新，得到所述第二位姿作为当前的机器人位姿；所述因子图的因子包括所述第一位姿数据、所述激光雷达里程计数据和所述特征点云的高度。

9.一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如权利要求1至8中任一项所述的方法。

10.一种基于高度信息的激光SLAM系统，其特征在于，所述系统包括：

机器人，设有惯性测量单元和激光雷达装置；其中，所述惯性测量单元用于获取机器IMU数据，所述激光雷达装置用于获取三维激光雷达数据；和，

与所述机器人连接的计算机装置，所述计算机装置包括处理器及存储介质，所述处理器用于执行存储介质中存储的指令序列，以执行如权利要求1至8中任一项所述的方法。