Salvatore Lubertino asked . 2023-05-18

How can I implement an HybridA* Path Planning from a specific Map?

load("VI_Track_map.mat");
figure
show(VI_Track_map)
grid on
%% HybridA* Path Planning 
clear validator;
validator = validatorOccupancyMap;
validator.Map = VI_Track_map;
validator.ValidationDistance = 100;
planner = plannerHybridAStar(validator,'MinTurningRadius',20,'MotionPrimitiveLength',20);
figure
show(planner)
start = [7100 7100 0];
goal = [7500 7100 0];
show(planner,"Tree","off")
hold on
%% RRT Path Planning
ss=stateSpaceSE2;
sv=validatorOccupancyMap(ss);
sv.Map=VI_Track_map;
sv.ValidationDistance=30;
ss.StateBounds=[VI_Track_map.XWorldLimits; VI_Track_map.YWorldLimits; [-pi/2 pi/2]];
planner1=plannerRRT(ss,sv);
planner1.MaxConnectionDistance=80;
rng(10000,'twister');
start = [7100 7100 0];
goal = [7500 7100 0];
[pathObj,solnInfo]=plan(planner1,start,goal);
show(VI_Track_map)
hold on
plot(start(1),start(2),'*');
plot(goal(1),goal(2),'*');
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'b.-'); % tree expansion
plot(pathObj.States(:,1),pathObj.States(:,2),'r-','LineWidth',2) % draw path

MATLAB , Navigation Toolbox , path planning , navigation tool