CN110927740A - 一种移动机器人定位方法 - Google Patents

一种移动机器人定位方法 Download PDF

Info

Publication number
CN110927740A
CN110927740A CN201911244060.7A CN201911244060A CN110927740A CN 110927740 A CN110927740 A CN 110927740A CN 201911244060 A CN201911244060 A CN 201911244060A CN 110927740 A CN110927740 A CN 110927740A
Authority
CN
China
Prior art keywords
point cloud
pose
ptcloud
robot
map
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
CN201911244060.7A
Other languages
English (en)
Other versions
CN110927740B (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.)
Hefei Technological University Intelligent Robot Technology Co ltd
Original Assignee
KEDA INTELLIGENT ELECTRICAL TECHNOLOGY Co Ltd
KEDA INTELLIGENT TECHNOLOGY Co Ltd
Hefei Technological University Intelligent Robot 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 KEDA INTELLIGENT ELECTRICAL TECHNOLOGY Co Ltd, KEDA INTELLIGENT TECHNOLOGY Co Ltd, Hefei Technological University Intelligent Robot Technology Co ltd filed Critical KEDA INTELLIGENT ELECTRICAL TECHNOLOGY Co Ltd
Priority to CN201911244060.7A priority Critical patent/CN110927740B/zh
Publication of CN110927740A publication Critical patent/CN110927740A/zh
Application granted granted Critical
Publication of CN110927740B publication Critical patent/CN110927740B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明公开了一种移动机器人定位方法，属于移动机器人定位技术领域，包括将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云；采用自适应蒙特卡洛定位算法，估计出机器人当前的位姿；利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan；将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T；根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿。通过将采用自适应蒙特卡洛定位算法得到的位姿作为参考值变换二维激光雷达的点云坐标系，使其与当前的地图点云足够近，以加速迭代最近点的匹配速度，提高定位精度。

Description

