姿态控制

摘要：讨论不同姿态表示方法下的姿态运动学和动力学控制。包含的姿态控制方法有：基于欧拉角的控制、基于四元数的控制、基于李群李代数的控制(几何控制)。

1 姿态运动学控制

 这里的运动学考虑姿态运动的一阶模型，通过设定期望的角速度让刚体到达期望的姿态。通过对简单的运动学控制效果进行比较，能够直观地展示不同姿态表示法的特点。

1.1 基于欧拉角的运动学控制

 使用绕机体坐标系旋转的z-y-x欧拉角$\boldsymbol{\Phi}=[\phi,\theta,\psi]^{\mathrm{T}}$来表示姿态，系统的输入为刚体坐标系下的角速度$\boldsymbol{\omega}_b$，采用欧拉角表示的姿态运动学模型可以写作：

$$\begin{align} &\dot{\boldsymbol{\Phi}}=T(\boldsymbol{\Phi})\boldsymbol{\boldsymbol{\omega}}_b\\ \end{align}\tag{1.1}$$

其中

$$T(\boldsymbol{\Phi})=\begin{bmatrix} 1 & \mathrm{sin}\phi \mathrm{tan}\theta&\mathrm{cos}\phi\mathrm{tan}\theta\\ 0 & \mathrm{cos}\phi & -\mathrm{sin}\phi\\ 0 & \mathrm{sin}\phi \mathrm{sec}\theta & \mathrm{cos}\phi \mathrm{sec}\theta \end{bmatrix} \tag{1.2}$$

当$\theta\neq\pm \pi/2$时，$T(\boldsymbol{\Phi})$的逆为：

$$T^{-1}(\boldsymbol{\Phi})=\begin{bmatrix} 1 & 0&-\mathrm{sin}\theta\\ 0 & \mathrm{cos}\phi & \mathrm{sin}\phi\mathrm{cos}\theta\\ 0 & -\mathrm{sin}\phi & \mathrm{cos}\phi \mathrm{cos}\theta \end{bmatrix}\tag{1.3}$$

 由于欧拉角无法直接表示$SO(3)$空间中的旋转误差，因此采用线性误差来表示旋转误差，定义期望的姿态为$\boldsymbol{\Phi}_d$，这里考虑姿态的定点控制，则$\dot{\boldsymbol{\Phi}}_d=0$。定义欧拉角的误差为：

$$\boldsymbol{\Phi}_e=\boldsymbol{\Phi}-\boldsymbol{\Phi}_d \tag{1.4}$$

将公式(1.1)改写作关于欧拉角误差的运动学模型：

$$\dot{\boldsymbol{\Phi}}_e=\dot{\boldsymbol{\Phi}}-\dot{\boldsymbol{\Phi}}_d=T(\boldsymbol{\Phi})\boldsymbol{\boldsymbol{\omega}}_b \tag{1.5}$$

设定控制输入$\boldsymbol{\omega}_b$为：

$$\boldsymbol{\omega}_b=-T^{-1}(\boldsymbol{\Phi})K\boldsymbol{\Phi}_e \tag{1.6}$$

其中$K>0\in\mathbb{R}^{3\times3}$为控制增益，是正定的对角矩阵。特别地，如果设定三个旋转方向的增益都相同，$K$可以使用$k\in\mathbb{R}$代替。
 将(1.6)代入(1.1)中可以得到:

$$\dot{\boldsymbol{\Phi}}_e+K\boldsymbol{\Phi}_e=0 \tag{1.7}$$

解上述微分方程可知，当$t\rightarrow \infty$时，$\boldsymbol{\Phi}_e$以指数误差收敛到$0$，即刚体的姿态最后能收敛到期望姿态。

奇异点和线性误差导致的问题
 由于欧拉角存在万向节死锁问题，且欧拉角误差范数在$SO(3)$并不是两个姿态之间的最短路径，因而基于欧拉角的控制会有以下两个问题。

