***(姓名)

***(學(xué)院)***@***.

第5章人形機(jī)器人導(dǎo)航控制

《人形機(jī)器人技術(shù)基礎(chǔ)與應(yīng)用》5.1概述目錄5.2

SLAM定位與建圖5.3機(jī)器人路徑規(guī)劃5.4機(jī)器人導(dǎo)航應(yīng)用實(shí)例SLAM建圖/hku-mars/FAST_LIO路徑規(guī)劃/ai-winter/python_motion_planning宇樹機(jī)器人導(dǎo)航運(yùn)動(dòng)仿真/unitreerobotics/unitree_guide5.1概述機(jī)器人導(dǎo)航是機(jī)器人研究中的一個(gè)核心問題，涉及如何在已知或未知環(huán)境中從起點(diǎn)移動(dòng)到目標(biāo)點(diǎn)。經(jīng)典導(dǎo)航方法主要分為：啟發(fā)式導(dǎo)航、端到端導(dǎo)航等。移動(dòng)機(jī)器人實(shí)現(xiàn)自主導(dǎo)航涉及到三個(gè)基本問題：機(jī)器人的定位、地圖構(gòu)建和路徑規(guī)劃。5.1概述根據(jù)使用的傳感器和應(yīng)用場景的不同，導(dǎo)航方式可以分為以下幾類：慣性導(dǎo)航：通過慣性傳感器實(shí)現(xiàn)自主定位，但因累積誤差常與其他傳感器結(jié)合使用。衛(wèi)星定位導(dǎo)航：衛(wèi)星定位系統(tǒng)如GPS通過衛(wèi)星信號提供高精度定位，但在動(dòng)態(tài)環(huán)境中容易丟失信號。光學(xué)導(dǎo)航和磁感導(dǎo)航：通過傳感器檢測地面標(biāo)記或磁場來糾正機(jī)器人運(yùn)動(dòng)方向。激光導(dǎo)航：通過激光測距確定物體位置，具備高精度和抗干擾能力，但受天氣影響且成本較高視覺導(dǎo)航：通過攝像機(jī)拍攝環(huán)境圖像確定位置，設(shè)備簡單、成本低，但在復(fù)雜環(huán)境中精度較低。5.1概述5.1.1同時(shí)定位與建圖原理SLAM（同步定位與地圖構(gòu)建）理論指的是機(jī)器人在未知環(huán)境中一邊定位自身位置，一邊構(gòu)建環(huán)境地圖的技術(shù)。圖5-1導(dǎo)航的三個(gè)關(guān)鍵步驟5.1.1同時(shí)定位與建圖原理準(zhǔn)確構(gòu)建地圖受多個(gè)因素的制約，主要包括以下幾個(gè)方面：定位于地圖構(gòu)建的同時(shí)進(jìn)行高緯度大范圍建圖傳感器和執(zhí)行器噪聲感知模糊閉環(huán)檢測5.1.2定位技術(shù)定位是確定機(jī)器人相對于環(huán)境地圖的姿態(tài)，是機(jī)器人感知中的基本問題。定位可以看作坐標(biāo)變換問題，通過建立全局坐標(biāo)系與機(jī)器人坐標(biāo)系之間的關(guān)系，實(shí)現(xiàn)環(huán)境位置描述。根據(jù)初始位姿是否已知以及周圍環(huán)境的性質(zhì)可以將定位問題分為四個(gè)維度：局部定位與全局定位靜態(tài)環(huán)境與動(dòng)態(tài)環(huán)境定位主動(dòng)與被動(dòng)定位單一或多個(gè)機(jī)器人定位5.1.2定位技術(shù)5.1概述5.1.3路徑規(guī)劃技術(shù)路徑規(guī)劃是移動(dòng)機(jī)器人導(dǎo)航中的關(guān)鍵問題。目標(biāo)是計(jì)算出一條滿足機(jī)器人運(yùn)動(dòng)學(xué)約束、避免與障礙物碰撞的最佳路徑。路徑規(guī)劃通常分為全局路徑規(guī)劃和局部路徑規(guī)劃兩個(gè)階段。5.1.3路徑規(guī)劃技術(shù)Dijkstra算法：逐層擴(kuò)展，從起始點(diǎn)出發(fā)，不斷更新各節(jié)點(diǎn)的最短路徑，最終找到最優(yōu)路徑。缺點(diǎn)：時(shí)間復(fù)雜度較高。全局路徑規(guī)劃全局路徑規(guī)劃：基于已知地圖進(jìn)行的路徑規(guī)劃，計(jì)算出機(jī)器人從起始點(diǎn)到目標(biāo)點(diǎn)的整體路徑。A*算法：綜合考慮實(shí)際距離和啟發(fā)式估計(jì)距離，能夠高效地搜索出起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑。5.1.3路徑規(guī)劃技術(shù)動(dòng)態(tài)窗口法（DWA）：限制機(jī)器人的速度和加速度范圍，實(shí)時(shí)評分并選擇最佳組合，以避免障礙物碰撞并生成安全路徑。局部路徑規(guī)劃局部路徑規(guī)劃：根據(jù)實(shí)時(shí)傳感器數(shù)據(jù)進(jìn)行的路徑規(guī)劃，以避免與動(dòng)態(tài)障礙物碰撞。人工勢場法：通過引力和斥力場引導(dǎo)機(jī)器人避障并朝目標(biāo)移動(dòng)，但可能面臨局部最小和在狹窄通道中的擺動(dòng)問題。5.2SLAM定位與建圖圖5-2SLAM系統(tǒng)框架圖5.2.1SLAM基礎(chǔ)知識SLAM是指機(jī)器人或自主移動(dòng)系統(tǒng)在未知環(huán)境中同時(shí)完成自身定位和地圖構(gòu)建的過程。SLAM的目標(biāo)是使機(jī)器人在未知環(huán)境中自主移動(dòng)，實(shí)時(shí)獲取位置信息并構(gòu)建環(huán)境地圖。過程主要包括定位和建圖兩個(gè)部分。5.2SLAM定位與建圖5.2.1SLAM基礎(chǔ)知識SLAM方法根據(jù)傳感器不同可分為激光SLAM和視覺SLAM。激光SLAM：激光SLAM通過激光雷達(dá)測量物體距離，獲取周圍環(huán)境的三維點(diǎn)云數(shù)據(jù)用于地圖構(gòu)建。視覺SLAM：利用攝像頭獲取環(huán)境的視覺信息，進(jìn)行定位和建圖。5.2.1SLAM基礎(chǔ)知識數(shù)據(jù)采集：激光雷達(dá)掃描周圍環(huán)境，獲取高精度的距離信息，生成點(diǎn)云數(shù)據(jù)。特征提取與匹配：從點(diǎn)云中提取特征點(diǎn)并匹配，估計(jì)機(jī)器人運(yùn)動(dòng)。位姿估計(jì)：通過匹配的特征點(diǎn)，使用ICP等算法來計(jì)算機(jī)器人的位姿。地圖構(gòu)建：將每次掃描的數(shù)據(jù)合并，逐步構(gòu)建出環(huán)境的二維或三維地圖。閉環(huán)檢測與優(yōu)化：檢測機(jī)器人是否回到之前的位置，并使用圖優(yōu)化修正累積誤差，優(yōu)化地圖和軌跡。激光SLAM激光SLAM建圖過程大致可分為以下幾個(gè)步驟：5.2.1SLAM基礎(chǔ)知識數(shù)據(jù)采集：攝像頭捕捉環(huán)境圖像，獲取視覺信息。特征提取與匹配：使用ORB等特征提取算法從圖像中提取并匹配特征點(diǎn)。位姿估計(jì)：通過匹配的特征點(diǎn)，使用PnP算法或VO估計(jì)相機(jī)的位姿變化。地圖構(gòu)建：將位姿估計(jì)結(jié)果和特征點(diǎn)結(jié)合，逐步構(gòu)建環(huán)境的稀疏或稠密點(diǎn)云地圖。閉環(huán)檢測與優(yōu)化：檢測相機(jī)是否回到之前的位置，并使用圖優(yōu)化修正累積誤差，優(yōu)化地圖和軌跡。視覺SLAM視覺SLAM建圖過程大致可分為以下幾個(gè)步驟：5.2SLAM定位與建圖5.2.2SLAM算法在未知環(huán)境中，描述機(jī)器人的運(yùn)動(dòng)和觀測狀態(tài)首先需要建立合適的數(shù)學(xué)模型。在離散時(shí)域，可以將機(jī)器人的連續(xù)運(yùn)動(dòng)分割為離散時(shí)間，記為t=1,2,3,…k。在每個(gè)時(shí)間點(diǎn)t上，機(jī)器人的位置分別表示為(x1,x2,x3,…,xk)。機(jī)器人從時(shí)間k-1到k的位置變化可表示為Δx=xk–xk-1。5.2.2

