自動(dòng)駕駛軌跡規劃之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ì)上就是把車(chē)輛簡(jiǎn)化為質(zhì)點(diǎn)，并且移動(dòng)方向是固定的八個(gè)方向（或四個(gè)方向），移動(dòng)距離也是確定的。但這不符合實(shí)際的車(chē)輛運動(dòng)學(xué)模型。

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

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

1.2 車(chē)輛運動(dòng)學(xué)模型

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

1.3 reeds-shepp曲線(xiàn)的引入

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

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

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

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

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

1.4 G的定義更豐富

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

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

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

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

1.5 H的定義更豐富

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

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

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

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

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

1.6 Voronoi勢場(chǎng)函數

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

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

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

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