机器人的定位场景退化处理方法、快速定位方法及系统与流程

本发明涉及导航，具体来说是机器人的定位场景退化处理方法、快速定位方法及系统。

背景技术：

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

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

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

4、当前室内外定位算法存在一些解决方案，如采用gps来弥补室外定位的算法缺陷，然而，gps在室内的应用会受到很大的干扰，激光雷达在室外的应用会受到障碍物的影响，都不能同时适用室内和室外两种场景。其中，gps增加了系统的硬件成本，另外基于gps和激光雷达的定位存在两套定位系统，一般gps的使用只是在初识点进行获取参考位置，需要将gps的位置和地图坐标系在建立地图的时候就进行融合，增加算法的复杂度。采用视觉信息来弥补信息获取的丰富程度来解决室内外定位问题，其中，基于视觉的信息融合在夜晚或者光线不好的雨雾天气，引入其数据相当于引入了更大的误差，严重影响系统的鲁棒性。

5、但是当机器人在狭小空间如封闭的充电房间内，或狭长廊道空间，存在激光雷达数据信息获取不足无法定位的情况，可以通过基于imu和里程计数据融合的方式来实现，但是传统的基于滤波的数据融合存在需要动态维护和调整imu、odom、laser_pose中每个传感器数据6纬度（xyzrpy），一共是18*18维度的协方差矩阵或信息矩阵，维护这个矩阵复杂，参数多，传感器融合过程鲁棒性差、数据容易发散的问题。

技术实现思路

1、本发明所要解决的技术问题在于针对小范围区间或狭长空间的情况下，如何提高机器人的定位精度。

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

3、机器人的定位场景退化处理方法，包括以下步骤：

4、对机器人搭载的激光雷达获取的测量数据进行预处理，得到点云数据；

5、基于所述点云数据和全局地图信息，建立激光雷达测量模型：

6、zlidar = h(xrobot, map) + ε

7、式中，zlidar表示激光雷达测量值，xrobot表示机器人位置与姿态，map表示全局地图信息， h表示对应的非线性映射关系，即从机器人位姿到地图之间的映射，ε表示存在的误差；通过对所述激光雷达测量模型不断迭代，最小化误差ε即为对应的机器人位置；

8、其中，最小化误差ε的方法为：定义误差函数e，表示激光雷达测量值与机器人当前位置和地图之间的非线性映射之后的差异：

9、e(xrobot,map) = zlidar − h(xrobot, map)

10、式中，e(xrobot, map)表示误差向量；构建误差的最小二乘mine 如下：

11、mine = e(xrobot, map)te(xrobot,map)

12、式中， e(xrobot, map)t为e(xrobot, map)转置，采用高斯-牛顿法对上述方程进行迭代处理，在每次迭代中，更新参数dx，使目标函数下降；高斯牛顿法最后化简公式如下：

13、(jtj)dx = -jte

14、式中dx为未知待求解变量，j为雅可比矩阵，jt为雅可比矩阵的转置，e为当前测量的误差，该式可进一步简化表示成如下形式：

15、hdx = b

16、其中，h为jtj，用雅可比矩阵的转置乘以雅可比矩阵，可以用来近似表示黑塞矩阵，其为一个半正定的对称方阵，b为常量，通过eigen库中基于矩阵特征值分解方法可求解dx，即对应的迭代变化量；

17、针对黑塞矩阵h进行分解，得到特征值数组v和特征向量，获取特征值最大值和最小值的比值：

18、d = fabs（max（v）/ min（v））

19、其中，v为特征值数组，max(v)表示数组中的最大值，min(v)表示数组中的最小值，fabs()为求绝对值函数，d表示对应的比值的绝对值大小，若d小于设定第一阈值，则用imu传感器和里程计辅助激光雷达定位，若d大于设定第一阈值时，采用激光雷达的数据进行定位。

20、进一步的，所述激光雷达测量模型的迭代更新规则为：

21、[xrobot，map]←[xrobot, map]+ (jtj)−1jte

22、式中，xrobot表示机器人实际位姿，map表示全局地图信息，j是误差函数关于位姿的雅可比矩阵，jt为其转置，e为当前测量的误差向量；重复以上步骤直至收敛到一定阈值，得到xrobot相对于map之间的变换关系，即机器人当前时刻的定位结果数据。

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

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

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

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

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

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

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

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

31、本发明还提供一种机器人的定位场景退化快速定位方法，包括以下步骤：

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

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

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

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

36、s05、基于初始位姿作为实时定位算法输入，执行上述机器人的定位场景退化处理方法；最后计算实时位姿解算得分；

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

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

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

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

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

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

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

44、本发明还提供一种机器人的定位场景退化快速定位系统，包括：

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

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

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

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

49、定位计算模块，用于基于初始位姿作为实时定位算法输入，执行上述机器人的定位场景退化处理方法；最后计算实时位姿解算得分；

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

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

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

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

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

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

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

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

58、本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行上述的方法。

59、本发明的优点在于：

60、本发明基于特征值分解的高斯牛顿法进行算法迭代匹配，降低算法复杂度，提高实时性；该方法能够处理针对环境中定位算法退化问题，针对场景退化的限定，采用最大特征值和最小特征值之间的比值进行判断；特征值对外部环境的敏感性较强，机器人在运动过程中可能会遇到不同的退化场景，不同场景对应的特征值数据有比较明显的区别，特征值的最大值和最小值的比值属于一个相对量而非绝对量，可以有效地提升出系统的稳定性，如果比值过大，说明特征对比明显，否则不明显，采用一个合适的阈值，可以适应不同场景下的算法退化问题。如果采用绝对量进行判断，由于需要针对不同场景来进行设置阈值进行判断，增加了系统的复杂度，降低稳定性。

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