讓機器人自主導航項目9CONTENT目錄項目導入01項目任務02學習目標03知識鏈接04項目準備05任務實施06任務評價07任務拓展08項目小結09項目導入0141項目導入

導航技術是指移動機器人通過傳感器感知環境信息和自身狀態，實現在有障礙的環境中面向目標的自主運動，該技術是移動機器人的一項核心技術之一。本項目主要在項目8的基礎上，確定機器人在已建地圖中的實時位置，并規劃一條從起始點到目標點的無碰路線，并使機器人沿著規劃路線運動。這也就是機器人的自主導航問題。CONTENT目錄項目導入01項目任務02學習目標03知識鏈接04項目準備05任務實施06任務評價07任務拓展08項目小結09項目任務0272項目任務本項目需基于Ubuntu18.04ROSmelodic環境，在項目8建立環境地圖的基礎上，基于ROSNavigation導航功能包，完成機器人自主導航任務所需的配置和測試工作。具體任務包含：1.配置機器人導航應用的工作環境；2.配置ROSNavigation導航功能包；3.使用可視化工具rviz進行重定位；5.使用可視化工具rviz進行導航測試。CONTENT目錄項目導入01項目任務02學習目標03知識鏈接04項目準備05任務實施06任務評價07任務拓展08項目小結09學習目標03知識目標職業素養目標技能目標1.了解移動機器人定位與導航的概念；2.了解移動機器人全局規劃與局部規劃的常用算法；3.熟悉ROSNavigation導航功能包的基本框架move_base、參數文件和參數配置；4.熟悉AMCL節點，及其服務，訂閱、發布的話題；5.掌握移動機器人定位和導航的實現步驟；知識目標：知識目標10學習目標3113學習目標1.能配置參數并調用ROSNavigation導航功能包；2.能調用move_base節點進行機器人路徑規劃和導航控制3.能配置ROSNavigation導航功能包中的全局規劃器和局部規劃器的參數4.能配置ROSNavigation導航功能包中的代價地圖的參數5.能在可視化工具rviz中進行機器人局部重定位和全局重定位6.能在可視化工具rviz中進行機器人導航測試。

技能目標：技能目標123學習目標1.培養工程調試與應用的堅持和耐心2.培養并提高學生自主學習和持續學習的意識和能力3.培養精益求精的工匠精神4.培養團隊合作與分工的意識。職業素養目標：職業素養目標CONTENT目錄項目導入01項目任務02學習目標03知識鏈接04項目準備05任務實施06任務評價07任務拓展08項目小結09知識鏈接04移動機器人定位移動機器人導航局部路徑規劃避障全局路徑規劃RosNavigation功能包介紹AMCL功能包導航功能包參數文件配置定位被稱為“提供移動機器人自主能力的最基本問題”移動機器人定位15知識鏈接4導航技術是移動機器人的一項核心技術之一，指移動機器人通過傳感器感知環境信息和自身狀態，實現在有障礙的環境中面向目標的自主運動。在復雜環境下，實現導航的三要素為：1）我在哪；2）我要去哪；3）如何去。在前面我們已經討論了前兩個要素涉及的問題：建圖與定位，而第三個問題是指機器人如何規劃出一條路徑，既要能從出發點達到目標點，又要避免行程中所出現的障礙物。移動機器人導航164知識鏈接移動機器人導航移動機器人定位與導航的流程如下：1.首先獲得相關的地圖信息（map）和機器人目標點位姿（goal）2.結合傳感器數據，如激光數據（scan）與里程計數據（odom）3.從初始位置到目標位置的角度考慮，調用全局路徑規劃器（globalplanner），規劃出一條大致可行的路線4.在地圖中出現了未知的障礙物的情況下，調用局部路徑規劃器（localplanner），并根據代價地圖（costmap）的信息，規劃局部避障的路線5.發送規劃的路線數據到機器人底層控制，讓機器人沿著路線運動。174知識鏈接移動機器人導航定位與導航框架主要算法模塊及其可使用的算法184知識鏈接算法模塊默認算法可擴展的算法定位AMCLRobot_pose_ekf,…全局路徑規劃DijkstraA*,Carrot，…局部路徑規劃TEBplannerDWAplanner…避障Costmap（基于激光雷達構建）RGBD、超聲、紅外…全局路徑規劃全局路徑規劃（globalpathplanning）是在已知的環境中，給機器人規劃一條路徑，路徑規劃的精度取決于環境獲取的準確度，全局路徑規劃可以找到最優解，但是需要預先知道環境的準確信息，且環境中的障礙物是靜態的。優點：它能夠離線計算出最優路徑，是一種事前規劃（即機器人運動前規劃路徑），因此對機器人系統的實時計算能力要求不高。缺點：對環境模型的錯誤及噪聲魯棒性差，且由于機器人運動規劃的內在復雜性，全局規劃方法速度慢，如果環境模型在動態變化，則無法處理。三種典型的全局路徑規劃方法：Dijkstra算法、BFS算法和A*算法194知識鏈接全局路徑規劃1.Dijkstra算法Dijkstra算法是一種經典的廣度優先的狀態空間搜索算法，即算法會從初始點開始一層一層地搜索整個自由空間直到到達目標點。優點：不需要預先探明地圖，不足：遍歷計算的節點非常多，時間計算量大、數據量大，所以效率低。204知識鏈接Dijkstra算法規劃路徑示例全局路徑規劃2.BFS算法最佳優先搜索（BFS）算法基于貪心策略，貪心策略指求解問題時，總是做出在當前看來最好的選擇，即不從整體最優考慮。優點：運行速度較快。不足：不能保證找到一條最短路徑。214知識鏈接BFS算法規劃路徑示例全局路徑規劃3.A*算法一種啟發式搜索（heuristicallysearch）算法被提出，利用問題擁有的啟發式信息來引導搜索，減少搜索范圍、降低問題復雜度，達到搜索出圖中指定節點對之間的最小代價路徑。224知識鏈接A*算法在簡單環境（左）和復雜環境（右）中的路徑規劃示例