1. 控制器设计基于$\theta\neq\pm \pi/2$的假设。$\theta=\pm \pi/2$是奇异位置，当姿态无限接近$\theta\neq\pm \pi/2$时，一个小的旋转变化就可能导致欧拉角数值出现明显的变化，容易导致数值计算的不稳定。
2. 欧拉角的线性误差在$SO(3)$空间中表示的不是最短路径，因此姿态的收敛路径在$SO(3)$空间也不是最短的，并且越接近奇异位置收敛路径与最短路径差的越多。

1.2 基于四元数的运动学控制

 定义刚体姿态的四元数为$\boldsymbol{q}=[s,\boldsymbol{v}]^{\mathrm{T}}$，期望姿态的四元数为$\boldsymbol{q}_d=[s_d,\boldsymbol{v}_d]^{\mathrm{T}}$。考虑姿态的定点控制，则$\dot{\boldsymbol{q}}_d=0$。$\mathbb{S}^3$空间中的旋转误差可以使用四元数乘法表示：

$$\boldsymbol{q}_e=\bar{\boldsymbol{q}}_d\otimes\boldsymbol{q} \tag{1.8}$$

其中$\boldsymbol{q}_e=[s_e,\boldsymbol{v}_e]^{\mathrm{T}}$为旋转的四元数误差。这一误差的范数表示了当前姿态到期望姿态在$\mathbb{S}^3$或$SO(3)$空间中的最短路径。当$\boldsymbol{q}_e\rightarrow[1,0,0,0]^{\mathrm{T}}$时，表明目标姿态和期望姿态重合。
 对公式(8)求导：

$$\begin{aligned} \dot{\boldsymbol{q}}_e=&\dot{\bar{\boldsymbol{q}}}_d\otimes\boldsymbol{q}+\bar{\boldsymbol{q}}_d\otimes\dot{\boldsymbol{q}}\\ =&\frac{1}{2}\bar{\boldsymbol{q}}_d\otimes\boldsymbol{q}\otimes\begin{bmatrix}0\\\boldsymbol{\omega}_b\end{bmatrix}\\ =&\frac{1}{2}\boldsymbol{q}_e\otimes\begin{bmatrix}0\\\boldsymbol{\omega}_b\end{bmatrix} \end{aligned} \tag{1.9}$$

将公式(1.8)展开可以得到：

$$\begin{align} &\dot{s}_e=-\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}\boldsymbol{\omega}_b \tag{1.10}\\ &\dot{\boldsymbol{v}}_e=\frac{1}{2}(s_e\boldsymbol{\omega}_b+\boldsymbol{v}_e\times\boldsymbol{\omega}_b) \tag{1.11}\\ \end{align}$$

四元数本身有一个单位范数的约束，因此让$\boldsymbol{v}_e$收敛到0的同时$s_e$也会收敛到1。容易想到将控制器$\boldsymbol{\omega}_b$设计为：$\boldsymbol{\omega}_b=-\frac{2}{s_e}K\boldsymbol{v}_e$，这样公式(11)就可以改写为$\dot{\boldsymbol{v}}_e+K\boldsymbol{v}_e=0$的形式。但是$s_e$的值可能为0的，因此这么设计是不可行的。
 考虑控制器为：

$$\boldsymbol{\omega}_b=-2K\boldsymbol{v}_e\tag{1.12}$$

其中$K$为对角矩阵，则公式(1.10)和(1.11)可以改写为：

$$\begin{align} &\dot{s}_e=\boldsymbol{v}_e^{\mathrm{T}}K\boldsymbol{v}_e \tag{1.13}\\ &\dot{\boldsymbol{v}}_e=-s_eK\boldsymbol{v}_e\tag{1.14} \end{align}$$

其中$K>0\in\mathbb{R}^{3\times3}$为控制增益，是正定的对角矩阵。公式(1.13)中存在一个$s_e$，如果$s_e$是负值，仅从公式看，公式(13)是不收敛的。不过因为$s_e$和$\boldsymbol{v}_e$是相互关联的变量，这种固定一个变量去分析另一个变量本身就是不可行的。因此，这里使用李亚普诺夫方法进行分析。定义李雅普诺夫函数为：

$$V=\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}\boldsymbol{v}_e+\frac{1}{2}(1-s_e)^2>0 \tag{1.15}$$

