一种外部旋转3D激光雷达设备及其同时定位与建图方法

本发明涉及一种外部旋转3d激光雷达设备及其同时定位与建图方法，特别涉及一种外部旋转3d激光雷达设备以及适配于该设备的同时定位与建图方法，属于机器人感知定位与建图。

背景技术：

1、同时定位与建图(simultaneous localization and mapping，slam)是当前在机器人感知定位与自主导航领域非常具有研究前景的技术。这项技术的核心意义在于，当机器人处在大范围未知环境下，不依赖于外部测量给出的信息确定自身的位置，而是依赖于自身传感器去确定自身位置的增量，从而确定本体在环境地图中的位置，同时根据位置和当前位置的传感器数据建立环境点云地图。slam技术领域目前主要分为激光slam、视觉slam以及多传感器融合slam，其中激光slam技术由于定位精度高，鲁棒性强，建图质量高，目前已经被广泛应用于自动驾驶，智能机器人，三维重建，智慧城市等各个领域中。而且目前新兴的混合固态激光雷达成本大幅降低，预计未来这项技术会得到更为广泛的应用。但是，目前适用于未知大场景下探测距离远的混合固态激光雷达水平视场角十分有限，这使得其应用于机器人时会产生感知能力受限，定位效果差等诸多问题，给目前的slam算法带来了极大的挑战。

2、目前针对激光slam技术，已有的解决方案有如下几种：

3、方案1：文献(j.lin and f.zhang,"loam livox:a fast,robust,high-precisionlidar odometry and mapping package for lidars of small fov,"2020ieeeinternational conference on robotics and automation(icra),paris,france,2020,pp.3126-3131)文献提出了loam livox算法，该算法是loam算法针对混合固态激光雷达上的应用。loam livox的主要贡献有两个，一个是针对混合固态激光雷达扫描特性的有效点筛选和特征点提取，另一个是迭代的位姿优化策略，通过迭代的方式不断对畸变的点云进行校正。该算法的提出为之后slam算法适配于混合固态激光雷达提供了思路，但其并未解决混合固态激光雷达感知能力受限所导致的定位偏差问题，与此同时该算法过度依赖环境中的特征数量，在非结构化的环境下无法取得较好的定位结果。

4、方案2：文献(t.shan,b.englot,d.meyers,w.wang,c.ratti and d.rus,"lio-sam:tightly-coupled lidar inertial odometry via smoothing and mapping,"2020ieee/rsj international conference on intelligent robots and systems(iros),las vegas,nv,usa,2020,pp.5135-5142)该文献提出了lio-sam算法，该算法利用图优化的理念对激光里程计和惯性里程计进行紧耦合，使得里程计积分出的位姿更加精确。同时，在后端优化中，使用gps对部分位姿节点进行约束，从而能获得更加贴近于真实的建图效果。但是该算法要求必须使用9轴的惯性测量单元(inertial measurement unit，imu)进行融合，并且激光雷达和imu之间需要精确的标定，实现条件较为苛刻。与此同时该算法无法适用于机器人高速运动情况，当运动速度较快时，会产生定位失效和地图重叠的问题。

5、方案3：文献(w.xu,y.cai,d.he,j.lin and f.zhang,"fast-lio2:fast directlidar-inertial odometry,"in ieee transactions on robotics,vol.38,no.4,pp.2053-2073,aug.2022)该文献提出了fast-lio2算法，这是一种快速鲁棒的雷达惯性里程计，在紧耦合迭代卡尔曼滤波的基础上，主要做了两个关键创新，一个是采用直接法，不进行特征提取，通过利用环境中的细微特征来提高系统的准确性。另一个是通过增量式的kd-tree来进行地图维护，支持增量式的插入和删除，维护树的动态平衡，最终依靠这两点实现了快速准确的激光雷达定位。但美中不足的是该算法只是一个激光里程计，没有后端优化，导致机器人在未知大场景下进行定位时会产生较大累积误差，最终建立的点云地图与真实场景相差甚远。

技术实现思路

1、本发明的技术解决问题是：克服现有技术的不足，提出一种外部旋转3d激光雷达设备及其同时定位与建图方法，针对小视场角3d混合固态激光雷达应用于机器人时产生的感知能力受限，定位误差大，建图效率低等问题，提出了一种外部旋转3d激光雷达设备以及适用于该设备的slam方法，通过该设备让系统获得360度全向环境感知能力，利用与之适配的slam方法可以解决由于激光雷达视场角小导致的定位偏移问题，提高系统的定位鲁棒性和建图效率。

2、本发明的技术解决方案是：

3、一种外部旋转3d激光雷达设备，该3d激光雷达设备包括处理器(1)、混合固态激光雷达(2)、相机(3)、滑环(4)、联轴器(5)、步进电机(6)、脉冲发生器(7)、降压模块(8)、分电板(9)、电源(10)、电机驱动(11)；

