0. Outline

1. Graph Search Basis

Configuration Space等概念

機(jī)器人配置: 指機(jī)器人位置和所有點(diǎn)的表示。
DOF: 指用于表示機(jī)器人配置所需的最小的實(shí)數(shù)坐標(biāo)的數(shù)量n。
C-space: 包含機(jī)器人n維所有配置的空間。
在C-space中機(jī)器人的pose是一個點(diǎn)。

• 機(jī)器人在C-space中被表示為一個點(diǎn)，pose包含為R，t
• 空間中的障礙物也需要映射到C-space中，并且根據(jù)機(jī)器人的尺寸做膨脹（該過程是one-time work，可一次完成，因?yàn)槿绻c(diǎn)碰不到膨脹邊緣，就不會碰到障礙物），叫做C-obstacle
• S-space=(C-obstacle) U (C-free)
• path是在C-free中從起點(diǎn)start到終點(diǎn)goal

eg：如果將機(jī)器人建模為一個半徑為${\delta }_r$的球，那么將obstacle向外膨脹${\delta }_r$，即可對一個點(diǎn)進(jìn)行palnning

2. Graph and Search Method

無向圖，有向圖，加權(quán)圖(權(quán)值根據(jù)具體問題定義，可能是長度，可能是消耗的能量，可能是風(fēng)險(xiǎn)等等)

基于搜索的圖有柵格，本身就包含關(guān)系，而基于采樣的圖則沒有這種關(guān)系。

搜索的過程即構(gòu)建搜索樹的過程，但該過程往往代價(jià)較大，我們的目標(biāo)是設(shè)計(jì)盡可能快且不失最優(yōu)性的路徑搜索算法。

DFS容器是棧，BFS容器是隊(duì)列。

圖示中DFS先順時針，后逆時針，訪問最大的，出棧，入棧。

圖始終BFS始終逆時針，從小到大依次訪問，出隊(duì)，入隊(duì)，不斷地一層一層地推進(jìn)frontier。

BFS回溯可以找到最短路徑，而DFS不可以，所以常用DFS。（先有一個直觀的印象）

貪心算法：

貪心算法依靠某種規(guī)則來挑選最優(yōu)的節(jié)點(diǎn)，叫做啟發(fā)。啟發(fā)即對于最近節(jié)點(diǎn)的一個猜測，比如圖中紅色到紫色的距離有兩種猜測：歐式距離和曼哈頓距離。

• 無障礙物時貪心算法與BFS對比：
  
  貪心算法又快又準(zhǔn)。

• 有障礙物時對比
  
  貪心算法雖然快，但是陷入局部最優(yōu)，而BFS雖然慢，但是路徑確實(shí)全局最優(yōu)。

雖然貪心算法有時不是最優(yōu)，但仍然有很多我們可以借鑒的性質(zhì)。

實(shí)際中需要考慮每小段路徑的cost，即C，當(dāng)每個C=1時，BFS最優(yōu)，但通常情況C不為1，于是引入了Dijkstra和A*。

3. Dijkstra

算法流程可以看浙大的這節(jié)視頻

dist最小可以使用priority_queue來實(shí)現(xiàn)，按照鍵值來排序，彈出鍵值最小的。

這里的priority queue就是open list，已經(jīng)擴(kuò)展過的(已經(jīng)收錄的node)放在close list中(實(shí)際上不是方node本身，只需使用容器管理這些node的下標(biāo))。

原來數(shù)據(jù)結(jié)構(gòu)里面講的Dijkstra和這里講的Dijkstra，在實(shí)現(xiàn)上的不同在于：

• 原來while()循環(huán)中一次只收錄一個node；
• 這里的while()循環(huán)，只要第一次遇到未被收錄的node，就將其收錄（也就是加入到open list中），并標(biāo)記為已收錄(id=1)，下次遇到id=1的直接判斷g值，并判斷是否更新g值。(實(shí)際上這里的Diskstra算法在便利neighbors nodes時id只有0和1，即只有new node和node already in open list，被遍歷完neighbor nodes的node，在下一次while循環(huán)中會被從open list中erase掉，并加入到close list中(set id to -1))

