本文目的

由于在carla-autoware的示例中使用了hybrid A* 算法，所以本文基于以下兩篇文章對hybrid A* 算法過程進行整理：（文中挑選了一些個人認為便于理解算法的圖片，均來自于以下這兩篇文章）

《Practical Search Techniques in Path Planning for Autonomous Driving》

《Path Planning in Unstructured Environments: A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle》

第二篇論文ROS實現的github鏈接：https://github.com/karlkurzer/path_planner

其實我覺得對這兩篇文章還有一些理解不到位的地方，望指正！下周看完autoware對應的源碼之后再來更新這篇。

一、算法簡介

解決的問題

該算法解決的問題是：假設無人車有充分的感知和定位能力，則其能夠在線重新規劃，生成障礙物地圖，并且能夠在未知環境中行駛。

其他算法中常見問題

開發實際無人駕駛的路徑規劃器存在的問題：

機器人的控制空間、軌跡空間都是連續的，而現存算法基本都是適用于離散情況的，所以生成的路徑是不光滑的。

不滿足機器人的非完整性約束

算法的大致過程

在連續坐標系下進行啟發式搜索（改進了A*算法）

第二個階段用數值非線性優化的方法（即共軛梯度下降法）對生成路徑的“質量”進行提升，使其至少是局部最優的。（下文對這兩個過程進行詳細說明）

二、STAGE 1: 啟發式搜索+損失函數設計

第一個階段其實是對傳統的A* 算法進行改進，與之不同的是，hybrid A* 是在連續坐標系下進行啟發式搜索，并且能夠保證生成的軌跡滿足車輛非完整性約束（下文對車輛的非完整性約束進行補充），但算法運行過程中該路徑并不一定是全局最優的，盡管如此，這條路徑是在全局最優解的“附近”（這里的全局最優解指的是由傳統A* 算法生成的）。下圖（來自文獻1）可以很好地解釋傳統A* 與hybrid A* 的異同。兩種算法都是基于網格世界的（grid world），A* 是賦予每個網格的中心點相應的損失并且算法只訪問這些中心點，而hybrid A* 是先在這些網格中挑選滿足車輛3D連續狀態的點，并將損失賦值給這些點。（下圖中間的圖是用其他A* 變體算法實現的）

1. 搜索中所用的兩種啟發函數

在傳統的A* 算法中，由于啟發函數的選取不同，運行算法后節點擴張（expansion）的效率也就不同（目的是希望算法在遍歷最少的節點的情況下找到最優路徑）。傳統的A* 算法的啟發函數一般是2D歐幾里得距離，而hybrid A* 算法構造了兩個啟發函數。

第一個啟發函數是Constrained heuristics ，只考慮車輛的非完整性約束而不考慮障礙物（優點是相比直接用歐幾里得距離損失要好一個數量級）。該啟發函數忽略了環境中的障礙物等信息，只考慮車輛的運動學特性，從終止點開始，計算從該點到其他點的最短路徑。具體的返回值就是：

2. 第二個啟發函數是第一個的對偶，稱為Unconstrained heuristics，只考慮障礙物信息而不考慮車輛的非完整性約束條件（優點是引入該啟發函數后能夠發現2D空間中所有的U形障礙物和死胡同dead end）。隨后使用2D動態規劃的方法（其實就是傳統的2D A* 算法）計算每個節點到終點的最短路徑。

算法使用的損失函數就是兩種啟發函數的最大值。下圖很好地解釋了兩種啟發函數：圖a,c是第一種啟發函數下生成的路徑，可以看出是連續的；而圖b,d則是在第二種啟發函數下生成的離散路徑（與傳統A* 算法得到的結果是類似的）

補充：車輛的微分/運動學約束——非完整性約束
車輛基本的構型空間是：

，車輛的速度是。但在實際行駛中，車輛不能直接向左向右平移，也就是說垂直于車輛heading方向的速度為0，將下圖中的分解到坐標下可以得到：

兩者聯立可以得到：可以得到車輛的非完整性約束條件為：

2. 路徑節點擴張的過程

論文中稱節點擴張的過程為Analytic Expansions（解析擴張？）。首先狀態的表示為：

，其中是節點的位置，而是節點的朝向（heading），在前文所述的搜索過程中，使用到的是離散的控制動作（control actions, or steering），因此之前每個網格中的連續狀態點是不可達的。為了進一步改進搜索速度和提高準確度，這就可以利用Reed-Shepp曲線。A* 的搜索過程都是用直線相連接，而hybrid A* 則是在與網格精度一致的前提下（對應某一小段時間）使用三種控制動作：最大左轉，最大右轉，不轉向來生成路徑，因此該路徑是一些受車輛轉彎半徑約束的圓弧和直線。