一种移动机器人定位方法
技术领域
本发明涉及移动机器人定位技术领域，特别涉及一种移动机器人定位方法。
背景技术
随着科学技术的快速发展以及人工智能时代的来临，移动机器人发展迅速，且在各行各业中得到了广泛应用。其中如何让机器人知道自己在当前所处环境中的位姿并且在移动过程中也反馈位姿需要用到定位技术。在多数机器人工作时，首先需要高精度的定位才能够执行后续的任务，所以，如何对机器人实时、准确、高效的定位成为问题的关键。
现在已有的定位技术有基于卡尔曼滤波(Kalman filtering，KF)，KF是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。以及对KF衍生的EKF、UKF等不同模型假设来近似求最优后验概率，以此来定位机器人。自适应蒙特卡洛定位(adaptive Monte Carlo localization，AMCL)是一种改良版的MCL算法，针对与MCL无法从机器人绑架或全局定位失败中恢复过来，AMCL设置了增加随机粒子来解决这一问题。迭代最近点(adaptive Monte Carlo localization，ICP)方法是不停的求解实时激光点云与地图的变换关系，获的旋转矩阵R和平移向量T来确定机器人的实时位姿。
但是，通过卡尔曼滤波定位方式由于里程计有累计误差，加上不同噪声干扰，会使得定位过程中，误差会随着时间增加越来越大。而自适应蒙特卡洛定位则需要虽然比卡尔曼滤波算法相对稳定，但是其总体定位的精度不高，且需要大量粒子来维持较高的定位精度。使用ICP求解地图和当前激光点关系的定位方式会有大量的计算，尤其在大地图中点云较多，定位时间耗时较长，在机器人移动过程中不能具有时效性，无法应用于后续导航功能。
发明内容
本发明的目的在于克服现有技术的不足，以在移动机器人导航过程中，提高当前位姿定位的精准性。
为实现以上目的，本发明采用一种移动机器人定位方法，包括如下步骤：
将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云；
采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，并根据混入噪声对各个粒子位姿进行更新，并根据粒子的位姿权重估计出机器人当前的位姿；
利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan；
将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T；
根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿。
进一步地，所述将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云，包括：
将所述搭载二维激光雷达的机器人置于当前定位的环境中，并加载预先建立好的当前定位环境二维占据栅格地图；
将当前定位环境二维占据栅格地图中障碍物的点坐标提取存入地图点云中，得到障碍物的地图点云。
进一步地，所述采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，并根据混入噪声对各个粒子位姿进行更新，并根据粒子的位姿权重估计出机器人当前的位姿，包括：
采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子；
根据混入噪声的实际获取里程计速度对各个粒子位姿进行更新，得到更新后的粒子位姿；
采用所述二维激光雷达所获取数据与栅格地图的似然域模型，获取每个粒子的观测模型，赋予粒子相应的位姿权重；
利用所述二维激光雷达的测量概率对每个粒子的位姿权重进行修正，得到机器人运动过程中不断迭代更新的粒子位姿及位姿权重；
根据粒子的位姿权重，估计出机器人当前的位姿。
进一步地，所述混入噪声的实际获取里程计速度(v,ω)为：
Figure BDA0002307036290000031
其中，ε是一个方差为b2均值为0的误差变量，参数α1～α4是指定的机器人误差参数，v′是平移速度，ω′是旋转速度，v是计算得到的加入噪声后的实际平移速度，ω是计算得到的加入噪声后的实际旋转速度。
进一步地，所述误差变量ε的正态分布由如下概率密度函数给定：
Figure BDA0002307036290000032
其中，a是正态分布的自变量。
进一步地，所述二维激光雷达的测量概率由一个以0为中心的高斯函数给出：
Figure BDA0002307036290000033
令dist表示测量坐标
Figure BDA0002307036290000034
与栅格地图m上最近物体的欧氏距离，phit由二维激光雷达轴正交(归一化)似然域得到。
进一步地，所述利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan，其转换公式为：
PTCloudscan(x)＝scan(x)+x1
PTCloudscan(y)＝scan(y)+y1
其中，PTCloudscan(x)是点云PTCloudscan下的所有x坐标值，PTCloudscan(y)是点云PTCloudscan下的所有y坐标值，scan为从激光雷达传入的以初始值(默认0，0)的激光点云数据，(x1，y1)是当前定位得到的坐标。
进一步地，还包括：
将所述点云PTCloudscan中的每一个点(P1，P2.........Pn)通过八叉树搜索在地图点云PTCloudmap指定半径内的所有点，生成地图点云PTCloudmap’。
进一步地，所述将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T，包括：
对每一帧经变换后激光雷达数据生成的点云PTCloudscan作为ICP的输入；
将每一帧地图点云PTCloudmap’作为目标点云，使用PointToPoint-ICP算法迭代生成当前帧激光点云PTCloudscan相对于地图点云PTCloudmap’的旋转矩阵R和平移矩阵T。
进一步地，所述根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿，包括：
Figure BDA0002307036290000051
θicp＝θamcl+θ
其中，旋转矩阵
Figure BDA0002307036290000052
平移矩阵
Figure BDA0002307036290000053
将picp(xicp，yicp，θicp)作为最终机器人的位姿，xamcl和yamcl分别是经amcl方法估计出来的定位位姿坐标，θamcl是amcl估计出来的方向角度。
与现有技术相比，本发明存在以下技术效果：本发明将采用自适应蒙特卡洛定位算法得到的位姿作为参考值变换二维激光雷达的点云坐标系，使其与当前的地图点云足够接近，以加速迭代最近点的匹配速度，提高定位精度。本方案所定位距离与角度的平均误差与方差均小于单独使用适应蒙特卡洛定位算法所得数据。
附图说明
下面结合附图，对本发明的具体实施方式进行详细描述：
图1是一种移动机器人定位方法的步骤示意图；
图2是一种移动机器人定位方法的流程示意图；
图3是栅格地图；
图4是地图点云；
图5是粒子分布示意图；
图6是激光点云；
图7是八叉树搜索的地图点云；
图8是本发明方法与自适应蒙特卡洛定位算法所计算距离误差对比示意图；
图9是本发明方法与自适应蒙特卡洛定位算法所计算角度误差对比示意图。
具体实施方式
为了更进一步说明本发明的特征，请参阅以下有关本发明的详细说明与附图。所附图仅供参考与说明之用，并非用来对本发明的保护范围加以限制。
如图1所示，本实施例公开了一种移动机器人定位方法，包括如下步骤S1至S5：
S1、将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云；
S2、采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，并根据混入噪声对各个粒子位姿进行更新，并根据粒子的位姿权重估计出机器人当前的位姿；
S3、利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan；
需要说明的是，PTCloudscan是指当前的帧激光点云。
S4、将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T；
S5、根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿。
需要说明的是，本发明将采用自适应蒙特卡洛定位算法得到的位姿作为参考值变换二维激光雷达的点云坐标系，使其与当前的地图点云足够接近，以加速迭代最近点的匹配速度，提高定位精度。
具体来说，在上述步骤S1：将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云，包括如下细分步骤S11至S12：
S11、将所述搭载二维激光雷达的机器人置于当前定位的环境中，并加载预先建立好的当前定位环境二维占据栅格地图；
需要说明的是，栅格地图m可通过同时定位与地图构建(SimultaneousLocalization And Mapping，SLAM)技术构建，将数据存入mapdata中。
S12、将当前定位环境二维占据栅格地图中障碍物的点坐标提取存入地图点云中，得到障碍物的地图点云。
需要说明的是，栅格地图中值为100的点为障碍物，值为0的点为空地。将栅格地图中的所有值为100的点坐标(x，y)提取存入地图点云PTCloudmap中，生成以障碍物点构成的地图点云PTCloudmap：
PTCloudmap(x)＝(i％width+0.5)*resolution+originx
Figure BDA0002307036290000071
式中：i为栅格地图mapdata中值为100的点的索引，resolution为栅格地图分别率，originx和originy分别为地图左下角点对应的实际坐标，width是栅格地图mapdata的宽，PTCloudmap(x)是和PTCloudmap(y)分别是地图点云PTCloudmap的x和y坐标。
具体来说，在上述步骤S2：采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，并根据混入噪声对各个粒子位姿进行更新，并根据粒子的位姿权重估计出机器人当前的位姿，包括如下细分步骤S21至S25：
S21、采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子；
需要说明的是，采用AMCL定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，每一个粒子代表了一个机器人可能的位姿。
S22、根据混入噪声的实际获取里程计速度对各个粒子位姿进行更新，得到更新后的粒子位姿；
需要说明的是，当机器人开始运动时，由于机器人在移动时里程计速度会受到噪声影响，所以混入噪声的实际速度(v，ω)可由以下公式给定：
Figure BDA0002307036290000081
其中，ε是一个方差为b2均值为0的误差变量，参数α1～α4是指定的机器人误差参数，v′是平移速度，ω′是旋转速度，v是计算得到的加入噪声后的实际平移速度，ω是计算得到的加入噪声后的实际旋转速度。
其中，ε的正态分布由以下概率密度函数给定：
Figure BDA0002307036290000082
其中，a是正态分布的自变量，然后根据混入噪声的实际获取里程计速度传入运动模型更新后的各个粒子位姿。
S23、采用所述二维激光雷达所获取数据与栅格地图的似然域模型，获取每个粒子的观测模型，赋予粒子相应的位姿权重；
需要说明的是，通过采集二维激光雷达数据与地图的似然域模型获取当前每个粒子的观测模型，赋予粒子相应的位姿权重。似然域模型(likelihood_field model)主要思想是将传感器即二维激光雷达扫描的终点zt映射到栅格地图，计算与栅格地图最近障碍之间的距离。
S24、利用所述二维激光雷达的测量概率对每个粒子的位姿权重进行修正，得到机器人运动过程中不断迭代更新的粒子位姿及位姿权重；
需要说明的是，二维激光雷达测量的概率可以由一个以0为中心的高斯函数给出：
Figure BDA0002307036290000083
令dist表示测量坐标
Figure BDA0002307036290000084
与占据栅格地图m上最近物体的欧氏距离，phit可以由传感器轴正交(归一化)似然域得到，εhit是表示一个以0为中心的高斯函数。
需要说明的是，广义上公式
Figure BDA0002307036290000085
中的m可以表示任意格式的地图，本实施例中是指占据栅格地图。但m是表示地图中的所信息(包括环境中的物体列表和属性)。
S25、根据粒子的位姿权重，估计出机器人当前的位姿。
需要说明的是，由得到的二维激光雷达概率修正每一个粒子的权重，运动过程中不断迭代更新粒子的位姿和权重，保留权重较大的粒子，为防止粒子的多样性减少，系统间断性重采样，AMCL算法可根据粒子权重实时估计出当前机器人的位姿pamcl(xamcl，yamcl，θamcl)，传感器修正粒子权重是AMCL中的算法。
具体来说，上述步骤S3：利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan，其转换公式具体为：
PTCloUdscan(x)＝scan(x)+x1
PTCloudscan(y)＝scan(y)+y1
其中，PTCloudscan(x)是点云PTCloudscan下的所有x坐标值，PTCloudscan(y)是点云PTCloudscan下的所有y坐标值，scan为从激光雷达传入的以初始值(默认0，0)为原点的激光点云数据，(x1，y1)和是当前定位得到的坐标。
需要说明的是，该转换公式是将PTCloudscan点云的所有点的横坐标x和纵坐标y均加上x1和y1。
需要说明的是，由于机器人在实际环境中定位时，由当前环境的建立的地图较大，范围较广，其点云中点数较多，如果直接与激光点云做ICP运算会消耗大量的时间，不能保证实时性，因为大地图中多数点在ICP算法是无效点。所以本实施例将PTCloudscan点云中的每一个点(P1，P2………Pn)通过八叉树搜索在地图点云PTCloudmap指定半径内的所有点，生成点云PTCloudmap’，PTCloudmap’是指通过八叉树搜索临近点后的点云，其点云个数远远小于原地图点云PTCloudmap，此时PTCloudmap’中的点仅保存原地图中位于当前帧激光雷达点云附近的有效匹配点，可提高匹配准确度，减少匹配时间。
本实施例中通过实时八叉树搜索领域最近点作为目标地图，较传统ICP直接与全部地图匹配，可以大量减少计算时间，提高算法效率，可以满足移动机器人实时定位。
具体来说，上述步骤S4：将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T，具体为：
对每一帧经变换后激光雷达数据生成的点云PTCloudscan作为ICP的输入，将每一帧激光雷达搜索出的半径内地图点云PTCloudmap’作为目标点云，使用PointToPoint-ICP算法迭代生成当前帧激光点云PTCloudscan相对于地图点云PTCloudmap’的旋转矩阵R和平移矩阵T。
具体来说，上述步骤S5：根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿，具体为：
将ICP求解的旋转矩阵R和平移矩阵根据如下公式作用于pamcl(xamcl，yamcl，θamcl)：
Figure BDA0002307036290000101
θicp＝θamcl+θ
其中，旋转矩阵
Figure BDA0002307036290000102
平移矩阵
Figure BDA0002307036290000103
将picp(xicp，yicp，θicp)作为最终机器人的位姿，xamcl和yamcl分别是经amcl方法估计出来的定位位姿坐标，θamcl是amcl方法估计出来的方向角。
以下通过一具体示例对本发明方案进行详细解释：
本实例使用机器人CSG-PIR-DS400进行算法演示，机器人搭载sick151二位激光雷达、运动控制模块、信息采集模块、轮式里程计、电源管理模块、工控机(处理器为i5-4300U)。本发明实例所用的栅格地图由谷歌开源算法cartographer生成，所测试地点为办公室和楼道。选择地图中的三个点A、B、C分别用本发明的定位方法和单独使用AMCL方法测试十次重复定位精度。经本实验对比，本发明的定位方法在距离和角度的得定位精度都明显高于单独使用AMCL定位方法，本发明方法的流程图为图2，以下是具体操作步骤：
(1)将搭载二维激光雷达的机器人置于办公室中，正式开始定位前需加载已经建立好的办公室和楼道二维占据栅格地图，如图3所示。其中地图中值为100的点为障碍物，值为0的点为空地。将地图中的所有值为100的点x、y坐标提取存入地图点云PTCloudmap中，生成的点云地图如图4。
(2)采用AMCL定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，每一个粒子代表了一个机器人可能的位姿。当机器人开始运动时，获取里程计数据传入运动模型更新后的各个粒子位姿，利用观测模型即当前每个粒子的二维激光雷达数据与地图的似然域模型获取相应的位姿权重。最后通过贝叶斯概率理论修正每一个粒子的权重，运动过程中不断迭代，间断性重采样，AMCL算法可根据粒子权重实时估计出当前机器人的位姿，AMCL移动中的粒子分布如图5所示。
(3)利用AMCL输出的位姿将实时的激光雷达数据转换为与地图PTCloudmap同一坐标系下的的点云PTCloudscan。转换后激光点云如图6所示。
由于地图较大，其点云中点数较多，如何直接与激光点云做ICP运算会消耗大量的时间，不能保证实时性，因为大地图中多数点在ICP算法是无效的，所以将PTCloudscan点云中的每一个点(P1,P2………Pn)通过八叉树搜索在点云PTCloudmap指定半径内的所有点，生成子地图的点云如图7所示。
步骤四：对每一帧经变换后激光雷达数据生成的点云PTCloudscan最为ICP的输入，将每一帧激光雷达搜索出的半径内地图点云PTCloudmap’作为目标点云，做ICP算法迭代生成PTCloudscan相对于PTCloudmap’的旋转矩阵R和平移矩阵T。
步骤五：从不同方向和路线移动机器人，经过A、B、C三点时，记录最终ICP定位位姿和AMCL定位位姿，与真实位姿作为对比。其距离误差如图8所示，角度误差如图9所示。其中编号1-10为点A，11-20为点B，21-30为点C。经试验测得通过AMCL定位距离平局误差为4.29cm，标准差为1.05,定位角度的平均误差为2.5°，标准差为0.92。通过本算法最终ICP后的定位距离平均误差为1.59cm，标准差为0.64。定位角度平均误差为0.75°，标准差为0.35。本实例选择了在环境中选择3个试验点分别用单独AMCL和本算发进行重复定位精度测试，分析每次距离和角度平均误差和标准差，试验结果证明，本算法在稳定性和精准性均优于当前较为流行的AMCL算法。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种移动机器人定位方法，其特征在于，包括：
将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云；
采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，并根据混入噪声对各个粒子位姿进行更新，并根据粒子的位姿权重估计出机器人当前的位姿；
利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan；
将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T；
根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿。
2.如权利要求1所述的移动机器人定位方法，其特征在于，所述将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云，包括：
将所述搭载二维激光雷达的机器人置于当前定位的环境中，并加载预先建立好的当前定位环境二维占据栅格地图；
将当前定位环境二维占据栅格地图中障碍物的点坐标提取存入地图点云中，得到障碍物的地图点云。
3.如权利要求2所述的移动机器人定位方法，其特征在于，所述采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子，并根据混入噪声对各个粒子位姿进行更新，并根据粒子的位姿权重估计出机器人当前的位姿，包括：
采用自适应蒙特卡洛定位算法，在机器人初始位姿附近生成服从高斯分布的粒子；
根据混入噪声的实际获取里程计速度对各个粒子位姿进行更新，得到更新后的粒子位姿；
采用所述二维激光雷达所获取数据与栅格地图的似然域模型，获取每个粒子的观测模型，赋予粒子相应的位姿权重；
利用所述二维激光雷达的测量概率对每个粒子的位姿权重进行修正，得到机器人运动过程中不断迭代更新的粒子位姿及位姿权重；
根据粒子的位姿权重，估计出机器人当前的位姿。
4.如权利要求3所述的移动机器人定位方法，其特征在于，所述混入噪声的实际获取里程计速度(v,ω)为：
Figure FDA0002307036280000021
其中，ε是一个方差为b2均值为0的误差变量，参数α1～α4是指定的机器人误差参数，v′是平移速度，ω′是旋转速度，v是计算得到的加入噪声后的实际平移速度，ω是计算得到的加入噪声后的实际旋转速度。
5.如权利要求4所述的移动机器人定位方法，其特征在于，所述误差变量ε的正态分布由如下概率密度函数给定：
Figure FDA0002307036280000022
其中，a是正态分布的自变量。
6.如权利要求3所述的移动机器人定位方法，其特征在于，所述二维激光雷达的测量概率由一个以0为中心的高斯函数给出：
Figure FDA0002307036280000023
令dist表示测量坐标
Figure FDA0002307036280000024
与当前机器人所在环境的占据栅格地图m上最近物体的欧氏距离，phit由二维激光雷达轴正交似然域得到。
7.如权利要求1所述的移动机器人定位方法，其特征在于，所述利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan，其转换公式为：
PTCloudscan(x)＝scan(x)+x1
PTCloudscan(y)＝scan(y)+y1
其中，PTCloudscan(x)是点云PTCloudscan下的所有x坐标值，PTCloudscan(y)是点云PTCloudscan下的所有y坐标值，(x1,y1)是当前定位得到的坐标。
8.如权利要求7所述的移动机器人定位方法，其特征在于，还包括：
将所述点云PTCloudscan中的每一个点(P1,P2………Pn)通过八叉树搜索在地图点云PTCloudmap指定半径内的所有点，生成地图点云PTCloudmap’。
9.如权利要求8所述的移动机器人定位方法，其特征在于，所述将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T，包括：
对每一帧经变换后激光雷达数据生成的点云PTCloudscan作为ICP的输入；
将每一帧地图点云PTCloudmap’作为目标点云，使用PointToPoint-ICP算法迭代生成当前帧激光点云PTCloudscan相对于地图点云PTCloudmap’的旋转矩阵R和平移矩阵T。
10.如权利要求1或9所述的移动机器人定位方法，其特征在于，所述根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿，包括：
Figure FDA0002307036280000031
θicp＝θamcl+θ
其中，旋转矩阵
Figure FDA0002307036280000041
平移矩阵
Figure FDA0002307036280000042
将picp(xicp，yicp，θicp)作为最终机器人的位姿，xamcl和yamcl分别是经amcl方法估计出来的定位位姿坐标，θamcl是amcl估计出来的方向角度。
CN201911244060.7A 2019-12-06 2019-12-06 一种移动机器人定位方法 Active CN110927740B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911244060.7A CN110927740B (zh) 2019-12-06 2019-12-06 一种移动机器人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911244060.7A CN110927740B (zh) 2019-12-06 2019-12-06 一种移动机器人定位方法

