機(jī)器人多種運(yùn)動(dòng)模態(tài)的自主導(dǎo)航設(shè)計(jì)方案

想象我們要去某個(gè)地方游玩，你是不是會(huì)先在腦海中勾勒出一條路線，然后出發(fā)前往這個(gè)地方？

人類的導(dǎo)航如此，那對(duì)于機(jī)器人來講，該如何實(shí)現(xiàn)導(dǎo)航功能呢？

1.機(jī)器人自主導(dǎo)航

我們先來理清自主導(dǎo)航的框架，其關(guān)鍵是自主定位和路徑規(guī)劃。針對(duì)這兩個(gè)核心功能，ROS提供了一套完整的框架支持，收到導(dǎo)航目標(biāo)位置后，機(jī)器人只需要發(fā)布必要的傳感器信息，框架中的功能包即可幫助機(jī)器人完成導(dǎo)航。

其中，move_base功能包實(shí)現(xiàn)機(jī)器人導(dǎo)航中的最優(yōu)路徑規(guī)劃，amcl實(shí)現(xiàn)二維地圖中的機(jī)器人定位。

為了實(shí)現(xiàn)機(jī)器人全局最優(yōu)路徑規(guī)劃與實(shí)時(shí)避障路徑規(guī)劃，move_base需要訂閱機(jī)器人發(fā)布的深度傳感器信息（sensor_msgs/LaserScan或 sensor_msgs/PointCloud）和里程計(jì)信息（nav_msgs/Odometry），同時(shí)完整的TF坐標(biāo)變換也是實(shí)現(xiàn)路徑規(guī)劃的重要基礎(chǔ)。

導(dǎo)航框架最終的輸出是控制機(jī)器人的速度指令（geometry_msgs/Twist），這就要求機(jī)器人控制節(jié)點(diǎn)具備解析控制指令中線速度、角速度的能力，并且控制機(jī)器人完成相應(yīng)的運(yùn)動(dòng)。

注：

導(dǎo)航框架所包含的功能包很多，可以直接使用如下命令安裝：

$ sudo apt-get install ros-melodic-navigation

2.機(jī)器人定位

導(dǎo)航功能的順利進(jìn)行，離不開機(jī)器人的精準(zhǔn)定位。自主定位即機(jī)器人在任意狀態(tài)下都可以推算出自己在地圖中所處的位置。ROS為開發(fā)者提供了一種自適蒙特卡羅定位方法（Adaptive Monte Carlo Localization，amcl），這是一種概率統(tǒng)計(jì)方法，針對(duì)已有地圖使用粒子濾波器跟蹤一個(gè)機(jī)器人的姿態(tài)。

給定初始位姿后，AMCL會(huì)在機(jī)器人周圍隨機(jī)撒一些粒子，隨著機(jī)器人的運(yùn)動(dòng)，每個(gè)粒子也會(huì)實(shí)時(shí)跟隨機(jī)器人的速度更新位姿，當(dāng)粒子周邊的環(huán)境狀態(tài)與機(jī)器人差距較大時(shí)，就會(huì)被逐漸淘汰，反之，則會(huì)在機(jī)器人周邊產(chǎn)生更多粒子。以此類推，粒子都集中在機(jī)器人所在位置可能性高的地方，也就是定位的結(jié)果。

為了調(diào)教以上粒子濾波算法，AMCL功能包中可配置的參數(shù)很多，一般初次上手不建議，只需要注意訂閱和發(fā)布的話題名匹配即可，感興趣的小伙伴可以在官網(wǎng)（http://wiki.ros.org/amcl）查看各個(gè)參數(shù)的詳細(xì)介紹，相關(guān)的理論算法請(qǐng)參考《概率機(jī)器人》一書。

3.機(jī)器人路徑規(guī)劃

機(jī)器人知道自己的位置后，如何像人一樣根據(jù)自己的經(jīng)驗(yàn)規(guī)劃出一條路徑來呢？move_base功能包就是負(fù)責(zé)這樣的功能，主要由全局路徑規(guī)劃器和本地實(shí)時(shí)規(guī)劃器（局部路徑規(guī)劃器）組成。

全局規(guī)劃就好比我們依靠經(jīng)驗(yàn)（或地圖數(shù)據(jù)）規(guī)劃點(diǎn)到點(diǎn)最優(yōu)路徑的過程；局部規(guī)劃可以理解為去往目的地途中，不斷調(diào)整機(jī)器人姿態(tài)、躲避障礙物，以貼合最優(yōu)路徑的過程。

