文章目录
前言
一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956
更多保姆级PX4+ROS学习视频：https://b23.tv/ZeUDKqy
分享知识，传递正能量，如有疏漏或不当之处，恳请指出．
DWA算法一般用于局部路径规划，该算法在速度空间内采样线速度和角速度,并根据机器人的运动学模型预测其下一时间间隔的轨迹。 对待评价轨迹进行评分,从而获得更加安全、平滑的最优局部路径。
一、机器人运动学模型
动态窗口法将移动机器人的位置控制转换为速度控制。 在利用速度模式对机器人运动轨迹进行预测时,首先需要对机器人的运动模型进行分析。v ( t ) v(t)v (t )和w ( t ) w(t)w (t )分别表示机器人在世界坐标系下的平移速度和角速度，在机器人的采样周期Δ t \Delta t Δt内,位移较小,机器人作匀速直线运动,则机器人运动模型为：
{ x ( t ) = x ( t − 1 ) + v ( t ) Δ t cos ( θ ( t − 1 ) ) y ( t ) = y ( t − 1 ) + v ( t ) Δ t sin ( θ ( t − 1 ) ) θ ( t ) = θ ( t − 1 ) + w ( t ) Δ t \left{\begin{array}{l}x(t)=x(t-1)+v(t) \Delta t \cos (\theta(t-1)) \ y(t)=y(t-1)+v(t) \Delta t \sin (\theta(t-1)) \ \theta(t)=\theta(t-1)+w(t) \Delta t\end{array}\right.⎩⎨⎧x (t )=x (t −1 )+v (t )Δt cos (θ(t −1 ))y (t )=y (t −1 )+v (t )Δt sin (θ(t −1 ))θ(t )=θ(t −1 )+w (t )Δt
x ( t ) x(t)x (t )，y ( t ) y(t)y (t )，θ ( t ) \theta(t)θ(t )为t 时刻机器人在世界坐标下的位姿
二、速度空间
动态窗口法将避障问题描述为速度空间中带约束的优化问题,其中约束主要包括机器人的非完整约束、环境障碍物的约束以及机器人结构的动力学约束。 DWA 算法的速度矢量空间示意图如图所示：
,横坐标为机器人角速度 ,纵坐标为机器人线速度 ,整个区域分为安全区域和不安全区域,所有白色区域为机器人安全区域,中间红色矩形框内为考虑电机扭矩在控制周期内限制的机器人可达速度范围,排除不安全区域后,剩余的绿色区域为最终确定的动态窗口
; 三、评价函数
通过设计评价函数,在速度空间内有选取最优轨迹。 在局部规划中,需要保证最优轨迹
(1)靠近全局路径
(2)完成避障任务
(3)朝向目标快速运动。
定义评价函数为:
G ( v , w ) = k ( α Heading ( v , w ) + β Goal ( v , w ) + γ Path ( v , w ) + σ Occ ( v , w ) ) \begin{aligned} G(v, w)=& k(\alpha \text { Heading }(v, w)+\beta \operatorname{Goal}(v, w)+\gamma \operatorname{Path}(v, w)+\sigma \operatorname{Occ}(v, w)) \end{aligned}G (v ,w )=k (αHeading (v ,w )+βGoal (v ,w )+γPath (v ,w )+σOcc (v ,w ))
当G ( v , w ) G(v, w)G (v ,w )值最小时,获得最佳路径。
式中:
k k k:平滑函数
α \alpha α,β \beta β,γ \gamma γ,σ \sigma σ:各子函数的加权系数
Heading ( v , w ) \text { Heading }(v, w)Heading (v ,w ):方位角不断地朝向终点位置函数
在移动过程中,Heading ( v , w ) \text { Heading }(v, w)Heading (v ,w )函数用于使机器人的朝向不断趋向终点方向,越小,说明与终点的方位角越小,其示意图如图所示,计算公式为：
Head ( v , w ) = 18 0 ∘ − θ \operatorname{Head}(v, w)=180^{\circ}-\theta Head (v ,w )=18 0 ∘−θ
Goal ( v , w ) \operatorname{Goal}(v, w)Goal (v ,w ):用于评价机器人局部路径末端与终点的距离,通过函数不断缩短与终点的距离,其示意图如图所示。
Path ( v , w ) \operatorname{Path}(v, w)Path (v ,w ):路径评价函数,计算轨迹末端点到全局路径的距离,如下图：距离越短说明轨迹越靠近全局路径.
Path ( v , w ) = min ( x 1 − x 2 ) 2 + ( y 1 − y 2 ) 2 \operatorname{Path}(v, w)=\min \sqrt{\left(x_{1}-x_{2}\right)^{2}+\left(y_{1}-y_{2}\right)^{2}}Path (v ,w )=min (x 1 −x 2 )2 +(y 1 −y 2 )2
（ x 1 ， y 1 ） （x_{1}，y_{1}）（x 1 ，y 1 ）：机器人根据运动学模型估计的局部路径末端点坐标
（ x 2 ， y 2 ） （x_{2}，y_{2}）（x 2 ，y 2 ）：全局路径规划的节点坐标
Occ ( v , w ) \operatorname{Occ}(v, w)Occ (v ,w ):用于评价机器人轨迹到障碍物的距离,体现了机器人的避障能力,如果机器人的轨迹到障碍物的距离大于机器人半径,则没有发生碰撞的危险;反之,就说明碰撞风险大,舍弃这条轨迹。其示意图如下图所示,R 为机器人底盘半径,H 为机器人轨迹到障碍物的距离。
四、movebase代码解析
代码位置
命名空间
namespace dwa_local_planner {
配置轨迹规划器参数
void DWAPlanner::reconfigure(DWAPlannerConfig &config)
{
boost::mutex::scoped_lock l(configuration_mutex_);
generator_.setParameters(
config.sim_time,
config.sim_granularity,
config.angular_sim_granularity,
config.use_dwa,
sim_period_);
double resolution = planner_util_->getCostmap()->getResolution();
path_distance_bias_ = resolution * config.path_distance_bias;
path_costs_.setScale(path_distance_bias_);
alignment_costs_.setScale(path_distance_bias_);
goal_distance_bias_ = resolution * config.goal_distance_bias;
goal_costs_.setScale(goal_distance_bias_);
goal_front_costs_.setScale(goal_distance_bias_);
occdist_scale_ = config.occdist_scale;
obstacle_costs_.setScale(occdist_scale_);
stop_time_buffer_ = config.stop_time_buffer;
oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle);
forward_point_distance_ = config.forward_point_distance;
goal_front_costs_.setXShift(forward_point_distance_);
alignment_costs_.setXShift(forward_point_distance_);
obstacle_costs_.setParams(config.max_vel_trans, config.max_scaling_factor, config.scaling_speed);
twirling_costs_.setScale(config.twirling_scale);
int vx_samp, vy_samp, vth_samp;
vx_samp = config.vx_samples;
vy_samp = config.vy_samples;
vth_samp = config.vth_samples;
if (vx_samp 0) {
ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
vx_samp = 1;
config.vx_samples = vx_samp;
}
if (vy_samp 0) {
ROS_WARN("You've specified that you don't want any samples in the y dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
vy_samp = 1;
config.vy_samples = vy_samp;
}
if (vth_samp 0) {
ROS_WARN("You've specified that you don't want any samples in the th dimension. We'll at least assume that you want to sample one value... so we're going to set vth_samples to 1 instead");
vth_samp = 1;
config.vth_samples = vth_samp;
}
vsamples_[0] = vx_samp;
vsamples_[1] = vy_samp;
vsamples_[2] = vth_samp;
}
构造函数
DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
planner_util_(planner_util),
obstacle_costs_(planner_util->getCostmap()),
path_costs_(planner_util->getCostmap()),
goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
alignment_costs_(planner_util->getCostmap())
{
ros::NodeHandle private_nh("~/" + name);
goal_front_costs_.setStopOnFailure( false );
alignment_costs_.setStopOnFailure( false );
std::string controller_frequency_param_name;
if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
sim_period_ = 0.05;
} else {
double controller_frequency = 0;
private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
if(controller_frequency > 0) {
sim_period_ = 1.0 / controller_frequency;
} else {
ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
sim_period_ = 0.05;
}
}
ROS_INFO("Sim period is set to %.2f", sim_period_);
oscillation_costs_.resetOscillationFlags();
bool sum_scores;
private_nh.param("sum_scores", sum_scores, false);
obstacle_costs_.setSumScores(sum_scores);
private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
map_viz_.initialize(name, planner_util->getGlobalFrame(), boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));
private_nh.param("global_frame_id", frame_id_, std::string("odom"));
traj_cloud_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("trajectory_cloud", 1);
private_nh.param("publish_traj_pc", publish_traj_pc_, false);
std::vector<base_local_planner::TrajectoryCostFunction*> critics;
critics.push_back(&oscillation_costs_);
critics.push_back(&obstacle_costs_);
critics.push_back(&goal_front_costs_);
critics.push_back(&alignment_costs_);
critics.push_back(&path_costs_);
critics.push_back(&goal_costs_);
critics.push_back(&twirling_costs_);
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
generator_list.push_back(&generator_);
scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
private_nh.param("cheat_factor", cheat_factor_, 1.0);
}
计算地图网格单元的代价。
bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
path_cost = path_costs_.getCellCosts(cx, cy);
goal_cost = goal_costs_.getCellCosts(cx, cy);
occ_cost = planner_util_->getCostmap()->getCost(cx, cy);
if (path_cost == path_costs_.obstacleCosts() ||
path_cost == path_costs_.unreachableCellCosts() ||
occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
return false;
}
total_cost =
path_distance_bias_ * path_cost +
goal_distance_bias_ * goal_cost +
occdist_scale_ * occ_cost;
return true;
}
设置飞行路径
bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
oscillation_costs_.resetOscillationFlags();
return planner_util_->setPlan(orig_global_plan);
}
检查轨迹对于位置/速度对是否合法。
bool DWAPlanner::checkTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples){
oscillation_costs_.resetOscillationFlags();
base_local_planner::Trajectory traj;
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf2::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
generator_.generateTrajectory(pos, vel, vel_samples, traj);
double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
if(cost >= 0) {
return true;
}
ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vel_samples[0], vel_samples[1], vel_samples[2], cost);
return false;
}
在规划之前更新代价函数。
void DWAPlanner::updatePlanAndLocalCosts(
const geometry_msgs::PoseStamped& global_pose,
const std::vector<geometry_msgs::PoseStamped>& new_plan,
const std::vector<geometry_msgs::Point>& footprint_spec) {
global_plan_.resize(new_plan.size());
for (unsigned int i = 0; i < new_plan.size(); ++i) {
global_plan_[i] = new_plan[i];
}
obstacle_costs_.setFootprint(footprint_spec);
path_costs_.setTargetPoses(global_plan_);
goal_costs_.setTargetPoses(global_plan_);
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
double sq_dist =
(pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
(pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);
std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
forward_point_distance_ * cos(angle_to_goal);
front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
sin(angle_to_goal);
goal_front_costs_.setTargetPoses(front_global_plan);
if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
alignment_costs_.setScale(path_distance_bias_);
alignment_costs_.setTargetPoses(global_plan_);
} else {
alignment_costs_.setScale(0.0);
}
}
给定机器人的当前位置和速度，找到执行的最佳轨迹。
base_local_planner::Trajectory DWAPlanner::findBestPath(
const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& global_vel,
geometry_msgs::PoseStamped& drive_velocities) {
boost::mutex::scoped_lock l(configuration_mutex_);
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y, tf2::getYaw(global_vel.pose.orientation));
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf2::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
result_traj_.cost_ = -7;
std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
if(publish_traj_pc_)
{
sensor_msgs::PointCloud2 traj_cloud;
traj_cloud.header.frame_id = frame_id_;
traj_cloud.header.stamp = ros::Time::now();
sensor_msgs::PointCloud2Modifier cloud_mod(traj_cloud);
cloud_mod.setPointCloud2Fields(5, "x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32,
"theta", 1, sensor_msgs::PointField::FLOAT32,
"cost", 1, sensor_msgs::PointField::FLOAT32);
unsigned int num_points = 0;
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
if (t->cost_<0)
continue;
num_points += t->getPointsSize();
}
cloud_mod.resize(num_points);
sensor_msgs::PointCloud2Iterator<float> iter_x(traj_cloud, "x");
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
if(t->cost_<0)
continue;
for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
double p_x, p_y, p_th;
t->getPoint(i, p_x, p_y, p_th);
iter_x[0] = p_x;
iter_x[1] = p_y;
iter_x[2] = 0.0;
iter_x[3] = p_th;
iter_x[4] = t->cost_;
++iter_x;
}
}
traj_cloud_pub_.publish(traj_cloud);
}
if (publish_cost_grid_pc_) {
map_viz_.publishCostCloud(planner_util_->getCostmap());
}
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_vel_trans);
if (result_traj_.cost_ < 0) {
drive_velocities.pose.position.x = 0;
drive_velocities.pose.position.y = 0;
drive_velocities.pose.position.z = 0;
drive_velocities.pose.orientation.w = 1;
drive_velocities.pose.orientation.x = 0;
drive_velocities.pose.orientation.y = 0;
drive_velocities.pose.orientation.z = 0;
} else {
drive_velocities.pose.position.x = result_traj_.xv_;
drive_velocities.pose.position.y = result_traj_.yv_;
drive_velocities.pose.position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, result_traj_.thetav_);
tf2::convert(q, drive_velocities.pose.orientation);
}
return result_traj_;
}
};
[1]劳彩莲,李鹏,冯宇.基于改进A^*与DWA算法融合的温室机器人路径规划[J].农业机械学报,2021,52(1):14-22.
Original: https://blog.csdn.net/qq_38768959/article/details/122860891
Author: 超维空间科技
Title: 路径规划算法（一）：DWA动态窗口算法
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/613513/
转载文章受原作者版权保护。转载请注明原作者出处！