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

I have a problem in the Plan using the HybridA* while for what concerns the case of a RRT Path planning, I have no problem to run the script.

I don't know why the different Path Planning can create this error.

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

Answer 1

[Cameron Stabile]

Hi Salvatore,

There appear to be quite a few small issues with the script, so let's address them step-by-step:

%% HybridA* Path Planning

1) The plannerHybridAStar object has been created and configured, but the script never calls the plan method. That explains the lack of a tree/path when you call the show method.

2) If you do call path = plan(planner,start,goal) using the hybridA* planner, you should find that your validatorOccupancyMap throws an error saying that the start-state is invalid.

3) The reason for the invalid start-state is that the validator has been configured with no inputs, meaning it contains a default stateSpaceSE2 object in the StateSpace property. plannerHybridAStar's validator uses the state-space to determine whether states passed by the planner are in-/out-of-bounds. With the default StateBounds = [-100 100; -100 100; -pi pi], your hybridA* planner will not be capable of generating a plan for the current start/goal, so these should be updated (similar to what you've done in the RRT section).

4) The ValidationDistance on your hybridA* planner is currently set to 100, which is 5x longer than your primitive length (20). It is important to remember that ValidationDistance is effectively the distance between points sampled/validated along edges in your tree, so if this value is too large you run the risk of your planner jumping through obstacles. For your problem, a ValidationDistance=1 (or lower) is more appropriate.

5) Lastly, you should carefully consider the relationship between your MinTurningRadius and MotionPrimitiveLength. Right now they are both set to 20, which means the planner will need to add a large number of nodes to explore the space (around 3 nodes to carry out a 90-degree turn using the sharpest motion-primitive). For the problem you've shown here, that may actually be fine, but you should keep these thoughts in mind as you tune your planner or work out different problems.

%% RRT Path Planning

Unlike the plannerHybridAStar planner, whose edges between nodes are defined/shaped by the planner object itself, RRT-style planners in the Navigation Toolbox rely on the nav.StateSpace object to define the shape of edges between nodes in the RRT tree (see Plan Mobile Robot Paths Using RRT for an example of how the state-space affects the planner).

For your purposes, your plannerRRT object should really be using either the stateSpaceDubins or stateSpaceReedsShepp state-spaces, or you could create you own.

Aside from that, here are two final suggestions:

1) I would suggest using plannerRRTStar rather than plannerRRT, as it will generate smoother, more-optimal paths.

2) If you have access to R2022a, you might benefit from the new optimizePath feature. This is a map-aware optimizer which can improve the path results generated by your RRT/hybridA* planner. See Optimization Based Path Smoothing for Autonomous Vehicles for an example on how this can be used.

Hope this helps!

Cameron

[Salvatore Lubertino]

Thank you! Now I have solved the problem.