当前位置:   article > 正文

DWA算法学习(带matlab源码)_dwa算法matlab

dwa算法matlab

机器人状态

位置 (x,y) 航向yaw 速度v 角速度w

机器人运动模型

最高速度m/s]

最高旋转速度[rad/s]

加速度[m/ss]

旋转加速度[rad/ss]

%速度分辨率[m/s]

转速分辨率[rad/s]]

评价函数参数

航向得分的比重

距离得分的比重

速度得分的比重

向前模拟轨迹的时间

速度采样,产生可用轨迹

通过机器人模型,以机器人的速度限制和角速度限制为界限,以速度分辨率和角速度分辨率遍历所有采样速度。

通过仿真时间确定机器人的轨迹,机器人轨迹是由点位进行表示,一定距离的点位连接形成轨迹。

计算制动距离、

速度采样下,以最大加速度进行减速至停止,最短的制动距离。

障碍物距离评价

当前预测轨迹的重点与最近的障碍物之间的距离,需要用终点去遍历所有障碍物,以找到最短障碍物距离。

这个地方可能会导致机器人左右抖动,类似于人工势场法带来的影响。

### heading朝向评价函数 当前机器人航向相对于目标点的偏航,偏离程度越小,分数越高。

如果与全局路径规划进行融合的话,主要是修正这个地方,通过全局路径规划提供一个目标点,每次机器人的移动要尽可能的贴近。

速度采样得分

主要是为了保证机器人能够更快的达到目标位置,速度越高,评分越高。

控制频率问题

