引言
在控制航天器姿态之前，我们首先需要知道它当前的姿态是什么。这就是姿态确定（Attitude Determination）的任务。姿态确定本质上是一个估计问题：利用星敏感器、太阳敏感器、磁强计等传感器测量到的矢量信息，结合已知的参考矢量（如星表、太阳星历、地磁模型），计算出航天器本体坐标系相对于惯性参考系的旋转矩阵（或四元数）。
本章将介绍两种最经典的静态姿态确定方法：确定性的TRIAD算法和基于最优化的Wahba问题解法，并简要提及动态估计方法（EKF）。
1. 观测模型
假设我们在惯性系 $\mathcal{N}$ 中有两个已知的参考单位矢量 $\mathbf{r}_1, \mathbf{r}_2$（例如太阳矢量和地磁矢量）。 同时，我们在航天器本体坐标系 $\mathcal{B}$ 中测量到了这两个矢量，记为 $\mathbf{b}_1, \mathbf{b}_2$。
理想情况下，如果不存在测量噪声，它们之间满足如下关系：
其中 $\mathbf{C}_{BN}$ 是我们需要求解的姿态矩阵（DCM）。
2. TRIAD 算法
TRIAD（Tri-Axial Attitude Determination）算法是最早被提出的姿态确定算法之一（Black, 1964）。它的核心思想是利用两个非共线的矢量观测值构建两个正交坐标系，从而求解姿态矩阵。
2.1 算法步骤
构建参考系基底： 利用惯性系下的参考矢量 $\mathbf{r}_1, \mathbf{r}_2$ 构建一个正交三合基 $\{\mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3\}$：
构建本体系基底： 利用测量矢量 $\mathbf{b}_1, \mathbf{b}_2$ 构建对应的正交三合基 $\{\mathbf{w}_1, \mathbf{w}_2, \mathbf{w}_3\}$：
求解姿态矩阵： 由于这两个基底描述的是同一个物理架构在不同坐标系下的表示，因此存在唯一的旋转矩阵 $\mathbf{C}_{BN}$ 使得 $\mathbf{w}_i = \mathbf{C}_{BN} \mathbf{v}_i$。 定义矩阵 $\mathbf{V} = [\mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3]$ 和 $\mathbf{W} = [\mathbf{w}_1, \mathbf{w}_2, \mathbf{w}_3]$。 则有 $\mathbf{W} = \mathbf{C}_{BN} \mathbf{V}$。 由于 $\mathbf{V}$ 是正交矩阵（$\mathbf{V}^T = \mathbf{V}^{-1}$），我们可以直接解出：
2.2 优缺点分析
- 优点：计算极其简单，不需要迭代，适合算力有限的星载计算机。
- 缺点：它只利用了两个矢量信息，无法处理多于两个观测矢量的情况。而且它人为地赋予了第一个矢量 $\mathbf{r}_1$ 更高的权重（因为 $\mathbf{w}_1 = \mathbf{b}_1$ 完全保留了 $\mathbf{b}_1$ 的信息，而 $\mathbf{b}_2$ 的部分信息被丢弃了）。因此，通常将精度更高的传感器（如星敏感器）作为第一个矢量。
3. Wahba 问题
为了克服TRIAD算法的局限性，Grace Wahba 在1965年提出了著名的 Wahba 问题。这是一个最小二乘优化问题：寻找一个正交矩阵 $\mathbf{C}$，使得所有观测矢量 $\mathbf{b}_i$ 与参考矢量 $\mathbf{r}_i$ 之间的加权误差平方和最小。
3.1 损失函数
其中 $a_i$ 是第 $i$ 个观测矢量的权重，通常取为测量方差的倒数 $\sigma_i^{-2}$。$N$ 是观测矢量的数量（$N \ge 2$）。
展开损失函数：
由于 $\mathbf{b}_i$ 和 $\mathbf{r}_i$ 都是单位矢量，前两项为常数。因此，最小化 $J(\mathbf{C})$ 等价于最大化增益函数 $g(\mathbf{C})$：
其中 $\mathbf{B}$ 称为姿态轮廓矩阵 (Attitude Profile Matrix)：
3.2 Davenport’s q-method
求解 Wahba 问题的方法有很多（如QUEST, ESOQ, FOAM），其中 Davenport 的 q-method 是最经典且基于四元数的方法。
将姿态矩阵 $\mathbf{C}$ 用四元数 $\mathbf{q} = [\boldsymbol{\epsilon}^T, \eta]^T$ 表示，代入增益函数，经过繁琐但巧妙的代数推导，可以将最大化问题转化为求解特征值问题：
其中 $\mathbf{K}$ 是一个 $4 \times 4$ 的对称矩阵，称为 Davenport 矩阵：
这里：
- $\mathbf{S} = \mathbf{B} + \mathbf{B}^T$
- $\mathbf{Z} = [\mathbf{B}_{23}-\mathbf{B}_{32}, \mathbf{B}_{31}-\mathbf{B}_{13}, \mathbf{B}_{12}-\mathbf{B}_{21}]^T = \sum a_i (\mathbf{b}_i \times \mathbf{r}_i)$
- $\sigma = \text{tr}(\mathbf{B})$
为了最大化 $\mathbf{q}^T \mathbf{K} \mathbf{q}$，在约束 $\mathbf{q}^T \mathbf{q} = 1$ 下，最优的四元数 $\mathbf{q}_{opt}$ 正是矩阵 $\mathbf{K}$ 的最大特征值对应的特征向量。
这一结论极其优美，它将非线性的姿态估计问题转化为了标准的线性代数特征值问题，保证了全局最优解的存在性和唯一性。
3.3 QUEST 算法 (Quaternion Estimator)
q-method 虽然精确，但求解 $4 \times 4$ 矩阵的特征值在早期的星载计算机上仍然比较耗时。Shuster (1981) 提出了 QUEST 算法。 QUEST 利用牛顿-拉夫逊迭代法快速求解特征方程 $\det(\mathbf{K} - \lambda \mathbf{I}) = 0$ 的最大根 $\lambda_{max}$，通常只需一次迭代即可收敛。
QUEST 是目前应用最广泛的静态姿态确定算法。
4. 动态姿态估计 (Kalman Filtering)
上述方法（TRIAD, Wahba）都是静态的，即只利用当前时刻的测量值。但在实际中，我们还有陀螺仪（Gyro）测量角速度。 利用陀螺仪积分可以得到姿态的先验估计，再利用矢量观测（星敏感器）来修正这个估计。这就是扩展卡尔曼滤波 (EKF) 的基本思想。 EKF 不仅能提高姿态精度，还能实时估计陀螺仪的漂移误差（Bias），是现代高精度姿态确定系统的核心算法。
总结
- TRIAD：简单快速，适用于双矢量观测，精度受限于第一个矢量的精度。
- Wahba问题：基于最小二乘的最优估计，能融合任意数量的矢量观测，精度更高。
- q-method：Wahba问题的解析解，通过求解 $4 \times 4$ 矩阵的特征向量直接得到最优四元数。
- EKF：融合陀螺仪和矢量观测的动态估计算法，是工业界的标准。
下一章，我们将探讨如何利用环境力矩（如重力梯度）来实现航天器的被动稳定，而无需消耗任何燃料。