,, ,

## Directed D* algorithm for dynamic path planning of mobile robots

,, ,

Received: 2019-05-15

Abstract

A directed D* route planning algorithm was proposed, aiming at the low efficiency and high cost of the traditional D* route planning algorithms. In the proposed directed D* algorithm, the key nodes are defined by utilizing the location information of target points and obstacles so that the feasible routes could be determined by a stepwise expanding way, and a directed function is introduced to control the single searching range of nodes in order to improve the searching efficiency. A path smoothness function which punishes the deviation of paths is introduced to the algorithm in addition to Euclidean evaluation function, in order to avoid the redundant turning of robots and reduce the costs. The length of the path and the smoothness are taken into account simultaneously by the turning factor of the path smoothness function. The piecewise principle of the path smoothness function and the determining method of the turning factor are proposed. The convergence of the algorithm was also proved. Simulation experiments in different environments show that the proposed algorithm can balance the local searching and the global optimality, and it is especially suitable for complex environments with many obstacles.

Keywords： dynamic path planning ; directed D* algorithm ; steering function ; path smoothness function ; turning factor

LIU Jun, FENG Shuo, REN Jian-hua. Directed D* algorithm for dynamic path planning of mobile robots. Journal of Zhejiang University(Engineering Science)[J], 2020, 54(2): 291-300 doi:10.3785/j.issn.1008-973X.2020.02.010

D*算法[16]是由Stentz提出的动态规划算法，机器人在向目标点移动过程中只检查最短路径临近节点的变化情况，因此具有较高的动态搜索效率，且可以处理任何成本参数发生变化的路径成本优化问题[17]. 由于其通过等势线逐级扩展的方式进行搜索，搜索空间较大，尤其在初始路径规划、环境复杂程度较高时，搜索空间大的问题更加突出；在单次扩展搜索时仅考虑欧几里得距离，易出现在小范围区域内多次转弯的问题，增加移动时间成本[18]. 为此，Stentz[19]提出Focussed D*（focussed dynamic A*）算法，通过增加偏置函数来降低搜索空间，在一定程度上降低了路径规划时间，但机器人在小范围区域内多次转弯的问题没有得到有效解决；Koenig等[20]提出D* Lite（dynamic A* Lite）算法，该算法在一定程度上提高了算法效率，但存在节点重复计算问题；Ganapathy等[21]提出Enhanced D* Lite（enhanced dynamic A* Lite）算法，避免了移动机器人穿越尖角等一系列不安全路径，但机器人在小范围区域内多次转弯的问题依然没有得到有效解决.

## 1. 问题描述

### 图 1

Fig.1   Schematic diagram of grid model

$O\left( {{t_i}} \right)$为移动机器人依据预存地图或环境变化进行第 $i$次路径规划所需时间，称其为第 $i$次离线规划时间；令 $F\left( {{t_i}} \right)$为机器人依据规划路径进行第 $i$次移动的移动时间，称其为第 $i$次在线移动时间；令 $N$为机器人依据环境变化进行路径规划的总次数，则机器人路径动态规划的目标函数如下：

$\min\; {\sum\limits_{i = 1}^N {\left( {O\left( {{t_i}} \right) + F\left( {{t_i}} \right)} \right)} } .$

### 图 2

Fig.2   Directed D* algorithm system structure diagram

### 2.2. 可行路径确定

D* 算法在初始路径规划时使用类似等势线逐级扩展的方式，会遍历较多不必要节点，导致D*算法搜索空间较大. 为了缩小搜索空间，提出以下技术改进措施.

1）改进搜索方式. 对节点进行区分，将障碍物近似为四边形，以其4个顶点作为关键节点，主要针对关键节点进行搜索、筛选，并从初始点开始由前往后通过由父节点逐级确定子节点的方式确定可行路径. 采用逐级扩展的方式既能将每一个关键节点访问到，同时可以避免对不必要的关键节点进行路径成本计算，以提高规划效率.

### 图 3

Fig.3   Searching method comparison between traditional D* algorithm and proposed algorithm

2）关键节点筛选. 传统D*算法由目标点开始反向采用类似等势线逐级扩展的方式确定可行路径；本研究从出发点开始，对于关键节点采用由父节点逐级确定子节点的方式，通过引入导向函数 $p\left( x \right)$，对关键节点进行搜索和筛选. 该导向函数由2个节点构造直线方程，通过判断该直线方程是否与障碍物区域有交集从而对子节点进行筛选：若存在交集则舍弃；若不存在则保留，后续进一步进行路径寻优. 该方式能降低路径规划单次迭代的搜索空间，减少规划时间. 导向函数定义为