当$V\rightarrow 0$时，有$\boldsymbol{v}_e\rightarrow 0$和$s_e\rightarrow 1$，刚体收敛到期望姿态。因此如果$\dot{V}$是负定的，就可以证明控制器是收敛的。对公式(1.15)求导，并将(1.13)和(1.14)代入，可得：

$$\begin{aligned} \dot{V}=&\boldsymbol{v}_e^{\mathrm{T}}\dot{\boldsymbol{v}}_e-(1-s_e)\dot{s}_e\\ =&-s_e\boldsymbol{v}_e^{\mathrm{T}}K\boldsymbol{v}_e-(1-s_e)\boldsymbol{v}_e^{\mathrm{T}}K\boldsymbol{v}_e\\ =&-\boldsymbol{v}_e^{\mathrm{T}}K\boldsymbol{v}_e \le 0\\ \end{aligned}\tag{1.16}$$

由于$V$的下界是0，$\dot{V}\le0$，并且：当且仅当$\boldsymbol{v}_e=0$时$\dot{V}=0$，因此当$t \rightarrow \infty$时，$\boldsymbol{v}_e\rightarrow 0$。(这一结论具体可以参考拉塞尔不变性原理)

双倍覆盖导致的问题
 根据四元数的定义可知，四元数是双倍覆盖的，即四元数$\boldsymbol{q}$和$-\boldsymbol{q}$表示的其实是同一个旋转。这在控制中会导致一些问题：

1. 如果从仿真环境或者传感器解算出的四元数在正负之间反复横跳，会使得(12)中的控制器计算出来的控制量也在正负之间切换，导致控制不能收敛。这一问题可以通过选取四元数的$s$值为正的那一组四元数作为采样数值来解决。

2. (1.12)中给出的控制器和(1.15)中给出的李雅普诺夫函数，证明了四元数误差$\boldsymbol{q}_e$是收敛到$[1,0,0,0]^{\mathrm{T}}$的。如果初始时刻为$s_e=-0.9$，四元数误差会以$s_e: -0.9\rightarrow 0\rightarrow 1$的方式收敛到期望位置。但实际上$s_e:-0.9\rightarrow-1$在旋转空间中才是最短的收敛路径，反映的现象就是姿态会反向绕一个大圈收敛到期望姿态。因此，对于$s_e>0$，希望$s_e\rightarrow 1$，而对于$s_e<0$，希望$s_e\rightarrow -1$，这样刚体的姿态会以最短的那条路径收敛到期望姿态。可以定义如下李雅普诺夫函数

   $$\begin{aligned} V_{+}=&\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}\boldsymbol{v}_e+\frac{1}{2}(1-s_e)^2,~when:s_e>0\\ V_{-}=&\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}\boldsymbol{v}_e+\frac{1}{2}(1+s_e)^2,~when:s_e<0 \end{aligned}\tag{1.17}$$

   控制器可以改进为：

   $$\boldsymbol{\omega}_b=-2\mathrm{sgn}(s_e)K\boldsymbol{v}_e\tag{1.18}$$

   其中$\mathrm{sgn}(.)$为符号函数。可以证明(1.18)中的控制器对于$V_+$和$V_-$都有：

   $$\dot{V}_+=\dot{V}_-=-\boldsymbol{v}_e^{\mathrm{T}}K\boldsymbol{v}_e \le 0 \tag{1.19}$$

1.3 基于$SO(3)$的运动学控制

 记当前姿态和期望姿态的旋转姿态分别为$R$和$R_d$。考虑定点控制，则$\dot{R}_d=0$。$SO(3)$空间中的旋转误差可以表示为：

$$R_e=R_d^{\mathrm{T}}R \tag{1.20}$$

同时：

$$\dot{R}_e=R_e\mathrm{sk}(\boldsymbol{\omega}_b) \tag{1.21}$$

 当刚体姿态逼近期望姿态时，$R_e\rightarrow I$，因此可以考虑如下李雅普诺夫函数：

$$V=\mathrm{tr}(I-R_e)>0 \tag{1.22}$$

对公式(1.22)求导：

$$\dot{V}=-\mathrm{tr}[R_e\mathrm{sk}(\boldsymbol{\omega}_b)] \tag{1.23}$$

