2020年第39卷第12期传感器与微系统（Transducer and Microsystem Technologies)71
DOI：10.13873/J. 1000-9787(2020)12-0071-03基于D* Lite算法的三维路径规划研究*
程志，张志安，乐伟扬，牛坤
(南京理工大学机械工程学院，江苏南京210094)
摘要：为实现三维地形环境下的路径规划，建立地面栅格高程地图，并对二维平面的IT Lite算法进行
扩展；在传统IT Lite算法的代价函数中引人相邻节点间高程差，通过最大爬坡坡度和最大侧倾坡度限制
路径扩展方向，同时设立爬坡因子和侧倾因子来平衡路径的长度与安全性，避免规划路径经过大坡度危险
区域。在仿真环境中对相关参数进行整定，同时进行了路径的对比验证和遇到临时障碍物时的路径重规
划测试，结果表明：该算法可以有效均衡三维地形下路径长度和安全性问题。
关键词：三维路径规划；Lite算法；栅格高程图；路径安全性
中图分类号：TP242.6 文献标识码：A 文章编号：1000~9787(2020) 12-0071-03
Rearch on path planning in 3D terrain bad on
D* Lite algorithm*
CHENG Zhi, ZHANG Zhian, YUE Weiyang, NIU Kun
(School of Mechanical Engineering,Nanjing University of Science and Technology,Nanjing 210094,China)
Abstract：In order to realize the path planning in 3D terrain, the gridmap bad on elevation information is
established,and the D* Lite algorithm bad on two-dimensional is improved； the elevation difference between
adjacent nodes is introduced into the cost function of the traditional D* Lite algorithm, and the maximum slope of
climbing and the maximum slope of rolling limit the extension direction of path,while the climbing coefficient and
the rolling coefficient are t up to balance the length and safety of the path, and to avoid the planning path passing
through dangerous areas with large slope. In the simulation environment, the relevant parameters are adjusted,and
the comparison of the planning path and the re-planning when obstacles encountered show that the improved
algorithm can effectively balance the path length and safety in three-dimensional terrain.
Keywords：path planning in 3D terrain；D* Lite algorithm；gridmap bad on elevation information；safety of path
〇引言
三维地面路径规划主要针对起伏的地形环境，综合考 虑障碍物情况和地形坡度等情况进行的路径规划，相比于 空间规划，其受环境制约的程度更大，对算法要求也越高。目前已研究的三维地面路径规划算法包括粒子群算法、遗 传算法[1]、蚁群算法％等，但这些算法多用于静态环境下 的路径规划问题，在遇到动态环境时重规划耗时较长，实时 性方面有所欠缺，且算法存在易陷人局部最优解等问题。
相比之下，D $ Lite算法[3]是典型的应用于动态环境下的路 径规划算法，在遇到环境变化时可以进行快速重规划，并且 其通过复用先前的搜索信息，大大减少了重规划路径时的 计算量；但目前对于其研究多停留于二维环境中，针对三维 地形下的应用研究较少。
本文通过建立栅格高程地图模型，在传统IT Lite路径规划算法的代价函数中引人地图高程信息，在扩展节点满 足最大爬坡坡度与最大侧彳呢妓度的前提下利用爬坡因子与 侧倾因子平衡规划路径的长度与安全性。实验中对爬坡因 子与侧倾因子进行了参数整定，同时对改进算法的有效性 进行了对比仿真。
1环境地图的建立
针对移动机器人的路径规划问题，首先获知环境地图 信息，主要分为基于地形和固定障碍物的静态地图和基于 移动障碍物的动态地图；其获取途径主要有两种：利用已知 的地图信息导人即静态地图，或者利用机载传感器探测的 动态环境地图。
目前常见的环境模型包括有可视图法、切线图法、Vorcmoi图法、自由空间法和栅格图法等[4]。其中，栅格图 法将环境地图划分为大小相等的相邻方格，每一格可以单独
收稿日期:2019-06-28
*基金项目：国家自然科学基金资助项目（11772160，11472008)
72传感器与微系统第39卷
记录该栅格点的地形信息，方法简单有效且易于实现。数 字高程地图（DEM)是表示地形空间分布的有限三维向量 组U,F，Z L通过尤和F描述位置信息描述高度信息[5]。本文对数字高程地图进行栅格化，将高程地图的高 度信息赋予栅格点并建立静态地图模型，如图1所示。
4
3
2
]
图1栅格高程地图
为便于研究和实验，本文假设如下:1)假设高程图数 据已知，本文只进行栅格高程地图的建立;2)假设栅格为 八邻域扩展，在条件允许情况下机器人可以向周围8个栅 格运动;3)动态地图信息由机载传感器捕捉并实时更新，即机器人搭载的传感器在当前栅格点可以探测到周边8个
栅格的地形状态。
2三维路径规划
2.1 D*Lite 算法
D'Lite[31算法是Koeing S等人提出的基于二维栅格地 图模型下的增量式动态路径规划算法，算法主要代价函数 及相关定义如下
rh s(5)
0，i“=V丨
min (c(s,s,)) ,otherwi
s' eS u cc(s)
(1)式中Succ(s)为节点s的后继节点集，g(s')为节点s'到目 标点的实际代价值，c(s，y)为节点，到s的代价值，；^0)
为节点S到目标点的实际代价值；当s U)时，代表 节点S呈局部一致性可以通行，当S) #g(.0时，节点S 局部非一致，代表路径上出现障碍物或者障碍物被清除，此 时该点路径信息不准确，需要重新更新信息。
节点s在优先列表Key中的定义如下~
Key(s) =[keyx (s),key2(s)1(2)
其中
C(s) =m in(g(s) ,r^.s(.s) ) +h(s) +k m
(s)=m in(g(s) ,rhs(s))
(3)式中为起点到节点*的启发代价值，k为最初起始 点到当前起始点的/i值。
算法流程图如图2所示，首先从目标点开始扩展，以优 先列表Key中最小节点确定搜索方向，若该节点呈局部一 致则将其移出优先列表，同时更新周边节点信息并将其加 人优先列表待扩展；重复该过程直至到达起始点而终止搜
索，此时完成节点信息更新。从起始点出发，以满足m in
s' eS u cc(s) (C(S,Z) +g(S'))的节点S'作为当前节点S的下一前进节点，重复该步骤直至到达目标点完成规划路径。若路径节 点信息改变，则更新该节点周边代价值，将当前节点作为新 的起始点，再选取/fey中最小节点重新更新节点信息。
图2算法流程图
2.2 三维D*L ite算法
三维地形的路径规划质量不仅需要考虑路径长度，还 需要对路径安全性进行限制。
1)路径长度可以表示为
[=X v V G i J i+l) +(z(s‘) _Z(S‘+1) )2⑷i=0
2)路径安全性主要表现在坡度问题上，规划路径的爬坡坡度不可超过机器人所允许通行的最大爬坡坡度也
(f>^max arctan
s; e P a th H i^n
|z(s;) -2(s i+1)I
(5)式中n为规划路径Paf/i中的总节点数，s,.为P—中的第 i个节点，P为两节点间的欧氏距离，z为节点的高程值。
为了解决IT Lite在三维环境下的应用问题，本文对其 进行了相应改进:1)将两相邻节点间的高程差作为节点间 代价值引人c(s，，）中，如式(6)所示;2)设立坡度因子
用于设定斜坡路线的优先程度;3)加人最大爬坡坡度‘ 对生成路径进行限制。
c(5,s,)=^/p i{s,s') +k p x(z(s) -z(s'))2(6)
对于相同的两节点高程差，坡度因子\值越大时，爬 坡的花费越高，路径中最大爬坡坡度降低，算法趋向于平坦 地面搜索，但过大的丨p值也会增加规划路径长度，因此在 保证路径安全性式(5)的条件下A p可尽能低。在MATLAB 中进行路线仿真，取\=50，结果如图3所示。
图3三维D* Lite路径规划
由图3可以看出规划路径基本避开了坡度较大的地 形，但路线经过的4区域与f i区域相比，其侧向坡度较大, 路径安全性较低，而从5处通行路径长度并不会大幅增长。
2.3 改进三维D*L ite算法
针对2.2节出现的问题，
改进算法在路径安全性的判
第12期程志，等：基于n r Lite算法的三维路径规划研究73
定上增加了最大侧倾坡度屯
|z(5f ) -z(sf ) |
小C a r c t a n P(K)(7)式中Z1和/2分别为当前点S垂直于当前路径线的两侧节点。
设立侧倾因子 <，用于设定侧倾路线的优先程度，则 节点间代价值call(h)为
Call(«，>«) =^/c(s',s)2 +kc X(z(/1) -zis^))2(8)
在MATLAB中进行路线仿真，取< =50人=50结果如图4所示。当爬坡因子\增大时，爬坡坡度大幅下降，路径长度与侧 倾坡度存在一定上升幅度。
综合考虑路径中爬坡坡度与侧倾坡度的安全性问题和 路径长度，平衡选取\ =30，< =30作为后续测试参数。3.2算法性能对比
将改进三维D4Lite算法与文献[2]中改进蚁群算法进 行对比实验，如图7所示。
图4改进前后规划路径对比
可以看出，规划路径综合选取了爬坡坡度和侧向坡度 较小的S区域，路径长度由33.48增加到了 36. 68，最大爬 坡坡度由18. 03°增加到了 19.87°，最大侧向坡度由32.61° 被限制到21 _42°，侧向坡度减小明显。
3仿真实验与分析
为验证算法的有效性并明确相关参数范围，在MAT-LAB R2017h 中进行仿真实验，计算机配置为 macOS 系统，i7~6920HQ处理器，主频2. 8 GHz，运行内存16 G。路径规 划空间为如图5所示的50 x50的栅格地图。最大侧倾坡 度屯=30°，最大爬坡坡度A=30°。
5
4
3
2
I
图S仿真实验地图模型
3.1相关参数整定
针对改进三维IT Lite算法中坡度因子和侧倾因子t，取不同数值搭配进行仿真对比，结果如图6。
(a)*p=30 (b)A：c=30
图6不同参数结果对比
图6(a)中，取\ =30人=10 ~50进行测试，结果显示，侧倾因子越大，侧倾坡度下降明显，路径长度和爬坡坡 度有上升趋势；图6 (b)中人=30，& =10〜50，结果显示，
图7算法对比实验
相关参数设置如下:1)改进三维ET Lite算法：坡度因 子\=30，侧倾因子< =30。2)改进蚁群算法：蚂蚁数量 20，算法循环次数200，启发因子《=1，期望启发因子召= 1，信息素更新系数P=〇.2，信息素衰减系数s=0.2。
对两种算法进行20次实验，结果如图8。
5 10 15 20
实验组序号
图8算法仿真结果对比
由图8曲线可知，改进三维IT Lite算法与改进的蚁群 算法在三维地形上的所规划的路径长度相差不多，但由于 蚁群算法采用的是迭代寻优机制，其耗时远大于基于启发 式搜索方式的IT Lite算法。
3.3 动态规划测试
在地图中随机添加若干临时障碍物，测试改进算法的 动态规划能力，结果如图9所示。
图9动态环境中的规划仿真
可见，改进三维IT Lite算法在遇到临时障碍物时可以 及时改变原规划路线，并根据当前障碍物位置更新周边代 价，重规划出一条有效路径。
(下转第77页
）
第12期
桂斌，等：基于特征建模的静电除尘脉冲电源控制方法研究77
误差和性能指标的特征模型。其后，通过SIMULINK 搭建 了基于特征建模的静电除尘脉冲电源的控制系统并进行仿 真分析，得到如下结论:1)特征建模控制方法适用于不能 或难以建模的复杂系统，避免了对静电除尘脉冲电源进行 复杂建模。2)静电除尘脉冲电源的特征建模控制方法与
PID 控制方法相比具有超调量小，上升时间短和稳态误差 小的优点。参考文献：
[1] 国家环境保护总局.火电厂大气污染物排放标准:GB13223-
2011 [ S ].北京：中国环境科学出版社，2012:1 —5.
[2] 张谷勋.脉冲电源一电除尘电源的第三个里程碑[J ].电源
世界,2015,18(12) :42—48.[3] 王冠龙，崔靓，朱学军.基于数字PID 算法的温度控制系统设
计[J ].传感器与微系统，2019，38(1) :86 —88 ,96.
[4] 黄秋霞.基于特征建模的自适应迭代学习控制及应用研
究[d ]•杭州：浙江工业大学，2〇13:66—
徐亦雯.基于特征建模的倒立摆智能自适应控制 系统设 计[D] •上海：上海应用技术大学,2016:53 —56.[6] 吴宏鑫，解永春，李智斌，等.基于对象特征模型描述的智能
控制[J ].自动化学报，1999,25(1):9 —17.[7] 张浩然，李呈森，伍利娟，等.E P P -II 型电除尘用高压脉冲电
源[C ]//第十六届中国电除尘学术会议，武汉：中国环保产业 协会电除尘委员会,2015:572 —575.
[8] SOEIRO T B, MUKLETHALER J.UNNER J,et al. Automated
design of a high-power high-frequency LCC resonant converter for electrostatic precipitators [ J ]. IEEE Transactions on Industrial E- lectronics,2013,60(ll) ：4805 -4819.
作者简介：
桂斌
(1992 —），男，通讯作者，硕士研究生，研究方向为检测
技术与自动化装置，E-mail =186****************。
章飞（1979 -），男，副教授，研究领域为检测技术与自动化
装置。
[2] 吴玉香，王超.一种改进的移动机器人三维路径规划方 法[J ].华南理I 大学学报（自然科学版），2016,44(9) :53 -60.[3] KOENIG S, LIKHACHEV M. Fast replanning for navigation in
unknown terrain [ J ]. IEEE Transactions on Robotics, 2005 , 21(3) ：354-363.[4] 卜新苹，苏虎，邻伟，等.基于复杂环境非均匀建模的蚁群路
径规划[J ].机器人,2016，38(3) :276 —284.
[5] 马丽莎.基于数字髙程模型栅格地图的移动机器人路径规划
研究[D ].杭州：浙江大学，2012.[6] 张晓冉，居鹤华.基于D 4 Lite 算法的估价函数分析[_!].计算
机工程，2012，38(1) :154 —156.
作者简介：
程志
（1994 -），男，硕士研究生，研究方向为移动机器人智
能控制，路径规划，E-mail:****************。
张志安（1979 -），男，博士，副教授，研究领域为移动机器人,
智能弹药，控制工程。
输出脉冲数反馈至特征建模控制模块，特征建模控制模 块计算出PFM 模块的输人频率，通过PFM 模块得到IGBT 的 触发脉冲，进而得到高压脉冲单元的输出脉冲。高压脉冲单 元的特征建模控制方法SIMULINK 仿真电路如图7所示。
Lgl3
脉冲变压器
Discrclc,1—
1(—
TL=2xl 〇-7s.Cs3
Lsl2 ^
powerGUI
时间
/S
图8
高压脉冲单元控制效果对比
从图8中可知，因此特征建模控制方法的控制效果更 好。其高压脉冲单元性能对比如表3所示。
表3
脉冲单元性能数据对比
性能指标PID
特征建模
脉冲数1920响应时间
/ms
6.7
3.5
4结论
通过对静电除尘脉冲电源结构进行分析，建立了基于
(上接第73页)4
结论
本文提出了一种基于IT Lite 算法的三维路径规划方 法，在代价地图中引入高程信息，并将相邻节点的高程差带 人到路径代价函数中；针对路径安全性问题提出最大爬坡 坡度和最大侧倾坡度，使得规划路径在减少路径长度的基 础上具有相对的安全性。由于改进算法采用空间换时间的 策略，针对每个遍历节点都存储了其八向的爬坡坡度和对 应的侧倾坡度，有效减少了路径规划中的重复计算，但对内 存空间的占用较大，下一步将会针对算法的占用空间进行 优化。参考文献：
[1 ] CHUNG W K,XU Y. A generalized 3-D path planning method for
robots using genetic algorithm with an adaptive evolution process [C]/^Intelligent Control & Automatio,IEEE,2010.
1
L g l 4 十
DC 1 IGBT /
|
Diodc3
--T --K --
g cr
Measurement Lsl3
Dc 2T
0~
u ju =s -
k + Constant^ ^ ^-----[-^S w itch PFM 模块
特征建模模块
rF
Outl Ini
Outl
r i i i ■!Triggered Subsystem
Add
Logical Operator (-I not K -Display
图7
高压脉冲单元的特征建模控制方法仿真电路
高压脉冲单元的特征建模控制方法和PID 控制方法的 控制效果对比如图8所示。
-4-8
1/
1
