【基础推导】MPC控制器及其车辆模型详细推导 （附代码链接及详细推导说明）

0. 参考与前言

1. Python 代码：github AtsushiSakai/PythonRobotics
2. C++ 代码：github jchengai/gpir/mpc_controller
3. 相关参考博文：
   1. 无人车系统（十一）：轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
   2. PythonRobotics 的文档

来源 参考3.a

PID，pure pursuit方法，Stanley方法都只是利用当前的系统误差来设计控制器。

人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计（或者说利用模型估计未来的运动趋势）。每个控制周期只选择一个目标路点作为跟踪对象，因此，我们也可以说以上控制器只利用了模型进行向前一步预测

那么如果在更远的未来，参考轨迹变化不是那么平缓，并且有很多弯度小（急）的部分，那么只利用一步预测很难对整条轨迹进行有效的跟踪。为了让无人车的控制器更有前瞻性，设计控制器必须得利用模型对未来状态进行多步预测

假设向前预测步长为 T ，那么T步的时空模型要比原始的空间模型要大很多。MPC在每个控制周期都需要重新利用未来T步的模型计算得到当前执行的控制指令。MPC利用了一个比原始空间模型大很多的模型（更高的计算成本）仅仅只得到当前一步的最优控制器（跟PID，pure pursuit, stanley做了同样的事）
————————————————
版权声明：本文为CSDN博主「windSeS」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/u013468614/article/details/103519721

1. 公式推导及讲解

以下为公式车辆状态和线性化等求解过程： 大部分参考于3.b文档，添加了更详细的为什么是这样的公式的碎碎念；主要是感觉用latex打太累了，感觉总是想画一些线之类的，所以就手写了 再也不用担心公式乱码了 hhh；其中@阿信同学参与了常微分方程的推导和一些解释（给我解释了好久 hhh 😄）

2. 代码对应

至此我们已经有了 所有我们需要的部分，接下来对应到代码，以python为例：

1. 求A, B, C

   def get_linear_model_matrix(v, phi, delta):
   
       A = np.zeros((NX, NX))
       A[0, 0] = 1.0
       A[1, 1] = 1.0
       A[2, 2] = 1.0
       A[3, 3] = 1.0
       A[0, 2] = DT * math.cos(phi)
       A[0, 3] = - DT * v * math.sin(phi)
       A[1, 2] = DT * math.sin(phi)
       A[1, 3] = DT * v * math.cos(phi)
       A[3, 2] = DT * math.tan(delta) / WB
   
       B = np.zeros((NX, NU))
       B[2, 0] = DT
       B[3, 1] = DT * v / (WB * math.cos(delta) ** 2)
   
       C = np.zeros(NX)
       C[0] = DT * v * math.sin(phi) * phi
       C[1] = - DT * v * math.cos(phi) * phi
       C[3] = - DT * v * delta / (WB * math.cos(delta) ** 2)
   
       return A, B, C
   

2. minimize的cost形式

   # mpc parameters
   R = np.diag([0.01, 0.01])  # input cost matrix
   Rd = np.diag([0.01, 1.0])  # input difference cost matrix
   Q = np.diag([1.0, 1.0, 0.5, 0.5])  # state cost matrix
   Qf = Q  # state final matrix
   T = 5  # horizon length
   for t in range(T):
       cost += cvxpy.quad_form(u[:, t], R) # 对应 R u_t^2 第三项
   
       if t != 0:
           cost += cvxpy.quad_form(xref[:, t] - x[:, t], Q) # 对应 第二项
   
       A, B, C = get_linear_model_matrix(
           xbar[2, t], xbar[3, t], dref[0, t])
       constraints += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C] #车辆模型约束
   
       if t < (T - 1):
           cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd) # 对应 第四项
           constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <=
                           MAX_DSTEER * DT] #最大最小输入
   
   cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf) # 对应第一项
   

   所以一开始会磕出一个水平的长度，也就是mpc t:T考虑的部分

3. 物理约束，详情见上面推导的第一幅图右边有写约束

   constraints += [x[:, 0] == x0]
   constraints += [x[2, :] <= MAX_SPEED]
   constraints += [x[2, :] >= MIN_SPEED]
   constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL]
   constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]
   

4. 求解，使用python cvxpy库进行优化求解，优化对象是刚刚写的一系列cost，然后添加了各个约束

   prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
   prob.solve(solver=cvxpy.ECOS, verbose=False)
   
   if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
       ox = get_nparray_from_matrix(x.value[0, :])
       oy = get_nparray_from_matrix(x.value[1, :])
       ov = get_nparray_from_matrix(x.value[2, :])
       oyaw = get_nparray_from_matrix(x.value[3, :])
       oa = get_nparray_from_matrix(u.value[0, :])
       odelta = get_nparray_from_matrix(u.value[1, :])
   
   else:
       print("Error: Cannot solve mpc..")
       oa, odelta, ox, oy, oyaw, ov = None, None, None, None, None, None
   
   return oa, odelta, ox, oy, oyaw, ov
   

完整代码可见参考 运行后类似于下面的gif图：https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py

https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif

C++代码 则是基于ROS和CARLA实验的跟随，整体是高斯过程的路径规划问题求解等，但是因为jg实现了mpc在里面，所以也可以参考直接用：

https://github.com/jchengai/gpir/blob/main/planning_core/simulation/controller/mpc_controller.cc

3. Optional: CARLA实现

接下来则是 python下的CARLA实现，因为CARLA全部都是PID跟随器，想来写一个MPC的试一试，不过上面给出的已经能 让大家自己心里有数 整个过程步骤是怎样的了，下面仅实验实现

TODO

---

赠人点赞 手有余香 😆；正向回馈 才能更好开放记录 hhh