自主移動(dòng)機(jī)器人教學(xué)課件第4章 導(dǎo)航規(guī)劃 1 概述及路徑規(guī)劃

第四章移動(dòng)機(jī)器人導(dǎo)航規(guī)劃熊蓉浙江大學(xué)控制科學(xué)與工程學(xué)院4.1概述導(dǎo)航規(guī)劃?rùn)C(jī)器人地圖目標(biāo)規(guī)劃定位地圖構(gòu)建導(dǎo)航規(guī)劃執(zhí)行在給定環(huán)境的全局或局部知識(shí)以及一個(gè)或者一系列目標(biāo)位置的條件下，使機(jī)器人能夠根據(jù)知識(shí)和傳感器感知信息高效可靠地到達(dá)目標(biāo)位置感知導(dǎo)航方式固定路徑導(dǎo)引：有人工標(biāo)識(shí)導(dǎo)引無(wú)軌導(dǎo)航：有人工標(biāo)識(shí)導(dǎo)引的無(wú)固定路徑（無(wú)軌）導(dǎo)航無(wú)標(biāo)識(shí)導(dǎo)引的自然無(wú)軌導(dǎo)航方式1：有人工標(biāo)識(shí)導(dǎo)引的固定路徑導(dǎo)航磁條導(dǎo)航磁感應(yīng)線導(dǎo)航磁釘導(dǎo)航二維碼導(dǎo)航優(yōu)點(diǎn)：技術(shù)成熟、穩(wěn)定可靠、價(jià)格優(yōu)惠缺點(diǎn)：需要施工和維護(hù)、路線無(wú)法調(diào)整AGV：Automatic

GuidedVehicle自動(dòng)導(dǎo)引車方式2：有人工標(biāo)識(shí)導(dǎo)引的無(wú)軌導(dǎo)航激光反射板導(dǎo)航優(yōu)點(diǎn)：技術(shù)成熟、路徑可調(diào)缺點(diǎn)：需要施工和維護(hù)、價(jià)格昂貴方式3：無(wú)人工標(biāo)識(shí)導(dǎo)引的無(wú)軌導(dǎo)航自然導(dǎo)航優(yōu)點(diǎn)：無(wú)需施工、路徑可調(diào)、精確定位、室內(nèi)外通用缺點(diǎn)：算法復(fù)雜，環(huán)境變化影響定位可靠性和穩(wěn)定性導(dǎo)航規(guī)劃問(wèn)題分解路徑規(guī)劃：避障規(guī)劃：軌跡生成：根據(jù)所給定的地圖和目標(biāo)位置，規(guī)劃一條使機(jī)器人到達(dá)目標(biāo)位置的路徑（只考慮工作空間的幾何約束，不考慮機(jī)器人的運(yùn)動(dòng)學(xué)模型和約束）根據(jù)所得到的實(shí)時(shí)傳感器測(cè)量信息，調(diào)整路徑/軌跡以避免發(fā)生碰撞根據(jù)機(jī)器人的運(yùn)動(dòng)學(xué)模型和約束，尋找適當(dāng)?shù)目刂泼?，將可行路徑轉(zhuǎn)化為可行軌跡。路徑規(guī)劃、避障規(guī)劃、軌跡規(guī)劃三者關(guān)系路徑規(guī)劃根據(jù)所給定的地圖和目標(biāo)位置，規(guī)劃一條使機(jī)器人到達(dá)目標(biāo)位置的路徑避障規(guī)劃根據(jù)所得實(shí)時(shí)傳感器測(cè)量信息，調(diào)整軌跡以避免發(fā)生碰撞戰(zhàn)略方法戰(zhàn)術(shù)方法互補(bǔ)軌跡規(guī)劃根據(jù)機(jī)器人的運(yùn)動(dòng)學(xué)模型和約束，尋找適當(dāng)?shù)目刂泼?，將可行路徑轉(zhuǎn)化為可行軌跡機(jī)器人執(zhí)行互補(bǔ)4.2路徑規(guī)劃

