CN113483747B - 基于具有墙角信息的语义地图改进amcl定位方法及机器人 - Google Patents

基于具有墙角信息的语义地图改进amcl定位方法及机器人 Download PDF

Info

Publication number
CN113483747B
CN113483747B CN202110708956.7A CN202110708956A CN113483747B CN 113483747 B CN113483747 B CN 113483747B CN 202110708956 A CN202110708956 A CN 202110708956A CN 113483747 B CN113483747 B CN 113483747B
Authority
CN
China
Prior art keywords
particle
corner
robot
map
semantic
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
CN202110708956.7A
Other languages
English (en)
Other versions
CN113483747A (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.)
Wuhan University of Science and Engineering WUSE
Original Assignee
Wuhan University of Science and Engineering WUSE
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 Wuhan University of Science and Engineering WUSE filed Critical Wuhan University of Science and Engineering WUSE
Priority to CN202110708956.7A priority Critical patent/CN113483747B/zh
Publication of CN113483747A publication Critical patent/CN113483747A/zh
Application granted granted Critical
Publication of CN113483747B publication Critical patent/CN113483747B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005Navigation; 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804Creation or updating of map data
    • G01C21/3833Creation or updating of map data characterised by the source of data
    • G01C21/3841Data obtained from two or more sources, e.g. probe vehicles
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88Lidar systems specially adapted for specific applications
    • G01S17/89Lidar systems specially adapted for specific applications for mapping or imaging

Abstract

公开了一种基于具有墙角信息的语义地图改进AMCL定位方法及机器人。本发明利用建立的二维语义栅格地图,该语义地图融合了基于深度学习的目标检测方法提取环境中墙角和其他各类物体在栅格地图中的中心点位置,形成角点周围语义信息图。对加载栅格地图进行临近障碍物距离值计算,生成角点周围语义查找表。再结合提出的视觉预定位方法使得移动机器人在少量先验信息和运动的情况下更迅速地实现初定位。改进粒子权重更新方式,同步结合AMCL算法与环境地图匹配进行精定位。在不同环境条件下进行对比实验,验证了该方法对于机器人粒子收敛速率的提升,以及特别在环境有一定相似性或环境发生一定的改变时本文方法对定位准确性、鲁棒性的提升。

Description

