Using ROS Navigation Package 实现导航（二）

上一期主要是将如何将navigation 在不同的硬件平台上跑起来,主要讨论的是软硬件接口与配置, 这一期主要着眼点在于通过主要算法原理讲解与参数解析,希望能彻底把一些应用上的问题讲清楚.

 

从移动机器人学的角度来看, ROS navigation package:

两层规划模型global && local planner , costmap_2d 统一 环境表示模型, move_base 组织导航过程中的规划与执行逻辑, 定位数据通过 与tf tree的交互得到, 最大程度上做到了与定位算法细节的解耦. 定位算法在本篇不细讲, 以后开专门的slam 的专题吧. 那就开始了.

 

1. Navigation 之前要做的事情:

上期说过, navigation 部分 主要输入有 两部分,tf tree, 传感器的数据, 而tf tree 除了sensor在机器人本体上的偏移量以外主要就是定位的数据, 所以, navigation上机器人之前所要做的事情, 就是把输入彻底调试正确, 以免出现无法定位问题的情况.

• TF tree: 定位是一个很大的话题, 但是对于导航来说, 定位问题就完全用tf tree 做接口进行了封装. 首先是odom,由广义的里程计数据生成, 确保这个坐标系转换在局部是准确的对于navigation的正常运转来说是非常重要. 而不同的传感器不同算法调试又多种多样, 隔离细节来说, 最好的测试方法就是定量测试, 比如计算1米, 5米, 10米机器人移动观测量与groud truth之间的或者是做loop closure的检测, 走一个环形,看回到原点的error. 在有地图或者slam的情况下, 用于生成map-> odom 的算法也需要良好的调试, 方法和上面类似.
• Sensor data: 这个用ROS传感器驱动和通讯, 基本不会出问题, 主要传感器坐标系到本体坐标系别转错了. 关于不同的传感器对于navigation的适配这个问题放在costmap部分详细解答.

 

2. Navigation 中组件:

1>costmap

首先是costmap, costmap_2d package中实现了 带权栅格地图 的环境表示方式. 作为导航架构中环境表示模块,承担了与传感器数据流交互,暴露接口给planner的任务.这一部分可以说是navigation 中最容易出问题的部分.

Costmap_2d主要流程为输入激光雷达laserscan 或者点云 pointcloud 数据(当然可以自己定义别的), 从tf tree上获取定位数据, 进行已知定位数据的建图 处理. 因为是根据已知定位数据的建图算法,假设定位数据精确. 所以如何选取全局定位坐标系,很大程度上决定了建图质量, 很多问题也源于此.

Costmap 除去参数设置和多线程调度, 已知定位数据的建图 核心步骤是trajectory过程.算法过程如下:

<1 得到当前时刻的机器人当前的全局位置pos, 传感器相对机器人中心坐标系偏移offset, 将不同的传感器数据统一处理成点云, 将传感器中心以及点云数据转换到全局坐标系.

<2 根据传感器模型,从传感器中心到点云的连线, 这一部分空间为没有障碍物的安全空间free, 点云的位置为障碍物所在位置occupied, 根据我们对于costmap中, 栅格cost的定义, 对连线上赋权值. 一般使用bresenham算法将直线离散化到栅格中.

<3 确定了free 和 occupied的栅格, 根据costmap 中对栅格权值的分类,将unkown 和inflation 等部分的权值填上, inflation 使用广度优先的方式进行扩展栅格, 将occupied 的栅格入队, 然后层层扩展,得到膨胀出去的栅格,并赋值.

对于 costmap 栅格cost设置, 没有考虑传感器概率建模,应该也用不着, 因为costmap设计之初是给用激光雷达的机器人使用, 激光雷达在室内使用的误差相对栅格地图的分辨率来说基本可以忽略. 而255 -0 的cost设置并没有限制我们给它新的含义, 我们定制costmap着手点也是这里.

从软件架构来讲, David.Lu 在13 – 14 年接手navigation 重新做的一些工作的很重要的一部分就是重新整理了costmap的软件框架, 通过使用costmap_layer 抽象出costmap 给栅格赋权值的部分,使costmap变得更加灵活.具体做法是将不同类型的栅格赋值过程通过pluginlib进行解耦和隔离, 每一种不同的栅格类型抽象成一层,layer,每一层可以使用不同的传感器数据输入, 不同的栅格赋值策略.David.Lu 写了一个range_sensor_layer接受超声波测距数据sensor_msgs/Range 构建costmap,social_layer, 结合people package 找到人之后将2d激光检测到障碍物中人的部分的cost值修改, 以达到机器人行为的改变. 大家可以参考一下.通过定制这一部分,可以实现costmap对不同应用情况, 不同传感器数据的适配与定制.

Costmap 中比较重要的几个参数:

<1 global_frame & transform_tolerance : costmap获取定位数据, 在tf tree上选择哪个坐标系作为建图的全局坐标系, 由 global_frame 规定. 而定位数据更新,查询tf 数据的tolerance 由 transform_tolerance规定, 如果超出了这个阈值,costmap会有warning. Navigation中, 两层planner 使用的 global frame 一般不同, 看需求来制定, 而transform_tolerance 一般要看tf tree 的完整性和计算机的性能.

<2 observation_source : 规定了costmap的输入, 一般来说,costmap主要数据来源来自于激光雷达,点云数据也可以输入.之前提到过,costmap 会统一将数据处理成点云. 而voxel_grid 参数规定了raytrace 过程是在二维平面上还是三维平面, 虽然最后结果都会被映射到二维.

<3 map_update_rate & publish_frequency : map_update_rate 规定了传感器数据到grid map的更新速率, 跟硬件性能有关, 而publish_frequency 为rviz 可视化costmap数据时, costmap的发布速率, 当使用先验地图生成static costmap, 请将这个参数调低, 大地图发布出去数据传输受不了.

