本文經(jīng)自動駕駛之心公眾號授權(quán)轉(zhuǎn)載，轉(zhuǎn)載請聯(lián)系出處。

背景：

16-18年做過一陣子無人駕駛，那時(shí)候癡迷于移動規(guī)劃；然而當(dāng)時(shí)可學(xué)習(xí)的資料非常少，網(wǎng)上的論文也不算太多。基本就是Darpa的幾十篇無人越野幾次比賽的文章，基本沒有成系統(tǒng)的文章和代碼講解實(shí)現(xiàn)。所以對移動規(guī)劃的認(rèn)識不算全面，這幾年隨著自動駕駛、無人機(jī)的研究和應(yīng)用的增多，很多的論文課程成體系的開始介紹這方面的內(nèi)容。對于一個理工男來說機(jī)器人并且是能自動的、智能規(guī)劃的，相信沒有多少理工男是可以抗拒不想去做進(jìn)一步了解的。所以一直在收集資料，籌劃這哪一天可以出一個這方面系列，然后在code一個項(xiàng)目出來在機(jī)器人上搗騰各種實(shí)現(xiàn)。再一次加速本人對這一想法落實(shí)是兩年前看到fast-lab高飛團(tuán)隊(duì)出的一系列飛行走廊解決無人機(jī)路徑規(guī)劃的工作視頻。第一次看到視頻時(shí)候真被震驚到，移動規(guī)劃原來還可以這么玩，如此優(yōu)美的數(shù)學(xué)框架。講了這么多只是想致敬過去的經(jīng)歷，開啟這個專題第一講。這個系列主線就是圍繞高飛老師《移動機(jī)器人動態(tài)規(guī)劃》課程講稿，里面會補(bǔ)充一些算法細(xì)節(jié)和自己的思考。這個課程對移動規(guī)劃體系框架構(gòu)建非常棒，內(nèi)容排布的也非常好，唯一缺憾就是對于動態(tài)不確定障礙物的規(guī)劃會少一些，因?yàn)檎n程本來就是針對無人機(jī)設(shè)計(jì)的。

正文：

現(xiàn)代機(jī)器人學(xué)和自動駕駛等領(lǐng)域，路徑規(guī)劃是一個重要的主題. 它涉及到在給定的環(huán)境中找到從起點(diǎn)到終點(diǎn)的最優(yōu)路徑. 這個過程通常分為兩個部分：前端路徑搜索和后端軌跡規(guī)劃. 前端路徑搜索在地圖中搜索出一條避開障礙物的軌跡，而后端軌跡規(guī)劃則對搜索到的軌跡進(jìn)行優(yōu)化，使其符合機(jī)器人的運(yùn)動學(xué)和動力學(xué)約束.
實(shí)環(huán)境中的機(jī)器人運(yùn)動規(guī)劃是一個比較復(fù)雜的問題，對于復(fù)雜的問題人類的解法一般都是分步求解：先做個大概、然后在大概輪廓上逐步的復(fù)雜精細(xì)。機(jī)器人運(yùn)動規(guī)劃的學(xué)院派解法也是如此：

1.前端：路徑規(guī)劃

• 基于搜索的方法
• 通用圖搜索：深度優(yōu)先搜索（DFS），廣度優(yōu)先搜索（BFS）
• Dijkstra 和 A* 搜索
• 跳點(diǎn)搜索
• 基于采樣的方法
• 概率路線圖（PRM）
• 快速探索隨機(jī)樹（RRT）
• RRT，有信息的 RRT
• 帶動力學(xué)約束路徑規(guī)劃
• 狀態(tài)-狀態(tài)邊界值最優(yōu)控制問題
• 狀態(tài)柵格搜索
• 動力學(xué)RRT*
• 混合A*

2.后端：軌跡生成

• 最小抖動軌跡生成
• 微分平坦性
• 最小抖動優(yōu)化
• 最小抖動的閉式解
• 時(shí)間分配
• 實(shí)際應(yīng)用
• 軟硬約束軌跡優(yōu)化
• 軟約束軌跡優(yōu)化
• 硬約束軌跡優(yōu)化

3.不確定性狀態(tài)求解：移動障礙物、突變環(huán)境、設(shè)備建模變化

• 基于馬爾可夫決策過程的規(guī)劃（MDP）
• 規(guī)劃中的不確定性和馬爾可夫決策過程
• 最小最大成本規(guī)劃和期望成本最小規(guī)劃
• 值迭代和實(shí)時(shí)動態(tài)規(guī)劃
• 機(jī)器人規(guī)劃的模型預(yù)測控制（MPC）
• 線性模型預(yù)測控制
• 非線性模型預(yù)測控制

前端——搜索路徑規(guī)劃

在開始這部分內(nèi)容介紹前，需要介紹幾個概念。介紹這幾個概念的目的在于更貼近實(shí)際的去理解搜索在業(yè)務(wù)中應(yīng)用。搜索路徑規(guī)劃中是把機(jī)器人當(dāng)成一個質(zhì)點(diǎn)來考慮的，然而實(shí)際的機(jī)器人是有一定形狀和占用空間的，如果把機(jī)器人當(dāng)成質(zhì)點(diǎn)來考慮很可能是會搜索出一條實(shí)際上不可行的（會碰到障礙物的）路徑的。為了解決這個問題呢，我們可以簡單的物體的形狀轉(zhuǎn)移到地圖（讓地圖障礙物區(qū)域加上物體占用空間）。在這樣的地圖里把機(jī)器人當(dāng)成質(zhì)點(diǎn)來搜索可行路徑。

在配置空間中規(guī)劃123

• 機(jī)器人在C-space中被表示為一個點(diǎn)，例如，位置（在R3中的一個點(diǎn)），姿態(tài)（在 (3)中的一個點(diǎn)），等等???
• 障礙物需要在配置空間中表示（在運(yùn)動規(guī)劃之前的一次性工作），稱為配置空間障礙物，或C-障礙???
• C-space = (C-障礙) ∪ (C-自由)???
• 路徑規(guī)劃是在C-自由中找到從起點(diǎn)qstart到目標(biāo)點(diǎn)qgoal的路徑?[10]1?

在工作空間中

