全局路徑規(guī)劃，是指在已知的環(huán)境中為機(jī)器人規(guī)劃一條最優(yōu)行駛路徑。本文將對(duì)比經(jīng)典的A*算法，深度探討機(jī)器人常用全局路徑規(guī)劃算法——Hybrid A*算法的原理，包括Hybrid A*算法特性、RS曲線、代價(jià)函數(shù)與啟發(fā)式、節(jié)點(diǎn)拓展、碰撞檢測(cè)，以及局部?jī)?yōu)化與平滑等內(nèi)容。?

01 Hybrid A*算法特性

Hybrid A*算法是一種圖搜索算法，是基于A*算法的一種「變形」。

A*算法采用貪心策略，結(jié)合啟發(fā)式的引導(dǎo)，在靜態(tài)網(wǎng)路中求解最短路徑有著非常不錯(cuò)的效果。但它在規(guī)劃過程中，將機(jī)器人看做一個(gè)質(zhì)點(diǎn)，只考慮（x，y）兩個(gè)維度，未考慮機(jī)器人的實(shí)際運(yùn)動(dòng)約束。

?而Hybrid A*算法增加了θ維度，優(yōu)化了節(jié)點(diǎn)的擴(kuò)展方式，可以更好地滿足車輛的運(yùn)動(dòng)特性，在有障礙物、低速等場(chǎng)景的阿克曼底盤、無人車等存在運(yùn)動(dòng)約束的機(jī)器人中，具有更好的使用表現(xiàn)。至于差速、全向機(jī)器人等不存在運(yùn)動(dòng)約束的機(jī)器人場(chǎng)景中，則不需要用到Hybrid A*算法。

應(yīng)用中，A*算法規(guī)劃的路徑點(diǎn)，都在各個(gè)柵格的中心；Hybrid A*算法的路徑點(diǎn)可以在柵格的任意位置，并且通常使用Reeds-Shepp曲線連接，生成平滑路徑。

相較于Dubins曲線只允許車輛向前運(yùn)動(dòng)，Reeds-Shepp曲線運(yùn)動(dòng)模型既允許車輛向前運(yùn)動(dòng)，也允許車輛向后運(yùn)動(dòng)。

?據(jù)J Reeds和L.A. Shepp證明，Reeds-Shepp Car從起點(diǎn)qI到終點(diǎn)qG的最短路徑，一定在下面的word之中：

且所有word組合不超過48種：

注：word中的"|"表示車輛運(yùn)動(dòng)朝向，由正向轉(zhuǎn)為反向或者由反向轉(zhuǎn)為正向。每個(gè)word都由L+，L-，R+，R-，S+，S-這六種primitives組成。其中，L+表示車輛左轉(zhuǎn)前進(jìn)；L-表示車輛左轉(zhuǎn)后退；R+表示車輛右轉(zhuǎn)前進(jìn)；R-表示車輛右轉(zhuǎn)后退，S+表示車輛直行前進(jìn)；S-表示車輛直行后退。 表示兩個(gè)曲線長(zhǎng)度一樣的曲線部分， 表示曲線部分有90度。

02 代價(jià)函數(shù)與啟發(fā)式

在A*算法中，代價(jià)函數(shù)通常是從起點(diǎn)到當(dāng)前節(jié)點(diǎn)的距離之和。

Hybrid A*算法則結(jié)合實(shí)際情況，對(duì)車輛的角度、擋位、前后向變化等因素添加懲罰，使得規(guī)劃的軌跡更加符合實(shí)際運(yùn)動(dòng)場(chǎng)景。

?另外，啟發(fā)式也是非常重要的一環(huán)，主要采用了兩種啟發(fā)式算法。

第一種啟發(fā)式——無障礙物的非完整約束啟發(fā)式。主要考慮車輛的運(yùn)動(dòng)特性，但是忽略障礙物。

即將車輛的最小半徑作為輸入，并且獲取從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)曲線路徑（如RS曲線、Dubins曲線等），然后計(jì)算最優(yōu)曲線的長(zhǎng)度作為啟發(fā)函數(shù)的代價(jià)值。

第二種啟發(fā)式是第一種啟發(fā)式的對(duì)偶——有障礙物的完整性啟發(fā)式。主要考慮環(huán)境中的障礙物信息，忽略車輛的運(yùn)動(dòng)特性。

通過在每個(gè)節(jié)點(diǎn)使用Djikstra算法，獲得該節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)的最近距離作為其代價(jià)函數(shù)的代價(jià)值。Djikstra算法能夠在類似迷宮環(huán)境中，獲得擴(kuò)展節(jié)點(diǎn)到目標(biāo)點(diǎn)的最近路徑，因此可以避免Hybrid A*算法在擴(kuò)展過程中朝向不正確的問題。

之后比較兩種啟發(fā)式的代價(jià)值大小，選擇其中較大的代價(jià)作為Hybrid A*的代價(jià)值。實(shí)踐證明，這樣可以極大提升規(guī)劃效率和效果。

?注：（a）二維歐氏距離擴(kuò)展21515個(gè)節(jié)點(diǎn)。（b）無障礙物的非完整約束啟發(fā)式是一個(gè)顯著的改進(jìn)，因?yàn)樗鼣U(kuò)展了1465個(gè)節(jié)點(diǎn)，但是如（c）所示，它會(huì)導(dǎo)致在更復(fù)雜的環(huán)境中（68730個(gè)節(jié)點(diǎn)）對(duì)死胡同的浪費(fèi)性探索。結(jié)合使用兩種啟發(fā)式之后，有比較明顯的改善（10588節(jié)點(diǎn)）如（d）所示。

