2.2.3.6. mpc module

Python module to implement bicycle model and cost function for MPC as model and respective MPC.

This MPC predicts vehicle behaviour to follow a planned trajectory.

class mpc.MPC(prediction_steps_n: int, step_size: float, obj_factor_x_pos: float, obj_factor_y_pos: float, obj_factor_a_x: float, obj_factor_delta_rate: float, obj_factor_delta: float, ackermann_angle_upper_boundary: float, ackermann_angle_lower_boundary: float, l_r: float, l_f: float, m: float, c_aero: float, is_skidpad: bool)

Bases: object

Implement functionality to predict states and control inputs with MPC based on trajectory, vehicle pose and model.

Model Predictive Controller class to implement functionality to predict states and control inputs with Model Predictive Controller based on planned trajectory, current vehicle pose and actual steering wheel angle and non linear bicycle model.

trajectory

Current planned trajectory calculated by Motion Planning, shape: (n, 3) with n equal number trajectory points.

Type:

Optional[npt.NDArray]

cumulative_arc_length

Respective cumulative arc length of the trajectory, shape: (n, ) with n equal number trajectory points.

Type:

Optional[npt.NDArray]

heading

Respective vehicle heading on the trajectory, shape: (n, ) with n equal number trajectory points.

Type:

Optional[npt.NDArray]

curvature

Respective curvature of the trajectory, shape: (n, ) with n equal number trajectory points.

Type:

Optional[npt.NDArray]

time

Respective time when the vehicle will be there on the trajectory, shape: (n, ) with n equal number trajectory points.

Type:

Optional[npt.NDArray]

closed_track

Indicates whether the trajectory is closed or open.

Type:

Optional[bool]

ackermann_angle_upper_boundary

Allowed upper boundary of ackermann angle.

Type:

float

ackermann_angle_lower_boundary

Allowed lower boundary of ackermann angle.

Type:

float

is_skidpad

Indicates whether the current selected Autonomous Mission is Skidpad.

Type:

bool

last_trajectory_index

Trajectory index of last closest trajectory point to the vehicle. Used for driving Skidpad.

Type:

Optional[int]

exitflag

Exitflag of last prediction.

Type:

Optional[int]

info

Information of last prediction.

Type:

Optional[Any]

x

Current state vector of the bicycle model (x, y, psi, v_x, delta), shape: (5, ).

Type:

npt.NDArray

prediction_steps_n

Number of prediction steps for prediction.

Type:

int

step_size

Delta t [s] between two subsequent prediction steps.

Type:

float

obj_factor_x_pos

Factor for objective function for deviation of x position.

Type:

float

obj_factor_y_pos

Factor for objective function for deviation of y position.

Type:

float

obj_factor_a_x

Factor for objective function for acceleration in x direction.

Type:

float

obj_factor_delta_rate

Factor for objective function for delta rate.

Type:

float

obj_factor_delta

Factor for objective function for delta.

Type:

float

l_r

tbd.

Type:

float

l_f

tbd.

Type:

float

m

Mass of vehicle.

Type:

float

c_aero

C Aero of vehicle.

Type:

float

model

Bicycle model used as basis for the model predictive controller.

Type:

Model

solver

Forces Pro Solver for solving the optimization problem of the model predictive controller prediction.

Type:

forcespro.nlp.Solver

Initialize Model Predictive Controller.

Intialize all parameters and intializes bicycle model and Forces Pro Solver.

Parameters:

• prediction_steps_n (int) – Number of prediction steps for prediction.

• step_size (float) – Delta t [s] between two subsequent prediction steps.

• obj_factor_x_pos (float) – Factor for objective function for deviation of x position.

• obj_factor_y_pos (float) – Factor for objective function for deviation of y position.

• obj_factor_a_x (float) – Factor for objective function for acceleration in x direction.

• obj_factor_delta_rate (float) – Factor for objective function for delta rate.

• obj_factor_delta (float) – Factor for objective function for delta.

• ackermann_angle_upper_boundary (float) – Allowed upper boundary of ackermann angle.

• ackermann_angle_lower_boundary (float) – Allowed lower boundary of ackermann angle.

• l_r (float) – tbd.

• l_f (float) – tbd.

• m (float) – Mass of vehicle.

• c_aero (float) – C Aero of vehicle.

• is_skidpad (bool) – Indicates whether the current selected Autonomous Mission is Skidpad.

generate_pathplanner() → Tuple[Model, forcespro.nlp.Solver]

Generate FORCESPRO solver to calculate paths based on constraints and dynamics by minimizing an objective func.

Returns:

Bicycle model used for the MPC and the respective solver for the MPC.

Return type:

Tuple[Model, forcespro.nlp.Solver]

static interp_2d(x: ndarray[Any, dtype[ScalarType]], xp: ndarray[Any, dtype[ScalarType]], fp: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]]

Interpolates 2d function for given points.

Parameters:

• x (npt.NDArray) – Points for which the function should be interpolated, shape: (m, ) with m equal to number of to be interpolated points.

• xp (npt.NDArray) – X values of the 2d function, shape: (n, ) with n equal to the support points of the function.

• fp (npt.NDArray) – Y values of the 2d function, shape: (n, 2) with n equal to the support points of the function.

Returns:

Interpolated function for the given points, shape: (m, 2) with m equal to number of to be interpolated points.

Return type:

npt.NDArray

localize_vehicle_on_path() → float

