路径规划算法
文章目錄
- 前言
- 一、傳統(tǒng)路徑規(guī)劃算法
- 1.Dijkstra算法
- 2.A*算法
- 3.D*算法
- 4.人工勢場法
- 二、基于采樣路徑規(guī)劃算法
- 1.PRM算法
- 2.RRT算法
- 三、智能仿生算法
- 1.神經(jīng)網(wǎng)絡(luò)算法
- 2.蟻群算法
- 3.遺傳算法
前言
隨著機器人技術(shù)、智能控制技術(shù)、硬件傳感器的發(fā)展，機器人在工業(yè)生產(chǎn)、軍事國防以及日常生活等領(lǐng)域得到了廣泛的應(yīng)用。而作為機器人行業(yè)的重要研究領(lǐng)域之一，移動機器人行業(yè)近年來也到了迅速的發(fā)展。移動機器人中的路徑規(guī)劃便是重要的研究方向。移動機器人的路徑規(guī)劃方法主要分為傳統(tǒng)的路徑規(guī)劃算法、基于采樣的路徑規(guī)劃算法、智能仿生算法。傳統(tǒng)的路徑規(guī)劃算法主要有A算法、Dijkstra算法、D算法、人工勢場法，基于采樣的路徑規(guī)劃算法有PRM算法、RRT算法，智能仿生路徑規(guī)劃算法有神經(jīng)網(wǎng)絡(luò)算法、蟻群算法、遺傳算法等。
一、傳統(tǒng)路徑規(guī)劃算法
1.Dijkstra算法
Dijkstra算法是Edsger Wybe Dijkstra在1956年提出的一種用來尋找圖形中結(jié)點之間最短路徑的算法。Dijkstra算法的基本思想是貪心思想，主要特點是以起始點為中心向外層層擴展，直到擴展到目標點為止。Dijkstra算法在擴展的過程中，都是取出未訪問結(jié)點中距離該點距離最小的結(jié)點，然后利用該結(jié)點去更新其他結(jié)點的距離值。
Dijkstra算法流程：
注：該算法不允許圖中存在負權(quán)邊。
優(yōu)點：
1） 如果最優(yōu)路徑存在，那么一定能找到最優(yōu)路徑
缺點：
1） 有權(quán)圖中可能是負邊
2） 擴展的結(jié)點很多，效率低
2.A*算法
A算法發(fā)表于1968年，A算法是將Dijkstra算法與廣度優(yōu)先搜索算法（BFS）二者結(jié)合而成，通過借助啟發(fā)式函數(shù)的作用，能夠使該算法能夠更快的找到最優(yōu)路徑。A算法是靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法。
A算法的啟發(fā)式函數(shù)：f(n)=g(n)+h(n)
f(n)表示結(jié)點的綜合優(yōu)先級，在選擇結(jié)點時考慮該結(jié)點的綜合優(yōu)先級；
g(n)表示起始點到當前結(jié)點的代價值；
h(n)表示當前結(jié)點到目標點的代價估計值，啟發(fā)式函數(shù)。
當h(n)趨近于0時，此時算法退化為Dijkstra算法，路徑一定能找到，但速度比較慢；當g(n)趨近于0時，算法退化為BFS算法，不能保證一定找到路徑，但速度特別快。我們可以通過調(diào)節(jié)h(n)的大小來調(diào)整算法的精度與速度。
在A*算法中，采用最多的是歐幾里得距離，即dist = srqt((y2-y1)2+(x2-x1)2)。
A*算法流程：
1. 將起始點s加入到開啟列表openlist中 2. 重復(fù)以下過程： a) 遍歷開啟列表openlist，尋找F值最小的結(jié)點，并將其作為當前要處理的結(jié)點 b) 將要處理的結(jié)點移到關(guān)閉列表closelist c) 對當前結(jié)點的8個相鄰結(jié)點的每個結(jié)點： i. 如果他是不可抵達的或者已經(jīng)在關(guān)閉列表closelist中，忽略； ii. 如果他不在開啟列表openlist中，將其加入openlist，并把當前結(jié)點設(shè)置為其父節(jié)點，記錄當前結(jié)點的F、G、H值； iii. 如果他已經(jīng)在開啟列表openlist中，檢查這條路徑（即經(jīng)由當前結(jié)點到達相鄰結(jié)點）是否更好，用G值做參考，更小的G值表示這個更好的路徑，如果是這樣，將其父節(jié)點設(shè)置為當前結(jié)點，并重新計算他的G值和F值，如果開啟列表openlist是按F值進行排序，改變后需要重新排序。 d) 停止，當 i. 終點加入到了開啟列表openlist中，此時路徑已經(jīng)找到 ii. 查找重點失敗，并且開啟列表openlist中是空的，此時沒有路徑 3. 保存路徑，從終點開始，每個結(jié)點沿著其父節(jié)點移動直到起點。
優(yōu)點：
1） 利用啟發(fā)式函數(shù)，搜索范圍小，提高了搜索效率
2） 如果最優(yōu)路徑存在，那么一定能找到最優(yōu)路徑
缺點：
1） A算法不適用于動態(tài)環(huán)境
2） A算法不太適合于高維空間，計算量大
3） 目標點不可達時會造成大量性能消耗
3.D*算法
D算法是卡耐基梅隆機器人中心的Stentz在1994年提出的主要用于機器人探路，并且美國火星探測器上就是應(yīng)用的D路徑規(guī)劃算法。A算法適用于在靜態(tài)路網(wǎng)中尋路，在環(huán)境變化后，往往需要replan，由于A不能有效利用上次計算的信息，故計算效率較低。D算法由于儲存了空間中每個點到終點的最短路徑信息，故在重規(guī)劃時效率大大提升。A是正向搜索，而D特點是反向搜索，即從目標點開始搜索過程。在初次遍歷時候，與Dijkstra算法一致，它將每個節(jié)點的信息都保存下來。
D*算法流程：
優(yōu)點：
1）適用于動態(tài)環(huán)境的路徑規(guī)劃，搜索效率高
缺點：
1） 不適用于高維空間，計算量大
2） 不太適用于在距離較遠的最短路徑上發(fā)生變化的場景
4.人工勢場法
人工勢場法是由Khatib在1985年提出的一種基于虛擬力場的局部路徑規(guī)劃算法，該算法的基本思想是當機器人在周圍環(huán)境中運動時，將環(huán)境設(shè)計成一種抽象的人造引力場，目標點對移動機器人產(chǎn)生“引力”，障礙物對移動機器人產(chǎn)生“斥力”，最后通過二者的合力來控制移動機器人的運動。應(yīng)用人工勢場法規(guī)劃出來的路徑一般是比較平滑并且安全的，但是存在著局部最優(yōu)的問題。
利用勢場函數(shù)U來建立人工勢場，勢場函數(shù)是一種可微函數(shù)，空間中某點處勢場函數(shù)值的大小，代表了該點的勢場強度。我們采用引力勢場函數(shù)與斥力勢場函數(shù)，用U（q）表示二者之和。
引力勢場函數(shù)：
e表示引力增益，p(q,qgoal)表示當前點到目標點的距離；
斥力勢場函數(shù)：
n表示斥力增益，p(q,qgoal)表示當前點到障礙物的距離，p0表示障礙物作用距離閾值。
優(yōu)點：
1） 規(guī)劃出的路徑一般是比較平滑且安全
2） 人工勢場法是一種反饋控制策略，具有一定的魯棒性
缺點：
1） 容易陷入局部最優(yōu)的問題
2） 距離目標點較遠時，引力特別大，斥力相對較小，可能會發(fā)生碰撞
3） 當目標點附近有障礙物時，斥力非常大，引力較小，很難到達目標點
二、基于采樣路徑規(guī)劃算法
1.PRM算法
隨機路線圖（PRM）算法是一種基于圖搜索的算法，可以將連續(xù)狀態(tài)空間轉(zhuǎn)換成離散狀態(tài)空間，在利用A＊等搜索算法在路線圖上尋找路徑，提高搜索效率。PRM算法是概率完備且不是最優(yōu)的算法。
PRM算法流程：
優(yōu)點：
1） 適用于高維空間和復(fù)雜約束的路徑規(guī)劃問題
2） 搜索效率高，搜索速度快
缺點：
1）概率完備但不是最優(yōu)
2.RRT算法
RRT算法是適用于高維空間，通過對狀態(tài)空間中的采樣點進行碰撞檢測，避免了對空間的建模，較好的處理帶有非完整約束的路徑規(guī)劃問題，有效的解決了高維空間和復(fù)雜約束的路徑規(guī)劃問題。該算法是概率完備但不是最優(yōu)的算法。
RRT算法以初始點qinit作為根節(jié)點，通過隨機采樣增加葉子節(jié)點的方式，生成一個隨機擴展樹，當目標點位于隨機擴展樹上時，能夠找到一天初始點到目標點的路徑。首先，需要從狀態(tài)空間中隨機選擇一個采樣點qrand，然后從隨機樹中選擇一個距離qrand最近的結(jié)點qnearest，從qnearest向qrand擴展一個步長的距離，得到一個新的結(jié)點qnew，如果qnew與障礙物發(fā)生碰撞，則返回空；否則，將qnew加入到隨機樹中，重復(fù)上述步驟直到qnearest和qgoal距離小于一個閾值。
優(yōu)點：
1） 搜索效率比較高，搜索速度比較快
2） 適用于高維空間，不會產(chǎn)生維度災(zāi)難的問題
3） 只需對狀態(tài)空間采樣點進行碰撞檢測，避免了對空間的建模
缺點：
1） 規(guī)劃出的路徑質(zhì)量一般，可能存在棱角、不夠光滑
2） RRT算法不太適用于存在狹長空間的環(huán)境
3） 規(guī)劃出的路徑可能不是最優(yōu)路徑
4） 不適用于動態(tài)環(huán)境的路徑規(guī)劃
三、智能仿生算法
1.神經(jīng)網(wǎng)絡(luò)算法
隨著機器人自主性的不斷提高，使其具有環(huán)境感知以及環(huán)境學(xué)習(xí)的能力，許多學(xué)者提出了深度強化學(xué)習(xí)算法來解決移動機器人處于動態(tài)復(fù)雜環(huán)境中路徑規(guī)劃的問題。深度強化學(xué)習(xí)算法充分利用了深度學(xué)習(xí)的感知能力與強化學(xué)習(xí)的決策能力，通過機器人與環(huán)境的交互過程不斷試錯，通過環(huán)境評價性的反饋，實現(xiàn)系統(tǒng)更加智能的決策控制，幫助移動機器人在某些復(fù)雜未知的環(huán)境中實現(xiàn)一定程度的自主化與智能化。
路徑規(guī)劃就是一個標準的MDP問題，強化學(xué)習(xí)可以通過值迭代等方法建立一個表格，用以存儲狀態(tài)s到動作a的映射。但是在復(fù)雜環(huán)境中會產(chǎn)生維度災(zāi)難的問題，因此神經(jīng)網(wǎng)絡(luò)可以解決維度災(zāi)難的問題。以DQN算法為例，來講解神經(jīng)網(wǎng)絡(luò)算法在路徑規(guī)劃中的應(yīng)用。DQN算法是Q-Learning算法與卷積神經(jīng)網(wǎng)絡(luò)（CNN）二者進行結(jié)合。DQN算法是由兩個網(wǎng)絡(luò)結(jié)構(gòu)、初始參數(shù)相同的結(jié)構(gòu)組成，一個是估計網(wǎng)絡(luò)，用來輸出估計值函數(shù)；另一個是目標網(wǎng)絡(luò)，用來輸出目標值函數(shù)。通過強化學(xué)習(xí)使機器人與環(huán)境進行交互得到樣本（s,a,r,s’），將所有的樣本集放入到經(jīng)驗回放池中。神經(jīng)網(wǎng)絡(luò)進行訓(xùn)練時，隨機的從經(jīng)驗回放池中抽取batchsz數(shù)量的樣本，將樣本輸入進神經(jīng)網(wǎng)絡(luò)，利用神經(jīng)網(wǎng)絡(luò)的非線性擬合能力，擬合出非線性函數(shù)來表達我們的Q值，利用e-greedy策略來進行選擇智能體的動作。智能體執(zhí)行完相應(yīng)的動作之后，環(huán)境會反饋一個狀態(tài)和獎勵值，最后經(jīng)過神經(jīng)網(wǎng)絡(luò)模型的訓(xùn)練和優(yōu)化得到網(wǎng)絡(luò)的訓(xùn)練參數(shù)，得到相對準確的動作輸出。
其中w表示估計網(wǎng)絡(luò)的參數(shù)， w-表示目標網(wǎng)絡(luò)的參數(shù)。
優(yōu)點：
1） 適合于動態(tài)復(fù)雜環(huán)境
2） 適用于高維空間，避免維度災(zāi)難的問題
缺點：
1） 對硬件的要求比較高
2） 參數(shù)調(diào)節(jié)比較困難
2.蟻群算法
蟻群算法（Ant Colony Optimization，ACO）是Dorigo在1992年提出的一種用來尋找優(yōu)化路徑的概率型算法，是由一群無智能或有輕微智能的個體通過相互寫作而表現(xiàn)出智能行為，為求解復(fù)雜問題提供了一個新的可能性。該算法的主要思想是螞蟻在尋找食物過程中發(fā)現(xiàn)路徑的行為。該算法具有分布計算、信息正反饋和啟發(fā)式搜索的特征，本質(zhì)上是進化算法中的一種啟發(fā)式全局優(yōu)化算法。
蟻群算法的基本原理是利用螞蟻在覓食過程中會釋放信息素。
影響蟻群算法的因素：
1） 信息素如何撒播
2） 信息素如何揮發(fā)
3） 以何種方式讓螞蟻選擇運動方向，減少盲目性和不必要性
4） 給予螞蟻和環(huán)境一定的記憶能力能夠幫助減少搜索空間
個體分布越均勻，種群多樣性就越好，得到全局最優(yōu)解的概率就越大，但是尋優(yōu)時間就越長；個體分布越集中，種群多樣性就越差，不利于發(fā)揮算法的探索能力。正反饋加快了蟻群算法的收斂速度，卻使算法較早地集中于部分候選解，因此正反饋降低了種群的多樣性，也不利于提高算法的全局尋優(yōu)能力。
優(yōu)點：
1） 蟻群算法的魯棒性強
2） 采用正反饋機制，能夠逼近最優(yōu)解
3） 易與其他算法進行結(jié)合
缺點：
1） 盲目的隨機搜索，搜索時間較長，搜索速度慢
2） 蟻群算法容易陷入局部最優(yōu)
3） 蟻群算法參數(shù)的選擇依賴于經(jīng)驗或試錯
3.遺傳算法
遺傳算法（Genetic Algorithm，GA）是由John Holland于20世紀70年代提出，該算法是根據(jù)大自然中生物體進化規(guī)律而設(shè)計提出的，來模擬達爾文生物進化論的自然選擇和遺傳學(xué)機理的生物進化過程的計算模型，是一種通過模擬自然進化過程搜索最優(yōu)解的方法。其本質(zhì)是一種高效、并行、全局搜索的方法，能在搜索過程中自動獲取和積累有關(guān)搜索空間的知識，并自適應(yīng)地控制搜索過程以求得最佳解。
遺傳算法的流程：
優(yōu)點：
1） 遺傳算法具有廣泛的應(yīng)用領(lǐng)域
2） 遺傳算法具有群體搜索的特性
3） 遺傳算法基于概率規(guī)則，搜索更為靈活
4） 遺傳算法直接以目標函數(shù)作為搜索信息，不涉及目標函數(shù)值求微分的過程
缺點：
1） 遺傳算法效率比較低
2） 遺傳算法容易過早收斂
3） 遺傳算法在編碼時容易出現(xiàn)不規(guī)范不準確的問題
更多內(nèi)容請關(guān)注微信公眾號：深度學(xué)習(xí)與路徑規(guī)劃
總結(jié)
- 上一篇： 矩阵的秩（Rank）
- 下一篇： 【ExtJs】Extjs RowNumb