大規(guī)模動態(tài)環(huán)境下混合路徑規(guī)劃方法研究

摘" 要：該研究提出一種新的混合路徑規(guī)劃方法，用來改善大規(guī)模動態(tài)環(huán)境中常見的全局路徑規(guī)劃、實時跟蹤和避障等問題。首先，該文設計一種安全的A*算法，簡化風險成本函數和距離成本的計算。其次，從安全A*生成的規(guī)劃路徑中提取關鍵路徑點，從而減少網格節(jié)點數，實現平滑路徑跟蹤。最后，采用基于自適應窗口的實時運動規(guī)劃方法，實現關鍵路徑點切換的同時路徑跟蹤和避障（SPTaOA）。通過仿真和實際實驗驗證該方法的可行性和性能。研究結果表明，所提出的混合路徑規(guī)劃方法能夠滿足移動機器人在大規(guī)模動態(tài)環(huán)境中的應用需求，實現全局路徑規(guī)劃、跟蹤和避障的有效結合。

關鍵詞：混合路徑規(guī)劃；安全A*算法；自適應窗口方法；避障運動規(guī)劃；關鍵路徑點

中圖分類號：TP242" " " 文獻標志碼：A" " " " " 文章編號：2095-2945（2025）01-0039-06

Abstract： This research proposes a new hybrid path planning method to improve common global path planning， real-time tracking and obstacle avoidance problems in large-scale dynamic environments. Firstly， this paper designs a secure A* algorithm that simplifies the calculation of risk cost function and distance cost. Secondly， key path points are extracted from the planned path generated by safety A* algorithm， thereby reducing the number of grid nodes and achieving smooth path tracking. Finally， a real-time motion planning method based on adaptive windows is used to realize simultaneous path tracking and obstacle avoidance （SPTaOA） while switching critical path points. The feasibility and performance of the method are verified by simulation and practical experiments. The research results show that the proposed hybrid path planning method can meet the application requirements of mobile robots in large-scale dynamic environments， and realize an effective combination of global path planning， tracking and obstacle avoidance.

Keywords： hybrid path planning; safety A* algorithm; adaptive window method; obstacle avoidance motion planning; critical path point

在大規(guī)模動態(tài)環(huán)境中，移動機器人在實時路徑規(guī)劃和無碰撞路徑跟蹤方面面臨更大的挑戰(zhàn)。路徑規(guī)劃是自主移動機器人的核心技術，也是導航研究的重要課題。通常，路徑規(guī)劃可以分為2種類型：全局路徑規(guī)劃和局部路徑規(guī)劃，這取決于環(huán)境和任務的性質。全局路徑規(guī)劃（GPP）在已知環(huán)境下已經得到廣泛研究。其中，A算法是一種在柵格地圖中尋找最短路徑的高效方法，也是目前應用最廣泛的算法之一。然而，傳統(tǒng)的A算法生成的路徑與障礙物相鄰，這在機器人跟蹤路徑時增加了碰撞風險。

為了提高路徑的安全性，Bayili和Polat提出了一種適用于計算機游戲的路徑搜索算法，稱為有限損傷A*，該算法以損傷程度作為可行性準則，并考慮碰撞風險以獲得更安全的路徑[1]。Park等[2]引入了一種適用于移動機器人的安全全局路徑規(guī)劃（SGPP）方法，稱為修正A算法，該方法使用配置空間計算模型處理潛在風險，并將風險成本納入A的啟發(fā)式函數。Jie等[3]開發(fā)了多啟發(fā)式A*算法，利用不同的啟發(fā)式函數來引導復雜環(huán)境中移動機器人的多目標規(guī)劃。

當環(huán)境包含動態(tài)變化時，Murillo等[4]提出了實時A搜索算法（D算法）來適應這種變化。此外，Bhattacharya等[5]開發(fā)了適用于導航網格的動態(tài)修剪A算法，用于重新規(guī)劃。

傳統(tǒng)A*算法生成的路徑由大量相鄰的網格節(jié)點組成，導致相鄰節(jié)點之間距離很短。在路徑跟蹤過程中，由于路徑節(jié)點密集，相鄰節(jié)點之間的距離較小，機器人難以實現平滑的路徑跟蹤。而且，當環(huán)境包含大量動態(tài)變化時，傳統(tǒng)的A算法和重新規(guī)劃方法可能會面臨計算復雜性問題，導致路徑搜索時間顯著增加。因此，本研究旨在提出一種創(chuàng)新的混合路徑規(guī)劃方法，結合A*算法和自適應窗口法，得以在大規(guī)模動態(tài)環(huán)境中實現機器人的全局路徑規(guī)劃、實時跟蹤和避障。該方法的設計和性能將在后續(xù)章節(jié)中詳細討論和驗證，以展示其在復雜動態(tài)環(huán)境中的潛力和實用性。

