四舵轮线性模型离散化

在上面的运动学模型中，我们得到了：

$$\left\{\begin{array}{c} \dot{X}=V \cos (\beta+\delta) \\ \dot{Y}=V \sin (\beta+\delta)\\ \dot{\psi}=\frac{V \cos \beta}{L_{f}+L_{r}}\left(\tan \theta_{1}-\tan \theta_{2}\right) \end{array}\right.\tag{1}$$

选取状态量为：,控制量，对于参考轨迹上的任意参考点用r表示，即：

$$\dot{X}=f(x, u) \stackrel{r}{\Rightarrow} \dot{X}_{r}=f\left(x_{r}, u_{r}\right) \\\tag{2}$$

其中经过泰勒级数展开，并忽略高阶项：

$$\dot{\chi}=f\left(\chi_{r}, \boldsymbol{u}_{r}\right)+\frac{\partial f(\chi, \boldsymbol{u})}{\partial \chi}\left(\chi-\chi_{r}\right)+\frac{\partial f(\chi, \boldsymbol{u})}{\partial \boldsymbol{u}}\left(\boldsymbol{u}-\boldsymbol{u}_{r}\right) \\\tag{3}$$

对和求雅可比矩阵，有：

$$\frac{\partial f(x, u)}{\partial x}=\left[\begin{array}{ccc} \frac{\partial f_{1}}{\partial x} & \frac{\partial f_{1}}{\partial y} & \frac{\partial f_{1}}{\partial \psi} \\ \frac{\partial f_{2}}{\partial x} & \frac{\partial f_{2}}{\partial y} & \frac{\partial f_{2}}{\partial \psi} \\ \frac{\partial f_{3}}{\partial x} & \frac{\partial f_{3}}{\partial y} & \frac{\partial f_{3}}{\partial \psi} \end{array}\right]=\left[\begin{array}{ccc} 0 & 0 & -v_{r \sin \left(\delta_{r}+\beta\right)} \\ 0 & 0 & v_{r \cos \left(\delta_{r}+\beta\right)} \\ 0 & 0 & 0 \end{array}\right] \\ \frac{\partial f(x, u)}{\partial u}=\left[\begin{array}{ll} \frac{\partial f_{1}}{\partial v} & \frac{\partial f_{1}}{\partial \theta} \\ \frac{\partial f_{2}}{\partial v} & \frac{\partial f_{2}}{\partial \theta} \\ \frac{\partial f_{3}}{\partial v} & \frac{\partial f_{3}}{\partial \theta} \end{array}\right]=\left[\begin{array}{cc} \cos \left(\delta_{r}+\beta\right) & 0 \\ \sin \left(\delta_{r}+\beta\right) & 0 \\ \frac{\tan \theta_{1}-\tan \theta_{2}}{L} & \frac{v_{r}}{L \cos ^{2} \delta_{r}} \end{array}\right] \\\tag{4}$$

则，状态误差的变化量

$$\dot{x}=\left[\begin{array}{c} \dot{x}-\dot{x}_{r} \\ \dot{y}-y_{r} \\ \dot{\psi}-\dot{\psi}_{r} \end{array}\right]=\left[\begin{array}{ccc} 0 & 0 & -v_{r s \sin \left(\delta_{r}+\beta\right)} \\ 0 & 0 & v_{r \cos \left(\delta_{r}+\beta\right)} \\ 0 & 0 & 0 \end{array}\right]\left[\begin{array}{c} x-x_{r} \\ y-y_{r} \\ \psi-\psi_{r} \end{array}\right]+\left[\begin{array}{cc} \cos \left(\delta_{r}+\beta\right) & 0 \\ \sin \left(\delta_{r}+\beta\right) & 0 \\ \frac{\tan \theta_{1}-\tan \theta_{2}}{L} & \frac{v_{r}}{L \cos ^{2} \delta_{r}} \end{array}\right]\left[\begin{array}{} v-v_{r} \\ \theta-\theta_{r} \end{array}\right] \tilde{u}(k)=\tilde{A} \tilde{x}(k)+\tilde{B} \tilde{u}(k) \\\tag{5}$$

接下来进行线性离散化，

$$\dot{\tilde{x}}(k+1)=(T\tilde{A}+I) \tilde{x}(k)+T\tilde{B} \tilde{u}(k) \\\tag{6}$$ $$\tilde{x}(k+1)=\left[\begin{array}{ccc} 1 & 0 & -T v_{r} \sin \psi_{r} \\ 0 & 1 & T v_{r} \cos \psi_{r} \\ 0 & 0 & 1 \end{array}\right] \tilde{x}(k)+\left[\begin{array}{cc} T \cos \psi_{r} & 0 \\ T \sin \psi_{r} & 0 \\ \frac{T\left(\tan \theta_{1}-\tan \theta_{2}\right)}{L} & \frac{v_{r} T}{L \cos ^{2} \delta_{r}} \end{array}\right] \tilde{u}(k)=\tilde{A} \tilde{x}(k)+\tilde{B} \tilde{u}(k) \\\tag{7}$$

其中，T为采样步长，I为单位矩阵，维度与A同。

本篇文章受 CC BY-NC-SA 4.0 协议保护，转载请注明出处。