这个包使用Trajectory Rollout and Dynamic Window approaches来做平面上运动的机器人局部导航。 控制器使用规划和代价地图生成速度命令后发送给移动基座。该包适用于全向移动和非全向移动机器人，机器人轮廓可以表示为凸多边形或者圆，ROS参数可以在启动文件指定。 这个包进行了ROS封装，继承了BaseLocalPlanner接口。

 

 

1 概述

 

  base_local_planner包提供了驱动底座在平面移动的控制器，控制器可以连接路径规划器和机器人底座。为了让机器人从起始位置到达目标位置，规划器使用地图创建运动轨迹。向目标移动的路上，规划器至少需要在机器人周围创建一个可以表示成珊格地图的本地评价函数。该评价函数输入珊格单元的cost。 该控制器任务就是用这个评价函数确定发什么速度给机器人底座。

Trajectory Rollout and Dynamic Window Approach (DWA)算法基本理念如下:

1. 在机器人控制空间离散的采样(dx,dy,dtheta)
2. 拿采样的离散点做前向模拟，基于机器人当前状态，预测如果使用空间采样点的速度运动一段时间，会发生什么
3. 评估前向模拟的每条轨迹，评估标准包括（接近障碍，接近目标，接近全局路径和速度）。丢弃不合法的轨迹（如可能碰到障碍物的轨迹）
4. 采用得分最高的轨迹，并将其对应速度发给底座Pick the highest-scoring trajectory and send the associated velocity to the mobile base
5. 重复以上步骤

   DWA区别于Trajectory Rollout的地方在机器人的控制空间采样差异。Trajectory Rollout采样点来源于整个前向模拟阶段所有可用速度集合，而DWA采样点仅仅来源于一个模拟步骤中的可用速度集合。这意味着相比之下DWA是一种更加有效算法，因为其使用了更小采样空间；然而对于低加速度的机器人来说可能Trajectory Rollout更好，因为DWA不能对常加速度做前向模拟。 但是在实践中，我们经过多次实验发现2种方法性能基本相当，这样的话我们推荐使用效率更高的DWA算法。

一些有用的参考链接:

• Brian P. Gerkey and Kurt Konolige. "Planning and Control in Unstructured Terrain ". Discussion of the Trajectory Rollout algorithm in use on the LAGR robot.

• D. Fox, W. Burgard, and S. Thrun. "The dynamic window approach to collision avoidance". The Dynamic Window Approach to local control.

• Alonzo Kelly. "An Intelligent Predictive Controller for Autonomous Vehicles". An previous system that takes a similar approach to control.

1.1 Map Grid

    为了给轨迹的效率进行打分，地图珊格被引入。 每个控制周期，都会在机器人周围创建珊格（大小为局部代价地图）区域，并且全局路径会被映射到这个区域上。 这意味着有一定珊格将被标记为到路径点距离为0，到目标距离为0。接着利用传播算法可以将剩下的标记，记录它们到各自最近的标记为0的点的距离。

     然后，利用地图珊格计算轨迹分数。

     全局目标一般不会出现在地图珊格标记的小区域内，所以用接近目标给轨迹打分的话，这个目标应该是个局部目标，这意味着该小区域内第一个路径点一定是该区域外还有其连续点的那个点。 该小区域的尺寸大小由move_base确定。

2 震荡抑制

   当在x, y, or theta维度出现震荡后, 正负值会连续出现。因此，为了抑制震荡影响，当机器人在某方向移动时，对下一个周期的与其相反方向标记为无效，直到机器人从标记震荡的位置处离开一定距离。

3 通用局部规划

   从ROS groovy版本开始有了一种新的局部规划包（DWA）。 这种实现更模块化，其可复用的代码使客制化一种局部规划器时候更加容易。 base_local_planner基础代码已经被扩展，增加了几个头文件和新的类。

   局部规划器的工作原则是在每一个控制周期搜索最佳的局部规划。基于这样的考虑点创建很多候选轨迹来选择最佳。 对于任何一个生成的轨迹，都会被检查是否会与障碍物碰撞，如果不会的话，会给出一个打分评级并与其它已有的轨迹打分做比较以选出最好的轨迹。但是很显然，由于机器人外形（和制动器）以及应用领域的差异，这种工作原则的实例化会不尽相同。

    以下的类和接口遵照通用局部规划器工作原则允许进行不同实例化。以dwa_local_planner为模板加入自己的代价函数或者轨迹生成器，来创建客制化的局部规划器是可能的。

