基于DWA的动态避障代码【MATLAB】实现

% ----------------------------------------------------------------------------- %参考：https://blog.csdn.net/heyijia0327/article/details/44983551 %2022/6/8 : 构建障碍物的运动学模型，障碍物改成动态,速度跟车辆速度一致 % 加入小车动画 %% close all; clear all; disp('Dynamic Window Approach sample program start!!') %% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] % x=[0 0 pi/2 0 0]'; % 5x1矩阵 列矩阵 位置 0，0 航向 pi/2 ,速度、角速度均为0 x = [0.001 0.01 pi/10 0 0]'; % 下标宏定义 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] POSE_X = 1; %坐标 X POSE_Y = 2; %坐标 Y YAW_ANGLE = 3; %机器人航向角 V_SPD = 4; %机器人速度 W_ANGLE_SPD = 5; %机器人角速度 goal = [10,10]; % 目标点位置 [x(m),y(m)] xb = [3 2 0]'; obstacle = [6 6; 4 4; 2 5; 1 2; % Dynamic 4 4; 7 6; 7 9; % Dynamic 6 4; 8 9; 9 6]; obstacleR = 0.5;% 冲突判定用的障碍物半径 global dt; dt = 0.1;% 时间[s] % 机器人运动学模型参数 % 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], % 速度分辨率[m/s],转速分辨率[rad/s]] Kinematic = [2.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)]; %定义Kinematic的下标含义 MD_MAX_V = 1;% 最高速度m/s] MD_MAX_W = 2;% 最高旋转速度[rad/s] MD_ACC = 3;% 加速度[m/ss] MD_VW = 4;% 旋转加速度[rad/ss] MD_V_RESOLUTION = 5;% 速度分辨率[m/s] MD_W_RESOLUTION = 6;% 转速分辨率[rad/s]] % 评价函数参数 [heading,dist,velocity,predictDT] % 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间 evalParam = [0.05, 0.2 ,0.5, 0.0,0.3, 3.0]; area = [-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax] % 模拟实验的结果 result.x=[]; %累积存储走过的轨迹点的状态值 tic; % 估算程序运行时间开始 %% 小车动画 figure('color',[1 1 1]) h1 = axes(); data = my_gritsbot_patch; this.robot_body = data.vertices; th = x(YAW_ANGLE)-pi/2; rotation_matrix = [ cos(th) -sin(th) x(POSE_X); sin(th) cos(th) x(POSE_Y); 0 0 1]; transformed = this.robot_body*rotation_matrix'; % movcount=0; %% Main loop 循环运行 5000次 指导达到目的地 或者 5000次运行结束 max_flag = 0; % 障碍物到达最大边界的标志 min_flag = 0; % 障碍物到达最小边界的标志 %% draw picture for i = 1:5000 % DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹 [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR); x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度 ub = [0.7,0]'; if xb(1) >= 8 max_flag = 1; min_flag = 0; elseif xb(1) <= 0 max_flag = 0; min_flag = 1; end if max_flag == 1 ub = -ub; % 障碍物的速度， elseif min_flag == 1 ub = ub; end xb = fb(xb,ub); % obstacle = [0 2; % 2 4; % 2 5; % 10-xb(1) 2; % 4 10-xb(1); % 6 xb(1); % 5 9 % 8 5 % xb(1) 8 % 9 9]; obstacle(4,1) = 10-xb(1); obstacle(5,2) = 10-xb(1); obstacle(6,2) = xb(1); obstacle(7,1) = xb(1); % 历史轨迹的保存 result.x = [result.x; x']; %最新结果 以列的形式 添加到result.x % 是否到达目的地 if norm(x(POSE_X:POSE_Y)-goal')<0.5 % norm函数来求得坐标上的两个点之间的距离 disp('Arrive Goal!!');break; end delete(h1) % 新图出现时，取消原图的显示。防止重复绘图。 h1 = axes(); ArrowLength = 0.5; % 箭头长度 hold on th = x(YAW_ANGLE)-pi/2; rotation_matrix = [ cos(th) -sin(th) x(POSE_X); sin(th) cos(th) x(POSE_Y); 0 0 1]; transformed = this.robot_body*rotation_matrix'; patch(... 'Vertices', transformed(:, 1:2), ... 'Faces', data.faces, ... 'FaceColor', 'flat', ... 'FaceVertexCData', data.colors, ... 'EdgeColor','none'); % 绘制小车 hold on; plot(result.x(:,POSE_X),result.x(:,POSE_Y),'-b');hold on; % 绘制走过的所有位置 所有历史数据的 X、Y坐标 plot(goal(1),goal(2),'*r');hold on; % 绘制目标位置 DrawObstacle_plot(obstacle,obstacleR); % 绘制障碍物 % 探索轨迹 画出待评价的轨迹 if ~isempty(traj) %轨迹非空 for it=1:length(traj(:,1))/5 %计算所有轨迹数 traj 每5行数据 表示一条轨迹点 ind = 1+(it-1)*5; %第 it 条轨迹对应在traj中的下标 plot(traj(ind,:),traj(ind+1,:),'--g');hold on %根据一条轨迹的点串画出轨迹 traj(ind,:) 表示第ind条轨迹的所有x坐标值 traj(ind+1,:)表示第ind条轨迹的所有y坐标值 end end axis(area); %根据area设置当前图形的坐标范围，分别为x轴的最小、最大值，y轴的最小最大值 xlabel('$x(m)$','interpreter','latex','FontSize',12); ylabel('$y(m)$','interpreter','latex','FontSize',12); grid on; %% 保存为gif frame=getframe(gcf); im = frame2im(frame); [imind,cm] = rgb2ind(im,256); if i==1 imwrite(imind,cm,'dynamic.gif','gif', 'LoopCount',inf,'DelayTime',0.000001); end if rem(i,2)==0 imwrite(imind,cm,'dynamic.gif','gif','WriteMode','append','DelayTime',0.000001); end drawnow; %刷新屏幕. 当代码执行时间长，需要反复执行plot时，Matlab程序不会马上把图像画到figure上，这时，要想实时看到图像的每一步变化情况，需要使用这个语句。 end toc %输出程序运行时间 形式：时间已过 ** 秒。 %% 绘制所有障碍物位置 % 输入参数：obstacle 所有障碍物的坐标 obstacleR 障碍物的半径 function DrawObstacle_plot(obstacle,obstacleR) r = obstacleR; theta = 0:pi/20:2*pi; for id=1:length(obstacle(:,1)) x = r * cos(theta) + obstacle(id,1); y = r *sin(theta) + obstacle(id,2); if id == 4 || id == 5 || id == 6 || id == 7 plot(x,y,'-k');hold on; else plot(x,y,'-m');hold on; end end plot(obstacle(:,1),obstacle(:,2),'*r');hold on; % 绘制所有障碍物位置 end %% DWA算法实现 % model 机器人运动学模型 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], 速度分辨率[m/s],转速分辨率[rad/s]] % 输入参数：当前状态、模型参数、目标点、评价函数的参数、障碍物位置、障碍物半径 % 返回参数：控制量 u = [v(m/s),w(rad/s)] 和 轨迹集合 N * 31 （N：可用的轨迹数） % 选取最优参数的物理意义：在局部导航过程中，使得机器人避开障碍物，朝着目标以较快的速度行驶。 function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,ob,R) % Dynamic Window [vmin,vmax,wmin,wmax] 最小速度 最大速度 最小角速度 最大角速度速度 Vr = CalcDynamicWindow(x,model); % 根据当前状态 和 运动模型 计算当前的参数允许范围 % 评价函数的计算 evalDB N*5 每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分 % trajDB 每5行一条轨迹 每条轨迹都有状态x点串组成 [eval,evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam); %evalParam 评价函数参数 [heading,dist,velocity,predictDT] % dete_dist = CalcDistEval(x,ob(4:7,:),R) dete_dist = min(evalDB(:,7)); if dete_dist < 1 u=[0;0];return; end if isempty(evalDB) disp('no path to goal!