CN109682382B - 基于自适应蒙特卡洛和特征匹配的全局融合定位方法 - Google Patents

基于自适应蒙特卡洛和特征匹配的全局融合定位方法 Download PDF

Info

Publication number
CN109682382B
CN109682382B CN201910149967.9A CN201910149967A CN109682382B CN 109682382 B CN109682382 B CN 109682382B CN 201910149967 A CN201910149967 A CN 201910149967A CN 109682382 B CN109682382 B CN 109682382B
Authority
CN
China
Prior art keywords
pose
laser radar
particles
map
particle
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.)
Active
Application number
CN201910149967.9A
Other languages
English (en)
Other versions
CN109682382A (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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN201910149967.9A priority Critical patent/CN109682382B/zh
Publication of CN109682382A publication Critical patent/CN109682382A/zh
Application granted granted Critical
Publication of CN109682382B publication Critical patent/CN109682382B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/20—Instruments for performing navigational calculations
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其包括提取栅格地图中的地图线特征存储至地图特征数据库中；提取非第二帧激光雷达扫描图的扫描图线特征，采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集位姿初始化；对于第二帧激光雷达扫描图，采用里程计数据、运动模型、激光雷达扫描图和测量模型更新粒子的位姿和权重；对于余下帧激光雷达扫描图，采用激光雷达扫描图计算的估算位姿更新粒子集中部分粒子的初始位姿，之后更新相应粒子的权重，同时采用里程计数据、运动模型、激光雷达扫描图和测量模型更新粒子集中余下部分粒子的位姿和权重，最后将两个更新过位姿和权重的粒子集合并成新的粒子集。

Description

基于自适应蒙特卡洛和特征匹配的全局融合定位方法
技术领域
本发明涉及机器人定位技术领域，具体涉及一种基于自适应蒙特卡洛和特征匹配的全局融合定位方法。
背景技术
移动机器人是指在未知环境或者部分未知环境中，完成环境感知、动态决策与规划、行为控制以及自主移动等功能的装置。对于移动机器人来说，定位就是确定机器人在环境地图中的位姿(包括位置和航向角)，它不仅是移动机器人完成环境地图的基本环节，也是机器人实现自主导航的关键技术。
目前机器人在实现全局定位时，仅仅根据传感器信息来估计自身在环境地图中的全局位置，由于没有初始位姿的先验信息，不能使用单峰分布来表示位姿误差，因此全局定位问题比位姿跟踪更困难，而且包含位姿跟踪问题。此外，全局定位问题也至关重要，它能够为移动机器人提供初始位姿，当机器人遇到跟踪失败或者机器人绑架问题时，能够重新定位机器人。
针对机器人的全局定位，目前有采用基于自适应蒙特卡洛(Adaptive MonteCarlo Localization，AMCL)的全局定位算法，其采用输入栅格地图m，激光雷达的观测信息z1:t以及里程计的运动信息u1:t，输出估计位姿
Figure BDA0001981243950000011
其处理的大体思路参考图1，其虽能解决定位失效的问题，但是仍存在如下缺点：
1、定位误差较低，定位误差与随机产生的粒子是否出现在真实位姿的附近有关，AMCL采用的方式是增加粒子数，增加粒子的多样性，另一方面，重采样过程使得粒子的多样性不断减少，当粒子没有出现在真实位姿的附近或者当出现一组不正确的随机粒子的权重大于真实位姿附近的粒子权重时，随着粒子的重采样过程，真实位姿附近的粒子将逐渐耗尽，定位误差也将逐渐增大。
2、粒子数的确定，AMCL使用有限粒子数来估计位姿的后验分布，在对大规模地图进行全局定位中，当粒子数较低时，无法正确的估计位姿的后验分布，当粒子数较高时，将增加定位的时间。
3、定位收敛时间较慢，AMCL需要经过多次的激光雷达的观测信息来更新粒子权重，从而使估算粒子的位姿接近真实位姿。
发明内容
针对现有技术中的上述不足，本发明提供的基于自适应蒙特卡洛和特征匹配的全局融合定位方法解决了现有的自适应蒙特卡洛的全局定位算法存在的缺点。
为了达到上述发明目的，本发明采用的技术方案为：
提供一种基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其包括：
S1、获取机器人在检测环境行走时生成的栅格地图，并提取栅格地图中的地图线特征存储至地图特征数据库中；
S2、当不存在机器人的激光雷达扫描图和里程计数据输入时，退出全局融合定位；
S3、当存在机器人的激光雷达扫描图和里程计数据输入时，进入步骤S4；
S4、判断当前激光雷达扫描图是否为第一帧、第二帧或余下帧，若是第一帧进入步骤S5，若是第二帧进入步骤S9，若是余下帧进入步骤S12；
S5、提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中，查找地图特征数据库与地图特征模板中长度差小于长度阈值的地图线特征和扫描图线特征；
S6、计算长度差小于长度阈值的地图线特征和扫描图线特征重合时，激光雷达坐标系与栅格地图坐标系的转换关系，并利用转换关系平移和旋转激光雷达坐标系中原点计算得到候选位姿；
S7、计算候选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离的累加作为候选位姿的匹配拟合度，并将匹配拟合度最小的候选位姿作为估算位姿；
S8、采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集χt位姿初始化，其中粒子用于评估机器人的位姿，之后返回步骤S4；
S9、根据机器人的里程计数据和运动模型，更新粒子的位姿；根据激光雷达扫描图和测量模型，计算粒子的权重；
S10、根据粒子集χt中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，之后计算粒子权重的平均值、长期均值、短期均值及长期均值与短期均值的相对差值；
S11、根据相对差值与零的大小关系，将粒子集χt划分成粒子集
Figure BDA0001981243950000031
和粒子集
Figure BDA0001981243950000032
并对粒子集
Figure BDA0001981243950000033
进行重采样，之后返回步骤S4；
S12、采用步骤S9更新粒子集
Figure BDA0001981243950000034
中粒子的位姿和权重；
S13、根据当前激光雷达扫描图执行步骤S5至步骤S7，并采用得到的估算位姿更新粒子集
Figure BDA0001981243950000035
中粒子的位姿，之后采用当前激光雷达扫描图和测量模型，计算粒子集
Figure BDA0001981243950000036
中粒子的权重；
S14、采用更新了权重和位姿的粒子集
Figure BDA0001981243950000037
和粒子集
Figure BDA0001981243950000038
合并的新粒子集更新粒子集χt，之后返回步骤S10。
进一步地，所述根据相对差值与零的大小关系，将粒子集χt划分成粒子集
Figure BDA0001981243950000039
和粒子集
Figure BDA00019812439500000310
进一步包括：
判断相对差值是否大于零；
若是，则令max＝相对差值；否则令max＝零；
根据粒子集χt中的总粒子数量M和max，划分粒子集
Figure BDA0001981243950000041
的粒子总数为M·(1-max)，粒子集
Figure BDA0001981243950000042
的粒子总数为M·max。
进一步地，所述提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中进一步包括：
计算当前激光雷达扫描图中相邻数据点之间的距离；
判断所述距离是否大于预设阈值；
若是，则将两个数据点划分至不同的区域；否则，将两个数据点划分至同一区域；
采用分割-合并算法提取每个区域覆盖的激光雷达扫描图中的扫描图线特征存储至地图特征模板中。
进一步地，计算候选位姿的匹配拟合度时，全局扫描数据点的获取方法为：
根据候选位姿表征的在重合过程中计算出来的从激光雷达坐标系转换为栅格地图坐标系的平移和旋转量，将激光雷达坐标系中的扫描数据点转换为栅格地图坐标系下的全局扫描数据点。
进一步地，每对长度差小于长度阈值的地图线特征和扫描图线特征重合时，存在四个候选位姿及四个相对应的平移和旋转。
进一步地，更新粒子的位姿的计算公式为：
Figure BDA0001981243950000043
其中，p(xt|xt-1,ut)为运动模型；ut为里程计数据；xt-1和xt分别为t-1时刻和t时刻的机器人在栅格地图坐标系下的位姿；
Figure BDA0001981243950000044
和
Figure BDA0001981243950000045
分别为t-1时刻和t时刻第m个粒子在栅格地图坐标系下的位姿。
进一步地，所述根据机器人的里程计数据和运动模型，更新粒子的位姿进一步包括：
A1、根据里程计数据
Figure BDA0001981243950000051
计算初次旋转δrot1、平移δtrans和第二次旋转δrot2：
Figure BDA0001981243950000052
Figure BDA0001981243950000053
Figure BDA0001981243950000054
其中，里程计数据ut表示从t-1时刻到t时刻机器人的相对运动；
Figure BDA0001981243950000055
为t时刻的机器人在其内部坐标系下的位姿，
Figure BDA0001981243950000056
为
Figure BDA0001981243950000057
的横坐标，
Figure BDA0001981243950000058
为
Figure BDA0001981243950000059
的纵坐标，
Figure BDA00019812439500000510
为
Figure BDA00019812439500000511
的航向角；
Figure BDA00019812439500000512
为t时刻的机器人在其内部坐标系下的位姿，
Figure BDA00019812439500000513
为
Figure BDA00019812439500000514
的横坐标；
Figure BDA00019812439500000515
为
Figure BDA00019812439500000516
的纵坐标；
Figure BDA00019812439500000517
为
Figure BDA00019812439500000518
的航向角；atan2(.)为反正切函数；
A2、对初次旋转δrot1、平移δtrans和第二次旋转δrot2进行高斯采样：
Figure BDA00019812439500000519
Figure BDA00019812439500000520
Figure BDA00019812439500000521
其中，sample(σ)表示均值为0，偏差为σ的高斯采样，αi(i＝1,…,4)表示给定的参数；
A3、根据上一时刻的位姿xt-1＝(x y θ)T和采样的旋转平移量
Figure BDA00019812439500000522
Figure BDA00019812439500000523
更新粒子的位姿xt＝(x′ y′ θ′)T：
Figure BDA00019812439500000524
Figure BDA00019812439500000525
Figure BDA00019812439500000526
其中，x′为更新后机器人的横坐标；y′为更新后机器人的纵坐标；θ′为更新后机器人的航向角。
进一步地，粒子的权重的计算公式为：
Figure BDA00019812439500000527
其中，p(zt|xt)为激光雷达的测量模型；xt为t时刻的机器人在栅格地图坐标系下的位姿；
Figure BDA0001981243950000061
为t时刻第m个粒子在栅格地图坐标系下的位姿；
Figure BDA0001981243950000062
为t时刻第m个粒子的权重；zt为t时刻激光雷达测量值。
进一步地，所述激光雷达的测量模型为激光雷达测量值
Figure BDA0001981243950000063
的局部测量误差phit与随机测量误差prand和的累乘，K为激光雷达测量数的总和，其计算步骤为：
B1、计算每个测量值的测量误差qk：
Figure BDA0001981243950000064
Figure BDA0001981243950000065
Figure BDA0001981243950000066
其中，zhit为局部测量误差权重参数；zrand为随机测量误差权重参数；
Figure BDA0001981243950000067
为激光雷达第k个测量数据；m为栅格地图信息；xt为机器人位姿；prand为随机测量误差；phit为局部测量误差；zmax为激光雷达最大测量距离；
Figure BDA0001981243950000068
为测量
Figure BDA0001981243950000069
的真实距离；
Figure BDA00019812439500000610
为具有均值为
Figure BDA00019812439500000611
标准偏差σhit的正态分布；
Figure BDA00019812439500000612
为归一化因子；zmax为激光雷达的最大测量值；
B2、计算所有测量值的测量误差p(zt|xt)：
Figure BDA00019812439500000613
进一步地，所述估计位姿的计算公式为：
Figure BDA00019812439500000614
其中，
Figure BDA00019812439500000615
为粒子归一化；
Figure BDA00019812439500000616
为机器人的估计位姿。
本发明的有益效果为：
1、具有较高的定位精度，特征匹配的度量采用全局扫描数据点到栅格地图中最近栅格占据点的距离的累加，与栅格地图特征更贴切，从而使得定位精度更准确。
2、使用粒子数目较少，当蒙特卡洛定位用作于位姿跟踪时，可以用比较少的粒子数完成定位，用作全局定位时，需要比较多的粒子数才能完成定位，由于本方案仅在位姿跟踪时采用蒙特卡洛定位，故使用较少的粒子数来完成位姿跟踪。
3、具有较快的收敛时间，先使用特征匹配完成全局定位，再进行位姿跟踪，使得在全局定位中只用少量的观测信息就能完成收敛，具有较快的收敛时间。
4、具有较低的经济成本，在全局定位时，仅需要传感器上的里程计数据和激光雷达测量数据，无需部署其他任何信标，从而减少系统部署成本。
附图说明
图1为现有技术基于自适应蒙特卡洛的全局定位算法的流程图。
图2为本方案的基于自适应蒙特卡洛和特征匹配的全局融合定位方法的流程图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
参考图2，图2示出了基于自适应蒙特卡洛和特征匹配的全局融合定位方法的流程图；如图2所示，该方法S包括步骤S1至步骤S14。
在步骤S1中，获取机器人在检测环境行走时生成的栅格地图，并提取栅格地图中的地图线特征存储至地图特征数据库中；步骤S1可以进一步细化为：
接收到栅格地图m后，首先将栅格地图m转换为二值图像，图像中的像素值即为栅格地图中的栅格值，之后采用LSD(Line Segment Detector)算法提取栅格地图的线特征，地图线特征是在全局地图坐标系(栅格地图坐标系)下，通过计算栅格地图中每条线段的斜率k，截距b，起点A坐标(x1,y1)，终点B坐标(x2,y2)，线段长度len五个参数来描述，地图特征数据库记录栅格地图中所有线特征。
在步骤S2中，当不存在机器人的激光雷达扫描图和里程计数据输入时，退出全局融合定位；在步骤S3中，当存在机器人的激光雷达扫描图和里程计数据输入时，进入步骤S4。
在步骤S4中，判断当前激光雷达扫描图是否为第一帧、第二帧或余下帧，若是第一帧进入步骤S5，若是第二帧进入步骤S9，若是余下帧进入步骤S12。
在步骤S5中，提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中，查找地图特征数据库与地图特征模板中长度差小于长度阈值的地图线特征和扫描图线特征。
在本发明的一个实施例中，所述提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中进一步包括：
计算当前激光雷达扫描图中相邻数据点之间的距离；
判断所述距离是否大于预设阈值；
若是，则将两个数据点划分至不同的区域；否则，将两个数据点划分至同一区域；
采用分割-合并算法提取每个区域覆盖的激光雷达扫描图中的扫描图线特征存储至地图特征模板中。
采用上述分成若干区域的方式进行扫描图线特征的提取，可以避免分开合并算法引入错误的线特征。
在步骤S6中，计算长度差小于长度阈值的地图线特征和扫描图线特征重合时，激光雷达坐标系与栅格地图坐标系的转换关系，并利用转换关系平移和旋转激光雷达坐标系中原点计算得到候选位姿。
下面以一个具体的实例对候选位姿的计算进行说明：
以地图特征模板中一条扫描图线特征lM中的线段长度len为标准，在地图特征数据库中寻找长度差小于阈值的地图线特征lD，通过平移和旋转激光雷达坐标系，使得扫描图线特征lM和地图线特征lD能够重合，而将激光雷达坐标系中原点通过上述的平移和旋转后即为候选位姿。
其中每对长度差小于长度阈值的地图线特征和扫描图线特征重合时，存在四个候选位姿及四个相对应的平移和旋转。下面继续以扫描图线特征lM和地图线特征lD为例对四个候选位姿及四个相对应的平移和旋转进行说明：
由于扫描图线特征lM和地图线特征lD的长度不完全相等，则存在四种重合情况，即
Figure BDA0001981243950000091
其中，AM，BM为扫描图线特征lM的起点和终点，AD，BD为地图线特征lD的起点和终点，
Figure BDA0001981243950000092
表示lM和lD重合时，起点AM与起点AD重合，
Figure BDA0001981243950000093
表示lM和lD重合时，起点AM与终点BD重合，
Figure BDA0001981243950000094
表示lM和lD重合时，终点BM与起点AD重合，
Figure BDA0001981243950000095
表示lM和lD重合时，终点BM与终点BD重合。因此，每对线特征lM和lD重合时，会有四个候选位姿
Figure BDA0001981243950000096
以及四个相应的平移和旋转。
在步骤S7中，计算候选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离的累加作为候选位姿的匹配拟合度，并将匹配拟合度最小的候选位姿作为估算位姿。
其中，选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离di的计算公式为：
Figure BDA0001981243950000101
匹配拟合度
Figure BDA0001981243950000102
的计算公式为：
Figure BDA0001981243950000103
估算位姿
Figure BDA0001981243950000104
为所有候选位姿集
Figure BDA0001981243950000105
中最小匹配拟合度的候选位姿：
Figure BDA0001981243950000106
其中，(x′,y′)为栅格地图m中占据点的位置，{(x′,y′)}为所有占据点位置的集合，di为全局扫描数据点i到最近栅格占据点的距离；
Figure BDA0001981243950000107
为候选位姿集
Figure BDA0001981243950000108
中的候选位姿，
Figure BDA0001981243950000109
为估算位姿。
实施时，本方案优选计算候选位姿的匹配拟合度时，全局扫描数据点的获取方法为：
根据候选位姿表征的在重合过程中计算出来的从激光雷达坐标系转换为栅格地图坐标系的平移和旋转量，将激光雷达坐标系中的扫描数据点转换为栅格地图坐标系下的全局扫描数据点。
在步骤S8中，采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集χt位姿初始化，之后返回步骤S4，其中粒子用于评估机器人的位姿；
在步骤S9中，根据机器人的里程计数据和运动模型，更新粒子的位姿；根据激光雷达扫描图和测量模型，计算粒子的权重。
在本发明的一个实施例中，更新粒子的位姿的计算公式为：
Figure BDA00019812439500001010
其中，p(xt|xt-1,ut)为运动模型；ut为里程计数据；xt-1和xt分别为t-1时刻和t时刻的机器人在栅格地图坐标系下的位姿；
Figure BDA0001981243950000111
和
Figure BDA0001981243950000112
分别为t-1时刻和t时刻第m个粒子在栅格地图坐标系下的位姿。
实施时，本方案优选所述根据机器人的里程计数据和运动模型，更新粒子的位姿进一步包括：
A1、根据里程计数据
Figure BDA0001981243950000113
计算初次旋转δrot1、平移δtrans和第二次旋转δrot2：
Figure BDA0001981243950000114
Figure BDA0001981243950000115
Figure BDA0001981243950000116
其中，里程计数据ut表示从t-1时刻到t时刻机器人的相对运动；
Figure BDA0001981243950000117
为t时刻的机器人在其内部坐标系下的位姿，
Figure BDA0001981243950000118
为
Figure BDA0001981243950000119
的横坐标，
Figure BDA00019812439500001110
为
Figure BDA00019812439500001111
的纵坐标，
Figure BDA00019812439500001112
为
Figure BDA00019812439500001113
的航向角；
Figure BDA00019812439500001114
为t时刻的机器人在其内部坐标系下的位姿，
Figure BDA00019812439500001115
为
Figure BDA00019812439500001116
的横坐标；
Figure BDA00019812439500001117
为
Figure BDA00019812439500001118
的纵坐标；
Figure BDA00019812439500001119
为
Figure BDA00019812439500001120
的航向角；atan2(.)为反正切函数；
A2、对初次旋转δrot1、平移δtrans和第二次旋转δrot2进行高斯采样：
Figure BDA00019812439500001121
Figure BDA00019812439500001122
Figure BDA00019812439500001123
其中，sample(σ)表示均值为0，偏差为σ的高斯采样，αi(i＝1,…,4)表示给定的参数；
A3、根据上一时刻的位姿xt-1＝(x y θ)T和采样的旋转平移量
Figure BDA00019812439500001124
Figure BDA00019812439500001125
更新粒子的位姿xt＝(x′ y′ θ′)T：
Figure BDA0001981243950000121
Figure BDA0001981243950000122
Figure BDA0001981243950000123
其中，x′为更新后机器人的横坐标；y′为更新后机器人的纵坐标；θ′为更新后机器人的航向角。
在本发明的一个实施例中，，粒子的权重的计算公式为：
Figure BDA0001981243950000124
其中，p(zt|xt)为激光雷达的测量模型；xt为t时刻的机器人在栅格地图坐标系下的位姿；
Figure BDA0001981243950000125
为t时刻第m个粒子在栅格地图坐标系下的位姿；
Figure BDA0001981243950000126
为t时刻第m个粒子的权重；zt为t时刻激光雷达测量值。
实施时，本方案优选所述激光雷达的测量模型为激光雷达测量值
Figure BDA0001981243950000127
的局部测量误差phit与随机测量误差prand和的累乘，K为激光雷达测量数的总和，其计算步骤为：
B1、计算每个测量值的测量误差qk：
Figure BDA0001981243950000128
Figure BDA0001981243950000129
Figure BDA00019812439500001210
其中，zhit为局部测量误差权重参数；zrand为随机测量误差权重参数；
Figure BDA00019812439500001211
为激光雷达第k个测量数据；m为栅格地图信息；xt为机器人位姿；prand为随机测量误差；phit为局部测量误差；zmax为激光雷达最大测量距离；
Figure BDA00019812439500001212
为测量
Figure BDA00019812439500001213
的真实距离；
Figure BDA00019812439500001214
为具有均值为
Figure BDA00019812439500001215
标准偏差σhit的正态分布；
Figure BDA0001981243950000131
为归一化因子；zmax为激光雷达的最大测量值；
B2、计算所有测量值的测量误差p(zt|xt)：
Figure BDA0001981243950000132
在步骤S10中，根据粒子集χt中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，之后计算粒子权重的平均值、长期均值、短期均值及长期均值与短期均值的相对差值。
其中，所述估计位姿的计算公式为：
Figure BDA0001981243950000133
其中，
Figure BDA0001981243950000134
为粒子归一化；
Figure BDA0001981243950000135
为机器人的估计位姿。
实施时，本方案优选粒子权重的平均值wavg，长期均值wslow，短期均值wfast以及长期均值与短期均值的相对差值wdiff的计算公式分别为：
Figure BDA0001981243950000136
wslow＝wslow+αslow(wavg-wslow)
wfast＝wfast+αfast(wavg-wfast)
Figure BDA0001981243950000137
其中，M为粒子集χt的总数，0≤αfast＜＜αslow，αfast，αslow分别为短期均值和长期均值的衰减率。
在步骤S11中，根据相对差值与零的大小关系，将粒子集χt划分成粒子集
Figure BDA0001981243950000138
和粒子集
Figure BDA0001981243950000139
并对粒子集
Figure BDA00019812439500001310
进行重采样，之后返回步骤S4。
在本发明的一个实施例中，所述根据相对差值与零的大小关系，将粒子集χt划分成粒子集
Figure BDA00019812439500001311
和粒子集
Figure BDA00019812439500001312
进一步包括：
判断相对差值是否大于零；
若是，则令max＝相对差值；否则令max＝零；
根据粒子集χt中的总粒子数量M和max，划分粒子集
Figure BDA0001981243950000141
的粒子总数为M·(1-max)，粒子集
Figure BDA0001981243950000142
的粒子总数为M·max。
在步骤S12中，采用步骤S9更新粒子集
Figure BDA0001981243950000143
中粒子的位姿和权重；
在步骤S13中，根据当前激光雷达扫描图执行步骤S5至步骤S7，并采用得到的估算位姿更新粒子集
Figure BDA0001981243950000144
中粒子的位姿，之后采用当前激光雷达扫描图和测量模型，计算粒子集
Figure BDA0001981243950000145
中粒子的权重；
在步骤S14中，采用更新了权重和位姿的粒子集
Figure BDA0001981243950000146
和粒子集
Figure BDA0001981243950000147
合并的新粒子集更新粒子集χt，之后返回步骤S10。
综上所述，本方案采用MCL与特征匹配相联合进行机器人全局定位，首先先使用特征匹配完成全局定位，再使用MCL完成位姿跟踪，通过该种方式具有定位精准、采用粒子数量少、收敛时间快和经济成本低等优点。

Claims (9)

1.基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其特征在于，包括：
S1、获取机器人在检测环境行走时生成的栅格地图，并提取栅格地图中的地图线特征存储至地图特征数据库中；
S2、当不存在机器人的激光雷达扫描图和里程计数据输入时，退出全局融合定位；
S3、当存在机器人的激光雷达扫描图和里程计数据输入时，进入步骤S4；
S4、判断当前激光雷达扫描图是否为第一帧、第二帧或余下帧，若是第一帧进入步骤S5，若是第二帧进入步骤S9，若是余下帧进入步骤S12；
S5、提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中，查找地图特征数据库与地图特征模板中长度差小于长度阈值的地图线特征和扫描图线特征；
S6、计算长度差小于长度阈值的地图线特征和扫描图线特征重合时，激光雷达坐标系与栅格地图坐标系的转换关系，并利用转换关系平移和旋转激光雷达坐标系中原点计算得到候选位姿；
S7、计算候选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离的累加作为候选位姿的匹配拟合度，并将匹配拟合度最小的候选位姿作为估算位姿；
S8、采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集χt位姿初始化，其中粒子用于评估机器人的位姿，之后返回步骤S4；
S9、根据机器人的里程计数据和运动模型，更新粒子的位姿；根据激光雷达扫描图和测量模型，计算粒子的权重；
S10、根据粒子集χt中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，之后计算粒子权重的平均值、长期均值、短期均值及长期均值与短期均值的相对差值；
S11、根据相对差值与零的大小关系，将粒子集χt划分成粒子集
Figure FDA0002594902820000021
和粒子集
Figure FDA0002594902820000022
并对粒子集
Figure FDA0002594902820000023
进行重采样，之后返回步骤S4；
S12、采用步骤S9更新粒子集
Figure FDA0002594902820000024
中粒子的位姿和权重；
S13、根据当前激光雷达扫描图执行步骤S5至步骤S7，并采用得到的估算位姿更新粒子集
Figure FDA0002594902820000025
中粒子的位姿，之后采用当前激光雷达扫描图和测量模型，计算粒子集
Figure FDA0002594902820000026
中粒子的权重；
S14、采用更新了权重和位姿的粒子集
Figure FDA0002594902820000027
和粒子集
Figure FDA0002594902820000028
合并的新粒子集更新粒子集χt，之后返回步骤S10；
所述根据相对差值与零的大小关系，将粒子集χt划分成粒子集
Figure FDA0002594902820000029
和粒子集
Figure FDA00025949028200000210
进一步包括：
判断相对差值是否大于零；
若是，则令max＝相对差值；否则令max＝零；
根据粒子集χt中的总粒子数量M和max，划分粒子集
Figure FDA00025949028200000211
的粒子总数为M·(1-max)，粒子集
Figure FDA00025949028200000212
的粒子总数为M·max。
2.根据权利要求1所述的全局融合定位方法，其特征在于，所述提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中进一步包括：
计算当前激光雷达扫描图中相邻数据点之间的距离；
判断所述距离是否大于预设阈值；
若是，则将两个数据点划分至不同的区域；否则，将两个数据点划分至同一区域；
采用分割-合并算法提取每个区域覆盖的激光雷达扫描图中的扫描图线特征存储至地图特征模板中。
3.根据权利要求1所述的全局融合定位方法，其特征在于，计算候选位姿的匹配拟合度时，全局扫描数据点的获取方法为：
根据候选位姿表征的在重合过程中计算出来的从激光雷达坐标系转换为栅格地图坐标系的平移和旋转量，将激光雷达坐标系中的扫描数据点转换为栅格地图坐标系下的全局扫描数据点。
4.根据权利要求1所述的全局融合定位方法，其特征在于，每对长度差小于长度阈值的地图线特征和扫描图线特征重合时，存在四个候选位姿及四个相对应的平移和旋转。
5.根据权利要求1所述的全局融合定位方法，其特征在于，更新粒子的位姿的计算公式为：
Figure FDA0002594902820000031
其中，
Figure FDA0002594902820000032
为运动模型；ut为里程计数据；xt-1和xt分别为t-1时刻和t时刻的机器人在栅格地图坐标系下的位姿；
Figure FDA0002594902820000033
和
Figure FDA0002594902820000034
分别为t-1时刻和t时刻第m个粒子在栅格地图坐标系下的位姿。
6.根据权利要求5所述的全局融合定位方法，其特征在于，所述根据机器人的里程计数据和运动模型，更新粒子的位姿进一步包括：
A1、根据里程计数据
Figure FDA0002594902820000035
计算初次旋转δrot1、平移δtrans和第二次旋转δrot2：
Figure FDA0002594902820000036
Figure FDA0002594902820000037
Figure FDA0002594902820000038
其中，里程计数据ut表示从t-1时刻到t时刻机器人的相对运动；
Figure FDA0002594902820000039
为t-1时刻的机器人在其内部坐标系下的位姿，
Figure FDA0002594902820000041
为
Figure FDA0002594902820000042
的横坐标，
Figure FDA0002594902820000043
为
Figure FDA0002594902820000044
的纵坐标，
Figure FDA0002594902820000045
为
Figure FDA0002594902820000046
的航向角；
Figure FDA0002594902820000047
为t时刻的机器人在其内部坐标系下的位姿，
Figure FDA0002594902820000048
为
Figure FDA0002594902820000049
的横坐标；
Figure FDA00025949028200000410
为
Figure FDA00025949028200000411
的纵坐标；
Figure FDA00025949028200000412
为
Figure FDA00025949028200000413
的航向角；atan2(.)为反正切函数；
A2、对初次旋转δrot1、平移δtrans和第二次旋转δrot2进行高斯采样：
Figure FDA00025949028200000414
Figure FDA00025949028200000415
Figure FDA00025949028200000416
其中，sample(σ)表示均值为0，偏差为σ的高斯采样，α1、α2、α3、α4均为给定的参数；
A3、根据上一时刻的位姿xt-1＝(x y θ)T和采样的旋转平移量
Figure FDA00025949028200000417
Figure FDA00025949028200000418
更新粒子的位姿xt＝(x′ y′ θ′)T：
Figure FDA00025949028200000419
Figure FDA00025949028200000420
Figure FDA00025949028200000421
其中，x′为更新后机器人的横坐标；y′为更新后机器人的纵坐标；θ′为更新后机器人的航向角。
7.根据权利要求1所述的全局融合定位方法，其特征在于，粒子的权重的计算公式为：
Figure FDA00025949028200000422
其中，
Figure FDA00025949028200000423
为t时刻激光雷达所有测量值的测量误差；
Figure FDA00025949028200000424
为t时刻第m个粒子在栅格地图坐标系下的位姿；
Figure FDA00025949028200000425
为t时刻第m个粒子的权重；zt为t时刻激光雷达测量值。
8.根据权利要求7所述的全局融合定位方法，其特征在于，所述激光雷达的p(zt|xt)为激光雷达测量值
Figure FDA00025949028200000426
的局部测量误差phit与随机测量误差prand和的累乘，K为激光雷达测量数的总和，其计算步骤为：
B1、计算每个测量值的测量误差qk：
Figure FDA0002594902820000051
Figure FDA0002594902820000052
Figure FDA0002594902820000053
其中，zhit为局部测量误差权重参数；zrand为随机测量误差权重参数；
Figure FDA0002594902820000054
为激光雷达第k个测量数据，k＝1,…,K；m'为栅格地图信息；prand为随机测量误差；phit为局部测量误差；zmax为激光雷达最大测量距离；
Figure FDA0002594902820000055
为测量
Figure FDA0002594902820000056
的真实距离；
Figure FDA0002594902820000057
为具有均值为
Figure FDA0002594902820000058
标准偏差σh的正态分布；
Figure FDA0002594902820000059
为归一化因子；zmax为激光雷达的最大测量值；
B2、计算所有测量值的测量误差p(zt|xt)：
Figure FDA00025949028200000510
9.根据权利要求5-8任一所述的全局融合定位方法，其特征在于，所述估计位姿的计算公式为：
Figure FDA00025949028200000511
其中，
Figure FDA00025949028200000512
为粒子归一化。
CN201910149967.9A 2019-02-28 2019-02-28 基于自适应蒙特卡洛和特征匹配的全局融合定位方法 Active CN109682382B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910149967.9A CN109682382B (zh) 2019-02-28 2019-02-28 基于自适应蒙特卡洛和特征匹配的全局融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910149967.9A CN109682382B (zh) 2019-02-28 2019-02-28 基于自适应蒙特卡洛和特征匹配的全局融合定位方法

Publications (2)

Publication Number Publication Date
CN109682382A CN109682382A (zh) 2019-04-26
CN109682382B true CN109682382B (zh) 2020-09-08

Family

ID=66197011

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910149967.9A Active CN109682382B (zh) 2019-02-28 2019-02-28 基于自适应蒙特卡洛和特征匹配的全局融合定位方法

Country Status (1)

Country Link
CN (1) CN109682382B (zh)

Families Citing this family (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110208783B (zh) * 2019-05-21 2021-05-14 同济人工智能研究院(苏州)有限公司 基于环境轮廓的智能车辆定位方法
CN110412596A (zh) * 2019-07-10 2019-11-05 上海电机学院 一种基于图像信息和激光点云的机器人定位方法
CN110686676A (zh) * 2019-09-12 2020-01-14 深圳市银星智能科技股份有限公司 机器人重定位方法、装置及机器人
CN112781591A (zh) * 2019-11-06 2021-05-11 深圳市优必选科技股份有限公司 机器人定位方法、装置、计算机可读存储介质及机器人
CN110888125B (zh) * 2019-12-05 2020-06-19 奥特酷智能科技(南京)有限公司 一种基于毫米波雷达的自动驾驶车辆定位方法
CN110986956B (zh) * 2019-12-23 2023-06-30 苏州寻迹智行机器人技术有限公司 一种基于改进的蒙特卡洛算法的自主学习全局定位方法
CN111421548B (zh) * 2020-04-21 2021-10-19 武汉理工大学 一种移动机器人定位方法及系统
CN111754566A (zh) * 2020-05-12 2020-10-09 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 机器人场景定位方法和施工操作方法
CN111578958A (zh) * 2020-05-19 2020-08-25 山东金惠新达智能制造科技有限公司 移动机器人导航实时定位方法、系统、介质及电子设备
CN111895990B (zh) * 2020-07-14 2022-04-15 武汉科技大学 一种基于多指标绑架检测及移动机器人重定位的方法
CN111830985A (zh) * 2020-07-24 2020-10-27 中南大学 一种多机器人定位方法、系统及集中式通信系统
CN112325871A (zh) * 2020-09-16 2021-02-05 安徽意欧斯物流机器人有限公司 一种自动导引运输车的激光导航方法
CN112268561A (zh) * 2020-10-12 2021-01-26 西北工业大学 一种融合磁场信息的机器人蒙特卡罗定位方法
CN112344966B (zh) * 2020-11-24 2023-06-16 深兰科技（上海）有限公司 一种定位失效检测方法、装置、存储介质及电子设备
CN112750161B (zh) * 2020-12-22 2023-11-03 苏州大学 用于移动机器人的地图更新方法
CN112612862B (zh) * 2020-12-24 2022-06-24 哈尔滨工业大学芜湖机器人产业技术研究院 一种基于点云配准的栅格地图定位方法
CN112987027B (zh) * 2021-01-20 2024-03-15 长沙海格北斗信息技术有限公司 一种基于高斯模型的amcl算法的定位方法及存储介质
CN112909576B (zh) * 2021-02-02 2022-04-15 西安电子科技大学 分布式大型相控阵天线的平面度控制方法、装置及应用
CN113376650B (zh) * 2021-08-09 2021-11-23 浙江华睿科技股份有限公司 移动机器人定位方法及装置、电子设备及存储介质
CN113932790A (zh) * 2021-09-01 2022-01-14 北京迈格威科技有限公司 一种地图更新方法、装置、系统、电子设备及存储介质
CN114199251B (zh) * 2021-12-03 2023-09-15 江苏集萃智能制造技术研究所有限公司 一种机器人的防碰撞定位方法
CN115014352B (zh) * 2022-06-01 2023-05-26 浙江大学 一种基于建议分布地图的室内全局定位方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101625572A (zh) * 2009-08-10 2010-01-13 浙江大学 基于改进重采样方法和粒子选取的FastSLAM算法
CN102818567A (zh) * 2012-08-08 2012-12-12 浙江大学 集合卡尔曼滤波－粒子滤波相结合的auv组合导航方法
CN107390681A (zh) * 2017-06-21 2017-11-24 华南理工大学 一种基于激光雷达与地图匹配的移动机器人实时定位方法
CN108458715A (zh) * 2018-01-18 2018-08-28 亿嘉和科技股份有限公司 一种基于激光地图的机器人定位初始化方法
CN109144056A (zh) * 2018-08-02 2019-01-04 上海思岚科技有限公司 移动机器人的全局自定位方法及设备

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9037396B2 (en) * 2013-05-23 2015-05-19 Irobot Corporation Simultaneous localization and mapping for a mobile robot
CN104180799A (zh) * 2014-07-15 2014-12-03 东北大学 一种基于自适应蒙特卡罗定位的机器人定位方法
CN105509755B (zh) * 2015-11-27 2018-10-12 重庆邮电大学 一种基于高斯分布的移动机器人同步定位与地图构建方法
CN107289951B (zh) * 2017-07-31 2020-05-12 电子科技大学 一种基于惯性导航的室内移动机器人定位方法
CN107314773B (zh) * 2017-08-18 2019-10-01 广东宝乐机器人股份有限公司 移动机器人的地图创建方法及基于该地图的路径规划方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101625572A (zh) * 2009-08-10 2010-01-13 浙江大学 基于改进重采样方法和粒子选取的FastSLAM算法
CN102818567A (zh) * 2012-08-08 2012-12-12 浙江大学 集合卡尔曼滤波－粒子滤波相结合的auv组合导航方法
CN107390681A (zh) * 2017-06-21 2017-11-24 华南理工大学 一种基于激光雷达与地图匹配的移动机器人实时定位方法
CN108458715A (zh) * 2018-01-18 2018-08-28 亿嘉和科技股份有限公司 一种基于激光地图的机器人定位初始化方法
CN109144056A (zh) * 2018-08-02 2019-01-04 上海思岚科技有限公司 移动机器人的全局自定位方法及设备

Also Published As

Publication number Publication date
CN109682382A (zh) 2019-04-26

Similar Documents

Publication Publication Date Title
CN109682382B (zh) 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
KR102210715B1 (ko) 도로 중의 차도선을 확정하기 위한 방법, 장치 및 기기
Doherty et al. Multimodal semantic slam with probabilistic data association
Wilbers et al. Localization with sliding window factor graphs on third-party maps for automated driving
US11790542B2 (en) Mapping and localization system for autonomous vehicles
CN110132284B (zh) 一种基于深度信息的全局定位方法
CN108021886B (zh) 一种无人机重复纹理影像局部显著特征点匹配方法
CN111578946A (zh) 一种基于深度学习的激光导航agv重定位方法和装置
CN113822996B (zh) 机器人的位姿估计方法及装置、电子设备、存储介质
CN113759928B (zh) 用于复杂大尺度室内场景的移动机器人高精度定位方法
CN113781563B (zh) 一种基于深度学习的移动机器人回环检测方法
Christensen et al. 3-d modelling and robot localization from visual and range data in natural scenes
CN114046790A (zh) 一种因子图双重回环的检测方法
CN113532438A (zh) 一种大初始定位误差下的改进iccp地形匹配方法
CN116385866B (zh) 基于sar图像的铁路沿线彩钢房变化检测方法和装置
CN112652003A (zh) 一种基于ransac测度优化的三维点云配准方法
CN114374931B (zh) 基于近邻成分分析的度量学习的指纹定位方法
CN115239902A (zh) 移动设备的周边地图建立方法、装置、设备及存储介质
CN112305558B (zh) 一种利用激光点云数据的移动机器人轨迹确定方法及装置
Kumar et al. An efficient method for road tracking from satellite images using hybrid multi-kernel partial least square analysis and particle filter
CN114488247A (zh) 一种基于高精度北斗差分定位分析装备机动能力的方法
CN114943741A (zh) 一种动态场景下基于目标检测和几何概率的视觉slam方法
CN113484843A (zh) 一种激光雷达与组合导航间外参数的确定方法及装置
CN111832548A (zh) 一种列车定位方法
CN111275748A (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