Main Content

optimizePathOptions

Create optimization options for optimizePath function

Since R2022a

expand all in page

    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

    example

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

    Properties

    expand all

    Trajectory Parameters

    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

    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

    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

    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

    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

    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

    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

    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.

    Example: options.ObstacleSafetyMargin = 0.6

    Data Types: single | double

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

    Example: options.ObstacleCutOffDistance = 1.5

    Data Types: single | double

    Obstacle inclusion distance, specified as a positive scalar in meters. The path optimizer considers all obstacles within the inclusion distance, but only the closest obstacle on the left and on the right between the inclusion and cutoff distances.

    Example: options.ObstacleInclusionDistance = 0.5

    Data Types: single | double

    Solver Parameters

    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

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

    Example: options.MaxSolverIteration = 12

    Data Types: single | double

    Weights

    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

    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

    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

    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

    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

    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

    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

    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

    collapse all

    Open Live Script

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

    Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 3 objects of type image, quiver. These objects represent Input Path, Optimized Path.

    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