CN111474535B - 一种基于特征热力图的移动机器人全局定位方法 - Google Patents

Abstract

一种基于特征热力图的移动机器人全局定位方法，包括步骤输入数据信息，场景地图m、雷达测量点集rp、雷达扫描点数rn、雷达扫描角度rθ以及雷达扫描半径rr；获取场景地图中的可行进点；获取所有的可行进点的模拟雷达仿真数据；以整体点集作为输入参数，计算在不同扫描半径下的点特征数和线特征数；计算在不同半径下的点特征和线特征所有组合的结果点集；计算雷达测量点集在不同半径下的点特征数和线特征数；根据雷达测量点集计算得到的点特征数和线特征数，在由整体点集计算得到的点特征数和线特征数中查找对应的点集，并取交集作为集合点集；将集合点集作为输入点集进行全局定位计算。本发明在大型车间等地图特征明显的环境下能够表现出优异的鲁棒性。

Description

一种基于特征热力图的移动机器人全局定位方法

技术领域

本发明属于移动机器人定位技术领域，更具体地，涉及一种基于特征热力图的移动机器人全局定位方法。

背景技术

随着国内工业化进程的加快，移动机器人在国内的多数场景均实现了大量实际应用，如家庭中的扫地机器人、车间中的上下料机器人以及智能驾驶领域的无人车。无论是哪种类型的移动机器人，其主要技术可大概分为导航技术与控制技术，而作为导航技术中最为关键的定位技术，其往往用于实现定位机器人当前所处位置，而全局定位正是其中的主要问题。

全局定位是指机器人对于自己的初始位置是未知的，这意味着机器人可能在环境中的任何地方。全局定位的实现方式首先要依赖于当前场景的地图，然后依赖于当前机器人传感器所获得的测量值。将地图与传感器数据匹配的方式有多种，比如传感器数据图像与地图的Map-Map匹配、地图与传感器数据点云的Map-Scan匹配以及地图整体似然域场与传感器数据似然域场的Scan-Scan匹配等。最终得到的全局定位结果往往也不是一个确切的位置，而是机器人当前在场景地图中所处位置的概率分布，通常会选取最大概率的位置作为全局定位最终结果。

对于移动机器人来说，高精度的全局定位结果可以用于校正小车运行过程中由于位置跟踪、定位波动与系统误差等原因导致的累计误差，有助于保证机器人的整体运行精度。而所选取的全局定位方式对全局定位的精度与效率存在直接的影响，目前的全局定位方法存在的一些问题如下：

1)常用的全局定位算法如蒙特卡洛算法依赖于初始定位空间的选取，在实际运算中往往以整张地图作为散播区间，导致其计算速度较为缓慢。

2)进行了地图预处理的栅格定位算法需要对地图进行似然域计算，其计算效率虽高但是随着地图尺寸的增大整体匹配时间也会提升，导致计算速度缓慢。

发明内容

本发明要解决的技术问题是提供一种基于特征热力图的移动机器人全局定位方法。

为了解决上述技术问题，本发明采取以下技术方案：

一种基于特征热力图的移动机器人全局定位方法，包括以下步骤：

输入数据信息，场景地图m、雷达测量点集rp、雷达扫描点数rn、雷达扫描角度rθ以及雷达扫描半径rr；

S1，获取场景地图中的可行进区域的可行进点，所有的可行进点构成点集p；

S2，获取所有的可行进点的模拟雷达仿真数据，得到存储所有雷达仿真数据的整体点集p_(β,L)，表示每个点对应自己的一组雷达模拟点集；

S3，以整体点集作为输入参数，计算在不同扫描半径下的点特征数和线特征数；

S4，计算在不同半径下的点特征和线特征所有组合的结果点集；

S5，计算雷达测量点集在不同半径下的点特征数和线特征数；

S6，根据雷达测量点集计算得到的点特征数和线特征数，在由整体点集计算得到的点特征数和线特征数中查找对应的点集，通过步骤S5的结果在步骤4的结果中查找点集并取交集；

