笔记-基于对偶四元数的姿轨耦合动力学模型1

基于对偶四元数的航天器姿轨耦合建模

对偶四元数能一体化描述空间刚体转动和平动，基于对偶四元数建立航天器相对运动的姿轨耦合动力学模型。对偶四元数表示的动力学模型形式简洁，具有明确的物理意义，与四元数描述的相对姿态动力学模型形式相似，继承了四元数不含三角运算、无奇异性的优点。

研究现状

传统的航天器六自由度建模多采用“独立-耦合”方法，即用不同的数学方法描述航天器姿态运动和轨道运动，然后考虑姿轨耦合因素，联立实现航天器姿轨一体化建模过程。由于所建立的姿轨模型中姿态和轨道参数描述不统一，模型求解过程比较困难，这不是真正的姿轨一体化模型。

传统的航天器六自由度相对运动模型实际上是一种“伪六自由度模型”，忽略了相对姿态运动和相对位置运动之间的耦合，本质上仍为两个三自由度分离模型。为了更加贴合航天器姿轨一体化控制任务的实际需求，六自由度运动描述方法建立姿轨耦合运动学和动力学模型。

对偶四元数不仅可以描述航天器相对姿态运动，还可以同时且协同地描述相对位置变化。基于误差对偶四元数建立的航天器姿态和轨道相对运动学和动力学模型与基于误差四元数的航天器相对姿态运动模型具有相同的形式。

在同一数学框架内建立航天器的姿态运动和轨道运动模型，才是实现姿轨一体化的正确方法。

邹晖：几何代数的方法。

坐标系定义

惯性坐标系$O_i{x}_i{y}_i{z}_i$，原点为地心，简称I系

追踪航天器本体坐标系$O_b{x}_b{y}_b{z}_b$，原点为追踪航天器质心$O_b$，简称B系。

目标航天器本体坐标系$O_t{x}_t{y}_t{z}_t$，原点为目标航天器质心$O_t$，简称T系。

期望坐标系$O_d{x}_d{y}_d{z}_d$，根据任务需求而定义，简称D系。交会对接任务中，D系与I系保持相对静止；绕飞和监视任务中，D系相对I系实时变化。D系的引入增加了控制目标设计的灵活性。

四元数

四元数的定义、运算法则和物理含义

四元数的基本定义：
$$q=q_0+q_1\vec{i}+q_2\vec{j}+q_3\vec{k}=q_0+\vec{q}$$
其中，$q_0$是四元数的标量部分（Scalar Part），$\vec{q}$是四元数的矢量部分（Vector Part）。也可以表示为列向量的形式$q=(q_0,\vec{q})$。

四元数乘法：

令$q=\eta +{\xi }_1\vec{i}+{\xi }_2\vec{j}+{\xi }_3\vec{k}=[\eta ,{\vec{\xi }}^T{]}^T$，其中，$\vec{\xi }=[{\xi }_1,{\xi }_2,{\xi }_3{]}^T$，那么$q_1=[{\eta }_1,{\vec{\xi }}_1^T]$，$q_2=[{\eta }_2,{\vec{\xi }}_2^T]$。则其乘法运算法则为：
$$q_1\otimes q_2=[{\eta }_1{\eta }_2-{\vec{\xi }}_1^T{\vec{\xi }}_2,({\eta }_1{\vec{\xi }}_2+{\eta }_2{\vec{\xi }}_1+{\vec{\xi }}_1\times {\vec{\xi }}_2){]}^T=[q_1{]}_{\otimes }{q}_2$$
其中，$[q_1{]}_{\otimes }$表示一个参数矩阵，定义为：
$$[q_1{]}_{\otimes }=\left[\begin{array}{cc}\eta & -{\vec{\xi }}^2\\ \vec{\xi }& \eta {\mathbf{I}}_{3\times 3}+S(\vec{\xi })\end{array}\right]$$
参数矩阵$S(\cdot )$表示对应三维向量的反对称矩阵，以$\vec{\xi }$为例，其表达式为：
$$S(\vec{\xi })=\left[\begin{array}{ccc}0& -{\xi }_3& {\xi }_2\\ {\xi }_3& 0& -{\xi }_1\\ -{\xi }_2& {\xi }_1& 0\end{array}\right]$$
欧拉转动定理指出，在三维空间中，一个刚体在位移时，若刚体上至少有某一点保持固定不动，则此位移等价于绕着通过这一固定点的某固定轴的旋转。记这一固定轴（欧拉旋转轴）的单位矢量方向为$\vec{e}$，并以$\theta$表示等价旋转运动的旋转角（欧拉角），则这一等价的旋转运动可由一个单位四元数$q=[\eta ,{\vec{\xi }}^T{]}^T$表示，且$\eta$和$\vec{\xi }$满足：
$$\begin{gathered} \eta =\cos (\frac{\theta }{2}) \\ \vec{\xi }=\vec{e}\sin (\frac{\theta }{2}) \end{gathered}$$
定性来讲，$\eta$描述了欧拉角的大小，而$\vec{\xi }$描述了欧拉旋转轴的方向，且为单位四元数。

