基于改進HybridA*的非完整約束機器人路徑規(guī)劃算法研究

田雨　林松　房殿軍　江競宇

摘要：為了實現(xiàn)單舵輪AGV在物流場景下的精準(zhǔn)自主導(dǎo)航，針對基于HybridA*算法搜索的路徑容易貼近障礙物的缺陷和算法在路徑平滑后可能與障礙物沖突的問題，本文提出一種基于改進HybridA*的非完整約束輪式移動機器人路徑規(guī)劃方法。對于路徑搜索部分，將距離場地圖引入啟發(fā)式函數(shù)，以提高搜索效率，并保持搜索路徑遠離障礙物；對于路徑平滑部分，將優(yōu)化問題轉(zhuǎn)化為二次規(guī)劃問題，通過邊界約束保證避障效果。在ROS平臺上仿真實驗的結(jié)果表明，該路徑規(guī)劃方法可有效用于非完整約束系統(tǒng)，為單舵輪AGV自主導(dǎo)航的準(zhǔn)確運動奠定了路徑智能規(guī)劃的基礎(chǔ)。

關(guān)鍵詞：非完整約束機器人；路徑規(guī)劃；改進HybridA*；二次規(guī)劃

中圖分類號：TP242.2 文獻標(biāo)志碼：A doi：10.3969/j.issn.1006-0316.2023.03.010

文章編號：1006-0316 （2023） 03-0063-09

Path Planning of Robot with Nonholonomic Constraint based on Improved Hybrid A* algorithm

TIAN Yu1，LIN Song1，F(xiàn)ANG Dianjun1，2，JIANG Jingyu1

（ 1.School of Mechanical Engineering， Tongji University， Shanghai 201804， China;

2.Qingdao Sino-German Institute of Intelligent Technologies， Qingdao， 266000， China ）

Abstract：In order to achieve precise autonomous driving of the single steering wheel automated guided vehicle （AGV） in logistics scene， this paper proposes a path planning method for wheeled mobile robots （WMRs） with nonholonomic constraint based on improved Hybrid A* algorithm， so that the problems of original Hybrid A* algorithm on obstacles avoidance could be solved. For the path-search part， a distance field is introduced into the heuristic function， which could improve the searching efficiency and keep the search path away from obstacles. For the path-smoothing part， a quadratic programming problem is described to guarantee the obstacle avoidance through boundary constraints. The results of simulated experiments on ROS show that the method proposed could be effectively applied to nonholonomic constrained robots， and could lay the foundation for the accurate movement of single steering wheel AGV on autonomous navigation.

Key words：robot with nonholonomic constraint；path planning；Hybrid A* algorithm；quadratic programming

AGV（Automated Guided Vehicle，自動引導(dǎo)車）作為一種無人操作的智能化搬運設(shè)備，在制造業(yè)生產(chǎn)、倉儲物流等作業(yè)環(huán)節(jié)中發(fā)揮著重要作用。其中，單舵輪AGV具有底盤結(jié)構(gòu)簡單、驅(qū)動方式簡明、成本低廉等優(yōu)點，在工業(yè)物流領(lǐng)域被大量應(yīng)用，但對該類型的AGV進行路徑規(guī)劃及其運動控制時，需要額外考慮車輪的非完整約束條件，讓機器人在給定運動路徑下始終保持車輪無滑動滾動的狀態(tài)[1]，才能使其在自主導(dǎo)航時保證較高的可控運動精度。如果能為單舵輪AGV自主導(dǎo)航規(guī)劃出合理、可達、無碰撞的全局路徑，就可降低軌跡跟蹤控制的難度，顯著提高AGV的運行效率和軌跡跟蹤精度，以此為AGV自主導(dǎo)航的準(zhǔn)確運動提供技術(shù)支撐。

