一种激光建图优化中异常边剔除方法及系统与流程

1.本发明涉及激光建图优化领域，特别是涉及一种激光建图优化中异常边剔除方法及系统。


背景技术：

2.目前，即时定位与地图构建(simultaneous localization and mapping，slam)被广泛应用。slam是指机器人在未知的环境中从一个未知位置开始移动，在移动过程中根据位置和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。对于封闭园区场景激光建图，现有的异常边剔除方法为：使用robustkernel函数，对该函数设置固定的阈值能够将异常边去除；将去除异常边后的值重新加入优化进行计算。
3.现有的异常边剔除方法，具有如下缺陷：采用的鲁棒核(robust kernel)函数的阈值是固定的，在面对不同路线或者封闭园区不同场景时，容易过度删除异常边导致优化中约束不足，降低建图精度。为了提高建图精度，通常手动经验调节阈值，而这种调节方式在面对大图的构建中效率低。


技术实现要素：

4.基于此，本发明实施例提供一种激光建图优化中异常边剔除方法及系统，能在保证建图精度的同时，提高建图效率。
5.为实现上述目的，本发明提供了如下方案：
6.一种激光建图优化中异常边剔除方法，包括：
7.获取gps位置数据、惯性测量数据、激光雷达点云数据和里程数据；
8.对所述惯性测量数据和所述里程数据进行航迹推算，得到航位推算位姿；
9.对所述激光雷达点云数据进行前端匹配，得到激光雷达变换位姿；
10.对所述gps位置数据进行回环检测，得到回环相对位姿信息；
11.动态调节鲁棒核函数的阈值，对所述gps位置数据、所述航位推算位姿、所述激光雷达变换位姿和所述回环相对位姿信息进行后端优化，得到优化后的位姿；所述优化后的位姿用于生成地图。
12.可选地，所述动态调节鲁棒核函数的阈值，对所述gps位置数据、所述航位推算位姿、所述激光雷达变换位姿和所述回环相对位姿信息进行后端优化，得到优化后的位姿，具体包括：
13.采用图优化方法，将待优化位姿作为图的顶点，分别将所述gps位置数据、所述航位推算位姿、所述激光雷达变换位姿和所述回环相对位姿信息作为所述图的边；
14.计算当前迭代次数下各个边的方差值；
15.将当前迭代次数下各个边的方差值分别与当前迭代次数下的设定边方差进行比较；
16.根据比较结果确定第一数量和第二数量；所述第一数量为当前迭代次数下方差值小于设定边方差的边的数量；所述第二数量为当前迭代次数下方差值大于或等于设定边方差的边的数量；
17.根据所述第一数量和所述第二数量计算当前迭代次数下的内边率；
18.若当前迭代次数下的内边率大于或等于内边占有率设定阈值，则将所述图中的异常边剔除，并根据剔除后的边计算待优化位姿，得到优化后的位姿；所述异常边为当前迭代次数下方差值大于或等于设定边方差的边；
19.若当前迭代次数下的内边率小于内边占有率设定阈值，则对当前迭代次数下的设定边方差进行更新，并将更新后的设定边方差作为下次迭代的设定边方差，返回计算当前迭代次数下各个边的方差值的步骤；所述鲁棒核函数的阈值包括设定边方差和内边占有率设定阈值。
20.可选地，当前迭代次数为第1次迭代时，设定边方差为2，内边占有率设定阈值为0.1。
21.可选地，所述对当前迭代次数下的设定边方差进行更新，具体为：
22.sensor_chi1＝sensor_chi*2；
23.其中，sensor_chi表示当前迭代次数下的设定边方差；sensor_chi1表示更新后的设定边方差。
24.可选地，所述根据所述第一数量和所述第二数量计算当前迭代次数下的内边率，具体为：
25.inlier_ratio＝cnt_inlier/(cnt_inlier+out_inlier)；
26.inlier_ratio表示内边率；cnt_inlier表示第一数量；out_inlier表示第二数量。
27.可选地，所述对所述gps位置数据进行回环检测，得到回环相对位姿信息，具体包括：
28.对所述gps位置数据进行距离判断，并当所述gps位置数据中两帧数据之间的距离小于设定阈值时，确定两帧数据为回环帧数据；
29.采用距离变换法对所述回环帧数据进行匹配，得到回环相对位姿信息。
30.可选地，所述对所述激光雷达点云数据进行前端匹配，得到激光雷达变换位姿，具体包括：
31.对所述激光雷达点云数据中单帧点云之间进行匹配，得到粗匹配变换位姿；
32.将所述激光雷达点云数据中单帧点云与历史局部地图进行匹配，得到精匹配变换位姿；所述激光雷达变换位姿包括粗匹配变换位姿和精匹配变换位姿。
33.本发明还提供了一种激光建图优化中异常边剔除系统，包括：
34.数据获取模块，用于获取gps位置数据、惯性测量数据、激光雷达点云数据和里程数据；
35.航迹推算模块，用于对所述惯性测量数据和所述里程数据进行航迹推算，得到航位推算位姿；
36.前端匹配模块，用于对所述激光雷达点云数据进行前端匹配，得到激光雷达变换位姿；
37.回环检测模块，用于对所述gps位置数据进行回环检测，得到回环相对位姿信息；
38.后端优化模块，用于动态调节鲁棒核函数的阈值，对所述gps位置数据、所述航位推算位姿、所述激光雷达变换位姿和所述回环相对位姿信息进行后端优化，得到优化后的位姿；所述优化后的位姿用于生成地图。
39.与现有技术相比，本发明的有益效果是：
40.本发明实施例提出了一种激光建图优化中异常边剔除方法及系统，对惯性测量数据和里程数据进行航迹推算，得到航位推算位姿；对激光雷达点云数据进行前端匹配，得到激光雷达变换位姿；对gps位置数据进行回环检测，得到回环相对位姿信息；动态调节鲁棒核函数的阈值，对gps位置数据、航位推算位姿、激光雷达变换位姿和回环相对位姿信息进行后端优化，得到优化后的位姿。本发明动态调节鲁棒核函数的阈值实现优化，能在保证建图精度的同时，提高建图效率。
附图说明
41.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
42.图1为本发明实施例提供的激光建图优化中异常边剔除方法的流程图；
43.图2为本发明实施例提供的激光建图优化中异常边剔除系统的结构图。
具体实施方式
44.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
45.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
46.图1为本发明实施例提供的激光建图优化中异常边剔除方法的流程图。参见图1，本实施例的激光建图优化中异常边剔除方法，包括：
47.步骤101：获取gps位置数据、惯性测量数据、激光雷达点云数据和里程数据。具体的：
48.采用全球导航定位系统(global navigation satelite ststem，gnss)的接收机采集gps位置数据。全球导航定位系统是能在地球表面或近似空间的任何地点为用户提供全天候的三维坐标和速度以及时间信息的空基无线电导航定位系统。
49.采用惯性测量单元(inertial measurement unit，imu)采集惯性测量数据。惯性测量数据包括角速度和角加速度数据。惯性测量单元为测量物体三轴姿态角以及加速度的装置。
50.采用多线激光雷达(light detection and ranging，lidar)采集激光雷达点云数据。激光雷达是以激光器作为发射源，通过量测光发射及返回的时间获取位置、强度等特征的主动遥感设备。
51.采用轮式里程计(wheel odometry，odom)采集里程数据。轮式里程计通过获取车辆的脉冲信息，得到车辆行驶的实际里程。
52.步骤102：对所述惯性测量数据和所述里程数据进行航迹推算，得到航位推算位姿。
53.步骤103：对所述激光雷达点云数据进行前端匹配，得到激光雷达变换位姿。
54.步骤103，具体包括：
55.对所述激光雷达点云数据中单帧点云之间进行匹配，得到粗匹配变换位姿。
56.将所述激光雷达点云数据中单帧点云与历史局部地图进行匹配，得到精匹配变换位姿；所述激光雷达变换位姿包括粗匹配变换位姿和精匹配变换位姿。
57.步骤104：对所述gps位置数据进行回环检测，得到回环相对位姿信息。
58.步骤104，具体包括：
59.对所述gps位置数据进行距离判断，并当所述gps位置数据中两帧数据之间的距离小于设定阈值时，确定两帧数据为回环帧数据。
60.采用距离变换法对所述回环帧数据进行匹配，得到回环相对位姿信息。
61.步骤105：动态调节鲁棒核函数的阈值，对所述gps位置数据、所述航位推算位姿、所述激光雷达变换位姿和所述回环相对位姿信息进行后端优化，得到优化后的位姿；所述优化后的位姿用于生成地图。
62.其中，步骤105，具体包括：
63.1)采用图优化方法，将待优化位姿作为图的顶点，分别将所述gps位置数据、所述航位推算位姿、所述激光雷达变换位姿和所述回环相对位姿信息作为所述图的边。
64.图优化(pose-graph):指一个图graph，由若干个顶点及连接的边组成，顶点表示待优化的位姿，边表示两帧之间的旋转矩阵即误差项，通过建立最小二乘的非线性优化，使得误差达到最小。
65.2)计算当前迭代次数下各个边的方差值。
66.3)将当前迭代次数下各个边的方差值分别与当前迭代次数下的设定边方差进行比较。当前迭代次数为第1次迭代时，设定边方差为2。
67.4)根据比较结果确定第一数量和第二数量；所述第一数量为当前迭代次数下方差值小于设定边方差的边的数量；所述第二数量为当前迭代次数下方差值大于或等于设定边方差的边的数量。
68.5)根据所述第一数量和所述第二数量计算当前迭代次数下的内边率。所述内边率的计算公式为：
69.inlier_ratio＝cnt_inlier/(cnt_inlier+out_inlier)；
70.inlier_ratio表示内边率；cnt_inlier表示第一数量；out_inlier表示第二数量。
71.6)若当前迭代次数下的内边率大于或等于内边占有率设定阈值，则将所述图中的异常边剔除，并根据剔除后的边计算待优化位姿，得到优化后的位姿。所述异常边为当前迭代次数下方差值大于或等于设定边方差的边。当前迭代次数为第1次迭代时，内边占有率设定阈值为0.1。
72.若当前迭代次数下的内边率小于内边占有率设定阈值，则对当前迭代次数下的设定边方差进行更新，并将更新后的设定边方差作为下次迭代的设定边方差，返回计算当前
迭代次数下各个边的方差值的步骤；所述鲁棒核函数的阈值包括设定边方差和内边占有率设定阈值。
73.其中，所述对当前迭代次数下的设定边方差进行更新，具体为：
74.sensor_chi1＝sensor_chi*2；
75.其中，sensor_chi表示当前迭代次数下的设定边方差；sensor_chi1表示更新后的设定边方差。
76.在实际应用中，上述实施例中的激光建图优化中异常边剔除方法的一个更为具体的实现过程如下：
77.1.预处理：
78.对传感器数据进行预处理。传感器数据包括：gps位置数据(gnss值)、惯性测量数据(imu值)、激光雷达点云数据(lidar值)和里程数据(odom值)。预处理过程为：将惯性测量数据中的零偏值去掉，对里程数据进行标定处理；传感器数据中都包含时间戳数据，将gps位置数据、去掉零偏值的惯性测量数据、标定处理后的里程数据数据保存在各自的队列中，找到与激光雷达点云数据时间点最相邻的数据，进行时间同步；
79.对时间同步后的惯性测量数据与时间同步后的里程数据进行航迹推算(deadreckoning，dr)生成航位推算位姿dr_path。dr是指利用惯性测量数据与里程数据通过运动学求解车辆行驶的相对位姿。
80.预处理部分保存的数据有gnss_pose\dr_path\lidar_pointcloud；gnss_pose表示时间同步后的gps位置数据，lidar_pointcloud表示时间同步后的激光雷达点云数据。
81.2.前端匹配:
82.利用步骤1中的粗匹配和精匹配。
83.粗匹配：即两帧激光点云的相互匹配(scan_scan)。lidar_pointcloud中单帧点云之间的匹配，对点云进行特征提取，利用前后两帧的相似特征，求解相对位姿，得到粗匹配变换位姿。
84.精匹配：即一帧点云和多帧点云构成的地图进行匹配(scan_map)。将lidar_pointcloud中单帧点云与历史局部地图进行匹配(可采用ndt或icp匹配方法)得到激光里程计，从而确定粗匹配变换位姿。根据时间、距离以及角度选取合适的关键帧并保存到lidar_data。关键帧的作用就是能够将所有传感器的数据在这一帧获取，比如每2m获取一个关键帧，这样减少了数据处理的量。激光雷达变换位姿lidar_pose包括粗匹配变换位姿和精匹配变换位姿。
85.3.回环检测：
86.回环检测(loop closure detection):判断机器人是否到达过先前的位置，如果检测到回环，他会把信息提供给后端进行处理。具体的：
87.通过步骤1中的gnss或者融合数据gnss_pose进行距离判断，当两帧数据之间的距离小于设定阈值时，认为两帧加上闭环，两帧数据为回环帧数据；将回环帧数据进行法线距离变换(normal distance transform，ndt)匹配，并将两帧数据之间的相对位姿信息(即回环相对位姿信息)保存到loop_pose，输送到后端。在进行距离判断时，可具体通过计算欧式距离实现判断。ndt是非线性优化的一种，通过计算正态分布，得到目标函数，并求解目标函数局部值域最小的位姿参数。
88.4.后端优化：
89.利用g2o图优化方法，确定每个顶点以及边(此处的边是传感器gnss_pose\lidar_pose\dr_path\loop_pose的值)，边相当于优化过程中的约束，是优化过程中必不可少的条件，优化过程中会对这些传感器的边进行剔除，比如边的误差大于2m，就将这个边剔除，此处边的误差计算在鲁棒核(robustkernel)函数中得到。边的剔除利用非线性优化进行平差计算，得到关键帧优化后的位姿，步骤如下：
90.(1)首先设定误差值，如设定边方差sensor_chi＝2.0，内边占有率设定阈值sensor_inlier_ratio＝0.1；
91.(2)计算每条边的方差chi()，并根据每条边的方差与上述设置的设定边方差进行比较，小于上述设置的设定边方差的边的数量记为第一数量cnt_inlier；大于上述设置的设定边方差的边的数量记为第二数量out_inlier；
92.(3)根据上述得到的第一数量和第二数量，来进一步计算内边率inlier_ratio＝cnt_inlier/(cnt_inlier+out_inlier)；
93.(4)根据步骤(3)得到的内边率来判断异常边的判断，若inlier_ratio》0.1，剔除异常边继续加入优化；inlier_ratio《0.1，设定sensor_chi＝sensor_chi*2，重复步骤(1)-(4)。
94.5.生成地图：
95.将关键帧与其对应的优化后的位姿通过姿态转换生成pcd地图，并对地图进行体元过滤。
96.上述实施例的激光建图优化中异常边剔除方法，具有如下优点：
97.(1)能够最大化保存阈值之内的边的数量，增加优化中的约束，提高了建图质量。
98.(2)能够动态调节鲁棒核函数的阈值，不仅提高了建图精度，而且省去手动修改阈值的重复操作，提高了建图效率。
99.本发明还提供了一种激光建图优化中异常边剔除系统，图2为本发明实施例提供的激光建图优化中异常边剔除系统的结构图。参见图2，所述系统，包括：
100.数据获取模块201，用于获取gps位置数据、惯性测量数据、激光雷达点云数据和里程数据。
101.航迹推算模块202，用于对所述惯性测量数据和所述里程数据进行航迹推算，得到航位推算位姿。
102.前端匹配模块203，用于对所述激光雷达点云数据进行前端匹配，得到激光雷达变换位姿。
103.回环检测模块204，用于对所述gps位置数据进行回环检测，得到回环相对位姿信息。
104.后端优化模块205，用于动态调节鲁棒核函数的阈值，对所述gps位置数据、所述航位推算位姿、所述激光雷达变换位姿和所述回环相对位姿信息进行后端优化，得到优化后的位姿；所述优化后的位姿用于生成地图。
105.本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说
明即可。
106.本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。