参考资料

• bilibili的DR_CAN讲解的MPC模型预测控制器
• 知乎上一个比较通俗易懂的解释
• 模型预测控制
• 轨迹跟踪模型预测控制(MPC)原理与python实现
• DR_CAN笔记MPC
• MPC控制笔记

1. 基本概念

• 模型预测控制（MPC）的核心思想就是以优化方法求解最优控制器，其中优化方法大多时候采用二次规划（Quadratic Programming）。

• MPC控制器优化得到的控制输出也是系统在未来有限时间步的控制序列。 当然，由于理论构建的模型与系统真实模型都有误差，所以，实际上更远未来的控制输出对系统控制的价值很低，故MPC仅执行输出序列中的第一个控制输出。

模型(Model)

分为机理模型和基于数据的模型（例如用神经网络训练的一个model）使用基于数据的模型的MPC可以结合model based RL使用。

预测(Predict)

模型就是用来预测的，预测的目的是为了更好的决策

控制(Control)

控制即决策，根据预测来作出决策。

MPC利用一个已有的模型、系统当前的状态和未来的控制量，来预测系统未来的输出，然后与我们期望的系统输出做比较，得到一个损失函数（代价函数），即：

损失函数=(未来输出(模型，当前状态，未来控制量)−期望输出)2损失函数 = (未来输出(模型，当前状态，未来控制量)-期望输出)^2 损失函数=(未来输出(模型，当前状态，未来控制量)−期望输出)2

由于上式中模型、当前状态、期望输出都是已知的，因此只有未来控制量一个自变量。采用二次规划的方法求解出某个未来控制量，使得损失函数最小，前面提到，这个未来控制量的第一个元素就是当前控制周期的控制量。

1.1 MPC vs optimal control

最优控制（optimal control）指的是在一定的约束情况下达到最优状态的系统表现，其中约束情况通常是实际环境所带来的限制，例如汽车中的油门不能无限大等。

最优控制强调的是“最优”，一般最优控制需要在整个时间域上进行求优化(从0时刻到正无穷时刻的积分)，这样才能保证最优性，这是一种很贪婪的行为，需要消耗大量算力。同时，系统如果是一个时变系统，或者面临扰动的话，前一时刻得到的最优并不一定是下一时刻的最优值。

J=∫0∞ETQE+UTRUdtJ=\int_{0}^{\infty} E^{T} Q E+U^{T} R U d t J=∫0∞​ETQE+UTRUdt

最优控制常用解法有： 变分法，极大值原理，动态规划，LQR（LQR可以参考博客）。

MPC仅考虑未来几个时间步，一定程度上牺牲了最优性。

1.2 MPC优点

• MPC善于处理多输入多输出系统（MIMO）；

• MPC可以处理约束，如安全性约束，上下阈值；

• MPC是一种向前考虑未来时间步的有限时域优化方法（一定的预测能力）

  最优控制要求在整个时间优化

  实际上MPC采用了一个折中的策略，既不是像最优控制那样考虑整个时域，也不是仅仅考虑当前，而是考虑未来的有限时间域。

2. MPC整体流程

2.1 预测区间与控制区间

对于一般的离散化系统（因为实际计算机实现的控制系统都是离散的系统，连续系统可以进行离散化操作），在k时刻，我们可以测量出系统的当前状态y(k)y(k)y(k)，再通过计算得到的u(k),u(k+1),u(k+2)...u(k+j)u(k),u(k+1),u(k+2)...u(k+j)u(k),u(k+1),u(k+2)...u(k+j)得到系统未来状态的估计值y(k+1),y(k+2)...y(k+j)y(k+1),y(k+2)...y(k+j)y(k+1),y(k+2)...y(k+j)；

将预测状态估计的部分称为预测区间（Predictive Horizon）,指的是一次优化后预测未来输出的时间步的个数。

将控制估计的部分称为控制区间（Control Horizon），在得到最优输入之后，我们只施加当前时刻的输入u(k)，即控制区间的第一位控制输入。