结合$\mathfrak{so}(3)$的定义，$SO(3)$的切空间是一个反对称矩阵或反对称矩阵对应的$\mathbb{R}^3$向量。记$\boldsymbol{\phi}_e=(R_e-R_e^{\mathrm{T}})^{\vee}$，容易想到将$\boldsymbol{\omega}_b$设计为：

$$\boldsymbol{\omega}_b=-K\boldsymbol{\phi}_e\tag{1.24}$$

则：

$$\mathrm{sk}(\boldsymbol{\omega}_b)=-K(R_e-R_e^{\mathrm{T}}) \tag{1.25}$$

其中$K$为对角矩阵。将(1.25)代入(1.23)中，得到：

$$\dot{V}=-\mathrm{tr}[K(I-R_eR_e)] \le -k_{\mathrm{min}}\mathrm{tr}(I-R_eR_e)\le 0 \tag{1.26}$$

其中$k_{\mathrm{min}}$为$K$矩阵中最小的元素。对于$\dot{V}$，当且仅当$R_e=I$时$\dot{V}=0$，根据拉塞尔不变性原理，当$t\rightarrow 0$时，$R_e\rightarrow I$。

2 姿态动力学控制

 考虑完整的姿态轨迹跟踪问题，对于不同的姿态表示方法，分别使用反步控制来设计姿态控制器。

2.1 基于欧拉角的控制

 记当前姿态和期望姿态的欧拉角分别为$\boldsymbol{\Phi}=[\phi,\theta,\psi]^{\mathrm{T}}$和$\boldsymbol{\Phi}=[\phi_d,\theta_d,\psi_d]^{\mathrm{T}}$，欧拉角的线性误差为$\boldsymbol{\Phi}_e=\boldsymbol{\Phi}-\boldsymbol{\Phi}_d $。欧拉角表示的旋转误差的动力学可以写为：

$$\begin{align} &\dot{\boldsymbol{\Phi}}_e=T(\boldsymbol{\Phi})\boldsymbol{\omega}_b-\dot{\boldsymbol{\Phi}}_d \tag{2.1}\\ &\dot{\boldsymbol{\omega}}_b=J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau})\tag{2.2} \end{align}$$

其中$J>0$为刚体的惯性矩阵，$\boldsymbol{\tau}$为刚体在自身坐标系下的受力，即待设计的控制量。

 选取李雅普诺夫函数为：

$$V_1=\frac{1}{2}\boldsymbol{\Phi}_e^{\mathrm{T}}\boldsymbol{\Phi}_e \tag{2.3}$$

则：

$$\dot{V}_1=\boldsymbol{\Phi}_e^{\mathrm{T}}\dot{\boldsymbol{\Phi}}_e=\boldsymbol{\Phi}_e^{\mathrm{T}}[T(\boldsymbol{\Phi})\boldsymbol{\omega}_b-\dot{\boldsymbol{\Phi}}_d] \tag{2.4}$$

当$T(\boldsymbol{\Phi})\boldsymbol{\omega}_b=-(K_1\boldsymbol{\Phi}_e-\dot{\boldsymbol{\Phi}}_d)$时，有$\dot{V}_1=-\boldsymbol{\Phi}_e^{\mathrm{T}}K_1\boldsymbol{\Phi}_e\le 0$，因此，可以定义虚拟控制量：

$$\boldsymbol{\Phi}_z=T(\boldsymbol{\Phi})\boldsymbol{\omega}_b+(K_1\boldsymbol{\Phi}_e-\dot{\boldsymbol{\Phi}}_d) \tag{2.5}$$

则$\dot{V}_1$可以被改写为：

$$\dot{V}_1=-\boldsymbol{\Phi}_e^{\mathrm{T}}K_1\boldsymbol{\Phi}_e+\boldsymbol{\Phi}_e^{\mathrm{T}}\boldsymbol{\Phi}_z \tag{2.6}$$

对(2.5)求导可以得到$\boldsymbol{\Phi}_z$的导数：