• 機(jī)器人有形狀和大?。矗y以進(jìn)行運(yùn)動規(guī)劃）
• 在配置空間：C-space中
• 機(jī)器人是一個點(diǎn)（即，易于進(jìn)行運(yùn)動規(guī)劃）?
• 在進(jìn)行運(yùn)動規(guī)劃之前，障礙物在C-space中表示?[10]
• 在C-space中表示障礙物可能非常復(fù)雜。因此，在實(shí)踐中使用近似（但更保守）的表示。

如果我們保守地將機(jī)器人建模為半徑為 _ 的球，那么可以通過在所有方向上膨脹障礙物 _ 來構(gòu)造C-space1。這是一種常見的機(jī)器人碰撞檢測方法，通過確保球體中心在膨脹地圖的自由空間中來實(shí)現(xiàn)碰撞評估1。然而，這種保守的方法并未考慮到機(jī)器人的形狀和大小。

構(gòu)建地圖：

在路徑規(guī)劃中，構(gòu)建搜索地圖是一個關(guān)鍵步驟。這通常涉及到將實(shí)際環(huán)境抽象為一個圖（Graph），其中節(jié)點(diǎn)（Nodes）代表可能的位置，邊（Edges）代表從一個位置到另一個位置的移動。以下是一個詳細(xì)的例子：
假設(shè)我們有一個機(jī)器人需要在一個室內(nèi)環(huán)境中導(dǎo)航。這個環(huán)境可以是一個房間，有一些障礙物，比如桌子和椅子。

1. 定義節(jié)點(diǎn)（Nodes）：首先，我們需要確定節(jié)點(diǎn)的位置。在這個例子中，我們可以將房間的每一個可達(dá)的位置定義為一個節(jié)點(diǎn)。例如，我們可以創(chuàng)建一個網(wǎng)格（Grid），每一個網(wǎng)格單元都是一個節(jié)點(diǎn)。
2. 定義邊（Edges）：然后，我們需要確定邊。如果機(jī)器人可以直接從一個節(jié)點(diǎn)移動到另一個節(jié)點(diǎn)，那么這兩個節(jié)點(diǎn)之間就有一條邊。在我們的例子中，如果兩個網(wǎng)格單元相鄰，并且沒有障礙物阻擋，那么這兩個網(wǎng)格單元（即節(jié)點(diǎn)）之間就有一條邊。
3. 定義權(quán)重（Weights）：最后，我們需要為每一條邊定義一個權(quán)重。權(quán)重可以根據(jù)實(shí)際的移動成本來確定。例如，如果從一個節(jié)點(diǎn)到另一個節(jié)點(diǎn)的距離更遠(yuǎn)，或者路徑上有斜坡，那么這條邊的權(quán)重就應(yīng)該更大。

地圖種類：

柵格地圖（Grid Map）則是把環(huán)境劃分成一系列柵格，在數(shù)學(xué)視角下是由邊聯(lián)結(jié)起來的結(jié)點(diǎn)的集合，一個基于圖塊拼接的地圖可以看成是一個柵格圖，每個圖塊(tile)是一個結(jié)點(diǎn)，圖塊之間的連接關(guān)系如短線。
概率圖（Cost Map）如果在柵格圖的基礎(chǔ)上，每一柵格給定一個可能值，表示該柵格被占據(jù)的概率，則該圖為概率圖。
特征地圖（Feature Map）特征地圖用有關(guān)的幾何特征（如點(diǎn)、直線、面）表示環(huán)境。常見于vSLAM（視覺SLAM）技術(shù)中。它一般通過如GPS、UWB以及攝像頭配合稀疏方式的vSLAM算法產(chǎn)生，優(yōu)點(diǎn)是相對數(shù)據(jù)存儲量和運(yùn)算量比較小，多見于最早的SLAM算法中。
拓?fù)涞貓D（Topological Map）是指地圖學(xué)中一種統(tǒng)計(jì)地圖, 一種保持點(diǎn)與線相對位置關(guān)系正確而不一定保持圖形形狀與面積、距離、方向正確的抽象地圖。包括有有向圖和無向圖（字面意思）。

柵格地圖

概率圖

特征地圖

拓?fù)涞貓D-有向圖

拓?fù)涞貓D-無向圖

搜索算法介紹

有了這么多種的地圖，那么對應(yīng)每種圖可以用什么對應(yīng)的算法來做路徑的規(guī)劃呢？下面是地圖對應(yīng)路徑搜索算法：

1. 柵格地圖 / 概率圖
             1. Dijkstra
             2. BFS（Best-First-Search）
             3. A*
             4. hybrid A*
             5. D  *
             6. RRT
             7. RRT*
             8. 蟻群算法
             9. Rectangular Symmetry Reduction (RSR) 
             10. BUG
             11. Beam search
             12. Iterative Deepeningc
             13. Dynamic weighting
             14. Bidirectional search
             15. Dynamic A* and Lifelong Planning A *
             16. Jump Point Search
             17. Theta *
     2. 拓?fù)涞貓D
              1. Dijkstra
              2. BFS（Best-First-Search）
              3.  A*
              4. CH
              5. HH
              6. CRP

圖搜索算法結(jié)構(gòu)

:::success

• 維護(hù)一個容器來存儲所有待訪問的節(jié)點(diǎn)
• 該容器以起始狀態(tài)XS進(jìn)行初始化
• 循環(huán)
• 根據(jù)某個預(yù)定義的評分函數(shù)從容器中移除一個節(jié)點(diǎn)
• 訪問一個節(jié)點(diǎn)
• 擴(kuò)展：獲取該節(jié)點(diǎn)的所有鄰居
• 發(fā)現(xiàn)所有的鄰居
• 將它們（鄰居）推入容器
• 擴(kuò)展：獲取該節(jié)點(diǎn)的所有鄰居
• 結(jié)束循環(huán) :::

通用搜索算法結(jié)構(gòu)

常用的圖搜索有3大類的搜索結(jié)構(gòu)，其它算法都是在這三個大的框架之下做改進(jìn)。
深度優(yōu)先搜索（Depth-First Search, DFS）：

