CN113759928B - 用于复杂大尺度室内场景的移动机器人高精度定位方法 - Google Patents
用于复杂大尺度室内场景的移动机器人高精度定位方法 Download PDFInfo
- Publication number
- CN113759928B CN113759928B CN202111104812.7A CN202111104812A CN113759928B CN 113759928 B CN113759928 B CN 113759928B CN 202111104812 A CN202111104812 A CN 202111104812A CN 113759928 B CN113759928 B CN 113759928B
- Authority
- CN
- China
- Prior art keywords
- positioning
- robot
- laser
- pose
- value
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 238000001514 detection method Methods 0.000 claims abstract description 24
- 238000009826 distribution Methods 0.000 claims abstract description 17
- 238000012545 processing Methods 0.000 claims abstract description 14
- 230000000694 effects Effects 0.000 claims description 17
- 230000009466 transformation Effects 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000013441 quality evaluation Methods 0.000 claims description 7
- 238000002156 mixing Methods 0.000 claims description 5
- 238000002310 reflectometry Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 238000009499 grossing Methods 0.000 claims description 4
- 238000001303 quality assessment method Methods 0.000 claims description 4
- 238000013519 translation Methods 0.000 claims description 4
- 238000012163 sequencing technique Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 239000002245 particle Substances 0.000 abstract description 13
- 238000001914 filtration Methods 0.000 description 15
- 238000010586 diagram Methods 0.000 description 9
- 230000000007 visual effect Effects 0.000 description 8
- 238000011156 evaluation Methods 0.000 description 4
- 230000007246 mechanism Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 230000015556 catabolic process Effects 0.000 description 2
- 238000006731 degradation reaction Methods 0.000 description 2
- 239000011521 glass Substances 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000008713 feedback mechanism Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 239000002184 metal Substances 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Optics & Photonics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种用于复杂大尺度室内场景的移动机器人高精度定位方法,通过对全局地图进行降采样处理组成全局地图金字塔,在接收到最新激光点时,从位于金字塔最顶部的地图开始进行机器人位姿得分的检索,得到机器人的全局位姿;通过对人工路标进行检测和识别,得到环境中的人工路标用于移动机器人定位,当检测到少于三个人工路标时,构建人工路标的观测模型更新粒子分布,当检测到多于三个人工路标时,利用高精度定位方法对人工路标进行处理,实现移动机器人在目标区域的高精度定位,最后通过定位质量检测模块实现移动机器人定位质量的评估,通过反馈策略实现定位方法的鲁棒性;可以在目标区域实现毫米级定位,能够满足工业场景的精度需求。
Description
技术领域
本发明属于机器人定位技术领域,具体涉及一种用于复杂大尺度室内场景的移动机器人高精度定位方法。
背景技术
移动机器人定位是移动机器人长期自主导航过程中一个重要部分,在很多应用中是最为基础的问题,比如在服务机器人、物流机器人、工业AGV(Automated GuidedVehicle)、自动驾驶等领域。移动机器人定位问题主要分为全局重定位和位姿跟踪。全局定位主要是通过对全局地图的搜索实现移动机器人的位姿校正,尤其是对于移动机器人上电启动或者对于移动机器人绑架问题,全局定位对于保证***鲁棒运行非常有意义。位姿跟踪问题主要解决的是动态环境下的鲁棒定位问题和目标点高精度定位问题。比如对于工业物流机器人,为了能够精准地运动到上货区,并且实现准确地完成物体的托运,需要实现小于1cm的定位误差。对于工厂的动态环境,由于货架的变动与更新,会使环境发生较大的变化,为了移动机器人在这种环境下正常工作,需要保证移动机器人鲁棒定位。因此,对于室内移动机器人来说,尤其是工业AGV来说,从全局定位到高精度鲁棒定位的整个定位过程都是非常重要的。
传统的全局定位问题的研究中,GPS经常被用来提供全局位置,然而GPS在密闭空间和室内环境会失效。在GPS失效的室内场景下,全局定位主要是通过对全局地图数据或者全局地图的特征进行检索来实现移动机器人的全局定位。近年来,有很多基于视觉信息的位置识别方法来实现机器人全局定位,视觉可以获取到环境中非常丰富的信息。如中国专利“CN110533722A一种基于视觉词典的机器人快速重定位方法及***”提出了一种使用图像与视觉地图中保存的关键帧进行对比,找出最接近的相似关键帧。然后将当前图像帧与相似关键帧进行特征匹配,得出当前图像帧位姿相对于相似关键帧的位姿关系。最后根据相似关键帧在激光地图中的位姿以及当前图像帧相对于相似关键帧的位姿关系,得到当前机器人的位姿信息来实现机器人重定位。所述的技术方法中主要使用视觉来实现移动机器人位姿重定位。视觉图像可以获取到环境中非常丰富的信息,所以可以实现很好的机器人重定位效果。然而,在频繁变化的工厂环境或者视觉退化场景,如光照变化,会对移动机器人的位置识别或者全局定位产生致命的影响,这就使得基于视觉信息无法实现移动机器人定位。
移动机器人位姿跟踪问题的研究中,主要面临的挑战有动态环境下鲁棒定位、目标点高精度定位。如中国专利“CN112068547A一种基于AMCL的机器人定位方法、装置和机器人”提供了一种基于粒子滤波的AMCL定位方法。该方法利用激光数据与地图文件,对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理,得到更为集中的粒子分布,并采用AMCL算法对优化后的粒子群进行处理,实现机器人的实时定位。所述的技术方法中使用粒子滤波方法实现移动机器人定位,由于粒子滤波使用粒子作为位姿的假设,可以实现很高的鲁棒性,但只能实现厘米级定位精度。然而在现代工业场景下,移动机器人定位达到这样的定位精度还远远不够,工业场景要求毫米级定位才能保证安全的工业生产。
发明内容
基于上述问题,本发明提供一种用于复杂大尺度室内场景的移动机器人高精度定位方法,包括:
步骤1:将机器人获取的原始全局地图进行降采样处理并组合成金字塔形式的全局地图;
步骤2:根据采集到的当前时刻的激光点,从金字塔全局地图最顶层的最低分辨率开始进行机器人位姿得分的搜索实现全局定位;
步骤3:设置人工路标优化机器人的当前位姿;
步骤4:构建定位质量评估函数进行定位效果判断,保证机器人在前进过程中的定位精度。
所述步骤1包括:
步骤1.1:采用分辨率为3*3的滑动窗口对原始全局地图进行滑动降采样;
步骤1.2:通过比较3*3滑动窗口中各个单元格得分值px获得最小得分值;
式中,zhit、zrand表示加权的混合权值,di表示当前位姿下第i个激光点在全局地图中的坐标与地图中最近被占用区域之间的距离,如果是空闲区域或者是未知区域,那么di被设置为固定值maxdist,如果是被占用区域,则di被设置为0,prand表示随机噪声,σ表示设置的激光的方差;
步骤1.3:将全局地图进行2倍率的降采样处理,得到不同分辨率下的全局地图,直到得到地图分辨率为1*1停止降采样处理;
步骤1.4:将每一分辨率下的全局地图按照分辨率从高到低的顺序组合为金字塔形式,得到金字塔形式的全局地图。
所述步骤2包括:
步骤2.1:当机器人接收到当前时刻的激光点时,计算当前位姿下当前帧激光点与全局地图中每个像素点对应的概率得分值p:
式中,Nscan表示当前帧激光点的总数,Ninliner表示在全局地图中有最近点对应的激光点数目,k1、k2表示加权因子;
步骤2.2:将所有可能的位姿的概率得分值从大到小进行排序,将概率得分值最小的位姿作为全局定位位姿。
所述步骤3包括:
步骤3.1:在室内的机器人行进路线两旁间隔设置反光柱作为人工路标来辅助机器人进行定位;
步骤3.2:通过激光雷达检测第i个反光柱距离激光雷达的距离Di和方位角
式中,表示序号为/>的激光点测量得到的距离,/>表示序号为的激光点测量得到的距离,/>表示序号为/>的激光点测量得到的距离,表示序号为/>的激光点测量得到的角度,/>表示序号为/>的激光点测量得到的角度,/>表示序号为/>的激光点测量得到的角度,r表示人工路标的半径,m表示人工路标收到的激光点起始序号,n表示人工路标收到的激光点结束序号;
步骤3.3:对设置的人工路标进行成对一致性检验;
步骤3.4:当检测到人工路标的个数少于3时,通过建立观测模型更新移动机器人当前位姿的概率分布得到最优当前位姿;
步骤3.5:当检测到人工路标的个数大于等于3时,根据奇异值分解方法确定机器人精确的当前位姿。
所述步骤3.3包括:
步骤3.3.1:对于采集到的反射率大于一定阈值的激光点,根据相邻激光点之间的距离进行聚类,找到当前机器人检测到的N个目标对象;
步骤3.3.2:统计每个目标对象对应的激光点数g1;
步骤3.3.3:计算当前距离检测到的目标对象的理论角度θ:
步骤3.3.4:根据理论角度θ计算每个目标对象理论上对应的激光点数g2;
g2=θ/R (6)
式中R表示激光分辨率;
步骤3.3.5:计算目标对象与最近的人工路标之间的实际距离s;
步骤3.3.6:如果激光点数g1与g2之间的差值小于设定阈值g',且距离s小于设定阈值s'则说明检测到的目标对象是人工路标;否则认为是误检测。
所述步骤3.4包括:
步骤3.4.1:建立检测到的人工路标的观测模型Pp:
式中,σ1表示坐标值x的标准差,σ2表示坐标值y的标准差,ρ表示x,y之间的协方差,(x,y)表示人工路标的坐标值,μ1表示坐标值x的真实值,μ2表示坐标值y的真实值;
步骤3.4.2:根据观测模型Pp更新移动机器人当前位姿的概率分布。
所述步骤3.5包括:
步骤3.5.1:计算第i个人工路标的真实坐标值与实时检测到的坐标值之间的误差项ei:
式中,R、t表示待求解的欧式变换中的旋转矩阵、平移矩阵,表示人工路标的真实坐标值,pi表示实时检测到的人工路标的坐标值;
步骤3.5.2:构建最小二乘函数当J取最小值时得到R、t的具体值,其中,n′表示人工路标的个数;
步骤3.5.3:根据R、t的具体值生成机器人精确的当前位姿xi':
x'i=Rxi+t (9)
式中,xi表示机器人的当前位姿;
步骤3.5.4:利用高斯滤波对当前位姿x'i进行平滑处理得到最终精确的当前位姿。
所述步骤4包括:
步骤4.1:对每个激光点在全局地图中以一定半径进行搜索,统计在全局地图中能够搜索到的关键点数量Numkeypoint;
步骤4.2:如果Numkeypoint>1则将该激光点对应的定位质量评估得分pi'取正值;否则将pi'取为0;
步骤4.3:累计计算所有激光点的定位质量评估得分:
式中,Pp表示所有激光点的定位质量评估总分值;
步骤4.4:如果Pp小于设定阈值则说明当前机器人的定位效果差需要重复步骤2进行重定位过程,如果Pp大于等于设定阈值/>则说明当前机器人的定位效果好不需要进行重定位过程。
本发明的有益效果是:
本发明提出了一种用于复杂大尺度室内场景的移动机器人高精度定位方法,以减少环境变化对***精度的影响,使得在频繁变化的工厂环境或者视觉退化场景依旧能够实现鲁棒定位的效果;
1)本发明通过对原始全局地图进行降采样处理,得到不同分辨率的地图,并组成了全局地图金字塔;
2)本发明通过构建得分函数对所有可能的位姿的概率得分值从大到小进行排序,实现了移动机器人全局定位;
3)本发明通过检测人工路标实现了机器人的高精度鲁棒定位;当检测到人工路标的个数少于3时,通过建立观测模型更新移动机器人当前位姿的概率分布得到最优当前位姿;当检测到人工路标的个数大于等于3时,根据奇异值分解方法确定机器人精确的位姿;
4)本发明通过求解人工路标的真实坐标值与实时检测到的坐标值之间的变换矩阵,并将变换矩阵作用到当前的机器人位姿,实现了更精确的位姿;
本发明方法可以在目标区域实现毫米级定位,能够满足工业场景的精度需求。
附图说明
图1为本发明中用于复杂大尺度室内场景的移动机器人高精度定位方法流程图;
图2为本发明中用于复杂大尺度室内场景的移动机器人高精度定位方法原理图;
图3为本发明中金字塔形式全局地图的示意图;
图4为本发明中人工路标的检测与识别实物图、原理图;其中(a)为移动机器人检测人工路标的实际场景;(b)为移动机器人检测人工路标的原理示意图;
图5为本发明中成对一致性检查实物图、可视化检测效果图;其中(a)为成对一致性检查实物图;(b)为成对一致性检查可视化检测效果图;
图6为本发明中高斯滤波基本原理图。
具体实施方式
下面结合附图和具体实施实例对发明做进一步说明。如图2所示,本发明方法主要包含三个部分:全局定位、位姿优化和定位质量检测。
全局定位是对全局地图进行处理得到全局地图金字塔来提高移动机器人启动状态或者被绑架的场景下的定位速度和准确度;位姿优化部分是利用人工路标作为辅助,来实现移动机器人在高动态环境下的鲁棒定位以及目标点的高精度定位;定位质量检测部分是通过对当前激光与全局地图的匹配程度来对当前定位质量进行实时检测与监控,并反馈到全局定位部分,提高鲁棒性。
将全局定位得到的全局位姿传输到位姿优化部分,实现移动机器人位姿的初始化;基于人工路标和激光混合的机器人位姿优化部分得到的位姿结果作为最终的机器人位姿;定位质量判断部分模块得到的定位评估结果反馈到全局定位部分,使得移动机器人定位***鲁棒性更高。
基于上述原理,本发明提出的一种用于复杂大尺度室内场景的移动机器人高精度定位方法,如图1所示,包括:
为了加速全局定位的搜索速度,按照滑窗的方法对原始全局地图按照每3*3块中取最小的得分值依次进行2倍率降采样,直到得到地图分辨率为1*1才停止降采样。图3展示了全局地图金字塔的基本思想示意图。根据金字塔的基本思想,在金字塔的最底部是原始的全局地图,金字塔的顶部是分辨率为1*1的地图。通过将处理之后的全局地图一层一层组合起来,就组成了全局地图金字塔,层级越高,则对应的全局地图越小,对应的分辨率越低。
步骤1:将机器人获取的原始全局地图进行降采样处理并组合成金字塔形式的全局地图;包括:
步骤1.1:采用分辨率为3*3的滑动窗口对原始全局地图进行滑动降采样;
步骤1.2:通过比较3*3滑动窗口中各个单元格得分值px获得最小得分值;
式中,zhit、zrand表示加权的混合权值,di表示当前位姿下第i个激光点在全局地图中的坐标与地图中最近被占用区域之间的距离,如果是空闲区域或者是未知区域,那么di被设置为固定值maxdist,如果是被占用区域,则di被设置为0,prand表示随机噪声,σ表示设置的激光的方差;
步骤1.3:将全局地图进行2倍率的降采样处理,得到不同分辨率下的全局地图,直到得到地图分辨率为1*1停止降采样处理;
步骤1.4:将每一分辨率下的全局地图按照分辨率从高到低的顺序组合为金字塔形式,得到金字塔形式的全局地图;
本发明将图像领域金字塔思想应用到全局地图的处理上。通过对全局地图进行降采样处理,并将处理之后的全局地图逐层组合,组成全局地图金字塔,层级越高,则对应的全局地图越小,对应的分辨率越低。
全局定位:首先利用金字塔思想对全局地图进行处理,得到全局地图金字塔。然后利用本发明提出的得分机制对机器人可能的位姿进行评分,最终选择最小得分的位姿作为机器人的全局位姿,并将全局定位模块输出的全局位姿用于移动机器人的初始位姿。
使用得分机制来实现全局定位,提高全局定位的运行效率。在接收到最新激光点时,根据全局地图金字塔,从位于最顶部的最低分辨率全局地图开始进行机器人位姿得分的检索。为了加快检索的准确度,在全局地图中设置一定的采样点,在接收到最新的实时激光点之后,同时计算在当前位姿下当前帧激光点与全局地图之间的匹配程度。
步骤2:根据采集到的当前时刻的激光点,从金字塔全局地图最顶层的最低分辨率开始进行机器人位姿得分的搜索实现全局定位;通过对机器人可能的位姿进行评分来实现全局定位,提高全局定位的运行效率;包括:
步骤2.1:当机器人接收到当前时刻的激光点时,计算当前位姿下当前帧激光点与全局地图中每个像素点对应的概率得分值p:
式中,Nscan表示当前帧激光点的总数,Ninliner表示在全局地图中有最近点对应的激光点数目,zhit、zrand表示加权的混合权值,di表示当前位姿下第i个激光点在全局地图中的坐标与地图中最近被占用区域之间的距离,如果是空闲区域或者是未知区域,那么di被设置为固定值maxdist,如果是被占用区域,则di被设置为0,prand表示随机噪声,σ表示设置的激光的方差,k1、k2表示加权因子;
步骤2.2:将所有可能的位姿的概率得分值从大到小进行排序,将概率得分值最小的位姿作为全局定位位姿。
为了提高机器人的定位精度,首先通过对人工路标进行检测和识别,得到环境中可能的人工路标用于移动机器人定位;人工路标的成对一致性检查,能够实现去除人工路标的误检测,实现准确检测和识别人工路标;通过利用检测到的人工路标的观测信息,对人工路标构建人工路标的观测模型,并利用提出的观测模型更新粒子分布;利用激光对环境进行检测,当移动机器人检测到多于三根反光柱时,利用高精度定位方法对人工路标进行处理,实现移动机器人在目标区域的高精度定位。实现过程如下:
步骤3:设置人工路标优化机器人的当前位姿;包括:
步骤3.1:在室内的机器人行进路线两旁间隔设置反光柱作为人工路标来辅助机器人进行定位;
步骤3.2:为了能够实现基于激光和人工路标的机器人定位,首先需要对人工路标进行检测和识别。由于反光柱制作简单、成本低、并且对原环境影响较小,因此主要采用反光柱作为人工路标进行辅助定位。在检测的初期阶段,主要通过环境中不同物体之间的反射强度不同来实现对反光柱的初步检测。假设检测到第i个反光柱,并且使用Di和分别代表在激光坐标系下激光雷达检测到的反光柱距离激光雷达的距离和方位角。如图4所示,对于某一个反光柱,得到一系列反光柱数据点Pm,Pm+1,…,Pn。并考虑到检测第i个反光柱对应的激光点数n-m的奇偶性。通过激光雷达检测第i个反光柱距离激光雷达的距离Di和方位角
式中,表示序号为/>的激光点测量得到的距离,/>表示序号为的激光点测量得到的距离,/>表示序号为/>的激光点测量得到的距离,表示序号为/>的激光点测量得到的角度,/>表示序号为/>的激光点测量得到的角度,/>表示序号为/>的激光点测量得到的角度,r表示人工路标的半径,m表示人工路标收到的激光点起始序号,n表示人工路标收到的激光点结束序号;
本发明使用2D激光雷达作为主要传感器,对于反光柱检测主要是通过激光雷达得到的强度信息进行区分。但是对于室内场景的复杂环境,实现准确的数据关联也是一个重要挑战,特别是对于含有玻璃等其他高反射率的室内场景,很容易发生误检测。如图5所示,运行环境含有落地玻璃墙或者含有其他表面光滑的金属物体。
为了避免其他表面光滑的物体产生错误的数据关联,使用成对一致性检查方式来实现。当机器人从实时激光点云中检测到高反射率的激光点时,首先会对激光点根据相邻点之间的距离关系进行聚类,得到N个人工路标。根据聚类得到的每一个人工路标进行激光点数的计数。
步骤3.3:对设置的人工路标进行成对一致性检验;包括:
步骤3.3.1:对于采集到的反射率大于一定阈值的激光点,根据相邻激光点之间的距离进行聚类,找到当前机器人检测到的N个目标对象;
步骤3.3.2:统计每个目标对象对应的激光点数g1;
步骤3.3.3:计算当前距离检测到的目标对象的理论角度θ;当前激光配置下,当前的距离检测到的反光柱点的理论角度,就可以得到反光柱理论对应的激光点数,如果当前检测到的激光点数与理论计算得到的激光点数之间的差值大于阈值,当前检测到的人工路标与最近的人工路标真值之间的距离,如果当前检测到的激光点数与理论计算得到的激光点数之间的差值大于阈值,认为是误检测。如果当前检测到的激光点数与理论计算得到的激光点数之间的差值小于阈值,并且当前人工路标与最近的人工路标真值之间的距离小于阈值,那么认为当前人工路标是真正的人工路标。利用成对一致性检查的方法来排除误检测,提高鲁棒性;
步骤3.3.4:根据理论角度θ计算每个目标对象理论上对应的激光点数g2;
g2=θ/R (6)
式中R表示激光分辨率;
步骤3.3.5:计算目标对象与最近的人工路标之间的实际距离s;
步骤3.3.6:如果激光点数g1与g2之间的差值小于设定阈值g',且距离s小于设定阈值s'则说明检测到的目标对象是人工路标;否则认为是误检测;
步骤3.4:当检测到人工路标的个数少于3时,通过建立观测模型更新移动机器人当前位姿的概率分布得到最优当前位姿;包括:
步骤3.4.1:建立检测到的人工路标的观测模型Pp:
式中,σ1表示坐标值x的标准差,σ2表示坐标值y的标准差,ρ表示x,y之间的协方差,(x,y)表示人工路标的坐标值,μ1表示坐标值x的真实值,μ2表示坐标值y的真实值;
根据前面得到的全局定位位姿初始化粒子分布,本发明采用高斯分布数学模型初始化关于机器人位姿的粒子分布,利用提出的观测模型更新粒子分布,考虑到移动机器人有三个自由度,所以对于机器人位置信息,只需考虑坐标x和坐标y;基于观测到的反光柱信息,利用公式(7)对反光柱观测值建立人工路标的观测模型,利用观测模型更新移动机器人当前位姿的概率分布。
其中,x,y是根据轮式里程计得到的机器人位置坐标,μ1,μ2分别是反光柱的坐标真值坐标x、真值坐标y,σ1,σ2分别是反光柱真值坐标x的标准差、真值坐标y的标准差,这两个参数决定了对于反光柱观测的信任程度,ρ是反光柱的坐标x与坐标y之间的协方差,由于反光柱的坐标x与坐标y相互独立,所以这里的协方差ρ设置为0。
步骤3.4.2:根据观测模型Pp更新移动机器人当前位姿的概率分布;
利用激光对环境进行检测,当移动机器人检测到多于三根反光柱时,利用高精度定位方法对人工路标进行处理,实现移动机器人在目标区域的高精度定位。通过线性代数的SVD方法来求解反光柱真值与检测到的反光柱之间的变换关系。将迭代最近点(Iterative Closest Point,ICP)进行求解两组点之间运动估计的思想应用到本发明。针对一组已经匹配好的反光柱点集,分别是反光柱真值点集检测到的反光柱点集P={p1,p2,...,pn}。需要找到一个欧式变换R,t满足公式/>使用代数方法进行求解。
步骤3.5:当检测到人工路标的个数大于等于3时,根据奇异值分解(简称SVD)方法确定机器人精确的当前位姿;包括:
步骤3.5.1:计算第i个人工路标的真实坐标值与实时检测到的坐标值之间的误差项ei:
式中,R、t表示待求解的欧式变换中的旋转矩阵、平移矩阵,表示人工路标的真实坐标值,pi表示实时检测到的人工路标的坐标值;
步骤3.5.2:构建最小二乘函数当J取最小值时得到R、t的具体值,其中,n′表示人工路标的个数;
步骤3.5.3:根据坐标变换参数的旋转矩阵R、平移矢量t的具体值生成机器人精确的当前位姿xi':
x'i=Rxi+t (9)
式中,xi表示机器人的当前位姿;
由于室内环境中半静态物体和动态物体的存在,他们会使环境发生较大的变化,甚至有可能会遮挡人工路标。人工路标遮挡会使移动机器人的定位产生偏差,甚至出现位姿跳变的问题。针对这个问题,基于高斯滤波提高定位的鲁棒性,主要是利用高斯滤波对计算得到的位姿进行滤波处理,来减少由于各方面带来的误差叠加而产生的较大的位姿跳变。通过增加高斯滤波的方式来实现对位姿的平滑处理,保证移动机器人在运动过程中的稳定性和鲁棒性。
步骤3.5.4:利用高斯滤波对当前位姿xi'进行平滑处理得到最终精确的当前位姿;通过引入高斯滤波方法解决动态环境人工路标可能遮挡情况下,移动机器人的定位产生偏差,甚至出现位姿跳变的问题,实现移动机器人的鲁棒定位。
其中,μ是高斯模板的均值,为了保证当前的位姿占的最大的比率,并且对于输出的位姿能够起到决定作用,本发明将x=μ对应的高斯权重设置为最新时刻位姿的权重;σ是高斯分布的标准差,σ越大代表先前的位姿对当前位姿产生的影响越大,得到的位姿结果之间的跳变就越小,位姿也就越平滑。
由于反光柱的坐标x、坐标y、航向角θ相互独立,所以使用一维高斯滤波来分别对坐标x、坐标y、航向角θ进行处理。图6展示了对移动机器人位姿做高斯滤波的示意图,为了能够达到很好的滤波效果,并且保证位姿不出现错误,需要选择合适的σ和合适的高斯核大小,其中这里σ设置为2,高斯核大小设置为3*1,最后将高斯滤波处理之后的结果作为机器人当前的位姿。
通过引入高斯滤波方法解决动态环境人工路标可能遮挡情况下,移动机器人的定位产生偏差,甚至出现位姿跳变的问题,实现移动机器人的鲁棒定位。
基于人工路标和激光混合的机器人高精度定位:通过从实时激光点中检测出来可能的人工路标点,然后利用人工路标的成对一致性方法去除人工路标的误检测,并通过高斯滤波方法对当前位姿进行平滑,最终实现基于人工路标的移动机器人在动态环境下鲁棒定位。通过对人工路标的真值与检测到的人工路标之间的变换关系的计算,得到位姿变换关系,并将计算结果作用到当前机器人位姿,最终得到更加准确的位姿,实现移动机器人在目标位置高精度定位。
考虑到最新的机器人定位***没有对于当前定位质量的评估和触发重定位的机制,通过集成定位质量检测功能,对当前的定位质量进行评估,并将评估结果反馈到全局定位,实现对当前机器人定位效果的反馈,使得本发明提出的定位方法更加鲁棒。
定位质量检测是指通过给定机器人当前位姿xt、实时激光点St和全局地图M,判断两者在当前位姿下匹配程度。重定位机制的触发是为了保证整个定位***的鲁棒性和可靠性,通过对当前激光与全局地图的匹配程度来对定位的质量进行实时监测,同时为了提高当前定位质量的评估速度,所以通过构建全局地图的k维搜索树的方式来实现。k维搜索树是一种空间划分的数据结构,基本思想是在K维数据集合中选择具有最大方差的维度k,然后在该维度上选择中值对该数据集合进行划分,得到两个子集合,同时创建一个树结点用于存储,两个子集合重复该过程,直至所有子集合都不能再划分为止。
对于每一帧激光点云和全局地图之间的匹配,本发明使用最近邻查找的方法来实现查找他们之间的特征对应关系。在当前机器人位姿下,通过对每一个实时激光点在全局地图中以一定半径进行搜索,如果在全局地图中能够搜索到关键点,并且关键点的数量Numkeypoint大于1,那么该激光点对应的定位质量评估得分pi'为正值,否则的话,该激光点对应的得分pi'为0。通过对所有激光点进行遍历,并且将所有的概率进行累加,得到所有激光点的得分之和,也就是得到了最终的定位质量评估的结果。
步骤4:构建定位质量评估函数进行定位效果判断,实现对当前的移动机器人定位质量进行评估,并将评估结果反馈给全局定位部分,实现对当前机器人定位效果的反馈,保证机器人在前进过程中的定位精度,使得本发明提出的定位方法鲁棒性更好;包括:
步骤4.1:对每个激光点在全局地图中以一定半径进行搜索,统计在全局地图中能够搜索到的关键点数量Numkeypoint;
步骤4.2:如果Numkeypoint>1则将该激光点对应的定位质量评估得分pi'取正值;否则将pi'取为0;
步骤4.3:累计计算所有激光点的定位质量评估得分:
式中,Pp表示所有激光点的定位质量评估总分值;
步骤4.4:如果Pp小于设定阈值则说明当前机器人的定位效果差需要重复步骤2进行重定位过程,如果Pp大于等于设定阈值/>则说明当前机器人的定位效果好不需要进行重定位过程。通过定位质量检测部分实现了当前机器人定位质量的反馈,使得移动机器人定位***在长时间运行过程中鲁棒性更好。
在室内场景下,本发明方法能够实现移动机器人长期的、鲁棒的全局定位到高精度定位;通过提出基于人工路标的高精度鲁棒定位方法,借助人工路标实现机器人高精度、鲁棒定位,实现位置误差小于0.004m,角度误差小于0.01rad,并且比现有的其他方法性能更好;提出了含有定位质量反馈的鲁棒定位方式。利用定位质量检测部分来对当前机器人的定位效果进行实时检测;并且带有反馈机制,提高移动机器人的鲁棒性。
实验表明,无论是在真实室内环境下还是在工厂环境下,本发明提出的方法能够实现移动机器人全局定位、动态环境下的鲁棒定位、实时定位质量检测,并且在目标区域实现位置误差小于0.004m,角度误差小于0.01rad。通过本发明方法提高移动机器人在复杂大尺度场景下的高精度鲁棒定位。
Claims (5)
1.一种用于复杂大尺度室内场景的移动机器人高精度定位方法,其特征在于,包括:
步骤1:将机器人获取的原始全局地图进行降采样处理并组合成金字塔形式的全局地图;
步骤2:根据采集到的当前时刻的激光点,从金字塔全局地图最顶层的最低分辨率开始进行机器人位姿得分的搜索实现全局定位;
步骤3:设置人工路标优化机器人的当前位姿;
步骤4:构建定位质量评估函数进行定位效果判断,保证机器人在前进过程中的定位精度;
所述步骤1包括:
步骤1.1:采用分辨率为3*3的滑动窗口对原始全局地图进行滑动降采样;
步骤1.2:通过比较3*3滑动窗口中各个单元格得分值px获得最小得分值;
式中,zhit、zrand表示加权的混合权值,di表示当前位姿下第i个激光点在全局地图中的坐标与地图中最近被占用区域之间的距离,如果是空闲区域或者是未知区域,那么di被设置为固定值maxdist,如果是被占用区域,则di被设置为0,prand表示随机噪声,σ表示设置的激光的方差;
步骤1.3:将全局地图进行2倍率的降采样处理,得到不同分辨率下的全局地图,直到得到地图分辨率为1*1停止降采样处理;
步骤1.4:将每一分辨率下的全局地图按照分辨率从高到低的顺序组合为金字塔形式,得到金字塔形式的全局地图;
所述步骤3包括:
步骤3.1:在室内的机器人行进路线两旁间隔设置反光柱作为人工路标来辅助机器人进行定位;
步骤3.2:通过激光雷达检测第i个反光柱距离激光雷达的距离Di和方位角
式中,表示序号为/>的激光点测量得到的距离,/>表示序号为的激光点测量得到的距离,/>表示序号为/>的激光点测量得到的距离,表示序号为/>的激光点测量得到的角度,/>表示序号为/>的激光点测量得到的角度,/>表示序号为/>的激光点测量得到的角度,r表示人工路标的半径,m表示人工路标收到的激光点起始序号,n表示人工路标收到的激光点结束序号;
步骤3.3:对设置的人工路标进行成对一致性检验;
步骤3.4:当检测到人工路标的个数少于3时,通过建立观测模型更新移动机器人当前位姿的概率分布得到最优当前位姿;
步骤3.5:当检测到人工路标的个数大于等于3时,根据奇异值分解方法确定机器人精确的当前位姿;
所述步骤3.3包括:
步骤3.3.1:对于采集到的反射率大于一定阈值的激光点,根据相邻激光点之间的距离进行聚类,找到当前机器人检测到的N个目标对象;
步骤3.3.2:统计每个目标对象对应的激光点数g1;
步骤3.3.3:计算当前距离检测到的目标对象的理论角度θ:
步骤3.3.4:根据理论角度θ计算每个目标对象理论上对应的激光点数g2;
g2=θ/R (6)
式中R表示激光分辨率;
步骤3.3.5:计算目标对象与最近的人工路标之间的实际距离s;
步骤3.3.6:如果激光点数g1与g2之间的差值小于设定阈值g',且距离s小于设定阈值s'则说明检测到的目标对象是人工路标;否则认为是误检测。
2.根据权利要求1所述的一种用于复杂大尺度室内场景的移动机器人高精度定位方法,其特征在于,所述步骤2包括:
步骤2.1:当机器人接收到当前时刻的激光点时,计算当前位姿下当前帧激光点与全局地图中每个像素点对应的概率得分值p:
式中,Nscan表示当前帧激光点的总数,Ninliner表示在全局地图中有最近点对应的激光点数目,k1、k2表示加权因子;
步骤2.2:将所有可能的位姿的概率得分值从大到小进行排序,将概率得分值最小的位姿作为全局定位位姿。
3.根据权利要求1所述的一种用于复杂大尺度室内场景的移动机器人高精度定位方法,其特征在于,所述步骤3.4包括:
步骤3.4.1:建立检测到的人工路标的观测模型Pp:
式中,σ1表示坐标值x的标准差,σ2表示坐标值y的标准差,ρ表示x,y之间的协方差,(x,y)表示人工路标的坐标值,μ1表示坐标值x的真实值,μ2表示坐标值y的真实值;
步骤3.4.2:根据观测模型Pp更新移动机器人当前位姿的概率分布。
4.根据权利要求1所述的一种用于复杂大尺度室内场景的移动机器人高精度定位方法,其特征在于,所述步骤3.5包括:
步骤3.5.1:计算第i个人工路标的真实坐标值与实时检测到的坐标值之间的误差项ei:
式中,R、t表示待求解的欧式变换中的旋转矩阵、平移矩阵,表示人工路标的真实坐标值,pi表示实时检测到的人工路标的坐标值;
步骤3.5.2:构建最小二乘函数当J取最小值时得到R、t的具体值,其中,n′表示人工路标的个数;
步骤3.5.3:根据R、t的具体值生成机器人精确的当前位姿x′i:
x′i=Rxit (9)
式中,xi表示机器人的当前位姿;
步骤3.5.4:利用高斯滤波对当前位姿x′i进行平滑处理得到最终精确的当前位姿。
5.根据权利要求1所述的一种用于复杂大尺度室内场景的移动机器人高精度定位方法,其特征在于,所述步骤4包括:
步骤4.1:对每个激光点在全局地图中以一定半径进行搜索,统计在全局地图中能够搜索到的关键点数量Numkeypoint;
步骤4.2:如果Numkeypoint>1则将该激光点对应的定位质量评估得分pi'取正值;否则将pi'取为0;
步骤4.3:累计计算所有激光点的定位质量评估得分:
式中,Pp表示所有激光点的定位质量评估总分值;
步骤4.4:如果Pp小于设定阈值则说明当前机器人的定位效果差需要重复步骤2进行重定位过程,如果Pp大于等于设定阈值/>则说明当前机器人的定位效果好不需要进行重定位过程。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111104812.7A CN113759928B (zh) | 2021-09-18 | 2021-09-18 | 用于复杂大尺度室内场景的移动机器人高精度定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111104812.7A CN113759928B (zh) | 2021-09-18 | 2021-09-18 | 用于复杂大尺度室内场景的移动机器人高精度定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113759928A CN113759928A (zh) | 2021-12-07 |
CN113759928B true CN113759928B (zh) | 2023-07-18 |
Family
ID=78796627
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111104812.7A Active CN113759928B (zh) | 2021-09-18 | 2021-09-18 | 用于复杂大尺度室内场景的移动机器人高精度定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113759928B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114371710B (zh) * | 2022-01-07 | 2024-04-30 | 牧原肉食品有限公司 | 基于反光柱的移动机器人的导航方法、设备及可读存储介质 |
CN117570998B (zh) * | 2024-01-17 | 2024-04-02 | 山东大学 | 基于反光柱信息的机器人定位方法及*** |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278170A (zh) * | 2013-05-16 | 2013-09-04 | 东南大学 | 基于显著场景点检测的移动机器人级联地图创建方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108550168A (zh) * | 2018-04-18 | 2018-09-18 | 湖南牛顺科技有限公司 | 智能化机械臂基于视觉的多目标快速高精度识别与定位方法 |
CN109613549B (zh) * | 2018-12-28 | 2023-04-07 | 芜湖哈特机器人产业技术研究院有限公司 | 一种基于卡尔曼过滤的激光雷达定位方法 |
CN110132284B (zh) * | 2019-05-30 | 2022-12-09 | 东北大学 | 一种基于深度信息的全局定位方法 |
CN111427047B (zh) * | 2020-03-30 | 2023-05-05 | 哈尔滨工程大学 | 一种大场景下自主移动机器人slam方法 |
CN113091736B (zh) * | 2021-04-02 | 2023-04-07 | 京东科技信息技术有限公司 | 机器人定位方法、装置、机器人及存储介质 |
-
2021
- 2021-09-18 CN CN202111104812.7A patent/CN113759928B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278170A (zh) * | 2013-05-16 | 2013-09-04 | 东南大学 | 基于显著场景点检测的移动机器人级联地图创建方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113759928A (zh) | 2021-12-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Wang et al. | Intensity scan context: Coding intensity and geometry relations for loop closure detection | |
Chen et al. | OverlapNet: Loop closing for LiDAR-based SLAM | |
Li et al. | Efficient laser-based 3D SLAM for coal mine rescue robots | |
Steder et al. | Place recognition in 3D scans using a combination of bag of words and point feature based relative pose estimation | |
Zhou et al. | T-LOAM: Truncated least squares LiDAR-only odometry and mapping in real time | |
US7446766B2 (en) | Multidimensional evidence grids and system and methods for applying same | |
CN113759928B (zh) | 用于复杂大尺度室内场景的移动机器人高精度定位方法 | |
US11790542B2 (en) | Mapping and localization system for autonomous vehicles | |
Brand et al. | Submap matching for stereo-vision based indoor/outdoor SLAM | |
CN111678516B (zh) | 一种基于激光雷达的有界区域快速全局定位方法 | |
CN114088081B (zh) | 一种基于多段联合优化的用于精确定位的地图构建方法 | |
Meng et al. | Efficient and reliable LiDAR-based global localization of mobile robots using multiscale/resolution maps | |
CN110906924A (zh) | 一种定位初始化方法和装置、定位方法和装置及移动装置 | |
US20230117498A1 (en) | Visual-inertial localisation in an existing map | |
US20240094343A1 (en) | Method, device, system, and storage medium for tracking moving target | |
Zhang et al. | A LiDAR-intensity SLAM and loop closure detection method using an intensity cylindrical-projection shape context descriptor | |
CN113160280A (zh) | 一种基于激光雷达的动态多目标跟踪方法 | |
Nielsen | Robust lidar-based localization in underground mines | |
CN116127405A (zh) | 一种融合点云地图、运动模型和局部特征的位置识别方法 | |
CN113835101A (zh) | 一种基于雷达点云的车辆定位方法、装置及存储介质 | |
Huang et al. | Research on lidar slam method with fused point cloud intensity information | |
Bernay-Angeletti et al. | A top-down perception approach for vehicle pose estimation | |
Zhang et al. | Multi-Constellation-Inspired Single-Shot Global LiDAR Localization | |
Jin et al. | Global localization for single 3D point cloud using voting mechanism | |
Huang et al. | Multi-object detection, tracking and prediction in rugged dynamic environments |
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 |