本質(zhì)上兩種實(shí)現(xiàn)方法相同，但是這里講的方法結(jié)合了Dijkstra算法的BFS的“隊(duì)列”的數(shù)據(jù)結(jié)構(gòu)特性，所以更好理解，以下是本章作業(yè)中主要的A*實(shí)現(xiàn)(只是比Dijkstra多了個heuristic)：

void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)
{   
    ros::Time time_1 = ros::Time::now();    

    //index of start_point and end_point
    Vector3i start_idx = coord2gridIndex(start_pt);
    Vector3i end_idx   = coord2gridIndex(end_pt);
    goalIdx = end_idx;

    //position of start_point and end_point
    start_pt = gridIndex2coord(start_idx);
    end_pt   = gridIndex2coord(end_idx);

    //Initialize the pointers of struct GridNode which represent start node and goal node
    GridNodePtr startPtr = new GridNode(start_idx, start_pt);
    GridNodePtr endPtr   = new GridNode(end_idx,   end_pt);

    //openSet is the open_list implemented through multimap in STL library
    openSet.clear();
    // currentPtr represents the node with lowest f(n) in the open_list
    GridNodePtr currentPtr  = NULL;
    GridNodePtr neighborPtr = NULL;

    //put start node in open set
    startPtr -> gScore = 0;
    startPtr -> fScore = startPtr -> gScore + getHeu(startPtr,endPtr);
    //STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
    startPtr -> id = 1; 
    startPtr -> coord = start_pt;
    openSet.insert( make_pair(startPtr -> fScore, startPtr) );//默認(rèn)按照key升序排序
    /*
    STEP 2 :  some else preparatory works which should be done before while loop
    please write your code below
    */
    double tentative_gScore;
    vector<GridNodePtr> neighborPtrSets;
    vector<double> edgeCostSets;
    //計(jì)算graph中所有的node到start_pt的heuristic

    //將start node賦予地圖(因?yàn)槭莿偛舗ew出來的，不是直接從地圖里面取出來的)
    GridNodeMap[start_idx(0)][start_idx(1)][start_idx(2)] = startPtr;
    // this is the main loop
    while ( !openSet.empty() ){
        /*
        step 3: Remove the node with lowest cost function from open set to closed set
        please write your code below
        
        IMPORTANT NOTE!!!
        This part you should use the C++ STL: multimap, more details can be find in Homework description
        */
        //彈出一個就要找到他所有的未被擴(kuò)展的鄰居
//        ROS_DEBUG("\nhere3");
        currentPtr = openSet.begin()->second;
        openSet.erase(openSet.begin());
        currentPtr->id = -1;//mark as visited

        // if the current node is the goal 
        if( currentPtr->index == goalIdx ){
            ros::Time time_2 = ros::Time::now();
            terminatePtr = currentPtr;
            ROS_WARN("[A*]{sucess}  Time in A*  is %f ms, path cost if %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution );            
            return;
        }
        //get the succetion(找到currentPtr的neighbor nodes)
        AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets);  //STEP 4: finish AstarPathFinder::AstarGetSucc yourself     

        /*
        STEP 5:  For all unexpanded neigbors "m" of node "n", please finish this for loop
        please write your code below     
        */
        //在這個循環(huán)里面，id只會為0/1，為-1的都是遍歷完之后被erase掉了
        for(int i = 0; i < (int)neighborPtrSets.size(); i++){
            neighborPtr = neighborPtrSets[i];
            tentative_gScore = currentPtr->gScore + edgeCostSets[i];
//            ROS_DEBUG("\nneighborPtr->id: %d", neighborPtr->id);
            /*
            Judge if the neigbors have been expanded
            please write your code below
            
            IMPORTANT NOTE!!!
            neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close set
            neighborPtrSets[i]->id = 1 : unexpanded, equal to this node is in open set      
            */
            if(neighborPtr -> id == 0 ){ //discover a new node, which is not in the closed set and open set
                /*
                STEP 6:  As for a new node, do what you need do ,and then put neighbor in open set and record it
                please write your code below 
                */

                if(neighborPtr -> gScore == inf) {
                    neighborPtr->cameFrom = currentPtr;
                    neighborPtr -> gScore = tentative_gScore;
                    neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr, endPtr); //getHeu(startPtr,neighborPtr);
                    neighborPtr -> id = 1;//標(biāo)記為expanded
                    openSet.insert( make_pair(neighborPtr->fScore, neighborPtr) );//加入到open list中，注意，這里需要使用fscore，而不是gscore，如果是gscore，就成了Dijkstra了
//                    ROS_DEBUG("\nneighborPtr -> gScore is inf");
                    continue;//這里可以直接退出本輪循環(huán)，因?yàn)橄旅娴臈l件不會滿足
                } else {
                    ROS_DEBUG_STREAM("\nA* here neighborPtr -> gScore is not inf");
                }
            }
            //這里不能取等，去等了如果前面在找neighbors時沒有去掉本身，就會陷入自己來自于自己的bug
            if(tentative_gScore < neighborPtr-> gScore){ //this node is in open set and need to judge if it needs to update, the "0" should be deleted when you are coding
                /*
                STEP 7:  As for a node in open set, update it , maintain the openset ,and then put neighbor in open set and record it
                please write your code below  
                */
                neighborPtr -> cameFrom = currentPtr;
                neighborPtr -> gScore = tentative_gScore;
                neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr, endPtr);

            }
            //不滿足條件的就是在close set中，不用處理，在下次的while循環(huán)中會被erase掉
        }      
    }
    //if search fails
    ros::Time time_2 = ros::Time::now();
    if((time_2 - time_1).toSec() > 0.1)
        ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() );
}

