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

详见：
参考链接

自行车运动模型

运动模型方程：
[x.y.θ.]=[vcos(θ)vsin(θ)vtan(δ)L]\left[ \begin{matrix} \overset{.}{x} \\ \overset{.}{y} \\ \overset{.}{\theta} \\ \end{matrix} \right]= \left[ \begin{matrix} vcos(\theta) \\ vsin(\theta) \\ v\frac{tan(\delta)}{L} \\ \end{matrix} \right] ⎣⎡x.y.θ.⎦⎤=⎣⎡vcos(θ)vsin(θ)vLtan(δ)⎦⎤

可写为：
[x.y.θ.]=[cos(θ)sin(θ)0]v+[001]w\left[ \begin{matrix} \overset{.}{x} \\ \overset{.}{y} \\ \overset{.}{\theta} \\ \end{matrix} \right]= \left[ \begin{matrix} cos(\theta) \\ sin(\theta) \\ 0 \\ \end{matrix} \right]v+ \left[ \begin{matrix} 0 \\ 0 \\ 1 \\ \end{matrix} \right]w ⎣⎡x.y.θ.⎦⎤=⎣⎡cos(θ)sin(θ)0⎦⎤v+⎣⎡001⎦⎤w

模型线性化

利用泰勒展开，只保留一阶项
X.=[x.y.θ.]=[vcos(θ)vsin(θ)vtan(δ)L]=[f1f2f3]=f(X,u)\overset{.}{X}= \left[ \begin{matrix} \overset{.}{x} \\ \overset{.}{y} \\ \overset{.}{\theta} \\ \end{matrix} \right]= \left[ \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} \right]= f(X,u) X.=⎣⎡x.y.θ.⎦⎤=⎣⎡vcos(θ)vsin(θ)vLtan(δ)⎦⎤=⎣⎡f1f2f3⎦⎤=f(X,u)

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

△X.=X.−Xr.=[x.−xr.y.−yr.θ.−θr.]≈[σf1σxσf1σyσf1σθσf2σxσf2σyσf2σθσf3σxσf3σyσf3σθ][x−xry−yrθ−θr]+[σf1σvσf1σδσf2σvσf2σδσf3σvσf3σδ][v−vrδ−δr]\bigtriangleup\overset{.}{X}=\overset{.}{X}-\overset{.}{X_r}= \left[ \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{\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 \\ \theta -\theta_r \\ \end{matrix} \right] + \left[ \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 \\ \delta-\delta_r\\ \end{matrix} \right] △X.=X.−Xr.=⎣⎡x.−xr.y.−yr.θ.−θr.⎦⎤≈⎣⎢⎡σxσf1σxσf2σxσf3σyσf1σyσf2σyσf3σθσf1σθσf2σθσf3⎦⎥⎤⎣⎡x−xry−yrθ−θr⎦⎤+⎣⎡σvσf1σvσf2σvσf3σδσf1σδσf2σδσf3⎦⎤[v−vrδ−δr]

写为
△X.=Am△X+Bm△uAm=[00−vsin(θ)00vcos(θ)000]Bm=[cos(θ)0sin(θ)0tan(δ)LvLcos2δ]\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.AmBm=Am△X+Bm△u=⎣⎡000000−vsin(θ)vcos(θ)0⎦⎤=⎣⎡cos(θ)sin(θ)Ltan(δ)00Lcos2δv⎦⎤

线性模型离散化

Am△X(k)+Bm△u(k)=△X.=△X(k+1)−△X(k)TA_m\bigtriangleup X(k) +B_m\bigtriangleup u(k)=\bigtriangleup\overset{.}{X}= \frac{\bigtriangleup X(k+1)-\bigtriangleup X(k)}{T} Am△X(k)+Bm△u(k)=△X.=T△X(k+1)−△X(k)
变形可得：
△X(k+1)=(I+TAm)△X(k)+TBm△u(k)=A△X(k)+B△u(k)Am=[10−vTsin(θ)01vTcos(θ)001]Bm=[Tcos(θ)0Tsin(θ)0Ttan(δ)LTvLcos2δ]\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 &= \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+TAm)△X(k)+TBm△u(k)=A△X(k)+B△u(k)AmBm=⎣⎡100010−vTsin(θ)vTcos(θ)1⎦⎤=⎣⎡Tcos(θ)Tsin(θ)LTtan(δ)00Lcos2δTv⎦⎤