S7，将集合点集作为输入点集进行全局定位计算，计算结果即为全局定位的结果。

所述步骤S1具体为：

S1.1，获取场景地图m后对该场景地图进行灰度二值化处理：

m_grid＝cvtcolor(m)

m_cvt＝threshold(m_grid)

S1.2，针对场景地图中的移动机器人与边界的距离设为d，对二值化的场景地图进行腐蚀处理，将场景地图中的黑色障碍物区域扩散，得到腐蚀地图m_ero：

m_ero＝erode(d,m_grid)

S1.3，对腐蚀地图进行遍历处理，获取其中的白色点作为可行进点。

所述步骤S2具体为：

S2.1，获取点集p中当前点的位姿向量β，之后遍历其他的所有点：

β＝{β_x,β_y,β_θ}；

S2.2，获取每个可行进点在进行雷达数据模拟时的初始角度A与角度增量A_gap：

A＝β_θ,A_gap＝rθ/(rn-1)

将步骤S2.2循环rn次，每次循环时执行角度A的递增操作：

A＝A+A_gap；

S2.2.1，针对所有的角度A，在当前可行进点的中心处开始发射射线，其中先计算当前射线端点当前所处位置在场景地图m中的相对坐标(rx,ry)，并设射线当前长度为rl，最后计算射线的xy轴上的增量rx_gap与ry_gap：

rx＝(β_x-m_xmin)/m_r,rx_gap＝cos(A)

ry＝(β_y-m_ymin)/m_r,ry_gap＝sin(A)

m_r表示地图分辨率；

S2.2.2对应递增计算发射过程中射线端点(rx,ry)的位置，进行三类条件的判断，首先判断当前射线位置当前是否触及到地图中的障碍物，障碍物即为场景地图中的黑色位置：

然后判断当前射线长度rl是否已经到达雷达的半径长度rr：

最后判断当前射线端点位置(rx,ry)是否已经相对场景地图发生了越界：

如果当前射线端点位置(rx,ry)同时满足上述三个条件，则对当前射线端点进行递增操作，然后继续循环执行步骤S2.2.2：

rx＝rx+rx_gap,ry＝ry+ry_gap,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的值进行半径区间选取：

rr_i＝{rr_i/1,rr_i/2,...,rr_i/t}

上式中，t为预设值，

然后遍历所有的半径rr_i来分别进行计算；

S3.2，遍历点集p_(e,L)中的每个点β所有子点集L，计算子点集中的线特征值与点特征值，其中涉及到直线检测过程与角点检测过程；

S3.2.1，直线检测过程：

首先建立一个累计矩阵M(k,b)，其中的k表示所取的斜率，选取范围[k_max,k_min]，b表示所取的截距，选取范围[b_max,b_min]，根据不同地图的不同需求，设计不同k与b的取值范围，以及两者的计算增量值k_gap与b_gap，

然后遍历当前点集L中的所有点L_i，根据斜率k的范围计算对应的每个点的截距b的数值，并将对应的数值累加到累计矩阵M(k,b)中：

b＝-kL_ix+L_iy

M(rd(k),rd(b))＝M(rd(k),rd(b))+1

k＝k+k_gap,b＝b+b_gap

上式中，函数rd表示对当前数值取整；

最后设计阈值st来遍历判断每个子点集L的累计矩阵M(k,b)，并根据最终超过阈值的结果来确定当前子点集L中的线特征个数L_l，同时将识别为线特征的直线以两点形式存入直线特征结果

中：

S3.2.2，角点检测过程：

首先遍历子点集L中的每个点，计算每个点在当前位置的梯度G^(i,j)：

然后遍历每个点的所有临近点计算窗口矩阵W，并根据该矩阵W的特征值λ₁与λ₂来计算角点特征结合值C以进行下一步计算：

C^(i,j)＝λ₁λ₂-K(λ₁+λ₂)²

上式中K为预设系数，

接着设计阈值ct来限定所选取的角点特征结合值C，当点集L中存在点的值大于阈值ct时，将该满足阈值ct要求的点插入结果点集

中，并递增点特征个数L_c的值：