3.1 TrajectorySampleGenerator

   该接口描述了一种可以生成很多轨迹发生器，每调用一次nextTrajectory()就会返回一个新的轨迹。利用trajectory rollout or the DWA的方法，SimpleTrajectoryGenerator类可以生成许多轨迹。

3.2 TrajectoryCostFunction

   这个接口包含了最重要的函数scoreTrajectory(Trajectory &traj), 该函数输入轨迹后会输出轨迹评价分数。如果输出负分数意味着轨迹无效；如果输出正直，对于代价函数来说值越小越好。每个代价函数有一个比例因子，与其它代价函数比较时候，通过调节比例因子，代价函数影响可以被改变。base_local_planner包附带了一些在PR2上使用过的代价函数。

3.3 SimpleScoredSamplingPlanner

   这是轨迹搜索的一种简单实现，利用了TrajectorySampleGenerator产生的轨迹和一系列TrajectoryCostFunction代价函数。 它会一直调用nextTrajectory()直到发生器停止生成轨迹。对于每一个生成的轨迹，将会把列表中的代价函数都主意使用，并把代价函数返回的正直加总，负值丢弃。

   利用代价函数的比例因子， 最佳轨迹就是代价函数加权求和后最好的的那条轨迹。

3.4 Helper classes

3.4.1 LocalPlannerUtil

   该帮助接口提供了通用的接口可供所有规划器使用。它管理当前的全局规划，当前的运动约束，以及当前的代价地图（感知障碍物的局部代价地图）。

3.4.2 OdometryHelperRos

  该类为机器人提供odometry信息。

3.4.3 LatchedStopRotateController

   理想情况下，局部规划器可以让机器人准确停到它应该停止地方。然而在现实中，由于传感器噪声和执行器的不稳定性，可能会出现机器人从目标位置滑过去，这不是我们想要的结果。

  LatchedStopRotateController是一个好的控制器，当机器人足够靠近目标时可以迅速启用。 该控制器将执行完全停止操作，让机器人停在目标允许的公差范围内。

3.5 Cost Functions

3.5.1 ObstacleCostFunction

   该代价函数类基于感知到的障碍物评估轨迹。如果轨迹经过障碍物则返回负值，其它返回0。

3.5.2 MapGridCostFunction

   该代价函数类基于轨迹与全局路径或者目标点的接近程度来评价轨迹。它尝试对所有轨迹使用相同的到某个目标或者路径距离的预计算图来优化计算速度。

   在dwa_local_planner中, 因目的不同（为让轨迹尽可能接近全局路径，为让机器人朝着局部目标前进，还有为让机器人的头保持指向局部目标），该代价函数具体实现也会不尽相同。因该代价函数使用了试探机制，因此如果使用了不合适的参数可能给出的结果很差甚至根本不能工作。

3.5.3 OscillationCostFunction

   该代价函数类用以减少一定程度的震荡。虽然该代价函数能有效防止震荡，但是如果使用参数不合适也可能会阻止一些好的solution。

3.5.4 PreferForwardCostFunction

    该代价函数类适用于类似PR2那种在机器人前方有很好传感器（如倾角雷达）布置的机器人。 该代价函数鼓励前向运动，惩罚后向或者其它周围方向性运动。但是这种特性在某些领域机器人可能并不是所期望的，所以仅适合于特定应用的机器人。

 

4 TrajectoryPlannerROS

    base_local_planner::TrajectoryPlannerROS是对base_local_planner::TrajectoryPlanner的ROS化封装。它在初始化时确定的ROS命名空间内运行，该接口继承了nav_core包的nav_core::BaseLocalPlanner接口。

    如下是base_local_planner::TrajectoryPlannerROS的一个应用案例:

 

1. #include <tf/transform_listener.h>