由國內(nèi)外相關(guān)研究可知，考慮非完整約束系統(tǒng)的路徑規(guī)劃方法主要包括曲線插值方法、采樣方法、最優(yōu)控制方法和機器學(xué)習(xí)方法[2]。其中，曲線插值和采樣方法由于具備較高的求解速度，是目前實際運用最為廣泛的方法，也更符合AGV的低算力特點和高實時性要求，因此是本文的主要研究方向。Dolgov等[3]提出的HybridA*算法，便是此類考慮非完整約束的早期典型算法，該算法由A*算法改進而來，通過輪式底盤可實現(xiàn)的曲線軌跡來進行節(jié)點拓展，滿足輪式車輛的非完整約束。隨后，部分學(xué)者以該方法為基礎(chǔ)算法，從不同角度提出了改進方法。任秉韜等[4]設(shè)計了可變半徑的RS（Reeds- Shepp）曲線，增加了搜索靈活性；Saeid等[5]引入可見性圖來引導(dǎo)節(jié)點擴展，大大降低了擴展節(jié)點的數(shù)量；還有其他文獻[6-8]在HybridA*基礎(chǔ)上提出不同的子節(jié)點拓展方式、啟發(fā)函數(shù)或節(jié)點搜索策略，以提高搜索效率。

另一方面，通過原始HybridA*算法搜索出的路徑存在包含不必要的轉(zhuǎn)彎和曲率不連續(xù)的問題，這在實用中還有待進一步平滑優(yōu)化。Dolgov[3]采用梯度下降法對HybridA*算法搜索出的路徑進行平滑，此后也有文獻[7-9]采用了基于梯度下降的數(shù)值優(yōu)化方法來對圖搜索出的路徑進行平滑后處理。然而，采用梯度下降法優(yōu)化得到的路徑無法保證無碰撞，需要反復(fù)修改優(yōu)化目標(biāo)，直到通過碰撞檢測[7]。此外，HybridA*算法還存在初始搜索的路徑太過貼近障礙物的現(xiàn)象，這不僅不利于AGV的行駛，也會壓縮路徑平滑優(yōu)化的空間，增加路徑平滑的優(yōu)化難度。

針對以上問題，本文在HybridA*算法的路徑搜索過程中引入距離場，通過動態(tài)調(diào)整距離閾值和修改啟發(fā)式函數(shù)的形式，使得初始搜索路徑遠離障礙物；在平滑后處理中，將優(yōu)化問題描述為二次規(guī)劃問題，以線性約束的形式保證優(yōu)化后路徑的無碰撞性，最終得到適合單舵輪AGV的自主導(dǎo)航路徑規(guī)劃方法。其規(guī)劃框架如圖1所示。

1 路徑搜索

1.1? HybridA*算法原理

傳統(tǒng)A*算法是一種基于柵格化地圖的圖搜索算法，它的目標(biāo)是搜索出一條與障礙物無碰撞的距離最短路徑。在柵格地圖中，算法從起點出發(fā)，并以此為初始節(jié)點，不斷擴展節(jié)點周圍的子節(jié)點，并且選擇代價函數(shù)總值最小的子節(jié)點作為下一個搜索點，不斷往復(fù)，直至到達終點。公式為：

f（n）＝g（n）＋h（n）? ? ? ? ? ? ? ? ? ?（1）

式中：g（n）為累計代價函數(shù)，物理意義是從起始節(jié)點到當(dāng)前節(jié)點的累計距離；h（n）為啟發(fā)式函數(shù)，物理意義是當(dāng)前節(jié)點到目標(biāo)節(jié)點的預(yù)估距；f（n）為代價函數(shù)，代表在途經(jīng)當(dāng)前節(jié)點這一條件

下的路徑預(yù)估總距離。

核心原理為：

（1）不斷尋找f（n）值最小的無碰撞節(jié)點進行擴展。如圖2（a）所示，在初始節(jié)點0的周圍，優(yōu)先擴展無碰撞節(jié)點1；

（2）如新節(jié)點已經(jīng)被擴展過，則通過比較g（n）的方式選擇更優(yōu)路徑。如圖2（b）所示，雖然2節(jié)點的f（n）值小于3節(jié)點，會被優(yōu)先擴展，但0－1－3路徑得到的g（n）值小于0－1－2－3的路徑，因此，當(dāng)3節(jié)點擴展后，2節(jié)點會被廢棄。

采用這樣的思路，可以將搜索方向引導(dǎo)向預(yù)估總路徑最短的方向，而并非沒有目標(biāo)的依次遍歷，因此可以高效率搜索到總距離最短的無碰撞路徑。