如下图 [k,k+m][k, k+m][k,k+m]范围为控制区间，之后的红色部分称为 held constant，其中控制区间是要通过优化器来进行优化的参数。

过小的控制区间，可能无法做到较好的控制，而较大的控制区间，比如与预测区间相等，则会导致只有前一部分的控制范围才会有较好的效果，而后一部分的控制范围则收效甚微，而且将带来大量的计算开销。

2.2 约束

对于约束，一般分为Hard约束和Soft约束，Hard约束是不可违背必须遵守的，在控制系统中，输入输出都可能会有约束限制，但是在设计时不建议将输入输出都给予Hard约束，因为这两部的约束有可能是有重叠的，导致优化器会产生不可行解。

Hard约束不能违反，Soft约束可以；比如Hard约束是刹车踩的幅度；Soft约束是速度

建议输出采用Soft约束，而输入的话建议输入和输入参数变化率二者之间不要同时为Hard约束，可以一个Hard一个Soft。

2.3 MPC流程

模型预测控制在k时刻共需三步；

• 第一步：获取系统的当前状态；

• 第二步：基于u(k),u(k+1),u(k+2)...u(k+m)u(k),u(k+1),u(k+2)...u(k+m)u(k),u(k+1),u(k+2)...u(k+m)进行最优化处理；

  离散系统的代价函数可以参考

  J=∑km−1EkTQEk+ukTRuk+ENTFENJ=\sum_{k}^{m-1}E_k^TQE_k+u_k^TRu_k+E_N^TFE_NJ=k∑m−1​EkT​QEk​+ukT​Ruk​+ENT​FEN​

  其中ENE_NEN​表示误差的终值，也是衡量优劣的一种标准。

• 第三步：只取u(k)u(k)u(k)作为控制输入施加在系统上。

在下一时刻重复以上三步，在下一步进行预测时使用的就是下一步的状态值，我们将这样的方案称为滚动优化控制（Receding Horizon Control）。

预测控制的优化不是一次离线进行，而是随着采样时刻的前进反复地在线进行，故称为滚动优化。这种滚动优化虽然得不到理想的全局最优解，但是反复对每一采样时刻的偏差进行优化计算，将可及时地校正控制过程中出现的各种复杂情况。

2.4 MPC vs. LQR

从以下几个方面进行阐述：

• 研究对象：是否线性化
• 状态方程：是否离散化
• 目标函数：误差和控制量的极小值
• 工作时域：预测时域，控制时域，滚动优化，求解一次
• 求解方法：QP求解器，变分法求解黎卡提方程
• LQR和MPC的优缺点：滚动优化，求解时域，实时性，算力，工程常用方法

具体可参考博客

3. MPC设计

当模型是线性的时候（非线性系统可以线性化），MPC的设计求解一般使用二次规划方法。

设线性模型为以下形式：
xk+1=Axk+Buk+C(1)x_{k+1}=Ax_k+Bu_k+C \tag{1} xk+1​=Axk​+Buk​+C(1)

假定未来 mmm步的控制输入已知，为 uk,uk+1,uk+2,...,uk+mu_k, u_{k+1}, u_{k+2}, ..., u_{k+m}uk​,uk+1​,uk+2​,...,uk+m​​，根据以上模型与输入，我们可以计算未来 mmm步的状态：

