基于多线激光雷达的机器人定位方法及系统与流程

本发明涉及机器人，特别涉及一种基于多线激光雷达的机器人定位方法及系统。

背景技术：

1、随着国家智能制造产业发展，轮式机器人技术越来越成熟，各行各业的应用场景也越来越丰富，目前轮式机器人通过建立场景地图，随后按照任务需求规划路径顶点导航，导航和指点任务都需要机器人实时输出在当前地图中的位姿信息。所以，如何获得机器人当前准确的实时位姿成为问题关键。由于需求场景越来越多，轮式机器人需要同时满足室内外定位导航的要求，这也增加了获取准确定位信息的难度。

2、机器人在已有地图上的准确定位，目前常用的技术有通过gps实时给机器人定位，但是在室内场景gps无法完全覆盖到，定位会失效；在应用场景中安装反光板、磁钉或者二维码辅助计算机器人位姿，该方法需要添加辅助设备，对原场景进行改造，并且当场景较大时，施工不便，不适用于通用场景；仅使用单线激光雷达做slam定位输出，仅仅有二维约束，室外场景又坡道和复杂路况时满足多变的环境。

技术实现思路

1、基于此，有必要提供一种提高定位性能的基于多线激光雷达的机器人定位方法。

2、同时，提供一种提高定位性能的基于多线激光雷达的机器人定位系统。

3、一种基于多线激光雷达的机器人定位方法，包括：

4、初始化系统：接收激光数据、里程计数据、地图数据；

5、栅格滤波：对接收的激光进行栅格滤波，将当前激光点云划入固定边长的体素栅格内，落入同一个格子中的所有点使用该格子的中心点坐标表示；

6、位姿初始化：加载地图，给与机器人初始位姿或计算初始位姿，初始化；

7、里程计预测位姿：根据机器人运动输出定位位姿，根据里程计增量预测当前里程计预测位姿并储存；

8、激光匹配预测位姿：根据多线激光的帧间匹配计算出上一帧点云和当前帧点云之间的位姿变化关系，预测激光匹配预测位姿；

9、预测位姿：里程计预测位姿与激光匹配预测位姿按照设置权重进行融合，输出预测位姿；

10、优化位姿：根据激光与地图匹配消除累计误差，优化位姿。

11、在优选实施例中，所述里程计预测位姿步骤中，根据里程计增量预测当前里程计预测位姿计算公式为：

12、，

13、，

14、式中 odom current和 odom last分别为本次里程计位姿和上一次里程计位姿， odom last( inv)表示上一次里程计位姿 odom last的逆， odom_ delta( inv)表示计算出的  odom_ delta的逆， pose_ odom current和  pose last分别为本次里程计预测的位姿和上一次对外发布的位姿。

15、在优选实施例中，所述激光匹配预测位姿步骤中，预测激光匹配预测位姿，设上一帧点云为 p＇，当前帧点云为 p，上一帧点云经过旋转和平移后与当前帧点云相减得到的残差越小，则两组点云匹配越好，如下公式表示：

16、，

17、式中 j表示残差， r表示旋转矩阵， t表示平移矩阵，根据上式构建最小而成问题，求的使误差平方和达到最小时的 r和 t为匹配最优解，用如下公式表示：

18、，

19、式中 n为点云总数， i为当前计算的点云排序，计算出最优 r和 t后，由如下公式计算出激光匹配预测的位姿 pose_ laser current ：

20、。

21、在优选实施例中，所述预测位姿步骤中，预测位姿为：

22、，

23、式中 pose current为融合后的预测位姿， w为权重，其取值区间为0≤ w≤1，对于斜坡，坑洼地面不平整场景，根据传感器数据判断是否为斜坡、坑洼地面、不平整场景，此时赋予里程计预测位姿权重为相对高权重，给激光匹配预测位姿相对低权重，增大 w；当在激光匹配失效场景时，赋予激光匹配预测位姿相对高权重，里程计预测位姿相对低权重，降低 w。

24、在优选实施例中，激光匹配失效场景包括：狭窄空间、空旷区域、电梯内等。

25、在优选实施例中，所述优化位姿步骤中，将点云地图按照 pose current位姿和当前帧点云进行最近邻搜索，搜索出的点云为 p map，将 p map所占的空间划分为指定大小的体素，计算体素中的每一个中心点均值和协方差矩阵：

26、，

27、，

28、式中q为每一个中心点均值，∑为协方差矩阵，n为 p map点云个数， p i ＇代表 p map中第i个点，获得均值q和协方差∑，构建正态分布n(q,∑ ),概率密度函数为：

29、，

30、将当前帧点云 x经过旋转和平移后的点云 x＇映射到 p map上,点云 x变换到点云 x＇的公式如下：

31、，

32、式中 r表示旋转矩阵， t表示平移矩阵，评估每一个映射点的分布并对结果累加，公式如下：

33、，

34、式中 q i第 i个体素的中点，根据上述式子寻找最优的r和t使得 x＇能够满足 score (p)最大，此时当前帧点云和地图最为匹配；

