基于欧拉角的PD控制+基于反步法的控制律设计

基于欧拉角的PD控制+基于反步法的控制律设计

1.动力学模型

以最简单的单刚体姿态动力学模型为例

$$\mathbf{J}\dot{\mathbf{\omega }}+\mathbf{\omega }\times \mathbf{J}\mathbf{\omega }=\mathbf{T}\text{\hspace{1em}(1)}$$

其中$\mathbf{J}$为相对于刚体质心的主转动惯量在本体系下的列阵,$\mathbf{\omega }$、$\dot{\mathbf{\omega }}$分别为角速度和角加速度在本体系下的列阵,$\mathbf{T}$控制力矩在本体系下的列阵。

2.运动学模型

2.1基于欧拉角的运动学模型

采用ZYX的旋转顺序得到角速度$\mathbf{\omega }$到欧拉角导数的$[\dot{\phi },\dot{\theta },\dot{\psi }{]}^{\mathrm{T}}$，其中$\dot{\phi }$表示绕X轴旋转的角速度，$\dot{\theta }$表示绕Y轴旋转的角速度，$\dot{\psi }$表示绕Z轴旋转的角速度。（推导过程TODO ）

$$\mathbf{\omega }=\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& -S\theta \\ 0& C\phi & C\theta S\phi \\ 0& -S\phi & C\theta C\phi \end{array}\right]\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\underline{T}\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\text{\hspace{1em}(2)}$$

$$\dot{\mathbf{\varphi }}=\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\frac{1}{C\theta }\left[\begin{array}{ccc}C\theta & S\phi S\theta & C\phi S\theta \\ 0& C\phi C\theta & -S\phi C\theta \\ 0& S\phi & C\phi \end{array}\right]\mathbf{\omega }=\underline{M}(\varphi )\mathbf{\omega }\text{\hspace{1em}(3)}$$

在小角度下，可以进行线性化，即，$C\theta =1$,$S\theta =\theta$，$\theta$，$\dot{\theta }$等均为小值，二阶及以上小值=0。

上式可以写为：

$$\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& -\theta \\ 0& 1& -\phi \\ 0& -\phi & 1\end{array}\right]\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}\dot{\phi }-\theta \dot{\psi }\\ \dot{\theta }-\phi \dot{\psi }\\ \dot{\psi }-\phi \dot{\theta }\end{array}\right]=\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\text{\hspace{1em}(4)}$$

$$\dot{\mathbf{\varphi }}=\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \phi \theta & \theta \\ 0& 1& -\phi \\ 0& \phi & 1\end{array}\right]\mathbf{\omega }=\left[\begin{array}{c}{\omega }_x+\theta {\omega }_z\\ {\omega }_y-\phi {\omega }_z\\ {\omega }_z+\phi {\omega }_y\end{array}\right]\text{\hspace{1em}(5)}$$

将式子4带入式1得到

$$\left[\begin{array}{c}{J}_x\ddot{\phi }\\ J_y\ddot{\theta }\\ J_z\ddot{\psi }\end{array}\right]=\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right]\text{\hspace{1em}(6)}$$

3.控制算法

3.1 欧拉角PID控制

设目标欧拉角为

$${\mathbf{\varphi }}_d=\left[\begin{array}{c}{\phi }_d\\ {\theta }_d\\ {\psi }_d\end{array}\right]=\left[\begin{array}{c}-20^{\circ }\\ 5^{\circ }\\ 15^{\circ }\end{array}\right]$$

当前的欧拉角为

$$\mathbf{\varphi }=\left[\begin{array}{c}\phi \\ \theta \\ \psi \end{array}\right]$$

则误差欧拉角为

$${\mathbf{\varphi }}_e={\mathbf{\varphi }}_d-\mathbf{\varphi }$$

有

$${\dot{\mathbf{\varphi }}}_e=\dot{{\mathbf{\varphi }}_d}-\dot{\mathbf{\varphi }}$$

可以看到，在小角度时，可以看成线性化模型，使用pid控制律如下所示

$$Kp=8\ast diag(\left[\begin{array}{ccc}1& 1& 1\end{array}\right])$$

$$Kd=10\ast diag(\left[\begin{array}{ccc}1& 1& 1\end{array}\right])$$

控制力矩为
$$u=Kp{\mathbf{\varphi }}_e+Kd{\dot{\mathbf{\varphi }}}_e$$

