控制算法|车辆控制-运动学模型(Kinematic Model) 运动模型|车辆模型

文章目录

- 概述
- 单车模型
- 运动方程
- - 概述
  - 姿态信息(偏航角)
  - - 滑移角 β \beta β的计算
  - 位置信息
- 补充
- 结论
- - 微分方程形式
- 参考

概述车辆横向运动学模型描述了车辆横向运动的数学模型，该模型不考虑车辆的受力情况。一般考虑运动学模型时，将车辆模型简化成单车模型(bicycle model)。
单车模型

文章图片

单车模型中：

左右轮等效为单个轮子
左右前轮合并为单个轮子，其中心点为 A A A点，同样后轮等效后的中心点为 B B B点。
转向角
前后轮的转向角用 δ f \delta_f δf?和 δ r \delta_r δr?表示，模型中前后轮都可以转向，对于只有前轮转向的系统，后轮转向角 δ r \delta_r δr?可以设置为0.
重心
点 C C C代表车辆的重心， A A A点和 B B B点到重心的距离分别用 l f l_f lf?和 l r l_r lr?表示，轴距表示为 L = l f + l r L = l_f + l_r L=lf?+lr?。
速度
车辆重心的速度用 V V V表示，与车辆纵向轴的夹角为 β \beta β，该角度叫做车辆的滑移角。
运动描述
假设车辆平动，车辆运动状态可以用三个坐标量描述： X X X 、Y Y Y 和ψ \psi ψ。其中 ( X , Y ) (X,Y) (X,Y)代表车辆重心的位置， Ψ \Psi Ψ描述了车辆的方向。
条件假设
假设速度矢量 V V V的方向在点 A A A点和 B B B点的方向与转向角的方向相同，换句话说，在A点的速度矢量与车辆纵轴的夹角为 δ f \delta_f δf?，同样 B B B点的速度矢量与车辆纵轴的夹角为 δ r \delta_r δr?。也就是说前后轮的滑移角都为0。该条件假设成立前提的是车辆速度很低(<5m/s)，此时轮胎产生的横向力很小，可以忽略。
轨迹半径
点 O O O代表车辆的瞬时旋转中心，线段 A O AO AO与 B O BO BO与前后两个转轮方向垂直，他们的交点即为 O O O点，线段 O C OC OC的长度代表车辆的轨迹半径 R R R。
航迹角
车辆重心处的速度垂直于 O C OC OC,车辆速度矢量与车辆纵轴的夹角为 β \beta β，车辆的航向角为 ψ \psi ψ，则航迹角为 γ = ψ + β \gamma = \psi + \beta γ=ψ+β。

