GPOPS-II 调试心得（一）：四元数约束
在六自由度轨迹优化中，四元数问题往往不是单一公式写错，而是姿态约定、终端约束和数值离散共同作用的结果。下面只总结通用调试方法，不涉及具体研究模型、参数或算例结果。
1. 先固定四元数约定
在编写优化模型前，应明确并保持以下约定一致：
- 四元数是标量在前还是标量在后；
- 四元数表示从哪个坐标系旋转到哪个坐标系；
- 方向余弦矩阵是主动旋转还是被动坐标变换；
- 角速度位于本体系还是参考系；
- 四元数乘法和欧拉角旋转顺序。
例如，采用标量在前形式
\[ q=[q_0,q_1,q_2,q_3]^\mathsf{T} \]
时，姿态运动学常写成
\[ \dot q=\frac{1}{2}\Omega(\omega)q. \]
\(\Omega(\omega)\) 的符号取决于上述约定，不能脱离坐标变换定义直接套用公式。实现后至少应独立验证：
- 单位四元数对应单位矩阵；
- \(q\) 与 \(-q\) 得到相同的方向余弦矩阵；
- \(R(q)R(q)^\mathsf{T}=I\) 且 \(\det R(q)=1\)；
- 单轴恒定角速度的数值积分与解析旋转一致；
- 由四元数得到的初始速度、重力和推力方向符合物理直觉。
这些检查应在调用 GPOPS-II 前单独完成。若轨迹方向明显错误，应先检查坐标变换，而不是调整优化器容差。
2. 初值和初始猜测先归一化
论文表格或文本中的四元数通常只保留有限位小数，因此可能不严格满足
\[ q^\mathsf{T}q=1. \]
进入优化前可对初始四元数及初始猜测执行一次
\[ q\leftarrow\frac{q}{\lVert q\rVert}. \]
不建议在连续动力学函数中悄悄对每个四元数重新归一化。这样会改变优化器实际看到的函数及其导数，还可能掩盖姿态动力学本身的错误。
3. 单位模约束不宜盲目收紧
对于正确构造的四元数运动学，\(\Omega\) 为反对称矩阵，因此
\[ \frac{\mathrm d}{\mathrm dt}(q^\mathsf{T}q) =q^\mathsf{T}\bigl(\Omega^\mathsf{T}+\Omega\bigr)q/2 =0. \]
也就是说，连续系统会从归一化初值自然保持单位模。但 GPOPS-II 使用配点法把连续问题转录为有限维非线性规划，离散解仍可能产生小幅模长误差。
如果在所有配点施加极紧的 \(q^\mathsf{T}q=1\) 路径约束，它可能与离散动力学近似冗余，使约束 Jacobian 条件变差。较稳妥的调试顺序是：
- 归一化初值与初始猜测；
- 求解后记录 \(\max_i|q_i^\mathsf{T}q_i-1|\)；
- 分别测试无显式单位模约束、带状约束和更严格约束；
- 同时比较 NLP 状态、网格误差、实际模长误差和目标函数；
- 只有敏感性测试支持时，才继续收紧约束。
声明的约束容差并不等于最终实际误差，因此判断结果时应以计算得到的 \(q^\mathsf{T}q-1\) 为准。
4. 避免在极值处使用退化的终端约束
若用
\[ \sin\theta_f=1 \]
表示 \(\theta_f=90^\circ\)，则目标点附近有 \(\cos\theta_f=0\)。这种极值形式的局部一阶信息较弱，可能出现姿态已经非常接近目标，但非线性规划仍难以进一步降低约束残差的情况。
更合适的表达取决于任务需要：
- 固定完整终端姿态时，可构造误差四元数 \(q_e=q_d^{-1}\otimes q_f\)，并约束其向量部分为零；
- 只约束某个轴的指向时，可直接对方向余弦矩阵中的方向向量建立独立约束；
- 在特定四元数约定下，也可把奇异终端姿态改写为等价的线性四元数关系。
替换前应同时验证物理等价性和约束 Jacobian 的秩。不能只因为新表达式容易收敛，就默认它仍代表原来的终端姿态。
5. 推荐的排错顺序
四元数相关问题可按以下顺序定位：
- 在优化器外验证四元数、方向余弦矩阵和角速度约定；
- 检查初始姿态、速度、重力及推力经过坐标变换后的方向；
- 暂时移除复杂终端姿态约束，确认基本动力学可求解；
- 加入数值性质良好的终端姿态表达；
- 监测单位模实际误差，再决定是否增加或收紧路径约束；
- 最后调整网格精度、缩放和 NLP 容差。
每次只改变一个设置，并分别记录终端姿态误差、四元数模长误差、网格误差和约束违反量。这样才能区分模型错误、约束退化和离散精度问题。
参考资料
- Patterson, M. A., & Rao, A. V. (2014). GPOPS-II: A MATLAB Software for Solving Multiple-Phase Optimal Control Problems Using hp-Adaptive Gaussian Quadrature Collocation Methods and Sparse Nonlinear Programming. ACM Transactions on Mathematical Software, 41(1), Article 1.
- GPOPS-II User’s Guide.
本文是个人调试经验的概括，仅讨论通用数值方法；不包含 GPOPS-II 源代码、第三方图片或具体论文的模型与数据。