不難看出，A*算法最初的目標(biāo)是路徑長度最短，因而代價函數(shù)主要考慮距離因素；如果在代價函數(shù)引入其他因素，則可以得到滿足實際需求的路徑，如時間最優(yōu)、能量最優(yōu)等。

HybridA*與A*的算法基本思路相似，區(qū)別點主要是拓展節(jié)點的方式和代價函數(shù)的組成。

在拓展節(jié)點部分，A*算法可以向臨近柵格直接擴展，如圖3（a）所示；HybridA*算法則考慮了車輛的非完整約束，子節(jié)點位置為車輛以R的轉(zhuǎn)彎半徑行進l的距離后所在的位置，如圖3（b）所示。

在啟發(fā)式函數(shù)部分，HybridA*算法對同一個節(jié)點用兩種不同的方式計算，取兩種計算方式結(jié)果的較大值作為啟發(fā)代價。

第一種計算方式考慮非完整約束但忽略障礙物，計算當(dāng)前節(jié)點與目標(biāo)節(jié)點間符合運動學(xué)約束的最短路徑，在車輛允許倒車時，一般通過計算RS曲線得到。RS曲線是在無障礙物情況下滿足車輛運動學(xué)約束（起點位姿、終點位姿、最小轉(zhuǎn)彎半徑、允許倒車）的距離最優(yōu)曲線，如圖4所示。

第二種計算方式為考慮障礙物但忽略非完整約束，計算當(dāng)前節(jié)點與目標(biāo)節(jié)點間的無碰撞最短路徑，一般通過A*等圖搜索算法計算得到。第二種計算方式在障礙物較稠密的區(qū)域能夠產(chǎn)生更良好的效果[3]。

此外，HybridA*算法還會在每拓展n個節(jié)點時計算該節(jié)點到目標(biāo)位姿的RS曲線，如果曲線與環(huán)境障礙物無碰撞，則直接采納該路徑作為剩余路徑，結(jié)束搜索過程。這可以減少拓展的節(jié)點數(shù)量，大大提升搜索效率。完整的Hybrid A*算法流程如圖5所示。

1.2 啟發(fā)式函數(shù)改進

為使搜索出的路徑遠離障礙物，引入距離場地圖[10]思想，其原理是將柵格地圖基于廣度優(yōu)先搜索的思想進行遍歷，得到每個柵格點和最近障礙物的距離，被障礙物占據(jù)的柵格數(shù)值為1，距離障礙物越遠則數(shù)值越高。原始柵格地圖和由此生成的距離場地圖對比如圖6所示。

本文所涉及的物流場景一般不具備開闊空間，在該種環(huán)境下只考慮障礙物但忽略非完整約束的啟發(fā)函數(shù)往往效果更優(yōu)，因此本文舍棄了計算RS曲線作為啟發(fā)式函數(shù)的部分，而是只對障礙物進行考慮。引入了距離場系數(shù)后的啟發(fā)式函數(shù)可表示為：

式中：Xi為A*算法搜索得到的從當(dāng)前節(jié)點n到目標(biāo)節(jié)點所途徑的總共k個節(jié)點中的第i個節(jié)點； 為與該節(jié)點對應(yīng)的障礙物距離代價函數(shù)； 為該節(jié)點對應(yīng)的距離場數(shù)值；K為大于1的安全距離閾值。

式（3）可理解為，節(jié)點與障礙物的距離小于閾值時，賦較大的代價值；節(jié)點與障礙物的距離大于閾值時，代價值隨距離的增大而減小。

為了驗證啟發(fā)式函數(shù)的改進效果，本文在ROS Rviz平臺上建立仿真障礙地圖，分別以路徑搜索中較常見三種情況（S型連續(xù)彎、U型彎、長直線避障）設(shè)定初始位姿和目標(biāo)位姿進行路徑搜索實驗，如圖7所示。其中，完整障礙地圖尺寸為30 m×20 m，搜索分辨率0.1 m，因而搜索柵格尺寸為300×200；三次實驗的始末位姿已在圖中標(biāo)出?？梢钥闯觯倪M后的路徑能夠相較原路徑離障礙物更遠。

