改進D*lite 和時間彈性帶法的移動機器人路徑規劃*

吳 濤謝志軍陳科偉

(寧波大學信息科學與工程學院，浙江 寧波 315000)

路徑規劃對于實現移動機器人自主導航具有重要的意義，其目的是在規定環境中規劃出一條由起點到終點的最優路徑，評價指標可以是時間、距離或安全性等[1]。 根據對環境的了解程度可分為全局路徑規劃和局部路徑規劃[2]。 全局路徑規劃常見的有迪杰斯特拉算法[3]、A*算法[4]、跳點搜索算法[5]、D*lite 算法[6]、RRT*算法[7]等。 迪杰斯特拉算法、A*算法及跳點搜索算法多用于靜態環境下。 迪杰斯特拉算法利用貪心策略得到起始點到地圖上全部點的最短距離，因此在大型地圖上會消耗較多算力。 A*算法通過比較當前節點的八鄰域啟發函數值搜索路徑，提高了搜索的效率，但是當八鄰域中出現相同的最小值時，可能無法求出最優解。而跳點搜索算法是在A*算法的基礎上只對跳點進行搜索[8]，進一步提高了搜索效率。 然而上述3 種算法在動態環境下都難以保證效果。 D*lite 算法雖適用于動態環境，但算法效率卻有待提高。RRT*算法也可用于動態的路徑規劃，其優勢在于對環境建模要求不高，可在不同的地圖模型上進行規劃；但是其規劃精度和迭代次數有較大關系，更高的精度意味著更加耗時。

局部路徑規劃算法常見的有動態窗口法[8]、粒子群算法[9]、時間彈性帶法[10]等。 動態窗口法可實現實時避障，通過在一個動態的窗口中估計機器人可能的軌跡決定機器人的下一個動作，但在復雜的環境下對于軌跡的評估將變得十分復雜，使得效果下降。 粒子群算法將每一條規劃的路徑視為一個粒子，通過對粒子群進行演化，選擇最優的粒子個體，該算法隨機性較大，同樣不適合復雜環境。 時間彈性帶法通過配置多個優化目標，包括路徑長度、規劃時長、安全距離以及運動學約束等，利用圖優化的方式進行問題的求解，但該算法對于全向機器人的支持較差[11]。

D*lite 算法(以下簡稱D*lite)是一種增量式啟發算法，相比于廣泛使用的A*算法，更適用于在未知環境下的路徑規劃。 Setiawan 等[12]通過對A*算法和D*lite 進行仿真和試驗的對比，得出結論:D*lite 在動態環境下，多數情況要好于A*算法。由于D*lite 還存在一些問題，各種改進方法也被提出，Xie 等[13]提出了一種改進的D*lite，通過子節點選擇優先級策略，由垂直方向的節點狀態決定對角方向的節點是否展開，使得規劃的路徑與障礙物之間產生了安全距離，但該方法會增加節點數量導致非最優解的出現。 Yao 等人[14]提出了一種將D*lite 和人工勢場法相結合的融合算法，并將其應用于復雜環境下的無人船自動駕駛，仿真實驗表明該算法規劃的路徑平滑且安全，但是該算法并未對D*lite 的規劃效率進行優化。 Li 等人[15]將D*lite和其提出的人工不可遍歷頂點的概念相結合，提出了一種可以快速重規劃的多機器人路徑規劃算法，在檢測到碰撞發生前可基于人工不可遍歷頂點和D*lite 進行路徑的重規劃。 黃魯等[16]提出了一種基于路徑優化的D*lite，通過將懶惰視線法和距離變換引入D*lite 中，使得算法可以在任意方向進行拓展，同時和障礙物保持了一定的安全距離，但是這樣會降低算法的搜索效率。 劉軍等[17]通過觀察目標點與障礙物之間的關系，引入關鍵節點概念來降低搜索空間，讓算法只對障礙物附近的關鍵點進行訪問，路徑由相互連接的關鍵點構成，很好地降低了搜索空間，但是產生了路徑貼近障礙物的問題。