4.2.1基本概念路徑規(guī)劃根據(jù)所給定的地圖和目標(biāo)位置，規(guī)劃一條使機(jī)器人到達(dá)目標(biāo)位置的路徑通常簡(jiǎn)化為只考慮工作空間的幾何約束，不考慮機(jī)器人的運(yùn)動(dòng)學(xué)模型和約束工作空間與位形空間(C-Space)工作空間：移動(dòng)機(jī)器人上的參考點(diǎn)能達(dá)到的空間集合，機(jī)器人采用位置和姿態(tài)描述，并需考慮體積位形空間：機(jī)器人成為一個(gè)可移動(dòng)點(diǎn)，不考慮姿態(tài)、體積和非完整運(yùn)動(dòng)學(xué)約束障礙物按機(jī)器人半徑進(jìn)行膨脹忽略非完整約束對(duì)姿態(tài)的限制位形空間障礙物空間：不可行的位形集合在該空間中，機(jī)器人會(huì)與障礙物發(fā)生碰撞自由空間：可行的位形集合在該空間中，機(jī)器人將無(wú)碰地安全移動(dòng)路徑規(guī)劃就是在自由位形空間中為機(jī)器人尋找一條路徑，使其從初始位置運(yùn)行到目標(biāo)位置路徑規(guī)劃方法需要具備完備性完備性：當(dāng)解存在時(shí)，能夠在有限的時(shí)間內(nèi)找到解路徑規(guī)劃算法挑戰(zhàn)：在連續(xù)空間內(nèi)搜索，難以保證時(shí)間確保完備性的方法基本思路：對(duì)空間作離散化分辨率完備：解析性離散化，確保獲得可行解行車圖法：基于障礙物幾何形狀分解姿態(tài)空間單元分解法：區(qū)分空閑單元和被占單元?jiǎng)輬?chǎng)法：

根據(jù)障礙物和目標(biāo)對(duì)空間各點(diǎn)施加虛擬力概率完備:基于概率進(jìn)行隨機(jī)采樣離散化，使獲得解的概率趨近于1PRM（ProbabilisticRoadMaps）RRT（Rapid-ExploringRadomTrees）連通圖中搜索最優(yōu)路徑的方法精確最優(yōu)搜索法：深度優(yōu)先法、寬度優(yōu)先法近似最優(yōu)搜索法啟發(fā)式搜索法：A*，D*準(zhǔn)啟發(fā)式搜索算法：退火、進(jìn)化和蟻群優(yōu)化等4.2.2分辨率完備的路徑規(guī)劃方法1.行車圖法基本思想：基于障礙物幾何形狀分解位形空間，將自由空間的連通性用一維曲線的網(wǎng)格表示，在加入起始點(diǎn)和目標(biāo)點(diǎn)后，在該一維無(wú)向連通圖中尋找一條無(wú)碰路徑構(gòu)建行車圖的典型方法：可視圖（Visibilitygraph）Voronoidiagram1.1可視圖法可視圖由所有連接可見(jiàn)頂點(diǎn)對(duì)的邊組成可見(jiàn)指頂點(diǎn)之間無(wú)障礙物初始位置和目標(biāo)位置也作為頂點(diǎn)1.1可視圖法優(yōu)點(diǎn)：非常簡(jiǎn)單，特別是當(dāng)環(huán)境地圖用多邊形描述物體時(shí)可得到在路徑長(zhǎng)度上最優(yōu)的解缺點(diǎn)：所得路徑過(guò)于靠近障礙物，不夠安全。常用的解決方法：以遠(yuǎn)大于機(jī)器人半徑的尺寸膨脹障礙物，但容易造成可行路徑的消失在路徑規(guī)劃后修改所得路徑，使其與障礙物保持一定的距離1.2

Voronoidiagram基本思想：取障礙物之間的中間點(diǎn)，以最大化機(jī)器人和障礙物之間的距離1.2