• 原理：DFS是一種用于遍歷或搜索樹或圖的算法。這個算法會盡可能深地搜索樹的分支。當(dāng)節(jié)點(diǎn)v的所在邊都己被探尋過，搜索將回溯到發(fā)現(xiàn)節(jié)點(diǎn)v的那條邊的起始節(jié)點(diǎn)。這一過程一直進(jìn)行到已發(fā)現(xiàn)從源節(jié)點(diǎn)可達(dá)的所有節(jié)點(diǎn)為止。如果還存在未被發(fā)現(xiàn)的節(jié)點(diǎn)，則選擇其中一個作為源節(jié)點(diǎn)并重復(fù)以上過程，整個進(jìn)程反復(fù)進(jìn)行直到所有節(jié)點(diǎn)都被訪問為止。
• 優(yōu)點(diǎn)：實(shí)現(xiàn)簡單，當(dāng)目標(biāo)明確時(shí)，搜索效率高。
• 缺點(diǎn)：不保證找到最短路徑，有可能會導(dǎo)致搜索陷入無限循環(huán)。

廣度優(yōu)先搜索（Breadth-First Search, BFS）：

• 原理：BFS是一種廣度優(yōu)先的搜索算法，用于搜索樹或圖。這個算法從根節(jié)點(diǎn)開始，沿著樹的寬度遍歷樹的節(jié)點(diǎn)，如果所有節(jié)點(diǎn)均被訪問，則算法結(jié)束。
• 優(yōu)點(diǎn)：可以找到最短路徑，結(jié)果可靠。
• 缺點(diǎn)：空間復(fù)雜度高，當(dāng)解空間大時(shí)，內(nèi)存消耗大。

貪婪搜索（Greedy Search）：

• 原理：貪婪搜索是一種在每一步選擇中都采取在當(dāng)前看來最好的選擇，希望通過一系列的最優(yōu)選擇，能夠產(chǎn)生一個全局最優(yōu)的解決方案。
• 優(yōu)點(diǎn)：簡單，易于實(shí)現(xiàn)，計(jì)算速度快。
• 缺點(diǎn)：不能保證找到全局最優(yōu)解，只能保證找到局部最優(yōu)解。
• 深度優(yōu)先搜索（DFS）：DFS會沿著一條路徑不斷往下搜索直到不能再繼續(xù)為止，然后再折返，開始搜索下一條路徑。這種搜索策略可以看作是“先入后出”，因此在實(shí)現(xiàn)DFS時(shí)通常使用棧（Stack）這種數(shù)據(jù)結(jié)構(gòu)。DFS的優(yōu)點(diǎn)是實(shí)現(xiàn)簡單，當(dāng)目標(biāo)明確時(shí)，搜索效率高。然而，DFS的缺點(diǎn)是不保證找到最短路徑，有可能會導(dǎo)致搜索陷入無限循環(huán)。
• 廣度優(yōu)先搜索（BFS）：相比之下，BFS會根據(jù)離起點(diǎn)的距離，按照從近到遠(yuǎn)的順序?qū)Ω鞴?jié)點(diǎn)進(jìn)行搜索。這種搜索策略可以看作是“先入先出”，因此在實(shí)現(xiàn)BFS時(shí)通常使用隊(duì)列（Queue）這種數(shù)據(jù)結(jié)構(gòu)。BFS的優(yōu)點(diǎn)是可以找到最短路徑，結(jié)果可靠。然而，BFS的缺點(diǎn)是空間復(fù)雜度高，當(dāng)解空間大時(shí)，內(nèi)存消耗大。

算法核心的三個問題是：

• 問題1：何時(shí)結(jié)束循環(huán)？
• 可能的選項(xiàng)：當(dāng)容器為空時(shí)結(jié)束循環(huán)
• 問題2：如果圖是循環(huán)的怎么辦？
• 當(dāng)一個節(jié)點(diǎn)從容器中移除（擴(kuò)展/訪問）后，它就不應(yīng)該再被添加回容器
• 問題3：如何移除正確的節(jié)點(diǎn)以便盡快到達(dá)目標(biāo)狀態(tài)，從而減少圖節(jié)點(diǎn)的擴(kuò)展。

深度優(yōu)先算法：數(shù)據(jù)結(jié)構(gòu)維護(hù)一個后進(jìn)先出（LIFO）的容器（即棧），算法移除/擴(kuò)展容器中最深的節(jié)點(diǎn)

#生成示例數(shù)據(jù)
graph = {}
graph["A"] = ["B", "D", "F"]
graph["B"] = ["C", "E"]
graph["D"] = ["C"]
graph["F"] = ["G", "H"]
graph["C"] = []
graph["E"] = []
graph["G"] = []
graph["H"] = []

from collections import deque
search_queue = deque()  # 創(chuàng)建一個節(jié)點(diǎn)列表
search_queue += graph["A"]  # 表示將"A"的相鄰節(jié)點(diǎn)都添加到節(jié)點(diǎn)列表中
from collections import deque

def search(start_node):
    search_queue = deque()
    search_queue += graph[start_node]
    searched = []  # 這個數(shù)組用于記錄檢查過的節(jié)點(diǎn)

    while search_queue:  # 只要節(jié)點(diǎn)列表不為空
        node = search_queue.pop()     #深度優(yōu)先
        #node = search_queue.popleft()  # 廣度優(yōu)先取出節(jié)點(diǎn)列表中最左邊的節(jié)點(diǎn)
        print(node, end=' ')  # 打印出當(dāng)前節(jié)點(diǎn)
        if not node in searched:  # 如果這個節(jié)點(diǎn)沒檢查過
            if node == 'G':  # 檢查這個節(jié)點(diǎn)是否為終點(diǎn)"G"
                print("\nfind the destination!")
                return True
            else:
                search_queue += graph[node]  # 將此節(jié)點(diǎn)的相鄰節(jié)點(diǎn)都添加到節(jié)點(diǎn)列表中
                searched.append(node)  # 將這個節(jié)點(diǎn)標(biāo)記為檢查過

    # 如果節(jié)點(diǎn)列表為空仍沒找到終點(diǎn)，則返回False
    return False

print(search("A"))