SLAM算法

xk?：機(jī)器人在時(shí)刻??的位置。f：運(yùn)動(dòng)模型，基于前一狀態(tài)和當(dāng)前控制輸入預(yù)測當(dāng)前位置。uk?：控制輸入（如傳感器讀數(shù)）。wk?：噪聲

zk,i?：觀測數(shù)據(jù)。h(yi,xk)：觀測模型，描述如何從機(jī)器人當(dāng)前位置和路標(biāo)位置預(yù)測觀測值。vk,i

：觀測噪聲。5.2.2

SLAM算法將運(yùn)動(dòng)模型與觀測模型結(jié)合，可得SLAM過程的聯(lián)合數(shù)學(xué)表達(dá)：

5.2.2

SLAM算法SLAM問題可以表述為：在給定傳感器數(shù)據(jù)??和運(yùn)動(dòng)測量??的條件下，估計(jì)機(jī)器人的路徑????和地圖????。該問題可模型化為狀態(tài)估計(jì)問題，解決方案依賴于觀測模型的特性和噪聲分布。早期解決方案基于擴(kuò)展卡爾曼濾波器（EKF），而現(xiàn)代方法以圖優(yōu)化為主流。視覺里程計(jì)：通過連續(xù)幀間特征匹配估計(jì)攝像機(jī)位姿，生成短期軌跡和地圖，但誤差隨時(shí)間積累。后端優(yōu)化：核心是集束調(diào)整（BA），但計(jì)算復(fù)雜度較高，通常僅在回環(huán)檢測或全局校準(zhǔn)時(shí)執(zhí)行。位姿圖優(yōu)化通過僅優(yōu)化關(guān)鍵幀位姿，減少計(jì)算負(fù)擔(dān)，提高實(shí)時(shí)性和大規(guī)模環(huán)境中的效率。5.2.2

