车辆运动控制（7）考虑道路倾角和曲率
- 1. 考虑道路倾角及曲率影响
-
考虑道路倾角及曲率影响
道路倾角与曲率对无人驾驶车辆的操纵稳定性有着重要影响
直接关系到车辆动力学模型的准确程度
结合《车辆运动控制（6）考虑侧倾约束》得到的综合考虑横摆、侧滑和侧候约束的车辆动力学模型
建立考虑道路倾角及曲率影响的车辆动力学模型，如图所示：
考虑侧倾约束的车辆动力学模型跟踪时变曲率参考道路的跟踪误差方程可由式(11)得到：
{ e φ ˙ = φ ˙ − κ r e f v x e d ˙ = v x e φ + v y \begin{cases} \dot{e_\varphi}=\dot\varphi-\kappa_{ref}v_x\ \dot{e_d}=v_xe_\varphi+ v_y \end{cases}{e φ˙=φ˙−κr e f v x e d ˙=v x e φ+v y
考虑存在道路倾角中的情况如图：
车辆质心处受到的合力在y轴上的分力可表示为：
F y = F y f + F y r − m g sin ϕ t = F y f + F y r − m g ϕ t (39) F_y=F_{yf}+F_{yr}-mg\sin\phi_t=F_{yf}+F_{yr}-mg\phi_t\tag{39}F y =F y f +F y r −m g sin ϕt =F y f +F y r −m g ϕt (3 9 )
由于车辆悬架系统变形产生的车体侧倾角为中 ϕ r = ϕ − ϕ t \phi_r=\phi-\phi_t ϕr =ϕ−ϕt ，其变化率为 ϕ ˙ r = ϕ ˙ − ϕ ˙ t \dot\phi_r=\dot\phi-\dot\phi_t ϕ˙r =ϕ˙−ϕ˙t
假设 ϕ ˙ t ≈ 0 \dot\phi_t≈0 ϕ˙t ≈0，则车辆悬架系统所产生的侧倾阻力矩M x M_x M x 可以表示为
M x = K ϕ ( ϕ − ϕ t ) + D ϕ ϕ ˙ (40) M_x=K_{\phi}(\phi-\phi_t)+D_{\phi}\dot{\phi}\tag{40}M x =K ϕ(ϕ−ϕt )+D ϕϕ˙(4 0 )
此时，式( 36 ) (36)(3 6 ) 和 式( 38 ) (38)(3 8 ) 可改写为：
m v ˙ y = − m v x φ ˙ + m h C G φ ¨ + ( F y f + F y r ) − m g ϕ (41) m \dot{v}y = – mv_x\dot{\varphi}+mh{CG}\ddot{\varphi}+(F_{yf}+F_{yr})-mg\phi \tag{41}m v ˙y =−m v x φ˙+m h C G φ¨+(F y f +F y r )−m g ϕ(4 1 )
I x ϕ ¨ = m h C G ( v ˙ y + φ ˙ v x − h C G ϕ ¨ ) + m g h C G ϕ − K ϕ ( ϕ − ϕ t ) − D ϕ ϕ ˙ (42) I_x\ddot{\phi}=mh_{CG}( \dot{v}y+\dot{\varphi}v_x-h{CG}\ddot{\phi})+mgh_{CG}\phi-K_{\phi}(\phi-\phi_t)-D_{\phi}\dot{\phi} \tag{42}I x ϕ¨=m h C G (v ˙y +φ˙v x −h C G ϕ¨)+m g h C G ϕ−K ϕ(ϕ−ϕt )−D ϕϕ˙(4 2 )
将 式( 22 ) (22)(2 2 ) 和 式( 23 ) (23)(2 3 ) 代人 式( 41 ) (41)(4 1 )、式( 38 ) (38)(3 8 ) 和 式( 42 ) (42)(4 2 ) 可以得到：
m v ˙ y − m h C G φ ¨ + ( m v x − l f C ˉ a f − l r C ˉ a r v x ) φ ˙ − ( C ˉ a f + C ˉ a r v x ) v y + m g ϕ = − C ˉ a f δ f (44) m \dot{v}y -mh{CG}\ddot{\varphi}+ (mv_x-\frac{l_f\bar{C}{af}-l_r\bar{C}{ar}}{v_x})\dot{\varphi}-(\frac{\bar{C}{af}+\bar{C}{ar}}{v_x})v_y+mg\phi =-\bar{C}_{af}\delta_f \tag{44}m v ˙y −m h C G φ¨+(m v x −v x l f C ˉa f −l r C ˉa r )φ˙−(v x C ˉa f +C ˉa r )v y +m g ϕ=−C ˉa f δf (4 4 )
I z φ ¨ − ( l f C ˉ a f − l r C ˉ a r v x ) v y − ( l f 2 C ˉ a f + l r 2 C ˉ a r v x ) φ ˙ = − l f C ˉ a f δ f (43) I_z\ddot{\varphi}-(\frac{l_f\bar{C}{af}-l_r\bar{C}{ar}}{v_x})v_y- (\frac{{l_f}^2\bar{C}{af}+{l_r}^2\bar{C}{ar}}{v_x})\dot{\varphi}=-l_f\bar{C}_{af}\delta_f\tag{43}I z φ¨−(v x l f C ˉa f −l r C ˉa r )v y −(v x l f 2 C ˉa f +l r 2 C ˉa r )φ˙=−l f C ˉa f δf (4 3 )
( I x + m 2 h C G ) ϕ ¨ − m h C G v ˙ y − m h C G φ ˙ v x + ( K ϕ − m g h C G ) ϕ + D ϕ ϕ ˙ = K ϕ ϕ t (45) (I_x+m^2h_{CG})\ddot{\phi} -mh_{CG}\dot{v}y-mh{CG}\dot{\varphi}v_x+(K_{\phi}-mgh_{CG})\phi+D_{\phi}\dot{\phi}=K_{\phi}\phi_t \tag{45}(I x +m 2 h C G )ϕ¨−m h C G v ˙y −m h C G φ˙v x +(K ϕ−m g h C G )ϕ+D ϕϕ˙=K ϕϕt (4 5 )
; 2. 综合等效约束车辆动力学模型
选取状态量为 ξ = [ v y , φ ˙ , ϕ ˙ , ϕ , e d , e φ ] T \xi = [v_y, \dot{\varphi}, \dot{\phi}, \phi, e_d, e_{\varphi}]^T ξ=[v y ,φ˙,ϕ˙,ϕ,e d ,e φ]T，控制量为 u 1 = δ f u_{1}=\delta_f u 1 =δf ，附加控制量为 u 2 = [ ϕ ˙ t , κ r e f ] T u_{2}=[\dot\phi_t, \kappa_{ref}]^T u 2 =[ϕ˙t ,κr e f ]T
则式( 12 ) (12)(1 2 ) 、式( 43 ) (43)(4 3 ) 、式( 44 ) (44)(4 4 ) 和 式( 45 ) (45)(4 5 ) 可以改写为：
M i n t ξ ˙ + N i n t ξ = F 1 , i n t u 1 + F 2 , i n t u 2 (46) M_{int}\dot{\xi} + N_{int}\xi=F_{1, int}u_{1}+F_{2, int}u_{2} \tag{46}M i n t ξ˙+N i n t ξ=F 1 ,i n t u 1 +F 2 ,i n t u 2 (4 6 )
其中，M i n t = [ m 0 − m h C G 0 0 0 0 I z 0 0 0 0 − m h C G 0 I x + m h C G 2 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] , M_{int}= \begin{bmatrix} m &0 & -mh_{CG} &0&0&0 \0 &I_{z} &0&0&0&0\-mh_{CG} &0 &I_{x}+mh_{CG}^2 &0&0&0\0&0&0&1&0&0\0&0&0&0&1&0\0&0&0&0&0&1\end{bmatrix},M i n t =⎣⎢⎢⎢⎢⎢⎢⎡m 0 −m h C G 0 0 0 0 I z 0 0 0 0 −m h C G 0 I x +m h C G 2 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ⎦⎥⎥⎥⎥⎥⎥⎤,
N i n t = [ − ( C ˉ a f + C ˉ a r v x ) ( m v x − l f C ˉ a f − l r C ˉ a r v x ) 0 m g 0 0 − ( l f C ˉ a f − l r C ˉ a r v x ) − ( l f 2 C ˉ a f + l r 2 C ˉ a r v x ) 0 0 0 0 0 0 − m h C G v x D ϕ K ϕ − m g h C G 0 0 0 − 1 0 0 0 0 0 0 0 0 v x 0 1 0 0 0 0 ] , N_{int}=\begin{bmatrix} -(\frac{\bar{C}{af}+\bar{C}{ar}}{v_x}) & (mv_x-\frac{l_f\bar{C}{af}-l_r\bar{C}{ar}}{v_x}) & 0 &mg&0&0 \\-(\frac{l_f\bar{C}{af}-l_r\bar{C}{ar}}{v_x}) &- (\frac{{l_f}^2\bar{C}{af}+{l_r}^2\bar{C}{ar}}{v_x}) &0&0&0&0\0&0 &-mh_{CG}v_x &D_{\phi}&K_{\phi}-mgh_{CG}&0\0&0&-1&0&0&0\0&0&0&0&0&v_x\0&1&0&0&0&0\end{bmatrix},N i n t =⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−(v x C ˉa f +C ˉa r )−(v x l f C ˉa f −l r C ˉa r )0 0 0 0 (m v x −v x l f C ˉa f −l r C ˉa r )−(v x l f 2 C ˉa f +l r 2 C ˉa r )0 0 0 1 0 0 −m h C G v x −1 0 0 m g 0 D ϕ0 0 0 0 0 K ϕ−m g h C G 0 0 0 0 0 0 0 v x 0 ⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤,
F 1 , i n t = [ − C ˉ a f − l f C ˉ a f 0 0 0 0 ] ， F 2 , i n t = [ 0 0 0 0 K ϕ 0 0 0 0 0 0 − v x ] F_{1, int}=\begin{bmatrix} -\bar{C}{af}\-l_f\bar{C}{af}\0\0\0\0\end{bmatrix}，F_{2, int}=\begin{bmatrix} 0&0\0&0\K_{\phi}&0\0&0\0&0\0&-v_x\\end{bmatrix}F 1 ,i n t =⎣⎢⎢⎢⎢⎢⎢⎡−C ˉa f −l f C ˉa f 0 0 0 0 ⎦⎥⎥⎥⎥⎥⎥⎤，F 2 ,i n t =⎣⎢⎢⎢⎢⎢⎢⎡0 0 K ϕ0 0 0 0 0 0 0 0 −v x ⎦⎥⎥⎥⎥⎥⎥⎤。
通过对 式( 46 ) (46)(4 6 ) 进行变形，可以得到考虑道路倾角与曲率的综合等效约束车辆动力学模型：
ξ ˙ = A ξ + B 1 u 1 + B 2 u 2 (47) \dot{\xi} = A\xi+B_1u_1+B_2u_2\tag{47}ξ˙=A ξ+B 1 u 1 +B 2 u 2 (4 7 )
其中，A = M i n t − 1 N i n t ， B 1 = M i n t − 1 F 1 , i n t ， B 2 = M i n t − 1 F 2 , i n t A=M_{int}^{-1}N_{int}，B_1=M_{int}^{-1}F_{1, int}，B_2= M_{int}^{-1}F_{2, int}A =M i n t −1 N i n t ，B 1 =M i n t −1 F 1 ,i n t ，B 2 =M i n t −1 F 2 ,i n t
设 Θ 1 = C ˉ a f + C ˉ a r ， Θ 2 = l f C ˉ a f − l r C ˉ a r ， Θ 3 = l f 2 C ˉ a f + l r 2 C ˉ a r ， Θ 4 = 1 / m + h C G 2 / I x ， Θ 5 = − K ϕ h C G / I x − g \Theta_1=\bar{C}{af}+\bar{C}{ar}，\Theta_2=l_f\bar{C}{af}-l_r\bar{C}{ar}，\Theta_3=l_f^2\bar{C}{af}+l_r^2\bar{C}{ar}，\Theta_4=1/m+h_{CG}^2/I_x，\Theta_5=-K_{\phi}h_{CG}/I_x-g Θ1 =C ˉa f +C ˉa r ，Θ2 =l f C ˉa f −l r C ˉa r ，Θ3 =l f 2 C ˉa f +l r 2 C ˉa r ，Θ4 =1 /m +h C G 2 /I x ，Θ5 =−K ϕh C G /I x −g，则可以得到：
A = [ Θ 1 Θ 4 v x Θ 2 Θ 4 v x − v x − h C G D ϕ I x Θ 5 0 0 Θ 2 I z v x Θ 3 I z v x 0 0 0 0 h C G Θ 1 I x v x h C G Θ 2 I x v x − D ϕ I x − K ϕ I x 0 0 0 0 1 0 0 0 1 0 0 0 0 v x 0 1 0 0 0 0 ] , B 1 = [ − C ˉ a f Θ 4 − l f C ˉ a f I z − h C G C ˉ a f I x 0 0 0 ] ， B 2 = [ h C G K ϕ I x 0 0 0 K ϕ I x 0 0 0 0 0 0 − v x ] A= \begin{bmatrix} \frac{\Theta_1\Theta_4}{v_x} &\frac{\Theta_2\Theta_4}{v_x}-v_x & \frac{-h_{CG}D_{\phi}}{I_x} &\Theta_5&0&0 \\\frac{\Theta_2}{I_zv_x} &\frac{\Theta_3}{I_zv_x} &0&0&0&0\\\frac{h_{CG}\Theta_1}{I_xv_x} &\frac{h_{CG}\Theta_2}{I_xv_x} &\frac{-D_{\phi}}{I_x} &\frac{-K_{\phi}}{I_x}&0&0\0&0&1&0&0&0\1&0&0&0&0&v_x\0&1&0&0&0&0\end{bmatrix},B_1=\begin{bmatrix} -\bar{C}{af}\Theta_4\\-\frac{l_f\bar{C}{af}}{I_z}\\-\frac{h_{CG}\bar{C}{af}}{I_x}\\0\0\0\end{bmatrix}，B_2=\begin{bmatrix} \frac{h{CG}K_{\phi}}{I_x}&0\\0&0\\\frac{K_{\phi}}{I_x}&0\\0&0\0&0\0&-v_x\\end{bmatrix}A =⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡v x Θ1 Θ4 I z v x Θ2 I x v x h C G Θ1 0 1 0 v x Θ2 Θ4 −v x I z v x Θ3 I x v x h C G Θ2 0 0 1 I x −h C G D ϕ0 I x −D ϕ1 0 0 Θ5 0 I x −K ϕ0 0 0 0 0 0 0 0 0 0 0 0 0 v x 0 ⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤,B 1 =⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−C ˉa f Θ4 −I z l f C ˉa f −I x h C G C ˉa f 0 0 0 ⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤，B 2 =⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡I x h C G K ϕ0 I x K ϕ0 0 0 0 0 0 0 0 −v x ⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤。
谢谢
Original: https://blog.csdn.net/qq_32618327/article/details/124844685
Author: 氢键H-H
Title: 车辆运动控制（7）考虑道路倾角和曲率
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/597798/
转载文章受原作者版权保护。转载请注明原作者出处！