Modell-prädiktiven Regelung
Kinematic bicycle model
we define a cost function to optimize our path with the trajectory
our cost includes:
- Cross-track error,
- Heading error,
- Speed cost (prefer staying at say 100 miles/hr ),
- Steering cost (prefer zero steering)
- Acceleration cost (prefer zero acceleration)
- Steering rate change (prefer small values)
- Acceleration rate change (prefer small values)
Optimize MPC
We solve the control problem as an optimization problem using a cost function under constraints include those for actuators controlling the gas/brake pedal and the steering wheel.
- We read the next 6 waypoints from the lane detection and compute a 3rd order polynomial to model our trajectory.
- We read the current speed v, heading ψ, steering angle δ, and acceleration a from the censors.
- We compute our first vehicle state using these readings and the dynamic model.
- We want to optimize the actions for the next 1 sec while adjusting steering angle and acceleration every 100 ms. i.e. we have 10 time periods.
- We define an MPC with 2 independent variables: one controls the acceleration (positive for the gas pedal and negative for the brake) and the other for the steering angle.
- We define the allowable range of values (constraints) for acceleration and steering.
- We unroll the dynamic model 9 times, so we can compute the next 9 states.
- We also define the cost from each time period.
- We use an optimizer for MPC to find the best actions for time period 1 to time period 9 that have the minimum total cost under the defined constraints. (Note, in our notation, time period does not start from 0. Instead, we start from 1: period 1 to 10.)
- We will only take the actions suggested in the period 1 and ignore others.
- But, we delay 100 ms before issuing the actions to the simulator. This imitates the real world which commands take time to process and execute.
- We repeat step 1 again to find the next optimized actions.