CN105844364A - 基于启发函数的服务机器人最优路径规划方法 - Google Patents

Abstract

基于启发函数的服务机器人最优路径规划方法，涉及到移动式服务机器人导航系统中的路径规划方法技术领域。解决目前常用的最优路径规划方法效率低，搜索的准确度差的技术不足，采用改进启发函数的A‑Star算法，鉴于方向角与距离量纲不同的问题，对每一个关联节点相应的距离和方向角度进行归一化处理，避免了对同一节点的重复访问，极大地缩小了搜索空间，从而缩短了算法的运行时间。本发明提出的启发函数包含了方向和距离2个因素，并且针对距离和方向两个量纲不统一的问题，提出将距离和方向归一化处理，提高了搜索效率，规划了最优路径。

Description

基于启发函数的服务机器人最优路径规划方法

技术领域

本发明涉及到移动式服务机器人导航系统中的路径规划方法技术领域。

背景技术

放眼当今世界，机器人的研究、开发与制造水平标志着一个国家或地区的综合科技实力。荟萃着世界高、精、尖科学技术成果及理论在内的新型机器人体现了一个国家和民族的尖端科技水平。随着制造技术、计算机技术、传感技术、信息处理技术、人工智能理论等的不断成熟和发展，机器人研究已经发展到一个暂新的阶段。服务机器人作为一个重要分支，形形色色不同种类的服务机器人正被研制用来满足家庭、商业、军事、国防、反恐救灾、医疗娱乐等国民经济及国防建设各项领域的需求。

移动性是服务机器人的核心要素之一，而路径规划技术是移动式服务机器人导航系统中不可缺少的重要组成部分。路径规划不仅是服务机器人完成任务的安全保障，同时也是服务机器人智能化程度的重要标志。

路径规划中的最短路径问题属于图论范畴，遵循能量最优原则，旨在寻找图中两结点之间的最短路径。最短路经计算分为静态最短路计算和动态最短路计算。静态最短路径算法是外界环境不变，计算最短路径。动态最短路径算法是外界环境不断发生变化，在不能预测的情况下计算最短路径。

目前常用的最优路径规划方法包括有Dijkstra(迪杰斯特拉)算法、A-Star算法等。

1、Dijkstra算法是典型的最短路径路由算法，用于计算一个节点到其他所有节点的最短路径。主要特点是以起始点为中心向外层层扩展，直到扩展到终点为止。Dijkstra算法能得出最短路径的最优解，但由于它遍历计算的节点很多，所以效率低。Dijkstra一般的表述通常有两种方式，一种用永久和临时标号方式，一种是用OPEN,CLOSE表方式；大概过程:创建两个表，Open表和Close表。OPEN表保存所有已生成而未考察的节点，CLOSE表中记录已访问过的节点。1.访问路网中距离起始点最近且没有被检查过的点，把这个点放入OPEN组中等待检查。2.从OPEN表中找出距起始点最近的点，找出这个点的所有子节点，把这个点放到CLOSE表中。3.遍历考察这个点的子节点。求出这些子节点距起始点的距离值，放子节点到OPEN表中。4.重复第2和第3步,直到OPEN表为空，或找到目标点。

2.1A-star算法

A-Star算法是建立在Dijkstra算法基础上的启发式搜索算法，其主要特点在于选择下一个路径节点时引入了已知的路网信息，特别是目标点信息，计算所有候选节点到目标点之间的某种目标函数(比如最短距离、最小时间等)，以此作为评价候选节点是否属于最优路径节点的指标，优先选择具有最优目标函数的候选节点作为下一个路径节点。

A-Star算法的关键是确立启发函数，启发函数可表示为：

F(n)＝G(n)+H(n)

式中，G(n)为从初始节点到任意节点n的实际代价，H(n)为从节点n到目标节点最佳路径的估计代价。