$p(x) = \mathop {\lim }\limits_{x \to {x_{Y_i}}}\; \left(\frac{{(x - {x_{{X_i}}})({y_{{Y_i}}} - {y_{{X_i}}})}}{{{x_{{Y_i}}} - {x_{{X_i}}}}} + {y_{{X_i}}} + \alpha \right).$

$f\left( {{\theta _i}} \right) = \left\{\begin{array}{*{20}{l}} - 8\mu , & 0 \leqslant \left| \theta \right| < {\text{π}} /6;\\ - 4\mu ,&{\text{π}} /6 \leqslant \left| \theta \right| < {\text{π}} /3;\\ \mu , & {\text{π}} /3 \leqslant \left| \theta \right| < {\text{π}} /2;\\ 50\mu ,&{\text{π}} /2 \leqslant \left| \theta \right| \leqslant {\text{π}}. \end{array} \right.$

#### 2.3.1. 平滑度函数 $f\left( {{\theta _i}} \right)$的分段

1）相似关键节点的位置. 考虑障碍物四边形的建模方式，可以认为障碍物的顶点均在网格交点（即路径节点）处，则路径位置为正方形对角线、栅格边、长方形对角线3种情况. 当路径位置为正方形对角线时，由于 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区域内仅存在2个相似关键节点且两者 $\left| \theta \right|$相等即平滑度相等，两者取其一即可，故不再考虑这种情况；当路径位置为栅格边时，由于 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区域内仅存在3个相似关键节点且其中2个关于另一个对称，故只须将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$平均分为2段，即可避免出现相同距离成本而无法选择出最优路径的问题；当路径位置为长方形对角线时，由于 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区域内存在4个相似关键节点且关于水平线对称，因此只须将水平线上2个相似关键节点和水平线下2个相似关键节点分开即可，故将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$平均分为3段.

2）不同路径之间的区分度. 不同的 $\theta$表示不同路径和理想路径之间的偏移程度，为了选出具有较好平滑度的路径，须对具有不同 $\theta$的路径进行不同程度的罚值，因此要从不同路径之间的区分度角度来考虑如何分段. 在将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$均分为 $b$段时，路径落入其中一段的概率为 ${1 / ({2b})}$、路径落入其他区域的概率为 $1 - {{1 /( {2b})}}$，且每一条路径的位置不受其他路径位置的影响. 因此，将路径角度概率问题抽象为二项分布数学模型，设路径位置在 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$区间内其中一段区的情况为事件 $A$，发生的概率为 $p$；路径位置在其他段的情况为事件 $\overline A$，发生概率为 $q$；在单次迭代中可行路径条数即为独立重复试验的次数 $z$，在 $z$次重复实验中，事件 $A$恰好发生 $k$次的概率如下：

${P_k} = {\rm C}_z^k{p^k}{q^{z - k}}\,;\;\; {k = 0,1,2, \cdots ,z},$

$P\left( A \right) = p = {1 / {\left( {2b} \right)}}\,;\;\; {0 < p < 1},$

$P\left( {\overline A } \right) = q = 1 - p = 1 - {1 / {\left( {2b} \right)}}.$

$b$的取值通过实验的方式获得，实验方法如下：在不同的独立实验次数中，分别对 $b$取不同值的情况下事件 $A$发生 $k$次的概率进行统计，部分统计结果如表1所示. 可以看出，在 $z$$b一定的情况下，事件 A发生概率的总体趋势为随着 k的增大而减小；在 z$$k$一定的情况下，事件 $A$发生概率的总体趋势为随着 $b$的增大而减小. 当有少数几条路径同时位于同一段时，分段数大于2，不存在距离成本相同的情况，即可通过距离成本函数 $f\left( {{X_i},{Y_i}} \right)$进行筛选，达到路径成本最优，故将 $0 \leqslant \left| \theta \right| \leqslant {{\text{π}} / 2}$平均分为6段.

Tab.1  Probability of event A occurring k times under different segmentation conditions at z of 15, 25 and 50

 k z=15 z=25 z=50 b=4 b=6 b=8 b=10 b=4 b=6 b=8 b=10 b=4 b=6 b=8 b=10 3 1.8×10−1 9.3×10−2 5.1×10−2 3.1×10−2 2.4×10−1 2.0×10−1 1.4×10−1 9.3×10−2 7.2×10−2 1.9×10−1 2.3×10−1 2.2×10−1 6 5.7×10−3 7.7×10−4 1.7×10−4 4.9×10−5 5.4×10−2 1.1×10−2 3.1×10−3 1.0×10−3 1.7×10−1 1.2×10−1 5.5×10−2 2.6×10−2 9 1.7×10−5 5.8×10−7 4.9×10−8 7.2×10−9 1.8×10−3 9.8×10−5 1.1×10−5 1.8×10−6 7.8×10−2 1.4×10−2 2.6×10−3 6.0×10−4 12 4.4×10−9 3.9×10−11 1.3×10−12 9.5×10−14 1.3×10−5 1.9×10−7 8.0×10−9 6.5×10−10 1.1×10−2 5.0×10−4 3.7×10−5 4.2×10−6

#### 2.3.2. 平滑度函数 $f\left( {{\theta _i}} \right)$中μ的取值

1）当障碍物覆盖面积一定，环境大小不同时，对平均距离进行多次近似计算，受篇幅限制，现仅给出环境维数ES为502、1002、5002、1 0002，障碍物节点数NO为环境总节点数NE的60%、关键节点数NK为障碍物节点数的20%情况下平均距离的计算结果，如表2所示. 由表2及其他试验结果可以看出，在不同大小的环境下，若障碍物覆盖率相同且关键节点占障碍物节点数比率相同，地图环境大小对两相邻关键节点平均距离DK基本无影响.

Tab.2  Relationship between average distance of key nodes and environmental dimension

 ES NE NO NK DK 502 2.5×103 1.5×103 3.0×102 8 1002 1.0×104 6.0×103 1.2×103 8 5002 2.5×105 1.5×105 3.0×104 8 1 0002 1.0×106 6.0×105 1.2×105 8

2）当环境大小一定，障碍物覆盖面积不同时，对2个关键节点距离进行多次测试、计算，结果如图4所示. 当障碍物覆盖率为10%时，两关键节点间平均距离最大；当障碍物覆盖率为80%时，两关键节点间平均距离最小. 可以看出，两关键节点间平均距离随碍物覆盖率的增加而减小.

### 图 4

Fig.4   Relationship between average distance and obstacle coverage rate

#### 2.6. 算法流程

1）初始化栅格地图，设置起点、终点以及障碍物. 2）依据地图中的障碍物位置信息选取关键节点. 3）所有关键节点分为3组：CLOSED列表、OPEN列表、DELETE列表. 4）通过导向函数 $p\left( x \right)$获得单次扩展须访问的关键节点，选取须访问关键节点中的最优关键节点. 迭代通过OPEN列表中反复删除位置状态进行，直到当终点G加入CLOSED列表或起点S所能到达的所有节点都包含于CLOSED、DELETE列表中，搜索结束. 5）移动机器人沿初始最优路径前进，若传感器探测到实际地图信息与离线地图信息不符时，将移动机器人的当前位置设为起始节点，进入第4）步，对路径进行重新规划找到避开障碍物的安全路径，当移动机器人移动到目标节点 $G$时，算法结束. 有向D*算法流程图如图5所示.

### 图 5

Fig.5   Flow chart of directed D* algorithm

### 3.1. 仿真环境和测试对象

Tab.3  Environment parameters with varying complexity

 环境 起始节点 目标节点 502 (1, 1) (50, 50) 1002 (1, 1) (100, 100) 5002 (1, 1) (500, 500)

### 图 6

Fig.6   Environment diagrams with varying degrees of complexity

### 3.2. 实验结果与分析

Tab.4  Comparison of route length in different environments

 ES 有向D* D* Lite Focussed D* 502 72.06 77.09 75.24 1002 155.28 161.49 158.78 5002 828.62 860.72 1 134.80

Tab.5  Comparison of route planning time in different environments

 算法 ES=502 ES=1002 ES=5002 O（T） F（T） O（T） F（T） O（T） F（T） 有向D* 14.56 0.65 55.70 1.12 113.50 18.90 D* Lite 14.02 2.14 50.70 10.46 114.34 764.20 Focussed D* 17.29 2.67 65.01 14.41 150.88 860.41

### 图 7

Fig.7   Route planning results for different maps

Tab.6  Number of inflection points in different environments

 算法 拐点数目 ES=502 ES=1002 ES=5002 有向D* 3 10 12 D* Lite 5 13 17 Focussed D* 5 20 25

## 参考文献 原文顺序 文献年度倒序 文中引用次数倒序 被引期刊影响因子

DADI R, PASHA S N, SALLAUDDIN M. Cognitive-based adaptive path planning for mobile robot in dynamic environment [C]// 1st International Conference on Artificial Intelligence and Cognitive Computing. Singapore: AICC, 2018: 117-123.

FARIDI A Q, SHARMA S, SHUKLA A, et al

Multi-robot multi-target dynamic path planning using artificial bee colony and evolutionary programming in unknown environment

[J]. Intelligent Service Robotics, 2018, 11 (2): 171- 186

KADRY S, ABDALLAH A, JOUMAA C. On the optimization of Dijkstras algorithm [M]// Informatics in control, automation and robotics. Berlin: Springer, 2012: 393-397.

LIU Y, LIANG J H, LI J, et al

Robot path planning based on Dijkstra’s algorithm and genetic algorithm

[J]. Frontier Computing, 2016, 422: 397- 408

[J]. 吉林大学学报:信息科学版, 2018, 36 (6): 639- 647

HUO Feng-cai, CHI Jin, HUANG Zi-jian, et al

Overview of path planning algorithms for mobile robots

[J]. Journal of Jilin University: Information Science Edition, 2018, 36 (6): 639- 647

HART P E, NILSSON N J, RAPHAEL B

A formal basis for the heuristic determination of minimum cost paths

[J]. IEEE Transactions on Systems Science and Cybernetics, 1968, 4 (2): 100- 107

MOHD N Z, MOHANTA J C

Methodology for path planning and optimization of mobile robots: a review

[J]. Procedia Computer Science, 2018, 133: 141- 152

[J]. 控制与决策, 2010, 25 (7): 961- 967

ZHU Da-qi, YAN Ming-zhong

Overview of path planning techniques for mobile robots

[J]. Control and Decision-making, 2010, 25 (7): 961- 967

KHATIB O

Real-time obstacle avoidance for manipulators and mobile robot

[J]. The International Journal of Robotics Research, 1986, 5 (1): 90- 98

YANG G W, DENG Y, ZHUANG X D, et al. An improved real-time path planning of mobile robot in a complex and dynamic environment [C]// 2017 1st International Conference on Electronics Instrumentation and Information Systems (EIIS). Harbin: IEEE, 2017: 1-4.

WANG S M, ZHAO T T, LI W J. Mobile robot path planning based on improved artificial potential field method [C]// International Conference of Intelligent Robotic and Control Engineering. Lanzhou: IEEE, 2018: 29-33.

LIANG X X, LIU C Y, SONG X L, et al. Research on mobile robot path planning in dynamic environment [C]// 2017 Chinese Automation Congress (CAC). Jinan: IEEE, 2017: 3890-3894.

KOENIG S, LIKHACHEY M, FURCY D

Lifelong planning A*

[J]. Artificial Intelligence, 2004, 155 (1/2): 93- 146

PATLE B K, PANDEY A, JAGADEESH A, et al

Path planning in uncertain environment by using firefly algorithm

[J]. Defence Technology, 2018, 14 (6): 691- 701

PANDA R K, CHOUDHURY B B. An effective path planning of mobile robot using genetic algorithm [C]// IEEE International Conference on Computational Intelligence and Communication Technology. Ghaziabad: IEEE, 2015: 287-291.

STENTZ A. The D* algorithm for real-time planning of optimal traverses [EB/OL]. (1994-09-01) [2019-04-15]. http://pdfs.semanticscholar.org/8e60/573c99d2d290dd9163272ea21727c382d00b.pdf.

STENTZ A. Optimal and efficient path planning for partially-known environments [C]// IEEE International Conference on Robotics and Automation. Pittsburgh: IEEE, 1994: 3310-3317.

[J]. 计算机应用研究, 2015, 32 (12): 3609- 3612

SHI Jiu-gen, LI Kai-ye

Indoor path planning based on hierarchical improved D~* algorithm

[J]. Computer Application Research, 2015, 32 (12): 3609- 3612

STENTZ A. The focussed D* algorithm for real-time replanning [C]// Proceedings of the 14th International Joint Conference on Artificial Intelligence. Montreal: [s.n.], 1995: 1652-1659.

KOENIG S, LIKHACHEV M

Fast replanning for navigation in unknown terrain

[J]. IEEE Transactions on Robotics, 2005, 21 (3): 354- 363

GANAPATHY V, YUN S, CHIEN T

Enhanced D* Lite algorithm for autonomous mobile robot

[J]. International Journal of Applied, 2011, 1 (1): 58- 73

/

 〈 〉