$$\begin{aligned} \dot{\boldsymbol{\Phi}}_z=&\dot{T}(\boldsymbol{\Phi})\boldsymbol{\omega}_b+T(\boldsymbol{\Phi})\dot{\boldsymbol{\omega}}_b+K_1\dot{\boldsymbol{\Phi}}_e-\ddot{\boldsymbol{\Phi}}_d\\ =&T(\boldsymbol{\Phi})J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau})+\dot{T}(\boldsymbol{\Phi})\boldsymbol{\omega}_b\\ &+K_1T(\boldsymbol{\Phi})\boldsymbol{\omega}_b-(K_1\dot{\boldsymbol{\Phi}}_d+\ddot{\boldsymbol{\Phi}}_d) \end{aligned}\tag{2.7}$$

 重新选取李雅普诺夫函数：

$$V_2=V_1+\frac{1}{2}\boldsymbol{\Phi}_z^{\mathrm{T}}\boldsymbol{\Phi}_z \tag{2.8}$$

则：

$$\begin{aligned} \dot{V}_2=&-\boldsymbol{\Phi}_e^{\mathrm{T}}K_1\boldsymbol{\Phi}_e+\boldsymbol{\Phi}_e^{\mathrm{T}}\boldsymbol{\Phi}_z\\ &+\boldsymbol{\Phi}_z^{\mathrm{T}}[T(\boldsymbol{\Phi})J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau})+\dot{T}(\boldsymbol{\Phi})\boldsymbol{\omega}_b\\ &~~~~~~~~~~~+K_1T(\boldsymbol{\Phi})\boldsymbol{\omega}_b-(K_1\dot{\boldsymbol{\Phi}}_d+\ddot{\boldsymbol{\Phi}}_d)] \end{aligned} \tag{2.9}$$

设计如下控制器：

$$\begin{aligned} \boldsymbol{\tau}=\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b-JT^{-1}(\boldsymbol{\Phi})[&K_2\boldsymbol{\Phi}_z+\dot{T}(\boldsymbol{\Phi})\boldsymbol{\omega}_b+K_1T(\boldsymbol{\Phi})\boldsymbol{\omega}_b\\ &+\boldsymbol{\Phi}_e-(K_1\dot{\boldsymbol{\Phi}}_d+\ddot{\boldsymbol{\Phi}}_d)] \end{aligned}\tag{2.10}$$

其中$K_1,K_2$为正定对角矩阵。$\dot{T}(\boldsymbol{\Phi})$的展开如下：

$$\dot{T}(\boldsymbol{\Phi})=\left [\boldsymbol{0},AT(\boldsymbol{\Phi})\boldsymbol{\omega}_b, BT(\boldsymbol{\Phi})\boldsymbol{\omega}_b\right ]\tag{2.11}$$

其中：

$$A=\begin{bmatrix} \mathrm{cos}\phi\mathrm{tan}\theta&\mathrm{sin}\phi\mathrm{sec}^2\theta &0 \\ -\mathrm{sin}\phi&0 &0 \\ \mathrm{cos}\phi\mathrm{sec}\theta&\mathrm{sin}\phi\mathrm{sec}\theta\mathrm{tan}\theta &0 \end{bmatrix}, B=\begin{bmatrix} -\mathrm{sin}\phi\mathrm{tan}\theta&\mathrm{cos}\phi\mathrm{sec}^2\theta &0 \\ -\mathrm{cos}\phi&0 &0 \\ -\mathrm{sin}\phi\mathrm{sec}\theta&\mathrm{cos}\phi\mathrm{sec}\theta\mathrm{tan}\theta &0 \end{bmatrix}$$

将(2.10)代入(2.9)可以得到：

$$\dot{V}_2\le-\boldsymbol{\Phi}_e^{\mathrm{T}}K_1\boldsymbol{\Phi}_e-\boldsymbol{\Phi}_z^{\mathrm{T}}K_2\boldsymbol{\Phi}_z \tag{2.12}$$

对于$\dot{V}_2$，当且仅当$\boldsymbol{\Phi}_e=0$和$\boldsymbol{\Phi}_z=0$时$\dot{V}_2=0$，根据拉塞尔不变性原理，当$t\rightarrow \infty$时，$\boldsymbol{\Phi}_e\rightarrow 0$和$\boldsymbol{\Phi}_z \rightarrow 0$。

