2021年4月
第28卷第4期
控制工程
Control Engineering of China
Apr. 2021
Vol.28, No.4
文章编号：1671-7848(2021)04-0736-08DOI: 10.14107/jki.kzgc.20190179基于改进D*算法的A U V三维动态路径规划 朱蟋蟋，孙兵，朱大奇
(上海海事大学智能海事搜救与水下机器人上海工程技术研宄中心，上海201306) n摘要：传统D*算法应用于路径规划时，规划的路径一般由折线段组成，且紧靠障碍物 边缘。由于自身运动学约束，自治水下机器人(autonomous underwater vehicle,A U V)难以实
现折线等情况的快速转向；同时由于自身体积约束，A U V可能会碰撞到障碍物边缘。针对
A U V三維水下动态路径规划问题，在原D*算法的代价函数基础上增加了障碍物威胁代价
项，以保证规划的路径更安全；利用均勾B样条曲线拟合模型进行路径平滑处理，进而得
到优化的A U V规划路径。最后，仿真结果表明，改进的D*算法可以达到预期效果。
关键词：自治水下机器人；三维路径规划：D*算法；威胁代价
中图分类号：T P242 文献标识码：A
Three-dimensional Dynamic Path Planning of AUV Based on
Improved D* Algorithm
Z H U X i-xi,S U N B i n g,Z H U Da-q i
(Shanghai Engineering Research Center of Intelligent Maritime Search & Rescue and Underwater Vehicles, Shanghai Maritime
University, Shanghai 201306, China)
Abstract:W h e n the traditional D* algorithm is applied to path planning,the planned path i s generally compo s e d of fold lines and close to the edges of o bstacles.D u e to i t s o w n kinematic constraints,i t i s difficult for autonomous underwater vehicle (A U V)to achieve rapid steering of the fold line.At the same time,due to i t s o w n volume constraints,A U V m a y collide with the edges of obstacles.A i m i n g at the three-dimensional underwater dynamic path planning problem of A U V,the obstacle threat cost item i s added based on the cost function of the original D* algorithm to ensure that the planned path i s safer.T h e uniform B-spline curve fitting model is used to smooth the path,and then an optimized A U V planning path is obtained.Finally,the simulation results s h o w that the improved D* algorithm can achieve the expected results.
K e y w o r d s:A u t o n o m o u s underwater vehicle;three-dimensional path planning;D* algorithm;threat cost
i引言
水下路径规划一直是自治水下机器人(autonom o u s underwater vehicle,A U V)安全航行的关键问 题[1，2]。解决路径规划类问题经常采用栅格法进行环境建模，进而演化出了 DijkstraW、和 等算法。D*算法是在前两种算法的基础上发展而来 的，具有速度快、寻路优等特点，因此广泛应用于
基于传感器路径规划【1()]的研宄上。但是，D*算法规 划存在一定不足，即规划出的路径可能紧靠障碍物 边缘，且由折线段组成。针对这些问题，张飞等[1|]提出基于改进D*算法的无人机室内路径规划，在进 行栅格拓展时，如果子节点的前、后、左、右四个 方向有障碍物，则将碰揸因子加入到路径代价中，从而保证规划路径的安全，但该方法不能解决路径 偏向角大的问题，且仅考虑二维环境。刘晓涛等[12]提出基于支持向量机的改进D*算法，使无人车在受 约束情况下规划的路径更平滑，但此方法在障碍物 密集的环境中，不能保证路径完全避开障碍物。
H u a n g等[13]提出基于改进的高德地图映射D*算法
收稿日期：2019-04-01；修回日期：2019-08-08
基金项目：国家自然科学基金资助项目(61873161,111706224)；上海市青年科技启明星计划资助项目(20QA1404200)；国家重点研发计划课题(2017YFC0306302)
作者简介：朱蟋蟋(1993-)，女，安撇阜阳人，研宄生，主要研究方向为AUV路径规划等：孙兵(1987-)，男，江苏南通人，博士，副教授，主要从事水下机器人规划与控制等方面的教学与科研工作（本文通信作者，E-mail: hmsun ************)；朱大奇(1964-)，男，安徽安庆人，博士，教授，主要从事水下机器人等方面的教学与科研工作。
第4期朱蟋蟋等：基于改进D *算法的A U V 三维动态路径规划• 737 •
的动态路径规划[13]，改进启发函数以减少路径总长 度并提高搜索效率，但该方法可能会出现路径与障 碍物距离太近而影响航行安全的问题。S a d i q 等【14] 提出采用粒子算法结合D *算法来解决路径规划 中的门限状态，但没有考虑机器人的性能约束和三 维航迹规划问题。
以上方法主要应用于无人机和移动机器人的 路径规划。对于A U V 水下三维环境的路径规划问 题，由于A U V 存在运动学约束，水下环境存在不 确定性和动态变化（静态和动态障碍物），因此前 述方法存在一定的局限性，不能完全适用于水下环
境。国内外有不少学者在此问题上做了改进，包括 添加A U V 与障碍物之间的安全距离、加入A U V 的转向角约束等。本文是将已有工作进行系统融合， 并将其应用于A U V 的三维水下路径规划中，创新 点如下。
①
本文修改了原D *算法的代价函数，添加障
碍物威胁代价项，使得规划出的路径尽量躲避障碍 物边缘，特别是动态障碍物。
②
D *算法在路面规划的路径往往是由折线段
组成的，考虑A U V 姿态角不能发生突变，本文采 用三次均匀B 样条曲线平滑方法对规划的路径进行 平滑处理。
2 D *算法描述
本文研究的A U V 路径规划算法主要基于D *算 法进行改进设计，因此本节介绍A *和D *算法进行 路径规划的基本原理，并给出仿真实例，分析这两 种算法如何处理动态障碍物环境下的路径规划问题。
2.1 A *算法
A *算法是在Dijsktra 算法基础之上形成的，算
法核心是代价函数：
F («) =
G («)+ //(«)
(1)
点向目标点移动，直到目标点从O P E N 表移除，进 入C L O S E 表，或者不存在合适的路径。
2.2 D *算法
D *算法是在A *算法基础上发展而来的，相较
于A *算法有两个重要改进。
①
D *算法将启发函数变成可携带的，每一次
中心栅格向周围栅格拓展，均可以将其启发值传递 给周围邻点。
②
当动态障碍物出现时，D *算法可重复利用
大部分数据，只需要将动态障碍物周边受到影响的 栅格的数据更新，重新定向。
D *算法的代价公式是由A *算法的代价公式发
展而来的，由G (s )和//(s )两项构成：
F (s ) =
G (s ) +
H (s )
针对启发值//的，D *算法采用的是增量式启发
函数：H (s ) =
H (s ') + C (s ,s '),
S =V
丨
其他
(3)
式中，V 为当前节点s 的拓展节点；V l 为目标节点; //(V )为V 的启发值；C (&V )为相邻两个节点的代高纯度的氧化铜
价值。每一次进行相邻栅格拓展和计算启发值时，被拓展点s 的启发值由拓展点Y 的启发值和相邻两 个节点的代价值计算得来。此计算过程在二维环境 中如图1所示，在三维环境中如图2所示。
ff(s)
H(s')
丰
♦
C(s >s')
起始栅格
目标栅格
式中，<7(«)为当前节点到起始节点的实际代价值； //(«)为当前节点到目标节点的启发值。
A *算法在执行路径规划时主要用两个表来实
现节点的扩展和最优节点的选取：O P E N 表和
C L O S E 表。O P E N 表的作用是保存搜索过程中遇到
的扩展节点，同时将这些节点按代价值进行排序；
C L O S E 表的作用是保存O P E N 表中代价值最小的
可扩展节点。每向前移动一个栅格，以占据点为中 心点，计算周围8个栅格的代价值；将8个拓展栅 格的坐标放入O P E N 表，根据代价值进行从大到小 排序，最小代价值代表的栅格坐标排在O P E N 表的 表尾；下一次拓展选取O P E N 表的表尾坐标进行拓 展，拓展过的点进入C L O S E 表。这样依次从起始
图
1二维启发函数示意图
Fig. 1 Schematic diagram of two-dimensional heuristic
function
起始栅格
图
2三维启发函数示意图
Fig. 2 Schematic diagram of three-dimensional heuristic
function
• 738 •控制工程第28卷
8
7
8
9
10
11
12
13
14
15
栅格数
(a )指针变化前
(a) Before the pointer changes
(17,15)，黑栅格代表障碍物。从目标点到起始点， 进入过O P E N 表的栅格都使用方向指针记录最优映 射的方向，阴影栅格便是根据最优指向形成的最优 路径。
D *算法在处理动态障碍物时的方法如图4所示。
同A *算法一样，D *算法也使用O P E N 表和
C L O S E 表来完成点的拓展。从目标点开始，将目标
点放入O P E N 表中，计算周围8个栅格的启发值和 代价值（三维环境中，计算周围26个栅格的启发值 和代价值），选取代价值最小的栅格点继续拓展，被拓展过的点从O P E N 表中删除，未被拓展的点继 续留在O P E N 表中等待拓展。这样依次从目标点拓 展到初始点，每次栅格拓展选取代价值最小的栅格 为拓展对象，同时使用方向指针[15]来记录最优映射， 如图3所示。
7 8 9 10 11 12 13 14 15
栅格数
(b )指针变化后 (b) After the pointer changes
图
4动态障碍物周围栅格的方向指针
Fig. 4 Direction pointers of the grids around the dynamic
obstacle
图4中，深栅格为临时出现的动态障碍物。 当动态障碍物出现时，由于其占据了原规划路径上 的节点，影响了被占节点代价值的计算，栅格的拓 展无法正常进行。在未知的动态环境中，当动态障 碍物出现时，经过计算发现，只有动态障碍物周边 少数几个栅格的指针指向受到影响，见图4(a )中浅 栅格的方向指针。因此，只要受到影响的栅格的 指针重新定向便可。图4(b )中，浅阴影栅格中的 方向指针代表受到障碍物影响的栅格重新定向后的 状态。基于方向指针的动态规划方法，D *算法能够 快速处理动态障碍物环境下的路径规划问题。
3改进D *算法
D *算法在环境发生动态变化时，仅处理局部受
到影响的栅格，从而可以快速进行重规划。但是， 基于传统D *算法代价函数，D *算法规划出的路径 很可能紧贴障碍物边缘。考虑A U V 自身大小，这 种情况对A U V 安全航行有一定威胁，因此在D *算 法的代价函数中添加威胁代价项改进后D * 算法的代价函数计算公式为
F (s ) =
G (s ) +
H (s ) + j (s )
(4)
式中，//的为当前节点到目标节点的启发值；G ⑴ 为当前节点到起始节点的实际代价值；为周围
障碍物对当前栅格产生的威胁代价值。
3.1障碍物威胁代价模型
障碍物对A U V 航行的威胁是一个复杂的问题， 本文对其做了简化模型，用不同形状的阴影栅格代 表障碍物，空白栅格代表A U V 位置，如图5所示。
\
\\i
—*
—►
—►
—*产
//•t \///\t ///t t —►\\\—►
//\t t
-—♦—►—♦—♦
/t 个t t -—►//•/•/t t t t \-/W
t \
—►/\
—►
/\\
/
\
'
3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
栅格数
图
3 OPEN 表中栅格的方向指针
Fig. 3 Direction pointers of the grids in OPEN table
图3中，起始点坐标为(3,5)，目标点坐标为6
5 4
3
2
第4期朱蟋蟋等：基于改进D *算法的A U V 三维动态路径规划• 739 •
图7三维环境图
Fig. 7 Three-dimensional environment diagram
图7中，五角星所在位置是起始点，菱形所在 位置是目标点，黑立方体表示静态障碍物，灰 的立方体表示临时出现的动态障碍物，除此之外的 白区域表示的是A U V 可以通过的自由点。A U V 从起始点出发，避开障碍物，选择一条到达目标点 的最优路径，从而完成路径规划P 0,211。
A U V 在水下的通信以及信息采集工作主要利
用声呐来进行，并以此建立三维环境地图信息[22】,
A U V 携带的声呐模型如图8所示。
以三次样条曲线为例，由于/t 次均匀B 样条的 控制点有i t +1个，所以P 。W
3控制段曲线，
V
4控制《2m 3段曲线，
控制w 3m 4段曲线。
ccc360
S ⑴表示的是段曲线，A ：表示的是A ：次均匀B
样条曲线，所以就是控制点与基函数的乘积之 和。其中，控制点是从点到f 点，一共/ - +1个。 平滑后的曲线S ⑴的表达式为
5(〇 = ± R N.^t )
(6)
j =i -k
控制点g 的计算公式为
(7)
1=0
式(7)和式(6)的参数意义略有不同。在式(7)中，
J 表示的是起始的控制点，A 表示的是A + 1次均匀 B 样条曲线，/表示的是迭代参数，相当于控制点是
从乃到尸;+*。
iV #为基函数：
N m (~1)r it + k -j -r )k, y = 0,1, • ■ ■, A :
(8)
当= 3时，式(8)即为三次均匀B 样条曲线基 函数的表达式。3.3三维水下场景设置
三维水下场景设置如图7所示。
图5障碍物威胁代价模型
Fig. 5 Obstacle threat cost model
以栅格中心点为中心，以A U V 当前位置到障 碍物的距离为自变量r f ，建立障碍物威胁代价模型：
0，
d >r r ^
j{s) = -cQ-\ r ^<d <rnm (5)
式中，％为最大效用半径；
为最小限制半径；
防辐射材料c 为常系数。
3 d
时■，障碍物对栅格不生威胁作用; 当
时，障碍物对栅格产生最大限制。当d 在 两个距离之间时，产生的代价与距离呈负指数关系。
c 的值越大，表示障碍物威胁代价项占总代价
项的比重越大。其物理意义是，障碍物威胁代价部 分约束影响力越大，规划出的路径与障碍物边缘距 离越远。本文仿真部分统一设定c 的值为5。
3.2三次均匀B 样条曲线平滑
对于规划好的折线路径,A U V 在转向过程中存 在转向角度限制，可能无法按照规划的线路行驶， 而且折线路径会增加A U V 在实际航行中的能量消 耗。规划出的折线路径如果能处理为平滑的线路， 则更能适应A U V 的水下航行。
本文基于三次均匀B 样条曲线对改进D * 算法规划后的折线路径进行平滑处理，拟合曲线如 图6所示。
图6拟合曲线
步态识别
Fig. 6 Fit curve
• 740 •
控制工程第28卷
个A U V 移动方向
3.4改进D *算法的公式定义及实现 3.
4.1改进D *算法的公式定义
改进D *算法使用O P E N 表来进行栅格点的拓 展，用ta g 来标记每一个状态点义。未进入过O P E N 表
的
标
记
为
缎纹织物N E W ，处于O P E N 表中的
吨(X )标记为O P E N ，己经从O P E N 表中移出进入
C L O S E 表的啤(Z )标记为C L O S E 。尤有两种状态： R A I S E 型和L O W E R 型。尺。1(1(；〇函数用来记录启
发函数修改前的最小值。如果A ；l d (Z )< //(%)，则义为R A I S E 型，说明环境发生了变化， 相邻节点代价C C s y ')增加，启发值超过了原来的
;
如果 &l d (Z ) = //(X )，则 Z 为 L O W E R
型，说明环境没有发生动态变化。除了目标点G 以 外，每一个状态点尤都可以用r = 表示其对应
的下一个状态点F 。
被拓展的栅格在O P E N 表中的排序由偏置函数 /B (z ,/?,)的值决定：
/B (Z ,i ?,)二/(U
,.) + A 尺，A )
(9)f (X ,R i ) = H (X ) + g (X ,R i ) + j (X )
(10)
g d ^
〇
)
+ g (尺2,
尺
1) +…+
(⑴
g (A U + k
式中，e {尽，忒，…，/^}为最优拓展栅格的状态占
据值；/(义，尺）为最优拓展栅格的代价函数；
为从起始状态点/?〇到当前状态点&的累
计偏差；g d /f ,.)为Z 到/?，的实际代价值；y (I ) 为周围障碍物对I 产生的威胁代价值。3.4.2改进D *算法的实现
改进D *算法主要由P R O C E S S _S T A T E 、
苏州反光背心
M O D I F Y _C O S T 、M O V E _A U Y 三大主功能函数和 D E L E T E 、P U T —S T A T E 、G E T _S T A E T 、I N S E R T 、 M I N _S T A T E 、M I N _V A L 等子功能函数构成。下面
介绍几个重要的功能函数。
①P R O C E S S _S T A T E 函数从O P E N 表的表尾
取出坐标值（具有最小代价值），对这个点周围相
邻点进行拓展。
②
I N S E R T 函数根据状态点Z 的启发值//(X )
和障碍物威胁代价值火义），计算其代价函数值 /(X )和偏置函数值/B (X )，并以此决定其在O P E N 表中的位置排序。
③ M O V E _A U Y 函数是主功能函数，反映了整 个D *算法的流程。首先将目标点放入O P E N 表， 以目标点为中心，反向从目标点向起始点进行启发 搜索，最终根据方向指针的方向规划出从起始点到 目标点的最优路径。
改进D *算法的具体流程如图9所示。
录入地图信息
初始化
将目标点插人 O P E N 表中A ^P R O C E S S —S T A T E O
令当前节点
图9改进D *算法流程图
Fig. 9 Flow chart of improved D* algorithm
① 初始化，令所有节点的tog 值为N E W ，令
目标节点G 的启发值为零，即7/(G ) = 0，把目标 节点G 放入O P E N 表。
②
重复调用I N S E R T 、P R O C E S S _S T A T E 函数，
直到A U V 所在的起始节点被移出O P E N 表，生成最短路径序列。
③
跟随路径序列的方向指针行进，当轨迹内出
现动态障碍物时，利用M O D I F Y _C O S T 函数进行不一致点检测。
④
调用M O D I F Y _C O S T 函数更新相关节点的
代价函数，把受到影响的节点重新放入O P E N 表
。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议