# DWA

**Repository Path**: mwh2035/dwa

## Basic Information

- **Project Name**: DWA
- **Description**: DWA的MATLAB实现。
- **Primary Language**: Unknown
- **License**: GPL-3.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 13
- **Forks**: 1
- **Created**: 2024-02-24
- **Last Updated**: 2026-03-30

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 应用代码思路解析

*该文档只对$MATLAB$代码进行解释，不对DWA原理进行详细阐述，具体原理请详细查看另一文件。*

## 仿真效果

![dynamic](https://gitee.com/mwh2035/logimg/raw/master/img/202402261006890.gif)

## 代码框架

```c
│  Dwa_main.m
│  README.en.md
├─DwaFunction
└─InitializeData
```

1. $main.m$运行$DWA$主要过程。
2. $README.en.md$为中文讲解文档。
3. $DwaFunction$存储$DWA$用到的函数。
4. $InitializeData$存储栅格地图、运动障碍等信息。

## 初始化过程

```matlab
%% 初始化参数设置
% 设置仿真步长
global dt;
dt = 0.1;
% 设置最长仿真时间200s
% 超出仿真时间路径依旧不收敛记为规划失败
End_Time = 200;
% 循环次数
cycles = End_Time/dt;
% 起点下标[y,x]
startpossub = [20,36];
% 起点索引
startposind = sub2ind([42,42],startpossub(1),startpossub(2));
% 目标点下标[y,x]
goal = [15,5];
% 目标点索引
goalposind = sub2ind([42,42],goal(1),goal(2));
% 初始化参数
[field,obstacle,RM_parameters,Lidar] = DwaInitialization(field,startpossub);
```

仿真步长$dt$设置为全局变量，仿真时间($End_Time$)设置为$200s$超出时间未找到终点视为规划失败，$cycles$为仿真的循环次数。

起始点可自行设置，$startposind,goalposind$为一维索引，这里对应$42\times42$主要是因为后续对$40\times40$地图进行边缘扩充。

### DwaInitialization

```matlab
function [field,obstacle,RM_parameters,Lidar,IMM] = DwaInitialization(field,startpossub)
global dt;
% 填充边缘
field = padarray(field, [1 1], 1);
% 获取障碍二维坐标
obstacle = GetAllobstacle(field);
%% 机器人模型参数
% 状态变量[x,y,v,theta,w]
RM_parameters.x = [startpossub(1);startpossub(2);0;0;toRadian(0)];
% 机器人运动学限制:最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],速度分辨率[m/s],转速分辨率[rad/s]
% RM_parameters.KinematicLimit = [2.3,toRadian(100.0),1.0,toRadian(80.0),0.1,toRadian(5)];
RM_parameters.KinematicLimit = [1.5,toRadian(100.0),1.0,toRadian(80.0),0.1,toRadian(5)];
% 动态窗口
RM_parameters.DynamicWin = [];
% 评价函数参数 [heading,dist,velocity,predictDT]
RM_parameters.evalParam=[0.08,0.3,0.12,0.1,20*dt];
% 记录一次评价参数
RM_parameters.evalDB = [];
% 记录一次动态窗口轨迹
RM_parameters.trajDB = [];
% 机器人移动状态汇总
RM_parameters.savex = [];
%% Lidar参数
% 雷达半径
Lidar.r = 10;
% 雷达全局坐标
Lidar.x = RM_parameters.x(1);
Lidar.y = RM_parameters.x(2);
% 雷达数据(极坐标格式)
Lidar.Data = [];
% 局部障碍位置
Lidar.PartObstacle = [];
% 最小距离索引
Lidar.indmin = 0;
% 检测动态点标志
Lidar.dynamic = 0;
end
```

边缘填充目的是对原地图增加边框限制，防止跑出栅格地图外。

![padding](C:\Users\86180\Desktop\NDwa\READMEpic\padding.jpg)

获取栅格地图内所有障碍的二维索引保存至$obstacle$，用于局部窗口的筛选和雷达的仿真。

### 预处理

```matlab
%% 预处理
% 速度色阶
colorv = cmap(1+ceil(255*RM_parameters.x(3)/RM_parameters.KinematicLimit(1)),:);
Fig=figure;
filename = 'dynamic2.gif';      % 输出路径+保存的文件名.gif
% 移动障碍索引
j = 1;
```

1. $RM_parameters.x(3)$为当前线速度。
2. $RM_parameters.KinematicLimit(1)$为动力学最大线速度。
3. $ceil(255*RM_parameters.x(3)/RM_parameters.KinematicLimit(1))+1$相当于将当前速度映射到$0-255$的色阶上。

$colorv$用于存储每次仿真速度对应的$RGB$信息。

$j$为移动障碍位置信信息的索引。

## Lidar模拟

### DWA过程

![image-20240210085326366](C:\Users\86180\AppData\Roaming\Typora\typora-user-images\image-20240210085326366.png)

#### 动态窗口计算

```matlab
%{
动态窗口计算
输入：当前机器人状态，机器人动力学限制
输出：动态窗口[vmin,vmax,wmin,wmax](下一个步长的所有可能速度和角速度范围)
%}
function DynamicWin=CalcDynamicWindow(x,KinematicLimit)  
% 仿真步长
global dt;  
% 参数重命名
v = x(3);
w = x(5);
vmax = KinematicLimit(1);
wmax = KinematicLimit(2);
a = KinematicLimit(3);
wa = KinematicLimit(4);
%% 计算动态窗口
% 机器人速度和角速度限制
Vs=[0 vmax -wmax wmax];  
% 根据当前速度以及加速度限制计算的动态窗口  
Vd=[v - a*dt v+a*dt w-wa*dt w+wa*dt];  
% 最终的Dynamic Window  
Vtmp=[Vs;Vd];  
DynamicWin = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];  
end
```

#### Lidar局部区域检索

```matlab
%{
仿真Lidar获取周围无遮挡障碍信息
输入：所有障碍坐标，雷达参数，动态障碍信息
输出：Lidar参数
%}
function Lidar = LidarSearch(obstacle,Lidar,dynamicOb)
% 清除标志
Lidar.dynamic = 0;
%% 重命名坐标
x = Lidar.x;
y = Lidar.y;
r = Lidar.r;
% 记录局部无遮挡障碍(局部极坐标形式)
partObstacle = [];
% 获取局部障碍
for i = floor(x-r):1:ceil(x+r)
    for j = floor(y-r):1:ceil(y+r)
        ind = find(obstacle(:,1) == i&obstacle(:,2) == j);
        if ~isempty(ind)
            partObstacle = [partObstacle;obstacle(ind,:)+0.5];
        end
    end
end
% 添加动态点
if sqrt((dynamicOb(1) - x)^2+(dynamicOb(2) - y)^2) < r
    Lidar.dynamic = 1;
    partObstacle = [partObstacle;dynamicOb'];
end
[p,Lidar.PartObstacle] = obstacleDetection(x,y,partObstacle);
Lidar.Data = p ;
end
```

$floor$为向下取整，$ceil$为向上取整

```matlab
for i = floor(x-r):1:ceil(x+r)
    for j = floor(y-r):1:ceil(y+r)
        ind = find(obstacle(:,1) == i&obstacle(:,2) == j);
        if ~isempty(ind)
            partObstacle = [partObstacle;obstacle(ind,:)+0.5];
        end
    end
end
```

上面部分代码实际选取的是$[x-r，x+r],[y-r,y+r]$的矩形区域障碍。

```MATLAB
% 添加动态点
if sqrt((dynamicOb(1) - x)^2+(dynamicOb(2) - y)^2) < r
    Lidar.dynamic = 1;
    partObstacle = [partObstacle;dynamicOb'];
end
```

动态点运行到以雷达坐标为中心、检测半径的圆形范围内时，将动态点加入局部障碍。

##### obstacleDetection

```matlab
%{
仿真Lidar检索局部范围内无遮挡障碍
输入：Lidar x坐标，Lidar y坐标，局部障碍集合
输出：无遮挡的障碍信息(极坐标格式)，无遮挡的障碍信息(全局二维格式)，
%}
function [polar,partob] = obstacleDetection(x,y,ob,r)
polarsum = [];
partob = [];
replace = 0;
% 遍历局部区域障碍筛选满足条件障碍
for i = 1:size(ob,1)
    % 计算新障碍的极坐标模长(长度)
    L = sqrt((x - ob(i,1))^2+(y - ob(i,2))^2);
    % 在雷达检测范围内才会进行测距
    if L<=r
        % 计算新障碍的极坐标角度(弧度制)
        angle = atan2(ob(i,1) - x,ob(i,2) - y);
        % 使用新障碍信息与所有已录入障碍信息进行比较
        j = 1;
        while j <= size(polarsum,1)
            % 由于该循环内有对polarsum的删减，所以需要注意数组访问越界问题
            if j > size(polarsum,1)
                break;
            end
            % 如果已录入障碍与新障碍的角度相近
            if abs(polarsum(j,2) - angle) <= atan(0.75/polarsum(j,1))
                % 新增障碍的模长比已录入障碍的模长短则进行替换
                if L < polarsum(j,1)
                    % 如果新障碍信息已替换，删除当前障碍信息
                    if replace == 1
                        partob(j,:) = [];
                        polarsum(j,:)=[];
                        % 抵消当前索引增加
                        j = j - 1;
                    else % 新障碍信息还未录入
                        % 将已录入障碍信息替换为新障碍信息
                        replace = 1;
                        polarsum(j,:) = [L,angle];
                        partob(j,:) = [ob(i,:)];
                    end
                else % 新增障碍的模长比已录入障碍的模长更长，则不在录入，仅进行删减
                    replace = 1;
                end
            end
            j = j + 1;
        end
        % 如果没有发生障碍的删减，将新障碍信息录入局部障碍
        if ~replace
            polarsum = [polarsum;[L,angle]];
            partob = [partob;ob(i,:)];
        end
        replace = 0;
    end
end
    polar = polarsum;
end
```

该函数主要解决的问题是：**如何选取圆形区域内无遮挡的障碍信息？**

传入该函数的$ob$为边长为$r$的矩形区域内的所有障碍位置信息，后续通过

```matlab
    % 计算新障碍的极坐标模长(长度)
    L = sqrt((x - ob(i,1))^2+(y - ob(i,2))^2);
    % 在雷达检测范围内才会进行测距
    if L<=r
    ...
    END
```

筛选圆形区域内障碍，再进行遮挡区域去除。

<img src="C:\Users\86180\Desktop\NDwa\READMEpic\Lidar.jpg" alt="Lidar" style="zoom: 33%;" />

例如上图中绿色直虚线指向的障碍实际应该是测不到的(由于有更近的障碍物遮挡)，所以筛选的主要思路是排除指定角度范围内，距离更远的障碍，因为距离更近的障碍会遮挡远距离的障碍。

<img src="C:\Users\86180\AppData\Roaming\Typora\typora-user-images\image-20240211160854143.png" alt="image-20240211160854143" style="zoom: 67%;" />

角度相邻的范围$\theta$，可以由上述示意图计算。其中红色虚线部分在$\theta$范围内，且被该角度范围内更近的障碍物遮挡，所以会排除红色虚线指向的障碍。地图中的栅格尺寸为$1\times1$，所以$\theta=arctan(\sqrt2/2/L)$，其中$L$为到该障碍的距离。

```matlab
            % 如果已录入障碍与新障碍的角度相近
            if abs(polarsum(j,2) - angle) <= atan(0.75/polarsum(j,1))
                % 新增障碍的模长比已录入障碍的模长短则进行替换
                if L < polarsum(j,1)
                    % 如果新障碍信息已替换，删除当前障碍信息
                    if replace == 1
                        partob(j,:) = [];
                        polarsum(j,:)=[];
                        % 抵消当前索引增加
                        j = j - 1;
                    else % 新障碍信息还未录入
                        % 将已录入障碍信息替换为新障碍信息
                        replace = 1;
                        polarsum(j,:) = [L,angle];
                        partob(j,:) = [ob(i,:)];
                    end
                else % 新增障碍的模长比已录入障碍的模长更长，则不在录入，仅进行删减
                    replace = 1;
                end
            end
```

## DWA 过程

另一文档中有原理说明不在赘述。

## 绘图部分

```matlab
    % 计算速度色阶
    colorv = [colorv;cmap(1+ceil(254*RM_parameters.x(3)/RM_parameters.KinematicLimit(1)),:)];
```

将每次循环计算的速度映射到$0-255$的色阶上，并保存到$colorv$中。

```matlab
% 绘制速度映射的色阶轨迹
if USE_Traj
    for c =1:size(RM_parameters.savex,2)
        scatter(RM_parameters.savex(2,c),RM_parameters.savex(1,c),6,...
            'MarkerEdgeColor',colorv(c,:),...
            'MarkerFaceColor',colorv(c,:)...
            );hold on;
    end
end
```

从$RM_parameters$结构体中，找出指定的位置信息，并配合该速度的色阶信息，绘制圆点。

```matlab
    % 动态窗口轨迹
    if ~isempty(RM_parameters.trajDB)
        for it=1:length(RM_parameters.trajDB(:,1))/5
            ind=1+(it-1)*5;
            plot(RM_parameters.trajDB(ind+1,:),RM_parameters.trajDB(ind,:),'-g');hold on;
        end
    end
```

$trajDB$中每五行为动态窗口的一条预测轨迹，所以每绘制完一条轨迹线，$ind$索引需要增加5。