在此基礎(chǔ)上，為定量分析改進算法的性能，本文在該地圖上隨機設(shè)定起始位姿和目標(biāo)位姿進行50次重復(fù)實驗，箱型圖結(jié)果如圖8所示。箱型圖是一種體現(xiàn)數(shù)據(jù)分布情況的統(tǒng)計圖，其中箱體部分的紅線代表數(shù)據(jù)中位數(shù)（Q2）位置，箱體實線上界與下界分別代表數(shù)據(jù)上四分位數(shù)（Q3）和下四分位數(shù)（Q1）；箱體外部的藍色圓圈代表大于Q3－1.5×（Q3－Q1）或小于Q1－1.5×（Q3－Q1）的值，被稱作離群值；箱體虛線上界和下界分別為未離群的最大值和最小值。

圖8中表征兩種算法結(jié)果區(qū)別的規(guī)劃路徑相對長度差為：

式中： 為兩種算法規(guī)劃路徑的長度差； 為原算法規(guī)劃出的路徑長度； 為改進算法規(guī)劃出的路徑長度。

由圖8可以看出，相較于原始HybridA*算法，改進算法在搜索路徑時拓展節(jié)點的個數(shù)更少、算法總用時更短，在考慮了距離場地圖生成的額外開銷下，總體計算效率仍更優(yōu)。需要注意的是，為保證顯示尺度，圖8（a）（b）中出現(xiàn)的離群值僅用虛線代替，未標(biāo)注正確位置，但離群數(shù)據(jù)中改進算法的結(jié)果仍相較原算法效率更高。此外，改進算法搜索的路徑長度在大多數(shù)情況下會略大于原始算法的搜索結(jié)果，這是考慮了安全避障、放棄距離最優(yōu)的合理結(jié)果，與預(yù)期相符。

2 路徑平滑

上述路徑搜索算法能夠粗略得到一條符合非完整約束的可達路徑，但存在不必要的拐彎、路徑曲率變化不夠連續(xù)等問題。本文將其描述為標(biāo)準(zhǔn)二次規(guī)劃問題，借以來對路徑作進一步平滑處理：以軌跡的距離代價和曲率代價作為優(yōu)化目標(biāo)，把原始離散點與障礙物的距離引入約束條件，以保證優(yōu)化后的軌跡不會與障礙物發(fā)生碰撞，最后進行優(yōu)化求解。

二次規(guī)劃問題是一種特殊的非線性規(guī)劃問題，其數(shù)學(xué)模型可表示為：

式中：X為由所要優(yōu)化的變量構(gòu)成的矩陣；Q為二次型代價構(gòu)成的矩陣；c為線性代價構(gòu)成的矩陣；D為X的可行域。

考慮到由線性約束構(gòu)成的可行域一定是凸集；而當(dāng)Q矩陣為半正定矩陣時，目標(biāo)函數(shù)為凸函數(shù)。同時滿足上述兩個條件時，該問題為凸二次規(guī)劃問題，問題的局部最優(yōu)解即為全局最優(yōu)解，因此可以較方便地通過數(shù)值方法求得最優(yōu)解。

2.1 約束條件

二次規(guī)劃的目標(biāo)是對路徑搜索得到的路徑離散點進行進一步優(yōu)化，對于每一個離散點Pi，用三個變量{xi， yi， di}進行描述，因此對于初始搜索路徑上的n個離散點，應(yīng)共有3n個變量。如圖9所示。

di為優(yōu)化后的位置相對原始位置在路徑法線方向上的偏移距離；（xi0， yi0）為離散點原始坐標(biāo)； 為離散點的朝向，即笛卡爾坐標(biāo)系中該離散點指向下一離散點的射線與x軸的夾角。

有位置約束：

式中：（ ，? ）為離散點Pi優(yōu)化后的坐標(biāo)。

為保證偏移后的離散點不會與障礙物發(fā)生碰撞，對偏移量 增加線性約束，其中 的約束上下界 和 根據(jù)與障礙物的實際距離計算得到，如圖10所示，滿足：

2.2 優(yōu)化目標(biāo)