Pros：Dijkstra算法是完備且最優(yōu)的：完備即一定能找到解；最優(yōu)即解一定是最優(yōu)的。
Cons：由于沒有g(shù)oal的任何信息，所以是盲目地，均勻地向各個方向擴(kuò)散的。

結(jié)合貪心算法和info和Dijkstra的單源帶權(quán)搜索，將二者的優(yōu)點(diǎn)結(jié)合，引出了A*，相較于Dijkstra，在g值的計(jì)算方面添加了對goal的估計(jì)h，類似于DL中為loss添加正則項(xiàng)。

4. A*

4.1 算法介紹

A*與Dijkstra的唯一不同就是g值的計(jì)算，由單純的$g(n)$變?yōu)?span id="n5n3t3z" class="katex--inline">$f(n)=g(n)+h(n)$，其中所有節(jié)點(diǎn)的$h(n)$是在初始化階段one-time計(jì)算好的（如所有節(jié)點(diǎn)到goal的歐氏距離）。

4.2 A*最優(yōu)性討論

上例說明A* 不一定是最優(yōu)的，

$f_A=7,f_G=5$，A*找到的路徑為S->G

而實(shí)際cost，$g_{SG}=5,g_{SAG}=4$，顯然SAG最優(yōu)。

A*滿足最優(yōu)的條件：當(dāng)所有節(jié)點(diǎn)的guess的h $\le$ 真實(shí)g。

用實(shí)際數(shù)據(jù)來驗(yàn)證：

1. 設(shè)$h_s=6，h_A=5$，則
   $S:f=g+h=0+6=6$
   $A:f=g+h=1+5=6$
   $G:f=g+h=5+0=5$
   輸出S->G，非最優(yōu)。

2. 設(shè)，則
   $S:f=g+h=0+5=5$
   $A:f=g+h=1+4=5$
   $G:f=g+h=5+0=5$
   輸出方式涉及到路徑對稱性問題，在5.3節(jié)tie breaker中會詳細(xì)討論。