廣度優(yōu)先搜索算法：
數(shù)據(jù)結(jié)構(gòu)：維護(hù)一個先進(jìn)先出（FIFO）的容器（即隊(duì)列），算法操作：移除/擴(kuò)展容器中最淺的節(jié)點(diǎn)。具體代碼參考上面深度搜索算法，把“node = search_queue.pop() #深度優(yōu)先”換成“node = search_queue.popleft() # 廣度優(yōu)先取出節(jié)點(diǎn)列表中最左邊的節(jié)點(diǎn)”即可。可以看出BFS和DFS差別就在于根據(jù)“先入”或“后入”的原則，從邊界中選擇下一個節(jié)點(diǎn)。

貪婪搜索（Greedy Search）：
貪心算法的特點(diǎn)是考慮了從目標(biāo)節(jié)點(diǎn)找到任意點(diǎn)的代價(jià)，而一般算法考慮的是從起始節(jié)點(diǎn)到任意點(diǎn)的代價(jià)。即貪心算法考慮的是如何快速的找到目標(biāo)節(jié)點(diǎn)，使得到達(dá)目標(biāo)節(jié)點(diǎn)的時(shí)間成本最?。欢话闼惴紤]的是目標(biāo)節(jié)點(diǎn)到達(dá)目標(biāo)節(jié)點(diǎn)的花費(fèi)代價(jià)是最小的，而不是快速找到目標(biāo)節(jié)點(diǎn)?；谪澬牟呗栽噲D向目標(biāo)移動盡管這不是正確的路徑。由于它僅僅考慮到達(dá)目標(biāo)的代價(jià)，而忽略了當(dāng)前已花費(fèi)的代價(jià)，于是盡管路徑變得很長，它仍然繼續(xù)走下去。
貪婪算法中“行動的成本”可以用啟發(fā)式函數(shù)h(n)來算從任意結(jié)點(diǎn)n到目標(biāo)結(jié)點(diǎn)的最小代價(jià)評估值；啟發(fā)函數(shù)決定了貪婪算法運(yùn)算書讀，所以選擇一個好的啟發(fā)函數(shù)很重要。

• 實(shí)際的搜索問題中，從一個節(jié)點(diǎn)到其鄰居有一個“C”的成本
• 可以作為啟發(fā)函數(shù)計(jì)算代價(jià)的有：長度，時(shí)間，能量等
• 當(dāng)所有權(quán)重都為1時(shí)，貪婪算法找到最優(yōu)解
• 對于一般情況，如何盡快找到最小成本路徑？
  Dijkstra算法：
  Dijkstra算法算是貪心思想實(shí)現(xiàn)的，其可以適用與拓?fù)鋱D或者柵格圖，具體實(shí)現(xiàn)方法是，首先把起點(diǎn)到所有點(diǎn)的距離存下來找個最短的，然后松弛一次再找出最短的，所謂的松弛操作就是，遍歷一遍看通過剛剛找到的距離最短的點(diǎn)作為中轉(zhuǎn)站會不會更近，如果更近了就更新距離，這樣把所有的點(diǎn)找遍之后就存下了起點(diǎn)到其他所有點(diǎn)的最短距離：

• 策略：擴(kuò)展/訪問具有最低累積成本g(n)的節(jié)點(diǎn)
• g(n)：從起始狀態(tài)到節(jié)點(diǎn)“n”的累積成本的當(dāng)前最佳估計(jì)
• 更新所有未擴(kuò)展鄰居“m”的累積成本g(m)
• 已經(jīng)被擴(kuò)展/訪問的節(jié)點(diǎn)保證具有從起始狀態(tài)到該節(jié)點(diǎn)的最小成本 :::success
• 維護(hù)一個優(yōu)先隊(duì)列來存儲所有待擴(kuò)展的節(jié)點(diǎn)
• 用起始狀態(tài)XS初始化優(yōu)先隊(duì)列
• 設(shè)置g(XS)=0, 對圖中的其他所有節(jié)點(diǎn)設(shè)置g(n)=無窮
• 循環(huán):
• 如果隊(duì)列為空,返回FALSE并退出循環(huán)
• 從優(yōu)先隊(duì)列中取出g(n)最小的節(jié)點(diǎn)“n”
• 將節(jié)點(diǎn)“n”標(biāo)記為已擴(kuò)展
• 如果節(jié)點(diǎn)“n”是目標(biāo)狀態(tài),返回TRUE并退出循環(huán)
• 對節(jié)點(diǎn)“n”的所有未擴(kuò)展的鄰居節(jié)點(diǎn)“m”:
• 如果g(m) = 無窮
• g(m)= g(n) + Cnm
• 將節(jié)點(diǎn)“m”加入隊(duì)列
• 如果g(m) > g(n) + Cnm
• g(m)= g(n) + Cnm
• 結(jié)束對鄰居節(jié)點(diǎn)的循環(huán)
• 結(jié)束主循環(huán) ::: BFS（Best-First-Search）算法
  BFS（Best-First-Search）算法也是可以看作基于啟發(fā)式的深度優(yōu)先算法，其按照和Dijkstra類似的流程運(yùn)行，不同的是它能夠評估任意結(jié)點(diǎn)到目標(biāo)點(diǎn)的代價(jià)（即啟發(fā)式函數(shù)）。與選擇離初始結(jié)點(diǎn)最近的結(jié)點(diǎn)不同的是，它選擇離目標(biāo)最近的結(jié)點(diǎn)。BFS不能保證找到一條最短路徑。但是它比Dijkstra算法快的多，因?yàn)樗昧艘粋€啟發(fā)式函數(shù)（heuristic ）能快速地導(dǎo)向目標(biāo)結(jié)點(diǎn)。例如，如果目標(biāo)位于出發(fā)點(diǎn)的南方，BFS將趨向于導(dǎo)向南方的路徑。在下面的圖中，越黃的結(jié)點(diǎn)代表越高的啟發(fā)值（移動到目標(biāo)的代價(jià)高），而越黑的結(jié)點(diǎn)代表越低的啟發(fā)值（移動到目標(biāo)的代價(jià)低）。這表明了與Dijkstra 算法相比，BFS運(yùn)行得更快。

然而，這兩個例子都僅僅是最簡單的情況——地圖中沒有障礙物，最短路徑是直線的?，F(xiàn)在我們來考慮前邊描述的凹型障礙物。Dijkstra算法運(yùn)行得較慢，但確實(shí)能保證找到一條最短路徑：

