ROS Navigation Stack的整體設(shè)計(jì)思路和功能包

前言

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

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

01 概述

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

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

02 odometry（里程計(jì)）

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

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

nav_msgs/Odometry.msg

03 sensor（傳感器）

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

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

04 tf

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

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

點(diǎn)擊可查看大圖

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

05 map_server

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

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

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

06 amcl（自適應(yīng)蒙特卡洛定位）

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

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

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

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

點(diǎn)擊可查看大圖

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

07 costmap_2d（代價(jià)地圖）

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

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

點(diǎn)擊可查看大圖

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

08 move_base

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

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

09 nav_core

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

點(diǎn)擊可查看大圖

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

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

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

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

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

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

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

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

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

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

12 recovery_behaviors（恢復(fù)行為）

點(diǎn)擊可查看大圖

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

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

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

13 總結(jié)

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

審核編輯：湯梓紅