以上過程會生成一些子節點，在此基礎上，假設不考慮環境（對應第一個啟發函數），算法會通過計算從起點到終點的最優Reed-Shepp曲線的方式，再生成一個額外的子節點；之后算法基于現有的障礙物地圖對該路徑進行碰撞檢測，無碰撞路徑對應的點會加到擴張樹中。可以看出與直線相比，Reed-Shepp曲線的計算量是很大的。所以論文中作者使用簡單的selection rule，在每N個節點中選取一個計算Reed-Shepp曲線（這里的N隨啟發函數遞減而減少，即越發靠近終點時，N越小）。下面這兩張圖也很好的解釋了擴張過程。

3. 路徑損失函數的設計

損失函數的設計困難在于所生成路徑的要求太多：首先路徑長度或損失應該是接近最優的；路徑必須是光滑的；生成的路徑必須與障礙物保持一定的距離（傳統的人工勢場法的缺點是在狹窄路段構造了高勢場，使得機器人或車輛無法通過）

The key advantage of the Voronoi field over a conventional potential fields is the fact that the field value is scaled in proportion to the total available clearance for navigation. As a result, even narrow openings remain navigable, which is not always the case for standard potential fields.

解決方法：構造Voronoi勢場函數（具體公式是下列三項連乘，voronoi場的物理意義我還沒有搞清楚，之后查下相關資料），voronoi場的值隨著導航中所有可行空間的大小成比例縮放。公式中

分別代表路徑節點到最近的障礙物和最近的GVD（廣義voronoi圖）的長度， 控制場的衰減率（Falloff Rate）

下圖很好地闡釋了voronoi場的效果，使用voronoi場之后車輛也能很好地通過狹窄路段。

voronoi場有如下的一些特點：

時，場的值為0

該場函數的區間時[0, 1]，且連續，不能同時為0。

在障礙物附近Voronoi場的值達到最大值

Voronoi場的值在GVD的邊上達到最小值

三、STAGE2: 對路徑進行后處理

1. 進行后處理的目的

在上一步子節點擴張的過程中，路徑會有一些額外的不必要控制動作（即steering），所以算法的第二個部分就是對生成點曲線進行平滑處理。

2. 后處理所用目標函數的組成

目標函數由以下四部分組成。

障礙物項：懲罰與障礙物的碰撞。公式中是路徑節點的坐標，是最近障礙物的坐標，是最大障礙物的距離（類似閾值）。選擇二次函數作為懲罰項的目的是放大障礙物與節點越來越靠近的效果。從公式可以看出，節點靠近障礙物的時候，第一項 的值會減小，因此該差值的二次方變大。

2. 曲率項

該項相當于對路徑的每個節點的瞬時曲率設置一個上限，這里，，并且，懲罰函數是二次函數，即。

3. 光滑度項smoothness——

光滑度項計算每個節點之間位移向量的差值的平方。這一項將損失值賦給非均勻分布和方向變化的節點，以保證路徑的平滑性。

4. Voronoi Term——

這一項基于之前定義的voronoi場函數，使路徑遠離障礙物， （向量x就是路徑點的坐標x, y）

優化階段分為兩個階段： a. 對路徑的頂點坐標進行非線性優化規劃問題的建模（也就是上文的損失函數定義） b. 采用共軛梯度方法進行非參數化插值（non-parametrc interpolation） （這里具體的共軛梯度方法，學習了之后再更新）

四、hybrid A* 算法偽代碼及實現流程

1. hybrid A* 算法偽代碼： （這一部分之后看完autoware源碼之后再詳細地對比、解釋）

與A*算法類似，算法也是維護兩個列表，一個open list, 一個是closed list。算法的結束條件是：open list為空或者已經搜索到終點。Line 21: 算法不一定會準確搜索到終點，因此引入RoundState函數，在判斷當前節點是否到達終點之前對此進行估算。如果沒有達到終點，算法會通過執行動作空間中的所有動作對路徑節點進行擴張。如果生成的節點不在C中（也就是沒有被算法遍歷過），則計算cost-so-far；如果生成的節點不在O中（已被遍歷過）或者所得到的cost-so-far小于當前節點已有的cost，這時用當前得到的較小的cost更新cost-so-far

2. 算法實現的流程： 這里是第二篇文獻中的實現流程，算法的輸入為事先定義好的障礙物地圖，經過以上的兩步：hybrid A* 搜索和路徑平滑之后在rviz中顯示路徑。

總結

以上是生活随笔為你收集整理的为什么a*算法采用哈密尔顿距离作为启发函数比不在位数为启发函数的性能要好?_【论文研读】路径规划中的Hybrid A*算法...的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。