Main Content

Vehicle Path Planner System

Plan collision-free path from initial pose to target pose

  • Library:
  • Model Predictive Control Toolbox

  • Vehicle Path Planner System block

Description

The Vehicle Path Planner System block simulates a vehicle path planner (VPP) system that plans a collision-free trajectory from an initial pose to a target pose using a multistage nonlinear MPC controller. You can use this block to plan a path for a vehicle, such as a vehicle entering and exiting a parking spot or a robot moving in a warehouse.

Once you plan a path, you can have the vehicle track the path using a path-following control system.

To customize your controller, for example to modify the controller cost function or change the controller initial conditions, click Create VPP subsystem.

Ports

Input

expand all

Specify the initial vehicle pose as the vector signal [x0 y0 θ0], where:

  • (x0, y0) is the initial position for the center of the vehicle rear axle.

  • θ0 is the initial vehicle yaw angle.

Specify the target vehicle pose as the vector signal [xt yt θt], where:

  • (xt, yt) is the target position for the center of the vehicle rear axle.

  • θt is the initial vehicle yaw angle.

To plan a path using the current pose, target pose, and obstacles, enable the path planner by setting this signal to a positive value.

When planning is complete, set the enable signal to zero, which resets the nonlinear MPC controller.

The time for which the path planner block is enabled must be:

  • Longer than the worst-case execution time of the planner. To guarantee a worst-case execution time, configure the planning controller to apply a suboptimal solution after a specified maximum number of iterations by selecting the Use suboptimal solution parameter.

  • Shorter than the sample time of the controller, as specified using the Sample time parameter.

Specify obstacle information as a matrix with N rows, where N is equal to the Number of obstacles parameter. Each row contains the following information for an obstacle in the order [X Y Φ L W]:

  • (X, Y) — Position, specified in meters

  • Φ — Yaw angle, specified in radians

  • L — Length, specified in meters

  • W — Width, specified in meters

Dependencies

To enable this port, select the corresponding Use external source parameter.

Vehicle velocity range in meters per second, specified as the two-element row vector [Vmin Vmax], where:

  • Vmin is the minimum vehicle velocity.

  • Vmax is the maximum vehicle velocity.

Dependencies

To enable this port, select the corresponding Use external source parameter.

Front steering angle range in radians, specified as the two-element row vector [ψmin ψmax], where:

  • ψmin is the minimum steering angle.

  • ψmax is the maximum steering angle.

The front steering angle is the angle between the front tires and the longitudinal axis of the vehicle. The steering angle is positive towards the positive lateral axis of the ego vehicle.

Dependencies

To enable this port, select the corresponding Use external source parameter.

Output

expand all

Planned vehicle path, output as a three column matrix. Each row contains one vehicle pose [x y θ] along the vehicle path, where:

  • (x, y) is the position for the center of the vehicle rear axle.

  • θ is the vehicle yaw angle.

The optimization status of the nonlinear MPC controller, output as an integer as follows:

  • Positive — Optimal solution found

  • 0 — Maximum number of optimization iterations reached, solution might not be optimal

  • Negative — Infeasible optimization problem

Dependencies

To enable this port, select the Optimization status parameter.

Parameters

expand all

Parameters Tab

Vehicle length, specified in meters.

Programmatic Use

Block Parameter: egoLength
Type: string, character vector
Default: "4.7"

Vehicle width, specified in meters.

Programmatic Use

Block Parameter: egoWidth
Type: string, character vector
Default: "1.8"

The distance between the front and rear axles, specified in meters.

Programmatic Use

Block Parameter: wheelbase
Type: string, character vector
Default: "2.8"

The longitudinal distance from the center of the vehicle to the rear axle, specified in meters.

Programmatic Use

Block Parameter: dist2Center
Type: string, character vector
Default: "1.4"

The number of obstacles in the environment.

Programmatic Use

Block Parameter: numObs
Type: string, character vector
Default: "0"

Specify obstacle information as a matrix with N rows, where N is equal to the Number of obstacles parameter. Each row contains the following information for an obstacle in the order [X Y Φ L W]:

  • (X, Y) — Position, specified in meters

  • Φ — Yaw angle, specified in radians

  • L — Length, specified in meters

  • W — Width, specified in meters

If the obstacle positions and orientations vary over time, add the Obstacles input port to the block by selecting Use external source.

Dependencies

To enable this parameter, set the Number of obstacles parameter to a nonzero value.

Programmatic Use

Block Parameter: obsMat
Type: string, character vector
Default: "zeros(1,5)"

Programmatic Use: Use external source

Block Parameter: obs_ext
Type: string, character vector
Values: "off" | "on"
Default: "off"

Minimum safe distance between the ego vehicle and any obstacle in the environment, specified in meters.

Dependencies

To enable this parameter, set the Number of obstacles parameter to a nonzero value.

Programmatic Use

Block Parameter: minDist
Type: string, character vector
Default: "0.1"

Controller sample time, specified in seconds.

Programmatic Use

Block Parameter: Ts
Type: string, character vector
Default: "1"

Prediction horizon steps for planning. The total prediction time is the product of the sample time and the prediction horizon.

Programmatic Use

Block Parameter: pPlanning
Type: string, character vector
Default: "20"

Vehicle velocity range in meters per second, specified as the two-element row vector [Vmin Vmax], where:

  • Vmin is the minimum vehicle velocity.

  • Vmax is the maximum vehicle velocity.

If the velocity range varies over time, add the Velocity range input port to the block by selecting Use external source.

Programmatic Use

Block Parameter: v_range
Type: string, character vector
Default: "[-6.5,6.5]"

Programmatic Use: Use external source

Block Parameter: v_ext
Type: string, character vector
Values: "off" | "on"
Default: "off"

Front steering angle range in radians, specified as the two-element row vector [ψmin ψmax], where:

  • ψmin is the minimum steering angle.

  • ψmax is the maximum steering angle.

The front steering angle is the angle of the front tires from the longitudinal axis of the vehicle. The steering angle is positive towards the positive lateral axis of the ego vehicle.

If the steering angle range varies over time, add the Steering angle range input port to the block by selecting Use external source.

Programmatic Use

Block Parameter: steer_range
Type: string, character vector
Default: "[-pi/4,pi/4]"

Programmatic Use: Use external source

Block Parameter: steer_ext
Type: string, character vector
Values: "off" | "on"
Default: "off"
Block Tab

Configure the controller to apply a suboptimal solution after a specified maximum number of iterations, which guarantees the worst-case execution time for your planning controller. To set the maximum number of iterations, use the Maximum iteration number parameter.

For more information, see Suboptimal QP Solution.

Programmatic Use

Block Parameter: suboptimal
Type: string, character vector
Values: "off" | "on"
Default: "off"

Maximum number of optimization iterations.

Dependencies

To enable this parameter, select the Use suboptimal solution parameter.

Programmatic Use

Block Parameter: maxIter
Type: string, character vector
Default: "200"

Select this parameter to add the Exitflag output port to the block.

Programmatic Use

Block Parameter: output_exitflag
Type: string, character vector
Values: "off" | "on"
Default: "off"

Generate a custom VPP subsystem, which you can modify for your application. The configuration data for the custom controller is exported to the MATLAB® workspace as a structure.

Extended Capabilities

Version History

Introduced in R2022b