全局路徑規(guī)劃常用Dijkstra算法和A*算法。Dijkstra算法深度優(yōu)先，往往可以找到全局最優(yōu)路徑，不過搜索時(shí)間長、消耗資源多，而A*算法加入了啟發(fā)函數(shù)，雖然不一定可以找到全局最優(yōu)路徑，但搜索時(shí)間更快，適合大空間范圍的規(guī)劃。移動(dòng)機(jī)器人大部分是在室內(nèi)有限范圍內(nèi)使用，兩者搜索時(shí)間和消耗資源的差距并不明顯，一般使用Dijkstra算法即可。

本地實(shí)時(shí)規(guī)劃常用Dynamic Window Approaches（DWA）和Time Elastic Band（TEB）算法，兩種算法的核心思想如下，具體算法實(shí)現(xiàn)大家可以在網(wǎng)上搜索相關(guān)的論文。

為了完成以上這些規(guī)劃器算法的配置，move_base功能包的啟動(dòng)也需要配置不少參數(shù)，比如下圖所示的這些參數(shù)文件：

包含通用配置文件（costmap_common）、全局規(guī)劃配置文件（global_costmap）、本地規(guī)劃配置文件（local_costmap）、本地規(guī)劃器（planer）、路徑規(guī)劃算法（全局base_global_planner和局部base_local_planner）和控制頻率（controller_frequency）、規(guī)劃頻率（planner_frequency）等一些參數(shù)。這些參數(shù)的詳細(xì)介紹可以去官網(wǎng)（http://wiki.ros.org/move_base）查看。

4.代價(jià)地圖的配置

人在導(dǎo)航去某個(gè)地方的時(shí)候可以通過眼睛和經(jīng)驗(yàn)判斷哪里有墻、哪里有水坑、哪里能走和哪里不能走。機(jī)器人如何判斷呢？

move_base導(dǎo)航使用兩種代價(jià)地圖存儲(chǔ)周圍環(huán)境中的障礙信息：一種用于全局路徑規(guī)劃（global_costmap），一種用于本地實(shí)時(shí)路徑規(guī)劃（local_costmap）。兩種代價(jià)地圖需要使用一些共用的或獨(dú)立的配置文件：通用配置文件（costmap_common_params）、全局規(guī)劃配置文件（global_costmap_params）和本地規(guī)劃配置文件（local_costmap_params）。

通用配置文件costmap_common_params.yaml的代碼和關(guān)鍵參數(shù)的含義如下：

注意話題名不要設(shè)置錯(cuò)了，footprint也需要根據(jù)實(shí)際機(jī)器人的大小進(jìn)行設(shè)置。

全局規(guī)劃配置文件global_costmap_params.yaml的代碼和關(guān)鍵參數(shù)的含義如下：

由于我們這里是已經(jīng)有地圖了，所以將static_map設(shè)置true。若我們想使用機(jī)器人探索未知的地方并且進(jìn)行建圖的話，則將static_map設(shè)置為false。

本地規(guī)劃配置文件local_costmap_params.yaml的代碼和關(guān)鍵參數(shù)的含義如下：

其中width、height、resolution參數(shù)用于設(shè)置代價(jià)地圖的長（米）、高（米）和分辨率（米/格）。rolling_window參數(shù)用來設(shè)置在機(jī)器人移動(dòng)過程中是否需要滾動(dòng)窗口，以保持機(jī)器人處于中心位置。

既然導(dǎo)航的各個(gè)問題都已經(jīng)梳理清楚，那么是否真的可以實(shí)現(xiàn)導(dǎo)航呢？以及在不同運(yùn)動(dòng)模態(tài)下，機(jī)器人的導(dǎo)航效果又會(huì)怎樣？接下來我們就用LIMO機(jī)器人試一試。

注：關(guān)于該機(jī)器人運(yùn)動(dòng)模態(tài)的配置與SLAM，請(qǐng)參考以下內(nèi)容：

《 細(xì)數(shù)移動(dòng)機(jī)器人那些常用的運(yùn)動(dòng)模態(tài) 》

《 如何在移動(dòng)機(jī)器人中部署Gmapping SLAM算法 》

5. 差速移動(dòng)機(jī)器人自主導(dǎo)航

打開機(jī)器人的啟動(dòng)文件limo_start.launch，將use_mcnamu設(shè)置為false，如下圖所示。

