一种融合IMU的双激光雷达相对位姿标定方法及系统与流程

本发明属于位姿标定，更具体地，本发明涉及一种融合imu的双激光雷达相对位姿标定方法及系统。

背景技术：

1、随着激光agv在工业物流中的应用越来越广泛，应用场景对于定位导航的精度要求也原来越高，这个精度不仅仅依赖激光定位导航算法精度，还依赖于硬件之间位姿的标定精度。单激光雷达安装在agv上有时视野只有一半，为了提高环境感知定位精度，需要安装第二个激光雷达来补充另一半视野，因此就引出了双激光雷达位姿标定的要求。

2、为了解决上述问题，现有的方案主要包含如下两种：1)匹配共视区域特征，通过扫描提取相同的墙面直线和墙角位置坐标，通过多次旋转获取多组对应值，然后进行多组对应值的匹配。2)双激光雷达分别建立点云地图及相互匹配。

3、上述这两种方法都存在一些问题，第一种方法要求双雷达必须有共视区域，并且视野能扫描到一定范围的墙体才能提取精准的直线特征，第二种方法依赖于建图精度和相互匹配精度，如果地图的边界较厚那么匹配的精度就会比较差，且通过多次旋转和停止来累积扫描帧构建地图的标定效率较低。

技术实现思路

1、本发明提供一种融合imu的双激光雷达相对位姿标定方法，旨在改善上述问题。

2、本发明是这样实现的，一种融合imu的双激光雷达相对位姿标定方法，所述方法包括如下步骤：

3、(1)初步测量第二激光雷达lidar2在第一激光雷达lidar1下的估计位姿

4、(2)基于第一激光雷达lidar1点云和imu数据建立位姿推算器；

5、(3)通过imu数据和位姿推算器矫正第二激光雷达lidar2点云，基于矫正后的第二激光雷达lidar2点云计算第二激光雷达lidar2点云的定位位姿t_tracklidar2；

6、(4)基于定位位姿t_tracklidar2计算第二激光雷达相对于第一激光雷达、imu的初始位姿

7、(5)计算第二激光雷达相对于第一激光雷达的准确位姿及第二激光雷达lidar2相对imu的准确位姿

8、进一步的，将第一激光雷达lidar1点云变换到imu坐标系下，通过icp_slam方法得到机器人在当前位置的定位位姿t_tracklidar1和点云地图m，将定位位姿t_tracklidar1作为对应时刻的局部位姿loc_pose放入位姿推算器，基于位移推算器来计算不同扫描点的局部位姿。

9、进一步的，位姿推算器ep具体如下：

10、ep:{loc_poset-1,loc_poset-2,linear_v,angle_v}；

11、其中，loc_poset-1、loc_poset-2分别表示t-1时刻、t-2时刻的机器人位姿，通过位姿loc_poset-1、loc_poset-2和时间差δt计算出机器人的线速度linear_v和角速度angle_v，进而推算t时刻的位姿loc_poset：

12、loc_poset＝loc_poset-1+δt{linear_v,angle_v}

13、进一步的，第二激光雷达lidar2点云的矫正过程具体如下：

14、(31)计算第二激光雷达lidar2到imu的估计位姿

15、(32)基于估计位姿将第二激光雷达lidar2点云中的点pi变换到世界坐标系下，loc_poset_i表示在t_i时刻的局部位姿，t_i表示点pi的扫描时刻；

16、(33)将第二激光雷达lidar2点云中的点pi矫正到lidar2点云中尾点时刻t_n的imu坐标系下，获取矫正后的lidar2点云p_i mi，p_imui＝loc_poset_n-1*pi，loc_poset_n表示第二激光雷达lidar2点云中最后一个点所在时刻t_n的局部位姿。

17、进一步的，第二激光雷达lidar2点云的定位位姿t_tracklidar2的获取过程具体如下：

18、(41)将矫正的点云p_imui通过当前定位位姿t_tracklidar2投影到点云地图m中，构建的非线性最小二乘目标函数f如下：

19、

20、其中，mi是点p_imui投影到地图m中的最近点，t_tracklidar2的初值是

21、(42)通过高斯牛顿优化求解最小目标函数f的定位位姿t_tracklidar2；

22、(43)检测当前迭代次数是否达到设定的最大次数，若未达到，将该定位位姿作为当前定位位姿，执行步骤(41)，若达到，该最小目标函数f的定位位姿t_tracklidar2即为lidar2投影到imu坐标系下的点云和地图m匹配的最优。

23、进一步的，初始位姿和初始位姿的获取方法具体如下：

24、将初始位姿放入位姿集中；

25、将第二激光雷达lidar2相对于imu的初始位姿放入位姿集中；

26、循环步骤(3)至(4)，循环执行k次，计算位姿集和位姿集中的位姿均值，获取初始位姿和初始位姿

27、进一步的，将初始位姿和初始位姿分别替换估计位姿和估计位姿重新进行步骤(3)到步骤(4)，更新初始位姿和初始位姿

28、进一步的，第一激光雷达相对于第二激光雷达的准确位姿计算公式具体如下：

29、

30、进一步的，第二激光雷达lidar2相对imu的准确位姿

31、

32、本发明是这样实现的，一种融合imu的双激光雷达相对位姿标定系统，所述系统包括：