對離散軌跡點的優(yōu)化目標(biāo)有兩個，一是使其減少不必要的拐彎，二是保證路徑的平滑性，避免曲率突變。因此代價函數(shù)應(yīng)包含兩部分，即距離代價和離散曲率代價，分別對應(yīng)兩個優(yōu)化目標(biāo)。

對于第一目標(biāo)，一般來說，離散點間距之和越小，說明路徑軌跡越直，拐彎越少，同時路徑總長度越短，因此描述為距離代價，建立函數(shù)為：

式中： 為距離代價函數(shù)； 為距離代價權(quán)重； 為離散點間距。

對于第二目標(biāo)，主要考慮軌跡離散點的離散曲率和曲率的變化率，其中，離散曲率之和越小，說明路徑的平均離散曲率越??；離散曲率的變化率之和越小，說明曲率越連續(xù)，兩者都會對路徑的平滑性產(chǎn)生影響。

首先考慮離散曲率的計算方式。如圖11所示，對于在圓弧上的三個離散點，假設(shè)離散點間距s近似相等，構(gòu)造Pi+1相對PiPi+2對稱的輔助點P'i+1，則可由△Pi+1OPi+2和△P'i+1Pi+2Pi+1的相似關(guān)系推導(dǎo)得到：

式中： 為Pi處的離散曲率。

在優(yōu)化函數(shù)中為了保持二次形式，將其簡化，得到：

式中： 為離散曲率代價函數(shù)； 為離散點處的曲率： 為曲率的變化率； 和 分別為曲率和曲率變化率對應(yīng)的權(quán)重系數(shù)。

綜合式（9）和式（13），得到：

式中： 為優(yōu)化問題的代價函數(shù)。

綜上，便將路徑平滑問題描述成一個標(biāo)準(zhǔn)的二次規(guī)劃問題，本文將約束條件和優(yōu)化目標(biāo)以稀疏矩陣的形式表達，采用OSQP開源求解器進行求解。

2.3 平滑效果

本文針對圖7（b）II所示的初步搜索路徑，采用不同的權(quán)重系數(shù)進行路徑平滑，對應(yīng)的路徑離散曲率折線圖如圖12所示，平滑效果如圖13所示。

從圖12可以看出，初始搜索路徑中包含大量折線尖點，說明初始路徑的離散曲率突變劇烈；而二次規(guī)劃優(yōu)化后的路徑有效改善了初始搜索路徑的形狀，減少了不必要的拐彎，并盡可能拉直，大大降低了路徑的離散曲率。

此外，對比三種不同權(quán)重的二次規(guī)劃優(yōu)化結(jié)果也可以看出，權(quán)重系數(shù)的取值會影響路徑的特性。利用這一優(yōu)化算法，本文在該地圖上隨機設(shè)定起始位姿和目標(biāo)位姿，選用不同的平滑權(quán)重系數(shù)進行50次重復(fù)實驗。

系數(shù)選取如表1所示，箱型圖結(jié)果如圖14所示。

圖14（a）（b）中，分布越靠下方，說明路徑越平滑；圖14（c）為平滑后路徑與初始搜索路徑的長度差，距離差在大于0的前提下越大，說明平滑后路徑的距離越短，“拉直”效果越好，其表達方式為：

式中： 為平滑后路徑與初始搜索路徑的長度差； 為初始路徑長度（對照組）； 為平滑路徑長度。

通過對比s1、s2兩組權(quán)重的實驗結(jié)果可以看出，Wsmo2的增大可以使路徑更平滑；結(jié)合圖13的實驗結(jié)果分析，Wsmo2的增大還可以使曲率的變化更加連續(xù)。而s3/s4與s1/s2相比的區(qū)別在于引入了Wdis，從圖14可以看出，Wdis的引入會縮短路徑的長度，對路徑的“拉直”效果更好，但隨著Wdis的增加，路徑曲率最大值會明顯增大，這會導(dǎo)致局部曲率偏大。