运动方程概述
运动方程可以根据单车模型几何关系推导出，下面根据单车模型的几何关系，推导出车辆运动的微分方程。
姿态信息(偏航角)
三角形 O C A OCA OCA根据正弦定理得
sin ? ( δ f ? β ) l f = sin ? ( π 2 ? δ f ) R (1) \frac{\sin(\delta_f - \beta)}{l_f} = \frac{\sin(\frac{\pi}{2} - \delta_f)}{R} \tag{1} lf?sin(δf??β)?=Rsin(2π??δf?)?(1)
同理，三角形 O C B OCB OCB根据正弦定理可得
sin ? ( β ? δ r ) l r = sin ? ( π 2 + δ r ) R (2) \frac{\sin(\beta - \delta_r)}{l_r} = \frac{\sin(\frac{\pi}{2} + \delta_r)}{R} \tag{2} lr?sin(β?δr?)?=Rsin(2π?+δr?)?(2)
根据和差定理，等式(1)得
sin ? ( δ f ) cos ? ( β ) ? cos ? ( δ f ) sin ? ( β ) l f = cos ? ( δ f ) R (3) \frac{\sin(\delta_f)\cos(\beta) - \cos(\delta_f)\sin(\beta)}{l_f} = \frac{\cos(\delta_f)}{R} \tag{3} lf?sin(δf?)cos(β)?cos(δf?)sin(β)?=Rcos(δf?)?(3)
同理，根据等式(2)得
cos ? ( δ r ) sin ? ( β ) ? cos ? ( β ) sin ? ( δ r ) l r = cos ? ( δ f ) R (4) \frac{\cos(\delta_r)\sin(\beta) - \cos(\beta)\sin(\delta_r)}{l_r} = \frac{\cos(\delta_f)}{R} \tag{4} lr?cos(δr?)sin(β)?cos(β)sin(δr?)?=Rcos(δf?)?(4)
等式(3)两边同时乘 l f cos ? ( δ f ) \frac{l_f}{\cos(\delta_f)} cos(δf?)lf??得
tan ? ( δ f ) cos ? ( β ) ? s i n ( β ) = l f R (5) \tan(\delta_f)\cos(\beta) - sin(\beta) = \frac{l_f}{R} \tag{5} tan(δf?)cos(β)?sin(β)=Rlf??(5)
同理，等式(4)两边同时乘 l r cos ? ( δ r ) \frac{l_r}{\cos(\delta_r)} cos(δr?)lr??得
s i n ( β ) ? tan ? ( δ r ) cos ? ( β ) = l r R (6) sin(\beta) - \tan(\delta_r)\cos(\beta) = \frac{l_r}{R} \tag{6} sin(β)?tan(δr?)cos(β)=Rlr??(6)
等式(5)和(6)相加得
{ tan ? ( δ f ) ? tan ? ( δ r ) } cos ? ( β ) = l f + l r R (7) \{\tan(\delta_f) - \tan(\delta_r)\}\cos(\beta) = \frac{l_f + l_r}{R} \tag{7} {tan(δf?)?tan(δr?)}cos(β)=Rlf?+lr??(7)
根据条件假设，车辆速度很低，车辆的轨迹半径变化就很慢。车辆偏航角的变化率( ψ ˙ \dot{\psi} ψ˙?)可以近似等于车辆的角速度( ω \omega ω)。根据车辆角速度 ω = V R \omega = \frac{V}{R} ω=RV?得
ψ ˙ = V R (8) \dot{\psi} = \frac{V}{R} \tag{8} ψ˙?=RV?(8)
将等式(8)带入等式(7)中，消除 R R R项得
ψ ˙ = V cos ? ( β ) l f + l r { tan ? ( δ f ) ? tan ? ( δ r ) } (9) \dot{\psi} = \frac{V\cos(\beta)}{l_f + l_r}\{\tan(\delta_f) - \tan(\delta_r)\} \tag{9} ψ˙?=lf?+lr?Vcos(β)?{tan(δf?)?tan(δr?)}(9)
上述等式一共三个输入变量： δ f \delta_f δf? 、 δ r \delta_r δr?和 V V V。其中 δ f \delta_f δf?和 δ r \delta_r δr?是车辆的转向角，可以通过车身传感器获得。 V V V是一个外部变量，可以假设其为时变函数，可以从纵向控制中获得。
滑移角 β \beta β的计算等式(5)乘以 l r l_r lr?，
tan ? ( δ f ) cos ? ( β ) l r ? s i n ( β ) l r = l f ? l r R (10) \tan(\delta_f)\cos(\beta)l_r - sin(\beta)l_r = \frac{l_f*l_r}{R} \tag{10} tan(δf?)cos(β)lr??sin(β)lr?=Rlf??lr??(10)
等式(6)乘以 l f l_f lf?得
s i n ( β ) l f ? tan ? ( δ r ) cos ? ( β ) l f = l f ? l r R (11) sin(\beta)l_f - \tan(\delta_r)\cos(\beta)l_f = \frac{l_f*l_r}{R} \tag{11} sin(β)lf??tan(δr?)cos(β)lf?=Rlf??lr??(11)
等式(10)和(11)相减得
cos ? ( β ) ( l f tan ? ( δ r ) + l r tan ? ( δ f ) ) = sin ? ( β ) ( l f + l r ) (12) \cos(\beta)(l_f\tan(\delta_r) + l_r\tan(\delta_f)) = \sin(\beta)(l_f + l_r) \tag{12} cos(β)(lf?tan(δr?)+lr?tan(δf?))=sin(β)(lf?+lr?)(12)
等式(12)两端同时乘以 1 cos ? ( β ) \frac{1}{\cos(\beta)} cos(β)1?得
tan ? ( β ) = l f tan ? ( δ r ) + l r tan ? ( δ f ) l f + l r (13) \tan(\beta) = \frac{l_f\tan(\delta_r) + l_r\tan(\delta_f)}{l_f + l_r} \tag{13} tan(β)=lf?+lr?lf?tan(δr?)+lr?tan(δf?)?(13)
故取反三角函数得
β = arctan ? ( l f tan ? ( δ r ) + l r tan ? ( δ f ) l f + l r ) (13) \beta = \arctan(\frac{l_f\tan(\delta_r) + l_r\tan(\delta_f)}{l_f + l_r} \tag{13}) β=arctan(lf?+lr?lf?tan(δr?)+lr?tan(δf?)?)(13)
位置信息
根据单车模型得
X ˙ = V cos ? ( β + ψ ) (14) \dot{X} = V\cos(\beta + \psi) \tag{14} X˙=Vcos(β+ψ)(14)
Y ˙ = V sin ? ( β + ψ ) (15) \dot{Y} = V\sin(\beta + \psi) \tag{15} Y˙=Vsin(β+ψ)(15)
补充

文章图片