控制结果如下图所示。

3.2 基于反步法的非线性控制

参考博客

基于上述讨论，我们有

$${\dot{\mathbf{\varphi }}}_e=\dot{{\mathbf{\varphi }}_d}-\dot{\mathbf{\varphi }}=\dot{{\mathbf{\varphi }}_d}-\underline{M}(\varphi )\mathbf{\omega }\text{\hspace{1em}(10)}$$

$$\dot{\mathbf{\omega }}={\mathbf{J}}^{-1}(\mathbf{T}-\mathbf{\omega }\times \mathbf{J}\mathbf{\omega })\text{\hspace{1em}(11)}$$

其中式10为运动学，由角速度得到角度，这一层可以通过改变角速度，控制角度。式11为动力学，由力矩得到角速度，这一次可以通过改变力矩，控制角速度。

反步法(backstepping)将非线性系统分解成不超过系统阶数的子系统，然后为每个子，系统设计部分Lyapunov 函数（简称V函数）和中间虚拟控制量，一直“后退到整个系统将它们集成起来完成整M个控制律的设计。

对于运动学层，需要将$\boldsymbol{\phi}_e $控制到0，设李雅普诺夫函数为：

$$V_1=\frac{1}{2}{\mathbf{\varphi }}_e^{\mathrm{T}}{\mathbf{\varphi }}_e\text{\hspace{1em}(12)}$$

则

$${\dot{V}}_1={\mathbf{\varphi }}_e^{\mathrm{T}}(\dot{{\mathbf{\varphi }}_d}-\underline{M}(\varphi )\mathbf{\omega })\text{\hspace{1em}(12)}$$

当$\underline{M}(\varphi )\mathbf{\omega }=\dot{{\mathbf{\varphi }}_d}+K_1{\mathbf{\varphi }}_e$,则有${\dot{V}}_1=-\frac{1}{2}{\mathbf{\varphi }}_e^{\mathrm{T}}{K}_1{\mathbf{\varphi }}_e<0$当$K_1>0$时。

因此，对于动力学层，需要将$\mathbf{\omega }$控制到满足$\underline{M}(\varphi )\mathbf{\omega }=\dot{{\mathbf{\varphi }}_d}+K_1{\mathbf{\varphi }}_e$,定义新的控制量${\mathbf{\omega }}_d=\underline{M}(\varphi {)}^{-1}（\dot{{\mathbf{\varphi }}_d}+K_1{\mathbf{\varphi }}_e）$，则角速度误差定义为：

$${\mathbf{\omega }}_e={\mathbf{\omega }}_d-\mathbf{\omega }\text{\hspace{1em}(13)}$$

带入式12,重写$\dot{V_1}$有

$$\dot{V_1}=-\frac{1}{2}{\mathbf{\varphi }}_e^{\mathrm{T}}{K}_1{\mathbf{\varphi }}_e+{\mathbf{\varphi }}_e^{\mathrm{T}}{\underline{T}}^{-1}{\mathbf{\omega }}_e$$

结合上述推推导，我们有

$$\begin{gathered} {\dot{\mathbf{\omega }}}_e={\dot{\mathbf{\omega }}}_d-\dot{\mathbf{\omega }}= \\ \dot{\underline{M}(\varphi {)}^{-1}}（\dot{{\mathbf{\varphi }}_d}+K_1{\mathbf{\varphi }}_e）+\underline{M}(\varphi {)}^{-1}（\ddot{{\mathbf{\varphi }}_d}+K_1{\dot{\mathbf{\varphi }}}_e）- \\ {\mathbf{J}}^{-1}(\mathbf{T}-\mathbf{\omega }\times \mathbf{J}\mathbf{\omega })\text{\hspace{1em}(13)} \end{gathered}$$

其中，

$$\underline{T}=\left[\begin{array}{ccc}1& 0& -S\theta \\ 0& C\phi & C\theta S\phi \\ 0& -S\phi & C\theta C\phi \end{array}\right]$$

$$\underline{M}=\frac{1}{C\theta }\left[\begin{array}{ccc}C\theta & S\phi S\theta & C\phi S\theta \\ 0& C\phi C\theta & -S\phi C\theta \\ 0& S\phi & C\phi \end{array}\right]$$

$$\underline{T}={\underline{M}}^{-1}$$