2.2 基于四元数的控制

 定义刚体姿态的四元数为$\boldsymbol{q}=[s,\boldsymbol{v}]^{\mathrm{T}}$，期望姿态的四元数为$\boldsymbol{q}_d=[s_d,\boldsymbol{v}_d]^{\mathrm{T}}$，四元数误差为$\boldsymbol{q}_e=\bar{\boldsymbol{q}}_d\otimes\boldsymbol{q}=[s_e,\boldsymbol{v}_e]^{\mathrm{T}}$。对$\boldsymbol{q}_d\boldsymbol{q}_e=\boldsymbol{q}$两边同时求导，可以得到：

$$\dot{\boldsymbol{q}}_e=\frac{1}{2}\boldsymbol{q}_e\otimes\begin{bmatrix}0\\\boldsymbol{\omega}_b\end{bmatrix}-\frac{1}{2}\begin{bmatrix}0\\\boldsymbol{\omega}_d\end{bmatrix}\otimes\boldsymbol{q}_e\tag{2.13}$$

其中$\boldsymbol{\omega}_d$为期望的角速度。将(2.13)展开，四元数表示的姿态动力学可以写作：

$$\begin{align} &\dot{s}_e=-\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}(\boldsymbol{\omega}_b-\boldsymbol{\omega}_d) \tag{2.14}\\ &\dot{\boldsymbol{v}}_e=\frac{1}{2}[s_e(\boldsymbol{\omega}_b-\boldsymbol{\omega}_d)+\boldsymbol{v}_e\times(\boldsymbol{\omega}_b+\boldsymbol{\omega}_d)] \tag{2.15}\\ &\dot{\boldsymbol{\omega}}_b=J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau})\tag{2.16} \end{align}$$

 选取李雅普诺夫函数为：

$$V_1=\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}\boldsymbol{v}_e+\frac{1}{2}(1-s_e)^2\tag{2.17}$$

则(这里利用了向量混合积的性质)：

$$\begin{aligned} \dot{V}_1=&\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}[s_e(\boldsymbol{\omega}_b-\boldsymbol{\omega}_d)+\boldsymbol{v}_e\times(\boldsymbol{\omega}_b+\boldsymbol{\omega}_d)]-(1-s_e)\dot{s}_e\\ =&\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}[s_e(\boldsymbol{\omega}_b-\boldsymbol{\omega}_d)]+\frac{1}{2}(1-s_e)\boldsymbol{v}_e^{\mathrm{T}}(\boldsymbol{\omega}_b-\boldsymbol{\omega}_d) \end{aligned}\tag{2.18}$$

当$\boldsymbol{\omega}_b=-2K_1\boldsymbol{v}_e+\boldsymbol{\omega}_d$时，有$\dot{V}_1=-\boldsymbol{v}_e^{\mathrm{T}}K_1\boldsymbol{v}_e\le0$。因此，定义虚拟控制量为

$$\boldsymbol{v}_z=\boldsymbol{\omega}_b+2K_1\boldsymbol{v}_e-\boldsymbol{\omega}_d \tag{2.19}$$

则：

$$\dot{V}_1=-\boldsymbol{v}_e^{\mathrm{T}}K_1\boldsymbol{v}_e+\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}\boldsymbol{v}_z \tag{2.20}$$

对公式(2.19)求导，可以得到$\boldsymbol{v}_z$的导数：

$$\begin{aligned} \dot{\boldsymbol{v}}_z=&\dot{\boldsymbol{\omega}}_b+2K_1\dot{\boldsymbol{v}}_e-\dot{\boldsymbol{\omega}}_d\\ =&J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau})+2K_1\dot{\boldsymbol{v}}_e-\dot{\boldsymbol{\omega}}_d \end{aligned}\tag{2.21}$$

其中：$\dot{\boldsymbol{v}}_e$在公式(2.15)给出。

 重新选择李雅普诺夫函数：

$$V_2=V_1+\frac{1}{2}\boldsymbol{v}_z^{\mathrm{T}}\boldsymbol{v}_z\tag{2.22}$$

则：

