当前位置: article > 正文

自行车运动模型及其线性化_自行车运动学模型的状态方程

作者：很楠不爱3 | 2024-04-22 07:59:43

踩

自行车运动学模型的状态方程

自行车运动模型及其线性化

自行车运动模型

自行车运动模型
运动模型方程：
$\left[$

\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}

$\begin{matrix} \overset{.}{x} \\ \overset{.}{y} \\ \overset{.}{\theta} \\ \end{matrix}$ \right]= \left[

\begin{matrix} v c o s (θ) \\ v s i n (θ) \\ v \frac{t a n (δ)}{L} \end{matrix}

$\begin{matrix} vcos(\theta) \\ vsin(\theta) \\ v\frac{tan(\delta)}{L} \\ \end{matrix}$ \right]

⎣ ⎡ x . y . θ . ⎦ ⎤ = ⎣ ⎡ v c o s (θ) v s i n (θ) v \frac{t a n ( δ )}{L} ⎦ ⎤

可写为：
$\left[$

\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}

$\begin{matrix} \overset{.}{x} \\ \overset{.}{y} \\ \overset{.}{\theta} \\ \end{matrix}$ \right]= \left[

\begin{matrix} c o s (θ) \\ s i n (θ) \\ 0 \end{matrix}

$\begin{matrix} cos(\theta) \\ sin(\theta) \\ 0 \\ \end{matrix}$ \right]v+ \left[

\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}

$\begin{matrix} 0 \\ 0 \\ 1 \\ \end{matrix}$ \right]w

⎣ ⎡ x . y . θ . ⎦ ⎤ = ⎣ ⎡ c o s (θ) s i n (θ) 0 ⎦ ⎤ v + ⎣ ⎡ 001 ⎦ ⎤ w

模型线性化

利用泰勒展开，只保留一阶项
$\overset{.}{X}= \left[$

\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}

$\begin{matrix} \overset{.}{x} \\ \overset{.}{y} \\ \overset{.}{\theta} \\ \end{matrix}$ \right]= \left[

\begin{matrix} v c o s (θ) \\ v s i n (θ) \\ v \frac{t a n (δ)}{L} \end{matrix}

$\begin{matrix} vcos(\theta) \\ vsin(\theta) \\ v\frac{tan(\delta)}{L} \\ \end{matrix}$ \right]= \left[

\begin{matrix} f_{1} \\ f_{2} \\ f_{3} \end{matrix}

$\begin{matrix} f_1 \\ f_2 \\ f_3 \\ \end{matrix}$ \right]= f(X,u)

X . = ⎣ ⎡ x . y . θ . ⎦ ⎤ = ⎣ ⎡ v c o s (θ) v s i n (θ) v \frac{t a n ( δ )}{L} ⎦ ⎤ = ⎣ ⎡ f_{1} f_{2} f_{3} ⎦ ⎤ = f (X, u)

其中 $X=[x,y,\theta]^T,u=[v,\delta]^T$

$\bigtriangleup\overset{.}{X}=\overset{.}{X}-\overset{.}{X_r}= \left[$

\begin{matrix} \dot{x} - \dot{x_{r}} \\ \dot{y} - \dot{y_{r}} \\ \dot{θ} - \dot{θ_{r}} \end{matrix}

$\begin{matrix} \overset{.}{x} -\overset{.}{x_r} \\ \overset{.}{y} -\overset{.}{y_r} \\ \overset{.}{\theta} -\overset{.}{\theta_r}\\ \end{matrix}$ \right] \approx \left[

\begin{matrix} \frac{σ f_{1}}{σ x} & \frac{σ f_{1}}{σ y} & \frac{σ f_{1}}{σ θ} \\ \frac{σ f_{2}}{σ x} & \frac{σ f_{2}}{σ y} & \frac{σ f_{2}}{σ θ} \\ \frac{σ f_{3}}{σ x} & \frac{σ f_{3}}{σ y} & \frac{σ f_{3}}{σ θ} \end{matrix}

$\begin{matrix} \frac{\sigma f_1}{\sigma x} & \frac{\sigma f_1}{\sigma y} & \frac{\sigma f_1}{\sigma \theta} \\ \frac{\sigma f_2}{\sigma x} & \frac{\sigma f_2}{\sigma y} & \frac{\sigma f_2}{\sigma \theta} \\ \frac{\sigma f_3}{\sigma x} & \frac{\sigma f_3}{\sigma y} & \frac{\sigma f_3}{\sigma \theta} \\ \end{matrix}$ \right] \left[

\begin{matrix} x - x_{r} \\ y - y_{r} \\ θ - θ_{r} \end{matrix}

$\begin{matrix} x -x_r \\ y - y_r \\ \theta -\theta_r \\ \end{matrix}$ \right] + \left[

\begin{matrix} \frac{σ f_{1}}{σ v} & \frac{σ f_{1}}{σ δ} \\ \frac{σ f_{2}}{σ v} & \frac{σ f_{2}}{σ δ} \\ \frac{σ f_{3}}{σ v} & \frac{σ f_{3}}{σ δ} \end{matrix}

$\begin{matrix} \frac{\sigma f_1}{\sigma v} & \frac{\sigma f_1}{\sigma \delta} \\ \frac{\sigma f_2}{\sigma v} & \frac{\sigma f_2}{\sigma \delta} \\ \frac{\sigma f_3}{\sigma v} & \frac{\sigma f_3}{\sigma \delta} \\ \end{matrix}$ \right] \left[

\begin{matrix} v - v_{r} \\ δ - δ_{r} \end{matrix}

$\begin{matrix} v-v_r \\ \delta-\delta_r\\ \end{matrix}$ \right]

