ROS源码阅读---局部路径规划之DWAPlannerROS分析

1 体系结构

（1）主要成员

base_local_planner::LocalPlannerUtil planner_util_; 用来存储运动控制参数以及costmap2d、tf等，会被传入dp_

costmap_2d::Costmap2DROS* costmap_ros_;

base_local_planner::OdometryHelperRos odom_helper_; 用来辅助获取odom信息，会被传入dp_

boost::shared_ptr<DWAPlanner> dp_; 正常的dwa运动控制类

base_local_planner::LatchedStopRotateController latchedStopRotateController_; 到达目标点后的停止旋转运动控制类

（2）主要接口

void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)---初始化

bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)---设置全局路径

bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel)---计算控制速度

bool isGoalReached()---判断是否达到目标点

2 总体流程

在move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，然后调用DWAPlanner::setPlan将路径传递给DWAPlanner（同时复位latchedStopRotateController_中的锁存相关的标志位）。

在运动规划之前，move_base先利用DWAPlannerROS::isGoalReached（内部利用LatchedStopRotateController::isGoalReached函数实现）判断是否已经到达了目标点（即位置在xy_goal_tolerance以内，朝向在yaw_goal_tolerance以内，且速度小于trans_stopped_velocity，rot_stopped_velocity），如果是则控制结束，即发送0速，且复位move_base的相关控制标记，并返回action成功的结果。否则，利用DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)计算局部规划速度。

在DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)中，会先将全局路径映射到局部地图中，并更新对应的打分项（参考DWAPlanner::updatePlanAndLocalCosts函数，和具体的打分项更新）。然后，利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令（取决于是否机器人已经停稳，进而决定实现减速停止或者旋转至目标朝向），否则利用DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel)计算DWA局部路径速度。

对于停止时的处理逻辑，即LatchedStopRotateController的所有逻辑，参考“停止”章节。

对于DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel)函数，直接利用DWAPlanner::findBestPath(

tf::Stamped<tf::Pose> global_pose,

tf::Stamped<tf::Pose> global_vel,

tf::Stamped<tf::Pose>& drive_velocities,

std::vector<geometry_msgs::Point> footprint_spec)获取最优局部路径对应的速度指令。

在DWAPlanner::findBestPath函数中，先利用SimpleTrajectoryGenerator::initialise(

const Eigen::Vector3f& pos,

const Eigen::Vector3f& vel,

const Eigen::Vector3f& goal,

base_local_planner::LocalPlannerLimits* limits,

const Eigen::Vector3f& vsamples,

bool discretize_by_time = false)初始化轨迹产生器，即产生速度空间。

然后，利用SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored = 0)查找最优的局部路径。在该函数中，先调用每个打分项的prepare函数进行准备，参考“打分”章节。然后，利用SimpleScoredSamplingPlanner里面存储的TrajectorySampleGenerator轨迹产生器列表（目前，只有SimpleTrajectoryGenerator一个产生器）分别进行轨迹产生和打分，并根据打分选出最优轨迹。对于每个轨迹的打分，调用SimpleScoredSamplingPlanner::scoreTrajectory执行，在该函数中，遍历各个打分项进行打分并进行求和，如果某个打分项打分小于0，则将该负值作为路径打分项进行立刻返回。在对每个打分项打分之前，先获取对应的scale参数，如果scale为0证明该打分项无效，则跳过。如果获取到正常的打分项打分后（利用打分项的scoreTrajectory函数），将打分项乘以对应的scale作为最终的打分项打分。

具体的速度采样和轨迹规划参考“运动规划”章节。

接着在DWAPlanner::findBestPath中，根据使能参数，选择发布轨迹相关的可视化数据。

然后，如果最优轨迹有效，则利用最优轨迹更新摆动打分项的标志位。

最后，在返回最优规划轨迹之前，判断最优轨迹代价，如果代价小于0，则代表最优轨迹无效，即没有有效轨迹，则将返回速度指令，设置为0。

最后，如果DWAPlannerROS::dwaComputeVelocityCommands会判断得到的最优轨迹代价值，如果小于0，则该函数会返回false，从而引起move_base进行进一步的处理，如重新规划、recovery等。

至此，完成了整个dwa local planner的流程。

3 运动规划

在每个控制周期内，DWAPlanner::findBestPath会查找最优局部轨迹。