基于具有墙角信息的语义地图改进AMCL定位方法及机器人
技术领域
本发明属于机器人建图定位导航领域,具体涉及一种基于具有墙角信息的语义地图改进AMCL定位方法及机器人。
背景技术
基于AMCL(adaptive Monte Carlo Localization,自适应蒙特卡洛定位)算法的机器人定位在普遍情况下粒子集能顺利收敛,但是由于算法本身的一些限制,也会有收敛失败的情况,依然存在一些值得改进的不足之处。AMCL算法考虑到定位过程中可能会出现粒子更新过快而导致收敛错误,同时也是出于增加粒子观测时数据的多样性,对粒子滤波进行更新的速度进行了限制,防止粒子出现缺失或者退化迅速的不良现象。正是因为如此,机器人由于先验信息的不足,往往需要较多的运动才能较为准确地定位到全局初始位置,这个过程需要不断给机器人发布运动指令使其保持运动直至粒子集收拢汇聚。
发明内容
本发明提供一种基于具有墙角信息的语义地图改进的AMCL机器人定位方法,语义地图中包含墙角等语义信息,定位过程中利用深度学习模型SSD进行物体识别检测,对环境中墙角及其周围其它物体语义信息进行识别并提取,将获取到的视觉检测数据与构建好的语义地图进行匹配,辅助激光定位方法以实现更好地定位效果。
根据本发明实施例的一方面,提供一种基于具有墙角信息的语义地图改进AMCL定位方法,该定位方法用于机器人定位,所述方法包括:
加载激光地图以及包含视觉语义信息的二维栅格语义地图,并且对语义地图进行预处理;
初始化粒子群,在激光地图中随机撒布M个粒子,每个粒子的权重设置相同值,所有粒子权重和为1,在激光地图中初始化粒子集χ0
利用机器人自身获取的相关运动量的传感器数据,结合机器人运动模型对粒子集进行更新,通过得到的传感器数据计算出一定时间的机器人运动量,在对其进行坐标变换,将机器人坐标系下的运动量转换到世界坐标系下,利用上一时刻粒子状态估计值以及运动量结合运动模型,实现粒子集中各粒子位姿预测,得到预测粒子集χt
结合似然域观测模型和波束模型进行激光匹配,计算粒子集中各粒子权重值;再根据由语义地图视觉预定位方法,利用深度相机获取机器人视角彩色图像,将视角彩色图像输入到神经网络预测,识别到视角彩色图像中非墙角的语义标签;结合目标检测模型对视觉彩色图像进行识别,得到视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型,将得到的非墙角的三维点云坐标、墙角的三维点云坐标分别通过基于统计方法的滤波器进行点云滤波处理,得到滤波后非墙角的三维点云坐标、滤波后墙角的三维点云坐标,最后将得到的语义与语义地图进行匹配实现预定位,对得到的估计位姿值集合进行权重值二次更新;
根据粒子的权重值对粒子集进行重新采样得到新粒子集,保留权重大的粒子,舍弃权重小的粒子,复制权重大的粒子根据权重的比例重新分布粒子,经过反复迭代,获得机器人准确位姿,实现机器人的定位。
本发明的至少一个实施例提供一种机器人,包括:处理器;用于存储处理器可执行指令的存储器;其中,所述处理器被配置为执行所述方法的全部或部分步骤。
本发明的至少一个实施例提供一种非临时性计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现项所述方法的全部或部分步骤。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例的附图作简单地介绍。
图1为本发明一实施例提供的基于具有墙角信息的语义地图改进AMCL定位方法流程图。
图2为激光地图,其中图2(a)为二维栅格激光地图,图2(b)为障碍物似然灰度图。
图3为加载的环境语义地图,其中图3(a)为二维栅格语义地图,图3为(b)优化表示后的语义地图。
图4为角点周围语义信息图,其中图4(a)为角点示例图,图4(b)为角点周围语义图。
图5为角点周围语义查找表。
图6为物体识别检测图,其中图6(a)为实验场景图,图6(b)物体识别检测结果图。
图7为识别物GrabCut算法分割图,其中图7(a)为柜子分割,图7(b)为门分割。
图8为物体语义提取图,其中图8(a)为机器人视角图,图8(b)为语义映射图。
图9为筛选角点图,其中图9(a)为所有角点示意图,图9(b)为剩下角点示意图。
图10为相似实验环境地图,图10(a)为环境地图,图10(b)为机器人所在区域。
图11为实施本发明基于具有墙角信息的语义地图改进AMCL定位方法得到的粒子收敛图。
具体实施方式
本发明针对AMCL定位算法存在的某些场景下易定位失败的问题,提出基于具有墙角信息的语义地图改进AMCL定位方法,定位过程中利用深度学习模型(包括但不限于SSD模型)进行物体识别检测,对环境中墙角及其周围其他物体语义信息进行识别并提取,通过定位过程中提取物体语义位置关系构建墙角信息查找表,利用构建具有墙角信息的语义地图先实现视觉预定位,使得机器人可以在少量先验信息和运动的情况下更迅速地实现初定位,结合AMCL算法与环境地图匹配,改进了粒子权重更新方式,进行精定位,有效提高了AMCL算法的定位效率和准确性。下文将结合图1对本发明基于具有墙角信息的语义地图改进AMCL定位方法进行详细介绍。
步骤1:机器人在室内运动时其上的激光雷达实时采集物体与机器人的距离、物体与机器人的方向角,机器人主控机通过Gmapping算法处理物体与机器人的距离、物体与机器人的方向角得到环境栅格地图,加载的激光地图(如图2)以及包含视觉语义信息的二维栅格语义地图(如图3),并且对语义地图进行相关预处理用于后面匹配。
步骤1中对语义地图进行预处理时,首先提取语义地图中墙角以及其他各类物体在栅格地图中的中心点位置,以及每个中心点对应的物体语义映射点集(如图4),相应的范围值rs的公式为:
Figure BDA0003132553800000031
式中θs表示机器人相机水平视场角,本实施例采用Kinect v2相机,但不限定于此,它的水平深度视角为70°;ds表示设置的语义提取最大视距,Kinect v2相机的最佳观测视距为0.5m~4.5m。
步骤1中加载利用Gmapping算法建立的二维栅格地图用于定位,根据似然域观测模型的原理对加载的地图栅格点进行临近障碍物距离值dist计算,生成相应的查找表(如图5),在后续利用激光似然域模型实现权重更新时可以直接从生成的查找表中读取相应值进行计算,可以提升算法运行效率。
其中当前帧激光似然地图中各点的概率值p(x)计算公式为:
Figure BDA0003132553800000032
当选中激光击中点在地图边界内时,dist为击中点与最近的障碍物点之间的欧式距离;在边界外时,dist选取击中点与地图中障碍物点的最大距离。σ为指定的测量噪声的标准方差。
步骤2:初始化粒子群。当机器人无先验信息的情况下,其在地图中的初始位姿未知,在地图中随机撒布M个粒子,每个粒子的权重设置相同值,所有粒子权重和为1,在激光地图中初始化粒子集χ0,权重为1/M。
步骤2粒子初始化阶段,粒子群为:
Figure BDA0003132553800000041
粒子群大小为M,当t=0时,表示0时刻的初始粒子集。
步骤3:利用机器人自身获取的相关运动量的传感器数据(如里程计),结合机器人运动模型对粒子集进行更新。通过得到的传感器数据计算出一定时间的机器人运动量,再对其进行坐标变换,将机器人坐标系下的运动量转换到世界坐标系下,利用上一时刻粒子状态估计值以及运动量结合运动模型,实现粒子集中各粒子位姿预测,得到预测粒子集χt
步骤3粒子位姿预测中,根据里程计模型预测粒子集χt公式为:
Figure BDA0003132553800000042
其中,(xt,yt)表示时间t时刻的位姿,θt为机器人的偏航角,V是机器人线速度,△t是上一时刻t-1到下一时刻t的时间差。
步骤4:结合似然域观测模型和波束模型进行激光匹配,计算粒子集中各粒子权重值;再根据由语义地图视觉预定位方法,利用机器人上深度相机获取视角彩色图像,将视角彩色图像输入到神经网络(可以是DeepLab v2)预测,识别到视角彩色图像中非墙角的语义标签;结合优化后目标检测模型(可以是SSD)对视觉彩色图像进行识别,得到视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型,将得到的非墙角的三维点云坐标、墙角的三维点云坐标分别通过基于统计方法的滤波器进行点云滤波处理,得到滤波后非墙角的三维点云坐标、滤波后墙角的三维点云坐标,最后将得到的语义与语义地图进行匹配实现预定位,对得到的估计位姿值集合进行权重值二次更新。
步骤4中对深度相机获取的视角彩色图像进行处理时涉及到对物体进行背景分割以及坐标分割变换映射,具体方法如下:
相机获取到当前视角下的彩色图和深度图,视角彩色图像输入到优化后DeepLabv2网络预测,识别到视角彩色图像中非墙角的语义标签,利用提前训练好的SSD模型对彩色图进行检测识别(如图6),识别到视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型。可以看出检测框中包含前景和背景,先使用图像分割算法(可以是GrabCut)对其进行背景去除。利用深度学习模型得到的检测框代替用户绘制目标框。得到视角下彩色图的目标分割图后(如图7),结合其对应的深度图信息,结合深度相机标定参数将目标在图像坐标系中的像素坐标转换到相机坐标系,再进行语义映射,为了减少点云处理计算量,只对检测框中的中心1/2部分进行点云处理,在点云滤波和坐标变换后将距离机器人最近的一簇点云映射到二维平面;对于角点而言是将检测框中心的圆心部分的点云进行二维映射,得到映射结果(如图8)。
若当前视角下识别到墙角时,计算出映射图中每个物体的中心坐标,并计算其它各类物体中心到该角点中心的距离dc。公式为:
Figure BDA0003132553800000051
其中,Dc表示计算角距值,dcn表示映射图中第n个物体(非墙角)中心到角点中心距。
检测结果中,同时检测到了门和柜子的语义信息,根据当前语义映射图以及之前由语义地图建立好的角点信息查找表,首先筛选出同时含有映射图中各类物体的墙角点,计算这些墙角的角距
Figure BDA0003132553800000052
为:
Figure BDA0003132553800000053
其中,d1,d2,d3,d4分别对应角点到其范围内对应椅子、垃圾桶、门、柜子的中心距离;n1,n2,n3,n4分别表示映射图中椅子、垃圾桶、门、柜子的数量;若该角点周围存在n个同类物体,d1s(s=0,1,2,...,n-1)表示这些椅子到该角点距离从小到大排序,d2s(s=0,1,2,...,n-1)表示这些垃圾桶到该角点距离从小到大排序,d3s(s=0,1,2,...,n-1)表示这些门到该角点距离从小到大排序,d4s(s=0,1,2,...,n-1)表示这些柜子到该角点距离从小到大排序。在得到当前视角下的真实角距以及各个可能位置角点的角距后,求取各可能角点与真实视角的角距差为:
Figure BDA0003132553800000054
其中,将这些角点按照角距差ΔDc大小依次存储,设置角距差阈值Ω(本实施例可为1),对角点进行过滤,保留其中ΔDc<Ω较小的一部分角点,过滤之后得到可能角点位置图(如图9)。
步骤4中语义地图视觉预定位方法得到的估计位姿值集合:
Xc={(x1,y12),(x2,y22),...};
步骤4中权重值二次更新公式为:
wt=ηwt
式中η为激光扫描匹配与视觉预定位结果的相似度系数,对于粒子集中在预估位姿附近的粒子权重值变大,即η≥1,远离的粒子权重值变小,即0<η<1,具体的实现方法如下:
a)若
Figure BDA0003132553800000061
那么粒子权重值不作更新,其中η=1,wt保持当前值不变;
b)若
Figure BDA0003132553800000062
针对粒子集中每一个粒子(xt,ytt),由下式可计算离散值Md,判断该粒子是否在Xc集合中某个位姿附近,再以此确定权重更新系数η。
Figure BDA0003132553800000063
当Md≤2且
Figure BDA0003132553800000064
为相机的水平视角,本实施例中使用的Kinect v2相机水平视角为70度,则认为该粒子位于视觉预定位得到的位姿附近,它在真实位置附近的概率值高,粒子权重值应增大,系数η按下式计算得出;
Figure BDA0003132553800000065
当Md≤2且
Figure BDA0003132553800000066
则认为该粒子虽然位于视觉预定位得到的位姿附近,但偏航角误差较大,权重值应减小,系数η按下式计算;
Figure BDA0003132553800000067
当Md>2,则该粒子远离视觉预定位得到的位姿,权重值减小,系数η按下式计算:
Figure BDA0003132553800000068
以ηwt作为每个粒子新的权重值,遍历完粒子集χt中所有粒子后,对粒子集进行权重归一化。
步骤5:根据粒子的权重值对粒子集进行重新采样得到新粒子集,以权重为wt的概率来作为标准,保留权重大的粒子,舍弃权重小的粒子,利用KLD采样对粒子数量进行动态调整,提高粒子利用率,减少粒子冗余,复制权重大的粒子根据权重的比例重新分布粒子。将重采样后的粒子代入到状态转移函数中,就能获得新的预测粒子,经过反复迭代,获得机器人准确位姿,实现机器人的定位。
当机器人初始处于环境中相似的区域中时,容易出现定位失败的情况,在相似环境情况下的有效性和优越性。在相似环境下操作机器人原地旋转,利用本发明方法进行定位测试(如图10),在粒子收敛图中(如图11),开始与现有AMCL算法的收敛趋势类似,出现了两个粒子聚类,分别位于相似的两个区域中,此时激光数据和栅格地图匹配度已基本匹配,但“假位姿”的粒子数量仍较多;然后通过相机获取机器人前方语义信息,结合视觉预定位方法进行粒子权重更新,在多次观测后,重采样器复制高权重粒子,而逐渐删除低权重粒子,整体上粒子向地图中存在检测物体周围且结构环境相似环境位置处收敛。在多次重采样后,最后收敛至机器人的真实位姿处。
综上,本发明利用建立的二维语义栅格地图,该语义地图融合了基于深度学习的目标检测方法提取环境中墙角和其他各类物体在栅格地图中的中心点位置,形成角点周围语义信息图。对加载栅格地图进行临近障碍物距离值计算,生成角点周围语义查找表。再结合提出的视觉预定位方法使得移动机器人在少量先验信息和运动的情况下更迅速地实现初定位。改进粒子权重更新方式,同步结合AMCL算法与环境地图匹配进行精定位。在不同环境条件下进行对比实验,验证了该方法对于机器人粒子收敛速率的提升,以及特别在环境有一定相似性或环境发生一定的改变时本文方法对定位准确性、鲁棒性的提升。
在示例性实施例中,还提供一种机器人,该机器人包括处理器、激光雷达、深度相机,其中,所述处理器被配置为执行所述方法的全部或部分步骤。
在示例性实施例中,还提供了一种非临时性计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现所述方法的全部或部分步骤。例如,所述非临时性计算机可读存储介质可以是ROM、RAM、CD-ROM、磁带、软盘和光数据存储设备等。

