optimizePathOptions

Create optimization options for optimizePath function

Since R2022a

Description

Optimization options are grouped into four categories:

• Trajectory Parameters — Specify the desired robot motion throughout the path.

• Obstacle Parameters — Specify the distances which dictate the influence of obstacle on the path.

• Solver Parameters — Specify the options for solver used to optimize the path.

• Weights — Specify the cost function weights.

Note

At a very high level, two primary operations happen inside optimizePath function:

1. Path Adjustment — Increase (interpolate) or decrease (de-interpolate) poses in the path.

2. Optimization — Minimize the cost of the path by invoking the solver (Levenberg–Marquardt).

NumIteration property determines how many times Step 1 and 2 will be executed in a loop. MaxSolverIteration property determines the maximum iterations for Levenberg–Marquardt every time it is invoked in Step 2. For the default values, Path Adjustment and Optimization (Solver invocation) will happen four times, and at each invocation the solver will iterate a maximum of 15 times.

Creation

Syntax

options = optimizePathOptions

Description

options = optimizePathOptions creates a set of default optimization options for optimizePath function.

Properties

Trajectory Parameters

MaxPathStates — Maximum number of poses allowed in path

200 (default) | integer greater than or equal to 3

Maximum number of poses allowed in the path, specified as an integer greater than or equal to 3.

Example: options.MaxPathStates = 100

Data Types: single | double

ReferenceDeltaTime — Travel time between two consecutive poses

0.3 (default) | positive scalar

Travel time between two consecutive poses, specified as a positive scalar in seconds. This parameter along with MaxVelocity impacts the interpolation distance between poses. Increase this value to have lesser number of poses and reduce it to have higher number of poses in the output path.

Example: options.ReferenceDeltaTime = 0.5

Data Types: single | double

MinTurningRadius — Minimum turning radius in path

1 (default) | positive scalar

Minimum turning radius in the path, specified as a positive scalar in meters. Note that this is a soft constraint and may be ignored based on the value of WeightMinTurningRadius parameter with respect to other weights.

Example: options.MinTurningRadius = 1.5

Data Types: single | double

MaxVelocity — Maximum velocity along path

0.4 (default) | positive scalar

Maximum velocity along the path, specified as a positive scalar in meters per second. Note that this is a soft constraint and may be ignored based on the value of WeightVelocity parameter with respect to other weights.

Example: options.MaxVelocity = 0.5

Data Types: single | double

MaxAngularVelocity — Maximum angular velocity along path

0.3 (default) | positive scalar

Maximum angular velocity along the path, specified as a positive scalar in radians per second. Note that this is a soft constraint and may be ignored based on the value of WeightAngularVelocity parameter with respect to other weights.

Example: options.MaxAngularVelocity = 0.5

Data Types: single | double

MaxAcceleration — Maximum acceleration along path

0.5 (default) | positive scalar

Maximum acceleration along the path, specified as a positive scalar in meters per second squared. Note that this is a soft constraint and may be ignored based on the value of WeightAcceleration parameter with respect to other weights.

Example: options.MaxAcceleration = 0.6

Data Types: single | double

MaxAngularAcceleration — Maximum angular acceleration along path

0.5 (default) | positive scalar

Maximum angular acceleration along path, specified as a positive scalar in radians per second squared. Note that this is a soft constraint and may be ignored based on the value of WeightAngularAcceleration parameter with respect to other weights.

Example: options.MaxAngularAcceleration = 0.6

Data Types: single | double

Obstacle Parameters

ObstacleSafetyMargin — Safety distance from obstacles

0.5 (default) | positive scalar

Safety distance from the obstacles, specified as a positive scalar in meters. Note that this is a soft constraint and may be ignored based on the value of WeightObstacles parameter with respect to other weights. See Obstacle Parameters.

Example: options.ObstacleSafetyMargin = 0.6

Data Types: single | double

ObstacleCutOffDistance — Obstacle cutoff distance

2.5 (default) | positive scalar

Obstacle cutoff distance, specified as a positive scalar in meters. The path optimizer ignores obstacles beyond the cutoff distance. See Obstacle Parameters.

Example: options.ObstacleCutOffDistance = 1.5

Data Types: single | double

ObstacleInclusionDistance — Obstacle inclusion distance

0.75 (default) | positive scalar

Obstacle inclusion distance, specified as a positive scalar in meters. The path optimizer considers all obstacles within the inclusion distance. Additionally, the path optimizer also considers the closest obstacle on the left and on the right of the robot between the inclusion and the cutoff region. See Obstacle Parameters.

Example: options.ObstacleInclusionDistance = 0.5

Data Types: single | double

Solver Parameters

NumIteration — Number of solver invocations

4 (default) | positive integer

Number of solver invocations, specified as a positive integer. This value also specifies the number of times interpolation occurs during optimization.

Example: options.NumIteration = 5

Data Types: single | double

MaxSolverIteration — Maximum number of iterations for each solver invocation

15 (default) | positive integer

Maximum number of iterations for each solver invocation, specified as a positive integer.

Example: options.MaxSolverIteration = 12

Data Types: single | double

Weights

WeightTime — Cost function weight for time

10 (default) | nonnegative scalar

Cost function weight for time, specified as a nonnegative scalar. To lower the travel time, increase this weight value.

Example: options.WeightTime = 12

Data Types: single | double

WeightSmoothness — Cost function weight for nonholonomic motion

1000 (default) | nonnegative scalar

Cost function weight for nonholonomic motion, specified as a nonnegative scalar. To obtain smoother path, increase this weight value.

Example: options.WeightSmoothness = 500

Data Types: single | double

WeightMinTurningRadius — Cost function weight for complying with minimum turning radius

10 (default) | nonnegative scalar

Cost function weight for complying with minimum turning radius, specified as a nonnegative scalar. To ensure the turning radius is above minimum turning radius, increase this weight value.

Example: options.WeightMinTurningRadius = 15

Data Types: single | double

WeightVelocity — Cost function weight for velocity

100 (default) | nonnegative scalar

Cost function weight for velocity, specified as a nonnegative scalar. To maintain the velocity below MaxVelocity, increase this weight value.

Example: options.WeightVelocity = 120

Data Types: single | double

WeightAngularVelocity — Cost function weight for angular velocity

10 (default) | nonnegative scalar

Cost function weight for angular velocity, specified as a nonnegative scalar. To maintain the angular velocity below MaxAngularVelocity, increase this weight value.

Example: options.WeightAngularVelocity = 15

Data Types: single | double

WeightAcceleration — Cost function weight for acceleration

10 (default) | nonnegative scalar

Cost function weight for acceleration, specified as a nonnegative scalar. To maintain the acceleration below MaxAcceleration, increase this weight value.

Example: options.WeightAcceleration = 15

Data Types: single | double

WeightAngularAcceleration — Cost function weight for angular acceleration

10 (default) | nonnegative scalar

Cost function weight for angular acceleration, specified as a nonnegative scalar. To maintain the angular acceleration below MaxAngularAcceleration, increase this weight value.

Example: options.WeightAngularAcceleration = 15

Data Types: single | double

WeightObstacles — Cost function weight for maintaining safe distance from obstacles

50 (default) | nonnegative scalar

Cost function weight for maintaining safe distance from obstacles, specified as a nonnegative scalar. To maintain the safe distance from obstacles, increase this weight value.

Example: options.WeightObstacles = 60

Data Types: single | double

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

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.

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