I try to modify the motion planning with RRT for fixed wing UAV but the path is not reach the goal

Hello everybody, I tried to modify the motion planning from this example Motion Planning with RRT for Fixed-Wing UAV. I create new 3D map from my binaryOccupancyMap and extrude it. Then I using the code from the example to simulte the UAV motion planning but the output shows that the path is not reach to the goal. Could anyone help me why the path is not reach the goal point? If it because of setting in planner or statespace or my map have a problem?

Thank you so much

• The picture of the result look like this

%load 2D map
load map_warehouse.mat map_warehouse B
BOmap = binaryOccupancyMap(B);
show(BOmap)
%change to 3D
my3Dmap = occupancyMap3D(1);
x = linspace(0,50)';  
y = linspace(0,55)'; 
z = linspace(0,10,25);  

mapXY =[repelem(x,100) repmat(y,100,1)]; 
mapXYZ =[repmat(mapXY,25,1) repelem(z,10000)'];   

occ3D = checkOccupancy(BOmap,mapXY);
occ3D = repmat(occ3D,25,1);

setOccupancy(my3Dmap, mapXYZ, occ3D);
show(my3Dmap)

my3Dmap.FreeThreshold = my3Dmap.OccupiedThreshold;
startPose = [3 4 0 pi/2];
goalPose = [11 10 5 pi/2];

figure("Name","StartAndGoal")
hMap = show(my3Dmap);
hold on
scatter3(hMap,startPose(1),startPose(2),startPose(3),30,"red","filled")
scatter3(hMap,goalPose(1),goalPose(2),goalPose(3),30,"green","filled")
hold off
view([-80 85])

ss = ExampleHelperUAVStateSpace("MaxRollAngle",pi/6,...
                                "AirSpeed",6,...
                                "FlightPathAngleLimit",[-0.1 0.1],...
                                "Bounds",[-20 100; -20 100; 0 20; -pi pi]);

threshold = [(goalPose-0.5)' (goalPose+0.5)'; -pi pi];
setWorkspaceGoalRegion(ss,goalPose,threshold)
sv = validatorOccupancyMap3D(ss,"Map",my3Dmap);
sv.ValidationDistance = 0.1;

planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 1;
planner.GoalBias = 0.10;  
planner.MaxIterations = 20000;
planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3)) < 5);

tic
[pthObj,solnInfo] = plan(planner,startPose,goalPose);
tock

if (solnInfo.IsPathFound)
    figure("Name","OriginalPath")
    % Visualize the 3-D map
    show(my3Dmap)
    hold on
    scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled")
    scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled")
    
    interpolatedPathObj = copy(pthObj);
    interpolate(interpolatedPathObj,1000)
    
    % Plot the interpolated path based on UAV Dubins connections
    hReference = plot3(interpolatedPathObj.States(:,1), ...
        interpolatedPathObj.States(:,2), ...
        interpolatedPathObj.States(:,3), ...
        "LineWidth",2,"Color","g");
    
    % Plot simulated UAV trajectory based on fixed-wing guidance model
    % Compute total time of flight and add a buffer
    timeToReachGoal = 1.05*pathLength(pthObj)/ss.AirSpeed;
    waypoints = interpolatedPathObj.States;
    [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal);
    hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r");
    legend([hReference,hSimulated],"Reference","Simulated","Location","best")
    hold off
    view([-80 85])
end

%smoother
if (solnInfo.IsPathFound)
    smoothWaypointsObj = exampleHelperUAVPathSmoothing(ss,sv,pthObj);
    
    figure("Name","SmoothedPath")
    % Plot the 3-D map
    show(my3Dmap)
    hold on
    scatter3(startPose(1),startPose(2),startPose(3),30,"red","filled")
    scatter3(goalPose(1),goalPose(2),goalPose(3),30,"green","filled")
    
    interpolatedSmoothWaypoints = copy(smoothWaypointsObj);
    interpolate(interpolatedSmoothWaypoints,1000)
    
    % Plot smoothed path based on UAV Dubins connections
    hReference = plot3(interpolatedSmoothWaypoints.States(:,1), ...
        interpolatedSmoothWaypoints.States(:,2), ...
        interpolatedSmoothWaypoints.States(:,3), ...
        "LineWidth",2,"Color","g");
    
    % Plot simulated flight path based on fixed-wing guidance model
    waypoints = interpolatedSmoothWaypoints.States;
    timeToReachGoal = 1.05*pathLength(smoothWaypointsObj)/ss.AirSpeed;
    [xENU,yENU,zENU] = exampleHelperSimulateUAV(waypoints,ss.AirSpeed,timeToReachGoal);
    hSimulated = plot3(xENU,yENU,zENU,"LineWidth",2,"Color","r");
    
    legend([hReference,hSimulated],"SmoothedReference","Simulated","Location","best")
    hold off
    view([-85 80]);
end

Answer 1

[Ajay Pattassery]

In the above code, GoalReachedFcn is defined such that the planner will terminate when a path is found within a distance of 5m from the goal position. This is why in the output, the planner returns a path that may be away from the actual goal position.

If you want the planner to find a path closer to the goal, you can adjust the GoalReachedFcn. The following line of code will make the planner return a path that is within 1m of the goal position."

planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3)) < 1);

Refer here for more details about GoalReachedFcn