$$\dot{\underline{T}}=\left[\begin{array}{ccc}0& 0& -C\theta \\ 0& -S\phi & -S\theta S\phi +C\theta C\phi \\ 0& -C\phi & -S\theta C\phi -C\theta S\phi \end{array}\right]$$

所有式13可用写为
$$\begin{gathered} {\dot{\mathbf{\omega }}}_e={\dot{\mathbf{\omega }}}_d-\dot{\mathbf{\omega }}= \\ \dot{\underline{T}}（\dot{{\mathbf{\varphi }}_d}+K_1{\mathbf{\varphi }}_e）+\underline{T}（\ddot{{\mathbf{\varphi }}_d}+K_1({\dot{\mathbf{\varphi }}}_d-{\underline{T}}^{-1}\mathbf{\omega })）- \\ {\mathbf{J}}^{-1}(\mathbf{T}-\mathbf{\omega }\times \mathbf{J}\mathbf{\omega })\text{\hspace{1em}(14)} \end{gathered}$$

为使系统稳定，选取李雅普诺夫函数：

$$\begin{gathered} V_2=V_1+\frac{1}{2}{\mathbf{\omega }}_e^{\mathrm{T}}{\mathbf{\omega }}_e= \\ \text{\hspace{1em}(15)} \end{gathered}$$

$$\begin{gathered} {\dot{V}}_2={\dot{V}}_1+{\mathbf{\omega }}_e^{\mathrm{T}}{\dot{\mathbf{\omega }}}_e= \\ -\frac{1}{2}{\mathbf{\varphi }}_e^{\mathrm{T}}{K}_1{\mathbf{\varphi }}_e+{\mathbf{\varphi }}_e^{\mathrm{T}}{\underline{T}}^{-1}{\mathbf{\omega }}_e+{\mathbf{\omega }}_e^{\mathrm{T}}{\dot{\mathbf{\omega }}}_e= \\ -\frac{1}{2}{\mathbf{\varphi }}_e^{\mathrm{T}}{K}_1{\mathbf{\varphi }}_e+{\mathbf{\varphi }}_e^{\mathrm{T}}{\underline{T}}^{-1}{\mathbf{\omega }}_e+ \\ {\mathbf{\omega }}_e^{\mathrm{T}}(\dot{\underline{T}}（\dot{{\mathbf{\varphi }}_d}+K_1{\mathbf{\varphi }}_e）+\underline{T}（\ddot{{\mathbf{\varphi }}_d}+K_1({\dot{\mathbf{\varphi }}}_d-{\underline{T}}^{-1}\mathbf{\omega })）- \\ {\mathbf{J}}^{-1}(\mathbf{T}-\mathbf{\omega }\times \mathbf{J}\mathbf{\omega })) \end{gathered}$$

令
$$\begin{gathered} \mathbf{T}=(\mathbf{\omega }\times \mathbf{J}\mathbf{\omega })+ \\ \underline{J}(K_2{\mathbf{\omega }}_e+({\underline{T}}^{-1}{)}^{\mathrm{T}}{\mathbf{\varphi }}_e+\dot{\underline{T}}(\dot{{\mathbf{\varphi }}_d}+K_1{\mathbf{\varphi }}_e)+\underline{T}(\ddot{{\mathbf{\varphi }}_d}+K_1({\dot{\mathbf{\varphi }}}_d-{\underline{T}}^{-1}\mathbf{\omega }))) \end{gathered}$$

可得

$${\dot{V}}_2=-\frac{1}{2}{\mathbf{\varphi }}_e^{\mathrm{T}}{K}_1{\mathbf{\varphi }}_e-\frac{1}{2}{\mathbf{\omega }}_e^{\mathrm{T}}{K}_2{\mathbf{\omega }}_e\text{\hspace{1em}(17)}$$

当$K_1>0$,$K_2>0$时，系统稳定。其中，$\mathbf{T}$中的$K_2{\mathbf{\omega }}_e$项使式17中多出$-\frac{1}{2}{\mathbf{\omega }}_e^{\mathrm{T}}{K}_2{\mathbf{\omega }}_e$，为了${\mathbf{\varphi }}_e=0$而${\mathbf{\omega }}_e\ne 0$时，控制力矩仍然能向目标值控制。

设置参数$K_1=2$，$K_2=2$。

控制结果如下图所示。