当前位置:   article > 正文

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

matlab模拟小车轨迹规划程序

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

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

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

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

 astar.m

  1. function [path] = astar(map, start, goal)
  2. % A*算法搜索路径,输入:
  3. % map:二值占用栅格地图
  4. % start:起点坐标(行向量,x和y)
  5. % goal:终点坐标(行向量,x和y)
  6. % 初始化开放列表和闭合列表
  7. start=start*5;
  8. goal=goal*5;
  9. open_list = [];
  10. closed_list = [];
  11. % 将起点加入开放列表
  12. open_list = [open_list; start, 0, 0, 0, -1, -1];
  13. % 循环直到找到路径或耗尽搜索空间
  14. while ~isempty(open_list)
  15. % 从开放列表中找到具有最小代价的节点
  16. [~, idx] = min(open_list(:, 4));
  17. current_node = open_list(idx, :);
  18. % 检查目标是否已到达
  19. if isequal(current_node(1:2), goal)
  20. % 重建路径
  21. path = reconstruct_path(closed_list, current_node);
  22. return;
  23. end
  24. % 将当前节点从开放列表移动到闭合列表
  25. open_list(idx, :) = [];
  26. closed_list = [closed_list; current_node];
  27. % 遍历相邻节点
  28. for dx = -1:1
  29. for dy = -1:1
  30. % 跳过当前节点
  31. if dx == 0 && dy == 0
  32. continue;
  33. end
  34. % 计算相邻节点坐标
  35. neighbor = current_node(1:2) + [dx, dy];
  36. % 检查邻居是否位于地图范围内
  37. if neighbor(1) < 1 || neighbor(1) > size(map, 2) || neighbor(2) < 1 || neighbor(2) > size(map, 1)
  38. continue;
  39. end
  40. % 检查相邻节点是否为障碍物
  41. if map(neighbor(2), neighbor(1)) == 1
  42. continue;
  43. end
  44. % 计算从当前节点到相邻节点的代价
  45. g_cost = current_node(3) + sqrt(dx^2 + dy^2);
  46. h_cost = norm(neighbor - goal);
  47. f_cost = g_cost + h_cost;
  48. % 检查相邻节点是否已在闭合列表中
  49. in_closed_list = false;
  50. for i = 1:size(closed_list, 1)
  51. if isequal(neighbor, closed_list(i, 1:2))
  52. in_closed_list = true;
  53. break;
  54. end
  55. end
  56. if in_closed_list
  57. continue;
  58. end
  59. % 检查相邻节点是否已在开放列表中
  60. in_open_list = false;
  61. for i = 1:size(open_list, 1)
  62. if isequal(neighbor, open_list(i, 1:2))
  63. in_open_list = true;
  64. % 检查是否找到更好的路径
  65. if g_cost < open_list(i, 3)
  66. open_list(i, 3:6) = [g_cost, h_cost, f_cost, current_node(1:2)];
  67. end
  68. break;
  69. end
  70. end
  71. % 将相邻节点添加到开放列表
  72. if ~in_open_list
  73. open_list = [open_list; neighbor, g_cost, h_cost, f_cost, current_node(1:2)];
  74. end
  75. end
  76. end
  77. end
  78. % 未找到路径
  79. path = [];
  80. end
  81. function [path] = reconstruct_path(closed_list, current_node)
  82. % 根据闭合列表重建路径
  83. path = [current_node(1:2)];
  84. while current_node(5) ~= -1 && current_node(6) ~= -1
  85. for i = 1:size(closed_list, 1)
  86. if closed_list(i, 1:2)==current_node(6:7)
  87. current_node = closed_list(i, :);
  88. path = [current_node(1:2); path];
  89. break;
  90. end
  91. end
  92. end
  93. end

pid_controller.m

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

add_obstacle.m

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