xk+1=Axk+Buk+Cxk+2=Axk+1+Buk+1+C=A(Axk+Buk+C)+Buk+1+C=A2xk+ABuk+Buk+1+AC+Cxk+3=A3xk+A2Buk+ABk+1+Buk+2+A2C+AC+C...xk+m=Amxk+Am−1Buk+Am−2Buk+1+...+Am−iBuk+i−1+...+Buk+m−1+Am−1C+Am−2C+...+C\begin{aligned} x_{k+1}&=Ax_k+Bu_k+C \\ x_{k+2}&=Ax_{k+1}+Bu_{k+1}+C=A(Ax_k+Bu_k+C)+Bu_{k+1}+C=A^2x_{k}+ABu_k+Bu_{k+1}+AC+C \\ x_{k+3}&=A^3x_k+A^2Bu_{k}+AB_{k+1}+Bu_{k+2}+A^2C+AC+C\\ ...\\ x_{k+m}&=A^{m}x_{k}+A^{m-1}Bu_k+A^{m-2}Bu_{k+1}+...+A^{m-i}Bu_{k+i-1}+...+Bu_{k+m-1}+A^{m-1}C+A^{m-2}C+...+C \end{aligned} xk+1​xk+2​xk+3​...xk+m​​=Axk​+Buk​+C=Axk+1​+Buk+1​+C=A(Axk​+Buk​+C)+Buk+1​+C=A2xk​+ABuk​+Buk+1​+AC+C=A3xk​+A2Buk​+ABk+1​+Buk+2​+A2C+AC+C=Amxk​+Am−1Buk​+Am−2Buk+1​+...+Am−iBuk+i−1​+...+Buk+m−1​+Am−1C+Am−2C+...+C​

将上面mmm步写成矩阵向量形式：

X=Axk+Bu+C(2)\mathcal{X}=\mathcal{A}x_k+\mathcal{B}\mathbf{u}+\mathcal{C} \tag{2} X=Axk​+Bu+C(2)

其中，
X=[xk+1,xk+2,xk+3,...xk+m]T\mathcal{X}=\left[x_{k+1}, x_{k+2}, x_{k+3},...x_{k+m}\right]^TX=[xk+1​,xk+2​,xk+3​,...xk+m​]T
u=[uk,uk+1,uk+2,...,uk+m−1]T\mathbf{u}=\left[u_k,u_{k+1},u_{k+2},...,u_{k+m-1}\right]^Tu=[uk​,uk+1​,uk+2​,...,uk+m−1​]T
A=[A,A2,A3,...,Am]T\mathcal{A}=\left[A, A^2 ,A^3 ,... ,A^m\right]^TA=[A,A2,A3,...,Am]T

B=(00...0B0...0ABB...0............Am−1BAm−2B...B)\mathcal{B}=\begin{pmatrix}0&0&...&0\\ B&0&...&0\\ AB&B&...&0\\ ...&...&...&...\\ A^{m-1}B&A^{m-2}B&...&B\end{pmatrix}B=⎝⎛​0BAB...Am−1B​00B...Am−2B​...............​000...B​⎠⎞​

C=[CAC+CA2C+AC+C…Ak+m−1C+…+C]\mathcal{C}=\left[\begin{array}{c} C \\ A C+C \\ A^{2} C+A C+C \\ \ldots \\ A^{k+m-1} C+\ldots+C \end{array}\right] C=⎣⎡​CAC+CA2C+AC+C…Ak+m−1C+…+C​⎦⎤​

上式B\mathcal{B}B中的下三角形式，直接反映了系统在时间上的因果关系，即k+1k+1k+1时刻的输入对kkk 时刻的输出没有影响， k+2k+2k+2 时刻的输入对 kkk 和 k+1k+1k+1 时刻没有影响。

假定参考轨迹为 X‾=[xˉk+1xˉk+2xˉk+3…xˉk+m]T\overline{\mathcal{X}}=\left[\begin{array}{lllll}\bar{x}_{k+1} & \bar{x}_{k+2} & \bar{x}_{k+3} & \ldots & \bar{x}_{k+m}\end{array}\right]^{T}X=[xˉk+1​​xˉk+2​​xˉk+3​​…​xˉk+m​​]T,则MPC的一个简单的目标代价函数如下：
min⁡J=ETQE+uTRus.t. umin≤u≤umax(3)\min \mathcal{J}=\mathcal{E}^T Q \mathcal{E}+\mathbf{u}^T R \mathbf{u} \\ \text{s.t. } u_{min}\leq \mathbf{u}\leq u_{max} \tag{3} minJ=ETQE+uTRus.t. umin​≤u≤umax​(3)