另一方面，BFS運(yùn)行得較快，但是它找到的路徑明顯不是一條好的路徑：

由于BFS是基于貪心策略的，它試圖向目標(biāo)移動盡管這不是正確的路徑。由于它僅僅考慮到達(dá)目標(biāo)的代價(jià)，而忽略了當(dāng)前已花費(fèi)的代價(jià)，于是盡管路徑變得很長，它仍然繼續(xù)走下去。
結(jié)合兩者的優(yōu)點(diǎn)不是更好嗎？1968年發(fā)明的A算法就是把啟發(fā)式方法（heuristic approaches）如BFS，和常規(guī)方法如Dijsktra算法結(jié)合在一起的算法。有點(diǎn)不同的是，類似BFS的啟發(fā)式方法經(jīng)常給出一個近似解而不是保證最佳解。然而，盡管A基于無法保證最佳解的啟發(fā)式方法，A卻能保證找到一條最短路徑。
A: 帶有啟發(fā)式函數(shù)的Dijkstra算法*
把Dijkstra算法（靠近初始點(diǎn)的結(jié)點(diǎn)）和BFS算法（靠近目標(biāo)點(diǎn)的結(jié)點(diǎn)）的信息塊結(jié)合起來。在A的標(biāo)準(zhǔn)術(shù)語中，g(n)表示從初始結(jié)點(diǎn)到任意結(jié)點(diǎn)n的代價(jià)，h(n)表示從結(jié)點(diǎn)n到目標(biāo)點(diǎn)的啟發(fā)式評估代價(jià)（heuristic estimated cost）。當(dāng)從初始點(diǎn)向目標(biāo)點(diǎn)移動時(shí)，A* 權(quán)衡這兩者。每次進(jìn)行主循環(huán)時(shí)，它檢查f(n)最小的結(jié)點(diǎn)n，其中f(n) = g(n) + h(n)。

• 累積成本
• g(n): 從起始狀態(tài)到節(jié)點(diǎn)“n”的累積成本的當(dāng)前最佳估計(jì)
• 啟發(fā)式函數(shù)
• h(n): 從節(jié)點(diǎn)n到目標(biāo)狀態(tài)（即目標(biāo)成本）的預(yù)計(jì)最小成本
• 從起始狀態(tài)到通過節(jié)點(diǎn)“n”的目標(biāo)狀態(tài)的最小預(yù)計(jì)成本是 f(n) = g(n) + h(n)
• 策略: 擴(kuò)展具有最便宜的 f(n) = g(n) + h(n) 的節(jié)點(diǎn)
• 更新所有未擴(kuò)展鄰居“m”的節(jié)點(diǎn)“n”的累積成本 g(m)
• 已經(jīng)擴(kuò)展的節(jié)點(diǎn)保證具有從起始狀態(tài)到該節(jié)點(diǎn)的最小成本 :::success
• 維護(hù)一個優(yōu)先隊(duì)列來存儲所有待擴(kuò)展的節(jié)點(diǎn)
• 對所有節(jié)點(diǎn)預(yù)定義啟發(fā)函數(shù)h(n)
• 用起始狀態(tài)XS初始化優(yōu)先隊(duì)列
• 設(shè)置g(XS)=0,對圖中的其他節(jié)點(diǎn)設(shè)置g(n)=無窮
• 循環(huán):
• 如果隊(duì)列為空,返回FALSE并退出循環(huán)
• 從隊(duì)列中取出f(n)=g(n)+h(n)最小的節(jié)點(diǎn)“n”
• 將節(jié)點(diǎn)“n”標(biāo)記為已擴(kuò)展
• 如果節(jié)點(diǎn)“n”是目標(biāo)狀態(tài),返回TRUE并退出循環(huán)
• 對節(jié)點(diǎn)“n”的所有未擴(kuò)展鄰居節(jié)點(diǎn)“m”:
• 如果g(m)=無窮
• g(m)= g(n) + Cnm
• 將節(jié)點(diǎn)“m”加入隊(duì)列
• 如果g(m)>g(n)+Cnm
• g(m)= g(n) + Cnm
• 結(jié)束對鄰居節(jié)點(diǎn)循環(huán)
• 結(jié)束主循環(huán) ::: 通過對啟發(fā)式函數(shù)的調(diào)節(jié)，可以達(dá)成控制A* 的行為：
• 一種極端情況，如果h(n)是0，則只有g(shù)(n)起作用，此時(shí)A* 演變成Dijkstra算法，這保證能找到最短路徑。
• 如果h(n)經(jīng)常都比從n移動到目標(biāo)的實(shí)際代價(jià)?。ɑ蛘呦嗟龋瑒tA保證能找到一條最短路徑。h(n)越小，A擴(kuò)展的結(jié)點(diǎn)越多，運(yùn)行就得越慢。
• 如果h(n)精確地等于從n移動到目標(biāo)的代價(jià)，則A 將會僅僅尋找最佳路徑而不擴(kuò)展別的任何結(jié)點(diǎn)，這會運(yùn)行得非常快。盡管這不可能在所有情況下發(fā)生，但是你仍可以在一些特殊情況下讓它們精確地相等。只要提供完美的信息，A會運(yùn)行得很完美，認(rèn)識這一點(diǎn)很好。
• 如果h(n)有時(shí)比從n移動到目標(biāo)的實(shí)際代價(jià)高，則A* 不能保證找到一條最短路徑，但它運(yùn)行得更快。
• 另一種極端情況，如果h(n)比g(n)大很多，則只有h(n)起作用，A* 就演變成了BFS算法。
  如果目標(biāo)的引力太低，會得到最短路徑，不過速度變慢了；如果目標(biāo)引力太高，那就放棄了最短路徑，但A運(yùn)行得更快，所以最優(yōu)路徑和最快搜索在復(fù)雜情況下需要有一個取舍/平衡。
  A的這個特性非常有用。例如，你會發(fā)現(xiàn)在某些情況下，你希望得到一條好的路徑，而不是一條完美的路徑，為了權(quán)衡g(n)和h(n)，你可以修改任意一個。

