wordpress查看网站内容,大学生做的美食网站,wordpress文章缩略图功能,单页设计图片文章目录 1 单车模型2 线性化3 实现效果4 参考资料 1 单车模型 这里讨论的是以后轴为中心的单车运动学模型#xff0c;由下式表达#xff1a; S ˙ [ x ˙ y ˙ ψ ˙ ] [ v c o s ( ψ ) v s i n ( ψ ) v t a n ( ψ ) L ] \dot S \begin{bmatrix} \dot x\\ \dot y\\ \d… 文章目录 1 单车模型2 线性化3 实现效果4 参考资料 1 单车模型 这里讨论的是以后轴为中心的单车运动学模型由下式表达 S ˙ [ x ˙ y ˙ ψ ˙ ] [ v c o s ( ψ ) v s i n ( ψ ) v t a n ( ψ ) L ] \dot S \begin{bmatrix} \dot x\\ \dot y\\ \dot \psi \\ \end{bmatrix} \begin{bmatrix} vcos(\psi)\\ vsin(\psi)\\ v\frac{tan(\psi)}{L} \\ \end{bmatrix} S˙ x˙y˙ψ˙ vcos(ψ)vsin(ψ)vLtan(ψ)
其中 S S S代表系统状态 δ \delta δ为单车模型的前轮转角 x , y x,y x,y是车辆在世界坐标系下的位置 L L L为轴距 v v v为车辆速度 ψ \psi ψ代表航向角单车模型中应该是假设了质心侧偏角为0所以 ψ \psi ψ应该也等于横摆角即车头朝向。 如果把加速度和前轮转角的变化加上则可以写成 S ˙ [ x ˙ y ˙ ψ ˙ v ˙ δ ˙ ] [ v c o s ( ψ ) v s i n ( ψ ) v t a n ( ψ ) L a γ ] [ f 1 f 2 f 3 f 4 f 5 ] f ( S ) \dot S \begin{bmatrix} \dot x\\ \dot y\\ \dot \psi \\ \dot v \\ \dot \delta \end{bmatrix} \begin{bmatrix} vcos(\psi)\\ vsin(\psi)\\ v\frac{tan(\psi)}{L} \\ a \\ \gamma \end{bmatrix} \begin{bmatrix} f_1\\ f_2\\ f_3 \\ f_4 \\ f_5 \end{bmatrix} f(S) S˙ x˙y˙ψ˙v˙δ˙ vcos(ψ)vsin(ψ)vLtan(ψ)aγ f1f2f3f4f5 f(S) 这里 γ \gamma γ是我自己取的名字看别的博客把速度和前轮转角用额外的矩阵表示了这里我放在一起推一推。
2 线性化 将上述系统 S ˙ f ( S ) \dot S f(S) S˙f(S)在 S S r [ x r y r ψ r v r δ r ] S S_r \begin{bmatrix}x_r\\y_r\\\psi_r\\v_r\\\delta_r\end{bmatrix} SSr xryrψrvrδr 处泰勒展开忽略高次项 S ˙ ≈ f ( S r ) d f d S ( S − S r ) \dot S \approx f(S_r) \cfrac{df}{dS}(S - S_r) S˙≈f(Sr)dSdf(S−Sr)
进而有 S ˙ − S ˙ r ( d f 1 d x d f 1 d y d f 1 d ψ d f 1 d v d f 1 d δ d f 2 d x d f 2 d y d f 2 d ψ d f 2 d v d f 2 d δ d f 3 d x d f 3 d y d f 3 d ψ d f 3 d v d f 3 d δ d f 4 d x d f 4 d y d f 4 d ψ d f 4 d v d f 4 d δ d f 5 d x d f 5 d y d f 5 d ψ d f 5 d v d f 5 d δ ) [ x − x r y − y r ψ − ψ r v − v r δ − δ r ] \dot S - \dot S_r \begin{pmatrix} \frac{df_1}{dx} \frac{df_1}{dy} \frac{df_1}{d\psi} \frac{df_1}{dv} \frac{df_1}{d\delta} \\ \frac{df_2}{dx} \frac{df_2}{dy} \frac{df_2}{d\psi} \frac{df_2}{dv} \frac{df_2}{d\delta} \\ \frac{df_3}{dx} \frac{df_3}{dy} \frac{df_3}{d\psi} \frac{df_3}{dv} \frac{df_3}{d\delta} \\ \frac{df_4}{dx} \frac{df_4}{dy} \frac{df_4}{d\psi} \frac{df_4}{dv} \frac{df_4}{d\delta} \\ \frac{df_5}{dx} \frac{df_5}{dy} \frac{df_5}{d\psi} \frac{df_5}{dv} \frac{df_5}{d\delta} \\ \end{pmatrix} \begin{bmatrix}x - x_r\\y - y_r\\\psi - \psi_r\\v - v_r\\\delta - \delta_r\end{bmatrix} S˙−S˙r dxdf1dxdf2dxdf3dxdf4dxdf5dydf1dydf2dydf3dydf4dydf5dψdf1dψdf2dψdf3dψdf4dψdf5dvdf1dvdf2dvdf3dvdf4dvdf5dδdf1dδdf2dδdf3dδdf4dδdf5 x−xry−yrψ−ψrv−vrδ−δr
求导可得 S ˙ − S ˙ r ( 0 0 − v s i n ( ψ ) c o s ( ψ ) 0 0 0 v c o s ( ψ ) s i n ( ψ ) 0 0 0 0 t a n ( δ ) L v L c o s 2 ( δ ) 0 0 0 0 0 0 0 0 0 0 ) [ x − x r y − y r ψ − ψ r v − v r δ − δ r ] J Δ S \dot S - \dot S_r \begin{pmatrix} 0 0 -vsin(\psi) cos(\psi) 0 \\ 0 0 vcos(\psi) sin(\psi) 0 \\ 0 0 0 \frac{tan(\delta)}{L} \frac{v}{Lcos^2(\delta)} \\ 0 0 0 0 0 \\ 0 0 0 0 0 \\ \end{pmatrix}\begin{bmatrix}x - x_r\\y - y_r\\\psi - \psi_r\\v - v_r\\\delta - \delta_r\end{bmatrix} J\Delta S S˙−S˙r 0000000000−vsin(ψ)vcos(ψ)000cos(ψ)sin(ψ)Ltan(δ)0000Lcos2(δ)v00 x−xry−yrψ−ψrv−vrδ−δr JΔS
最后可得线性化表示 Δ S ˙ S ˙ − S ˙ r J Δ S \Delta \dot S \dot S - \dot S_r J\Delta S ΔS˙S˙−S˙rJΔS
在实际应用中需要把它写成差分的形式假设采样时间间隔为 Δ t \Delta t Δt则有 Δ S k 1 − Δ S k Δ t J Δ S k \cfrac{\Delta S_{k1} - \Delta S_{k}}{\Delta t} J \Delta S_k ΔtΔSk1−ΔSkJΔSk 最后得到车辆状态变化的差分方程形式 Δ S k 1 ( J Δ t I ) Δ S k \Delta S_{k1} (J\Delta t I)\Delta S_k ΔSk1(JΔtI)ΔSk
3 实现效果 现在假设采样时间为0.1秒车辆初始状态在原点车头朝向是45度运行1.5秒分别用单车模型和线性化方法实现两个单车模型看看跑出来的误差是怎样的。这里给的加速度是 4 m / s 2 4m/s^2 4m/s2前轮转角不变。效果如下红色是单车模型绿色是线性化后的单车模型这里的动画运行时间间隔是0.2s方便看效果。可以看到在前1秒内这个线性化算出来的车辆状态和单车模型本身的误差还是可以的。
4 参考资料
无人车系统一运动学模型及其线性化汽车运动学模型的线性化推导过程上面的视频制作软件是manim3B1B的视频制作软件哦用来自己做做简单的动画还是ok的
