最近一直在看京天Turtlebot3 waffle pi的导航部分,这篇文章就介绍一下相关内容。导航模块是一个独立完整的模块,内容比较多也很深入。因为笔者没有看过源码,只是一些概念上的了解,做个整理,为后续的源码阅读做准备。本文整体上分为两个的大部分,第一部分针对Turtlebot下Navigation模块的理论介绍,第二部分结合京天Turtlebot3 waffle pi做一些具体应用阐述。

  关于Navigation的介绍,起点要从这张图开始:

  图中的方块有三种颜色,代表不同含义。白色的部分是必须且已实现的组件,灰色的部分是可选且已实现的组件,蓝色的部分是必须为每一个机器人平台定制创建的组件。简单点儿说,就是一个机器人开发商,白灰都可以直接拿轮子用,蓝色的需要自主开发。
  Navigation整体可以划分成几个子模块:map_server、amcl、move_base

  1)map_server

  严格上来讲map_server不算Navigation里面的,只是使用到了其中的一些功能。map_server最直接的功能就是提供了地图(静态地图可输入,所以提供的应该是实时地图),map_server会发布/map话题,实时的获取一些激光雷达信息。

  2)amcl

  amcl全称是adaptive Monte Carlo localization,自适应蒙特卡洛定位。主要功能就是负责定位(机器人位姿确定),输入是激光雷达(包括激光地图,激光扫描)、里程计的数据、tf转换,输出机器人在地图中的位姿。这个作为可选的因素是在Navigation也有默认的定位方式:里程计直接定位(fake_localization)。二者的对比关系如下图:

图源:https://blog.csdn.net/x_r_su/article/details/53396564
留一个amcl的学习资料:https://www.bilibili.com/video/BV18D4y1c7jz/?spm_id_from=333.337.search-card.all.click

  很多学者对于tf总能看到,却没有一个明确的概念。借此机会介绍下tf转换树:
  tf变换树:管理不同坐标系之间的平移和旋转,用树形结构保证存在单向的转换。树中每个节点代表一个坐标系,节点之间的连线代表坐标系转换。tf库已经集成好,只要添加节点关系就能实现自动转换
  tf库学习:https://blog.csdn.net/x_r_su/article/details/53332959
  tf作用:确定机器人在物理世界位置以及传感器数据和静态地图关系

  3)move_base

  move_base是Navigation中最核心的内容,它主要负责的功能是路径规划以及驱动控制。move_base包含四个组成:全局规划器(global_planner)、局部规划器(local_planner)、代价地图(costmap)、恢复行为(recover_bechivor)。
  move_base的整体工作流程是:目标点信息→全局规划器(利用全局代价地图信息计算全局路径)→局部规划器(计算速度)→控制器。
  设定目标点以及静态地图,全局规划就会规划一个大概路径,然后在路径上产生一些路径点,把这些路径点交给局部规划器进行局部规划,最后由局部规划器控制移动。
  Move_base是基于状态机的,三个状态
  1)PLANNING状态 全局路径规划状态
  2)CONTROLLING状态 控制移动状态(判断是否到达目标点)
  3)CLEARING状态 执行恢复行为状态

  下面更为详细的介绍下四个组成部分:

  (1)global_planner

  关于global_planner没有太多的东西,这里就简单说一下分类:global planner,navfn planner

  (2)local_planner

local_planner稍微重要一点,先说下分类:base_local_planner、eband_local_planner、teb_local_planner、dwa_local_planner
  关于这四种,详细的介绍下dwa_local_planner(因为这个是京天实际选择的)
  dwa全称为Dynamic Window Approach,这种方法在1997年,这里附上原论文:The Dynamic Window Approach to Collision Avoidance 1997
  dwa整体的算法逻辑是先离散采样,对于每个采样速度结合当前状态进行移动模拟,结合几个方面评价正向模拟轨迹并打分,选择分最高。
  直接这么看可能不太好理解,结合一个图说一下:

图源:https://scm_mos.gitlab.io/motion-planner/dwa-local-planner/
  dwa的算法步骤先通过离散采样获取一些状态信息,然后对每一个采样进行前向模拟(就是上图的虚线路径),路径会有好几条,对每条进行打分,最后选择分最高的。至于打分的评判有以下七个方面:

参考:https://blog.csdn.net/weixin_44126988/article/details/126147666?spm=1001.2014.3001.5502
  在实现上dwa_local_planner的大部分处理在base_local_planner中,大部分包括实际的路径规划、打分的实现

  (3)costmap

  costmap是平面的,所以也常常被叫做costmap_2d。这个有两类:local_costmap和global_costmap。分别针对于global_planner和local_planner。其余方面没有差别。
  明确costmap的作用:保存障碍物信息,所以costmap的来源有两个——静态地图和传感器数据。上面说costmap是2d,其实是由无数个小方格组成的地图,每一个格都有代价值,两个格的差值就是移动代价。
  在实现上costmap以层的结构维护,层包括static map layer,obstacle map layer,inflation layer,自定义层(自定义层存在的价值是可以随意控制,即使没有障碍物也能够避开该区域)
  有了costmap就可以规划路径了,常用的路径有以下两种:
    栅格路径:终点开始上下左右四个点最小的直到起点
    梯度路径:周围八个栅格中下降梯度最大的点
map_server负责初始化costmap_2d

  (4)恢复行为

  恢复行为发生在导航出现故障时,这个故障指暂时找不到合适的路径。就会执行一些行为恢复之前的状态,行为比如抛弃之前的costmap,在现实中的体现就是机器人会原地旋转,不过也是可以自行设置的。
以上对move_base的四个组成部分进行了介绍,其实在实现上move_base对于每个模块提供接口,以插件的形式供ros使用,然后做整合。所以每一个部分都是可以自主定制的。

  下面结合一下京天Turtlebot3_waffle_pi对上面的理论进行下夯实
  执行命令:

roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

  这里面的map.yaml其实就是静态地图的导入,再此说明下参数含义:

来看看turtlebot3_navigation.launch,可以分为下面几个部分

  其中Turtlebot3_remote.launch是waffle_pi的模型导入,map_server提供一些地图操作,amcl.launch提供定位,move_base.launch提供路径规划。turtlebot3_navigation.rviz提供可视化。
  京天Turtlebot3_waffle_pi使用的local_planner默认是dwa_local_planner,它的参数放在了dwa_local_planner_params_waffle_pi.yaml中,里面有很多参数,对参数含义进行一下解释:

DWAPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.26    #x方向最大线速度绝对值,单位m/s,(负值代表可以后退)
  min_vel_x: -0.26   #x方向最小线速度绝对值,单位m/s,(负值代表可以后退)

  max_vel_y: 0.0   #y方向最大线速度绝对值,单位m/s,差分机器人没有y
  min_vel_y: 0.0   #y方向最小线速度绝对值,单位m/s,差分机器人没有y


#注意不要将min_trans_vel设置为0,否则DWA认为平移速度不可忽略,将创建较小的旋转速度
# The velocity when robot is moving in a straight line(纯直线移动时,与前面max_vel_x不冲突)
  max_vel_trans:  0.26      #最大平移速度绝对值,单位m/s
  min_vel_trans:  0.13      # 最小平移速度绝对值,单位m/s

  max_vel_theta: 1.82   #最大旋转角速度绝对值,单位rad/s
  min_vel_theta: 0.9     #最小旋转角速度绝对值,单位rad/s

  acc_lim_x: 2.5   #x方向极限加速度,单位m/(s^2)
  acc_lim_y: 0.0   #y方向极限加速度,单位m/(s^2),差分机器人y为0
  acc_lim_theta: 3.2 #旋转极限加速度,单位rad/(s^2)

# Goal Tolerance Parametes(目标距离差距容忍参数)
  xy_goal_tolerance: 0.05 #xy方向上容忍误差,单位m
  yaw_goal_tolerance: 0.17 #控制器偏航时弧度容忍误差,单位rad
  latch_xy_goal_tolerance: false #设置为true时表示:如果到达容错距离内,机器人就会原地旋转;即使转动是会跑出容错距离外
#注:这三个参数的设置及影响讨论请参考《ROS导航功能调优指南》

# Forward Simulation Parameters
  sim_time: 2.0     #前向模拟轨迹时间,单位为s
  vx_samples: 20   #x方向速度空间采样点数
  vy_samples: 0     #y方向速度空间采样点数,差分机器人为0
  vth_samples: 40  #旋转方向速度空间采样点数
  controller_frequency: 10.0   