其中， E=X−X‾=[xk+1−xˉk+1xk+2−xˉk+2…xk+m−xˉk+m]T\mathcal{E}=\mathcal{X}-\overline{\mathcal{X}}=\left[\begin{array}{llll}x_{k+1}-\bar{x}_{k+1} & x_{k+2}-\bar{x}_{k+2} & \ldots & x_{k+m}-\bar{x}_{k+m}\end{array}\right]^{T}E=X−X=[xk+1​−xˉk+1​​xk+2​−xˉk+2​​…​xk+m​−xˉk+m​​]T

uTRu\mathbf{u}^T R \mathbf{u}uTRu这一项是为了让控制输入不会太大，因此代价函数中添加了一项对控制量的约束。

将式(2)代入式(3)，则优化变量仅剩 u\mathbf{u}u。以上最优化问题可用二次规划方法求解，得到满足目标代价函数的最优控制序列 u={uk,​​uk+1,​​uk+2​​...​uk+m−1}\mathbf{u}=\left\{u_k,​​u_{k+1},​​u_{k+2}​​...​u_{k+m−1}\right\}u={uk​,​​uk+1​,​​uk+2​​​...​uk+m−1​}。

当转换成式（3）后，可以利用凸优化库进行二次规划求解，例如python的cvxopt，OSQP: An Operator Splitting Solver for Quadratic Programs，Casdi等。

4. MPC应用——无人车轨迹跟踪

4.1 MPC建模

设车辆的状态量偏差和控制量偏差如式 ( 4 ) 所示：
x~=[x˙−x˙ry˙−y˙rφ˙−φ˙r],u~=[v−vrδ−δr](4)\tag{4} \tilde{\boldsymbol{x}}=\left[\begin{array}{c} \dot{x}-\dot{x}_{r} \\ \dot{y}-\dot{y}_{r} \\ \dot{\varphi}-\dot{\varphi}_{r} \end{array}\right], \tilde{\boldsymbol{u}}=\left[\begin{array}{c} v-v_{r} \\ \delta-\delta_{r} \end{array}\right] x~=⎣⎡​x˙−x˙r​y˙​−y˙​r​φ˙​−φ˙​r​​⎦⎤​,u~=[v−vr​δ−δr​​](4)
基于先前的运动学模型的离散状态空间方程如下，
x~(k+1)=[10−Tvrsin⁡φr01Tvrcos⁡φr001]x~(k)+[Tcos⁡φr0Tsin⁡φr0Ttan⁡δrlTvrlcos⁡2δr]u~(k)=Ax~(k)+Bu~(k)(5)\tag{5} \tilde{\boldsymbol{x}}(k+1)=\left[\begin{array}{ccc} 1 & 0 & -T v_{r} \sin \varphi_{r} \\ 0 & 1 & T v_{r} \cos \varphi_{r} \\ 0 & 0 & 1 \end{array}\right] \tilde{\boldsymbol{x}}(k)+\left[\begin{array}{cc} T \cos \varphi_{r} & 0 \\ T \sin \varphi_{r} & 0 \\ T \frac{\tan \delta_{r}}{l} & T \frac{v_{r}}{l \cos ^{2} \delta_{r}} \end{array}\right] \tilde{\boldsymbol{u}}(k)=\boldsymbol{A} \tilde{\boldsymbol{x}}(k)+\boldsymbol{B} \tilde{\boldsymbol{u}}(k) x~(k+1)=⎣⎡​100​010​−Tvr​sinφr​Tvr​cosφr​1​⎦⎤​x~(k)+⎣⎡​Tcosφr​Tsinφr​Tltanδr​​​00Tlcos2δr​vr​​​⎦⎤​u~(k)=Ax~(k)+Bu~(k)(5)

