CN111474535A - 一种基于特征热力图的移动机器人全局定位方法 - Google Patents
一种基于特征热力图的移动机器人全局定位方法 Download PDFInfo
- Publication number
- CN111474535A CN111474535A CN202010191640.0A CN202010191640A CN111474535A CN 111474535 A CN111474535 A CN 111474535A CN 202010191640 A CN202010191640 A CN 202010191640A CN 111474535 A CN111474535 A CN 111474535A
- Authority
- CN
- China
- Prior art keywords
- point
- point set
- current
- points
- radar
- 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
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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
- G01S13/06—Systems determining position data of a target
- G01S13/42—Simultaneous measurement of distance and other co-ordinates
-
- 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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
-
- 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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/10—Terrestrial scenes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/35—Categorising the entire scene, e.g. birthday party or wedding scene
- G06V20/36—Indoor scenes
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
一种基于特征热力图的移动机器人全局定位方法,包括步骤输入数据信息,场景地图m、雷达测量点集rp、雷达扫描点数rn、雷达扫描角度rθ以及雷达扫描半径rr;获取场景地图中的可行进点;获取所有的可行进点的模拟雷达仿真数据;以整体点集作为输入参数,计算在不同扫描半径下的点特征数和线特征数;计算在不同半径下的点特征和线特征所有组合的结果点集;计算雷达测量点集在不同半径下的点特征数和线特征数;根据雷达测量点集计算得到的点特征数和线特征数,在由整体点集计算得到的点特征数和线特征数中查找对应的点集,将这些对应的点集合并到集合点集Fr;将集合点集Fr作为输入点集进行全局定位计算。本发明在大型车间等地图特征明显的环境下能够表现出优异的鲁棒性。
Description
技术领域
本发明属于移动机器人定位技术领域,更具体地,涉及一种基于特征热力图的移动机器人全局定位方法。
背景技术
随着国内工业化进程的加快,移动机器人在国内的多数场景均实现了大量实际应用,如家庭中的扫地机器人、车间中的上下料机器人以及智能驾驶领域的无人车。无论是哪种类型的移动机器人,其主要技术可大概分为导航技术与控制技术,而作为导航技术中最为关键的定位技术,其往往用于实现定位机器人当前所处位置,而全局定位正是其中的主要问题。
全局定位是指机器人对于自己的初始位置是未知的,这意味着机器人可能在环境中的任何地方。全局定位的实现方式首先要依赖于当前场景的地图,然后依赖于当前机器人传感器所获得的测量值。将地图与传感器数据匹配的方式有多种,比如传感器数据图像与地图的Map-Map匹配、地图与传感器数据点云的Map-Scan匹配以及地图整体似然域场与传感器数据似然域场的Scan-Scan匹配等。最终得到的全局定位结果往往也不是一个确切的位置,而是机器人当前在场景地图中所处位置的概率分布,通常会选取最大概率的位置作为全局定位最终结果。
对于移动机器人来说,高精度的全局定位结果可以用于校正小车运行过程中由于位置跟踪、定位波动与***误差等原因导致的累计误差,有助于保证机器人的整体运行精度。而所选取的全局定位方式对全局定位的精度与效率存在直接的影响,目前的全局定位方法存在的一些问题如下:
1)常用的全局定位算法如蒙特卡洛算法依赖于初始定位空间的选取,在实际运算中往往以整张地图作为散播区间,导致其计算速度较为缓慢。
2)进行了地图预处理的栅格定位算法需要对地图进行似然域计算,其计算效率虽高但是随着地图尺寸的增大整体匹配时间也会提升,导致计算速度缓慢。
发明内容
本发明要解决的技术问题是提供一种基于特征热力图的移动机器人全局定位方法。
为了解决上述技术问题,本发明采取以下技术方案:
一种基于特征热力图的移动机器人全局定位方法,包括以下步骤:
输入数据信息,场景地图m、雷达测量点集rp、雷达扫描点数rn、雷达扫描角度rθ以及雷达扫描半径rr;
S1,获取场景地图中的可行进区域的可行进点,所有的可行进点构成点集p;
S2,获取所有的可行进点的模拟雷达仿真数据,得到存储所有雷达仿真数据的整体点集p(β,L),表示每个点对应自己的一组雷达模拟点集;
S3,以整体点集作为输入参数,计算在不同扫描半径下的点特征数和线特征数;
S4,计算在不同半径下的点特征和线特征所有组合的结果点集;
S5,计算雷达测量点集在不同半径下的点特征数和线特征数;
S6,根据雷达测量点集计算得到的点特征数和线特征数,在由整体点集计算得到的点特征数和线特征数中查找对应的点集,将这些对应的点集合并到集合点集Fr;
S7,将集合点集Fr作为输入点集进行全局定位计算,计算结果即为全局定位的结果。
所述步骤S1具体为:
S1.1,获取场景地图m后对该场景地图进行灰度二值化处理:
mgrid=cvtcolor(m)
mcvt=threshold(mgrid)
S1.2,针对场景地图中的移动机器人与边界的距离设为d,对二值化的场景地图进行腐蚀处理,将场景地图中的黑色障碍物区域扩散,得到腐蚀地图mero:
mero=erode(d,mgrid)
S1.3,对腐蚀地图进行遍历处理,获取其中的白色点作为可行进点。
所述步骤S2具体为:
S2.1,获取点集p中当前点的位姿向量β,之后遍历其他的所有点:
β={βx,βy,βθ};
S2.2,获取每个可行进点在进行雷达数据模拟时的初始角度A与角度增量Agap:
A=βθ,Agap=rθ/(rn-1)
将步骤S2.2循环rn次,每次循环时执行角度A的递增操作:
A=A+Agap;
S2.2.1,针对所有的角度A,在当前可行进点的中心处开始发射射线,其中先计算当前射线端点当前所处位置在场景地图m中的相对坐标(rx,ry),并设射线当前长度为rl,最后计算射线的xy轴上的增量rxgap与rygap:
rx=(βx-mxmin)/mr,rxgap=cos(A)
ry=(βy-mymin)/mr,rygap=sin(A)
mr表示地图分辨率;
S2.2.2对应递增计算发射过程中射线端点(rx,ry)的位置,进行三类条件的判断,首先判断当前射线位置当前是否触及到地图中的障碍物,障碍物即为场景地图中的黑色位置:
然后判断当前射线长度rl是否已经到达雷达的半径长度rr:
最后判断当前射线端点位置(rx,ry)是否已经相对场景地图发生了越界:
如果当前射线端点位置(rx,ry)同时满足上述三个条件,则对当前射线端点进行递增操作,然后继续循环执行步骤S2.2.2:
rx=rx+rxgap,ry=ry+rygap,rl=rl+1
如果当前射线端点位置(rx,ry)没有同时满足上述条件,则跳出步骤S2.2.2步骤,进入下一步骤S2.2.3;
S2.2.3,判断当前射线端点(rx,ry)是否是已经到达当前雷达半径长度,
如果是,则继续执行步骤S2.2;如果不是,则将当前射线端点(rx,ry)存入结果点集L中,并继续执行步骤S2.2;
S2.3,将结果点集L输出,其中结果点集L存储着角度与其角度方向上射线的终止坐标的映射关系;
S2.4,将点集p中的所有点计算完毕后,得到存储所有雷达仿真数据的整体点集p(β,L),表示每个点对应自己的一组雷达模拟点集。
所述步骤S3具体为:
S3.1,以点集p(e,L)为主要输入参数,根据雷达的半径长度rr的值进行半径区间选取:
rri={rri/1,rri/2,...,rri/t}
上式中,t为预设值,
然后遍历所有的半径rri来分别进行计算;
S3.2,遍历点集p(e,L)中的每个点β所有子点集L,计算子点集中的线特征值与点特征值,其中涉及到直线检测过程与角点检测过程;
S3.2.1,直线检测过程:
首先建立一个累计矩阵M(k,b),其中的k表示所取的斜率,选取范围[kmax,kmin],b表示所取的截距,选取范围[bmax,bmin],根据不同地图的不同需求,设计不同k与b的取值范围,以及两者的计算增量值kgap与bgap,
然后遍历当前点集L中的所有点Li,根据斜率k的范围计算对应的每个点的截距b的数值,并将对应的数值累加到累计矩阵M(k,b)中:
b=-kLix+Liy
M(rd(k),rd(b))=M(rd(k),rd(b))+1
k=k+kgap,b=b+bgap
上式中,函数rd表示对当前数值取整;
S3.2.2,角点检测过程:
首先遍历子点集L中的每个点,计算每个点在当前位置的梯度G(i,j):
然后遍历每个点的所有临近点计算窗口矩阵W,并根据该矩阵W的特征值λ1与λ2来计算角点特征结合值C以进行下一步计算:
C(i,j)=λ1λ2-k(λ1+λ2)2
上式中k为预设系数,
接着设计阈值ct来限定所选取的角点特征结合值C,当点集L中存在点的值大于阈值ct时,将该满足阈值ct要求的点***结果点集Lscol中,并递增点特征个数Lc的值:
S3.3,通过步骤S3.1与S3.2,得到所有半径rri下,点集p(e,L)的点特征数值Lc与线特征数值Ll,此时界定该数据整合存储为F(R,P),其中R表示当前点集的计算半径,P表示存储了当前计算半径下所有点的坐标信息与特征数量的数据结构,表示为:
Pi={x,y,Ll,Lc}i=sizeof(p(e,L))。
所述步骤S4具体为:
S4.1,根据步骤S3的结果F(R,P),遍历所有的半径R进行计算:
S4.2,遍历当前点集P中所有点的数据,获取其中最大的点特征数Lcmax与线特征数Llmax:
Lcmax=searchmaxLcinF(R,P),Llmax=searchmaxLlinF(R,P)
所述步骤S5中,计算雷达测量点集所使用到的半径,与步骤S3中计算整体点集所使用到的半径相同,以雷达测量点集rp作为输入参数,按照步骤S3的具体步骤计算得到数据结果为表示在不同半径下雷达测量点集的点特征数量与线特征数量。
所述步骤S6具体为:根据半径遍历步骤S5的结果得到在当前半径R下的雷达点集的点特征数Lc与线特征数Ll,并根据点特征数Lc与线特征数Ll结果在步骤S4的结果中选取对应的点集,最终将这些点集合并至最终集合点集Fr,该集合点集Fr初始化拥有场景地图中全部可行进点,并将交集在场景地图中使用红点标记,
Ll=LlinrF(R,Ll,Lc),Lc=LcinrF(R,Ll,Lc)
Fr=Fr∪F(R,Ll,Lc)。
所述步骤S7具体为:
S7.1,将步骤S6的集合点集Fr作为初始粒子点集,设置粒子整体个数pN与粒子权重分布ωt;
S7.2,设计一个单一的随机数pr用于采样计算,并将粒子群的第一个粒子的权值作为计算阈值pt:
U=pr+(pn-1)·pN-1
U>pt
i=i+1
上式中,Fsol为输出点集,最终的定位结果将会选取该点集的中心点作为全局定位的结果。
本发明具有以下有益效果:
1、本发明提出了一种新的使用特征热力图来对场景地图进行预处理,并通过预处理结果来进行全局定位方法,这种方法通过精度计算得到准确度较高的预处理结果,使得全局定位的过程变为了处理雷达数据后直接查找预处理数据即可,在合理运用下可以极大地提高全局定位的速度与精度。
2、本发明提出的全局定位方法实时处理对象为雷达数据转换为的图像,而并非传统的全局定位算法中的地图信息,在实时计算时本发明计算量远低于传统算法。并且在设计足够的计算阈值后本发明方法可以将大量外界干扰因素屏除,仅探查环境中的明显特征线与特征点,在大型车间等地图特征明显的环境下能够表现出优异的鲁棒性。
附图说明
图1是本发明实施例的基于特征热力图的移动机器人全局定位计算流程图;
图2所示为本发明所实验用的场景地图。
图3所示为本发明在地图的左下角大厅位置获取的雷达仿真数据。
图4所示为本发明计算过程中一张特征热力图。
图5所示为本发明的一种计算用例得到的结果点集。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。此外,下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
如图1所示,本发明提出了一种基于特征热力图的移动机器人全局定位方法,其中特征热力图是指场景地图中每个位置的点特征与线特征数量所形成的结果图像,在符合当前组合点特征数量与线特征数量的点标记为红色,少于当前组合点特征数量的为蓝色系,多于当前组合点特征数量的为绿色系。具体由以下步骤实现。其中步骤S1至步骤S4为预处理阶段,步骤S5至步骤S7为计算阶段。
输入:场景地图m、雷达测量点集rp、雷达扫描点数rn、雷达扫描角度rθ以及雷达扫描半径rr。
其中场景地图m属于提前输入用于预处理,雷达测量点集rp在需要进行全局定位时输入。
步骤S1:获取场景地图中的可行进区域的可行进点,所有的可行进点构成点集p,具体为:
S1.1,如图2所示,获取场景地图m后首先对其进行灰度二值化处理:
mgrid=cvtcolor(m)
mcvt=threshold(mgrid)
上式中,mgrid为灰度地图,即将原RGB地图转换为单色地图,mcvt为二值化地图,该二值化地图保证其中只存在黑色与白色区域。
S1.2,针对场景地图中的移动机器人与边界的距离设为d,对二值化后的场景地图进行腐蚀处理,将原场景地图中的黑色障碍物区域扩散,得到腐蚀地图mero:
mero=erode(d,mgrid)。
S1.3,对腐蚀地图进行遍历处理,获取其中的白色点作为可行进点:
forallPoint(i,j)inmerodo
ifpixelofPoint(i,j)=255
insertPoint(i,j)intop
endif
endfor
上式中,p为可行进的点集结果。
步骤S2:获取步骤S1的点集结果p中所有点的模拟雷达仿真数据。
如图3所示,由于实际雷达测量点集与场景地图截取所得到的地图信息并不匹配,所以针对步骤S1的点集结果p中的所有点使用在点上模拟雷达射线发射的方式来模拟雷达数据,以此统一两者信息关系:
2.1首先获取p中当前点的位姿向量β,之后遍历其中的所有点:
β={βx,βy,βθ}
上式中,βx表示当前点的x轴坐标,βy表示当前点的y轴坐标,βθ表示当前点的角度偏转数值。
S2.2,获取每个可行进点在进行雷达数据模拟时的初始角度A与角度增量Agap:
A=βθ,Agap=rθ/(rn-1)
将步骤S2.2循环rn次,每次循环时执行角度A的递增操作:
A=A+Agap。
S2.2.1,针对所有的角度A,在当前可行进点的中心处开始发射射线,其中先计算当前射线端点当前所处位置在场景地图m中的相对坐标(rx,ry),并设射线当前长度为rl,最后计算该射线xy轴上的增量rxgap与rygap:
rx=(βx-mxmin)/mr,rxgap=cos(A)
ry=(βy-mymin)/mr,rygap=sin(A)
上式中,mr表示地图分辨率,可理解为实际尺寸(单位为米)与场景地图中一个像素点的比值大小。
S2.2.2,对应递增计算发射过程中射线端点(rx,ry)的位置,随着射线的发射,端点位置是不断产生距离变化的,对其进行三类条件的判断:
1)首先判断该位置当前是否触及到场景地图中的障碍物,即场景地图中的黑色区域位置:
ifthresholdofPoint(rx,ry)inm≠255
2)然后判断当前射线长度rl是否已经到达雷达的半径长度rr:
ifrl≠rr
3)最后判断当前射线端点位置(rx,ry)是否已经相对场景地图发生了越界:
ifrx<mxmaxandrx>mxminandry<mymaxandry>mymin
如果当前射线端点位置(rx,ry)同时满足上述条件,即同时没有触及到场景地图中的障碍物,当前射线长度没有到达雷达的半径长度,以及当前射线端点位置还在场景地图内没有发生越界,则对该端点进行递增操作,然后继续循环执行S步骤2.2.2:
rx=rx+rxgap,ry=ry+rygap,rl=rl+1;直到获取所有的行进点在进行雷达数据模拟时的角度A与角度增量Agap。
如果当前射线端点位置(rx,ry)没有同时满足上述条件,则跳出步骤S2.2.2步骤。比如,当前射线端点位置已经触及到场景地图中的障碍物,那么跳出S2.2.2;或者虽然没有触及到场景地图中的障碍物,但是,当前射线长度已经达到了雷达的半径长度,同样跳出S2.2.2;或者没有触及到场景地图中的障碍物,以及当前射线长度没有达到雷达的半径长度,但是发生了越界情况,那么同样跳出S2.2.2。
S2.2.3,判断当前射线端点(rx,ry)是否是已经到达当前雷达半径长度:
如果rl等于rr,则继续执行步骤S2.2,直到获取所有的行进点在进行雷达数据模拟时的角度A与角度增量Agap。
如果rl不等于rr,则将当前射线端点(rx,ry)存入结果点集L中,并继续执行步骤S2.2,直到获取所有的行进点在进行雷达数据模拟时的角度A与角度增量Agap。
S2.3,将结果点集L输出,其中点集L存储着角度与其角度方向上射线的终止坐标的映射关系。
S2.4,将所有p中的点计算完毕后,得到存储所有雷达仿真数据的整体点集p(β,L),其表示每个点对应自己的一组雷达模拟点集。
步骤S3:计算步骤S2的整体点集p(β,L)在不同扫描半径下的点特征数与线特征数,具体为:
3.1以步骤S2的结果点集p(e,L)为本步骤的主要输入参数,根据rr的值进行适当的半径区间选取:
rri={rri/1,rri/2,...,rri/t}
上式中,t的取值方式与取值个数根据实际方式而定;
然后遍历所有的半径rri来分别进行之后的计算过程。
S3.2,遍历整体点集p(e,L)中的每个点β所有子点集L,计算子点集中的线特征值与点特征值,其中涉及到直线检测过程与角点检测过程。
S3.2.1直线检测过程:
首先建立一个累计矩阵M(k,b),其中的k表示所取的斜率,存在选取范围[kmax,kmin],b表示所取的截距,存在选取范围[bmax,bmin]。根据不同地图的不同需求,可以设计不同k与b的取值范围,以及两者的计算增量值kgap与bgap。
然后遍历当前子点集L中的所有点Li,根据斜率k的范围计算对应的每个点的截距b的数值,并将对应的数值累加到累计矩阵M(k,b)中:
b=-kLix+Liy
M(rd(k),rd(b))=M(rd(k),rd(b))+1
k=k+kgap,b=b+bgap
上式中,函数rd表示对当前数值取整。
最后通过设计阈值st来遍历判断每个子点集L的累计矩阵M(k,b),并根据最终超过阈值的结果来确定当前子点集L中的线特征个数Ll,同时将识别为线特征的直线以两点形式存入直线特征结果Lslol中:
M(k,b)>st
Ll=Ll+1。
S3.2.2,角点检测过程:
首先遍历子点集L中的每个点,计算每个点的在当前位置的梯度G(i,j):
然后遍历每个点的所有临近点计算窗口矩阵W,并根据该矩阵W的特征值λ1与λ2来计算角点的特征结合值C以进行下一步计算:
C(i,j)=λ1λ2-k(λ1+λ2)2,
上式中k为系数,根据实际情况不同进行选定。
C(i,j)>ct
最后需要根据在步骤S3.2.1中所选取的直线进行角点位置的限定,因为此时角点检测出的角点是在模拟雷达点集下检测出来的,可能有一些角点在直线检测的对比中会失效,所以此处需要加以限制如下:
上式中,距离阈值dt与角度阈值at根据实际所需计算情况而限定,(xn,yn)表示直线上任意端点,(xc,yc)表示当前角点c的坐标,(x0,y0)与(x1,y1)表示一条直线的两端点,(x0,y0)与(x1,y1)表示另一条直线的两端点。
S3.3,通过步骤S3.1与S3.2,得到所有半径rri下,点集p(e,L)的点特征数值Lc与线特征数值Ll。此时界定该数据整合存储为F(R,P),其中R表示当前点集的计算半径,P表示存储了当前计算半径下所有点的坐标信息与特征数量的数据结构,表达为:
Pi={x,y,Ll,Lc}i=sizeof(p(e,L))
步骤S4:计算步骤S3获取的结果F(R,P)在不同半径下点特征与线特征所有组合的结果点集。具体为:
S4.1,根据步骤S3的结果F(R,P),遍历所有的半径R进行计算:
S4.2,遍历当前点集P中所有点的数据,获取其中最大的点特征数Lcmax与线特征数Llmax:
Lcmax=searchmaxLcinF(R,P),Llmax=searchmaxLlinF(R,P)。
步骤S5:计算雷达测量点集在不同半径下的点特征数与线特征数。
其计算方式与步骤3的基本相同,区别在于其输入参数变为雷达测量点集rp,并且仅计算这一个点集。同时这里的半径选取需要与步骤S3中的一致,保证计算结果能够正常匹配。
根据半径遍历步骤S5的结果得到在当前半径R下的雷达点集的点特征数Lc与线特征数Ll,并根据两特征数结果在步骤S4的结果中选取对应的点集,最终将这些点集合并至最终集合点集Fr(该点集Fr初始化拥有场景地图中全部可行进点),并将交集在场景地图中使用红点标记,如图5所示:
步骤S7:将步骤S6的结果Fr作为输入点集进行全局定位计算,具体如下所示:
S7.1,全局定位算法需要步骤S6的结果Fr作为初始粒子点集,并根据实际情况设计粒子整体个数pN与粒子权重分布ωt。
S7.2,设计一个单一的随机数pr用于采样计算,并将粒子群的第一个粒子的权值作为计算阈值pt:
S7.3,遍历所有的粒子,计算当前粒子的计算权值U与阈值pt的关系:
U=pr+(pn-1)·pN-1
需要说明的是,以上仅为本发明的优选实施例而已,并不用于限制本发明,尽管参照实施例对本发明进行了详细的说明,对于本领域的技术人员来说,其依然可以对前述实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换,但是凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (8)
1.一种基于特征热力图的移动机器人全局定位方法,包括以下步骤:
输入数据信息,场景地图m、雷达测量点集rp、雷达扫描点数rn、雷达扫描角度rθ以及雷达扫描半径rr;
S1,获取场景地图中的可行进区域的可行进点,所有的可行进点构成点集;
S2,获取所有的可行进点的模拟雷达仿真数据,得到存储所有雷达仿真数据的整体点集p(β,L),表示每个点对应自己的一组雷达模拟点集;
S3,以整体点集作为输入参数,计算在不同扫描半径下的点特征数和线特征数;
S4,计算在不同半径下的点特征和线特征所有组合的结果点集;
S5,计算雷达测量点集在不同半径下的点特征数和线特征数;
S6,根据雷达测量点集计算得到的点特征数和线特征数,在由整体点集计算得到的点特征数和线特征数中查找对应的点集,将这些对应的点集合并到集合点集Fr;
S7,将集合点集Fr作为输入点集进行全局定位计算,计算结果即为全局定位的结果。
2.根据权利要求1所述的基于特征热力图的移动机器人全局定位方法,其特征在于,所述步骤S1具体为:
S1.1,获取场景地图m后对该场景地图进行灰度二值化处理:
mgrid=cvtcolor(m)
mcvt=threshold(mgrid)
上式中,mgrid为灰度地图,即将原RGB地图转换为单色地图,mcvt为二值化地图,该二值化地图保证其中只存在黑色与白色区域;
S1.2,针对场景地图中的移动机器人与边界的距离设为d,对二值化后的场景地图进行腐蚀处理,将场景地图中的黑色障碍物区域扩散,得到腐蚀地图mero:
mero=erode(d,mgrid)
S1.3,对腐蚀地图进行遍历处理,获取其中的白色点作为可行进点。
3.根据权利要求1所述的基于特征热力图的移动机器人全局定位方法,其特征在于,所述步骤S2具体为:
S2.1,获取点集p中当前点的位姿向量β,之后遍历其他的所有点:
β={βx,βy,βθ};
S2.2,获取每个可行进点在进行雷达数据模拟时的初始角度A与角度增量Agap:
A=βθ,Agap=rθ/(rn-1)
将步骤S2.2循环rn次,每次循环时执行角度A的递增操作:
A=A+Agap;
S2.2.1,针对所有的角度A,在当前可行进点的中心处开始发射射线,其中先计算当前射线端点当前所处位置在场景地图m中的相对坐标(rx,ry),并设射线当前长度为rl,最后计算射线的xy轴上的增量rxgap与rygap:
rx=(βx-mxmin)/mr,rxgap=cos(A)
ry=(βy-mymin)/mr,rygap=sin(A)
mr表示地图分辨率;
S2.2.2对应递增计算发射过程中射线端点(rx,ry)的位置,进行三类条件的判断,首先判断当前射线位置当前是否触及到地图中的障碍物,障碍物即为场景地图中的黑色位置:
然后判断当前射线长度rl是否已经到达雷达的半径长度rr:
最后判断当前射线端点位置(rx,ry)是否已经相对场景地图发生了越界:
如果当前射线端点位置(rx,ry)同时满足上述三个条件,则对当前射线端点进行递增操作,然后继续循环执行步骤S2.2.2:
rx=rx+rxgap,ry=ry+rygap,rl=rl+1
如果当前射线端点位置(rx,ry)没有同时满足上述条件,则跳出步骤S2.2.2步骤,进入下一步骤S2.2.3;
S2.2.3,判断当前射线端点(rx,ry)是否是已经到达当前雷达半径长度,
如果是,则继续执行步骤S2.2;如果不是,则将当前射线端点(rx,ry)存入结果点集L中,并继续执行步骤S2.2;
S2.3,将结果点集L输出,其中结果点集L存储着角度与其角度方向上射线的终止坐标的映射关系;
S2.4,将点集p中的所有点计算完毕后,得到存储所有雷达仿真数据的整体点集p(β,L),表示每个点对应自己的一组雷达模拟点集。
4.根据权利要求1所述的基于特征热力图的移动机器人全局定位方法,其特征在于,所述步骤S3具体为:
S3.1,以点集p(e,L)为主要输入参数,根据雷达的半径长度rr的值进行半径区间选取:
rri={rri/1,rri/2,...,rri/t}
上式中,t为预设值,
然后遍历所有的半径rri来分别进行计算;
S3.2,遍历点集p(e,L)中的每个点β所有子点集L,计算子点集中的线特征值与点特征值,其中涉及到直线检测过程与角点检测过程;
S3.2.1,直线检测过程:
首先建立一个累计矩阵M(k,b),其中的k表示所取的斜率,选取范围[kmax,kmin],b表示所取的截距,选取范围[bmax,bmin],根据不同地图的不同需求,设计不同k与b的取值范围,以及两者的计算增量值kgap与bgap,
然后遍历当前点集L中的所有点Li,根据斜率k的范围计算对应的每个点的截距b的数值,并将对应的数值累加到累计矩阵M(k,b)中:
b=-kLix+Liy
M(rd(k),rd(b))=M(rd(k),rd(b))+1
k=k+kgap,b=b+bgap
上式中,函数rd表示对当前数值取整;
S3.2.2,角点检测过程:
首先遍历子点集L中的每个点,计算每个点在当前位置的梯度G(i,j):
然后遍历每个点的所有临近点计算窗口矩阵W,并根据该矩阵W的特征值λ1与λ2来计算角点特征结合值C以进行下一步计算:
C(i,j)=λ1λ2-k(λ1+λ2)2
上式中k为预设系数,
S3.3,通过步骤S3.1与S3.2,得到所有半径rri下,点集p(e,L)的点特征数值Lc与线特征数值Ll,此时界定该数据整合存储为F(R,P),其中R表示当前点集的计算半径,P表示存储了当前计算半径下所有点的坐标信息与特征数量的数据结构,表示为:
Pi={x,y,Ll,Lc}i=sizeof(p(e,L))。
8.根据权利要求1所述的基于特征热力图的移动机器人全局定位方法,其特征在于,所述步骤S7具体为:
S7.1,将步骤S6的集合点集Fr作为初始粒子点集,设置粒子整体个数pN与粒子权重分布ωt;
S7.2,设计一个单一的随机数pr用于采样计算,并将粒子群的第一个粒子的权值作为计算阈值pt:
U=pr+(pn-1)·pN-1
U>pt
i=i+1
上式中,Fsol为输出点集,最终的定位结果将会选取该点集的中心点作为全局定位的结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010191640.0A CN111474535B (zh) | 2020-03-18 | 2020-03-18 | 一种基于特征热力图的移动机器人全局定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010191640.0A CN111474535B (zh) | 2020-03-18 | 2020-03-18 | 一种基于特征热力图的移动机器人全局定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111474535A true CN111474535A (zh) | 2020-07-31 |
CN111474535B CN111474535B (zh) | 2022-03-15 |
Family
ID=71747540
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010191640.0A Active CN111474535B (zh) | 2020-03-18 | 2020-03-18 | 一种基于特征热力图的移动机器人全局定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111474535B (zh) |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20050095384A (ko) * | 2004-03-26 | 2005-09-29 | 삼성전자주식회사 | 로봇의 전역 위치 추정 방법 |
US20140350839A1 (en) * | 2013-05-23 | 2014-11-27 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
KR101468545B1 (ko) * | 2013-07-10 | 2014-12-03 | 연세대학교 산학협력단 | 이동 로봇의 전역위치인식 및 지도 업데이트 장치 및 방법 |
CN104915949A (zh) * | 2015-04-08 | 2015-09-16 | 华中科技大学 | 一种结合点特征和线特征的图像匹配算法 |
CN106444757A (zh) * | 2016-09-27 | 2017-02-22 | 成都普诺思博科技有限公司 | 基于直线特征地图的ekf‑slam算法 |
CN109323697A (zh) * | 2018-11-13 | 2019-02-12 | 大连理工大学 | 一种针对室内机器人任意点启动时粒子快速收敛的方法 |
CN110488818A (zh) * | 2019-08-08 | 2019-11-22 | 深圳市银星智能科技股份有限公司 | 一种基于激光雷达的机器人定位方法、装置和机器人 |
CN110686677A (zh) * | 2019-10-10 | 2020-01-14 | 东北大学 | 一种基于几何信息的全局定位方法 |
-
2020
- 2020-03-18 CN CN202010191640.0A patent/CN111474535B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20050095384A (ko) * | 2004-03-26 | 2005-09-29 | 삼성전자주식회사 | 로봇의 전역 위치 추정 방법 |
US20140350839A1 (en) * | 2013-05-23 | 2014-11-27 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
KR101468545B1 (ko) * | 2013-07-10 | 2014-12-03 | 연세대학교 산학협력단 | 이동 로봇의 전역위치인식 및 지도 업데이트 장치 및 방법 |
CN104915949A (zh) * | 2015-04-08 | 2015-09-16 | 华中科技大学 | 一种结合点特征和线特征的图像匹配算法 |
CN106444757A (zh) * | 2016-09-27 | 2017-02-22 | 成都普诺思博科技有限公司 | 基于直线特征地图的ekf‑slam算法 |
CN109323697A (zh) * | 2018-11-13 | 2019-02-12 | 大连理工大学 | 一种针对室内机器人任意点启动时粒子快速收敛的方法 |
CN110488818A (zh) * | 2019-08-08 | 2019-11-22 | 深圳市银星智能科技股份有限公司 | 一种基于激光雷达的机器人定位方法、装置和机器人 |
CN110686677A (zh) * | 2019-10-10 | 2020-01-14 | 东北大学 | 一种基于几何信息的全局定位方法 |
Non-Patent Citations (6)
Title |
---|
GEN LI: "《Hybrid Maps Enhanced Localization System for Mobile Manipulator in Harsh Manufacturing Workshop》", 《IEEE ACCESS》 * |
JUNG H. OH: "Global Alignment of Deep Features for Robot Localization in Changing Environment", 《2019 3RD EUROPEAN CONFERENCE ON ELECTRICAL ENGINEERING AND COMPUTER SCIENCE (EECS)》 * |
WENPENG ZONG: "A Fast and Accurate Planar-Feature-Based Global Scan Registration Method", 《IEEE SENSORS JOURNAL》 * |
杨晶东等: "基于联合点线特征的医疗服务机器人同时定位与地图构建算法研究", 《第二军医大学学报》 * |
梅瑞冬等: "双舵轮AGV 视觉里程计的设计方法", 《计算机***应用》 * |
满增光等: "从激光扫描数据中提取角点特征的方法", 《南京航空航天大学学报》 * |
Also Published As
Publication number | Publication date |
---|---|
CN111474535B (zh) | 2022-03-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107179768B (zh) | 一种障碍物识别方法及装置 | |
CN107239076B (zh) | 基于虚拟扫描与测距匹配的agv激光slam方法 | |
CN108958282B (zh) | 基于动态球形窗口的三维空间路径规划方法 | |
CN103424112B (zh) | 一种基于激光平面辅助的运动载体视觉导航方法 | |
US20150015702A1 (en) | Moving-Object Position/Attitude Estimation Apparatus and Moving-Object Position/Attitude Estimation Method | |
CN103295239A (zh) | 一种基于平面基准影像的激光点云数据的自动配准方法 | |
CN112700479B (zh) | 一种基于cnn点云目标检测的配准方法 | |
CN109146990B (zh) | 一种建筑轮廓的计算方法 | |
CN110597249A (zh) | 一种机器人及其回充定位方法和装置 | |
CN113538620A (zh) | 一种面向二维栅格地图的slam建图结果评价方法 | |
CN115541030A (zh) | 高炉炉顶料面温度分布的识别方法、装置及存储介质 | |
CN111474535B (zh) | 一种基于特征热力图的移动机器人全局定位方法 | |
US20230382426A1 (en) | Method and system for mapping a physical environment using an occupancy grid | |
US20130332110A1 (en) | Non-iterative mapping of capped cylindrical environments | |
CN108805896B (zh) | 一种应用于城市环境的距离图像分割方法 | |
CN116385292A (zh) | 一种IMU辅助的LiDAR动态点云剔除方法 | |
CN115657049A (zh) | 一种隧道车载激光雷达定位纠偏方法及*** | |
JP2011511943A (ja) | コンピュータを援用してセンサデータから物体の運動を計算する方法 | |
CN114488026A (zh) | 基于4d毫米波雷达的地下停车库可通行空间检测方法 | |
CN112069445A (zh) | 一种2d slam算法评估及量化方法 | |
CN114694106A (zh) | 道路检测区域的提取方法、装置、计算机设备和存储介质 | |
CN114924259B (zh) | 一种不同车载激光雷达的性能评估方法 | |
CN116740681B (zh) | 目标检测方法、装置、车辆和存储介质 | |
CN113358058B (zh) | 基于离散序列点的焊缝轮廓特征计算机视觉检测方法 | |
CN116863176B (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 |