基于运动学的MPC控制盲点解析

文章目录

• • 误差模型和离散化
  • 预测模型
  • 目标函数的设计
  • 约束设计
  • 优化求解
  • 小结

---

误差模型和离散化

当达到控制步长k+Nc时，后面的预测控制量有两种处理逻辑，第一种就是控制步长Nc之后的预测控制量保持不变，如黄色虚线；第二种就是Nc之后的预测控制量全部置零，如红色线部分。

---

此处离散化同这篇文章推导的离散化异曲同工，注意此处的t+T表示的就是第k+1时刻。

---

补充：

个人认为此处的状态误差表述有问题，不应该是加点。应该是

{x-xr,

y-yr,

phi - phi_r}

---

预测模型

为什么不用上述推导出的离散化的状态空间方程来推导我们的预测模型，以此来设计MPC控制器？

主要考虑在设计目标函数的时候，想将我们的控制增量设计到我们的目标函数中去，而非直接使用控制量，以控制增量为代价，能有效改善控制的平滑性。如下图目标函数设计：

---

这里构建新的状态量以便推导出新的状态空间方程。主要想对离散化的状态控制方程的控制量化为控制增量的形式。如绿色方框内的状态空间方程。

其次基于新的状态空间方程也可以进行下一步的状态的预测。即基于上一时刻的状态量和控制增量即可预测下一步的新的状态量（仅能预测一步）。（当然基于离散化的状态空间方程也可以进行预测，但是没有新的状态空间方程预测的准确）

最终推导出的系统的预测模型Y, 其可以根据系统当前的状态量，以及施加一个未来一段时间Nc序列的控制增量，我们就可以知道系统未来(Np步)的表现是什么样子的。即y(k+1)~y(k+Np)

根据新的状态空间方程推出系统预测模型，如果想推出预测时域Np内的状态输出量，就需要知道预测时域内的每一步的状态量。每一步的状态量如何推导：

当达到控制步长k+Nc时，后面的预测控制量有两种处理逻辑，第一种就是控制步长Nc之后的预测控制量保持不变，；第二种就是Nc之后的预测控制量全部置零，如上述图所示，本文采用第二种方式。

---

补充：

此处的Nu为控制量的个数，为2，Nx为状态量的个数为3

此处输出方程的[INx 0] =

{1 0 0 0 0

0 1 0  0 0

0 0 1  0 0}

即C为3x5的矩阵，输出方程仅输出三个状态量，无需输出2个控制量，因此控制量前的系数矩阵是零矩阵。也即输出是3x1的状态矩阵。因此预测的系统未来的状态也就是纵向误差和横向误差以及航线误差三个误差的状态量。

---

---

目标函数设计

设计目标：希望我们的路径跟踪误差在预测时域内尽可能趋于0

目标函数最后一项引入松弛因子(在程序中是一个具体的数值)的作用：

松弛因子设置原则

---

权重矩阵的设计直接影响调试结果

• 如果更看重误差，则Q矩阵可以设计大一点，能快速使得误差趋于0，但过大会引起震荡，平稳性变差，即如果想位置误差更小，则增大对应状态的q1 q2等进行调试；
• 如果想控制的更加平稳，则可以增大R矩阵，使得控制的更加柔和，但响应性变差，误差会相应放大；
• 如果更看重终端误差，则可增大F矩阵；

---

补充：

此处的E表示状态的误差Error

此外，应该是g的转置等于第二部分，此处是g等于后面部分应该是不对的。欢迎指正？！

从下面推导可以看出错误：

---

约束设计

此处关于控制增量的约束，比如说，我们的前轮转角打30°需要2s,则1s需要打15°，然而我们的控制周期T=0.05s，则控制周期我们需要下发的转角最大为0.75°，也即我们下发转角的最大速率不超过0.75°/T。

---

优化求解

第一步：目标函数如何转化为标准二次型

目标函数化简后的每一项其实都是一个具体的数，第三项表示具体数的转置等于其自身。因此合并第二项和第三项，二者实质相等。其次化成二次型后，G属于已知项，且Q也是已知项，故二次型的最后一项其实不影响最终的优化求解最小值问题，因此放在最后一项，且求解过程可以忽略。

%% MPC预设参数
Nx = 3;         % 状态量的个数
Nu = 2;         % 控制量的个数
Np = 60;        % 预测步长
Nc = 30;        % 控制步长
row = 10;       % 松弛因子
Q = 100*eye(Np*Nx);      % (Np*Nx) × (Np*Nx) 此处的Q就是ppt中目标函数设计处的Qq
R = 1*eye(Nc*Nu);        % (Nc*Nu) × (Nc*Nu)