在上述计算完成后将结果点集

与角点特征数L_c输出。

S3.3，通过步骤S3.1与S3.2，得到所有半径rr_i下，点集p_(e,L)的点特征数值L_c与线特征数值L_l，此时界定该数据整合存储为F_(R,P)，其中R表示当前点集的计算半径，P表示存储了当前计算半径下所有点的坐标信息与特征数量的数据结构，表示为：

P_i＝{x,y,L_l,L_c}i＝sizeof(p_(e,L))。

所述步骤S4具体为：

S4.1，根据步骤S3的结果F_(R,P)，遍历所有的半径R进行计算：

S4.2，遍历当前点集P中所有点的数据，获取其中最大的点特征数L_cmax与线特征数L_lmax：

L_cmax＝searchmaxL_c inF(R,P),L_lmax＝searchmaxL_l inF(R,P)

S4.3，根据L_cmax与L_lmax形成点特征数与线特征数的所有组合，并遍历每个组合，选取当前点集P中符合当前组合的点，加入到结果

中，并根据每个组合绘制特征热力图。

所述步骤S5中，计算雷达测量点集所使用到的半径，与步骤S3中计算整体点集所使用到的半径相同，以雷达测量点集rp作为输入参数，按照步骤S3的具体步骤计算得到数据结果为

表示在不同半径下雷达测量点集的点特征数量与线特征数量。

所述步骤S6具体为：根据半径遍历步骤S5的结果

得到在当前半径R下的雷达点集的点特征数L_c与线特征数L_l，并根据点特征数L_c与线特征数L_l结果在步骤S4的结果中选取对应的点集，最终将交集在场景地图中使用红点标记，

L_l＝L_l in rF(R,L_l,L_c),L_c＝L_c in rF(R,L_l,L_c)

所述步骤S7具体为：

S7.1，将步骤S6的集合点集作为初始粒子点集，设置粒子整体个数pN与粒子权重分布ω_t；

S7.2，设计一个单一的随机数pr用于采样计算，并将粒子群的第一个粒子的权值作为计算阈值pt：

pr＝rand(0；pN^-1),

i＝1

S7.3，遍历所有的粒子，计算当前粒子的计算权值U与阈值pt的关系，当粒子的计算权值U大于阈值pt时，对阈值根据当前粒子i的权重

来进行递增操作，直至当前计算权值U小于阈值pt：

U＝pr+(pn-1)·pN^-1

U>pt

i＝i+1

上式中，F^sol为输出点集，最终的定位结果将会选取该点集的中心点作为全局定位的结果。

本发明具有以下有益效果：

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后首先对其进行灰度二值化处理：

m_grid＝cvtcolor(m)

m_cvt＝threshold(m_grid)

上式中，m_grid为灰度地图，即将原RGB地图转换为单色地图，m_cvt为二值化地图，该二值化地图保证其中只存在黑色与白色区域。

S1.2，针对场景地图中的移动机器人与边界的距离设为d，对二值化后的场景地图进行腐蚀处理，将原场景地图中的黑色障碍物区域扩散，得到腐蚀地图m_ero：

m_ero＝erode(d,m_grid)。

S1.3，对腐蚀地图进行遍历处理，获取其中的白色点作为可行进点：

forallPoint(i,j)inm_ero do

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与角度增量A_gap：

A＝β_θ,A_gap＝rθ/(rn-1)

将步骤S2.2循环rn次，每次循环时执行角度A的递增操作：

A＝A+A_gap。

S2.2.1，针对所有的角度A，在当前可行进点的中心处开始发射射线，其中先计算当前射线端点当前所处位置在场景地图m中的相对坐标(rx,ry)，并设射线当前长度为rl，最后计算该射线xy轴上的增量rx_gap与ry_gap：

rx＝(β_x-m_xmin)/m_r,rx_gap＝cos(A)

ry＝(β_y-m_ymin)/m_r,ry_gap＝sin(A)

上式中，m_r表示地图分辨率，可理解为实际尺寸(单位为米)与场景地图中一个像素点的比值大小。

