《鴻蒙機(jī)器人編程》課件-4-1-移動(dòng)機(jī)器人SLAM與導(dǎo)航

1.ROS導(dǎo)航與定位課程內(nèi)容移動(dòng)機(jī)器人導(dǎo)航概述SLAM技術(shù)介紹移動(dòng)機(jī)器人定位技術(shù)介紹移動(dòng)機(jī)器人路徑規(guī)劃介紹代價(jià)地圖全局路徑規(guī)劃局部路徑規(guī)劃行為恢復(fù)ROSNavigation功能包參數(shù)介紹?NXROBO20222移動(dòng)機(jī)器人導(dǎo)航概述移動(dòng)機(jī)器人導(dǎo)航?NXROBO20224移動(dòng)機(jī)器人導(dǎo)航：機(jī)器人能自主地從當(dāng)前位置無碰撞地運(yùn)動(dòng)到指定的目標(biāo)位置。導(dǎo)航模塊的輸入和輸出分別是：目標(biāo)位置、運(yùn)動(dòng)速度（運(yùn)動(dòng)指令）設(shè)置目標(biāo)位置輸入：目標(biāo)位置輸出：運(yùn)動(dòng)速度底盤控制器控制指令導(dǎo)航模塊導(dǎo)航要解決3個(gè)問題：我在哪？我將要去哪？我如何到達(dá)？?NXROBO20225Navigation是ROS中最常用的子系統(tǒng)，是移動(dòng)機(jī)器人導(dǎo)航相關(guān)包的集合，實(shí)現(xiàn)定位規(guī)劃避障等相關(guān)功能Navigation提供了諸多功能包ROS中的Navigation導(dǎo)航系統(tǒng)ROS中的Navigation導(dǎo)航系統(tǒng)?NXROBO20226Navigation導(dǎo)航包總體框架地圖定位路徑規(guī)劃ROS中的Navigation導(dǎo)航系統(tǒng)?NXROBO20227ROS中的Navigation導(dǎo)航系統(tǒng)?NXROBO20228Navigation這個(gè)導(dǎo)航系統(tǒng)的架構(gòu)設(shè)計(jì)比較復(fù)雜，但是卻非常經(jīng)典以至于后來出現(xiàn)的其他機(jī)器人系統(tǒng)，在導(dǎo)航部分都基本借鑒了Navigation這個(gè)經(jīng)典架構(gòu)該架構(gòu)將導(dǎo)航功能分為三大塊：地圖構(gòu)建(SLAM)機(jī)器人定位路徑規(guī)劃導(dǎo)航要解決3個(gè)問題：我在哪？我將要去哪？我如何到達(dá)？SLAM技術(shù)SLAM的核心思想?NXROBO202210SLAM：叫同步定位與構(gòu)圖（SimultaeousLocalizationAndMapping），即在機(jī)器人移動(dòng)時(shí)，同時(shí)估計(jì)機(jī)器人的位姿和環(huán)境地圖?NXROBO202211SLAM的輸入和輸出輸入輸出?NXROBO202212按傳感器類型分：SLAM的分類?NXROBO202213按所采用的方法分：SLAM的分類?NXROBO202214基于圖優(yōu)化的SLAM處理的數(shù)學(xué)模型?NXROBO202215SLAM的一般框架?NXROBO202216SLAM的一般處理流程?NXROBO202217ROS中流行的SLAM功能包Gmapping：最為常用和成熟的slam功能包，其集成了Rao-Blackwellized粒子濾波算法，訂閱機(jī)器人的深度信息、IMU信息和里程計(jì)信息，同時(shí)完成一些必要參數(shù)配置，即可創(chuàng)建并輸出基于概率的二維格柵地圖hector_slam：使用高斯牛頓方法，不需要里程計(jì)數(shù)據(jù)，只根據(jù)激光信息進(jìn)行地圖構(gòu)建，該算法對雷達(dá)幀率要求較高，一般要求高于20HZ。若雷達(dá)幀率較低，就要調(diào)低機(jī)器人運(yùn)行速度小于0.3m/s，如果不降低機(jī)器人行駛速度，建圖效果會(huì)非常差，甚至無法完成建圖Cartographer：可以實(shí)現(xiàn)機(jī)器人在二維和三維條件下的定位及建圖功能。目前該算法采用基于圖網(wǎng)絡(luò)的優(yōu)化方法，主要基于激光雷達(dá)來實(shí)現(xiàn)SLAMRTAB-Map：是具有實(shí)時(shí)約束的全局閉環(huán)檢測的RGB-DSLAM方法……?NXROBO202218GmappingGmapping概述?NXROBO202219GmappingGmapping的相關(guān)話題?NXROBO202220CartographerCartographer的總體框架移動(dòng)機(jī)器人定位?NXROBO202222蒙特卡洛定位方法蒙特卡洛方法簡介?NXROBO202223蒙特卡洛定位方法蒙特卡洛定位（MCL,MonteCarloLocalization）?NXROBO202224蒙特卡洛定位方法自適應(yīng)蒙特卡洛方法簡介移動(dòng)機(jī)器人路徑規(guī)劃?NXROBO202226回顧：navigation總體框架路徑規(guī)劃全局路徑規(guī)劃局部路徑規(guī)劃代價(jià)地圖?NXROBO202227代價(jià)地圖costmap：分類與構(gòu)成?NXROBO202228代價(jià)地圖costmap：分類與構(gòu)成局部代價(jià)地圖全局代價(jià)地圖全局地圖?NXROBO202229全局路徑規(guī)劃主要任務(wù)：從外部獲得導(dǎo)航的目標(biāo)點(diǎn)，然后在全局代價(jià)地圖里找出“代價(jià)最小”的那條路線，這條路線就是我們苦苦尋找的導(dǎo)航路線輸入：目標(biāo)點(diǎn)和全局代價(jià)地圖輸出：全局導(dǎo)航路徑點(diǎn)，作為局部路徑規(guī)劃的輸入全局路徑規(guī)劃算法基于搜索的規(guī)劃算法A*、Dijkstra算法、D*、D*Lite、Floyd算法、BellmanFord算法、……基于采樣的規(guī)劃算法概率路圖算法(ProbabilisticRoadMap,PRM)、快速隨機(jī)擴(kuò)展樹算法(Rapidly-exploringRandomTree,RRT)、PRM和RRT的變種、……?NXROBO202230Navigation中全局路徑規(guī)劃的實(shí)現(xiàn)?NXROBO202231局部路徑規(guī)劃有了全局路徑，按理說直接循線走即可，無奈世界變化太快，需要個(gè)機(jī)靈點(diǎn)的老司機(jī)去應(yīng)對路上可能的突發(fā)情況，這位老司機(jī)就是局部規(guī)劃器主要任務(wù)：從全局規(guī)劃器獲得導(dǎo)航路線，根據(jù)這個(gè)路線向機(jī)器人發(fā)送速度，驅(qū)動(dòng)機(jī)器人“盡可能”按照路線去行走輸入：全局路徑和局部代價(jià)地圖輸出：機(jī)器人的速度局部路徑規(guī)劃算法基于采樣的算法：動(dòng)態(tài)窗口法（dwa）基于數(shù)值優(yōu)化的算法：mpc、teb基于曲線擬合的算法：貝塞爾曲線、多項(xiàng)式曲線、B樣條曲線?NXROBO202232Navigation中局部路徑規(guī)劃的實(shí)現(xiàn)?NXROBO202233動(dòng)態(tài)窗口法(dwa)的思路基本思路：一定程度上采用了粒子濾波的思想，在速度空間（v,w）中采樣多組速度，并模擬出這些速度在一定時(shí)間內(nèi)的運(yùn)動(dòng)軌跡，并通過評價(jià)函數(shù)對這些軌跡進(jìn)行評價(jià)，選取最優(yōu)軌跡對應(yīng)的速度驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)主要步驟：1）采樣機(jī)器人當(dāng)前的狀態(tài)（dx,dy,dtheta）；2）針對每個(gè)采樣的速度，計(jì)算機(jī)器人以該速度行駛一段時(shí)間后的狀態(tài)，得出一條行駛的路線；3）利用一些評價(jià)標(biāo)準(zhǔn)為多條路線打分；4）根據(jù)打分，選擇最優(yōu)路徑；5）重復(fù)上面過程。?NXROBO202234動(dòng)態(tài)窗口法(dwa)的算法流程?NXROBO202235恢復(fù)行為Navigation導(dǎo)航包參數(shù)配置?NXROBO202237Navigation導(dǎo)航包中的參數(shù)amcl_demo_rviz.launch文件（1）啟動(dòng)機(jī)器人導(dǎo)航功能，可以在終端上運(yùn)行如下命令：$roslaunchspark_navigationamcl_demo_rviz.launch?NXROBO202238Navigation導(dǎo)航包中的參數(shù)amcl_demo_rviz.launch文件（2）?NXROBO202239Navigation導(dǎo)航包中的參數(shù)move_base.launch.xml文件?NXROBO202240Navigation導(dǎo)航包中的參數(shù)costmap_common_params.yaml文件?NXROBO202241Navigation導(dǎo)航包中的參數(shù)global_costmap_common_params.yaml文件?NXROBO202242Navigation導(dǎo)航包中的參數(shù)local_costmap_common_params.yaml文件?NXROBO202243Navigation導(dǎo)航包中的參數(shù)move_base_params.yaml文件?NXROBO202244Navigation導(dǎo)航包中的參數(shù)dwa_local_planner_params.yaml文件小結(jié)?NXROBO202245簡要介紹了ROSNavigation的總體框架講解了SLAM的基本工作原理及分類簡要介紹了AMCL移動(dòng)機(jī)器人定位方法講解了代價(jià)地圖、全局路徑規(guī)劃、局部路徑規(guī)劃、行為恢復(fù)等移動(dòng)機(jī)器人路徑規(guī)劃相關(guān)內(nèi)容介紹了ROSNavigation功能包參數(shù)?NXROBO202246代價(jià)地圖costmap：初始化2.實(shí)踐課-房間建圖課程內(nèi)容測試SLAM用到的傳感器操控spark機(jī)器人進(jìn)行地圖構(gòu)建（運(yùn)行SLAM程序）讓spark室內(nèi)導(dǎo)航?NXROBO202248啟動(dòng)spark機(jī)器人在終端運(yùn)行一鍵腳本，啟動(dòng)Spark：選擇1讓機(jī)器人動(dòng)起來然后根據(jù)提示，按“回車”鍵啟動(dòng)機(jī)器人49$cd~/spark_noetic$./onekey.sh?NXROBO2022測試深度視覺自動(dòng)啟動(dòng)rviz可視化工具可以看到深度視覺的RGB和深度圖像程序還將深度圖像信息轉(zhuǎn)化為點(diǎn)云信息和激光雷達(dá)信息，這些信息將用于地圖構(gòu)建。50?NXROBO2022里程計(jì)數(shù)據(jù)可視化啟動(dòng)Spark后，可以在rviz觀測里程計(jì)數(shù)據(jù)首先在Displays面板的左下角點(diǎn)擊Add，然后在對話框的Bytopic頁面中選擇/odom，最后點(diǎn)擊OK這樣就可以看到里程計(jì)信息了51?NXROBO2022里程計(jì)數(shù)據(jù)可視化可以在rviz中，看到紅色箭頭。52?NXROBO2022鍵盤遙控把控制終端窗口設(shè)為當(dāng)前窗口，通過WASD鍵控制機(jī)器人移動(dòng)。53?NXROBO2022手機(jī)遙控重新運(yùn)行一鍵腳本，選擇2根據(jù)提示，事先到指定網(wǎng)址下載手機(jī)遠(yuǎn)程遙控App（安卓），并安裝好在手機(jī)App上進(jìn)行相應(yīng)配置按提示操作即可手機(jī)控制SPARK機(jī)器人了54?NXROBO2022地圖構(gòu)建Spark機(jī)器人實(shí)現(xiàn)了多種SLAM算法：gmapping、hector、karto、rtab_map等重新運(yùn)行一鍵腳本，選擇5，可以看到各類SLAM算法，可根據(jù)需要選擇若選擇1，則以gmapping方式構(gòu)建地圖55?NXROBO2022地圖構(gòu)建這時(shí)，使用鍵盤，手機(jī)或者其他方式控制spark就可以建立室內(nèi)地圖當(dāng)然如果需要建立較好的地圖，需要閱讀源碼并依據(jù)機(jī)器人情況認(rèn)真調(diào)整參數(shù)56?NXROBO2022地圖構(gòu)建57?NXROBO2022在地圖繪制完成后，選擇之前彈出的保存地圖終端窗口，進(jìn)行地圖保存，以備下一步導(dǎo)航之用。室內(nèi)導(dǎo)航使用上一步建好的圖即可實(shí)現(xiàn)室內(nèi)導(dǎo)航，可自主避障運(yùn)動(dòng)運(yùn)動(dòng)指定位置在一鍵腳本中輸入7，再選擇1接著在rviz中使用2DPoseEstimate估計(jì)初始位姿然后使用2DNavGoal設(shè)置目標(biāo)點(diǎn)這樣機(jī)器人即可避開障礙物運(yùn)動(dòng)到目標(biāo)點(diǎn)處。58?NXROBO2022室內(nèi)導(dǎo)航并且可以通過rostopic查看相關(guān)主題數(shù)據(jù)，移動(dòng)前后的數(shù)據(jù)變化和位姿如下所示：59$rostopicecho/move_base/goalgoal:pose:position:x:1.07281732559y:0.118636846542z:0.0orientation:x:0.0y:0.0z:0.056415118308w:0.998407399024?NXROBO20223DSLAM可進(jìn)一步嘗試使用rtabmap實(shí)現(xiàn)3DSLAM（/rtabmap）。運(yùn)行一鍵腳本，選擇5，再選擇5，就可以通過WASD鍵進(jìn)行3D建圖。60?NXROBO20223DSLAM地圖建完后可以直接使用2DNavGoal進(jìn)行導(dǎo)航61?NXROBO2022Spark一鍵腳本Spark一鍵腳本其實(shí)就是一些Linux及ROS指令的集合，其目的是為了大家更方便地使用我們的機(jī)器人Spark。在學(xué)習(xí)過程中，建議大家打開這個(gè)腳本文件，查看這個(gè)腳本里的功能具體都執(zhí)行了哪些指令，以鞏固和提高學(xué)習(xí)的成果。目前SPARK的源碼在GITHUB上托管并開源，一些新的功能會(huì)