在DWAPlanner::findBestPath中，首先调用SimpleTrajectoryGenerator::initialise进行速度采样，然后利用SimpleScoredSamplingPlanner::findBestTrajectory根据速度采样空间进行轨迹产生、打分、筛选，从而得到最优局部轨迹。

（1）采样

速度采样在SimpleTrajectoryGenerator::initialise中进行，总体思路是先获取速度空间边界，然后根据采样个数参数在空间内进行采样。

在获取空间边界时，根据use_dwa参数，采用两套策略。

如果use_dwa==false，首先用当前位置与目标位置的距离处理仿真时间sim_time（模拟仿真时间内匀减速到0，刚好到达目标点的情景）得到max_vel_x和max_vel_y边界，然后基于acc_lim * sim_time得到一种边界（上边界），还有设置的速度极限参数（max_vel_xxx，min_vel_xxx）作为一种边界，然后选取三种边界中空间较小的边界。这种策略，能够获得较大的采样空间（因为用了sim_time）。

如果use_dwa==true，则直接用acc_lim * sim_period得到边界，然后还有设置的速度极限参数作为边界，然后选取两种边界中空间较小的边界。

得到速度空间边界后，根据x，y，theta三个采样个数进行插补，进而组合出整个速度采样空间。

（2）获取最优轨迹

获取最优轨迹在SimpleScoredSamplingPlanner::findBestTrajectory中进行，在该函数中，首先调用各个打分项的prepare函数进行准备。然后遍历std::vector<TrajectorySampleGenerator*>轨迹产生器列表（目前只有一个）进行打分，并且一旦某个产生器发现了最优轨迹则返回（该做法较为粗略，即使有多个产生器也无法得到全局最优）。

对于每个打分器，由于上一步已经进行了速度采样，因此可以逐个速度产生轨迹，并进行打分，同时记录最优打分和最优轨迹。

其中，由速度产生轨迹由SimpleTrajectoryGenerator::generateTrajectory(

Eigen::Vector3f pos,

Eigen::Vector3f vel,

Eigen::Vector3f sample_target_vel,

base_local_planner::Trajectory& traj)函数实现。

在该函数中，先判断速度是否满足“平移速度不小于min_trans_vel且旋转速度不小于min_rot_vel”和“平移速度不大于max_trans_vel”两个条件，如果满足，则直接返回false，此时，traj为空路径。否则，继续正常的轨迹产生流程。

为了产生轨迹，需要由pos，vel，sample_target_vel三个参数离散仿真出轨迹中的所有点位姿。首先，需要确定仿真步数（即多少个点）（虽然由参数discretize_by_time_确定是由sim_time/sim_granularity计算仿真步数，还是由std::max(sim_time_distance / sim_granularity_, sim_time_angle / angular_sim_granularity_)计算，但该参数目前保持为false，即由后者计算仿真步数）。有了仿真步数num_steps，可以由sim_time/num_steps计算出每步的时间间隔dt。然后，就可以根据具体策略计算轨迹点。

如果use_dwa==false，则采用连续加速的策略，即仿真出的轨迹中不同点对应的速度是变化的，此时将轨迹中保存的对应速度设为基于当前速度第一次加速出的速度。否则，轨迹中的各个点为同样的速度，即sample_target_vel，此时轨迹中保存的速度也是该速度。

然后，开始计算各个轨迹点，如果use_dwa==false，则边计算轨迹点位姿，边变化速度，否则用恒速计算轨迹点位姿。

这样，就完成了轨迹的生成。

每个速度对应轨迹产生后，调用SimpleScoredSamplingPlanner::scoreTrajectory进行打分，在该函数中，遍历各个打分项进行打分并进行求和，如果某个打分项打分小于0，则将该负值作为路径打分项进行立刻返回。在对每个打分项打分之前，先获取对应的scale参数，如果scale为0证明该打分项无效，则跳过。如果获取到正常的打分项打分后（利用打分项的scoreTrajectory函数），将打分项乘以对应的scale作为最终的打分项打分。

至此，就完成了速度采样、轨迹生成、轨迹打分和选优。

4 路径

（1）存储全局路径

路径类型为std::vector<geometry_msgs::PoseStamped>。

在move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，然后调用DWAPlanner::setPlann将路径传递给DWAPlanner（同时复位latchedStopRotateController_中的锁存相关的标志位）。