S2.2.2，对应递增计算发射过程中射线端点(rx,ry)的位置，随着射线的发射，端点位置是不断产生距离变化的，对其进行三类条件的判断：

1)首先判断该位置当前是否触及到场景地图中的障碍物，即场景地图中的黑色区域位置：

if threshold ofPoint(rx,ry)inm≠255

2)然后判断当前射线长度rl是否已经到达雷达的半径长度rr：

if rl≠rr

3)最后判断当前射线端点位置(rx,ry)是否已经相对场景地图发生了越界：

ifrx＜m_xmax andrx＞m_xmin andry＜m_ymax andry＞m_ymin

如果当前射线端点位置(rx,ry)同时满足上述条件，即同时没有触及到场景地图中的障碍物，当前射线长度没有到达雷达的半径长度，以及当前射线端点位置还在场景地图内没有发生越界，则对该端点进行递增操作，然后继续循环执行S步骤2.2.2：

rx＝rx+rx_gap,ry＝ry+ry_gap,rl＝rl+1；直到获取所有的行进点在进行雷达数据模拟时的角度A与角度增量A_gap。

如果当前射线端点位置(rx,ry)没有同时满足上述条件，则跳出步骤S2.2.2步骤。比如，当前射线端点位置已经触及到场景地图中的障碍物，那么跳出S2.2.2；或者虽然没有触及到场景地图中的障碍物，但是，当前射线长度已经达到了雷达的半径长度，同样跳出S2.2.2；或者没有触及到场景地图中的障碍物，以及当前射线长度没有达到雷达的半径长度，但是发生了越界情况，那么同样跳出S2.2.2。

S2.2.3，判断当前射线端点(rx,ry)是否是已经到达当前雷达半径长度：

如果rl等于rr，则继续执行步骤S2.2，直到获取所有的行进点在进行雷达数据模拟时的角度A与角度增量A_gap。

如果rl不等于rr，则将当前射线端点(rx,ry)存入结果点集L中，并继续执行步骤S2.2，直到获取所有的行进点在进行雷达数据模拟时的角度A与角度增量A_gap。

S2.3，将结果点集L输出，其中点集L存储着角度与其角度方向上射线的终止坐标的映射关系。

S2.4，将所有p中的点计算完毕后，得到存储所有雷达仿真数据的整体点集p_(β,L)，其表示每个点对应自己的一组雷达模拟点集。

步骤S3：计算步骤S2的整体点集p_(β,L)在不同扫描半径下的点特征数与线特征数，具体为：

3.1以步骤S2的结果点集p_(e,L)为本步骤的主要输入参数，根据rr的值进行适当的半径区间选取：

rr_i＝{rr_i/1,rr_i/2,...,rr_i/t}

上式中，t的取值方式与取值个数根据实际方式而定；

然后遍历所有的半径rr_i来分别进行之后的计算过程。

S3.2，遍历整体点集p_(e,L)中的每个点β所有子点集L，计算子点集中的线特征值与点特征值，其中涉及到直线检测过程与角点检测过程。

S3.2.1直线检测过程：

首先建立一个累计矩阵M(k,b)，其中的k表示所取的斜率，存在选取范围[k_max,k_min]，b表示所取的截距，存在选取范围[b_max,b_min]。根据不同地图的不同需求，可以设计不同k与b的取值范围，以及两者的计算增量值k_gap与b_gap。

然后遍历当前子点集L中的所有点L_i，根据斜率k的范围计算对应的每个点的截距b的数值，并将对应的数值累加到累计矩阵M(k,b)中：

b＝-kL_ix+L_iy

M(rd(k),rd(b))＝M(rd(k),rd(b))+1

k＝k+k_gap,b＝b+b_gap

上式中，函数rd表示对当前数值取整。

最后通过设计阈值st来遍历判断每个子点集L的累计矩阵M(k,b)，并根据最终超过阈值的结果来确定当前子点集L中的线特征个数L_l，同时将识别为线特征的直线以两点形式存入直线特征结果

中：

M(k,b)>st

L_l＝L_l+1。

