车辆模型-跟踪误差模型
文章目录
- 概述
- 基本概念
- 目标参数
- 误差定义
- 跟踪误差动力学模型
概述
使用相对与目标曲线的位置和方向误差作为动力学模型的状态变量开发转向控制系统似乎更合适一些，对于车辆模型-动力学模型(Dynamics Model)章节中的动力学模型，需重新定义一下误差变量：
- e y e_y ey:车辆重心到目标曲线的距离；
- e ψ e_{\psi} eψ:车辆相对于目标曲线的方向误差；
假设车辆纵向速度 V x V_x Vx恒定且行驶路径的转弯半径 R R R不变，其中转弯半径 R R R足够大，以满足上述章节的小角度近似假设。
基本概念
目标参数
- 定义车辆目标偏航角速度为
ψ ˙ d e s = V x R (1) \dot{\psi}_{des} = \frac{V_x}{R} \tag{1} ψ˙des=RVx(1) - 定义车辆目标向心加速度为
a d e s = V x 2 R = V x V x R = V x ψ ˙ d e s (2) a_{des} = \frac{V_x^2}{R} = V_x\frac{V_x}{R}=V_x\dot{\psi}_{des}\tag{2} ades=RVx2=VxRVx=Vxψ˙des(2)
误差定义
- 定义车辆偏航角误差为
e ψ = ψ − ψ d e s (3) e_{\psi} = \psi - \psi_{des} \tag{3} eψ=ψ−ψdes(3) - 定义车辆偏航角速度误差为
e ˙ ψ = ψ ˙ − ψ ˙ d e s (4) \dot{e}_{\psi} = \dot{\psi} - \dot{\psi}_{des} \tag{4} e˙ψ=ψ˙−ψ˙des(4) - 定义车辆偏航角加速度误差为
e ¨ ψ = ψ ¨ − ψ ¨ d e s (5) \ddot{e}_{\psi} = \ddot{\psi} - \ddot{\psi}_{des} \tag{5} e¨ψ=ψ¨−ψ¨des(5) - 定义车辆 y y y轴方向的加速度误差为
e ¨ y = a y − a d e s = ( y ¨ + V x ψ ˙ ) − V x ψ ˙ d e s = y ¨ + V x ( ψ ˙ − ψ ˙ d e s ) (3) \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} e¨y=ay−ades=(y¨+Vxψ˙)−Vxψ˙des=y¨+Vx(ψ˙−ψ˙des)(3) - 定义车辆 y y y轴方向的速度误差为
当车辆纵向速度恒定时， y y y轴方向的速度误差可以表示为
e ˙ y = ∫ e ¨ y d t = y ˙ + V x ( ψ − ψ d e s ) (6) \dot{e}_y = \int \ddot{e}_y\mathrm{d}t = \dot{y} + V_x(\psi - \psi_{des}) \tag{6} e˙y=∫e¨ydt=y˙+Vx(ψ−ψdes)(6)
当纵向速度不再恒定，随着时间变化时，对等式(3)积分得
e ˙ y = ∫ e ¨ y d t = y ˙ + ∫ V x ( ψ − ψ d e s ) d t (7) \dot{e}_y = \int \ddot{e}_y\mathrm{d}t = \dot{y} +\int V_x(\psi - \psi_{des})\mathrm{d}t \tag{7} e˙y=∫e¨ydt=y˙+∫Vx(ψ−ψdes)dt(7)
这就使得模型非线性且时变，不利于控制系统的设计。因此解决方法就是假设纵向速度是恒定，这就获得了一个线性时不变(LTI)模型。如果速度变化，LTI模型就需要使用线性参变模型(LPV)替代，这个模型中纵向速度是一个随着时间变化的参数。
跟踪误差动力学模型
上述等式(3)、(6)可以变换如下：
y ¨ = e ¨ y + V x ψ ˙ d e s − V x ψ ˙ (8) \ddot{y} =\ddot{e}_y + V_x\dot{\psi}_{des} - V_x\dot{\psi}\tag{8} y¨=e¨y+Vxψ˙des−Vxψ˙(8)
y ˙ = e ˙ y − V x e ψ (9) \dot{y} =\dot{e}_y - V_xe_{\psi} \tag{9} y˙=e˙y−Vxeψ(9)
根据车辆模型-动力学模型(Dynamics Model)章节中的等式(14)
y ¨ = − 2 C α f + 2 C α r m V x y ˙ − ( V x + 2 C α f l f − 2 C α r l r m V x ) ψ ˙ + 2 C α f m δ (10) \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} y¨=−mVx2Cαf+2Cαry˙−(Vx+mVx2Cαflf−2Cαrlr)ψ˙+m2Cαfδ(10)
将等式(8)和(9)代入等式(10)得
e ¨ y + V x ψ ˙ d e s = − 2 C α f + 2 C α r m V x ( e ˙ y − V x e ψ ) − ( V x + 2 C α f l f − 2 C α r l r m V x ) ψ ˙ + 2 C α f m δ (11) \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} e¨y+Vxψ˙des=−mVx2Cαf+2Cαr(e˙y−Vxeψ)−(Vx+mVx2Cαflf−2Cαrlr)ψ˙+m2Cαfδ(11)
对等式(11)进行简化，提取 e ¨ y \ddot{e}_y e¨y、 e ˙ y \dot{e}_y e˙y、 e y e_y ey、 ψ ˙ d e s \dot{\psi}_{des} ψ˙des和 δ \delta δ项得
e ¨ y = − 2 C α f − 2 C α r m V x e ˙ y + 2 C α f + 2 C α r m e ψ + − 2 C α f l f + 2 C α r l r m V x e ˙ ψ + ( − 2 C α f l f + 2 C α r l r m V x − V x ) ψ ˙ d e s + 2 C α f m δ (12) \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} e¨y=mVx−2Cαf−2Cαre˙y+m2Cαf+2Cαreψ+mVx−2Cαflf+2Cαrlre˙ψ+(mVx−2Cαflf+2Cαrlr−Vx)ψ˙des+m2Cαfδ(12)
整理成矩阵形式为
d d t e ˙ y = [ 0 − 2 C α f + 2 C α r m V x 2 C α f + 2 C α r m − 2 C α f l f + 2 C α r l r m V x ] [ e y e ˙ y e ψ e ˙ ψ ] + ( − 2 C α f l f + 2 C α r l r m V x − V x ) ψ ˙ d e s + 2 C α f m δ (13) \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} dtde˙y=[0−mVx2Cαf+2Cαrm2Cαf+2CαrmVx−2Cαflf+2Cαrlr]⎣⎢⎢⎡eye˙yeψe˙ψ⎦⎥⎥⎤+(mVx−2Cαflf+2Cαrlr−Vx)ψ˙des+m2Cαfδ(13)
同理根据车辆模型-动力学模型(Dynamics Model)章节中的等式(17)
ψ ¨ = − 2 l f C α f − 2 l r C α r I z V x y ˙ − 2 l f 2 C α f + 2 l r 2 C α r I z V x ψ ˙ + 2 l f C α f I z δ (14) \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} ψ¨=−IzVx2lfCαf−2lrCαry˙−IzVx2lf2Cαf+2lr2Cαrψ˙+Iz2lfCαfδ(14)
将等式(5)、(8)和(9)代入等式(14)得
e ¨ ψ + ψ ¨ d e s = − 2 l f C α f − 2 l r C α r I z V x ( e ˙ y − V x e ψ ) − 2 l f 2 C α f + 2 l r 2 C α r I z V x ψ ˙ + 2 l f C α f I z δ (15) \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} e¨ψ+ψ¨des=−IzVx2lfCαf−2lrCαr(e˙y−Vxeψ)−IzVx2lf2Cαf+2lr2Cαrψ˙+Iz2lfCαfδ(15)
对等式(15)进行简化，提取 e ¨ y \ddot{e}_y e¨y、 e ˙ y \dot{e}_y e˙y、 e y e_y ey、 ψ ˙ d e s \dot{\psi}_{des} ψ˙des和 δ \delta δ项得
e ¨ ψ = − 2 l f C α f − 2 l r C α r I z V x e ˙ y + 2 l f C α f − 2 l r C α r I z e ψ − 2 l f 2 C α f + 2 l r 2 C α r I z V x e ˙ ψ − 2 l f 2 C α f + 2 l r 2 C α r I z V x ψ ˙ d e s + 2 l f C α f I z δ − ψ ¨ d e s (16) \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} e¨ψ=−IzVx2lfCαf−2lrCαre˙y+Iz2lfCαf−2lrCαreψ−IzVx2lf2Cαf+2lr2Cαre˙ψ−IzVx2lf2Cαf+2lr2Cαrψ˙des+Iz2lfCαfδ−ψ¨des(16)
由于上述假设为线性时不变系统(LTI)( V ˙ x = 0 \dot{V}_x = 0 V˙x=0)，故 ψ ¨ d e s = V ˙ x R = 0 \ddot{\psi}_{des}=\frac{\dot{V}_x}{R} = 0 ψ¨des=RV˙x=0，将上述等式整理成矩阵形式得
d d t e ˙ ψ = [ 0 − 2 l f C α f − 2 l r C α r I z V x 2 l f C α f − 2 l r C α r I z − 2 l f 2 C α f + 2 l r 2 C α r I z V x ] [ e y e ˙ y e ψ e ˙ ψ ] − 2 l f 2 C α f + 2 l r 2 C α r I z V x ψ ˙ d e s + 2 l f C α f I z δ (17) \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} dtde˙ψ=[0−IzVx2lfCαf−2lrCαrIz2lfCαf−2lrCαr−IzVx2lf2Cαf+2lr2Cαr]⎣⎢⎢⎡eye˙yeψe˙ψ⎦⎥⎥⎤−IzVx2lf2Cαf+2lr2Cαrψ˙des+Iz2lfCαfδ(17)
根据等式(13)和(17)，基于跟踪误差变量的状态空间模型表示为
d d t [ e y e ˙ y e ψ e ˙ ψ ] = [ 0 1 0 0 0 − 2 C α f + 2 C α r m V x 2 C α f + 2 C α r m − 2 C α f l f + 2 C α r l r m V x 0 0 0 1 0 − 2 l f C α f − 2 l r C α r I z V x 2 l f C α f − 2 l r C α r I z − 2 l f 2 C α f + 2 l r 2 C α r I z V x ] [ e y e ˙ y e ψ e ˙ ψ ] + [ 0 2 C α f m 0 2 l f C α f I z ] δ + [ 0 ( − 2 C α f l f + 2 C α r l r m V x − V x ) 0 − 2 l f 2 C α f + 2 l r 2 C α r I z V x ] ψ ˙ d e s (18) \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} dtd⎣⎢⎢⎡eye˙yeψe˙ψ⎦⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡00001−mVx2Cαf+2Cαr0−IzVx2lfCαf−2lrCαr0m2Cαf+2Cαr0Iz2lfCαf−2lrCαr0mVx−2Cαflf+2Cαrlr1−IzVx2lf2Cαf+2lr2Cαr⎦⎥⎥⎥⎥⎥⎤⎣⎢⎢⎡eye˙yeψe˙ψ⎦⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎡0m2Cαf0Iz2lfCαf⎦⎥⎥⎥⎥⎥⎤δ+⎣⎢⎢⎢⎢⎢⎡0(mVx−2Cαflf+2Cαrlr−Vx)0−IzVx2lf2Cαf+2lr2Cαr⎦⎥⎥⎥⎥⎥⎤ψ˙des(18)
上述状态空间模型，将转向控制系统的目标跟踪问题转化为动力学的稳定性问题。
状态空间模型一般形式如下：
x ˙ = A x + B 1 δ + B 2 ψ ˙ d e s \dot{x} = Ax + B_1\delta + B_2\dot{\psi}_{des} x˙=Ax+B1δ+B2ψ˙des