35、当计算出的 score(p)大于设定阈值时，使用 x＇位姿作为当前最优位姿对外发布，后重复里程计预测位姿步骤至优化位姿步骤；如果 score(p)小于设定阈值，则使用 pose current作为最优位姿对外发布。

36、在优选实施例中，还包括定位评估：若当前帧计算出的 score(p)小于设定阈值，并且距上一次匹配成功之间位移超过设定阈值，上报异常。

37、一种基于多线激光雷达的机器人定位系统，包括：

38、初始化系统模块：接收激光数据、里程计数据、地图数据；

39、栅格滤波模块：对接收的激光进行栅格滤波，将当前激光点云划入固定边长的体素栅格内，落入同一个格子中的所有点使用该格子的中心点坐标表示；

40、位姿初始化模块：加载地图，给与机器人初始位姿或计算初始位姿，初始化；

41、里程计预测位姿：根据机器人运动输出定位位姿，根据里程计增量预测当前里程计预测位姿并储存；

42、激光匹配预测位姿模块：根据多线激光的帧间匹配计算出上一帧点云和当前帧点云之间的位姿变化关系，预测激光匹配预测位姿；

43、预测位姿模块：里程计预测位姿与激光匹配预测位姿按照设置权重进行融合，输出预测位姿；

44、优化位姿模块：根据激光与地图匹配消除累计误差，优化位姿。

45、在优选实施例中，还包括：所述里程计预测位姿模块中，根据里程计增量预测当前里程计预测位姿计算公式为：

46、，

47、，

48、式中 odom current和 odom last分别为本次里程计位姿和上一次里程计位姿， odom last( inv)表示上一次里程计位姿 odom last的逆， odom_ delta( inv)表示计算出的  odom_ delta的逆， pose_ odom current和  pose last分别为本次里程计预测的位姿和上一次对外发布的位姿；

49、所述激光匹配预测位姿步骤中，预测激光匹配预测位姿，设上一帧点云为 p＇，当前帧点云为 p，上一帧点云经过旋转和平移后与当前帧点云相减得到的残差越小，则两组点云匹配越好，如下公式表示：

50、，

51、式中 j表示残差， r表示旋转矩阵， t表示平移矩阵，根据上式构建最小而成问题，求的使误差平方和达到最小时的 r和 t为匹配最优解，用如下公式表示：

52、，

53、式中 n为点云总数， i为当前计算的点云排序，计算出最优 r和 t后，由如下公式计算出激光匹配预测的位姿 pose_ laser current ：

54、。

55、在优选实施例中，所述预测位姿模块中，预测位姿为：

56、，

57、式中 pose current为融合后的预测位姿， w为权重，其取值区间为0≤ w≤1，对于斜坡，坑洼地面不平整场景，根据传感器数据判断是否为斜坡、坑洼地面、不平整场景，此时赋予里程计预测位姿权重为相对高权重，给激光匹配预测位姿相对低权重，增大 w；当在激光匹配失效场景时，赋予激光匹配预测位姿相对高权重，里程计预测位姿相对低权重，降低 w。

58、在优选实施例中，激光匹配失效场景包括：狭窄空间、空旷区域、电梯内等。

59、在优选实施例中，所述优化位姿模块中，将点云地图按照 pose current位姿和当前帧点云进行最近邻搜索，搜索出的点云为 p map，将 p map所占的空间划分为指定大小的体素，计算体素中的每一个中心点均值和协方差矩阵：

60、，

61、，

62、式中q为每一个中心点均值，∑为协方差矩阵，n为 p map点云个数， p i ＇代表 p map中第i个点，

63、获得均值q和协方差∑，构建正态分布n(q,∑),概率密度函数为：

64、，

65、将当前帧点云 x经过旋转和平移后的点云 x＇映射到 p map上,点云 x变换到点云 x＇的公式如下：

66、，

67、式中 r表示旋转矩阵， t表示平移矩阵，

68、评估每一个映射点的分布并对结果累加，公式如下：

69、，

70、式中 q i第 i个体素的中点，根据上述式子寻找最优的r和t使得 x＇能够满足 score (p)最大，此时当前帧点云和地图最为匹配；

71、当计算出的 score(p)大于设定阈值时，使用 x＇位姿作为当前最优位姿对外发布，后重复里程计预测位姿步骤至优化位姿步骤；如果 score(p)小于设定阈值，则使用 pose current作为最优位姿对外发布。

72、在优选实施例中，还包括定位评估模块：若当前帧计算出的 score(p)小于设定阈值，并且距上一次匹配成功之间位移超过设定阈值，上报异常。

73、上述基于多线激光雷达的机器人定位方法及系统，提出一种轮式机器人使用多线激光雷达融合里程计和imu（inertial measuring unit惯性测量仪）数据在已知点云地图中输出实时位姿，无需对环境进行独单改造，可以实现同时满足室内外定位要求。无需安装gps，可以室内外通用，不受信号不稳定等因素干扰，通用性强。无需对环境进行贴磁条、标签或者反光板等专门改造，部署使用方便。对于不同场景可以自适应调整预测权重，适应性强。定位过程中，遇到定位异常状态，可以及时上报，安全性强。