2020年第39卷第12期傳感器與微系統(tǒng)（Transducer and Microsystem Technologies)71
DOI：10.13873/J. 1000-9787(2020)12-0071-03基于D* Lite算法的三維路徑規(guī)劃研究*
程志，張志安，樂偉揚，牛坤
(南京理工大學(xué)機械工程學(xué)院，江蘇南京210094)
摘要：為實現(xiàn)三維地形環(huán)境下的路徑規(guī)劃，建立地面柵格高程地圖，并對二維平面的IT Lite算法進行
擴展；在傳統(tǒng)IT Lite算法的代價函數(shù)中引人相鄰節(jié)點間高程差，通過最大爬坡坡度和最大側(cè)傾坡度限制
路徑擴展方向，同時設(shè)立爬坡因子和側(cè)傾因子來平衡路徑的長度與安全性，避免規(guī)劃路徑經(jīng)過大坡度危險
區(qū)域。在仿真環(huán)境中對相關(guān)參數(shù)進行整定，同時進行了路徑的對比驗證和遇到臨時障礙物時的路徑重規(guī)
劃測試，結(jié)果表明：該算法可以有效均衡三維地形下路徑長度和安全性問題。
關(guān)鍵詞：三維路徑規(guī)劃；Lite算法；柵格高程圖；路徑安全性
中圖分類號：TP242.6 文獻標(biāo)識碼：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
〇引言
三維地面路徑規(guī)劃主要針對起伏的地形環(huán)境，綜合考 慮障礙物情況和地形坡度等情況進行的路徑規(guī)劃，相比于 空間規(guī)劃，其受環(huán)境制約的程度更大，對算法要求也越高。目前已研究的三維地面路徑規(guī)劃算法包括粒子群算法、遺 傳算法[1]、蟻群算法％等，但這些算法多用于靜態(tài)環(huán)境下 的路徑規(guī)劃問題，在遇到動態(tài)環(huán)境時重規(guī)劃耗時較長，實時 性方面有所欠缺，且算法存在易陷人局部最優(yōu)解等問題。
相比之下，D $ Lite算法[3]是典型的應(yīng)用于動態(tài)環(huán)境下的路 徑規(guī)劃算法，在遇到環(huán)境變化時可以進行快速重規(guī)劃，并且 其通過復(fù)用先前的搜索信息，大大減少了重規(guī)劃路徑時的 計算量；但目前對于其研究多停留于二維環(huán)境中，針對三維 地形下的應(yīng)用研究較少。
本文通過建立柵格高程地圖模型，在傳統(tǒng)IT Lite路徑規(guī)劃算法的代價函數(shù)中引人地圖高程信息，在擴展節(jié)點滿 足最大爬坡坡度與最大側(cè)彳呢妓度的前提下利用爬坡因子與 側(cè)傾因子平衡規(guī)劃路徑的長度與安全性。實驗中對爬坡因 子與側(cè)傾因子進行了參數(shù)整定，同時對改進算法的有效性 進行了對比仿真。
1環(huán)境地圖的建立
針對移動機器人的路徑規(guī)劃問題，首先獲知環(huán)境地圖 信息，主要分為基于地形和固定障礙物的靜態(tài)地圖和基于 移動障礙物的動態(tài)地圖；其獲取途徑主要有兩種：利用已知 的地圖信息導(dǎo)人即靜態(tài)地圖，或者利用機載傳感器探測的 動態(tài)環(huán)境地圖。
目前常見的環(huán)境模型包括有可視圖法、切線圖法、Vorcmoi圖法、自由空間法和柵格圖法等[4]。其中，柵格圖 法將環(huán)境地圖劃分為大小相等的相鄰方格，每一格可以單獨
收稿日期:2019-06-28
*基金項目：國家自然科學(xué)基金資助項目（11772160，11472008)
72傳感器與微系統(tǒng)第39卷
記錄該柵格點的地形信息，方法簡單有效且易于實現(xiàn)。數(shù) 字高程地圖（DEM)是表示地形空間分布的有限三維向量 組U,F，Z L通過尤和F描述位置信息描述高度信息[5]。本文對數(shù)字高程地圖進行柵格化，將高程地圖的高 度信息賦予柵格點并建立靜態(tài)地圖模型，如圖1所示。
4
3
2
]
圖1柵格高程地圖
為便于研究和實驗，本文假設(shè)如下:1)假設(shè)高程圖數(shù) 據(jù)已知，本文只進行柵格高程地圖的建立;2)假設(shè)柵格為 八鄰域擴展，在條件允許情況下機器人可以向周圍8個柵 格運動;3)動態(tài)地圖信息由機載傳感器捕捉并實時更新，即機器人搭載的傳感器在當(dāng)前柵格點可以探測到周邊8個
柵格的地形狀態(tài)。
2三維路徑規(guī)劃
2.1 D*Lite 算法
D'Lite[31算法是Koeing S等人提出的基于二維柵格地 圖模型下的增量式動態(tài)路徑規(guī)劃算法，算法主要代價函數(shù) 及相關(guān)定義如下
rh s(5)
0，i“=V丨
min (c(s,s,)) ,otherwi
s' eS u cc(s)
(1)式中Succ(s)為節(jié)點s的后繼節(jié)點集，g(s')為節(jié)點s'到目 標(biāo)點的實際代價值，c(s，y)為節(jié)點，到s的代價值，；^0)
為節(jié)點S到目標(biāo)點的實際代價值；當(dāng)s U)時，代表 節(jié)點S呈局部一致性可以通行，當(dāng)S) #g(.0時，節(jié)點S 局部非一致，代表路徑上出現(xiàn)障礙物或者障礙物被清除，此 時該點路徑信息不準(zhǔn)確，需要重新更新信息。
節(jié)點s在優(yōu)先列表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)式中為起點到節(jié)點*的啟發(fā)代價值，k為最初起始 點到當(dāng)前起始點的/i值。
算法流程圖如圖2所示，首先從目標(biāo)點開始擴展，以優(yōu) 先列表Key中最小節(jié)點確定搜索方向，若該節(jié)點呈局部一 致則將其移出優(yōu)先列表，同時更新周邊節(jié)點信息并將其加 人優(yōu)先列表待擴展；重復(fù)該過程直至到達起始點而終止搜
索，此時完成節(jié)點信息更新。從起始點出發(fā)，以滿足m in
s' eS u cc(s) (C(S,Z) +g(S'))的節(jié)點S'作為當(dāng)前節(jié)點S的下一前進節(jié)點，重復(fù)該步驟直至到達目標(biāo)點完成規(guī)劃路徑。若路徑節(jié) 點信息改變，則更新該節(jié)點周邊代價值，將當(dāng)前節(jié)點作為新 的起始點，再選取/fey中最小節(jié)點重新更新節(jié)點信息。
圖2算法流程圖
2.2 三維D*L ite算法
三維地形的路徑規(guī)劃質(zhì)量不僅需要考慮路徑長度，還 需要對路徑安全性進行限制。
1)路徑長度可以表示為
[=X v V G i J i+l) +(z(s‘) _Z(S‘+1) )2⑷i=0
2)路徑安全性主要表現(xiàn)在坡度問題上，規(guī)劃路徑的爬坡坡度不可超過機器人所允許通行的最大爬坡坡度也
(f>^max arctan
s; e P a th H i^n
|z(s;) -2(s i+1)I
(5)式中n為規(guī)劃路徑Paf/i中的總節(jié)點數(shù)，s,.為P—中的第 i個節(jié)點，P為兩節(jié)點間的歐氏距離，z為節(jié)點的高程值。
為了解決IT Lite在三維環(huán)境下的應(yīng)用問題，本文對其 進行了相應(yīng)改進:1)將兩相鄰節(jié)點間的高程差作為節(jié)點間 代價值引人c(s，，）中，如式(6)所示;2)設(shè)立坡度因子
用于設(shè)定斜坡路線的優(yōu)先程度;3)加人最大爬坡坡度‘ 對生成路徑進行限制。
c(5,s,)=^/p i{s,s') +k p x(z(s) -z(s'))2(6)
對于相同的兩節(jié)點高程差，坡度因子\值越大時，爬 坡的花費越高，路徑中最大爬坡坡度降低，算法趨向于平坦 地面搜索，但過大的丨p值也會增加規(guī)劃路徑長度，因此在 保證路徑安全性式(5)的條件下A p可盡能低。在MATLAB 中進行路線仿真，取\=50，結(jié)果如圖3所示。
圖3三維D* Lite路徑規(guī)劃
由圖3可以看出規(guī)劃路徑基本避開了坡度較大的地 形，但路線經(jīng)過的4區(qū)域與f i區(qū)域相比，其側(cè)向坡度較大, 路徑安全性較低，而從5處通行路徑長度并不會大幅增長。
2.3 改進三維D*L ite算法
針對2.2節(jié)出現(xiàn)的問題，
改進算法在路徑安全性的判
第12期程志，等：基于n r Lite算法的三維路徑規(guī)劃研究73
定上增加了最大側(cè)傾坡度屯
|z(5f ) -z(sf ) |
小C a r c t a n P(K)(7)式中Z1和/2分別為當(dāng)前點S垂直于當(dāng)前路徑線的兩側(cè)節(jié)點。
設(shè)立側(cè)傾因子 <，用于設(shè)定側(cè)傾路線的優(yōu)先程度，則 節(jié)點間代價值call(h)為
Call(?，>?) =^/c(s',s)2 +kc X(z(/1) -zis^))2(8)
在MATLAB中進行路線仿真，取< =50人=50結(jié)果如圖4所示。當(dāng)爬坡因子\增大時，爬坡坡度大幅下降，路徑長度與側(cè) 傾坡度存在一定上升幅度。
綜合考慮路徑中爬坡坡度與側(cè)傾坡度的安全性問題和 路徑長度，平衡選取\ =30，< =30作為后續(xù)測試參數(shù)。3.2算法性能對比
將改進三維D4Lite算法與文獻[2]中改進蟻群算法進 行對比實驗，如圖7所示。
圖4改進前后規(guī)劃路徑對比
可以看出，規(guī)劃路徑綜合選取了爬坡坡度和側(cè)向坡度 較小的S區(qū)域，路徑長度由33.48增加到了 36. 68，最大爬 坡坡度由18. 03°增加到了 19.87°，最大側(cè)向坡度由32.61° 被限制到21 _42°，側(cè)向坡度減小明顯。
3仿真實驗與分析
為驗證算法的有效性并明確相關(guān)參數(shù)范圍，在MAT-LAB R2017h 中進行仿真實驗，計算機配置為 macOS 系統(tǒng)，i7~6920HQ處理器，主頻2. 8 GHz，運行內(nèi)存16 G。路徑規(guī) 劃空間為如圖5所示的50 x50的柵格地圖。最大側(cè)傾坡 度屯=30°，最大爬坡坡度A=30°。
5
4
3
2
I
圖S仿真實驗地圖模型
3.1相關(guān)參數(shù)整定
針對改進三維IT Lite算法中坡度因子和側(cè)傾因子t，取不同數(shù)值搭配進行仿真對比，結(jié)果如圖6。
(a)*p=30 (b)A：c=30
圖6不同參數(shù)結(jié)果對比
圖6(a)中，取\ =30人=10 ~50進行測試，結(jié)果顯示，側(cè)傾因子越大，側(cè)傾坡度下降明顯，路徑長度和爬坡坡 度有上升趨勢；圖6 (b)中人=30，& =10?50，結(jié)果顯示，
圖7算法對比實驗
相關(guān)參數(shù)設(shè)置如下:1)改進三維ET Lite算法：坡度因 子\=30，側(cè)傾因子< =30。2)改進蟻群算法：螞蟻數(shù)量 20，算法循環(huán)次數(shù)200，啟發(fā)因子《=1，期望啟發(fā)因子召= 1，信息素更新系數(shù)P=〇.2，信息素衰減系數(shù)s=0.2。
對兩種算法進行20次實驗，結(jié)果如圖8。
5 10 15 20
實驗組序號
圖8算法仿真結(jié)果對比
由圖8曲線可知，改進三維IT Lite算法與改進的蟻群 算法在三維地形上的所規(guī)劃的路徑長度相差不多，但由于 蟻群算法采用的是迭代尋優(yōu)機制，其耗時遠(yuǎn)大于基于啟發(fā) 式搜索方式的IT Lite算法。
3.3 動態(tài)規(guī)劃測試
在地圖中隨機添加若干臨時障礙物，測試改進算法的 動態(tài)規(guī)劃能力，結(jié)果如圖9所示。
圖9動態(tài)環(huán)境中的規(guī)劃仿真
可見，改進三維IT Lite算法在遇到臨時障礙物時可以 及時改變原規(guī)劃路線，并根據(jù)當(dāng)前障礙物位置更新周邊代 價，重規(guī)劃出一條有效路徑。
(下轉(zhuǎn)第77頁
）
第12期
桂斌，等：基于特征建模的靜電除塵脈沖電源控制方法研究77
誤差和性能指標(biāo)的特征模型。其后，通過SIMULINK 搭建 了基于特征建模的靜電除塵脈沖電源的控制系統(tǒng)并進行仿 真分析，得到如下結(jié)論:1)特征建模控制方法適用于不能 或難以建模的復(fù)雜系統(tǒng)，避免了對靜電除塵脈沖電源進行 復(fù)雜建模。2)靜電除塵脈沖電源的特征建模控制方法與
PID 控制方法相比具有超調(diào)量小，上升時間短和穩(wěn)態(tài)誤差 小的優(yōu)點。參考文獻：
[1] 國家環(huán)境保護總局.火電廠大氣污染物排放標(biāo)準(zhǔn):GB13223-
2011 [ S ].北京：中國環(huán)境科學(xué)出版社，2012:1 —5.
[2] 張谷勛.脈沖電源一電除塵電源的第三個里程碑[J ].電源
世界,2015,18(12) :42—48.[3] 王冠龍，崔靚，朱學(xué)軍.基于數(shù)字PID 算法的溫度控制系統(tǒng)設(shè)
計[J ].傳感器與微系統(tǒng)，2019，38(1) :86 —88 ,96.
[4] 黃秋霞.基于特征建模的自適應(yīng)迭代學(xué)習(xí)控制及應(yīng)用研
究[d ]?杭州：浙江工業(yè)大學(xué)，2〇13:66—
徐亦雯.基于特征建模的倒立擺智能自適應(yīng)控制 系統(tǒng)設(shè) 計[D] ?上海：上海應(yīng)用技術(shù)大學(xué),2016:53 —56.[6] 吳宏鑫，解永春，李智斌，等.基于對象特征模型描述的智能
控制[J ].自動化學(xué)報，1999,25(1):9 —17.[7] 張浩然，李呈森，伍利娟，等.E P P -II 型電除塵用高壓脈沖電
源[C ]//第十六屆中國電除塵學(xué)術(shù)會議，武漢：中國環(huán)保產(chǎn)業(yè) 協(xié)會電除塵委員會,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 —），男，通訊作者，碩士研究生，研究方向為檢測
技術(shù)與自動化裝置，E-mail =186****************。
章飛（1979 -），男，副教授，研究領(lǐng)域為檢測技術(shù)與自動化
裝置。
[2] 吳玉香，王超.一種改進的移動機器人三維路徑規(guī)劃方 法[J ].華南理I 大學(xué)學(xué)報（自然科學(xué)版），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] 卜新蘋，蘇虎，鄰偉，等.基于復(fù)雜環(huán)境非均勻建模的蟻群路
徑規(guī)劃[J ].機器人,2016，38(3) :276 —284.
[5] 馬麗莎.基于數(shù)字髙程模型柵格地圖的移動機器人路徑規(guī)劃
研究[D ].杭州：浙江大學(xué)，2012.[6] 張曉冉，居鶴華.基于D 4 Lite 算法的估價函數(shù)分析[_!].計算
機工程，2012，38(1) :154 —156.
作者簡介：
程志
（1994 -），男，碩士研究生，研究方向為移動機器人智
能控制，路徑規(guī)劃，E-mail:****************。
張志安（1979 -），男，博士，副教授，研究領(lǐng)域為移動機器人,
智能彈藥，控制工程。
輸出脈沖數(shù)反饋至特征建模控制模塊，特征建模控制模 塊計算出PFM 模塊的輸人頻率，通過PFM 模塊得到IGBT 的 觸發(fā)脈沖，進而得到高壓脈沖單元的輸出脈沖。高壓脈沖單 元的特征建模控制方法SIMULINK 仿真電路如圖7所示。
Lgl3
脈沖變壓器
Discrclc,1—
1(—
TL=2xl 〇-7s.Cs3
Lsl2 ^
powerGUI
時間
/S
圖8
高壓脈沖單元控制效果對比
從圖8中可知，因此特征建模控制方法的控制效果更 好。其高壓脈沖單元性能對比如表3所示。
表3
脈沖單元性能數(shù)據(jù)對比
性能指標(biāo)PID
特征建模
脈沖數(shù)1920響應(yīng)時間
/ms
6.7
3.5
4結(jié)論
通過對靜電除塵脈沖電源結(jié)構(gòu)進行分析，建立了基于
(上接第73頁)4
結(jié)論
本文提出了一種基于IT Lite 算法的三維路徑規(guī)劃方 法，在代價地圖中引入高程信息，并將相鄰節(jié)點的高程差帶 人到路徑代價函數(shù)中；針對路徑安全性問題提出最大爬坡 坡度和最大側(cè)傾坡度，使得規(guī)劃路徑在減少路徑長度的基 礎(chǔ)上具有相對的安全性。由于改進算法采用空間換時間的 策略，針對每個遍歷節(jié)點都存儲了其八向的爬坡坡度和對 應(yīng)的側(cè)傾坡度，有效減少了路徑規(guī)劃中的重復(fù)計算，但對內(nèi) 存空間的占用較大，下一步將會針對算法的占用空間進行 優(yōu)化。參考文獻：
[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