如果alpha是0，則改進(jìn)后的代價(jià)函數(shù)的值總是1。這種情況下，地形代價(jià)被完全忽略，A工作變成簡單地判斷一個網(wǎng)格可否通過。如果alpha是1，則最初的代價(jià)函數(shù)將起作用，然后你得到了A的所有優(yōu)點(diǎn)。你可以設(shè)置alpha的值為0到1的任意值。
可以考慮對啟發(fā)式函數(shù)的返回值做選擇：絕對最小代價(jià)或者期望最小代價(jià)。例如，如果你的地圖大部分地形代價(jià)為2，其它一些地方是代價(jià)為1的道路，那么你可以考慮讓啟發(fā)式函數(shù)不考慮道路，而只返回2距離。
速度和精確度之間的選擇并不是全局固定對。在地圖上的某些區(qū)域，精確度是重要的，你可以基于此進(jìn)行動態(tài)選擇。例如，假設(shè)我們可能在某點(diǎn)停止重新計(jì)算路徑或者改變方向，則在接近當(dāng)前位置的地方，選擇一條好的路徑則是更重要的，對于在地圖上的一個安全區(qū)域，最短路徑也許并不十分重要，但是當(dāng)從一個危險(xiǎn)區(qū)域脫離對時(shí)候，軌跡的精度是最重要的。
同樣通過對g(n)或者f(n)的調(diào)節(jié)，也可以達(dá)成A具體動作的控制

• 通過加上障礙物cost function到g(n)或者f(n)（這兩個動作是一個意思），可以實(shí)現(xiàn)規(guī)劃路徑在障礙物中間。
• 通過加上車輛幾何或者軌跡kappa平滑度cost function的到g(n)或者f(n)，可以實(shí)現(xiàn)規(guī)劃出來的路徑是平滑變化的。
• 通過加上到way point的cost function的到g(n)或者f(n)，規(guī)劃出來的路徑則傾向于走way points的方向。
• 構(gòu)造精確啟發(fā)函數(shù)的一種方法是預(yù)先計(jì)算任意一對結(jié)點(diǎn)之間最短路徑的長度。有幾種方法可以近似模擬這種啟發(fā)函數(shù)：

1.  【降采樣地圖-預(yù)計(jì)算】在密集柵格圖的基礎(chǔ)上添加一個分辨率更大的稀疏柵格圖。預(yù)計(jì)算稀疏圖中任意兩個柵格的最短路徑。

2.  【waypoings-預(yù)計(jì)算】在密集柵格圖上，預(yù)計(jì)算任意兩個way points的的最短路徑。

通過以上方法，添加一個啟發(fā)函數(shù)h’用于評估從任意位置到達(dá)鄰近導(dǎo)航點(diǎn)/中繼點(diǎn)（waypoints）的代價(jià)。最終的啟發(fā)式函數(shù)可以是：
h(n) = h'(n, w1) + distance(w1, w2), h'(w2, goal)
網(wǎng)格地圖中的啟發(fā)式算法
在網(wǎng)格地圖中，有一些眾所周知的啟發(fā)式函數(shù)計(jì)算方法：

1.   曼哈頓距離
2.   對角線距離
3.   歐幾里得距離
4.   歐幾里德距離平方

曼哈頓距離
標(biāo)準(zhǔn)的啟發(fā)式函數(shù)是曼哈頓距離（Manhattan distance）。考慮代價(jià)函數(shù)并找到從一個位置移動到鄰近位置的最小代價(jià)D。因此，h是曼哈頓距離的D倍：

``` H(n) = D \  * (abs ( n.x – goal.x ) + abs ( n.y – goal.y ) ) ```

對角線距離

如果在地圖中允許對角運(yùn)動那么需要考慮對角線距離。（4 east, 4 north）的曼哈頓距離將變成8D。然而，可以簡單地移動（4 northeast）代替，所以啟發(fā)函數(shù)應(yīng)該是4D。這個函數(shù)使用對角線，假設(shè)直線和對角線的代價(jià)都是D：

H(n) = D * max(abs(n.x - goal.x), abs(n.y - goal.y))

如果對角線運(yùn)動的代價(jià)不是D，但類似于D2 = sqrt(2) * D，則準(zhǔn)確的計(jì)算方法如下：

h_diagonal(n) = min(abs(n.x - goal.x), abs(n.y - goal.y))

          h_straight(n) = (abs(n.x - goal.x) + abs(n.y - goal.y))

          H(n) = D2\* h_diagonal(n) + D\* (h_straight(n) - 2\ *h_diagonal(n)))

計(jì)算h_diagonal(n)：沿著斜線可以移動的步數(shù)；h_straight(n)：曼哈頓距離；然后合并這兩項(xiàng)，讓所有的斜線步都乘以D2，剩下的所有直線步(注意這里是曼哈頓距離的步數(shù)減去2倍的斜線步數(shù))都乘以D。
歐幾里德距離
如果單位可以沿著任意角度移動（而不是網(wǎng)格方向），那么應(yīng)該使用直線距離：

``` H(n) = D* sqrt((n.x-goal.x)^2 + (n.y-goal.y)^2) ```

然而，如果是這樣的話，直接使用A時(shí)將會遇到麻煩，因?yàn)榇鷥r(jià)函數(shù)g不匹配啟發(fā)函數(shù)h。因?yàn)闅W幾里得距離比曼哈頓距離和對角線距離都短，你仍可以得到最短路徑，不過A將運(yùn)行得更久一些：

歐幾里德距離平方

還有一個方法是，使用距離的平方替代距離，避免進(jìn)行平方根開方運(yùn)算，從而減少計(jì)算消耗：

``` H(n) = D* ((n.x-goal.x)^2 + (n.y-goal.y)^2) ```

不過這樣做會明顯地導(dǎo)致衡量單位的問題。當(dāng)A計(jì)算f(n) = g(n) + h(n)，距離的平方將比g的代價(jià)大很多，并且會因?yàn)閱l(fā)式函數(shù)評估值過高而停止。對于更長的距離，這樣做會靠近g(n)的極端情況而不再計(jì)算任何東西，A退化成BFS：

啟發(fā)函數(shù)的啟發(fā)因子

導(dǎo)致A搜索低性能的另外一個原因是啟發(fā)函數(shù)的啟發(fā)因子。當(dāng)某些路徑具有相同的f值的時(shí)候，它們都會被探索，比較函數(shù)無法打破比較平衡點(diǎn)，盡管我們這時(shí)候只需要搜索其中的一條，下圖為沒有添加啟發(fā)因子的效果：

