用于机器人室内外多传感器少参数的位姿优化方法、定位方法与流程

本发明涉及导航，具体来说是用于机器人室内外多传感器少参数的位姿优化方法、定位方法。

背景技术：

1、随着机器人技术的发展，出现了自主移动机器人(autonomous mobile robots，amr)。amr是具备see-think-act(看见、思考、行动)能力的机器，可以感知环境，自主建立环境模型，从而获取自身在环境中的位姿信息，规划并控制机器人移动。

2、传统技术中，先对机器人进行定位，确定机器人的当前位置；再根据机器人的当前位置，确定机器人的移动方向。而机器人的当前位置，可以利用gps(globalpositioningsystem，全球定位系统)确定机器人在世界地图上的位置，也可以利用激光雷达扫描得到点云数据，将点云数据与点云地图进行匹配，得到机器人在点云地图上的位置。

3、近年来很多公司都投入到对移动机器人的研究，也让移动机器人广泛的走入人们的生产生活当中，例如京东所研发的室外无人配送机器人，石头科技的智能扫地机器人，各大港口的智能agv叉车等。但是这些机器人的环境感知和导航设计均只能适用于室内或是室外的单一环境，难以实现机器人在室内室外环境切换时的定位导航无缝衔接。

4、当前室内外定位算法存在一些解决方案，如采用gps来弥补室外定位的算法缺陷，然而，gps在室内的应用会受到很大的干扰，激光雷达在室外的应用会受到障碍物的影响，都不能同时适用室内和室外两种场景。其中，gps增加了系统的硬件成本，另外基于gps和激光雷达的定位存在两套定位系统，一般gps的使用只是在初识点进行获取参考位置，需要将gps的位置和地图坐标系在建立地图的时候就进行融合，增加算法的复杂度。采用视觉信息来弥补信息获取的丰富程度来解决室内外定位问题，其中，基于视觉的信息融合在夜晚或者光线不好的雨雾天气，引入其数据相当于引入了更大的误差，严重影响系统的鲁棒性。此外，传统采用基于多传感器松或紧耦合等方式融合的机器人定位算法，需要设计机器人系统并进行多传感器之间位置和姿态关系的标定，工作量大，且相关融合参数数量多，参数设计或配置不合理时，定位系统容易发散，进而增加系统的复杂度和工作量，降低系统的鲁棒性。如传统的基于滤波的数据融合存在需要动态维护和调整imu、odom、laser_pose中每个传感器数据6纬度(xyzrpy)，一共是18*18维度的协方差矩阵或信息矩阵，需要通过不断维护这个参数矩阵，否则传感器融合过程鲁棒性差、数据容易发散的问题。

技术实现思路

1、本发明所要解决的技术问题在于针对多传感器少参数融合的定位算法，解决机器人室内外无缝衔接定位优化问题。

2、本发明通过以下技术手段实现解决上述技术问题的：

3、一种用于机器人室内外多传感器少参数的位姿优化方法，包括以下步骤：

4、步骤1.对机器人搭载的激光雷达获取的测量数据进行预处理，得到点云数据；并基于ceres优化剔除点云数据的地面点云的面点云；

5、步骤2.遍历去除地面点云和平面点云之后的点云数据，对每个点进行处理，将当前点根据用户给定的初始定位数据或上一次计算结果作为输入，将激光雷达坐标系转换到map坐标系；

6、步骤3.使用k-d树在map中找到当前点的最近的n个邻居点。如果最远的邻居点与当前点的距离小于阈值m，则继续处理，否则跳过当前点；基于n个邻居点拟合出了一个平面；

7、步骤4.计算当前点到平面的距离dist，并计算当前点在平面上的投影点point_proj；

8、步骤5.构建定位模型：创建一个代价函数，代价函数定义了一个名为costplan的结构体，在costplan结构体中，operator()函数被配置成接收两个参数：

9、const t*pri：为指向6维向量的指针，表示机器人的位姿，6维向量的前三个元素是位置，后三个元素是旋转的对数映射；

10、t*residual：为指向3维向量的指针，用于存储计算出的残差；

