一种基于Cartographer算法的改进激光建图方法

本发明属于巡检无人车安全与激光雷达建图定位，具体涉及一种基于cartographer算法的改进激光建图方法。

背景技术：

1、近年来，无人驾驶技术逐渐成熟，应用领域越加广泛，巡检无人车成为大型工业企业不可或缺的辅助；巡检无人车作为一种具有自主导航能力的移动机器人，可通过搭载各类传感器，如激光雷达、相机、红外摄像头、危险气体浓度监测器等代替巡检人员对厂区进行动态巡逻，完成日常巡检任务。

2、cartographer算法是google在2016年提出的一种适用于大尺度建图的激光slam方法，利用基于网格映射和基于ceres库的扫描匹配器，搭载各种传感器进行建图。在slam方法中由于传感器的精度、噪音、温度等因素会产生相应的误差，误差的累计会导致地图的漂移；使用更大规模的扫描匹配器对地图进行扫描匹配可以减少地图的累计误差，但是会拉高处理器的运算需求，从而降低运算性能。

技术实现思路

1、针对上述的技术问题，本技术方案提供了一种基于cartographer算法的改进激光建图方法，根据上位机激光雷达扫描的采样频率，使激光雷达调节器自适应选择雷达扫描距离，其中包括允许雷达扫描的最大距离和允许雷达扫描的最小距离，降低上位机处理器对于采样的性能要求，保证获取雷达数据的实时性；在子图生成的概率网格地图m中添加weightsubmap权重，来增加子图在小范围内的比重，提高概率网格地图的准确度；使用ceres库的扫描匹配器求解非线性最小二乘问题，该最小二乘问题为将新的雷达扫描数据匹配至子图当中，为实时更新概率网格地图中每个网格的概率，在扫描匹配器中增加最大搜索窗口wmax和最小搜索窗口wmin，控制扫描匹配器的匹配范围，从而提高处理速度；能有效的解决上述问题。

2、本发明通过以下技术方案实现：

3、一种基于cartographer算法的改进激光建图方法，包括以下步骤：

4、步骤一：获取激光雷达扫描周围环境的点云数据时，使用激光距离调节器控制激光雷达的扫描范围；具体的操作方式为：

5、步骤1.1：通过激光雷达扫描获取雷达点云r，获取上位机的采样频率，设置激光距离调节器的步长初值，设置允许雷达扫描的最大距离dmax和允许雷达扫描的最小距离dmin；

6、步骤1.2：获取激光雷达在上位机中的采样频率，将其与当前激光雷达调节器的步长值作比较，若相等执行步骤1.3，若不相等执行步骤1.4；

7、步骤1.3：若相等，将激光扫描的点云r中所有的点p与允许雷达扫描的最大距离dmax作比较，删除点云r中所有大于该距离的点；

8、步骤1.4：若不相等，将激光扫描的点云r中所有的点p与允许雷达扫描的最小距离dmin作比较，删除点云r中所有大于该距离的点；

9、步骤1.5：结束此次判断，并将当前调节器的步长值与当前时间累加，作为下一次调节器的步长值；

10、步骤1.6：输出当前筛选后的激光扫描点云r，并结束该方法；

11、步骤二：使用imu传感器获取雷达加速度方向来校正雷达扫描数据在子图坐标系下的位姿；

12、步骤三：使用当前雷达扫描帧中所有扫描点插入至概率网格地图中，根据概率网格更新方式计算每一个网格被雷达扫描命中的概率；以第一个雷达扫描帧为子图的中心位置，此片范围的概率网格地图生成一个子图；

13、步骤四：对扫描匹配器设置搜索窗口，使用ceres库将在该窗口下的每一帧雷达扫描数据匹配至子图中，当后续雷达扫描数据与当前子图的概率网格地图无交点时，则该子图生成完毕；该子图相关的雷达扫描位姿存储在内存中，用于闭环优化；继续更新下一个子图；

14、步骤五：使用分支界定方法将新的雷达扫描数据与子图进行回环检测，分支界定方法在最上层匹配概率网格地图的网格较多，直至最底层匹配每一个网格；