全局路徑規劃局部路徑規劃（localpathplanning）允許環境信息完全未知或部分可知，能夠達到在機器人運動時規劃路徑。側重于考慮機器人當前的局部環境信息，讓機器人具有良好的避障能力，通過傳感器對機器人的工作環境進行探測，以獲取障礙物的位置和幾何性質等信息。特點：要求機器人系統具有高速的信息處理能力和計算能力，對環境誤差和噪聲有較高的魯棒性，能對規劃結果進行實時反饋和校正。但是由于缺乏全局環境信息，容易陷入局部最優，甚至可能找不到正確路徑或完整路徑。兩種局部路徑規劃算法：TEB算法和DWA。234知識鏈接全局路徑規劃1.TEB算法TEB算法全稱為TimedElasticBand。TEB算法的核心思想，是通過加權多目標優化模型，考慮各種約束的目標函數，來調整與優化機器人位姿和時間間隔，即獲取最優路徑（點）。244知識鏈接TEB控制流程全局路徑規劃2.DWA算法DWA（動態窗口）算法4全稱為DynamicWindowApproach，原理主要是在速度空間中采樣多組速度（一組速度（v，w）由線速度與角速度組成），并模擬機器人在這些速度下一定時間內的運動軌跡。在得到多組軌跡后，通過一個目標函數對這些軌跡評估，選取最優軌跡對應的速度來驅動機器人運動。254知識鏈接避障避障是指移動機器人根據采集的障礙物的狀態信息，在行走過程中通過傳感器感知到妨礙其通行的靜態和動態物體時，按照一定的方法進行有效地避障，最后達到目標點。實現避障與導航的必要條件是環境感知,代價地圖在機器人感知避障中作為了重要載體。264知識鏈接避障代價地圖（costmap）用于描述環境中的障礙物信息，利用激光雷達、聲吶、紅外測距等探測傳感器的數據來建立和更新的二維或三維地圖。274知識鏈接紅色單元格：代價地圖中的障礙物藍色單元格：通過機器人內切圓半徑膨脹出的障礙物紅色多邊形：機器人的占地面積(機器人輪廓的垂直投影)。為了避免碰撞，機器人位置不能和紅色部分有交叉，且機器人中心位置不能與藍色部分有交叉。

