ROS 学习篇（八）机器人的导航功能－－navigation

ROS 学习篇（八）机器人的导航功能－－navigation

1. 概述

ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是，如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程。作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时，为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能，配置导航功能包的一些参数。

• 在ROS中，进行导航需要使用到的三个包是：

  （1） move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；
  （2） gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；
  （3） amcl：根据已经有的地图进行定位。
  源码解析：https://blog.csdn.net/qq_27753669/article/details/80011156
  

• 在总体框架图中可以看到，move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分：

  （1） 全局路径规划（global planner）：根据给定的目标位置进行总体路径的规划.

• 在ROS的导航中，首先会通过全局路径规划，计算出机器人到目标位置的全局路线。这一功能是navfn这个包实现的。

• navfn通过Dijkstra或A*最优路径的算法，计算costmap上的最小花费路径，作为机器人的全局路线。

  （2） 本地实时规划（local planner）：根据附近的障碍物进行躲避路线规划。

• 本地的实时规划是利用base_local_planner包实现的。该包使用Trajectory Rollout 和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度（dx，dy，dtheta velocities）。

• base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。

• 其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：
  （1） 采样机器人当前的状态（dx,dy,dtheta）；
  （2） 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。
  （3） 利用一些评价标准为多条路线打分。
  （4） 根据打分，选择最优路径。
  （5） 重复上面过程。

２. 硬件要求

尽管导航功能包设计得尽可能通用，但是仍然对机器人的硬件有以下三个要求：
（1）导航功能包仅对差分等轮式机器人有效，并且假设机器人可直接使用速度指令进行控制，速度指令的格式为：x方向速度、y方向速度、速度向量角度。
（2）导航功能包要求机器人必须安装有激光雷达等二维平面测距设备。
（3）导航功能包以正方型的机器人为模型进行开发，所以对于正方形或者圆形外形的机器人支持度较好，而对于其他外形的机器人来讲，虽然仍然可以正常使用，但是表现则很有可能不佳。

３. 机器人配置

3.1、ROS
首先，请确保你的机器人安装了ROS框架。

3.2、tf变换(sensortransforms)
导航功能包要求机器人以tf树的形式发布各个相关参考系的变换关系。

• base_link 和 base_laser 间的tf转换 核心代码：

  tf::TransformBroadcaster broadcaster;
  

  broadcaster.sendTransform(tf::StampedTransform(
          tf::Transform(tf::Quaternion(0,0,0,1),tf::Vector3(0.1,0.0,0.2)),
          ros::Time::now(),"base_link","base_laser"));
  

       /*通过TransformBroadcaster来发送转换关系，需要附带5个参数。
   1.旋转变换
   ２．平移变换
   ３、时间戳
   ４、ｐａｒｅｎｔ节点名字
   ５、ｃｈｉｌｄ节点名字
   */ 
  

查看tf关系：

• 使用rqt_tf_tree
  rosrun rqt_tf_tree rqt_tf_tree
• 使用tf_echo
  rosrun tf tf_echo [parent_frame_id] [child_frame_id]

3.3、传感器信息(sensor sources)
导航功能包需要采集机器人的传感器信息，以达到实时避障的效果。这些传感器要求能够通过ROS发布sensor_msgs/LaserScan或者sensor_msgs/PointCloud 格式的消息，也就是二维雷达信息或者三维点云数据。ROS社区已经支持大部分激光雷达、Kinect等设备的驱动，可以直接使用社区提供的驱动功能包发布满足要求的传感器信息。如果你使用的传感器没有ROS支持，或者你想使用自己的驱动，也可以自己将传感器信息封装成要求的格式。

Velodyne VLP16驱动.

• Velodyne VLP16型激光雷达横向视角360°，纵向视角30°

roslaunch velodyne_pointcloud VLP16_points.launch

3.4、里程计信息(odometrysource)
导航功能包要求机器人发布nav_msgs/Odometry格式的里程计信息，同时在也要发布相应的tf变换 odom-to-base_link。里程计包含2 方面的信息，一方面是位姿（位置x,y和转角yaw），另一方面是速度（前进速度Vx,Vy和转向速度Vth）。

	tf::TransformBroadcaster odom_broadcaster;
    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
       
//first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;
    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

//next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

• 获取方法：

• IMU数据积分获取
  IMU获取三轴线加速度和三轴角速度数据，通过数据解析节点封装成数据类型为sensor_msgs/Imu的消息，通过ROS提供的相关包imu_tools进行滤波，得到滤波后的sensor_msgs/Imu的消息