1" 全局路徑規(guī)劃和關鍵路徑點

在傳統(tǒng)的A*算法的基礎上，本研究引入了一種基于配置空間（C空間）的方法，設計了一種安全的全局路徑規(guī)劃方法，旨在解決機器人路徑規(guī)劃過程中的碰撞風險問題。此外，為了應對規(guī)劃路徑中網格節(jié)點過多的問題，還提出了一種關鍵路徑點提取方法，以實現路徑的平滑跟蹤。

1.1" 安全全局路徑規(guī)劃

在全局路徑規(guī)劃階段，首要任務是獲取環(huán)境的網格表示，通常使用同時定位與地圖構建（SLAM）方法來獲得初始網格圖。然后，引入了配置空間（C空間）的概念，將障礙物進行適度擴展，以考慮機器人的尺寸和半徑，從而得到C空間的網格圖。這一過程的示意如圖1（a）所示。

接下來，定義了風險函數R（n），將C空間映射轉換為安全網格映射。

R（n）=α/r2。

式中：r表示節(jié)點n與最近障礙節(jié)點之間的距離；α（αgt;0）則表示風險因子。所有節(jié)點的風險成本都通過R（n）來評估，以獲得具有風險區(qū)域的安全網格地圖，如圖1（b）所示。很明顯，在障礙物節(jié)點附近會生成一層灰度值遞減的風險節(jié)點。

接下來，修改了傳統(tǒng)A*算法的代價函數如下

Fn（n）=Gn（n）+Hn（n）+Rn（n），

式中：Gn（n）為從起點S到當前節(jié)點n的實際代價；Hn（n）為從當前節(jié)點n到目標點G的估計代價（通過曼哈頓距離計算）；Rn（n）為當前節(jié)點n的風險值。

在安全網格地圖上，可以使用帶有上述代價函數的安全A*算法進行全局路徑規(guī)劃（在上、右、下和左方向上進行搜索）。得到的全局路徑表示為

Path=S，P1，P2，…，Pw，G，

式中：（P1，P2，…，Pw）為全局路徑上所有連接的網格節(jié)點。

如圖2（a）所示，當不考慮R（n）時可以獲得最短路徑；當考慮風險評估R（n）時，可以獲得相對安全的路徑，如圖2（b）所示。

1.2" 關鍵路徑點提取

一旦安全全局路徑規(guī)劃完成，將面臨著另一個挑戰(zhàn)，即路徑上包含了大量的網格節(jié)點，這可能導致機器人在跟蹤路徑時產生不連續(xù)的運動。為了解決這個問題，提出了關鍵路徑點的概念，并采用一種有效的方法來提取這些關鍵路徑點。

關鍵路徑點是規(guī)劃路徑上的特殊節(jié)點，它們被精心選擇以減少路徑中的節(jié)點數量，并實現路徑的平滑跟蹤。這些關鍵路徑點具有2個重要的特征：首先，它們必須覆蓋整個規(guī)劃路徑，確保路徑的完整性；其次，它們之間的距離足夠大，以確保機器人能夠在相鄰路徑點之間實現平滑的運動。

關鍵路徑點被表示為路徑Path的子集，即

KPath=S，KP1，KP2，…，KPm，G ，

式中：（KP1，KP2，…，KPm）?（P1，P2，…，Pw）。

尋找和提取關鍵路徑點KPath的方法和步驟如下所示。

步驟1：將起點S存儲在集合KPath中，作為第一個關鍵路徑點，并將其記錄為O。

步驟2：將O的下一個路徑點（節(jié)點）設為M，然后判斷M是否是目標點G，如果M是目標點，則跳轉到步驟5。

步驟3：將M的下一個路徑點（節(jié)點）設為N，然后判斷N是否是目標點G，如果N是目標點，則跳轉到步驟5。

步驟4：計算沿著從O到N的線段上任何節(jié)點i的風險值R（i）。如果R（i）≥ε，則將M設置為O之后的下一個關鍵路徑點，并將其添加到KPath中，然后設置O=M，跳轉到步驟2；如果R（i）lt;ε，則將M向路徑上的前一個節(jié)點移動一步，然后返回到步驟3。