SLAM算法視覺里程計(jì)與后端優(yōu)化核心目標(biāo)：優(yōu)化相機(jī)位姿和特征點(diǎn)空間位置，最小化誤差，獲取最優(yōu)的3D模型和相機(jī)參數(shù)。5.2.2

SLAM算法BA優(yōu)化全局BA：優(yōu)化所有關(guān)鍵幀的位姿和路標(biāo)點(diǎn)，提供全局一致性。局部BA：針對當(dāng)前關(guān)鍵幀及其共視關(guān)鍵幀優(yōu)化，快速修正軌跡。從世界坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換5.2.2

SLAM算法BA優(yōu)化

從世界坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換5.2.2

SLAM算法BA優(yōu)化

觀測方程與重投影誤差5.2.2

SLAM算法BA優(yōu)化

觀測方程與重投影誤差5.2.2

SLAM算法BA優(yōu)化

代價(jià)函數(shù)與優(yōu)化5.2.2

SLAM算法BA優(yōu)化

代價(jià)函數(shù)與優(yōu)化5.2.2

SLAM算法BA優(yōu)化最小化代價(jià)函數(shù)：目標(biāo)是通過調(diào)整相機(jī)位姿和三維點(diǎn)位置來使重投影誤差盡可能小。引入增量Δ??，將代價(jià)函數(shù)轉(zhuǎn)化為可以通過迭代方法求解的形式。

ξi?：機(jī)器人李代數(shù)位姿表示；Fi,j和Ei,j：代價(jià)函數(shù)關(guān)于機(jī)器人位姿和三維點(diǎn)位置的偏導(dǎo)數(shù)。代價(jià)函數(shù)與優(yōu)化5.2.2

SLAM算法BA優(yōu)化優(yōu)化方法：通過最小化代價(jià)函數(shù)，優(yōu)化相機(jī)位姿和地圖點(diǎn)的參數(shù)。優(yōu)化過程通常采用高斯-牛頓方法，將問題轉(zhuǎn)化為線性增量方程。其中，H是由雅可比矩陣組成的大矩陣，Δ??是增量。由于??的稀疏性，計(jì)算可以通過稀疏矩陣技術(shù)進(jìn)行高效求解。

稀疏矩陣優(yōu)化5.2.2

SLAM算法BA優(yōu)化優(yōu)化過程：利用稀疏結(jié)構(gòu)，優(yōu)化計(jì)算過程，避免全矩陣計(jì)算的昂貴代價(jià)，從而加快整個(gè)優(yōu)化過程。

在SLAM系統(tǒng)中，盡管BA優(yōu)化提高了定位與建圖精度，但大規(guī)模BA優(yōu)化非常耗時(shí)，且當(dāng)系統(tǒng)接近收斂時(shí)，特征點(diǎn)位置變化微小。位姿圖優(yōu)化通過構(gòu)建位姿圖來專注于優(yōu)化相機(jī)位姿，簡化計(jì)算過程，減少計(jì)算量，提高效率。5.2.2