△ X . = X . - X_{r} . = ⎣ ⎡ x . - x_{r} . y . - y_{r} . θ . - θ_{r} . ⎦ ⎤ \approx ⎣ ⎢ ⎡ \frac{σ f _{1}}{σ x} \frac{σ f _{2}}{σ x} \frac{σ f _{3}}{σ x} \frac{σ f _{1}}{σ y} \frac{σ f _{2}}{σ y} \frac{σ f _{3}}{σ y} \frac{σ f _{1}}{σ θ} \frac{σ f _{2}}{σ θ} \frac{σ f _{3}}{σ θ} ⎦ ⎥ ⎤ ⎣ ⎡ x - x_{r} y - y_{r} θ - θ_{r} ⎦ ⎤ + ⎣ ⎡ \frac{σ f _{1}}{σ v} \frac{σ f _{2}}{σ v} \frac{σ f _{3}}{σ v} \frac{σ f _{1}}{σ δ} \frac{σ f _{2}}{σ δ} \frac{σ f _{3}}{σ δ} ⎦ ⎤ [v - v_{r} δ - δ_{r}]

写为

\begin{aligned} △ \dot{X} & = A_{m} △ X + B_{m} △ u \\ A_{m} & = [\begin{matrix} 0 & 0 & - v s i n (θ) \\ 0 & 0 & v c o s (θ) \\ 0 & 0 & 0 \end{matrix}] \\ B_{m} & = [\begin{matrix} c o s (θ) & 0 \\ s i n (θ) & 0 \\ \frac{t a n (δ)}{L} & \frac{v}{L c o s^{2} δ} \end{matrix}] \end{aligned}

$\begin{aligned} \bigtriangleup\overset{.}{X}&=A_m\bigtriangleup X +B_m\bigtriangleup u \\ A_m &= \left[ \begin{matrix} 0 & 0 & -vsin(\theta) \\ 0 & 0 & vcos(\theta) \\ 0 & 0 & 0\\ \end{matrix} \right] \\ B_m &= \left[ \begin{matrix} cos(\theta) & 0\\ sin(\theta) & 0\\ \frac{tan(\delta)}{L} &\frac{v}{Lcos^2\delta} \\ \end{matrix} \right] \end{aligned}$

△ X . A_{m} B_{m} = A_{m} △ X + B_{m} △ u = ⎣ ⎡ 000000 - v s i n (θ) v c o s (θ) 0 ⎦ ⎤ = ⎣ ⎡ c o s (θ) s i n (θ) \frac{t a n ( δ )}{L} 00 \frac{v}{L c o s ^{2} δ} ⎦ ⎤

线性模型离散化

$A_m\bigtriangleup X(k) +B_m\bigtriangleup u(k)=\bigtriangleup\overset{.}{X}= \frac{\bigtriangleup X(k+1)-\bigtriangleup X(k)}{T}$
变形可得：

\begin{aligned} △ X (k + 1) & = (I + T A_{m}) △ X (k) + T B_{m} △ u (k) \\ = A △ X (k) + B △ u (k) \end{aligned}

$\begin{aligned} \bigtriangleup X(k+1)&=(I+TA_m)\bigtriangleup X(k)+TB_m\bigtriangleup u(k)\\&=A\bigtriangleup X(k)+B \bigtriangleup u(k) \end{aligned}$ \\

\begin{aligned} A_{m} & = [\begin{matrix} 1 & 0 & - v T s i n (θ) \\ 0 & 1 & v T c o s (θ) \\ 0 & 0 & 1 \end{matrix}] \\ B_{m} & = [\begin{matrix} T c o s (θ) & 0 \\ T s i n (θ) & 0 \\ \frac{T t a n (δ)}{L} & \frac{T v}{L c o s^{2} δ} \end{matrix}] \end{aligned}

$\begin{aligned} A_m &= \left[ \begin{matrix} 1 & 0 & -vTsin(\theta) \\ 0 & 1 & vTcos(\theta) \\ 0 & 0 & 1\\ \end{matrix} \right] \\ B_m &= \left[ \begin{matrix} Tcos(\theta) & 0\\ Tsin(\theta) & 0\\ \frac{Ttan(\delta)}{L} &\frac{Tv}{Lcos^2\delta} \\ \end{matrix} \right] \end{aligned}$

△ X (k + 1) = (I + T A_{m}) △ X (k) + T B_{m} △ u (k) = A △ X (k) + B △ u (k) A_{m} B_{m} = ⎣ ⎡ 100010 - v T s i n (θ) v T c o s (θ) 1 ⎦ ⎤ = ⎣ ⎡ T c o s (θ) T s i n (θ) \frac{T t a n ( δ )}{L} 00 \frac{T v}{L c o s ^{2} δ} ⎦ ⎤

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/很楠不爱3/article/detail/467586