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

Question

Setthivadee 2023-6-10

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1980994-i-try-to-modify-the-motion-planning-with-rrt-for-fixed-wing-uav-but-the-path-is-not-reach-the-goal

回答： Ajay Pattassery 2023-6-22

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Ajay Pattassery 2023-6-22

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1980994-i-try-to-modify-the-motion-planning-with-rrt-for-fixed-wing-uav-but-the-path-is-not-reach-the-goal#answer_1260434

在 MATLAB Online 中打开

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