Claims (10)

1.一种基于具有墙角信息的语义地图改进AMCL定位方法,该定位方法用于机器人定位,其特征在于,所述方法包括:
加载激光地图以及包含视觉语义信息的二维栅格语义地图,并且对语义地图进行预处理;
初始化粒子群,在激光地图中随机撒布M个粒子,每个粒子的权重设置相同值,所有粒子权重和为1,在激光地图中初始化粒子集χ0
利用机器人自身获取的相关运动量的传感器数据,结合机器人运动模型对粒子集进行更新,通过得到的传感器数据计算出一定时间的机器人运动量,再对其进行坐标变换,将机器人坐标系下的运动量转换到世界坐标系下,利用上一时刻粒子状态估计值以及运动量结合运动模型,实现粒子集中各粒子位姿预测,得到预测粒子集χt
根据由语义地图视觉预定位方法得到估计位姿值集合Xc,其具体方法为:利用深度相机获取机器人视角彩色图像,将视角彩色图像输入到神经网络预测,识别视角彩色图像中非墙角的语义标签;结合目标检测模型对视觉彩色图像进行识别,得到视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型,对检测框中的中心部分进行点云处理,将得到的非墙角的三维点云坐标、墙角的三维点云坐标分别通过基于统计方法的滤波器进行点云滤波处理,得到滤波后非墙角的三维点云坐标、滤波后墙角的三维点云坐标,将距离机器人最近的一簇点云映射到二维平面,对于墙角而言是将检测框中心的圆心部分的点云进行二维映射,最后将得到的语义与语义地图进行匹配,根据当前语义映射图以及由语义地图建立好的墙角信息查找表,首先筛选出同时含有映射图中各类物体的墙角,计算这些墙角角点的角距,在得到当前视角下的真实角距以及各个可能位置墙角角点的角距后,求取各可能位置墙角角点与真实视角的角距差,将这些墙角角点按照角距差ΔDc大小依次存储,设置角距差阈值Ω,对墙角角点进行过滤,保留其中ΔDc<Ω较小的一部分墙角角点,过滤之后得到可能墙角角点位置图;
结合似然域观测模型和波束模型进行激光匹配,计算粒子集中各粒子权重值,根据激光扫描匹配与视觉预定位结果的相似度来改变粒子权重的大小,对得到的估计位姿值集合进行权重值二次更新,对于粒子集中在语义地图视觉预估位姿附近的粒子权重值变大,远离的粒子权重值变小;如果视觉预定位方法的估计位姿值集合Xc不为空,判断粒子集中粒子是否在Xc集合附近,再以此确定权重更新系数η;
根据粒子的权重值对粒子集进行重新采样得到新粒子集,以激光扫描匹配与视觉预定位结果的相似度系数和各粒子权重积的概率作为标准,保留权重大的粒子,舍弃权重小的粒子,复制权重大的粒子根据权重的比例重新分布粒子,经过反复迭代,获得机器人准确位姿,实现机器人的定位。
2.根据权利要求1所述的定位方法,其特征在于,对语义地图进行预处理时,首先提取语义地图中墙角以及其他各类物体在栅格地图中的中心点位置,以及每个中心点对应的物体语义映射点集,相应的范围值rs的公式为:
Figure FDA0003947023910000021
式中θs表示机器人相机水平视场角,ds表示设置的语义提取最大视距;
根据似然域观测模型的原理对加载的地图栅格点进行临近障碍物距离值dist计算,生成相应的查找表,在利用激光似然域模型实现权重更新时直接从生成的查找表中读取相应值进行计算;
其中当前帧激光似然地图中各点的概率值p(x)计算公式为:
Figure FDA0003947023910000022
当选中激光击中点在地图边界内时,dist为击中点与最近的障碍物点之间的欧式距离;在边界外时,dist选取击中点与地图中障碍物点的最大距离;σ为指定的测量噪声的标准方差。
3.根据权利要求1所述的定位方法,其特征在于,初始化的粒子集表示为:
Figure FDA0003947023910000023
粒子集大小为M,当t=0时,表示0时刻的初始粒子集。
4.根据权利要求1所述的定位方法,其特征在于,所述粒子位姿预测中,根据里程计模型预测粒子集χt公式为:
Figure FDA0003947023910000024
(xt,yt)为时间t时刻机器人的位姿,θt为机器人的偏航角,V是机器人线速度,△t是上一时刻t-1到下一时刻t的时间差,Δθt=θtt-1
5.根据权利要求1所述的定位方法,其特征在于,深度相机获取机器人视角下的彩色图和深度图,视角彩色图像输入到优化后DeepLab v2网络预测,识别到视角彩色图像中非墙角的语义标签,利用提前训练好的SSD模型对彩色图进行检测识别,识别到视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型,先使用GrabCut分割算法对检测框中进行背景去除,利用深度学习模型得到的检测框,得到视角下彩色图的目标分割图后,结合其对应的深度图信息,结合深度相机标定参数将目标在图像坐标系中的像素坐标转换到相机坐标系,再进行语义映射,只对检测框中的中心1/2部分进行点云处理,将点云滤波和坐标变换后将距离机器人最近的一簇点云映射到二维平面,对于墙角而言是将检测框中心的圆心部分的点云进行二维映射,得到映射结果;
若当前视角下识别到墙角角点时,计算出映射图中每个物体的中心坐标,并计算其它各类物体中心到该墙角角点中心的距离dc,公式为:
Figure FDA0003947023910000031
其中,Dc表示计算角距值,dcn表示映射图中第n个非墙角物体中心到墙角角点中心距;
检测结果中,同时检测到了门和柜子的语义信息,根据当前语义映射图以及之前由语义地图建立好的角点信息查找表,首先筛选出同时含有映射图中各类物体的墙角角点,计算这些墙角角点的角距
Figure FDA0003947023910000032
为:
Figure FDA0003947023910000033
其中,d1,d2,d3,d4分别对应墙角角点到其范围内对应椅子、垃圾桶、门、柜子的中心距离;n1,n2,n3,n4分别表示映射图中椅子、垃圾桶、门、柜子的数量;若该墙角角点周围存在n个同类物体,d1s表示这些椅子到该墙角角点距离从小到大排序,d2s表示这些垃圾桶到该墙角角点距离从小到大排序,d3s表示这些门到该墙角角点距离从小到大排序,d4s表示这些柜子到该墙角角点距离从小到大排序,s=0,1,2,...,n-1,
在得到当前视角下的真实角距以及各个可能位置墙角角点的角距后,求取各可能墙角角点与真实视角的角距差为:
Figure FDA0003947023910000034
6.根据权利要求1所述的定位方法,其特征在于,语义地图视觉预定位方法得到的估计位姿值集合表示为:
Xc={(x1,y11),(x2,y22),...}
Xc表示视觉预定位估计位姿值集合,(x1,y1),(x2,y2)分别为视觉预定位估计机器人的位姿,θ1,θ2为机器人假定朝向与墙角的夹角。
7.根据权利要求4所述的定位方法,其特征在于,权重值二次更新公式为:
wt=ηwt
式中η为激光扫描匹配与视觉预定位结果的相似度系数,对于粒子集中在预估位姿附近的粒子权重值变大,即η≥1,远离的粒子权重值变小,即0<η<1,具体的实现方法如下:
a)若
Figure FDA0003947023910000041
那么粒子权重值不作更新,其中η=1,wt保持当前值不变;
b)若
Figure FDA0003947023910000042
针对粒子集中每一个粒子(xt,ytt),由下式计算离散值Md,判断该粒子是否在Xc集合中某个位姿附近,再以此确定权重更新系数η;
Figure FDA0003947023910000043
当Md≤2且
Figure FDA0003947023910000044
Figure FDA0003947023910000045
为相机的水平视角,则认为该粒子位于视觉预定位得到的位姿附近,它在真实位置附近的概率值高,粒子权重值应增大,系数η按下式计算得出;
Figure FDA0003947023910000046
当Md≤2且
Figure FDA0003947023910000047
则认为该粒子虽然位于视觉预定位得到的位姿附近,但偏航角误差较大,权重值应减小,系数η按下式计算;
Figure FDA0003947023910000048
当Md>2,则该粒子远离视觉预定位得到的位姿,权重值减小,系数η按下式计算:
Figure FDA0003947023910000049
以ηwt作为每个粒子新的权重值,遍历完粒子集χt中所有粒子后,对粒子集进行权重归一化。
8.根据权利要求7所述的定位方法,其特征在于,根据粒子的权重值对粒子集进行重新采样得到新粒子集,以权重为wt的概率作为标准,保留权重大的粒子,舍弃权重小的粒子,利用KLD采样对粒子数量进行动态调整,复制权重大的粒子根据权重的比例重新分布粒子,将重采样后的粒子代入到状态转移函数中,获得新的预测粒子,经过反复迭代,获得机器人准确位姿,实现机器人的定位。
9.一种机器人,其特征在于,包括:
处理器;
用于存储处理器可执行指令的存储器;
其中,所述处理器被配置为执行权利要求1至8任一项所述的方法的步骤。
10.一种非临时性计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现如权利要求1至8任一项所述方法的步骤。
CN202110708956.7A 2021-06-25 2021-06-25 基于具有墙角信息的语义地图改进amcl定位方法及机器人 Active CN113483747B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110708956.7A CN113483747B (zh) 2021-06-25 2021-06-25 基于具有墙角信息的语义地图改进amcl定位方法及机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110708956.7A CN113483747B (zh) 2021-06-25 2021-06-25 基于具有墙角信息的语义地图改进amcl定位方法及机器人

