李群、李代数和刚体转动
一、近世代数基础
1.1 代数结构
1.1.1 代数结构的定义
对于带有二元运算\(*\)的非空集合\(S\)，如果二元运算满足下列性质，则称\(S\)为代数结构.
- 封闭性：对\(\forall a,b\in S\)，有\(a*b\in S\)
- 运算唯一性：\(a*b\)结果唯一，即\(*\)是有两个变量的函数或映射，即\(*:S\times S\rightarrow S\).
注意二元运算符号的选取是任意的，但习惯写成类似于加法或者乘法的形式，如：
\[p=ab,a\times b,a\circ b, a+b,a*b \]如果要强调二元运算，也可以说\((S,*)\)是代数结构.
1.1.2 代数结构的同态映射
设\(f:(A,*)\rightarrow (B,\circ)\)为一个代数结构到另一个代数结构的映射，若对于\(\forall x,y\in A\)，有\(f(x*y)=f(x)\circ f(y)\)，则称\(f\)是两代数结构的同态映射.同态映射可以是单射，也可以是满射. 双射的同态称为同构.
1.2 群
1.2.1 群的定义
若代数结构\(G\)的二元运算满足以下性质，则称\(G\)为群.
- 结合律：对\(\forall a,b,c\in G\)，有\(a*(b*c)=(a*b)*c\).
- 有单位元：\(\exists e\in G\)，使得对于\(\forall a\in G\)，有\(a*e=e*a=a\).
- 有逆元：对\(\forall a\in G, \exists a^{-1}\in G\)，使得\(a*a^{-1}=a^{-1}*a=e\).
类似地，要强调二元运算，也可以说满足上述条件的\((G,*)\)是群. 如果二元运算写成乘法形式，则单位元一般写为\(1\). 若二元运算写成加法形式，则单位元一般写为\(0\).
1.2.2 矩阵群
矩阵群由数域\(\mathbb{K}\)（不特别说明本文都假定为实数域）上的可逆方阵组成，二元运算即矩阵乘法。矩阵群举例如下：
- 一般线性群：\(GL(n)=\{A\in\mathbb{R}^{n\times n}\ |\ \text{det}(A)\neq0\}\).
- 特殊线性群：\(SL(n)=\{A\in\mathbb{R}^{n\times n}\ |\ \text{det}(A)=1\}\).
- 正交群：\(O(n)=\{A\in GL(n) | A^TA=I\}\).
- 特殊正交群：\(SO(n)=\{A\in SL(n) | A^TA=I\}\)
- \(4\times4\)特殊欧式群：\(SE(3)=\left\{\begin{bmatrix} \boldsymbol{R} &\boldsymbol{d}\\ \boldsymbol{0}^T &1 \end{bmatrix}\ |\ \boldsymbol{R}\in SO(3), \boldsymbol{d}\in\mathbb{R}^3\right\}\)
- \(6\times6\)特殊欧式群：\(SE(3)=\left\{\begin{bmatrix} \boldsymbol{R} &\boldsymbol{0}\\ \boldsymbol{AR} &\boldsymbol{R} \end{bmatrix}\ |\ \boldsymbol{R}\in SO(3), \boldsymbol{A}\in\mathbb{R}^{3\times3}\right\}\)
1.2.3 拓扑群和李群
- 拓扑群：满足以下条件的群\((G,*)\)称为拓扑群：
- 是拓扑空间，定义如下.
对于集合\(X\)，若存在\(X\)子集的集合\(\tau\)（\(\tau\)的元素称为开集），满足：
(1) 开集公理：空集和\(X\)都是\(\tau\)的元素，即\(\emptyset,X\in \tau\).
(2) 并集公理：\(\tau\)中任意(有限或无限个)元素的并集属于\(\tau\).
(3) 交集公理：\(\tau\)中任意有限个元素的并集属于\(\tau\).则称\(\tau\)为\(X\)的拓扑，\((X,\tau)\)为拓扑空间.
- 群操作连续，具体包括：
(1) 二元运算\(*\)连续.
(2) 逆映射\(a\rightarrow a^{-1}\)连续.
- 李群：满足以下条件的拓扑群\((G,*)\)称为李群：
- 是光滑流形，即对于\(\forall x\in G\)，存在\(x\)的一个邻域\(\mathcal{N}(x)\)能微分同胚地映射到欧氏空间\(\mathbb{R}^n\)的某个开子集\(T\)上，其中\(n\)为李群的维度.
注：映射\(f: \mathcal{N}(x)\rightarrow T\)如果是光滑双射，且其逆映射\(f^{-1}:T\rightarrow \mathcal{N}(x)\)也是光滑的，则称\(f\)为微分同胚映射。
- 群操作光滑，即无穷阶连续可导.
由于李群的这些特点，其常被用来研究几何结构，是微分拓扑学和微分几何学的基础. 需要注意的是，群和李群的定义中都没有要求集合\(G\)的元素是矩阵，因此李群并不一定是矩阵群. 同理，矩阵群也不都是李群. 不特别说明，本文讨论的李群都是矩阵群，又叫矩阵李群，例如前面定义的特殊正交群\(SO(3)\)和两种特殊欧式群\(SE(3)\)都是矩阵李群.
1.3 李代数
李代数是研究李群、微分流形和微小变换等几何体的代数工具. 在20世纪40年代德国数学家与理论物理学家Hermann Weyl引进术语“李代数”之前，李代数一直被称为“无穷小群”，即群元素非常接近恒等，为李群流形在单位元处的切空间. 下面给出李代数的定义.
1.3.1 李代数的定义
对于代数结构\(\mathfrak{g}\)，其定义在数域\(\mathcal{F}\)上的二元运算\([\cdot,\cdot]:\mathfrak{g}\times\mathfrak{g}\rightarrow \mathfrak{g}\)若满足以下性质，则称\(\mathfrak{g}\)为李代数.
- 双线性：对\(\forall a,b\in \mathcal{F}\)，\(\forall x,y,z\in \mathfrak{g}\),有\([ax+by,z]=a[x,z]+b[y,z]\), \([x,ay+bz]=a[x,y]+b[x,z]\).
- 反对称性：对\(\forall x,y\in \mathfrak{g}\)，有\([x,y]=-[y,x]\). 由此可得\([x,x]=0\).
- Jacobi恒等式：对\(\forall x,y,z\in \mathfrak{g}\)，等式\([x,[y,z]]+[z,[x,y]]+[y,[z,x]]=0\)恒成立.
李代数的二元运算又叫李括号. 需要注意的是李括号一般不满足结合律，因此李代数本身并不是群.
例1 具有向量叉积赋予的李括号运算的三维欧氏空间\(\mathbb{E}^3\)是三维李代数.
例2 刚体旋转运动的轴线方向向量\(\boldsymbol{s}\)为李代数\(\mathfrak{so}(3)\)的元素.
例3 刚体绕方向向量为\(\boldsymbol{s}\)的轴线（轴线过原点，或者说轴线位置向量为\(\boldsymbol{0}\)）旋转的瞬时角速度\(\boldsymbol{\omega}\)为李代数\(\mathfrak{so}(3)\)的元素.
1.3.2 李代数的表示和伴随表示
由同态映射的定义可知，李代数\(\mathfrak{g}\)经过同态映射\(f:\mathfrak{g}\rightarrow\mathfrak{h}\)后到达的新的代数结构\(\mathfrak{h}\)仍然保持原代数结构二元运算的所有性质，因此\(\mathfrak{h}\)也是一个李代数. 如果\(\mathfrak{g}\)中的元素是抽象的，而\(\mathfrak{h}\)中的元素是向量或矩阵等具体的元素，我们就可以通过同态映射\(f\)实现对抽象李代数\(\mathfrak{h}\)的具体表示.
李群\(SO(3)\)相关的李代数\(\mathfrak{so}(3)\)一般可以表示为三维实向量空间\(\mathbb{R}^3\)，也可以表示为三维矩阵\(\mathbb{R}^{3\times3}\). 李代数\(\mathfrak{so}(3)\)的伴随算子\(\text{ad}(\boldsymbol{\cdot})\)将李代数向量空间元素\(\boldsymbol{s}\)映射为矩阵表示元素\(\hat{\boldsymbol{s}}\). 对于\(\boldsymbol{s}=[s_x,s_y,s_z]^T\)，有：
\[\text{ad}(\boldsymbol{\boldsymbol{s}})=\hat{\boldsymbol{s}}= \begin{bmatrix} 0 & -s_z & s_y\\ s_z & 0 & -s_x\\ -s_y & s_x & 0 \end{bmatrix} \]李代数\(\mathfrak{so}(3)\)的向量表示的李括号为叉乘，矩阵表示的李括号为：
\[[\hat{\boldsymbol{s}}_1,\hat{\boldsymbol{s}}_2]=\hat{\boldsymbol{s}}_1 \hat{\boldsymbol{s}}_2-\hat{\boldsymbol{s}}_2\hat{\boldsymbol{s}}_1=(\boldsymbol{s}_1\times\boldsymbol{s}_2)^\wedge \]李代数\(\mathfrak{so}(3)\)的伴随运算有以下性质：
- \(\hat{\boldsymbol{s}}\hat{\boldsymbol{s}}\hat{\boldsymbol{s}}=-\hat{\boldsymbol{s}}\)
- \(\hat{\boldsymbol{a}}\boldsymbol{b}=\boldsymbol{a}\times\boldsymbol{b}\)
- \(\hat{\boldsymbol{s}}\hat{\boldsymbol{s}}=\boldsymbol{s}\boldsymbol{s}^T-\boldsymbol{I}\)
对于\(\boldsymbol{s}\in \mathfrak{so}(3)，\boldsymbol{A}\in SO(3)\)，有\((\boldsymbol{As})^\wedge=\boldsymbol{A}\hat{\boldsymbol{s}}\boldsymbol{A}^{-1}=\boldsymbol{A}\hat{\boldsymbol{s}}\boldsymbol{A}^T\).
二、刚体转动基础
2.1 坐标系和坐标轴定义
本文以四旋翼无人机为例讨论三轴六自由度刚体的转动. 坐标轴满足右手定则，如下图[1]所示，这里不再赘述.
设地球固连坐标系\(\text{e}\)在\(x,y,z\)轴的单位向量为别为\(\boldsymbol{e}_1,\boldsymbol{e}_2,\boldsymbol{e}_3\)，机体坐标系\(\text{b}\)在\(x,y,z\)轴的单位向量为\(\boldsymbol{b}_1,\boldsymbol{b}_2,\boldsymbol{b}_3\).设任意向量\(\boldsymbol{a}\in\mathbb{R}^3\)在坐标系m的坐标为\({}^\text{m}\boldsymbol{a}\)，则显然有：
- \({}^\text{e}\boldsymbol{e}_1={}^\text{b}\boldsymbol{b}_1=[1,0,0]^T\)
- \({}^\text{e}\boldsymbol{e}_2={}^\text{b}\boldsymbol{b}_2=[0,1,0]^T\)
- \({}^\text{e}\boldsymbol{e}_3={}^\text{b}\boldsymbol{b}_3=[0,0,1]^T\)
不指明参考坐标系时，本文默认参考系为地球固连坐标系.
2.2 旋转矩阵
定义旋转矩阵\(\boldsymbol{R}=[{}^\text{e}\boldsymbol{b}_1,{}^\text{e}\boldsymbol{b}_2,{}^\text{e}\boldsymbol{b}_3]^T\)，则有：
\[{}^\text{e}\boldsymbol{b}_i=\boldsymbol{R}{}^\text{e}\boldsymbol{e}_i \]\[{}^\text{e}\boldsymbol{a}=\boldsymbol{R}{}^\text{b}\boldsymbol{a} \]其中\(i=1,2,3\). 定义式说明旋转矩阵表示了机体的姿态. 上面第一个式子说明\(\boldsymbol{R}\)可以看作对向量的旋转操作，会改变向量的实际方向，而第二个式子说明\(\boldsymbol{R}\)可以看作坐标变换矩阵，将向量在机体坐标系下的坐标变换为在固连坐标系下的坐标，但向量的实际朝向不发生变化.
由于\(\{\boldsymbol{b}_1,\boldsymbol{b}_2,\boldsymbol{b}_3\}\)为正交向量组，所以\(R\)是一个正交矩阵（正交矩阵的行列式为1或-1），又因为\(\boldsymbol{R}\)表示旋转操作，所以\(|\boldsymbol{R}|=1\).因此\(\boldsymbol{R}\)是矩阵李群\(SO(3)\)的元素，即\(\boldsymbol{R}\in SO(3)\).
【注】一个对于绕单位瞬轴\(\boldsymbol{s}\)的旋转\(\theta\)的操作，必须要指定参考坐标系\(\text{m}\)才能写出对应的旋转矩阵\({}^\text{m}\boldsymbol{R}_\boldsymbol{s}(\theta)\). 指定的参考系不同则旋转矩阵的表示不同，即对于不同的参考系\(\text{m,n}\)，有\({}^\text{m}\boldsymbol{R}_\boldsymbol{s}(\theta)\neq{}^\text{n}\boldsymbol{R}_\boldsymbol{s}(\theta)\). 本文如果不特别说明参考系，则默认为固联坐标系\(\text{e}\)，即\(\boldsymbol{R}_\boldsymbol{s}(\theta)={}^\text{e}\boldsymbol{R}_\boldsymbol{s}(\theta)\).
2.3 欧拉角定义方式
欧拉角按照旋转轴分为经典欧拉角(Proper Euler angles)和泰特布莱恩角(Tait-Bryan angles)两类，共12种旋转方式[2]：
- 经典欧拉角：\(ZXZ, XYX, YZY, ZYZ, XZX, YXY\)
- 泰特布莱恩角：\(XYZ, YZX, ZXY, XZY, ZYX, YXZ\)
另外，旋转方式按照旋转的坐标系可以分为内旋和外旋两种，内旋以机体坐标系轴线为转轴，而外旋以固连坐标系轴线为转轴. 需要说明的是，内旋和外旋如果采取完全相反的顺序，则二者等价，例如\(ZYX\)内旋等价于\(XYZ\)外旋. 除特别说明，本文中的旋转方式默认为内旋\(ZYX\). 下面是旋转过程，其中旋转矩阵\(\boldsymbol{R}_{\boldsymbol{s}}(\alpha)\)对应绕转轴\(\boldsymbol{s}\)遵循右手定则旋转角度\(\alpha\)的操作.
【注】由于转动与无人机当前在固连坐标系的位置坐标\(\boldsymbol{p}\)没有直接关系，转轴的位置向量都考虑为\(\boldsymbol{0}\)，即考虑转动时默认所有坐标系原点重合.
- 内旋\(ZYX\)
- 外旋\(XYZ\)
2.4 旋转矩阵的计算
旋转矩阵\(\boldsymbol{R}\)可由下式计算得来：
\[\boldsymbol{R}= \boldsymbol{R}_{\boldsymbol{n}_1}(\phi) \boldsymbol{R}_{\boldsymbol{k}_2}(\theta) \boldsymbol{R}_{\boldsymbol{e}_3}(\psi) = \boldsymbol{R}_{\boldsymbol{e}_3}(\psi) \boldsymbol{R}_{\boldsymbol{e}_2}(\theta) \boldsymbol{R}_{\boldsymbol{e}_1}(\phi) \]由于\(\boldsymbol{n}_1\)和\(\boldsymbol{k}_2\)是经过若干次坐标系绕轴旋转才得到的单位向量，而\(\boldsymbol{e}_1,\boldsymbol{e}_2,\boldsymbol{e}_3\)则一直保持不变，因此用外旋\(XYZ\)的三个绕轴旋转向量计算\(\boldsymbol{R}\)较为方便.
易得：
\[\boldsymbol{R}_{\boldsymbol{e}_3}(\psi)= \begin{bmatrix} c_\psi & -s_\psi & 0\\ s_\psi & c_\psi & 0\\ 0 & 0 & 1\\ \end{bmatrix} \]\[\boldsymbol{R}_{\boldsymbol{e}_2}(\theta)= \begin{bmatrix} c_\theta & 0 &s_\theta\\ 0 & 1 & 0\\ -s_\theta & 0 & c_\theta \\ \end{bmatrix} \]\[\boldsymbol{R}_{\boldsymbol{e}_1}(\phi)= \begin{bmatrix} 1 & 0 & 0\\ 0 & c_\phi & -s_\phi\\ 0 & s_\phi & c_\phi \end{bmatrix} \]【注】\(s_\alpha=\sin\alpha\)，\(c_\alpha=\cos\alpha\).
带入
\[\boldsymbol{R}= \boldsymbol{R}_{\boldsymbol{e}_3}(\psi) \boldsymbol{R}_{\boldsymbol{e}_2}(\theta) \boldsymbol{R}_{\boldsymbol{e}_1}(\phi) \]可得
\[\boldsymbol{R}= \begin{bmatrix} c_\theta c_\psi & c_\psi s_\theta s_\phi-s_\psi c_\phi & c_\psi s_\theta c_\phi+s_\psi s_\phi\\ s_\psi c_\theta & s_\psi s_\theta s_\phi+c_\psi c_\phi & s_\psi s_\theta c_\phi-c_\psi s_\phi\\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix} \]2.5 角速度和欧拉角变化率的关系
\(ZYX\)内旋定义的欧拉角存在一定的旋转次序，因此欧拉角的变化率\(\dot{\boldsymbol{\Theta}}=[\dot{\phi},\dot{\theta},\dot{\psi}]^T\)本身并不等于绕三个旋转轴\(\{\boldsymbol{n}_1,\boldsymbol{k}_2,\boldsymbol{e}_3\}\)旋转的角速度\(\boldsymbol{\omega}=[\omega_x,\omega_y,\omega_z]^T\in \mathfrak{so}(3)\). 实际上，二者的关系可以用下面的公式表示[3]：
\[\boldsymbol{\omega}=[{}^\text{b}\boldsymbol{n}_1,{}^\text{b}\boldsymbol{k}_2,{}^\text{b}\boldsymbol{e}_3]\dot{\boldsymbol{\Theta}}=[{}^\text{b}\boldsymbol{b}_1,{}^\text{b}\boldsymbol{n}_2,{}^\text{b}\boldsymbol{k}_3]\dot{\boldsymbol{\Theta}}=\boldsymbol{W}\dot{\boldsymbol{\Theta}} \]容易算得：
\[\left\{ \begin{array}{l} {}^\text{b}\boldsymbol{b}_1=[1,0,0]^T\\ {}^\text{b}\boldsymbol{n}_2={}^\text{n}\boldsymbol{R}_{\boldsymbol{n}_1}^T(\phi)\cdot{}^\text{n}\boldsymbol{n}_2=\boldsymbol{R}_{\boldsymbol{e}_1}^T(\phi)\cdot{}^\text{n}\boldsymbol{n}_2=[0,c_\phi,-s_\phi]^T\\ {}^\text{b}\boldsymbol{k}_3=({}^\text{k}\boldsymbol{R}_{\boldsymbol{k}_2}(\theta){}^\text{n}\boldsymbol{R}_{\boldsymbol{n}_1}(\phi))^T\cdot{}^\text{k}\boldsymbol{k}_3=(\boldsymbol{R}_{\boldsymbol{e}_2}(\theta)\boldsymbol{R}_{\boldsymbol{e}_3}(\phi))^T\cdot{}^\text{k}\boldsymbol{k}_3=[-s_\theta,c_\theta s_\phi,c_\theta c_\phi]^T\\ \end{array} \right. \]即：
\[\boldsymbol{\omega}=\boldsymbol{W}\dot{\boldsymbol{\Theta}}= \begin{bmatrix} 1 & 0 & -s_\theta\\ 0 & c_\phi & c_\theta s_\phi\\ 0 & -s_\phi & c_\theta c_\phi \end{bmatrix} \begin{bmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix} \]【注】这里定义的\(\boldsymbol{\omega}\)实际上是以机体坐标系\(\text{b}\)为参考坐标系的. 为了简化书写，本文都省去左上标，在此特别说明.
三、指数映射及其物理意义
3.1 从旋转运动到欧拉-罗德里格斯方程[4]
【注】图中的坐标系\(Oxyz\)并不一定是固连坐标系.
如图所示，向量\(\boldsymbol{v}_1\)绕单位转轴\(\boldsymbol{s}=[s_x,s_y,s_z]^T\in\mathfrak{so}(3)\)旋转\(\theta\)至\(\boldsymbol{v}_2\), \(O\)为刚体中心，\(Oz\)轴与\(\boldsymbol{s}\)方向相同，\(\boldsymbol{v}_1\)在\(Oxz\)平面上. \(\boldsymbol{v}_{1p},\boldsymbol{v}_{2p}\)分别为\(\boldsymbol{v}_1,\boldsymbol{v}_2\)在\(Oxy\)平面上的投影向量，\(\boldsymbol{v}_z\)为\(\boldsymbol{v}_1\)（或\(\boldsymbol{v}_2\)）在\(Oz\)上的投影向量. 易得
\[\boldsymbol{v}_{1p}=\boldsymbol{v}_1-\boldsymbol{v}_z=\boldsymbol{v}_1-(\boldsymbol{s}\cdot\boldsymbol{v}_1)\boldsymbol{s} \]定义
\[\boldsymbol{u}=\boldsymbol{s}\times\boldsymbol{v}_1=\boldsymbol{s}\times\boldsymbol{v}_{1p} \]则
\[\boldsymbol{v}_2=\boldsymbol{v}_{1p}\cos\theta+\boldsymbol{u}\sin\theta+\boldsymbol{v}_z \]代入\(\boldsymbol{v}_{1p},\boldsymbol{u}\)和\(\boldsymbol{v}_z\)，有
\[\begin{aligned} \boldsymbol{v}_2&=(\boldsymbol{v}_1-(\boldsymbol{s}\cdot\boldsymbol{v}_1)\boldsymbol{s})\cos\theta+(\boldsymbol{s}\times\boldsymbol{v}_1)\sin\theta+(\boldsymbol{s}\cdot\boldsymbol{v}_1)\boldsymbol{s}\\ &=\boldsymbol{v}_1\cos\theta+(\boldsymbol{s}\times\boldsymbol{v}_1)\sin\theta+(\boldsymbol{s}\cdot\boldsymbol{v}_1)\boldsymbol{s}(1-\cos\theta)\\ &=\boldsymbol{v}_1\cos\theta+\hat{\boldsymbol{s}}\boldsymbol{v}_1\sin\theta+\boldsymbol{s}\boldsymbol{s}^T\boldsymbol{v}_1(1-\cos\theta)\\ &=\boldsymbol{v}_1\cos\theta+\hat{\boldsymbol{s}}\boldsymbol{v}_1\sin\theta+(\hat{\boldsymbol{s}}\hat{\boldsymbol{s}}+\boldsymbol{I})\boldsymbol{v}_1(1-\cos\theta)\\ &=[\boldsymbol{I}+\sin\theta\hat{\boldsymbol{s}}+(1-\cos\theta)\hat{\boldsymbol{s}}\hat{\boldsymbol{s}}]\boldsymbol{v}_1 \end{aligned}\]又\(\boldsymbol{v}_2=\boldsymbol{R}_{\boldsymbol{s}}(\theta)\boldsymbol{v}_2\)，所以求得
\[\boldsymbol{R}_{\boldsymbol{s}}(\theta)=\boldsymbol{I}+\sin\theta\hat{\boldsymbol{s}}+(1-\cos\theta)\hat{\boldsymbol{s}}\hat{\boldsymbol{s}} \]这就是旋转运动的欧拉-罗德里格斯方程.
3.2 \(\mathfrak{so}(3)\)到\(SO(3)\)的指数映射
指数映射是李代数\(\boldsymbol{V}\)到李群\(G\)的光滑映射，表示为\(\text{exp}: \boldsymbol{V}\rightarrow G\). 上节推导的欧拉-罗德里格斯方程给出了\(\mathfrak{so}(3)\)到\(SO(3)\)的指数映射，这是一个满射. 指数映射和欧拉-罗德里格斯方程的关系可以由下面的Taylor展开式体现.
\[\begin{aligned} \boldsymbol{R}_{\boldsymbol{s}}(\theta)&= e^{\theta \hat{\boldsymbol{s}}}=\sum_{k=0}^\infty\frac{(\theta \hat{\boldsymbol{s}})^k}{k!}=\boldsymbol{I}+\theta\hat{\boldsymbol{s}}+\frac{1}{2}(\theta\hat{\boldsymbol{s}})^2+\frac{1}{6}(\theta\hat{\boldsymbol{s}})^3+\cdots\\ &=\boldsymbol{I}+\hat{\boldsymbol{s}}(\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-\cdots)+\hat{\boldsymbol{s}}^2(\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}-\cdots)\\ &=\boldsymbol{I}+\sin\theta\hat{\boldsymbol{s}}+(1-\cos\theta)\hat{\boldsymbol{s}}\hat{\boldsymbol{s}} \end{aligned} \]四、利用\(\mathfrak{so}(3)\)研究\(SO(3)\)的微分性质
4.1 绕瞬轴\(\boldsymbol{s}\)旋转时旋转矩阵对时间的导数
当刚体绕瞬轴\(\boldsymbol{s}\)转动时，旋转矩阵\(\boldsymbol{R}_{\boldsymbol{s}}(\theta)\)由罗德里格斯公式给出. 注意到\(\boldsymbol{s}\)为单位向量，因此角速度计算公式为：
\[\boldsymbol{\omega}=\dot{\theta}\boldsymbol{s} \]因为刚体绕轴旋转时转轴不会发生变化，即\(\dot{\boldsymbol{s}}=0\)，所以旋转矩阵对时间的导数为
\[\dot{\boldsymbol{R}}_{\boldsymbol{s}}(\theta)=\dot{\theta}\cos\theta\hat{\boldsymbol{s}}+\dot{\theta}\sin\theta\hat{\boldsymbol{s}}\hat{\boldsymbol{s}}=\boldsymbol{R}_{\boldsymbol{s}}(\theta)\hat{\boldsymbol{\omega}}=\hat{\boldsymbol{\omega}}\boldsymbol{R}_{\boldsymbol{s}}(\theta) \]4.2 内旋\(ZYX\)旋转矩阵对时间的导数
2.4节已经给出了旋转矩阵的表达式为\(\boldsymbol{R}= \boldsymbol{R}_{\boldsymbol{e}_3}(\psi) \boldsymbol{R}_{\boldsymbol{e}_2}(\theta) \boldsymbol{R}_{\boldsymbol{e}_1}(\phi) \). 由矩阵微积分可知，该矩阵对时间的导数为
\[\dot{\boldsymbol{R}}= \dot{\boldsymbol{R}}_{\boldsymbol{e}_3}(\psi) \boldsymbol{R}_{\boldsymbol{e}_2}(\theta) \boldsymbol{R}_{\boldsymbol{e}_1}(\phi) + \boldsymbol{R}_{\boldsymbol{e}_3}(\psi) \dot{\boldsymbol{R}}_{\boldsymbol{e}_2}(\theta) \boldsymbol{R}_{\boldsymbol{e}_1}(\phi) + \boldsymbol{R}_{\boldsymbol{e}_3}(\psi) \boldsymbol{R}_{\boldsymbol{e}_2}(\theta) \dot{\boldsymbol{R}}_{\boldsymbol{e}_1}(\phi) \]带入
\[\left\{ \begin{array}{l} \dot{\boldsymbol{R}}_{\boldsymbol{e}_3}(\psi)=\dot{\psi}\boldsymbol{R}_{\boldsymbol{e}_3}(\psi)\hat{\boldsymbol{e}}_3\\ \dot{\boldsymbol{R}}_{\boldsymbol{e}_2}(\theta)=\dot{\theta}\boldsymbol{R}_{\boldsymbol{e}_2}(\theta)\hat{\boldsymbol{e}}_2\\ \dot{\boldsymbol{R}}_{\boldsymbol{e}_1}(\phi)=\dot{\theta}\boldsymbol{R}_{\boldsymbol{e}_1}(\phi)\hat{\boldsymbol{e}}_1 \end{array} \right. \]即可算得\(\dot{\boldsymbol{R}}\). 计算过程和结果比较繁琐，这里以代码的形式展示. 其中\(A,B,C\)分别表示\(\dot{\phi},\dot{\theta},\dot{\psi}\).
计算代码(MATLAB)
syms A B C psi theta phi
R_S3 = [cos(psi) -sin(psi) 0;
sin(psi) cos(psi) 0;
0 0 1]
R_S2 = [cos(theta) 0 sin(theta);
0 1 0;
-sin(theta) 0 cos(theta)]
R_S1 = [1 0 0;
0 cos(phi) -sin(phi);
0 sin(phi) cos(phi)]
R=R_S3*R_S2*R_S1
d3 = [0 -C 0;
C 0 0;
0 0 0]
d2 = [0 0 B;
0 0 0;
-B 0 0]
d1 = [0 0 0;
0 0 -A;
0 A 0]
R_d = R_S3*d3*R_S2*R_S1+R_S3*R_S2*d2*R_S1+R_S3*R_S2*R_S1*d1
计算结果(LaTeX)
$\left(\begin{array}{ccc}
-B\,\cos \left(\psi \right)\,\sin \left(\theta \right)-C\,\cos \left(\theta \right)\,\sin \left(\psi \right) & A\,\sin \left(\phi \right)\,\sin \left(\psi \right)-C\,\cos \left(\phi \right)\,\cos \left(\psi \right)+A\,\cos \left(\phi \right)\,\cos \left(\psi \right)\,\sin \left(\theta \right)+B\,\cos \left(\psi \right)\,\cos \left(\theta \right)\,\sin \left(\phi \right)-C\,\sin \left(\phi \right)\,\sin \left(\psi \right)\,\sin \left(\theta \right) & A\,\cos \left(\phi \right)\,\sin \left(\psi \right)+C\,\cos \left(\psi \right)\,\sin \left(\phi \right)+B\,\cos \left(\phi \right)\,\cos \left(\psi \right)\,\cos \left(\theta \right)-A\,\cos \left(\psi \right)\,\sin \left(\phi \right)\,\sin \left(\theta \right)-C\,\cos \left(\phi \right)\,\sin \left(\psi \right)\,\sin \left(\theta \right)\\
C\,\cos \left(\psi \right)\,\cos \left(\theta \right)-B\,\sin \left(\psi \right)\,\sin \left(\theta \right) & A\,\cos \left(\phi \right)\,\sin \left(\psi \right)\,\sin \left(\theta \right)-C\,\cos \left(\phi \right)\,\sin \left(\psi \right)-A\,\cos \left(\psi \right)\,\sin \left(\phi \right)+B\,\cos \left(\theta \right)\,\sin \left(\phi \right)\,\sin \left(\psi \right)+C\,\cos \left(\psi \right)\,\sin \left(\phi \right)\,\sin \left(\theta \right) & C\,\sin \left(\phi \right)\,\sin \left(\psi \right)-A\,\cos \left(\phi \right)\,\cos \left(\psi \right)+B\,\cos \left(\phi \right)\,\cos \left(\theta \right)\,\sin \left(\psi \right)+C\,\cos \left(\phi \right)\,\cos \left(\psi \right)\,\sin \left(\theta \right)-A\,\sin \left(\phi \right)\,\sin \left(\psi \right)\,\sin \left(\theta \right)\\
-B\,\cos \left(\theta \right) & A\,\cos \left(\phi \right)\,\cos \left(\theta \right)-B\,\sin \left(\phi \right)\,\sin \left(\theta \right) & -A\,\cos \left(\theta \right)\,\sin \left(\phi \right)-B\,\cos \left(\phi \right)\,\sin \left(\theta \right)
\end{array}\right)$
下面我们讨论\(\dot{\boldsymbol{R}}\)和\(\boldsymbol{\omega}\)的关系. 由于\(\boldsymbol{R}^T\boldsymbol{R}=\boldsymbol{R}\boldsymbol{R}^T=\boldsymbol{I}\)，对时间求导有\(\dot{\boldsymbol{R}}^T\boldsymbol{R}+\boldsymbol{R}^T\dot{\boldsymbol{R}}=\dot{\boldsymbol{R}}\boldsymbol{R}^T+\boldsymbol{R}\dot{\boldsymbol{R}}^T=\boldsymbol{O}\)，因此\(\boldsymbol{R}^T\dot{\boldsymbol{R}}\)和\(\dot{\boldsymbol{R}}\boldsymbol{R}^T\)都为反对称矩阵，因此存在\(\boldsymbol{\alpha,\beta}\in\mathbb{R}^3\)，使得\(\boldsymbol{R}^T\dot{\boldsymbol{R}}=\hat{\boldsymbol{\alpha}},\dot{\boldsymbol{R}}\boldsymbol{R}^T=\hat{\boldsymbol{\beta}}\).
经计算得：
\[\boldsymbol{\alpha}= \begin{bmatrix} \dot{\phi}-\dot{\psi}s_\theta\\ \dot{\theta}c_\phi+\dot{\psi}c_\theta s_\phi\\ -\dot{\theta}s_\phi+\dot{\psi}c_\theta c_\phi \end{bmatrix}= \begin{bmatrix} 1 & 0 & -s_\theta\\ 0 & c_\phi & c_\theta s_\phi\\ 0 & -s_\phi & c_\theta c_\phi \end{bmatrix} \begin{bmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix} \]\[\boldsymbol{\beta}= \begin{bmatrix} -\dot{\theta}s_\psi+\dot{\phi}c_\psi c_\theta\\ \dot{\theta}c_\psi+\dot{\phi}s_\psi c_\theta\\ \dot{\psi}-\dot{\phi}s_\theta \end{bmatrix}= \begin{bmatrix} c_\psi c_\phi & -s_\psi & 0\\ s_\psi c_\theta & c_\psi & 0\\ -s_\theta & 0 & 1 \end{bmatrix} \begin{bmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix} \]注意到\(\boldsymbol{\alpha}=\boldsymbol{\omega}, \boldsymbol{\beta}=\boldsymbol{R\omega}\)，所以\(\dot{\boldsymbol{R}}\)和\(\boldsymbol{\omega}\)的关系为
\[\dot{\boldsymbol{R}}=\boldsymbol{R}\hat{\boldsymbol{\omega}}=(\boldsymbol{R\omega})^\wedge\boldsymbol{R} \]4.3 姿态误差、姿态误差向量和角速度误差向量
控制刚体转动时，如果有预期姿态矩阵\(\boldsymbol{R}_d\)和预期角速度\(\boldsymbol{\omega}_d\)，常常需要计算当前姿态的姿态和角速度误差. 具体而言，需要设计下面三个函数[5]：
\[\begin{aligned} \Psi&: SO(3)\times SO(3)\rightarrow \mathbb{R}\\ \boldsymbol{e}_R&: SO(3)\times SO(3)\rightarrow \mathbb{R}^3\\ \boldsymbol{e}_\omega&: SO(3)\times\mathbb{R}^3\times SO(3)\times\mathbb{R}^3\rightarrow \mathbb{R} \end{aligned} \]分别叫做姿态误差、姿态误差以及角速度误差向量. 我们先设计\(\Psi\)，再根据\(\Psi\)的性质设计剩下两个函数. 一般来说姿态误差可以通过\(\text{tr}(\boldsymbol{R}_d^T\boldsymbol{R})=\boldsymbol{b}_{d1}^T\boldsymbol{b}_1+\boldsymbol{b}_{d2}^T\boldsymbol{b}_2+\boldsymbol{b}_{d3}^T\boldsymbol{b}_3\)设计，该值越大说明姿态越接近期望姿态，越小则说明姿态越偏离期望姿态. 特别地，当\(\boldsymbol{R}=\boldsymbol{R}_d\)时，\(\text{tr}(\boldsymbol{R}_d^T\boldsymbol{R})\)取最大值3，而当姿态与期望姿态相反（两个轴与对应期望轴向相反）时，\(\text{tr}(\boldsymbol{R}_d^T\boldsymbol{R})\)取最小值-1. 例如\(\Psi,\boldsymbol{e}_R,\boldsymbol{e}_\omega\)可以设计为
\[\begin{aligned} \Psi(\boldsymbol{R},\boldsymbol{R}_d)&=\frac{1}{2}\text{tr}(\boldsymbol{I}-\boldsymbol{R}_d^T\boldsymbol{R})\\ \boldsymbol{e}_R&=\frac{1}{2}(\boldsymbol{R}_d^T\boldsymbol{R}-\boldsymbol{R}^T\boldsymbol{R}_d)^\vee=[e_\phi,e_\theta,e_\psi]^T\\ \boldsymbol{e}_\omega&=\boldsymbol{\omega}-\boldsymbol{R}^T\boldsymbol{R}_d\boldsymbol{\omega}_d=[e_{\omega_x},e_{\omega_y},e_{\omega_z}]^T \end{aligned} \]其中\({}^\vee:SO(3)\rightarrow\mathfrak{so}(3)\)是\(^\wedge\)的反函数.
【引理1】\(\frac{d}{dt}(\boldsymbol{R}_d^T\boldsymbol{R})=\boldsymbol{R}_d^T\boldsymbol{R}\hat{\boldsymbol{e}}_\omega\)
\[\begin{aligned} \frac{d}{dt}(\boldsymbol{R}_d^T\boldsymbol{R})&=\dot{\boldsymbol{R}}_d^T\boldsymbol{R}+\boldsymbol{R}_d^T\dot{\boldsymbol{R}}\\ &=-\hat{\boldsymbol{\omega}}_d\boldsymbol{R}_d^T\boldsymbol{R}+\boldsymbol{R}_d^T\boldsymbol{R}\hat{\boldsymbol{\omega}}\\ &=-\boldsymbol{R}_d^T\boldsymbol{R}(\boldsymbol{R}^T\boldsymbol{R}_d)\hat{\boldsymbol{\omega}}_d(\boldsymbol{R}^T\boldsymbol{R}_d)^T+\boldsymbol{R}_d^T\boldsymbol{R}\hat{\boldsymbol{\omega}}\\ &=\boldsymbol{R}_d^T\boldsymbol{R}[\hat{\boldsymbol{\omega}}-(\boldsymbol{R}^T\boldsymbol{R}_d\boldsymbol{\omega}_d)^\wedge]\\ &=\boldsymbol{R}_d^T\boldsymbol{R}\hat{\boldsymbol{e}}_\omega \end{aligned} \]
【证】
【引理2】对\(\forall \boldsymbol{A}\in\mathbb{R}^{3\times3}, \boldsymbol{x}\in\mathbb{R}^3\)，有\(\text{tr}(\boldsymbol{A}\hat{\boldsymbol{x}})=-\boldsymbol{x}^T(\boldsymbol{A}-\boldsymbol{A}^T)^\vee\)
\[\text{tr}(\boldsymbol{A}\hat{\boldsymbol{x}})=\frac{1}{2}\text{tr}[\hat{\boldsymbol{x}}\boldsymbol{A}+(\boldsymbol{A}\hat{\boldsymbol{x}})^T] =\frac{1}{2}\text{tr}[\boldsymbol{\hat{\boldsymbol{x}}(\boldsymbol{A}-\boldsymbol{A}^T)}] \]
【证】由于\(\text{tr}(\boldsymbol{A}\hat{\boldsymbol{x}})=\text{tr}(\hat{\boldsymbol{x}}\boldsymbol{A})=\text{tr}[(\boldsymbol{A}\hat{\boldsymbol{x}})^T]\)，所以有：设\(\boldsymbol{x}=[x_1,x_2,x_3]^T,(\boldsymbol{A}-\boldsymbol{A}^T)^\vee=[a_1,a_2,a_3]^T\)，则：
\[\begin{aligned} \boldsymbol{\hat{\boldsymbol{x}}(\boldsymbol{A}-\boldsymbol{A}^T)}&= \begin{bmatrix} 0 & -x_3 & x_2\\ x_3 & 0 & -x_1\\ -x_2 & x_1 & 0\\ \end{bmatrix} \begin{bmatrix} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0\\ \end{bmatrix}\\ &= \begin{bmatrix} -x_3a_3-x_2a_2 & & *\\ & -x_3a_3-x_1a_1 & \\ *& & -x_2a_2-x_1a_1\\ \end{bmatrix}\\ \end{aligned} \]所以
\[\begin{aligned} \text{tr}(\boldsymbol{A}\hat{\boldsymbol{x}})&=\frac{1}{2}\text{tr}[\hat{\boldsymbol{x}}(\boldsymbol{A}-\boldsymbol{A}^T)]\\ &=-x_1a_1-x_2a_2-x_3a_3\\ &=-\boldsymbol{x}^T(\boldsymbol{A}-\boldsymbol{A}^T)^\vee \end{aligned} \]
【定理】\(\dot{\Psi}(\boldsymbol{R},\boldsymbol{R}_d)=\boldsymbol{e}_R\cdot\boldsymbol{e}_\omega\)
\[\begin{aligned} \dot{\Psi}(\boldsymbol{R},\boldsymbol{R}_d)&=-\frac{1}{2}\text{tr}[\frac{d}{dt}(\boldsymbol{R}_d^T\boldsymbol{R})]\\ &=-\frac{1}{2}\text{tr}(\boldsymbol{R}_d^T\boldsymbol{R}\hat{\boldsymbol{e}}_\omega)\\ &=\frac{1}{2}\boldsymbol{e}_\omega^T(\boldsymbol{R}_d^T\boldsymbol{R}-\boldsymbol{R}^T\boldsymbol{R}_d)^\vee\\ &=\boldsymbol{e}_R\cdot\boldsymbol{e}_\omega \end{aligned} \]
【证】由引理1和引理2可知：
可以验证，这里设计的姿态误差满足下面的性质[6]：
另外，\(\Psi,\boldsymbol{e}_R,\boldsymbol{e}_\omega\)还可以设计为[7]：
\[\begin{aligned} \Psi(\boldsymbol{R},\boldsymbol{R}_d)&=2-\sqrt{1+\text{tr}(\boldsymbol{R}_d^T\boldsymbol{R})}\\ \boldsymbol{e}_R&=\frac{1}{2\sqrt{1+\text{tr}(\boldsymbol{R}_d^T\boldsymbol{R})}}(\boldsymbol{R}_d^T\boldsymbol{R}-\boldsymbol{R}^T\boldsymbol{R}_d)^\vee\\ \boldsymbol{e}_\omega&=\boldsymbol{\omega}-\boldsymbol{R}^T\boldsymbol{R}_d\boldsymbol{\omega}_d \end{aligned} \]也满足上述定理，请读者自行验证.
参考文献：
全权. 多旋翼飞行器设计与控制. 电子工业出版社, 2018. ↩︎
戴建生. 旋量代数与李群、李代数. 高等教育出版社, 2014. ↩︎
Lee, T., 2011. Robust Adaptive Geometric Tracking Controls on SO (3) with an Application to the Attitude Dynamics of a Quadrotor UAV. arXiv preprint arXiv:1108.6031. ↩︎
Bullo, F. and Lewis, A. Geometrix Control of Mechanical Systems. ser. Texts in Applied Mathematics. New York: Springer-Verlag, 2005, vol.49, Modeling, Analysis, and Design for Simple Mechanical Control Systems. ↩︎
Wang, Y., Hong, H., Guo, J., Wang, X. and Shang, W., 2020. Configuration Error Function Design and Application to Fixed-time Geometric Terminal Sliding-mode Control on SE(3). Acta Astronautica, 174, pp.61-71. ↩︎