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

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

详见：
参考链接

自行车运动模型

运动模型方程：
$$\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\theta }\end{array}\right]=\left[\begin{array}{c}vcos(\theta )\\ vsin(\theta )\\ v\frac{tan(\delta )}{L}\end{array}\right]$$

可写为：
$$\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\theta }\end{array}\right]=\left[\begin{array}{c}cos(\theta )\\ sin(\theta )\\ 0\end{array}\right]v+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]w$$

模型线性化

利用泰勒展开，只保留一阶项
$$\stackrel{.}{X}=\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\theta }\end{array}\right]=\left[\begin{array}{c}vcos(\theta )\\ vsin(\theta )\\ v\frac{tan(\delta )}{L}\end{array}\right]=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=f(X,u)$$

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

$$△\stackrel{.}{X}=\stackrel{.}{X}-\stackrel{.}{X_r}=\left[\begin{array}{c}\stackrel{.}{x}-\stackrel{.}{x_r}\\ \stackrel{.}{y}-\stackrel{.}{y_r}\\ \stackrel{.}{\theta }-\stackrel{.}{{\theta }_r}\end{array}\right]\approx \left[\begin{array}{ccc}\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{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \theta -{\theta }_r\end{array}\right]+\left[\begin{array}{cc}\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{array}\right]\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]$$

写为
$$\begin{array}{rl}△\stackrel{.}{X}& =A_m△X+B_m△u\\ A_m& =\left[\begin{array}{ccc}0& 0& -vsin(\theta )\\ 0& 0& vcos(\theta )\\ 0& 0& 0\end{array}\right]\\ B_m& =\left[\begin{array}{cc}cos(\theta )& 0\\ sin(\theta )& 0\\ \frac{tan(\delta )}{L}& \frac{v}{Lco{s}^2\delta }\end{array}\right]\end{array}$$

线性模型离散化

$$A_m△X(k)+B_m△u(k)=△\stackrel{.}{X}=\frac{△X(k+1)-△X(k)}{T}$$
变形可得：
$$\begin{gathered} \begin{array}{rl}△X(k+1)& =(I+T{A}_m)△X(k)+T{B}_m△u(k)\\ & =A△X(k)+B△u(k)\end{array} \\ \begin{array}{rl}{A}_m& =\left[\begin{array}{ccc}1& 0& -vTsin(\theta )\\ 0& 1& vTcos(\theta )\\ 0& 0& 1\end{array}\right]\\ B_m& =\left[\begin{array}{cc}Tcos(\theta )& 0\\ Tsin(\theta )& 0\\ \frac{Ttan(\delta )}{L}& \frac{Tv}{Lco{s}^2\delta }\end{array}\right]\end{array} \end{gathered}$$