考慮上述三個系數(shù)對路徑形狀的影響后，本文在該尺度地圖上最終選擇的權(quán)重系數(shù)比值為Wdis：Wsmo1：Wsmo2＝0.04：1：10，選取不同目標(biāo)點進行全局路徑規(guī)劃實驗，最終效果如圖15所示?？梢钥闯?，本文所提出的基于改進HybridA*的非完整約束機器人路徑規(guī)劃方法在各種障礙場景中均能規(guī)劃出平滑且無碰撞的全局路徑。

3 結(jié)論

本文針對傳統(tǒng)HybridA*算法在應(yīng)用于非完整約束機器人全局路徑規(guī)劃時路徑容易貼近障礙物的缺陷，和算法在路徑平滑后可能與障礙物沖突的問題，將全局路徑規(guī)劃分為路徑搜索和路徑平滑兩部分進行算法改進。

其中，在路徑搜索部分，采用HybridA*圖搜索方法，在搜索的啟發(fā)式函數(shù)部分引入距離場地圖，使搜索路徑遠離障礙物并提高搜索效率；在路徑平滑部分，將平滑問題構(gòu)建成二次規(guī)劃問題進行求解，通過約束優(yōu)化后路徑離散點的極限位置以保證路徑的無碰撞性，并基于向前差分的思想引入表征曲率變化率的優(yōu)化代價，進一步提升路徑的平滑程度。

在基于ROS平臺的仿真實驗結(jié)果表明，本文提出的全局路徑規(guī)劃算法可有效應(yīng)用于非完整約束AGV，能夠同時保證避障和路徑平滑性，并基本滿足系統(tǒng)的實時性要求，為AGV自主導(dǎo)航的精準(zhǔn)運動奠定了基礎(chǔ)。

參考文獻：

[1]譚躍剛. 非完整機器人的原理與控制[M]. 北京：科學(xué)出版社，2011.

[2]李柏，張友民，邵之江. 自動駕駛車輛運動規(guī)劃方法綜述[J]. 控制與信息技術(shù)，2018（6）：1-6.

[3]Dolgov D，Thrun S，Montemerlo M，et al. Path Planning for Autonomous Vehicles in Unknown Semi-structuredEnvironments[J]. The International Journal of Robotics Research，2010，29（5）：485-501.

[4]任秉韜，王淅淅，鄧偉文，等. 基于混合A～*和可變半徑RS曲線的自動泊車路徑優(yōu)化方法[J]. 中國公路學(xué)報，2022，35（7）：317-327.

[5]Saeid S，Duong-Van N，Klaus-Dieter K. Guided Hybrid A-Star Path Planning Algorithm for ValetParking Applications[C]. Beijing， China：2019 5th International Conference on Control， Automation and Robotics，2019：570-575.

[6]Tu K.，Yang S.，Zhang H.，et al. Hybrid A* Based Motion Planning for Autonomous Vehicles inUnstructured Environment[C]. Sapporo， Japan：2019 IEEE International Symposium on Circuits and Systems，2019：1-4.

[7]陳鑫鵬，徐彪，胡滿江，等. 一種基于等步長分層拓展的混合A～*路徑規(guī)劃方法[J]. 控制與信息技術(shù)，2021（1）：17-22，29.

[8]齊堯，徐友春，李華，等. 一種基于改進混合A～*的智能車路徑規(guī)劃算法[J]. 軍事交通學(xué)院學(xué)報，2018，20（8）：85-90.

[9]Kurzer K. Path Planning in Unstructured Environments： A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle[D]. Stockholm：KTH Royal Institute of Technology，2016.

[10]Lau B，Sprunk C，Burgard W. Improved updating of Euclidean distance maps and Voronoi diagrams[C]. IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE，2010.

收稿日期：2022-12-15

作者簡介：田雨（1997－），男，天津人，碩士，主要研究方向為機械制造及其自動化，E-mail：tianyu9741@163.com。*通訊作者：林松（1957－），男，四川廣元人，工學(xué)博士（德），主要研究方向為產(chǎn)品研發(fā)方法及其智能設(shè)計、虛擬產(chǎn)品生產(chǎn)及其數(shù)字孿生、智能裝置及其人機協(xié)調(diào)、技術(shù)系統(tǒng)可靠性及其安全設(shè)計，E-mail：slin@#edu.cn。