1. 火箭着陆制导算法概述
火箭着陆制导算法是航天器精确着陆的核心技术，其核心任务是在考虑各种约束条件下，生成最优的推进下降轨迹和控制指令。传统方法如阿波罗任务中使用的多项式制导虽然简单可靠，但存在两个主要缺陷：燃料消耗非最优和着陆散布范围过大（可达数公里）。这显然无法满足现代任务对百米级甚至更高精度的需求。
随着凸优化理论的发展，21世纪初出现的无损凸化(lcvx)技术彻底改变了这一局面。该技术通过数学变换将原本非凸的火箭着陆问题转化为凸优化问题，从而保证能获得全局最优解。基于此的3自由度(3-DoF)制导算法已在实际飞行测试中验证了其有效性，能够在实时条件下生成燃料最优的大机动轨迹。
然而，3-DoF模型仅考虑平移运动，将推力矢量作为姿态的替代变量，这使得对飞行器姿态的显式约束变得困难。为此，研究者开发了6自由度(6-DoF)制导算法，通过双四元数统一描述平移和旋转运动，实现了状态间的自然耦合。这种表述方式特别适合处理如视线约束等关键操作限制。
2. 双四元数建模与6-DoF动力学
2.1 双四元数基础
双四元数是描述刚体运动的强大数学工具，它由两个四元数组成，可以同时表示平移和旋转。在火箭制导中，我们使用单位双四元数，其形式为：
q̌ = q + ε(1/2)r ⊗ q
其中q是姿态四元数，r是位置向量，ε是双数单位(ε²=0但ε≠0)，⊗表示四元数乘法。这种表示法的优势在于：
- 自然地耦合了平移和旋转运动
- 某些复杂约束（如视线约束）可以转化为凸约束
- 计算效率高，适合实时应用
2.2 系统状态与动力学方程
6-DoF火箭模型的状态向量包含15个元素：
- 质量 m ∈ ℝ
- 8维单位双四元数 q̌ ∈ ℝ⁸
- 6维"双速度" ω ∈ ℝ⁶
动力学方程包括：
- 质量消耗方程：考虑主发动机和RCS推力器的燃料消耗
- 双四元数运动学方程：描述姿态和位置的演变
- 动力学方程：基于牛顿-欧拉方程，考虑时变惯量矩阵
特别值得注意的是，我们假设惯量矩阵是质量的线性函数，从而考虑了燃料消耗对姿态动力学的影响。这种建模比常值惯量假设更接近实际情况。
3. 控制参数化与约束处理
3.1 推力矢量参数化
推力矢量的参数化方式对问题凸性有决定性影响。我们采用球坐标表示法：
- T：推力大小
- δ：从机体z轴测量的偏转角度
- ϕ：绕机体z轴的方位角度
这种表示法的优势在于：
- 所有控制约束都自然保持凸性
- 可以精确处理控制速率约束
- 推力大小解具有分段仿射特性
相比之下，笛卡尔坐标参数化会导致非凸约束，增加求解难度。
3.2 控制约束
控制约束包括：
推力矢量边界：
- 推力大小：T_min ≤ T ≤ T_max
- 偏转角：0 ≤ δ ≤ δ_max
- 方位角：0 ≤ ϕ ≤ 2π
推力矢量速率限制：
- 推力变化率：|Ṫ| ≤ Ṫ_max
- 角度变化率：|δ̇| ≤ δ̇_max, |ϕ̇| ≤ ϕ̇_max
扭矩限制：
- 体轴扭矩：‖τ‖_∞ ≤ τ_max
这些约束确保了推进系统的物理限制得到遵守，同时保持了问题的凸结构。
4. 状态约束与触发条件
4.1 全局状态约束
在整个飞行过程中必须满足的约束包括：
- 最大倾斜角：限制机体与当地垂直方向的偏离
- 最大角速度：防止超出姿态控制能力
- 最大速度：确保安全飞行包线
- 最小高度：避免与地面碰撞
这些约束通过双四元数形式表达，可以保持数学上的简洁性。
4.2 状态触发约束(STC)
当飞行器进入特定区域（如接近目标点时）激活的额外约束：
- 更严格的倾斜角限制
- 更小的角速度限制
- 视线角约束：确保传感器对准目标区域
- 速度限制：为精确着陆做准备
这些约束对于实现地形相对导航(TRN)和危险检测规避(HDA)至关重要。触发机制基于斜距测量，确保在关键阶段施加适当限制。
5. 序列凸优化(SeCO)框架
5.1 问题转化与求解流程
6-DoF着陆制导本质上是非凸最优控制问题。SeCO框架通过迭代求解一系列凸子问题来逼近原问题的解，主要步骤包括：
- 时间归一化：将自由终端时间问题转化为固定时间问题
- 线性化：在当前参考轨迹附近进行泰勒展开
- 离散化：采用一阶保持(FOH)参数化控制输入
- 凸优化：求解得到的凸子问题
- 迭代更新：用新解作为参考轨迹重复过程
5.2 关键技术改进
相比传统方法，我们实现了多项创新：
- 无逆传播：避免矩阵求逆，提高数值稳定性
- 虚拟状态法：保持动态可行性和约束形状
- 软信任区域：通过惩罚项而非硬约束控制线性化误差
- 预处理：自动调整求解器参数，优化收敛性
这些改进显著提升了算法的鲁棒性和计算效率。
6. 比例积分投影梯度(PIPG)求解器
6.1 一阶优化优势
PIPG作为一阶优化算法，相比传统内点法(IPM)具有明显优势：
- 仅需矩阵-向量乘法等简单线性代数运算
- 避免大型稀疏矩阵分解
- 易于热启动
- 代码量小（约1万行 vs IPM的60万行）
- 验证和确认(V&V)更简单
6.2 PIPG定制化实现
我们针对火箭着陆问题定制了PIPG求解器，主要特点包括：
- 利用问题结构完全避免稀疏矩阵操作
- 实现热启动和外推加速收敛
- 专为序列凸优化设计的投影操作
- 自动参数调整机制
实测表明，这种实现方式能在3秒内完成一次6-DoF轨迹优化，满足NASA对制导更新频率的要求。
7. 实际应用与测试结果
7.1 硬件在环测试
算法已在SPLICE项目的下降与着陆计算机(DLC)上进行了硬件在环测试，使用真实任务参数。测试验证了：
- 实时性能满足闭环控制要求
- 在各种初始条件下都能生成可行轨迹
- 约束满足情况良好
- 计算资源消耗在可接受范围内
7.2 飞行测试准备
该算法已被选为NASA SPLICE项目的候选6-DoF制导算法，并计划用于即将进行的闭环火箭着陆飞行测试。测试将验证算法在实际飞行环境中的表现，特别是：
- 对不确定性的鲁棒性
- 与导航、控制系统的集成
- 复杂约束下的性能
8. 实现注意事项与经验分享
8.1 数值稳定性处理
在实际实现中发现几个关键点：
- 双四元数归一化：必须定期重新归一化，防止数值误差累积
- 时间步长选择：过大会导致线性化误差大，过小增加计算量
- 约束松弛：适当松弛避免数值病态
8.2 性能优化技巧
- 热启动策略：利用上一周期解初始化当前优化
- 并行计算：将雅可比矩阵计算等任务并行化
- 代码优化：关键部分使用内联汇编
8.3 常见问题排查
- 发散问题：检查信任区域权重，必要时调整
- 收敛慢：尝试更好的初始猜测或调整PIPG参数
- 约束违反：检查线性化误差，减小步长
9. 未来发展方向
虽然当前算法已表现出良好性能，仍有改进空间：
- 连续时间约束满足：确保离散解在连续时间也满足约束
- 超参数自动调整：减少人工调参工作量
- 多阶段优化：将下降过程分为不同阶段分别优化
- 机器学习辅助：利用学习技术提高初始猜测质量
这些方向将进一步提升算法性能和适用性，为更复杂的探测任务提供支持。