4、所述处理器(1)搭载在混合固态激光雷达(2)的上方，用于接收混合固态激光雷达(2)的实时数据；

5、所述相机(3)固定安装在混合固态激光雷达(2)的一侧，用于录制视频；

6、所述步进电机(6)和滑环(4)均通过联轴器(5)与混合固态激光雷达(2)连接，即步进电机(6)、滑环(4)、联轴器(5)与混合固态激光雷达(2)一起组成旋转机构；

7、所述脉冲发生器(7)用于控制电机驱动(11)进而控制步进电机(6)；

8、所述电源(10)通过分电板(9)输出四路，两路用于脉冲发生器(7)和电机驱动(11)的供电，另外两路通过降压模块(8)和滑环(4)为处理器(1)和混合固态激光雷达(2)供电。

9、一种适配于外部旋转3d激光雷达设备的同时定位与建图方法，该方法的步骤包括：

10、(1)获取imu数据，并根据获取的imu数据对imu状态进行预测，得到imu状态初值；

11、(2)根据步骤(1)得到的imu状态初值对激光雷达的原始点云进行最近邻运动补偿，恢复真实场景结构；

12、(3)对步骤(2)运动补偿后的点云进行滤波处理；

13、(4)对滤波处理后的点云进行knn最近邻搜索，针对点云中的每个点寻找局部地图中离该点最近的5个点进行平面拟合，符合平面条件后，计算平面的法向量，并计算点到平面的距离作为激光雷达残差；

14、(5)当步骤(4)的激光雷达残差构建好后与步骤(1)的imu状态初值一起输入误差状态迭代卡尔曼滤波中，构建最大后验概率问题并进行迭代求解，当更新量小于设定的阈值则认为本次迭代收敛，输出当前帧的最优位姿估计；

15、(6)根据步骤(5)得到的最优位姿估计和步骤(3)中滤波处理后的点云进行回环帧检测，检测到回环帧后进行回环验证，如果验证为回环，则进行位姿图优化，并将优化结果更新到里程计和地图中，如果不是回环则不做处理，当机器人任务结束后，输出全局地图。

16、所述步骤(1)中，对imu状态进行预测的方法为：

17、设需要预测的状态变量为：

18、

19、其中，分别表示imu在全局坐标系(g)下的位置、姿态和速度，分别表示imu角速度和加速度的bias偏差，ggt表示在全局坐标系(g)下的重力矢量；

20、设状态预测过程中噪声为0，则状态预测公式为：

21、

22、其中，表示第m+1帧imu的粗略估计状态量，表示第m帧imu的粗略估计状态量，表示广义加运算符，δt表示相邻两帧imu的时间差，表示以第m帧imu的粗略估计状态量和第m帧imu数据um为输入的增量函数；

23、接着利用下面的误差状态动态模型来传播协方差：

24、

25、其中，表示第m+1帧imu状态量真值xm+1和第m+1帧imu粗略估计状态量的差值，表示广义减运算符，xm表示第m帧imu的状态量真值，g(xm，um，wm)表示以第m帧imu的状态量真值xm、第m帧imu数据um和第m帧imu的数据噪声wm为输入的增量函数；

26、至此，状态预测完成，得到位姿估计的初值即imu状态初值；

27、所述步骤(2)中，进行最近邻运动补偿的方法为：

28、位置、速度、姿态所需补偿量为：

29、

30、

31、

32、其中，n＝1，2，3…k，k表示激光雷达点云总帧数，j＝1，2，3…n，n表示一帧激光雷达点云采样点的总数，分别表示在imu坐标系下，将第j-1，j个点补偿到第n帧点云的结束时刻所需要的位置补偿量，δt表示第个点到最近邻imu帧的时间差。分别表示在imu坐标系下将第j-1，j个点补偿到第n帧点云的结束时刻所需要的速度补偿量，表示第m-1帧imu的加速度输入，表示第n帧点云结束时刻的加速度bias偏差，表示第n帧点云结束时刻imu坐标系下的重力矢量。分别表示在imu坐标系下，将第j-1，j个点补偿到第n帧点云的结束时刻所需要的姿态补偿量，表示第n帧点云结束时刻的角速度bias偏差，表示第m-1帧imu的角速度输入；

33、补偿后的点为：

34、

35、其中，表示第j个点补偿到第n帧点云结束时刻后的点位置，即补偿后的点，表示在imu坐标系下，将第j个点补偿到第n帧点云的结束时刻所需要的补偿量，itl表示激光雷达坐标系到imu坐标系的变换，表示第个点在激光雷达坐标系下的位置，即未补偿的点；

