局部路径规划算法——实现DWA（dynamic window approach）控制空间采样

原创

已于 2023-02-20 23:51:32 修改 · 1.5k 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶

于 2023-02-20 19:27:20 首次发布

DWA算法是一种局部路径规划方法，用于机器人避障和导航。它在全局路径规划后，根据当前机器人位置、障碍物和目标位置，计算速度和角速度的动态窗口，通过评价函数选择最优解，确保安全到达目标。代码实现中涉及了速度、角速度的采样、预测时间、障碍物检测和加速度限制等关键点。

DWA算法是局部路径规划算法，在全局路径规划算法完成后，DWA算法能够根据当前小车（机器人）位置、障碍物、终点的位置进行控制空间（速度、角速度）的采用，从而完成局部路径规划。
在这里插入图片描述

DWA算法流程：

初始化一一小车最大最小速度、加速度，评价函数权重等循环
{
判断是否到达目的地
计算当前采样的速度范围(动态窗口)
遍历所有速度v&w，根据模型模拟一段时间的路径根据评价函数打分(包括评价函数、归一化、权重)选取最优解——v&w，下发给运动底盘
小车继续移动
}

下面是DWA算法的代码实现，主要步骤给了注释：
变量定义：

#include <iostream>
#include <vector>

using namespace std;

#define M_PI 3.1415927
#define MAX_VELOCITY 1.0						//弧形轨迹：最大速度
#define MIN_VELOCITY 0							//弧形轨迹：最小速度
#define MAX_OMEGA 20.0 / 180.0 * M_PI			//弧形轨迹：最大角速度
#define MIN_OMEGA 0								//弧形轨迹：最小角速度
#define MAX_ACCELERATE 0.2						//动态窗口：最大加速度
#define MAX_ACCOMEGA 50.0 / 180.0 * M_PI		//动态窗口：最大角加速度
#define SAMPLING_VELOCITY 0.01					//速度采样间隔
#define SAMPLING_OMEGA 1 / 180.0 * M_PI			//角速度采样间隔
#define DT 0.1									//采样时间间隔
#define PREDICT_TIME 3.0						//预测时间
#define WEIGHT_HEADING 0.05						//HEADING权重——小车与终点的角度
#define WEIGHT_CLEARANCE 0.2					//CLEARANCE权重——小车与最近障碍物的距离
#define WEIGHT_VELOCITY 0.1						//VELOCITY权重——小车速度
#define GOAL_X 10								//目标横坐标
#define GOAL_Y 10								//目标纵坐标
#define ROBOT_RADIUS 0.5						//机器人半径

struct RobotState
{
   
   
	// x坐标，y坐标，机器朝向，速度，角速度
	float xPosition, yPosition, orientation, velocity, omega;
};

// 障碍物
int obstacle[18][2] = {
   
    {
   
    0, 2 },
							{
   
    4, 2 },
							{
   
    4, 4 },
							{
   
    5, 4 },
							{
   
    5, 5 },
							{
   
    5, 6 },
							{
   
    5, 9 },
							{
   
    8, 8 },
							{
   
    8, 9 },
							{
   
    7, 9 },
							{
   
    6, 5 },
							{
   
    6, 3 },
							{
   
    6