避障初始的分層代價地圖（layeredcostmap）分為三層和主圖：靜態地圖層（StaticMapLayer）、障礙物層（ObstaclesLayer）、膨脹層（InflationLayer）以及主圖（MasterMap）。284知識鏈接避障基礎的代價地圖由三層組成：(1)靜態地圖層為了做全局規劃，機器人需要一個“超越”其傳感器的地圖，以了解墻壁和其他靜態障礙物的位置。靜態地圖可以優先用SLAM算法生成，也可以從架構圖中創建。在構建代價地圖流程中，每次檢查待更新區域時（即調用UpdateBounds方法），由于靜態地圖層是基本不變的，它一直是代價地圖的底層，所以它將其值直接復制到主圖中。

294知識鏈接避障基礎的代價地圖由三層組成：(2)障礙物層該層從高精度傳感器（例如激光傳感器和RGB-D相機）動態地收集數據，并將其放置在二維網格中。傳感器和傳感器測量區域之間的空間被標記為空白（free），傳感器測量的區域被標記為占用（occupied）。在每次調用UpdateBounds時，邊界框可擴展以適應所有新數據。

304知識鏈接避障基礎的代價地圖由三層組成：(3)膨脹層膨脹層在前兩層地圖上進行膨脹，膨脹過程在每個致命障礙周圍插入一個緩沖區，機器人肯定會發生碰撞的地點被標記為致命代價（lethalcost），而緊鄰的區域具有很小的非致命代價（non-lethalcost）。這些值確保機器人不會與致命障礙物碰撞，并且不希望它們過于接近。由于該層特點，在調用UpdateBounds時，將增加前一個邊界框，以確保新的致命障礙被膨脹的同時，在前一個邊界框外的、舊的致命障礙可能會膨脹到連該邊界框的部分仍然起作用。314知識鏈接ROSNavigation功能包介紹ROSNavigation導航功能包能夠在開源SLAM算法完成地圖構建后實現機器人定位、路徑規劃等功能。該導航功能包包含完成機器人定位、路徑規劃、導航等組件，可以通過配置直接使用324知識鏈接ROS-Navigation功能包框架

ROSNavigation功能包介紹move_base節點，是導航過程運動控制中的核心節點，在導航任務中處于核心位置，其他package都是它的插件。括下列三種package：base_local_planner：局部規劃器：為局部代價地圖，記錄機器人附近的障礙物信息，通常由傳感器實時刷新，用于局部路徑規劃。base_global_planner：為全局代價地圖，記錄整個地圖上的障礙物信息，用于生成全局路徑。recovery_behavior：恢復策略：當機器人無法規劃出導航路徑時采取的策略，如清除局部代價圖，嘗試轉圈尋找路徑等。334知識鏈接ROSNavigation功能包介紹344知識鏈接三種package可選的插件：base_local_planner局部規劃器base_local_planner:實現了TrajectoryRollout和DWA兩種局部規劃算法。dwa_local_planner:實現了DWA局部規劃算法，可以看作是base_local_planner的改進版本。base_global_planner全局規劃器：parrot_planner:實現了較簡單的全局規劃算法。

navfn:實現了Dijkstra和A*全局規劃算法。global_planner:重新實現了Dijkstra和A*全局規劃算法,可以看作navfn的改進版。recovery_behavior恢復策略clear_costmap_recovery:實現了清除代價地圖的恢復行為。rotate_recovery:實現了旋轉的恢復行為。move_slow_and_clear:實現了緩慢移動的恢復行為。ROSNavigation功能包介紹引子：蒙特卡洛定位算法354知識鏈接使用粒子來表達一個位置的置信度（belief）。粒子越多，機器人處于這個位置的可能性越高。

機器人位置仍全局不確定機器人的置信度仍集中在4個可能的位置機器人自身的位置得到了確定。