在DWAPlanner::setPlann中，会先将OscillationCostFunction中的摆动标志位复位，然后利用planner_util_->setPlan将路径传入planner_util_，直接保存为global_plan_。

此时的路径时相对于全局地图的全局坐标系的（通常为"map")。

（2）局部路径映射及存储

在computeVelocityCommands计算速度前，会先将全局路径映射到局部地图坐标系下（通常为“odom”），通过LocalPlannerUtil::getLocalPlan(tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan)实现。

在getLocalPlan中，先将较长的全局路径映射并截断到局部地图内（即坐标系转换为局部地图，且范围完全在局部地图内，超出地图的则抛弃）。然后，裁减全局路径和局部路径（与机器人当前位置距离超过1m的旧的路径会被裁减掉）。

裁减过的全局路径还保存在planner_util_中，映射并经过裁减后的局部路径会在computeVelocityCommands函数中传入dp_中（利用DWAPlanner::updatePlanAndLocalCosts函数）。

在updatePlanAndLocalCosts中，会将局部路径保存到dp_的global_plan_中（对于dp_来说，局部地图中映射的全局路径就是全局的，而dp_自己规划的路径是局部的）。然后，将该路径信息传入各个打分对象，具体参考打分章节。

5 打分

打分对象一共6个，分别为：

base_local_planner::OscillationCostFunction oscillation_costs_（摆动打分）
base_local_planner::ObstacleCostFunction obstacle_costs_（避障打分）
base_local_planner::MapGridCostFunction path_costs_（路径跟随打分）
base_local_planner::MapGridCostFunction goal_costs_（指向目标打分）
base_local_planner::MapGridCostFunction goal_front_costs_（前向点指向目标打分）
base_local_planner::MapGridCostFunction alignment_costs_（对齐打分）

其中，摆动打分和避障打分项是独立的类型，后四个打分项是同一类型。

打分计算过程中出现的负的值可以认为是错误代码（用于指示具体的出错原因），而不是得分，该说明对下面所有的打分描述有效。

如果轨迹为空（在产生轨迹时出错，例如不满足最大最小速度要去），则各个打分项对应的得分都为0。

（1）打分对象初始化及更新

oscillation_costs_

在DWAPlanner的构造函数中，利用oscillation_costs_.resetOscillationFlags()复位摆动标志位（侧移、旋转、前进方向的相关参数strafe_pos_only_，strafe_neg_only_，strafing_pos_，strafing_neg_，rot_pos_only_，rot_neg_only_，rotating_pos_，rotating_neg_，forward_pos_only_，forward_neg_only_，forward_pos_，forward_neg_）。

然后将oscillation_costs传入打分项列表（也会加入其它打分项），并将打分项列表std::vector<base_local_planner::TrajectoryCostFunction*>和采样规划器列表std::vector<base_local_planner::TrajectorySampleGenerator*>传入打分采样规划器base_local_planner::SimpleScoredSamplingPlanner。

在DWAPlannerROS的动态配置回调函数中，会将动态配置参数再传入DWAPlanner::reconfigure函数，在该函数中利用oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle)初始化该打分项。

在正常行走过程中（没有到达目标点范围），DWAPlannerROS会调用dwaComputeVelocityCommands实现computeVelocityCommands函数，在dwaComputeVelocityCommands中，会调用DWAPlanner::findBestPath方法，在该方法中会先采样计算出最优路径，然后利用oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel)更新摆动打分项的标志位。在打分项更新标志位的函数中，如果判断最优路径的代价也小于0（即无效），则不更新，否则，则根据最优路径和最小平移速度限制设置打分标志setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans)。然后，如果在某些方向上有了限制，则检查是否能够复位标志位resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev)。

对于setOscillationFlags函数，先设置x方向的摆动：一旦规划的速度与当前的方向不同，则设置forward_xxx_only标志位，表示后续只能沿规划的速度方向运行。对于侧向摆动和旋转摆动，必须在x方向速度小于min_vel_trans时，才会判断，判断逻辑和x方向一致。

由于前面可能变过方向，即设置过xxx_xxx_only标志位，短时间内不能变向。此时，需要利用resetOscillationFlagsIfPossible判断是否可以取消标志位，即是否可以开始变向。通过判断当前位置相对于上次换向的位置的平移和旋转位置是否超过oscillation_reset_dist_和oscillation_reset_angle_，如果是则复位所有标志位。

