一种结合人工势场的概率图谱路径规划方法

1.本发明属于路径规划领域，特别涉及适用于含有狭窄通道的静态环境中的路径规划。


背景技术：

2.近年来，自主移动机器人在许多领域得到了广泛应用，如货物运输、搜索救援、水下巡航等。路径规划是使机器人能够自主规划从起始位置到目的地的可行无碰撞路径的最关键的技术之一。它能够确保所规划的路径在某些条件下是最优的，例如最小的工作成本、最短的步行路线、最短步行时间等。在过去几十年里，已经有许多种用于机器人路径规划的方法被提出。其中，概率路线图法(probabilistic roadmap methon，prm)由于其良好的性能得到了广泛的应用。prm能够将连续复杂环境种的路径规划问题转化为离散空间种的路径规划问题，降低了路径规划的复杂度。同时通过随机采样的方法，能够有效避免空间维度对算法的影响，具有概率完备性。同时，因为其基于随机采样的特性，环境中的每个区域的采样概率相同，所以当任务环境中存在狭窄通道是，通常会出现由于通道内部没有足够的采样点而导致路径规划失败的情况。这也被称为狭窄通道问题。因此如何解决狭窄通道问题，提高通道内的有效采样点的数量，成为了改进概率路线图法的主要方向。
3.为了解决prm面临的狭窄南干道问题，提高通道内的采样点的有效性，本发明提出了一种改进的基于势场的概率路线图算法，通过提高采样点的有效性和减少计算时间来克服狭窄通道问题。这项工作的主要贡献有三个方面。首先，利用人工势场的方法，用可量化的势场图表示目标环境，从而清晰地表达障碍物信息。其次，提出了一种基于分区的采样策略，在保持采样点均匀分布的同时，提高密集障碍区域的采样点数量。最后，提出一种新的八方位检测法，识别并筛选出了关键点，通过增加一些关键点，从而有效地提高了路径规划的成功率。


技术实现要素：

