MPC在驾驶控制中的跟车场景应用举例

之前借助highway_env环境尝试了比例控制和PID控制在驾驶中车速控制中的应用。但无论是比例控制还是PID都是较为基础的方法，对于多输入、多输入和多优化目标的控制问题，求解能力较弱。所以要探索新的方法。

MPC简介

模型预测控制（Model Predictive Control），简称MPC。顾名思义，就是在进行优化控制前要先建模、再预测，其中的建模和预测方式都不固定，有很大的自定义发挥空间，也就是说MPC并不是某种优化算法，而是一种优化控制思路的框架。它要求在进行优化控制之前，被控对象的特性能够被一定描述，以此为基础建模进行未来一段时间的状态预测，并求出未来一段时间内的连续最优控制量取值。原理如同梯度下降，搜索范围越大，计算结果越接近全局最优解，但计算开销也越大，or vice versa.

驾驶控制举例

之前写了两篇文章利用PID尝试过两个简单的控制目标（分别是车速、航向），现在设计一个新的控制任务，和之前的任务相比更加复杂：在高速公路上前后分别有两辆车，无论前车速度如何变化，希望后车能够与前车始终保持固定的距离。借助highway_env环境完成实验，环境简介可看前文。

与之前的车速控制相比，汽车的位移控制相当于对车速又进行了一次积分，而且对汽车位移的控制不可能不考虑速度。因此这个任务实际上在位移和速度两方面都要进行控制。设计反馈控制策略就需要考虑两方面的误差。

MPC建模

MPC需要系统提前以一个模型的预测结果为指导，考虑未来一段时间的最优控制量。因此需要提前建立一个机理或数据驱动的模型，在这个例子中的车辆运动学方程较为简单，直接拿已知的模型作为预测模型即可，相当于是一种不存在建模误差的理想状况。在汽车速度控制任务中需要考虑的模型是运动学模型，离散形式描述如下：
$$\{\begin{array}{l}{\dot{v}}_{k+1}=v_k+a_{k+1}\Delta t\\ {\dot{x}}_{k+1}=x_k+v_{k+1}\Delta t\end{array}$$
MPC用该模型将未来$N$个时刻的优化动作生成的预测状态与期望的状态（参考信号）进行对比，可以写出代价函数：
$$\begin{gathered} minJ=\sum _{i=0}^N(w_1\ast ({\bar{x}}_i-x_i{)}^2+w_2\ast u_i^2) \\ \begin{array}{rl}s.t.& a_{min}\le u\le a_{max}\\ & {\bar{x}}_0=X_{init}\\ & u_0=0\\ & x_i=X_{reference}\\ & {\bar{v}}_{i+1}={\bar{v}}_i-u_{i+1}\ast \Delta t\\ & {\bar{x}}_{i+1}={\bar{x}}_i+{\bar{v}}_{i+1}\ast \Delta t\end{array} \end{gathered}$$
其中$u$代表控制量，在highway_env环境中，对车辆的直接控制方式就是设置加速度的值，所以将加速度作为优化控制量，$x$代表前后车的距离，因为控制的是后车，所以当后车加速时，两车的速度差变小。代价函数的第一项是误差项，第二项是正则项，优化目标是在降低两车实际距离与期望距离的误差的同时尽可能减小控制量，二者的trade-off关系用权重系数来调节，保持$w_1+w_2=1$。有了代价函数和约束，就可以使用优化控制算法进行最优求解，典型的数学方法我已在上一篇文章中介绍过，但既然用代码的方式解决数学问题，自然优先考虑现成的工具包，这里我们使用CasADi——一个用于非线性优化和算法微分的开源工具，用pip安装后直接导入python环境即可。

import casadi as ca

代码实现

现在用python语言把这个控制器表示出来：

def mpc_controller(xr,dv,dx,N,dt):
    u = ca.MX.sym('u',N) #控制量，未来N个时刻
    v_= dv
    x_= dx #当前时刻的距离差

    obj=0  #优化函数初始值
    w1=0.7
    w2=1-w1
    weight=[w1,w2]

    #预测
    for step in range(0,N,1):        
        v_=v_-u[step]*dt
        x_=x_+v_*dt  #模型       
        obj=obj+weight[0]*(x_-xr)**2+weight[1]*u[step]**2

    nlp = {'x': u, 'f': obj}

    solver = ca.nlpsol('solver','ipopt', nlp)    
    solution = solver(x0=0,lbx=-5,ubx=5)
    u=solution['x'].full()[0][0]
    return u

将控制器嵌入highway_env交互环境，选择比例控制和PID作为参照组，根据结果可画出曲线图：

具体的程序文件我已上传至资源，欢迎免费下载。