S3.2.2，角点检测过程：

首先遍历子点集L中的每个点，计算每个点的在当前位置的梯度G^(i,j)：

然后遍历每个点的所有临近点计算窗口矩阵W，并根据该矩阵W的特征值λ₁与λ₂来计算角点的特征结合值C以进行下一步计算：

C^(i,j)＝λ₁λ₂-K(λ₁+λ₂)²

上式中K为预设系数，根据实际情况不同进行选定。

接着根据实际情况设计阈值ct来限定所选取的角点特征结合值C，当点集L中存在点满足阈值要求时，将其插入结果点集

中，并递增点特征个数L_c的值：

C^(i,j)＞ct

最后需要根据在步骤S3.2.1中所选取的直线进行角点位置的限定，因为此时角点检测出的角点是在模拟雷达点集下检测出来的，可能有一些角点在直线检测的对比中会失效，所以此处需要加以限制如下：

遍历所有的角点c，在其内部循环遍历结果点集

中的所有直线端点，判断是否存在有且仅有两条直线与当前角点的欧式距离维持在距离阈值dt之内，同时两条直线的角度也满足角度阈值at：

上式中，距离阈值dt与角度阈值at根据实际所需计算情况而限定，(x_n,y_n)表示直线上任意端点，(x_c,y_c)表示当前角点c的坐标，(x₀,y₀)与(x₁,y₁)表示一条直线的两端点，(x₀,y₀)与(x₁,y₁)表示另一条直线的两端点。

一旦发现无法满足上述条件，则从果点集

中删除掉当前角点c，同时角点特征数L_c也将自减1。

在上述计算完成后将结果点集

与角点特征数L_c输出。

S3.3，通过步骤S3.1与S3.2，得到所有半径rr_i下，点集p_(e,L)的点特征数值L_c与线特征数值L_l。此时界定该数据整合存储为F_(R,P)，其中R表示当前点集的计算半径，P表示存储了当前计算半径下所有点的坐标信息与特征数量的数据结构，表达为：

P_i＝{x,y,L_l,L_c}i＝sizeof(p_(e,L))

步骤S4：计算步骤S3获取的结果F_(R,P)在不同半径下点特征与线特征所有组合的结果点集。具体为：

S4.1，根据步骤S3的结果F_(R,P)，遍历所有的半径R进行计算：

S4.2，遍历当前点集P中所有点的数据，获取其中最大的点特征数L_cmax与线特征数L_lmax：

L_cmax＝search max L_c inF_(R,P),L_lmax＝search max L_l inF_(R,P)。

S4.3，根据L_cmax与L_lmax形成点特征数与线特征数的所有组合，并遍历每个组合，选取当前点集P中符合当前组合的点，加入到结果

中，并根据每个组合绘制特征热力图，具体如图4所示。

步骤S5：计算雷达测量点集在不同半径下的点特征数与线特征数。

其计算方式与步骤3的基本相同，区别在于其输入参数变为雷达测量点集rp，并且仅计算这一个点集。同时这里的半径选取需要与步骤S3中的一致，保证计算结果能够正常匹配。

最终步骤S5得到的数据结果为

其表示在不同半径下雷达测量点集的点特征数量与线特征数量。

步骤S6：通过步骤S5的结果

在步骤4的结果中查找点集并取交集。交集在场景地图中使用红点标记，如图5所示：

步骤S7：将步骤S6的结果作为输入点集进行全局定位计算，具体如下所示：

S7.1，全局定位算法需要步骤S6的结果作为初始粒子点集，并根据实际情况设计粒子整体个数pN与粒子权重分布ω_t。

S7.2，设计一个单一的随机数pr用于采样计算，并将粒子群的第一个粒子的权值作为计算阈值pt：

pr＝rand(0；pN^-1),

i＝1

S7.3，遍历所有的粒子，计算当前粒子的计算权值U与阈值pt的关系：

U＝pr+(pn-1)·pN^-1

当粒子的计算权值U大于阈值pt时，对阈值根据当前粒子i的权重

来进行递增操作，直至当前计算权值U小于阈值pt。