11、operator()首先将输入的当前点和投影点转换为模板类型t，然后将输入的位姿映射为一个6维向量，使用基于c++和eigen库开发的sophus库，其中用来表示三维旋转群so(3)类将旋转部分的对数映射转换为四元数，然后计算出从世界坐标系到局部坐标系的旋转和平移，最后计算出当前点在map上的位置，并计算出当前点到投影点的距离，将其保存到残差中；

12、步骤6.为了使得残差的平方和最小，调用ceres库中的solve函数来求解，得到相应的定位结果x0。

13、步骤7.将获取的定位结果x0输出，作为步骤2的输入，重复以上步骤2-步骤7直至收敛到一定阈值，得到机器人位置姿态与map之间的变换关系，即当前时刻的定位数据。

14、进一步的，针对激光雷达获取的测量数据进行预处理方法具体为：

15、先将测量数据转换为pcl格式，根据获取到的测量数据，即点云数据，计算每个点云在点云中的行号，并进行重投影；

16、然后计算地面点，对所有点位中水平角度以下的点进行提取，对于i束中的激光点，在i+1束找到对应点，计算这两点与雷达的连线所形成的向量的差向量，若该差向量与水平面所形成的夹角小于设定值，则认为是一个地面点；

17、对剩余点云进行聚类，并记录下来聚类后的点云，并对地面点云稀疏化，保存分割后的点云；

18、基于imu的激光去畸变，计算曲率，计算光滑度并去除不可靠点；

19、对当前点云帧按线束，分别提取特征，将每一份中的点云的曲率按从小到大排列，提取线特征，将剩下所有的点设置为面点，然后进行下采样来减小计算量，最后进行点云发布。

20、进一步的，所述激光去畸变的具体方法为：

21、实时订阅imu传感器数据并保存在队列中，获取到imu传感器数据的每帧的姿态、交底、角速度、角加速度，并记录每帧的imu传感器时间戳；记录激光雷达初始点时刻imu传感器姿态；针对激光雷达的每个点数据进行分析，得到点云中每个点对应的时间戳；针对当前时间戳寻找imu队列中的前后imu数据；通过slerp插补到对应点对应的姿态；得到每个点云对应的姿态，通过将当前点通过姿态逆变换到初始位姿，得到每个点相对于初始点云的位置；遍历完所有一帧点云数据后，得到整个点云相对于激光初始时刻的位置；将计算出新的点云数据赋值给原来存储点云的位置作为去畸变后的点云。

22、进一步的，所述步骤3中拟合平面的方法为：

23、对找到的n个邻居点，计算它们的均值，并构造一个nxm的矩阵mata0，每一行是一个邻居点的坐标；使用列主元qr分解求解线性方程组_mata0*_matx0＝_matb0，其中_matb0是一个nx1的矩阵，所有元素都是-1，方程组的解_matx0是平面的参数。从解_matx0中获取平面的法向量(pa,pb,pc)和距离原点的距离pd，并进行归一化；保证其长度为1，拟合出一个平面。

24、本发明还提供一种移动机器人室内外高频实时定位方法，包括以下步骤：

25、s01、获取移动机器人初始激光雷达定位数据，并保存为初始位姿；

26、s02、基于该定位数据获取附近一定范围内的关键点，基于该关键点找到对应角点和面点对应的点云地图数据；

27、s03、将点云地图数据转换到map坐标系下，得到转换点云地图；

28、s04、将转换点云地图与map进行icp算法匹配，如果匹配得分高于第二阈值，则执行步骤s01，否则将获取的icp算法初始位姿作为初始值带入步骤s05；

29、s05、基于初始位姿作为实时定位算法输入，执行权利要求1至4任一所述方法；最后计算实时位姿解算得分；

30、s06、当得分大于第三阈值时，使用激光雷达数据与imu数据、里程计数据进行融合，否则采用激光雷达计算出的结果作为融合后的结果，此时imu和里程计不参与计算；；将融合的数据实时传输给步骤s05，作为预测值进行下一次迭代计算。

31、进一步的，所述步骤s06中的融合方法为：