步驟5：當找到目標點G時，所有關鍵路徑點都已找到，然后將G添加到集合KPath中作為最后一個關鍵路徑點。

上述查找和提取關鍵路徑點的過程如圖3所示。在路徑Path的跟蹤過程中，KPath中的關鍵路徑點將被視為機器人的實時子目標點Gt。

如圖3（b）所示，在判斷線段ON是否通過障礙節(jié)點時（即存在線段ON上任何網格節(jié)點i的R（i）gt;ε），首先需要計算線段上節(jié)點i的坐標。節(jié)點O的網格坐標表示為（Ox，Oy），節(jié)點N的坐標表示為（Nx，Ny）。然后可以使用以下方程計算dx和dy。

dx=Nx-Ox，

dy=Ny-Oy。

如圖4（a）所示，當|dx|≥|dy|時，線段ON上節(jié)點的數量為s=|dx|-1，并設置Δy=|dy/dx|。然后可以計算出線段ON上第i個節(jié)點的坐標，如圖4所示。

當|dx|≥|dy|時，線段ON上第i個節(jié)點的坐標可以通過以下公式計算

xi=Ox+sign（dx）×i，

yi=Oy+？sign（dy）×Δy×i」+0.5bc，

式中：sign（ ）為符號函數；？」為取整函數；i=1，2，…，s。

同理，當|dx|lt;|dy|時，線段ON上第i個節(jié)點的坐標可以通過以下公式計算

xi=Ox+？sign（dx）×Δx×i」+0.5bc，

yi=Oy+sign（dy）×i，

式中：i=1，2，…，s；（xi，yi）為線段ON上第i個節(jié)點的網格坐標。

這些公式用于計算線段ON上各個節(jié)點的坐標，其中dx和dy表示線段的水平和垂直位移，Δx和Δy表示水平和垂直位移的比例。sign（ ）函數返回參數的符號，負數返回-1，零返回0，正數返回1。取整函數？」將參數向下取整到最接近的整數，0.5bc表示一個常數偏移量，用于確保節(jié)點坐標在網格中正確對齊。這些公式是為了計算路徑上各個節(jié)點的坐標，以便在路徑跟蹤過程中機器人能夠按照這些坐標點順序移動。

2" 避障運動規(guī)劃

在移動機器人的路徑跟蹤過程中，需要進行局部路徑規(guī)劃以避開新的障礙物。本文采用了基于自適應窗口方法的改進型實時運動規(guī)劃來進行障礙物避讓。機器人的規(guī)劃控制周期為T。

2.1" 實時檢測局部環(huán)境

如圖5所示，機器人使用2D LiDAR來檢測障礙物的當前信息，假設LiDAR坐標系與機器人坐標系重合。測量的最大范圍為dmax，視場范圍為[Φmin，Φmax]，角度分辨率為ΔΦ；在機器人前進方向上的掃描角度（θR）被記錄為Φrob。在每次掃描之后，LiDAR獲取到的距離測量值為{d1，d2，…，dN}，距離di（i=1，2，…，N）對應的掃描角度為Φi=Φmin+（i-1）ΔΦ。

2.2" 規(guī)劃窗口內的可通行方向

如圖6所示，在LiDAR的范圍內設計了一個虛擬規(guī)劃窗口，其大?。窗霃剑閞win。在規(guī)劃窗口中，距離{d1，d2，…，dN}被表示為{l1，l2，…，lN}，其中l(wèi)i={Φi，dwi}，表示掃描角度Φi和相應的距離dwi（i=1，2，…，N）。

首先，根據與rwin的比較，將di調整以獲得dwi。方法是：如果di≥rwin，則設置dwi=rwin；否則設置dwi=di。

根據機器人的半徑（rrob），需要擴展障礙物的邊緣，因此dwi將再次進行調整。方法是：對于任何dwilt;rwin（i=1，2，…，N），如果dwi+1=rwin，則計算θ1=arctan（rrob/dwi），并使角度θ1內的所有距離（即[Φj，Φj+θ1]）等于dwi；如果dwi-1=rwin，則計算θ2=arctan（rrob/dwi），并使角度θ2內的所有距離（即[Φj，Φj-θ2]）等于dwi。

在規(guī)劃窗口中，調整初始范圍{d1，d2，…，dN}的示例如圖6所示。因此，在規(guī)劃窗口中，對于任何li={Φi，dwi}，如果dwi=rwin，這意味著機器人在Φi方向上可以通行（即沒有障礙物）。

2.3" 下一時刻的移動方向