2. #include <costmap_2d/costmap_2d_ros.h>

3. #include <base_local_planner/trajectory_planner_ros.h>

4.  

5. ...

6.  

7. tf::TransformListener tf(ros::Duration(10));

8. costmap_2d::Costmap2DROS costmap("my_costmap", tf);

9.  

10. base_local_planner::TrajectoryPlannerROS tp;

11. tp.initialize("my_trajectory_planner", &tf, &costmap);

 

 

4.1 API Stability

• C++ API是稳定的
• ROS API也是稳定的

 

 

4.1.1 Published Topics

~<name>/global_plan (nav_msgs/Path)

• 表示的是局部规划器目前正在跟随的全局规划中的一部分，主要用于可视化。

~<name>/local_plan (nav_msgs/Path)

• 表示的是上一个周期局部规划或者轨迹得分最高者，主要用于可视化。

~<name>/cost_cloud (sensor_msgs/PointCloud2)

• 用于表示规划的代价珊格，也是用于可视化目的。参数publish_cost_grid_pc用来使能或者关闭该可视化。New in navigation 1.4.0

 

4.1.2 Subscribed Topics

odom (nav_msgs/Odometry)

• 该Odometry信息用于向局部规划器提供当前机器人的速度。 这个里程消息中的速度信息被假定使用的坐标系与TrajectoryPlannerROS对象内代价地图的robot_base_frame参数指定的坐标系相同。有关该参数详细介绍请参照 costmap_2d 包。

4.2 Parameters

    有许多ROS参数可以用来客制化base_local_planner::TrajectoryPlannerROS的行为。 这些参数被分成几类：robot configuration, goal tolerance, forward simulation, trajectory scoring, oscillation prevention, and global plan.

4.2.1 机器人配置参数（Robot Configuration Parameters）

~<name>/acc_lim_x (double, default: 2.5)

• The x acceleration limit of the robot in meters/sec^2

~<name>/acc_lim_y (double, default: 2.5)

• The y acceleration limit of the robot in meters/sec^2

~<name>/acc_lim_theta (double, default: 3.2)

• The rotational acceleration limit of the robot in radians/sec^2

~<name>/max_vel_x (double, default: 0.5)

• The maximum forward velocity allowed for the base in meters/sec

~<name>/min_vel_x (double, default: 0.1)

• The minimum forward velocity allowed for the base in meters/sec. It is useful to specify this to guarantee that velocity commands sent to a mobile base are high enough to allow the base to overcome friction.

~<name>/max_vel_theta (double, default: 1.0)

• The maximum rotational velocity allowed for the base in radians/sec

~<name>/min_vel_theta (double, default: -1.0)

• The minimum rotational velocity allowed for the base in radians/sec

~<name>/min_in_place_vel_theta (double, default: 0.4)

• The minimum rotational velocity allowed for the base while performing in-place rotations in radians/sec

~<name>/backup_vel (double, default: -0.1)

• DEPRECATED (use escape_vel):Speed used for backing up during escapes in meters/sec. Note that it must be negative in order for the robot to actually reverse. A positive speed will cause the robot to move forward while attempting to escape.

~<name>/escape_vel (double, default: -0.1)

• Speed used for driving during escapes in meters/sec. Note that it must be negative in order for the robot to actually reverse. A positive speed will cause the robot to move forward while attempting to escape.New in navigation 1.3.1

~<name>/holonomic_robot (bool, default: true)

• Determines whether velocity commands are generated for a holonomic or non-holonomic robot. For holonomic robots, strafing velocity commands may be issued to the base. For non-holonomic robots, no strafing velocity commands will be issued.

下面的参数只有在holonomic_robot被设置为true时候才能使用：

~<name>/y_vels (list, default: [-0.3, -0.1, 0.1, 0.3])

• The strafing velocities that a holonomic robot will consider in meters/sec

4.2.2 目标公差参数（Goal Tolerance Parameters）

~<name>/yaw_goal_tolerance (double, default: 0.05)

• The tolerance in radians for the controller in yaw/rotation when achieving its goal

~<name>/xy_goal_tolerance (double, default: 0.10)