為了解決這個問題，我們可以為啟發(fā)函數(shù)添加一個較小的啟發(fā)因子。啟發(fā)因子對于每個結(jié)點(diǎn)必須是確定的唯一的，而且它必須讓f值體現(xiàn)區(qū)別。因?yàn)锳將會對f值進(jìn)行堆排序，讓f值不同，意味著只有一個f值會被檢測。

一種添加啟發(fā)因子的方式是稍微改變h的衡量單位。如果我們減少衡量單位，那么當(dāng)我們朝著目標(biāo)移動的時(shí)候f將逐漸增加。很不幸，這意味著A傾向于擴(kuò)展到靠近初始點(diǎn)的結(jié)點(diǎn)，而不是靠近目標(biāo)的結(jié)點(diǎn)。我們可以稍微的微調(diào)h的衡量單位（甚至是0.1%），A就會傾向于擴(kuò)展到靠近目標(biāo)的結(jié)點(diǎn)。

``` heuristic  \  \ *= (1.0 + p) ```

其中這里的啟發(fā)因子需要滿足

p < 移動一步的最小代價(jià) / 期望的最長路徑長度。

假設(shè)你不希望你的路徑超過1000步（step），你可以使p = 1 / 1000。添加這個附加值的結(jié)果是，A比以前搜索的結(jié)點(diǎn)更少了。如下圖所示。

當(dāng)存在障礙物時(shí)，當(dāng)然仍要在它們周圍尋找路徑，但要意識到，當(dāng)繞過障礙物以后，A搜索的區(qū)域非常少：

1. Steven van Dijk的建議是：直接把h放到比較函數(shù)中。當(dāng)f值相等時(shí)，比較函數(shù)將會通過比較兩個節(jié)點(diǎn)h的大小實(shí)現(xiàn)啟發(fā)因子的功能，打破比較平衡點(diǎn)。
2. Cris Fuhrman的建議的啟發(fā)因子是：給每個節(jié)點(diǎn)加一個決定性的任意數(shù)，例如所在坐標(biāo)系中位置的hash值
3. 最后一種方法類似于frenet坐標(biāo)系的做法：對比起點(diǎn)到終點(diǎn)的直連線段的投影距離，計(jì)算方法入下：

dx1 = current.x - goal.x
dy1 = current.y - goal.y
dx2 = start.x - goal.x
dy2 = start.y - goal.y
cross = abs(dx1*dy2 - dx2*dy1)
heuristic += cross*0.001

其目的是：計(jì)算初始->目標(biāo)向量和當(dāng)前->目標(biāo)向量的向量叉乘（cross-product）。當(dāng)向量偏離方向后，其叉乘將會提供一個較大的啟發(fā)因子。結(jié)果是，這段代碼選擇的路徑稍微傾向于從初始點(diǎn)到目標(biāo)點(diǎn)的直線。當(dāng)沒有障礙物時(shí)，A不僅搜索很少的區(qū)域，而且它找到的路徑看起來非常棒：

跳點(diǎn)搜索

Jump Point Search (JPS) 是一種改進(jìn)的 A_ 算法，它保留了 A_ 算法的主體框架，但在尋找后繼節(jié)點(diǎn)的操作上進(jìn)行了優(yōu)化。在 A 算法中，會將當(dāng)前節(jié)點(diǎn)的所有未訪問鄰居節(jié)點(diǎn)加入 openlist。而 JPS 則使用一些方法將有“價(jià)值”的節(jié)點(diǎn)加入 openlist。JPS 的核心就是尋找跳點(diǎn) (Jump Point)，在 JPS 中，就是將跳點(diǎn)加入 openlist。跳點(diǎn)就是路徑的轉(zhuǎn)折點(diǎn)。

JPS明智地進(jìn)行探索，因?yàn)樗偸歉鶕?jù)規(guī)則向前看。強(qiáng)調(diào)了其在搜索過程中的智能性和前瞻性。
JPS 算法的基本流程與 A 一致，代價(jià)函數(shù) f(n) 仍然表示如下：f(n)=g(n)+h(n)。
JPS 算法的優(yōu)點(diǎn)在于，由于它只擴(kuò)展跳點(diǎn)，跳點(diǎn)間的柵格被跳過，不加入 OpenList，因此，它的搜索效率比 A 算法提高了一個等級。
在實(shí)現(xiàn)JPS前先了解它的規(guī)則

1. 強(qiáng)迫鄰居：節(jié)點(diǎn)X的鄰居節(jié)點(diǎn)有障礙物，且X的父節(jié)點(diǎn)P經(jīng)過X到達(dá)N的距離代價(jià)，比不經(jīng)過X到達(dá)N的任一路徑的距離代價(jià)都小，則稱N是X的強(qiáng)迫鄰居。

2. 跳點(diǎn)(Jump Point)：什么樣的節(jié)點(diǎn)可以作為跳點(diǎn)
   (1)節(jié)點(diǎn) A 是起點(diǎn)、終點(diǎn).
   (2)節(jié)點(diǎn)A 至少有一個強(qiáng)迫鄰居.
   (3)父節(jié)點(diǎn)在斜方向(斜向搜索)，節(jié)點(diǎn)A的水平或者垂直方向上有滿足 (1)、(2) 的節(jié)點(diǎn)
   在搜索過程中它可以將水平和垂直方向兩個分量，分解為一個方向?yàn)?1, 0)的水平搜索，一個方向?yàn)?0, 1)的垂直搜索
   同理斜向有四種方向
   左上 (-1, 1) ——>對應(yīng)的水平 (-1, 0)，垂直 ( 0, 1)
   右上 ( 1, 1) ——>對應(yīng)的水平 ( 1, 0)，垂直 ( 0, 1)
   右下 ( 1, -1) ——>對應(yīng)的水平 ( 1, 0)，垂直 ( 0, -1)
   左下 (-1, -1) ——>對應(yīng)的水平 (-1, 0)，垂直 ( 0, -1)
   如上所說(3)的情形即為如下

