汽车运动学模型

1. 汽车运动学模型

1. 运动学模型

汽车运动学模型_第1张图片
在后轴处,即 ( x r , y r ) (x_r,y_r) (xr,yr)处,速度为
v r = x ˙ r c o s ( φ ) + y ˙ r s i n ( φ ) v_r = \dot{x}_r cos(\varphi)+\dot{y}_rsin(\varphi) vr=x˙rcos(φ)+y˙rsin(φ)
在沿车横向的约束条件为:
{ x ˙ f s i n ( φ + δ f ) − y ˙ f c o s ( φ + δ f ) x ˙ r s i n ( φ ) − y ˙ r c o s ( φ ) \left\{ \begin{aligned} &\dot{x}_fsin(\varphi+\delta_f)-\dot{y}_f cos(\varphi+\delta_f) \\ &\dot{x}_rsin(\varphi)-\dot{y}_rcos(\varphi) \end{aligned} \right. {x˙fsin(φ+δf)y˙fcos(φ+δf)x˙rsin(φ)y˙rcos(φ)
并且x方向和y方向速度与车纵向速度的关系为:
{ x ˙ r = v r c o s ( φ ) y ˙ r = v r s i n ( φ ) \left\{ \begin{aligned} &\dot{x}_r=v_rcos(\varphi)\\ &\dot{y}_r=v_rsin(\varphi) \end{aligned} \right. {x˙r=vrcos(φ)y˙r=vrsin(φ)
前后轮的关系为:
{ x ˙ f = x r + l c o s ( φ ) y ˙ f = y r + l s i n ( φ ) \left\{ \begin{aligned} &\dot{x}_f=x_r+lcos(\varphi)\\ &\dot{y}_f=y_r+lsin(\varphi) \end{aligned} \right. {x˙f=xr+lcos(φ)y˙f=yr+lsin(φ)
对上述关系求导得:
{ x ˙ f = v r c o s ( φ ) − ω l s i n ( φ ) y ˙ f = v r s i n ( φ ) + ω l c o s ( φ ) \left\{ \begin{aligned} &\dot{x}_f=v_rcos(\varphi)-\omega lsin(\varphi)\\ &\dot{y}_f=v_rsin(\varphi)+\omega lcos(\varphi) \end{aligned} \right. {x˙f=vrcos(φ)ωlsin(φ)y˙f=vrsin(φ)+ωlcos(φ)
联立上面速度关系可得到:
s i n ( φ + δ f ) c o s ( φ + δ f ) = v r s i n ( φ + ω l c o s ( φ ) ) v r c o s ( φ ) − ω l s i n ( φ ) \frac{sin(\varphi+\delta_f)}{cos(\varphi+\delta_f)}=\frac{v_rsin(\varphi+\omega lcos(\varphi))}{v_rcos(\varphi)-\omega lsin(\varphi)} cos(φ+δf)sin(φ+δf)=vrcos(φ)ωlsin(φ)vrsin(φ+ωlcos(φ))
对上式展开,即得到:
ω = v r l t a n ( δ f ) \omega = \frac{v_r}{l}tan(\delta_f) ω=lvrtan(δf)
即,我们得到如下非线性运动学模型:
[ x ˙ r y ˙ r φ ˙ ] = [ c o s ( φ ) s i n ( φ ) t a n ( δ f ) l ] v r { \left[ \begin{array}{ccc} \dot{x}_r\\ \dot{y}_r\\ \dot{\varphi} \end{array} \right ]}={ \left[ \begin{array}{ccc} cos(\varphi)\\ sin(\varphi)\\ \frac{tan(\delta_f)}{l} \end{array} \right ]}v_r x˙ry˙rφ˙=cos(φ)sin(φ)ltan(δf)vr
其中,状态变量为 :
[ x r y r φ ] T { \left[\begin{array}{ccc} x_r & y_r &\varphi \end{array} \right ]}^{T} [xryrφ]T
控制量为 v r 和 δ f v_r和\delta_f vrδf

2. 非线性模型线性化

利用泰勒展开:
设参考系统为:
ξ ˙ r = f ( ξ r , u r ) \dot{\xi}_r=f(\xi_r,u_r) ξ˙r=f(ξr,ur)
当前系统在参考系统点处展开得:
ξ ˙ = f ( ξ r , u r ) + ∂ f ∂ ξ ∣ ξ = ξ r , u = u r ( ξ − ξ r ) + ∂ f ∂ u ∣ ξ = ξ r , u = u r ( u − u r ) \dot{\xi} = f(\xi_r,u_r)+\frac{\partial f}{\partial \xi}|_{\xi=\xi_r,u=u_r}(\xi-\xi_r)+\frac{\partial f}{\partial u}|_{\xi=\xi_r,u=u_r}(u-u_r) ξ˙=f(ξr,ur)+ξfξ=ξr,u=ur(ξξr)+ufξ=ξr,u=ur(uur)
即我们对非线性模型进行一阶泰勒展开并离散化得到:
ξ ~ k i n ( k + 1 ) = A k i n ( k ) ξ ~ k i n ( k ) + B k i n ( k ) u ~ k i n ( k ) \widetilde{\xi}_{kin}(k+1) = A_{kin}(k)\widetilde{\xi}_{kin}(k)+B_{kin}(k)\widetilde{u}_{kin}(k) ξ kin(k+1)=Akin(k)ξ kin(k)+Bkin(k)u kin(k)
其中

ξ ~ k i n ( k ) = [ x − x r y − y r φ − φ r ] A k i n = [ 1 0 − v r s i n ( φ r ) T 0 1 v r c o s ( φ r ) T 0 0 1 ] B k i n = [ c o s ( φ r ) T 0 s i n ( φ r ) T 0 t a n ( δ f ) T l v r T l c o s 2 ( δ f ) ] \widetilde{\xi}_{kin}(k)= { \left[ \begin{array}{ccc} x-x_r\\ y-y_r\\ \varphi-\varphi_r \end{array} \right ]} A_{kin}= { \left[ \begin{array}{ccc} 1 & 0 & -v_rsin(\varphi_r)T\\ 0 & 1 & v_rcos(\varphi_r)T\\ 0 & 0 & 1 \end{array} \right ] } B_{kin}= { \left[ \begin{array}{ccc} cos(\varphi_r)T & 0 \\ sin(\varphi_r)T & 0\\ \frac{tan(\delta_f)T}{l} & \frac{v_rT}{lcos^{2}(\delta_f)} \end{array} \right ] } ξ kin(k)=xxryyrφφrAkin=100010vrsin(φr)Tvrcos(φr)T1Bkin=cos(φr)Tsin(φr)Tltan(δf)T00lcos2(δf)vrT

你可能感兴趣的:(算法)