MPC模型推导

前言

本文基于人机共驾型智能汽车的共享控制方法研究进行学习分析，虽然文章中存在一些错误(Ac不可逆却使用其逆进行离散化等)，但是对模型的整体的推导并无大碍。为了和学长学姐的已有框架一致，对文章中的模型做了一些调整与简化(调整了状态量的选择，删去曲率约束，暂定了汽车纵向速度为恒定值)。

模型推导与建立

文章使用道路坐标下的车辆线性二自由度动力学模型，具体如下图： a是车辆质心距前轴距离、b是车辆质心距后轴距离。在车辆不发生明显质心转移的情况下，一般认为这两个量为常数。V是车辆的纵向速度、v是车辆的横向速度、是车辆的横摆角速度，这三个量构成描述车辆运动的速度量。 \(\alpha_f\)和\(\alpha_r\)分别是前后轮侧偏角，表示前后轮胎面与车轮运动方向的夹角(注：在车辆转弯时，车轮朝向一般不是车轮的实际运动方向，车轮会因此出现磨损)。\(F_{yf}\)与\(F_{yr}\)分别是前后轮所受路面侧偏力(磨损时地面对轮胎的力)，当车轮垂向载荷及路面附着系数固定的情况下与侧偏角存在一定函数关系，该函数关系称为轮胎的侧偏特性。\(\delta\)是前轮转向角(前轮朝向与车辆坐标系x轴的夹角)，一般与作为模型输入的方向盘转角u成固定比例\(\delta = u/i_s\)，其中\(i_s\)为转向比。

定义车辆的状态量为\(x=[v, \omega, y, psi]\)，v时车辆质心处的纵向速度，\(\omega\)是质心处的角速度，y为汽车坐标系下y方向的距离，psi是车辆坐标系x轴与世界坐标系x轴的夹角。

根据平面刚体运动理论，车辆质心的横向加速度\(a_y\)可以表示为： \[ a_y = \dot{v}+V\omega \] 可以看到是横向速度的变化量加上向心加速度。对前轮运动进行分析，其横向速度分量为\(v+a\omega\)、纵向速度分量为V，可以看作质心为转轴，车身为连杆的旋转运动。根据前轮侧偏角的定义有： \[ \frac{v+a\omega}{V}=\tan{\delta-\alpha_f} \] 对上式进行变换，有： \[ \alpha_f = \delta- \arctan{\frac{v+a\omega}{V}} \] 这里将示意图画清楚是很容易理解的。

以同样的方法对后轮运动进行分析，其横向速度分量为\(v-b\omega\)、纵向速度分量为V，注意到这里我们的对\(\alpha_r\)与\(\alpha_f\)的定义，是从车轮处实际运动方向指向车轮朝向，逆时针为正，因此画一下图不难发现这里是\(v-b\omega\)而非加号。然后由于后轮不会转向，其朝向一直为车辆坐标系的x轴，所以后轮的\(\delta\)恒为0。所以根据后轮侧偏角的定义有： \[ \alpha_r = -\arctan{\frac{v-b\omega}{V}} \] 对二自由度车辆模型的横向运动和横摆运动分别列写动力学微分方程(F=ma，合外力矩=转动惯量*角加速度)，有： \[ 2F_{yf}\cos{\delta}+2F_{yr}=ma_y=m(\dot{v}+V\omega) \] \[ 2aF_{yf}\cos{\delta}-2bF_{yr}=I_z\dot{w} \] 式中，\(m\)为车身质量、\(I_z\)为车身转动惯量。由于自行车模型将左右车轮进行了集中等效处理，因此车辆前后两端所受侧向力为相应轮胎侧偏力的两倍。这两个等式对着示意图看还是很清楚的，均是对质心列写的方程。

小角度假设：车辆前轮转向角\(\delta\)在小范围内（\(\delta\)<5°）变化

