【自动驾驶】动力学横向控制误差模型

文章目录

  • 参考资料
  • 1. 基本概述
  • 2. 误差动力学模型

参考资料

  • 车辆模型-跟踪误差模型
  • Vehicle Dynamics and Control

1. 基本概述

车辆横向控制主要通过控制轮胎转角实现,而对于驾驶员来说,可直接操控的是方向盘角度,因此在搭建车辆动力学模型时,可以建立以相对于道路的方向和距离误差为状态变量的动力学模型。

【自动驾驶】动力学横向控制误差模型_第1张图片

假设——

  • 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) \tag{1} \dot\psi_{des}=\frac{V_x}{R} ψ˙des=RVx(1)

  • 所需的横向加速度(即期望的向心加速度)为
    a d e s = V x 2 R = V x ψ ˙ d e s (2) \tag{2} a_{des}=\frac{V_x^2}{R}=V_x\dot\psi_{des} ades=RVx2=Vxψ˙des(2)

  • 车辆偏航角误差

e ψ = ψ − ψ d e s (3) \tag{3} e_{\psi}=\psi-\psi_{d e s} eψ=ψψdes(3)

  • 车辆偏航角速度误差
    e ˙ ψ = ψ ˙ − ψ ˙ des  (4) \tag{4} \dot{e}_{\psi}=\dot{\psi}-\dot{\psi}_{\text {des }} e˙ψ=ψ˙ψ˙des (4)

  • 车辆偏航角加速度误差
    e ¨ ψ = ψ ¨ − ψ ¨ des  (5) \tag{5} \ddot{e}_{\psi}=\ddot{\psi}-\ddot{\psi}_{\text {des }} e¨ψ=ψ¨ψ¨des (5)

  • 车辆 横向加速度误差
    e ¨ y = a y − a d e s = ( y ¨ + V x ψ ˙ ) − V x ψ ˙ des  = y ¨ + V x ( ψ ˙ − ψ ˙ d e s ) (6) \tag{6} \begin{aligned} \ddot{e}_{y} &=a_{y}-a_{d e s} \\ &=\left(\ddot{y}+V_{x} \dot{\psi}\right)-V_{x} \dot{\psi}_{\text {des }} \\ &=\ddot{y}+V_{x}\left(\dot{\psi}-\dot{\psi}_{d e s}\right) \end{aligned} e¨y=ayades=(y¨+Vxψ˙)Vxψ˙des =y¨+Vx(ψ˙ψ˙des)(6)
    其中, a y a_y ay由两 种力共同作用产生 : 车辆延 y y y 轴产生的惯性加速度 y ¨ \ddot{y} y¨ 和车辆绕旋转中心 O O O 旋转产生的向心加速度 a c = V x 2 R = V x ψ ˙ ∘ a_{c}=\frac{{V_x}^2}{R}=V_{x} \dot{\psi}_{\circ} ac=RVx2=Vxψ˙

  • 车辆 横向速度误差

    当车辆纵向速度 V x V_x Vx恒定时, y y y 轴方向的速度误差可以表示为
    e ˙ y = ∫ e ¨ y   d t = y ˙ + V x ( ψ − ψ d e s ) (7) \tag{7} \dot{e}_{y}=\int \ddot{e}_{y} \mathrm{~d} t=\dot{y}+V_{x}\left(\psi-\psi_{d e s}\right) e˙y=e¨y dt=y˙+Vx(ψψdes)(7)
    当纵向速度 V x V_x Vx随着时间变化时,对等式(6)积分得
    e ˙ y = ∫ e ¨ y   d t = ∫ y ¨ + V x ( ψ ˙ − ψ ˙ des  ) d t = y ˙ + ∫ V x ( ψ ˙ − ψ ˙ des  ) d t (8) \tag{8} \dot{e}_{y}=\int \ddot{e}_{y} \mathrm{~d} t=\int \ddot{y}+V_{x}\left(\dot{\psi}-\dot\psi_{\text {des }}\right) \mathrm{d} t=\dot{y}+\int V_{x}\left(\dot\psi-\dot\psi_{\text {des }}\right) \mathrm{d} t e˙y=e¨y dt=y¨+Vx(ψ˙ψ˙des )dt=y˙+Vx(ψ˙ψ˙des )dt(8)

    参考资料中的等式(8)为
    e ˙ y = ∫ e ¨ y   d t = y ˙ + ∫ V x ( ψ − ψ des  ) d t \dot{e}_{y}=\int \ddot{e}_{y} \mathrm{~d} t=\dot{y}+\int V_{x}\left(\psi-\psi_{\text {des }}\right) \mathrm{d} t e˙y=e¨y dt=y˙+Vx(ψψdes )dt
    根据一步一步推导看的话书中的这个等式应该是有问题的,这里先贴在这,有懂的朋友也可以帮忙解惑一下~~

    这就使得模型非线性且时变,不利于控制系统的设计。因此这里假设纵向速度 V x V_x Vx恒定, 即当作一个线性时不变(LTI)模型。如果速度变化,就需要使用线性参变模型(LPV) 替代, 其纵向速度是一个随着时间变化的参数。

