運動規劃之搜索算法：前端規劃、后端軌跡生成到狀態求解

作者：遠洋之帆 2023-11-30 10:02:45

本文介紹了motion plan學院派的框架，并且詳細介紹了前端路徑規劃常用的搜索規劃，介紹了搜索規劃的一些前置知識。

本文經自動駕駛之心公眾號授權轉載，轉載請聯系出處。

背景：

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

正文：

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

1.前端：路徑規劃

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

2.后端：軌跡生成

最小抖動軌跡生成
微分平坦性
最小抖動優化
最小抖動的閉式解
時間分配
實際應用
軟硬約束軌跡優化
軟約束軌跡優化
硬約束軌跡優化

3.不確定性狀態求解：移動障礙物、突變環境、設備建模變化

基于馬爾可夫決策過程的規劃（MDP）
規劃中的不確定性和馬爾可夫決策過程
最小最大成本規劃和期望成本最小規劃
值迭代和實時動態規劃
機器人規劃的模型預測控制（MPC）
線性模型預測控制
非線性模型預測控制

前端——搜索路徑規劃

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

在配置空間中規劃123

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

在工作空間中

機器人有形狀和大小（即，難以進行運動規劃）
在配置空間：C-space中
機器人是一個點（即，易于進行運動規劃）?
在進行運動規劃之前，障礙物在C-space中表示?[10]
在C-space中表示障礙物可能非常復雜。因此，在實踐中使用近似（但更保守）的表示。

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

構建地圖：

在路徑規劃中，構建搜索地圖是一個關鍵步驟。這通常涉及到將實際環境抽象為一個圖（Graph），其中節點（Nodes）代表可能的位置，邊（Edges）代表從一個位置到另一個位置的移動。以下是一個詳細的例子：
假設我們有一個機器人需要在一個室內環境中導航。這個環境可以是一個房間，有一些障礙物，比如桌子和椅子。

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

地圖種類：

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

柵格地圖

概率圖

特征地圖

拓撲地圖-有向圖

拓撲地圖-無向圖

搜索算法介紹

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

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. 拓撲地圖
              1. Dijkstra
              2. BFS（Best-First-Search）
              3.  A*
              4. CH
              5. HH
              6. CRP

圖搜索算法結構

:::success

維護一個容器來存儲所有待訪問的節點
該容器以起始狀態XS進行初始化
循環
根據某個預定義的評分函數從容器中移除一個節點
訪問一個節點
擴展：獲取該節點的所有鄰居
發現所有的鄰居
將它們（鄰居）推入容器
擴展：獲取該節點的所有鄰居
結束循環 :::

通用搜索算法結構

常用的圖搜索有3大類的搜索結構，其它算法都是在這三個大的框架之下做改進。
深度優先搜索（Depth-First Search, DFS）：

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

廣度優先搜索（Breadth-First Search, BFS）：

原理：BFS是一種廣度優先的搜索算法，用于搜索樹或圖。這個算法從根節點開始，沿著樹的寬度遍歷樹的節點，如果所有節點均被訪問，則算法結束。
優點：可以找到最短路徑，結果可靠。
缺點：空間復雜度高，當解空間大時，內存消耗大。

貪婪搜索（Greedy Search）：

原理：貪婪搜索是一種在每一步選擇中都采取在當前看來最好的選擇，希望通過一系列的最優選擇，能夠產生一個全局最優的解決方案。
優點：簡單，易于實現，計算速度快。
缺點：不能保證找到全局最優解，只能保證找到局部最優解。
深度優先搜索（DFS）：DFS會沿著一條路徑不斷往下搜索直到不能再繼續為止，然后再折返，開始搜索下一條路徑。這種搜索策略可以看作是“先入后出”，因此在實現DFS時通常使用棧（Stack）這種數據結構。DFS的優點是實現簡單，當目標明確時，搜索效率高。然而，DFS的缺點是不保證找到最短路徑，有可能會導致搜索陷入無限循環。
廣度優先搜索（BFS）：相比之下，BFS會根據離起點的距離，按照從近到遠的順序對各節點進行搜索。這種搜索策略可以看作是“先入先出”，因此在實現BFS時通常使用隊列（Queue）這種數據結構。BFS的優點是可以找到最短路徑，結果可靠。然而，BFS的缺點是空間復雜度高，當解空間大時，內存消耗大。

算法核心的三個問題是：

問題1：何時結束循環？
可能的選項：當容器為空時結束循環
問題2：如果圖是循環的怎么辦？
當一個節點從容器中移除（擴展/訪問）后，它就不應該再被添加回容器
問題3：如何移除正確的節點以便盡快到達目標狀態，從而減少圖節點的擴展。

深度優先算法：數據結構維護一個后進先出（LIFO）的容器（即棧），算法移除/擴展容器中最深的節點

#生成示例數據
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()  # 創建一個節點列表
search_queue += graph["A"]  # 表示將"A"的相鄰節點都添加到節點列表中
from collections import deque

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

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

    # 如果節點列表為空仍沒找到終點，則返回False
    return False

print(search("A"))

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

貪婪搜索（Greedy Search）：
貪心算法的特點是考慮了從目標節點找到任意點的代價，而一般算法考慮的是從起始節點到任意點的代價。即貪心算法考慮的是如何快速的找到目標節點，使得到達目標節點的時間成本最小；而一般算法考慮的是目標節點到達目標節點的花費代價是最小的，而不是快速找到目標節點。基于貪心策略試圖向目標移動盡管這不是正確的路徑。由于它僅僅考慮到達目標的代價，而忽略了當前已花費的代價，于是盡管路徑變得很長，它仍然繼續走下去。
貪婪算法中“行動的成本”可以用啟發式函數h(n)來算從任意結點n到目標結點的最小代價評估值；啟發函數決定了貪婪算法運算書讀，所以選擇一個好的啟發函數很重要。

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

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

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

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

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

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

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

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

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

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

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

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

曼哈頓距離
標準的啟發式函數是曼哈頓距離（Manhattan distance）。考慮代價函數并找到從一個位置移動到鄰近位置的最小代價D。因此，h是曼哈頓距離的D倍：

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

對角線距離

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

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

如果對角線運動的代價不是D，但類似于D2 = sqrt(2) * D，則準確的計算方法如下：

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)))

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

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

然而，如果是這樣的話，直接使用A時將會遇到麻煩，因為代價函數g不匹配啟發函數h。因為歐幾里得距離比曼哈頓距離和對角線距離都短，你仍可以得到最短路徑，不過A將運行得更久一些：

歐幾里德距離平方

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

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

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

啟發函數的啟發因子

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

為了解決這個問題，我們可以為啟發函數添加一個較小的啟發因子。啟發因子對于每個結點必須是確定的唯一的，而且它必須讓f值體現區別。因為A將會對f值進行堆排序，讓f值不同，意味著只有一個f值會被檢測。

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

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

其中這里的啟發因子需要滿足

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

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

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

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

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

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

跳點搜索

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

JPS明智地進行探索，因為它總是根據規則向前看。強調了其在搜索過程中的智能性和前瞻性。
JPS 算法的基本流程與 A 一致，代價函數 f(n) 仍然表示如下：f(n)=g(n)+h(n)。
JPS 算法的優點在于，由于它只擴展跳點，跳點間的柵格被跳過，不加入 OpenList，因此，它的搜索效率比 A 算法提高了一個等級。
在實現JPS前先了解它的規則

強迫鄰居：節點X的鄰居節點有障礙物，且X的父節點P經過X到達N的距離代價，比不經過X到達N的任一路徑的距離代價都小，則稱N是X的強迫鄰居。

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

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