Clear Filters
Clear Filters

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

9 views (last 30 days)
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

Answers (1)

Ajay Pattassery
Ajay Pattassery on 22 Jun 2023
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

Products


Release

R2023a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!