通过控制启发函数可以对A-Star算法的行为进行控制。当H(n)＝0时，只有G(n)发挥作用，则A-Star算法退化为Dijkstra算法，能够找到最短路径但是计算量较大；H(n)不大于从节点n到目标节点的实际代价，则A-Star能够得出最短的路径，H(n)越小，需要计算的节点越多，运行速度越慢；当H(n)等于节点n到目标节点的实际代价时，运行速度和路径都是最优的；当H(n)大于节点n到目标结点的实际代价时，无法得到最短的路径。

2.2A-Star算法的启发函数

服务机器人的导航地图几乎都是栅格地图，在栅格地图中较为常用的启发函数有曼哈顿距离、欧几里得距离等。

曼哈顿距离：

H(n)＝|n(x)-goal(x)|+|n(y)-goal(y)|

欧几里得距离：

H(n)＝sqrt((n(x)-goal(x))²+(n(y)-goal(y))²)

对角线距离的启发函数为：

$H\left(n\right)=\sqrt{2}H\_diagonal\left(n\right)+(H\_straight-2*H\_diagonal)$

H_diagonal(n)＝min(|n(x)-goal(x)|,|n(y)-goal(y)|)

H_straight(n)＝|n(x)-goal(x)|+|n(y)-goal(y)|

传统的以欧氏距离或者曼哈顿距离为估价函数的启发值与实际的代价相差太大，对角线距离没有考虑方向因素，导致了A*算法搜索速度的下降。

发明内容

综上所述，本发明的目的在于考虑到实际地图环境的复杂性，解决目前常用的最优路径规划方法效率低，搜索的准确度差的技术不足，而提出基于启发函数的服务机器人最优路径规划方法。

为解决本发明所提出的技术问题，采用的技术方案为：基于启发函数的服务机器人最优路径规划方法，其特征在于所述方法使用的启发函数包含了方向和距离2个因素，根据:

$\tilde{H}\left(n\right)={\mathrm{\α}}_1\·\mathrm{\θ}+{\mathrm{\α}}_2\·D$

D＝max(|n(x)-goal(x)|,|n(y)-goal(y)|)

式中，α₁为方向启发因子，其决定方向信息所占的权重；θ为候选节点与起点构成的矢量跟终点与起点构成的矢量之间的夹角，其反映了搜索的方向是否指向终点，θ值越小，搜索越朝着终点进行；α₂为距离启发因子，其决定距离信息所占的权重；D为候选节点与终点间的对角距离；

对每一个关联节点相应的距离和方向角度进行归一化处理，改进后的启发函数可表示为：

$H\left(n\right)={\mathrm{\α}}_1\·\mathrm{\θ}/\stackrel{\‾}{\mathrm{\θ}}+{\mathrm{\α}}_2\·D/\stackrel{\‾}{D}$

$\stackrel{\‾}{\mathrm{\θ}}=\frac{1}{j}\underset{i=1}{\overset{j}{\Σ}}{\mathrm{\θ}}_i$

$\stackrel{\‾}{D}=\frac{1}{j}\underset{i=1}{\overset{j}{\Σ}}{D}_i$

式中，j为关联节点的个数；即先计算当前节点所有关联节点相应的方向角度值，然后求平均值；即先计算当前节点所有关联节点相应的对角线距离值，然后求平均值；

所述方法实施步骤如下：

1)初始化设置；构造两个链表，分别存储待扩展的节点和已扩展的节点，分别称为Open表和Close表，将起点信息加载到Open表中，Close表赋值为空，令G(n)＝0；

2)搜索起点周围的8个节点，求的最小值；将节点1从Open表中删除并加载到Close表中；

3)搜索离当前节点最近的节点，求F(n),(n>1)的最小值，将节点n从Open表中删除并加载到Close表中；判断节点n是否为终点，如果是，终点转向步骤5；否则转向步骤4；

4)判断节点n的节点信息是否在确定的范围内，如果在范围内，则扩展节点n；否则加载节点n的节点信息并进行扩展，转向步骤3；由于启发函数包含了方向与对角线距离信息，搜索效率更高；

5)从节点n开始，利用回溯的方法输出起始节点到目标节点的最优路径，以及最短距离，算法终止。

