MPC求解（基于apollo代码的理解）

MPC求解（基于apollo代码的理解）

转载请引用本文链接

参考follow轻尘并做了修正
方向盘控制模型[Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006.]
$$\frac{d}{dt}\left(\begin{array}{c}lateral\_error\\ lateral\_error\_rate\\ heading\_error\\ heading\_error\_rate\\ \begin{array}{c}station\_error\\ speed\_error\end{array}\end{array}\right)=\left(\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& -\frac{c_f+c_r}{mv}& \frac{c_f+c_r}{m}& \frac{c_r{l}_r-c_f{l}_f}{mv}& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& \frac{c_r{l}_r-c_f{l}_f}{I_{z}v}& \frac{c_r{l}_r-c_f{l}_f}{I_z}& -\frac{c_r{l}_r^2+c_f{l}_f^2}{I_{z}v}& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right)\left(\begin{array}{c}lateral\_error\\ lateral\_error\_rate\\ heading\_error\\ heading\_error\_rate\\ \begin{array}{c}station\_error\\ speed\_error\end{array}\end{array}\right)$$

$$+\left(\begin{array}{cc}0& 0\\ \frac{c_f}{m}& 0\\ 0& 0\\ \frac{c_f{l}_f}{I_z}& 0\\ 0& 0\\ 0& -1\end{array}\right)\left(\begin{array}{c}{\delta }_f\\ a\end{array}\right)+\left(\begin{array}{c}0\\ \frac{c_r{l}_r-c_f{l}_f}{mv}-v\\ 0\\ -\frac{c_r{l}_r^2+c_f{l}_f^2}{I_{z}v}\\ 0\\ 1\end{array}\right)\dot{\phi }$$
经双线性变换得到
$$A=(I-\frac{t_s{A}^{\prime }}{2})-1(I+\frac{t_s{A}^{\prime }}{2})，B=t_s{B}^{\prime }，C=t_s{C}^{\prime }\dot{\phi }$$
其中$A^{\prime }，B^{\prime }，C^{\prime }$分别对应上述模型的系数矩阵，因此新的离散模型可以写成
$$x(k+1)=Ax(k)+Bu(k)+C$$
这里对于代码的关系：
matrix_a:$A$，matrix_b:$B$，matrix_c:$C$
对于模型预测控制，预测未来10（假设）个阶段内的状态变量，有如下关系
$$\{\begin{array}{c}x(1)=Ax(0)+Bu(0)+C\\ x(2)=A^{2}x(0)+ABu(0)+Bu(1)+C+AC\\ x(3)=A^{3}x(0)+A^{2}Bu(0)+ABu(1)+Bu(2)+C+AC+A^{2}C\\ ...\end{array}$$
对于任意$k$阶段，有
$$x(k)=A^{k}x(0)+\sum _{i=0}^{k-1}{A}^{i}Bu(k-1-i)+\sum _{i=0}^{k-1}{A}^{i}C$$
因此，对于10个预测的阶段，可以写成矩阵形式（*）
$$\left(\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_{10}\end{array}\right)=\left(\begin{array}{cccc}A& A^2& \cdots & A^{10}\end{array}\right)\left(\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_9\end{array}\right)+\left(\begin{array}{cccc}B& & & \\ AB& B& & \\ ⋮& & B& \\ A^{9}B& A^{8}B& \cdots & B\end{array}\right)\left(\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_9\end{array}\right)+\left(\begin{array}{c}C\\ C+AC\\ ⋮\\ C+AC+...+A^{9}C\end{array}\right)(\ast )$$

与代码对应关系：
matrix_aa: $$=\tilde{A}=\left(\begin{array}{cccc}A& A^2& \cdots & A^{10}\end{array}\right)$$
matrix_k:
$$=\tilde{K}=\left(\begin{array}{cccc}B& & & \\ AB& B& & \\ ⋮& & B& \\ A^{9}B& A^{8}B& \cdots & B\end{array}\right)$$
matrix_cc
$$=\tilde{C}=\left(\begin{array}{c}C\\ C+AC\\ ⋮\\ C+AC+...+A^{9}C\end{array}\right)$$
另外的权重矩阵：$Q$=matrix_q，$R$=matrix_r，则有
matrix_qq
$$=\tilde{Q}=\left(\begin{array}{cccc}Q& & & \\ & Q& & \\ & & \ddots & \\ & & & Q\end{array}\right)$$
matrix_rr
$$=\tilde{R}=\left(\begin{array}{cccc}R& & & \\ & R& & \\ & & \ddots & \\ & & & R\end{array}\right)$$
matrix_m
$$=M=\left(\begin{array}{c}A\ast matrix\_initial\_state\\ A^2\ast matrix\_initial\_state\\ ⋮\\ A^{10}\ast matrix\_initial\_state\end{array}\right)$$

下面解释由mpc控制问题转换成二次规划问题的过程
对于每一轮滚动优化，我们希望找到最优的10个
$$u_i=\left(\begin{array}{c}{\delta }_{fi}\\ a_i\end{array}\right)$$
也即
$$U=\left(\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_9\end{array}\right)$$
使得性能函数最优,进一步的，我们希望采用如下二次规划的形式来求解这个问题
$$\{\begin{array}{c}\begin{array}{cc}\min & f(x)=U^{T}HU+G^{T}U\end{array}\\ \begin{array}{cc}s.t.& \end{array}\\ \begin{array}{cc}& M_{1}U=N_1\end{array}\\ \begin{array}{c}\begin{array}{cc}& M_{2}U\le N_2\end{array}\\ b_1\le U\le b_2\end{array}\end{array}$$
具体到路径跟踪问题，构造如下形式的目标函数
$$f(x)=(X-X_{ref}{)}^T\tilde{Q}(X-X_{ref})+U^T\tilde{R}U$$
其中$X_{ref}$为10个状态量组合的参考（期望）值，$\tilde{Q}$和$\tilde{R}$分别为状态量和控制量的权重矩阵，由于本控制问题的状态变量已经是偏差量了，所以$X_{ref}$为零向量，与代码相符
根据式（*）代入相关量，对于$f(x)$进行推导
$$f(x)=(\tilde{A}X+\tilde{K}U+\tilde{C}-X_{ref}{)}^T\tilde{Q}(\tilde{A}X+\tilde{K}U+\tilde{C}-X_{ref})+U^T\tilde{R}U$$
令$e=\tilde{A}X+\tilde{C}-X_{ref}$，则有
$$\begin{gathered} f(x)=(e+\tilde{K}U{)}^T\tilde{Q}(e+\tilde{K}U)+U^T\tilde{R}U \\ \begin{array}{c}=(e^T\tilde{Q}+{(\tilde{K}U)}^T\tilde{Q})(e+\tilde{K}U)+U^T\tilde{R}U\\ =e^T\tilde{Q}e+e^T\tilde{Q}\tilde{K}U+{(\tilde{K}U)}^T\tilde{Q}e+{(\tilde{K}U)}^T\tilde{Q}\tilde{K}U+U^T\tilde{R}U\\ \approx 2{(\tilde{K}U)}^T\tilde{Q}e+U^T{\tilde{K}}^T\tilde{Q}\tilde{K}U+U^T\tilde{R}U\\ \begin{array}{c}=U^T({\tilde{K}}^T\tilde{Q}\tilde{K}+\tilde{R})U+2U^T{\tilde{K}}^T\tilde{Q}e\\ \begin{array}{c}=U^T({\tilde{K}}^T\tilde{Q}\tilde{K}+\tilde{R})U+2U^T{\tilde{K}}^T\tilde{Q}(\tilde{A}X+\tilde{C}-X_{ref})\\ =U^T({\tilde{K}}^T\tilde{Q}\tilde{K}+\tilde{R})U+2U^T{\tilde{K}}^T\tilde{Q}(M+\tilde{C}-X_{ref})\end{array}\end{array}\end{array} \end{gathered}$$
根据上面式子，得到一个二次规划的目标函数
$$f(x)=2\left(\frac{1}{2}{U}^{T}HU+U^{T}G\right)$$
其中，各符号与代码的对应关系如下
matrix_m1$=H={\tilde{K}}^T\tilde{Q}\tilde{K}+\tilde{R}$
matrix_m2$=G={\tilde{K}}^T\tilde{Q}(\tilde{A}X+\tilde{C}-X_{ref})$
推导结果与代码一致
对
$$U=\left(\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_9\end{array}\right)$$添加约束条件，即matrix_ll$\le U\le$matrix_uu
matrix_ll$$=\left(\begin{array}{c}\mathrm{m}\mathrm{a}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{x}\_\mathrm{l}\mathrm{o}\mathrm{w}\mathrm{e}\mathrm{r}\\ \mathrm{m}\mathrm{a}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{x}\_\mathrm{l}\mathrm{o}\mathrm{w}\mathrm{e}\mathrm{r}\\ ⋮\\ \mathrm{m}\mathrm{a}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{x}\_\mathrm{l}\mathrm{o}\mathrm{w}\mathrm{e}\mathrm{r}\end{array}\right)$$
matrix_uu$$=\left(\begin{array}{c}\mathrm{m}\mathrm{a}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{x}\_\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}\\ \mathrm{m}\mathrm{a}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{x}\_\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}\\ ⋮\\ \mathrm{m}\mathrm{a}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{x}\_\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}\end{array}\right)$$
代码中二次规划求解过程，不等式约束设置为单位阵，等式约束、等式边界均设置为零矩阵