单位四元数既可以描述两坐标系的相对位姿关系，也可以表达某一向量在不同坐标系中的投影映射关系。

基于四元数的航天器姿态运动学模型

见“基于误差四元数的相对姿态运动学模型”。

基于四元数的航天器姿态动力学学模型

见“基于误差四元数的相对姿态动力学模型”。

基于误差四元数的相对姿态运动学模型

追踪器的姿态运动学方程为：
$${\dot{q}}_{BI}=\frac{1}{2}{q}_{BI}\otimes {\vec{\omega }}_{BI}^B=\frac{1}{2}{\vec{\omega }}_{BI}^I\otimes q_{BI}$$
目标器的姿态运动学方程为：
$${\dot{q}}_{TI}=\frac{1}{2}{q}_{TI}\otimes {\vec{\omega }}_{TI}^B=\frac{1}{2}{\vec{\omega }}_{TI}^I\otimes q_{TI}$$
其中，${\vec{\omega }}_{BI}^B$表示追踪器相对于I系的姿态角速度在B系下的分量，${\vec{\omega }}_{TI}^T$表示目标相对于I系的姿态角速度在T系下的分量。

定义误差四元数为：
$$q_{BT}=q_{TI}^{\ast }\otimes q_{BI}$$
则相对姿态运动学方程为：
$${\dot{q}}_{BT}=\frac{1}{2}{q}_{BT}\otimes {\vec{\omega }}_{BT}^T=\frac{1}{2}{\vec{\omega }}_{BT}^T\otimes q_{BT}$$
其中，相对角速度在B系和T系下的分量分别为：
$$\begin{gathered} {\vec{\omega }}_{BT}^B={\vec{\omega }}_{BI}^B-{\vec{\omega }}_{TI}^B \\ {\vec{\omega }}_{BT}^T={\vec{\omega }}_{BI}^T-{\vec{\omega }}_{TI}^T \end{gathered}$$

基于误差四元数的相对姿态动力学模型

追踪器姿态动力学方程：
$$\bar{J}{\dot{\vec{\omega }}}_{BI}^B+{\vec{\omega }}_{BI}^B\times (\bar{J}{\vec{\omega }}_{BI}^B)=T^B$$
目标器姿态动力学方程在追踪器本体坐标系B系下的表达式为（我猜）：
$$\bar{J}{\dot{\vec{\omega }}}_{TI}^B+{\vec{\omega }}_{TI}^B\times (\bar{J}{\vec{\omega }}_{TI}^B)=T^B$$
其中，$\bar{J}$表示本体系下追踪器or目标转动惯量矩阵，$T^B$表示作用于追踪器或目标的总力矩。

