【局部路径规划算法】—— 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号窗口），如此反复循环动态执行，故命名为动态窗口法

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

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

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;
    // 1
    obstacle_tmp.center_x = 13.0;
    obstacle_tmp.center_y = -1.75;
    obstacle_tmp.radius = 2.0;
    obstacles_.push_back(obstacle_tmp);

    // 2
    obstacle_tmp.center_x = 27.0;
    obstacle_tmp.center_y = -1.75;
    obstacle_tmp.radius = 2.0;
    obstacles_.push_back(obstacle_tmp);

    // 3
    obstacle_tmp.center_x = 40.0;
    obstacle_tmp.center_y = 1.75;
    obstacle_tmp.radius = 2.0;
    obstacles_.push_back(obstacle_tmp);

    // 4
    obstacle_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.1s
    if (to_goal_dist_flag == false)
    {
        matplotlibcpp::show(false); // 阻塞标志位置为false
        matplotlibcpp::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 COMPONENTS
  roscpp
  std_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.cpp
                   src/main.cpp)

target_link_libraries(dwa
  ${catkin_LIBRARIES}
  ${PYTHON2.7_LIB}
)

可视化结果：

---

DWA算法不正之处望指教