36、所述步骤(3)中，对运动补偿后的点云进行滤波处理的方法为：采用最小预处理的方式，不做特征提取，只采用两个滤波器：盒子滤波器和降采样体素滤波器；通过盒子滤波器滤除设备平台周围的正方体范围点云，防止自身点云数据影响算法处理，通过降采样体素滤波器对采集到的环境点云进行降采样，设置好体素网格大小后进行筛选，在保持周围环境的主要结构的同时，减少需要处理的点云数量；

37、所述步骤(4)中，激光雷达残差公式为：

38、

39、其中，i＝1，2，3…m，m表示总迭代次数，表示第j个点在第i次迭代的残差，uj表示拟合后平面的法向量，表示在第i次迭代时imu在第n帧点云结束时刻的位姿，itl表示激光雷达坐标系到imu坐标系的变换，表示补偿后的点，gqj表示第j个点的对应平面上一点在全局坐标系下的坐标；

40、所述步骤(5)中，构建的最大后验概率问题为：

41、

42、其中，r表示滤波处理后的一帧点云中点的个数，表示第i次迭代中第个点对应残差的雅可比，表示第i次迭代中在第n帧点云结束时刻的状态误差量，rj表示原始点云测量的方差，分别表示在第n帧点云结束时刻的状态量真值和估计值，表示传播到第n帧点云结束时刻的噪声协方差；

43、所述步骤(6)中，回环帧检测的方法为：

44、采用深度图匹配的方式对两帧激光点云的相似度进行判断，根据不同栅格中最高点云的高度值生成每帧点云不同的特征表达，在获得每一帧点云的特征图描述之后，计算不同特征图之间的相似度；

45、两帧特征图之间的向量距离为：

46、

47、其中，分别表示i，j特征图的第k列向量；

48、通过对所有列向量的距离取均值，得到两个特征图的相似度函数：

49、

50、其中，fi，fj分别表示i，j两张特征图，ns表示特征图中列向量的个数；

51、针对两张特征图可能出现的相位差的问题，在计算相似度函数时，采用了第一张特征图和将第二张特征图旋转后的相似度函数最大值作为最终的相似度函数，即为：

52、

53、其中，表示了将特征图fj的前n列移动至特征图的末尾组成的新的特征图，通过这样的方式，解决了不同观测角度导致的相位差问题，最终根据相似度函数计算结果将相似度最高且超过相似度阈值的点云帧作为回环帧；

54、验证是否为回环的方法为：当回环检测检测出回环帧后，对当前帧进行最近邻搜索，搜索在12m范围内的点云帧，并根据距离对点云帧从小到大排序，如果回环帧的id号位于前10(条件一)，且回环帧的时间戳距离当前时间超过25s(条件二)，那么就认为该回环帧是准确的，并进行位姿图优化，如果不同时满足这两个条件，则认为该回环帧不成立，不做处理；

55、所述位姿图优化的方法为：对经过验证后形成的回环约束进行求解，将回环帧附近的k个点云帧特征点投影到回环帧时刻坐标系下，得到该时刻坐标系下的局部地图，然后建立点线残差和点面残差方程，构建出关于在回环帧坐标系下的当前帧相对位姿变换的最小二乘优化问题：

56、

57、其中，分别表示局部线特征和局部面特征的数量，表示关于匹配到的线特征点pk和i，j两关键帧之间的位姿变换的点线残差约束，表示关于匹配到的面特征点pk和i，j两关键帧之间的位姿变换的点面残差约束，表示距离方差；

58、通过lm方法对该问题进行求解，得到两个回环帧之间的相对位姿变换，最后将优化后的位姿结果更新到里程计输出中，进而根据优化位姿结果进行地图更新，完成最终优化过程。

59、有益效果

60、本发明针对小视场角3d混合固态激光雷达应用于机器人时产生的感知能力受限，定位误差大，建图效率低等问题，提出了一种外部旋转3d激光雷达设备以及适用于该设备的slam算法，该发明的创新点主要体现在以下几个方面。

61、一、本发明设计了一种通过外部电机旋转混合固态3d激光雷达设备，该设备可以显著提升激光雷达的水平视场角，搭载于地面机器人后可全面提升机器人的360度环境感知能力。

62、二、本发明提出了一种可应用于该设备的slam算法，该算法将误差状态卡尔曼滤波与位姿图优化相结合，整体框架分为前端里程计和后端回环优化两部分，不仅可以实时输出满足机器人需求的高频里程计，还能通过后端的回环优化消除累计误差，修正里程计结果，进一步更新地图，最后输出高精度的全局点云地图。

63、三、经过实物测试后发现，将该算法应用于设备后可显著提升系统整体定位精度和建图质量，并相比之前大幅提升环境建图效率，十分适用于机器人在未知大场景下的定位与地图构建。