ROS1 DWB 与 ROS2 DWA 比较

“DWA算法(dynamic window approach)是移动机器人在运动模型下推算(v,w)对应的轨迹,确定速度采样空间或者说是动态窗口(三种限制);在速度空间(v,w)中采样多组速度,并模拟这些速度在一定时间内的运动轨迹,通过一个评价函数对这些轨迹打分,选取最优的轨迹来驱动机器人运动”。ROS1导航中局部路径规划便可选用该方法，DWB是在DWA的基础上进行了一定的优化和扩展，被ROS2采用。

在该算法中，重要的函数主要是两个：采样轨迹生成和轨迹评分，下面结合相关源码进行简单的解读和比较。

一. DWB代码和原理简单解读

轨迹采样函数generateTrajectory：根据机器人当前位姿start_pose，速度start_vel和目标速度cmd_vel，在预设的采样时间内进行速度空间采样，并利用离散机器人运动学模型，生成对应的轨迹，用于后续的评分，中的computeNewVelocity会在机器人运动学约束下使得速度尽可能接近目标速度，而最终的目标速度是指机器人在采样时间段内可达最终速度。

dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
const geometry_msgs::msg::Pose2D & start_pose,
const nav_2d_msgs::msg::Twist2D & start_vel,
const nav_2d_msgs::msg::Twist2D & cmd_vel)
{
dwb_msgs::msg::Trajectory2D traj;
traj.velocity = cmd_vel;
// simulate the trajectory
geometry_msgs::msg::Pose2D pose = start_pose;
nav_2d_msgs::msg::Twist2D vel = start_vel;
double running_time = 0.0;
std::vector<double> steps = getTimeSteps(cmd_vel);
traj.poses.push_back(start_pose);
for (double dt : steps) {
// calculate velocities
vel = computeNewVelocity(cmd_vel, vel, dt);
// update the position of the robot using the velocities passed in
pose = computeNewPosition(pose, vel, dt);
traj.poses.push_back(pose);
traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
running_time += dt;
} // end for simulation steps
if (include_last_point_) {
traj.poses.push_back(pose);
traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
}
return traj;
}

评分函数：使用每一个预设加载的评分函数对轨迹进行评分，然后进行加权求和，评分越低，代表该轨迹越优。

dwb_msgs::msg::TrajectoryScore
DWBLocalPlanner::scoreTrajectory(
const dwb_msgs::msg::Trajectory2D & traj,
double best_score)
{
dwb_msgs::msg::TrajectoryScore score;
score.traj = traj;
for (TrajectoryCritic::Ptr & critic : critics_) {
dwb_msgs::msg::CriticScore cs;
cs.name = critic->getName();
cs.scale = critic->getScale();
if (cs.scale == 0.0) {
score.scores.push_back(cs);
continue;
}
double critic_score = critic->scoreTrajectory(traj);
cs.raw_score = critic_score;
score.scores.push_back(cs);
score.total += critic_score * cs.scale;
if (short_circuit_trajectory_evaluation_ && best_score > 0 && score.total > best_score) {
// since we keep adding positives, once we are worse than the best, we will stay worse
break;
}
}
return score;
}

可选加载的评分函数有：

轨迹评分：评分越低越优
base_obstacle: 将代价地图在轨迹上每个位姿处的cost值进行加和，如果碰到障碍物或者出界，则会抛出异常，轨迹被视为非法;
GoalAlign：使得机器人在导航过程中方向慢慢朝向目标方向，避免到点后再原地旋转，因为这种方式可能不稳定;
RotateToGoal：
如果不够接近目标，返回0
如果足够接近目标，且当前平移速度大于停止速度，但采样速度超过当前速度，该轨迹不行;
如果足够接近目标，且当前平移速度大于停止速度，采样速度小于当前速度，则采样轨迹终点角度越接近目标角度，速度值越小，评分越低;
如果足够接近目标，且当前平移速度小于停止速度，则有平移的速度都不行;
否则，采样轨迹终点角度越接近目标角度，评分越低;
Oscillation：
轨迹中第一个采样速度为停止（线速度和角速度有一个停止，可能需要修改判断逻辑），则路径非法
否则返回0
PathAlign
如果足够接近目标（参数：forward_point_distance），返回0
否则返回轨迹末尾位姿往前延伸forward_point_distance处的cost值

PathDist：轨迹离规划路径越接近，得分越低，它偏好更接近给定参考轨迹的轨迹;
GoalDist：找到轨迹在局部代价地图中离当前位姿最远的位姿，使得轨迹最后的位姿更接近该位姿;

ObstacleFootprintCritic：考虑了机器人形状的障碍物规避评估，该形状在局部地图中设置，即footprint

 TwirlingCritic：惩罚旋转，轨迹初始旋转速度越大，得分越高。对于差速机器人，没必要选这个，但对于全向或者接近全向的机器人,有时候机器人在去往目标的过程中，比期望的旋转太多，这个评分提供了一个方式用于惩罚纯旋转速度。

二. DWA代码和原理简单解读

bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
// dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
ROS_ERROR("Could not get local plan");
return false;
}

//if the global plan passed in is empty... we won't do anything
if(transformed_plan.empty()) {
ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());
if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
//publish an empty plan because we've reached our goal position
std::vector<geometry_msgs::PoseStamped> local_plan;
std::vector<geometry_msgs::PoseStamped> transformed_plan;
publishGlobalPlan(transformed_plan);
publishLocalPlan(local_plan);
base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
return latchedStopRotateController_.computeVelocityCommandsStopRotate(
cmd_vel,
limits.getAccLimits(),
dp_->getSimPeriod(),
&planner_util_,
odom_helper_,
current_pose_,
boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
} else {
bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
if (isOk) {
publishGlobalPlan(transformed_plan);
} else {
ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
std::vector<geometry_msgs::PoseStamped> empty_plan;
publishGlobalPlan(empty_plan);
}
return isOk;
}
}

DWB是DWA算法的扩展和改进，它们的不同之处在于

1. DWA在轨迹生成器初始化和轨迹生成时，根据参数use_dwa_进行一定的选择，而DWB等同于前者use_dwa_默认为true的情况，但在初始化时并没有对第一个采样速度进行加速度约束，造成速度指令可能超出加速度上限。

2. DWA在位置到达时，进行纯旋转调整方向，而DWB是使用RotateToGoal评分在位置到达时优先选择纯旋转轨迹;

3. DWA的评分项中没有考虑机器人底盘（footprint）可能会落入障碍物。

三. 局限性
DWA/DWB采样时，采样速度会倾向于目标速度，这使得速度只会单调变化（如单调减少，或者单调增加），对于线速度而言没什么明显不合适，但对于旋转而言，则造成每条采样轨迹只能朝一个方向延伸，如下图所示（黑色表示不合理采样轨迹，绿色无箭头附着的表示合理轨迹，绿色有箭头附着的为想跟随的参考轨迹），不适用于Z字形连续弯道和需要频繁转向的动态避障，这也是一个值得优化的点。有点则在于算法简单高效，低动态场景下适用。