移動機(jī)器人學(xué) 課件第七章 移動機(jī)器人路徑規(guī)劃與自主導(dǎo)航

華南理工大學(xué)自動化科學(xué)與工程學(xué)院《移動機(jī)器人學(xué)》

華南理工大學(xué)自動化科學(xué)與工程學(xué)院第七章

移動機(jī)器人路徑規(guī)劃與自主導(dǎo)航

7.1路徑規(guī)劃算法概述路徑規(guī)劃整個過程包括目標(biāo)給定、全局路徑規(guī)劃、局部路徑規(guī)劃和機(jī)器人軌跡跟蹤控制四個部分。7.2基于A*算法全局路徑規(guī)劃全局路徑規(guī)劃算法主要可以分為廣度優(yōu)先搜索算法（Breadth-firstSearch，BFS）、深度優(yōu)先搜索算法（Depth-firstSearch，DFS）、啟發(fā)搜索算法（HeuristicSearch，HS）等。BFS算法和DFS算法都屬于遍歷搜索算法，分別對圖逐層遞歸搜索和不斷向下一層遞歸搜索，通常都需要對全圖進(jìn)行完整的遍歷，計算量大且結(jié)果無法保證最優(yōu)。7.2基于A*算法全局路徑規(guī)劃Dijkstra算法的基本思想是通過逐步擴(kuò)展已知最短路徑的集合，來找出從起點到所有其他節(jié)點的最短路。A*算法是在Dijkstra算法基礎(chǔ)上巧妙地加入了啟發(fā)函數(shù)，是一種典型的啟發(fā)式的搜索算法。A*加入的啟發(fā)函數(shù)大幅降低了需要遍歷的地圖節(jié)點數(shù)量，同時保證了選取的節(jié)點總是在起點到目標(biāo)節(jié)點的最優(yōu)路徑上。7.2基于A*算法全局路徑規(guī)劃A*算法的核心原理是從初始柵格節(jié)點開始搜索鄰域8個柵格節(jié)點，通過具有啟發(fā)函數(shù)的評估函數(shù)F(n)計算這8個節(jié)點的代價數(shù)值，以代價值最小的節(jié)點作為選取點，并基于這個新的柵格節(jié)點繼續(xù)拓展，不斷循環(huán)重復(fù)這個搜索過程，直到搜索到的拓展點為目標(biāo)點或搜索次數(shù)超過閾值則停止路徑搜索。F(n)=G(n)+H(n)其中，G(n)為初始節(jié)點nstart

到當(dāng)前節(jié)點n的最短路徑長度計算函數(shù)；H(n)則為A*算法引入的啟發(fā)函數(shù)，可以估算當(dāng)前節(jié)點n到目標(biāo)節(jié)點nobject

的路徑長度，以此引導(dǎo)路徑的搜索方向。7.2基于A*算法全局路徑規(guī)劃

7.2基于A*算法全局路徑規(guī)劃基于曼哈頓距離的估算函數(shù)、歐氏距離的估算函數(shù)以及切比雪夫距離的估算函數(shù)進(jìn)行仿真，三種估算函數(shù)在同一種障礙柵格圖中的路徑規(guī)劃分別如圖圖7-2A*全局路徑規(guī)劃算法仿真7.3基于DWA局部路徑規(guī)劃基于（DWA）的局部路徑規(guī)劃原理是機(jī)器人在未知或動態(tài)變化的環(huán)境中實現(xiàn)實時路徑規(guī)劃的核心技術(shù)。DWA通過結(jié)合機(jī)器人的動力學(xué)約束和傳感器信息，在動態(tài)變化的環(huán)境中為機(jī)器人規(guī)劃出一條安全、可行的局部路徑。DWA的核心思想是在機(jī)器人的運動學(xué)約束范圍內(nèi)，根據(jù)當(dāng)前的環(huán)境信息和目標(biāo)位置，生成一組可行的速度控制指令，即動態(tài)窗口。這些速度指令反映了機(jī)器人在未來短時間內(nèi)可能的運動軌跡。算法通過評估每個軌跡的潛在碰撞風(fēng)險、目標(biāo)接近程度以及其他優(yōu)化指標(biāo)，選擇出最優(yōu)的速度指令來控制機(jī)器人的運動。7.3基于DWA局部路徑規(guī)劃移動機(jī)器人的直線速度模型如圖

所示，它是機(jī)器人導(dǎo)航與控制中的關(guān)鍵組成部分，描述了機(jī)器人在直線運動過程中速度與加速度、時間以及位置之間的動態(tài)關(guān)系。7.3基于DWA局部路徑規(guī)劃

7.3基于DWA局部路徑規(guī)劃路徑評價的原理主要基于多屬性決策理論，融合了多種評價標(biāo)準(zhǔn)和指標(biāo)，對路徑的優(yōu)劣進(jìn)行量化評估。這些指標(biāo)可能包括路徑的長度、所需時間、安全性、平滑性、能源消耗等，根據(jù)應(yīng)用場景的不同，指標(biāo)的選取和權(quán)重分配也會有所差異。通過構(gòu)建評價模型，將不同指標(biāo)歸一化并加權(quán)求和，可以得到每條路徑的綜合評價得分，進(jìn)而選出最優(yōu)路徑。7.3基于DWA局部路徑規(guī)劃(1)heading(v,ω)方位角評價函數(shù)實驗先固定dist(v,ω)和velocity(v,ω)的權(quán)重為0.5，即β=0.5,γ=0.5，分別取α為0.1、0.5、1.0三個梯度權(quán)重，具體實驗結(jié)果如圖

所示α=0.1

α=0.5