发布速度指令后,以仿真周期为时间间隔。进行固定频率的采样过程。

  1. % -------------------------------------------------------------------------
  2. %
  3. % File : DynamicWindowApproachSample.m
  4. %
  5. % Discription : Mobile Robot Motion Planning with Dynamic Window Approach
  6. %
  7. % Environment : Matlab
  8. %
  9. % Author : Atsushi Sakai
  10. %
  11. % Copyright (c): 2014 Atsushi Sakai
  12. %
  13. % License : Modified BSD Software License Agreement
  14. % log: 20181031 增加详细的注释信息 下标的定义
  15. % 20181101 :增加画出障碍物的大小,更直观的看到障碍物和机器人之间的位置关系
  16. % -------------------------------------------------------------------------
  17. function [] = DynamicWindowApproachSample()
  18. close all;
  19. clear all;
  20. disp('Dynamic Window Approach sample program start!!')
  21. %% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
  22. % x=[0 0 pi/2 0 0]'; % 5x1矩阵行矩阵 位置 0,0 航向 pi/2 ,速度、角速度均为0
  23. x = [0 0 pi/10 0 0]';
  24. % 下标宏定义 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
  25. POSE_X = 1; %坐标 X
  26. POSE_Y = 2; %坐标 Y
  27. YAW_ANGLE = 3; %机器人航向角
  28. V_SPD = 4; %机器人速度
  29. W_ANGLE_SPD = 5; %机器人角速度
  30. goal = [6,10]; % 目标点位置 [x(m),y(m)]
  31. % 障碍物位置列表 [x(m) y(m)]
  32. obstacle=[0 2;
  33. 2 4;
  34. 2 5;
  35. 4 2;
  36. 4 4;
  37. 5 4;
  38. 5 5;
  39. 5 6;
  40. 5 9
  41. 8 8
  42. 8 9
  43. 7 9];
  44. obstacleR = 0.5;% 冲突判定用的障碍物半径
  45. global dt;
  46. dt = 0.1;% 时间[s]
  47. % 机器人运动学模型参数
  48. % 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],
  49. % 速度分辨率[m/s],转速分辨率[rad/s]]
  50. Kinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];
  51. %定义Kinematic的下标含义
  52. MD_MAX_V = 1;% 最高速度m/s]
  53. MD_MAX_W = 2;% 最高旋转速度[rad/s]
  54. MD_ACC = 3;% 加速度[m/ss]
  55. MD_VW = 4;% 旋转加速度[rad/ss]
  56. MD_V_RESOLUTION = 5;% 速度分辨率[m/s]
  57. MD_W_RESOLUTION = 6;% 转速分辨率[rad/s]]
  58. % 评价函数参数 [heading,dist,velocity,predictDT]
  59. % 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间
  60. evalParam = [0.01, 0.2 ,0.1, 3.0];
  61. area = [-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax]
  62. % 模拟实验的结果
  63. result.x=[]; %累积存储走过的轨迹点的状态值
  64. tic; % 估算程序运行时间开始
  65. % movcount=0;
  66. %% Main loop 循环运行 5000次 指导达到目的地 或者 5000次运行结束
  67. for i = 1:1000
  68. % DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹
  69. [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
  70. x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度
  71. % 历史轨迹的保存
  72. result.x = [result.x; x']; %最新结果 以列的形式 添加到result.x
  73. % 是否到达目的地
  74. if norm(x(POSE_X:POSE_Y)-goal')<0.5 % norm函数来求得坐标上的两个点之间的距离
  75. disp('Arrive Goal!!');break;
  76. end
  77. %====Animation====
  78. hold off; % 关闭图形保持功能。 新图出现时,取消原图的显示。
  79. ArrowLength = 0.5; % 箭头长度
  80. % 机器人
  81. % quiver(x,y,u,v) 在 x 和 y 中每个对应元素对组所指定的坐标处将向量绘制为箭头
  82. quiver(x(POSE_X), x(POSE_Y), ArrowLength*cos(x(YAW_ANGLE)), ArrowLength*sin(x(YAW_ANGLE)), 'ok'); % 绘制机器人当前位置的航向箭头
  83. hold on; %启动图形保持功能,当前坐标轴和图形都将保持,从此绘制的图形都将添加在这个图形的基础上,并自动调整坐标轴的范围
  84. plot(result.x(:,POSE_X),result.x(:,POSE_Y),'-b');hold on; % 绘制走过的所有位置 所有历史数据的 X、Y坐标
  85. plot(goal(1),goal(2),'*r');hold on; % 绘制目标位置
  86. %plot(obstacle(:,1),obstacle(:,2),'*k');hold on; % 绘制所有障碍物位置
  87. DrawObstacle_plot(obstacle,obstacleR);
  88. % 探索轨迹 画出待评价的轨迹
  89. if ~isempty(traj) %轨迹非空
  90. for it=1:length(traj(:,1))/5 %计算所有轨迹数 traj 每5行数据 表示一条轨迹点
  91. ind = 1+(it-1)*5; %第 it 条轨迹对应在traj中的下标
  92. plot(traj(ind,:),traj(ind+1,:),'-g');hold on; %根据一条轨迹的点串画出轨迹 traj(ind,:) 表示第ind条轨迹的所有x坐标值 traj(ind+1,:)表示第ind条轨迹的所有y坐标值
  93. end
  94. end
  95. axis(area); %根据area设置当前图形的坐标范围,分别为x轴的最小、最大值,y轴的最小最大值
  96. grid on;
  97. drawnow; %刷新屏幕. 当代码执行时间长,需要反复执行plot时,Matlab程序不会马上把图像画到figure上,这时,要想实时看到图像的每一步变化情况,需要使用这个语句。
  98. %movcount = movcount+1;
  99. %mov(movcount) = getframe(gcf);% 记录动画帧
  100. end
  101. toc %输出程序运行时间 形式:时间已过 ** 秒。
  102. %movie2avi(mov,'movie.avi'); %录制过程动画 保存为 movie.avi 文件
  103. %% 绘制所有障碍物位置
  104. % 输入参数:obstacle 所有障碍物的坐标 obstacleR 障碍物的半径
  105. function [] = DrawObstacle_plot(obstacle,obstacleR)
  106. r = obstacleR;
  107. theta = 0:pi/20:2*pi;
  108. for id=1:length(obstacle(:,1))
  109. x = r * cos(theta) + obstacle(id,1);
  110. y = r *sin(theta) + obstacle(id,2);
  111. plot(x,y,'-m');hold on;
  112. end
  113. % plot(obstacle(:,1),obstacle(:,2),'*m');hold on; % 绘制所有障碍物位置
  114. %% DWA算法实现
  115. % model 机器人运动学模型 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], 速度分辨率[m/s],转速分辨率[rad/s]]
  116. % 输入参数:当前状态、模型参数、目标点、评价函数的参数、障碍物位置、障碍物半径
  117. % 返回参数:控制量 u = [v(m/s),w(rad/s)] 和 轨迹集合 N * 31 (N:可用的轨迹数)
  118. % 选取最优参数的物理意义:在局部导航过程中,使得机器人避开障碍物,朝着目标以较快的速度行驶。
  119. function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,ob,R)
  120. % Dynamic Window [vmin,vmax,wmin,wmax] 最小速度 最大速度 最小角速度 最大角速度速度
  121. Vr = CalcDynamicWindow(x,model); % 根据当前状态 和 运动模型 计算当前的参数允许范围
  122. % 评价函数的计算 evalDB N*5 每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
  123. % trajDB 每5行一条轨迹 每条轨迹都有状态x点串组成
  124. [evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam); %evalParam 评价函数参数 [heading,dist,velocity,predictDT]
  125. if isempty(evalDB)
  126. disp('no path to goal!!');
  127. u=[0;0];return;
  128. end
  129. % 各评价函数正则化
  130. evalDB = NormalizeEval(evalDB);
  131. % 最终评价函数的计算
  132. feval=[];
  133. for id=1:length(evalDB(:,1))
  134. feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %根据评价函数参数 前三个参数分配的权重 计算每一组可用的路径参数信息的得分
  135. end
  136. evalDB = [evalDB feval]; % 最后一组
  137. [maxv,ind] = max(feval);% 选取评分最高的参数 对应分数返回给 maxv 对应下标返回给 ind
  138. u = evalDB(ind,1:2)';% 返回最优参数的速度、角速度
  139. %% 评价函数 内部负责产生可用轨迹
  140. % 输入参数 :当前状态、参数允许范围(窗口)、目标点、障碍物位置、障碍物半径、评价函数的参数
  141. % 返回参数:
  142. % evalDB N*5 每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
  143. % trajDB 每5行一条轨迹 每条轨迹包含 前向预测时间/dt + 1 = 31 个轨迹点(见生成轨迹函数)
  144. function [evalDB,trajDB] = Evaluation(x,Vr,goal,ob,R,model,evalParam)
  145. evalDB = [];
  146. trajDB = [];
  147. for vt = Vr(1):model(5):Vr(2) %根据速度分辨率遍历所有可用速度: 最小速度和最大速度 之间 速度分辨率 递增
  148. for ot=Vr(3):model(6):Vr(4) %根据角度分辨率遍历所有可用角速度: 最小角速度和最大角速度 之间 角度分辨率 递增
  149. % 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻 到 预测时刻之间的轨迹(由轨迹点组成)
  150. [xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4),model); %evalParam(4),前向模拟时间;
  151. % 各评价函数的计算
  152. heading = CalcHeadingEval(xt,goal); % 前项预测终点的航向得分 偏差越小分数越高
  153. dist = CalcDistEval(xt,ob,R); % 前项预测终点 距离最近障碍物的间隙得分 距离越远分数越高
  154. vel = abs(vt); % 速度得分 速度越快分越高
  155. stopDist = CalcBreakingDist(vel,model); % 制动距离的计算
  156. if dist > stopDist % 如果可能撞到最近的障碍物 则舍弃此路径 (到最近障碍物的距离 大于 刹车距离 才取用)
  157. evalDB = [evalDB;[vt ot heading dist vel]];
  158. trajDB = [trajDB;traj]; % 每5行 一条轨迹
  159. end
  160. end
  161. end
  162. %% 归一化处理
  163. % 每一条轨迹的单项得分除以本项所有分数和
  164. function EvalDB=NormalizeEval(EvalDB)
  165. % 评价函数正则化
  166. if sum(EvalDB(:,3))~= 0
  167. EvalDB(:,3) = EvalDB(:,3)/sum(EvalDB(:,3)); %矩阵的数除 单列矩阵的每元素分别除以本列所有数据的和
  168. end
  169. if sum(EvalDB(:,4))~= 0
  170. EvalDB(:,4) = EvalDB(:,4)/sum(EvalDB(:,4));
  171. end
  172. if sum(EvalDB(:,5))~= 0
  173. EvalDB(:,5) = EvalDB(:,5)/sum(EvalDB(:,5));
  174. end
  175. %% 单条轨迹生成、轨迹推演函数
  176. % 输入参数: 当前状态、vt当前速度、ot角速度、evaldt 前向模拟时间、机器人模型参数(没用到)
  177. % 返回参数;
  178. % x : 机器人模拟时间内向前运动 预测的终点位姿(状态);
  179. % traj: 当前时刻 到 预测时刻之间 过程中的位姿记录(状态记录) 当前模拟的轨迹
  180. % 轨迹点的个数为 evaldt / dt + 1 = 3.0 / 0.1 + 1 = 31
  181. %
  182. function [x,traj] = GenerateTrajectory(x,vt,ot,evaldt,model)
  183. global dt;
  184. time = 0;
  185. u = [vt;ot];% 输入值
  186. traj = x; % 机器人轨迹
  187. while time <= evaldt
  188. time = time+dt; % 时间更新
  189. x = f(x,u); % 运动更新 前项模拟时间内 速度、角速度恒定
  190. traj = [traj x]; % 每一列代表一个轨迹点 一列一列的添加
  191. end
  192. %% 计算制动距离
  193. %根据运动学模型计算制动距离, 也可以考虑成走一段段圆弧的累积 简化可以当一段段小直线的累积
  194. function stopDist = CalcBreakingDist(vel,model)
  195. global dt;
  196. MD_ACC = 3;%
  197. stopDist=0;
  198. while vel>0 %给定加速度的条件下 速度减到0所走的距离
  199. stopDist = stopDist + vel*dt;% 制动距离的计算
  200. vel = vel - model(MD_ACC)*dt;%
  201. end
  202. %% 障碍物距离评价函数 (机器人在当前轨迹上与最近的障碍物之间的距离,如果没有障碍物则设定一个常数)
  203. % 输入参数:位姿、所有障碍物位置、障碍物半径
  204. % 输出参数:当前预测的轨迹终点的位姿距离所有障碍物中最近的障碍物的距离 如果大于设定的最大值则等于最大值
  205. % 距离障碍物距离越近分数越低
  206. function dist = CalcDistEval(x,ob,R)
  207. dist=100;
  208. for io = 1:length(ob(:,1))
  209. disttmp = norm(ob(io,:)-x(1:2)')-R; %到第io个障碍物的距离 - 障碍物半径 !!!有可能出现负值吗??
  210. if dist > disttmp % 大于最小值 则选择最小值
  211. dist = disttmp;
  212. end
  213. end
  214. % 障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
  215. if dist >= 2*R %最大分数限制
  216. dist = 2*R;
  217. end
  218. %% heading的评价函数计算
  219. % 输入参数:当前位置、目标位置
  220. % 输出参数:航向参数得分 当前车的航向和相对于目标点的航向 偏离程度越小 分数越高 最大180分
  221. function heading = CalcHeadingEval(x,goal)
  222. theta = toDegree(x(3));% 机器人朝向
  223. goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));% 目标点相对于机器人本身的方位
  224. if goalTheta > theta
  225. targetTheta = goalTheta-theta;% [deg]
  226. else
  227. targetTheta = theta-goalTheta;% [deg]
  228. end
  229. heading = 180 - targetTheta;
  230. %% 计算动态窗口
  231. % 返回 最小速度 最大速度 最小角速度 最大角速度速度
  232. function Vr = CalcDynamicWindow(x,model)
  233. V_SPD = 4;%机器人速度
  234. W_ANGLE_SPD = 5;%机器人角速度
  235. MD_MAX_V = 1;%
  236. MD_MAX_W = 2;%
  237. MD_ACC = 3;%
  238. MD_VW = 4;%
  239. global dt;
  240. % 车子速度的最大最小范围 依次为:最小速度 最大速度 最小角速度 最大角速度速度
  241. Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)];
  242. % 根据当前速度以及加速度限制计算的动态窗口 依次为:最小速度 最大速度 最小角速度 最大角速度速度
  243. Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt];
  244. % 最终的Dynamic Window
  245. Vtmp = [Vs;Vd]; %2 X 4 每一列依次为:最小速度 最大速度 最小角速度 最大角速度速度
  246. Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
  247. %% Motion Model 根据当前状态推算下一个控制周期(dt)的状态
  248. % u = [vt; wt];当前时刻的速度、角速度 x = 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
  249. function x = f(x, u)
  250. global dt;
  251. F = [1 0 0 0 0
  252. 0 1 0 0 0
  253. 0 0 1 0 0
  254. 0 0 0 0 0
  255. 0 0 0 0 0];
  256. B = [dt*cos(x(3)) 0
  257. dt*sin(x(3)) 0
  258. 0 dt
  259. 1 0
  260. 0 1];
  261. x= F*x+B*u;
  262. %% degree to radian
  263. function radian = toRadian(degree)
  264. radian = degree/180*pi;
  265. %% radian to degree
  266. function degree = toDegree(radian)
  267. degree = radian/pi*180;
  268. %% END
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/繁依Fanyi0/article/detail/914338
推荐阅读
相关标签
  

闽ICP备14008679号