32、imu数据处理：对imu数据被用于积分计算，得到机器人的位移和旋转；

33、里程计数据处理：对里程计数据被用于积分计算，得到机器人的位移；

34、激光雷达数据处理：将激光雷达与地图数据匹配计算出的数据赋值给一个变量，获取的数据即为机器人的位姿；

35、定位数据融合：首先，如果激光雷达的位姿和上一次的位姿不同，并且实时位姿解算得分小于第三阈值，那么认为激光雷达的位姿是准确的，直接用作融合后的位姿；同时使用imu和里程计的数据，来估计机器人上一帧激光雷达和当前帧之间位姿；这些位姿以上一帧的激光雷达位姿为起始使用黎曼积分，得到融合后的位姿；

36、发布数据：融合后的位姿被发布对应的主题上，最后获取的数据频率不低于100hz。

37、本发明还提供一种移动机器人室内外高频实时定位系统，包括：

38、初始位姿获取模块，用于获取移动机器人初始激光雷达定位数据，并保存为初始位姿；

39、点云地图数据获取模块，用于基于该定位数据获取附近一定范围内的关键点，基于该关键点找到对应角点和面点对应的点云地图数据；

40、地图转换模块，用于将点云地图数据转换到map坐标系下，得到转换点云地图；

41、位姿匹配模块，用于将转换点云地图与map进行icp算法匹配，如果匹配得分高于第二阈值，则执行初始位姿获取模块，否则将获取的icp算法初始位姿作为初始值带入定位计算模块；

42、定位计算模块，用于基于初始位姿作为实时定位算法输入，执行权利要求1至4任一所述方法；最后计算实时位姿解算得分；

43、融合计算模块，用于当得分大于第三阈值时，使用激光雷达数据与imu数据、里程计数据进行融合，否则采用激光雷达计算出的结果作为融合后的结果，此时imu和里程计不参与计算；将融合的数据实时传输给步骤s05，作为预测值进行下一次迭代计算。

44、进一步的，所述融合计算模块中的融合方法为：

45、imu数据处理：对imu数据被用于积分计算，得到机器人的位移和旋转；

46、里程计数据处理：对里程计数据被用于积分计算，得到机器人的位移；

47、激光雷达数据处理：将激光雷达与地图数据匹配计算出的数据赋值给一个变量，获取的数据即为机器人的位姿；

48、定位数据融合：首先，如果激光雷达的位姿和上一次的位姿不同，并且实时位姿解算得分小于第三阈值，那么认为激光雷达的位姿是准确的，直接用作融合后的位姿；同时使用imu和里程计的数据，来估计机器人上一帧激光雷达和当前帧之间位姿；这些位姿以上一帧的激光雷达位姿为起始使用黎曼积分，得到融合后的位姿；

49、发布数据：融合后的位姿被发布对应的主题上，最后获取的数据频率不低于100hz。

50、本发明还提供一种处理设备，其包括至少一个处理器，以及与所述处理器通信连接的至少一个存储器，其中：所述存储器存储有可被处理器执行的程序指令，所述处理器调用所述程序指令能够执行如权利要求1至6任一所述的方法。

51、本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如权利要求1至6任一所述的方法。

52、本发明的优点在于：

53、本发明基于ceres去除地面点云特征点迭代优化方法，明显地提高实时定位算法处理速度；对算力要求小，实时性高，鲁棒性高；可兼容白天和夜晚以及光线不好的雨雾天气；

54、传感器融合过程鲁棒、避免融合之后发散、调整和维护一个imu、odom、laser_pose中每个变量6纬度，一共是18*18维度的协方差矩阵或信息矩阵；无需进行2d地图切换，直接对3d地图进行匹配定位；在充电房内激光雷达定位不好的地方仍然能够进行100hz融合定位结果输出；本算法适当降低数据的处理量，优化处理逻辑，最终在intel处理器的cpu单核实时处理利用率小于50％，支持嵌入式处理器实时运行；

55、本发明采用imu、里程计、激光雷达匹配算法融合，无需采用基于传统滤波算法，参数少，速度快，鲁棒性高，实时输出100hz定位数据。