3. 設(shè)$h_s=5，h_A=3$，均滿足小于等于actual $g$，則
   $S:f=g+h=0+5=5$
   $A:f=g+h=1+3=4$
   $G:f=g+h=5+0=5$
   pop A;
   pop G;
   輸出S->A->G，最優(yōu)。

于是重點(diǎn)就轉(zhuǎn)移到了如何設(shè)計(jì)heuristic guess函數(shù)h

啟發(fā)式函數(shù)需要滿足 $h(n)\le h^{?}(n)$
我們稱這種啟發(fā)式函數(shù)為admissible(可接受的、可容許的)的。

admissible heuristic function的本質(zhì)為向量的范數(shù)，以下為一些admissible heuristic function的列舉：

• 歐式距離（向量2范數(shù)）：$||x|{|}_2=\sqrt{\sum _{i=1}^n|x_i{|}^2}$，always admissible
• 曼哈頓距離（向量1范數(shù)）：$|||x|{|}_1=\sum _{i=1}^n|x_i|$，Depends admissible
• 向量的無窮范數(shù)：$||x|{|}_{\infty }=\underset{i}{\max }(x_i)$，always admissible
• Octile distance: 基于8個方向的直線移動和對角線移動的最短路徑進(jìn)行計(jì)算。具體而言，Octile距離考慮了水平、垂直和對角線方向的移動。在直角移動（水平或垂直）時，距離為1；在對角線移動時，距離為根號2（即1.414）。這樣設(shè)計(jì)是為了模擬在規(guī)定的移動方向上的最短路徑。在路徑規(guī)劃算法（如A*算法）中，Octile距離經(jīng)常用于啟發(fā)式估計(jì)，以幫助算法找到最優(yōu)路徑。

上例說明A*有了貪心算法的對于目標(biāo)的引導(dǎo)性。

4.3 A*的改進(jìn)

由$f$中引入$h$的結(jié)果可知，h的引入也引入了貪心算法的特性：強(qiáng)目的性，faster。如果對h賦予權(quán)值則可以改變該引入特性的比重，于是出現(xiàn)了weighted A*，給h增減權(quán)重$?(?>=0)$，即$$f=g+?h$$

• 當(dāng)$?>1$時，貪心占比大，算法強(qiáng)suboptimal，faster；$?=+\infty$時，忽略$g$，算法退化為貪心算法
• 當(dāng)$?=1$時，為正常A*，solution suboptimal
• 當(dāng)$0<?<1$時，Dijkstra占比大，更接近optimal，slower；極端情況$?=0$退化為Dijkstra算法，solution optimal

weighted A*改進(jìn)的點(diǎn)：

1. $?>1$實(shí)際上是使用solution的optimality來換取speed，速度提升是A*的數(shù)量級等級(幾十倍等級別)
2. 學(xué)界已證明weighted A*的$?$-suboptimal，即$cost(solution)\le ??cost(optimalcost)$

還有weight A的改進(jìn)Anytime A，ARA*，D*等研究，本課程不做介紹，可自行拓展。

更改權(quán)值的對比：

該網(wǎng)頁可以調(diào)參進(jìn)行對比： http://qiao.github.io/PathFinding.js/visual/

以下是我的實(shí)驗(yàn)結(jié)果：

可以看出貪心算法operation最少，耗時最短，但solution為suboptimal(path較長)，Weight A*(a=1,b=1)和Dijkstra的solution為optimal，但Weight A*(a=1,b=1)明顯耗時更少，此實(shí)驗(yàn)中耗時提升了2.23倍，大規(guī)模問題上提升應(yīng)該會更大。

5. Engineering considerations

工程實(shí)現(xiàn)對于算法性能影響可能有10倍，100倍都不止。

5.1 地圖表示

以O(shè)ccupancy grid map為例：

2Dgrid-map的表示，8連通($2^3?1$)，3D為26連通($3^3?1$)