Voronoidiagram構(gòu)建方法：對(duì)于自由空間中的每一點(diǎn)，計(jì)算它到最近障礙物的距離；在垂直于二維空間平面的軸上用高度表示該點(diǎn)到障礙物的距離，類似于畫(huà)直方圖；當(dāng)某個(gè)點(diǎn)到兩個(gè)或多個(gè)障

礙物距離相等時(shí)，其距離

點(diǎn)處出現(xiàn)尖峰，Voronoi

diagram就由連接這些

尖峰點(diǎn)的邊組成。1.2

Voronoidiagram優(yōu)點(diǎn)：安全性高缺點(diǎn)：計(jì)算復(fù)雜、路徑長(zhǎng)度較可視圖法長(zhǎng)、不適用于短距離定位傳感器2.單元分解法基本思想首先，將位形空間中的自由空間分為若干的小區(qū)域，每一個(gè)區(qū)域作為一個(gè)單元，以單元為頂點(diǎn)、以單元之間的相鄰關(guān)系為邊構(gòu)成一張連通圖；其次，在連通圖中尋找包含初始姿態(tài)和目標(biāo)姿態(tài)的單元，搜索連接初始單元和目標(biāo)單元的路徑；最后，根據(jù)所得路徑的單元序列生成單元內(nèi)部的路徑主要方法精確單元分解近似單元分解2.1精確單元分解單元邊界嚴(yán)格基于環(huán)境幾何形狀分解，所得單元完全空閑2.1精確單元分解優(yōu)點(diǎn)：機(jī)器人不需要考慮它在每個(gè)空閑單元中的具體位置，只需要考慮如何從一個(gè)單元移動(dòng)到相鄰的空閑單元單元數(shù)與環(huán)境大小無(wú)關(guān)缺點(diǎn)：計(jì)算效率極大地依賴于環(huán)境中物體的復(fù)雜度2.2

近似單元分解柵格表示法，將環(huán)境分解成若干個(gè)大小相同的柵格并不是每個(gè)單元都是完全被占或者完全空閑的，因此分解后的單元集合是對(duì)實(shí)際地圖的一種近似2.2近似單元分解優(yōu)點(diǎn)非常簡(jiǎn)單，與環(huán)境的疏密和物體形狀的復(fù)雜度無(wú)關(guān)缺點(diǎn)：對(duì)存儲(chǔ)空間有要求可變大小的近似單元分解四叉樹(shù)表示法：遞歸地把環(huán)境分為4個(gè)大小相等的子區(qū)域。直到每個(gè)區(qū)域中所包含的基本元素全為0或全為1。3.人工勢(shì)場(chǎng)法基本思想：目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生吸引力，障礙物對(duì)機(jī)器人產(chǎn)生排斥力所有力的合成構(gòu)成機(jī)器人的控制律3.

人工勢(shì)場(chǎng)法步驟1：構(gòu)建人工勢(shì)場(chǎng)(ArtificialPotentialField)目標(biāo)點(diǎn)：吸引勢(shì)場(chǎng)人工勢(shì)場(chǎng)法步驟1：構(gòu)建人工勢(shì)場(chǎng)(ArtificialPotentialField)目標(biāo)點(diǎn)：吸引勢(shì)場(chǎng)障礙物：推斥勢(shì)場(chǎng)被評(píng)估點(diǎn)和障礙物點(diǎn)之間的距離預(yù)定義距離閾值3.人工勢(shì)場(chǎng)法步驟2：根據(jù)人工勢(shì)場(chǎng)計(jì)算力對(duì)勢(shì)場(chǎng)求偏導(dǎo)數(shù)3.人工勢(shì)場(chǎng)法步驟3：計(jì)算合力，并進(jìn)而由力計(jì)算得到控制律力的方向就是機(jī)器人運(yùn)動(dòng)方向，大小可以對(duì)應(yīng)加速度控制3.人工勢(shì)場(chǎng)法機(jī)器人是受人工勢(shì)場(chǎng)影響的一個(gè)點(diǎn)，沿著勢(shì)場(chǎng)方向就可以避開(kāi)障礙物達(dá)到目標(biāo)點(diǎn)3.

