matlab混合A星算法并呈现效果（Hybrid-A-Star）

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]; grid_space_{child_node_ind(1),child_node_ind(2),child_node_ind(3)} = child_node_update; end continue; end child_node = zeros(1,16); % Now the child node is ensured to be newly expanded if (~Is3DNodeValid(child_node_config)) child_node(8) = 1; grid_space_{child_node_ind(1),child_node_ind(2),child_node_ind(3)} = child_node; continue; end % We did not calculate H for the child node till now to avoid wasiting CPU. % Now the child node is both new and collision-free. child_node(1:3) = child_node_config; child_node(5) = child_g; child_node(6) = CalculateH(child_node_config, end_config); child_node(4) = child_node(5) + hybrid_astar_.multiplier_H * child_node(6); child_node(7) = 1; child_node(9:11) = child_node_ind; child_node(12:14) = cur_ind; child_node(15:16) = [child_node_v, child_node_phy]; openlist_ = [openlist_; child_node]; grid_space_{child_node_ind(1),child_node_ind(2),child_node_ind(3)} = child_node; % Failure-safe solution preparation if (child_node(6) < best_ever_val) best_ever_val = child_node(6); best_ever_ind = child_node_ind; end % If child node is the goal node if (~any(child_node_ind - goal_ind)) completeness_flag = 1; best_ever_ind = goal_ind; break; end end end % Output hybrid A* path cur_best_parent_ind = grid_space_{best_ever_ind(1), best_ever_ind(2), best_ever_ind(3)}(12:14); x = grid_space_{best_ever_ind(1), best_ever_ind(2), best_ever_ind(3)}(1); y = grid_space_{best_ever_ind(1), best_ever_ind(2), best_ever_ind(3)}(2); theta = grid_space_{best_ever_ind(1), best_ever_ind(2), best_ever_ind(3)}(3); while (cur_best_parent_ind(1) > -1) path_length = path_length + hybrid_astar_.simulation_step; cur_node = grid_space_{cur_best_parent_ind(1), cur_best_parent_ind(2), cur_best_parent_ind(3)}; cur_best_parent_ind = cur_node(12:14); x = [cur_node(1), x]; y = [cur_node(2), y]; theta = [cur_node(3), theta]; end if (completeness_flag) if (complete_via_rs_flag) x_rs = x_rs'; y_rs = y_rs'; theta_rs = theta_rs'; if (size(x_rs, 2) > 1) x = [x, x_rs(2:end)]; y = [y, y_rs(2:end)]; theta = [theta, theta_rs(2:end)]; end else x = [x, end_config(1)]; y = [y, end_config(2)];