上面两个函数实现了xxx_xxx_only标志位的置位和复位。

obstacle_costs_

在DWAPlanner的构造函数中，利用local costmap初始化避障打分项，然后通过sum_scores参数（默认为false）设置避障打分项的打分汇总方式。

然后，将obstacle_costs_传入打分项列表，同上。

在DWAPlanner::reconfigure函数中，利用obstacle_costs_.setScale函数设置比例参数为resolution * occdist_scale（碰撞参数），并在obstacle_costs中记录为scale_。然后利用obstacle_costs_.setParams函数设置max_trans_vel, max_scaling_factor, scaling_speed（这些参数可以引起避障打分项中的footprint变化，从而根据不同的速度膨胀不同的大小）。

在每一次通过DWAPlanner的findBestPath函数计算最优局部路径时，都会利用obstacle_costs_.setFootprint为obstacle_costs_设置一次footprint参数（带padding），会被obstacle_costs_的footprintr_spec_（std::vector<geometry_msgs::Point>类型）参数记录。

至此，就记录了所有参数，并且更新了碰撞打分相关的footprint。

path_costs_

path_costs_，goal_costs，goal_front_costs_，alignment_costs_四个打分项都是MapGridCostFunction类型，因此打分思路都一致，只是通过设置不同参数即可实现不同的打分方式。MapGridCostFunction类的构造函数为

MapGridCostFunction(costmap_2d::Costmap2D* costmap

double xshift = 0.0,

double yshift = 0.0,

bool is_local_goal_function = false,

CostAggregationType aggregationType = Last);

其中，path_costs_和alignment_costs_都是参考整个路径信息，因此is_local_goal_function参数为false，同理goal_costs_，goal_front_costs_对应的该参数为true。

另外，由于goal_front_costs_和alignment_costs_两个打分项都是参考前向打分点，因此还是利用MapGridCostFunction::setXShift函数记录了forward_point_distance_参数，而另外两个打分项对应的该参数为0。

另外，goal_front_costs_和alignment_costs_两个打分项还利用MapGridCostFunction::setStopOnFailure函数设置了stop_on_failure参数为false。

对于path_costs_的初始化，在DWAPlanner的构造函数中，利用local costmap初始化路径打分项。

在DWAPlanenr::reconfigure函数中，利用path_costs_.setScale函数设置比例参数为resolution * pdist_scale_ * 0.5，并在path_costs_中记录为scale_。scale_参数会在打分采样规划器的计算轨迹代价时用到，即用打分项得到的得分，乘以打分项记录的scale_。

在DWAPlanner::updatePlanAndLocalCosts函数（每个控制周期都会将映射到local costmap中的路径信息在这里更写到相关的打分项或者其他部件中）中，会利用path_costs_.setTargetPoses函数将全局路径在局部地图中的映射传入到path_costs中，作为局部的打分参考路径，MapGridCostFunction::setTargetPoses函数将路径信息存储为target_poses_。

至此，就完成了初始化和初步的更新。

goal_costs_

对于goal_costs_的初始化，在DWAPlanner的构造函数中，利用local costmap初始化该打分项，同时也设置了is_local_goal_function标志位true。参考path_costs_打分项初始化说明。

在DWAPlanenr::reconfigure函数中，利用goal_costs_.setScale函数设置比例参数为resolution * gdist_scale_ * 0.5，并在path_costs_中记录为scale_。

至此，就完成了初始化和初步的更新。

goal_front_costs_

对于goal_front_costs_的初始化，在DWAPlanner的构造函数中，利用local costmap初始化该打分项，同时也设置了is_local_goal_function标志位为true。另外，设置了stop_on_failure标志位。参考path_costs_打分过程的说明。

在DWAPlanner::reconfigure函数中，利用goal_front_costs_.setScale函数设置比例参数为resolution * gdist_scale_ * 0.5，并在goal_front_costs_中记录为scale_。然后，利用goal_front_costs_.setXShift函数将forward_point_distance_设为该打分项的xshift_参数（即打分点相对于机器人原点的偏移）。