辦公環境中蒙特卡洛定位示例AMCL功能包364知識鏈接蒙特卡羅定位算法：優點：能解決全局定位問題，不足：不能從機器人綁架中或全局定位失效中恢復。該方法在獲取機器人位置的過程中，不在最可能位姿處的粒子會逐漸消失。在某個時刻，只有單一位姿的粒子“存活”，但如果這個位姿碰巧是錯誤的，算法并不能恢復。當粒子數很小，并且粒子擴散到整個范圍較大的區域時，這個問題就特別重要了。解決方法：自適應蒙特卡羅定位AMCL功能包374知識鏈接AMCL(AdaptiveMonteCarloLocalization)即自適應蒙特卡洛定位（縮寫中字母A也可以理解為augmented），它在蒙特卡洛方法基礎上添加自適應的粒子數調整和隨機撒粒子機制，當它發現粒子們的平均分數突然降低了（意味著正確的粒子在某次迭代中未“存活”），就注入隨機粒子，在運動模型中產生一些隨機狀態，從而解決了機器人的綁架問題。AMCL算法原理圖AMCL功能包384知識鏈接AMCL功能包的通信架構如圖所示：訂閱的話題/tf：坐標轉換；/scan：激光數據；/initialpose：初始位置和均值和方差；/map：地圖信息。

/tf：發布從odom到map的轉換位置；/amcl_pose：機器人在地圖中的位姿估計，包括估計方差；/particlecloudpoint：濾波器估計的位置；AMCL功能包通信架構ROSNavigation功能包介紹3.AMCL節點的服務（1）Gloval_localization（std_srvs/Empty）：初始化全局定位，所有粒子完全隨機分布在地上；（2）Request_nomotion_update（std_srvs/Empty）：手動更新粒子并發布更新后的粒子；（3）Static_map（nav_msgs/GetMap）：AMCL調用此服務接收地圖，用于基于激光掃描的定位。394知識鏈接導航功能包參數文件配置404知識鏈接CONTENT目錄項目導入01項目任務02知識鏈接04項目準備05學習目標03任務評價07任務拓展08項目小結09任務實施06項目準備0543項目準備51.檢查機器人急停開關，若急停開關被按下，需將其旋開；2.一臺配置有Ubuntu18.04+ROSmelodic環境或虛擬機環境的電腦；3.電腦和機器人處于同一局域網網段，且網絡連接正常；4.實驗場地沒有影響或遮蔽激光雷達探測的障礙物；5.實驗場地已掃描的可用地圖文件；6.預習知識鏈接中的內容，重點在ROS導航功能包的通信架構以及各參數文件的配置信息。CONTENT目錄項目導入01項目任務02知識鏈接04任務實施06學習目標03任務評價07任務拓展08項目小結09項目準備05任務實施0646項目準備51.配置機器人導航應用的工作環境工作環境配置：/devel/setup.bash配置文件PC端和機器人端共享ROS環境配置：/~./bashrc和/ect/hosts配置文件。2.配置ROSNavigation導航功能包導航功能包由功能包cruzr_tutorials中的navigation.launch文件啟動。該文件會啟動amcl、move_base、map_server三個節點，并加載相關的插件和配置文件。47任務實施63.啟動ROSNavigation導航功能包ssh連接機器人連接命令：sshcruiser@<ip>啟動機器人遙控器

命令：roslaunchcruzr_tutorialsteleop.launch準備機器人端地圖文件。打開文件夾，在左下角點擊“OtherLocations”

486任務實施3.啟動ROSNavigation導航功能包準備機器人端地圖文件。界面右下角會出現一行“ConnecttoServer”和一個輸入框，這里我們就可以輸入機器人的名稱然后連接進入其文件系統。在輸入框中輸入“sftp://ubt-robot”。點擊connect。496任務實施在彈出窗口中輸入用戶名cruiser和默認密碼aa3.啟動ROSNavigation導航功能包準備機器人端地圖文件。506任務實施連接成功后，我們就能看到文件夾左側出現了一個名為“ubt-robot”的磁盤，我們就可以直接復制電腦端的文件到機器人端了。3.啟動ROSNavigation導航功能包準備機器人端地圖文件。516任務實施把保存需要使用的地圖都復制到目錄/cruzr_open3.啟動ROSNavigation導航功能包準備機器人端地圖文件。526任務實施3.啟動ROSNavigation導航功能包在機器人端開啟navigation啟動文件命令格式：roslaunchcruzr_tutorialsnavigation.launchmap_file:=<地圖絕對路徑>536任務實施3.啟動ROSNavigation導航功能包電腦端啟動rviz進行觀察：命令為：roscdcruzr_tutorials/rvizrviz-dnavigation.rviz開啟rviz