• 遞歸應(yīng)用直線剪枝規(guī)則，并將 y 識別為 x 的跳點(diǎn)后繼。這個節(jié)點(diǎn)很有趣，因?yàn)樗幸粋€鄰居 z，除非通過訪問 x 然后 y 的路徑，否則無法最優(yōu)地到達(dá)。
• 遞歸應(yīng)用對角線剪枝規(guī)則，并將 y 識別為 x 的跳點(diǎn)后繼。
• 在每次對角線步驟之前，我們首先遞歸直線。只有當(dāng)兩次直線遞歸都未能識別出跳點(diǎn)時(shí)，我們才再次對角線步進(jìn)。
• 節(jié)點(diǎn) w，x 的強(qiáng)制鄰居，正常擴(kuò)展。（也推入開放列表，優(yōu)先隊(duì)列）。
  記住:你只能直線跳躍或?qū)蔷€跳躍;不能分段跳躍。:::success
• 維護(hù)一個優(yōu)先隊(duì)列來存儲所有待擴(kuò)展的節(jié)點(diǎn)
• 對所有節(jié)點(diǎn)預(yù)先定義啟發(fā)函數(shù)h(n)
• 用起始狀態(tài)XS初始化優(yōu)先隊(duì)列
• 設(shè)置g(XS)=0,對圖中的其他節(jié)點(diǎn)設(shè)置g(n)=無窮
• 循環(huán):
• 如果隊(duì)列為空,返回FALSE并退出循環(huán)
• 從隊(duì)列中取出f(n)=g(n)+h(n)最小的節(jié)點(diǎn)“n”
• 將節(jié)點(diǎn)“n”標(biāo)記為已擴(kuò)展
• 如果節(jié)點(diǎn)“n”是目標(biāo)狀態(tài),返回TRUE并退出循環(huán)
• 對節(jié)點(diǎn)“n”的所有未擴(kuò)展鄰居節(jié)點(diǎn)“m”:
• 如果g(m)=無窮
• g(m)= g(n) + Cnm
• 將節(jié)點(diǎn)“m”加入隊(duì)列
• 如果g(m)>g(n)+Cnm
• g(m)= g(n) + Cnm
• 結(jié)束對鄰居節(jié)點(diǎn)的循環(huán)
• 結(jié)束主循環(huán) ::: openlist查找具體流程如下2：
• 初始化起點(diǎn)節(jié)點(diǎn) start ，將起點(diǎn)周圍四個角落的空閑節(jié)點(diǎn)相對于起點(diǎn)的相對位置加入起點(diǎn)節(jié)點(diǎn)的 forced_neighbor_list。
• 創(chuàng)建一個 openlist ，將 start 加入 openlist。
• while openlist is not empty:
• node ← openlist.Pop ()
• 從 node 開始跳躍，首先進(jìn)行直線跳躍，再進(jìn)行對角線跳躍。
• 用 parent 表示從 node 進(jìn)行對角線跳躍得到的節(jié)點(diǎn)，用 current 表示從 parent 進(jìn)行直線跳躍得到的節(jié)點(diǎn)。
• 如果 current 是跳點(diǎn)，而 parent 與 node 是同一個節(jié)點(diǎn)，則將 current 加入 openlist，同時(shí)將 current 的父節(jié)點(diǎn)指向 node；
• 如果 current 是跳點(diǎn)，而 parent 與 node 不是同一個節(jié)點(diǎn)，則將 parent 和 current 加入 openlist，同時(shí)將 current 的父節(jié)點(diǎn)指向 parent，將 parent 的父節(jié)點(diǎn)指向 node；
• 如果 current 是障礙物或者邊界，則進(jìn)行對角線跳躍；
• 如果 parent 是障礙物或者邊界，則進(jìn)入下一輪循環(huán)。

例子：

• 擴(kuò)展—>對角線移動
• 最終找到一個關(guān)鍵節(jié)點(diǎn),將其加入開放列表。
• 從開放列表中彈出它(唯一的節(jié)點(diǎn))。
• 垂直擴(kuò)展,在障礙物處結(jié)束。

• 水平擴(kuò)展,遇到一個具有強(qiáng)制鄰居的節(jié)點(diǎn)。
• 將其添加到開放列表。

• 對角線擴(kuò)展,擴(kuò)展后沒有發(fā)現(xiàn)任何新的節(jié)點(diǎn)。
• 完成當(dāng)前節(jié)點(diǎn)的擴(kuò)展。

• 對角線移動。
• 先沿垂直和水平方向擴(kuò)展。

• 沒有發(fā)現(xiàn)任何新的節(jié)點(diǎn)。
• 對角線移動。

更詳細(xì)跳點(diǎn)搜索可以參考下面文章：
https://blog.csdn.net/LIQIANGEASTSUN/article/details/118766080

小結(jié)：

本文介紹了motion plan學(xué)院派的框架：

1. 前端路徑規(guī)劃
2. 后端軌跡生成
3. 不確定障礙物預(yù)估規(guī)劃

并且詳細(xì)介紹了前端路徑規(guī)劃常用的搜索規(guī)劃，介紹了搜索規(guī)劃的一些前置知識：

1. c-space，為了方便物體質(zhì)點(diǎn)化處理，建圖時(shí)把物體形狀構(gòu)建轉(zhuǎn)移到圖
2. 各種不同圖如何構(gòu)建成適合搜索算法的數(shù)據(jù)格式，以及不同圖適合的搜索算法
3. 搜索算法的三個基本框架：深度搜索、廣度搜索、貪心搜索

詳細(xì)介紹了了幾種貪心搜索算法原理和實(shí)現(xiàn)思路：

1. Dijkstra算法：
2. A* 搜索
3. 跳點(diǎn)搜索

并且介紹了：累計(jì)成本，啟發(fā)函數(shù)，以及這兩個函數(shù)的物理意義；如何調(diào)控兩個參數(shù)來實(shí)現(xiàn)計(jì)算速度和最優(yōu)路徑的平衡。

• 累積成本
• g(n): 從起始狀態(tài)到節(jié)點(diǎn)“n”的累積成本的當(dāng)前最佳估計(jì)
• 啟發(fā)式函數(shù)
• h(n): 從節(jié)點(diǎn)n到目標(biāo)狀態(tài)（即目標(biāo)成本）的預(yù)計(jì)最小成本