2. 误差动力学模型

将等式(6)(7)变换如下:

y ¨ = e ¨ y + V x ψ ˙ d e s − V x ψ ˙ y ˙ = e ˙ y − V x e ψ (9) \tag{9} \begin{aligned} &\ddot{y}=\ddot{e}_{y}+V_{x} \dot{\psi}_{d e s}-V_{x} \dot{\psi}\\ &\dot{y}=\dot{e}_{y}-V_{x} e_{\psi} \end{aligned} y¨=e¨y+Vxψ˙desVxψ˙y˙=e˙yVxeψ(9)
根据车辆动力学模型中的等式(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) \tag{10} \ddot{y}=-\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} \dot{y}-\left(V_{x}+\frac{2 C_{\alpha f} l_{f}-2 C_{\alpha r} l_{r}}{m V_{x}}\right) \dot{\psi}+\frac{2 C_{\alpha f}}{m} \delta y¨=mVx2Cαf+2Cαry˙(Vx+mVx2Cαflf2Cαrlr)ψ˙+m2Cαfδ(10)

将等式(4)(9)代入等式(10)得

e ¨ y + V x ψ ˙ d e s − V x ( e ˙ ψ + ψ ˙ des  ) = − 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 ) ( e ˙ ψ + ψ ˙ des  ) + 2 C α f m δ (11) \tag{11} \begin{aligned} \ddot{e}_{y}+V_{x} \dot{\psi}_{d e s}-V_{x} \left(\dot{e}_{\psi}+\dot{\psi}_{\text {des }}\right)&=-\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} \left(\dot{e}_{y}-V_{x} e_{\psi}\right)-\\ &\left(V_{x}+\frac{2 C_{\alpha f} l_{f}-2 C_{\alpha r} l_{r}}{m V_{x}}\right) \left(\dot{e}_{\psi}+\dot{\psi}_{\text {des }}\right)+\frac{2 C_{\alpha f}}{m} \delta \end{aligned} e¨y+Vxψ˙desVx(e˙ψ+ψ˙des )=mVx2Cαf+2Cαr(e˙yVxeψ)(Vx+mVx2Cαflf2Cαrlr)(e˙ψ+ψ˙des )+m2Cαfδ(11)
对等式(11)提取 e ¨ y \ddot{e}_y e¨y​、 e ˙ y \dot{e}_y e˙y​、 e y e_y ey​、 e ψ e_{\psi} eψ e ˙ ψ \dot e_{\psi} e˙ψ ψ ˙ 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 ) ψ ˙ des  + 2 C α f m δ (12) \tag{12} \begin{gathered} \ddot{e}_{y}=\frac{-2 C_{\alpha f}-2 C_{\alpha r}}{m V_{x}} \dot{e}_{y}+\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} e_{\psi}+\frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}} \dot{e}_{\psi} \\ +\left(\frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x}\right) \dot{\psi}_{\text {des }}+\frac{2 C_{\alpha f}}{m} \delta \end{gathered} e¨y=mVx2Cαf2Cαre˙y+m2Cαf+2Cαreψ+mVx2Cαflf+2Cαrlre˙ψ+(mVx2Cαflf+2CαrlrVx)ψ˙des +m2Cαfδ(12)
整理成矩阵形式为

d d t e ˙ y = [ 0 − 2 C α f + 2 C α r 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) \tag{13} \frac{\mathrm{d}}{\mathrm{d} t} \dot{e}_{y}=\left[\begin{array}{llll}0 & -\frac{2 C_{\alpha f}+2 C_{\alpha r} r}{m V_{x}} & \frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} & \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}\end{array}\right]\left[\begin{array}{c}e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi}\end{array}\right]+\\ \left(\frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x}\right) \dot{\psi}_{d e s}+\frac{2 C_{\alpha f}}{m} \delta dtde˙y=[0mVx2Cαf+2Cαrrm2Cαf+2CαrmVx2Cαflf+2Cαrlr]eye˙yeψe˙ψ+(mVx2Cαflf+2CαrlrVx)ψ˙des+m2Cαfδ(13)

同理可得,根据车辆动力学模型中的等式(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) \tag{14} \ddot{\psi}=-\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} v_{x}} \dot{y}-\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} v_{x}} \dot{\psi}+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta ψ¨=Izvx2lfCαf2lrCαry˙Izvx2lf2Cαf+2lr2Cαrψ˙+Iz2lfCαfδ(14)

将等式(4)(5)(9)代入等式(14),得
e ¨ ψ + ψ ¨ des  = − 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 ( e ˙ ψ + ψ ˙ des  ) + 2 l f C α f I z δ (15) \tag{15} \begin{aligned} \ddot{e}_{\psi}+\ddot{\psi}_{\text {des }}=-\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}}\left(\dot{e}_{y}-V_{x} e_{\psi}\right) -\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \left(\dot{e}_{\psi}+\dot{\psi}_{\text {des }}\right)+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta \end{aligned} e¨ψ+ψ¨des =IzVx2lfCαf2lrCαr(e˙yVxeψ)IzVx2lf2Cαf+2lr2Cαr(e˙ψ+ψ˙des )+Iz2lfCαfδ(15)