一般在平缓道路进行车道保持时，前轮转角的小角度假设都是成立的。在该假设下，有: \[ \begin{cases} \cos{\delta}\approx 1 \\\\ \alpha_f \approx \delta- {\frac{v+a\omega}{V}} \\\\ \alpha_r \approx -{\frac{v-b\omega}{V}}\notag % \notag 取消编号 \end{cases} \] 另外，前轮转角的小角度假设会使得轮胎侧偏角\(\alpha_f\)和\(\alpha_r\)同样在小角度范围内变化。此时，前后轮均工作于线性区间，轮侧偏力与侧偏角之间存在近似线性关系：\(F_{yf} =C_f\alpha_f\)、\(F_{yr} = C_r\alpha_r\)，其中\(C_f\)和\(C_r\)分别表示前后轮侧偏刚度。将这些式子代入前面的动力学微分方程，并代入\(u=i_s\delta\)，可得： \[ \begin{cases} \dot{v}=-\frac{2(C_f+C_r)}{MV}v-[\frac{2(aC_f-bC_r)}{mV} +V]\omega+\frac{2C_f}{i_sm}u\\\\ \dot{\omega}=-\frac{2(aC_f-bC_r)}{I_zV}v-\frac{2(a^2C_f+2b^2C_r)}{I_zV}\omega+\frac{2aC_f}{i_sI_z}u\notag % \notag 取消编号 \end{cases} \] 考虑到车辆状态\(x=[v, \omega, y, psi]\)，可将其表达为如下线性定常形式：

但目前这里有一个问题，线性方程需要\(\dot{x}\)与\(x\)之间的关系，\(\dot{v},\dot{\omega}\)已经求解出来了，若y是世界坐标系下的纵坐标，则\(\dot{y}=v\cos{psi}+V\sin{psi}\)。\(psi\)无法被认为是小角度，这一步我没有想到什么好的线性化方式。因此，这里使用汽车坐标系下的y进行计算。

这里还需要强调一下，所谓汽车坐标系下的y，是指在每一次进行MPC的时域预测开始时，y值为0，以当前汽车坐标系计算当前的参考位置。在下一步，汽车按初始速度前进了一段，汽车坐标系也因此发生了变化，这一步的参考位置需要按照新的汽车坐标系进行计算。(这里已经很像与参考位置的距离误差了)。

具体的，模型为： \[ \dot{x}=A_cx+B_cu \] 其中 \[A_c=\begin{bmatrix} \frac{-2(C_f+C_r)}{MV}&\frac{-2(aC_f-bC_r)}{mV} -V&0&0\\ \frac{-2(aC_f-bC_r)}{I_zV}&\frac{-2(a^2C_f+2b^2C_r)}{I_zV}&0&0\\ 1&0&0&0\\ 0&1&0&0 \end{bmatrix} \] \[ B_c=\begin{bmatrix} \frac{2C_f}{i_sm}\\ \frac{2aC_f}{i_sI_z}\\ 0\\ 0 \end{bmatrix} \] 采用欧拉法对其进行离散化，具体的\(\dot{x}=\frac{x_{k+1}-x_{k}}{T_s}\)，\(T_s\)为时间步长，可以得到： \[ \begin{cases} A=A_cT_s+I(4)\\ B=B_cT_s \end{cases} \] 其中\(I(4)\)表示4x4的单位矩阵。

无约束模型预测轨迹跟踪算法的构造与求解

