一种融合IMU的三维激光雷达定位与建图方法

本发明属于机器人感知与定位，具体涉及一种融合imu的三维激光雷达定位与建图方法。

背景技术：

1、此部分的陈述仅仅提供与本公开有关的背景技术信息，并且这些陈述可能构成现有技术。在实现本发明过程中，发明人发现现有技术中至少存在如下问题。

2、同时定位与建图(simultaneous localization and mapping,slam)技术是移动机器人领域的一项重要技术，该技术主要研究机器人在未知场景下的建图与定位问题，通过机器人自身携带的传感器，完成机器人的位姿估计和环境的地图构建，实现未知环境下机器人的自主工作。移动机器人的工作环境从单一的实验测试场景转换到愈加复杂的真实世界，如自动驾驶，服务机器人，智能农业等。这些不确定因素给移动机器人的自主定位与环境感知带来巨大的挑战。

3、目前主流的slam方法有三种，分别为基于相机的视觉slam、基于雷达的激光slam和基于多元传感器融合的slam。基于相机的视觉slam容易受到光线的干扰而产生错误信息。激光雷达相较于视觉相机，其具有可准确获取目标的信息、抗干扰能力强、探测范围广、近全天候工作等优点。激光slam包含二维和三维激光slam，后者能够通过主动地发射多条激光束进行环境探测和精准测距，实现实时位姿估计、建立周围环境的三维地图、场景识别等功能，因而能够使用的范围更加广阔。随着激光slam技术的快速发展，目前三维激光slam成为最先进移动机器人slam技术之一。

4、然而三维激光slam在空旷和较为相似的长廊中会出现退化现象。其退化现象是由于单一传感器激光雷达的工作原理以及环境特点造成的。激光雷达在空旷和相似长廊环境中容易受到多路径干扰、反射面的限制、环境噪音以及功率损失等因素的影响，从而导致数据的退化和性能下降。

5、目前大多采用的是多传感器来解决上述问题。如申请号202211616723.5专利名称为“一种机器人定位方法、装置、设备及存储介”，通过融合多种传感器并在已知地图中添加语义信息来矫正机器人位姿，以解决单一传感器在特征相似且稀少的环境中定位易丢失及错误匹配的问题。

6、但很少有采用单一传感器激光雷达来解决上述问题。

技术实现思路

1、针对上述问题，本发明的目的在于解决现有技术中的一部分问题，或至少缓解这些问题。

2、一种融合imu的三维激光雷达定位与建图方法，包括如下步骤：

3、获取激光雷达采集的原始点云及惯性测量单元imu采集的高频信号；

4、将所述原始点云数据预处理，并进行点云分割；所述点云分割包括地面点云分割和点云聚类分割，以将原始点云转换为只包含障碍物的目标点云；

5、对所述目标点云进行点云畸变矫正，得到完成运动畸变补偿的特征点云；

6、通过计算特征点云曲率，提取线面特征并添加到局部特征图中，并将连续时刻的特征点配准；

7、引入scan-context描述子来进行回环检测；

8、通过imu的状态预测对点云进行畸变消除，将处理后雷达位姿作为帧间配准初始值，构建联合优化的误差函数以准确的求解雷达位姿，并生成最终轨迹和全局一致性地图。

9、获取激光雷达采集的原始点云及惯性测量单元imu采集的高频信号后，还包括对所述高频信号引入预积分计算，计算公式为：

10、

11、

12、

13、式中，δpij、δvij、qij分别为在上一时刻i到当前时刻j的位置、速度、旋转变化量；t∈(i,j)，符号表示四元数相乘，qi、qj分别为上一时刻i和当前时刻j的旋转量，为角速度测量值，bgk、bak分别表示陀螺仪和加速度计的偏置误差，ηgk、ηak分别表示陀螺仪和加速度计的测量噪声；δrik为上一时刻i的预积分理想值，与测量值之间的存在偏置误差和噪声；δvik为上一时刻i的速度理想值，与速度测量值之间的存在误差；δt为时间间隔；为imu在世界坐标系下的加速度。

14、进一步的，所述点云分割包括如下步骤：

15、地面点云分割：通过地面分割算法将点云预处理后的原始点云图像快速分割为地面点云和目标点云，提取目标点云；

16、点云聚类分割：使用聚类分割算法对地面点云分割提取后的目标点云进行聚类分割。

17、分割地面点云和目标点云的步骤如下：

18、利用地面分割算法确定角度θ：ok、om为激光雷达发出的两条相邻激光线，对应的深度分别为rr和rr-1；θ是km和mn的夹角，βi、βj为激光雷达线束与x轴的夹角，角度θ的计算方法如下：

19、

20、引入基于激光雷达安装高度而变化的高度阈值hpc，以过滤贴近地面的小型障碍物：

21、hpc＝0.15hl

22、其中，hl代表雷达的安装高度；

23、区分并分割目标点云和地面点云：当角度θ小于15度且m、k两点点云高度大于高度阈值hpc，或者角度θ大于15度时，判定为目标点云；否则为地面点云。

