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

本文參考論文：

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

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

1.1 搜索方式

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

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

三個維度的空間中進行。符合車輛運動學(xué)模型。

1.2 車輛運動學(xué)模型

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

1.3 reeds-shepp曲線的引入

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

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

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

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

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

1.4 G的定義更豐富

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

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

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

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

1.5 H的定義更豐富

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

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

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

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

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

1.6 Voronoi勢場函數(shù)

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

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

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

當然不可能就照著cell的邊界去運動，因為設(shè)計的出發(fā)點是為了通過較窄的地方。在路較寬時，沒有必要，所以我們需要構(gòu)建一個勢場，能夠讓車輛或機器人趨于cell的邊界去運動。勢場函數(shù)如下