數(shù)據(jù)結(jié)構(gòu)在搜索過程中其實(shí)就是在找到該節(jié)點(diǎn)的相鄰節(jié)點(diǎn)時需要用到。

推薦使用stl中的priority_queue和multimap，對vector排序可以使用make_heap方法。(VINS-MONO中用unordered_map較多)。

5.2 the best heuristic

前面降到可以使用Euclidean distance作為heuristic，盡管Euclidean distance滿足$h(n)\le h^{?}(n)$即可找到optimal solution，但是在搜索過程中會擴(kuò)展很多不必要的節(jié)點(diǎn)，$h(n)\le h^{?}(n)$小于的非常多，即Euclidean distance不夠tight

對于結(jié)構(gòu)化的地圖和結(jié)構(gòu)化的移動rule，在2D情況下可以推導(dǎo)出任何一個節(jié)點(diǎn)到終點(diǎn)的heuristic的estimate，可直接用作我們的heuristic，我們稱之為Diagonal Heuristic（對角Heuristic）

TODO：這個2D Heuristic estimate結(jié)果怎么推出來的？

上例中DIagonal Heuristic和L2 Heuristic的solution path的cost（即f）相同，即出現(xiàn)了路徑對稱性問題，因?yàn)樵谟?jì)算f時他們之間沒有difference，于是引入了Tie breaker。

5.3 Tie breaker

直觀理解是打破等號。

Tie breaker用于打破5.2上述的對稱性問題，核心思想是 在cost相同的path中找到preference。

目的是減少不必要的node的拓展，提高搜索效率。

• 方法1：在計(jì)算Heuristic時加入一個系數(shù)$$p=\frac{一步的最小cost}{可能的path的最大cost(比如矩形地圖的斜對角線cost)}$$（這里還不是很理解為什么加入一個$p$就能使f不同，需要結(jié)合代碼具體理解。）
  這樣會輕微打破admissible條件，因?yàn)閔有小幅的增大，admissible heuristic取等號時是在沒有任何障礙物情況下，而實(shí)際工程中由于存在較多obstacle，計(jì)算出來的h通常不會達(dá)到tight heuristic。

• 方法2：定義rule：當(dāng)f相同時，取h更小的。

• 方法3：在f或者g上加一個deterministic random numbers(提前確定好的隨機(jī)數(shù)，確定之后不能變，使用hash映射)，工程實(shí)現(xiàn)可能較復(fù)雜，但效果會比前面的好。

• 方法4：傾向于走直線，同樣是slightly change heuristic，給heuristic加上一個corss，讓其更傾向走符合直觀感受的對角線path。

還有很多其他方法，可以看論文。

但是Tie breaker也會帶來一些負(fù)面影響，我們最終給到robotic的是一條可執(zhí)行的軌跡trajectory，trajectory是由我們的規(guī)劃出的path來生成的，會涉及到traj光滑的處理，如下圖，當(dāng)我們使用Tie breaker在有obsacle的情況下生成了一條path，理想的trajectory是如圖所示的紅色光滑曲線，使用tie breaker生成了虛線所示的path，在trajectory generation過程中就比較難得到紅色光滑traj，反而是矩形的path更易得到光滑traj，所以使用什么tie breaker是有講究的，針對這種情況，引入了本章的第3塊內(nèi)容：JPS。

6. JPS(Jump Point Search)

JPS是一種系統(tǒng)性的打破路徑平衡性的圖搜索方法，其core idea是找到對稱性并打破它。

6.1 look ahead rule(pruning rule)

Look ahead rule，以例子來介紹這樣一個規(guī)則：

1. 對于無障礙物，straight到達(dá)兒子節(jié)點(diǎn)的情況：