SLAM算法位姿圖優(yōu)化位姿圖優(yōu)化的基本原理5.2.2

SLAM算法位姿圖優(yōu)化

誤差模型與優(yōu)化目標(biāo)5.2.2

SLAM算法位姿圖優(yōu)化

雅可比矩陣計(jì)算5.2.2

SLAM算法位姿圖優(yōu)化

圖優(yōu)化目標(biāo)函數(shù)5.2.2

SLAM算法位姿圖優(yōu)化

eij?：是邊上的誤差向量；Σ?1：是誤差的逆協(xié)方差矩陣；?：

是圖中所有邊的集合。圖優(yōu)化的實(shí)現(xiàn)5.2.2

SLAM算法位姿圖優(yōu)化圖優(yōu)化過程可以通過圖優(yōu)化庫如g2o或Ceres實(shí)現(xiàn)。這些庫利用圖的稀疏性高效地進(jìn)行大規(guī)模優(yōu)化，從而提升SLAM系統(tǒng)在大規(guī)模環(huán)境中的性能和精度。5.2SLAM定位與建圖5.2.3SLAM實(shí)例隨著多傳感器融合技術(shù)的進(jìn)步，激光SLAM與視覺SLAM的融合成為趨勢。優(yōu)點(diǎn)：這種方案通過結(jié)合不同傳感器的信息，彌補(bǔ)各自的不足，為復(fù)雜動(dòng)態(tài)場景中的SLAM應(yīng)用帶來巨大潛力。激光SLAM和視覺SLAM中有許多經(jīng)典算法，Cartographer和ORB-SLAM3是兩種具有代表性的經(jīng)典方法。5.2.3SLAM實(shí)例CartographerCartographer是谷歌開發(fā)的基于圖優(yōu)化的激光SLAM系統(tǒng)。支持2D和3D激光SLAM兼容多種傳感器（如激光雷達(dá)、IMU、GPS等）它廣泛應(yīng)用于清潔機(jī)器人、倉儲機(jī)器人等服務(wù)機(jī)器人領(lǐng)域。5.2.3SLAM實(shí)例Cartographer圖5-3展示了Cartographer基于利用子圖構(gòu)建全局地圖的思想，將地圖劃分為小的區(qū)域，并對每個(gè)區(qū)域進(jìn)行建模和優(yōu)化，這樣能夠有效地避免建圖過程中環(huán)境里移動(dòng)物體的干擾圖5-3CartographerSLAM流程圖5.2.3SLAM實(shí)例Cartographer前端：利用激光雷達(dá)等傳感器信息構(gòu)建局部區(qū)域的子圖。后端：優(yōu)化并整合這些子圖，生成一張完整的可用地圖。圖5-4Cartographer在室內(nèi)環(huán)境中構(gòu)建的地圖5.2.3SLAM實(shí)例CartographerCartographer具有以下幾個(gè)特點(diǎn)和優(yōu)勢：高精度：使用先進(jìn)的定位和建圖算法，能夠在各種復(fù)雜環(huán)境中實(shí)現(xiàn)高精度的定位和建圖。實(shí)時(shí)性：能夠?qū)崿F(xiàn)實(shí)時(shí)的定位和建圖，適用于移動(dòng)機(jī)器人在動(dòng)態(tài)環(huán)境中的應(yīng)用。開發(fā)源代碼：是一個(gè)開源項(xiàng)目，用戶可以自由獲取源代碼并根據(jù)需要進(jìn)行修改和定制。5.2.3SLAM實(shí)例ORB-SLAM3ORB-SLAM3是西班牙薩拉戈薩大學(xué)開源的一款經(jīng)典的視覺SLAM系統(tǒng)圖5-5ORB-SLAM3系統(tǒng)框架圖5.2.3SLAM實(shí)例ORB-SLAM3的主要改進(jìn)ORB-SLAM3基于特征點(diǎn)的視覺定位與建圖技術(shù)，相比前兩個(gè)版本，做出了以下幾個(gè)改進(jìn)：引入新型閉環(huán)檢測算法，增強(qiáng)軌跡閉合準(zhǔn)確性。采用增量式優(yōu)化方法，實(shí)時(shí)更新地圖與優(yōu)化軌跡，提升系統(tǒng)實(shí)時(shí)性與穩(wěn)健性。支持多相機(jī)系統(tǒng)，提供更多配置選項(xiàng)和用戶友好的界面。5.2.3SLAM實(shí)例ORB-SLAM3的技術(shù)特性O(shè)RB-SLAM3是ORB-SLAM系列的最新版本，基于前兩版的改進(jìn)，提供更高的定位和建圖性能。支持單目、立體、RGB-D相機(jī)。兼容針孔與魚眼相機(jī)模型。提供視覺、視覺+慣導(dǎo)和多地圖綜合SLAM方案。5.2.3SLAM實(shí)例ORB-SLAM3ORB-SLAM3具有高效、穩(wěn)健和易用的特點(diǎn)，適用于室內(nèi)外定位與建圖任務(wù)，為移動(dòng)機(jī)器人和增強(qiáng)現(xiàn)實(shí)等應(yīng)用提供了強(qiáng)大解決方案，推動(dòng)了SLAM技術(shù)的發(fā)展。圖5-6ORB-SLAM3特征點(diǎn)提取與建圖5.3機(jī)器人路徑規(guī)劃5.3.1全局路徑規(guī)劃5.3.2局部路徑規(guī)劃5.3機(jī)器人路徑規(guī)劃機(jī)器人路徑規(guī)劃：作為機(jī)器人導(dǎo)航系統(tǒng)中的關(guān)鍵組成部分，其目標(biāo)是讓機(jī)器人能夠從一個(gè)位置移動(dòng)到另一個(gè)位置，同時(shí)避免迷路和與其他物體碰撞。全局路徑規(guī)劃：通過使用已知的靜態(tài)地圖來規(guī)劃從起始點(diǎn)到目標(biāo)點(diǎn)的最佳可行路徑。局部路徑規(guī)劃：根據(jù)實(shí)時(shí)傳感器采集的環(huán)境信息，獲取動(dòng)態(tài)的地圖，并規(guī)劃從當(dāng)前節(jié)點(diǎn)到下一個(gè)節(jié)點(diǎn)的安全路徑。5.3.1全局路徑規(guī)劃代價(jià)地圖占用柵格地圖：將柵格描述為被占用、未被占用和未知三種信息。代價(jià)地圖：基于占用柵格地圖的一種空間通行性的描述形式，將占用柵格地圖中的每個(gè)位置再關(guān)聯(lián)一個(gè)代價(jià)值，用于描述機(jī)器人在該位置移動(dòng)的難易程度。圖5-7仿真環(huán)境與占用柵格地圖5.3.1全局路徑規(guī)劃代價(jià)地圖的生成代價(jià)地圖的生成分為：添加靜態(tài)層，添加障礙物層和設(shè)置膨脹層。圖5-8代價(jià)地圖靜態(tài)層靜態(tài)層：占用柵格地圖所提供的信息又被稱為靜態(tài)層。被占用的部分轉(zhuǎn)為紫色，即占有最高的代價(jià)，表示不可通過。5.3.1全局路徑規(guī)劃代價(jià)地圖的生成圖5-9代價(jià)地圖障礙物層障礙物層：將傳感器的信息添加到代價(jià)地圖中，如圖5-9a所示，在仿真環(huán)境中添加圓柱體障礙物后，在圖5-9b展示的代價(jià)地圖中會多出一塊弧形區(qū)域，這是障礙物層的效果。5.3.1全局路徑規(guī)劃代價(jià)地圖的生成膨脹層：基于靜態(tài)地圖層和障礙物地圖層生成，主要作用是通過“膨脹”障礙物的區(qū)域來增加安全距離。設(shè)置膨脹層的原因：（1）激光雷達(dá)獲取的激光點(diǎn)是稀疏的，可能會出現(xiàn)障礙物間存在縫隙的情況，導(dǎo)致規(guī)劃出的路徑可能會從機(jī)器人是無法通行的縫隙內(nèi)穿過。（2）沒有膨脹層，路徑規(guī)劃會緊貼障礙物的邊緣，這會對下游的局部路徑規(guī)劃造成一些麻煩。5.3.1全局路徑規(guī)劃代價(jià)地圖的生成圖5-10帶有膨脹層的代價(jià)地圖膨脹層設(shè)置的三個(gè)主要參數(shù)：機(jī)器人半徑、膨脹半徑和代價(jià)衰減系數(shù)。其中，膨脹半徑?jīng)Q定了膨脹層的最大范圍，衰減系數(shù)決定了膨脹層代價(jià)的衰減速度。如圖5-10所展示的實(shí)例，三個(gè)實(shí)例的機(jī)器人半徑均設(shè)置為0.2米，對應(yīng)圖中青色的區(qū)域。5.3.1全局路徑規(guī)劃圖搜索算法常用的全局路徑規(guī)劃圖搜索算法：遺傳算法、A*算法、快速搜索隨機(jī)樹算法等路徑規(guī)劃算法。遺傳算法：受進(jìn)化思想的提示，對遺傳和自然選擇時(shí)發(fā)生選擇、交叉和變異進(jìn)行分析和仿真，結(jié)合優(yōu)勝劣汰思想，根據(jù)每一層的結(jié)果選擇出每一層的候選解，并對候選解進(jìn)行比對，最終從得到的候選解中選出最優(yōu)解。5.3.1全局路徑規(guī)劃圖搜索算法遺傳算法流程圖如圖5-11所示。圖5-11遺傳算法流程圖5.3.1全局路徑規(guī)劃圖搜索算法RRT算法的步驟如下：（1）初始化：獲得整個(gè)地圖，并且獲取相應(yīng)的移動(dòng)的起點(diǎn)和終點(diǎn)。（2）采樣：自行設(shè)定隨機(jī)樹，隨機(jī)樹朝向不同的方向隨機(jī)生長。（3）選擇路徑：在空間中隨機(jī)產(chǎn)生種子，在生長的隨機(jī)樹中找到距離這個(gè)