人工勢(shì)場(chǎng)法不僅是一種路徑規(guī)劃方法，所構(gòu)建的勢(shì)場(chǎng)也構(gòu)成了機(jī)器人的控制律，能夠較好地適應(yīng)目標(biāo)的變化和環(huán)境中的動(dòng)態(tài)障礙物，可以作為實(shí)時(shí)避障算法3.

人工勢(shì)場(chǎng)法缺點(diǎn)：存在局部最小，容易產(chǎn)生振蕩和死鎖4.2.概率完備的連通圖構(gòu)建1.

PRM(ProbabilisticRoadmap)基本思想：通過(guò)隨機(jī)采樣和碰撞檢測(cè)找到自由位形空間中的路徑點(diǎn)和無(wú)碰路徑，構(gòu)建連通圖1.

PRM(ProbabilisticRoadmap)在位形空間坐標(biāo)系中隨機(jī)取點(diǎn)1.

PRM(ProbabilisticRoadmap)在位形空間坐標(biāo)系中隨機(jī)取點(diǎn)1.

PRM(ProbabilisticRoadmap)對(duì)采樣的姿態(tài)進(jìn)行碰撞檢測(cè)1.

PRM(ProbabilisticRoadmap)無(wú)碰撞姿態(tài)成為圖節(jié)點(diǎn)1.

PRM(ProbabilisticRoadmap)每個(gè)圖節(jié)點(diǎn)和其最近相鄰的k個(gè)節(jié)點(diǎn)直線連接1.

PRM(ProbabilisticRoadmap)保留無(wú)碰路徑為圖的邊PRM(ProbabilisticRoadmap)構(gòu)成自由位形空間中的Roadmap1.

PRM(ProbabilisticRoadmap)加入起始點(diǎn)和終止點(diǎn)1.

PRM(ProbabilisticRoadmap)在PRM中搜索一條從起始點(diǎn)到終止點(diǎn)的路徑PRM優(yōu)點(diǎn)：簡(jiǎn)化了對(duì)環(huán)境的解析計(jì)算，可以快速構(gòu)建得到行車圖適用于高維度自由位形空間中的規(guī)劃是一個(gè)近似完備的路徑規(guī)劃方法缺點(diǎn)：對(duì)自由空間連通性表達(dá)的完整性依賴于采樣次數(shù)從算法通用性上來(lái)講難以評(píng)估需要多少時(shí)間做充分采樣不考慮機(jī)器人執(zhí)行的可行性2.

RRT(Rapid-ExploringRandomTree)1998年由美國(guó)愛(ài)荷華州立大學(xué)StevenM.Lavalle教授提出基本思想：連通圖采用樹(shù)的形式，以起始點(diǎn)作為樹(shù)的根節(jié)點(diǎn)采用在空間中隨機(jī)采樣、連接樹(shù)中最近節(jié)點(diǎn)的方式拓展樹(shù)考慮機(jī)器人的運(yùn)動(dòng)執(zhí)行能力通過(guò)樹(shù)結(jié)構(gòu)可以直接回溯得到路徑2.

RRT(Rapid-ExploringRandomTree)2.

