一、动态窗口法基本概念

动态窗口方法(DynamicWindowApproach) 是一种可以实现实时避障的局部规划算法，通过将轮式机器人的位置约束转化为速度约束，根据约束进行速度采样，并由一系列的选定速度的动作生成轨迹，结合评价函数选择评分最高的轨迹，实现执行最优速度的问题。

DWA算法的流程图可表述为：

1.1 速度采样空间

初始速度搜索空间：在任意t时刻，机器人速度(v，ω) 的二维空间中可以形成无穷多组运动轨迹，即构成一个初始的速度搜索空间。但是受以下机器人的运动学约束和环境约束等可以使采样速度缩小至更加精确的区域：

①机器人本身的速度极限：

②机器人驱动的电机性能及摩擦等影响，即匀加（减）速运动的启停动作：

③避障，需要保证以最大加速度能在最近的障碍物前停下来（速度从v，ω 减为0）：

式中，dist（v，ω ）为当前采样速度（v，ω ）下距离障碍物最小的间距值。

综上，速度搜索空间为以上三个速度集合的交集，也称为该机器人的动态窗口，即Vr=Vs∩Vd∩Va 。

1.2 评价函数

设置为如下评价函数，用以确定当前速度状态下的最佳路径：

（1）heading（v，ω）为机器人方位角评价函数，使机器人能够不断地对准下一个参考点。

设定heading（v，ω）=180°-θ ，θ 为机器人当前位置与目标点连线在全局坐标系下的位姿角。

（2）distance（v，ω）为机器人距离障碍物的评价函数，用以提高机器人与避障能力。

设定，其中d为机器人与障碍物的最近距离；L为预先设定的避障阈值，当轨迹上没有障碍物时，为防止避障评价指标对函数G（v，ω）的影响过大，distance（v，ω）将输出一个数值较大的定值，提高寻找最短路径的执行效率。

（3）velocity（v，ω）为速度评价函数。

设定velocity（v，ω）=|vg| ，vg 为当前待评价的轨迹线速度，速度值越大，得分越高。

（4）σ，α，β，γ 均为评价子函数的系数。

综上，引入评价函数使机器人能够完成避障的局部规划，并以较快的速度朝着目标点位置移动。

二、基于Matlab的机器人局部避障仿真

Matlab代码如下：


close all;
clear all;  x=[0 0 0 0 0]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
goal=[10,10];% 目标点位置 [x(m),y(m)]  obstacle=[2 2;4 5; 5 9;8 6;7 9];%设置五个障碍物
obstacleR=0.5;% 冲突判定用的障碍物排斥半径
global dt; dt=0.1;% 时间[s]
T = 1000;
traj_X = zeros(1,T);%记录所走轨迹的x坐标
traj_Y = zeros(1,T);%记录所走轨迹的y坐标% 机器人运动学模型
% 最高速度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];
area=[-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax]  % 模拟实验的结果
result.x=[];
tic;  % Main loop  Fig=figure;filename = 'test.gif';      % 输出路径+保存的文件名.gif
for i=1:T% 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  %====Animation动画仿真====  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),'or');hold on;  r = obstacleR;for id=1:length(obstacle(:,1))         %绘制矩形障碍物（黄色表示障碍物）rectangle('Position',[obstacle(id,1)-r/2,obstacle(id,2)-r/2,r,r],'Linewidth',2,'LineStyle','-','EdgeColor','y');end% 探索轨迹  if ~isempty(traj)  for it=1:length(traj(:,1))/5  ind=1+(it-1)*5;  traj_X(i) = traj(1,31);traj_Y(i) = traj(2,31);plot(traj(ind,:),traj(ind+1,:),'-g');hold on;  end  end  axis(area);  grid on;  xlabel('x / m')ylabel('y / m')title('动态窗口法生成避障规划轨迹')drawnow;  frame = getframe(Fig); im = frame2im(frame); [imind,cm] = rgb2ind(im,256);if i == 1imwrite(imind,cm,filename,'gif','WriteMode','overwrite', 'Loopcount',inf);%Loopcount只是在i==1的时候才有用elseimwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.05);%DelayTime:帧与帧之间的时间间隔endsave Trajectory.mat traj_X traj_Y
end
toc
%movie2avi(mov,'movie.avi');  function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)  % Dynamic Window [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)';%
endfunction [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)  % 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻 到 预测时刻之间的轨迹  [xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);  %evalParam(4),前向模拟时间;  % 各评价函数的计算  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
endfunction EvalDB=NormalizeEval(EvalDB)
% 评价函数正则化
if sum(EvalDB(:,3))~=0  EvalDB(:,3)=EvalDB(:,3)/sum(EvalDB(:,3));
end
if sum(EvalDB(:,4))~=0  EvalDB(:,4)=EvalDB(:,4)/sum(EvalDB(:,4));
end
if sum(EvalDB(:,5))~=0  EvalDB(:,5)=EvalDB(:,5)/sum(EvalDB(:,5));
end
endfunction [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
endfunction stopDist=CalcBreakingDist(vel,model)
% 根据运动学模型计算制动距离
global dt;
stopDist=0;
while vel>0  stopDist=stopDist+vel*dt;% 制动距离的计算  vel=vel-model(3)*dt;%   model(3)为加速度
end
endfunction 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
endfunction heading=CalcHeadingEval(x,goal)
% heading的评价函数计算  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  heading=180-targetTheta;
endfunction Vr=CalcDynamicWindow(x,model)
%  计算动态窗口，即速度交集Vr
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))];
endfunction 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;
endfunction radian = toRadian(degree)
% 角度转为弧度制
radian = degree/180*pi;
endfunction degree = toDegree(radian)
% 弧度转为角度
degree = radian/pi*180;
end