由上图可知， l w l_w lw?为车轴宽度，为了避免与上述单车模型的转向角定义重复，使用 δ o \delta_o δo?表示外部转向角， δ i \delta_i δi?表示内部转向角。由于轴距 L = l f + l r L = l_f + l_r L=lf?+lr?远远小于轨迹半径 R R R，滑移角 β \beta β接近于0。一般车辆模型后轴为固定轴，故 δ r \delta_r δr?为0，所以等式(9)可以近似为
ψ ˙ = V L tan ? ( δ f ) (16) \dot{\psi} = \frac{V}{L}\tan(\delta_f) \tag{16} ψ˙?=LV?tan(δf?)(16)
由于 δ f \delta_f δf?很小
tan ? ( δ f ) ≈ δ f (17) \tan(\delta_f) \approx \delta_f \tag{17} tan(δf?)≈δf?(17)
根据等式(8)和等式(16)得
ψ ˙ V ≈ δ f L = 1 R (18) \frac{\dot{\psi}}{V} \approx \frac{\delta_f}{L} = \frac{1}{R} \tag{18} Vψ˙??≈Lδf??=R1?(18)
故不区分前后轴，等效转向角为
δ = L R (19) \delta = \frac{L}{R} \tag{19} δ=RL?(19)
由于内外轮的转弯半径不同，根据等式(19),外轮转角为
δ o = L R + l w 2 (20) \delta_o = \frac{L}{R + \frac{l_w}{2}} \tag{20} δo?=R+2lw??L?(20)
内轮转角为
δ i = L R ? l w 2 (21) \delta_i = \frac{L}{R - \frac{l_w}{2}} \tag{21} δi?=R?2lw??L?(21)
故前轮平均转向角为
δ = δ o + δ i 2 = L R ? l w 2 4 R (22) \delta = \frac{\delta_o + \delta_i}{2} = \frac{L}{R - \frac{l_w^2}{4R}} \tag{22} δ=2δo?+δi??=R?4Rlw2??L?(22)
由于 l w 2 4 R \frac{l_w^2}{4R} 4Rlw2??项中， l w l_w lw?远远小于 R R R,且 l w l_w lw?的二次项更小，故
l w 2 4 R ? 0 (23) \frac{l_w^2}{4R} \cong 0 \tag{23} 4Rlw2???0(23)
所以等式(22)可以近似为
δ = L R (24) \delta = \frac{L}{R} \tag{24} δ=RL?(24)
比较等式(20)和(21)知， δ i \delta_i δi?始终大于 δ o \delta_o δo?，故
δ i ? δ o = L R ? l w 2 ? L R + l w 2 = L l w R 2 ? l w 2 4 ? L R 2 l w = δ 2 l w L (25) \delta_i - \delta_o = \frac{L}{R - \frac{l_w}{2}} - \frac{L}{R + \frac{l_w}{2}} \quad\\ =\frac{Ll_w}{R^2 - \frac{l_w^2}{4}} \qquad\\ \cong \frac{L}{R^2}l_w = \delta^2\frac{l_w}{L} \tag{25} δi??δo?=R?2lw??L??R+2lw??L?=R2?4lw2??Llw???R2L?lw?=δ2Llw??(25)
根据等式(25)可知，前轮内外转向角的差值接近于平均转向角的二次方，所以当前轮转向角较大时，内外轮的转向角误差就越大。
结论 【控制算法|车辆控制-运动学模型(Kinematic Model)】车辆运动模型基于单车模型推导，推导过程不考虑车辆受到的横向力，故该模型只适用于车辆速度很低的情形。
a = m V 2 R (26) a = \frac{mV^2}{R} \tag{26} a=RmV2?(26)
根据等式(26)知，速度很小时，车辆受到的向心力可以忽略不记，所以才有公式(8)的成立。所以当车辆的运动场景速度较低时，可以使用该模型描述车辆的运动。
微分方程形式
根据等式(9)、(14)和(15)，对于前轮转向系统， δ r = 0 \delta_r = 0 δr?=0，可得
X ˙ = V cos ? ( β + ψ ) Y ˙ = V sin ? ( β + ψ ) ψ ˙ = V cos ? ( β ) tan ? ( δ f ) L \dot{X} = V\cos(\beta + \psi) \qquad\\ \dot{Y} = V\sin(\beta + \psi) \qquad\\ \dot{\psi} = \frac{V\cos(\beta)\tan(\delta_f)}{L} X˙=Vcos(β+ψ)Y˙=Vsin(β+ψ)ψ˙?=LVcos(β)tan(δf?)?
其中
β = arctan ? ( l r tan ? ( δ f ) l f + l r ) \beta = \arctan(\frac{l_r\tan(\delta_f)}{l_f + l_r}) β=arctan(lf?+lr?lr?tan(δf?)?)
假设车辆重心点在后轴中心点，则 b e t a ? 0 beta \cong 0 beta?0，由上述微分方程可得
X ˙ = V cos ? ( ψ ) Y ˙ = V sin ? ( ψ ) ψ ˙ = V tan ? ( δ f ) L \dot{X} = V\cos(\psi) \quad\\ \dot{Y} = V\sin(\psi) \quad\\ \dot{\psi} = \frac{V\tan(\delta_f)}{L} X˙=Vcos(ψ)Y˙=Vsin(ψ)ψ˙?=LVtan(δf?)?
参考