由于機器人在實際應用中需要考慮的因素更為復雜，單一的全局路徑規劃常常難以勝任。 全局路徑規劃和局部路徑規劃相結合成為一種更為常見的做法。 當前和全局規劃算法結合較多的局部規劃算法有動態窗口法及時間彈性帶法。 龐磊等[18]將A*算法和時間彈性帶法相融合，通過激光點云標記非行人動態信息，生成全局靜態地圖和局部動態地圖。 利用A*算法在靜態地圖中進行全局規劃，時間彈性帶法在局部地圖中進行動態規劃實現了行人目標的動態跟蹤。 李秀智等[19]提出了基于復合式協同策略的機器人自主探索方法，利用改進的RTT 算法進行機器人自主探索，將A*算法和時間彈性帶法相融合進行路徑規劃。 通過增加約束條件，解決機器人倒退時容易碰撞到障礙物的問題，實現了機器人自主探索及路徑規劃。

針對以上提到的問題，本文提出一種結合改進D*lite 和時間彈性帶法的融合算法，通過全局優化提高搜索效率，減少冗余節點，通過局部優化降低動態物體對算法規劃效率的影響，并提高路徑安全性。

1 全局路徑優化

1.1 D*lite

D*lite 采用一種從目標點向起始點規劃的策略，并適用于起始點改變的情況。 算法流程如圖1 所示。

圖1 D*lite 流程圖

D*lite 在初始化地圖后，首先預規劃出一條最優路徑。 當機器人運動后，每運動一次就判斷當前位置是否為目標點。 若為否，從優先列表中選出k 值最小的節點作為下一個起點。 當路徑出現障礙物后，將路徑上受影響的點更新狀態后再次置入優先列表中，并重新從目標點開始規劃出一條路徑。由于采用了先前的節點信息，可以實現較快的重規劃。

在生成最優路徑這個階段，D*lite 會在地圖上建立一個“路徑場”，增量式的靠近目標。 使得算法資源消耗巨大，同時會產生一些冗余拐點。

為了提高規劃效率，在D*lite 拓展路徑節點前增加一個篩選跳點的過程，減少對一些無關節點的計算和存儲。

1.2 跳點篩選策略

傳統D*lite 拓展節點時，是在當前節點的八鄰域中找到下一個節點，同時將這些點都存儲在優先列表中，導致搜索效率下降。 而傳統跳點搜索策略也會在對角方向產生冗余，本文通過在對角方向搜索時，增加一個判定過程以消除這種冗余。 具體可分為2 種情況:

情況1 遍歷的方向為水平或豎直

①在這兩個方向上的節點若為起始節點或者目標點，則認為是關鍵點。

②若在這兩個方向上的某一節點有強迫鄰點則認為是關鍵點。

如圖2 所示，節點Y的八鄰域中存在障礙物，且節點X經過Y到達節點N的距離代價相比于不經過Y到達N的任意路徑的距離代價小，就認為Y有強迫鄰點N。

圖2 關鍵點篩選策略

情況2 當遍歷方向為對角方向

當沿著對角方向搜索時，若對角方向的節點在水平或者垂直方向上的延伸節點滿足情況1，且這些滿足情況1 的延伸節點都與出發點間無障礙物，則將這些延伸節點置入優先列表。 如圖2 所示，從出發點Y開始進行對角方向的搜索，在T節點的水平方向上有節點Z是個關鍵點，且節點Z與出發點Y之間無障礙物，則將Z置入優先列表。 值得注意的是，情況2 時，對角節點在水平或豎直方向的符合條件1 的全部點都需與遍歷的出發點間沒有障礙物，才可舍棄對角方向上的點，否則必須保留對角方向上的點。

1.3 優化搜索步長

在每次搜索關鍵點時都會從當前節點出發一直遍歷到關鍵點或者邊界。 當地圖較大時，會導致搜索效率的下降。 可通過計算當前點和起始點的距離，動態地調整搜索的步長，提高搜索效率。 常見的距離算法有歐式距離、對角距離和曼哈頓距離。 為確保最大可能地搜索到關鍵點，本文選用曼哈頓距離計算搜索步長，公式如下:

式中:d(c，s)表示搜索的步長，c和s分別表示當前點和起始點。 當在搜索步長內未發現關鍵點時，則將步長依次乘以一個倍數繼續進行搜索，L為地圖的最大邊長。

2 局部路徑優化

針對動態物體對D*lite 規劃效率影響大、規劃路徑安全性低的問題。 本文通過改進的時間彈性帶法進行局部規劃加以解決，當時間彈性帶法無法完成局部規劃時，再調用改進D*lite 進行重規劃。