Publications (2)

Publication Number Publication Date
CN110927740A true CN110927740A (zh) 2020-03-27
CN110927740B CN110927740B (zh) 2023-09-08

Family

ID=69857547

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911244060.7A Active CN110927740B (zh) 2019-12-06 2019-12-06 一种移动机器人定位方法

Country Status (1)

Country Link
CN (1) CN110927740B (zh)

Cited By (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111209978A (zh) * 2020-04-20 2020-05-29 浙江欣奕华智能科技有限公司 三维视觉重定位方法、装置及计算设备、存储介质
CN111402413A (zh) * 2020-06-04 2020-07-10 浙江欣奕华智能科技有限公司 三维视觉定位方法、装置及计算设备、存储介质
CN111421548A (zh) * 2020-04-21 2020-07-17 武汉理工大学 一种移动机器人定位方法及系统
CN111474560A (zh) * 2020-04-16 2020-07-31 苏州大学 一种障碍物定位方法、装置及设备
CN111765883A (zh) * 2020-06-18 2020-10-13 浙江大华技术股份有限公司 机器人蒙特卡罗定位方法、设备及存储介质
CN111812669A (zh) * 2020-07-16 2020-10-23 南京航空航天大学 绕机检查装置及其定位方法、存储介质
CN111895990A (zh) * 2020-07-14 2020-11-06 武汉科技大学 一种基于多指标绑架检测及移动机器人重定位的方法
CN112045655A (zh) * 2020-08-14 2020-12-08 华中科技大学 用于大尺度多站点场景的移动机器人位姿测量方法及系统
CN112284403A (zh) * 2020-12-28 2021-01-29 深兰人工智能芯片研究院（江苏）有限公司 定位方法、装置、电子设备和存储介质
CN112396656A (zh) * 2020-11-24 2021-02-23 福州大学 一种视觉与激光雷达融合的室外移动机器人位姿估计方法
CN112561998A (zh) * 2020-12-16 2021-03-26 国网江苏省电力有限公司检修分公司 一种基于三维点云配准的机器人定位和自主充电方法
CN112764053A (zh) * 2020-12-29 2021-05-07 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质
CN112880664A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN112882056A (zh) * 2021-01-15 2021-06-01 西安理工大学 基于激光雷达的移动机器人同步定位与地图构建方法
CN112987027A (zh) * 2021-01-20 2021-06-18 长沙海格北斗信息技术有限公司 一种基于高斯模型的amcl算法的定位方法及存储介质
CN113050116A (zh) * 2021-03-05 2021-06-29 深圳市优必选科技股份有限公司 机器人定位方法、装置、机器人和可读存储介质
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113504543A (zh) * 2021-06-16 2021-10-15 国网山西省电力公司电力科学研究院 无人机LiDAR系统定位定姿系统及方法
WO2021208797A1 (zh) * 2020-04-13 2021-10-21 京东数科海益信息科技有限公司 障碍物数据处理方法、装置、设备及存储介质
CN113552585A (zh) * 2021-07-14 2021-10-26 浙江大学 一种基于卫星地图与激光雷达信息的移动机器人定位方法
CN113554705A (zh) * 2021-07-14 2021-10-26 南京航空航天大学 一种变化场景下的激光雷达鲁棒定位方法
CN113625296A (zh) * 2021-07-30 2021-11-09 深圳市优必选科技股份有限公司 基于反光板的机器人定位方法、装置及机器人
CN113640822A (zh) * 2021-07-07 2021-11-12 华南理工大学 一种基于非地图元素过滤的高精度地图构建方法
WO2022007504A1 (zh) * 2020-07-09 2022-01-13 北京京东乾石科技有限公司 位置确定方法、装置、系统和计算机可读存储介质
CN114331966A (zh) * 2021-12-02 2022-04-12 北京斯年智驾科技有限公司 基于高斯过程占位图估计辅助的港口锁站方法及其系统
WO2022257196A1 (zh) * 2021-06-09 2022-12-15 成都信息工程大学 基于点云地图动态加载的自主移动装置定位方法
CN117289294A (zh) * 2023-11-27 2023-12-26 睿羿科技(山东)有限公司 一种基于多分辨率贝叶斯网格的融合定位方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180088234A1 (en) * 2016-09-27 2018-03-29 Carnegie Mellon University Robust Localization and Localizability Prediction Using a Rotating Laser Scanner
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180088234A1 (en) * 2016-09-27 2018-03-29 Carnegie Mellon University Robust Localization and Localizability Prediction Using a Rotating Laser Scanner
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法

Cited By (47)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021208797A1 (zh) * 2020-04-13 2021-10-21 京东数科海益信息科技有限公司 障碍物数据处理方法、装置、设备及存储介质
CN111474560B (zh) * 2020-04-16 2023-11-24 苏州大学 一种障碍物定位方法、装置及设备
CN111474560A (zh) * 2020-04-16 2020-07-31 苏州大学 一种障碍物定位方法、装置及设备
CN111209978A (zh) * 2020-04-20 2020-05-29 浙江欣奕华智能科技有限公司 三维视觉重定位方法、装置及计算设备、存储介质
CN111209978B (zh) * 2020-04-20 2020-07-10 浙江欣奕华智能科技有限公司 三维视觉重定位方法、装置及计算设备、存储介质
CN111421548A (zh) * 2020-04-21 2020-07-17 武汉理工大学 一种移动机器人定位方法及系统
CN111421548B (zh) * 2020-04-21 2021-10-19 武汉理工大学 一种移动机器人定位方法及系统
CN111402413A (zh) * 2020-06-04 2020-07-10 浙江欣奕华智能科技有限公司 三维视觉定位方法、装置及计算设备、存储介质
CN111765883A (zh) * 2020-06-18 2020-10-13 浙江大华技术股份有限公司 机器人蒙特卡罗定位方法、设备及存储介质
CN111765883B (zh) * 2020-06-18 2023-12-15 浙江华睿科技股份有限公司 机器人蒙特卡罗定位方法、设备及存储介质
WO2022007504A1 (zh) * 2020-07-09 2022-01-13 北京京东乾石科技有限公司 位置确定方法、装置、系统和计算机可读存储介质
CN111895990A (zh) * 2020-07-14 2020-11-06 武汉科技大学 一种基于多指标绑架检测及移动机器人重定位的方法
CN111895990B (zh) * 2020-07-14 2022-04-15 武汉科技大学 一种基于多指标绑架检测及移动机器人重定位的方法
CN111812669A (zh) * 2020-07-16 2020-10-23 南京航空航天大学 绕机检查装置及其定位方法、存储介质
CN112045655A (zh) * 2020-08-14 2020-12-08 华中科技大学 用于大尺度多站点场景的移动机器人位姿测量方法及系统
CN112045655B (zh) * 2020-08-14 2021-12-31 华中科技大学 用于大尺度多站点场景的移动机器人位姿测量方法及系统
CN112396656A (zh) * 2020-11-24 2021-02-23 福州大学 一种视觉与激光雷达融合的室外移动机器人位姿估计方法
CN112561998B (zh) * 2020-12-16 2024-02-20 国网江苏省电力有限公司检修分公司 一种基于三维点云配准的机器人定位和自主充电方法
CN112561998A (zh) * 2020-12-16 2021-03-26 国网江苏省电力有限公司检修分公司 一种基于三维点云配准的机器人定位和自主充电方法
CN112284403A (zh) * 2020-12-28 2021-01-29 深兰人工智能芯片研究院（江苏）有限公司 定位方法、装置、电子设备和存储介质
CN112764053A (zh) * 2020-12-29 2021-05-07 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质
WO2022142992A1 (zh) * 2020-12-29 2022-07-07 深圳市普渡科技有限公司 融合定位方法、装置、设备和计算机可读存储介质
CN112764053B (zh) * 2020-12-29 2022-07-15 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质
CN112882056A (zh) * 2021-01-15 2021-06-01 西安理工大学 基于激光雷达的移动机器人同步定位与地图构建方法
CN112882056B (zh) * 2021-01-15 2024-04-09 西安理工大学 基于激光雷达的移动机器人同步定位与地图构建方法
CN112987027A (zh) * 2021-01-20 2021-06-18 长沙海格北斗信息技术有限公司 一种基于高斯模型的amcl算法的定位方法及存储介质
CN112987027B (zh) * 2021-01-20 2024-03-15 长沙海格北斗信息技术有限公司 一种基于高斯模型的amcl算法的定位方法及存储介质
CN112880664A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN113050116B (zh) * 2021-03-05 2024-02-27 深圳市优必选科技股份有限公司 机器人定位方法、装置、机器人和可读存储介质
CN113050116A (zh) * 2021-03-05 2021-06-29 深圳市优必选科技股份有限公司 机器人定位方法、装置、机器人和可读存储介质
WO2022183785A1 (zh) * 2021-03-05 2022-09-09 深圳市优必选科技股份有限公司 机器人定位方法、装置、机器人和可读存储介质
WO2022257196A1 (zh) * 2021-06-09 2022-12-15 成都信息工程大学 基于点云地图动态加载的自主移动装置定位方法
CN113375658B (zh) * 2021-06-15 2023-05-09 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113504543A (zh) * 2021-06-16 2021-10-15 国网山西省电力公司电力科学研究院 无人机LiDAR系统定位定姿系统及方法
CN113640822B (zh) * 2021-07-07 2023-08-18 华南理工大学 一种基于非地图元素过滤的高精度地图构建方法
CN113640822A (zh) * 2021-07-07 2021-11-12 华南理工大学 一种基于非地图元素过滤的高精度地图构建方法
CN113554705A (zh) * 2021-07-14 2021-10-26 南京航空航天大学 一种变化场景下的激光雷达鲁棒定位方法
CN113552585B (zh) * 2021-07-14 2023-10-31 浙江大学 一种基于卫星地图与激光雷达信息的移动机器人定位方法
CN113554705B (zh) * 2021-07-14 2024-03-19 南京航空航天大学 一种变化场景下的激光雷达鲁棒定位方法
CN113552585A (zh) * 2021-07-14 2021-10-26 浙江大学 一种基于卫星地图与激光雷达信息的移动机器人定位方法
CN113625296B (zh) * 2021-07-30 2023-11-21 深圳市优必选科技股份有限公司 基于反光板的机器人定位方法、装置及机器人
CN113625296A (zh) * 2021-07-30 2021-11-09 深圳市优必选科技股份有限公司 基于反光板的机器人定位方法、装置及机器人
CN114331966B (zh) * 2021-12-02 2024-02-13 北京斯年智驾科技有限公司 基于高斯过程占位图估计辅助的港口锁站方法及其系统
CN114331966A (zh) * 2021-12-02 2022-04-12 北京斯年智驾科技有限公司 基于高斯过程占位图估计辅助的港口锁站方法及其系统
CN117289294B (zh) * 2023-11-27 2024-03-15 睿羿科技(山东)有限公司 一种基于多分辨率贝叶斯网格的融合定位方法
CN117289294A (zh) * 2023-11-27 2023-12-26 睿羿科技(山东)有限公司 一种基于多分辨率贝叶斯网格的融合定位方法

Also Published As

Publication number Publication date
CN110927740B (zh) 2023-09-08

Similar Documents

Publication Publication Date Title
CN110927740B (zh) 一种移动机器人定位方法
CN112859859B (zh) 一种基于三维障碍物体素对象映射的动态栅格地图更新方法
CN110645974B (zh) 一种融合多传感器的移动机器人室内地图构建方法
Li et al. Multi-sensor fusion for navigation and mapping in autonomous vehicles: Accurate localization in urban environments
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统
CN109579849A (zh) 机器人定位方法、装置和机器人及计算机存储介质
CN110866927A (zh) 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法
Xu et al. Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation
CN111578958A (zh) 移动机器人导航实时定位方法、系统、介质及电子设备
CN109885046B (zh) 一种基于粒子滤波的移动机器人定位加速方法
Yin et al. Robust derivative unscented Kalman filter under non-Gaussian noise
CN111707279B (zh) 一种激光点云与地图的匹配评价方法、介质、终端和装置
Lu et al. Robot indoor location modeling and simulation based on Kalman filtering
CN113589809B (zh) 可避障的挖掘机工作装置作业轨迹规划方法及装置
Yong et al. An autonomous navigation strategy based on improved hector slam with dynamic weighted a* algorithm
CN112859110A (zh) 一种基于三维激光雷达的定位导航方法
CN116659500A (zh) 基于激光雷达扫描信息的移动机器人定位方法及系统
Lin et al. Indoor mapping using gmapping on embedded system
CN114815899A (zh) 基于3d激光雷达传感器的无人机三维空间路径规划方法
WO2021109166A1 (zh) 三维激光定位方法及系统
CN112747752A (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
Shen et al. A novel self-positioning based on feature map creation and laser location method for RBPF-SLAM
Zhou et al. A multi-innovation with forgetting factor based EKF-SLAM method for mobile robots
Dai et al. Improved Grid-Based Rao-Blackwellized Particle Filter SLAM Based on Grey Wolf Optimizer
Chen et al. Multirobot FastSLAM algorithm based on landmark consistency correction

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
TA01 Transfer of patent application right

Effective date of registration: 20201021

Address after: 230088 No. 5111 Wangjiang West Road, hi tech Zone, Anhui, Hefei

Applicant after: Hefei Technological University Intelligent Robot Technology Co.,Ltd.

Address before: 230088 No. 5111 Wangjiang West Road, hi tech Zone, Anhui, Hefei

Applicant before: Hefei Technological University Intelligent Robot Technology Co.,Ltd.

Applicant before: CSG SMART SCIENCE & TECHNOLOGY Co.,Ltd.

Applicant before: CSG SMART ELECTRIC TECHNOLOGY Co.,Ltd.

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant