Vehicle Path Planner System

Plan collision-free path from initial pose to target pose

Since R2022b

Libraries:
Model Predictive Control Toolbox

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.

Examples

Plan Parallel Parking Path Using Multistage Nonlinear Model Predictive Control

You can plan a parking path for a vehicle using the Vehicle Path Planner System block.

Open Live Script

Parking Valet Using Multistage Nonlinear Model Predictive Control

Design controller for parking garage valet using nonlinear model predictive control.

Open Live Script

Ports

Input

expand all

Pose — Initial vehicle pose
three-element row vector

Specify the initial vehicle pose as the vector signal [x₀ y₀ θ₀], where:

(x₀, y₀) is the initial position for the center of the vehicle rear axle.
θ₀ is the initial vehicle yaw angle.

Target Pose — Target vehicle pose
three-element row vector

Specify the target vehicle pose as the vector signal [x_t y_t θ_t], where:

(x_t, y_t) is the target position for the center of the vehicle rear axle.
θ_t is the initial vehicle yaw angle.

Enable — Enable planning
logical scalar

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.

Obstacles — List of obstacles
five-column matrix

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.

Velocity range — Velocity range
two-element vector

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

V_min is the minimum vehicle velocity.
V_max is the maximum vehicle velocity.

Dependencies

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

Steering angle range — Front steering angle range
two-element vector

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

Path — Planned vehicle path
three-column matrix

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.

Exitflag — Optimization status
integer

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

Length — Vehicle length
`4.7` (default) | nonnegative scalar

Vehicle length, specified in meters.

Programmatic Use

Block Parameter: egoLength

Type: string, character vector

Default: "4.7"

Width — Vehicle width
`1.8` (default) | nonnegative scalar

Vehicle width, specified in meters.

Programmatic Use

Block Parameter: egoWidth

Type: string, character vector

Default: "1.8"

Wheelbase — Distance between front and rear axles
`2.8` (default) | nonnegative scalar

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

Programmatic Use

Block Parameter: wheelbase

Type: string, character vector

Default: "2.8"

Center to rear axle — Distance from center of vehicle to rear axle
`1.4` (default) | nonnegative scalar

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"

Number of obstacles — Number of obstacles in environment
`0` (default) | nonnegative integer

The number of obstacles in the environment.

Programmatic Use

Block Parameter: numObs

Type: string, character vector

Default: "0"

Obstacle list — Obstacles in environment
`zeros(1,5)` (default) | five-column matrix

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 distance from ego vehicle to obstacles — Minimum safe distance between vehicle and any obstacle
`0.1` (default) | positive scalar

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"

Sample time — Controller sample time
`1` (default) | positive scalar

Controller sample time, specified in seconds.

Programmatic Use

Block Parameter: Ts

Type: string, character vector

Default: "1"

Prediction horizon — Prediction horizon
`20` (default) | positive integer

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"

Velocity range — Velocity range
`[-6.5 6.5]` (default) | two-element row vector

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

V_min is the minimum vehicle velocity.
V_max 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"

Steering angle range — Front steering angle range
`[-pi/4 pi/4]` | two-element row vector

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

Use suboptimal solution — Apply suboptimal solution after specified number of iterations
`off` (default) | `on`

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 iteration number — Maximum optimization iterations
`200` (default) | positive integer

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"

Optimization status — Add optimization status output port
off (default) | on

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"

Create VPP subsystem — Create custom planner
button

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

C/C++ Code Generation
Generate C and C++ code using Simulink® Coder™.

Usage notes and limitations:

Code generation for single-precision or fixed-point computations is not supported.
In the Configuration Parameters dialog box, on the Code Generation > Interface pane, you must select the following parameters.
- Support non-finite numbers — This parameter is selected by default.
- Support variable-size signals when using Embedded Coder^® — This parameter is not selected by default.

Version History

Introduced in R2022b

Vehicle Path Planner System

Description

Examples

Plan Parallel Parking Path Using Multistage Nonlinear Model Predictive Control