RRT(Rapid-ExploringRandomTree)影響規(guī)劃收斂速度的三個(gè)步驟隨機(jī)狀態(tài)的采樣在搜索樹(shù)中查找與隨機(jī)狀態(tài)距離最近的節(jié)點(diǎn)新生成節(jié)點(diǎn)的防碰檢測(cè)RRT改進(jìn)1針對(duì)問(wèn)題：RRT擴(kuò)張偏向狀態(tài)空間未探測(cè)部分，但不是偏向目標(biāo)點(diǎn)，當(dāng)環(huán)境復(fù)雜時(shí)計(jì)算效率低雙向搜索Bidirectional-RRTRRT改進(jìn)2針對(duì)問(wèn)題：隨機(jī)性小步擴(kuò)展導(dǎo)致路徑曲折，成本高RRT*：為實(shí)現(xiàn)漸近最優(yōu)，考慮路徑成本尋找樹(shù)中新節(jié)點(diǎn)鄰域內(nèi)到新節(jié)點(diǎn)路徑最短的節(jié)點(diǎn)，建立連接，加入樹(shù)集合對(duì)樹(shù)中新節(jié)點(diǎn)鄰域內(nèi)節(jié)點(diǎn)進(jìn)行判斷，如果從新節(jié)點(diǎn)到該節(jié)點(diǎn)形成的路徑優(yōu)于現(xiàn)有樹(shù)中路徑，則將該節(jié)點(diǎn)父節(jié)點(diǎn)修改為新節(jié)點(diǎn)RRT*RRT*4.2.4最優(yōu)路徑搜索算法最優(yōu)路徑搜索算法在構(gòu)建形成的連通圖中搜索最優(yōu)路徑322333434EBCDFAG最優(yōu)路徑搜索算法精確算法：生成精確的最優(yōu)解深度優(yōu)先法、廣度優(yōu)先法遍歷獲得所有路徑后選擇最優(yōu)解優(yōu)化：優(yōu)先級(jí)定義的廣度優(yōu)先法、Dijstra算法存在問(wèn)題：耗時(shí)，難以滿足機(jī)器人在線快速規(guī)劃要求近似算法啟發(fā)式搜索算法：A*,D*,FocusedD*等準(zhǔn)啟發(fā)式搜索算法：退火、進(jìn)化和蟻群優(yōu)化等深度優(yōu)先法從起始節(jié)點(diǎn)開(kāi)始按深度方式依次探索節(jié)點(diǎn)未被訪問(wèn)過(guò)的相鄰節(jié)點(diǎn)在一個(gè)相鄰節(jié)點(diǎn)被訪問(wèn)后，優(yōu)先訪問(wèn)該節(jié)點(diǎn)下一個(gè)未被訪問(wèn)相鄰節(jié)點(diǎn)，直到擴(kuò)展到圖的最深層，即沒(méi)有可訪問(wèn)的后繼相鄰節(jié)點(diǎn)，或者到達(dá)目標(biāo)節(jié)點(diǎn)返回上一層，探索該節(jié)點(diǎn)的其他未被訪問(wèn)相鄰節(jié)點(diǎn)322333434EBCDFAG深度優(yōu)先法322333434EBCDFAGABCDG路徑長(zhǎng)11322333434EBCDFAG深度優(yōu)先法ABCDG路徑長(zhǎng)11ABCFG路徑長(zhǎng)10322333434EBCDFAG深度優(yōu)先法ABCDG路徑長(zhǎng)11ABCFG路徑長(zhǎng)10ABCEFG路徑BCDFAG深度優(yōu)先法ABCDG路徑長(zhǎng)11ABCFG路徑長(zhǎng)10AECFG路徑長(zhǎng)13ABCEFG路徑長(zhǎng)14AECDG路徑長(zhǎng)14AEFCDG路徑長(zhǎng)16AEFG路徑長(zhǎng)9問(wèn)題：可能會(huì)重新訪問(wèn)已經(jīng)訪問(wèn)過(guò)的節(jié)點(diǎn)，或者進(jìn)入冗余路徑