2.1 時間彈性帶法及其約束

時間彈性帶法是一種適用于競速機器人的局部路徑規劃算法，具有規劃速度快、參數配置靈活的特點。 但是對于可以原地旋轉的機器人會產生不必要的倒退及轉折，增加運動時間，在某些情況下甚至會導致路徑規劃失敗[20]。 本文通過增加轉向約束來避免以上情況的發生。

定義機器人在時刻k的位姿為:

式中:xk、yk為機器人在地圖坐標系中的坐標，βk∈S1，S1∈[－π，π]為機器人的偏航角。 機器人的位姿序列可表示為:Q＝{Sk}k＝1，…，n。 設相鄰的兩個位姿間的時間間隔為ΔT，則時間序列可表示為ΔT＝{ΔTk}k＝1，…，n－1，其中ΔTk表示從位姿Sk到位姿Sk＋1所用的時間。 將位姿和時間序列合并可得到時間彈性帶b＝{Q，T}。 圖3 為三個連續的時間彈性帶。

圖3 時間彈性帶序列

而時間彈性帶法的優化模型可以表述為找到在最短時間內讓機器人從初始位姿S1轉換到目標位姿Sn的一系列路徑點，同時需要滿足運動學上的約束及保證機器人的安全。 該過程可以轉化為求解一個非線性的函數:

它由以下約束限制:

式中:初始位姿S1和最終位姿Sn分別受到Sstart和Sfinal的約束。 機器人的速度及加速度分別由不等式vk(·)及αk(·)約束。 不等式Ok(Sk)約束了機器人和障礙物之間的最小距離。

2.2 速度和加速度約束

機器人的平均線速度vk和平均角速度ωk可以由兩個連續位姿Sk＋1，Sk的距離范數、偏航角的差值及對應的時間間隔ΔTk分別計算得到，如式(4)和式(5)所示:

令vmax，ωmax分別表示機器人的最大線速度和最大角速度。 則速度的約束可用式(6)表示:

機器人的平均線加速度ak和平均角加速度表示如下:

令amax，分別表示機器人的最大線加速度和最大角加速度。 則加速度的約束可用式(9)表示:

2.3 躲避障礙物

設機器人在位姿Sk時，局部區域的障礙物集合為O，οi表示集合中的障礙物，i＝1，2，…，R。 與障礙物的最小間距設為ρmin。 則機器人與障礙物之間的約束可表示為:

2.4 轉向約束

本文研究的是可原地旋轉的全向機器人，利用式11 來提取機器人運動的方向。

式中:qk＝[cosβk，sinβk＋1，0]T，而κ是一個可變的比例因子(本文中取κ＝102)，dk，k＋1表示兩個位姿間的距離向量，符號<>計算向量的混合積。γ(Sk，Sk＋1)的取值為{－1，1}。 當取值為－1 時機器人向后運動。 當機器人需要向后運動時，通過建立轉向約束，使得機器人進行原地旋轉。 約束如下:

式中:β表示全局規劃路徑上離機器人最近的點和機器人x軸的夾角，β0為機器人當前位姿的偏航角。ωmax為機器人最大的角速度。

3 算法融合

改進D*lite 和時間彈性帶法融合后，在擁有動態規劃能力的基礎上，提高了搜索效率及路徑的安全性。 融合步驟為:改進D*lite 規劃全局路徑，得到全局路徑點。 將兩個相鄰的路徑點確定為局部路徑規劃的起始點和目標點。 之后，進入到局部規劃中，若局部規劃成功，判斷是否為全局的終點；否則調用改進D*lite 進行重規劃。 融合算法流程圖如圖4 所示。

圖4 融合算法流程圖

4 仿真及實驗驗證

為了驗證改進算法的有效性，對本文提出的改進D*lite(以下簡稱JD*lite)、改進時間彈性帶法及融合算法分別進行仿真實驗。 之后，將融合算法應用在真實的物理環境中進行實驗驗證。 硬件平臺為:Intel Core i5 8400 處理器，16 GB 內存；編程語言Python，C＋＋。

4.1 全局仿真實驗

在20×20 及200×200 的柵格地圖上分別進行隨機的障礙物填充，各生成10 份不同的地圖。 規定起點為(1，1)，終點分別為(20，20)及(200，200)。分別用D*lite、RRT*及JD*lite 進行仿真實驗。并設定RRT *在兩種情況下的迭代次數分別為2 000次及15 000 次。 仿真實驗結果如圖5、圖6所示。