最终将

中的数据全部插入到输出点集F^sol中，最终的定位结果将会选取该点集的中心点作为全局定位的结果。

需要说明的是，以上仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，但是凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于特征热力图的移动机器人全局定位方法，包括以下步骤：

输入数据信息，场景地图m、雷达测量点集rp、雷达扫描点数rn、雷达扫描角度rθ以及雷达扫描半径rr；

S1，获取场景地图中的可行进区域的可行进点，所有的可行进点构成点集；

S2，获取所有的可行进点的模拟雷达仿真数据，得到存储所有雷达仿真数据的整体点集p_(β,L)，表示每个点对应自己的一组雷达模拟点集；

S3，以整体点集作为输入参数，计算在不同扫描半径下的点特征数和线特征数；

S4，计算在不同半径下的点特征和线特征所有组合的结果点集；

S5，计算雷达测量点集在不同半径下的点特征数和线特征数；

S6，通过步骤S5的结果

在步骤4的结果中查找点集并取交集；

S7，将步骤S6的结果作为输入点集进行全局定位计算，计算结果即为全局定位的结果；

所述步骤S4具体为：

S4.1，根据步骤S3的结果F_(R,P)，遍历所有的半径R进行计算：

S4.2，遍历当前点集P中所有点的数据，获取其中最大的点特征数L_cmax与线特征数L_lmax：

L_cmax＝search max L_c in F(R,P),L_lmax＝search max L_l in F(R,P)

S4.3，根据L_cmax与L_lmax形成点特征数与线特征数的所有组合，并遍历每个组合，选取当前点集P中符合当前组合的点，加入到结果

中，并根据每个组合绘制特征热力图；

所述步骤S5中，计算雷达测量点集所使用到的半径，与步骤S3中计算整体点集所使用到的半径相同，以雷达测量点集rp作为输入参数，按照步骤S3的具体步骤计算得到数据结果为

表示在不同半径下雷达测量点集的点特征数量与线特征数量；

所述步骤S6具体为：根据半径遍历步骤S5的结果

得到在当前半径R下的雷达点集的点特征数L_c与线特征数L_l，并根据点特征数L_c与线特征数L_l结果在步骤S4的结果中选取对应的点集，最终将交集在场景地图中使用红点标记，

L_l＝L_l in rF(R,L_l,L_c),L_c＝L_c in rF(R,L_l,L_c)

2.根据权利要求1所述的基于特征热力图的移动机器人全局定位方法，其特征在于，所述步骤S1具体为：

S1.1，获取场景地图m后对该场景地图进行灰度二值化处理：

m_grid＝cvtcolor(m)

m_cvt＝threshold(m_grid)

上式中，m_grid为灰度地图，即将原RGB地图转换为单色地图，m_cvt为二值化地图，该二值化地图保证其中只存在黑色与白色区域；

S1.2，针对场景地图中的移动机器人与边界的距离设为d，对二值化后的场景地图进行腐蚀处理，将场景地图中的黑色障碍物区域扩散，得到腐蚀地图m_ero：

m_ero＝erode(d,m_grid)

S1.3，对腐蚀地图进行遍历处理，获取其中的白色点作为可行进点。

3.根据权利要求1所述的基于特征热力图的移动机器人全局定位方法，其特征在于，所述步骤S2具体为：

S2.1，获取点集p中当前点的位姿向量β，之后遍历其他的所有点：

β＝{β_x,β_y,β_θ}；

S2.2，获取每个可行进点在进行雷达数据模拟时的初始角度A与角度增量A_gap：

A＝β_θ,A_gap＝rθ/(rn-1)

将步骤S2.2循环rn次，每次循环时执行角度A的递增操作：

A＝A+A_gap；

S2.2.1，针对所有的角度A，在当前可行进点的中心处开始发射射线，其中先计算当前射线端点当前所处位置在场景地图m中的相对坐标(rx,ry)，并设射线当前长度为rl，最后计算射线的xy轴上的增量rx_gap与ry_gap：

rx＝(β_x-m_xmin)/m_r,rx_gap＝cos(A)