<4 static_map or rolling_window : static_map 参数规定costmap是否通过先验地图直接生成, 而rolling_window 规定数据由实时传感器数据生成, 以robot_base_frame坐标系原点为滑动窗口中心, 需要规定窗口大小, 分辨率等参数. 过大的窗口与过小的分辨率对计算性能都是考验, 根据需求选择吧.

 

2>Local Planner

Navigation中规划分了两层,较为底层的是local planner,负责做局部避障的规划. 具体navigation中实现的算法是Dynamic Windows Approach, 因为速度的采样空间不同, ROS中的两种实现DWA和 trajectory rollout 有细微的差别. 这个方法最早96年提出. 具体流程如下:

<1 由移动底盘的运动学模型得到速度的采样空间.

在给定的速度加速度限制下, 在给定时间间隔下, 没有碰撞的速度为admission velocity.

(这些给定的限制都是我们需要调试的参数)

<2 在采样空间中, 我们计算每个样本的目标函数:

NF =α ⋅vel + β ⋅nf +γ ⋅Δnf +δ ⋅ goal

Vel 当前速度值

Nf  到当前目标点的相关的cost 值

Δnf 与全局路径的贴合程度的cost 值

Goal 到全局目标点的距离值.

还有一些cost可以自己定义, navigation实现中还有对最大最小障碍物距离的cost 与倾向于向前走的cost.

然后α,β,γ,δ 都是权重参数, 调节这些参数可以极大影响机器人避障行为

<3 得到期望速度, 插值成轨迹输出给move_base

由以上算法流程,costmap作为检验速度采样值是否合法, 计算障碍物距离的依据存在, 所以local_planner 出了问题首先检查local_costmap 是否正常. Local planner 需要订阅里程计信息获得当前机器人移动速度, 所以保证里程计odom topic中twist部分的正确性也是非常重要的.

local planner 部分是navigation中参数最多, 最能直接影响机器人行为的部分. 调节local planner 中的 acceleration & velocity limit 可以直接控制机器人速度/加速度上限. 而设置holonomic_robot 与 y_vels 参数, 是否采样y 方向速度, 全向移动机器人可以通过调整这些参数实现平移避障等动作.

Local_planner的调试在于观察机器人行为, 而有具体的应用场景时, 适当增加与删除cost function 中的项可以取得不错的效果, 比如在机器人跟随人的动作中, 期望能控制机器人使得人一直在传感器视角之内, 适当将人的信息加入cost function 就可以达到相应效果,而不用自己重新实现一遍避障算法, 最大程度上复用navigation.

对于local planner, local costmap global frame 的选取一般是odom, odom坐标系在局部准确性满足要求而且实时性比map 更好(少经过一层处理). 一般要把rolling window选上, 因为局部避障要考虑先验地图中没有的障碍物.

3>Global Planner

全局路径规划navigation中实现的方法是基于栅格地图的cost 搜索找最优, 而我们所喜闻乐见得A*与dijkstra 只是扩展栅格路径的不同方式而已, dijstra 一般消耗时间和空间都较A*要多,但是A*找到的一般不是全局最优解,而这两种经典方法任何一本讲移动机器人路径规划的书上都有很详细的说明,一下不在赘述.值得说明的是.navigation中的这两层规划之间联系纽带就是local_planner中的cost function 有将全局路径考虑在内, 也就是说 local_planner 输出的local path会有靠近global path 的趋势, 而具体情况得看当时的速度以及障碍物情况(cost function 去最大值而global path 只是一个因素), 当然这个行为我们也是可以通过更改权值参数调整的.

对于global costmap, global frame的选取一般要看机器人需求, 如果是在已知地图中, 使用static map预生成costmap是又快又好的方法, 使用map坐标系做全局坐标系, 但是如果要执行一些探索任务, 没有先验地图, 定位数据选取就看你手上有什么类型的数据了, rolling_window也是看需求选取.

4>Move base

这个模块负责整个navigation 行为的调度, 包括初始化costmap与planner, 监视导航状态适时更换导航策略等. 涉及到行为的控制, move_base 具体实现就是有限状态机, 定义了若干recovery_behavior , 指定机器人导航过程中出问题后的行为. 出问题有这几种情况:

<1 控制失败, 找不到合法的速度控制量输出, 一般就是local_planner 出问题了.

<2 规划失败, 找不到合法的路径, 一般就是global planner 出问题了.

<3 局部规划震荡, 局部规划器在某些特定情况下, cost function 出现局部最小, 具体表现为以一个较小的速度来回移动,这个时候机器人光靠local planner无法给出合适的速度控制量.

前两种情况, 一般就是planner 在当前的cost map中无法找到合法路径, 所以recovery_behavior一般是清除costmap 中的occupied grid,重新通过传感器数据生成costmap, 这样可以消除一些costmap 建图不准产生的杂点所导致的失败.

后一种情况, move_base 会给底盘发送一小段时间的很小的速度.通过主动移动给一个扰动逃离local planner cost function的局部最小.

在前两轮的recovery 失败后,move_base默认会采用一种更加激进的方式,原地旋转来企图清除costmap中的障碍物.所以很多navigation 失败情况之前都会原地转上好几圈, 这个行为完全可以通过move_base的参数进行更改,recovery_behavior也是通过pluginlib构建,自己定制也是完全可行.

3. 一些经验:

使用默认参数,只要把navigation的输入搞好, 表现一般是不会差的.

逐模块调试, 步步为营, 基于原理理解navigation才是解决问题之道.

看准需求定制navigation可以完成绝大多数室内导航任务.

转自：西工大一小学生http://blog.exbot.net/archives/2308