本发明的有益效果为：本发明在路径规划运行过程中，避免了对同一节点的重复访问，极大地缩小了搜索空间，从而缩短了算法的运行时间。A-Star算法作为一种启发式路径规划搜索算法，它利用启发函数减小搜索空间，从而提高搜索效率，因此启发函数在A-Star算法中起着关键的作用。本专利提出的启发函数包含了方向和距离2个因素，并且针对距离和方向两个量纲不统一的问题，提出将距离和方向归一化处理，提高了搜索效率，规划了最优路径。

附图说明

图1为本发明具体案例示意图一；

图2为本发明具体案例示意图二；

图3为本发明具体案例示意图三；

图4为本发明具体案例示意图四；

图5为本发明具体案例示意图五。

具体实施方式

以下结合附图对本发明的路径规划方法作进一步地说明。

本发明采用改进启发函数的A-Star算法

考虑到实际地图环境的复杂性，本发明所使用的启发函数包含了方向和距离2个因素：

$\tilde{H}\left(n\right)={\mathrm{\α}}_1\·\mathrm{\θ}+{\mathrm{\α}}_2\·D$

D＝max(|n(x)-goal(x)|,|n(y)-goal(y)|)

式中，α₁为方向启发因子，其决定方向信息所占的权重；θ为候选节点与起点构成的矢量跟终点与起点构成的矢量之间的夹角，其反映了搜索的方向是否指向终点，θ值越小，搜索越朝着终点进行；α₂为距离启发因子，其决定距离信息所占的权重；D为候选节点与终点间的对角距离。这样在搜索下一节点时，既考虑了该点的方位信息，又兼顾了距离信息，加快了搜索的过程，提高了搜索的准确度。方向角与距离的单位存在不统一，即使角度的单位为弧度、距离的单位为千米，它们之间也很有可能出现距离值远大于角度值的情况，特别是在大区域路径规划过程中，问题更明显。当Dθ时，方向不再有约束力，而使得启发函数失去意义。

鉴于方向角与距离量纲不同的问题，对每一个关联节点相应的距离和方向角度进行归一化处理，改进后的启发函数可表示为：

$H\left(n\right)={\mathrm{\α}}_1\·\mathrm{\θ}/\stackrel{\‾}{\mathrm{\θ}}+{\mathrm{\α}}_2\·D/\stackrel{\‾}{D}$

$\stackrel{\‾}{\mathrm{\θ}}=\frac{1}{j}\underset{i=1}{\overset{j}{\Σ}}{\mathrm{\θ}}_i$

$\stackrel{\‾}{D}=\frac{1}{j}\underset{i=1}{\overset{i}{\Σ}}{D}_i$

式中，j为关联节点的个数；即先计算当前节点所有关联节点相应的方向角度值，然后求平均值；即先计算当前节点所有关联节点相应的对角线距离值，然后求平均值。

本发明方法实施步骤如下：

1)初始化设置；构造两个链表，分别存储待扩展的节点和已扩展的节点，分别称为Open表和Close表，将起点信息加载到Open表中，Close表赋值为空，令G(n)＝0；

2)搜索起点周围的8个节点，求的最小值；将节点1从Open表中删除并加载到Close表中；

3)搜索离当前节点最近的节点，求F(n),(n>1)的最小值，将节点n从Open表中删除并加载到Close表中；判断节点n是否为终点，如果是，终点转向步骤5；否则转向步骤4；

4)判断节点n的节点信息是否在确定的范围内，如果在范围内，则扩展节点n；否则加载节点n的节点信息并进行扩展，转向步骤3；由于启发函数包含了方向与对角线距离信息，搜索效率更高；

5)从节点n开始，利用回溯的方法输出起始节点到目标节点的最优路径，以及最短距离，算法终止。

路径规划运行过程中，避免了对同一节点的重复访问，极大地缩小了搜索空间，从而缩短了算法的运行时间。A-Star算法作为一种启发式路径规划搜索算法，它利用启发函数减小搜索空间，从而提高搜索效率，因此启发函数在A-Star算法中起着关键的作用。本专利提出的启发函数包含了方向和距离2个因素，并且针对距离和方向两个量纲不统一的问题，提出将距离和方向归一化处理，提高了搜索效率，规划了最优路径。