點(diǎn)的最近點(diǎn)，在此隨機(jī)點(diǎn)和隨機(jī)樹上的點(diǎn)中間生成路徑。進(jìn)行判定，

若路徑中間無障礙物，說明此路徑可行，將路徑加入隨機(jī)樹。（4）找到路徑：在尋找路徑時(shí)設(shè)置步長，最后一步的距離不能保證剛好到

達(dá)目標(biāo)點(diǎn)，因此，距離終點(diǎn)一定距離就可以判定為到達(dá)目標(biāo)點(diǎn)。快速搜索隨機(jī)樹算法（RRT，RapidlyExploringRandomTrees）：樹根據(jù)采樣進(jìn)行搜索，不僅適用于平面，在高維空間中搜索路徑同樣適用。5.3.1全局路徑規(guī)劃圖搜索算法因?yàn)榭焖偎阉麟S機(jī)樹的隨機(jī)性比較大，因此做了較多改進(jìn)，經(jīng)典的改進(jìn)有雙向搜索隨機(jī)樹（RRT-connect）。RRT-connect是在起點(diǎn)和目標(biāo)點(diǎn)共同生長種子，并迅速擴(kuò)展隨機(jī)樹以搜索當(dāng)前空間，當(dāng)起始點(diǎn)與目標(biāo)點(diǎn)的生長樹相連接時(shí)，完成當(dāng)前搜索。圖5-12RRT算法和RRT-connect算法二維空間搜索對比5.3.1全局路徑規(guī)劃圖搜索算法Dijkstra算法（Dijkstra‘salgorithm）：一種最短路徑算法，用來找出有向圖中兩個(gè)節(jié)點(diǎn)之間的最短路徑，Dijkstra算法的原理如下：（1）假設(shè)有一個(gè)帶權(quán)有向圖，集合S只包含起始節(jié)點(diǎn)，集合U包含

