DWA动态窗口法的MATLAB代码

以下是一个简单的用MATLAB实现DWA算法的示例代码：

function [v,w] = dwa(x,goal)

% x: 当前机器人的状态（x,y,theta,v,w）
% goal: 目标点的位置（x,y）

% 运动参数
MAX_VEL = 0.5; % 最大线速度
MAX_ROT_VEL = pi/4; % 最大角速度
MAX_ACC = 0.1; % 最大加速度
MAX_ROT_ACC = pi/4; % 最大角加速度

% 窗口参数
DW = 0.1; % 线速度窗口
DT = 0.1; % 角速度窗口

% 目标状态
target = [goal(1)-x(1), goal(2)-x(2), atan2(goal(2)-x(2), goal(1)-x(1))];

% 窗口
v_min = max(0, x(4)-MAX_ACC*DT);
v_max = min(MAX_VEL, x(4)+MAX_ACC*DT);
w_min = max(-MAX_ROT_VEL, x(5)-MAX_ROT_ACC*DT);
w_max = min(MAX_ROT_VEL, x(5)+MAX_ROT_ACC*DT);

% 初始化
v_best = x(4);
w_best = x(5);
cost_best = inf;

% 遍历窗口
for v = v_min:DW:v_max
  for w = w_min:DT:w_max
    % 计算新状态
    x_new = motion(x, v, w, DT);
    % 计算代价
    cost = eval_cost(x_new, target);
    % 更新最优解
    if cost < cost_best
      cost_best = cost;
      v_best = v;
      w_best = w;
    end
  end
end

v = v_best;
w = w_best;

end

function x_new = motion(x, v, w, dt)
% 计算新状态
x_new = x;
x_new(1) = x(1) + v*cos(x(3))*dt;
x_new(2) = x(2) + v*sin(x(3))*dt;
x_new(3) = x(3) + w*dt;
x_new(4) = v;
x_new(5) = w;
end

function cost = eval_cost(x, target)
% 计算代价
cost = norm([x(1)-target(1), x(2)-target(2), wrapToPi(x(3)-target(3))]);
end

该代码实现了一个简单的DWA算法，它接受机器人当前的状态和目标点的位置作为输入，并输出机器人应该采取的最佳线速度和角速度。它通过遍历一个线速度和角速度的窗口，计算每个状态的代价，并选择最小代价的状态作为最佳解。