以下结合附图和具体实施例对发明作进一步地说明：

参照图1中所示，以10*10的栅格地图XY中，假设中间灰色区域为障碍物，A0为起点，T0为终点。α₁＝1，α₂＝1。每个方格的边长为1。本发明的路径规划方法步骤如下：

1)如图2所示，将A0放到Open表中；Close表为0；θ(0)＝0，D(0)＝4，G(1)＝0

A1的D(1)＝3

A2的D(1)＝3

A3的D(1)＝3

A4的D(1)＝4

A5的D(1)＝5

A6的D(1)＝5。

A7的D(1)＝5

A8的D(1)＝4

最小的H(1)为A1，F(1)最小的也是A1。将节点A1从Open表中删除并加载到Close表中。

2)如图3中所示，搜索A1周围的节点，由于有障碍物与起点，所以需计算的节点为：

B1的D(2)＝2

B4的D(2)＝3

B6的D(2)＝4

B7的D(2)＝4

B8的D(2)＝3

最小的H(2)为B1，F(2)最小的也是B1。将节点B1从Open表中删除并加载到Close表中。

3)如图4中所示，搜索B1周围的节点，由于有障碍物与Close节点，所以需计算的节点为：

C1的D(3)＝1

C2的D(3)＝1

C3的D(3)＝1

C6的D(3)＝3

C7的D(3)＝3

C8的D(3)＝2

最小的H(3)为C2，F(3)最小的也是C2。将节点C2从Open表中删除并加载到Close表中。

4)如图5中所示，搜索C2周围的节点，由于周围有终点，搜索结束。

5)最终得出的路径规划为：A0-A1-B1-C2-T0。

Claims (1)

1.基于启发函数的服务机器人最优路径规划方法，其特征在于所述方法使用的启发函数包含了方向和距离2个因素，根据：

$\tilde{H}\left(n\right)={\mathrm{\α}}_1\·\mathrm{\θ}+{\mathrm{\α}}_2\·D$

D＝max(|n(x)-goal(x)|,|n(y)-goal(y)|)

式中，α₁为方向启发因子，其决定方向信息所占的权重；θ为候选节点与起点构成的矢量跟终点与起点构成的矢量之间的夹角，其反映了搜索的方向是否指向终点，θ值越小，搜索越朝着终点进行；α₂为距离启发因子，其决定距离信息所占的权重；D为候选节点与终点间的对角距离；

对每一个关联节点相应的距离和方向角度进行归一化处理，改进后的启发函数可表示为：

$H\left(n\right)={\mathrm{\α}}_1\·\mathrm{\θ}/\stackrel{\‾}{\mathrm{\θ}}+{\mathrm{\α}}_2\·D/\stackrel{\‾}{D}$

$\stackrel{\‾}{\mathrm{\θ}}=\frac{1}{j}\underset{i=1}{\overset{j}{\Σ}}{\mathrm{\θ}}_i$

$\stackrel{\‾}{D}=\frac{1}{j}\underset{i=1}{\overset{j}{\Σ}}{D}_i$

式中，j为关联节点的个数；即先计算当前节点所有关联节点相应的方向角度值，然后求平均值；即先计算当前节点所有关联节点相应的对角线距离值，然后求平均值；

所述方法实施步骤如下：

1)初始化设置；构造两个链表，分别存储待扩展的节点和已扩展的节点，分别称为Open表和Close表，将起点信息加载到Open表中，Close表赋值为空，令G(n)＝0；

2)搜索起点周围的8个节点，求的最小值；将节点1从Open表中删除并加载到Close表中；

3)搜索离当前节点最近的节点，求F(n),(n>1)的最小值，将节点n从Open表中删除并加载到Close表中；判断节点n是否为终点，如果是，终点转向步骤5；否则转向步骤4；

4)判断节点n的节点信息是否在确定的范围内，如果在范围内，则扩展节点n；否则加载节点n的节点信息并进行扩展，转向步骤3；

5)从节点n开始，利用回溯的方法输出起始节点到目标节点的最优路径，以及最短距离，算法终止。