Dynamic Model

정승균·2021년 2월 2일

자율주행 제어

목록 보기

2/8

F_{yf} = C_{\alpha f}\alpha_f \\ F_{yr} = C_{\alpha r}\alpha_r

Assumptions :

small steer angle at front wheel

0 steer angle at rear wheel

v_x is constant

linear tire model

\vec{X_F} = \vec{X_{CG}} + l_f\vec{i_x}\\ \vec{V_F} = \dot{\vec{X_F}} = \vec{V_{CG}} + l_f\dot{\psi}\vec{i_y} = v_x\vec{i_x} + (v_y + l_f\dot{\psi})\vec{i_y} \quad ...(1)

\vec{X_R} = \vec{X_{CG}} - l_r\vec{i_x}\\ \vec{V_R} = \dot{\vec{X_R}} = \vec{V_{CG}} - l_r\dot{\psi}\vec{i_y} = v_x\vec{i_x} + (v_y - l_f\dot{\psi})\vec{i_y} \quad ...(2)

\tan{(\delta - \alpha _f)} = \frac{v_{fy}}{v_{fx}} = \frac{v_y + l_f\dot{\psi}}{v_x}\\ \tan{(-\alpha _r)} = \frac{v_{ry}}{v_{rx}} = \frac{v_y - l_r\dot{\psi}}{v_x}

\alpha_f \approx \delta - \frac{v_y + l_f\dot{\psi}}{v_x} \quad ...(3)\\ \alpha_r \approx -\frac{v_y - l_r\dot{\psi}}{v_x} \quad ...(4)

F_{yf} = C_{\alpha f}\alpha_f = C_{\alpha f}(\delta - \frac{v_y + l_f\dot{\psi}}{v_x}) \quad ...(5) \\ F_{yr} = C_{\alpha r}\alpha_r = C_{\alpha r}(-\frac{v_y - l_r\dot{\psi}}{v_x}) \quad ...(6)

\vec{V} = v_x\vec{i_x} + v_y\vec{i_y}\\ \vec{A} = \dot{\vec{V}} = (\dot{v_x}+\dot\psi v_y)\vec{i_x} + (\dot{v_y}+\dot\psi v_x)\vec{i_y} \\ \therefore \; A_y = \dot{v_y}+\dot\psi v_x \quad ...(7)

\sum F_y = mA_y\\ \sum F_y \approx F_{yf} + F_{yr} = C_{\alpha f}(\delta - \frac{v_y + l_f\dot{\psi}}{v_x}) + C_{\alpha r}(-\frac{v_y - l_r\dot{\psi}}{v_x}) \quad and \quad mA_y = m(\dot{v_y}+\dot\psi v_x) \\ \therefore \; C_{\alpha f}(\delta - \frac{v_y + l_f\dot{\psi}}{v_x}) + C_{\alpha r}(-\frac{v_y - l_r\dot{\psi}}{v_x}) = m(\dot{v_y}+\dot\psi v_x) \quad

\dot v_y = -(\frac{C_{\alpha f}+C_{\alpha r}}{mv_x})\;v_y + (\frac{C_{\alpha r}l_r - C_{\alpha f}l_f}{mv_x} -v_x)\;\dot \psi \; + (\frac{C_{\alpha f}}{m})\; \delta \\ \therefore \; \ddot y = -(\frac{C_{\alpha f}+C_{\alpha r}}{mv_x})\;\dot y + (\frac{C_{\alpha r}l_r - C_{\alpha f}l_f}{mv_x} -v_x)\;\dot \psi \; + (\frac{C_{\alpha f}}{m})\; \delta \quad ...(8) \\

\sum \tau = I_z\ddot \psi \\ \sum \tau \approx l_f F_{yf} - l_r F_{yr} = l_f C_{\alpha f}(\delta - \frac{v_y + l_f\dot{\psi}}{v_x}) - l_rC_{\alpha r}(-\frac{v_y - l_r\dot{\psi}}{v_x}) = I_z\ddot \psi

\ddot \psi = (\frac{C_{\alpha r}l_r - C_{\alpha f}l_f}{I_z v_x})\;v_y \; - (\frac{C_{\alpha r}{l_r}^2 + C_{\alpha r}{l_r}^2}{I_z v_x}) \; + (\frac{C_{\alpha f}l_f}{I_z})\; \delta\\ \therefore \; \ddot \psi = (\frac{C_{\alpha r}l_r - C_{\alpha f}l_f}{I_z v_x})\;\dot y \; - (\frac{C_{\alpha r}{l_r}^2 + C_{\alpha r}{l_r}^2}{I_z v_x}) \; + (\frac{C_{\alpha f}l_f}{I_z})\; \delta \quad ...(9)

\frac{d}{dt} \begin{bmatrix} y\\ \dot y\\ \psi\\ \dot \psi \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & -(\frac{C_{\alpha f}+C_{\alpha r}}{mv_x}) & 0 & (\frac{C_{\alpha r}l_r - C_{\alpha f}l_f}{mv_x} -v_x) \\ 0 & 0 & 0 & 1\\ 0 & (\frac{C_{\alpha r}l_r - C_{\alpha f}l_f}{I_z v_x}) & 0 & -(\frac{C_{\alpha r}{l_r}^2 + C_{\alpha r}{l_r}^2}{I_z v_x}) \end{bmatrix} \begin{bmatrix} y\\ \dot y\\ \psi\\ \dot \psi \end{bmatrix} + \begin{bmatrix} 0\\ (\frac{C_{\alpha f}}{m})\\ 0\\ (\frac{C_{\alpha f}l_f}{I_z}) \end{bmatrix} \delta

m\ddot{x} = F_{xf}+F_{xr} -F_{aero} - R_{xf} -R_{xr} -mg\sin{\theta}