为了表示控制系统达到稳定控制所付出的代价，MPC控制的代价函数定义如下：
min⁡J(U)=∑k=0N−1(x~(k)TQx~(k)+u~(k)TRu~(k))+x~(N)TQfx~(N)(6)\tag{6} \min J(\boldsymbol{U})=\sum_{k=0}^{N-1}\left(\tilde{\boldsymbol{x}}(k)^{T} Q \tilde{\boldsymbol{x}}(k)+\tilde{\boldsymbol{u}}(k)^{T} R \tilde{\boldsymbol{u}}(k)\right)+\tilde{\boldsymbol{x}}(N)^{T} Q_{f} \tilde{\boldsymbol{x}}(N) minJ(U)=k=0∑N−1​(x~(k)TQx~(k)+u~(k)TRu~(k))+x~(N)TQf​x~(N)(6)
其中函数参数 U=(u0,u1,…,uN)U=\left(u_{0}, u_{1}, \ldots, u_{N}\right)U=(u0​,u1​,…,uN​) ，并且矩阵 Q,Qf,RQ, Q_{f}, RQ,Qf​,R 为正定矩阵，即
Q=QT≥0,Qf=QfT≥0,R=RT>0Q=Q^{T} \geq 0, \quad Q_{f}=Q_{f}^{T} \geq 0, \quad R=R^{T}>0 Q=QT≥0,Qf​=QfT​≥0,R=RT>0

Q	Q_(f)	R
给定状态代价矩阵	最终状态代价矩阵	输入代价矩阵

• NNN : 时间范围(Time Horizon)
• Q，RQ ， RQ，R : 分别设定状态偏差和输入的相对权重
• R>0R>0R>0 : 意味着任何非零输入都增加 JJJ 的代价
• x~(k)TQx~(k)\tilde{\boldsymbol{x}}(k)^{T} Q \tilde{\boldsymbol{x}}(k)x~(k)TQx~(k) : 衡量状态偏差
• u~(k)TRu~(k)\tilde{\boldsymbol{u}}(k)^{T} R \tilde{\boldsymbol{u}}(k)u~(k)TRu~(k) : 衡量输入大小
• x~(N)TQfx~(N)\tilde{\boldsymbol{x}}(N)^{T} Q_{f} \tilde{\boldsymbol{x}}(N)x~(N)TQf​x~(N) : 衡量最终状态偏差

