一种用于移动机器人自主探索的前沿目标点生成方法.pdf

一种用于移动机器人自主探索的前沿目标点生成方法，属于智能移动机器人领域。本方法能够解决移动机器人探索过程中前沿目标点的提取问题。该方法包括：原始激光数据的预处理，检测局部环境前沿，提取当前局部环境的前沿目标点，全局拓扑地图更新，前沿目标点全局更新。本发明针对未知环境下前沿目标点的提取问题，提出一种安全的、可到达的前沿目标点生成方法。该方法利用激光数据提取当前局部环境的前沿目标点，再结合创建的全局拓扑地图和已生成的前沿目标点，对前沿目标点进行全局更新，以保证前沿目标点全局最优。有效地提高了移动机器人自主探索的效率。该方法适用于与室内移动机器人自主探索有关的智能移动机器人领域。

1.一种用于移动机器人自主探索的前沿目标点生成方法，其特征在于：
在移动机器人探索的过程中，根据采集到的最新激光数据生成当前时刻的局部前沿目
标点，再结合全局拓扑地图信息和已生成的前沿目标点，对所有的前沿目标点进行全局更
新，以保证前沿目标点全局最优；本方法包括以下内容：第一，提出一种基于几何规则的初
始前沿目标点生成方法，根据当前激光数据，提取出局部环境的前沿，再结合几何规则生成
当前局部环境下的初始前沿目标点；第二，引入可通行空间的概念，对初始的前沿目标点进
行调整，保证前沿目标点是安全的、可到达的；第三，沿着移动机器人的运动轨迹，创建当前
时刻的拓扑节点，进而更新全局拓扑地图，用以标记移动机器人已探测到的区域；第四，结
合全局拓扑地图和已生成的前沿目标点，对前沿目标点进行全局更新，保证前沿目标点全
局最优；
本方法提供的方法包括以下步骤：
步骤一，原始激光数据的预处理；
在本方法中，激光测距仪被用于感知外界环境信息；然而，激光测距仪对环境中某些特
殊材质或者颜色是不敏感的，这些材质或者颜色为玻璃、黑色等，这些均会导致原始激光数
据中存在inf或者nan等无效数据；为提高本方法的鲁邦性，首先对原始的激光数据进行预
处理，以剔除原始数据中的无效数据；预处理方法如下：(1)顺序遍历原始激光数据，记录第
一个满足预设的有效数据；(2)若首尾的激光数据出现无效数据，则将该数据赋值为激光数
据的最大量程值；(3)从(1)中记录的有效数据开始，向下和向上顺序访问每一个原始激光
数据，若该数据为有效数据，跳转到第(4)步，否则跳转到第(5)步；(4)顺序访问下一个数
据，若遍历完原始激光数据，则预处理结束，否则跳转到第(3)步；(5)若与该数据相邻的激
光数据均为有效数据，则将较小的数据值赋给该数据，跳转到第(3)步，否则将与之相邻的
有效数据值赋给该数据，跳转到第(3)步；
步骤二，检测局部环境前沿；
本方法将局部环境前沿分为A、B两类前沿；A类前沿是由激光测距仪的量程制约而产生
的，位于激光测距仪最大量程处；B类前沿是由障碍物遮挡而产生的；本方法定义两类前沿
的长度均大于所用移动机器人的宽度，以保证移动机器人能够穿越该前沿；检测局部环境
前沿的方法如下：顺序遍历预处理后的激光数据，若出现连续的激光数据略小于激光测距
仪的最大量程，且该连续的激光数据扫描的弧长大于所用移动机器人的宽度，则认为检测
出了一个A类前沿；若出现相邻两激光数据距离差值的绝对值大于所用移动机器人的长度，
则认为检测出了一个B类前沿；保存本步骤中检测出的A、B两类前沿；
步骤三，提取当前局部环境的前沿目标点；
本方法将前沿目标点分成A、B两类，分别产生于A、B两类前沿附近；提取当前局部环境
前沿目标点的方法如下：(1)针对步骤二中检测出的不同类型的前沿，构建不同的几何规则
集以提取对应的初始前沿目标点；(2)为保证前沿目标点是安全的，且可到达的，构建两类
前沿目标点各自成立的条件，进而对初始的前沿目标点进行调整，使其满足各自成立的条
件；调整后的前沿目标点即为本步骤提取的当前局部环境的前沿目标点；
步骤四，全局拓扑地图更新；
为方便前沿目标点的全局更新，本方法构建全局拓扑地图，以此来标记移动机器人已
探测到的区域；在移动机器人运动的过程中，沿着移动机器人的运动轨迹构建一系列拓扑
节点，并按构建节点的先后顺序构建拓扑树，进而创建全局拓扑地图；全局拓扑地图更新的
方法如下：在移动机器人探索的过程中，每当移动机器人运动一段距离时，采集一次激光数
据，按步骤一、二、三中的方法生成当前局部环境的前沿目标点；再结合当前移动机器人的
位姿构建当前时刻的拓扑节点，并将该拓扑节点添加到全局拓扑地图中以完成全局拓扑地
图的更新操作；在本方法中，全局拓扑地图用来标记移动机器人已探索过的区域；
步骤五，前沿目标点全局更新；
在移动机器人探索的过程中，由于新的前沿目标点的产生和移动机器人的运动，可能
会导致某些前沿目标点出现在已探索的区域，也可能会出现前沿目标点之间过于接近的情
况，此时需利用前沿目标点的全局更新规则对所有的前沿目标点进行添加、绑定与删除更
新操作，以保证前沿目标点全局最优。
2.根据权利要求1所述的一种用于移动机器人自主探索的前沿目标点生成方法，其特
征在于：
本方法包括以下几个步骤：
步骤一，原始激光数据的预处理；
本方法所述方法中利用激光测距仪来感知环境信息，然而激光测距仪对环境中某些特
殊材质或者颜色是不敏感的，这些材质或者颜色为玻璃和黑色等，会导致原始激光数据中
存在inf或者nan无效数据，其中，inf表示距离值为无限大，nan表示没有数据；为提高本方
法所述方法的鲁邦性，本方法首先对原始的激光数据进行预处理，以剔除原始数据中的无
效数据；令d[i]表示原始激光数据中的第i个距离值，dmax和dmin分别表示激光测距仪的最
远和最小的有效距离值，size表示原始激光数据的个数；预处理方法如下：
(1)顺序遍历每个原始激光数据d[i]，若d[i]小于dmin，则将dmax赋值给d[i]；记录第
一个有效数据的索引号j；
(2)令i等于j，若原始激光数据中第0个数据d[0]为无效数据，则将d[0]赋值为dmax；若
原始数据中最后一个数据d[size-1]为无效数据，则将d[size-1]赋值为dmax；
(3)令i等于i+1，若d[i]为无效数据，且i小于size-1，则跳转到第(4)步；若d[i]为有效
数据，且i小于size-1，则重复第(3)；若i等于或者大于size-1，则令i等于j，跳转到第(5)
步；
(4)若与d[i]相邻的两个数据均为有效数据，则将两个数据中较小的值赋给d[i]；若与
d[i]相邻的两个数据中仅有一个有效数据，则将该有效数据赋给d[i]；若i大于j，则跳转到
第(3)步，否则跳转到第(5)步；
(5)令i等于i-1，若d[i]为无效数据，且i大于0，则跳转到第(4)步；若d[i]为有效数据，
且i大于0，则重复第(5)；若i等于或者小于0，则预处理结束；
步骤二，检测局部环境前沿；
本方法将局部环境前沿分为A、B两类前沿；A类前沿是由激光测距仪的量程制约而产生
的，位于激光测距仪最大量程处；B类前沿是由障碍物遮挡而产生的；本方法认为两类前沿
的长度均大于所用移动机器人的宽度，以保证移动机器人能够穿越该前沿；
令当前局部环境下的前沿集合
Fc＝<F0,…,Fi>，
针对每个前沿Fi有
Fi＝<idxs,idxe,type>，
其中，idxs表示该前沿起始端点在激光数据中对应的索引号，idxe表示该前沿终止端点
在激光数据中对应的索引号，type表示该前沿的类型；令fmax表示略小于dmax的一个数，
length表示前沿的长度，angle_inc表示激光测距仪的分辨率；针对A类前沿有
length＝fmax·angle_inc·(idxe-idxs) (1)
检测局部环境下A类前沿的方法如下：顺序遍历预处理后的激光数据，对于每个距离值
均大于fmax的激光数据段，分别用idxs和idxe激光该数据段的开始和终止的索引号，利用公
式(1)计算参数length，若length大于所用移动机器人的宽度，则检测出了A类前沿，并将<
idxs,idxe,A>添加到Fc集合中；继续遍历预处理后的激光数据，直至遍历完所有的激光数
据；
针对局部环境下B类前沿有
length＝|d[idxe]-d[idxs]|, (2)
其中，idxe与idxs是相邻的两个数；检测局部环境下B类前沿的方法如下：顺序遍历预处
理后的激光数据，对于任意两个相邻的激光数据，分别用idxe和idxs记录其索引号，利用公
式(2)计算参数length，若length大于所用移动机器人的宽度，则检测出了B类前沿，并将<
idxs,idxe,B>添加到Fc集合中；继续遍历预处理后的激光数据，直至遍历完所有的激光数
据；
步骤三，提取当前局部环境的前沿目标点；
本方法将前沿目标点类型分为A、B两类，前沿目标点分别产生于A、B两类前沿附近；考
虑到A、B两类前沿产生的原因不同，本方法利用不同的几何规则集来提取对应的前沿目标
点；假设当前时刻为t，令当前时刻的局部前沿目标点集合为则有
$P_c^t=\&lt;p_0,...,p_i\&gt;,$
针对每个前沿目标点pi有
pi＝<position,d,idx,typep>，
其中，position＝<x,y>为前沿目标点pi的全局坐标，d表示pi与激光测距仪中心的距
离，idx表示该目标点在激光数据中方向的索引号，typep表示该前沿目标点的类型；
提取当前局部环境前沿目标点的方法如下：(1)初始前沿目标点提取；针对步骤二中检
测出的不同类型的前沿，构建不同的几何规则集以提取对应的初始前沿目标点；(2)初始前
沿目标点调整；为保证前沿目标点是安全的，且可到达的，针对不同类型的初始前沿目标点
设定不同的成立条件，进而对初始的前沿目标点进行调整，使其满足各自成立的条件；调整
后的前沿目标点即为本步骤提取的当前局部环境的前沿目标点；
(1)初始前沿目标点提取
对A类初始前沿目标点的提取规则如下：
a.将A类前沿的中点与激光测距仪的中心相连；
b.在连线上取一点pi作为当前的一个初始的前沿目标点，使pi与激光测距仪的距离为
fmax；
则pi的各个元素分别为
$\left\{\begin{array}{c}{\mathrm{type}}_p=A\\ idx=({\mathrm{idx}}_s+{\mathrm{idx}}_e)/2\\ d=fmax\end{array}\right.;---\left(3\right)$
此时，会将初始的前沿目标点pi添加到集合
令safe_d略大于二分之一的机器人宽度，本方法对B类初始目标点的提取规则如下：
a.将B类前沿的两端分别标记为N、L；
b.在NL上取一点M，使MN＝safe_d；
c.以激光测距仪的中心位置为圆心，作经过点M的圆弧arc；
d.在过N点的垂线上取一点Q，使NQ＝safe_d，且点Q在自由区域内；
e.作一条由激光测距仪的中心指向点Q的射线，射线与圆弧arc的交点即为初始的前沿
目标点pi；
此时pi的各个元素分别为
$\left\{\begin{array}{c}{\mathrm{type}}_p=B\\ d=\min (d\&lsqb;{\mathrm{idx}}_e\&rsqb;,d\&lsqb;{\mathrm{idx}}_s\&rsqb;)+safe\_d\\ \begin{array}{ccc}idx={\mathrm{idx}}_e-skip,& if& d\&lsqb;{\mathrm{idx}}_s\&rsqb;\&gt;d\&lsqb;{\mathrm{idx}}_e\&rsqb;\end{array}\\ \begin{array}{ccc}idx={\mathrm{idx}}_s+skip,& if& d\&lsqb;{\mathrm{idx}}_s\&rsqb;\&lt;d\&lsqb;{\mathrm{idx}}_e\&rsqb;\end{array}\end{array}\right.;---\left(4\right)$
其中，skip＝safe_d/(angleinc·(d-safe_d))；此时，会将初始的前沿目标点pi添加到
集合
(2)对初始前沿目标点的调整
为方便对初始前沿目标点做调整，本方法引入可通行空间的概念；对机器人来说，可通
行空间内的点是安全的、可到达的；同时，也引入了弧的概念；利用预处理后的激光数据，将
距离机器人最近障碍物到激光最大量程的扫描区域划分为若干层，每层包含若干段弧；令
section表示一条弧，则对每条弧有
section＝{r,ss,se}，
其中，r表示弧的半径，ss和se分别表示弧的首末端点在激光数据中的索引值；
调整的目的是将初始的前沿目标点调整到可通行空间中，但是本方法不会直接计算可
通行空间，而是先构建前沿目标点的成立条件，然后调整初始的前沿目标点使其满足自身
成立的条件，进而保证前沿目标点在可通行空间中；P为当前初始的前沿目标点，实线标记
的圆弧表示与OP相交的一段弧，交点记为J，弧的首末端点分别标记为H、G，与可通行区域边
界的交点分别标记为E、F，θ表示直线OP与弧的右边线的夹角；
A类前沿目标点的成立条件为：

其中，|OP|表示前沿目标点P与激光测距仪中心的距离，d-tho是一个给定阈值，用于限
定A类前沿目标点与激光测距仪中心的最小距离，和分别表示点H与J、H与
E、J与G、F与G之间的弧长；为简化计算，条件a、b简化为

令β表示弧的圆心角，则有
β＝(se-ss)·angle_inc， (5)
那么，有

与A类前沿目标点相比，基于几何位置的考虑，仅对B类初始前沿目标点的方向做出调
整，故B类前沿目标点仅需满足条件a和b即可；本方法通过调整θ和|OP|使目标点P满足自身
成立的条件，其初始的|OP|＝d；针对所有与OP相交的弧，若调整后的θ和|OP|均能使P满足
自身成立的条件，则根据调整后的θ和|OP|计算调整后的前沿目标点的全局位置postion，
否则，舍弃该初始的前沿目标点；令poset＝<x,y,th>表示当前时刻的机器人位姿，其中x、y
分别表示机器人在全局坐标系下的横、纵坐标，th表示当前时刻机器人的正方向与全局坐
标系x轴正方向的夹角；令α表示OP与机器人正方向的夹角，则
$\mathrm{\&alpha;}=(idx-\frac{size}{2})\&CenterDot;angle\_inc,---\left(7\right)$
则有
$\left\{\begin{array}{c}postion.x={\mathrm{pose}}_t.x+d\&CenterDot;\cos \mathrm{\&alpha;}\&CenterDot;\cos th-d\&CenterDot;\sin \mathrm{\&alpha;}\&CenterDot;\sin th\\ postion.y={\mathrm{pose}}_t.y+d\&CenterDot;\cos \mathrm{\&alpha;}\&CenterDot;\sin th+d\&CenterDot;\sin \mathrm{\&alpha;}\&CenterDot;\cos th\end{array}\right.---\left(8\right)$
其中，postion.x、postion.y分别表示postion的横、纵坐标，poset.x、poset.y分别表示
poset的横、纵坐标；
步骤四，全局拓扑地图更新；
为方便前沿目标点的全局更新，本方法构建全局拓扑地图，以此来标记移动机器人已
探测到的区域；在移动机器人运动的过程中，沿着移动机器人的运动轨迹构建一系列拓扑
节点，并按构建节点的先后顺序构建拓扑树，进而创建全局拓扑地图；全局拓扑地图更新的
方法如下：在移动机器人自主探索的过程中，每当移动机器人运动一段距离时，采集一次激
光数据，按步骤一、二、三中的方法生成当前局部环境的前沿目标点；再结合当前移动机器
人的位姿构建当前时刻的拓扑节点，并将该拓扑节点添加到全局拓扑地图中以完成全局拓
扑地图的更新操作；在本方法中，全局拓扑地图用来标记移动机器人已感知到的区域；令
TMap表示全局拓扑地图，则有
TMap＝<T0,…,Tk,…Tt>
其中，TMap表示从0时刻到t时刻构建的全局拓扑地图，Tk表示k时刻构建的一个拓扑节
点；对于每个拓扑节点Tt有
$T_t=\&lt;{\mathrm{pose}}_t,P_c^t,parent,children\&gt;$
其中，poset、与前文所述意义一致，parent和children分别表示拓扑节点Tt的父节点
和子节点；
步骤五，前沿目标点全局更新；
在移动机器人探索的过程中，由于新的前沿目标点的产生和移动机器人的运动，可能
会导致某些前沿目标点出现在已探索的区域，也可能会出现前沿目标点之间过于接近的情
况，此时需利用前沿目标点的全局更新规则对所有的前沿目标点进行添加、绑定与删除更
新操作，以保证前沿目标点全局最优；
令Tt表示当前时刻的拓扑节点，Tk表示前面时刻生成的一个拓扑节点，表示在k时刻
生成的第i个前沿目标点，表示当前时刻生成的第j个前沿目标点，d(pi,T)表示一个前沿
目标点与拓扑节点之间的距离，C(pi,T)表示从拓扑节点T到前沿目标点pi是可通行的，其值
为true，表示pi与T连通，若为false，表示pi与T之间存在障碍物；Vr为一个给定阈值，用于限
定拓扑节点之间的距离，或者是前沿目标点与拓扑节点之间的距离，Vp为一个给定的阈值，
用于限定前沿目标点之间的距离；T＝T/pi表示从拓扑节点T中删除前沿目标点pi，T＝T∪pi
表示将前沿目标点pi与拓扑节点T绑定；前沿目标点全局更新规则如下(如附图5所示):
规则1：
$ifd(p_j^t,T_t)\&lt;V_r,then{T}_t=T_t/p_j^t;$
规则2：
$\begin{array}{c}\begin{array}{cc}if& d(p_j^t,T_k)\&lt;V_r,andC(p_j^t,T_k)==true\end{array}\\ then{T}_t=T_t/p_j^t\end{array};$
规则3：
$\begin{array}{cc}if& d(p_j^t,p_i^k)\&lt;V_p,then{T}_t=T_t/p_i^t\end{array};$
规则4：
$\begin{array}{c}\begin{array}{cc}if& d(p_j^t,T_k)\&lt;V_r,andC(p_j^t,T_k)==false\end{array}\\ then{T}_t=T_t\&cup;p_j^t\end{array};$
规则5：
$\begin{array}{c}\begin{array}{cc}if& d(p_i^k,T_t)\&lt;d(p_i^k,T_k),andC(p_i^k,T_k)==true\end{array}\\ then{T}_k=T_k/p_i^k,T_t=T_i\&cup;p_i^k\end{array}.$

一种用于移动机器人自主探索的前沿目标点生成方法

技术领域

本发明属于智能移动机器人领域，涉及一种前沿目标点的生成方法，尤其涉及一
种用于移动机器人自主探索的前沿目标点生成方法。

背景技术

随着人工智能技术和计算机技术的飞速发展，移动机器人的研究取得了重大突
破。未知环境下的移动机器人自主探索技术作为移动机器人领域的一项关键技术，已引起
了众多学者的关注，该技术在机器人搜救、行星探测和水下探测等领域均得到了广泛应用，
具有巨大的应用需求。移动机器人自主探索的目的是为了在有限的时间内尽可能多的获取
未知环境的信息，而前沿(Frontier)作为区分已探索区域与未知区域的分界线，它附近具
有较多不确定的环境信息。因此，在现存的自主探索方法中，基于前沿的探索策略备受瞩
目。

基于前沿的探索策略可简要的概括为如下过程：首先检测前沿，然后在前沿附近
生成目标点，再结合评价函数选择最具有探索价值的目标点作为最佳的目标点，最后控制
机器人向最佳的目标点运动。针对前沿检测问题，一种较为常见的方法是借用数字图像技
术来处理整个地图信息，进而检测出前沿区域。然而，随着地图数据的增大，这种前沿检测
算法的时间和空间复杂度也会急剧增加。这是制约移动机器人自主探索效率的一个重要因
素。

为提高前沿检测的效率，研究者们提出了多种有效的前沿检测算法。Matan
Keidar提出了一种波阵面前沿检测(Wavefront Frontier Detector,WFD)算法。WFD通过处
理已探索区域的地图信息来检测前沿，而不是直接处理整个地图信息。然而，当已探索的区
域增大时，需要处理的地图信息量随之较大，那么WFD算法的计算量也会急剧膨胀。
P.G.C.N.Senarathne提出一种基于安全和可到达的增量式前沿检测方法，该方法通过处理
被激光测距仪覆盖的局部栅格地图信息来增量式地生成安全的、可到达的前沿信息。然而，
当激光测距仪覆盖区域较大时，栅格地图上前沿检测的效率仍旧不会太理想。前沿检测的
目的是为了生成下一时刻最佳的目标区域或者目标点。鉴于此，Shuzhi Sam Ge利用当前激
光数据建立局部环境的可通行空间，并在可通行空间中提取可能的目标点来指导机器人探
索，该方法避开了基于栅格地图的前沿检测问题，然而在提取的可能目标点中存在非前沿
区域的目标点，会产生不必要的探索开销。

发明内容

为了克服上述前沿检测和目标点生成等方法中存在的问题，以不依赖于栅格地图
即可生成前沿目标点为目的，本发明提供了一种用于移动机器人自主探索的前沿目标点生
成方法。

本发明采用的技术方案如下：

针对未知环境下的移动机器人自主探索问题，提供了一种用于移动机器人自主探
索的前沿目标点生成方法。在移动机器人探索的过程中，根据采集到的最新激光数据生成
当前时刻的局部前沿目标点，再结合全局拓扑地图信息和已生成的前沿目标点，对所有的
前沿目标点进行全局更新，以保证前沿目标点全局最优。本发明提供的方法包括以下内容：
第一，提出一种基于几何规则的初始前沿目标点生成方法，根据当前激光数据，提取出局部
环境的前沿，再结合几何规则生成当前局部环境下的初始前沿目标点；第二，引入可通行空
间(AdmissibleSpace)的概念，对初始的前沿目标点进行调整，保证前沿目标点是安全的、
可到达的；第三，沿着移动机器人的运动轨迹，创建当前时刻的拓扑节点，进而更新全局拓
扑地图，用以标记移动机器人已探测到的区域；第四，结合全局拓扑地图和已生成的前沿目
标点，对前沿目标点进行全局更新，保证前沿目标点全局最优。

本发明提供的方法包括以下步骤：

步骤一，原始激光数据的预处理。

在本方法中，激光测距仪被用于感知外界环境信息。然而，激光测距仪对环境中某
些特殊材质或者颜色是不敏感的，这些材质或者颜色为玻璃、黑色等，这些均会导致原始激
光数据中存在inf或者nan等无效数据。为提高本方法的鲁邦性，首先对原始的激光数据进
行预处理，以剔除原始数据中的无效数据。预处理方法如下：(1)顺序遍历原始激光数据，记
录第一个满足预设的有效数据；(2)若首尾的激光数据出现无效数据，则将该数据赋值为激
光数据的最大量程值；(3)从(1)中记录的有效数据开始，向下和向上顺序访问每一个原始
激光数据，若该数据为有效数据，跳转到第(4)步，否则跳转到第(5)步；(4)顺序访问下一个
数据，若遍历完原始激光数据，则预处理结束，否则跳转到第(3)步；(5)若与该数据相邻的
激光数据均为有效数据，则将较小的数据值赋给该数据，跳转到第(3)步，否则将与之相邻
的有效数据值赋给该数据，跳转到第(3)步。

步骤二，检测局部环境前沿。

本方法将局部环境前沿分为A、B两类前沿。A类前沿是由激光测距仪的量程制约而
产生的，位于激光测距仪最大量程处；B类前沿是由障碍物遮挡而产生的。本方法定义两类
前沿的长度均大于所用移动机器人的宽度，以保证移动机器人能够穿越该前沿。检测局部
环境前沿的方法如下：顺序遍历预处理后的激光数据，若出现连续的激光数据略小于激光
测距仪的最大量程，且该连续的激光数据扫描的弧长大于所用移动机器人的宽度，则认为
检测出了一个A类前沿；若出现相邻两激光数据距离差值的绝对值大于所用移动机器人的
长度，则认为检测出了一个B类前沿。保存本步骤中检测出的A、B两类前沿。

步骤三，提取当前局部环境的前沿目标点。

本方法将前沿目标点分成A、B两类，分别产生于A、B两类前沿附近。提取当前局部
环境前沿目标点的方法如下：(1)针对步骤二中检测出的不同类型的前沿，构建不同的几何
规则集以提取对应的初始前沿目标点；(2)为保证前沿目标点是安全的，且可到达的，构建
两类前沿目标点各自成立的条件，进而对初始的前沿目标点进行调整，使其满足各自成立
的条件。调整后的前沿目标点即为本步骤提取的当前局部环境的前沿目标点。

步骤四，全局拓扑地图更新。

为方便前沿目标点的全局更新，本方法构建全局拓扑地图，以此来标记移动机器
人已探测到的区域。在移动机器人运动的过程中，沿着移动机器人的运动轨迹构建一系列
拓扑节点，并按构建节点的先后顺序构建拓扑树，进而创建全局拓扑地图。全局拓扑地图更
新的方法如下：在移动机器人探索的过程中，每当移动机器人运动一段距离时，采集一次激
光数据，按步骤一、二、三中的方法生成当前局部环境的前沿目标点。再结合当前移动机器
人的位姿构建当前时刻的拓扑节点，并将该拓扑节点添加到全局拓扑地图中以完成全局拓
扑地图的更新操作。在本方法中，全局拓扑地图用来标记移动机器人已探索过的区域。

步骤五，前沿目标点全局更新。

在移动机器人探索的过程中，由于新的前沿目标点的产生和移动机器人的运动，
可能会导致某些前沿目标点出现在已探索的区域，也可能会出现前沿目标点之间过于接近
的情况，此时需利用前沿目标点的全局更新规则对所有的前沿目标点进行添加、绑定与删
除更新操作，以保证前沿目标点全局最优。

本发明的有益效果是：针对未知环境下移动机器人自主探索问题，提出了一种用
于移动机器人自主探索的前沿目标点生成方法。该方法根据当前激光数据和全局拓扑地图
生成全局前沿目标点，提高了移动机器人在自主探索过程中寻找下一时刻目标区域或者目
标点的速度。有效解决了移动机器人自主探索过程中前沿目标点的提取问题，提高了移动
机器人自主探索的效率。

附图说明

图1为本发明所涉及的方法流程图；

图2为A、B两类初始前沿目标点的选取示意图：(a)为A类前沿及A类初始前沿目标
点示意图，(b)为B类前沿及B类初始前沿目标点示意图；

图3为当前局部环境下初始前沿目标点示意图；

图4为初始前沿目标点的调整流程图；

图5为前沿目标点全局更新规则图；

图6为应用本发明提取的前沿目标点示意图：(a)为走廊环境，(b)为走廊和房间等
环境；

图7为应用本发明辅助实现移动机器人自主探索的结果示意图：(a)为实验环境，
(b)为实验结果图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步的详细说明。

本发明所述方法的流程图如附图1所示，包括以下几个步骤：

步骤一，原始激光数据的预处理。

本发明所述方法中利用激光测距仪来感知环境信息，然而激光测距仪对环境中某
些特殊材质或者颜色是不敏感的，这些材质或者颜色为玻璃和黑色等，会导致原始激光数
据中存在inf或者nan无效数据，其中，inf表示距离值为无限大，nan表示没有数据。为提高
本发明所述方法的鲁邦性，本发明首先对原始的激光数据进行预处理，以剔除原始数据中
的无效数据。令d[i]表示原始激光数据中的第i个距离值，dmax和dmin分别表示激光测距仪
的最远和最小的有效距离值，size表示原始激光数据的个数。预处理方法如下：

(1)顺序遍历每个原始激光数据d[i]，若d[i]小于dmin，则将dmax赋值给d[i]。记
录第一个有效数据的索引号j；

(2)令i等于j，若原始激光数据中第0个数据d[0]为无效数据，则将d[0]赋值为
dmax；若原始数据中最后一个数据d[size-1]为无效数据，则将d[size-1]赋值为dmax；

(3)令i等于i+1，若d[i]为无效数据，且i小于size-1，则跳转到第(4)步；若d[i]为
有效数据，且i小于size-1，则重复第(3)；若i等于或者大于size-1，则令i等于j，跳转到第
(5)步；

(4)若与d[i]相邻的两个数据均为有效数据，则将两个数据中较小的值赋给d[i]；
若与d[i]相邻的两个数据中仅有一个有效数据，则将该有效数据赋给d[i]；若i大于j，则跳
转到第(3)步，否则跳转到第(5)步；

(5)令i等于i-1，若d[i]为无效数据，且i大于0，则跳转到第(4)步；若d[i]为有效
数据，且i大于0，则重复第(5)；若i等于或者小于0，则预处理结束。

步骤二，检测局部环境前沿。

本方法将局部环境前沿分为A、B两类前沿。A类前沿是由激光测距仪的量程制约而
产生的，位于激光测距仪最大量程处，如附图2(a)标记的弧形虚线所示；B类前沿是由障碍
物遮挡而产生的，如附图2(b)标记的虚线NL所示。本方法认为两类前沿的长度均大于所用
移动机器人的宽度，以保证移动机器人能够穿越该前沿。

令当前局部环境下的前沿集合

Fc＝<F0,…,Fi>，

针对每个前沿Fi有

Fi＝<idxs,idxe,type>，

其中，idxs表示该前沿起始端点在激光数据中对应的索引号，idxe表示该前沿终止
端点在激光数据中对应的索引号，type表示该前沿的类型。令fmax表示略小于dmax的一个
数，length表示前沿的长度，angle_inc表示激光测距仪的分辨率。针对A类前沿有

length＝fmax·angle_inc·(idxe-idxs) (1)

检测局部环境下A类前沿的方法如下：顺序遍历预处理后的激光数据，对于每个距
离值均大于fmax的激光数据段，分别用idxs和idxe激光该数据段的开始和终止的索引号，利
用公式(1)计算参数length，若length大于所用移动机器人的宽度，则检测出了A类前沿，并
将<idxs,idxe,A>添加到Fc集合中。继续遍历预处理后的激光数据，直至遍历完所有的激光
数据。

针对局部环境下B类前沿有

length＝|d[idxe]-d[idxs]|, (2)其中，idxe与idxs是相邻的两个数。检测局部
环境下B类前沿的方法如下：顺序遍历预处理后的激光数据，对于任意两个相邻的激光数
据，分别用idxe和idxs记录其索引号，利用公式(2)计算参数length，若length大于所用移动
机器人的宽度，则检测出了B类前沿，并将<idxs,idxe,B>添加到Fc集合中。继续遍历预处理
后的激光数据，直至遍历完所有的激光数据。

步骤三，提取当前局部环境的前沿目标点。

本方法将前沿目标点类型分为A、B两类，前沿目标点分别产生于A、B两类前沿附
近。考虑到A、B两类前沿产生的原因不同，本方法利用不同的几何规则集来提取对应的前沿
目标点。假设当前时刻为t，令当前时刻的局部前沿目标点集合为则有

$P_c^t=\&lt;p_0,\mathrm{...},p_i\&gt;,$

针对每个前沿目标点pi有

pi＝<position,d,idx,typep>，

其中，position＝<x,y>为前沿目标点pi的全局坐标，d表示pi与激光测距仪中心的
距离，idx表示该目标点在激光数据中方向的索引号，typep表示该前沿目标点的类型。

提取当前局部环境前沿目标点的方法如下：(1)初始前沿目标点提取。针对步骤二
中检测出的不同类型的前沿，构建不同的几何规则集以提取对应的初始前沿目标点；(2)初
始前沿目标点调整。为保证前沿目标点是安全的，且可到达的，针对不同类型的初始前沿目
标点设定不同的成立条件，进而对初始的前沿目标点进行调整，使其满足各自成立的条件。
调整后的前沿目标点即为本步骤提取的当前局部环境的前沿目标点。

(1)初始前沿目标点提取

对A类初始前沿目标点的提取规则如下：

a.将A类前沿的中点与激光测距仪的中心相连；

b.在连线上取一点pi作为当前的一个初始的前沿目标点，使pi与激光测距仪的距
离为fmax，如附图2(a)所示。

则pi的各个元素分别为

$\left\{\begin{array}{c}{\mathrm{type}}_p=A\\ idx=({\mathrm{idx}}_s+{\mathrm{idx}}_e)/2\\ d=fmax\end{array}\right..---\left(3\right)$

此时，会将初始的前沿目标点pi添加到集合

令safe_d略大于二分之一的机器人宽度，本方法对B类初始目标点的提取规则如
下：

a.将B类前沿的两端分别标记为N、L；

b.在NL上取一点M，使MN＝safe_d；

c.以激光测距仪的中心位置为圆心，作经过点M的圆弧arc；

d.在过N点的垂线上取一点Q，使NQ＝safe_d，且点Q在自由区域内；

e.作一条由激光测距仪的中心指向点Q的射线，射线与圆弧arc的交点即为初始的
前沿目标点pi，如附图2(b)所示。

此时pi的各个元素分别为

$\left\{\begin{array}{c}{\mathrm{type}}_p=B\\ d=\min (d\&lsqb;{\mathrm{idx}}_e\&rsqb;,d\&lsqb;{\mathrm{idx}}_s\&rsqb;)+safe\_d\\ idx={\mathrm{idx}}_e-skip,ifd\&lsqb;{\mathrm{idx}}_s\&rsqb;\&gt;d\&lsqb;{\mathrm{idx}}_e\&rsqb;\\ idx={\mathrm{idx}}_e+skip,ifd\&lsqb;{\mathrm{idx}}_s\&rsqb;\&lt;d\&lsqb;{\mathrm{idx}}_e\&rsqb;\end{array}\right..---\left(4\right)$

其中，skip＝safe_d/(angleinc·(d-safe_d))。此时，会将初始的前沿目标点pi添
加到集合

(2)对初始前沿目标点的调整

如附图3所示，为方便对初始前沿目标点做调整，本方法引入可通行空间的概念，，
对机器人来说，可通行空间内的点是安全的、可到达的。同时，也引入了弧的概念。利用预处
理后的激光数据，将距离机器人最近障碍物到激光最大量程的扫描区域划分为若干层，每
层包含若干段弧。令section表示一条弧，则对每条弧有

section＝{r,ss,se}，

其中，r表示弧的半径，ss和se分别表示弧的首末端点在激光数据中的索引值。

调整的目的是将初始的前沿目标点调整到可通行空间中，但是本方法不会直接计
算可通行空间，而是先构建前沿目标点的成立条件，然后调整初始的前沿目标点使其满足
自身成立的条件，进而保证前沿目标点在可通行空间中。如附图3所示，P为当前初始的前沿
目标点，实线标记的圆弧表示与OP相交的一段弧(section)，交点记为J，弧的首末端点分别
标记为H、G，与可通行区域边界的交点分别标记为E、F，θ表示直线OP与弧的右边线的夹角。

A类前沿目标点的成立条件为：

其中，|OP|表示前沿目标点P与激光测距仪中心的距离，d-tho是一个给定阈值，用
于限定A类前沿目标点与激光测距仪中心的最小距离，和分别表示点H与J、
H与E、J与G、F与G之间的弧长。为简化计算，条件a、b简化为

令β表示弧的圆心角，则有

β＝(se-ss)·angle_inc， (5)

那么，有

与A类前沿目标点相比，基于几何位置的考虑，仅对B类初始前沿目标点的方向做
出调整，故B类前沿目标点仅需满足条件a和b即可。本方法通过调整θ和|OP|使目标点P满足
自身成立的条件，其初始的|OP|＝d。针对所有与OP相交的弧，若调整后的θ和|OP|均能使P
满足自身成立的条件，则根据调整后的θ和|OP|计算调整后的前沿目标点的全局位置
postion，否则，舍弃该初始的前沿目标点。初始前沿目标点调整的程序流程图如附图4所
示。令poset＝<x,y,th>表示当前时刻的机器人位姿，其中x、y分别表示机器人在全局坐标
系下的横、纵坐标，th表示当前时刻机器人的正方向与全局坐标系x轴正方向的夹角。令α表
示OP与机器人正方向的夹角，则

$\mathrm{\&alpha;}=(idx-\frac{size}{2})\&CenterDot;angle\_inc,---\left(7\right)$

则有

$\left\{\begin{array}{c}posion.x={\mathrm{pose}}_t.x+d\&CenterDot;\cos \mathrm{\&alpha;}\&CenterDot;\cos th-d\&CenterDot;\sin \mathrm{\&alpha;}\&CenterDot;\sin th\\ postion.y={\mathrm{pose}}_t.y+d\&CenterDot;\cos \mathrm{\&alpha;}\&CenterDot;\sin th+d\&CenterDot;\sin \mathrm{\&alpha;}\&CenterDot;\cos th\end{array}\right.---\left(9\right)$

其中，postion.x、postion.y分别表示postion的横、纵坐标，poset.x、poset.y分别
表示poset的横、纵坐标。

步骤四，全局拓扑地图更新。

为方便前沿目标点的全局更新，本发明构建全局拓扑地图，以此来标记移动机器
人已探测到的区域。在移动机器人运动的过程中，沿着移动机器人的运动轨迹构建一系列
拓扑节点，并按构建节点的先后顺序构建拓扑树，进而创建全局拓扑地图。全局拓扑地图更
新的方法如下：在移动机器人自主探索的过程中，每当移动机器人运动一段距离时，采集一
次激光数据，按步骤一、二、三中的方法生成当前局部环境的前沿目标点。再结合当前移动
机器人的位姿构建当前时刻的拓扑节点，并将该拓扑节点添加到全局拓扑地图中以完成全
局拓扑地图的更新操作。在本方法中，全局拓扑地图用来标记移动机器人已感知到的区域。
令TMap表示全局拓扑地图，则有

TMap＝<T0,…,Tk,…Tt>

其中，TMap表示从0时刻到t时刻构建的全局拓扑地图，Tk表示k时刻构建的一个拓
扑节点。对于每个拓扑节点Tt有

$T_t=\&lt;{\mathrm{pose}}_t,P_c^t,parent,children\&gt;$

其中，poset、与前文所述意义一致，parent和children分别表示拓扑节点Tt的父
节点和子节点。

步骤五，前沿目标点全局更新。

在移动机器人探索的过程中，由于新的前沿目标点的产生和移动机器人的运动，
可能会导致某些前沿目标点出现在已探索的区域，也可能会出现前沿目标点之间过于接近
的情况，此时需利用前沿目标点的全局更新规则对所有的前沿目标点进行添加、绑定与删
除更新操作，以保证前沿目标点全局最优。

令Tt表示当前时刻的拓扑节点，Tk表示前面时刻生成的一个拓扑节点，表示在k
时刻生成的第i个前沿目标点，表示当前时刻生成的第j个前沿目标点，d(pi,T)表示一个
前沿目标点与拓扑节点之间的距离，C(pi,T)表示从拓扑节点T到前沿目标点pi是可通行的，
其值为true，表示pi与T连通，若为false，表示pi与T之间存在障碍物。Vr为一个给定阈值，用
于限定拓扑节点之间的距离，或者是前沿目标点与拓扑节点之间的距离，Vp为一个给定的
阈值，用于限定前沿目标点之间的距离。T＝T/pi表示从拓扑节点T中删除前沿目标点pi，T＝
T∪pi表示将前沿目标点pi与拓扑节点T绑定。前沿目标点全局更新规则如下(如附图5所
示):

规则1(如附图5(a)):

$ifd(p_j^t,T_t)\&lt;V_r,then{T}_t=T_t/p_j^t;$

规则2(如附图5(b)):

$\begin{array}{c}ifd(p_j^t,T_k)\&lt;V_r,andC(p_j^t,T_k)==true\\ then{T}_t=T_t/p_j^t\end{array};$

规则3(如附图5(c)):

$ifd(p_j^t,p_i^k)\&lt;V_p,then{T}_t=T_t/p_i^t;$

规则4(如附图5(d)):

$\begin{array}{c}ifd(p_j^t,T_k)\&lt;V_r,andC(p_j^t,T_k)==false\\ then{T}_t=T_t\&cup;p_j^t\end{array};$

规则5(如附图5(e)):

$\begin{array}{c}ifd(p_i^k,T_t)\&lt;d(p_i^k,T_k),andC(p_i^k,T_t)==true\\ then{T}_k=T_k/p_i^k,T_t=T_i\&cup;p_i^k\end{array}.$

下面给出应用本发明所述方法在室内真实环境下的实验实例。

实验选用自主搭建的移动机器人作为实验平台，机器人配备型号为A1M1的
RPLIDAR360度激光扫描测距仪。本实验实例给出了两个实验，分别是前沿目标点的提取实
验(如附图6所示)和利用本发明所述方法进行自主探索的实验(如附图7所示)。

实验一前沿目标点提取实验

如附图6所示，图中给出了两幅局部的栅格地图，图中分别为拓扑节点的位姿、提
取出的前沿目标点、前沿目标点与拓扑节点之间的从属关系。从图中可以看出，前沿目标点
均位于前沿(图中白色区域与灰色区域的交界线)附近，这表明，本发明所述方法能够用于
提取环境中的前沿目标点。

实验二利用本发明所述方法进行自主探索实验

本实验将本发明所述方法集成在一个单机器人自主探索的系统中，实验环境如附
图7所示，图7(a)为实验的真实场景，图7(b)为移动机器人进行自主探索时构建的栅格地图
和拓扑地图。移动机器人的线速度为0.15m/s，角速度为0.3rad/s，激光测距仪测量距离的
范围为(0，6]m，夹角范围为[-π,π]。

由图7(b)中可以看出，创建的地图完整地表达了室内的闭合环境，且与真实环境
相一致，这表明，本发明所述方法可用于未知环境下移动机器人的自主探索，能够指导移动
机器人完整遍历未知环境。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在
本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护
范围之内。