除以外的所有節(jié)點(diǎn)。（2）從集合U中選擇與起始節(jié)點(diǎn)距離最短的節(jié)點(diǎn)，添加到S集合中。（3）將節(jié)點(diǎn)作為中間節(jié)點(diǎn)，更新其與集合中所有節(jié)點(diǎn)的距離。（4）重復(fù)執(zhí)行步驟（2）和步驟（3），直到集合S包含所有節(jié)點(diǎn)。5.3.1全局路徑規(guī)劃圖搜索算法代價(jià)函數(shù)的表達(dá)式如下：

f(n)=g(n)+h(n)其中：

g(n):當(dāng)前節(jié)點(diǎn)到起始節(jié)點(diǎn)的實(shí)際距離代價(jià)

h(n):當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)距離代價(jià)

f(n):從起始節(jié)點(diǎn)經(jīng)過當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的總代價(jià)A*算法(A-staralgorithm)：一種基于狀態(tài)空間的啟發(fā)式搜索算法，核心思想是在搜索過程中引入啟發(fā)函數(shù)，通過建立啟發(fā)式搜索規(guī)則，優(yōu)先向目標(biāo)點(diǎn)方向進(jìn)行搜索，從而提高搜索效率。5.3.1全局路徑規(guī)劃圖搜索算法A*算法的基本流程如下：（1）將起始節(jié)點(diǎn)添加到開啟列表中。（2）從開啟列表中選擇具有最小值的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)，并將其