将${\dot{\vec{\omega }}}_{BI}^B={\dot{\vec{\omega }}}_{BT}^B+{\dot{\vec{\omega }}}_{TI}^B$代入到​追踪器姿态动力学方程中，可得相对姿态动力学方程为：
$${\dot{\vec{\omega }}}_{BT}^B=J^{-1}(T^B-({\vec{\omega }}_{BT}^B+{\vec{\omega }}_{TI}^B)\times (J({\vec{\omega }}_{BT}^B+{\vec{\omega }}_{TI}^B)))-q_{BT}^{\ast }\times {\dot{\vec{\omega }}}_{TI}^T\times q_{BT}-{\vec{\omega }}_{TI}^B\times {\vec{\omega }}_{BT}^B$$
$J$与$\bar{J}$的形式如下：
$$J=\left[\begin{array}{cc}1& 0_{1\times 3}\\ 0_{3\times 1}& \bar{J}\end{array}\right],\bar{J}=\left[\begin{array}{ccc}{J}_{11}& J_{12}& J_{13}\\ J_{21}& J_{22}& J_{23}\\ J_{31}& J_{32}& J_{33}\end{array}\right]$$

对偶数和对偶四元数

对偶数的定义和运算法则

对偶数（Dual Number）的定义：
$$\hat{z}=z+ϵz^{\prime }$$
其中，$z$和$z^{\prime }$是实数，$z$称作对偶数的实部，$z^{\prime }$称为对偶数的对偶部，$ϵ$称为对偶单位，可以理解为$ϵ=\left[\begin{array}{cc}0& 0\\ x& 0\end{array}\right]$，有${ϵ}^2=0$和$ϵ\ne 0$成立。

如果对偶数的实部和对偶部都是向量，即$\hat{\vec{v}}=\vec{v}+ϵ{\vec{v}}^{\prime }$，则该对偶数称为对偶向量，可以理解为实部和对偶部都是向量的的对偶数，也可以理解为元素为对偶数的向量。类似的，对偶矩阵和对偶四元数都可以这么理解。

如果对偶向量实部$\vec{v}$是定位矢量，即与参考点的选择无关，对偶部${\vec{v}}^{\prime }$为自由矢量，这样的对偶向量称为“旋量”，后面航天器动力学中的对偶速度矢量、对偶力矢量都可以看做旋量。

对偶四元数的定义和物理含义

对偶四元数的定义和运算法则

对偶四元数（Dual Quaternions）是对偶数和四元数在多维空间相结合的产物，可以理解为元素为四元数的对偶数，也可以理解为元素为对偶数的四元数。四元数只能表示3D旋转，对偶数只能表示平移，对偶四元数集成了两者的共同特性，从而能统一的表示旋转和平移。形式如下：
$$\begin{gathered} \hat{q}=[\hat{\eta },\hat{\vec{\xi }}]=[\eta +ϵ{\eta }^{\prime },\vec{\xi }+ϵ{\vec{\xi }}^{\prime }]=q_r+ϵq_d \\ =\eta +{\xi }_1\vec{i}+{\xi }_2\vec{j}+{\xi }_3\vec{k}+ϵ({\eta }^{\prime }+{{\xi }^{\prime }}_1\vec{i}+{{\xi }^{\prime }}_2\vec{j}+{{\xi }^{\prime }}_3\vec{k}) \\ =[\eta +ϵ{\eta }^{\prime },{\xi }_1+ϵ{{\xi }_1}^{\prime },{\xi }_2+ϵ{{\xi }_2}^{\prime },{\xi }_3+ϵ{{\xi }_3}^{\prime }] \end{gathered}$$
其中，$\hat{\vec{\eta }}$和$\hat{\vec{\xi }}$为对偶数和对偶向量，$q_r$和$q_d$是普通的四元数，分别称为对偶四元数的实部和对偶部。

