DWA动态窗口法的C++实现!!

Keep_moving2

已于 2024-07-24 17:59:00 修改

阅读量578

点赞数 12

文章标签： c++ 人工智能算法

于 2024-07-24 15:19:41 首次发布

本文链接：https://blog.csdn.net/Lee_zj123/article/details/140663626

版权

DWA（动态窗口法，Dynamic Window Approach）算法是一种在机器人路径规划中广泛使用的方法，特别是用于移动机器人在避障的同时进行实时局部路径规划。DWA算法通过计算机器人的动态窗口来选择最优的速度和转向角度，从而使机器人在避开障碍物的同时，高效地移动向目标位置。

DWA算法的基本原理：
1、速度空间采样：首先，算法在机器人的当前可行的速度（线速度和角速度）空间内进行采样。这个“窗口”考虑了机器人的加速度和速度限制。
2、模拟轨迹：对于每一对采样的速度和角速度，算法模拟一段时间内机器人的轨迹。
3、评价函数：每条模拟出的轨迹都会根据几个标准来进行评估，这些标准包括：
（1）到目标的距离：优先考虑那些能够使机器人更接近目标的轨迹。
（2）避障能力：评估轨迹与最近障碍物的距离，避免碰撞。
（3）行进速度：通常情况下，更快的速度是更受欢迎的。
4、选择最优轨迹：根据评价函数，选择得分最高的轨迹对应的速度和角速度作为当前时刻机器人的控制指令。
5、执行和反馈：机器人执行选定的速度和转向，然后算法根据机器人的新位置和环境信息更新状态，重新进行计算。

DWA算法的实现步骤：
1、初始化：设置机器人的速度范围、加速度限制、评价标准等参数。
2、循环开始：
（1）从当前机器人状态（位置、速度、方向）和环境信息（如障碍物位置）出发。
（2）在动态窗口内采样可能的速度（线速度和角速度）组合。
（3）对每个速度组合，计算一定时间内的机器人轨迹。
（4）应用评价函数对所有轨迹进行评分。
（5）选择评分最高的速度组合。
（6）发送控制指令到机器人执行。
（7）收集执行后的反馈信息，更新机器人状态和环境信息。
3、循环结束：如果机器人到达目标或者达到某些终止条件（如时间、距离限制等），则算法结束。

具体的原理可以参考这篇博客
（1）https://blog.csdn.net/sinat_52032317/article/details/128192125?spm=1001.2101.3001.6650.3&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7ECtr-3-128192125-blog-137360042.235%5Ev43%5Epc_blog_bottom_relevance_base2&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7ECtr-3-128192125-blog-137360042.235%5Ev43%5Epc_blog_bottom_relevance_base2&utm_relevant_index=6
（2）https://blog.csdn.net/Yong_Qi2015/article/details/125383210?utm_medium=distribute.pc_relevant.none-task-blog-2_defaultbaidujs_baidulandingword~default-1-125383210-blog-137360042.235^v43pc_blog_bottom_relevance_base2&spm=1001.2101.3001.4242.2&utm_relevant_index=4

代码部分参考了这篇博客
（1）https://bigdavid.blog.csdn.net/article/details/137360042?spm=1001.2014.3001.5502

相关文件有如下几个：
dwa.h

#include <stdio.h>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <memory>
#include <vector>
#include <unordered_map>
#include "matplotlibcpp.hpp"
#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 "dwa.h"
#include <iostream>
#include <fstream>
#include <string>

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));
}

dwa_test.cpp（也就是main函数）

#include <iostream>
#include <fstream>
#include <string>
#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}
)

由于这段代码需要使用到matplotlibcpp.h文件，因此需要到https://github.com/lava/matplotlib-cpp网站去下载对应的头文件
具体的配置可以参考这篇博客
https://blog.csdn.net/m0_73225361/article/details/137425712
核心部分就是把复制放进你的工程里面，确保能读得到这个头文件即可。

但因为matplotlibcpp.h这个头文件包含<Python.h>在这个头文件，为了能够编译通过，你需要查看你的python版本，然后将下面的指令添加到你的CmakeList.txt文件里，这样子就可以通过编译了。
查看python的版本指令

find /usr/include -name Python.h

在这里插入图片描述
最终的效果如下：
请添加图片描述

Keep_moving2

关注

12
点赞
踩
19

收藏

觉得还不错? 一键收藏
0
评论
DWA动态窗口法的C++实现!!

DWA算法通过计算机器人的动态窗口来选择最优的速度和转向角度，从而使机器人在避开障碍物的同时，高效地移动向目标位置。1、速度空间采样：首先，算法在机器人的当前可行的速度（线速度和角速度）空间内进行采样。5、执行和反馈：机器人执行选定的速度和转向，然后算法根据机器人的新位置和环境信息更新状态，重新进行计算。4、选择最优轨迹：根据评价函数，选择得分最高的轨迹对应的速度和角速度作为当前时刻机器人的控制指令。（1）从当前机器人状态（位置、速度、方向）和环境信息（如障碍物位置）出发。
复制链接

扫一扫