ROS Navigation Stack的整體設計思路和功能包

前言

ROS Navigation Stack是ROS提供的一個二維的導航功能包集合，通過輸入里程計、傳感器信息和目標位姿，輸出控制機器人到達目標狀態(tài)的安全速度指令。

ROS Navigation Stack為移動機器人的導航規(guī)劃提供了比較好的參考，通過實現(xiàn)功能包集合提供的接口，也可以比較容易地將自己的算法應用到移動機器人上。本文將幫助大家理解ROS Navigation Stack的設計思路，并對各個功能包進行講解。

01 概述

點擊可查看大圖 通過ROS wiki上的這張圖，我們可以比較清楚的看到ROS Navigation Stack的整體設計思路：整個功能包集合以move_base為核心，將里程計信息、傳感器信息、定位信息、地圖以及目標點輸入給move_base，move_base經(jīng)過規(guī)劃后會輸出速度指令。move_base包括三個關鍵部分：global_planner（全局規(guī)劃器）、local_planner（局部規(guī)劃器）和recovery_behaviors（恢復行為）。

這三個部分都是以插件的形式實現(xiàn)的，通過插件機制可以方便地切換不同算法實現(xiàn)的規(guī)劃器。恢復行為會在機器人移動過程中出現(xiàn)了異常狀態(tài)時被觸發(fā)，目的是幫助機器人擺脫異常狀態(tài)。另外，move_base還包括了global_costmap（全局代價地圖）和local_costmap（局部代價地圖），規(guī)劃器需要在代價地圖上進行導航規(guī)劃。下面我們來詳細看一下上面提到的各部分內容。

02 odometry（里程計）

簡單來說，里程計的作用就是估計機器人運動的距離和速度。通過閱讀源碼可以得知，在ROS Navigation Stack中，里程計信息有兩個作用，一個作用是提供給局部規(guī)劃器，當局部規(guī)劃器選取最優(yōu)路徑和判斷機器人是否停止的時候會使用到里程計的速度信息，另一個作用就是將估計位姿信息用于定位。

里程計信息一般從機器人底盤的輪式編碼器獲取，當然根據(jù)不同的機器人也可以選擇使用視覺里程計，還可以使用擴展卡爾曼濾波對輪式里程計和IMU進行數(shù)據(jù)融合，得到更加準確的位姿估計。消息類型nav_msgs/Odometry中包括了機器人的位姿和速度以及各自的協(xié)方差。

nav_msgs/Odometry.msg

03 sensor（傳感器）

傳感器數(shù)據(jù)一般來自于激光雷達、IMU和深度相機，可以用于定位和避障。使用傳感器需要設定傳感器參考系與機器人參考系之間的坐標變換關系，也就是常說的tf變換，這樣做是為了表示傳感器感知到的環(huán)境與機器人參考系之間的關系。如果使用amcl算法，激光雷達數(shù)據(jù)會用來與靜態(tài)地圖進行匹配，修正機器人的位姿，得到更加準確的定位。

激光雷達也可以感知到環(huán)境中障礙物的位置，通過將障礙物加入到代價地圖中，實現(xiàn)避障。具體使用哪幾種傳感器，依賴于使用的機器人平臺。理論上來說，使用的傳感器種類越多，定位和避障的效果更好。

04 tf

tf是一個讓用戶隨時間跟蹤多個參考系的功能包，它使用一種樹型數(shù)據(jù)結構，根據(jù)時間緩存并維護多個參考系之間的坐標變換關系，可以幫助用戶在任意時間，將點、向量等數(shù)據(jù)的坐標，在兩個參考系中完成坐標變換。

機器人系統(tǒng)通常有許多隨時間變化的三維參考系，例如世界參考系和機器人參考系。tf會隨著時間的變化跟蹤這些參考系?；赗OS Navigation Stack實現(xiàn)移動機器人的自主導航，必須維護一棵完整的tf樹，即map->odom->base_link->sensor_link。 實際上，我們所說的定位，就是維護map->base_link之間關系的過程。tf樹中記錄了機器人參考系與地圖參考系之間的關系，也就得到了機器人在地圖中的哪個位置，tf樹也記錄了傳感器參考系與機器人參考系的關系，也就得到了感知的數(shù)據(jù)與機器人之間的關系。

點擊可查看大圖

從上面這張圖我們就可以直觀地理解什么是tf變換了，由tf樹幫助我們管理激光雷達與機器人底盤之間的坐標變換關系，當激光雷達感知到某個位置存在障礙物時，通過tf變換就能得到障礙物與機器人底盤之間的距離。

05 map_server

map_server在ROS Navigation Stack中是可選的，其主要的作用就是給機器人導航提供地圖。提供地圖有兩種方式，一種方式是通過SLAM提供實時地圖，另一種方式是提供SLAM提前建立并保存或者通過其他方式制作的地圖，常用的SLAM算法有gmapping和hector_slam等。

