笔记-基于Lie群SE(3)的航天器姿轨一体化建模方法

航天器姿态和轨道运动一体化建模方法有：传统的姿轨独立建模方法、基于对偶四元数的建模方法、基于Lie群SE(3)的建模方法。这篇笔记主要针对Lie群SE(3)的航天器姿轨一体化建模方法。

什么是Lie群SO(3)和Lie群SE(3)

SO(3) = 特殊正交群 = $\left\{R\in {\mathbb{R}}^{3\times 3}|R{R}^T=I,\det (R)=1\right\}$

SE(3) = 特殊欧氏群 = $\left\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in \text{SO}(3),t\in {\mathbb{R}}^3\right\}$

直观上来理解，SO(3)的元素是$3\times 3$的正交矩阵，与之对应的就是两个坐标系之间的欧拉转移矩阵，SE(3)的元素是$4\times 4$的矩阵，这个矩阵包含$3\times 3$的正交矩阵（旋转）和$3\times 1$的矢量（平移）。也就是说SO(3)用来描述旋转，SE(3)用来描述旋转加平移。

注释1：默认都是在实数域内，
S: Special
O: Orthogonal
E: Euclidean
3: 3-Dimensional
SO(3): 3维特殊正交群，3D Special Orthogonal Group
SE(3): 3维特殊欧氏群，3D Special Euclidean Group

注释2：一般来讲，矩阵用大写字母不加粗来表示，这里上下文给出的变量符号都比较乱。

注释3：SE(3)是三维实空间（表示位置的向量）与特殊正交空间（表示姿态的矩阵）的 半直积，即$\text{SE}(3)={\mathbb{R}}^3⋉\text{SE}(3)$

Lie群和Lie代数^[2]

图1：李群和李代数

单航天器姿轨一体化模型^[1]-[3]