局部規(guī)劃器選用的算法為TEB，由于相關(guān)參數(shù)比較多，這里就不一一展示和說明，若想了解每個(gè)參數(shù)，可以前往官網(wǎng)查看相關(guān)說明：http://wiki.ros.org/teb_local_planner

下面是一些參數(shù)的配置與介紹：

注意，當(dāng)運(yùn)動(dòng)模式為差速運(yùn)動(dòng)時(shí)，y方向的速度必須設(shè)置為0。為了導(dǎo)航效果更好，這里將最小轉(zhuǎn)彎半徑（min_turning_radius）設(shè)置為0。

注意min_obstacle_dist不要設(shè)置的比costmap_common_params.yaml中的膨脹半徑小，不然會(huì)報(bào)錯(cuò)。

然后運(yùn)行下面的命令，開啟導(dǎo)航功能。

?

?

?

$ roslaunch limo_bringup limo_start.launch
$ roslaunch limo_bringup limo_navigation_diff.launch

啟動(dòng)的rviz如圖所示，可以通過途中黃色箭頭所指的工具設(shè)定機(jī)器人的初始位姿，通過紅色箭頭所指示的工具設(shè)定機(jī)器人的目標(biāo)位姿。機(jī)器人實(shí)現(xiàn)效果如下：

6. 全向移動(dòng)機(jī)器人自主導(dǎo)航

更改LIMO機(jī)器人的運(yùn)動(dòng)模態(tài)，打開機(jī)器人的啟動(dòng)文件limo_start.launch，將use_mcnamu設(shè)置為true，如下圖所示：

為了更好地體現(xiàn)出全向運(yùn)動(dòng)的效果，這里局部規(guī)劃器選用的算法為TEB。下面是一些參數(shù)的配置與介紹：

與差速模式不同，全向模式可以橫向移動(dòng)，所以y方向的速度不必設(shè)置為0，而是可以設(shè)置為任意速度。

weight_kinematics_nh用于調(diào)整順應(yīng)縱向運(yùn)動(dòng)和非順應(yīng)橫向運(yùn)動(dòng)（掃射）之間的權(quán)衡。當(dāng)此參數(shù)設(shè)置比較大時(shí)，將傾向于縱向運(yùn)動(dòng)；當(dāng)此參數(shù)設(shè)置較小時(shí)，將更傾向于與橫向運(yùn)動(dòng)。weight_optimaltime參數(shù)為最優(yōu)時(shí)間權(quán)重，如果設(shè)置較大時(shí)，那么機(jī)器人會(huì)在直道上快速加速，并且路徑規(guī)劃的也會(huì)切內(nèi)道。

運(yùn)行下面的命令，開啟導(dǎo)航功能。??

?

?

$ roslaunch limo_bringup limo_start.launch
$ roslaunch limo_bringup limo_navigation_mcnamu.launch

7. 阿克曼移動(dòng)機(jī)器人自主導(dǎo)航

更改LIMO機(jī)器人的運(yùn)動(dòng)模態(tài)，打開機(jī)器人的啟動(dòng)文件limo_start.launch，將use_mcnamu設(shè)置為false，如下圖所示。

由于阿克曼運(yùn)動(dòng)只適用TEB算法，所以我們這里仍然選用TEB算法。下面是一些參數(shù)的配置與介紹：

我們這里也將y方向的速度設(shè)置為0，但是最小轉(zhuǎn)彎半徑此時(shí)不能設(shè)置為0，而是需要根據(jù)機(jī)器人的實(shí)際最小轉(zhuǎn)彎半徑來設(shè)置。為了方便測(cè)量出機(jī)器人的最小轉(zhuǎn)彎半徑，我們可以用遙控器讓機(jī)器人以最大角速度做圓周運(yùn)動(dòng)，然后就可以用尺子測(cè)量出機(jī)器人的最小轉(zhuǎn)彎半徑。注意設(shè)置的最小轉(zhuǎn)彎半徑不能小于最小轉(zhuǎn)彎半徑。

然后運(yùn)行下面的命令，開啟導(dǎo)航功能。

?

$ roslaunch limo_bringup limo_start.launch

$ roslaunch limo_bringup limo_navigation_ackerman.launch

8. 總結(jié)

以上只演示了各種運(yùn)動(dòng)模態(tài)下使用某一種本地實(shí)時(shí)規(guī)劃算法的導(dǎo)航效果，每種本地實(shí)時(shí)規(guī)劃算法都有其優(yōu)缺點(diǎn)，各個(gè)算法介紹如下圖，可以根據(jù)需求選擇相應(yīng)的算法。

編輯：黃飛

?