在DWAPlanner::updatePlanAndLocalCosts函数（每个控制周期都会将映射到local costmap中的路径信息在这里更写到相关的打分项或者其他部件中）中，为了给前向打分点确定它的“goal”，采用的方法是先计算当前位置和局部路径末端点之间的连线朝向，然后在局部路径末端点的基础上沿该方向延伸forward_point_distance_距离，并将这个”goal“替换掉局部路径末端点。然后利用path_costs_.setTargetPoses函数将全局路径在局部地图中的映射传入到path_costs中，作为局部的打分参考路径，MapGridCostFunction::setTargetPoses函数将路径信息存储为target_poses_。

至此，就完成了初始化和初步的更新。

alignment_costs_

对于alignment_costs_的初始化，在DWAPlanner的构造函数中，利用local costmap初始化该打分项。同时设置了stop_on_failure标志位。

在DWAPlanenr::reconfigure函数中，利用path_costs_.setScale函数设置比例参数为resolution * pdist_scale_ * 0.5，并在path_costs_中记录为scale_。然后，利用alignment_costs_.setXShift函数将forward_point_distance_设为该打分项的xshift_参数（即打分点相对于机器人原点的偏移）。

在DWAPlanner::updatePlanAndLocalCosts函数（每个控制周期都会将映射到local costmap中的路径信息在这里更写到相关的打分项或者其他部件中）中，为了让前向打分点合理的靠近路径，需要针对不同的情况考虑。如果，当前位置和局部路径末端点之间的距离比forward_point_distance_大时，则仍然alignment_costs_.setTargetPoses(global_plan_)，即不做特殊处理，打分时参考前向打分点和局部地图路径的距离。否则，设置alignment_costs_的scale为0，从而忽略alignment_costs_这一代价项。

（2）代价打分

所有的打分都是基于各个打分项的scoreTrajectory(Trajectory &traj)函数实现。

oscillation_costs_

该打分项主要利用了轨迹中的xv_, yv_, thetav_三个参数进行摆动判断。摆动打分较为简单，如果xxx_xxx_only设置为了true，但是对应的规划路径速度方向相反，则返回代价值-5，否则返回0。

obstacle_cost_

碰撞打分的总思路是：

对轨迹上的每一点计算出对应的footprint（利用了x, y ,theta, footprint四个信息计算）进行footprintCost计算，最后汇总得分时，如果sum_scores_参数为true，则加和所有得分，否则只取最后一个点的得分（覆盖计算的方式）。但是，如果某个点的footprintCost为负，则直接返回该负的得分。

在进行对轨迹各个点计算footprintCost之前，会先计算缩放因子。如果当前平移速度小于scaling_speed，则缩放因子为1.0，否则，缩放因子为(vmag - scaling_speed) / (max_trans_vel - scaling_speed) * max_scaling_factor + 1.0。然后，该缩放因子会被用于计算轨迹中各个点的footprintCost。

对于footprintCost，其传入参数为 static double footprintCost(

const double& x,

const double& y,

const double& th,

double scale,

std::vector<geometry_msgs::Point> footprint_spec,

costmap_2d::Costmap2D* costmap,

base_local_planner::WorldModel* world_model);

但scale并没有使用，即目前还不能根据速度改变打分效果。

在footprintCost中，首先计算利用world_model->footprintCost计算footprint的代价值，如果footprint的代价值小于0，则footprintCost直接返回-6。接着，将x，y转换为地图坐标，转化时如果失败则footprintCost返回-7。得到地图坐标后，会对比footprint的代价值和机器人中心点（x，y）的代价值（可通过costmap直接获取），并以较大的值作为返回值。

对于world_model->footprintCost的计算，是对footprint中的每个线段进行独立计算代价的。首先尝试获取线段的地图坐标，如果获取失败则返回-1。然后，计算利用lineCost函数计算线段代价值，并用footprint_cost参数保存最大的线段代价值，最终如果正常，则返回的也是footprint_cost。但是，如果某个线段的代价值小于0，则world_model->footprintCost直接返回-1。

对于线段的代价计算函数lineCost，其实是对线段中的所有像素点（通过bresenham算法得到）进行代价计算，如果像素点的代价为LETHAL_OBSTACLE或者NO_INFORMATION，则该点代价为-1。如果某个点的代价为-1，则该线段的代价也为-1，否则则取所有点中最大的代价作为线段的代价。

总结下来，obstacle_cost_的打分，可能会返回-6，-7，或者具体的代价值。并且，速度scale参数没有使用，后续可以自己尝试实现。另外，最后汇总得分的方式两种都不好。如果是加和，则越长的轨迹越容易积累较大的代价，因此速度大的轨迹容易被抛弃。如果是最后一个点，则中间点如果发生碰撞就不会考虑，这种策略显得太粗略。可以尝试增加一种返回最大footprintCost的策略，即考虑轨迹中最大footprintCost的那个点。