一般在比較規(guī)則的場景，可以制作高精度地圖提供給機器人，定位和規(guī)劃都會有比較好的效果。通過SLAM提供實時地圖，需要把實時地圖以話題的形式發(fā)布。通過map_server提供的地圖為pgm格式，通過加載yaml配置文件，將地圖以話題的形式加載到系統(tǒng)中。在yaml文件中可以配置地圖的分辨率，原點以及表示占據(jù)/空閑的概率。下面為yaml配置文件的內容：

·地圖的默認參考系為map。占據(jù)的概率occ = (255 – color_avg) / 255.0，其中color_avg為像素RGB的平均值。

06 amcl（自適應蒙特卡洛定位）

amcl是ROS Navigation Stack中唯一指定的定位算法，全稱為自適應蒙特卡洛定位，它是一種用于在二維環(huán)境中移動的機器人的概率定位系統(tǒng)。簡單概括一下它的原理就是通過在全局地圖中撒粒子，粒子可以理解為機器人的可能位姿。

按照評價標準，例如激光雷達數(shù)據(jù)與地圖的匹配程度給粒子打分，分數(shù)越高代表機器人在這個位置的可能性越大，經(jīng)過粒子濾波器以后留下的就是分數(shù)高的粒子了。

經(jīng)過多次撒粒子，粒子就會集中到機器人位置可能性高的地方，稱之為粒子收斂。自適應其實簡單理解就是會根據(jù)粒子的平均分數(shù)或者粒子是否收斂來增加或減少粒子數(shù)，它能夠有效地解決機器人綁架問題和粒子數(shù)固定問題。

上圖中紅色的粒子簇即為amcl在全局地圖中撒下的粒子，可以看到一開始的粒子簇集中在給定的起始位姿周圍，此時的粒子簇還是比較分散的，隨著機器人的移動，粒子簇逐漸收斂，定位效果還是不錯的。

點擊可查看大圖

amcl在ROS Navigation Stack中的作用就是輸出map->odom的tf變換，來彌補里程計的漂移誤差，它要求在機器人的定位系統(tǒng)中要存在里程計位姿估計，即odom->base_link的tf變換，并給定起始位姿和輸入傳感器數(shù)據(jù)。

07 costmap_2d（代價地圖）

costmap_2d功能包提供了一種二維代價地圖的實現(xiàn)方案，該方案從實際環(huán)境中獲取傳感器數(shù)據(jù)，構建二維或三維的柵格占用地圖，以及基于占用柵格地圖和用戶定義膨脹半徑的二維代價地圖。該包也支持基于map_server初始化代價地圖，基于滾動窗口的代價地圖，以及訂閱和配置傳感器話題。

在ROS Navigation Stack中，代價地圖分為全局代價地圖和局部代價地圖，全局代價地圖使用基于map_server初始化代價地圖，也就是Static Map Layer（靜態(tài)地圖層），局部代價地圖為基于滾動窗口的代價地圖。 代價地圖還包括Obstacle Map Layer（障礙物層）和Inflation Layer（膨脹層），有時候根據(jù)應用場景的需要也可以加入用戶自定義層，用戶自定義層可以用插件來實現(xiàn)。障礙物層是將傳感器感知到的障礙物加入到代價地圖中。 在規(guī)劃的時候，我們會把機器人看做是一個質點，并沒有考慮機器人的實際模型，因此在代價地圖中需要膨脹層，來盡量保證規(guī)劃出的路徑不會使機器人和障礙物發(fā)生碰撞。

點擊可查看大圖

上面這張圖為ROS wiki上的一個示例，可以看到圖中灰色部分即為靜態(tài)地圖，紅色部分為傳感器感知到的障礙物，藍色部分為膨脹層。紅色多邊形表示機器人的形狀，為了避免碰撞，機器人形狀不應該和紅色部分相交，機器人中心點不應該和藍色部分相交。

08 move_base

move_base是整個ROS Navigation Stack的最頂層，它將各個功能模塊組合起來，通過SimpleActionServer接收目標點并完成導航任務。move_base支持任何遵循在nav_core包中指定的nav_core::BaseGlobalPlanner接口的全局規(guī)劃器和任何遵循在nav_core包中指定的 nav_core::BaseLocalPlanner 接口的局部規(guī)劃器。

move_base執(zhí)行的流程是通過狀態(tài)機來控制是執(zhí)行規(guī)劃行為還是恢復行為，當規(guī)劃失敗就會執(zhí)行恢復行為，如果所有恢復行為也都失敗就會退出導航并報告問題。規(guī)劃的流程是通過回調函數(shù)接收目標點，同時會有一個線程不斷地跑全局規(guī)劃器，然后局部規(guī)劃器以一定頻率對規(guī)劃出來的路徑進行跟蹤，計算出速度指令，最終到達目標點就重置規(guī)劃狀態(tài)。

