基于Matlab的简单小车路径规划及控制Demo_matlab模拟小车轨迹规划程序

 这是一个简单的小车仿真示例，使用A星算法在栅格地图上进行路径规划并使用PID控制器进行路径跟踪，只控制了航向角。

支持自定义障碍物，在这个简单的示例上可以替换不同的路径规划算法和控制算法进行验证。

使用方法很简单，将以下代码建立为对应文件名的脚本放置到同一文件路径下即可。

有帮助的话点个赞呗(*^_^*)

 astar.m

function [path] = astar(map, start, goal)
    % A*算法搜索路径，输入：
    % map：二值占用栅格地图
    % start：起点坐标（行向量，x和y）
    % goal：终点坐标（行向量，x和y）
    
    % 初始化开放列表和闭合列表
    start=start*5;
    goal=goal*5;
    open_list = [];
    closed_list = [];
    
    % 将起点加入开放列表
    open_list = [open_list; start, 0, 0, 0, -1, -1];
    
    % 循环直到找到路径或耗尽搜索空间
    while ~isempty(open_list)
        % 从开放列表中找到具有最小代价的节点
        [~, idx] = min(open_list(:, 4));
        current_node = open_list(idx, :);
        
        % 检查目标是否已到达
        if isequal(current_node(1:2), goal)
            % 重建路径
            path = reconstruct_path(closed_list, current_node);
            return;
        end
        
        % 将当前节点从开放列表移动到闭合列表
        open_list(idx, :) = [];
        closed_list = [closed_list; current_node];
        
        % 遍历相邻节点
        for dx = -1:1
            for dy = -1:1
                % 跳过当前节点
                if dx == 0 && dy == 0
                    continue;
                end
                
                % 计算相邻节点坐标
                neighbor = current_node(1:2) + [dx, dy];
                
                % 检查邻居是否位于地图范围内
                if neighbor(1) < 1 || neighbor(1) > size(map, 2) || neighbor(2) < 1 || neighbor(2) > size(map, 1)
                    continue;
                end
                
                % 检查相邻节点是否为障碍物
                if map(neighbor(2), neighbor(1)) == 1
                    continue;
                end
                
                % 计算从当前节点到相邻节点的代价
                g_cost = current_node(3) + sqrt(dx^2 + dy^2);
                h_cost = norm(neighbor - goal);
                f_cost = g_cost + h_cost;
                
                % 检查相邻节点是否已在闭合列表中
                in_closed_list = false;
                for i = 1:size(closed_list, 1)
                    if isequal(neighbor, closed_list(i, 1:2))
                        in_closed_list = true;
                        break;
                    end
                end
                
                if in_closed_list
                    continue;
                end
                
                % 检查相邻节点是否已在开放列表中
                in_open_list = false;
                for i = 1:size(open_list, 1)
                    if isequal(neighbor, open_list(i, 1:2))
                        in_open_list = true;
                        % 检查是否找到更好的路径
                        if g_cost < open_list(i, 3)
                            open_list(i, 3:6) = [g_cost, h_cost, f_cost, current_node(1:2)];
                        end
                        break;
                    end
                end
                
                % 将相邻节点添加到开放列表
                if ~in_open_list
                    open_list = [open_list; neighbor, g_cost, h_cost, f_cost, current_node(1:2)];
                end
            end
        end
    end
    
    % 未找到路径
    path = [];
end
 
function [path] = reconstruct_path(closed_list, current_node)
    % 根据闭合列表重建路径
    path = [current_node(1:2)];
    while current_node(5) ~= -1 && current_node(6) ~= -1
        for i = 1:size(closed_list, 1)
            if closed_list(i, 1:2)==current_node(6:7)
                current_node = closed_list(i, :);
                path = [current_node(1:2); path];
                break;
            end
        end
    end
end

pid_controller.m

% pid_controller.m
function [w, previous_error, integral_error] = pid_controller(theta_target, theta, previous_error, integral_error, Kp, Ki, Kd, time_step)
    % 计算航向角误差
    error = theta_target - theta;
 
    % 计算误差的积分和微分部分
    integral_error = integral_error + error * time_step;
    derivative_error = (error - previous_error) / time_step;
 
    % 计算 PID 控制器输出
    w = Kp * error + Ki * integral_error + Kd * derivative_error;  % 角速度
 
    % 更新误差变量
    previous_error = error;
end

add_obstacle.m

 
function [map, slam_map] = add_obstacle(map, slam_map, obstacle_x, obstacle_y, obstacle_width, obstacle_height, map_resolution)
    % 计算障碍物在地图矩阵中的索引
    obstacle_x_idx = round(obstacle_x * map_resolution) ;
    obstacle_y_idx = round(obstacle_y * map_resolution) ;
    obstacle_width_idx = round(obstacle_width * map_resolution)+1;
    obstacle_height_idx = round(obstacle_height * map_resolution)+1;
 
    % 标记地图矩阵中障碍物的位置
    map(obstacle_y_idx:(obstacle_y_idx + obstacle_height_idx), obstacle_x_idx:(obstacle_x_idx + obstacle_width_idx)) = 1;
 
    % 将障碍物添加到二值占用栅格地图对象中
    for i = obstacle_y_idx:(obstacle_y_idx + obstacle_height_idx)
        for j = obstacle_x_idx:(obstacle_x_idx + obstacle_width_idx)
            setOccupancy(slam_map, [j / map_resolution, i / map_resolution], 1);
        end
    end
end

main.m