圖5 20×20 地圖下3 種算法數據對比

由圖5(a)及圖6(a)可以看出，RRT*的路徑節點個數在兩種情況下都要多于另外兩種算法。 這是因為RRT*不是以單個柵格進行子節點的拓展，單個柵格中會出現多個拓展節點，這大大增加了節點的個數。 而JD*lite 在路徑節點個數上體現出較大的優勢，兩種情況下的平均節點個數僅為15個及32個。 遠低于RRT*的162個、382個和D*lite 的34個、293個。 這是因為JD*lite 在D*lite 的基礎上，引入了跳點搜索策略來搜尋子節點，同時刪除路徑上的冗余跳點，使得JD*lite 在柵格地圖上的路徑節點數為三者最優。

圖6 200×200 地圖下3 種算法數據對比

在規劃的路徑長度上，觀察圖5(b)、圖6(b)可知，JD*lite 在大多數情況下處于領先。 在兩種情況下的仿真實驗中，僅有一次路徑長度長于RRT*，其他均優于D*lite 及RRT*。 平均優于D*lite規劃長度7.57%及RRT*規劃長度9.83%。

出現上述現象的原因是D*lite 采用八鄰域做路徑規劃，而JD*lite 可跳出八鄰域的限制，實現更加靈活的規劃。 同時刪除了冗余的拐點使得路徑進一步縮短。 而RRT*的規劃方式雖然更加靈活，但是需要迭代的次數也大大增加，在本文設定的迭代次數下，效果弱于JD*lite。

在規劃時間方面，20×20 地圖下D *lite 和JD*lite的規劃時長差距不大，平均時間為0.04 s 和0.10 s。 而在200×200 的地圖上時，則產生了較大的差距，平均時間分別為0.93 s 和4.55 s。 這是因為JD*lite 遍歷地圖時僅需要判斷該點是否為跳點，無需存儲大量非關鍵點，每次進行子節點拓展時需要根據節點的關鍵值進行排序操作，而這個過程較為耗時。 減少排序隊列的長度可有效降低規劃時長。 RRT*的規劃時長為3 者最長，原因是RRT*在迭代次數固定的情況下，其規劃速度和規劃地圖的難度有較大的關系，當遇到狹窄的通道時，會明顯降低規劃速度。

兩種情況下各隨機選出一份規劃路徑圖進行比較，見圖7。 可以看到三種算法都能規劃成功，D*lite規劃的路徑有較多的轉折，從而導致路徑冗余。 而RRT*生成的路徑較為曲折，不適合直接用于機器人的路徑導航。 而觀察圖7 發現D*lite 規劃的路徑總是貼近障礙物的拐角處，這容易導致機器人的碰撞事故發送，需要結合局部路徑規劃，進行安全性優化。

圖7 3 種算法的規劃結果

4.2 改進時間彈性帶法有效性仿真實驗

為了驗證時間彈性帶法改進的有效性，本文使用Stage 仿真平臺進行仿真實驗。 首先，建立如圖8所示的仿真環境，并導入配備了激光雷達的Turtlebot2 機器人模型。

圖8 未優化前的時間彈性帶法

如圖8 和圖9 所示，圖中第1 行為虛擬仿真環境。 下面第2 行為第1 行中虛線區域的地圖及規劃路徑放大圖。 從圖8 中可以看到，當目標點在機器人正方向的側面及后面時，機器人會產生倒退的路徑。 在圖9 中，改進后的時間彈性帶法在原地調整自身的位姿后，克服了上述的問題。

圖9 優化后的時間彈性帶法

4.3 融合算法有效性仿真實驗