從某點(diǎn)(4)到達(dá)另一點(diǎn)(1)，不需要經(jīng)過兒子節(jié)點(diǎn)(x)時的path cost為$f_A$，經(jīng)過兒子節(jié)點(diǎn)的path cost為$f_B$，則
- 當(dāng)$f_B<f_A$時，才考慮從兒子節(jié)點(diǎn)到達(dá)該節(jié)點(diǎn)，稱該點(diǎn)為natural neighbors；
- 當(dāng)$f_B\ge f_A$，不考慮從x到達(dá)該節(jié)點(diǎn)(即neighbor pruning，discard)，稱該節(jié)點(diǎn)為inferior neighbors(劣性節(jié)點(diǎn))。

按照上述定義，$$(f_B=f_{4\to x\to 1}=1+\sqrt{2})>(f_A=f_{4\to 1}=1)$$
所以擴(kuò)展兒子節(jié)點(diǎn)$x$時不考慮節(jié)點(diǎn)1；

從4走到3是否需要經(jīng)過x：
$$(f_B=f_{4\to x\to 3}=1+\sqrt{2})=(f_A=f_{4\to 2\to 3}=\sqrt{2}+1)$$相等，所以不考慮節(jié)點(diǎn)3；

同理1，2，3，6，7，8均不需要考慮，只需要考慮5。

2. 對于無障礙物，diagonal對角線到達(dá)兒子節(jié)點(diǎn)的情況，rule稍微有些改動：

• 當(dāng)$f_B<=f_A$時，consider
• 當(dāng)$f_B>f_A$時，discard

eg1：到達(dá)1
$$(f_A=f_{6\to 4\to 1}=2)<(f_B=f_{6\to x\to 1}=2\sqrt{2})$$故discard 1

eg2：到達(dá)2
$$(f_A=f_{6\to 4\to 2}=\sqrt{2}+1)=(f_B=f_{6\to x\to 2}=\sqrt{2}+1)$$
相等，diagonal相等時需要consider，詳細(xì)參考論文

http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf Equation 1/2

3. 有障礙物，straight時，3號節(jié)點(diǎn)必須被考慮，稱為forced neighbors
4. 有障礙物，diagonal時，1號節(jié)點(diǎn)必須被考慮。

以上有/無障礙物的straight和diagonal情況可以涵蓋所有情況，向上下左右，向左上，左下，右上，右下均類似。

6.2 Jumping rule

為方便說明，將look ahead rule標(biāo)記為①~④，jumping的原則是先遞歸straight jump再遞歸diagonal jump，發(fā)現(xiàn)force neighbor則會遞歸直線recall最上面一個node（不允許折線recall）

例子1：
從$p(x)$出發(fā)進(jìn)行JPS擴(kuò)展：

1. 起始點(diǎn)使用④，綠框部分需要被consider，w為force beighbor
   
2. 在x處向上，向右運(yùn)用①最終都碰到障礙物（邊界也算障礙物），jump失敗
3. 直到遞歸diagonal jump到y(tǒng)時，對y向右遞歸straight jump到z觸發(fā)了③，綠框+藍(lán)框是consider，藍(lán)框?yàn)閒orce neighbor，故z為特殊節(jié)點(diǎn)，jump成功，遞歸返回到最上層發(fā)現(xiàn)z的節(jié)點(diǎn)，即y，將y加入到open list
   
4. 遞歸返回到x，結(jié)合第1步的圖，x節(jié)點(diǎn)的上、右遞歸straight jump，以及右上遞歸diagonal結(jié)束，該右下的force neighbor的遞歸diagonal jump，w天然是x的force neighbor，所以將w加入到open list中。

至此，完成了對x節(jié)點(diǎn)的所有JPS擴(kuò)展，將x節(jié)點(diǎn)加入到close list中。

例子2：

水平，豎直jump，無事發(fā)生，對角線跳一步

向右jump時發(fā)現(xiàn)了特殊，于是直線recall（不允許折線recall），將藍(lán)框的點(diǎn)加入到open list，綠色的起點(diǎn)即可加入close list，退出歷史舞臺。

例子3：

注意，當(dāng)在擴(kuò)展時發(fā)現(xiàn)了goal，視為發(fā)現(xiàn)了force neighbor，要recall并加入node到open list

最終path

例子4：

