Skip to main content

四舵轮线性模型离散化

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

{X˙=Vcos⁡(β+δ)Y˙=Vsin⁡(β+δ)ψ˙=Vcos⁡βLf+Lr(tan⁡θ1−tan⁡θ2)(1)\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}

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

X˙=f(x,u)⇒rX˙r=f(xr,ur)(2)\dot{X}=f(x, u) \stackrel{r}{\Rightarrow} \dot{X}_{r}=f\left(x_{r}, u_{r}\right) \\\tag{2}

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

χ˙=f(χr,ur)+∂f(χ,u)∂χ(χ−χr)+∂f(χ,u)∂u(u−ur)(3)\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}

对img和img求雅可比矩阵，有：

∂f(x,u)∂x=[∂f1∂x∂f1∂y∂f1∂ψ∂f2∂x∂f2∂y∂f2∂ψ∂f3∂x∂f3∂y∂f3∂ψ]=[00−vrsin⁡(δr+β)00vrcos⁡(δr+β)000]∂f(x,u)∂u=[∂f1∂v∂f1∂θ∂f2∂v∂f2∂θ∂f3∂v∂f3∂θ]=[cos⁡(δr+β)0sin⁡(δr+β)0tan⁡θ1−tan⁡θ2LvrLcos⁡2δr](4)\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}

则，状态误差的变化量

x˙=[x˙−x˙ry˙−yrψ˙−ψ˙r]=[00−vrssin⁡(δr+β)00vrcos⁡(δr+β)000][x−xry−yrψ−ψr]+[cos⁡(δr+β)0sin⁡(δr+β)0tan⁡θ1−tan⁡θ2LvrLcos⁡2δr][v−vrθ−θr]u~(k)=A~x~(k)+B~u~(k)(5)\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}

接下来进行线性离散化，

x~˙(k+1)=(TA~+I)x~(k)+TB~u~(k)(6)\dot{\tilde{x}}(k+1)=(T\tilde{A}+I) \tilde{x}(k)+T\tilde{B} \tilde{u}(k) \\\tag{6} x~(k+1)=[10−Tvrsin⁡ψr01Tvrcos⁡ψr001]x~(k)+[Tcos⁡ψr0Tsin⁡ψr0T(tan⁡θ1−tan⁡θ2)LvrTLcos⁡2δr]u~(k)=A~x~(k)+B~u~(k)(7)\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 协议保护，转载请注明出处。