path_costs_

在每个findBestPath周期时，会对各个打分项进行prepare操作，其中oscillation_costs_和obstacle_costs_的prepare不执行具体的动作，而剩余四个打分项在prepare中会对局部地图进行预计算，从而方便后边进行打分。

预计算的思想在于维护一个跟局部地图尺寸对应的路径计算地图（MapGrid类型），该地图中的每个元素为MapCell类型，该类型记录了该元素距离目标点的距离信息或者距离路径的距离信息。在每次打分前，先利用局部路径更新该地图信息，并根据is_local_goal_function参数决定记录距离目标点的距离信息还是距离路径的距离信息，这些信息在后面打分时可以直接使用。

在prepare函数中，首先复位整个路径计算地图。然后，根据is_local_goal_function参数，分别利用MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan)和MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan)两个函数分别计算对应的预处理信息。

对于setLocalGoal函数，是用来计算轨迹路径计算地图中所有点到局部地图路径末端点的最短距离。在该函数中，首先对映射到局部地图中的路径进行像素级的插补，从而使得整个路径是像素连续的。然后，查找路径在局部地图中的最后一个点，并将路径计算地图中的该点标记为已访问（标记为已访问的点可以进行最短路径计算），然后调用computeTargetDistance函数采用类似dijkstra算法的逐步探索的方式，计算出路径计算地图中所有点（像素级）相对于与标记点的最短距离。

对于setTargetCells函数，是用来计算轨迹路径计算地图中所有点到局部地图路径的最短距离。与setLocalGoal函数的区别主要在于，首先将局部地图路径中所有的点标记为已访问，然后调用computeTargetDistance函数就会计算路径计算地图中所有点到整个局部地图路径的最短距离，具体算法需要进一步了解。

因此，在经过预计算后，对于path_costs_和alignment_costs这两个考虑与路径距离的打分项，其内部的路径计算地图中存储的是所有点到路径的最短距离；对于goal_costs_和goal_front_costs_这两个考虑与局部目标点距离的打分项，其内部的路径计算地图中存储的是所有点到局部目标点的最短距离。

至此，完成了path，goal相关的打分项的预计算，下一步就是利用scoreTrajectory(Trajectory &traj)进行具体的打分。

在MapGridCostFunction::scoreTrajectory函数中，依次获取路径中的各个点坐标，并尝试将坐标转为地图坐标，如果转化失败，则scoreTrajecotry函数直接返回-4。否则，从路径计算地图中找到距离局部地图路径的距离，并将该距离（像素级）值作为该点的得分。

另外，对于path_costs_和goal_costs_两个打分项的stop_on_failure参数为true，即如果轨迹中的某个点未障碍物或者未再路径计算地图中探索到，则表示查找该点的得分失败，这会造成scoreTrajectory函数直接返回对应的负的得分。例如，如果轨迹中的某个点本身就是障碍物（在路径计算地图中会有标记），则scoreTrajectory直接返回-3作为轨迹得分；如果轨迹中某个点不可达（在路径计算地图中没有计算到），则scoreTrajectory直接返回-2作为轨迹得分。

对于goal_front_costs和alignment_costs_两个打分项对应的stop_on_failure参数为false，这是因为前向打分点有可能会出现计算距离出错的情况，例如超出地图范围，但由于前向打分点的打分失败不会带来危险，因此可以不立刻返回负得分。

轨迹所有点得分的汇总方式有Last、Sum、Product三种方式，默认为Last（且没有对应的配置参数，除非自己修改源码实现），即只考虑轨迹最后一个点的打分。

总的来说，path_costs_打分项只考虑最后一个点的得分，该得分可能会取-4（如果是path_costs_和goal_costs_，还有可能返回-2，-3），或者最后一个点与局部地图路径之间的像素距离。

goal_costs_

该打分项的思路在path_costs_的打分过程中已经描述，可以参考。

goal_front_costs_

该打分项和goal_costs_基本一样，只是参考的是机器人前向点和局部目标点的距离，而不是机器人原点和局部目标点的距离。

alignment_costs_

