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

文章目录

参考资料
1. 基本概念
- 1.1 MPC vs optimal control
- 1.2 MPC优点
2. MPC整体流程
- 2.1 预测区间与控制区间
- 2.2 约束
- 2.3 MPC流程
- 2.4 MPC vs. LQR
3. MPC设计
4. MPC应用——无人车轨迹跟踪
- 4.1 MPC建模
- 4.2 python代码实现
- - 4.2.1 参数
  - 4.2.2 运动学模型
  - 4.2.3 参考轨迹
  - 4.2.4 矩阵拍平
  - 4.2.5 角度归一化到[-pi，pi]
  - 4.2.6 MPC控制实现
  - 4.2.7 主函数
5. MPC开源库/程序

参考资料

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$

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

1.1 MPC vs optimal control

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

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

$TRUdtJ=\int_{0}^{\infty} E^{T} Q E+U^{T} R U d t$

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

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

1.2 MPC优点

MPC善于处理多输入多输出系统（MIMO）；
MPC可以处理约束，如安全性约束，上下阈值；
MPC是一种向前考虑未来时间步的有限时域优化方法（一定的预测能力）
最优控制要求在整个时间优化
实际上MPC采用了一个折中的策略，既不是像最优控制那样考虑整个时域，也不是仅仅考虑当前，而是考虑未来的有限时间域。

2. MPC整体流程

2.1 预测区间与控制区间

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

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

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

如下图 $[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) \dots u (k + m)$ 进行最优化处理；
离散系统的代价函数可以参考
$NJ=\sum_{k}^{m-1}E_k^TQE_k+u_k^TRu_k+E_N^TFE_N$
其中 $E_N$ 表示误差的终值，也是衡量优劣的一种标准。
第三步：只取 $u (k)$ 作为控制输入施加在系统上。

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

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

2.4 MPC vs. LQR

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

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

具体可参考博客

3. MPC设计

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

设线性模型为以下形式：
$x_{k+1}=Ax_k+Bu_k+C \tag{1}$

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

$C+…+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}$

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

$\mathcal{X}=\mathcal{A}x_k+\mathcal{B}\mathbf{u}+\mathcal{C} \tag{2}$

其中，
$]T\mathcal{X}=\left[x_{k+1}, x_{k+2}, x_{k+3},…x_{k+m}\right]^T$
$]T\mathbf{u}=\left[u_k,u_{k+1},u_{k+2},…,u_{k+m-1}\right]^T$
$m]T\mathcal{A}=\left[A, A^2 ,A^3 ,… ,A^m\right]^T$

$)\mathcal{B}=\begin{pmatrix}0&0&…&0\\ B&0&…&0\\ AB&B&…&0\\ …&…&…&…\\ A^{m-1}B&A^{m-2}B&…&B\end{pmatrix}$

$C+…+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]$

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

假定参考轨迹为 $\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}$ ,则MPC的一个简单的目标代价函数如下：
$\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}$

其中， $\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}$

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

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

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

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

4.1 MPC建模

设车辆的状态量偏差和控制量偏差如式 ( 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]$
基于先前的运动学模型的离散状态空间方程如下，
$\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)$

为了表示控制系统达到稳定控制所付出的代价，MPC控制的代价函数定义如下：
$\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)$
其中函数参数 $U=\left(u_{0}, u_{1}, \ldots, u_{N}\right)$ ，并且矩阵 $Q, Q_{f}, R$ 为正定矩阵，即
$T>0Q=Q^{T} \geq 0, \quad Q_{f}=Q_{f}^{T} \geq 0, \quad R=R^{T}>0$

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

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

对于公式(6)，它需要服从的约束条件包括
$\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.$

4.2 python代码实现

完整程序见GitHub仓库

4.2.1 参数

# mpc parametersNX = 3# x = x, y, yawNU = 2# u = [v,delta]T = 8# horizon lengthR = np.diag([0.1, 0.1])# input cost matrixRd = np.diag([0.1, 0.1])# input difference cost matrixQ = np.diag([1, 1, 1])# state cost matrixQf = Q# state final matrix#车辆dt=0.1 # 时间间隔，单位：sL=2 # 车辆轴距，单位：mv = 2 # 初始速度x_0=0 # 初始xy_0=-3 #初始ypsi_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. 基本概念

1.1 MPC vs optimal control

1.2 MPC优点

2. MPC整体流程

2.1 预测区间与控制区间

2.2 约束

2.3 MPC流程

2.4 MPC vs. LQR

3. MPC设计

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

4.1 MPC建模

4.2 python代码实现

4.2.1 参数

4.2.2 运动学模型

4.2.3 参考轨迹

4.2.4 矩阵拍平

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

4.2.6 MPC控制实现

4.2.7 主函数

5. MPC开源库/程序

最新关注

热文推荐

java旅游攻略管理系统Myeclipse开发mysql数据库web结构java编程计算机网页项目

自学前端，你必须要掌握的3种定时任务

剖析flutter_download_manager学习如何做下载管理,暂停和取消

头歌——一维数组和二维数组全对答案秒过

24万满分作文网作文大全ACCESS\EXCEL数据库

不同开发语言在进程、线程和协程的设计差异

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

文章目录

参考资料

1. 基本概念

1.1 MPC vs optimal control

1.2 MPC优点

2. MPC整体流程

2.1 预测区间与控制区间

2.2 约束

2.3 MPC流程

2.4 MPC vs. LQR

3. MPC设计

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

4.1 MPC建模

4.2 python代码实现

4.2.1 参数

4.2.2 运动学模型

4.2.3 参考轨迹

4.2.4 矩阵拍平

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

4.2.6 MPC控制实现

4.2.7 主函数

5. MPC开源库/程序

相关文章

最新关注

热文推荐