【局部路径规划算法】—— DWA动态窗口法（c++实现））

参考资料：
（1）机器人局部避障的动态窗口法(dynamic window approach)
（2）机器人局部避障的动态窗口法
（3）局部规划算法：DWA算法原理
（4）SLAM学习：DWA算法原理和编程实现（PYTHON）
（5）【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）

1 DWA动态窗口法

动态窗口法（Dynamic Window Approach）：常用的局部避障规划方法
在这里插入图片描述
(1) 轨迹预测：建立智能汽车关于速度和角速度的运动学模型
$\begin{array}{l} x=x+v \Delta t \cos \left(\theta_{t}\right) \\ y=y+v \Delta t \sin \left(\theta_{t}\right) \\ \theta_{t}=\theta_{t}+w \Delta t \end{array}$

上式表明在一定时间窗口内，车辆的位置和航向角可由当前车辆速度和当前角速度决定，那么不同的速度和角速度组合将产生不同的车辆位置和航向角，也就产生不同的车辆运动轨迹。
在速度和加速度所构成的二维空间，由于不受任何约束，存在着无穷多种组合，速度和加速度采样的目的就是将采样区域缩小在一定范围，并适当设置速度分辨率，以生成若干组数量有限的速度和加速度组合。缩小采样区域通过设置各类约束条件实现。

(2) 速度采样：充分考虑智能汽车的速度、加速度等物理限制约束条件，在速度和角速度空间内采样获取一对速度指令，通过设定一个时间区间获得一簇轨迹曲线
在这里插入图片描述

(3) 轨迹评价：构造一种轨迹性能评价指标选取最优轨迹

在每一个蓝色虚线框内，车辆都会根据当前的速度组合生成若干条备选运动轨迹，这些轨迹构成的轨迹曲线簇就称为一个时间窗
车辆在这个窗口内按照特定规则选择一条最优轨迹运动。当运动到1号窗口轨迹的末端后又重新生成一个新的轨迹曲线簇（即2号窗口），如此反复循环动态执行，故命名为动态窗口法

航向角评价函数。我们设在当前采样速度组合下的时间窗口轨迹末端的航向角为heading，该朝向与目标点的角度差值越小，则方位角评价函数值越高，表明此速度组合对应的运动轨迹越好。
障碍物距离评价函数。设dist为速度组合（v,w）对应的轨迹曲线与障碍车的最小距离，数值越大，则障碍物距离评价函数值越高，表明此速度组合对应的运动轨迹越好。
速度评价函数。我们希望车辆在正常行驶过程中与目标速度越接近越好，故速度越高，可以认为评价函数值越高。

在这里插入图片描述

最后，利用归一化思想对上述三种轨迹评价函数进行相加构成综合评价函数指标，以此作为标准筛选时间窗内的最优轨迹

在这里插入图片描述 DWA算法demo

BEGIN DWA(robotPose,robotGoal,robotModel)  laserscan = readScanner()  allowable_v = generateWindow(robotV, robotModel)  allowable_w  = generateWindow(robotW, robotModel)  // 根据能否及时刹车剔除不安全的速度for each v in allowable_v  for each w in allowable_w  dist = find_dist(v,w,laserscan,robotModel)  breakDist = calculateBreakingDistance(v)//刹车距离  if (dist > breakDist)  //如果能够及时刹车，该对速度可接收heading = hDiff(robotPose,goalPose, v,w)   //clearance与原论文稍不一样  clearance = (dist-breakDist)/(dmax - breakDist)   cost = costFunction(heading,clearance, abs(desired_v - v))  if (cost > optimal)  best_v = v  best_w = w  optimal = cost  set robot trajectory to best_v, best_w  
END

2 DWA算法流程

在这里插入图片描述

3 应用场景

应用模型：适用于两轮差分和全向移动模型、不能用在阿克曼模型。
优点：
（1）计算复杂度低：考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小
（2）可以实现避障：可以实时避障，但是避障效果一般
（3）适用于两轮差分和全向移动模型
缺点：
（1）前瞻性不足：只模拟并评价了下一步，如在机器人前段遇见“C”字形障碍时，不能很好的避障
（2）非全局最优路径：每次都选择下一步的最佳路径，而非全局最优路径
（3）动态避障效果差
（4）不能用在阿克曼模型

