自動(dòng)駕駛軌跡規(guī)劃之hybrid A*算法（1）

本文參考論文：

https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf

1 hybrid A* 算法創(chuàng)新點(diǎn)

1.1 搜索方式

A*算法：在二維網(wǎng)格中進(jìn)行搜索，本質(zhì)上就是把車輛簡(jiǎn)化為質(zhì)點(diǎn)，并且移動(dòng)方向是固定的八個(gè)方向（或四個(gè)方向），移動(dòng)距離也是確定的。但這不符合實(shí)際的車輛運(yùn)動(dòng)學(xué)模型。

hybrid A*算法：引入航向角，將搜索變成在 

三個(gè)維度的空間中進(jìn)行。符合車輛運(yùn)動(dòng)學(xué)模型。

1.2 車輛運(yùn)動(dòng)學(xué)模型

為了便于計(jì)算，hybrid A*采用車輛二自由度運(yùn)動(dòng)學(xué)模型（見(jiàn)上圖），但是忽略了車輛加速度與前輪轉(zhuǎn)角速度，于是經(jīng)過(guò)簡(jiǎn)化的運(yùn)動(dòng)學(xué)模型如下

1.3 reeds-shepp曲線的引入

但是reeds-shepp曲線完全未考慮避障因素，但是由于reeds-shepp曲線比較簡(jiǎn)單易計(jì)算，所以構(gòu)造速度非?？欤沟孟葮?gòu)造再檢驗(yàn)是否碰撞成為可能，這里的碰撞指的是整條軌跡是否會(huì)與障礙物有交集。

因此不像傳統(tǒng)A*不考慮中間過(guò)程，所以需要均勻采樣整段時(shí)間，進(jìn)行碰撞檢驗(yàn)。假如該段軌跡無(wú)碰撞則加入搜索樹中，作為候選軌跡，如下圖

圖a：假如每次搜索都用reeds-shepp，由下圖a可見(jiàn)這個(gè)搜索軌跡構(gòu)成的樹規(guī)模很龐大，節(jié)點(diǎn)很多將會(huì)造成極大的計(jì)算量。

圖b：但間歇性得用reeds-shepp和傳統(tǒng)A*去搜索，在每N個(gè)節(jié)點(diǎn)中選取一個(gè)計(jì)算Reed-Shepp曲線（這里的N隨啟發(fā)函數(shù)遞減而減少，即越發(fā)靠近終點(diǎn)時(shí)，N越?。Ｓ上聢Db可見(jiàn)搜索樹規(guī)模小，節(jié)點(diǎn)少。

但是在利用reeds-shepp曲線搜索時(shí)，若出現(xiàn)48種reeds-shepp曲線都會(huì)碰撞，也需要重新進(jìn)行經(jīng)典的A*搜索，這種混合的搜索使得搜索速度提升。

1.4 G的定義更豐富

在傳統(tǒng)A*算法中，G是從起點(diǎn)到當(dāng)前節(jié)點(diǎn)的路徑消耗，由于一段直線前進(jìn)的軌跡肯定優(yōu)于反復(fù)前進(jìn)倒退或扭曲的軌跡，因此我們對(duì)頻繁切換速度  和前輪轉(zhuǎn)向角  兩個(gè)控制量的值這種行為進(jìn)行懲罰，這樣就能使得最后的軌跡更加合理。

還有很多為了軌跡合理可以懲罰的地方，這其實(shí)就是一個(gè)評(píng)價(jià)函數(shù)的設(shè)計(jì)，具體可以參考；

https://blog.csdn.net/weixin_65089713/article/details/123835309

但需要說(shuō)明的是，如果是極端狹窄的泊車場(chǎng)景中，我們不得不采用復(fù)雜扭曲的軌跡。如下圖

1.5 H的定義更豐富

按理來(lái)說(shuō)，H函數(shù)應(yīng)該是從當(dāng)前節(jié)點(diǎn)位姿到終點(diǎn)節(jié)點(diǎn)位姿，同時(shí)滿足避障以及車輛運(yùn)動(dòng)學(xué)約束的最短路徑長(zhǎng)度，這是真實(shí)路徑，但這很難在還沒(méi)采樣搜索剩下的位置環(huán)境時(shí)就知道真實(shí)路徑。

所以hybrid A* 設(shè)計(jì)兩個(gè)H的子函數(shù)，H1代表符合車輛運(yùn)動(dòng)學(xué)約束但忽略碰撞因素的最短路徑，H2代表滿足避障約束但是忽略車輛運(yùn)動(dòng)學(xué)約束的最短路徑，H定義為H1與H2的最大值。

H1：當(dāng)前節(jié)點(diǎn)離終點(diǎn)較近時(shí)，更應(yīng)該關(guān)注車輛運(yùn)動(dòng)學(xué)約束，忽略障礙物的情況下，路徑不依賴于任何在線場(chǎng)景信息，可以通過(guò)離線的方式采樣枚舉所有reeds-shepp曲線可能，提前將路徑長(zhǎng)度記錄出來(lái)，在線調(diào)用該函數(shù)時(shí)只需索引、插值即可返回函數(shù)值。同時(shí)，這項(xiàng)啟發(fā)函數(shù)的主要目的是為修剪傳統(tǒng)A*搜索樹的分支，保證最后能精準(zhǔn)銜接終止位姿。如下圖

H2：當(dāng)前節(jié)點(diǎn)離終點(diǎn)較遠(yuǎn)時(shí)，更應(yīng)該關(guān)注避障行駛，防止陷入死胡同，利用傳統(tǒng)A*的H進(jìn)行計(jì)算。如下圖

可見(jiàn)這樣對(duì)啟發(fā)函數(shù)的設(shè)計(jì)有利于提高搜索速度。

1.6 Voronoi勢(shì)場(chǎng)函數(shù)

生成的路徑必須與障礙物保持一定的距離，這也是最優(yōu)軌跡的要求，由于傳統(tǒng)的人工勢(shì)場(chǎng)法的缺點(diǎn)是在狹窄路段構(gòu)造了高勢(shì)場(chǎng)，使得機(jī)器人或車輛無(wú)法通過(guò)，因此，構(gòu)造Voronoi勢(shì)場(chǎng)函數(shù)。首先，介紹一下Voronoi圖。

Voronoi圖，它是由一組由連接兩鄰點(diǎn)直線的垂直平分線組成的連續(xù)多邊形組成。每個(gè)點(diǎn)有一個(gè)它的最近鄰區(qū)域。

每個(gè)Cell中包含的都是距離當(dāng)前Cell距離最近的所有點(diǎn)，因此Cell的邊界就是距離種子點(diǎn)最遠(yuǎn)的點(diǎn)的集合。利用這個(gè)特性，將采樣障礙物的邊界當(dāng)做種子點(diǎn)，那么Cell的邊界就是遠(yuǎn)離所有障礙物的可行駛路徑。效果見(jiàn)下圖

當(dāng)然不可能就照著cell的邊界去運(yùn)動(dòng)，因?yàn)樵O(shè)計(jì)的出發(fā)點(diǎn)是為了通過(guò)較窄的地方。在路較寬時(shí)，沒(méi)有必要，所以我們需要構(gòu)建一個(gè)勢(shì)場(chǎng)，能夠讓車輛或機(jī)器人趨于cell的邊界去運(yùn)動(dòng)。勢(shì)場(chǎng)函數(shù)如下