• 电机编码器获取
  前进速度： 左右轮的平均速度， 这个是通过电机的编码器获取到的。
  转向速度：根据左右轮的在给定时间内的弧度差计算得到
  位置的获取：根据上面的前进速度推算出位置。
  姿态的获取：根据上面的转向速度推算出转角。

• laser_scan_matcher　package(indigo)
  目前,laser_scan_matcher在kinetic下还没有可以apt-get的包,要使用,必须要从源码编译,编译方法参考，ros kinetic 下编译laser_scan_matcher 方法

• robot_pose_ekf　package
  robot_pose_ekf开启扩展卡尔曼滤波器生成机器人姿态，
  -订阅的话题 ：(最少两个)
  - odom(nav_msgs/Odometry)（编码器）
  　用到topic中的信息：
  　　　odom->header.stamp　时间戳
  　　　odom->pose.pose.orientation　四元数
  　　　odom->pose.pose.position.x　ｘ位置
  　　　odom->pose.pose.position.y　ｙ位置
  　　　odom->pose.covariance　协方差矩阵６＊６　主对角元素
  - imu_data　(sensor_msgs/Imu)（IMU）
  　用到topic中的信息：
  　　　 imu->header.stamp　时间戳
  　　　 imu->orientation　四元数
  　　　imu->orientation_covariance　３＊３四元数的协方差矩阵的主对角线元素
  - vo　(nav_msgs/Odometry)（视觉里程计）
  - 还可以支持GPS 即下面介绍的gps_common　package
  -发布的话题：
  robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
  -提供的 tf :
  odom_combined → base_footprint
  发布一个topic， 类型需要注意下是PoseWithCovarianceStamped并非Odometry ，查看该topic信息可以看到odom_ekf订阅了该topic 。

• 查看robot_pose_ekf 的运行状态

rosservice call /robot_pose_efk/get_status

• gps_common　package
  订阅的话题
  fix (sensor_msgs/NavSatFix)
  GPS measurement and status
  发布的话题
  odom (nav_msgs/Odometry)
  UTM-encoded position　
  注：在源程序包中的的解释是 Ubuntu 16.04, the STATUS_DGPS_FIX flag was removed, so the gpsd_client package will be unable to indicate whether DGPS was used in a fix or not. ，所以在编译时会出错，我使用时是将gpsd_client包删除，编译通过（没搞清楚gpsd_client包是干啥的。。）。

运行转换节点

rosrun gps_common utm_odometry_node

