DWA的MATLAB实现dwa-master.zip

# 应用代码思路解析 *该文档只对$MATLAB$代码进行解释，不对DWA原理进行详细阐述，具体原理请详细查看另一文件。* ## 仿真效果  ## 代码框架 ```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 ``` 边缘填充目的是对原地图增加边框限制，防止跑出栅格地图外。  获取栅格地图内所有障碍的二维索引保存至$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过程  #### 动态窗口计算 ```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 % 新增障碍的模长比已录入障碍的模长更长，则不在录入，仅进行删