#注:参数的设置及影响讨论请参考《ROS导航功能调优指南》

# Trajectory Scoring Parameters (估计评分参数)
  path_distance_bias: 32.0   #控制器与给定路径接近程度的权重(越大说明越贴合)
  goal_distance_bias: 20.0   #控制器与局部目标点接近程度的权重,也用于速度控制
  occdist_scale: 0.02   #控制器躲避障碍物的程度
  forward_point_distance: 0.325  #以机器人为中心,额外放置一个计分点的距离
  stop_time_buffer: 0.2  #为防止碰撞,机器人必须提前停止的时间长度,单位为s
  scaling_speed: 0.25  #开始缩放机器人足迹时的速度的绝对值,单位为m/s。
  #在进行对轨迹各个点计算footprintCost之前,会先计算缩放因子。如果当前平移速度小于scaling_speed,则缩放因子为1.0,否则,缩放因子为(vmag - scaling_speed) / (max_trans_vel - scaling_speed) * max_scaling_factor + 1.0。然后,该缩放因子会被用于计算轨迹中各个点的footprintCost。
  # 参考:https://www.cnblogs.com/sakabatou/p/8297479.html
  #亦可简单理解为:启动机器人底盘的速度.(Ref.: https://www.corvin.cn/858.html)
  max_scaling_factor: 0.2   #最大缩放因子

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05    #机器人重置震荡标记的运动距离

# Debugging
  publish_traj_pc : true   #规划的轨迹是否在rviz上可视化
  publish_cost_grid_pc: true #是否将代价值可视化显示


#整篇参数注释参考:[https://www.codeleading.com/article/62641792851/

 还有导航时的一些可查询信息,此处列举了rosnode和rostopic,最后附带一下它们的节点话题图

user@ubuntu:~$ rostopic list
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
/battery_state
/cmd_vel
/cmd_vel_rc100
/diagnostics
/firmware_version
/imu
/initialpose
/joint_states
/magnetic_field
/map
/map_metadata
/map_updates
/motor_power
/move_base/DWAPlannerROS/cost_cloud
/move_base/DWAPlannerROS/global_plan
/move_base/DWAPlannerROS/local_plan
/move_base/DWAPlannerROS/parameter_descriptions
/move_base/DWAPlannerROS/parameter_updates
/move_base/DWAPlannerROS/trajectory_cloud
/move_base/NavfnROS/plan
/move_base/cancel
/move_base/current_goal
/move_base/feedback
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/global_costmap/footprint
/move_base/global_costmap/inflation_layer/parameter_descriptions
/move_base/global_costmap/inflation_layer/parameter_updates
/move_base/global_costmap/obstacle_layer/parameter_descriptions
/move_base/global_costmap/obstacle_layer/parameter_updates
/move_base/global_costmap/parameter_descriptions
/move_base/global_costmap/parameter_updates
/move_base/global_costmap/static_layer/parameter_descriptions
/move_base/global_costmap/static_layer/parameter_updates
/move_base/goal
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base/local_costmap/footprint
/move_base/local_costmap/inflation_layer/parameter_descriptions
/move_base/local_costmap/inflation_layer/parameter_updates
/move_base/local_costmap/obstacle_layer/parameter_descriptions
/move_base/local_costmap/obstacle_layer/parameter_updates
/move_base/local_costmap/parameter_descriptions
/move_base/local_costmap/parameter_updates
/move_base/parameter_descriptions
/move_base/parameter_updates
/move_base/result
/move_base/status
/move_base_simple/goal
/odom
/particlecloud
/reset
/rosout
/rosout_agg
/rpms
/scan
/sensor_state
/sound
/tf
/tf_static
/version_info

  在执行程序的时候会有两个错误:

#第一个代表恢复行为执行失败
[ERROR] [1673842142.773870082]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
#第二个代表导航实现不了
[ERROR] [1673842147.874115059]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors

下面留一些参考资料:
首先是Navigation的源码解读,都是一系列的博客,感觉十分有帮助:

https://blog.csdn.net/weixin_44126988?type=blog
https://blog.csdn.net/x_r_su/category_6374886.html
https://blog.csdn.net/heyijia0327/category_2768679.html
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=index#part4

还有过程中发现了一本讲ros的书,也在此做个记录
https://www.kancloud.cn/zhouws/robot_ustc_mooc/2153732