在規(guī)劃窗口中，所有可通行方向的集合稱為機器人在當前時刻的可通行角區(qū)域，表示為

Ψpath=Ψpath1+Ψpath2+…，

Φmin≤Φi≤Φmax且dwi=rwin，i=1，2，…，N，

式中：Ψpath1、Ψpath2等表示不同方向的可通行角度區(qū)域。

如圖7所示，機器人的當前姿態(tài)為（xR，yR，θR），其當前子目標為Gt（xGt，yGt）。然后，相對于機器人前進方向的角度為θGt，其中θGt∈[-π，π]，計算如下：

當xGt≥xR時，值為

θGt=arctan（yGt-yR）/（xGt-xR）。

當xGtlt;xR且yGt≥yR時，值為

θGt=arctan（yGt-yR）/（xGt-xR）+π。

當xGtlt;xR且yGtlt;yR時，值為

θGt=arctan（yGt-yR）/（xGt-xR）-π。

這些方程用于計算機器人當前子目標相對于機器人前進方向的角度θGt。這個角度將用于決定機器人下一時刻的移動方向。

3" 基于關鍵路徑點的路徑跟蹤

在移動機器人的導航中，依次選擇KPath={S，KP1，KP2，…，KPm，G}中的關鍵路徑點作為實時運動規(guī)劃的子目標Gt，這可以實現全局路徑的平滑跟蹤和同時避免障礙物。然而，為了實現路徑跟蹤和避障的協調一體化，重要的是機器人需要根據環(huán)境的當前信息將關鍵路徑點切換為Gt。方法和步驟如下。

步驟1：當機器人從起點S出發(fā)時，它將KP1設置為子目標，即將Gt=KP1。

步驟2：當機器人與子目標Gt（=KPi）之間的距離小于閾值|RGt|min時，子目標將切換到下一個關鍵路徑點，即將Gt=KPi+1。

步驟3：如圖8所示，當Gt=KPi并且路徑段（KPi，…，Pj，…，KPi+1）上的路徑節(jié)點Pj出現在規(guī)劃窗口的Ψpath中時，子目標將切換到下一個關鍵路徑點，即將Gt=KPi+1。

步驟4：當Gt=KPi并且規(guī)劃窗口的Ψpath中出現關鍵路徑點KPi+j（jgt;0）時，子目標將切換到Gt=KPi+j。

使用上述方法，當前子目標將連續(xù)切換到以下關鍵路徑點，直到Gt=G。

在子目標切換中，距離比較用于確定路徑節(jié)點Pj（或關鍵點KPj）是否位于Ψpath中。例如，如果機器人R與路徑節(jié)點Pj之間的距離為RPj，則線段RPj與機器人前進方向的夾角為∠（RPj）（相應的掃描角度為ΦRPj），與此方向相對應的測量距離為dwRPj，那么如果RPjlt;dwRPj，則路徑節(jié)點Pj位于Ψpath中。同樣，可以使用此距離比較方法判斷關鍵點KPj。

4" 混合路徑規(guī)劃的過程

結合上述要素，在大規(guī)模動態(tài)環(huán)境中的實時導航中，路徑規(guī)劃和路徑跟蹤分別執(zhí)行。也就是說，每當給定一個目標點G時，首先通過安全的A*算法進行全局路徑規(guī)劃，并使用關鍵點查找算法提取其關鍵點。然后，機器人使用自適應窗口方法（運動規(guī)劃）進行全局路徑的跟蹤和避免障礙物。

這是一種基于安全A*算法和自適應窗口（DWA）方法的新型混合路徑規(guī)劃，用于全局路徑規(guī)劃和SPTaOA（避障），其優(yōu)點是：①適用于移動機器人的通用方法，不需要速度空間計算的建模要求，這是DWA方法所需的；②可以用于各種尺寸的動態(tài)環(huán)境，甚至在大規(guī)模環(huán)境中（即不受環(huán)境大小的影響）。

5" 仿真實驗與驗證結果

使用ROS（機器人操作系統(tǒng)）進行仿真實驗。機器人被模擬為直徑為0.5 m的塊。最大線速度設置為0.5 m/s，最大角速度設置為0.8 rad/s。運動規(guī)劃周期T=50 ms。kv=kω=1.0。