4 DWA可视化

参考b站Ally大佬的代码
dwa.h

#include <stdio.h>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <memory>
#include <vector>
#include <unordered_map>
#include "matplotlibcpp.h"
#include <stdlib.h>
#include <algorithm>#define PI 3.14159
class DWA {
private:// 位姿typedef struct{double x = 0;double y = 0;} Pos;// 权重struct Weight{double heading = 1;double dist = 3;double speed = 3;} weight_;// 速度采样typedef struct{double linear_vel_min = 0;double linear_vel_max = 0;double angular_vel_min = 0;double angular_vel_max = 0;} SpeedRange;// 车辆状态typedef struct{double pos_x = 0;double pos_y = -1.75;double heading = 0;double linear_vel = 2;double angular_vel = 0;} VehicleState;// 窗口信息typedef struct{double linear_vel = 0;double angular_vel = 0;std::vector<VehicleState> trajectory;double heading_value;double dist_value;double vel_value;double value = 0;} WindowInfo;// 障碍物typedef struct{double center_x = 0;double center_y = 0;double radius = 0;} Obstacle;// 场景基本参数double road_width_ = 3.5; // 道路标准宽度double road_len_ = 60;    // 道路长度double veh_width_ = 1.6;  // 汽车宽度double veh_len_ = 4.5;    // 车长Pos goal_pos_;            // 目标点位置// 时间参数double dt_ = 0.1;        // 车辆单步运动时间double window_time_ = 3; // 窗口时间// 自车运动学模型参数double v_max_ = 15;                 // 最高速度double omega_max_ = 200 * PI / 180; // 最高角速度double acc_max_ = 3;                // 最高加速度double alpha_max_ = 50 * PI / 180;  // 最高角加速度double v_res_ = 0.1;                // 速度分辨率double omega_res_ = 2 * PI / 180;   // 角速度分辨率VehicleState vehicle_state_;std::vector<WindowInfo> window_info_;std::vector<Obstacle> obstacles_;std::vector<double> path_x;std::vector<double> path_y;
public:DWA() {}~DWA() {}bool run();// 设置障碍物环境void setObstacles();// 根据当前状态和约束计算当前的速度参数允许范围SpeedRange GetSpeedRange();// 获取窗口信息void GetWindowInfo(DWA::SpeedRange speed_range);// 更新自车状态void UpdateState(int max_value_idx);// 找到最大评价函数对应的索引int FindMaxValue();// 获取本时间窗内的轨迹void GetTrajectory(std::vector<VehicleState> *trajectory, double linear_vel, double angular_vel);// 可视化void Plot(bool to_goal_dist_flag, int max_value_idx);// 获取自车几个点位void GetEgoPoint(std::vector<double> *x_list, std::vector<double> *y_list);
};

dwa.cpp

