- 最近正在学习DWA,按照DynamicWindowApproachSample.m 提供的matlab仿真代码简单梳理下,但是这并不是ros里用的,只是一个简单的仿真。
- DWA的原理:假设一个机器人要从当前点通过一定的速度和方向到达目标点,在速度(v, w)空间中采样多组轨迹,为这些轨迹使用评价函数进行评价,选取最优的轨迹对应的速度(v_best, w_best)来驱动机器人,最终到达目标点。
代码分析:
首先定义一些初始状态
% 机器人的初始状态,包括初始位置,朝向(这里可以理解为yaw角)速度,角速度 % [x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] x=[0 0 pi/2 0 0]'; % 目标点位置 [x(m),y(m)] goal=[10,10]; % 障碍物位置列表 [x(m) y(m)] obstacle=[0 2;4 2;4 4;5 4;5 5;5 6; 6 7;7 4;9 8;9 11;9 6]; % 冲突判定用的障碍物半径 obstacleR=0.5; % 时间间隔[s],主要用于运动模型 global dt; dt=0.1; %% 机器人运动学模型参数 % 最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], % 速度分辨率[m/s],转速分辨率[rad/s]] Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)]; % 评价函数参数 [heading,dist,velocity,predictDT] evalParam=[0.05,0.2,0.1,3.0]; % 模拟区域范围 [xmin xmax ymin ymax] area=[-1 11 -1 11]; % 模拟实验的结果 result.x=[];
进入仿真主循环
循环主要函数为DynamicWindowApproach,输入机器人当前的状态x,运动模型参数,评价函数参数,障碍物,障碍-物半径,输出最优的控制量u(v, w)以及模拟轨迹,控制量传递给运动模型后机器人开始运动,最终到达目标点,后面绘图部分不重要,不予解释了。for i=1:5000 % DWA参数输入 [u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR); % 机器人运动模型 x=f(x,u); % 模拟结果的保存 result.x=[result.x; x']; % 是否到达目的地 if norm(x(1:2)-goal')<0.5 disp('Arrive Goal!!');break; end %% ====绘图部分=== hold off; ArrowLength=0.5;% % 机器人 quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on; plot(result.x(:,1),result.x(:,2),'-b');hold on; plot(goal(1),goal(2),'*r');hold on; plot(obstacle(:,1),obstacle(:,2),'*k');hold on; % 探索轨迹 if ~isempty(traj) for it=1:length(traj(:,1))/5 ind=1+(it-1)*5; plot(traj(ind,:),traj(ind+1,:),'-g');hold on; end end axis(area); grid on; drawnow; end
运动模型 x=f(x,u)指的是在世界坐标系下,机器人从a点运动到b点遵循的运动规则,w指的是世界坐标系
用矩阵的形式表示这个模型如下:
function x = f(x, u)
% u = [vt; wt];当前时刻的速度、角速度
global dt;
F = [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];
B = [dt*cos(x(3)) 0
dt*sin(x(3)) 0
0 dt
1 0
0 1];
x= F*x+B*u;
DynamicWindowApproach函数,这个是DWA函数的模型,输出最优的线速度和角速度
首先会根据机器人模型参数中的最高速度,最高旋转速度,加速度,旋转加速度等参数计算出机器人在dt时间内速度和角速度可以到达的最大最小范围,这个范围可以根据情况调整;
之后将这个窗口范围内的速度(包含角速度)进行采样,输入到评价函数Evaluation中,计算出每个采样速度对应的评价值evalDB,在对这些评价值进行归一化操作,分别乘以评价值对应的评价参数(cost = a*heading+b*dist+c*velocity),得到最优的评价值后找到其对应的u(v, w),输出这个最佳的速度和角速度function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R) % 动态窗口 [vmin,vmax,wmin,wmax] Vr=CalcDynamicWindow(x,model); % 评价函数的计算 [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam); if isempty(evalDB) disp('no path to goal!!'); u=[0;0];return; end % 各评价函数正则化 evalDB=NormalizeEval(evalDB); % 最终评价函数的计算 feval=[]; for id=1:length(evalDB(:,1)) feval=[feval;evalParam(1:3)*evalDB(id,3:5)']; end evalDB=[evalDB feval]; % 最优评价函数 [maxv,ind]=max(feval); % 最优的速度和角速度 u=evalDB(ind,1:2)';
CalcDynamicWindow
function Vr=CalcDynamicWindow(x,model) % global dt; % 车子速度的最大最小范围 Vs=[0 model(1) -model(2) model(2)]; % 根据当前速度以及加速度限制计算的动态窗口 Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt]; % 最终的Dynamic Window Vtmp=[Vs;Vd]; Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
Evaluation函数,评价函数的计算,输入初始x的状态,动态窗口,障碍物,冲突判定用的障碍物半径,机器人运动模型参数以及评价函数参数,通过两个for循环计算出需要评价的参数heading dist vel
function [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam) % 评价函数值 evalDB=[]; % 预测轨迹 trajDB=[]; for vt=Vr(1):model(5):Vr(2) for ot=Vr(3):model(6):Vr(4) % GenerateTrajectory轨迹推测 % 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻到预测时刻之间的轨迹 % evalParam(4),前向模拟时间; [xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model); %% 各评价函数的计算 % 当前机器人和目标点的朝向 heading=CalcHeadingEval(xt,goal); % 当前机器人和障碍物的距离 dist=CalcDistEval(xt,ob,R); vel=abs(vt); % 制动距离的计算 stopDist=CalcBreakingDist(vel,model); if dist>stopDist % evalDB=[evalDB;[vt ot heading dist vel]]; trajDB=[trajDB;traj]; end end end
GenerateTrajectory函数
function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model) % 轨迹生成函数 % evaldt:前向模拟时间; vt、ot当前速度和角速度; global dt; time=0; % 输入值 u=[vt;ot]; % 机器人轨迹 traj=x; while time<=evaldt % 时间更新 time=time+dt; % 运动更新 x=f(x,u); traj=[traj x]; end
CalcHeadingEval函数,heading的评价函数计算
function heading=CalcHeadingEval(x,goal) % 机器人朝向 theta = toDegree(x(3)); % 目标点的方位 goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1))); if goalTheta > theta targetTheta = goalTheta - theta;% [deg] else targetTheta = theta - goalTheta;% [deg] end % theta越小,评价越高 heading = 180 - targetTheta;
CalcDistEval函数,障碍物距离评价函数
function dist=CalcDistEval(x,ob,R) dist=100; for io=1:length(ob(:,1)) disttmp=norm(ob(io,:)-x(1:2)')-R; % 离障碍物最小的距离 if dist>disttmp dist=disttmp; end end % 障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重 if dist>=2*R dist=2*R; end