基于Matlab实现的S-T图与A星搜索算法实现速度决策（即一种非常粗略的局部速度规划）并呈现结果+源代码+文档说明

% MATLAB Source Codes for the book "Cooperative Dedcision and Planning for % Connected and Automated Vehicles" published by Mechanical Industry Press % in 2020. % 《智能网联汽车协同决策与规划技术》书籍配套代码 % Copyright (C) 2020 Bai Li % 2020.01.31 % ============================================================================== function [x, y, theta, path_length, completeness_flag] = ProvideCoarsePathViaHybridAStarSearch() global hybrid_astar_ environment_scale_ % 混合A星算法涉及的参数 hybrid_astar_.resolution_x = 0.2; hybrid_astar_.resolution_y = 0.2; hybrid_astar_.resolution_theta = 0.2; hybrid_astar_.num_nodes_x = ceil(environment_scale_.x_scale / hybrid_astar_.resolution_x) + 1; hybrid_astar_.num_nodes_y = ceil(environment_scale_.y_scale / hybrid_astar_.resolution_y) + 1; hybrid_astar_.num_nodes_theta = ceil(2 * pi / hybrid_astar_.resolution_theta) + 1; hybrid_astar_.penalty_for_backward = 1; hybrid_astar_.penalty_for_direction_changes = 3; hybrid_astar_.penalty_for_steering_changes = 0; hybrid_astar_.multiplier_H = 5.0; hybrid_astar_.multiplier_H_for_A_star = 2.0; hybrid_astar_.max_iter = 500; hybrid_astar_.max_time = 5; hybrid_astar_.simulation_step = 0.7; hybrid_astar_.Nrs = 10; % % 绘制环境中障碍物摆放情况 figure(1) global obstacle_vertexes_ vehicle_TPBV_ axis equal; box on; grid on; axis([environment_scale_.environment_x_min environment_scale_.environment_x_max ... environment_scale_.environment_y_min environment_scale_.environment_y_max]); set(gcf,'outerposition',get(0,'screensize')); hold on; for ii = 1 : length(obstacle_vertexes_) fill(obstacle_vertexes_{ii}.x, obstacle_vertexes_{ii}.y, [125, 125, 125] ./ 255); end Arrow([vehicle_TPBV_.x0, vehicle_TPBV_.y0], [vehicle_TPBV_.x0 + cos(vehicle_TPBV_.theta0), vehicle_TPBV_.y0 + sin(vehicle_TPBV_.theta0)], 'Length',16,'BaseAngle',90,'TipAngle',16,'Width',2); Arrow([vehicle_TPBV_.xtf, vehicle_TPBV_.ytf], [vehicle_TPBV_.xtf + cos(vehicle_TPBV_.thetatf), vehicle_TPBV_.ytf+ sin(vehicle_TPBV_.thetatf)], 'Length',16,'BaseAngle',90,'TipAngle',16,'Width',2); xlabel('x / m','FontSize',16); ylabel('y / m','FontSize',16); drawnow % % 调用混合A星算法搜索路径，如果成功，则显示搜索路径 [x, y, theta, ~, completeness_flag] = SearchHybridAStarPath(); if (completeness_flag) plot(x,y,'k','LineWidth',2); drawnow; title('用于速度决策的路径（由混合A*算法生成）'); [x, y, theta] = ResamplePathWithEqualDistance(x, y, theta); path_length = 0; for ii = 1 : (length(x) - 1) path_length = path_length + hypot(x(ii+1) - x(ii), y(ii+1) - y(ii)); end end end function [x, y, theta, path_length, completeness_flag] = SearchHybridAStarPath() % ============================================================================== % MATLAB Source Codes for the book "Cooperative Dedcision and Planning for % Connected and Automated Vehicles" published by Mechanical Industry Press % in 2020. % 《智能网联汽车协同决策与规划技术》书籍配套代码 % Copyright (C) 2020 Bai Li % 2020.01.29 % ============================================================================== % 第二章. 2.4.3小节. 混合A星函数的实现方法 % ============================================================================== global costmap_ costmap_ = CreateDilatedCostmap(); global hybrid_astar_ vehicle_TPBV_ vehicle_kinematics_ grid_space_ = cell(hybrid_astar_.num_nodes_x, hybrid_astar_.num_nodes_y, hybrid_astar_.num_nodes_theta); end_config = [vehicle_TPBV_.xtf, vehicle_TPBV_.ytf, vehicle_TPBV_.thetatf]; start_config = [vehicle_TPBV_.x0, vehicle_TPBV_.y0, vehicle_TPBV_.theta0]; goal_ind = Convert3DimConfigToIndex(end_config); init_node = zeros(1,16); % Information of each element in each node: % Dim # | Variable % 1 x % 2 y % 3 theta % 4 f % 5 g % 6 h % 7 is_in_openlist % 8 is_in_closedlist % 9-11 index of current node % 12-14 index of parent node % 15-16 expansion operation [v,phy] that yields the current node init_node(1:3) = start_config; init_node(6) = CalculateH(start_config, end_config); init_node(5) = 0; init_node(4) = init_node(5) + hybrid_astar_.multiplier_H * init_node(6); init_node(7) = 1; init_node(9:11) = Convert3DimConfigToIndex(start_config); init_node(12:14) = [-999,-999,-999]; openlist_ = init_node; grid_space_{init_node(9), init_node(10), init_node(11)} = init_node; expansion_pattern = [1, -vehicle_kinematics_.vehicle_phy_max; 1, 0; 1, vehicle_kinematics_.vehicle_phy_max; -1, -vehicle_kinematics_.vehicle_phy_max; -1, 0; -1, vehicle_kinematics_.vehicle_phy_max]; completeness_flag = 0; complete_via_rs_flag = 0; best_ever_val = Inf; best_ever_ind = init_node(9:11); path_length = 0; iter = 0; tic while ((~isempty(openlist_)) && (iter <= hybrid_astar_.max_iter) && (~completeness_flag) && (toc <= hybrid_astar_.max_time)) iter = iter + 1; cur_node_order = find(openlist_(:,4) == min(openlist_(:,4))); cur_node_order = cur_node_order(end); cur_node = openlist_(cur_node_order, :); cur_config = cur_node(1:3); cur_ind = cur_node(9:11); cur_g = cur_node(5); cur_v = cur_node(15); cur_phy = cur_node(16); % Specify analytical RS curve generator is activated. if (mod(iter-1, hybrid_astar_.Nrs) == 0) [x_rs, y_rs, theta_rs, path_length] = GenerateRsPath(cur_config, end_config); if (Is3DNodeValid([x_rs, y_rs, theta_rs])) completeness_flag = 1; complete_via_rs_flag = 1; best_ever_ind = cur_ind; break; end end % Remove cur_node from openlist and add it in closed list openlist_(cur_node_order, :) = []; grid_space_{cur_ind(1), cur_ind(2), cur_ind(3)}(7) = 0; grid_space_{cur_ind(1), cur_ind(2), cur_ind(3)}(8) = 1; % Expand the current node to its 6 children for ii = 1 : 6 child_node_v = expansion_pattern(ii,1); child_node_phy = expansion_pattern(ii,2); child_node_config = SimulateForUnitDistance(cur_config, child_node_v, child_node_phy, hybrid_astar_.simulation_step); child_node_ind = Convert3DimConfigToIndex(child_node_config); % If the child node has been explored ever before, and it has been closed: if ((~isempty(grid_space_{child_node_ind(1), child_node_ind(2), child_node_ind(3)})) && (grid_space_{child_node_ind(1), child_node_ind(2), child_node_ind(3)}(8) == 1)) continue; end child_g = cur_g + hybrid_astar_.simulation_step + hybrid_astar_.penalty_for_direction_changes * abs(cur_v - child_node_v) + hybrid_astar_.penalty_for_steering_changes * abs(cur_phy - child_node_phy); % Now, if the child node has been explored ever before, and it is still in the openlist: if (~isempty(grid_space_{child_node_ind(1), child_node_ind(2), child_node_ind(3)})) % If the previously found parent of the child is not good enough, a change is to be made if (grid_space_{child_node_ind(1), child_node_ind(2), child_node_ind(3)}(5) > child_g + 0.1) child_node_order1 = find(openlist_(:,9) == child_node_ind(1)); child_node_order2 = find(openlist_(child_node_order1,10) == child_node_ind(2)); child_node_order3 = find(openlist_(child_node_order1(child_node_order2),11) == child_node_ind(3)); openlist_(child_node_order1(child_node_order2(child_node_order3)), :) = []; child_node_update = grid_space_{child_node_ind(1),child_node_ind(2),child_node_ind(3)}; child_node_update(5) = child_g; child_node_update(4) = child_node_update(5) + hybrid_astar_.multiplier_H * child_node_update(6); child_node_update(12:14) = cur_ind; child_node_update(15:16) = [child_node_v, child_node_phy]; openlist_ = [openlist_; child_node_update];