Optimize path while maintaining safe distance from obstacle
optPath = optimizePath(
path in an empty map by parameters specified in
options. Parameters related to trajectory, obstacle, and solver,
along with the weights of those parameters can be specified in
kineticInfo] = optimizePath(___)
kineticInfo as a structure that contains the kinetic
information of the optimized path.
solutionInfo] = optimizePath(___)
solutionInfo as a structure that contains the solution
information of the optimized path.
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.
map — Map representation
binaryOccupancyMap object |
occupancyMap object |
Map representation, specified as a
vehicleCostmap (Automated Driving Toolbox) object. This object represents the environment of the
options — Optimization options
Optimization options, specified as
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.
 Rosmann, Christoph, Frank Hoffmann, and Torsten Bertram. “Kinodynamic Trajectory Optimization and Control for Car-like Robots.” In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 5681–86. Vancouver, BC: IEEE, 2017. https://doi.org/10.1109/IROS.2017.8206458.
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Introduced in R2022a