Parking Valet Using Multistage Nonlinear Model Predictive Control

Ports

Input

Pose — Initial vehicle pose three-element row vector

Target Pose — Target vehicle pose three-element row vector

Enable — Enable planning logical scalar

Obstacles — List of obstacles five-column matrix

Dependencies

Velocity range — Velocity range two-element vector

Dependencies

Steering angle range — Front steering angle range two-element vector

Dependencies

Output

Path — Planned vehicle path three-column matrix

Exitflag — Optimization status integer

Dependencies

Parameters

Length — Vehicle length 4.7 (default) | nonnegative scalar

Programmatic Use

Width — Vehicle width 1.8 (default) | nonnegative scalar

Programmatic Use

Wheelbase — Distance between front and rear axles 2.8 (default) | nonnegative scalar

Programmatic Use

Center to rear axle — Distance from center of vehicle to rear axle 1.4 (default) | nonnegative scalar

Programmatic Use

Number of obstacles — Number of obstacles in environment 0 (default) | nonnegative integer

Programmatic Use

Obstacle list — Obstacles in environment zeros(1,5) (default) | five-column matrix

Dependencies

Programmatic Use

Programmatic Use: Use external source

Minimum distance from ego vehicle to obstacles — Minimum safe distance between vehicle and any obstacle 0.1 (default) | positive scalar

Dependencies

Programmatic Use

Sample time — Controller sample time 1 (default) | positive scalar

Programmatic Use

Prediction horizon — Prediction horizon 20 (default) | positive integer

Programmatic Use

Velocity range — Velocity range [-6.5 6.5] (default) | two-element row vector

Programmatic Use

Programmatic Use: Use external source

Steering angle range — Front steering angle range [-pi/4 pi/4] | two-element row vector

Programmatic Use

Programmatic Use: Use external source

Use suboptimal solution — Apply suboptimal solution after specified number of iterations off (default) | on

Programmatic Use

Maximum iteration number — Maximum optimization iterations 200 (default) | positive integer

Dependencies

Programmatic Use

Optimization status — Add optimization status output port off (default) | on

Programmatic Use

Create VPP subsystem — Create custom planner button

Extended Capabilities

C/C++ Code Generation Generate C and C++ code using Simulink® Coder™.

Version History

See Also

Blocks

Topics

Pose — Initial vehicle pose
three-element row vector

Target Pose — Target vehicle pose
three-element row vector

Enable — Enable planning
logical scalar

Obstacles — List of obstacles
five-column matrix

Velocity range — Velocity range
two-element vector

Steering angle range — Front steering angle range
two-element vector

Path — Planned vehicle path
three-column matrix

Exitflag — Optimization status
integer

Length — Vehicle length
`4.7` (default) | nonnegative scalar

Width — Vehicle width
`1.8` (default) | nonnegative scalar

Wheelbase — Distance between front and rear axles
`2.8` (default) | nonnegative scalar

Center to rear axle — Distance from center of vehicle to rear axle
`1.4` (default) | nonnegative scalar

Number of obstacles — Number of obstacles in environment
`0` (default) | nonnegative integer

Obstacle list — Obstacles in environment
`zeros(1,5)` (default) | five-column matrix

Minimum distance from ego vehicle to obstacles — Minimum safe distance between vehicle and any obstacle
`0.1` (default) | positive scalar

Sample time — Controller sample time
`1` (default) | positive scalar

Prediction horizon — Prediction horizon
`20` (default) | positive integer

Velocity range — Velocity range
`[-6.5 6.5]` (default) | two-element row vector

Steering angle range — Front steering angle range
`[-pi/4 pi/4]` | two-element row vector

Use suboptimal solution — Apply suboptimal solution after specified number of iterations
`off` (default) | `on`

Maximum iteration number — Maximum optimization iterations
`200` (default) | positive integer

Optimization status — Add optimization status output port
off (default) | on

Create VPP subsystem — Create custom planner
button

C/C++ Code Generation
Generate C and C++ code using Simulink® Coder™.