Using ROS Navigation Package 实现导航

这个攻略是为一些有自己的硬件平台，想快速上手ROS navigaion的同学（老师 etc。。）准备的，并且假设大家已经对ROS的基本概念（进程间通讯topic service 数据类型 msg）有了基本的了解，有基本的C++/python编程技术，有基本的移动机器人技术概念。我的攻略与yuanbo she前辈的exbot_xi平台介绍有些不同，我将着眼与更深一层的讲解，期望大家能针对不同的硬件平台都能得心应手。

1. 首先是你需要的硬件平台：

一个可以与你上位机（运行ROS linux）通过某种硬件总线通信的移动平台。（移动平台需要能接受上位机的速度 指令，并且向上位机返回里程计数据，一般是编码器的累计值）

一个RGBD-camera （kinect xition etc。。。）或者一个激光雷达（hokuyo rplidar etc。。。）（这两个对以后的很多部分都有影响，会在后面仔细讲解）

以上是基本要求，一般移动平台类似与turtlebot或者exbot_xi都已经有非常成熟的ROS接口，大家问得比较多的也是ROS接口这部分的问题，我一直用的都是我们嵌入式同学做的底盘，我就着重讲一下这个接口怎么写。

首先是速度接口：

一般来说，导航规划层（不管是用什么自主移动的package），直接输出都是一个topic “cmd_vel”, 里面的数据类型为 geometry_msgs/Twist 这个数据类型表示的是3d空间中的速度，2d的移动机器人只会用到三个值 linear.x linear.y 与 angular.z 分别表示水平分速度，垂直分速度，与角速度。而对于移动平台来说，下位机大多接受到的是几个轮子分别的角速度（如果是封装完全的移动平台，可以跳过这一部分），我们需要将一个描述一个平面刚体的三个分速度映射成驱动部分的速度，这里涉及到一个底盘运动学解算的问题，一般来说分为差动底盘（turtlebot那种通过两个轮子转速不同来实现转弯）和全向底盘这两种，不同的底盘对应的解算公式也是不同的，详见自主移动机器人导论（Introduction to Autonomous Mobile Robots）第二章的讲解，具体公式可以搜索网上论文。

做完解算后一般情况就可以发送给下位机了，与下位机通讯的手段多种多样，则使用的编程接口也相应的不同。我用过串口与CAN总线，推荐使用c++ boost 串口与 python-can。

但是很多情况下针对不同的移动平台的下位机，有些细节需要注意：

1. 发送的频率：这个要看嵌入式部分的要求。

2. 关于速度的平滑： 对于规划层而言，即使有加速度等一些参数的限制，它输出的速度值可能还是对于下位机过于不友好（比如过大的加速减速，不定的发送频率等等），那么就在速度接口这边就要执行一个平滑的过程，turtlebot中给出了一个非常好的速度插值的包（http://wiki.ros.org/yocs_velocity_smoother）大家看情况使用。

总体来说，流程就是订阅 “cmd_vel”topic 然后将速度做处理，发送给下位机执行。

再者是里程计接口：

一般导航都会要求一个里程计数据的输入，这个可以解释为“通过编码器的转动推测轮子在时间片中的位移，进一步算出机器人整体的位移与速度”。和速度接口类似，一般移动平台返回的是轮子转角，需要做逆运动学解算，结果为机器人中心相对与计算开始的“原点”。

基本的编程教程如下（http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom）

这个部分需要注意的地方主要就是发布频率了。这个频率涉及到之后的costmap更新与坐标系的访问超时问题，之后会仔细讲解。

都准备好了之后，那就要对这几个部分进行调试，就我的经验而言，问题最多的部分是里程计部分，介绍大家一个小技巧，在rviz中显示 odometry ，然后移动机器人，观察其是否能回到原点。

硬件部分非常重要，我认为一定要在结合ROS的可视化工具进行充分的调试，要不然在之后的开发中会非常麻烦，硬件的问题比软件的问题隐蔽得多，而且难以定位。

还有就是传感器接口：

一般rgbd-camera与激光雷达都有相应的sensor driver 提供，roslaunch相应文件就可以了。

传感器一般在消息的 header中都需要相应的传感器采集坐标系，对于固定的传感器使用 http://wiki.ros.org/tf#static_transform_publisher tf中的static_transform_publisher 给定传感器与机器人中心相对位姿就可以了。

1. SLAM 与 navigaion ROS 工具综述：

很多应用ROS做 自主导航的新手都不清楚ROS提供的这些工具和工具之间的关系，接下来我将总体阐述下这些。