对偶四元数的乘法运算法则：
$$\begin{gathered} {\hat{q}}_1\otimes {\hat{q}}_2=[{\hat{\eta }}_1{\hat{\eta }}_2-{\hat{\vec{\xi }}}_1^T{\hat{\vec{\xi }}}_2,({\hat{\eta }}_1{\hat{\vec{\xi }}}_2+{\hat{\eta }}_2{\hat{\vec{\xi }}}_1+{\hat{\vec{\xi }}}_1\times {\hat{\vec{\xi }}}_2{)}^T{]}^T=(q_{1r}\otimes q_{2r})+ϵ(q_{1r}\otimes q_{2d}+q_{1d}\otimes q_{2r}) \\ =[q_1{]}_{\otimes }{q}_2 \end{gathered}$$

对偶四元数的物理解释

Chasles定理：任何一般性刚体运动都可以通过绕某个轴的旋转和沿相同轴的平移实现。这种旋转和平移的组合很像是一种螺旋式的运动。

两坐标系之间的螺旋运动变换如下图所示，$\hat{\vec{n}}$、$d$、$\theta$分别为螺旋轴、螺距和转角。类似于单位四元数可以描述姿态运动，单位对偶四元数可以用来同时描述坐标系的平动和转动。单位四元数$q_{BI}$的几何意义为一个大圆弧矢量，类似的，单位对偶四元数${\hat{q}}_{BI}$的几何意义是一个螺旋弧矢量，可以看做是四元数和大圆法线矢量的合成。${\hat{q}}_{BI}$可以写为如下形式：
$${\hat{q}}_{BI}=[\cos (\frac{\hat{\theta }}{2}),\sin (\frac{\hat{\theta }}{2})\hat{\vec{n}}],\hat{\theta }=\theta +ϵd$$

坐标系I到坐标系B的运动可以先转动$q_{BI}$再平动$r_{BI}^B$得到，也可以先平动$r_{BI}^B$再转动$q_{BI}$得到。空间暜吕克直线在两坐标系中的表达分别为${\hat{\vec{l}}}^I={\vec{l}}^I+ϵ{\vec{m}}^I$和${\hat{\vec{l}}}^B={\vec{l}}^B+ϵ{\vec{m}}^B$，可知${\vec{l}}^B=q_{BI}^{\ast }\otimes {\vec{l}}^I\otimes q_{BI}$。其中${\vec{l}}^I$等矢量与四元数运算时标量部分按0的矢量四元数来处理。那么可以得到如下空间暜吕克直线的旋转公式，可以理解为同一对偶矢量在不同坐标系下的表示，也可以理解为同一坐标系中对偶矢量${\hat{\vec{l}}}^I$进过六自由度运动到${\hat{\vec{l}}}^B$。
$${\hat{\vec{l}}}^B={\hat{q}}_{BI}^{\ast }\otimes {\hat{\vec{l}}}^I\otimes {\hat{q}}_{BI}$$
其中，${\hat{q}}_{BI}=q_{BI}+ϵq_d$，且$q_d=\frac{1}{2}{q}_{BI}\otimes {\vec{r}}_{BI}^B$。

基于对偶四元数的航天器运动学模型

对于单航天器，轨道运动学和轨道动力学+姿态运动学和姿态动力学。轨道运动学和轨道动力学都是将航天器当成质点来研究的，因此不对动力学和运动学进行区分，统称为轨道动力学。

运动学是从几何的角度（不涉及物体本身的物理性质和加在物体上的力）描述和研究物体位置随时间的变化规律的力学分支。

动力学主要研究作用于物体上的力与物体运动的关系。动力学普遍定理包括动量定理、动量矩定理、动能定理，以及由这三个基本定理推导出的其他定理。

因此简单来讲，二体问题的运动方程即为单航天器轨道运动学模型，加上控制力后即为单航天器轨道动力学模型。而航天器相对轨道运动学模型也可以理解为无控制力（控制加速度）的C-W方程（T-H方程等），航天器相对轨道动力学模型即为有控制力（控制加速度）的C-W方程（T-H方程等）。

