一种应用于动态环境的双匹配鲁棒激光定位方法及系统技术方案

本发明专利技术公开一种应用于动态环境的双匹配鲁棒激光定位方法及系统，所述方法首先在动态场景中构建静态先验地图，然后通过

【技术实现步骤摘要】
一种应用于动态环境的双匹配鲁棒激光定位方法及系统


[0001]本专利技术属于机器人自主定位
，具体涉及一种应用于动态环境的双匹配鲁棒激光定位方法及系统
。

技术介绍

[0002]定位任务在机器人学中是一项基础的技术，尤其是对于自主运动机器人，例如无人机，无人车，
AGV
等
。
目前一种常用的定位范式是机器人首先获取自身运动范围的地图，即先验地图，再根据机器人携带的感知环境的传感器与地图的匹配关系进行定位
。SLAM(Simultaneous Localization and Mapping)
技术是一种依靠机器人对环境的感知建立各种形式的地图，并且在地图上进行定位而不依赖人造信标的技术，并且在近些年大量应用于无人车上，并且依据使用的传感器的不同分为
LiDAR
‑
SLAM
和
Visual SLAM
等
。
在众多可选择的传感器组合中，比如单目相机
、RGB
‑
D
相机
、
激光雷达
(LiDAR)、
轮速计和惯性测量单元
(IMU)
中，
LiDAR
和
IMU
的组合激光惯性定位技术由于其测量的高准确性和鲁棒性在无人车的应用中脱颖而出
。
尽管
LiDAR
的测量足够准确，但是激光里程计
(LO)
和激光惯性里程计
(LIO)
系统在进行机器人的状态估计时，估计误差依然随时间增长而增长
。
并且
LO
和
LIO
都基于环境静态假设，即在算法运行的整个过程中，环境中没有运动物体，这种假设对于现实中的自主车辆等机器人来说是不合理的
。
当自主车辆在真实的动态环境运动时，实时获得的激光点云和先验点云地图的不一致性不仅导致了匹配算法的运行时间增加，并且使得激光点云或者先验点云地图中出现了非匹配点，进而会降低里程计的鲁棒性和准确性
。
[0003]通常在无人车以及普通车辆上，也会使用全球卫星导航系统
(GNSS)
系统来快速地获取全局定位信息，从而纠正激光点云到定位算法的匹配误差
。GNSS
系统是一种依赖卫星的全局定位系统，依赖了人造的移动信标，也就是卫星
。
目前基于差分信号的
GNSS
的精度很高
(
可以达到厘米级别
)
，并且在
SLAM
的常用数据集
kitti
和
TUM
上，都是利用了
RTK
‑
GNSS
作为定位系统的真值
。
虽然
RTK
‑
GNSS
系统，定位精度高，重定位开销低；但是他在城市环境中会因为信号的多径效应定位精度下降，并且在隧道和室内场景完全丢失定位信号，而这在可能运行在复杂环境的无人车上是不可接受的
。

技术实现思路