定义 \[z=\begin{bmatrix} y\\ psi \end{bmatrix} \] 且\(z_{k+i|k}\)表示在第k步预测第k+i步的z值，这里就是MPC预测框中的预测变量。由z的定义我们很容易得到： \[ z_{k+i|k} = Cx_{k+i|k} \] \[ C=\begin{bmatrix} 0&0&1&0\\ 0&0&0&1 \end{bmatrix} \] 用Q与R矩阵作为惩罚系数，可以得到约束项与优化项，其中\(z_{r,k+i|k}\)为对应时刻的参考状态项 \[ \min_{U_k}(\sum_{i=1}^{N}\parallel z_{k+i|k}-z_{r,k+i|k} \parallel^2_Q+\sum_{i=1}^{N-1}\parallel u_{k+i|k} \parallel^2_R) \] 使得 \[ \begin{cases} x_{k+i+1|k}=Ax_{k+i|k}+Bu_{k+i|k}, i=0,1,\dots,N-1\\ z_{k+i|k} = Cx_{k+i|k}, i=1,2,\dots,N\\ x_{k|k} = x_k \end{cases} \] 下面文章推导了轨迹跟踪算法的解析解形式。这一节仿照了Maciejowki书中所使用的无约束模型预测控制推导过程求解问题的解析控制律。首先，将预测方程由第k+1|k步迭代至第k+N|k步： \[ \begin{bmatrix} z_{k+1|k}\\ z_{k+2|k}\\ \vdots \\ z_{k+N|k} \end{bmatrix}= \begin{bmatrix} CA\\ CA^2\\ \vdots \\ CA^N \end{bmatrix}x_k+ \begin{bmatrix} CB& 0 &\cdots &0\\ CAB& CB &\cdots&0\\ \vdots & \vdots & \ddots & \vdots \\ CA^{N-1}B&CA^{N-2}B&\cdots&CB \end{bmatrix} \begin{bmatrix} u_{k+1|k}\\ u_{k+2|k}\\ \vdots \\ u_{k+N|k} \end{bmatrix} \] 为方便推导，引入如下所示的矩阵和向量符号：

\[ Z_k=\begin{bmatrix} z_{k+1|k}\\ z_{k+2|k}\\ \vdots \\ z_{k+N|k} \end{bmatrix}, Z_{r,k}=\begin{bmatrix} z_{r,k+1|k}\\ z_{r,k+2|k}\\ \vdots \\ z_{r,k+N|k} \end{bmatrix} \] \[ \Phi = \begin{bmatrix} CA\\ CA^2\\ \vdots \\ CA^N \end{bmatrix}, \Theta = \begin{bmatrix} CB& 0 &\cdots &0\\ CAB& CB &\cdots&0\\ \vdots & \vdots & \ddots & \vdots \\ CA^{N-1}B&CA^{N-2}B&\cdots&CB \end{bmatrix}, Q(R) = \begin{bmatrix} Q&&&\\ &Q&&\\ &&\ddots&\\ &&&Q \end{bmatrix} \] 综上，问题可以简化为： \[ Z_k=\Phi x_k+\Theta U_k \] 目标为： \[ \min_{U_k}(\parallel Z_{k}-Z_{r,k} \parallel^2_Q+\parallel U_{k} \parallel^2_R) \] 其解与如下最小二乘问题的解等价： \[ \min_{U_k}\parallel \begin{bmatrix} \sqrt{Q}\Theta\\ \sqrt{R} \end{bmatrix}U_k- \begin{bmatrix} \sqrt{Q}\\ \sqrt{0} \end{bmatrix}(Z_{r,k}-\Phi x_k) \parallel^2 \] 其最小二乘问题的解可表示为： \[ U_k=K(Z_{r.k}-\Phi x_k) \] \(K=\begin{bmatrix}\sqrt{Q}\Theta\\ \sqrt{R}\end{bmatrix}^{\top}\begin{bmatrix}\sqrt{Q}\\ 0\end{bmatrix}\)其中表示伪逆，具体的：\(A^{\top}=((A^TA)^{-1}A)\) 考虑到模型预测控制的原理是取最优输入序列的第一个值作为当前控制量，无约束模型预测控制轨迹跟踪问题的控制律可解析表示为： \[ u_k=\begin{bmatrix} 1&0&\cdots&0 \end{bmatrix}_N K(Z_{r.k}-\Phi x_k) \]