而航天器姿态运动学模型和姿态动力学模型就不能简单理解为有无控制力矩的问题（也可以这么理解？）。

坐标系I到坐标系B的运动可以先转动$q_{BI}$再平动$r_{BI}^B$得到，也可以先平动$r_{BI}^B$再转动$q_{BI}$得到。空间暜吕克直线在两坐标系中的表达分别为${\hat{\vec{l}}}^I={\vec{l}}^I+ϵ{\vec{m}}^I$和${\hat{\vec{l}}}^B={\vec{l}}^B+ϵ{\vec{m}}^B$，可知${\vec{l}}^B=q_{BI}^{\ast }\circ {\vec{l}}^I\circ q_{BI}$。其中${\vec{l}}^I$等矢量与四元数运算时标量部分按0的矢量四元数来处理。

单位对偶四元数的八个量中独立变量只有六个，正好对应着刚体空间运动六自由度。对偶四元数的实部代表航天器姿态运动，对偶部代表航天器轨道运动。

基于对偶四元数的刚体空间运动学方程为：
$${\dot{\hat{q}}}_{BI}=\frac{1}{2}{\hat{q}}_{BI}\otimes {\hat{\vec{\omega }}}_{BI}^B=\frac{1}{2}{\hat{\vec{\omega }}}_{BI}^I\otimes {\hat{q}}_{BI}$$
从上式可以发现对偶四元数描述的刚体六自由度运动与四元数描述的刚体旋转运动由相似的表达式。

基于对偶四元数的航天器动力学模型

对偶四元数在描述旋量时，表示描述方向的实部不因参考点的选择而变化，而对偶部则与参考点的选择有关。

对偶形式的动力学方程为：
$$\hat{M}{\dot{\hat{\vec{\omega }}}}_{BI}^B=-{\hat{\vec{\omega }}}_{BI}^B+{\hat{F}}^B$$
其中，$\hat{M}=m\frac{d}{dϵ}{\mathbf{I}}_{4\times 4}+ϵJ$是追踪器对偶惯量阵，${\hat{F}}^B=F^B+ϵT^B={\hat{F}}_a+{\hat{F}}_g+{\hat{F}}_d$是对偶力在B系下的分量，接下来的叙述省略上标$B$。${\hat{F}}_a$实部为控制力，对偶部为控制力矩；${\hat{F}}_g$实部为引力，对偶部为重力梯度力矩；对偶干扰力${\hat{F}}_d$在仿真时给出。

基于误差对偶四元数的相对姿轨耦合运动学模型

传统的航天器六自由度相对运动模型实际上式一种“伪六自由度模型”，忽略了相对姿态运动和相对位置运动之间的耦合，本质上仍为两个三自由度分离模型。为了更加贴合航天器姿轨一体化控制任务的实际需求，六自由度运动描述方法建立姿轨耦合运动学和动力学模型。对偶四元数可以同时且协同地描述相对位置变化。

坐标系B相对于坐标系T的误差对偶四元数定义为：
$${\hat{q}}_{BT}={\hat{q}}_{TI}^{\ast }\otimes {\hat{q}}_{BI}$$
显然，误差对偶四元数也是一个单位对偶四元数。${\hat{q}}_{BT}$满足如下表达式：
$$\begin{gathered} {\hat{q}}_{BT}=q_{BT}+ϵ\frac{1}{2}{q}_{BT}\otimes r_{BT}^B \\ =q_{BT}+ϵ\frac{1}{2}{r}_{BT}^T\otimes q_{BT}=q_{BT}+ϵq_{BTd} \end{gathered}$$
其中，$r_{BT}^B$和$r_{BT}^T$分别为追踪器与目标的相对位置矢量在B系与T系的分量。