添加到關(guān)閉列表中。（3）對于當(dāng)前節(jié)點(diǎn)n的相鄰節(jié)點(diǎn)進(jìn)行以下操作：①若相鄰節(jié)點(diǎn)m是障礙或已存在于關(guān)閉列表，則忽略該節(jié)點(diǎn)。②若相鄰節(jié)點(diǎn)m不在開啟列表，則將其添加進(jìn)開啟列表，更新

節(jié)點(diǎn)m父節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)n，記錄節(jié)點(diǎn)f(m)、g(m)、h(m)值。5.3.1全局路徑規(guī)劃圖搜索算法圖5-14A*算法的搜索范圍和規(guī)劃出的路徑③若相鄰節(jié)點(diǎn)m已經(jīng)在開啟列表中，計(jì)算新路徑經(jīng)過當(dāng)前節(jié)點(diǎn)n到達(dá)節(jié)點(diǎn)m的實(shí)際距離g(m)，如果g(m)值比原先g(m)值更小，則將新路徑設(shè)為最優(yōu)路徑，更新相鄰節(jié)點(diǎn)m的父節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)n，同時(shí)更新節(jié)點(diǎn)m的g(m)和f(m)值。（4）重復(fù)進(jìn)行步驟2和步驟3，直到目標(biāo)節(jié)點(diǎn)被添加到關(guān)閉列表中，表示路徑已找到。5.3.2局部路徑規(guī)劃局部路徑規(guī)劃：在機(jī)器人全局路徑規(guī)劃的基礎(chǔ)上，對機(jī)器人在當(dāng)前位置到達(dá)目標(biāo)位置之間的具體路徑進(jìn)行規(guī)劃。局部路徑規(guī)劃側(cè)重點(diǎn)：更注重于對當(dāng)前環(huán)境局部信息的感知，使機(jī)器人可以較好的實(shí)時(shí)避障。較為常用的局部路徑規(guī)劃算法：動(dòng)態(tài)窗口法、人工勢場法。5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法人工勢場法圖5-16人工勢場法示意圖核心思想：在機(jī)器人移動(dòng)空間中構(gòu)建虛擬的力場，在力場中對受力進(jìn)行分析，目標(biāo)點(diǎn)對機(jī)器人具有吸引力，障礙物對移動(dòng)機(jī)器人具有排斥力，在整個(gè)環(huán)境中的任意一點(diǎn)的勢力是在該點(diǎn)的引力和斥力的結(jié)合。5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法三維空間人工勢場與二維空間人工勢場相似，將空間比作一個(gè)地形區(qū)域，在地形中有起伏。人工勢場法圖5-17重力勢場示意圖5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法人工勢場可分為引力場、斥力場。分開對兩種勢場進(jìn)行分析。人工勢場法引力函數(shù)：求解引力：

5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法人工勢場法求解斥力：斥力函數(shù)：

