汽车运动学模型

1. 汽车运动学模型

1. 运动学模型

在后轴处，即$(x_r,y_r)$处，速度为
$$v_r={\dot{x}}_{r}cos(\phi )+{\dot{y}}_{r}sin(\phi )$$
在沿车横向的约束条件为：
$$\{\begin{array}{rl}& {\dot{x}}_{f}sin(\phi +{\delta }_f)-{\dot{y}}_{f}cos(\phi +{\delta }_f)\\ & {\dot{x}}_{r}sin(\phi )-{\dot{y}}_{r}cos(\phi )\end{array}$$
并且x方向和y方向速度与车纵向速度的关系为：
$$\{\begin{array}{rl}& {\dot{x}}_r=v_{r}cos(\phi )\\ & {\dot{y}}_r=v_{r}sin(\phi )\end{array}$$
前后轮的关系为：
$$\{\begin{array}{rl}& {\dot{x}}_f=x_r+lcos(\phi )\\ & {\dot{y}}_f=y_r+lsin(\phi )\end{array}$$
对上述关系求导得：
$$\{\begin{array}{rl}& {\dot{x}}_f=v_{r}cos(\phi )-\omega lsin(\phi )\\ & {\dot{y}}_f=v_{r}sin(\phi )+\omega lcos(\phi )\end{array}$$
联立上面速度关系可得到：
$$\frac{sin(\phi +{\delta }_f)}{cos(\phi +{\delta }_f)}=\frac{v_{r}sin(\phi +\omega lcos(\phi ))}{v_{r}cos(\phi )-\omega lsin(\phi )}$$
对上式展开，即得到：
$$\omega =\frac{v_r}{l}tan({\delta }_f)$$
即，我们得到如下非线性运动学模型：
$$\left[\begin{array}{c}{\dot{x}}_r\\ {\dot{y}}_r\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}cos(\phi )\\ sin(\phi )\\ \frac{tan({\delta }_f)}{l}\end{array}\right]v_r$$
其中，状态变量为 ：
$${\left[\begin{array}{ccc}{x}_r& y_r& \phi \end{array}\right]}^T$$
控制量为$v_r和{\delta }_f$

2. 非线性模型线性化

利用泰勒展开：
设参考系统为：
$${\dot{\xi }}_r=f({\xi }_r,u_r)$$
当前系统在参考系统点处展开得：
$$\dot{\xi }=f({\xi }_r,u_r)+\frac{\partial f}{\partial \xi }{|}_{\xi ={\xi }_r,u=u_r}(\xi -{\xi }_r)+\frac{\partial f}{\partial u}{|}_{\xi ={\xi }_r,u=u_r}(u-u_r)$$
即我们对非线性模型进行一阶泰勒展开并离散化得到：
$${\tilde{\xi }}_{kin}(k+1)=A_{kin}(k){\tilde{\xi }}_{kin}(k)+B_{kin}(k){\tilde{u}}_{kin}(k)$$
其中

$${\tilde{\xi }}_{kin}(k)=\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]A_{kin}=\left[\begin{array}{ccc}1& 0& -v_{r}sin({\phi }_r)T\\ 0& 1& v_{r}cos({\phi }_r)T\\ 0& 0& 1\end{array}\right]B_{kin}=\left[\begin{array}{cc}cos({\phi }_r)T& 0\\ sin({\phi }_r)T& 0\\ \frac{tan({\delta }_f)T}{l}& \frac{v_{r}T}{lco{s}^2({\delta }_f)}\end{array}\right]$$