優(yōu)先級(jí)定義的廣度優(yōu)先法從起始節(jié)點(diǎn)開(kāi)始訪問(wèn)該節(jié)點(diǎn)所有未曾訪問(wèn)的相鄰節(jié)點(diǎn)然后分別從這些相鄰節(jié)點(diǎn)出發(fā)依次訪問(wèn)它們的相鄰節(jié)點(diǎn)，訪問(wèn)優(yōu)先級(jí)為先被訪問(wèn)的節(jié)點(diǎn)的相鄰節(jié)點(diǎn)優(yōu)先于后被訪問(wèn)的節(jié)點(diǎn)的相鄰節(jié)點(diǎn)，直到所有被訪問(wèn)節(jié)點(diǎn)的相鄰節(jié)點(diǎn)都被訪問(wèn)到322333434EBCDFAG322333434EBCDFAG優(yōu)先級(jí)定義的廣度優(yōu)先法322333434EBCDFAG優(yōu)先級(jí)定義的廣度優(yōu)先法322333434EBCDFAG優(yōu)先級(jí)定義的廣度優(yōu)先法AEFG路徑長(zhǎng)9322333434EBCDFAG優(yōu)先級(jí)定義的廣度優(yōu)先法ABCDG路徑長(zhǎng)11AEFG路徑長(zhǎng)9由于廣度優(yōu)先法定義路訪問(wèn)優(yōu)先級(jí)，因此不會(huì)遍歷所有路徑，但可能會(huì)導(dǎo)致搜索不到最短路徑。

Dijkstra算法采用優(yōu)先級(jí)定義的廣度優(yōu)先搜索思想在有向圖中以起始點(diǎn)為中心按路徑長(zhǎng)度遞增方式層層向外擴(kuò)展，直到擴(kuò)展到終止點(diǎn)設(shè)置兩個(gè)集合S和T，S存放已經(jīng)找到最短路徑的頂點(diǎn)，T是尚未確定路徑的頂點(diǎn)集合，同時(shí)也描述了起始點(diǎn)經(jīng)過(guò)集合S中頂點(diǎn)到該點(diǎn)的最短路徑及長(zhǎng)度。每次更新時(shí)，從T中找出路徑最短的點(diǎn)加入到集合S，T中頂點(diǎn)最短路徑及長(zhǎng)度則根據(jù)加入點(diǎn)作為中間點(diǎn)后起始點(diǎn)到該點(diǎn)距離是否減小來(lái)決定是否更新Dijkstra算法

322333434EBCDFAGDijkstra算法

……問(wèn)題規(guī)模大時(shí)搜索耗時(shí)322333434EBCDFAG啟發(fā)式搜索算法：A*基于優(yōu)先級(jí)定義的廣度優(yōu)先搜索根據(jù)啟發(fā)式評(píng)估函數(shù)在連通圖中尋找最優(yōu)路徑當(dāng)選擇下一個(gè)探索結(jié)點(diǎn)時(shí)，通過(guò)啟發(fā)式評(píng)估函數(shù)進(jìn)行評(píng)估，選擇路徑代價(jià)最小的結(jié)點(diǎn)作為下一步探索結(jié)點(diǎn)而跳轉(zhuǎn)其上評(píng)估函數(shù)n表示節(jié)點(diǎn)g(n)表示從起始點(diǎn)到節(jié)點(diǎn)的實(shí)際代價(jià)h(n)為從節(jié)點(diǎn)到目標(biāo)點(diǎn)的最佳路徑的估計(jì)代價(jià)322333434EBCDFAGh(E)=5h(D)=4h(F)=3h(B)=5h(C)=3AEBf(B)=7f(E)=8AEBf(B)=7f(E)=8Cf(C)=7322333434EBCDFAGh(E)=5h(D)=4h(F)=3h(B)=5h(C)=3AEBf(E)=8CFDf(D)=11f(F)=10322333434EBCDFAGh(E)=5h(D)=4h(F)=3h(B)=5h(C)=3AEBf(E)=8CFDf(D)=11f(F)=10FCf(C)=10f(F)=9322333434EBCDFAGh(E)=5h(D)=4h(F)=3h(B)=5h(C)=3AEBCFDf(D)=11f(F)=10FCf(C)=10f(F)=9322333434EBCDFAGh(E)=5h(D)=4h(F)=3h(B)=5h(C)=3AEBCDf(D)=11Ff(F)=9G322333434EBCDFAGh(E)=5h(D)=4h(F)=3h(B)=5h(C)=3AEBCDf(D)=11FG322333434EBCDFAGh(E)=5h(D)=4h(F)=3h(B)=5h(