5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法人工勢場法（1）（2）總的勢場：斥力與引力的疊加，如式（1）所示?？偟牧Γ悍至Φ暮?，如式（2）所示。5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法人工勢場法人工勢場法可改進(jìn)的地方：在機(jī)器人遇到障礙物的時(shí)候，可以對引力加入系數(shù)，調(diào)整引力大小，防止因機(jī)器人在當(dāng)前位置距離目標(biāo)點(diǎn)過遠(yuǎn)而導(dǎo)致引力過大。終點(diǎn)周圍設(shè)置障礙物的時(shí)候，在原本的斥力影響下通過加入終點(diǎn)與機(jī)器人之間距離的影響因素，引導(dǎo)機(jī)器人到達(dá)目標(biāo)點(diǎn)。局部路徑算法的缺陷在于局部最優(yōu)，算法添加隨機(jī)擾動(dòng)，跳出局部最優(yōu)。5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法動(dòng)態(tài)窗口法算法思想：按照分辨率在速度空間中提取多對速度，不同的速度有不同的運(yùn)行軌跡，對路徑按照一定的評價(jià)體系進(jìn)行評價(jià)并選出最優(yōu)路徑，使機(jī)器人按照最優(yōu)路徑前進(jìn)。5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法動(dòng)態(tài)窗口法中的運(yùn)行軌跡前提：假定機(jī)器人只能向前和旋轉(zhuǎn)。把機(jī)器人的運(yùn)行軌跡拆成小段，對相鄰時(shí)間段的機(jī)器人路徑探索，同時(shí)為方便計(jì)算，將相近時(shí)間點(diǎn)內(nèi)的位移設(shè)計(jì)成直線。將移動(dòng)距離分別投影到X軸和Y軸可得：5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法圖5-18機(jī)器人位移示意圖動(dòng)態(tài)窗口法中的運(yùn)行軌跡向下推算，為了計(jì)算在一段時(shí)間內(nèi)的位移，將每小段時(shí)間的位移進(jìn)行累加?？傻茫?.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法動(dòng)態(tài)窗口法中的運(yùn)行軌跡進(jìn)一步推算，機(jī)器人在Xrobot軸也有速度，機(jī)器人在Yrobot軸的位移投影到坐標(biāo)軸：5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法上述步驟是假定短時(shí)間內(nèi)機(jī)器人地移動(dòng)是線性的，這樣計(jì)算雖然簡便，但不精確。更準(zhǔn)確的方法是假定機(jī)器人在相鄰時(shí)間內(nèi)的移動(dòng)是圓弧，同樣按照前面的方法進(jìn)行計(jì)算。動(dòng)態(tài)窗口法中的運(yùn)行軌跡將機(jī)器人在Xrobot的路徑軌跡與Yrobot的路徑軌跡進(jìn)行疊加可得在全向移動(dòng)情況下機(jī)器人的軌跡：5.3.2局部路徑規(guī)劃局部路徑規(guī)劃算法動(dòng)態(tài)窗口法中的運(yùn)行軌跡假定機(jī)器人在相鄰時(shí)間內(nèi)的移動(dòng)是圓弧，圓弧的半徑為：此時(shí)假定旋轉(zhuǎn)速度不為0，機(jī)器人當(dāng)前位置為：5.4機(jī)器人導(dǎo)航應(yīng)用實(shí)例5.4.1全局導(dǎo)航5.4.2局部導(dǎo)航5.4.3探索與建圖5.4機(jī)器人導(dǎo)航應(yīng)用實(shí)例通過具體的實(shí)例，詳細(xì)介紹機(jī)器人導(dǎo)航所需的核心技術(shù)，更直觀地理解如何應(yīng)用機(jī)器人導(dǎo)航技術(shù)，從而為機(jī)器人系統(tǒng)的設(shè)計(jì)、開發(fā)和部署提供實(shí)用的指導(dǎo)。如圖5-20所示，本節(jié)實(shí)例部分介紹了一種完全自主的探索系統(tǒng)，用于導(dǎo)航到全局目標(biāo)，無需人工控制或環(huán)境的先驗(yàn)信息。圖5-20自主導(dǎo)航系統(tǒng)總體框架圖5.4.1全局導(dǎo)航獲取興趣點(diǎn)（POI）為了讓機(jī)器人朝著最終目標(biāo)導(dǎo)航，需要從可用的興趣點(diǎn)（POI）中選擇一些中間點(diǎn)供局部導(dǎo)航使用?？梢酝ㄟ^兩種方法來實(shí)現(xiàn)獲取新的POI：間隙檢測：如果兩個(gè)連續(xù)的激光讀數(shù)之間的差值大于某個(gè)閾值，就會添加一個(gè)POI。自由空間：激光傳感器有一個(gè)最大范圍，超出這個(gè)范圍的讀數(shù)會返回一個(gè)非數(shù)值類型，表示環(huán)境中的自由空間。連續(xù)的激光讀數(shù)都返回非數(shù)值，那么就在環(huán)境中放置一個(gè)POI。5.4.1全局導(dǎo)航獲取興趣點(diǎn)（POI）圖

5-21POI提取示例上述兩種獲取興趣點(diǎn)的方法如圖5-21所示：5.4.1全局導(dǎo)航選擇最佳航點(diǎn)其中，索引為i的每個(gè)候選POIc的得分h是三個(gè)分量的總和。從可用的POI中，使用基于信息的距離限制探索（IDLE）評估方法選擇時(shí)間步長的最佳航路點(diǎn)。IDLE方法評估每個(gè)候選POI的適應(yīng)度如下：5.4.1全局導(dǎo)航選擇最佳航點(diǎn)其中，是歐拉數(shù)，和是用于降低得分的兩步距離限制。兩步距離限制是根據(jù)DRL訓(xùn)練環(huán)境的面積大小設(shè)置的。第二個(gè)分量表示候選目標(biāo)與全局目標(biāo)之間的歐幾里得距離。機(jī)器人在t時(shí)的位置p與候選POI之間的歐幾里得距離分量d(pt,ci)表示為雙曲正切tanh函數(shù)：5