# optimizePathOptions

## 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:

**Path Adjustment**— Increase (interpolate) or decrease (de-interpolate) poses in the path.**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

### Description

creates a set
of default optimization options for `options`

= optimizePathOptions`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

**Setup 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**

## See Also

## MATLAB Command

You clicked a link that corresponds to this MATLAB command:

Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.

Select a Web Site

Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .

You can also select a web site from the following list:

## How to Get Best Site Performance

Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.

### Americas

- América Latina (Español)
- Canada (English)
- United States (English)

### Europe

- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)

- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)