α=1.07.3基于DWA局部路徑規(guī)劃(2)dist(v,ω)障礙物距離評價函數(shù)實驗基于α參數(shù)的實驗結(jié)果，固定heading(v,ω)和velocity(v,ω)的權(quán)重，設(shè)置α=0.1,γ=0.5，分別取β為0.1、1.0、2.0三個梯度權(quán)重，具體實驗結(jié)果如圖所示α=0.1

α=0.5

α=1.07.3基于DWA局部路徑規(guī)劃(3)velocity(v,ω)速度評價函數(shù)實驗采用以上兩個實驗結(jié)果的較優(yōu)參數(shù)，固定heading(v,ω)和dist(v,ω)的權(quán)重，設(shè)置α=0.1,β=0.1，分別取γ為0.05、1.0、4.0、10.0四個梯度權(quán)重，具體實驗結(jié)果如圖所示7.4機(jī)器人路徑規(guī)劃實現(xiàn)在無障礙的環(huán)境中，機(jī)器人可以基于建立好的柵格地圖進(jìn)行全局路徑規(guī)劃，以保證在不會碰到道路兩旁的情況下，實現(xiàn)移動機(jī)器人準(zhǔn)確地駛向目標(biāo)點。起始點

中間點

目標(biāo)點7.4機(jī)器人路徑規(guī)劃實現(xiàn)在無障礙的環(huán)境中，機(jī)器人可以基于建立好的柵格地圖進(jìn)行全局路徑規(guī)劃，以保證在不會碰到道路兩旁的情況下，實現(xiàn)移動機(jī)器人準(zhǔn)確地駛向目標(biāo)點。起始點

中間點

目標(biāo)點7.4機(jī)器人路徑規(guī)劃實現(xiàn)在原先無障礙的地圖中加入臨時的障礙物后，機(jī)器人可以基于A*算法和DWA進(jìn)行全局的路徑規(guī)劃，并在檢測到障礙后立刻調(diào)整新的路線，得到繞開障礙物的路線，以保證移動機(jī)器人不會碰撞到臨時障礙，完成移動機(jī)器人臨時避障任務(wù)。7.4機(jī)器人路徑規(guī)劃實現(xiàn)在原先無障礙的地圖中加入臨時的障礙物后，機(jī)器人可以基于A*算法和DWA進(jìn)行全局的路徑規(guī)劃，并在檢測到障礙后立刻調(diào)整新的路線，得到繞開障礙物的路線，以保證移動機(jī)器人不會碰撞到臨時障礙，完成移動機(jī)器人臨時避障任務(wù)。7.5基于采樣的路徑規(guī)劃算法概率路線圖（ProbabilisticRoadMap,PRM）算法是一種廣泛應(yīng)用于機(jī)器人路徑規(guī)劃領(lǐng)域的基于采樣的方法。算法流程如下：1）采樣。首先，PRM算法在機(jī)器人的運動空間中進(jìn)行隨機(jī)采樣，生成一系列離散的無碰撞樣本點。這些樣本點構(gòu)成了后續(xù)路徑規(guī)劃的基礎(chǔ)。2）碰撞檢測。對于每個采樣點，算法會進(jìn)行碰撞檢測，確保這些點位于自由空間（即無碰撞區(qū)域）內(nèi)。碰撞檢測通常依賴于機(jī)器人的幾何模型和環(huán)境的幾何信息。3）構(gòu)建路線圖。在獲得一系列無碰撞的樣本點后，算法會計算這些點之間的連接關(guān)系。通常，算法會考慮每個樣本點的鄰域內(nèi)的其他樣本點，并嘗試在這些點之間建立連接。如果兩點之間的連線（或稱為邊）不與任何障礙物相交，則這條邊被認(rèn)為是可行的，并被添加到路線圖中。4）路徑搜索。一旦構(gòu)建了完整的路線圖，算法就可以在該圖上進(jìn)行路徑搜索了。這通常使用圖搜索算法（如Dijkstra算法、A*算法等）來實現(xiàn)。算法從起始點開始，搜索通過一系列邊到達(dá)目標(biāo)點的路徑。由于路線圖僅包含無碰撞的點和邊，因此搜索到的路徑也必然是無碰撞的。5）路徑平滑與優(yōu)化。在找到可行路徑后，算法還可以對其進(jìn)行平滑和優(yōu)化，以減小路徑長度或提高平滑度。這可以通過插值、樣條曲線等方法實現(xiàn)。7.5基于采樣的路徑規(guī)劃算法快速擴(kuò)展隨機(jī)樹（Rapidly-exploringRandomTree,RRT）算法是一種基于采樣的路徑規(guī)劃方法，主要用于解決高維空間和復(fù)雜環(huán)境中的機(jī)器人路徑規(guī)劃問題。在RRT算法中，搜索樹由一系列節(jié)點和連接這些節(jié)點的邊組成。每個節(jié)點代表一個無碰撞的狀態(tài)，邊則表示從一個狀態(tài)轉(zhuǎn)移到另一個狀態(tài)的過程。算法從一個初始節(jié)點開始，不斷執(zhí)行以下步驟：1）隨機(jī)采樣。2）尋找最近節(jié)點。3）擴(kuò)展樹。4）碰撞檢測。5）更新搜索樹。6）檢查終止條件。7.5基于采樣的路徑規(guī)劃算法RRT算法主要適用于以下三個場景：1）RRT算法在高維空間中的路徑規(guī)劃問題中表現(xiàn)尤為出色。2）RRT算法適用于復(fù)雜環(huán)境中的路徑規(guī)劃問題。3）RRT算法適用于實時性要求不高的場景。7.5基于采樣的路徑規(guī)劃算法Rapidly-exploringRandomTreeStar(RRT*)算法是一種高效且漸進(jìn)最優(yōu)的路徑規(guī)劃策略。與RRT算法一樣，RRT*也是基于隨機(jī)采樣