ry＝(β_y-m_ymin)/m_r,ry_gap＝sin(A)

m_r表示地图分辨率；

S2.2.2对应递增计算发射过程中射线端点(rx,ry)的位置，进行三类条件的判断，首先判断当前射线位置当前是否触及到地图中的障碍物，障碍物即为场景地图中的黑色位置：

然后判断当前射线长度rl是否已经到达雷达的半径长度rr：

最后判断当前射线端点位置(rx,ry)是否已经相对场景地图发生了越界：

如果当前射线端点位置(rx,ry)同时满足上述三个条件，则对当前射线端点进行递增操作，然后继续循环执行步骤S2.2.2：

rx＝rx+rx_gap,ry＝ry+ry_gap,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的值进行半径区间选取：

rr_i＝{rr_i/1,rr_i/2,...,rr_i/t}

上式中，t为预设值，

然后遍历所有的半径rr_i来分别进行计算；

S3.2，遍历点集p_(e,L)中的每个点β所有子点集L，计算子点集中的线特征值与点特征值，其中涉及到直线检测过程与角点检测过程；

S3.2.1，直线检测过程：

首先建立一个累计矩阵M(k,b)，其中的k表示所取的斜率，选取范围[k_max,k_min]，b表示所取的截距，选取范围[b_max,b_min]，根据不同地图的不同需求，设计不同k与b的取值范围，以及两者的计算增量值k_gap与b_gap，

然后遍历当前点集L中的所有点L_i，根据斜率k的范围计算对应的每个点的截距b的数值，并将对应的数值累加到累计矩阵M(k,b)中：

b＝-kL_ix+L_iy

M(rd(k),rd(b))＝M(rd(k),rd(b))+1

k＝k+k_gap,b＝b+b_gap

上式中，函数rd表示对当前数值取整；

最后设计阈值st来遍历判断每个子点集L的累计矩阵M(k,b)，并根据最终超过阈值的结果来确定当前子点集L中的线特征个数L_l，同时将识别为线特征的直线以两点形式存入直线特征结果

中：

S3.2.2，角点检测过程：

首先遍历子点集L中的每个点，计算每个点在当前位置的梯度G^(i,j)：

然后遍历每个点的所有临近点计算窗口矩阵W，并根据该矩阵W的特征值λ₁与λ₂来计算角点特征结合值C以进行下一步计算：

C^(i,j)＝λ₁λ₂-K(λ₁+λ₂)²

上式中K为预设系数，

接着设计阈值ct来限定所选取的角点特征结合值C，当点集L中存在点的值大于阈值ct时，将该满足阈值ct要求的点插入结果点集

中，并递增点特征个数L_c的值：

在上述计算完成后将结果点集

与角点特征数L_c输出；

S3.3，通过步骤S3.1与S3.2，得到所有半径rr_i下，点集p_(e,L)的点特征数值L_c与线特征数值L_l，此时界定该数据整合存储为F_(R,P)，其中R表示当前点集的计算半径，P表示存储了当前计算半径下所有点的坐标信息与特征数量的数据结构，表示为：

P_i＝{x,y,L_l,L_c} i＝sizeof(p_(e,L))。

5.根据权利要求1所述的基于特征热力图的移动机器人全局定位方法，其特征在于，所述步骤S7具体为：

S7.1，将步骤S6的集合点集作为初始粒子点集，设置粒子整体个数pN与粒子权重分布ω_t；

S7.2，设计一个单一的随机数pr用于采样计算，并将粒子群的第一个粒子的权值作为计算阈值pt：

pr＝rand(0；pN^-1),

S7.3，遍历所有的粒子，计算当前粒子的计算权值U与阈值pt的关系，当粒子的计算权值U大于阈值pt时，对阈值根据当前粒子i的权重

来进行递增操作，直至当前计算权值U小于阈值pt：

U＝pr+(pn-1)·pN^-1

U>pt

i＝i+1

上式中，F^sol为输出点集，最终的定位结果将会选取该点集的中心点作为全局定位的结果。

Images