optimizePath

Optimize path while maintaining safe distance from obstacle

Since R2022a

Syntax

optPath = optimizePath(path)

optPath = optimizePath(path,map)

optPath = optimizePath(path,options)

optPath = optimizePath(path,map,options)

[optPath,kineticInfo] = optimizePath(___)

[optPath,kineticInfo,solutionInfo] = optimizePath(___)

Description

optPath = optimizePath(path) optimizes the travel time and smoothness of the specified path for car-like robots in an empty map for the default parameters. The number of states in the optPath can be different than in the path.

optPath = optimizePath(path,map) optimizes the path in the specified map for the default parameters. The returned optimized path optPath tries to maintain a safety margin to obstacles in the map map.

optPath = optimizePath(path,options) optimizes the 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 options as an optimizePathOptions object.

optPath = optimizePath(path,map,options) optimizes the path in the specified map for by parameters specified in options.

[optPath,kineticInfo] = optimizePath(___) also returns kineticInfo as a structure that contains the kinetic information of the optimized path.

[optPath,kineticInfo,solutionInfo] = optimizePath(___) also returns solutionInfo as a structure that contains the solution information of the optimized path.

Examples

Optimize Planned Path

Set Up Environment

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;

Plan Path

Initialize the 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;

Optimize Path

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

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")

Input Arguments

path — Path states

real-valued M-by-2 matrix of the form [x y] | real-valued M-by-3 matrix of the form [x y theta]

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.

Data Types: single | double

map — Map representation

binaryOccupancyMap object | occupancyMap object | vehicleCostMap object

Map representation, specified as a binaryOccupancyMap, occupancyMap, or vehicleCostmap (Automated Driving Toolbox) object. This object represents the environment of the vehicle.

options — Optimization options

optimizePathOptions object

Optimization options, specified as optimizePathOptions object.

Output Arguments

optPath — Optimized path

real-valued N-by-2 matrix of the form [x y] | real-valued N-by-3 matrix of the form [x y theta]

Path states, returned as a real-valued N-by-2 matrix of the form [x y] or N-by-3 matrix of the form [x y 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 path.

kineticInfo — Kinetic Information

structure

Kinetic Information, returned as a structure. The fields of the structure are:

Fields of kineticInfo

Fields	Description
TimeStamps	Time corresponding to the pose in the optimized path.
Velocity	Velocity to travel between two consecutive poses.
AngularVelocity	Angular Velocity between two consecutive poses.

Data Types: struct

solutionInfo — Solution Information

structure

Solution Information, returned as a structure. The fields of the structure are:

Fields of solutionInfo

Fields	Description
NumIterations	Number of iterations performed.
Cost	Final cost of the path.

Data Types: struct

More About

Obstacle Parameters

The obstacle parameters ObstacleSafetyMargin, ObstacleInclusionDistance, and ObstacleCutOffDistance determine which obstacles are considered for optimization. Far away obstacles would lead to unnecessary computation and not impact the path optimization.

The ObstacleSafetyMargin property specifies a safe distance to maintain between the poses of the path and the obstacles. The ObstacleCutOffDistance property specifies the distance from the robot beyond which all obstacles are not considered when optimizing the path in that instance. The ObstacleInclusionDistance property specifies the distance from the robot within which all obstacles are considered while optimizing the path in that instance. The path optimizer also considers the closest obstacle to the left and right of the robot between the inclusion and the cutoff region.

Locally Optimal Solutions

The path optimizer can not guarantee globally optimal solution. If the robot is encapsulated laterally by two obstacles, the optimizer gets stuck to this local minimum. Moving the pose laterally toward the obstacle would further increase the total cost. There is no way for the trajectory (blue line) to jump over obstacles, and even the poses (purple arrow) themselves are moved away from the region between the obstacles. This circumstance must be avoided in practice.

References

[1] 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.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a