ROS 中的重要相关部分部分：

tf ： 坐标转换库，提供实时的坐标转换查询与更改服务。 它将坐标转换表示为树形结构，而坐标转换数据的更新速度直接影响 其他节点的实时性，进而导致整个系统的运行出错，出问题大部分也是在这部分。

actionlib：提供动作（action ROS中表示一些长时间，连续性的行为）的编程接口，提供动作执行时的查看状态，终止， 抢占等一系列功能。ROS中的自主移动规划层向上的编程接口一般都是通过这个库。

（可选了解）

pluginlib ： 提供可配置的组件（配置文件就可以规定组件的添加与更换，提供了运行时改变组件的可能性）navigaion 中提供了对于多种planner的支持。

dynamic_reconfigure : 提供运行时改变算法（节点）参数的功能。

SLAM：

提到SLAM， 在社区中应用最多的应该是 Gmapping 与 hector_slam这两种2d slam方法了，其实，SLAM算法的实现不得不说的就是 这个（http://openslam.org/），这上面有几乎所有主流slam方法的C++/C实现，ROS的 gmapping就是调用的这个上面的代码。不管是视觉还是激光雷达算法都非常丰富。

机器人的导航规划部分：

我了解最深的是ROS navigation metapackage ， 一般它输入为激光雷达（使用rgbd-camera 效果不佳）与里程计数据，输出为cmd_vel 机器人在2d平面上的速度。与之类似的是hector_navigation。这一部分主要解决安全路径的规划问题。

1. ROS navigation 软件框架介绍

如果要让 navigation 跑起来，有些知识我们是必须知道的。

（1） navigaion总体介绍：

这个图绝对是最好的。

我们可以清楚得看见之前说的navigation的输入：里程计odometry， 激光雷达或者rgbd-camera的信息sensor_topics，还有已知的先验地图（可选），坐标系变换信息，输出就是cmd_vel速度。

从软件架构角度来讲move_base类 作为navigation的逻辑核心存在。

从移动机器人体系结构来说，move_base规定了整个规划层的行为流程。

而如果要配置ROS navigation，重点就是move_base 与它组件的配置。

定位与navigation meta package的关系：

这一部分是论坛里面问得最多的。也是最不好搞清楚的部分。

首先我想先补充一些ROS tf tree的知识。

ROS 中对于多坐标系的处理是使用树型表示，在机器人自主导航中，ROS会构建这几个很重要的坐标系：

base_link: 一般位于tf tree的最根部，物理语义原点一般为表示机器人中心，为相对机器人的本体的坐标系。

odom：一般直接与base_link 相链接，语义为一个对于机器人全局位姿的粗略估计。取名来源于odometry（里程计），一般这个坐标系的数据也是来源于里程计。对于全局位姿的估计方法很多，比如在hector SLAM与导航体系中，就采用了imu数据估计全局位姿，还有很多视觉里程计的算法（visual odometry）也能提供位姿估计。原点为开始计算位姿那个时刻的机器人的位置。之

odom_combined 这个tf一般为好几种位姿估计方法的信息融合后的数据。在navigation metapackage中有 robot_pose_ekf 这个包是用扩展卡尔曼滤波算法（EKF）融合不同传感器的数据。

map: 一般与odom（或者odom_combined）相连，语义为一个经过先验（或者SLAM）地图数据矫正过的，在地图中的位姿信息。与odom同为全局坐标系。原点为地图原点（地图原点在地图相应的yaml文件中有规定）。

一个完整的ROS navigation 运行时的tf tree 如下：

（base_footprint 坐标系不是必须的）

而通过在tf， 你就可以在程序里询问机器人在全局坐标系中的信息了。这对于路径规划是非常重要的。

在机器人定位与导航体系中，定位模块作为规划层的输入与参考数据所存在。而对于ROS navigation 体系而言，因为它先天的模块间通讯方式实现了模块间的完全解耦，所以对于导航规划层而言（具体就是move_base 这个node），什么定位方法，静态还是动态的地图，对于导航层内部几乎没有区别。在这种思想指导下，navigation metapackage 中 就有了为仿真器环境下的定位工具包fake_localization： 用于把仿真器中的位姿（就是直接吧odom 变换成map）估计直接变换成关于全局地图的定位，简化定位部分；对于动态创建的地图slam， gmapping 在ROS中提供从 odom -》map的坐标转换，也可以作为navigation 中 move_base 的输入。

