无人驾驶——模型预测算法MPC

Overall of the MPC

模型预测控制（Model Predictive Control）指一类算法，周期性基于当帧测量信息在线求解一个有限时间开环优化问题，并将结果的前部分控制序列作用于被控对象。根据所用模型不同，分为动态矩阵控制（DMC），模型算法控制（MAC）、广义预测控制（GPC）。在智能驾驶方向，重点在于基于状态空间模型的模型预测控制。

如下图，MPC控制器在智能驾驶的应用

MPC应用

如下为Udacity对于Model、Cost Function、Constraints的Setup

MPC Setup

如何得到最优解？
Udacity给出的方案是基于CppAD的ipopt控制器（第三方库）进行二次规划求解最优解。

Vehicel Models

Kinematic Models

Kinematic models are simplifications of dynamic models that ignore tire forces, gravity, and mass.

Dynamic Models

Dynamic models aim to embody the actual vehicle dynamics as closely as possible. They might encompass tire forces, longitudinal and lateral forces, inertia, gravity, air resistance, drag, mass, and the geometry of the vehicle.

Vehicle Model

各State的解释如下

Explanation of x y psi v

关于CTE（车道偏离）和eψ（横摆角偏离）的解释如下：

Cross Track Error

Orientation Error

More Studying：
Dynamic Model Forces
Tire Slip Angle滑移角: Alpha = arctan(wheel lateral velocity / wheel longitudinal velocity)
Tire Slip Ratio滑移率: s = Radius of Wheel * Angular Velocity / Longitudinal Velocity
Tire Models

Additional resources: Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design presents a comparison between a kinematic and dynamic model.

Cost Function

包括CTE（车道偏离）和eψ（横摆角偏离）等。
我认为实际应用过程中，可以根据规划trajectory符合度和平顺性等评估，包括车速偏离、碰撞cost、纵向加速度、横向加速度、向心加速度等。

Constraints

包括Acceleration加速度和Yaw Rate横摆角加速度的极限约束。

Length and Duration 预测Step数和步长

MPC预测的control inputs [δ,a]仅限于一定范围的时间，包括N = Number of Timesteps 和 dt = Timestep Duration。
期望N越大越好，dt越小越好。

Latency系统执行延迟

In a real car, an actuation command won't execute instantly - there will be a delay as the command propagates through the system. A realistic delay might be on the order of 100 milliseconds.

This is a problem called "latency", and it's a difficult challenge for some controllers - like a PID controller - to overcome. But a Model Predictive Controller can adapt quite well because we can model this latency in the system.

最优解求解器

以上阐述了智驾MPC应用的整体思路和架构。但是到算法层面，如何得到最优解是最核心的。当然对于工程应用，可以选择第三方最优化求解器，当前这是一个非线性规划问题

Ipopt

关于Cppad::ipopt算法核心可见Interior Point OPTimizer 内点法
Ipopt is the tool we'll be using to optimize the control inputs [δ1​,a1​,…,δN−1​,aN−1​]. It's able to find locally optimal values (non-linear problem!) while keeping the constraints set directly to the actuators and the constraints defined by the vehicle model. Ipopt requires we give it the jacobians and hessians directly - it does not compute them for us. Hence, we need to either manually compute them or have a library do this for us. Luckily, there is a library called CppAD which does exactly this.

CppAD

CppAD is a library we'll use for automatic differentiation. By using CppAD we don't have to manually compute derivatives, which is tedious and prone to error.
Given a C++ algorithm that computes function values, CppAD generates an algorithm that computes corresponding derivative values (of arbitrary order using either forward or reverse mode).
我把CppAD理解为基于Ipopt的应用。

More Learning:
Use Ipopt to Solve a Nonlinear Programming Problem
Nonlinear Programming Using CppAD and Ipopt: Example and Test
IPOPT和CppAD简介

参考文献：
1.Udacity无人驾驶课程
2.https://blog.csdn.net/qq_42258099/article/details/95353986