对等式(15)提取 e ˙ y \dot{e}_y e˙y​、 e y e_y ey​、 e ψ e_{\psi} eψ e ¨ ψ \ddot{e}_{\psi} e¨ψ​、 e ˙ ψ \dot e_{\psi} e˙ψ ψ ˙ 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) \tag{16} \begin{gathered} \ddot{e}_{\psi}=-\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} \dot{e}_{y}+\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} e_{\psi} \\ -\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \dot{e}_{\psi}-\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \dot{\psi}_{d e s}+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta-\ddot{\psi}_{d e s} \end{gathered} e¨ψ=IzVx2lfCαf2lrCαre˙y+Iz2lfCαf2lrCαreψIzVx2lf2Cαf+2lr2Cαre˙ψIzVx2lf2Cαf+2lr2Cαrψ˙des+Iz2lfCαfδψ¨des(16)
由于上述假设为线性时不变系统 ( L T 1 ) ( V ˙ x = 0 ) (L T 1)\left(\dot{V}_{x}=0\right) (LT1)(V˙x=0) ,故 ψ ¨ d e s = V ˙ x R = 0 \ddot{\psi}_{d e s}=\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) \tag{17} \begin{aligned} \frac{\mathrm{d}}{\mathrm{d} t} \dot{e}_{\psi}=&\left[\begin{array}{llll} 0 & -\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} & \frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} & -\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\left[\begin{array}{c} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right] \\ &-\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \dot{\psi}_{d e s}+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta \end{aligned} dtde˙ψ=[0IzVx2lfCαf2lrCαrIz2lfCαf2lrCαrIzVx2lf2Cα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 ψ ˙ d e s 0 − 2 l f 2 C α f + 2 l r 2 C α r I z V x ] ψ ˙ d e s (18) \tag{18} \begin{aligned} \frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right]=& {\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & -\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} & \frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} & \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} & \frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} & -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right] } \\ &+\left[\begin{array}{c} 0 \\ \frac{2 C_{\alpha f}}{m} \\ 0 \\ \frac{2 l_{f} C_{\alpha f}}{I_{z}} \end{array}\right] \delta+\left[\begin{array}{c} 0\\ \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x} \\ \dot{\psi}_{d e s} \\ 0\\ -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\dot\psi_{des} \end{aligned} dtdeye˙yeψe˙ψ=00001mVx2Cαf+2Cαr0IzVx2lfCαf2lrCαr0m2Cαf+2Cαr0Iz2lfCαf2lrCαr0mVx2Cαflf+2Cαrlr1IzVx2lf2Cαf+2lr2Cαreye˙yeψe˙ψ+0m2Cαf0Iz2lfCαfδ+0mVx2Cαflf+2CαrlrVxψ˙des0IzVx2lf2Cαf+2lr2Cαrψ˙des(18)

写成一般形式,状态空间表达式如下:
t a g 19 x ˙ = A x + B 1 δ + B 2 ψ ˙ des  tag{19} \dot{x}=A x+B_{1} \delta+B_{2} \dot{\psi}_{\text {des }} tag19x˙=Ax+B1δ+B2ψ˙des 

如果考虑路面坡度角(road bank angle) ϕ \phi ϕ(定义可参考前文动力学模型),则等式(18)变为
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 ψ ˙ d e s 0 − 2 l f 2 C α f + 2 l r 2 C α r I z V x ] ψ ˙ d e s + [ 0 g 0 0 ] sin ⁡ ϕ (18) \tag{18} \begin{aligned} \frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right]=& {\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & -\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} & \frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} & \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} & \frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} & -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right] } \\ &+\left[\begin{array}{c} 0 \\ \frac{2 C_{\alpha f}}{m} \\ 0 \\ \frac{2 l_{f} C_{\alpha f}}{I_{z}} \end{array}\right] \delta+\left[\begin{array}{c} 0\\ \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x} \\ \dot{\psi}_{d e s} \\ 0\\ -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\dot\psi_{des}+\left[\begin{array}{c} 0 \\ g \\ 0 \\ 0 \end{array}\right] \sin{\phi} \end{aligned} dtdeye˙yeψe˙ψ=00001mVx2Cαf+2Cαr0IzVx2lfCαf2lrCαr0m2Cαf+2Cαr0Iz2lfCαf2lrCαr0mVx2Cαflf+2Cαrlr1IzVx2lf2Cαf+2lr2Cαreye˙yeψe˙ψ+0m2Cαf0Iz2lfCαfδ+0mVx2Cαflf+2CαrlrVxψ˙des0IzVx2lf2Cαf+2lr2Cαrψ˙des+0g00sinϕ(18)

你可能感兴趣的:(自动驾驶,自动驾驶,人工智能,机器学习)