基于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!