Localizes vehicle on the current path.

Returns which path point is closest to the vehicle by returning the respective cumulative arc length for this point.

Calculates the closest pathpoint based only on the the distance. In case a skidpad is driven, only the next few points since the last prediction steps are allowed. Calculates the respective cumulative arc length for this point.

Returns:

Respective arc length for the closest point on the path to the vehicle.

Return type:

float

predict(state_vector: ndarray[Any, dtype[ScalarType]]) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]]

Predict control inputs for the prediction horizon with the MPC based on the states & previously set trajectory.

Parameters:

state_vector (npt.NDArray) – Current state vector of the bicycle model (x, y, psi, v_x, delta).

Returns:

Predicted states (x, y, psi, v_x, delta) (shape: (n, 5)) with n equal number of prediction steps and control inputs (a_x, delta_rate) (shape: (n, 2)) with n equal number of prediction steps

Return type:

Tuple[npt.NDArray, npt.NDArray]

Raises:

NoIteration – Indicates that the prediction of the model predictive controller was not succesful.

resample_path_for_tracker() → ndarray[Any, dtype[ScalarType]]

Resamples path of trajectory for the tracker of the model predictive controller.

Therefore interpolates at which point of the path the vehicle should be for the respective predictions steps of the prediction horizon.

Returns:

Planned path for the prediction horizon, shape: (n, 2) with n equal number of prediction steps.

Return type:

npt.NDArray

set_trajectory(trajectory: ndarray[Any, dtype[ScalarType]], closed_track: bool) → None

Set current planned trajectory and calculate other important values like curvature based on it.

Calculates cumulative arc length, heading at each point, curvature and a respective time for each trajectory point.

Parameters:

• trajectory (npt.NDArray) – Planned trajectory calculated by Motion Planning, shape: (n, 3) with n equal number trajectory points.

• closed_track (bool) – Indicates whether the trajectory is closed.

class mpc.Model(prediction_steps_n: int, step_size: float, ackermann_angle_upper_boundary: float, ackermann_angle_lower_boundary: float, l_r: float, l_f: float, m: float, c_aero: float)

Bases: object

Bicycle model for the model predictive controller.

Implements system propagation function (continues_dynamics / F_x), objective functions and functions to create Forces Pro Model and Solver.

prediction_steps_n

Number of prediction steps for prediction.

Type:

int

step_size

Delta t [s] between two subsequent prediction steps.

Type:

float

ackermann_angle_upper_boundary

Allowed upper boundary of ackermann angle.

Type:

float

ackermann_angle_lower_boundary

Allowed lower boundary of ackermann angle.

Type:

float

l_r

tbd.

Type:

float

l_f

tbd.

Type:

float

m

Mass of vehicle.

Type:

float

c_aero

C Aero of vehicle.

Type:

float

Initialize bicycle model.

Parameters:

• prediction_steps_n (int) – Number of prediction steps for prediction.

• step_size (float) – Delta t [s] between two subsequent prediction steps.

• ackermann_angle_upper_boundary (float) – Allowed upper boundary of ackermann angle.

• ackermann_angle_lower_boundary (float) – Allowed lower boundary of ackermann angle.

• l_r (float) – tbd.

• l_f (float) – tbd.

• m (float) – Mass of vehicle.

• c_aero (float) – C Aero of vehicle.

continuous_dynamics(x: Any, u: Any) → Any

Calculate derivation of state vector based on state vector and control inputs.

Parameters:

• x (Any) – State vector (x, y, psi, v_x, delta), shape: (5, ).

• u (Any) – Control input vector (a_x, delta_rate), shape: (2, ).

Returns:

Derivation of state vector (dx, dy, dpsi, a_x, delta_rate), shape: (5, ).

Return type:

Any

generate_model() → forcespro.nlp.SymbolicModel

Generate Model based on bicycle model for the Forces Pro Solver.

Returns:

Model for Forces Pro Solver.

Return type:

forcespro.nlp.SymbolicModel

generate_solver() → forcespro.nlp.Solver

Generate Forces Pro Solver for bicycle model.

Returns:

Generated Forces Pro Solver.

Return type:

forcespro.nlp.Solver

objective(z: Any, runtime_params: Any) → float

Calculate cost for prediction step based on predictes states and control inputs and current target.

Todo

Which types do they really have?

Parameters:

• z (Any) – Predicted states and control inputs, shape: (7, ) (a_x, delta_rate, x, y, psi, v_x, delta).

• runtime_params (Any) – Current target path points and objective factors, shape: (7, ) (x, y, obj_factor_x_pos, obj_factor_y_pos, obj_factor_a_x, obj_factor_delta_rate, obj_factor_delta).

Returns:

Cost for prediction step.

Return type:

float

objective_n(z: Any, runtime_params: Any) → float

Calculate cost for last prediction step based on predictes states and control inputs and current target.

Todo

Which types do they really have?

Parameters:

• z (Any) – Predicted states and control inputs, shape: (7, ) (a_x, delta_rate, x, y, psi, v_x, delta).

• runtime_params (Any) – Current target path points and objective factors, shape: (7, ) (x, y, obj_factor_x_pos, obj_factor_y_pos, obj_factor_a_x, obj_factor_delta_rate, obj_factor_delta).

Returns:

Cost for the last prediction step.

Return type:

float

exception mpc.NoIteration

Bases: Exception

Exception to signal that the MPC did not solve the problem since no iteration was calculated.