24、进一步的，所述点云聚类分割中引入了一个动态的聚类阈值：

25、

26、其中，np为聚类点数，nl为竖直方向上的所占的扫描激光束数量，d代表雷达和激光扫描点的水平距离。

27、利用imu所产生的高频信号对所述目标点云进行点云畸变矫正，以完成运动畸变补偿；包括如下步骤：

28、同时接收激光雷达数据和imu的高频信号，并将其保存；

29、根据当前点云的时间戳以及激光雷达的采样频率，求出每个采样点时间；

30、根据当前时间戳在存储队列中查找相邻的imu姿态数据，通过球面线性插值估计当前激光点姿态；

31、将扫描起始到结束时刻内的所有点云姿态变换至扫描结束时刻下；

32、其中，在点云扫描起止时刻通过对imu的预积分位姿进行线性插值，以计算一帧点云内对应时刻的位姿；设lk-1和lk+1时刻平移向量分别为tk-1、tk+1，姿态向量分别为pk-1、pk+1；lj-1和lj+1时刻平移向量分别为tj-1、tj+1，姿态向量分别为pj-1、pj+1；

33、位置插值公式如下：

34、

35、球面线性插值的公式如下：

36、pk＝a(t)pk-1+b(t)pk+1

37、

38、式中，pk是要求取的姿态向量，pk-1和pk+1分别为imu在lj-1和lj+1时刻的位姿，a(t)和b(t)是根据时间t而变化的两个系数变量。

39、在对所述目标点云进行点云畸变矫正后，还包括对完成运动畸变补偿的特征点云滤除非特征点云，并区分平面特征和边缘特征，以减少数据运算量，提高计算效率；具体步骤为：

40、剔除非特征点云：把激光束和物体表面趋近于平行时的特征点和被遮挡部分的特征点视为不可靠的点云，不纳入到特征序列中；

41、区分平面特征和边缘特征：通过对点云粗糙度设置阈值来区分平面特征和边缘特征，以在点云数据中提取出不同类型的结构信息，便于更好地进行点云匹配及计算；

42、所述点云粗糙度的公式为：

43、

44、其中，为在k时刻同一扫描线上相邻点的集合；为激光雷达参考系下的当前帧点云坐标，为与在同一个扫描线且相邻的点云；当点云粗糙度大于阈值时，将其划分为边缘特征；当点云粗糙度小于阈值时，将其划分为平面特征。

45、引入scan-context描述子来进行回环检测，包括如下步骤：

46、引入scan-context描述子来进行回环检测，并将在每一区域中选取3个相邻的高度最大值的平均值，并作为该特征矩阵的数值；

47、若一个扫描上下文对的距离满足阈值条件，下一步执行基于欧氏距离的方式寻找匹配的回环帧来验证；

48、如果查询点云和候选点云基于欧式距离的回环匹配成功，则将回环约束添加到非线性优化函数中，以获得最小的误差。

49、构建联合优化的误差函数以准确的求解雷达位姿，包括如下步骤：

50、优化帧间匹配误差：构建有两帧点云配准误差，以提高算法的实时性，将关键帧的特征纳入到优化函数中；

51、优化imu误差：构建imu误差函数，以提高定位精度；

52、优化回环误差；构建回环误差，以提高定位精度；

53、构建激光雷达约束、imu测量约束和回环约束的非线性代价函数，利用高斯牛顿的非线性优化算法求解使得误差函数最小的最优状态。

54、一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的融合imu的三维激光雷达定位与建图方法的步骤。

55、本发明具有如下有益效果：

56、1、为了解决单一传感器激光雷达在空旷和较为相似的长廊中会出现退化现象，本发明沿用单一传感器激光雷达，将激光和imu数据进行松耦合的激光里程计算法运用于slam中，通过imu提供运动约束，imu预积分和回环检测协同作用，并尽可能的缩小帧间匹配误差，再联合imu预积分误差和回环检测约束构建联合优化函数来求解激光里程计位姿，从而最终提高定位精度；仿真结果显示，本发明的激光里程计精度比lego-loam方案高42％，更逼近真实轨迹；

57、2、基于定位精度的考虑，本发明在两个部分进行了具体考量：其一，通过imu估计的高频位姿，对激光点云进行线性插值，从而矫正点云运动畸变；其二，在回环检测部分使用scan-context描述子回环检测模块产生的距离信息，若符合判断标准，则下一步执行基于欧氏距离的方式寻找匹配的回环帧，验证成功后添加回环约束，有效避免了累计误差的影响；

58、3、为减少计算量，本发明对imu的高频信号引入了预积分计算，并在地面分割部分引入高度阈值，加之于点云聚类分割中引入动态的聚类阈值以判断分类，从而将原始点云转换为只包含障碍物的目标点云，以运用于之后的特征提取和匹配中；再配合于完成运动畸变补偿的特征点云中滤除非特征点云，从而可以大幅减少数据运算量，提高计算效率。