Publications (2)

Publication Number Publication Date
CN113483747A CN113483747A (zh) 2021-10-08
CN113483747B true CN113483747B (zh) 2023-03-24

Family

ID=77936064

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110708956.7A Active CN113483747B (zh) 2021-06-25 2021-06-25 基于具有墙角信息的语义地图改进amcl定位方法及机器人

Country Status (1)

Country Link
CN (1) CN113483747B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199251B (zh) * 2021-12-03 2023-09-15 江苏集萃智能制造技术研究所有限公司 一种机器人的防碰撞定位方法
CN114216452B (zh) * 2021-12-06 2024-03-19 北京云迹科技股份有限公司 一种机器人的定位方法及装置
CN115014352B (zh) * 2022-06-01 2023-05-26 浙江大学 一种基于建议分布地图的室内全局定位方法
CN115131656B (zh) * 2022-09-01 2022-12-13 深圳鹏行智能研究有限公司 空间识别方法、装置、电子设备及计算机可读存储介质
CN116309907A (zh) * 2023-03-06 2023-06-23 中国人民解放军海军工程大学 一种基于改进的Gmapping算法的移动机器人建图方法
CN117152258B (zh) * 2023-11-01 2024-01-30 中国电建集团山东电力管道工程有限公司 一种管道生产智慧车间的产品定位方法及系统
CN117437382B (zh) * 2023-12-19 2024-03-19 成都电科星拓科技有限公司 一种数据中心部件的更新方法及系统
CN117589153A (zh) * 2024-01-18 2024-02-23 深圳鹏行智能研究有限公司 一种地图更新的方法及机器人