基于误差对偶四元数的姿轨耦合相对运动方程：
$${\dot{\hat{q}}}_{BT}=\frac{1}{2}{\hat{q}}_{BT}\otimes {\hat{\vec{\omega }}}_{BT}^B$$
其中${\hat{\vec{\omega }}}_{BT}^B$为相对速度旋量在B系中的表达式。${\hat{\vec{\omega }}}_{BT}^B$也可以写为如下形式：
$${\hat{\vec{\omega }}}_{BT}^B={\vec{\omega }}_{BT}^B+ϵ({\dot{r}}_{BT}^B+{\vec{\omega }}_{BT}^B\times r_{BT}^B)$$

基于误差对偶四元数的相对姿轨耦合动力学模型

追踪航天器相对于目标航天器的姿轨耦合动力学方程为：
$${\dot{\hat{\vec{\omega }}}}_{BT}^B={\hat{M}}^{-1}(\hat{K}{\hat{\vec{\omega }}}_{BT}^B+\hat{L}+\hat{F})$$
其中：
$$\begin{gathered} \hat{K}=K_1\frac{d}{dϵ}+ϵK_2 \\ {K}_1=-m({\vec{\omega }}_{BT}^B+2{\vec{\omega }}_{TI}^B{)}^{\times } \\ {K}_2=(J({\vec{\omega }}_{BT}^B+{\vec{\omega }}_{TI}^B){)}^{\times }-({\vec{\omega }}_{TI}^B{)}^{\times }J-J({\vec{\omega }}_{TI}^B{)}^{\times } \\ \hat{L}=L_1+ϵL_2 \\ {L}_1=-m{q}_{BT}^{\ast }\otimes {\dot{v}}_{TI}^T\otimes q_{BT}-m(q_{BT}^{\ast }\otimes {\dot{\vec{\omega }}}_{TI}^T\otimes q_{BT})\times {\vec{r}}_{BT}^B-m{\vec{\omega }}_{TI}^B\times (v_{TI}^B+{\vec{\omega }}_{TI}^B\times {\vec{r}}_{BT}^B) \\ {L}_2=-J(q_{BT}^{\ast }\otimes {\dot{\vec{\omega }}}_{TI}^T\otimes q_{BT})-{\vec{\omega }}_{TI}^B\otimes J{\vec{\omega }}_{TI}^B \end{gathered}$$
为方便控制器设计，可以将上式进一步展开。可以看出，轨道部分包含姿态参数，即表明轨道相对运动受到姿态相对运动的影响，耦合项是由动量在动坐标系中求导和航天器受力在不同坐标系中的表达带来的。同时，相对姿态运动方程中的$T_g$和$T_d$与相对位置有关，但相对卫星轨道高度来说，相对位置是小量，可以忽略不计。

航天器相对运动控制中的问题

1. 质量与惯量不确定性问题。

   燃料消耗、载荷安装偏移、结构改变等因素将引起在轨航天器质量与惯量参数的变化。

2. 线速度和角速度不可测问题。

3. 运动约束问题。

   由于受制于敏感元件、执行机构或其他载荷的各种限制，以及处于飞行安全和完成某些特殊任务的考虑，在轨航天器经常需要满足各类运动约束。

4. 执行机构存在故障时的容错控制问题。

数值仿真

如何实现基于对偶四元数的航天器相对姿轨耦合运动学方程，需要搞懂每一个变量和系数的物理含义。

参考文献

1. 杨嘉庚. 航天器相对运动姿轨耦合跟踪控制[D]. 2016. 哈尔滨工业大学.
2. 吴锦杰. 航天器相对运动姿轨一体化动力学建模与控制技术研究[D]. 2013. 国防科技大学.
3. 机器人学代数基础——对偶四元数与速度旋量(或运动螺旋)之间的转换.
4. 董宏洋. 基于对偶四元数的航天器位姿一体化控制方法研究[D]. 2017. 哈尔滨工业大学.
5. 孙俊. 在轨服务航天器位姿一体化规划与控制[D]. 2017. 哈尔滨工业大学.