动态窗口法matlab代码

以下是动态窗口法的MATLAB代码：

function [v, w] = dynamic_window_approach(x, goal, v_max, w_max, v_res, w_res, dt, predict_time, obs, obs_radius)
% Dynamic window approach for motion planning
% x: current state [x; y; theta]
% goal: goal state [x; y; theta]
% v_max: maximum linear velocity
% w_max: maximum angular velocity
% v_res: linear velocity resolution
% w_res: angular velocity resolution
% dt: time step
% predict_time: prediction time horizon
% obs: obstacles [x1, y1; x2, y2; ...]
% obs_radius: obstacle radius
% v: selected linear velocity
% w: selected angular velocity

% compute dynamic window
v_min = 0;
w_min = -w_max;
v_window = v_min:v_res:v_max;
w_window = w_min:w_res:w_max;
dw = zeros(length(v_window), length(w_window));
for i = 1:length(v_window)
    for j = 1:length(w_window)
        x_next = motion(x, v_window(i), w_window(j), dt);
        if (x_next(1) < 0 || x_next(1) > 10 || x_next(2) < 0 || x_next(2) > 10)
            % robot out of the map
            dw(i, j) = -1;
        elseif check_collision(obs, obs_radius, x_next)
            % collision
            dw(i, j) = -1;
        else
            % compute heading and distance to goal
            heading = atan2(goal(2)-x_next(2), goal(1)-x_next(1));
            dist = norm(goal(1:2)-x_next(1:2));
            % compute cost
            heading_diff = abs(wrapToPi(heading-x_next(3)));
            dist_diff = abs(dist - predict_time*v_window(i));
            dw(i, j) = 1 - exp(-0.5*(heading_diff^2 + dist_diff^2));
        end
    end
end

% select velocities with highest score
[max_score, max_idx] = max(dw(:));
[v_idx, w_idx] = ind2sub(size(dw), max_idx);
v = v_window(v_idx);
w = w_window(w_idx);

function x_next = motion(x, v, w, dt)
% compute next state using motion model
x_next = x + dt*[v*cos(x(3)); v*sin(x(3)); w];
end

function collision = check_collision(obs, obs_radius, x)
% check collision with obstacles
collision = false;
for i = 1:size(obs, 1)
    if norm(obs(i,:)-x(1:2)') < obs_radius
        collision = true;
        break;
    end
end
end

function angle = wrapToPi(angle)
% wrap angle to [-pi, pi]
angle = angle - 2*pi*floor((angle+pi)/(2*pi));
end
end

其中，`x`是当前状态，`goal`是目标状态，`v_max`和`w_max`是线速度和角速度的最大值，`v_res`和`w_res`是线速度和角速度的分辨率，`dt`是时间步长，`predict_time`是用于计算代价的预测时间，`obs`是障碍物的坐标，`obs_radius`是障碍物的半径。函数的返回值是选择的线速度和角速度。