33、第一激光雷达lidar1，与imu通过机械部件刚性固连，第一激光雷达与imu间的相对位姿为imu采集机器人的线速度，角速度；

34、第二个激光雷达lidar2，设于机器人的另一侧，

35、设于imu上的处理单元，处理单元与第一激光雷达lidar1、第二个激光雷达lidar2通讯连接；

36、处理单元基于上述融合imu的双激光雷达相对位姿标定方法来标定第二激光雷达相对于第一激光雷达、imu的位姿。

37、本发明提供的融合imu的双激光雷达相对位姿标定方法具有如下有益技术效果：

38、(1)用第一激光雷达和imu建立一个点云地图，然后使用第二激光雷达去匹配点云地图，不需要两个雷达有共视区域，且本方法不需要建立两个点云地图，使得造成更大的累积误差；

39、(2)标定引入了imu进行点云的矫正，使得建图定位的点云畸变较小，建立的地图累积误差更小，地图点云和定位精度更高，最终提高了双雷达的标定精度。

技术特征：

1.一种融合imu的双激光雷达相对位姿标定方法，其特征在于，所述方法包括如下步骤：

2.如权利要求1所述融合imu的双激光雷达相对位姿标定方法，其特征在于，将第一激光雷达lidar1点云变换到imu坐标系下，通过icp_slam方法得到机器人在当前位置的定位位姿t_tracklidar1和点云地图m，将定位位姿t_tracklidar1作为对应时刻的局部位姿loc_pose放入位姿推算器，基于位移推算器来计算不同扫描点的局部位姿。

3.如权利要求2所述融合imu的双激光雷达相对位姿标定方法，其特征在于，位姿推算器ep具体如下：

4.如权利要求1所述融合imu的双激光雷达相对位姿标定方法，其特征在于，第二激光雷达lidar2点云的矫正过程具体如下：

5.如权利要求1所述融合imu的双激光雷达相对位姿标定方法，其特征在于，第二激光雷达lidar2点云的定位位姿t_tracklidar2的获取过程具体如下：

6.如权利要求1所述融合imu的双激光雷达相对位姿标定方法，其特征在于，初始位姿和初始位姿的获取方法具体如下：

7.如权利要求6所述融合imu的双激光雷达相对位姿标定方法，其特征在于，将初始位姿和初始位姿分别替换估计位姿和估计位姿重新进行步骤(3)到步骤(4)，更新初始位姿和初始位姿

8.如权利要求7所述融合imu的双激光雷达相对位姿标定方法，其特征在于，第一激光雷达相对于第二激光雷达的准确位姿计算公式具体如下：

9.如权利要求7所述融合imu的双激光雷达相对位姿标定方法，其特征在于，第二激光雷达lidar2相对imu的准确位姿

10.一种融合imu的双激光雷达相对位姿标定系统，其特征在于，所述系统包括：

技术总结
本发明公开一种融合IMU的双激光雷达相对位姿标定方法，包括如下步骤：(1)初步测量第二激光雷达lidar2在第一激光雷达lidar1下的估计位姿T_e<supgt;l</supgt;<subgt;l</subgt;<supgt;i</supgt;<subgt;i</subgt;<supgt;d</supgt;<subgt;d</subgt;<supgt;a</supgt;<subgt;a</subgt;<supgt;r</supgt;<subgt;r1</subgt;<supgt;2</supgt;；(2)基于第一激光雷达lidar1点云和IMU数据建立位姿推算器；(3)通过IMU数据和位姿推算器矫正第二激光雷达lidar2点云，基于矫正后的第二激光雷达lidar2点云计算第二激光雷达lidar2点云的定位位姿T_track<subgt;lidar2</subgt;；(4)基于定位位姿T_track<subgt;lidar2</subgt;计算第二激光雷达相对于第一激光雷达、IMU的初始位姿T_init<supgt;l</supgt;<subgt;l</subgt;<supgt;i</supgt;<subgt;i</subgt;<supgt;d</supgt;<subgt;d</subgt;<supgt;a</supgt;<subgt;a</subgt;<supgt;r</supgt;<subgt;r1</subgt;<supgt;2</supgt;、T_init<subgt;i</subgt;<supgt;li</supgt;<subgt;m</subgt;<supgt;da</supgt;<subgt;u</subgt;<supgt;r2</supgt;；(5)计算第二激光雷达相对于第一激光雷达的准确位姿T<subgt;l</subgt;<supgt;l</supgt;<subgt;i</subgt;<supgt;i</supgt;<subgt;d</subgt;<supgt;d</supgt;<subgt;a</subgt;<supgt;a</supgt;<subgt;r</subgt;<supgt;r</supgt;<subgt;1</subgt;<supgt;2</supgt;及第二激光雷达lidar2相对IMU的准确位姿T<subgt;i</subgt;<supgt;li</supgt;<subgt;m</subgt;<supgt;d</supgt;<subgt;u</subgt;<supgt;ar2</supgt;。使用IMU数据进行点云矫正，使用矫正后的点云使得标定过程中的定位位姿数据更准确，最终提高了双激光雷达的相对位姿标定精度。

技术研发人员：郝奇,陈智君,郑亮,方靖森,赵法龙,曹雏清,赵立军
受保护的技术使用者：长三角哈特机器人产业技术研究院
技术研发日：
技术公布日：2024/3/24