$$\dot{V}_2=-\boldsymbol{v}_e^{\mathrm{T}}K_1\boldsymbol{v}_e+\frac{1}{2}\boldsymbol{v}_e^{\mathrm{T}}\boldsymbol{v}_z+\boldsymbol{v}_z^{\mathrm{T}}[J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau})+2K_1\dot{\boldsymbol{v}}_e-\dot{\boldsymbol{\omega}}_d]\tag{2.23}$$

设计控制为：

$$\boldsymbol{\tau}=\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+J[-K_2\boldsymbol{v}_z-2K_1\dot{\boldsymbol{v}}_e+\dot{\boldsymbol{\omega}}_d-\frac{1}{2}\boldsymbol{v}_e]\tag{2.24}$$

其中$K_2$为正定对角矩阵。将公式(2.24)代入(2.23)中可以得到：

$$\dot{V}_2=-\boldsymbol{v}_e^{\mathrm{T}}K_1\boldsymbol{v}_e-\boldsymbol{v}_z^{\mathrm{T}}K_2\boldsymbol{v}_z\le0 \tag{2.25}$$

根据拉塞尔不变性原理，当$t\rightarrow \infty$时，$\boldsymbol{v}_e\rightarrow 0$和$\boldsymbol{v}_z \rightarrow 0$。对于前面提到的四元数数双倍覆盖导致的收敛问题，可以使用$\mathrm{sign}(s_e)\boldsymbol{q}_e$来代替$\boldsymbol{q}_e$解决。

2.3 基于$SO(3)$的控制

 为了便于后续的控制器的推导，先给出两个结论。对于$\boldsymbol{\phi}_e=(R_e-R_e^{\mathrm{T}})^{\vee}$和任意的向量$\boldsymbol{z}$，满足以下关系：

1. $\boldsymbol{\phi}_e^{\mathrm{T}}\boldsymbol{\phi}_e=\mathrm{tr}(I-R_eR_e)$
2. $\boldsymbol{z}^{\mathrm{T}}\boldsymbol{\phi}_e=-\mathrm{tr}[R_e\mathrm{sk}(\boldsymbol{z})]$

这两个结论都可以利用这一性质导出：对于任意的$\boldsymbol{x},\boldsymbol{y}\in\mathbb{R}^3$满足$\boldsymbol{x}^{\mathrm{T}}\boldsymbol{y}=-\frac{1}{2}\mathrm{tr}[\mathrm{sk}(\boldsymbol{x})\mathrm{sk}(\boldsymbol{y})]$。

 记当前姿态和期望姿态的旋转矩阵分别为$R$和$R_d$，旋转误差为$R_e=R_d^{\mathrm{T}}R$。$SO(3)$空间中的旋转误差的动力学可以表示为：

$$\begin{align} & \dot{R}_e=R_e\mathrm{sk}(\boldsymbol{\omega}_b)-\mathrm{sk}(\boldsymbol{\omega}_d)R_e \tag{2.26}\\ &\dot{\boldsymbol{\omega}}_b=J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau}) \tag{2.27} \end{align}$$

 选取李雅普诺夫函数：

$$V_1=\mathrm{tr}(I-R_e) \tag{2.28}$$

则：

$$\dot{V}_1=-\mathrm{tr}[R_e\mathrm{sk}(\boldsymbol{\omega}_b)-\mathrm{sk}(\boldsymbol{\omega}_d)R_e] \tag{2.29}$$

当$\mathrm{sk}(\boldsymbol{\omega}_b)=-K_1(R_e-R_e^{\mathrm{T}})+R_e^{\mathrm{T}}\mathrm{sk}(\boldsymbol{\omega}_d)R_e$时，有:$\dot{V}_1=-\mathrm{tr}[K_1(I-R_eR_e)] \le 0$。因此设计虚拟控制量$\mathrm{sk}(\boldsymbol{z})=\mathrm{sk}(\boldsymbol{\omega}_b)+K_1(R_e-R_e^{\mathrm{T}})-R_e^{\mathrm{T}}\mathrm{sk}(\boldsymbol{\omega}_d)R_e$，即：