3.5、机器人控制器（base_controller）
导航功能包集假定它可以通过话题"cmd_vel"发布geometry_msgs/Twist类型的消息，这个消息基于机器人的基座坐标系，它传递的是运动命令。这意味着必须有一个节点订阅"cmd_vel"话题， 将该话题上的速度命令(vx, vy, vtheta转换为电机命令(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z)发送给移动基座。

3.6、地图（map_server）
在导航过程中，地图并不是必须的，此时相当于是在一个无限大的空地上进行导航，并没有任何障碍物。但是考虑到实际情况，在我们使用导航的过程中还是要一个地图的。
**
3.7机器人位姿
robot_lacalization

４.costmap_2d

4.1基本概念
Voxel：体素，即顾名思义是体积的像素。用来在三维空间中表示一个显示基本点的单位。类似于二维平面下的pixel（像素）。voxel是三维空间中定义一个点的图象信息的单位。在平面中定义一个点要两个坐标X和Y就够了，而在三维世界中还要有一个坐标。光有3维坐标位置还不行，还要有颜色等信息，这就是voxel的含义。

机器人在costmap_2D中的模型：两个同心圆，一个机器人的轮廓外切圆和一个机器人内切圆，机器人在costmap里就能够简化成为这两个圆。根据机器人中心至边界或者障碍物的距离和两个同心圆半径比较来判断是否碰撞。

footprint：足迹，即机器人的轮廓。在ROS中，它由二维数组表示[x0,y0] ; [x1,y1] ; [x2,y2]……不需要重复第一个坐标。该占位面积将用于计算内切圆和外接圆的半径，用于以适合此机器人的方式对障碍物进行膨胀。为了安全起见，我们通常将足迹稍大于机器人的实际轮廓。要确定机器人的占地面积，最直接的方法是参考机器人的图纸。 此外，您可以手动拍摄其基座顶视图。 然后使用CAD软件（如Solidworks）适当缩放图像，并将鼠标移动到基座轮廓上并读取其坐标。 坐标的起点应该是机器人的中心。 或者，您可以将机器人移动到一张大纸上，然后绘制基座的轮廓。 然后选择一些顶点并使用标尺来确定它们的坐标。

cost：代价或者占用，0-255的取值，表示该机器人位于该网格点(grid cell)的代价，或者机器人的frootprint中心cell走到该网格点的代价（中心到达某个位置的代价与非中心部分到达某个位置付出的代价不同，如撞击造成的损伤程度等）。

cell：单元，网格，栅格

初始化costmap_2d :: Costmap2DROS对象的方法主要有两种。
第一种是使用用户生成的静态地图进行初始化（有关构建地图的文档，请参阅map_server包）。在这种情况下，代价地图被初始化来匹配与静态地图提供的宽度，高度和障碍物信息。这种方式通常与定位系统（如AMCL）结合使用，允许机器人在地图坐标系注册障碍物，并且当基座在环境中移动时可以根据传感器数据更新其代价地图。
第二种是给出一个宽度和高度，并将rolling_window参数设置为true。当机器人在移动时，rolling_window参数将机器人保持在代价地图的中心。然而当机器人在给定区域移动得太远时，障碍物信息将会被从地图中丢弃。这种方式的配置最常用于里程计坐标系中，机器人只关注局部区域内的障碍物。

４.2 costmap的网格代价计算
无论是激光雷达还是如kinect 或xtion pro深度相机作为传感器跑出的2D或3D SLAM地图，都不能直接用于实际的导航，必须将地图转化为costmap(代价地图),ROS的costmap通常采用grid(网格)形式。以前一直没有搞明白每个栅格的概率是如何算出来的，原因是之前一直忽略了内存的存储结构，栅格地图一个栅格占1个字节，也就是八位，可以存0-255中数据，也就是每个cell cost（网格的值）从0~255我们只需要三种情况：Occupied被占用（有障碍）, Free自由区域（无障碍）, Unknown Space未知区域。

Costmap是机器人收集传感器信息建立和更新的二维或三维地图，可以从下图简要了解。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2HaoDSP8-1589599216738)(http://wiki.ros.org/costmap_2d?action=AttachFile&do=get&target=costmap_rviz.png)]

空间状态（Occupied, Free, and Unknown）
ROS的代价地图（costmap）采用网格（grid）形式，每个网格的值（cell cost）从0~255。分成三种状态：被占用（有障碍）、自由区域（无障碍）、未知区域；以激光雷达为传感器（或者kinect之类的深度相机的伪激光雷达），根据激光测量的障碍物距离机器人中心的距离，结合机器人的内切和外切半径，搞一个映射，利用bresenham算法（计算方法参考https://www.cnblogs.com/zjiaxing/p/5543386.html）可以填充由激光雷达的位置到障碍物之间的栅格概率了。

虽然代价地图中每个cell可用255个不同值中任何一个值，可是下层数据结构仅需要3个值。 具体来说在这种下层结构中，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 当投影到代价地图时候，每种状态被赋一个特定的代价值，也就是说每个cell的cost值是由这个cell对应的各层中对应的cell的状态进行加权得到的。 如果列有一定量的占用就被赋代价值costmap_2d::LETHAL_OBSTACLE， 如果列有一定量的unknown cells 就被赋代价值costmap_2d::NO_INFORMATION, 剩余其它列赋代价值为costmap_2d::FREE_SPACE。
**

在某时刻和机器人当前前进方向上的网格点的代价计算示意图如下（如果机器人的前进方向改变，则网格点的代价也会发生变化）：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-y7cThppY-1589599216740)(http://wiki.ros.org/costmap_2d?action=AttachFile&do=get&target=costmapspec.png)]
　　　　　　　　　　　机器人模型以及在当前前进方向上各grid的代价分布示意图

上图下面的红色五边形区域为机器人的轮廓。坐标系内的区域可分为五部分，（cost的计算是以cell为单位进行的，而不是以某个障碍物为单位进行的，也就是一次只计算一个cell的cost值）
（1） Lethal（致命的）:机器人的中心（center cell）与该网格的中心重合，此时机器人必然与障碍物冲突。
（2） Inscribed（内切）：网格中心位于机器人的内切轮廓内，此时机器人也必然与障碍物冲突。
（3） Possibly circumscribed（可能受限）：网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于靠在障碍物附近，所以不一定冲突，取决于机器人的方位或者说姿态。
（4） Freespace（自由空间）：网格中心位于机器人外切圆之外，属于没有障碍物的空间。

（5） Unknown（未知）：未知的空间。

如果按照cell的三种状态划分，我个人认为上述前3种状态，都属于“被占用”状态。

这里介绍一下costmap_2d中计算cost的方法。假设，机器人内切半径为0.5m，外切半径为0.7m，当激光返回障碍距离在机器人中心附近，叫致命障碍，机器人一定能碰到障碍物，比如说0m,直接贴着机器人，或会取小于栅格的边长，比如小于0.1m范围内,则这个栅格值就设为254；当返回来的数值在0.1-0.5m之间，就设253；当在0.5-0.7之间，则可以设128，或者在252-128找个比例值（程序中可以控制），属于受限区域，可能发生碰撞，是否碰撞，取决于机器人的姿态；当0.7-膨胀半径之间，设1-127之间的映射值，不会发生碰撞；当大于膨胀距离，则设为0，称为freespace。Unknown – 意味着给定的单元没有相应的信息。我们看坐标系中较细的红色光滑曲线就是cost曲线，x是距离机器人footprint的圆心距离，而y是cost值，cost随着x的增大而减小距离，当x>=内切圆半径时开始有值；当x=0时，y=254；当x=resolution/2时，cost=253；（图中右上角的较粗的台阶状红线是单元格的边线，或者认为是障碍物（单元格化后）的边线）

4.3. Costmap的分层与更新
4.3.1 Costmap中的地图与分层
Costmap_2D提供了一种2D代价地图的实现方案，该方案利用输入传感器数据，构建数据2D或者3D（依赖于是否使用基于voxel的实现）代价地图。 此外，该包也支持利用map_server初始化代价地图，支持滚动窗口的代价地图，支持参数化订阅和配置传感器主题。

从Hydro发布版本开始， 用来写数据到代价地图的底层方法已经完全可配置了。 Costmap由多层组成，每种功能放置一层中。 例下图所示，静态地图是一层，障碍物是另一层。 缺省情况下，障碍物层维护的是3D信息，3D障碍物数据可以让层更加灵活的标记和清除障碍物。例如在costmap_2d包中，StaticLayer（静态地图层）是第一层，ObstacleLayer（障碍物层）是第二层，InflationLayer（膨胀层）是第三层。这三层组合成了master map（最终的costmap），供给路线规划模块使用。
　　　　　　　　　　
　　　　　　　　　　　　costmap_2D中的4个分层（从Hydro版本之后采用这种分层结构）

我自己定义的障碍物也可以是一层（假如我不想让机器人通过一个freespace就可以自己插入个障碍物，主要的接口是costmap_2d::Costmap2DROS，在每一层中使用pluginlib实例化Costmap2DROS并将每一层添加到LayeredCostmap），各个层可以被独立的编译。如下图所示：

costmap_2d包提供了一种可配置框架来维护机器人在代价地图上应该如何导航的信息。 代价地图使用来自传感器的数据和来自静态地图中的信息，通过costmap_2d::Costmap2DROS来存储和更新现实世界中障碍物信息。costmap_2d::Costmap2DROS给用户提供了纯2D的接口，这意味着查询障碍只能在列上进行。例如，在XY平面上位于同一位置的桌子和鞋，虽然在Z方向上有差异但是它们在costmap_2d::Costmap2DROS对象代价地图中对应的cell上拥有相同的代价值。 这种设计对平面空间进行路径规划是有帮助的。

costmap_2D提供的ROS化功能接口主要就是costmap_2d::Costmap2DROS，它使用costmap_2d::LayeredCostmap 来跟踪每一层。 每一层在Costmap2DROS中以插件方式被实例化，并被添加到LayeredCostmap。 每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改，即LayerdCostmap为Costmap2DROS（用户接口）提供了加载地图层的插件机制，每个插件（即地图层）都是Layer类型的。costmap_2d::Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构。

　　　　　　　　　costmap中的Layer的继承关系

costmap中各Layer之间的继承关系如上图所示，本文后面我们附的layeredcostmap的相关介绍还会用到这个图。

４.3.2 Costmap初始化流程
在navigation的主节点move_base中(costmap隶属于navigation包，或者说是navigation的一个子模块)，建立了两个costmap。其中planner_costmap_ros_是用于全局导航的地图，controller_costmap_ros_是用于局部导航用的地图。下图为costmap的初始化流程。
　　　　　　　　　
　　　　　　　　　　costmap的初始化流程

（1）Costmap初始化首先获得全局坐标系和机器人坐标系的转换
（2）加载各个Layer，例如StaticLayer，ObstacleLayer，InflationLayer。
（3）设置机器人的轮廓
（4）实例化了一个Costmap2DPublisher来发布可视化数据。
（5）通过一个movementCB函数不断检测机器人是否在运动
（6）开启动态参数配置服务，服务启动了更新map的线程。
参考：https://blog.csdn.net/jinking01/article/details/79455962