用于机器人的定位方法以及应用其的机器人与流程

1.本公开涉及机器人技术领域，尤其涉及一种用于机器人的定位方法以及应用其的机器人。


背景技术：

2.随着移动机器人的快速发展，特别是在自主导航车、无人车和服务机器人等应用领域，各种各样的自动化任务，如物体抓取、空间探索等，对移动机器人的定位提出了更高的要求，而里程计技术在其中扮演着极其重要的作用。其中，激光雷达和轮式里程计是最常用的提升机器人对环境的感知和自身定位的模块。
3.里程计利用观测的左右轮转速值得到的速度与角速度，代入移动机器人运动学模型中，推演出机器人当前的位置与航向角信息。由于，测量误差、执行机构误差、模型误差以及可能发生的侧滑等原因，里程计信息也产生较大的误差。由于里程计是利用几何模型不断推演出来的，误差会一直累积在结果里，最终会不可用。但里程计在静态时不会漂移，并且具有成本低廉的优点。
4.基于激光雷达和里程计的机器人定位方案中，机器人在当前时刻的位姿等于里程计的预测位姿和激光雷达的矫正位姿之和，但是，里程计的预测位姿的过程以及激光雷达矫正位姿的过程都存在误差，在确定最终结果时，存在误差累积的问题，并且在激光信息与地图匹配时的误匹配，也会导致矫正位姿的错误，最终定位结果也是错误的。
5.背景技术部分的内容仅仅是公开发明人所知晓的技术，并不当然代表本领域的现有技术。


技术实现要素：