对于公式(6)，它需要服从的约束条件包括
{运动学模型约束——x~(k+1)=Ax~(k)+Bu~(k)控制量约束——∣u~(k)∣≤u~max初始状态——x~(0)=x~0(7)\tag{7} \left\{ \begin{aligned} &\text{运动学模型约束——}&\tilde{\boldsymbol{x}}(k+1)=\boldsymbol{A} \tilde{\boldsymbol{x}}(k)+\boldsymbol{B} \tilde{\boldsymbol{u}}(k)\\ &\text{控制量约束——}&\left|\tilde{\boldsymbol{u}}(k)\right| \leq \tilde{\boldsymbol{u}}_{max}\\ &\text{初始状态——}&\tilde{\boldsymbol{x}}(0)=\tilde{\boldsymbol{x}}_0 \end{aligned} \right. ⎩⎨⎧​​运动学模型约束——控制量约束——初始状态——​x~(k+1)=Ax~(k)+Bu~(k)∣u~(k)∣≤u~max​x~(0)=x~0​​(7)

4.2 python代码实现

完整程序见GitHub仓库

4.2.1 参数

# mpc parameters
NX = 3  # x = x, y, yaw
NU = 2  # u = [v,delta]
T = 8  # horizon length
R = np.diag([0.1, 0.1])  # input cost matrix
Rd = np.diag([0.1, 0.1])  # input difference cost matrix
Q = np.diag([1, 1, 1])  # state cost matrix
Qf = Q  # state final matrix#车辆
dt=0.1 # 时间间隔，单位：s
L=2 # 车辆轴距，单位：m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-3 #初始y
psi_0=0 # 初始航向角MAX_STEER = np.deg2rad(45.0)  # maximum steering angle [rad]
MAX_DSTEER = np.deg2rad(45.0)  # maximum steering speed [rad/s]MAX_VEL = 2.0  # maximum accel [m/s]

4.2.2 运动学模型

import mathclass KinematicModel_3:"""假设控制量为转向角delta_f和加速度a"""def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = L# 实现是离散的模型self.dt = dtdef update_state(self, a, delta_f):self.x = self.x+self.v*math.cos(self.psi)*self.dtself.y = self.y+self.v*math.sin(self.psi)*self.dtself.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dtself.v = self.v+a*self.dtdef get_state(self):return self.x, self.y, self.psi, self.vdef state_space(self, ref_delta, ref_yaw):"""将模型离散化后的状态空间表达Args:ref_delta (_type_): 参考的转角控制量ref_yaw (_type_): 参考的偏航角Returns:_type_: _description_"""A = np.matrix([[1.0, 0.0, -self.v*self.dt*math.sin(ref_yaw)],[0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],[0.0, 0.0, 1.0]])B = np.matrix([[self.dt*math.cos(ref_yaw), 0],[self.dt*math.sin(ref_yaw), 0],[self.dt*math.tan(ref_delta)/self.L, self.v*self.dt /(self.L*math.cos(ref_delta)*math.cos(ref_delta))]])C = np.eye(3)return A, B, C

4.2.3 参考轨迹

class MyReferencePath:def __init__(self):# set reference trajectory# refer_path包括4维：位置x, 位置y， 轨迹点的切线方向, 曲率k self.refer_path = np.zeros((1000, 4))self.refer_path[:,0] = np.linspace(0, 100, 1000) # xself.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y# 使用差分的方式计算路径点的一阶导和二阶导，从而得到切线方向和曲率for i in range(len(self.refer_path)):if i == 0:dx = self.refer_path[i+1,0] - self.refer_path[i,0]dy = self.refer_path[i+1,1] - self.refer_path[i,1]ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0]ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1]elif i == (len(self.refer_path)-1):dx = self.refer_path[i,0] - self.refer_path[i-1,0]dy = self.refer_path[i,1] - self.refer_path[i-1,1]ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0]ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1]else:      dx = self.refer_path[i+1,0] - self.refer_path[i,0]dy = self.refer_path[i+1,1] - self.refer_path[i,1]ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0]ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1]self.refer_path[i,2]=math.atan2(dy,dx) # yaw# 计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).# 参考：https://blog.csdn.net/weixin_46627433/article/details/123403726self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k计算def calc_track_error(self, x, y):"""计算跟踪误差Args:x (_type_): 当前车辆的位置xy (_type_): 当前车辆的位置yReturns:_type_: _description_"""# 寻找参考轨迹最近目标点d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))] d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))] d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))]s = np.argmin(d) # 最近目标点索引yaw = self.refer_path[s, 2]k = self.refer_path[s, 3]angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))e = d[s]  # 误差if angle < 0:e *= -1return e, k, yaw, sdef calc_ref_trajectory(self, robot_state, dl=1.0):"""计算参考轨迹点，统一化变量数组，便于后面MPC优化使用参考自https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.pyArgs:robot_state (_type_): 车辆的状态(x,y,yaw,v)dl (float, optional): _description_. Defaults to 1.0.Returns:_type_: _description_"""e, k, ref_yaw, ind = self.calc_track_error(robot_state[0], robot_state[1])xref = np.zeros((NX, T + 1))dref = np.zeros((NU, T)) # 参考控制量ncourse = len(self.refer_path)xref[0, 0] = self.refer_path[ind,0]xref[1, 0] = self.refer_path[ind, 1]xref[2, 0] = self.refer_path[ind, 2]# 参考控制量[v,delta]ref_delta = math.atan2(L*k, 1)dref[0, :] = robot_state[3]dref[1, :] = ref_deltatravel = 0.0for i in range(T + 1):travel += abs(robot_state[3]) * dtdind = int(round(travel / dl))if (ind + dind) < ncourse:xref[0, i] = self.refer_path[ind + dind,0]xref[1, i] = self.refer_path[ind + dind,1]xref[2, i] = self.refer_path[ind + dind,2]else:xref[0, i] = self.refer_path[ncourse - 1,0]xref[1, i] = self.refer_path[ncourse - 1,1]xref[2, i] = self.refer_path[ncourse - 1,2]return xref, ind, dref

4.2.4 矩阵拍平

def get_nparray_from_matrix(x):return np.array(x).flatten()

4.2.5 角度归一化到[-pi，pi]

def normalize_angle(angle):"""Normalize an angle to [-pi, pi].:param angle: (float):return: (float) Angle in radian in [-pi, pi]copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html"""while angle > np.pi:angle -= 2.0 * np.piwhile angle < -np.pi:angle += 2.0 * np.pireturn angle

4.2.6 MPC控制实现

def linear_mpc_control(xref, x0, delta_ref,ugv):"""linear mpc controlxref: reference pointx0: initial statedelta_ref: reference steer angleugv:车辆对象"""x = cvxpy.Variable((NX, T + 1))u = cvxpy.Variable((NU, T)) cost = 0.0  # 代价函数constraints = []  # 约束条件for t in range(T):cost += cvxpy.quad_form(u[:, t]-delta_ref[:,t], R)if t != 0:cost += cvxpy.quad_form(x[:, t] - xref[:, t], Q)A, B, C = ugv.state_space(delta_ref[1,t], xref[2,t])constraints += [x[:, t + 1]-xref[:, t+1] == A @ (x[:, t]-xref[:, t]) + B @ (u[:, t]-delta_ref[:,t]) ]cost += cvxpy.quad_form(x[:, T] - xref[:, T], Qf)constraints += [(x[:, 0]) == x0]constraints += [cvxpy.abs(u[0, :]) <= MAX_VEL]constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]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:opt_x = get_nparray_from_matrix(x.value[0, :])opt_y = get_nparray_from_matrix(x.value[1, :])opt_yaw = get_nparray_from_matrix(x.value[2, :])opt_v = get_nparray_from_matrix(u.value[0, :])opt_delta = get_nparray_from_matrix(u.value[1, :])else:print("Error: Cannot solve mpc..")opt_v, opt_delta, opt_x, opt_y, opt_yaw = None, None, None, None, None, return opt_v, opt_delta, opt_x, opt_y, opt_yaw

4.2.7 主函数

from celluloid import Camera # 保存动图时用，pip install celluloid
# 使用随便生成的轨迹
def main():reference_path = MyReferencePath()goal = reference_path.refer_path[-1,0:2]# 运动学模型ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)x_ = []y_ = []fig = plt.figure(1)# 保存动图用camera = Camera(fig)# plt.ylim([-3,3])for i in range(500):robot_state = np.zeros(4)robot_state[0] = ugv.xrobot_state[1] = ugv.yrobot_state[2]=ugv.psirobot_state[3]=ugv.vx0 = robot_state[0:3]xref, target_ind, dref = reference_path.calc_ref_trajectory(robot_state)opt_v, opt_delta, opt_x, opt_y, opt_yaw = linear_mpc_control(xref, x0, dref, ugv)ugv.update_state(0, opt_delta[0])  # 加速度设为0，恒速x_.append(ugv.x)y_.append(ugv.y)# 显示动图plt.cla()plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], "-.b",  linewidth=1.0, label="course")plt.plot(x_, y_, "-r", label="trajectory")plt.plot(reference_path.refer_path[target_ind,0], reference_path.refer_path[target_ind,1], "go", label="target")# plt.axis("equal")plt.grid(True)plt.pause(0.001)# camera.snap()# 判断是否到达最后一个点if np.linalg.norm(robot_state[0:2]-goal)<=0.1:print("reach goal")break# animation = camera.animate()# animation.save('trajectory.gif')if __name__=='__main__':main()

跟踪效果如下：

（跟踪效果不是很好，我并没有进一步调整，就先这样吧···。）

5. MPC开源库/程序

• do-mpc
• mpc.pytorch

【自动驾驶】模型预测控制(MPC)实现轨迹跟踪相关推荐

1. 自动驾驶——模型预测控制（MPC）理解与实践

   当时在做路径跟踪.路径规划时,使用了MPC,通过项目的应用,对于MPC建立了一定的认识,但是一段时间过去后,认知又渐渐模糊了,当时学习过程中也是看了许多人的blog及代码才弄清楚,这里试图从理论到实践 ...

2. 【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制

   如果对Carsim的基础使用还不了解,可以参考:[Carsim Simulink自动驾驶仿真]基于MPC的速度控制 如果对MPC算法原理不清楚,可以参考:如何理解MPC模型预测控制理论 项目介绍: 教 ...

3. 无人车系统（十一）：轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】

   前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...

4. 基于扩展卡尔曼滤波EKF和模型预测控制MPC，自动泊车场景建模开发

   基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB(工程项目线上支持 ...

5. 模型预测控制_模型预测控制(MPC)算法之一MAC算法

   引言 随着自动驾驶技术以及机器人控制技术的不断发展及逐渐火热,模型预测控制(MPC)算法作为一种先进的控制算法,其应用范围与领域得到了进一步拓展与延伸.目前提出的模型预测控制算法主要有基于非参数模型的 ...

6. 【附C++源代码】模型预测控制(MPC)公式推导以及算法实现，Model Predictive control介绍

   2022年的第一篇博客,首先祝大家新年快乐! 提示:本篇博客主要集中在对MPC的理解以及应用.这篇博客可以作为你对MPC控制器深入研究的一个开始,起到抛砖引玉,带你快速了解其原理的作用. 这篇博客将介 ...

7. 地铁自动驾驶模型，地铁列车牵引系统整车模型。

   完整的列车模型包括目标曲线的优化,控制层进行跟随目标曲线,经神经网络控制和模糊控制给出推荐牵引力,牵引传动系统控制牵引电机输出牵引转矩,输入列车自动驾驶模型进行牵引计算.其中生成优化曲线通过遗传算法进 ...

8. 【控制control】机器人运动控制器----基于模型预测控制MPC方法

   系列文章目录 提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理 文章目录 系列文章目录 前言 一.模型预测控制(MPC)的介绍及构成 1.介绍 2.构成 二.模型 ...

9. Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码

   Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码

10. 基于模型预测控制(MPC)的悬架系统仿真分析

    目录 前言 1.悬架系统 2.基于MPC的悬架系统仿真分析 2.1 simulink模型 2.2仿真结果 2.2.1 随机C级路面 2.2.2 正弦路面 2.3 结论 3 总结 前言 模型预测控制是无 ...

最新文章

1. LazyT 延迟加载
2. Autism Course of Yale University Fred Volkman 2
3. shell学习之常用命令总结
4. Perl 安装 模块Net::SSH2 有些0day是ssh2
5. liferay mysql driver_Liferay更改数据库（oracle,mysql）
6. WCF服务寄宿IIS时.SVC文件无法浏览的解决办法
7. 方法参数的值调用+引用调用+深浅拷贝
8. ROS入门_1.10 理解ROS服务和参数
9. 使用IntelliJ IDEA导入 Flink 消费kafka报错 Error: A JNI error has occurred, please check your installation an
10. MySQL 基础————常用数据类型
11. nginx 没有cookie_Nginx入门学习（1）：一些概念
12. 在什么情况下自己应该辞职走人了，而不是等领导下“逐客令”？
13. 华为鸿蒙os升级怎么升,华为鸿蒙OS2.0系统怎么升级
14. mysql更改密码_Mysql更改密码、连接mysql、mysql常用命令
15. SpringBoot如何实现自动配置
16. 自定义list-style的兼容性问题解决办法
17. HCIE大师之路（一）——VRRP+DHCP+RSTP综合实验
18. 总结AUTOCAD快捷键，持续更新~
19. 阿里C++工程师面试小记
20. python校验用户登录信息

热门文章

1. [企业管理]邮政系统，真得悲哀呀！居然歪曲理由掩盖事实
2. 一文掌握有序logistic回归分析
3. 《游戏学习》| html5 飞翔的小鸟 小游戏代码示例
4. 三对角矩阵（特殊矩阵）
5. 单点登录sso原理及代码实现
6. 谈谈创业这点事(8)
7. K3 CLOUD API接口说明书V2.0
8. 同轴馈电微带天线(一)
9. 运维 之 常用运维工具
10. 医院设备管理系统方案/案列/软件/APP/小程序/网站