控制算法-跟踪误差动力学模型

概述

使用相对与目标曲线的位置和方向误差作为动力学模型的状态变量开发转向控制系统似乎更合适一些，对于车辆模型-动力学模型章节中的动力学模型，需重新定义一下误差变量：

• \(e_y\):车辆重心到目标曲线的距离；
• \(e_{\psi}\):车辆相对于目标曲线的方向误差；

假设车辆纵向速度\(V_x\)恒定且行驶路径的转弯半径\(R\)不变，其中转弯半径\(R\)足够大，以满足上述章节的小角度近似假设。

菊花展

基本概念

目标参数

• 定义车辆目标偏航角速度为

\[ \dot{\psi}_{des} = \frac{V_x}{R} \tag{1} \]

• 定义车辆目标向心加速度为

\[ a_{des} = \frac{V_x^2}{R} = V_x\frac{V_x}{R}=V_x\dot{\psi}_{des}\tag{2} \]

误差定义

• 定义车辆偏航角误差为

\[ e_{\psi} = \psi - \psi_{des} \tag{3} \]

• 定义车辆偏航角速度误差为

\[ \dot{e}_{\psi} = \dot{\psi} - \dot{\psi}_{des} \tag{4} \]

• 定义车辆偏航角加速度误差为

\[ \ddot{e}_{\psi} = \ddot{\psi} - \ddot{\psi}_{des} \tag{5} \]

• 定义车辆\(y\)轴方向的加速度误差为

\[ \ddot{e}_y = a_y - a_{des} \qquad\qquad\qquad\\ =(\ddot{y} + V_x\dot{\psi}) - V_x\dot{\psi}_{des} \\ =\ddot{y} + V_x(\dot{\psi} - \dot{\psi}_{des})\quad \tag{3} \]

• 定义车辆\(y\)轴方向的速度误差为

当车辆纵向速度恒定时，\(y\)轴方向的速度误差可以表示为

\[ \dot{e}_y = \int \ddot{e}_y\mathrm{d}t = \dot{y} + V_x(\psi - \psi_{des}) \tag{6} \]

当纵向速度不再恒定，随着时间变化时，对等式(3)积分得 \[ \dot{e}_y = \int \ddot{e}_y\mathrm{d}t = \dot{y} +\int V_x(\psi - \psi_{des})\mathrm{d}t \tag{7} \]

这就使得模型非线性且时变，不利于控制系统的设计。因此解决方法就是假设纵向速度是恒定，这就获得了一个线性时不变(LTI)模型。如果速度变化，LTI模型就需要使用线性参变模型(LPV)替代，这个模型中纵向速度是一个随着时间变化的参数。

跟踪误差动力学模型

上述等式(3)、(6)可以变换如下：

\[ \ddot{y} =\ddot{e}_y + V_x\dot{\psi}_{des} - V_x\dot{\psi}\tag{8} \]

\[ \dot{y} =\dot{e}_y - V_xe_{\psi} \tag{9} \]

根据车辆模型-动力学模型(Dynamics Model)章节中的等式(14)

\[ \ddot{y} = -\frac{2C_{\alpha f } + 2C_{\alpha r}}{mV_x}\dot{y} - ( V_x + \frac{2C_{\alpha f}l_f- 2C_{\alpha r}l_r}{mV_x})\dot{\psi} + \frac{2C_{\alpha f}}{m} \delta \tag{10} \]

将等式(8)和(9)代入等式(10)得

\[ \ddot{e}_y + V_x\dot{\psi}_{des} = -\frac{2C_{\alpha f } + 2C_{\alpha r}}{mV_x}(\dot{e}_y - V_xe_{\psi}) - ( V_x + \frac{2C_{\alpha f}l_f- 2C_{\alpha r}l_r}{mV_x})\dot{\psi} + \frac{2C_{\alpha f}}{m} \delta \tag{11} \]

对等式(11)进行简化，提取\(\ddot{e}_y\)、\(\dot{e}_y\)、\(e_y\)、\(\dot{\psi}_{des}\)和\(\delta\)项得

\[ \ddot{e}_y = \frac{-2C_{\alpha f}-2C_{\alpha r}}{mV_x}\dot{e}_y + \frac{2C_{\alpha f}+2C_{\alpha r}}{m}e_{\psi} + \frac{-2C_{\alpha f}l_f + 2C_{\alpha r}l_r}{mV_x}\dot{e}_{\psi} \\+ (\frac{-2C_{\alpha f}l_f + 2C_{\alpha r}l_r}{mV_x}-V_x)\dot{\psi}_{des} +\frac{2C_{\alpha f}}{m}\delta \tag{12} \]

整理成矩阵形式为

\[ \frac{\mathrm{d}}{\mathrm{d}t}\dot{e}_y = \begin{bmatrix} 0 & -\dfrac{2C_{\alpha f } + 2C_{\alpha r}}{mV_x} & \dfrac{2C_{\alpha f}+2C_{\alpha r}}{m} & \dfrac{-2C_{\alpha f}l_f + 2C_{\alpha r}l_r}{mV_x} \end{bmatrix} \begin{bmatrix} e_y\\ \dot{e}_y\\ e_{\psi}\\ \dot{e}_{\psi} \end{bmatrix}\\ +(\dfrac{-2C_{\alpha f}l_f + 2C_{\alpha r}l_r}{mV_x} - V_x)\dot{\psi}_{des}+\frac{2C_{\alpha f}}{m}\delta \tag{13} \]

同理根据车辆模型-动力学模型(Dynamics Model)章节中的等式(17)

\[ \ddot{\psi} = - \frac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_zV_x}\dot{y} - \frac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x}\dot{\psi} + \frac{2l_fC_{\alpha f}}{I_z}\delta\tag{14} \]

将等式(5)、(8)和(9)代入等式(14)得

\[ \ddot{e}_{\psi}+ \ddot{\psi}_{des} = - \frac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_zV_x}(\dot{e}_y - V_xe_{\psi}) - \frac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x}\dot{\psi} + \frac{2l_fC_{\alpha f}}{I_z}\delta\tag{15} \]

对等式(15)进行简化，提取\(\ddot{e}_y\)、\(\dot{e}_y\)、\(e_y\)、\(\dot{\psi}_{des}\)和\(\delta\)项得

\[ \ddot{e}_{\psi} = - \frac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_zV_x}\dot{e}_y + \frac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_z}e_{\psi}\\ -\frac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x}\dot{e}_{\psi} -\frac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x}\dot{\psi}_{des} + \frac{2l_fC_{\alpha f}}{I_z}\delta - \ddot{\psi}_{des} \tag{16} \]

由于上述假设为线性时不变系统(LTI)(\(\dot{V}_x = 0\))，故\(\ddot{\psi}_{des}=\frac{\dot{V}_x}{R} = 0\)，将上述等式整理成矩阵形式得

\[ \frac{\mathrm{d}}{\mathrm{d}t}\dot{e}_{\psi} = \begin{bmatrix} 0 & -\dfrac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_zV_x} & \dfrac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_z} & -\dfrac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x} \end{bmatrix} \begin{bmatrix} e_y\\ \dot{e}_y\\ e_{\psi}\\ \dot{e}_{\psi} \end{bmatrix}\\ -\frac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x}\dot{\psi}_{des}+\frac{2l_fC_{\alpha f}}{I_z}\delta \tag{17} \]

根据等式(13)和(17)，基于跟踪误差变量的状态空间模型表示为

\[ \frac{\mathrm{d}}{\mathrm{d}t} \begin{bmatrix} e_y\\ \dot{e}_y\\ e_{\psi}\\ \dot{e}_{\psi} \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & -\dfrac{2C_{\alpha f } + 2C_{\alpha r}}{mV_x} & \dfrac{2C_{\alpha f}+2C_{\alpha r}}{m} & \dfrac{-2C_{\alpha f}l_f + 2C_{\alpha r}l_r}{mV_x}\\ 0 & 0 & 0 & 1\\ 0 & -\dfrac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_zV_x} & \dfrac{2l_fC_{\alpha f} - 2l_rC_{\alpha r}}{I_z} & -\dfrac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x} \end{bmatrix} \begin{bmatrix} e_y\\ \dot{e}_y\\ e_{\psi}\\ \dot{e}_{\psi} \end{bmatrix}\\ + \begin{bmatrix} 0\\ \dfrac{2C_{\alpha f}}{m}\\ 0\\ \dfrac{2l_fC_{\alpha f}}{I_z} \end{bmatrix}\delta+ \begin{bmatrix} 0\\ (\dfrac{-2C_{\alpha f}l_f + 2C_{\alpha r}l_r}{mV_x} - V_x)\\ 0\\ -\dfrac{2{l_f}^2C_{\alpha f} + 2{l_r}^2C_{\alpha r}}{I_zV_x} \end{bmatrix}\dot{\psi}_{des}\tag{18} \]

上述状态空间模型，将转向控制系统的目标跟踪问题转化为动力学的稳定性问题。 状态空间模型一般形式如下：

\[ \dot{x} = Ax + B_1\delta + B_2\dot{\psi}_{des} \]