LIBRARY
Examples.ACADOSMPCRaceCarExample ​
Lap-time minimization of the dymos race car with ACADOSMPC as a receding-horizon controller, on the oval of the dymos example (https://openmdao.github.io/dymos/examples/racecar/racecar.html).
dymos solves one optimal control problem over the lap, with the arc length as the independent variable and the state at the end of the lap constrained to equal the state at its start, and minimizes the lap time directly. Here the controller runs on a clock and, at every tick, plans the next Tf seconds of driving (about a fifth of the lap, which reaches around the next corner from anywhere on the track), applies the first control and replans at the next tick. There is no periodicity constraint: the car crosses the start line at V0, about nine tenths of the speed at which it crosses the finish line, and drives a lap and a tenth, so that the lap is complete and the crossing of the finish line is seen.
The cost ​
ACADOSMPC has a quadratic cost only, and lap time is a linear objective: the rate of progress along the centerline, v_ref for q_n on the lateral offset resolves the flat direction of the problem on the straights, where any line is as fast as any other; it is far too weak to keep the car from using the whole width of the track in the corners. The controls are the rates of the steering angle, the throttle and the brake, weighted by Q2, which keeps the controls continuous (dymos: rate_continuity) and regularizes the problem.
Constraints ​
The track is the soft state bound RaceCarPlant inscribes in the friction ellipse (adh_fl[1..8], ...) and the engine power the normalized power_norm, all of them hard nonlinear constraints from the first predicted state on (nl_constraints_from_stage = 1): the controls are rates, so at stage 0 these constraints depend on the measured state alone and are either satisfied or infeasible there. Left out at stage 0, the hard constraints replace two slack variables per constraint and stage, which takes about a tenth off the solver time relative to the soft version (nl_constraints_soft) at the same lap; the plant is the model here, so no disturbance can make them infeasible. The track bound has to stay soft: the linearized QP is infeasible at the first tick when it is hard, since the initial guess, a straight line at constant speed, does not turn into the first corner within the horizon. The friction ellipse itself, in the form dymos constrains or multiplied through by the normal load, makes the sequential quadratic programming stall with or without scaling: it spans a slip angle of a few hundredths of a radian, so its curvature over one step of the iteration is large, whereas the facets of the polygon are linear in the slip angle. The two tables of the track (seg_curvature, seg_length) are parameters of the plant and online parameters of the controller, as the path is in ACADOSMPCOmniPathFollowingExample, so the same loop drives another track (the mirrored oval, in test/test_race_car.jl); they describe one lap and are evaluated at the arc length within the lap, so the loop drives any number of laps on them. The plant is the prediction model (dynamics = plant) and the tables are named by tunable_parameters; the plant outputs them as signals (seg_curvature_out, seg_length_out), which trackmux joins into the p input of the controller, so that a change of the plant, its track included, reaches the controller.
Solver ​
The state spans five orders of magnitude (the arc length in kilometres, the slip angle in milliradians), so x_scale and u_scale give the typical magnitude of every variable and the optimization runs in the scaled ones (the ref values of the dymos example); without it the iterations stall. The yaw and slip dynamics are stiff at every speed – the lateral tyre stiffness grows with the downforce, so the yaw time constant stays near 0.02 s – and are integrated with the implicit integrator and Radau IIA collocation, which damps them over the long shooting intervals of the stretched grid. RK4 needs a sub-step below about 0.05 s to be stable on them: with 3 or 4 sub-steps per interval the first QP fails, with 6 the loop diverges, and with 8 or 12 it runs at 1.3 and 1.6 times the solver time of the implicit integrator with two sub-steps. Each tick runs full-step SQP iterations with Levenberg-Marquardt regularization from the shifted previous solution: a single real-time iteration per tick (SQP_RTI) diverges from the start of the lap, where the initial guess is poor, and the merit-function line search converges an order of magnitude slower on this problem than the damped full steps. The regularization makes the final convergence linear, and running the iterations to the tolerance (set to the scale of the problem, the cost gradients being of order a hundred) takes 35 of them per tick. The cap max_iter is therefore set to 5: the shifted warm start carries the unconverged iterate into the next tick, the closed loop drives the same lap as with 60 iterations to within a hundredth of a second, the tyre constraints are violated by the same 0.3 % at the ticks, and the solver time falls by a factor of 6. Down to 2 iterations per tick the lap is still the same, but below 4 the first tick, which starts from the constant guess, applies a negative brake for one period before the iterations catch up; a single iteration per tick raises the tyre violation to 2.5 %. exitflag is 2 on nearly every tick, which here means the cap and not a failure. The Newton iterations of the implicit integrator (integrator_newton_iter) matter little in comparison: one instead of three saves a sixth of the linearization time, a twentieth of the total.
Results ​
From the start at 65 m/s the lap takes about 23 s against 22.27 s for the periodic dymos lap, which has the whole friction ellipse, no rate limits and a track that is 5 m longer. The car brakes to 38 m/s for the tight corners and reaches 78 m/s at the end of the long straight, uses the full width of the track from the outside to the inside and back, and holds the tyres at the limit of adherence through the corners and the engine at its power limit along the straights. Simulating the lap takes about 1 s of solver time, most of it in the QP: the sample interval, the number of shooting intervals and the horizon are the result of a trade-off against the lap time (40 intervals over 5 s at 0.1 s gave 22.9 s at three times the cost), condensing to more or fewer blocks changed nothing, and warm-starting the QP from the previous one (qp_warm_start = 2) halved its time. Verified in test/test_race_car.jl. examples/race_car.jl draws the figures of the dymos tutorial for the formulation over the arc length, ACADOSMPCRaceCarSpatialExample.
Usage ​
MPCComponents.Examples.ACADOSMPCRaceCarExample(v_ref=100.0, tol=1.0, levenberg_marquardt=1.0)
Parameters: ​
| Name | Description | Units | Default value |
|---|---|---|---|
Np | Prediction horizon (number of shooting intervals) | – | 25 |
Ts | Sample interval, the length of the first shooting interval | – | 0.15 |
Tf | Prediction horizon in seconds, spanned by the Np shooting intervals; about a fifth of the lap, around the next corner | – | 4.5 |
V0 | Speed at the start line, about nine tenths of the speed the car crosses the finish line at | – | 65.0 |
n_max | Half width of the track, the bound on the lateral offset | – | 4.0 |
V_min | Lower bound of the speed (the slip-angle dynamics divide by it) | – | 5.0 |
V_max | Upper bound of the speed | – | 200.0 |
delta_max | Bound of the steering angle | – | 0.15 |
thrust_max | Bound of the throttle and of the brake | – | 3.0 |
delta_rate_max | Bound of the steering rate | – | 1.0 |
thrust_rate_max | Bound of the throttle and brake rates | – | 10.0 |
c_max | Share of the adherence of each tyre the controller may use (one for the full limit) | – | 1.0 |
soft_weight | Quadratic penalty weight on the slack variables of the soft constraints | – | 1e6 |
output_trajectories | Record the predicted trajectories and solver residuals of the controller (for mpc_gui) | – | true |
time_steps | Lengths of the shooting intervals, growing linearly from Ts to cover Tf (derived) | – | linear_time...Ts, Np, Tf) |
constrained_signals | Nonlinear constrained signals: the polygon facets of the four tyres and the normalized power (derived) | – | race_car_co...d_signals() |
constrained_max | Upper bounds of the nonlinear constraints (derived from c_max) | – | race_car_co..._max(c_max) |
nseg | Number of segments in the track tables (derived) | – | racetrack_segment_count() |
q_progress | Weight on the progress-rate deficit v_ref - sdot | – | 1.0 |
q_n | Weight on the lateral offset, resolving the flat direction on the straights | – | 0.5 |
r_delta | Weight on the steering rate | – | 100.0 |
r_thrust | Weight on the throttle and brake rates | – | 0.1 |
max_iter | Iteration cap of the SQP at each tick | – | 5 |
v_ref | Reference for the rate of progress along the centerline, well above what the car can reach | – | 100.0 |
tol | Convergence tolerance of the SQP, an absolute residual on the scale of the cost gradients | – | 1.0 |
levenberg_marquardt | Levenberg-Marquardt regularization of the SQP steps | – | 1.0 |
Variables ​
| Name | Description | Units |
|---|---|---|
s | Arc length along the centerline | – |
n | Lateral offset from the centerline | – |
alpha | Heading relative to the centerline | – |
V | Speed | – |
lambda | Body slip angle | – |
omega | Yaw rate | – |
delta | Steering angle | – |
throttle | Throttle | – |
brake | Brake | – |
thrust | Throttle minus brake, the signed thrust of dymos | – |
sdot | Rate of progress along the centerline | – |
power_norm | Tractive power relative to the engine limit | – |
c_fl | Friction ellipse of the front-left tyre (one at the limit) | – |
c_fr | Friction ellipse of the front-right tyre | – |
c_rl | Friction ellipse of the rear-left tyre | – |
c_rr | Friction ellipse of the rear-right tyre | – |
flag | acados status | – |
Behavior ​
Behavior of this component cannot be rendered because it includes path variables.
Source ​
dyad
"""
Lap-time minimization of the dymos race car with `ACADOSMPC` as a receding-horizon controller, on the
oval of the dymos example (https://openmdao.github.io/dymos/examples/racecar/racecar.html).
dymos solves one optimal control problem over the lap, with the arc length as the independent variable
and the state at the end of the lap constrained to equal the state at its start, and minimizes the
lap time directly. Here the controller runs on a clock and, at every tick, plans the next `Tf` seconds
of driving (about a fifth of the lap, which reaches around the next corner from anywhere on the
track), applies the first control and replans at the next tick. There is no periodicity constraint:
the car crosses the start line at `V0`, about nine tenths of the speed at which it crosses the finish
line, and drives a lap and a tenth, so that the lap is complete and the crossing of the finish line is
seen.
# The cost
`ACADOSMPC` has a quadratic cost only, and lap time is a linear objective: the rate of progress along
the centerline, ``\\dot s = V \\cos(\\alpha - \\lambda) / (1 - n \\kappa(s))``, is what a lap-time
minimizer maximizes. The workaround is a reference `v_ref` for ``\\dot s`` that is out of reach: the
term ``q_\\mathrm{progress} (v_\\mathrm{ref} - \\dot s)^2`` then rewards progress at every stage with a
gradient ``2 q_\\mathrm{progress} (v_\\mathrm{ref} - \\dot s)`` that stays positive and nearly constant,
and ``\\dot s`` rather than ``V`` is what is rewarded because it credits the inside line through a
corner. ``\\dot s`` is a nonlinear function of the state, so the cost is a nonlinear least squares. A
small weight `q_n` on the lateral offset resolves the flat direction of the problem on the straights,
where any line is as fast as any other; it is far too weak to keep the car from using the whole width
of the track in the corners. The controls are the rates of the steering angle, the throttle and the
brake, weighted by `Q2`, which keeps the controls continuous (dymos: `rate_continuity`) and
regularizes the problem.
# Constraints
The track is the soft state bound ``|n| \\le n_\\mathrm{max}``; the steering angle, the throttle and
the brake are bounded states and their rates bounded controls. The adherence of each tyre is the
octagon `RaceCarPlant` inscribes in the friction ellipse (`adh_fl[1..8]`, ...) and the engine power the
normalized `power_norm`, all of them hard nonlinear constraints from the first predicted state on
(`nl_constraints_from_stage = 1`): the controls are rates, so at stage 0 these constraints depend on
the measured state alone and are either satisfied or infeasible there. Left out at stage 0, the hard
constraints replace two slack variables per constraint and stage, which takes about a tenth off the
solver time relative to the soft version (`nl_constraints_soft`) at the same lap; the plant is the
model here, so no disturbance can make them infeasible. The track bound has to stay soft: the
linearized QP is infeasible at the first tick when it is hard, since the initial guess, a straight
line at constant speed, does not turn into the first corner within the horizon. The friction ellipse itself,
in the form dymos constrains or multiplied through by the normal load, makes the sequential quadratic
programming stall with or without scaling: it spans a slip angle of a few hundredths of a radian, so
its curvature over one step of the iteration is large, whereas the facets of the polygon are linear
in the slip angle. The two tables of the track (`seg_curvature`, `seg_length`) are parameters of the plant and
online parameters of the controller, as the path is in `ACADOSMPCOmniPathFollowingExample`, so the
same loop drives another track (the mirrored oval, in `test/test_race_car.jl`); they describe one lap
and are evaluated at the arc length within the lap, so the loop drives any number of laps on them.
The plant is the prediction model (`dynamics = plant`) and the tables are named by
`tunable_parameters`; the plant outputs them as signals (`seg_curvature_out`, `seg_length_out`),
which `trackmux` joins into the `p` input of the controller, so that a change of the plant, its track
included, reaches the controller.
# Solver
The state spans five orders of magnitude (the arc length in kilometres, the slip angle in
milliradians), so `x_scale` and `u_scale` give the typical magnitude of every variable and the
optimization runs in the scaled ones (the `ref` values of the dymos example); without it the
iterations stall. The yaw and slip dynamics are stiff at every speed -- the lateral tyre stiffness
grows with the downforce, so the yaw time constant stays near 0.02 s -- and are integrated with the
implicit integrator and Radau IIA collocation, which damps them over the long shooting intervals of
the stretched grid. RK4 needs a sub-step below about 0.05 s to be stable on them: with 3 or 4
sub-steps per interval the first QP fails, with 6 the loop diverges, and with 8 or 12 it runs at
1.3 and 1.6 times the solver time of the implicit integrator with two sub-steps. Each tick runs full-step SQP iterations with Levenberg-Marquardt regularization from the
shifted previous solution: a single real-time iteration per tick (`SQP_RTI`) diverges from the start
of the lap, where the initial guess is poor, and the merit-function line search converges an order
of magnitude slower on this problem than the damped full steps. The regularization makes the final
convergence linear, and running the iterations to the tolerance (set to the scale of the problem, the
cost gradients being of order a hundred) takes 35 of them per tick. The cap `max_iter` is therefore
set to 5: the shifted warm start carries the unconverged iterate into the next tick, the closed loop
drives the same lap as with 60 iterations to within a hundredth of a second, the tyre constraints are
violated by the same 0.3 % at the ticks, and the solver time falls by a factor of 6. Down to 2
iterations per tick the lap is still the same, but below 4 the first tick, which starts from the
constant guess, applies a negative brake for one period before the iterations catch up; a single
iteration per tick raises the tyre violation to 2.5 %. `exitflag` is 2 on nearly every tick, which
here means the cap and not a failure. The Newton iterations of the implicit integrator
(`integrator_newton_iter`) matter little in comparison: one instead of three saves a sixth of the
linearization time, a twentieth of the total.
# Results
From the start at 65 m/s the lap takes about 23 s against 22.27 s for the periodic dymos lap, which has
the whole friction ellipse, no rate limits and a track that is 5 m longer. The car brakes to 38 m/s
for the tight corners and reaches 78 m/s at the end of the long straight, uses the full width of the
track from the outside to the inside and back, and holds the tyres at the limit of adherence through
the corners and the engine at its power limit along the straights. Simulating the lap takes about
1 s of solver time, most of it in the QP: the sample interval, the number of shooting intervals and
the horizon are the result of a trade-off against the lap time (40 intervals over 5 s at 0.1 s gave
22.9 s at three times the cost), condensing to more or fewer blocks changed nothing, and warm-starting
the QP from the previous one (`qp_warm_start = 2`) halved its time. Verified in
`test/test_race_car.jl`. `examples/race_car.jl` draws the figures of the dymos tutorial for the
formulation over the arc length, `ACADOSMPCRaceCarSpatialExample`.
"""
example component ACADOSMPCRaceCarExample
periodicclock = DiscreteComponents.PeriodicClock(final dt = Ts) {
"Dyad": {"placement": {"diagram": {"x1": 300, "y1": 140, "x2": 400, "y2": 240}}}
}
plant = MPCComponents.Examples.RaceCarPlant(V0 = V0) {
"Dyad": {"placement": {"diagram": {"x1": 900, "y1": 300, "x2": 1000, "y2": 400}}}
}
"Samplers of the eleven state variables of the car"
sx = [DiscreteComponents.Sampler() for i in 1:11] {
"Dyad": {
"placement": {"diagram": {"x1": 700, "y1": 600, "x2": 800, "y2": 700, "rot": 180}}
}
}
"Sampler of the progress-rate reference"
sr = DiscreteComponents.Sampler() {
"Dyad": {"placement": {"diagram": {"x1": 300, "y1": 300, "x2": 400, "y2": 400}}}
}
"Holds of the three control rates"
zoh = [DiscreteComponents.ZeroOrderHold() for i in 1:3] {
"Dyad": {"placement": {"diagram": {"x1": 700, "y1": 300, "x2": 800, "y2": 400}}}
}
"Reference for the rate of progress, out of reach on purpose so that the quadratic cost rewards progress"
vref = BlockComponents.Sources.Constant(k = v_ref) {
"Dyad": {"placement": {"diagram": {"x1": 100, "y1": 300, "x2": 200, "y2": 400}}}
}
"The two track tables of the plant, joined into the parameter input of the controller"
trackmux = MPCComponents.Examples.Mux(final n1 = nseg, final n2 = nseg) {
"Dyad": {
"placement": {"diagram": {"x1": 700, "y1": 450, "x2": 800, "y2": 550, "rot": 180}}
}
}
mpc = MPCComponents.ACADOSMPC(dynamics = plant, inputs = ["delta_rate", "throttle_rate", "brake_rate"], tunable_parameters = ["seg_curvature", "seg_length"], parameter_input = true, state_variables = ["s", "n", "alpha", "V", "lambda", "omega", "ax", "ay", "delta", "throttle", "brake"], outputs = ["sdot", "n"], final Ts = Ts, final Np = Np, final time_steps = time_steps, Q1 = diagonal([q_progress, q_n]), Q2 = diagonal([r_delta, r_thrust, r_thrust]), final umin = [-delta_rate_max, -thrust_rate_max, -thrust_rate_max], final umax = [delta_rate_max, thrust_rate_max, thrust_rate_max], constrained = ["n", "V", "delta", "throttle", "brake"], final constrained_min = [-n_max, V_min, -delta_max, 0.0, 0.0], final constrained_max = [n_max, V_max, delta_max, thrust_max, thrust_max], final nl_constrained = constrained_signals, final nl_constrained_max = constrained_max, nl_constraints_soft = false, nl_constraints_from_stage = 1, final soft_weight = soft_weight, final x_guess = [0.0, 0.0, 0.0, V0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], x_scale = [1000.0, 4.0, 0.15, 40.0, 0.01, 0.3, 8.0, 8.0, 0.04, 1.0, 1.0], u_scale = [1.0, 10.0, 10.0], nlp_solver = MPCComponents.ACADOSSolver.SQP(), max_iter = max_iter, tol = tol, levenberg_marquardt = levenberg_marquardt, integrator = MPCComponents.ACADOSIntegrator.IRK(), collocation = MPCComponents.ACADOSCollocation.RadauIIA(), integrator_stages = 2, integrator_steps = 2, qp_cond_N = 5, qp_warm_start = 2, final output_trajectories = output_trajectories) {
"Dyad": {"placement": {"diagram": {"x1": 500, "y1": 300, "x2": 600, "y2": 400}}}
}
"Prediction horizon (number of shooting intervals)"
structural parameter Np::Integer = 25
"Sample interval, the length of the first shooting interval"
structural parameter Ts::Real = 0.15
"Prediction horizon in seconds, spanned by the Np shooting intervals; about a fifth of the lap, around the next corner"
structural parameter Tf::Real = 4.5
"Speed at the start line, about nine tenths of the speed the car crosses the finish line at"
structural parameter V0::Real = 65.0
"Half width of the track, the bound on the lateral offset"
structural parameter n_max::Real = 4.0
"Lower bound of the speed (the slip-angle dynamics divide by it)"
structural parameter V_min::Real = 5.0
"Upper bound of the speed"
structural parameter V_max::Real = 200.0
"Bound of the steering angle"
structural parameter delta_max::Real = 0.15
"Bound of the throttle and of the brake"
structural parameter thrust_max::Real = 3.0
"Bound of the steering rate"
structural parameter delta_rate_max::Real = 1.0
"Bound of the throttle and brake rates"
structural parameter thrust_rate_max::Real = 10.0
"Share of the adherence of each tyre the controller may use (one for the full limit)"
structural parameter c_max::Real = 1.0
"Quadratic penalty weight on the slack variables of the soft constraints"
structural parameter soft_weight::Real = 1e6
"Record the predicted trajectories and solver residuals of the controller (for mpc_gui)"
structural parameter output_trajectories::Boolean = true
"Lengths of the shooting intervals, growing linearly from Ts to cover Tf (derived)"
final structural parameter time_steps::Real[:] = linear_time_steps(Ts, Np, Tf)
"Nonlinear constrained signals: the polygon facets of the four tyres and the normalized power (derived)"
final structural parameter constrained_signals::String[:] = race_car_constrained_signals()
"Upper bounds of the nonlinear constraints (derived from c_max)"
final structural parameter constrained_max::Real[:] = race_car_constrained_max(c_max)
"Number of segments in the track tables (derived)"
final structural parameter nseg::Integer = racetrack_segment_count()
"Reference for the rate of progress along the centerline, well above what the car can reach"
parameter v_ref::Real = 100.0
"Weight on the progress-rate deficit v_ref - sdot"
structural parameter q_progress::Real = 1.0
"Weight on the lateral offset, resolving the flat direction on the straights"
structural parameter q_n::Real = 0.5
"Weight on the steering rate"
structural parameter r_delta::Real = 100.0
"Weight on the throttle and brake rates"
structural parameter r_thrust::Real = 0.1
"Iteration cap of the SQP at each tick"
structural parameter max_iter::Integer = 5
"Convergence tolerance of the SQP, an absolute residual on the scale of the cost gradients"
parameter tol::Real = 1.0
"Levenberg-Marquardt regularization of the SQP steps"
parameter levenberg_marquardt::Real = 1.0
"Arc length along the centerline"
variable s::Real
"Lateral offset from the centerline"
variable n::Real
"Heading relative to the centerline"
variable alpha::Real
"Speed"
variable V::Real
"Body slip angle"
variable lambda::Real
"Yaw rate"
variable omega::Real
"Steering angle"
variable delta::Real
"Throttle"
variable throttle::Real
"Brake"
variable brake::Real
"Throttle minus brake, the signed thrust of dymos"
variable thrust::Real
"Rate of progress along the centerline"
variable sdot::Real
"Tractive power relative to the engine limit"
variable power_norm::Real
"Friction ellipse of the front-left tyre (one at the limit)"
variable c_fl::Real
"Friction ellipse of the front-right tyre"
variable c_fr::Real
"Friction ellipse of the rear-left tyre"
variable c_rl::Real
"Friction ellipse of the rear-right tyre"
variable c_rr::Real
"acados status"
variable flag::Real
relations
s = plant.s
n = plant.n
alpha = plant.alpha
V = plant.V
lambda = plant.lambda
omega = plant.omega
delta = plant.delta
throttle = plant.throttle
brake = plant.brake
thrust = plant.thrust
sdot = plant.sdot
power_norm = plant.power_norm
c_fl = plant.c_fl
c_fr = plant.c_fr
c_rl = plant.c_rl
c_rr = plant.c_rr
flag = mpc.exitflag
connect(plant.s_out, sx[1].u) {}
connect(plant.n_out, sx[2].u) {}
connect(plant.alpha_out, sx[3].u) {}
connect(plant.V_out, sx[4].u) {}
connect(plant.lambda_out, sx[5].u) {}
connect(plant.omega_out, sx[6].u) {}
connect(plant.ax_out, sx[7].u) {}
connect(plant.ay_out, sx[8].u) {}
connect(plant.delta_out, sx[9].u) {}
connect(plant.throttle_out, sx[10].u) {}
connect(plant.brake_out, sx[11].u) {}
connect(sx[1].y, mpc.x[1], periodicclock.y) {}
for i in 2:11
connect(sx[i].y, mpc.x[i])
end
connect(vref.y, sr.u) {}
connect(sr.y, mpc.r[1]) {}
mpc.r[2] = 0
for i in 1:3
connect(mpc.u[i], zoh[i].u)
end
connect(plant.seg_curvature_out, trackmux.u1) {}
connect(plant.seg_length_out, trackmux.u2) {}
connect(trackmux.y, mpc.p) {}
connect(zoh[1].y, plant.delta_rate) {}
connect(zoh[2].y, plant.throttle_rate) {}
connect(zoh[3].y, plant.brake_rate) {}
metadata {"Dyad": {"doc": {"behavior": false}}}
endFlattened Source
dyad
"""
Lap-time minimization of the dymos race car with `ACADOSMPC` as a receding-horizon controller, on the
oval of the dymos example (https://openmdao.github.io/dymos/examples/racecar/racecar.html).
dymos solves one optimal control problem over the lap, with the arc length as the independent variable
and the state at the end of the lap constrained to equal the state at its start, and minimizes the
lap time directly. Here the controller runs on a clock and, at every tick, plans the next `Tf` seconds
of driving (about a fifth of the lap, which reaches around the next corner from anywhere on the
track), applies the first control and replans at the next tick. There is no periodicity constraint:
the car crosses the start line at `V0`, about nine tenths of the speed at which it crosses the finish
line, and drives a lap and a tenth, so that the lap is complete and the crossing of the finish line is
seen.
# The cost
`ACADOSMPC` has a quadratic cost only, and lap time is a linear objective: the rate of progress along
the centerline, ``\\dot s = V \\cos(\\alpha - \\lambda) / (1 - n \\kappa(s))``, is what a lap-time
minimizer maximizes. The workaround is a reference `v_ref` for ``\\dot s`` that is out of reach: the
term ``q_\\mathrm{progress} (v_\\mathrm{ref} - \\dot s)^2`` then rewards progress at every stage with a
gradient ``2 q_\\mathrm{progress} (v_\\mathrm{ref} - \\dot s)`` that stays positive and nearly constant,
and ``\\dot s`` rather than ``V`` is what is rewarded because it credits the inside line through a
corner. ``\\dot s`` is a nonlinear function of the state, so the cost is a nonlinear least squares. A
small weight `q_n` on the lateral offset resolves the flat direction of the problem on the straights,
where any line is as fast as any other; it is far too weak to keep the car from using the whole width
of the track in the corners. The controls are the rates of the steering angle, the throttle and the
brake, weighted by `Q2`, which keeps the controls continuous (dymos: `rate_continuity`) and
regularizes the problem.
# Constraints
The track is the soft state bound ``|n| \\le n_\\mathrm{max}``; the steering angle, the throttle and
the brake are bounded states and their rates bounded controls. The adherence of each tyre is the
octagon `RaceCarPlant` inscribes in the friction ellipse (`adh_fl[1..8]`, ...) and the engine power the
normalized `power_norm`, all of them hard nonlinear constraints from the first predicted state on
(`nl_constraints_from_stage = 1`): the controls are rates, so at stage 0 these constraints depend on
the measured state alone and are either satisfied or infeasible there. Left out at stage 0, the hard
constraints replace two slack variables per constraint and stage, which takes about a tenth off the
solver time relative to the soft version (`nl_constraints_soft`) at the same lap; the plant is the
model here, so no disturbance can make them infeasible. The track bound has to stay soft: the
linearized QP is infeasible at the first tick when it is hard, since the initial guess, a straight
line at constant speed, does not turn into the first corner within the horizon. The friction ellipse itself,
in the form dymos constrains or multiplied through by the normal load, makes the sequential quadratic
programming stall with or without scaling: it spans a slip angle of a few hundredths of a radian, so
its curvature over one step of the iteration is large, whereas the facets of the polygon are linear
in the slip angle. The two tables of the track (`seg_curvature`, `seg_length`) are parameters of the plant and
online parameters of the controller, as the path is in `ACADOSMPCOmniPathFollowingExample`, so the
same loop drives another track (the mirrored oval, in `test/test_race_car.jl`); they describe one lap
and are evaluated at the arc length within the lap, so the loop drives any number of laps on them.
The plant is the prediction model (`dynamics = plant`) and the tables are named by
`tunable_parameters`; the plant outputs them as signals (`seg_curvature_out`, `seg_length_out`),
which `trackmux` joins into the `p` input of the controller, so that a change of the plant, its track
included, reaches the controller.
# Solver
The state spans five orders of magnitude (the arc length in kilometres, the slip angle in
milliradians), so `x_scale` and `u_scale` give the typical magnitude of every variable and the
optimization runs in the scaled ones (the `ref` values of the dymos example); without it the
iterations stall. The yaw and slip dynamics are stiff at every speed -- the lateral tyre stiffness
grows with the downforce, so the yaw time constant stays near 0.02 s -- and are integrated with the
implicit integrator and Radau IIA collocation, which damps them over the long shooting intervals of
the stretched grid. RK4 needs a sub-step below about 0.05 s to be stable on them: with 3 or 4
sub-steps per interval the first QP fails, with 6 the loop diverges, and with 8 or 12 it runs at
1.3 and 1.6 times the solver time of the implicit integrator with two sub-steps. Each tick runs full-step SQP iterations with Levenberg-Marquardt regularization from the
shifted previous solution: a single real-time iteration per tick (`SQP_RTI`) diverges from the start
of the lap, where the initial guess is poor, and the merit-function line search converges an order
of magnitude slower on this problem than the damped full steps. The regularization makes the final
convergence linear, and running the iterations to the tolerance (set to the scale of the problem, the
cost gradients being of order a hundred) takes 35 of them per tick. The cap `max_iter` is therefore
set to 5: the shifted warm start carries the unconverged iterate into the next tick, the closed loop
drives the same lap as with 60 iterations to within a hundredth of a second, the tyre constraints are
violated by the same 0.3 % at the ticks, and the solver time falls by a factor of 6. Down to 2
iterations per tick the lap is still the same, but below 4 the first tick, which starts from the
constant guess, applies a negative brake for one period before the iterations catch up; a single
iteration per tick raises the tyre violation to 2.5 %. `exitflag` is 2 on nearly every tick, which
here means the cap and not a failure. The Newton iterations of the implicit integrator
(`integrator_newton_iter`) matter little in comparison: one instead of three saves a sixth of the
linearization time, a twentieth of the total.
# Results
From the start at 65 m/s the lap takes about 23 s against 22.27 s for the periodic dymos lap, which has
the whole friction ellipse, no rate limits and a track that is 5 m longer. The car brakes to 38 m/s
for the tight corners and reaches 78 m/s at the end of the long straight, uses the full width of the
track from the outside to the inside and back, and holds the tyres at the limit of adherence through
the corners and the engine at its power limit along the straights. Simulating the lap takes about
1 s of solver time, most of it in the QP: the sample interval, the number of shooting intervals and
the horizon are the result of a trade-off against the lap time (40 intervals over 5 s at 0.1 s gave
22.9 s at three times the cost), condensing to more or fewer blocks changed nothing, and warm-starting
the QP from the previous one (`qp_warm_start = 2`) halved its time. Verified in
`test/test_race_car.jl`. `examples/race_car.jl` draws the figures of the dymos tutorial for the
formulation over the arc length, `ACADOSMPCRaceCarSpatialExample`.
"""
example component ACADOSMPCRaceCarExample
periodicclock = DiscreteComponents.PeriodicClock(final dt = Ts) {
"Dyad": {"placement": {"diagram": {"x1": 300, "y1": 140, "x2": 400, "y2": 240}}}
}
plant = MPCComponents.Examples.RaceCarPlant(V0 = V0) {
"Dyad": {"placement": {"diagram": {"x1": 900, "y1": 300, "x2": 1000, "y2": 400}}}
}
"Samplers of the eleven state variables of the car"
sx = [DiscreteComponents.Sampler() for i in 1:11] {
"Dyad": {
"placement": {"diagram": {"x1": 700, "y1": 600, "x2": 800, "y2": 700, "rot": 180}}
}
}
"Sampler of the progress-rate reference"
sr = DiscreteComponents.Sampler() {
"Dyad": {"placement": {"diagram": {"x1": 300, "y1": 300, "x2": 400, "y2": 400}}}
}
"Holds of the three control rates"
zoh = [DiscreteComponents.ZeroOrderHold() for i in 1:3] {
"Dyad": {"placement": {"diagram": {"x1": 700, "y1": 300, "x2": 800, "y2": 400}}}
}
"Reference for the rate of progress, out of reach on purpose so that the quadratic cost rewards progress"
vref = BlockComponents.Sources.Constant(k = v_ref) {
"Dyad": {"placement": {"diagram": {"x1": 100, "y1": 300, "x2": 200, "y2": 400}}}
}
"The two track tables of the plant, joined into the parameter input of the controller"
trackmux = MPCComponents.Examples.Mux(final n1 = nseg, final n2 = nseg) {
"Dyad": {
"placement": {"diagram": {"x1": 700, "y1": 450, "x2": 800, "y2": 550, "rot": 180}}
}
}
mpc = MPCComponents.ACADOSMPC(dynamics = plant, inputs = ["delta_rate", "throttle_rate", "brake_rate"], tunable_parameters = ["seg_curvature", "seg_length"], parameter_input = true, state_variables = ["s", "n", "alpha", "V", "lambda", "omega", "ax", "ay", "delta", "throttle", "brake"], outputs = ["sdot", "n"], final Ts = Ts, final Np = Np, final time_steps = time_steps, Q1 = diagonal([q_progress, q_n]), Q2 = diagonal([r_delta, r_thrust, r_thrust]), final umin = [-delta_rate_max, -thrust_rate_max, -thrust_rate_max], final umax = [delta_rate_max, thrust_rate_max, thrust_rate_max], constrained = ["n", "V", "delta", "throttle", "brake"], final constrained_min = [-n_max, V_min, -delta_max, 0.0, 0.0], final constrained_max = [n_max, V_max, delta_max, thrust_max, thrust_max], final nl_constrained = constrained_signals, final nl_constrained_max = constrained_max, nl_constraints_soft = false, nl_constraints_from_stage = 1, final soft_weight = soft_weight, final x_guess = [0.0, 0.0, 0.0, V0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], x_scale = [1000.0, 4.0, 0.15, 40.0, 0.01, 0.3, 8.0, 8.0, 0.04, 1.0, 1.0], u_scale = [1.0, 10.0, 10.0], nlp_solver = MPCComponents.ACADOSSolver.SQP(), max_iter = max_iter, tol = tol, levenberg_marquardt = levenberg_marquardt, integrator = MPCComponents.ACADOSIntegrator.IRK(), collocation = MPCComponents.ACADOSCollocation.RadauIIA(), integrator_stages = 2, integrator_steps = 2, qp_cond_N = 5, qp_warm_start = 2, final output_trajectories = output_trajectories) {
"Dyad": {"placement": {"diagram": {"x1": 500, "y1": 300, "x2": 600, "y2": 400}}}
}
"Prediction horizon (number of shooting intervals)"
structural parameter Np::Integer = 25
"Sample interval, the length of the first shooting interval"
structural parameter Ts::Real = 0.15
"Prediction horizon in seconds, spanned by the Np shooting intervals; about a fifth of the lap, around the next corner"
structural parameter Tf::Real = 4.5
"Speed at the start line, about nine tenths of the speed the car crosses the finish line at"
structural parameter V0::Real = 65.0
"Half width of the track, the bound on the lateral offset"
structural parameter n_max::Real = 4.0
"Lower bound of the speed (the slip-angle dynamics divide by it)"
structural parameter V_min::Real = 5.0
"Upper bound of the speed"
structural parameter V_max::Real = 200.0
"Bound of the steering angle"
structural parameter delta_max::Real = 0.15
"Bound of the throttle and of the brake"
structural parameter thrust_max::Real = 3.0
"Bound of the steering rate"
structural parameter delta_rate_max::Real = 1.0
"Bound of the throttle and brake rates"
structural parameter thrust_rate_max::Real = 10.0
"Share of the adherence of each tyre the controller may use (one for the full limit)"
structural parameter c_max::Real = 1.0
"Quadratic penalty weight on the slack variables of the soft constraints"
structural parameter soft_weight::Real = 1e6
"Record the predicted trajectories and solver residuals of the controller (for mpc_gui)"
structural parameter output_trajectories::Boolean = true
"Lengths of the shooting intervals, growing linearly from Ts to cover Tf (derived)"
final structural parameter time_steps::Real[:] = linear_time_steps(Ts, Np, Tf)
"Nonlinear constrained signals: the polygon facets of the four tyres and the normalized power (derived)"
final structural parameter constrained_signals::String[:] = race_car_constrained_signals()
"Upper bounds of the nonlinear constraints (derived from c_max)"
final structural parameter constrained_max::Real[:] = race_car_constrained_max(c_max)
"Number of segments in the track tables (derived)"
final structural parameter nseg::Integer = racetrack_segment_count()
"Reference for the rate of progress along the centerline, well above what the car can reach"
parameter v_ref::Real = 100.0
"Weight on the progress-rate deficit v_ref - sdot"
structural parameter q_progress::Real = 1.0
"Weight on the lateral offset, resolving the flat direction on the straights"
structural parameter q_n::Real = 0.5
"Weight on the steering rate"
structural parameter r_delta::Real = 100.0
"Weight on the throttle and brake rates"
structural parameter r_thrust::Real = 0.1
"Iteration cap of the SQP at each tick"
structural parameter max_iter::Integer = 5
"Convergence tolerance of the SQP, an absolute residual on the scale of the cost gradients"
parameter tol::Real = 1.0
"Levenberg-Marquardt regularization of the SQP steps"
parameter levenberg_marquardt::Real = 1.0
"Arc length along the centerline"
variable s::Real
"Lateral offset from the centerline"
variable n::Real
"Heading relative to the centerline"
variable alpha::Real
"Speed"
variable V::Real
"Body slip angle"
variable lambda::Real
"Yaw rate"
variable omega::Real
"Steering angle"
variable delta::Real
"Throttle"
variable throttle::Real
"Brake"
variable brake::Real
"Throttle minus brake, the signed thrust of dymos"
variable thrust::Real
"Rate of progress along the centerline"
variable sdot::Real
"Tractive power relative to the engine limit"
variable power_norm::Real
"Friction ellipse of the front-left tyre (one at the limit)"
variable c_fl::Real
"Friction ellipse of the front-right tyre"
variable c_fr::Real
"Friction ellipse of the rear-left tyre"
variable c_rl::Real
"Friction ellipse of the rear-right tyre"
variable c_rr::Real
"acados status"
variable flag::Real
relations
s = plant.s
n = plant.n
alpha = plant.alpha
V = plant.V
lambda = plant.lambda
omega = plant.omega
delta = plant.delta
throttle = plant.throttle
brake = plant.brake
thrust = plant.thrust
sdot = plant.sdot
power_norm = plant.power_norm
c_fl = plant.c_fl
c_fr = plant.c_fr
c_rl = plant.c_rl
c_rr = plant.c_rr
flag = mpc.exitflag
connect(plant.s_out, sx[1].u) {}
connect(plant.n_out, sx[2].u) {}
connect(plant.alpha_out, sx[3].u) {}
connect(plant.V_out, sx[4].u) {}
connect(plant.lambda_out, sx[5].u) {}
connect(plant.omega_out, sx[6].u) {}
connect(plant.ax_out, sx[7].u) {}
connect(plant.ay_out, sx[8].u) {}
connect(plant.delta_out, sx[9].u) {}
connect(plant.throttle_out, sx[10].u) {}
connect(plant.brake_out, sx[11].u) {}
connect(sx[1].y, mpc.x[1], periodicclock.y) {}
for i in 2:11
connect(sx[i].y, mpc.x[i])
end
connect(vref.y, sr.u) {}
connect(sr.y, mpc.r[1]) {}
mpc.r[2] = 0
for i in 1:3
connect(mpc.u[i], zoh[i].u)
end
connect(plant.seg_curvature_out, trackmux.u1) {}
connect(plant.seg_length_out, trackmux.u2) {}
connect(trackmux.y, mpc.p) {}
connect(zoh[1].y, plant.delta_rate) {}
connect(zoh[2].y, plant.throttle_rate) {}
connect(zoh[3].y, plant.brake_rate) {}
metadata {"Dyad": {"doc": {"behavior": false}}}
endTest Cases ​
No test cases defined.
Related ​
Examples
Experiments
Analyses