為驗證本文提出的融合算法的有效性，分別對D*lite，JD*lite 及融合算法在環境相同的柵格地圖上進行路徑規劃，并設置動態障礙物。 動態障礙物在水平或者豎直方向做往復的勻速運動。 圖10(a)為三種算法未與動態障礙物相遇時的情況，結合表2可知，JD*lite 在規劃路徑長度和規劃速度上處在領先的地位，同時發現JD*lite 規劃的路徑貼近障礙物，存在安全隱患。 而融合算法由于加入了局部規劃，在規劃時間上比JD*lite 要多；同時考慮了和障礙物的最小距離，使得路徑長度也有所增加，但是依然比原始的D*lite 提升了4.21%、45.58%。 圖10(b)為動態障礙物運動到D*lite 預規劃好的位置上后重規劃生成的路徑。 可以看到D*lite 成功地進行了重規劃，但是由于動態障礙物的運動導致規劃路徑變的不穩定，產生了路徑的分叉。 觀察圖10(c)并結合表1 可知，JD*lite 在動態物體的擾動下依然可以規劃出路徑，同時可以發現規劃路徑的距離變大了。 這是因為動態物體使搜索的跳點位置發生變動，選擇的跳點可能不是真正的最優點。 結合圖10(d)及表2 可知，當在已規劃好的路徑上出現動態障礙物時，融合算法可在局部路徑上進行局部動態規劃，避開動態障礙物之后，回到原來的路徑上，同時規劃的路徑更加平滑，符合機器人的運動約束。 在保持最優路徑的同時，做到安全高效的規劃。由表2 可以看到局部路徑規劃的重規劃用時要比JD*lite 的用時要長，但是規劃的路徑長度更接近原始長度。

表1 靜態路徑規劃性能對比

表2 動態路徑規劃性能對比

圖10 動態路徑規劃圖

4.4 融合實驗與分析

為進一步驗證本文提出的融合算法在移動機器人路徑規劃上的可行性，利用機器人操作系統(Robot Operating System，ROS)在真實環境下進行實驗。 系統環境為Ubuntu 16.04 操作系統，實驗機器人為Turblebot2 機器人，如圖11(a)所示。

本文的實驗場地為辦公室走廊到電梯口的一部分，如圖11(b)所示。 通過機器人所配置的激光雷達建立的地圖如圖11(c)所示。

圖11 實驗環境及建圖

分別運行A*算法、D*lite、JD*lite 及融合算法，得到的路徑如圖12 所示，圖12(a)為A*算法規劃的路徑，可以發現在規劃好的路徑上遇到未知障礙物時，A*算法不能繞過未知障礙物，導致路徑規劃失敗。 圖12(b)為傳統D*lite 算法生成的路徑，傳統的D*lite 繞過未知障礙物和動態障礙物后到達了目標點，但是從運動過程中可以看到，機器人會與障礙物發生碰撞，造成危險，這是由于D*lite 算法本身并未考慮機器人和障礙物的距離。 圖12(c)為JD*lite 規劃生成的路徑圖，其規劃的路徑比起D*lite 算法減少了一些多余的拐點，路徑的轉折角度幅度更大。 同時發現規劃路徑和障礙物的距離較近，雖然最終也繞過了障礙物，但是在這個過程中出現了一些停頓，并且在運動過程中還和障礙物發生了輕微的碰撞。 圖12(d)為融合算法生成的路徑，可以看到融合算法生成的路徑較為平滑，且與障礙物始終可以保持一個最小的距離，在遇到動態障礙物時成功地進行了局部動態規劃，避開了正在運動的物體。

圖12 對比實驗

表3 給出D*lite、JD*lite 及融合算法在圖12地圖中的性能表現。

表3 三種算法性能

由表3 可知，融合算法相比JD*lite 在時間及長度上雖然有所增加，但是提高了機器人的安全性。相比于D*lite 在規劃時長、路徑長度、最小距離及運動時長上分別優化了67.10%、11.04%、100%和11.99%。

5 結論

本文通過將改進的跳點搜索引入到D*lite 中進行關鍵點的篩選，融合時間彈性帶法進行局部路徑優化，提高了規劃效率、提升了動態情況下的規劃能力，同時提高了生成路徑的安全性。 算法在真實的環境下獲得了較為明顯的優化效果。

需要指出的是本文算法研究的是以柵格地圖為基礎的路徑規劃，規劃結果的精度是由柵格的大小決定的。 當選用較小柵格尺寸的模擬地圖環境時會導致算法的性能有所下降。 同時，本文的實驗環境較為簡單，在復雜環境中大量的動態物體對于算法的穩定性要求也會更加嚴格。 同時由于設置了和障礙物的安全距離約束，致使機器人在狹窄通道中的運動受到一定限制。

在接下來的研究中，將嘗試復雜動態場景下的移動機器人路徑規劃研究，進一步推動移動機器人路徑規劃的研究與實踐。