Main Content

optimizePath

Optimize path while maintaining safe distance from obstacle

Since R2022a

collapse all in page

    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.

    example

    [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

    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.

    Input Arguments

    collapse all

    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 representation, specified as a binaryOccupancyMap, occupancyMap, or vehicleCostmap (Automated Driving Toolbox) object. This object represents the environment of the vehicle.

    Optimization options, specified as optimizePathOptions object.

    Output Arguments

    collapse all

    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.

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

    Fields of kineticInfo

    FieldsDescription
    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

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

    Fields of solutionInfo

    FieldsDescription
    NumIterations

    Number of iterations performed.

    Cost

    Final cost of the path.

    Data Types: struct

    More About

    collapse all

    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.

    Figure shows a robot navigating through the obstacles in the environment. The figure also shows the obstacle safety margin, obstacle inclusion distance, and obstacle cutoff distance.

    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.

    Figure shows the optimizer getting stuck to a local minimum, if the robot is encapsulated laterally by two obstacles.

    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