4.本发明的目的是针对传统概率路线图方法，结合人工势场的思想，提出了一种能够识别狭窄通道内部关键采样点，通过采样关键点，增加狭窄通道的有效采样点的数量，提高路径规划成功率的方法。为了实现该目的，本发明所采用的步骤是：
5.步骤1：建立势力场地图。根据已知的环境信息、设定的斥力场系数η和斥力作用范围d0，结合人工势场法中的斥力场公式，计算得到环境中每一处空间配置点处的斥力场值，得到一张环境的斥力场地图；
6.步骤2：筛选关键点。基于已经的得到的斥力场地图，首先设定一个最大检测距离门限r
max
，根据斥力场公式计算得到在距离障碍物r
max
处的斥力场门限值rep
threshold
，对环境中的空间配置点进行筛选初步得到待筛选关键点集合p
cri
；针对p
cri
中的每一个点，根据检测半径rd对其进行八方位检测法进一步检测筛选，得到狭窄通道内部的三个关键点集，即通道内的关键点集q
passage
，通道口的关键点集q
door.
，通道拐角处的关键点集q
corner.
；
7.步骤3：学习阶段。将环境势力场地图在x和y方向上等分为n
x
和ny份，计算每个区域的总势场值p
tot
(i，j)；定义pm为p
tot
(i，j)的中值，将所有网格分为高电位场值和低电位场值两类；根据所需的采样点总数，确定每个区域所需的采样点数量nodenum(i，j)；针对被障碍物完全覆盖的区域进行采样点补偿；分别对划分的每个区域进行采样点采样，同时在环境中添加关键点；最后连接所有的采样点，形成一张连通图，也称路线图。
8.步骤4：查询阶段。根据获得的路线图，尝试将起点和目标点添加到路线图中。首先添加起点，计算环境中所有节点到起点的距离。邻域距离d内到起点的距离试图通过局部规划器与起点连接的这些点。如果满足条件的所有点都不能与起点连接，则表示起点和目标点之间没有可行路径。目标点的添加同上。如果成功地将起点和目标点添加到路线图中，则使用a*算法计算路线图上的最佳路径。
9.本发明提出的改进型概率路线图方法已经通过matlab中实现的具体案列得到了验证。仿真实验中设置的采样点总数为80和100，每种情况下进行100次重复试验，每个试验指标取平均值。附图1给出了本发明提出的方法流程图。附图2给出了本发明对于不同通道情况关键点的示意图。依据本发明提出的方法，可以识别并区分这些关键点。附图3本发明的实验环境模型，其中圆点为起点，菱形点为终点。附图4为本发明实现的成功率对比结果。附图5为本发明实现的平均路径长度对比结果。附图6为本发明实现的平均算法时间对比结果。
附图说明
10.图1是本发明的实验流程图；
11.图2是本发明的不同通道情况关键点示意图；
12.图3是本发明实验的环境模型；
13.图4是本发明与其他路径规划方法的成功率对比结果；
14.图5是本发明与其他路径规划方法的平均路径长度对比结果；
15.图6是本发明与其他路径规划方法的平均算法时间对比结果。
具体实施方式
16.下面结合附图和实施例对本发明作进一步详细描述。
17.在后面的叙述中，本说明书将本发明提出的改进的基于人工势场的概率路线图的路径规划方法简记为pcprm(an improved probabilistic roadmap method combined with potential field and critical point for path planning)。pcprm首先设定了以下场景参数：
18.1、采样点总数：n。在工作区域内，设定的用于路径规划的环境采样点q的数量。
19.2、领域长度：d。在进行路线图构造的时候，相邻两个邻居节点之间能够构成联通路径的最大长度。
20.3、斥力场系数：η。也称斥力增益，为一个常量。
21.4、斥力作用范围：d0。障碍物产生的斥力场的影响范围。只有当物体距离障碍物小于d0时，才会受到障碍物产生的斥力场的影响。
22.5、狭窄通道内关键点集与分类：q
passage
表示通道内的关键点集，q
door.
表示通道口
的关键点集，q
corner.
表示通道拐角处的关键点集。
23.以上述条件为基础，本发明提出的pcprm已在matlab中实现具体的案列，实现结果证明了该方法的有效性。pcprm的具体实施步骤为：
24.步骤1：构建环境势力场地图。
25.因为在后续的采样阶段只需要用到环境信息就可以合理的分布采样点，因此只需要在环境中构建斥力场地图。其中斥力场公式为
[0026][0027]
在公式中，η表示斥力场系数，也称斥力增益，为一个常量；d(q)表示空间配置点q处距离其附近障碍物的最近距离；d0表示障碍物产生的斥力的作用范围，只有当空间配置点q与障碍物之间的距离小于d0时，障碍物在位置q处才会产生一个斥力场，而当空间配置点q在障碍物作用范围之外时，也就是当d(q)大于d0，时，障碍物在其位置处产生的斥力场大小为0。
[0028]
步骤2：关键点的选择
[0029]
1、初步筛选环境中的关键点
[0030]
首先，设定一个最大距离r
max
作为检测门限距离，根据公式(1)，计算得到在距离障碍物r
max
处的斥力场门限值rep
threshold
。因为在斥力场地图中，障碍物处的斥力场值为最大值，即
[0031][0032]
所以，狭窄通道内的关键点处的斥力场大小rep
cri
满足
[0033][0034]
对于满足这部分条件的关键点集合记为p
cri
。
[0035]
2、通过八方位检测法进一步筛选关键点
[0036]
按照附图2所示的不同狭窄通道情况下关键点的示意图可知，当前筛选出的关键点为灰色区域内的点，所以需要进一步筛选得到关键点。
[0037]
(1)设定八方位检测法的检测半径为rd。
[0038]
(2)对于p
cri
中的每一个点q，分别对其进行上、下、左、右、左上、左下、右上、右下八个方位进行检测。
[0039]
对于待检测点q，通过对点q每个方向上分别进行检测，判断是否在距离点q的rd长范围内，是否检测到障碍物，如果检测到了，则将此检测方向对应的检测标识的值记为1，表示这一方向上检测到了障碍物，否则则记为0。
[0040]
(3)当八个方位都检测完毕后，每个方位的标识都有其对应的值。先通过对左上、左下、右上、右下四个方位的标识进行判断，判断条件为
[0041]
left_up&&right_down&&left_down&&right_up＝1
ꢀꢀꢀ
(4)
[0042]
满足此条件的点记为q
corner.
，同时给予一个判断标识符flag，记录此点为满足公式(4)的点。
[0043]
(4)因为此时q
corner.
中的点不仅包括拐角处的点，还包含了部分通道内的点。因此
需要对其进行上下左右方向标识符的判断，判断条件为
[0044]
up&&down&&left&&right＝1
ꢀꢀꢀ
(5)
[0045]
通过公式(5)，能够进一步筛选得到通道内的点q
passage
以及通道口的点q
door.
。
[0046]
步骤3：学习阶段。进行采样点的选择和路径的构建。
[0047]
(1)划分地图。将获得的势场图在x和y方向上等分为n
x
和ny份。在不同的环境中，在构建路线图时，可以根据环境特征和邻域距离来调整划分区域的数量。
[0048]
(2)计算每个区域的总势场值。对于每个网格(总共n
x
乘以ny)，计算每个网格中所有空间配置点的总势场，并将其记录为p
tot
(i，j)。
[0049]
(3)确定每个区域的采样点数量。将pm定义为p
tot
(i，j)的中值。将所有网格分为两类：高电位场值和低电位场值。p
tot
(i，j)大于pm的网格记为高电势场区域，p
tot
(i，j)小于pm的网格记为低电势场区域。然后通过(6)计算得到不同区域中的采样点的数量。
[0050][0051]
其中，v表示设置的总的采样点数，a，b为采样点分布系数，满足
[0052]
a＝1+k，b＝1-k(0＜k＜1)
ꢀꢀꢀ
(7)
[0053]
(4)采样点补偿。根据每块区域的总势场值p
tot
(i，j)找出被障碍物完全覆盖的区域，将这些区域内分配的采样点进行累加得到所需的补偿采样点的总数，即
[0054]
nodenum
comp
＝∑
(i，j)∈obstacle area
nodenum(i，j)
ꢀꢀꢀ
(8)
[0055]
然后将补偿点均匀分散到未被障碍物覆盖的剩余区域。
[0056]
(5)采样点采样。首先对于每一个区域(i，j)计算一个势场均值p
localm
(i，j)作为门限值。然后在区域(i，j)范围内开始进行采样点采样。当空间配置点q处的势场值repq小于p
localm
(i，j)时，将其作为有效采样点加入到采样点集中。当一个区域内的采样点数量达到指定数量时，停止该区域的采样，进行下一个区域的采样。
[0057]
(6)添加关键点。假设得到的关键点集中含有n个关键点，随机采样λn个关键点到采样点集中。其中λ为常数系数。因为前一步进行普通采样点采样过程中，能够保证在障碍物密度大的区域内同样获得有效的采样点，也就是说明，在狭窄通道内部已经存在一定数量的自由空间内的配置点。所以相对来说，我们更需要位于狭窄通道出入口和狭窄通道拐角处的点作为关键点添加进去。所以在这里，关键点集为狭窄通道出入口的点q
door.
和狭窄通道拐角处的点q
corner.
。
[0058]
(7)路径构造。连接所有采样点形成连通图。具体方法与传统prm相同。
[0059]
步骤4：查询阶段。在得到的路线图上，采用a*算法进行最优路径规划。
[0060]
与传统prm算法的查询阶段一样，根据获得的路线图，尝试将起点和目标点添加到路线图中。首先添加起点，计算环境中所有节点到起点的距离。邻域距离d内到起点的距离试图通过局部规划器与起点连接的这些点。如果满足条件的所有点都不能与起点连接，则表示起点和目标点之间没有可行路径。目标点的添加同上。如果成功地将起点和目标点添加到路线图中，则使用a*算法计算路线图上的最佳路径。
[0061]
本发明申请书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。