传统单航天器轨道和姿态运动模型，在航天器本体坐标系中表示：
$$\{\begin{array}{rl}{\dot{\mathbf{C}}}_B^I& ={\mathbf{C}}_B^I({\mathbf{\omega }}^B{)}^{\times }\\ {\dot{\mathbf{R}}}^I& ={\mathbf{C}}_B^I{\mathbf{v}}^B\\ \mathbf{J}{\dot{\mathbf{\omega }}}^B+({\mathbf{\omega }}^B{)}^{\times }\mathbf{J}{\mathbf{\omega }}^B& ={\mathbf{\tau }}_c+{\mathbf{\tau }}_d\\ m{\dot{\mathbf{v}}}^B+m({\mathbf{\omega }}^B{)}^{\times }{\mathbf{v}}^B& =-\left(\frac{m\mu }{||{\mathbf{R}}^I|{|}^3}\right){\mathbf{R}}_b+{\mathbf{u}}_c+{\mathbf{u}}_d\end{array}$$

引入Lie群SE(3)，将上述航天器轨道运动学和动力学方程、姿态运动学和动力学方程进行一体化描述。

用方向余弦矩阵来描述航天器的姿态运动，姿态矩阵$\mathbf{C}$满足：
$${\mathbf{C}}^T\mathbf{C}={\mathbf{I}}_{3\times 3},\det (\mathbf{C})=1$$

所有满足上式约束的矩阵构成一个特殊正交集合，称为Lie群SO(3)，表示为：
$$\text{SO}(3)=\left\{\mathbf{C}\in {\mathbb{R}}^{3\times 3}:{\mathbf{C}}^T\mathbf{C}={\mathbf{I}}_{3\times 3},\det (\mathbf{C})=1\right\}$$

航天器的构型通过$({\mathbf{R}}^I,{\mathbf{C}}_B^I)$来描述，Lie群SE(3)的定义：

$$\text{SE}(3)=\left\{({\mathbf{R}}^I,{\mathbf{C}}_B^I):{\mathbf{R}}^I\in {\mathbb{R}}^3,{\mathbf{C}}_B^I\in {\mathbb{R}}^{3\times 3},{\mathbf{C}}_B^I\in \text{SE}(3)\right\}={\mathbb{R}}^3⋉\text{SE}(3)$$

基于$\text{SE}(3)$的齐次形式来表示航天器的位姿构型：
$$\mathbf{g}=\left[\begin{array}{cc}{\mathbf{C}}_B^I& {\mathbf{R}}^I\\ 0_{1\times 3}& 1\end{array}\right]\in \text{SE}(3)\in {\mathbb{R}}^{4\times 4}$$

令$\mathbf{\xi }={\left[\begin{array}{c}({\mathbf{\omega }}^B{)}^T,({\mathbf{v}}^B{)}^T\end{array}\right]}^T\in {\mathbb{R}}^6$，相应的李代数为${\mathbf{\xi }}^{\vee }=(({\mathbf{\omega }}^B{)}^{\times },({\mathbf{v}}^B))\in \text{so}(3)\times {\mathbb{R}}^3$，齐次形式表达式为：
$${\mathbf{\xi }}^{\vee }=\left[\begin{array}{cc}({\mathbf{\omega }}^B{)}^{\times }& {\mathbf{v}}^B\\ 0_{1\times 3}& 1\end{array}\right]\in \text{se}(3)\in {\mathbb{R}}^{4\times 4}$$

因此，基于SE(3)的航天器姿轨耦合运动学方程：
$$\dot{\mathbf{g}}=\mathbf{g}{\mathbf{\xi }}^{\vee }\text{\hspace{1em}(1)}$$

Lie群$\text{SE}(3)$的两个伴随映射定义为：
$$\begin{array}{rl}{\text{Ad}}_g& =\left[\begin{array}{cc}{\mathbf{C}}_B^I& 0_{3\times 3}\\ ({\mathbf{R}}^I{)}^{\times }{\mathbf{C}}_B^I& {\mathbf{R}}^I\end{array}\right]\\ {\text{ad}}_{\mathbf{\xi }}& =\left[\begin{array}{cc}({\mathbf{\omega }}^B{)}^{\times }& 0_{3\times 3}\\ ({\mathbf{v}}^B{)}^{\times }& ({\mathbf{\omega }}^B{)}^{\times }\end{array}\right]\end{array}\text{\hspace{1em}(2)}$$

上式定义的伴随算子$\text{ad}$代表了李代数$\text{se}(3)$与李群$\text{SE}(3)$之间的线性运算，其反伴随算子可以通过李代数的对偶运算得到：
$${\text{ad}}_{\mathbf{\xi }}^{\ast }={\text{ad}}_{\mathbf{\xi }}^T=\left[\begin{array}{cc}-({\mathbf{\omega }}^B{)}^{\times }& -({\mathbf{v}}^B{)}^{\times }\\ 0_{3\times 3}& -({\mathbf{\omega }}^B{)}^{\times }\end{array}\right]$$

则基于SE(3)的航天器姿轨耦合动力学方程：
$$\mathbf{\Xi }\dot{\mathbf{\xi }}={\text{ad}}_{\mathbf{\xi }}^{\ast }\mathbf{\Xi }\mathbf{\xi }+\mathbf{f}(\mathbf{\Xi })+{\mathbf{\Gamma }}_c+{\mathbf{\Gamma }}_d\text{\hspace{1em}(3)}$$

其中，

$\mathbf{\Xi }=\text{diag}(\mathbf{J},m{\mathbf{I}}_3)\in {\mathbb{R}}^{6\times 6}$为航天器质量与转动惯量构成的矩阵；

$\mathbf{f}(\mathbf{\Xi })={\left[{\mathbf{M}}_g^{\mathrm{T}},({\mathbf{f}}_g-\frac{m\mu }{||{\mathbf{R}}^I|{|}^3}{\mathbf{R}}_b{)}^{\mathrm{T}}\right]}^{\mathrm{T}}$为航天器所受的重力梯度力矩和重力 (存疑)；

${\mathbf{\Gamma }}_c={\left[{\mathbf{\tau }}_c^{\mathrm{T}},{\mathbf{u}}_c^{\mathrm{T}}\right]}^{\mathrm{T}}\in {\mathbb{R}}^6$为航天器的控制输入；

${\mathbf{\Gamma }}_d={\left[\Delta {\mathbf{d}}_{\tau }^{\mathrm{T}},\Delta {\mathbf{d}}_f^{\mathrm{T}}\right]}^{\mathrm{T}}\in {\mathbb{R}}^6$为航天器所受的外部总干扰。

注释3：手写公式还是挺麻烦的。一般而言，转置符号$(\cdot {)}^{\mathrm{T}}$应该是正体，变量应该都是斜体，标量不加粗，矢量加粗，矩阵用大写，空间或域用空心体，下标、上标等符号一般用正体，函数名称或其他单词缩写简写都是正体。

注释4：文献[3]中的公式基本是正确的，小部分地方有瑕疵。

航天器相对姿轨一体化模型

航天器姿轨一体化相对动力学模型来描述航天器轨迹跟踪问题。设目标轨迹的期望位姿构型为${\mathbf{g}}_d$，跟踪航天器的实际位姿构型为${\mathbf{g}}_a$，则航天器的位置和姿态跟踪误差可以表示为：
$${\mathbf{g}}_e={\mathbf{g}}_d^{-1}{\mathbf{g}}_a=\left[\begin{array}{cc}{\mathbf{C}}_e& {\mathbf{R}}_e\\ 0_{1\times 3}& 1\end{array}\right]$$

其中，${\mathbf{C}}_e=({\mathbf{C}}_B^I{)}_d^T{\mathbf{C}}_B^I$，${\mathbf{R}}_e=({\mathbf{C}}_B^I{)}_d^T({\mathbf{R}}^I-{\mathbf{R}}_d^I)$，下角标$d$表示目标轨迹的位姿参数。该跟踪误差用$\text{SE}(3)$上的指数坐标来一体化描述：
$$\mathbf{\eta }=\left[\begin{array}{c}\mathbf{\Phi }\\ \mathbf{\phi }\end{array}\right]={\left[\begin{array}{c}{\log }_{\text{SE}(3)}{\mathbf{g}}_e\end{array}\right]}^{-1}\in {\mathbb{R}}^6$$

其中，$[\cdot {]}^{-1}$为$(\cdot {)}^{\vee }$的逆映射，${\log }_{\text{SE}(3)}$为$\text{SE}(3)$上的对数映射，可以表示为：
$${\log }_{\text{SE}(3)}=\left[\begin{array}{cc}{\mathbf{\Phi }}^{\times }& \mathbf{\phi }\\ 0& 0\end{array}\right]$$

式中，跟踪误差$\mathbf{\eta }$由两部分组成，$\mathbf{\Phi }\in {\mathbb{R}}^3$表示姿态部分的跟踪误差，由$\text{SO}(3)$上的指数坐标计算得到，$\mathbf{\phi }\in {\mathbb{R}}^3$表示位置跟踪误差，两者的具体表达式为：
$${\mathbf{\Phi }}^{\times }=\{\begin{array}{ll}{0}_{3\times 3}& \theta =0\\ \frac{\theta }{2\sin \theta }({\mathbf{C}}_e-{\mathbf{C}}_e^T)& \theta \in (-\pi ,\pi ),\theta \ne 0\end{array}$$

和
$$\{\begin{array}{rl}\mathbf{\phi }& ={\mathbf{S}}^{-1}(\mathbf{\Phi }){\mathbf{R}}_e\\ \mathbf{S}(\Phi )& ={\mathbf{I}}_{3\times 3}+\frac{1-\cos \theta }{{\theta }^2}{\mathbf{\Phi }}^{\times }+\frac{\theta -\sin \theta }{{\theta }^3}({\mathbf{\Phi }}^{\times }{)}^2\end{array}$$

其中，$\theta$为欧拉旋转角。

。。。。。。

速度跟踪误差可表示为：
$$\tilde{\mathbf{\xi }}={\left[\begin{array}{c}\Delta {\mathbf{\omega }}^T,\Delta {\mathbf{v}}^T\end{array}\right]}^T=\mathbf{\xi }-{\text{Ad}}_{{\mathbf{g}}_e^{-1}}{\mathbf{\xi }}_d$$

其中，${\mathbf{\xi }}_d$为目标轨迹的速度。

基于Lie群$\text{SE}(3)$指数坐标$\mathbf{\eta }$描述航天器位姿跟踪误差，则误差运动学方程可以表示为：
$$\dot{\mathbf{\eta }}=\mathbf{G}(\mathbf{\eta })\tilde{\mathbf{\xi }}\text{\hspace{1em}(4)}$$

其中，
$$\mathbf{G}(\mathbf{\eta })=\left[\begin{array}{cc}\mathbf{A}(\mathbf{\Phi })& 0\\ \mathbf{T}(\mathbf{\Phi },\mathbf{\phi })& \mathbf{A}(\mathbf{\Phi })\end{array}\right]$$

$$\mathbf{A}(\mathbf{\Phi })=\mathbf{I}+\frac{1}{2}{\mathbf{\Phi }}^{\times }+\left(\frac{1}{{\theta }^2}-\frac{1+\cos \theta }{2\theta \sin (\theta )}\right)({\mathbf{\Phi }}^{\times }{)}^2$$

$$\mathbf{T}(\mathbf{\Phi },\mathbf{\phi })=\frac{1}{2}(\mathbf{S}(\mathbf{\Phi })\mathbf{\phi }{)}^{\times }\mathbf{A}(\mathbf{\Phi })+\left(\frac{1}{{\theta }^2}-\frac{1+\cos \theta }{2\theta \sin (\theta )}\right)\left(\mathbf{\Phi }{\mathbf{\phi }}^T+{\mathbf{\Phi }}^T\mathbf{\phi }\mathbf{A}(\mathbf{\Phi })\right)$$

。。。。。。

航天器相对姿轨一体化动力学方程：
$$\mathbf{\Xi }\dot{\tilde{\mathbf{\xi }}}={\text{ad}}_{\mathbf{\xi }}^{\ast }\mathbf{\Xi }\mathbf{\xi }+\mathbf{f}(\mathbf{\Xi })+{\mathbf{\Gamma }}_c+{\mathbf{\Gamma }}_d+\mathbf{\Xi }\left({\text{ad}}_{\tilde{\mathbf{\xi }}}{\text{Ad}}_{{\mathbf{g}}_e^{-1}}{\mathbf{\xi }}_d-{\text{Ad}}_{{\mathbf{g}}_e^{-1}}{\dot{\mathbf{\xi }}}_d\right)\text{\hspace{1em}(5)}$$

当考虑模型不确定性时，实际的惯量和质量矩阵可以表示为${\mathbf{\Xi }}_1=\mathbf{\Xi }+\Delta \mathbf{\Xi }$，其中$\Delta \mathbf{\Xi }$即为模型不确定性部分。此时相对动力学方程可以表示为：
$$\dot{\tilde{\mathbf{\xi }}}=\mathbf{H}+\Delta \mathbf{d}+{\mathbf{\Xi }}^{-1}{\mathbf{\Gamma }}_c+{\mathbf{\Xi }}^{-1}\mathbf{f}(\mathbf{\Xi })\text{\hspace{1em}(6)}$$

其中，
$$\mathbf{H}=...$$

$$\Delta \mathbf{d}=...$$

$\Delta \mathbf{d}$为外部干扰和模型不确定性部分产生的系统总干扰。在进行控制器设计时，有如下假设：$\Delta \mathbf{d}$有界，即$||\Delta \mathbf{d}||\le {\delta }_1$，其中${\delta }_1$为一个正常数。

航天器姿轨一体化轨迹跟踪控制器设计

• 变量是哪些？ 轨迹跟踪误差$\mathbf{\eta }$和速度跟踪误差$\tilde{\mathbf{\xi }}$；
• 方程是哪个？ 方程$(3)$和方程$(6)$；
• 目的是什么？ 使变量$\mathbf{\eta }$和$\tilde{\mathbf{\xi }}$都收敛到0；
• 控制方法是哪种？ 快速终端滑模控制；
• 约束条件有哪些？ 外部干扰、模型不确定性。

文献[2]的3.3节采用的是快速终端滑模控制，第四章加入执行器输入饱和的约束条件，采用基于扩展状态观测器的事件驱动自适应滑模控制方法。

快速终端滑模控制方法

快速终端滑模面：
$$\mathbf{S}=\tilde{\mathbf{\xi }}+{\mathbf{\vartheta }}_1\mathbf{\eta }+{\mathbf{\vartheta }}_2{\text{sig}}^{\alpha }(\mathbf{\eta })$$

则
$$\dot{\mathbf{S}}=\dot{\tilde{\mathbf{\xi }}}+{\mathbf{\vartheta }}_1\dot{\mathbf{\eta }}+\alpha {\mathbf{\vartheta }}_2{\mathbf{\Lambda }}^{\alpha -1}(\mathbf{\eta })\dot{\mathbf{\eta }}$$

滑模控制器：
$$\begin{array}{rl}{\mathbf{\Gamma }}_c& =-\mathbf{\Xi }\left({\mathbf{\vartheta }}_1\mathbf{G}(\mathbf{\eta })\tilde{\mathbf{\xi }}+\alpha {\mathbf{\vartheta }}_2{\mathbf{\Lambda }}^{\alpha -1}(\mathbf{\eta })\mathbf{G}(\mathbf{\eta })\tilde{\mathbf{\xi }}\right)\\ & -\mathbf{f}(\mathbf{\Xi })-\mathbf{\Xi }\mathbf{H}-{\mathbf{K}}_1\text{sgn}(\mathbf{S})+{\mathbf{K}}_2\mathbf{S}\end{array}\text{\hspace{1em}(7)}$$

其中，${\mathbf{\vartheta }}_1=\text{diag}({\vartheta }_{11}{\mathbf{I}}_3,{\vartheta }_{12}{\mathbf{I}}_3)\in {\mathbb{R}}^{6\times 6}>0$，${\mathbf{\vartheta }}_2=\text{diag}({\vartheta }_{21}{\mathbf{I}}_3,{\vartheta }_{22}{\mathbf{I}}_3)\in {\mathbb{R}}^{6\times 6}>0$，${\mathbf{K}}_1=\text{diag}(k_{11},\cdots ,k_{16})$和${\mathbf{K}}_2=\text{diag}(k_{21},\cdots ,k_{26})$均为正定对角阵，且${\mathbf{K}}_1$满足：
$${\lambda }_{\min }({\mathbf{K}}_1)>c_1=||\mathbf{\Xi }\Delta \mathbf{d}||$$

$\alpha {\mathbf{\Lambda }}^{\alpha -1}(\mathbf{\eta })$代表什么？应该是${\text{sig}}^{\alpha }(\mathbf{\eta })$的导数。

证明过程和传统滑模方法一样分两个阶段：趋近模态阶段和滑动模态阶段。趋近阶段选取的Lyapunov函数为：
$$V_1=\frac{1}{2}{\mathbf{S}}^T\mathbf{\Xi }\mathbf{S}$$

滑动模态阶段选取的Lyapunov函数为：
$$V_2=\frac{1}{2}{\mathbf{\eta }}^T\mathbf{\eta }$$

对于滑模控制器$(7)$，由两部分组成，前半部分为等效控制，用来保证系统状态到达滑模面$\mathbf{S}=0$，后半部分$-{\mathbf{K}}_1\text{sgn}(\mathbf{S})+{\mathbf{K}}_2\mathbf{S}$为切换控制，用于保证系统对模型不确定性和外部扰动的鲁棒性。针对滑模控制方法固有的抖振问题，可以选用饱和函数${\mathbf{K}}_1\text{sat}(\mathbf{S},\rho )$代替符号函数${\mathbf{K}}_1\text{sgn}(\mathbf{S})$，也可以设计一种自适应控制器，用自适应项代替符号函数：
$${\mathbf{K}}_1\text{sgn}(\mathbf{S})\Rightarrow \text{diag}\left(\frac{K_{1j}}{|S_j|+\epsilon K_{3j}^2}\right)\mathbf{S}$$

其中，$\epsilon$为一个小的正数，${\mathbf{K}}_3\in {\mathbb{R}}^{6\times 1}$，并且$K_{3j}>0,j=1,\cdots ,6$，且满足如下自适应律：
$${\dot{K}}_{3j}=-\lambda \frac{\epsilon K_{1j}|S_j|K_{3j}}{|S_j|+\epsilon K_{3j}^2}$$

证明过程所选的Lyapunov函数为：
$$V_3=\frac{1}{2}{\mathbf{S}}^T\mathbf{\Xi }\mathbf{S}+\frac{1}{2\lambda }\sum _{j=1}^6{K}_{3j}^2$$

注释：文献[2]中关于$S_j$、$K_{1j}$和$K_{3j}$的符号都不应该加粗，这些是标量。文章中的小错误不少，但大错误没有。该文献所对比的方法为线性滑模方法（LSM）、终端滑模方法（TSM）和快速终端滑模方法（FTSM）：
$$\begin{array}{rl}\text{LSM：}& \mathbf{S}=\dot{\mathbf{x}}+{\mathbf{\vartheta }}_1\mathbf{x}\\ \text{TSM：}& \mathbf{S}=\dot{\mathbf{x}}+{\mathbf{\vartheta }}_2{\text{sig}}^{\alpha }(\mathbf{x})\\ \text{LSM：}& \mathbf{S}=\dot{\mathbf{x}}+{\mathbf{\vartheta }}_1\mathbf{x}+{\mathbf{\vartheta }}_2{\text{sig}}^{\alpha }(\mathbf{x})\end{array}$$

注释：几个定义：
$${\text{sig}}^{\alpha }(\mathbf{x})={\left[\begin{array}{c}|x_1{|}^{\alpha }\text{sgn}(x_1),\cdots ,|x_n{|}^{\alpha }\text{sgn}(x_n)\end{array}\right]}^T\in {\mathbb{R}}^n$$

$${\mathbf{\Lambda }}^{\alpha }(\mathbf{x})=\text{diag}\left(\left[\begin{array}{c}|x_1{|}^{\alpha },\cdots ,|x_n{|}^{\alpha }\end{array}\right]\right)$$

其中，$\text{sgn}(\cdot )$为符号函数，$\text{diag}(\cdot )$为对角矩阵。

仿真分析

初始参数设置：
目标航天器轨道参数、追踪航天器轨道参数
期望相对姿态和轨道参数
追踪航天器质量和转动惯量的标称部分+不确定性部分
追踪航天器所受的干扰力和干扰力矩
执行机构输出上限

控制器参数设置、对比仿真。

参考文献

1. 段玉瑞, 廖瑛, 王勇,等. 基于SE(3)的航天器姿轨一体化建模与控制[C]. 第二十届中国系统仿真技术及其应用学术年会, 2019.
2. 张剑桥. 航天器姿轨一体化建模与控制方法研究[D]. 哈尔滨工业大学, 2020.
3. 张剑桥, 叶东, 孙兆伟. SE(3)上姿轨耦合航天器高精度快速终端滑模控制[J]. 宇航学报, 2017, 38(2): 176-184.
4. 知乎：旋转之十 - SO(3) 与 so(3)
5. 知乎：旋转之十一 - SE(3) 和 se(3)
6. CSDN：学习IMU预积分(1)李群
7. CSDN：机器人运动学中李代数se(3)与李群SE(3)的基本概念与联系