$$\boldsymbol{z}=\boldsymbol{\omega}_b+K_1\boldsymbol{\phi}_e-R_e^{\mathrm{T}}\boldsymbol{\omega}_d \tag{2.30}$$

其中$K_1$为正定对角矩阵，则$\dot{V}_1$可以写作：

$$\begin{aligned} \dot{V}_1=&-\mathrm{tr}[R_e\mathrm{sk}(\boldsymbol{\omega}_b)-\mathrm{sk}(\boldsymbol{\omega}_d)R_e]+\mathrm{tr}[R_e\mathrm{sk}(\boldsymbol{z})]-\mathrm{tr}[R_e\mathrm{sk}(\boldsymbol{z})]\\ =&-\mathrm{tr}[K_1(I-R_eR_e)]-\mathrm{tr}[R_e\mathrm{sk}(\boldsymbol{z})]\\ \le&-k_{m1}\boldsymbol{\phi}_e^{\mathrm{T}}\boldsymbol{\phi}_e+\boldsymbol{z}^{\mathrm{T}}\boldsymbol{\phi}_e \end{aligned}\tag{2.31}$$

其中$k_{m1}$为$K$矩阵中的最小元素。对公式(2.30)求导可以得到$\boldsymbol{z}$的导数：

$$\begin{aligned} \dot{\boldsymbol{z}}=&\dot{\boldsymbol{\omega}}_b+K_1\dot{\boldsymbol{\phi}_e}-\boldsymbol{\Omega}_e\\ =&J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau}+\boldsymbol{d})+K_1\dot{\boldsymbol{\phi}_e}-\boldsymbol{\Omega}_e \end{aligned}\tag{2.32}$$

其中：$\dot{\boldsymbol{\phi}_e}=(\dot{R}_e-\dot{R}_e^{\mathrm{T}})^{\vee}$，$\boldsymbol{\Omega}_e=\dot{R}_e^{\mathrm{T}}\boldsymbol{\omega}_d+R_e^{\mathrm{T}}\dot{\boldsymbol{\omega}}_d$。

 重新选择李雅普诺夫函数为：

$$V_2=V_1+\frac{1}{2}\boldsymbol{z}^{\mathrm{T}}\boldsymbol{z}\tag{2.33}$$

则：

$$\begin{aligned} \dot{V}_2\le&-k_{m1}\boldsymbol{\phi}_e^{\mathrm{T}}\boldsymbol{\phi}_e+\boldsymbol{z}^{\mathrm{T}}\boldsymbol{\phi}_e+\boldsymbol{z}^{\mathrm{T}}\dot{\boldsymbol{z}}\\ =&-k_{m1}\boldsymbol{\phi}_e^{\mathrm{T}}\boldsymbol{\phi}_e+\boldsymbol{z}^{\mathrm{T}}\boldsymbol{\phi}_e\\ &+\boldsymbol{z}^{\mathrm{T}}[J^{-1}(-\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+\boldsymbol{\tau})+K_1\dot{\boldsymbol{\phi}_e}-\boldsymbol{\Omega}_e] \end{aligned}\tag{2.34}$$

设计控制器为：

$$\boldsymbol{\tau}=\boldsymbol{\omega}_b\times J\boldsymbol{\omega}_b+J(-K_2\boldsymbol{z}-\boldsymbol{\phi}_e-K_1\dot{\boldsymbol{\phi}}_e+\boldsymbol{\Omega}_e)\tag{2.35}$$

其中，$K_2$为正定对角矩阵，记$K_2$中的最小元素为$k_{m2}$，则：

$$\dot{V}_2\le-k_{m1}\boldsymbol{\phi}_e^{\mathrm{T}}\boldsymbol{\phi}_e-k_{m2}\boldsymbol{z}^{\mathrm{T}}\boldsymbol{z}\le 0\tag{2.36}$$

对于$\dot{V}_2$，当且仅当$\boldsymbol{\phi}_e=0$和$\boldsymbol{z}=0$时$\dot{V}_2=0$，根据拉塞尔不变性原理，当$t\rightarrow \infty$时，$\boldsymbol{\phi}_e\rightarrow 0$和$\boldsymbol{z}\rightarrow 0$，对应的$R_e\rightarrow R_d$。