為了驗證基于自適應窗口方法的運動規(guī)劃性能，給出了機器人在未知靜態(tài)環(huán)境中的仿真結果，如圖9所示。紅色方塊表示動態(tài)障礙物，藍色表示機器人根據實時運動規(guī)劃從點S到點G的軌跡。圖9（a）是在（kGt=0.6，ksmo=0.3，ksaf=0.1）時的軌跡，圖9（b）是在（kGt=0.5，ksmo=0.2，ksaf=0.3）時的軌跡。由圖9可以看出，軌跡（具有較大的參數ksaf）與障礙物有一定的距離，這有助于安全。圖9（c）是機器人速度（v，ω）和規(guī)劃窗口半徑的曲線。

如圖10所示，在前往目標的途中，機器人連續(xù)遇到2個動態(tài)障礙物。

實驗結果表明，當障礙物突然出現在機器人前面時，機器人可以迅速改變方向，安全地避開移動的障礙物。因此，這種運動規(guī)劃方法對動態(tài)環(huán)境具有良好的適應性。

此外，使用運動規(guī)劃，如果規(guī)劃窗口太小，如圖11（a）所示，機器人可能無法及時避開障礙物；如果規(guī)劃窗口太大，如圖11（b）所示，機器人可能無法找到可選的移動方向（只能沿著墻壁行走）。使用自適應窗口方法，機器人可以根據環(huán)境信息動態(tài)調整規(guī)劃窗口的大小，其結果如圖11（c）所示。可以看出，圖11（c）中的軌跡明顯優(yōu)于圖11（a）和（b）中的軌跡。

6" 結論

本研究聚焦于在大規(guī)模動態(tài)環(huán)境中，針對移動機器人的全局路徑規(guī)劃、跟蹤以及避障問題。為了滿足實時導航的需求，提出了一種混合規(guī)劃方法，該方法結合了安全A*算法用于全局路徑規(guī)劃以及運動規(guī)劃用于路徑跟蹤和避障。具體來說，本文設計的安全A*算法簡化了帶有風險評估的成本函數的計算。實時運動規(guī)劃方法基于自適應窗口，通過提取和切換關鍵路徑點作為子目標，實現了全局路徑的平滑無碰撞跟蹤。實驗結果驗證了該混合路徑規(guī)劃方法的有效性，它將A*算法和實時運動規(guī)劃相結合，能夠滿足移動機器人在大規(guī)模動態(tài)環(huán)境中的實際應用需求。盡管全局路徑規(guī)劃（GPP）和局部路徑規(guī)劃（LPP）已受得到廣泛研究，但本文提出的這種混合路徑規(guī)劃方法是第一個應用安全A*算法和自適應窗口方法進行全局路徑規(guī)劃和SPTaOA的方法[4，6-7]。這種方法不需要進行速度空間計算的建模要求，并且適用于各種尺寸的動態(tài)環(huán)境，不受場景大小的限制。然而，由于許多參數是基于經驗和模擬實驗確定的，因此我們未來的工作將集中在如何有效地優(yōu)化這些參數上[8]。

參考文獻：

[1] LIM W Z， HSU D， LEE S W .Adaptive informative path planning in metric spaces[J].The International Journal of Robotics Research，2016，35（5）：585-598.

[2] PARK J H， NO J H， HUH U Y. Safe global path planning of Mobile robots based on modified A* algorithm[M]// the 8th international conference on robotic， vision， Signal Processing amp; Power Applications， 2014：99-105.

[3] JIE J， AMIR K， WILLIAM W M， et al.Path Planning and Tracking for Vehicle Collision Avoidance Based on Model Predictive Control With Multiconstraints[J].IEEE Transactions on Vehicular Technology，2017，66（2）：952-964.

[4] MURILLO M， SáNCHEZ G， GENZELIS L， et al.A Real-Time Path-Planning Algorithm based on Receding Horizon Techniques[J].Journal of Intelligent amp; Robotic Systems，2018，91（3-4）：445-457.

[5] BHATTACHARYA， SUBHRAJIT， GHRIST， et al.Persistent Homology for Path Planning in Uncertain Environments[J].IEEE Transactions on Robotics： A publication of the IEEE Robotics and Automation Society，2015，31（3）：578-590.

[6] EE L S， ONG P， KAH C C .Solving the optimal path planning of a mobile robot using improved Q-learning[J].Robotics and Autonomous Systems，2018（115）：143-161.

[7] 郝兆明，安平娟，李紅巖，等.增強目標啟發(fā)信息蟻群算法的移動機器人路徑規(guī)劃[J].科學技術與工程，2023，23（22）：9585-9591.

[8] 王旭，朱其新，朱永紅.面向二維移動機器人的路徑規(guī)劃算法綜述[J].計算機工程與應用，2023，59（20）：51-66.