该打分项和path_costs_基本一样啊，只是参考的是机器人前向点和局部地图路径的距离，而不是机器人原点和局部地图路径的距离。

6 停止

对于DWAPlannerROS的停止处理逻辑，由LatchedStopRotateController类完成，主要包括了停止判断、加速停止、旋转至目标朝向三个大的部分。

（1）停止判断

在move_base的每个控制周期，都会利用利用DWAPlannerROS::isGoalReached（内部利用LatchedStopRotateController::isGoalReached函数实现）判断是否已经到达了目标点（即位置在xy_goal_tolerance以内，朝向在yaw_goal_tolerance以内，且速度小于trans_stopped_velocity，rot_stopped_velocity）。

对于LatchedStopRotateController::isGoalReached函数，分为锁存目标和不锁存目标两种处理逻辑。锁存目标，即如果机器人在行驶过程中出现过位置满足xy_goal_tolerance的条件时，则设置xy_tolerance_latch_标志位，代表已经达到过目标，不用考虑最终是否超出目标点，直接进行停止、旋转至目标朝向即可。

如果不锁存，则必须始终满足机器人当前位置是否满足xy_goal_tolerance的条件，满足则代表到达了目标位置。

在到达目标位置的前提下，还要判断机器人朝向是否满足目标朝向yaw_goal_tolerance的需求，如果也满足，则判断当前速度是否满足停止条件，即x和y的速度小于trans_stopped_velocity，theta速度小于rot_stopped_velocity。

只有到达位置、达到朝向、速度满足停止三个条件都满足的情况下，才算机器人到达了目标位姿，isGoalReached函数才会返回true。

（2）加速停止

在DWAPlannerROS::computeVelocityCommands的每个计算周期内，都会先利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令，否则才会使用DWAPlanner计算最优轨迹对应的速度命令。

对于isPostionReached的判断，和isGoalReached中判断达到位置的逻辑一样，即如果启动了锁存，且xy_tolerance_latch_标志位被标记（即满足过xy_goal_tolerance条件），则认为已经到达；如果未启用锁存，则需要基于当前位置是否满足xy_goal_tolerance确定是否到达了位置。

在LatchedStopRotateController::computeVelocityCommandsStopRotate函数中，会假设用户想要进行旋转停止了，即已经到达位置了，因此会先判断当前朝向是否满足yaw_goal_tolerance要求，如果满足，则将速度指令设置为0（这种做法太过粗略）。

如果朝向没有满足要求，则会判断是要进行加速停止还是旋转至目标点。

如果机器人还未处于旋转停止状态（通过rotating_to_goal标记），并且速度没有满足停止要求，则会调用stopWithAccLimits进行加速停止处理，否则代表应进入了旋转停止状态，则会调用rotateToGoal进行旋转至目标朝向处理，同时设置rotating_to_goal标志位true。

对于stopWithAccLimits函数，对于利用acc_lim参数进行最快的减速命令计算，然后利用DWAPlanner::checkTrajectory函数计算出轨迹，并进行轨迹打分。在计算轨迹时利用了SimpleTrajectoryGenerator::generateTrajectory(

Eigen::Vector3f pos,

Eigen::Vector3f vel,

Eigen::Vector3f sample_target_vel,

base_local_planner::Trajectory& traj)，如果速度不满足最小速度要求（min_rot_vel、min_trans_vel）则轨迹为空。

在对轨迹进行打分时，利用了SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost)，如果轨迹为空，则打分为0，此时虽然checkTrajectory返回true，但对应的速度可能过小（目前判断没有太大影响）。

如果checkTrajecotry返回true（轨迹打分大于等于0）则代表路径有效，stopWithAccLimits计算出的加速停止速度命令有效，否则设置速度命令为0。

（3）旋转至目标朝向

旋转至目标朝向有rotateToGoal函数计算，首先将x和y速度设置为0，对于theta速度，利用当前朝向与目标朝向的差的比例控制（系数为1）产生，然后用加速能力（acc_lim*sim_period）、速度限幅（max_rot_vel，min_rot_vel）、减速能力（v^2=2as公式）进行速度限幅。接着利用DWAPlanner::checkTrajectory进行轨迹打分检测，逻辑同前。

posted @ 2018-01-16 16:48 sakabatou 阅读(19250) 评论(1) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部

sakabatou

stay hungry, stay foolish

ROS源码阅读---局部路径规划之DWAPlannerROS分析

公告