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

本文參考論文：

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

1 hybrid A* 算法創新點

1.1 搜索方式

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

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

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

1.2 車輛運動學模型

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

1.3 reeds-shepp曲線的引入

但是reeds-shepp曲線完全未考慮避障因素，但是由于reeds-shepp曲線比較簡單易計算，所以構造速度非常快，使得先構造再檢驗是否碰撞成為可能，這里的碰撞指的是整條軌跡是否會與障礙物有交集。

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

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

圖b：但間歇性得用reeds-shepp和傳統A*去搜索，在每N個節點中選取一個計算Reed-Shepp曲線（這里的N隨啟發函數遞減而減少，即越發靠近終點時，N越小）。由下圖b可見搜索樹規模小，節點少。

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

1.4 G的定義更豐富

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

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

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

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

1.5 H的定義更豐富

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

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

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

H2：當前節點離終點較遠時，更應該關注避障行駛，防止陷入死胡同，利用傳統A*的H進行計算。如下圖

可見這樣對啟發函數的設計有利于提高搜索速度。

1.6 Voronoi勢場函數

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

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

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

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