从定位这部分扩展开来， 对于导航规划层来说，仿真器还是实物传回来的数据这些都无所谓。只要有相应的数据就可以执行相应功能。所以我们配置navigation的时候思路就是先把数据流接对，然后再根据自己机器人硬件与执行任务的不同修改相应的参数。

（2）navigation 各个部分的解析：

具体每个部分的参数列表在wiki上都有，我把一些比较重要的概念挑出来了。

move_base: 在之前说过了， move_base 部分作为navigation的逻辑核心。它实现了一个供上层节点调用的action 接口（通过actionlib 实现），即给定坐标信息与相应坐标位置，执行相应的安全导航动作。对于下层控制器，之前说过了输出为cmd_vel 2d速度。 它规定了这个navigation的行为。

ROS navigation 导航规划层提供一个在良好的定位条件下，安全导航到指定目标坐标的功能。

总体可以视为一个 慎思-反应 混合范式。

行为层： move_base 综合机器人状态与上层指令，给出机器人当前行为：正常导航，执行恢复动作，给上层节点返回失败，终止导航。其中恢复动作可以自己定义。

全局规划层：global_planner

局部规划层： local_planner

控制器层（一般就是之前自己写的速度发送部分）

costmap_2d ： 这一部分可看作为navigation的输入处理器。不同的传感器输入的数据差异很大（激光雷达 & RGBD-camera）通过costmap_2d ， 不同的数据被处理成统一的格式：栅格地图，权值用经过概率方法处理过的，表示空间中障碍物，未知与安全区域。生成出来的costmap则是planner的输入。

global_planner ： 为navigation的全局规划器，接受costmap生成的 global costmap 规划出从起始点到目标点的路径，为local_planner 作出参考。

local_planner ： 为navigation 的局部规划器，接受costmap 生成的local costmap 规划出速度。

recovery_behavior : 规定move_base 行为集合中处理异常情况的行为

这是主体部分。 要理解ROS navigation 最重要的部分是nav_core: 这个包里面就包含了global_planner ， local_planner 与 recovery_behavior的基类的头文件。但是极其重要。

我之前提到过的pluginlib , 而最重要的就是： 在ROS navigation中， move_base 提供的是框架，在move_base 中是通过nav_core 中规定的planner 与 recovery_behavior 的基类的接口进行调用。与具体的实现方法隔离开来。而具体采用的方法由pluginlib 根据不同参数导入。这样的实现方法使得navigation的可定制性大大增加。像base_local_planner 中就实现了两种局部路径规划方法，global_planner 实现了A* 与Dijkstra 两种方法，在navigation_experimental 中还有更多这样的实现。这赋予了这个框架很大的灵活性。通过不同的配置方法可以让navigation适应很多不同的任务。

（3）navigation 的配置

完成与嵌入式层的交互后，就可以着手配置navigation了。基本就是按照官网上这个教程把launch文件和相应的yaml文件配起来就好了（http://wiki.ros.org/navigation/Tutorials/RobotSetup）。我这里就不赘述了，我就讲一下最容易出问题的几个地方。

对于导航规划层来说，整个系统的表现与实时性息息相关，就我个人对于ROS navigation的实践来说，制约表现好坏的最重要一部分就是costmap的生成。costmap会分别生成两份，local_costmap 与 global_costmap 。这两份的参数是完全不同的。先是local_costmap ，local_planner 要求的实时性还是挺高的（特别是你把速度调高的时候），而local_costmap 所依赖的全局坐标系一般是odom，绘制costmap的时候会反复询问odom-》base_link 坐标系的信息，tf数据延迟要是大了会影响costmap，进而导致机器人planner实时性降低，机器人移动迟缓或者撞上障碍物。所以有个参数transform_tolerance一定要慎重。如果是使用静态先验地图做导航，那么全局的costmap可以选择使用static_map选项，这样的话在move_base 创建之初就会根据先验地图生成一次，以后不会再更新了。这样会节省一些计算量。

而如果采用动态地图（实时slam出来的）或者根本不使用先验地图，那可以将全局的costmap所依赖的全局坐标系也改为odom， rolling_window选项代替static选项，这样costmap就会实时更新，要注意的是这样的话你上层程序给出的目标点就不能超过rolling_window的范围。

基本调试好这些navigation就可以初步的运行起来了，而之后就是针对不同环境与不同任务对规划器进行选择与调试。

附录： 一些ROS中的视觉里程计算法：

http://wiki.ros.org/libviso2

http://wiki.ros.org/fovis_ros

amcl 包中的定位算法（adaptive monte carlo localization）与 gmapping中的基本概率方法：

《Probabilistic Robotics》

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