• The tolerance in meters for the controller in the x & y distance when achieving a goal

~<name>/latch_xy_goal_tolerance (bool, default: false)

• If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. -New in navigation 1.3.1

4.2.3 前向模拟参数（Forward Simulation Parameters）

~<name>/sim_time (double, default: 1.0)

• The amount of time to forward-simulate trajectories in seconds

~<name>/sim_granularity (double, default: 0.025)

• The step size, in meters, to take between points on a given trajectory

~<name>/angular_sim_granularity (double, default: ~<name>/sim_granularity)

• The step size, in radians, to take between angular samples on a given trajectory. -New in navigation 1.3.1

~<name>/vx_samples (integer, default: 3)

• The number of samples to use when exploring the x velocity space

~<name>/vtheta_samples (integer, default: 20)

• The number of samples to use when exploring the theta velocity space

~<name>/controller_frequency (double, default: 20.0)

• The frequency at which this controller will be called in Hz. Uses searchParam to read the parameter from parent namespaces if not set in the namespace of the controller. For use withmove_base, this means that you only need to set its "controller_frequency" parameter and can safely leave this one unset. -New in navigation 1.3.1

4.2.4 轨迹打分参数（Trajectory Scoring Parameters）

   该代价函数使用如下公式给每条轨迹打分：

 

1. cost =

2. pdist_scale * (distance to path from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter)

3. + gdist_scale * (distance to local goal from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter)

4. + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))

~<name>/meter_scoring (bool, default:false)

 

• Whether the gdist_scale andpdist_scale parameters should assume thatgoal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default.New in navigation 1.3.1

~<name>/pdist_scale (double, default: 0.6)

• The weighting for how much the controller should stay close to the path it was given

~<name>/gdist_scale (double, default: 0.8)

• The weighting for how much the controller should attempt to reach its local goal, also controls speed

~<name>/occdist_scale (double, default: 0.01)

• The weighting for how much the controller should attempt to avoid obstacles

~<name>/heading_lookahead (double, default: 0.325)

• How far to look ahead in meters when scoring different in-place-rotation trajectories

~<name>/heading_scoring (bool, default:false)

• Whether to score based on the robot's heading to the path or its distance from the path

~<name>/heading_scoring_timestep (double, default: 0.8)

• How far to look ahead in time in seconds along the simulated trajectory when using heading scoring

~<name>/dwa (bool, default: true)

• Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout (NOTE: In our experience DWA worked as well as Trajectory Rollout and is computationally less expensive. It is possible that robots with extremely poor acceleration limits could gain from running Trajectory Rollout, but we recommend trying DWA first.)

~<name>/publish_cost_grid_pc (bool, default:false)

• 确定当进行规划时候，是否发布规划器将要使用的代价珊格。 如果是true，将会在~<name>/cost_cloud topic上发布消息sensor_msgs/PointCloud2。 Each point cloud represents the cost grid and has a field for each individual scoring function component as well as the overall cost for each cell, taking the scoring parameters into account.New in navigation 1.4.0

~<name>/global_frame_id (string, default:odom)

• 设置cost_cloud工作坐标系，应该与局部代价地图的global坐标系一致。New in navigation 1.4.0

4.2.5 震荡抑制参数（Oscillation Prevention Parameters）

 

~<name>/oscillation_reset_dist (double, default: 0.05)

• 机器人必须运动几米远后才能复位震荡标记

4.2.6 全局规划参数（Global Plan Parameters）

~<name>/prune_plan (bool, default: true)

• Defines whether or not to eat up the plan as the robot moves along the path. If set to true, points will fall off the end of the plan once the robot moves 1 meter past them.

5 TrajectoryPlanner

    base_local_planner::TrajectoryPlanner实现了DWA and Trajectory Rollout算法。 如果要在ROS中想使用base_local_planner::TrajectoryPlanner, 请使用进过封装的TrajectoryPlannerROS wrapper。

5.1 API Stability

   虽然C++ API是稳定的。 但是仍然推荐使用TrajectoryPlannerROS wrapper 而不是base_local_planner::TrajectoryPlanner。