CN111283730A - 基于点线特征机器人初始位姿获取方法及开机自定位方法 - Google Patents

基于点线特征机器人初始位姿获取方法及开机自定位方法 Download PDF

Info

Publication number
CN111283730A
CN111283730A CN202010139727.3A CN202010139727A CN111283730A CN 111283730 A CN111283730 A CN 111283730A CN 202010139727 A CN202010139727 A CN 202010139727A CN 111283730 A CN111283730 A CN 111283730A
Authority
CN
China
Prior art keywords
pose
feature set
robot
global
features
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010139727.3A
Other languages
English (en)
Other versions
CN111283730B (zh
Inventor
李良源
赖志林
李睿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Saite Intelligent Technology Co Ltd
Original Assignee
Guangzhou Saite Intelligent Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangzhou Saite Intelligent Technology Co Ltd filed Critical Guangzhou Saite Intelligent Technology Co Ltd
Priority to CN202010139727.3A priority Critical patent/CN111283730B/zh
Publication of CN111283730A publication Critical patent/CN111283730A/zh
Application granted granted Critical
Publication of CN111283730B publication Critical patent/CN111283730B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators

Abstract

本发明公开了一种基于点线特征机器人初始位姿获取方法及开机自定位方法、存储介质。本发明的基于点线特征机器人初始位姿获取方法包括：离线提取机器人运行环境的全局特征集F1；在机器人启动时，通过二维激光雷达提取局部特征集F2；利用特征间的几何相似关系，初步匹配全局和局部特征集，并计算得到机器人的可能位姿集合{P}；过滤误匹配结果，得到可能位姿子集；评分每一个可能位姿，选取分数最优的位姿作为机器人的初始位姿P。本发明通过提取全局特征集和局部特征集，采用特征几何相似关系及位姿评分模型，获取机器人的初始位姿，无需移动机器人便可实现开机定位，提高了安全保障和定位的精确性、鲁棒性。

Description

基于点线特征机器人初始位姿获取方法及开机自定位方法
技术领域
本发明涉及机器人自主定位技术领域，具体涉及一种基于点线特征机器人初始位姿获取方法及开机自定位方法,以及存储介质。
背景技术
定位(Localization)是指移动机器人必须确定自身在该工作环境中的精确位置，精确的环境地图及移动机器人定位有助于高效地路径规划和决策，是保证移动机器人安全导航的基础。定位作为移动机器人的一种基础能力，对于机器人成功完成各项任务具有决定性的作用。定位通常可以分为局部定位、全局定位，其中全局定位的一种实例场景便是机器人的开机自定位。
自适应蒙特卡罗定位(Adaptive Monte Carlo Localization，AMCL)算法作为一种当前流行的粒子滤波类定位方法，虽然在理论上能够实现开机自定位，但由于该方法是通过全局采样粒子以实现全局定位，这就需要机器人移动一段时间后才能收敛粒子集以完成定位。在实际场景应用中，让没有准确定位结果的机器人移动是有潜在危险的。另一方面，在机器人的运行场景较大时，AMCL算法进行全局粒子采样所需的空间及时间代价也是巨大的。
发明内容
鉴于以上技术问题，本发明的目的在于提供一种基于点线特征机器人初始位姿获取方法及开机自定位方法,以及存储介质，解决现有技术不能实现开机自定位或粒子滤波类定位方法在实现开机自定位时没有准确定位结果及采样代价大的问题。
本发明采用技术方案如下：
基于点线特征机器人初始位姿获取方法，包括：
获取移动机器人运行环境的离线全局地图，基于全局地图坐标系map_frame,提取全局特征集F1；
在移动机器人启动时，基于移动机器人本体坐标系base_frame，通过二维激光雷达提取局部特征集F2；
利用特征间的几何相似关系，初步匹配全局特征集F1和局部特征集F2；
根据初步匹配的全局特征集F1和局部特征集F2，得到移动机器人的可能位姿集合{P}；
通过映射局部特征到全局特征方式，过滤可能位姿集合{P}中的误匹配结果，得到可能位姿子集；
对可能位姿子集中每一个位姿进行评分，选取分数最优的位姿作为机器人的初始位姿P。
进一步的，所述全局特征集F1包括角点特征集C1和直线特征集L1；局部特征集F2包括角点特征集C2和直线特征集L2。
进一步的，所述直线特征集L1中任一直线特征参数包括：直线编号idl、直线的法线方向
Figure BDA0002398639020000031
坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点X轴坐标xs、起始端点Y轴坐标ys、结束端点X轴坐标xe和结束端点Y轴坐标ye；直线特征集L2中任一直线特征参数包括直线编号
Figure BDA0002398639020000032
直线的法线方向
Figure BDA0002398639020000033
坐标系原点与直线的距离(极径)
Figure BDA0002398639020000034
直线长度
Figure BDA0002398639020000035
起始端点X轴坐标
Figure BDA0002398639020000036
起始端点Y轴坐标
Figure BDA0002398639020000037
结束端点X轴坐标
Figure BDA0002398639020000038
结束端点Y轴坐标
Figure BDA0002398639020000039
进一步的，所述角点特征集C2中任一角点特征参数包括角点编号
Figure BDA00023986390200000310
角点X轴坐标
Figure BDA00023986390200000311
和角点Y轴坐标
Figure BDA00023986390200000312
所述角点特征集C1中任一角点特征参数包括角点编号idc、角点X轴坐标xc和角点Y轴坐标yc。
进一步的，利用特征间的几何相似关系，初步匹配全局和局部特征集的步骤包括：
提取局部特征集F2中的几何关系；所述几何关系包括直线夹角，和/或角点间距，和/或点线距离；
在全局特征集F1中获取与局部特征集F2几何关系相似的参考全局特征集F3，所述参考全局特征集F3包括角点特征集C3和直线特征集L3；所述参考全局特征集F3是所述全局特征集F1的子集。
进一步的，根据初步匹配的全局和局部特征集，得到移动机器人的可能位姿集合{P}的步骤包括：
所述可能位姿集合{P}为移动机器人可能位姿的数据集合，所述移动机器人可能位姿数据包括位置(x,y)和姿态θ；
通过全局和局部特征集的初步匹配得到一个特征对集合，根据特征对集合解算出可能位姿集合{P}；其中，通过一对直线特征解算机器人的姿态θ，通过两对直线特征或者一对角点特征解算机器人的位置(x,y)；姿态θ表示移动机器人本体坐标系base_frame的X轴相对于全局地图坐标系map_frame X轴的旋转角度；位置(x,y)表示移动机器人本体坐标系base_frame原点在全局地图坐标系map_frame中的X轴及Y轴坐标。
进一步的，通过映射局部特征到全局特征方式，过滤可能位姿集合{P}中的误匹配结果，得到可能位姿子集的步骤包括：
基于所述可能位姿集合{P}中任一位姿P1，将所述局部特征集F2中所有特征映射至全局地图坐标系下，获得预测全局特征集F4；所述预测全局特征集F4是所述全局特征集F1的子集；
根据所述预测全局特征集F4，以及参考全局特征集F3，计算每对特征间的误差；累加所述特征集中每对特征间的所述误差，得到所述位姿P1的误差，过滤所述可能位姿集合{P}中超过设定误差的位姿，得到可能位姿子集。
进一步的，对可能位姿子集中每一个位姿进行评分，选取分数最优的位姿作为机器人的初始位姿P的步骤包括：基于所述全局地图，根据所述可能位姿子集中任一位姿P2，获取与二维激光雷达的实时测量数据相对应的参考测量数据；根据所述实时测量数据与所述参考测量数据的误差，计算所述位姿P2的评分；根据评分结果，选取可能位姿子集中评分最高的位姿P作为机器人的初始位姿。
基于点线特征机器人开机自定位方法，采用所述的基于点线特征机器人初始位姿获取方法获取移动机器人的初始位姿P，还包括：根据移动机器人的初始位姿P，采用粒子滤波算法对移动机器人进行定位。
一种计算机存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现所述的基于点线特征机器人初始位姿获取方法或所述的基于点线特征移动机器人开机自定位方法。
相比现有技术，本发明的有益效果在于：
本发明基于点线特征机器人开机自定位方法提供了一种基于激光雷达和点线特征的机器人定位方法，通过提取全局特征集和局部特征集，采用特征几何相似关系，获取机器人的初始位姿，该方法无需移动机器人便可实现开机定位，提高了安全保障，避免了机器人启动时的人工干预，同时提高了定位的精确性、鲁棒性、实时性。
附图说明
图1为本发明基于点线特征机器人初始位姿获取方法实施例的流程示意图；
图2为本发明基于点线特征机器人开机自定位方法实施例的流程示意图；
图3为本发明基于点线特征机器人开机自定位方法实施例中相似几何关系的示意图。
具体实施方式
下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。
实施例：
请参考图1-3所示，如图1所示，基于点线特征机器人初始位姿获取方法，包括以下步骤：
步骤S1:获取机器人运行环境的离线全局地图；基于全局地图坐标系map_frame,提取全局特征集F1。
在该步骤中，获取的离线全局地图可以通过获取其他存储介质存储的离线全局地图，也可通过环境建模(Mapping)建立移动机器人所处工作环境的准确的空间位置描述的空间模型或地图。也可通过机器人本身二维激光雷达实现对运行环境的感知，以构建全局地图。
地图可包括栅格地图和矢量地图，作为具体实施例，本发明的离线全局地图可采用栅格地图M。
具体的，所述全局特征集F1包括角点特征集C1和直线特征集L1；所述直线特征集L1中任一直线特征参数包括：直线编号idl、直线的法线方向
Figure BDA0002398639020000061
坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点X轴坐标xs、起始端点Y轴坐标ys、结束端点X轴坐标xe和结束端点Y轴坐标ye；所述角点特征集C1中任一角点特征参数包括角点编号idc、角点X轴坐标xc和角点Y轴坐标yc。
步骤S2:在移动机器人启动时，基于移动机器人本体坐标系base_frame，通过二维激光雷达提取局部特征集F2。
具体的，所述局部特征集F2包括角点特征集C2和直线特征集L2；所述直线特征集L2中任一直线特征参数包括直线编号
Figure BDA0002398639020000071
直线的法线方向
Figure BDA0002398639020000072
坐标系原点与直线的距离(极径)
Figure BDA0002398639020000073
直线长度
Figure BDA0002398639020000074
起始端点X轴坐标
Figure BDA0002398639020000075
起始端点Y轴坐标
Figure BDA0002398639020000076
结束端点X轴坐标
Figure BDA0002398639020000077
结束端点Y轴坐标
Figure BDA0002398639020000078
角点特征集C2中任一角点特征参数包括角点编号
Figure BDA0002398639020000079
角点X轴坐标
Figure BDA00023986390200000710
和角点Y轴坐标
Figure BDA00023986390200000711
在该步骤中，移动机器人获取移动机器人的点云数据，获取点云数据采用的传感器可以是SICK的单线激光雷达或速腾聚创的16线激光雷达，在点云数据中，提取局部角点及直线特征集F2。
提取角点及直线特征集是指在移动机器人的精度范围内，基于离线全局地图或基于移动机器人本体坐标系base_frame，提取环境空间中的所有直线的直线特征以及所有角点的角点特征。
步骤S3:利用特征的几何相似关系，初步匹配全局和局部特征集。
具体的，提取局部特征集F2中的几何关系；所述几何关系包括直线夹角，和/或角点间距，和/或点线距离。
在全局特征集F1中获取与局部特征集F2几何关系相似的参考全局特征集F3，所述参考全局特征集F3包括角点特征集C3和直线特征集L3；所述参考全局特征集F3是所述全局特征集F1的子集。
在特征匹配技术中，大多是从二维图像中基于像素梯度等进行特征提取，并利用优化技术进行特征匹配；本申请的特征匹配技术，是从二维激光雷达测量数据提取角点及直线特征，并利用特征间的几何相似关系，初步匹配全局和局部特征集；本申请所述初步匹配过程并不会得到严格的匹配结果，而是有一个全局和局部特征的初步匹配关系，即有正确的匹配关系，也可能包含有错误的匹配关系。
步骤S4:根据初步匹配的全局和局部特征集，得到移动机器人的可能位姿集合{P}。
具体的，所述可能位姿集合{P}中每一位姿的计算过程为：通过一对直线特征解算机器人的姿态θ，通过两对直线特征或者一对角点特征解算机器人的位置(x,y)，机器人的位姿包括位置(x,y)和姿态θ；通过全局和局部特征集的初步匹配得到一个特征对集合，不同的特征对可能解算出不同的位姿，根据特征对集合可以解算出可能位姿集合{P}。
作为实施例：位置(x,y)和姿态θ可通过如下方式解算：
所述姿态θ通过公式
Figure BDA0002398639020000081
计算；
所述位置(x,y)通过公式
Figure BDA0002398639020000082
计算；
其中，姿态θ表示移动机器人本体坐标系base_frame的X轴相对于全局地图坐标系map_frame X轴的旋转角度，
Figure BDA0002398639020000083
为直线特征集L1中直线的法线方向，
Figure BDA0002398639020000084
为直线特征集L2中直线的法线方向；位置(x,y)表示移动机器人本体坐标系base_frame原点在全局地图坐标系map_frame中的X轴及Y轴坐标，xc为角点特征集C1中角点X轴坐标；
Figure BDA0002398639020000085
为特征集C2中角点X轴坐标。
步骤S5:通过映射局部特征到全局特征，过滤可能位姿集合中的误匹配结果，得到可能位姿子集。
具体的，基于所述可能位姿集合{P}中任一位姿P1，将所述局部特征集F2中所有特征映射至全局地图坐标系下，获得预测全局特征集F4；所述预测全局特征集F4是所述全局特征集F1的子集。
根据所述预测全局特征集F4，以及参考全局特征集F3，计算每对特征间的误差；累加所述特征集中每对特征间的所述误差，得到所述位姿P1的误差；过滤所述可能位姿集合中具有较大误差的位姿，较大误差可自行设定，即过滤初步匹配步骤由于误匹配得到的位姿，得到可能位姿子集。
步骤S6:评分可能位姿子集中每一个位姿，选取分数最优的位姿作为机器人的初始位姿P。
具体的，基于所述全局地图，根据所述可能位姿子集中任一位姿P2，获取与所述二维激光雷达的实时测量数据相对应的参考测量数据；根据所述实时测量数据与所述参考测量数据的误差，计算所述位姿P2的评分；根据评分结果，选取可能位姿子集中评分最高的位姿P作为机器人的初始位姿。
具体的，所述实时测量数据是一个距离集合，集合中每一个元素表示：基于当前真实的机器人位姿，在机器人某一方向上，距离机器人最近的环境物体与机器人的距离。所述实时测量数据通过激光雷达扫描得到。所述参考测量数据是一个距离集合，集合中每一个元素表示：基于当前假设的机器人位姿P2，在机器人某一方向上，距离机器人最近的地图中物体与机器人的距离。所述参考测量数据通过在所述全局地图中查询得到。
所述评分可以通过公式
Figure BDA0002398639020000101
计算。
其中scorei、zi、
Figure BDA0002398639020000102
分别为在位姿P2下第i束激光的评分、实时测量数据、参考测量数据；λ、σ分别为归一化系数、标准差系数；通常σ可取值为0.1，n为自然数。
本发明的基于点线特征机器人开机自定位方法具体实施例如下：
在本实施例中，以室内配送机器人为例，粒子滤波算法以AMCL算法实现,请参照图2所示。
步骤S10：用户操控机器人移动，或者机器人自主探索移动，并利用二维激光雷达实现对运行环境的感知，以构建栅格地图M，构建完成的地图存储以备后续步骤使用。
步骤S20：利用步骤S10中的栅格地图M，通过特征提取算法，提取机器人运行环境中的全局特征集F1，并存储全局特征参数以备后续步骤使用。
其中全局特征集F1包括角点特征集C1和直线特征集L1；其中角点特征集C1中任一角点特征的参数(idc,xc,yc)包括：角点编号idc、角点X轴坐标xc、角点Y轴坐标yc；所述直线特征集L1中任一直线特征的参数
Figure BDA0002398639020000103
包括：直线编号idl、直线的法线方向
Figure BDA0002398639020000111
坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点X轴坐标xs、起始端点Y轴坐标ys、结束端点X轴坐标xe、结束端点Y轴坐标ye。
其中全局特征集F1基于步骤S10中栅格地图M的全局地图坐标系map_frame。
步骤S30：机器人启动后，通过二维激光雷达实时感知机器人周围环境，利用特征提取算法提取当前环境的局部特征集F2。
具体的，通过二维激光雷达获取机器人周围环境的距离信息，并从中提取局部特征集F2，具体的，从所述距离信息经过离群点过滤、搜索断点、分割点集、拟合直线等步骤，可以提取当前环境的直线特征、角点特征；直线特征描述了环境中具有明显特征的直线段，角点特征描述了环境中具有明显特征的角点；将这些直线特征、角点特征进行组合，即可得到直线特征集、角点特征集；直线特征集与角点特征集共同组成了所述局部特征集F2。
其中局部特征集F2包括角点特征集C2和直线特征集L2；其中角点特征集C2中任一角点特征的参数
Figure BDA0002398639020000112
包括：角点编号
Figure BDA0002398639020000113
角点X轴坐标
Figure BDA0002398639020000114
角点Y轴坐标
Figure BDA0002398639020000115
所述直线特征集L2中任一直线特征的参数
Figure BDA0002398639020000116
包括：直线编号
Figure BDA0002398639020000117
直线的法线方向
Figure BDA0002398639020000118
坐标系原点与直线的距离(极径)
Figure BDA0002398639020000119
直线长度
Figure BDA00023986390200001110
起始端点X轴坐标
Figure BDA0002398639020000121
起始端点Y轴坐标
Figure BDA0002398639020000122
结束端点X轴坐标
Figure BDA0002398639020000123
结束端点Y轴坐标
Figure BDA0002398639020000124
其中局部特征集F2基于机器人本体坐标系base_frame。
在本步骤中，角点特征集C1和直线特征集L1包含的元素与角点特征集C2和直线特征集L2包含的元素一一对应。
步骤S40:利用特征的几何相似关系，初步匹配全局和局部特征集。
具体的，提取局部特征集F2中的几何关系；所述几何关系包括直线夹角、角点间距、点线距离等。
在全局特征集F1中获取与局部特征集F2几何关系相似的参考全局特征集F3，所述参考全局特征集F3包括角点特征集C3和直线特征集L3；所述参考全局特征集F3是所述全局特征集F1的子集。
相似的几何关系示例请参照图3所示，全局特征和局部特征具有相同的直线夹角β和角点间距d，判定为具有相似的几何关系。
在特征匹配技术中，大多是从二维图像中基于像素梯度等进行特征提取，并利用优化技术进行特征匹配；本申请的特征匹配技术，是从二维激光雷达测量数据提取角点及直线特征，并利用特征间的几何相似关系，初步匹配全局和局部特征集；本申请所述初步匹配过程并不会得到严格的匹配结果，而是有一个全局和局部特征的初步匹配关系，即有正确的匹配关系，也可能包含有错误的匹配关系。
步骤S50:根据初步匹配的全局和局部特征集，得到移动机器人的可能位姿集合{P}。
具体的，所述可能位姿集合{P}中每一位姿的计算过程为：通过一对直线特征解算机器人的姿态θ，通过两对直线特征或者一对角点特征解算机器人的位置(x,y)，机器人的位姿包括位置(x,y)和姿态θ；通过全局和局部特征集的初步匹配得到一个特征对集合，不同的特征对可能解算出不同的位姿，根据特征对集合可以解算出可能位姿集合{P}。
所述姿态θ通过公式
Figure BDA0002398639020000131
计算。
所述位置(x,y)通过公式
Figure BDA0002398639020000132
计算。
其中，姿态θ表示移动机器人本体坐标系base_frame的X轴相对于全局地图坐标系map_frame X轴的旋转角度，
Figure BDA0002398639020000133
为直线特征集L1中直线的法线方向，
Figure BDA0002398639020000134
为直线特征集L2中直线的法线方向；位置(x,y)表示移动机器人本体坐标系base_frame原点在全局地图坐标系map_frame中的X轴及Y轴坐标，xc为角点特征集C1中角点X轴坐标；
Figure BDA0002398639020000135
为特征集C2中角点X轴坐标。
步骤S60:通过映射局部特征到全局特征，过滤可能位姿集合中的误匹配结果，得到可能位姿子集。
具体的，基于所述可能位姿集合{P}中任一位姿P1，将所述局部特征集F2中所有特征映射至全局地图坐标系下，获得预测全局特征集F4；所述预测全局特征集F4是所述全局特征集F1的子集。
根据所述预测全局特征集F4，以及参考全局特征集F3，计算每对特征间的误差；累加所述特征集中每对特征间的所述误差，得到所述位姿P1的误差；过滤所述可能位姿集合中具有较大误差的位姿，即过滤初步匹配步骤由于误匹配得到的位姿，得到可能位姿子集。
步骤S70:评分可能位姿子集中每一个位姿，选取分数最优的位姿作为机器人的初始位姿P。
具体的，基于所述全局地图，根据所述可能位姿子集中任一位姿P2，获取与所述二维激光雷达的实时测量数据相对应的参考测量数据；根据所述实时测量数据与所述参考测量数据的误差，计算所述位姿P2的评分；根据评分结果，选取可能位姿子集中评分最高的位姿P作为机器人的初始位姿。
所述评分可以通过公式
Figure BDA0002398639020000141
计算；
其中scorei、zi、
Figure BDA0002398639020000142
分别为在位姿P2下第i束激光的评分、实时测量数据、参考测量数据；λ、σ分别为归一化系数、标准差系数；通常σ可取值为0.1。
步骤S80：根据移动机器人的初始位姿P，采用粒子滤波算法对移动机器人进行定位。具体的，在初始位姿P附近进行粒子采样，并通过粒子滤波算法实现后续的局部定位过程。
根据初始位姿采用粒子滤波的方法进行定位的方法在现有技术中有多种，例如标准蒙特卡罗定位MCL算法、增广蒙特卡罗定位Aug-MCL算法、自适应蒙特卡罗定位AMCL算法等。
作为实施例，根据移动机器人的初始位姿P，采用粒子滤波算法的自适应蒙特卡罗定位AMCL算法采用进行定位，具体的步骤如下：
给定初始位姿P之后，在位姿P附近以先验分布p(x0)采样初始粒子集S0，该粒子集中所有粒子具有相同的权重，计算粒子集的初始置信度bel(x0)。AMCL算法的实现主要分为以下三步：
一、预测：基于t-1时刻的后验粒子集St-1和t时刻的控制信息ut，根据运动模型计算得到每个粒子t时刻的预测状态
Figure BDA0002398639020000151
生成预测粒子集
Figure BDA0002398639020000152
二、测量更新：基于t时刻的感知信息zt，根据测量模型更新
Figure BDA0002398639020000153
中每一个粒子的权重Wt，并归一化粒子权重；
三、重采样：以正比于粒子权重Wt的概率，从
Figure BDA0002398639020000154
中重采样M个粒子并加入后验粒子集St，并归一化粒子权重。
AMCL算法通过循环执行上面三个步骤，迭代计算机器人的后验位姿，以实现机器人的定位。
本发明基于点线特征机器人开机自定位方法提供了一种基于激光雷达和点线特征的机器人定位方法，通过提取全局特征集和局部特征集，采用特征几何相似关系及位姿评分模型，获取机器人的初始位姿，该方法无需移动机器人便可实现开机定位，提高了安全保障，避免了机器人启动时的人工干预，同时提高了定位的精确性、鲁棒性、实时性。
本发明还提供一种计算机存储介质，其上存储有计算机程序，本发明的方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在该计算机存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机存储介质不包括电载波信号和电信信号。
对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。

Claims (10)

1.基于点线特征机器人初始位姿获取方法，其特征在于，包括：
获取移动机器人运行环境的离线全局地图，基于全局地图坐标系map_frame,提取全局特征集F1；
在移动机器人启动时，基于移动机器人本体坐标系base_frame，通过二维激光雷达提取局部特征集F2；
利用特征间的几何相似关系，初步匹配全局特征集F1和局部特征集F2；
根据初步匹配的全局特征集F1和局部特征集F2，得到移动机器人的可能位姿集合{P}；
通过映射局部特征到全局特征方式，过滤可能位姿集合{P}中的误匹配结果，得到可能位姿子集；
对可能位姿子集中每一个位姿进行评分，选取分数最优的位姿作为机器人的初始位姿P。
2.根据权利要求1所述的基于点线特征机器人初始位姿获取方法，其特征在于，所述全局特征集F1包括角点特征集C1和直线特征集L1；局部特征集F2包括角点特征集C2和直线特征集L2。
3.根据权利要求2所述的基于点线特征机器人初始位姿获取方法，其特征在于，所述直线特征集L1中任一直线特征参数包括：直线编号idl、直线的法线方向
Figure FDA0002398639010000011
坐标系原点与直线的距离(极径)ρ、直线长度l、起始端点X轴坐标xs、起始端点Y轴坐标ys、结束端点X轴坐标xe和结束端点Y轴坐标ye；直线特征集L2中任一直线特征参数包括直线编号
Figure FDA0002398639010000021
直线的法线方向
Figure FDA0002398639010000022
坐标系原点与直线的距离(极径)
Figure FDA0002398639010000023
直线长度
Figure FDA0002398639010000024
起始端点X轴坐标
Figure FDA0002398639010000025
起始端点Y轴坐标
Figure FDA0002398639010000026
结束端点X轴坐标
Figure FDA0002398639010000027
结束端点Y轴坐标
Figure FDA0002398639010000028
4.根据权利要求3所述的基于点线特征机器人初始位姿获取方法，其特征在于，所述角点特征集C2中任一角点特征参数包括角点编号
Figure FDA0002398639010000029
角点X轴坐标
Figure FDA00023986390100000210
和角点Y轴坐标
Figure FDA00023986390100000211
所述角点特征集C1中任一角点特征参数包括角点编号idc、角点X轴坐标xc和角点Y轴坐标yc。
5.根据权利要求4所述的基于点线特征机器人初始位姿获取方法，其特征在于，利用特征间的几何相似关系，初步匹配全局和局部特征集的步骤包括：
提取局部特征集F2中的几何关系；所述几何关系包括直线夹角，和/或角点间距，和/或点线距离；
在全局特征集F1中获取与局部特征集F2几何关系相似的参考全局特征集F3，所述参考全局特征集F3包括角点特征集C3和直线特征集L3；所述参考全局特征集F3是所述全局特征集F1的子集。
6.根据权利要求5所述的基于点线特征机器人初始位姿获取方法，其特征在于，根据初步匹配的全局和局部特征集，得到移动机器人的可能位姿集合{P}的步骤包括：
所述可能位姿集合{P}为移动机器人可能位姿的数据集合，所述移动机器人可能位姿数据包括位置(x,y)和姿态θ；
通过全局和局部特征集的初步匹配得到一个特征对集合，根据特征对集合解算出可能位姿集合{P}；其中，通过一对直线特征解算机器人的姿态θ，通过两对直线特征或者一对角点特征解算机器人的位置(x,y)；姿态θ表示移动机器人本体坐标系base_frame的X轴相对于全局地图坐标系map_frame X轴的旋转角度；位置(x,y)表示移动机器人本体坐标系base_frame原点在全局地图坐标系map_frame中的X轴及Y轴坐标。
7.根据权利要求5所述的基于点线特征机器人初始位姿获取方法，其特征在于，通过映射局部特征到全局特征方式，过滤可能位姿集合{P}中的误匹配结果，得到可能位姿子集的步骤包括：
基于所述可能位姿集合{P}中任一位姿P1，将所述局部特征集F2中所有特征映射至全局地图坐标系下，获得预测全局特征集F4；所述预测全局特征集F4是所述全局特征集F1的子集；
根据所述预测全局特征集F4，以及参考全局特征集F3，计算每对特征间的误差；累加所述预测全局特征集F4和参考全局特征集F3中每对特征间的所述误差，得到所述位姿P1的误差，过滤所述可能位姿集合{P}中超过设定误差的位姿，得到可能位姿子集。
8.根据权利要求5所述的基于点线特征机器人初始位姿获取方法，其特征在于，对可能位姿子集中每一个位姿进行评分，选取分数最优的位姿作为机器人的初始位姿P的步骤包括：基于所述全局地图，根据所述可能位姿子集中任一位姿P2，获取与二维激光雷达的实时测量数据相对应的参考测量数据；根据所述实时测量数据与所述参考测量数据的误差，计算所述位姿P2的评分；根据评分结果，选取可能位姿子集中评分最高的位姿P作为机器人的初始位姿。
9.基于点线特征机器人开机自定位方法，其特征在于，采用权利要求1-8任一项所述的基于点线特征机器人初始位姿获取方法获取移动机器人的初始位姿P，还包括：根据移动机器人的初始位姿P，采用粒子滤波算法对移动机器人进行定位。
10.一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现如权利要求1-8任一项所述的基于点线特征机器人初始位姿获取方法或如权利要求9所述的基于点线特征移动机器人开机自定位方法。
CN202010139727.3A 2020-03-03 2020-03-03 基于点线特征机器人初始位姿获取方法及开机自定位方法 Active CN111283730B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010139727.3A CN111283730B (zh) 2020-03-03 2020-03-03 基于点线特征机器人初始位姿获取方法及开机自定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010139727.3A CN111283730B (zh) 2020-03-03 2020-03-03 基于点线特征机器人初始位姿获取方法及开机自定位方法

Publications (2)

Publication Number Publication Date
CN111283730A true CN111283730A (zh) 2020-06-16
CN111283730B CN111283730B (zh) 2021-04-20

Family

ID=71022585

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010139727.3A Active CN111283730B (zh) 2020-03-03 2020-03-03 基于点线特征机器人初始位姿获取方法及开机自定位方法

Country Status (1)

Country Link
CN (1) CN111283730B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022073172A1 (zh) * 2020-10-09 2022-04-14 浙江大学 一种基于点线特征的机器人全局最优视觉定位方法及装置

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20100040234A (ko) * 2008-10-09 2010-04-19 삼성전자주식회사 로봇의 ｓｌａｍ 장치 및 방법
KR20130142336A (ko) * 2012-06-19 2013-12-30 한양대학교 산학협력단 3차원 직선을 이용하여 위치를 인식하고 지도를 생성하는 방법 및 그 방법에 따른 이동체
US20160375583A1 (en) * 2015-06-23 2016-12-29 Electronics And Telecommunications Research Institute Apparatus and method for providing accuracy of robot location information by using sensor
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
US20190066329A1 (en) * 2017-08-23 2019-02-28 TuSimple System and method for centimeter precision localization using camera-based submap and lidar-based global map
CN109493385A (zh) * 2018-10-08 2019-03-19 上海大学 一种结合场景点线特征的移动机器人室内自主定位方法
CN110489501A (zh) * 2019-07-24 2019-11-22 西北工业大学 基于线特征的slam系统快速重定位算法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20100040234A (ko) * 2008-10-09 2010-04-19 삼성전자주식회사 로봇의 ｓｌａｍ 장치 및 방법
KR20130142336A (ko) * 2012-06-19 2013-12-30 한양대학교 산학협력단 3차원 직선을 이용하여 위치를 인식하고 지도를 생성하는 방법 및 그 방법에 따른 이동체
US20160375583A1 (en) * 2015-06-23 2016-12-29 Electronics And Telecommunications Research Institute Apparatus and method for providing accuracy of robot location information by using sensor
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
US20190066329A1 (en) * 2017-08-23 2019-02-28 TuSimple System and method for centimeter precision localization using camera-based submap and lidar-based global map
CN109493385A (zh) * 2018-10-08 2019-03-19 上海大学 一种结合场景点线特征的移动机器人室内自主定位方法
CN110489501A (zh) * 2019-07-24 2019-11-22 西北工业大学 基于线特征的slam系统快速重定位算法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022073172A1 (zh) * 2020-10-09 2022-04-14 浙江大学 一种基于点线特征的机器人全局最优视觉定位方法及装置
US11964401B2 (en) 2020-10-09 2024-04-23 Zhejiang University Robot globally optimal visual positioning method and device based on point-line features

Also Published As

Publication number Publication date
CN111283730B (zh) 2021-04-20

Similar Documents

Publication Publication Date Title
CN111325796B (zh) 用于确定视觉设备的位姿的方法和装置
CN110243360B (zh) 机器人在运动区域的地图构建及定位方法
KR100493159B1 (ko) 이동체의 효율적 자기 위치 인식을 위한 랜드마크 및 이를이용한 자기 위치 인식 장치 및 방법
CN111508021B (zh) 一种位姿确定方法、装置、存储介质及电子设备
CN110703268B (zh) 一种自主定位导航的航线规划方法和装置
JP5924862B2 (ja) 情報処理装置、情報処理方法及びプログラム
Huang Review on LiDAR-based SLAM techniques
Valiente et al. A comparison of EKF and SGD applied to a view-based SLAM approach with omnidirectional images
WO2022183785A1 (zh) 机器人定位方法、装置、机器人和可读存储介质
CN112767490A (zh) 一种基于激光雷达的室外三维同步定位与建图方法
CN111578958A (zh) 移动机器人导航实时定位方法、系统、介质及电子设备
Martinelli Simultaneous localization and mapping using the phase of passive UHF-RFID signals
CN111680596A (zh) 基于深度学习的定位真值校验方法、装置、设备及介质
CN111283730B (zh) 基于点线特征机器人初始位姿获取方法及开机自定位方法
KR100327120B1 (ko) 증강현실 기반의 3차원 트래커와 컴퓨터 비젼을 이용한영상 정합 방법
Drwięga Features matching based merging of 3D maps in multi-robot systems
CN115683100A (zh) 机器人的定位方法、装置、机器人以及存储介质
CN113671523A (zh) 机器人定位方法、装置、存储介质及机器人
Kim et al. Structured light camera base 3D visual perception and tracking application system with robot grasping task
KR101961171B1 (ko) 실내 환경에서 이동하는 로봇의 전역적 자기 위치 추정 시스템 및 이를 이용한 전역적 자기 위치 추정 방법
Oliveira et al. New trends on computer vision applied to mobile robot localization
Dong et al. Pose estimation of components in 3c products based on point cloud registration
CN115307641A (zh) 机器人定位方法、装置、机器人和存储介质
CN113960614A (zh) 一种基于帧-地图匹配的高程图构建方法
CN113192138A (zh) 一种机器人自主重定位方法、装置、机器人及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP02 Change in the address of a patent holder

Address after: 510000 201, building a, No.19 nanxiangsan Road, Huangpu District, Guangzhou City, Guangdong Province

Patentee after: GUANGZHOU SAITE INTELLIGENT TECHNOLOGY Co.,Ltd.

Address before: 510000 Room 303, 36 Kaitai Avenue, Huangpu District, Guangzhou City, Guangdong Province

Patentee before: GUANGZHOU SAITE INTELLIGENT TECHNOLOGY Co.,Ltd.

CP02 Change in the address of a patent holder