仿真结果如下图：

基于动态窗口法（DWA）的局部避障算法研究及MATALB的实现相关推荐

第2章无人艇局部危险避障算法研究
水面无人艇局部危险避障算法研究 Local Risk Obstacle Avoidance Algorithm of USV 博主的硕士毕业论文第2章无人艇局部危险避障算法研究 2.1 无人艇局 ...
水面无人艇局部危险避障算法研究参考文献
水面无人艇局部危险避障算法研究 Local Risk Obstacle Avoidance Algorithm of USV 博主的硕士毕业论文参考文献 [1] Manley J E. Unman ...
水面无人艇局部危险避障算法研究第1章绪论
水面无人艇局部危险避障算法研究 Local Risk Obstacle Avoidance Algorithm of USV 博主的硕士毕业论文第1章绪论 1.1 引言随着世界人口的不断增长以 ...
光流传感器定位精度_基于多传感器的无人机定位和避障技术研究
摘要: 随着无人机技术的快速发展,无人机已逐渐走进人们的生活和工作.现有常规的无人机仍依赖于专业人员的操纵,无人机技术存在着环境感知能力.自主飞行能力不足等问题.为进一步提高无人机的环境感知能力并扩展 ...
机器人局部避障的动态窗口法DWA (dynamic window approach)-matlab代码修改及动态障碍物避障效果
具体效果视频:[DWA动态障碍物-哔哩哔哩] https://b23.tv/pQp6ne 一.源码及问题最初的源码链接https://blog.csdn.net/heyijia0327/articl ...
【无人机编队】基于动态窗口法实现的无人机编队目标分配及路径规划问题研究附matlab代码
✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信.
基于ACO蚁群优化的机器人避障算法仿真
目录 1.算法概述 2.仿真效果 3.matlab仿真源码 1.算法概述蚁群算法(ant colony optimization)最早是由Marco Dorigo等人在1991年提出,他们在研究新型 ...
ros自己写避障算法_slam导航避障算法，让无人机自主避障教学研究迈向更高处...
随着消费级无人机技术的不断成熟,不断完善的自动避障系统可以极大的减少因操作失误而带来的各项损失,目前避障能力正逐渐成为了无人机自动化或智能化的关键点所在. 而根据无人机避障技术的原理和发展趋势,可以将 ...
机器人局部避障的动态窗口法(dynamic window approach) DWA
rosparam命令可对ROS参数服务器上的参数进行操作.通过rosparam -h命令,可以看到有下面的一些方法: Commands: rosparam set set parameter 设置参数 ...

基于动态窗口法（DWA）的局部避障算法研究及MATALB的实现

一、动态窗口法基本概念

1.1 速度采样空间

1.2 评价函数

二、基于Matlab的机器人局部避障仿真

基于动态窗口法（DWA）的局部避障算法研究及MATALB的实现相关推荐

最新文章

热门文章