546任務實施3.啟動ROSNavigation導航功能包電腦端啟動rviz進行觀察：rviz界面顯示的信息

556任務實施4.使用可視化工具rviz進行重定位在導航之前，需要在當前地圖上進行機器人重定位，使得實時的激光數據與地圖障礙物信息匹配。在可視化界面rviz中進行重定位的方式有兩種：局部重定位和全局重定位。開啟rviz開啟rviz566任務實施4.使用可視化工具rviz進行重定位(1)局部重定位局部重定位的操作如下：首先判斷初始狀態下機器人位姿是否準確，比如判斷機器人的真實位置與在地圖中的位置是否差異很大，并且圖中目前激光數據是否與地圖中的障礙物重合點擊界面上方的“2DPoseEstimate”進行定位：我們大概找到一個機器人所在的真實位置，在地圖中按下鼠標并往機器人正面朝向的方向拖出箭頭，這就確認了機器人的位置然后我們就會看到界面中的定位粒子群收斂，激光數據變動。開啟rviz開啟rviz576任務實施4.使用可視化工具rviz進行重定位(1)局部重定位機器人局部重定位前

機器人局部重定位后

586任務實施機器人局部重定位前

機器人局部重定位后

4.使用可視化工具rviz進行重定位(1)局部重定位596任務實施4.使用可視化工具rviz進行重定位(2)全局重定位全局重定位是另一種定位方式，這種方式不需要我們預先給機器人在地圖上定一個大概的位置，我們可以直接用命令行來啟動定位。rosservicecall/global_localization"{}"606任務實施(2)全局重定位使用teleop遙控機器人運動，若粒子群逐漸收斂到正確位置，激光數據和地圖重合程度高，如右圖，則全局重定位成功，否則重新執行命令。全局重定位前

全局重定位成功

616任務實施5.使用可視化工具rviz進行導航測試機器人定位成功后，我們就可以在當前位置開始單點導航了。與局部重定位操作類似，我們點擊rviz界面上方的“2DNavGoal”，在地圖上設置導航目標點位置及方向，然后機器人就會自動規劃路線并開始導航。在rviz上進行導航626任務實施5.使用可視化工具rviz進行導航測試通過可視化工具rviz觀察規劃的全局路徑、局部路徑，全局代價地圖和局部代價地圖，以及激光雷達的數據信息。全局路徑由全局路徑規劃器計算，為當前機器人位置到目標點的最優路徑，局部路徑由TEB局部路徑規劃器計算，根據局部和全局代價地圖不斷動態調整，為后續一系列時間點機器人將要執行的速度規劃。rviz界面顯示的機器人導航過程

636任務實施機器人導航前地圖狀態

5.使用可視化工具rviz進行導航測試646任務實施5.使用可視化工具rviz進行導航測試機器人導航中地圖狀態

656任務實施機器人導航后地圖狀態

5.使用可視化工具rviz進行導航測試666任務實施5.使用可視化工具rviz進行導航測試導航過程中，機器人遇到障礙物，會自動進行躲避并選擇其他合適的路徑，若當前規劃無法執行，機器人會停止運動并進入恢復模式，模式將重復執行清除代價地圖、機器人旋轉的行為，若恢復過程中找到另外的路徑，則繼續導航到目標點，若所有恢復流程完成后仍沒有有效的規劃，則顯示導航失敗。若導航過程中需要取消導航，可在機器人端新建終端中發送取消命令：rostopicpub/move_base/cancelactionlib_msgs/GoalID--{}開啟rviz開啟rviz676任務實施5.使用可視化工具rviz進行導航測試另外，在程序執行過程中，amcl和move_base很多參數可以通過rqt_reconfifigure工具進行動態調整。在電腦端開啟一個終端，可以通過動態修改參數，觀察不同參數對定位和導航的影響：rosrunrqt_reconfifigurerqt_reconfifigure開啟rviz