#include "../include/dwa.h"bool DWA::run() {goal_pos_.x = 57;goal_pos_.y = 2.5;// 设置障碍物环境DWA::setObstacles();bool to_goal_dist_flag = false;for (size_t i = 0; i < 1000; i++){// 根据当前状态和约束计算当前的速度参数允许范围DWA::SpeedRange speed_range = GetSpeedRange();// 根据速度范围和分辨率，生成若干条运动轨迹DWA::GetWindowInfo(speed_range);// 找到最大评价函数对应的索引int max_value_idx = DWA::FindMaxValue();// 更新自车状态DWA::UpdateState(max_value_idx);// 判断是否到达终点，并画图std::cout << "iter: " << i << "; pos_x: " << vehicle_state_.pos_x << "; pos_y: " << vehicle_state_.pos_y<< "; heading: " << vehicle_state_.heading << "; linear_vel: " << vehicle_state_.linear_vel<< "; angular_vel: " << vehicle_state_.angular_vel << std::endl;double to_goal_dist = std::sqrt(std::pow(vehicle_state_.pos_x - goal_pos_.x, 2) +std::pow(vehicle_state_.pos_y - goal_pos_.y, 2));if (to_goal_dist < 1){std::cout << "finish !" << std::endl;to_goal_dist_flag = true;DWA::Plot(to_goal_dist_flag, max_value_idx);break;}DWA::Plot(to_goal_dist_flag, max_value_idx);}return true;
}void DWA::setObstacles()
{DWA::Obstacle obstacle_tmp;// 1obstacle_tmp.center_x = 13.0;obstacle_tmp.center_y = -1.75;obstacle_tmp.radius = 2.0;obstacles_.push_back(obstacle_tmp);// 2obstacle_tmp.center_x = 27.0;obstacle_tmp.center_y = -1.75;obstacle_tmp.radius = 2.0;obstacles_.push_back(obstacle_tmp);// 3obstacle_tmp.center_x = 40.0;obstacle_tmp.center_y = 1.75;obstacle_tmp.radius = 2.0;obstacles_.push_back(obstacle_tmp);// 4obstacle_tmp.center_x = 50.0;obstacle_tmp.center_y = -1.75;obstacle_tmp.radius = 2.0;obstacles_.push_back(obstacle_tmp);// 将道路便捷模拟为若干个小的障碍物，下边界for (size_t i = 0; i < 120; i++){obstacle_tmp.center_x = i * 0.5;obstacle_tmp.center_y = 3.5;obstacle_tmp.radius = 0.5;obstacles_.push_back(obstacle_tmp);}// 将道路便捷模拟为若干个小的障碍物，上边界for (size_t i = 0; i < 120; i++){obstacle_tmp.center_x = i * 0.5;obstacle_tmp.center_y = -3.5;obstacle_tmp.radius = 0.5;obstacles_.push_back(obstacle_tmp);}
}DWA::SpeedRange DWA::GetSpeedRange()
{DWA::SpeedRange speed_range;speed_range.linear_vel_min = std::max(vehicle_state_.linear_vel - acc_max_ * dt_, 0.0);speed_range.linear_vel_max = std::min(vehicle_state_.linear_vel + acc_max_ * dt_, v_max_);speed_range.angular_vel_min = std::max(vehicle_state_.angular_vel - alpha_max_ * dt_, -omega_max_);speed_range.angular_vel_max = std::min(vehicle_state_.angular_vel + alpha_max_ * dt_, omega_max_);return speed_range;
}void DWA::GetWindowInfo(DWA::SpeedRange speed_range)
{window_info_.clear(); //清空DWA::WindowInfo window_info_tmp;std::vector<VehicleState> trajectory;double linear_vel = speed_range.linear_vel_min;while (true){double angular_vel = speed_range.angular_vel_min;while (true){// 初始化轨迹trajectory.clear();GetTrajectory(&trajectory, linear_vel, angular_vel);// 赋值window_info_tmp.linear_vel = linear_vel;window_info_tmp.angular_vel = angular_vel;window_info_tmp.trajectory = trajectory;window_info_.push_back(window_info_tmp);// 判断是否退出循环angular_vel = angular_vel + omega_res_;if (angular_vel >= speed_range.angular_vel_max){break;}}// 判断是否退出循环linear_vel = linear_vel + v_res_;if (linear_vel >= speed_range.linear_vel_max){break;}}
}void DWA::GetTrajectory(std::vector<DWA::VehicleState> *trajectory, double linear_vel, double angular_vel)
{DWA::VehicleState vehicle_state = vehicle_state_;trajectory->push_back(vehicle_state);// 循环获得窗口时间内的轨迹double time = 0;while (time <= window_time_){time = time + dt_; // 时间更新Eigen::Matrix<double, 5, 5> A;A << 1, 0, 0, 0, 0,0, 1, 0, 0, 0,0, 0, 1, 0, 0,0, 0, 0, 0, 0,0, 0, 0, 0, 0;Eigen::Matrix<double, 5, 2> B;B << dt_ * std::cos(vehicle_state.heading), 0,dt_ * std::sin(vehicle_state.heading), 0,0, dt_,1, 0,0, 1;Eigen::Matrix<double, 5, 1> x;x << vehicle_state.pos_x,vehicle_state.pos_y,vehicle_state.heading,vehicle_state.linear_vel,vehicle_state.angular_vel;Eigen::Matrix<double, 2, 1> u;u << linear_vel,angular_vel;// 状态更新Eigen::Matrix<double, 5, 1> state_new;state_new = A * x + B * u;// 更新state指针vehicle_state.pos_x = state_new(0);vehicle_state.pos_y = state_new(1);vehicle_state.heading = state_new(2);vehicle_state.linear_vel = state_new(3);vehicle_state.angular_vel = state_new(4);trajectory->push_back(vehicle_state);}
}int DWA::FindMaxValue()
{double heading_value_sum = 0;double dist_value_sum = 0;double vel_value_sum = 0;for (std::vector<DWA::WindowInfo>::iterator iter = window_info_.begin(); iter != window_info_.end();){DWA::VehicleState end_state = iter->trajectory[iter->trajectory.size() - 1];// 计算航向角评价函数double theta = end_state.heading * 180 / PI;double goal_theta = std::atan2(goal_pos_.y - end_state.pos_y, goal_pos_.x - end_state.pos_x) * 180 / PI;double target_theta = std::abs(goal_theta - theta);double heading_value = 180 - target_theta;// 计算轨迹终点距离最近障碍物距离的评价函数double dist_value = 1e5;for (size_t i = 0; i < obstacles_.size(); i++){double dist = std::sqrt(std::pow(end_state.pos_x - obstacles_[i].center_x, 2) +std::pow(end_state.pos_y - obstacles_[i].center_y, 2)) -obstacles_[i].radius;dist_value = std::min(dist_value, dist);}if (dist_value < 0){iter = window_info_.erase(iter);continue;}iter->dist_value = dist_value;// 计算速度的评价函数double vel_value = end_state.linear_vel;double stop_dist = std::pow(end_state.linear_vel, 2) / (2 * acc_max_);if (dist_value < stop_dist){iter = window_info_.erase(iter);continue;}iter->vel_value = vel_value;// 计算累计值，用于归一化处理heading_value_sum = heading_value_sum + heading_value;dist_value_sum = dist_value_sum + dist_value;vel_value_sum = vel_value_sum + vel_value;iter++;}// 归一化处理int idx = 0;double max_value = 0;for (size_t i = 0; i < window_info_.size(); i++){double heading_value_tmp = window_info_[i].heading_value / std::max(heading_value_sum, 0.01);double dist_value_tmp = window_info_[i].dist_value / std::max(dist_value_sum, 0.01);double vel_value_tmp = window_info_[i].vel_value / std::max(vel_value_sum, 0.01);window_info_[i].value = heading_value_tmp * weight_.heading +dist_value_tmp * weight_.dist + vel_value_tmp * weight_.speed;window_info_[i].value > max_value ? idx = i : idx = idx;}return idx;
}void DWA::UpdateState(int max_value_idx)
{std::vector<DWA::WindowInfo>::iterator iter = window_info_.begin() + max_value_idx;vehicle_state_ = *(iter->trajectory.begin() + 1);path_x.push_back(vehicle_state_.pos_x);path_y.push_back(vehicle_state_.pos_y);
}void DWA::Plot(bool to_goal_dist_flag, int max_value_idx)
{matplotlibcpp::clf();matplotlibcpp::title("DWA");matplotlibcpp::xlabel("X-axis");matplotlibcpp::ylabel("Y-axis");// 画灰色路面std::vector<double> x_list = {-5, -5, road_len_, road_len_};std::vector<double> y_list = {-road_width_ - 0.5, road_width_ + 0.5,road_width_ + 0.5, -road_width_ - 0.5};std::map<std::string, std::string> keywords = {{"color", "0.55"}};matplotlibcpp::fill(x_list, y_list, keywords);// 画车道线x_list = {-5, road_len_};y_list = {0, 0};keywords = {{"linestyle", "dashed"}, {"color", "w"}};matplotlibcpp::plot(x_list, y_list, keywords);x_list = {-5, road_len_};y_list = {road_width_, road_width_};matplotlibcpp::plot(x_list, y_list, "w");x_list = {-5, road_len_};y_list = {-road_width_, -road_width_};matplotlibcpp::plot(x_list, y_list, "w");// 画交通车辆for (size_t i = 0; i < 4; i++){x_list = {obstacles_[i].center_x, obstacles_[i].center_x,obstacles_[i].center_x - veh_len_, obstacles_[i].center_x - veh_len_};y_list = {obstacles_[i].center_y - veh_width_ / 2, obstacles_[i].center_y + veh_width_ / 2,obstacles_[i].center_y + veh_width_ / 2, obstacles_[i].center_y - veh_width_ / 2};keywords = {{"color", "b"}};matplotlibcpp::fill(x_list, y_list, keywords);}// 画自车GetEgoPoint(&x_list, &y_list);keywords = {{"color", "r"}};matplotlibcpp::fill(x_list, y_list, keywords);// 画窗口轨迹簇for (size_t i = 0; i < window_info_.size(); i++){x_list.clear();y_list.clear();for (size_t j = 0; j < window_info_[i].trajectory.size(); j++){x_list.push_back(window_info_[i].trajectory[j].pos_x);y_list.push_back(window_info_[i].trajectory[j].pos_y);}matplotlibcpp::plot(x_list, y_list, "g");}// 画本窗口内的最优轨迹x_list.clear();y_list.clear();for (size_t i = 0; i < window_info_[max_value_idx].trajectory.size(); i++){x_list.push_back(window_info_[max_value_idx].trajectory[i].pos_x);y_list.push_back(window_info_[max_value_idx].trajectory[i].pos_y);}matplotlibcpp::plot(x_list, y_list, "r");// 画历史轨迹matplotlibcpp::plot(path_x, path_y, "y");// 限制横纵范围matplotlibcpp::xlim(-5.0, road_len_);matplotlibcpp::ylim(-4.0, 4.0);// 设置横纵比例matplotlibcpp::set_aspect(1);// 若还未到终点，则每一次执行结束后停顿0.1sif (to_goal_dist_flag == false){matplotlibcpp::show(false); // 阻塞标志位置为falsematplotlibcpp::pause(0.01);}else{matplotlibcpp::show(true);}// 保存图片
matplotlibcpp::save("./dwa.png");}void DWA::GetEgoPoint(std::vector<double> *x_list, std::vector<double> *y_list)
{x_list->clear();y_list->clear();// 车头中心点double front_posx = vehicle_state_.pos_x;double front_posy = vehicle_state_.pos_y;double heading = vehicle_state_.heading;// 根据车头中心点位置和航向角计算车尾中心点位置double rear_posx = front_posx - veh_len_ * std::cos(heading);double rear_posy = front_posy - veh_len_ * std::sin(heading);// 根据前后中心点、航向角计算目障碍车四个轮廓点位置（顺时针编号）x_list->push_back(rear_posx - veh_width_ / 2 * std::sin(heading));y_list->push_back(rear_posy + veh_width_ / 2 * std::cos(heading));x_list->push_back(front_posx - veh_width_ / 2 * std::sin(heading));y_list->push_back(front_posy + veh_width_ / 2 * std::cos(heading));x_list->push_back(front_posx + veh_width_ / 2 * std::sin(heading));y_list->push_back(front_posy - veh_width_ / 2 * std::cos(heading));x_list->push_back(rear_posx + veh_width_ / 2 * std::sin(heading));y_list->push_back(rear_posy - veh_width_ / 2 * std::cos(heading));
}

main.cpp

#include <iostream>
#include <fstream>
#include <string>
#include "../include/dwa.h"int main()
{std::shared_ptr<DWA> dwa = std::make_shared<DWA>();dwa->run();return 0;
}

CmakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(DWA)set(CMAKE_CXX_STANDARD 11)file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so")
set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7")find_package(catkin REQUIRED COMPONENTSroscppstd_msgs
)catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES DWA
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)
# 找python包
find_package(Python3 COMPONENTS Development NumPy)include_directories(include${PYTHON2.7_INLCUDE_DIRS}
)add_executable(dwa src/dwa.cppsrc/main.cpp)target_link_libraries(dwa${catkin_LIBRARIES}${PYTHON2.7_LIB}
)