Family Cites Families (8)

* 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
CN104019813B (zh) * 2014-06-19 2017-01-25 无锡知谷网络科技有限公司 目标即时定位和构建地图的方法与系统
CN109724603A (zh) * 2019-01-08 2019-05-07 北京航空航天大学 一种基于环境特征检测的室内机器人导航方法
CN111486842B (zh) * 2019-01-29 2022-04-15 深圳市优必选科技有限公司 重定位方法及装置、机器人
CN111025309B (zh) * 2019-12-31 2021-10-26 芜湖哈特机器人产业技术研究院有限公司 一种融合折角板的自然定位方法及系统
CN111539994B (zh) * 2020-04-28 2023-04-18 武汉科技大学 一种基于语义似然估计的粒子滤波重定位方法
CN111486855B (zh) * 2020-04-28 2021-09-14 武汉科技大学 一种具有物体导航点的室内二维语义栅格地图构建方法
CN112785643A (zh) * 2021-02-02 2021-05-11 武汉科技大学 一种基于机器人平台的室内墙角二维语义地图构建方法

Also Published As

Publication number Publication date
CN113483747A (zh) 2021-10-08

Similar Documents

Publication Publication Date Title
CN113483747B (zh) 基于具有墙角信息的语义地图改进amcl定位方法及机器人
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN109144056B (zh) 移动机器人的全局自定位方法及设备
CN110097553B (zh) 基于即时定位建图与三维语义分割的语义建图系统
CN111429574B (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
Rummelhard et al. Conditional monte carlo dense occupancy tracker
CN111508002B (zh) 一种小型低飞目标视觉检测跟踪系统及其方法
Krull et al. 6-dof model based tracking via object coordinate regression
JP2022515591A (ja) ターゲットオブジェクトの3d検出方法、装置、媒体及び機器
CN111862201B (zh) 一种基于深度学习的空间非合作目标相对位姿估计方法
CN111260683A (zh) 一种三维点云数据的目标检测与跟踪方法及其装置
CN109934847B (zh) 弱纹理三维物体姿态估计的方法和装置
CN111060115A (zh) 一种基于图像边缘特征的视觉slam方法及系统
WO2022188663A1 (zh) 一种目标检测方法及装置
CN111665842A (zh) 一种基于语义信息融合的室内slam建图方法及系统
CN113985445A (zh) 一种基于相机与激光雷达数据融合的3d目标检测算法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
CN114399675A (zh) 一种基于机器视觉与激光雷达融合的目标检测方法和装置
Ait Abdelali et al. An adaptive object tracking using Kalman filter and probability product kernel
CN114817426A (zh) 地图建构装置及方法
CN109801309B (zh) 一种基于rgb-d相机的障碍物感知方法
CN113436251B (zh) 一种基于改进的yolo6d算法的位姿估计系统及方法
CN111739066B (zh) 一种基于高斯过程的视觉定位方法、系统及存储介质
CN113536959A (zh) 一种基于立体视觉的动态障碍物检测方法
CN113689459A (zh) 动态环境下基于gmm结合yolo实时跟踪与建图方法

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