現在，假如有一個機器人，它已經儲存好一個全域性的地圖（哪裡可通行，哪裡不可通行），並且知道自己在其中的位置。現在要從給定的起點走到終點，我們應該怎麼做？
有軌導航和無軌導航
在某些應用場景中，例如工廠或倉庫，環境相對固定且對路徑的準確性要求較高。這種情況下，我們可以使用有軌導航系統，比如透過在地面上鋪設磁條或貼上二維碼等方式進行人工標識。機器人透過讀取這些標識來確定自己的位置和導航路線。這種方法的優點是導航精度高，重複性好，適用於需要高度結構化環境中的重複工作。然而，它的缺點也很明顯，包括安裝成本高、環境改變後需要重新佈置標識、靈活性較差。
或者，另一種選擇是使用半有軌導航，這種方式不依賴於物理導軌，但會保留路線上的標識，如地面上的條形碼。機器人則透過發射鐳射或使用視覺系統讀取這些標識。這種方式結合了有軌導航的精確性和某些無軌導航的靈活性。其優點包括在有限的環境改動下仍能保持較好的導航效果，而缺點是仍然依賴於環境中的某些固定標識，且在標識被遮擋或損壞時容易出現導航錯誤。
更加通用且靈活的方式是無軌導航，機器人完全依靠自身裝備的感測器（如鐳射、視覺或超聲波）進行環境的地圖構建和自我定位。這種方法使得機器人能在沒有任何預設標記的環境中自由移動，非常適用於不斷變化的環境如家庭或辦公場所。機器人透過連續地掃描周圍環境，並與內部地圖進行匹配，來識別自己的位置和導航路線。這種方法的優點是極高的靈活性和適應性，但需要機器人自己的本事，挑戰在於需要複雜的演算法來處理環境資料，計算成本高。所以需要我們為機器人設計演算法，這也是本文的主要內容。
路徑規劃
現在，我們要根據起點，終點和地圖資訊，規劃出來一條路徑。
首先嘛簡單起見，我們假設地圖只分為可通行和不可通行，並且把機器人視作質點（也就是忽略運動學和幾何的約束，若要考慮機器人半徑，可以把障礙物向外擴充來進行等效，這不是本文的重點）。
行車圖 （Roadmap）
行車圖法是路徑規劃中常用的一種技術，旨在建立一個簡化的圖，這個圖反映了從起點到終點的可能路徑。
可檢視（Visibility Graph）是一種典型的行車圖。我們連線起點，終點和所有障礙物頂點形成的邊（去掉會穿過障礙物的），實際上就得到了一張圖，在此圖上執行搜尋或者最短路演算法就可以得到路徑。其簡單且路徑長度優，擴充障礙邊緣可以避免過於靠近障礙物。但是，對於邊界不規則或非多邊形的障礙物，這種方法可能不夠精確或效率不高。
沃羅諾伊圖（Voronoi Diagram）則提供了一種完全不同的方法。在此方法中，地圖中的每個點被分配給最近的障礙物。對於那些與多個障礙物距離相等的點，它們構成了沃羅諾伊圖的邊界，這些邊界理論上表示了遠離任何障礙物的“最安全”的路徑。透過連線這些邊界點，形成了一張反映機器人應該行進的路徑圖。
沃羅諾伊圖的優點在於它傾向於避免靠近障礙物，這在某些應用中非常有用，比如在擁擠的環境中。缺點是，由於這些路徑可能繞過障礙物較遠，所以行進的總距離可能比可檢視法得到的路徑長。此外，沃羅諾伊圖的計算成本也相對較高，尤其是在障礙物多或者空間大的環境中。
單元分解
單元分解是一種完全不同的方案。它將空間中的分為若干的小區域，每一個區域作為一個單元，以單元為頂點、以單元之間的相鄰關係為邊構成一張連通圖。
如果環境較為簡單，可以透過精確的方法如掃描線進行空間劃分。這種方法將每個障礙物精確地對映到單元中，保證了劃分的精確性。如左圖所示，可以明顯看到環境被劃分為多個清晰的單元，每個單元都精確對應環境中的可通行或不可通行區域。
這種方法的優點是精度高、執行效率好，適合於障礙物較少且佈局簡單的環境。然而，它的主要缺點是難以適應複雜多變的環境。在障礙物數量多或形狀複雜的場景中，精確的單元劃分可能導致計算量大增，降低了演算法的執行效率。
更通用的方法是使用柵格地圖進行近似分解，如右圖所示。在這種方法中，整個環境被劃分為規則的格子，每個格子代表一個單元。柵格地圖透過覆蓋整個環境的網格來近似表示障礙物和可通行區域。
柵格化單元分解的優點包括易於實現和適用於多種型別的環境。這種方法簡化了計算過程，使其更加適合動態或複雜的環境。然而，它的缺點也很明顯：解析度依賴於格子的大小，較大的格子可能導致路徑規劃不夠精確，而較小的格子則可能使計算成本急劇增加。此外，近似表示可能導致路徑規劃不夠最優，例如在細長障礙物旁邊可能不會生成最短路徑。
勢場法
假設地圖是一個力場，目標點對機器人產生吸引力，障礙物對機器人產生排斥力，那麼最終力的方向不就是行走的方向嗎？這一次，我們不構建圖，而是構建一個指示機器人行走的勢場。勢場中目標點對機器人產生吸引力，障礙物對機器人產生排斥力，所有力的合成決定機器人的合力大小和方向。當然具體怎麼構建有多種方案，這裡講述一種構建函式。
目標的吸引公式可以這樣設定：
其中，\(r\) 是機器人到目標點的距離，\(k_{\text{attr}}\) 是自己設定的吸引力系數，\(d_{\text{max}}\) 是閾值距離。當距離小於閾值 \(d_{\text{max}}\) 時，吸引力與距離的平方成正比；當距離大於閾值時，吸引力與距離成線性關係。這樣設定可以在機器人遠離目標點時減小吸引力，避免過沖。
障礙物的排斥公式可以這樣設定：
其中，\(r\) 是機器人到最近障礙物的距離，\(k_{\text{repel}}\) 是自己設定的排斥力系數，\(d_{\text{repel}}\) 是排斥力作用的最大距離。當機器人靠近障礙物到一定距離內時，排斥力顯著增加；當超出這個距離時，排斥力消失。
對勢場求梯度得到力，機器人在任何位置上的總力由所有吸引力和排斥力的向量合成得到。總力的方向指示了機器人的行進方向，力的大小也可以使用，用來調節機器人的速度或者加速度什麼的。透過對勢場求梯度，我們可以得到每個位置上的力向量。所以這不僅是一種離線路徑規劃方法，所構建的勢場也構成了機器人的控制律，能夠作為線上實時避障演算法
這種方法的優點是操作直觀且可根據環境動態調整；缺點則包括在某些複雜環境中可能會遇到區域性最小值問題，導致機器人陷入無法達到目標的狀態。為了解決這一問題，可以結合其他導航策略，如路徑平滑或動態重規劃。
機率行車圖（Probabilistic Roadmap, PRM）
我們開可以基於取樣，帶一點隨機來規劃路徑。機率行車圖（PRM）是一種基於取樣的路徑規劃演算法，廣泛用於處理高維自由位形空間中的導航問題。這種方法適用於複雜環境中的路徑規劃，尤其是在機器人和其他自動化系統中非常有效。以下是流程
- 隨機取樣：從自由位形空間中隨機取樣多個點。這些點代表機器人可能佔據的位置。
- 碰撞檢測：對每個取樣點進行碰撞檢測，以確保這些點不與環境中的障礙物重疊。
- 區域性路徑規劃：對於每對取樣點，如果它們之間可以透過不與障礙物相交的直線或曲線相連，則在這兩點之間建立一條邊。
- 構建圖：使用步驟3中的邊和步驟2中的有效取樣點構建一個圖。
- 圖搜尋：使用圖搜尋演算法（如A*或Dijkstra）在圖中找到從起點到終點的最短路徑。
其優點是明顯的簡單好算，並且由於取樣的隨機性和獨立性，PRM特別適用於高維度問題，能有效處理多自由度機器人的路徑規劃。不過其計算質量明顯很依賴取樣，取樣點太少可能無法完全表達自由空間的連通性，而有些時候我們也無法確定是不是充分取樣了。
快速隨機探索樹（Rapidly-exploring Random Tree, RRT）
快速隨機探索樹（RRT）是一種有效的路徑規劃演算法，特別適用於處理具有複雜障礙物和高自由度機器人的空間。RRT不僅在機器人領域得到廣泛應用，也適用於其他多維度空間的探索任務。這種演算法透過快速並隨機地增長搜尋樹來探索空間，從而找到從起點到終點的路徑。
RRT演算法的基本流程：
-
初始化：建立根節點，其位置為起點。
-
迭代增長：
- 隨機取樣：在自由空間內隨機選擇一個點作為目標點（稱為隨機取樣點）。
- 選擇最近節點：在當前樹中找到距離隨機取樣點最近的節點。
- 擴充套件節點：從最近節點向隨機取樣點方向擴充套件一定步長，建立新節點。這一步涉及到移動距離的限制，以確保控制在安全範圍內。
- 碰撞檢測：檢查新節點到最近節點之間的路徑是否與障礙物衝突。如果無碰撞，則將新節點新增到樹中。
-
檢查終點：如果新節點與終點的距離小於預設的閾值，則認為找到了路徑。
-
迴圈迭代：重複步驟2，直到達到終點或達到最大迭代次數。
特點和優勢：
- 高效探索：RRT透過不斷向未探索區域擴充套件來高效地覆蓋空間。
- 適用於複雜環境：RRT能夠有效處理複雜的障礙佈局和高維度問題。
- 靈活性：演算法可以容易地適應不同的環境和需求，例如透過修改步長或增長策略來最佳化效能。
RRT是一種非常強大的工具，尤其適合在未知或動態變化的環境中進行快速路徑規劃。不過它同樣是一個需要臉的演算法。演算法的效能受隨機取樣的影響，有時可能需要較長時間才能找到解，特別是在目標區域較小或障礙物較多的情況下。雙向搜尋是一種提升效率的好辦法，透過同時從起點和終點搜尋，可以顯著減少達到目標所需的總迭代次數。這是因為兩個方向的搜尋可以覆蓋空間的不同區域，使得搜尋樹更快地接近對方。
RRT 演算法顯然只能找到可行路徑，明顯不是最優路徑。RRT* 可以對此做出改進。具體來說：
- 在新增新取樣點之前，我們未必要將其連線到原來的“最近節點”上，可以再一定半徑內自行搜尋，連線到一個最短的節點。
- 新增新點之後，可以遍歷一定範圍內的其他節點，若其他節點連線到自己更優，則更新樹