% 控制量约束条件
umin = [-0.2; -0.54]; %第一项是速度，第二项是转角
umax = [0.2; 0.332];
%控制增量的约束
delta_umin = [-0.05; -0.64];
delta_umax = [0.05; 0.64];



%% 二次型目标函数的相关矩阵

% H矩阵
H_cell = cell(2,2);
H_cell{1,1} = THETA'*Q*THETA + R;  % (Nu * Nc) × (Nu * Nc)
H_cell{1,2} = zeros(Nu*Nc,1);
H_cell{2,1} = zeros(1,Nu*Nc);
H_cell{2,2} = row;  //松弛因子
H = cell2mat(H_cell);            % (Nu * Nc + 1) × (Nu * Nc + 1)

% E矩阵(对应上面G矩阵）
E = PHI*kesi;                    % (Nx * Np) × 1

% g矩阵（对应上面f矩阵）
g_cell = cell(1,2);
g_cell{1,1} = E'*Q*THETA;          % (Nu * Nc ) × 1，行数为了和H的列数匹配，新添加一列0
g_cell{1,2} = 0;
g = cell2mat(g_cell);              % (Nu * Nc + 1 ) × 1

---

下面是矩阵转置的性质：

最后化为标准二次型，计算出第一项和第二项矩阵相加。其中矩阵相加再设计中需要满足的条件：

---

第二步：约束的转化

此处约束的转化，仅转化了标准二次型的两个不等式约束，等式约束未用到，可以不管。具体两个不等式约束也即中间部分。

%% 约束条件的相关矩阵
  
  %A_I矩阵
  A_t = zeros(Nc,Nc);
  for i = 1:Nc
      A_t(i,1:i) = 1; %表示第i行，(1:i)从第一个元素到第i个元素全部赋值1  下三角的全部元素是1的30x30的矩阵
  end
  A_I = kron(A_t, eye(Nu));   % (Nu * Nc) × (Nu * Nc)  克罗内克积
  
  %U_t 矩阵
  Ut = kron(ones(Nc,1),U);  % (Nu * Nc) × 1
  
  % 控制量与控制增量的变化约束
  Umin = kron(ones(Nc,1),umin);
  Umax = kron(ones(Nc,1),umax);
  delta_Umin = kron(ones(Nc,1),delta_umin);
  delta_Umax = kron(ones(Nc,1),delta_umax);
  
  %用于quadprog函数的不等式约束Ax <= b 的矩阵A
  A_cons_cell = {A_I, zeros(Nu*Nc,1);
      -A_I, zeros(Nu*Nc,1)};  %% 列数为了和H的列数匹配，新添加一列0，(Nu * Nc) × (Nu * Nc), (Nu * Nc) ×1
  A_cons = cell2mat(A_cons_cell);
  
  % 用于quadprog函数不等式约束Ax <= b的向量b
  b_cons_cell = {Umax - Ut;-Umin+Ut};
  b_cons = cell2mat(b_cons_cell);
  
  %△U的上下界约束
  lb = delta_Umin;
  ub = delta_Umax;

%% 计算输出

% 只选取求解的delta_U的第一组控制量。注意：这里是v_tilde的变化量和Delta_tilde的变化量
delta_v_tilde = delta_U(1); %速度误差的增量
delta_Delta_tilde = delta_U(2); %转角误差的增量

% 更新这一时刻的控制量。注意，这里的“控制量”是v_tilde和Delta_tilde，而不是真正的v和Delta
U(1) = kesi(4) + delta_v_tilde;  %kesi(4)指的是上一时刻的速度控制量
U(2) = kesi(5) + delta_Delta_tilde;  %kesi(5)指的是上一时刻的转角控制量

% 求解真正的控制量v_real和Delta_real 
v_real = U(1) + v_r; %记得加上参考速度才是真正下发的速度量
Delta_real = U(2) + Delta_r;

---

---

补充：

---

小结

参考资源：

本文仅是学习总结，详细内容可以参考下述参考资源。

1、7.基于运动学的MPC路径跟踪-理论篇-小结_哔哩哔哩_bilibili

2、路径规划与轨迹跟踪系列算法学习_第13讲_模型预测控制(MPC)法_哔哩哔哩_bilibili