15、步骤六：每次回环检测成功后，将新的子图更新至全局地图当中，最后输出全局地图。

16、进一步的，步骤1.3和步骤1.4中所述激光扫描的点云r，其中雷达扫描帧的坐标与子图坐标转换关系为：

17、

18、上式中，rx为激光雷达点云r二维地图中的x轴坐标，ry为激光雷达点云r二维地图中的y轴坐标，rθ为旋转角rotation为旋转矩阵，translation为平移向量，当前雷达扫描帧在子图坐标系下的位姿为tr；

19、该关系式的作用为将雷达扫描帧的坐标转换至子图坐标中，为下面概率网格地图m的计算和每一帧雷达扫描数据位姿的优化做准备。

20、进一步的，在步骤三所述的计算每一个网格被雷达扫描命中的概率时，增加子图权重weightsubmap来优化概率网格地图更新方式；所述的子图是由多个概率网格地图构成的，概率网格地图m中的分辨率为r，每个网格的概率在pmin到pmax之间，连续几帧雷达扫描会构成一个子图；概率网格地图m的更新方式为：

21、

22、mnew(x)＝clamp(odds-1(odds(mold(x)))·odds(phit))

23、上式中，odds为描述当前概率网格地图m是否被激光雷达命中的比例，prob为概率参数，weightsubmap为子图权重即当前子图对于该概率网格地图m的权重值，phit为当前概率网格地图的网格被激光雷达命中的概率，clamp为当前概率网格地图m的区间限定函数。

24、进一步的，步骤四所述的使用ceres库将在该窗口下的每一帧雷达扫描数据匹配至子图中，是使用ceres库优化在该窗口下的每一帧雷达扫描数据的位姿，并将雷达扫描数据匹配至子图当中。

25、在连续的激光扫描点云r中需要对扫描数据的误差进行滤波，来获取当前无人车经过旋转平移后的位姿，使用ceres库来对该系统中的最小二乘问题进行优化求解。根据步骤一种的激光雷达调节器的方法步骤，该最小二乘问题中激光扫描点云r匹配到子图的点云搜索窗口应当设置在合理区间。根据步骤1.1设置的允许雷达扫描的最大距离dmax和允许雷达扫描的最小距离dmin设置最大搜索窗口wmax和最小搜索窗口wmin；则子图匹配的优化为：

26、

27、上式中，tr为当前雷达扫描帧在子图坐标系下的位姿，hk为一帧雷达扫描的扫描点，msmooth为平滑函数；

28、在雷达扫描数据与子图匹配的过程中，根据激光距离调节器的方法步骤，自适应调节搜索窗口，将雷达扫描数据匹配到子图中，从而提高子图匹配的处理速度。

29、有益效果

30、本发明提出的一种基于cartographer算法的改进激光建图方法，与现有技术相比较，其具有以下有益效果：

31、(1)本发明根据上位机激光雷达扫描的采样频率，使激光雷达调节器自适应选择雷达扫描距离，其中包括允许雷达扫描的最大距离和允许雷达扫描的最小距离，降低上位机处理器对于采样的性能要求，保证获取雷达数据的实时性；在子图生成的概率网格地图m中添加weightsubmap权重，来增加子图在小范围内的比重，提高概率网格地图的准确度；使用ceres库的扫描匹配器求解非线性最小二乘问题，该最小二乘问题为将新的雷达扫描数据匹配至子图当中，为实时更新概率网格地图中每个网格的概率，在扫描匹配器中增加最大搜索窗口wmax和最小搜索窗口wmin，控制扫描匹配器的匹配范围，从而提高处理速度。

32、(2)本发明通过激光距离调节器控制激光雷达的扫描范围，自适应选择设置允许雷达扫描的最大距离dmax和允许雷达扫描的最小距离dmin，提高了上位机对于短距离内激光雷达扫描处理效率。通过在子图生成的概率网格地图m中添加weightsubmap权重，提高了概率网格地图的准确度。对于通过增加最大搜索窗口wmax和最小搜索窗口wmin，控制扫描匹配器的匹配范围，从而提高了位机对于扫描匹配器的处理效率。该方法有效提升了实时建图的效率，缩短了建图时间。