% 导入所需的工具库
import robotics.BinaryOccupancyGrid
 
%% 定义小车的参数（如速度、角速度、轮距等），定义地图尺寸和初始位置：
% 双轮小车参数
v = 0.2; % 线速度 (m/s)
w = 0.3; % 角速度 (rad/s)
wheel_distance = 0.2; % 轮距 (m)
 
% 设置起点和终点（通过将它们转换为地图索引）
%格式为（y,x）,坐标大小都乘5
start_idx = [0, 0];
goal_idx = [8.8, 6];
 
% 可视化地图参数
map_size_x = 10; % 地图宽度 (m)
map_size_y = 10; % 地图高度 (m)
 
% slam地图分辨率（每米网格数）
map_resolution = 5;
 
% 初始化可视化地图矩阵
map = zeros(map_size_y * map_resolution, map_size_x * map_resolution);
% 创建二值占用栅格地图
slam_map = BinaryOccupancyGrid(map_size_x, map_size_y, map_resolution);
% 设置障碍物
obs=[2,2,2,2;
    7,5,1,1;
    5.5,3.5,1,1];
 
for i = 1:size(obs, 1)
    %输入障碍物点，矩形宽度，矩形高度
    [map, slam_map] = add_obstacle(map, slam_map, obs(i,1), obs(i,2), obs(i,3), obs(i,4), map_resolution);
end
 
% 初始位置
x = 0; % 初始x坐标 (m)
y = 0; % 初始y坐标 (m)
theta = 0; % 初始角度 (rad)
%% 初始化图形窗口，绘制地图和双轮小车的初始位置：
figure(1);
axis([0 map_size_x 0 map_size_y]);
hold on;
xlabel('X (m)');
ylabel('Y (m)');
title('Differential Drive Robot Simulation');
grid on;
 
% 绘制双轮小车
robot = plot(x, y, 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');  
% 添加表示小车航向角的线段  
robot_heading_length = 0.2; % 线段长度，可以根据需要调整  
robot_heading = [x, y; x + robot_heading_length * cos(theta), y + robot_heading_length * sin(theta)];  
robot_heading_line=plot(robot_heading(:,1), robot_heading(:,2), 'b-', 'LineWidth', 2);
% 绘制障碍物
for i = 1:size(obs, 1)
    obstacle = rectangle('Position', [obs(i,1), obs(i,2), obs(i,3), obs(i,4)]);
end
% 显示 slam_map
figure(2);
show(slam_map)
title('SLAM 图')
% 保持当前图形
hold on;
 
% 计算栅格线的间距
x_spacing = 0:1/map_resolution:map_size_x;
y_spacing = 0:1/map_resolution:map_size_y;
 
% 生成网格点
[X, Y] = meshgrid(x_spacing, y_spacing);
 
% 绘制水平和垂直栅格线
for i = 1:size(X, 1)
    plot(X(i, :), Y(i, :), 'k');
    plot(X(:, i), Y(:, i), 'k');
end
 
% 关闭保持图形
hold off;
%% 仿真主流程
 
% 调用A*算法进行路径规划
path = astar(map, start_idx, goal_idx);
 
% 规划失败时，显示提示信息
if isempty(path)
    disp('无法找到从起点到终点的路径');
else
    % 将路径转换为米
    path = path / map_resolution;
 
    % 绘制规划的路径
    figure(2);
    hold on;
    plot(path(:, 1), path(:, 2), 'r', 'LineWidth', 2);
    hold off;
    disp('寻路成功');
end
 
simulation_time = 80; % 仿真时长 (s)
time_step = 0.1; % 仿真时间步长 (s)
 
path_idx = 1;
 
% PID 控制器参数
Kp = 2;  % 比例增益
Ki = 2;  % 积分增益
Kd = 0.1;  % 微分增益
 
% 初始化误差变量
previous_error = 0;
integral_error = 0;
% 开始计时
tic;
for t = time_step:time_step:simulation_time
    % 计算新位置
    x_new = path(path_idx, 1);
    y_new = path(path_idx, 2);
    theta_target = atan2(y_new - y, x_new - x);  % 目标航向角
     
    % 调用 PID 控制器
    [w, previous_error, integral_error] = pid_controller(theta_target, theta, previous_error, integral_error, Kp, Ki, Kd, time_step);
    % 计算实际位姿
    x = x + v * cos(theta) * time_step;
    y = y + v * sin(theta) * time_step;
    theta = theta + w * time_step;
    
    if sqrt((x_new - x)^2 + (y_new - y)^2) < 0.1
        path_idx = path_idx + 1;
        if path_idx > size(path, 1)
            break;
        end
    end
 
    % 在模拟循环中更新小车位置和航向角线段 
    robot_heading(1,:) = [x, y];  
    robot_heading(2,:) = [x + robot_heading_length * cos(theta), y + robot_heading_length * sin(theta)];  
    set(robot, 'XData', x, 'YData', y);  
    set(robot_heading_line, 'XData', robot_heading(:,1), 'YData', robot_heading(:,2)); 
    drawnow;
    % 实时显示当前小车运行的时间
    elapsed_time = toc;
    fprintf('当前运行时间: %.2f 秒\n', elapsed_time);
    
    pause(time_step);
end
if path_idx > size(path, 1)
    disp('抵达终点');
else
    disp('用时耗尽');
end
% 给用户一些时间来查看图形
pause(5); % 暂停5秒
 
% 在程序结束时关闭所有打开的figure
close all;