09 nav_core

nav_core功能包為用于導航的機器人行為提供了通用的接口，其中定義了三個抽象類，分別是BaseGlobalPlanner、BaseLocalPlanner和RecoveryBehavior。

點擊可查看大圖

從上圖我們也可以清楚地看到move_base的整個導航流程以及各部分使用的nav_core功能包定義的接口。通過閱讀源碼我們可以知道，BaseGlobalPlanner中定義了純虛函數(shù)makePlan()，makePlan()用于實現(xiàn)規(guī)劃全局路徑。

BaseLocalPlanner中定義了純虛函數(shù)setPlan()、isGoalReached()和computeVelocityCommands()，setPlan()用于實現(xiàn)設定局部規(guī)劃器跟蹤的路徑，isGoalReached()用于實現(xiàn)判斷是否到達目標點，computeVelocityCommands()用于實現(xiàn)計算安全的速度指令。RecoveryBehavior定義了純虛函數(shù)runBehavior()，runBehavior()用于實現(xiàn)執(zhí)行恢復行為。

10 global_planner（全局規(guī)劃器）

對于move_base的全局規(guī)劃器，我們主要了解global_planner功能包。global_planner功能包實現(xiàn)了兩種路徑規(guī)劃算法：A*和dijkstra，并實現(xiàn)了OrientationFilter類，用于對路徑進行優(yōu)化。

global_planner功能包的GlobalPlanner繼承了nav_core:: BaseGlobalPlanner，實現(xiàn)了makePlan()函數(shù)。makePlan()函數(shù)的基本流程就是輸入起點和終點，讀取代價地圖，使用A*或者dijkstra算法進行規(guī)劃，調用OrientationFilter類中的方法對路徑進行優(yōu)化，將規(guī)劃好的路徑以話題的形式發(fā)布。

11 local_planner（局部規(guī)劃器）

局部規(guī)劃器是move_base中最復雜的部分，實現(xiàn)局部規(guī)劃器需要繼承nav_core包的BaseLocalPlanner類。常用的局部規(guī)劃器是dwa_local_planner，dwa_local_planner提供了在二維平面進行局部路徑規(guī)劃的動態(tài)窗口法的實現(xiàn)。動態(tài)窗口法主要是在速度空間中采樣多組速度，并模擬機器人在這些速度下一定時間內的軌跡。在得到多組軌跡以后，對這些軌跡進行評價，選取最優(yōu)軌跡所對應的速度來驅動機器人運動。

局部規(guī)劃器的一個基本流程就是讀取局部代價地圖，設定要跟蹤的全局路徑，對全局路徑進行分段，根據(jù)分段的全局路徑的坐標進行局部規(guī)劃，計算機器人每個周期（采樣周期）內的線速度、角速度，使之盡量符合全局最優(yōu)路徑，并實現(xiàn)實時避障。

以DWA局部規(guī)劃器為例，當規(guī)劃全局路徑的線程planThread在控制循環(huán)中輸出新的路徑時，新的路徑將通過setPlan()傳遞給DWAPlannerROS，直接保存為global_plan_。如果isGoalReached()返回false，程序繼續(xù)執(zhí)行調用computeVelocityCommands()。

在computeVelocityCommands()中會調用getLocalPlan()對全局路徑進行分段，并調用dwaComputeVelocityCommands()使用DWA算法計算最優(yōu)速度，最后在move_base中發(fā)布速度指令。當機器人到達目標位姿且速度小于停止速度時，isGoalReached()返回true。

12 recovery_behaviors（恢復行為）

點擊可查看大圖

上圖是move_base默認的恢復行為的執(zhí)行流程，可以理解為機器人通過旋轉底盤，來清除代價地圖中的障礙物，經(jīng)過清除以后如果可以找到可走的路徑，就繼續(xù)導航，否則認為目標點不可達并報告終止導航任務。

從源碼來看，move_base中包括了clear_costmap_recovery、move_slow_and_clear和rotate_recovery這三個有關恢復行為的功能包。clear_costmap_recovery實現(xiàn)的是將給定區(qū)域內的障礙物從代價地圖中清除。

move_slow_and_clear實現(xiàn)的是清除代價地圖中的障礙物，并在用戶給定的限制速度下移動一定的距離，這可能會導致機器人發(fā)生碰撞。rotate_recovery實現(xiàn)的是通過使機器人旋轉一周來清除代價地圖中的障礙物。

13 總結

本文介紹了ROS Navigation Stack的整體設計思路和各個功能包的作用以及它們之間的聯(lián)系，希望能夠幫助大家更好地理解ROS Navigation Stack，感謝大家的閱讀。

審核編輯：湯梓紅