Optimize path while maintaining safe distance from obstacle
Optimize Planned Path
Load a map into the workspace.
map = load("exampleMaps.mat").complexMap;
Create a binary occupancy map.
map = binaryOccupancyMap(map);
Create a state validator object.
stateValidator = validatorOccupancyMap;
Assign the map to the state validator object.
stateValidator.Map = map;
Set the validation distance for the validator.
stateValidator.ValidationDistance = 0.01;
plannerHybridAStar object with the state validator object. Specify the
MinTurningRadius property of the planner as 2 meters.
planner = plannerHybridAStar(stateValidator,MinTurningRadius=2);
Define start and goal poses as [x y theta] vectors. x and y specify the position in meters, and theta specifies the orientation angle in radians.
start = [6 3 pi/2]; goal = [32 32 0];
Plan a path from the start pose to the goal pose.
path = plan(planner,start,goal); inpath = path.States;
Configure options for optimization.
options = optimizePathOptions
options = optimizePathOptions Trajectory Parameters MaxPathStates: 200 ReferenceDeltaTime: 0.3000 MinTurningRadius: 1 MaxVelocity: 0.4000 MaxAngularVelocity: 0.3000 MaxAcceleration: 0.5000 MaxAngularAcceleration: 0.5000 Obstacle Parameters ObstacleSafetyMargin: 0.5000 ObstacleCutOffDistance: 2.5000 ObstacleInclusionDistance: 0.7500 Solver Parameters NumIteration: 4 MaxSolverIteration: 15 Weights WeightTime: 10 WeightSmoothness: 1000 WeightMinTurningRadius: 10 WeightVelocity: 100 WeightAngularVelocity: 10 WeightAcceleration: 10 WeightAngularAcceleration: 10 WeightObstacles: 50
Set the minimum turning radius value as same as in the planner.
options.MinTurningRadius = 2;
Specify the maximum number of poses allowed in the optimized path.
options.MaxPathStates = size(inpath,1) * 3;
Maintain a safe distance of
0.75 meters from the obstacles.
options.ObstacleSafetyMargin = 0.75;
Optimize the path generated by the planner.
optpath = optimizePath(inpath,map,options);
Visualize input path and optimized path in the map.
show(map) hold on quiver(inpath(:,1),inpath(:,2),cos(inpath(:,3)),sin(inpath(:,3)),0.1); quiver(optpath(:,1),optpath(:,2),cos(optpath(:,3)),sin(optpath(:,3)),0.1); legend("Input Path","Optimized Path")
path — Path states
real-valued M-by-2 matrix of the form [x
y] | real-valued M-by-3 matrix of the form [x
Path states, specified as a real-valued M-by-2 matrix of the form [x y] or M-by-3 matrix of the form [x y theta]. x and y specify the position in meters. theta specifies the orientation in radians. M is the number of states in the path. The number of states in the path must be greater than or equal to 3.
optPath — Optimized path
real-valued N-by-2 matrix of the form [x
y] | real-valued N-by-3 matrix of the form [x
Path states, returned as a real-valued N-by-2 matrix of the form
y] or N-by-3 matrix of the form
theta]. x and y specify the
position in meters. theta specifies the orientation in radians.
N is the number of states in the path. The number of states in the
optPath can be different than in the
kineticInfo — Kinetic Information
Kinetic Information, returned as a structure. The fields of the structure are:
Time corresponding to the pose in the optimized path.
Velocity to travel between two consecutive poses.
Angular Velocity between two consecutive poses.
solutionInfo — Solution Information
Solution Information, returned as a structure. The fields of the structure are:
Number of iterations performed.
Final cost of the path.