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

田雨　林松　房殿軍　江競(jìng)宇

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

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

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

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

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

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

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

1 路徑搜索

1.1? HybridA*算法原理

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

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

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

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

核心原理為：

（1）不斷尋找f（n）值最小的無(wú)碰撞節(jié)點(diǎn)進(jìn)行擴(kuò)展。如圖2（a）所示，在初始節(jié)點(diǎn)0的周?chē)瑑?yōu)先擴(kuò)展無(wú)碰撞節(jié)點(diǎn)1；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 路徑平滑

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

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

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

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

2.1 約束條件

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

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

有位置約束：

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

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

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

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

對(duì)于第一目標(biāo)，一般來(lái)說(shuō)，離散點(diǎn)間距之和越小，說(shuō)明路徑軌跡越直，拐彎越少，同時(shí)路徑總長(zhǎng)度越短，因此描述為距離代價(jià)，建立函數(shù)為：

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

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

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

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

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

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

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

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

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

2.3 平滑效果

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

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

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

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

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

式中： 為平滑后路徑與初始搜索路徑的長(zhǎng)度差； 為初始路徑長(zhǎng)度（對(duì)照組）； 為平滑路徑長(zhǎng)度。

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

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

3 結(jié)論

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

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

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

參考文獻(xiàn)：

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

[2]李柏，張友民，邵之江. 自動(dòng)駕駛車(chē)輛運(yùn)動(dòng)規(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曲線的自動(dòng)泊車(chē)路徑優(yōu)化方法[J]. 中國(guó)公路學(xué)報(bào)，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]陳鑫鵬，徐彪，胡滿江，等. 一種基于等步長(zhǎng)分層拓展的混合A～*路徑規(guī)劃方法[J]. 控制與信息技術(shù)，2021（1）：17-22，29.

[8]齊堯，徐友春，李華，等. 一種基于改進(jìn)混合A～*的智能車(chē)路徑規(guī)劃算法[J]. 軍事交通學(xué)院學(xué)報(bào)，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

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