轨迹规划 | 图解模型预测控制MPC算法(附ROS C++/Python/Matlab仿真)

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法


1 模型预测控制原理

模型预测控制(Model Predictive Control,MPC)是一种先进的控制策略,广泛应用于工程领域。它通过建立动态系统的数学模型,并基于当前状态进行预测,优化未来一段时间内的控制动作,以达到最优的控制效果。模型预测控制通过预测和优化方法,能够在系统动态变化和约束条件下,实现更高效、更灵活的控制,因此在工业应用中具有重要的实际价值。

形式化地,以下图阐述模型预测控制原理。不计较模型形式,以状态空间模型为例

{ x ( k + 1 ) = f ( x ( k ) , u ( k ) ) , x ( 0 ) = x 0 y ( k ) = h ( x ( k ) , u ( k ) ) \begin{cases} \boldsymbol{x}\left( k+1 \right) =\boldsymbol{f}\left( \boldsymbol{x}\left( k \right) ,\boldsymbol{u}\left( k \right) \right) ,\boldsymbol{x}\left( 0 \right) =\boldsymbol{x}_0\\ \boldsymbol{y}\left( k \right) =\boldsymbol{h}\left( \boldsymbol{x}\left( k \right) ,\boldsymbol{u}\left( k \right) \right)\\\end{cases} {x(k+1)=f(x(k),u(k)),x(0)=x0y(k)=h(x(k),u(k))

其中 x ( k ) ∈ R n \boldsymbol{x}\left( k \right) \in \mathbb{R} ^n x(k)Rn u ( k ) ∈ R l \boldsymbol{u}\left( k \right) \in \mathbb{R} ^l u(k)Rl y ( k ) ∈ R q \boldsymbol{y}\left( k \right) \in \mathbb{R} ^q y(k)Rq分别表示时刻 系统的状态、控制输入和输出向量。基于预测模型可以计算预测方程,根据预测方程可以求解系统起始于 y ( k ) \boldsymbol{y}\left( k \right) y(k)的未来一段时间内的输出序列

{ y p ( k + 1 ∣ k ) , y p ( k + 2 ∣ k ) , ⋯   , y ( k + p ∣ k ) } \left\{ \boldsymbol{y}_p\left( k+1|k \right) , \boldsymbol{y}_p\left( k+2|k \right) , \cdots , \boldsymbol{y}\left( k+p|k \right) \right\} {yp(k+1∣k),yp(k+2∣k),,y(k+pk)}

其中 p p p称为预测时域

在这里插入图片描述

控制输入序列

U k = d e f { u ( k ∣ k ) , u ( k + 1 ∣ k ) , ⋯   , u ( k + p − 1 ∣ k ) } U_k\xlongequal{\mathrm{def}}\left\{ \boldsymbol{u}\left( k|k \right) , \boldsymbol{u}\left( k+1|k \right) , \cdots , \boldsymbol{u}\left( k+p-1|k \right) \right\} Ukdef {u(kk),u(k+1∣k),,u(k+p1∣k)}

是MPC需要求解的优化问题的独立变量。最常见的,我们希望系统实际输出 跟踪期望的参考输入

{ r ( k + 1 ) , r ( k + 2 ) , ⋯   , r ( k + p ) } \left\{ \boldsymbol{r}\left( k+1 \right) , \boldsymbol{r}\left( k+2 \right) , \cdots , \boldsymbol{r}\left( k+p \right) \right\} {r(k+1),r(k+2),,r(k+p)}

同时满足实际问题的约束条件,例如控制约束和输出约束

{ u min ⁡ ⩽ u ( k + i ) ⩽ u max ⁡ , i ⩾ 0 y min ⁡ ⩽ y ( k + i ) ⩽ y max ⁡ , i ⩾ 0 \begin{cases} \boldsymbol{u}_{\min}\leqslant \boldsymbol{u}\left( k+i \right) \leqslant \boldsymbol{u}_{\max}, i\geqslant 0\\ \boldsymbol{y}_{\min}\leqslant \boldsymbol{y}\left( k+i \right) \leqslant \boldsymbol{y}_{\max}, i\geqslant 0\\\end{cases} {uminu(k+i)umax,i0yminy(k+i)ymax,i0

根据这些指标设计优化目标函数 J = J ( y ( k ) , U k ) J=J\left( y\left( k \right) , U_k \right) J=J(y(k),Uk),其最优解可记为

U k ∗ = a r g min ⁡ U k J ( y ( k ) , U k ) U_{k}^{*}=\mathrm{arg}\min _{U_k}J\left( y\left( k \right) ,U_k \right) Uk=argUkminJ(y(k),Uk)

k k k时刻优化解 U k ∗ U_{k}^{*} Uk的第一个分量实际作用于系统,并在 k + 1 k+1 k+1时刻以新得到的测量值 y ( k + 1 ) y(k+1) y(k+1)为初始条件重新预测系统未来输出并求解优化问题。随着当前时间向前推移,预测时域也向前滚动

2 差速模型运动学

根据差分机器人运动学模型

p ˙ = [ x ˙ y ˙ θ ˙ ] = [ v cos ⁡ θ v sin ⁡ θ ω ] = [ f 1 f 2 f 3 ] \boldsymbol{\dot{p}}=\left[ \begin{array}{c} \dot{x}\\ \dot{y}\\ \dot{\theta}\\\end{array} \right] =\left[ \begin{array}{c} v\cos \theta\\ v\sin \theta\\ \omega\\\end{array} \right] =\left[ \begin{array}{c} f_1\\ f_2\\ f_3\\\end{array} \right] p˙= x˙y˙θ˙ = vcosθvsinθω = f1f2f3

选择状态量 p = [ x y θ ] T \boldsymbol{p}=\left[ \begin{matrix} x& y& \theta\\\end{matrix} \right] ^T p=[xyθ]T和状态误差量 x = [ x − x r y − y r θ − θ r ] T \boldsymbol{x}=\left[ \begin{matrix} x-x_r& y-y_r& \theta -\theta _r\\\end{matrix} \right] ^T x=[xxryyrθθr]T,控制量 s = [ v ω ] T \boldsymbol{s}=\left[ \begin{matrix} v& \omega\\\end{matrix} \right] ^T s=[vω]T和控制误差量 u = [ v − v r ω − ω r ] T \boldsymbol{u}=\left[ \begin{matrix} v-v_r& \omega -\omega _r\\\end{matrix} \right] ^T u=[vvrωωr]T,可得

x ( k + 1 ) = ( T A + I ) x ( k ) + T B u ( k ) \boldsymbol{x}\left( k+1 \right) =\left( T\boldsymbol{A}+\boldsymbol{I} \right) \boldsymbol{x}\left( k \right) +T\boldsymbol{Bu}\left( k \right) x(k+1)=(TA+I)x(k)+TBu(k)

其中

A = [ 0 0 − v r sin ⁡ θ r 0 0 v r cos ⁡ θ r 0 0 0 ] , B = [ cos ⁡ θ r 0 sin ⁡ θ r 0 0 1 ] \boldsymbol{A}=\left[ \begin{matrix} 0& 0& -v_r\sin \theta _r\\ 0& 0& v_r\cos \theta _r\\ 0& 0& 0\\\end{matrix} \right] , \boldsymbol{B}=\left[ \begin{matrix} \cos \theta _r& 0\\ \sin \theta _r& 0\\ 0& 1\\\end{matrix} \right] A= 000000vrsinθrvrcosθr0 ,B= cosθrsinθr0001

定义输出方程

y ( k ) = C ( k ) x ( k ) \boldsymbol{y}\left( k \right) =\boldsymbol{C}\left( k \right) \boldsymbol{x}\left( k \right) y(k)=C(k)x(k)

其中 C ( k ) \boldsymbol{C}\left( k \right) C(k)定义为单位阵

3 基于差速模型的MPC控制

为了引入模型预测控制,设计状态向量

x ~ ( k ) = [ x ( k ) u ( k − 1 ) ] \boldsymbol{\tilde{x}}\left( k \right) =\left[ \begin{array}{c} \boldsymbol{x}\left( k \right)\\ \boldsymbol{u}\left( k-1 \right)\\\end{array} \right] x~(k)=[x(k)u(k1)]

x ~ ( k + 1 ) = [ A B 0 I N u ] x ~ ( k ) + [ B I N u ] Δ u ( k ) \boldsymbol{\tilde{x}}\left( k+1 \right) =\left[ \begin{matrix} \boldsymbol{A}& \boldsymbol{B}\\ \mathbf{0}& \boldsymbol{I}_{N_u}\\\end{matrix} \right]\boldsymbol{\tilde{x}}\left( k \right) +\boldsymbol{\left[ \begin{array}{c} \boldsymbol{B}\\ \boldsymbol{I}_{N_u}\\\end{array} \right]}\varDelta \boldsymbol{u}\left( k \right) x~(k+1)=[A0BINu]x~(k)+[BINu]Δu(k)

接着基于模型进行系统未来动态的预测

{ x ~ ( k + m ) = A ~ m x ~ ( k ) + ∑ i = 0 m − 1 A ~ i B ~ Δ u ( k + m − 1 − i ) x ~ ( k + p ) = A ~ p x ~ ( k ) + ∑ i = p − m p − 1 A ~ i B ~ Δ u ( k + p − 1 − i ) \begin{cases} \boldsymbol{\tilde{x}}\left( k+m \right) =\boldsymbol{\tilde{A}}^m\boldsymbol{\tilde{x}}\left( k \right) +\sum_{i=0}^{m-1}{\boldsymbol{\tilde{A}}^i\boldsymbol{\tilde{B}}\varDelta \boldsymbol{u}\left( k+m-1-i \right)}\\ \boldsymbol{\tilde{x}}\left( k+p \right) =\boldsymbol{\tilde{A}}^p\boldsymbol{\tilde{x}}\left( k \right) +\sum_{i=p-m}^{p-1}{\boldsymbol{\tilde{A}}^i\boldsymbol{\tilde{B}}\varDelta \boldsymbol{u}\left( k+p-1-i \right)}\\\end{cases} {x~(k+m)=A~mx~(k)+i=0m1A~iB~Δu(k+m1i)x~(k+p)=A~px~(k)+i=pmp1A~iB~Δu(k+p1i)

同理,可迭代计算系统输出

{ y ~ ( k + m ) = C ~ A ~ i x ~ ( k ) + ∑ i = 0 m − 1 C ~ A ~ i B ~ Δ u ( k + m − 1 − i ) y ~ ( k + p ) = C ~ A i x ~ ( k ) + ∑ i = p − m p − 1 C ~ A ~ i B ~ Δ u ( k + p − 1 − i ) \begin{cases} \boldsymbol{\tilde{y}}\left( k+m \right) =\boldsymbol{\tilde{C}\tilde{A}}^i\boldsymbol{\tilde{x}}\left( k \right) +\sum_{i=0}^{m-1}{\boldsymbol{\tilde{C}\tilde{A}}^i\boldsymbol{\tilde{B}}\varDelta \boldsymbol{u}\left( k+m-1-i \right)}\\ \boldsymbol{\tilde{y}}\left( k+p \right) =\boldsymbol{\tilde{C}A}^i\boldsymbol{\tilde{x}}\left( k \right) +\sum_{i=p-m}^{p-1}{\boldsymbol{\tilde{C}\tilde{A}}^i\boldsymbol{\tilde{B}}\varDelta \boldsymbol{u}\left( k+p-1-i \right)}\\\end{cases} {y~(k+m)=C~A~ix~(k)+i=0m1C~A~iB~Δu(k+m1i)y~(k+p)=C~Aix~(k)+i=pmp1C~A~iB~Δu(k+p1i)

定义预测输出向量和控制输入向量为

Y p ( k ) = d e f [ y ~ ( k + 1 ) y ~ ( k + 2 ) ⋮ y ~ ( k + p ) ] p × 1 , Δ U m ( k ) = d e f [ Δ u ( k ) Δ u ( k + 1 ) ⋮ Δ u ( k + m − 1 ) ] m × 1 \boldsymbol{Y}_p\left( k \right) \xlongequal{\mathrm{def}}\left[ \begin{array}{c} \boldsymbol{\tilde{y}}\left( k+1 \right)\\ \boldsymbol{\tilde{y}}\left( k+2 \right)\\ \vdots\\ \boldsymbol{\tilde{y}}\left( k+p \right)\\\end{array} \right] _{p\times 1}, \varDelta \boldsymbol{U}_m\left( k \right) \xlongequal{\mathrm{def}}\left[ \begin{array}{c} \varDelta \boldsymbol{u}\left( k \right)\\ \varDelta \boldsymbol{u}\left( k+1 \right)\\ \vdots\\ \varDelta \boldsymbol{u}\left( k+m-1 \right)\\\end{array} \right] _{m\times 1} Yp(k)def y~(k+1)y~(k+2)y~(k+p) p×1,ΔUm(k)def Δu(k)Δu(k+1)Δu(k+m1) m×1

则系统输出方程可表达为矩阵形式

Y p ( k ) = S x x ~ ( k ) + S u Δ U m ( k ) \boldsymbol{Y}_p\left( k \right) ={S} _x\boldsymbol{\tilde{x}}\left( k \right) +{S} _u\varDelta \boldsymbol{U}_m\left( k \right) Yp(k)=Sxx~(k)+SuΔUm(k)

基于预测方程定义开环优化目标函数

J = ( Y p − Y r ) T Q ~ ( Y p − Y r ) + Δ U m T R ~ Δ U m J=\left( \boldsymbol{Y}_p-\boldsymbol{Y}_r \right) ^T\boldsymbol{\tilde{Q}}\left( \boldsymbol{Y}_p-\boldsymbol{Y}_r \right) +\varDelta \boldsymbol{U}_{m}^{T}\boldsymbol{\tilde{R}}\varDelta \boldsymbol{U}_m J=(YpYr)TQ~(YpYr)+ΔUmTR~ΔUm

对于优化问题 Δ U m = a r g min ⁡ Δ U m J ( Δ U m ) \varDelta \boldsymbol{U}_m=\mathrm{arg}\min _{\varDelta \boldsymbol{U}_m}J\left( \varDelta \boldsymbol{U}_m \right) ΔUm=argminΔUmJ(ΔUm)

∂ J ∂ Δ U m = ∂ ∂ Δ U m [ 1 2 Δ U m T ( S u T Q ~ S u + R ~ ) Δ U m + ( S x x ~ − Y r ) T Q ~ S u Δ U m ] \frac{\partial J}{\partial \varDelta \boldsymbol{U}_m}=\frac{\partial}{\partial \varDelta \boldsymbol{U}_m}\left[ \frac{1}{2}\varDelta \boldsymbol{U}_{m}^{T}\left( {S} _{u}^{T}\boldsymbol{\tilde{Q}}{S} _u+\boldsymbol{\tilde{R}} \right) \varDelta \boldsymbol{U}_m+\left( {S} _x\boldsymbol{\tilde{x}}-\boldsymbol{Y}_r \right) ^T\boldsymbol{\tilde{Q}}{S} _u\varDelta \boldsymbol{U}_m \right] ΔUmJ=ΔUm[21ΔUmT(SuTQ~Su+R~)ΔUm+(Sxx~Yr)TQ~SuΔUm]

对应转化为标准的二次规划问题

Δ U m = a r g min ⁡ Δ U m ( 1 2 Δ U m T H Δ U m + g T Δ U m ) \varDelta \boldsymbol{U}_m=\mathrm{arg}\min _{\varDelta \boldsymbol{U}_m}\left( \frac{1}{2}\varDelta {\boldsymbol{U}_m}^T\boldsymbol{H}\varDelta \boldsymbol{U}_m+\boldsymbol{g}^T\varDelta \boldsymbol{U}_m \right) ΔUm=argΔUmmin(21ΔUmTHΔUm+gTΔUm)

4 仿真实现

4.1 ROS C++实现

核心代码如下所示,采用osqp求解

Eigen::Vector2d MPCPlanner::_mpcControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r,
                                        Eigen::Vector2d du_p)
{
  ...

  // Calculate kernel
  std::vector<c_float> P_data;
  std::vector<c_int> P_indices;
  std::vector<c_int> P_indptr;
  int ind_P = 0;
  for (int col = 0; col < dim_u * m_; ++col)
  {
    P_indptr.push_back(ind_P);
    for (int row = 0; row <= col; ++row)
    {
      P_data.push_back(P(row, col));
      // P_data.push_back(P(row, col) * 2.0);
      P_indices.push_back(row);
      ind_P++;
    }
  }
  P_indptr.push_back(ind_P);

  // Calculate affine constraints (4m x 2m)
  std::vector<c_float> A_data;
  std::vector<c_int> A_indices;
  std::vector<c_int> A_indptr;
  int ind_A = 0;
  A_indptr.push_back(ind_A);
  for (int j = 0; j < m_; ++j)
  {
    for (int n = 0; n < dim_u; ++n)
    {
      for (int row = dim_u * j + n; row < dim_u * m_; row += dim_u)
      {
        A_data.push_back(1.0);
        A_indices.push_back(row);
        ++ind_A;
      }
      A_data.push_back(1.0);
      A_indices.push_back(dim_u * m_ + dim_u * j + n);
      ++ind_A;
      A_indptr.push_back(ind_A);
    }
  }

  // Calculate offset
  std::vector<c_float> q_data;
  for (int row = 0; row < dim_u * m_; ++row)
  {
    q_data.push_back(q(row, 0));
  }

  // Calculate constraints
  std::vector<c_float> lower_bounds;
  std::vector<c_float> upper_bounds;
  for (int row = 0; row < 2 * dim_u * m_; row++)
  {
    lower_bounds.push_back(lower(row, 0));
    upper_bounds.push_back(upper(row, 0));
  }

  // solve
  OSQPWorkspace* work = nullptr;
  OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
  OSQPSettings* settings = reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
  osqp_set_default_settings(settings);
  settings->verbose = false;
  settings->warm_start = true;

  data->n = dim_u * m_;
  data->m = 2 * dim_u * m_;
  data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(), P_indices.data(), P_indptr.data());
  data->q = q.data();
  data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(), A_indices.data(), A_indptr.data());
  data->l = lower_bounds.data();
  data->u = upper_bounds.data();

  osqp_setup(&work, data, settings);
  osqp_solve(work);
  auto status = work->info->status_val;

  ...

  Eigen::Vector2d u(work->solution->x[0] + du_p[0] + u_r[0], regularizeAngle(work->solution->x[1] + du_p[1] + u_r[1]));

  // Cleanup
  osqp_cleanup(work);
  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);

  return u;
}

在这里插入图片描述

4.2 Python实现

核心代码如下所示

def mpcControl(self, s: tuple, s_d: tuple, u_r: tuple, u_p: tuple) -> np.ndarray:
	...
	
	# optimization
	Yr = np.zeros((3 * self.p, 1))              # (3p x 1)
	Q = np.kron(np.identity(self.p), self.Q)    # (3p x 3p)
	R = np.kron(np.identity(self.m), self.R)    # (2m x 2m)
	H = S_u.T @ Q @ S_u + R                     # (2m x 2m)
	g = S_u.T @ Q @ (S_x @ x - Yr)              # (2m x 1)
	
	# constriants
	I = np.eye(2 * self.m)
	A_I = np.kron(np.tril(np.ones((self.m, self.m))), np.diag([1, 1]))
	U_min = np.kron(np.ones((self.m, 1)), self.u_min)
	U_max = np.kron(np.ones((self.m, 1)), self.u_max)
	U_k_1 = np.kron(np.ones((self.m, 1)), np.array([[u_p[0]], [u_p[1]]]))
	
	# boundary
	dU_min = np.kron(np.ones((self.m, 1)), self.du_min)
	dU_max = np.kron(np.ones((self.m, 1)), self.du_max)
	
	# solve
	solver = osqp.OSQP()
	H = sparse.csc_matrix(H)
	A = sparse.csc_matrix(np.vstack([A_I, I]))
	l = np.vstack([U_min - U_k_1, dU_min])
	u = np.vstack([U_max - U_k_1, dU_max])
	solver.setup(H, g, A, l, u, verbose=False)
	res = solver.solve()
	dU_opt = res.x[:, None]
	
	# first element
	du = dU_opt[0:2]
	
	# real control
	u = du + np.array([[u_p[0]], [u_p[1]]]) + np.array([[u_r[0]], [u_r[1]]])
	
	return np.array([
	    [self.linearRegularization(float(u[0]))], 
	    [self.angularRegularization(float(u[1]))]
	]), (float(u[0]) - u_r[0], float(u[1]) - u_r[1])

在这里插入图片描述

4.3 Matlab实现

核心代码如下所示

function [u, u_p_new] = mpcControl(s, s_d, u_r, u_p, robot, param)
  ...
    
    % optimization
    Yr = zeros(3 * param.p, 1);                  % (3p x 1)
    Q = kron(eye(param.p), param.Q);    % (3p x 3p)
    R = kron(eye(param.m), param.R);    % (2m x 2m)
    H = S_u' * Q * S_u + R;                        % (2m x 2m)
    g = S_u' * Q * (S_x * x - Yr);                 % (2m x 1)
    
    % constriants
    A_I = kron(tril(ones(param.m, param.m)), diag([1, 1]));
    U_min = kron(ones(param.m, 1), param.u_min);
    U_max = kron(ones(param.m, 1), param.u_max);
    U_k_1 = kron(ones(param.m, 1), u_p');
    
    % boundary
    dU_min = kron(ones(param.m, 1), param.du_min);
    dU_max = kron(ones(param.m, 1), param.du_max);
    
    % solve
    options = optimoptions('quadprog', 'MaxIterations', 100, 'TolFun', 1e-16, 'Display','off');
    dU_opt = quadprog(H, g, [-A_I; A_I], [-U_min + U_k_1; U_max - U_k_1], [], [], dU_min, dU_max, [], options);
    
    % first element
    du = [dU_opt(1), dU_opt(2)];

    % real control
    u = du + u_p + u_r;
    
    u = [linearRegularization(robot, u(1), param), angularRegularization(robot, u(2), param)];
    u_p_new = u - u_r;
end

在这里插入图片描述

完整工程代码请联系下方博主名片获取


🔥 更多精彩专栏


👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