[0004]为了解决现有技术中存在的问题，本专利技术提供了一种同时利用激光点云到先验点云地图的匹配关系和激光点云到算法运行时构成的局部点云地图之间的相对位姿关系得到的更加鲁棒的定位方法
。
[0005]为了实现上述目的，本专利技术采用的技术方案是：一种应用于动态环境的双匹配鲁棒激光定位方法，包括以下步骤：
[0006]在动态场景中构建静态先验地图；
[0007]通过
IMU
预积分获得下一激光帧在所述静态先验地图的先验位姿信息；
[0008]利用实时获得的激光点云和所述先验位姿信息进行第一次里程计匹配，判断里程
计是否收敛，如果没有收敛，里程计将根据第一次匹配的结果作为初值进行第二次匹配；
[0009]第二次匹配通过计算激光点云与局部点云地图的匹配关系，当第一次的里程计匹配准确时，第二次里程计会立即判断收敛；当第一次里程计匹配不足够准时，第二次里程计对第一里程计进行补充迭代优化；
[0010]最后使用
GTSAM
进行后端优化，对两个里程计进行融合，更新所有位姿；
[0011]当最终里程计结果优化完后，将当前的激光点云移动到里程计指示的位置，构建局部点云地图并保存，作为第二次匹配的对象；
[0012]当一次完整的定位过程结束后，记录所有的局部点云地图和相应位姿计算静态先验地图
。
[0013]进一步的，通过
IMU
预积分获得下一激光帧在所述静态先验地图的先验位姿信息包括：
[0014]利用前段时间的
IMU
获得的加速度和角速度信息和之前的里程计数据进行
IMU
预积分，预测获取激光点云时刻的激光位姿
。
[0015]进一步的，将一帧新的激光点云分成边缘点和平面点，并与静态先验地图中的边缘点和平面点分别计算误差，以
IMU
预积分的结果作为匹配的初值，使用高斯牛顿法经过迭代匹配给出第一次里程计匹配的结果
。
[0016]进一步的，当新的一帧激光点云到达后，对每条线束上的激光点进行连续度判断，若判断为非连续则认为其是边缘点；若判断为连续则认为其是平面面点
。
[0017]进一步的，第一里程计的最后结果的残差进行保留，计算相邻帧位姿之间的协方差，累加相邻帧位姿之间的协方差，直到一个新的关键帧出现，将累加的协方差与关键帧保存，再将协方差清零，重新累加；用相邻帧位姿的协方差填充协方差矩阵的对角，用对角填满的协方差矩阵描述因子图中的边的不确定性，添加到边中；第二里程计的方差低于设定阈值，认为第二里程计收敛，否则进行第二次匹配
。
[0018]进一步的，第二里程计匹配时计算相邻帧之间的匹配关系如下：
[0019][0020]z
t
为每个时间点
t
的激光点云
、x
t
每个时间点
t
的机器人位姿，
M
为先验地图，
z
0:t
为一系列观测，
η
为常数，
[0021]进一步的，静态先验地图构建时，先验点云地图为
M
，静态先验地图为
M
，每个时刻的激光点云为
z
t
，机器人状态
x
t
，静态先验地图的构建过程描述为：
[0022][0023]具体包括以下步骤：
[0024]首先给出每个时间点
t
的激光点云
z
t
、
机器人位姿
x
t
以及先验地图
M
；通过将每个时间点
t
的激光本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种应用于动态环境的双匹配鲁棒激光定位方法，其特征在于，包括以下步骤：在动态场景中构建静态先验地图；通过
IMU
预积分获得下一激光帧在所述静态先验地图的先验位姿信息；利用实时获得的激光点云和所述先验位姿信息进行第一次里程计匹配，判断里程计是否收敛，如果没有收敛，里程计将根据第一次匹配的结果作为初值进行第二次匹配；第二次匹配通过计算激光点云与局部点云地图的匹配关系，当第一次的里程计匹配准确时，第二次里程计会立即判断收敛；当第一次里程计匹配不足够准时，第二次里程计对第一里程计进行补充迭代优化；最后使用
GTSAM
进行后端优化，对两个里程计进行融合，更新所有位姿；当最终里程计结果优化完后，将当前的激光点云移动到里程计指示的位置，构建局部点云地图并保存，作为第二次匹配的对象；当一次完整的定位过程结束后，记录所有的局部点云地图和相应位姿计算静态先验地图
。2.
根据权利要求1所述的应用于动态环境的双匹配鲁棒激光定位方法，其特征在于，通过
IMU
预积分获得下一激光帧在所述静态先验地图的先验位姿信息包括：利用前段时间的
IMU
获得的加速度和角速度信息和之前的里程计数据进行
IMU
预积分，预测获取激光点云时刻的激光位姿
。3.
根据权利要求1所述的应用于动态环境的双匹配鲁棒激光定位方法，其特征在于，将一帧新的激光点云分成边缘点和平面点，并与静态先验地图中的边缘点和平面点分别计算误差，以
IMU
预积分的结果作为匹配的初值，使用高斯牛顿法经过迭代匹配给出第一次里程计匹配的结果
。4.
根据权利要求3所述的应用于动态环境的双匹配鲁棒激光定位方法，其特征在于，当新的一帧激光点云到达后，对每条线束上的激光点进行连续度判断，若判断为非连续则认为其是边缘点；若判断为连续则认为其是平面面点
。5.
根据权利要求1所述的应用于动态环境的双匹配鲁棒激光定位方法，其特征在于，第一里程计的最后结果的残差进行保留，计算相邻帧位姿之间的协方差，累加相邻帧位姿之间的协方差，直到一个新的关键帧出现，将累加的协方差与关键帧保存，再将协方差清零，重新累加；用相邻帧位姿的协方差填充协方差矩阵的对角，用对角填满的协方差矩阵描述因子图中的边的不确定性，添加到边中；第二里程计的方差低于设定阈值，认为第二里程计收敛，否则进行第二次匹配
。6.
根据权利要求1所述的应用于动态环境的双匹配鲁棒激光定位方法，其特征在于，第二里程计匹配时计算相邻帧之间的匹配关系如下：
z
t
为每个时间点
t
的激光点云
、x
t
每个时间点
t
的机器人位姿，
M
为先验地图，
z
0:t
为一系列观测，
η
为常数，
7.
根据权利要求1所述的应用于动态环境的双匹配鲁棒激光定位方法，其特征在于，静态先验地图构建时，先验点云地图为
M
，静态先验地图为
M
，每个时刻的激光点云为...

【专利技术属性】
技术研发人员：郑南宁，朱子瑜，王丹，陈仕韬，
申请(专利权)人：宁波市舜安人工智能研究院，
类型：发明
国别省市：