6.3 Extension

3D JPS，本質(zhì)上和2D JPS相同，只是3D rules更復(fù)雜一些：Sikang之前在深藍(lán)講過公開課，可以看看。

大多數(shù)情況下JPS比A* 更優(yōu)，因?yàn)镴PS能夠在復(fù)雜環(huán)境中迅速找到一個個jump point，省去了中間不必要的加入openlist的搜索過程。

但是在機(jī)器人的FOV內(nèi)看不到障礙物后的點(diǎn)，當(dāng)存在大量空曠場景時，JPS為了搜索jump point，會不斷地collision query，雖然對于open list的操作變少了，但是大量的collision query耗時會增加，可能不一定比A*更優(yōu)。

我的實(shí)驗(yàn)結(jié)果：


這種情況下明顯A* 更優(yōu)。

6.4 JPS summary

JPS小結(jié)：

1. look ahead rule：遵照①~④進(jìn)行拓展，舉一反三

2. jumping rule：

   1. 先水平、豎直straight擴(kuò)展，再diagonal擴(kuò)展，
   2. 遇obstacle或map border都視為該方向上擴(kuò)展失敗，直至發(fā)現(xiàn)滿足look ahead rule中的force neighbor，
   3. 直線recall到j(luò)ump的起始節(jié)點(diǎn)(不允許折線recall)，加入到open list中
   4. 當(dāng)所有方向都擴(kuò)展完畢之后，將source node加入close list中。
   5. 當(dāng)在擴(kuò)展時發(fā)現(xiàn)了goal，視為發(fā)現(xiàn)了force neighbor，要recall并加入node到open list

3. A* 與JPS的唯一區(qū)別在于如何選取neighbors，前者選取幾何neighbors，后者根據(jù)上述2條rules選取neighbors，故用在A*上的trick均可用于JPS(diagonal heuristic，Tie breaker等)

4. JPS 和A* 都依賴的一個點(diǎn)是force point一定是最優(yōu)路徑需要經(jīng)過的點(diǎn)，即jump point，將起點(diǎn)到終點(diǎn)過程中的所有jump points連接起來就得到了最優(yōu)path，只不過JPS通過他的規(guī)則過濾掉了很多不需要拓展的點(diǎn)，而A* 是所有幾何neighbors都拓展。（JPS的jump point和外層循環(huán)所維護(hù)的最小cost是不一樣的，前者是JPS’s rules，后者是JPS和A* 的算法框架）。

5. 在復(fù)雜環(huán)境中JPS通常博A* 更優(yōu)，但遠(yuǎn)不是always更優(yōu)

6. JPS降低了對于open list的操作（修改鍵值，排序等），但增加了status query的次數(shù)

7. JPS的局限性：僅僅在uniform grid map中才能使用(1，1，$\sqrt{2}$這類幾何特征的grid map)，對于更general的圖搜索問題（如帶權(quán)值的非grid map）,A* 可以用，而JPS無法使用。

8. 幾種搜索算法的聯(lián)合對比。
   BFS：單源無權(quán)有向廣度優(yōu)先搜索；
   Dijkstra：單源有權(quán)有向最短路徑搜索；
   A*：啟發(fā)式(heuristic)的Dijkstra(找neighbor nodes規(guī)則是幾何的鄰居，2D最多有8個，3D最多有26個)
   JPS：在A*基礎(chǔ)上定義了找neighbor nodes的rules(Look ahead rules & Jumping rules)
   GPT-4.0今天給了我一個heuristic
   文章來源地址http://www.zghlxwxcb.cn/news/detail-815713.html

7. 論文推薦

1. http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf
2. Planning Dynamically Feasible Trajectories for Quadrotors using Safe Flight Corridors in 3-D Complex Environments, Sikang Liu, RAL 2017

到了這里，關(guān)于【深藍(lán)學(xué)院】移動機(jī)器人運(yùn)動規(guī)劃--第2章 基于搜索的路徑規(guī)劃--筆記的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！