由于這些啟發(fā)式不依賴Hybrid A*的任何特性，也同樣適用于A*算法等其他搜索方法。

?

03 節(jié)點(diǎn)擴(kuò)展

A*算法的擴(kuò)展節(jié)點(diǎn)，是在四個(gè)或者八個(gè)方向相鄰離散柵格的中心點(diǎn)，因此無法表達(dá)在連續(xù)空間中車輛的朝向和位置。

Hybrid A*算法則是通過考慮車輛運(yùn)動(dòng)特性生成的軌跡替代A*算法中的節(jié)點(diǎn)。同時(shí)在擴(kuò)展節(jié)點(diǎn)時(shí)，會(huì)充分考慮車輛的運(yùn)動(dòng)特性，所以可以生成更加符合車輛運(yùn)動(dòng)特性的軌跡。

Hybrid A*算法節(jié)點(diǎn)擴(kuò)展的通常做法是：從當(dāng)前節(jié)點(diǎn)出發(fā)，根據(jù)預(yù)先設(shè)定的運(yùn)動(dòng)步長(zhǎng)，同時(shí)考慮最大轉(zhuǎn)彎半徑，從前向和后向采樣車輛運(yùn)動(dòng)軌跡。對(duì)采樣的運(yùn)動(dòng)軌跡做碰撞檢測(cè)，滿足車輛運(yùn)動(dòng)條件的運(yùn)動(dòng)軌跡即可作為待選節(jié)點(diǎn)。

另外考慮到運(yùn)算量，普通節(jié)點(diǎn)間的擴(kuò)展通常不使用RS曲線，只有在接近目標(biāo)節(jié)點(diǎn)時(shí)才會(huì)使用RS曲線生成目標(biāo)軌跡。

04 碰撞檢測(cè)

在全局路徑規(guī)劃過程中，為了避免機(jī)器人與障礙物發(fā)生碰撞，需要將規(guī)劃的路徑與障礙物保持安全距離。

A*算法的處理方式是，通過檢查路徑經(jīng)過的柵格占據(jù)狀態(tài)來判斷碰撞。

而Hybrid A*算法是在機(jī)器人的路徑采樣過程中，在路徑的每個(gè)采樣點(diǎn)上結(jié)合機(jī)器人的外形和位姿，來判斷機(jī)器人各時(shí)刻的輪廓范圍內(nèi)是否存在障礙物。如果存在障礙物，就放棄這條采樣路徑。

這種碰撞檢測(cè)方法，可以很好地結(jié)合機(jī)器人實(shí)際尺寸，和各采樣時(shí)刻的姿態(tài)準(zhǔn)確判斷碰撞關(guān)系，但同時(shí)也大大增加了算法的復(fù)雜度。

05 局部?jī)?yōu)化和平滑

由于Hybrid A*算法產(chǎn)生的路徑往往不是最優(yōu)解，通常存在不自然轉(zhuǎn)向以及不必要轉(zhuǎn)向，需要進(jìn)一步改進(jìn)優(yōu)化。

第一步，先對(duì)路徑點(diǎn)進(jìn)行非線性優(yōu)化。

我們可以通過局部平滑的目標(biāo)函數(shù)，得到更加平滑的路徑：

注：?為Voronoi場(chǎng)，可以有效地引導(dǎo)機(jī)器人遠(yuǎn)離窄通道和寬通道中的障礙物； 是路徑的最大允許曲率，即與障礙物的碰撞懲罰； ?和 是懲罰函數(shù)，即每個(gè)節(jié)點(diǎn)上軌跡的瞬時(shí)曲率，并加強(qiáng)車輛的非完整約束； ?,? , , ?是權(quán)重，對(duì)路徑平滑性的度量。

其中，Voronoi場(chǎng)用來獲取與障礙物之間的關(guān)系，定義如下：

注： 是節(jié)點(diǎn)到達(dá)最近障礙物的距離； 是節(jié)點(diǎn)到達(dá)維諾地圖邊的最近距離； ?表示勢(shì)能下降速率，為常數(shù)，當(dāng) ? 值越大，(x,y)處的勢(shì)能下降得越慢；?表示勢(shì)能的控制范圍，為常數(shù)。其中，節(jié)點(diǎn)(x,y)處的勢(shì)能，取值從0到1，當(dāng) 時(shí)，(x,y)勢(shì)能為0，勢(shì)能值到達(dá)最大的時(shí)候是(x,y)在障礙物上或者里面，勢(shì)能值到達(dá)最小的時(shí)候是(x,y)在廣義泰森多邊形的邊上。

過程中，我們將Hybrid A*產(chǎn)生的軌跡用紅線表示，對(duì)結(jié)點(diǎn)優(yōu)化后的軌跡標(biāo)注為藍(lán)線。

?可以發(fā)現(xiàn)，優(yōu)化后得到的路徑比Hybrid A*得到的解更平滑，不過仍然是分段線性的。

?第二步，采用共軛梯度方法進(jìn)行非參數(shù)化插值，然后就得到了一條連貫且平滑的路徑。

?以上就是我們針對(duì)Hybrid A*算法原理和整體流程的分享，歡迎大家交流、斧正。文章來源地址http://www.zghlxwxcb.cn/news/detail-650820.html

到了這里，關(guān)于深度科普：機(jī)器人都在用的Hybrid A*算法，你知道多少？的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！