6.有鉴于现有的一个或多个缺陷，本发明提供一种用于机器人的定位方法，所述机器人包括激光雷达和里程计，所述定位方法包括：
7.通过所述里程计获取所述机器人的位姿；
8.根据上一时刻的位姿，预测当前时刻的位姿；
9.通过所述激光雷达获取当前时刻实测的点云；
10.将所述实测的点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度；
11.根据所述匹配的点云、所述位移置信度和旋转置信度，获取矫正位姿；
12.根据所述预测位姿和矫正位姿，确定当前时刻的位姿。
13.根据本发明的一个方面，其中所述将实测的点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度的步骤具体包括：通过icp算法，将所述实测的点云与当前运行环境的点云地图进行匹配，根据匹配结果获取位移置信度和旋转置信度。
14.根据本发明的一个方面，其中所述将实测的点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度的步骤具体包括：根据匹配的点云中每个点的法线方向，确
定第一方向和第二方向，根据第一方向的预设范围内的总点数以及第二方向的预设范围内的总点数，确定位移置信度。
15.根据本发明的一个方面，其中所述第一方向为所述匹配的点云中每个点的法线方向的平均方向，所述第二方向与所述第一方向垂直。
16.根据本发明的一个方面，其中根据第一方向的预设范围内的总点数以及第二方向的预设范围内的总点数确定位移置信度的操作包括：当第一方向的预设范围内的总点数大于阈值时，该方向的位移可信；当第二方向的预设范围内的总点数大于阈值时，该方向的位移可信。
17.根据本发明的一个方面，其中所述将点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度的步骤还包括：将所述匹配的点云中的点进行聚类，根据最大聚类点云确定第三方向，根据所述第三方向的点云分布，确定旋转置信度。
18.根据本发明的一个方面，其中所述第三方向为所述最大聚类点云中每个点的法线方向的平均方向。
19.根据本发明的一个方面，其中所述根据最大聚类点云确定第三方向，根据第三方向的点云分布，确定旋转置信度的操作具体包括：当所述第三方向的点云分布长度大于阈值时，对应的旋转角度可信。
20.根据本发明的一个方面，其中所述根据匹配的点云、位移置信度和旋转置信度，获取矫正位姿的步骤包括：
21.根据所述匹配的点云中位移可信的第一方向和/或第二方向的点，计算矫正位移；
22.根据所述匹配的点云中旋转角度可信的第三方向的点，计算矫正旋转角度；
23.根据所述矫正位移和所述矫正旋转角度，合成矫正位姿。
24.根据本发明的一个方面，其中所述根据匹配的点云中位移可信的第一方向和/或第二方向的点，计算矫正位移的操作具体包括：将位移可信的第一方向和/或第二方向在预设范围内每个点在该方向的位移分量合并为矫正位移。
25.根据本发明的一个方面，其中所述根据匹配的点云中旋转角度可信的第三方向的点，计算矫正旋转角度的操作具体包括：将旋转角度可信的第三方向的最大聚类点云中每个点的角度变化量合并为矫正旋转角度。
26.根据本发明的一个方面，其中所述根据预测位姿和矫正位姿，确定当前时刻的位姿的步骤具体包括：根据所述里程计的测量精度确定所述预测位姿的权重，根据当前运行环境确定所述矫正位姿的权重，根据预测位姿、矫正位姿以及各自的权重，确定当前时刻的位姿。
27.本发明还提供一种机器人，包括：
28.激光雷达，用于获取点云；
29.里程计，用于获取所述机器人的位姿；和
30.处理器，用于执行上述的定位方法，以确定所述机器人在当前时刻的位姿。
31.本发明根据里程计的预测位姿和激光雷达的矫正位姿确定机器人的当前位姿，还可以为预测位姿和矫正位姿分配权重，以降低预测过程和矫正过程产生的误差，可以避免或降低误匹配的发生，增加了定位结果的可靠性和稳定性。
附图说明
32.构成本公开的一部分的附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。在附图中：
33.图1a示出了本发明一个实施例的机器人示意图；
34.图1b示出了图1a的内部模块图；
35.图2示出了本发明一个实施例的用于机器人的定位方法流程图；
36.图3a示出了本发明一个实施例的机器人定位示意图；
37.图3b示出了本发明一个实施例的矫正过程示意图；
38.图4示出了本发明一个实施例的icp匹配示意图；
39.图5示出了本发明一个实施例的获取矫正位姿的示意图。
具体实施方式
40.在下文中，仅简单地描述了某些示例性实施例。正如本领域技术人员可认识到的那样，在不脱离本发明的精神或范围的情况下，可通过各种不同方式修改所描述的实施例。因此，附图和描述被认为本质上是示例性的而非限制性的。
41.在本发明的描述中，需要理解的是，术语"中心"、"纵向"、"横向"、" 长度"、"宽度"、"厚度"、"上"、"下"、"前"、"后"、"左"、"右"、"竖直"、 "水平"、"顶"、"底"、"内"、"外"、"顺时针"、"逆时针"等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语"第一"、" 第二"仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有"第一"、"第二"的特征可以明示或者隐含地包括一个或者更多个所述特征。在本发明的描述中，"多个"的含义是两个或两个以上，除非另有明确具体的限定。
42.在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语" 安装"、"相连"、"连接"应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接:可以是机械连接，也可以是电连接或可以相互通讯；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
43.在本发明中，除非另有明确的规定和限定，第一特征在第二特征之"上" 或之"下"可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征"之上"、"上方"和"上面"包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征"之下"、"下方 "和"下面"包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度小于第二特征。
44.下文的公开提供了许多不同的实施方式或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。当然，它们仅仅为示例，并且目的不在于限制本发明。此外，本发明可以在不同例子中重复参考数字和/或参考字母，这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施方式和/或设置之间的关系。此外，本发明提供了的各种特定的工艺和材料的例子，但是本领域普通技术人员可以
意识到其他工艺的应用和/或其他材料的使用。
45.本发明提供一种用于机器人的定位方法，所述机器人包括激光雷达和里程计，所述定位方法包括：通过所述里程计获取所述机器人的位姿；根据上一时刻的位姿，预测当前时刻的位姿；通过所述激光雷达获取当前时刻的实测的点云；将所述实测的点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度；根据匹配的点云、所述位移置信度和旋转置信度，获取矫正位姿；根据所述预测位姿和矫正位姿，确定当前时刻的位姿。本发明根据里程计的预测位姿和激光雷达的矫正位姿确定机器人的当前位姿，还可以为预测位姿和矫正位姿分配权重，以降低预测过程和矫正过程产生的误差，可以避免或降低误匹配的发生，增加了定位结果的可靠性和稳定性。
46.以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
47.图1a示出了本发明一个实施例的机器人示意图，图1b示出了图1a的内部模块图，结合图1a和图1b，机器人包括：承载物品的壳体20、移动底盘 10。移动底盘10设置至少两组驱动轮121，每组驱动轮121分别位于移动底盘10的一侧。机器人还包括提供用户操作的功能控制器、地图生成和路径规划的底层控制器、以及控制移动单元和环境检测单元的元件控制器，其中元件控制器用于控制驱动轮121的行进速度。至少一组驱动轮121用作左驱动轮，同时，至少一组驱动轮121用作右驱动轮，左驱动轮和右驱动轮位于底盘10的相对侧。可选的，机器人还可以包括至少两组从动轮，一组驱动轮对应一组从动轮，其中，至少一组从动轮用作左从动轮，同时，至少一组从动轮轮用作右从动轮，左从动轮和右从动轮用于协助左驱动轮和右驱动轮带动机器人的壳体20和移动底盘10运动，以减轻驱动轮120的负载压力。
48.继续参考图1a-1b，机器人还包括：激光雷达110、里程计120、存储器 130以及处理器140。
49.激光雷达110可以设置于机器人壳体开缝处，从而容易发出激光信号以对周围物体进行探测，在一个具体的实施例中，激光雷达包括光电接收阵列和激光发射单元阵列，从而在激光雷达110沿设定平面进行旋转时，光电接收阵列能够形成扫描柱面，从而增大了扫描面积，便于获取物体形态的细节，避免机器人设备磕碰物体的情况。在另一个具体的实施例中，激光雷达110 仅包含单个光电接收单元和单个激光发射单元，则激光雷达110沿设定平面旋转后,能测量一个圆周的物体形态，从而降低成本。可选的，上述设定平面可以为水平面，便于机器人在行进过程中进行物体探测，此外，还可以根据用户需求选取其他设定平面，例如垂直平面等，本发明对此不进行限定。
50.里程计120，例如为轮式里程计，利用至少两组驱动轮121的转速值获取所述机器人的线速度和角速度，代入移动机器人运动学模型中，预测机器人当前时刻的位姿，亦即位置与航向角信息。里程计120也可以采用其它里程计，或者配合imu(惯性测量单元)模块等，本发明不对里程计120的类型进行限定。
51.存储器130，用于当前运行环境的点云地图。在一些具体的实施例中，存储器130包括：便携式计算机盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、静态随机存取存储器 (sram)、便携式压缩盘只读存储器(cd-rom)、数字多功能盘(dvd)、记忆棒、软盘、机械编码设备、例如其上存储有指令的打孔卡或凹槽内凸
起结构、以及上述的任意合适的组合。
52.处理器140，用于执行定位方法，以确定机器人在当前时刻的位姿。图2示出了本发明一个实施例的用于机器人的定位方法流程图，所述定位方法包括步骤s11-s16，具体如下：
53.在步骤s11通过所述里程计120获取所述机器人的位姿。
54.里程计120，例如为轮式里程计，利用至少两组驱动轮121的转速值获取所述机器人的线速度和角速度，代入移动机器人运动学模型中，推演出机器人的位姿。
55.在步骤s12根据上一时刻的位姿，预测当前时刻的位姿。
56.里程计120作为机器人相对定位的有效传感器，其模型决定于机器人的结构和运动方式，即机器人运动学模型。以双轮差动机器人为例，里程计120 根据安装在左右两组驱动轮电机上的光电编码器来检测车轮在一定时间内转过的弧度，进而推算机器人相对位姿的变化。本发明不对里程计的类型和预测方式进行限定，只要已知上一时刻的机器人的位姿，通过里程计单元可以预测出当前时刻的机器人的位姿，就都在本发明的保护范围内。
57.在步骤s13通过所述激光雷达110获取当前时刻实测的点云。
58.图3a示出了本发明一个实施例的机器人定位示意图，基于激光雷达和里程计的机器人定位过程包括已知上一时刻的位姿，通过里程计120预测下一时刻的位姿，对应于本实施例的步骤s12，还包括通过激光雷达获取的点云信息与地图匹配，对预测位姿进行矫正，亦即，在本实施例的步骤s13通过激光雷达110获取当前时刻实测的点云，在步骤s14和s15获得矫正位姿，在步骤s16完成对预测位姿的修正，获取当前时刻的位姿。以下进一步介绍。
59.在步骤s14将所述实测的点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度。
60.图3b示出了本发明一个实施例的矫正过程示意图，矫正之前，实测的点云与点云地图错开；矫正之后，实测的点云与点云地图匹配。匹配结果包括位移和旋转两个部分，因此，在匹配后，根据匹配结果获取位移置信度和旋转置信度，根据位移置信度和旋转置信度确定激光雷达110的矫正过程是否可信，并进一步确定可信的矫正位姿。
61.根据本发明的一个优选实施例，其中所述将实测的点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度的步骤具体包括：通过icp算法，将所述实测的点云与当前运行环境的点云地图进行匹配，根据匹配结果获取位移置信度和旋转置信度。
62.迭代最近点(iterative closest point,icp)算法是一种解决三维点云配准问题的算法，可以用于物体识别或位姿估计。通过一定的旋转和平移变换，将不同坐标系下的两组点云数据统一到同一参考坐标系下。icp匹配结果分为位移和旋转两个部分。根据icp原理，位移置信度与该位移方向上点云中点的法线角度有关。法线角度与位移方向偏差越小，说明该方向的位移修正可信度越高。旋转置信度与连续匹配点云中点的范围有关，连续匹配点云中点的范围越大，说明产生的旋转结果越可靠。
63.在一个具体的实施例中，通过icp算法将实测的点云与当前运行环境的点云地图进行匹配的操作如下：
64.通过激光雷达获取的实测点云为待匹配的目标点云p，当前运行环境的点云地图为源点云q，按照一定的约束条件，找到最邻近点(pi，qi)，然后计算出最优匹配参数r和t，使得误差函数最小。误差函数e(r，t)为：
[0065][0066]
其中，n为最邻近点对的个数，pi为目标点云p中的一点，qi为源点云q中与pi对应的最邻近点，r为旋转矩阵，t为平移向量。
[0067]
通过icp算法根据目标点云和源点云进行匹配：
[0068]
(1)在目标点云p中取点集pi∈p；
[0069]
(2)在源点云q中找到对应点集qi∈q，使得||qi-pi||＝min；
[0070]
(3)计算旋转矩阵r和平移向量t，使得误差函数e(r，t)最小；
[0071]
(4)对pi使用旋转矩阵r和平移向量t进行旋转和平移变换，得到新的对应点集pi’＝{pi’＝rpi+t,pi∈p}；
[0072]
(5)通过如下公式计算pi’与对应点集qi的平均距离：
[0073][0074]
(6)如果平均距离d小于预设的阈值或者大于预设的最大迭代次数，则停止迭代计算；否则，返回第(2)步，直到满足收敛条件为止，从而完成对第一点云与标定室的点云地图的匹配。
[0075]
以上仅为示例性的介绍，不构成对通过icp算法进行匹配的具体方式的限定。
[0076]
通过icp算法将实测的点云与当前运行环境的点云地图进行匹配后，如图4所示，提取匹配的点云，对匹配的点云进行处理，以获取位移置信度和旋转置信度。
[0077]
根据本发明的一个优选实施例，其中所述将实测的点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度的步骤具体包括：根据匹配的点云中每个点的法线方向，确定第一方向和第二方向，根据第一方向的预设范围内的总点数以及第二方向的预设范围内的总点数，确定位移置信度。
[0078]
根据icp原理，位移置信度与该位移方向上点云中点的法线角度有关。法线角度与位移方向偏差越小，说明该方向的位移修正可信度越高。对匹配的点云中的每个点计算法向量，根据每个点的法向量的方向，确定第一方向和第二方向，分别求出两个方向上的总点数，根据总点数确定哪个方向的位移更可信，或者两个方向都可信。例如，总点数越多的方向，该方向的位移越可信。
[0079]
根据本发明的一个优选实施例，其中所述第一方向为所述匹配的点云中每个点的法线方向的平均方向，所述第二方向与所述第一方向垂直。
[0080]
对匹配的点云中的每个点计算法向量，在一个具体的实施例中，计算过程包括：对于匹配的点云中的每个点，采用kd-tree获取该点周围8个相邻点，作为法线计算点；对8个相邻点进行主成分分析，具体包括：
[0081]
1)计算这些点的方差矩阵cov_matrix；
[0082]
2)求解方差矩阵con_matrix的特征值和特征向量；
[0083]
3)最大特征值对应的特征向量即为该点的法线方向。
[0084]
根据上述实施例获取每个点的法线方向，将全部点的法线方向的平均方向作为第
一方向，与该方向垂直的方向作为第二方向。确定第一方向和第二方向后，再根据第一方向的预设范围内的总点数以及第二方向的预设范围内的总点数，确定位移置信度。
[0085]
根据本发明的一个优选实施例，其中根据第一方向的预设范围内的总点数以及第二方向的预设范围内的总点数确定位移置信度的操作包括：当第一方向的预设范围内的总点数大于阈值时，该方向的位移可信；当第二方向的预设范围内的总点数大于阈值时，该方向的位移可信。
[0086]
在一个具体的实施例中，预设范围为45
°
，阈值为n，当匹配的点云中法线方向与第一方向的锐角夹角小于45
°
的点的总数量大于n时，第一方向的位移可信；当匹配的点云中法线方向与第二方向的锐角夹角小于45
°
的点的总数量大于n时，第二方向的位移可信。
[0087]
在一个具体的实施例中，阈值n的确定过程如下：
[0088]
在测试环境中搭建不同类型的场景，例如长走廊、特殊材质房间、杂乱场所等，以保证获得在任何已知场景中通用的阈值n；
[0089]
将安装高精度里程计(例如精度《1/1000)和高精度imu的机器人置于该测试环境中，手动设定机器人的初始位置，以保证初始位置正确；
[0090]
控制机器人行走一定距离和角度，可以认为在行走的时间段里，里程计和imu预测位姿完全正确；
[0091]
以预测位姿+一定的位姿偏差进行icp算法匹配，查看匹配后的位姿是否与预测位姿一致，例如，匹配后的位姿与预测位姿的偏差值小于一定范围时，说明通过icp算法匹配的矫正结果正确；
[0092]
不断调整阈值n，重复进行icp算法匹配，以找到匹配后的位姿与预测位姿一致的位姿偏差，该位姿偏差对应的阈值n即为通用阈值，可以保证在任何已知场景中通过icp算法匹配的矫正结果均正确。
[0093]
当第一方向的预设范围内的总点数大于阈值n时，该方向的位移可信；当第二方向的预设范围内的总点数大于阈值n时，该方向的位移可信，从而确定位移置信度。根据icp原理，匹配结果分为位移和旋转两个部分，因此还需要确定旋转置信度。旋转置信度与连续匹配点云中点的范围有关，连续匹配点云中点的范围越大，说明产生的旋转结果越可靠。以下进一步介绍。
[0094]
根据本发明的一个优选实施例，其中所述将点云与地图进行匹配，根据匹配结果获取位移置信度和旋转置信度的步骤还包括：将所述匹配的点云中的点进行聚类，根据最大聚类点云确定第三方向，根据所述第三方向的点云分布，确定旋转置信度。
[0095]
根据本发明的一个优选实施例，其中所述第三方向为所述最大聚类点云中每个点的法线方向的平均方向。
[0096]
继续参考图4，对匹配的点云中的点进行聚类，计算最大聚类中每个点的法线方向，再取所有法线方向的平均方向作为第三方向，根据第三方向上的点云分布，确定旋转置信度。
[0097]
在一个具体的实施例中，计算最大聚类中每个点的法线方向的过程包括：对于最大聚类中的每个点，采用kd-tree获取该点周围8个相邻点，作为法线计算点；对8个相邻点进行主成分分析，具体包括：
[0098]
1)计算这些点的方差矩阵cov_matrix；
[0099]
2)求解方差矩阵con_matrix的特征值和特征向量；
[0100]
3)最大特征值对应的特征向量即为该点的法线方向。
[0101]
根据上述方法获取每个点的法线方向，将全部点的法线方向的平均方向作为第三方向。
[0102]
根据本发明的一个优选实施例，其中所述根据最大聚类点云确定第三方向，根据第三方向的点云分布，确定旋转置信度的操作具体包括：当所述第三方向的点云分布长度大于阈值时，对应的旋转角度可信。
[0103]
在一个具体的实施例中，点云分布长度为l，阈值为0.3m，当l大于0.3m 时，矫正的角度变化量视为正确，亦即，对应的旋转角度可信。
[0104]
在步骤s15根据匹配的点云、所述位移置信度和旋转置信度，获取矫正位姿。
[0105]
根据本发明的一个优选实施例，其中所述根据匹配的点云、位移置信度和旋转置信度，获取矫正位姿的步骤具体包括：
[0106]
根据所述匹配的点云中位移可信的第一方向和/或第二方向的点，计算矫正位移；
[0107]
根据所述匹配的点云中旋转角度可信的第三方向的点，计算矫正旋转角度；
[0108]
根据所述矫正位移和所述矫正旋转角度，合成矫正位姿。
[0109]
具体地，根据匹配的点云中每个点的法线方向，确定第一方向和第二方向，根据第一方向的预设范围内的总点数以及第二方向的预设范围内的总点数，确定位移置信度。如果第一方向的位移可信，则该方向的位移保留，反之，舍弃该方向的位移；如果第二方向的位移可信，则该方向的位移保留，反之，舍弃该方向的位移。最终把保留的位移合成矫正位移。将匹配的点云中的点进行聚类，根据最大聚类点云确定第三方向，根据第三方向的点云分布，确定旋转置信度。如果第三方向的点云分布长度大于阈值，对应的旋转角度可信，予以保留，反之，应当舍弃。最终把保留的旋转角度合成最终的矫正旋转角度。根据矫正位移和矫正旋转角度，合成矫正位姿。
[0110]
根据本发明的一个优选实施例，其中所述根据匹配的点云中位移可信的第一方向和/或第二方向的点，计算矫正位移的操作具体包括：将位移可信的第一方向和/或第二方向在预设范围内每个点在该方向的位移分量合并为矫正位移。
[0111]
例如，如果第一方向的位移可信，则该方向在预设范围内的点保留，反之，舍弃该方向的位移；如果第二方向的位移可信，则该方向在预设范围内的点保留，反之，舍弃该方向的位移。最终把两个方向保留的在预设范围内的每个点在该方向的位移分量合并为矫正位移。
[0112]
根据本发明的一个优选实施例，其中所述根据匹配的点云中旋转角度可信的第三方向的点，计算矫正旋转角度的操作具体包括：将旋转角度可信的第三方向的最大聚类点云中每个点的角度变化量合并为矫正旋转角度。
[0113]
例如，如果第三方向的点云分布长度大于阈值，对应的旋转角度可信，预设范围内的点予以保留，反之，预设范围内的点应当舍弃。最终把保留的每个点的角度变化量合并为矫正旋转角度。
[0114]
以上介绍了通过位移置信度和旋转置信度确定矫正位姿，以下通过一个具体的实施例进一步详述。
[0115]
图5示出了本发明一个实施例的获取矫正位姿的示意图，不规则边框表示当前运
行环境，实心黑点表示通过激光雷达110获取的点云，每个实心点旁边的箭头表示法线方向。矫正位姿c可以用(xc，yc，θc)表示，亦即，矫正位姿可以分解为x轴矫正因子，y轴矫正因子以及角度θ矫正因子。通过icp算法进行点云匹配并获取矫正位姿的具体方式就是获取这三个矫正因子。相应的，上一时刻的位姿a可以表示为(xa，ya，θa)，预测位姿b可以表示为(xb，yb，θb)。
[0116]
例如，在长走廊时，如图5左侧单边墙壁，实测的点云中的点的法线方向均对应第一方向，没有第二方向的分量，则第一方向的分量保留，并且由于长走廊的特殊地形导致点云分布长度大于阈值l，旋转角度不可信，因此，最后矫正位姿为(xc，0,0)。
[0117]
又例如，不在长走廊时，如图5左上侧单边墙壁，实测的点云中的点的法线方向既有第一方向的分量，又有第二方向的分量，且在两个方向上的总点数均大于阈值n，则两个方向都保留，进一步确定点云分布长度是否大于阈值m，如果大于阈值m，旋转角度不可信，则最后矫正位姿为(xc，yc，0)。基于激光雷达和里程计的机器人定位过程分为三步：第一、已知上一时刻的位姿，通过里程计120预测下一时刻的位姿，对应于本实施例的步骤s12；第二、通过激光信息与地图匹配，对预测的位姿进行修正，对应本实施例的步骤s15；第三、根据里程计120的预测位姿和激光雷达110的矫正位姿，确定当前时刻的位姿，对应本实施例的步骤s16。以下进一步介绍。
[0118]
在步骤s16根据所述预测位姿和矫正位姿，确定当前时刻的位姿。
[0119]
继续参考图3a，已知上一时刻的位姿，通过里程计120预测当前时刻的位姿，得到预测位姿。通过激光雷达110获取实测的点云，根据icp算法匹配获取矫正位姿，根据预测位姿和矫正位姿，确定当前时刻的位姿。
[0120]
根据本发明的一个优选实施例，其中所述根据预测位姿和矫正位姿，确定当前时刻的位姿的步骤具体包括：根据所述里程计的测量精度确定所述预测位姿的权重，根据当前运行环境确定所述矫正位姿的权重，根据预测位姿、矫正位姿以及各自的权重，确定当前时刻的位姿。
[0121]
在定位过程中，通过里程计120获取的预测位姿和通过激光雷达110获取的矫正位姿都可能包括误差，在确定最终结果时，是更相信里程计120预测的位姿还是更相信激光雷达的矫正位姿，可以根据两者的误差大小分配对应的权重，其中里程计120的权重与里程计的测量精度相关，测量精度越高，权重越大；激光雷达110的权重与当前运行环境相关，不同的运行环境，例如长走廊、特殊材质房间、杂乱场所等，其权重各不相同，可以搭建不同类型的场景，通过多次测量数据得到。
[0122]
综上所述，本发明根据里程计的预测位姿和激光雷达的矫正位姿确定机器人的当前位姿，还可以为预测位姿和矫正位姿分配权重，以降低预测过程和矫正过程产生的误差，可以避免或降低误匹配的发生，增加了定位结果的可靠性和稳定性。
[0123]
本说明书提供了如实施例或流程图所述的方法操作步骤，但基于常规或者无创造性的劳动可以包括更多或者更少的操作步骤。实施例中列举的步骤顺序仅仅为众多步骤执行顺序中的一种方式，不代表唯一的执行顺序。在实际中的系统或设备产品执行时，可以按照实施例或者流程图所示的方法顺序执行或者并行执行。
[0124]
本发明还涉及一种机器人，参考图1a-1b，所述机器人包括：
[0125]
激光雷达110，用于获取点云；
[0126]
里程计120，用于获取所述机器人的位姿；和
[0127]
处理器140，用于执行上述的定位方法，以确定所述机器人在当前时刻的位姿。
[0128]
本发明根据里程计的预测位姿和激光雷达的矫正位姿确定机器人的当前位姿，还可以为预测位姿和矫正位姿分配权重，以降低预测过程和矫正过程产生的误差，可以避免或降低误匹配的发生，增加了定位结果的可靠性和稳定性。
[0129]
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。