main.m

  1. % 导入所需的工具库
  2. import robotics.BinaryOccupancyGrid
  3. %% 定义小车的参数(如速度、角速度、轮距等),定义地图尺寸和初始位置:
  4. % 双轮小车参数
  5. v = 0.2; % 线速度 (m/s)
  6. w = 0.3; % 角速度 (rad/s)
  7. wheel_distance = 0.2; % 轮距 (m)
  8. % 设置起点和终点(通过将它们转换为地图索引)
  9. %格式为(y,x),坐标大小都乘5
  10. start_idx = [0, 0];
  11. goal_idx = [8.8, 6];
  12. % 可视化地图参数
  13. map_size_x = 10; % 地图宽度 (m)
  14. map_size_y = 10; % 地图高度 (m)
  15. % slam地图分辨率(每米网格数)
  16. map_resolution = 5;
  17. % 初始化可视化地图矩阵
  18. map = zeros(map_size_y * map_resolution, map_size_x * map_resolution);
  19. % 创建二值占用栅格地图
  20. slam_map = BinaryOccupancyGrid(map_size_x, map_size_y, map_resolution);
  21. % 设置障碍物
  22. obs=[2,2,2,2;
  23. 7,5,1,1;
  24. 5.5,3.5,1,1];
  25. for i = 1:size(obs, 1)
  26. %输入障碍物点,矩形宽度,矩形高度
  27. [map, slam_map] = add_obstacle(map, slam_map, obs(i,1), obs(i,2), obs(i,3), obs(i,4), map_resolution);
  28. end
  29. % 初始位置
  30. x = 0; % 初始x坐标 (m)
  31. y = 0; % 初始y坐标 (m)
  32. theta = 0; % 初始角度 (rad)
  33. %% 初始化图形窗口,绘制地图和双轮小车的初始位置:
  34. figure(1);
  35. axis([0 map_size_x 0 map_size_y]);
  36. hold on;
  37. xlabel('X (m)');
  38. ylabel('Y (m)');
  39. title('Differential Drive Robot Simulation');
  40. grid on;
  41. % 绘制双轮小车
  42. robot = plot(x, y, 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
  43. % 添加表示小车航向角的线段
  44. robot_heading_length = 0.2; % 线段长度,可以根据需要调整
  45. robot_heading = [x, y; x + robot_heading_length * cos(theta), y + robot_heading_length * sin(theta)];
  46. robot_heading_line=plot(robot_heading(:,1), robot_heading(:,2), 'b-', 'LineWidth', 2);
  47. % 绘制障碍物
  48. for i = 1:size(obs, 1)
  49. obstacle = rectangle('Position', [obs(i,1), obs(i,2), obs(i,3), obs(i,4)]);
  50. end
  51. % 显示 slam_map
  52. figure(2);
  53. show(slam_map)
  54. title('SLAM 图')
  55. % 保持当前图形
  56. hold on;
  57. % 计算栅格线的间距
  58. x_spacing = 0:1/map_resolution:map_size_x;
  59. y_spacing = 0:1/map_resolution:map_size_y;
  60. % 生成网格点
  61. [X, Y] = meshgrid(x_spacing, y_spacing);
  62. % 绘制水平和垂直栅格线
  63. for i = 1:size(X, 1)
  64. plot(X(i, :), Y(i, :), 'k');
  65. plot(X(:, i), Y(:, i), 'k');
  66. end
  67. % 关闭保持图形
  68. hold off;
  69. %% 仿真主流程
  70. % 调用A*算法进行路径规划
  71. path = astar(map, start_idx, goal_idx);
  72. % 规划失败时,显示提示信息
  73. if isempty(path)
  74. disp('无法找到从起点到终点的路径');
  75. else
  76. % 将路径转换为米
  77. path = path / map_resolution;
  78. % 绘制规划的路径
  79. figure(2);
  80. hold on;
  81. plot(path(:, 1), path(:, 2), 'r', 'LineWidth', 2);
  82. hold off;
  83. disp('寻路成功');
  84. end
  85. simulation_time = 80; % 仿真时长 (s)
  86. time_step = 0.1; % 仿真时间步长 (s)
  87. path_idx = 1;
  88. % PID 控制器参数
  89. Kp = 2; % 比例增益
  90. Ki = 2; % 积分增益
  91. Kd = 0.1; % 微分增益
  92. % 初始化误差变量
  93. previous_error = 0;
  94. integral_error = 0;
  95. % 开始计时
  96. tic;
  97. for t = time_step:time_step:simulation_time
  98. % 计算新位置
  99. x_new = path(path_idx, 1);
  100. y_new = path(path_idx, 2);
  101. theta_target = atan2(y_new - y, x_new - x); % 目标航向角
  102. % 调用 PID 控制器
  103. [w, previous_error, integral_error] = pid_controller(theta_target, theta, previous_error, integral_error, Kp, Ki, Kd, time_step);
  104. % 计算实际位姿
  105. x = x + v * cos(theta) * time_step;
  106. y = y + v * sin(theta) * time_step;
  107. theta = theta + w * time_step;
  108. if sqrt((x_new - x)^2 + (y_new - y)^2) < 0.1
  109. path_idx = path_idx + 1;
  110. if path_idx > size(path, 1)
  111. break;
  112. end
  113. end
  114. % 在模拟循环中更新小车位置和航向角线段
  115. robot_heading(1,:) = [x, y];
  116. robot_heading(2,:) = [x + robot_heading_length * cos(theta), y + robot_heading_length * sin(theta)];
  117. set(robot, 'XData', x, 'YData', y);
  118. set(robot_heading_line, 'XData', robot_heading(:,1), 'YData', robot_heading(:,2));
  119. drawnow;
  120. % 实时显示当前小车运行的时间
  121. elapsed_time = toc;
  122. fprintf('当前运行时间: %.2f 秒\n', elapsed_time);
  123. pause(time_step);
  124. end
  125. if path_idx > size(path, 1)
  126. disp('抵达终点');
  127. else
  128. disp('用时耗尽');
  129. end
  130. % 给用户一些时间来查看图形
  131. pause(5); % 暂停5秒
  132. % 在程序结束时关闭所有打开的figure
  133. close all;

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/从前慢现在也慢/article/detail/844879
推荐阅读
相关标签
  

闽ICP备14008679号