最近把“Apollo-11”这个项目收了尾趁着热乎劲把整套设计和踩坑记录整理一下。这个题目的灵感来自那个年代的登月制导计算机但我做的不是历史复刻而是用现在能买到的单片机思路重新实现一套简化的落月着陆制导控制仿真。说白了就是用一个几万行都不到的小项目把闭环制导、推力控制、传感器噪声这些核心环节串起来跑出一条从高度两千米到软着陆的完整轨迹。如果你是想接触飞行器控制、嵌入式实时系统或者单纯想理解当年那些“算力感人”的代码是怎么在极端条件下保持靠谱的这篇文章应该能给你一点参考。我会先讲整体架构为什么这样拆再讲制导律怎么设计然后是完整可复现代码最后是调参过程中遇到的经典问题。1. 项目定位与总体设计思路1.1 这个项目想解决什么问题着陆控制看起来简单无非是往地面落、快落地时减速。实际上要处理的问题很多水平速度要归零、垂直速度要控制在触地范围内、发动机推力有限还不能瞬间响应、燃料烧掉之后质量一直在变。动辄五六千字的技术文档写不清楚这些必须自己搭一套仿真跑一遍把每个环节暴露出来。这个项目定位于“半实物仿真前置验证”。我并没有真的去造一台落月着陆器而是先在PC上用Python完成了动力学模型和制导算法的验证把状态更新、推力计算、噪声注入这些模块都做成可以替换的结构。后面如果要做嵌入式版本直接把动力学模块换成传感器读数就行。这样做的最大好处是算法和硬件解耦问题可以在仿真阶段被隔离出来而不是等到飞起来才炸锅。为什么叫“Apollo-11”而不是“LanderSim”因为我在设计时确实对照了当年制导计算机的很多思路比如状态机驱动主循环、每个阶段都有明确入口条件和出口条件、关键数据用低速率周期刷新而不是靠循环里随意调用。这些设计习惯放到今天依然有价值。取名既是致敬也是提醒自己这个项目的核心不是华丽界面而是“在约束下不出错”。1.2 系统架构与模块边界整个项目划分为五个模块动力学模型、量测模型、制导律、控制分配、执行器模型。动力学模型负责根据推力和引力更新位置、速度、质量量测模型给速度和高度加噪声制导律根据当前状态算期望加速度控制分配把期望加速度换算成油门值执行器模型模拟推力建立时间和油门上下限。模块边界一定要清晰。我最初偷懒把制导和动力学写在同一个函数里结果每次改控制律都要担心会不会把物理模拟改坏。后来按上图拆开接缝处只传状态向量和命令向量调试效率明显提升。这也是从老代码里学到的教训数据和逻辑分离比什么都重要。主循环采用固定步长推进步长dt取0.05秒。这个值不是随手定的而是根据执行器响应速度选的。假如发动机推力建立时间大约0.2秒那么控制周期至少要比它快四倍才能近似认为推力命令是连续可变的。取0.05秒刚好满足同时仿真速度也足够快不需要为了跑完整个着陆过程等太久。2. 核心模块拆解与关键参数设计2.1 坐标系、状态量与动力学方程我使用二维平面模型忽略星球自转和轨道效应。状态量一共五个水平位置x、高度h、水平速度vx、垂直速度vy、质量m。坐标系定义为x向右为正h向上为正vy向上为正。着陆过程中高度逐渐降低vy是一个负值这个符号约定在写公式时很容易搞混建议注释里反复写清楚。动力学方程分两个维度。水平方向没有额外作用力只有倾斜推力分量的影响垂直方向受星球重力和推力垂直分量共同作用。公式如下$\dot{x} vx$$\dot{vx} (T \cdot \sin\theta) / m$$\dot{h} vy$$\dot{vy} (T \cdot \cos\theta) / m - g$这里的$\theta$是推力方向与垂直轴的夹角$g$近似取1.62m/s²。质量更新则依赖油门大小和比冲参数$\dot{m} -T / (I_{sp} \cdot g_0)$$g_0$取9.8m/s²。推力方向如果只在仿真里用二维角度后续换成三维会麻烦所以我在代码里直接保存推力在水平和垂直方向的分量而不是角度。2.2 制导律设计走廊公式加高位制动段制导律是这个项目最核心的部分。我采用的是“走廊公式”套“快速制动段”的组合策略先从物理上推导再写进代码。先看走廊公式。如果我们希望在高度为$h$的地方以当前速度$v$开始匀减速最终触地速度为$v_f$那么需要的减速加速度$a_{net}$是$$a_{net} \frac{v^2 - v_f^2}{2h}$$这个公式的前提是加速度恒定且方向与速度相反。需要注意的是这里的$v$指的是垂直方向上的速率向下为正而我在工程上习惯用vy表示向上为正代入时要取绝对值。算出来的$a_{net}$是净减速加速度也就是除了重力之外发动机额外提供的向上加速度。所以发动机实际需要产生的垂直加速度是$a_{net} g$。如果全程只用走廊公式会有一个隐蔽问题在高空、速度还不够大时算出来的$a_{net}$非常小发动机只需要悬停推力就能满足。但着陆器的速度是慢慢累积起来的如果不提前处理后面会发现无论如何都没法在高度的剩余距离内把速度压下来。这就是“制导饱和”问题。所以我在代码里增加了一个快速制动段当垂直速度低于某个阈值比如vy小于-8m/s或者走廊公式要求的$a_{net}$超过了当前可用过载的70%时直接全油门制动。等到速度被压到阈值附近再切回走廊公式做精细减速。这种分段策略在实际工程中也常见本质上是把“大偏差快消除”和“小偏差精细控制”分开处理。2.3 执行器与传感器模型执行器模型不是简单把油门命令直接加到动力学上。我模拟了一阶惯性环节也就是推力不会瞬间达到目标值而是按照一段时间常数逼近$$T_{current} (T_{target} - T_{current}) \cdot \frac{dt}{\tau}$$其中$\tau $取0.15秒。这个一阶延迟看似简单但它会显著影响制导稳定性尤其是走廊公式对加速度有精确要求时延迟会让实际减速滞后于期望减速必须通过限制最大允许的期望加速度来留出裕量。传感器模型里加白噪声。高度噪声标准差2米、速度噪声标准差0.5米每秒噪声通过一个均值滤波器做平滑。有一点要提醒滤波器会引入相位延迟如果滤波系数太强控制看到的“速度”会比真实速度滞后在快触地时反而容易炸。所以滤波系数要折中我试过把速度滤波深度从5改为10结果垂直速度在最后阶段出现了明显的相位滞后触地速度直接超了。3. 从零写一个可运行的着陆制导仿真3.1 仿真主循环的组织方式主循环采用状态机结构。整个着陆过程被切成三个大阶段快速制动段、精确减速段、触地判定。状态机的好处是每个阶段可以有自己独立的控制参数且阶段切换条件是显式的不会因为代码分支太多而乱。仿真流程如下初始化状态量包括初始高度、速度、质量、位置。进入主循环每个循环内先用量测模型伪造带噪声的高度和速度数据。根据当前状态和阶段计算期望加速度。把期望加速度换算成目标推力和油门值。通过执行器模型更新实际推力。根据实际推力更新位置、速度和质量。判断是否触地或者超过时间上限跳出循环。整个过程在代码里只有几十行但对应的物理含义非常密集。如果某个环节出错后续状态全部会偏所以调试时最好把关键变量打印出来或者画曲线不能只看最终是否成功。3.2 完整代码与参数说明为了方便复现我把参数集中放在一个配置块里仿真结束后输出任务结果。完整代码如下import numpy as np import matplotlib.pyplot as plt G_MOON 1.62 # 星球重力加速度 m/s^2 G0 9.8 # 地球重力加速度用于比冲计算 M_DRY 500.0 # 干质量 kg M_FUEL 200.0 # 燃料质量 kg T_MAX 5000.0 # 最大推力 N ISP 300.0 # 比冲 s VE ISP * G0 # 有效排气速度 m/s H0 2000.0 # 初始高度 m VX0 20.0 # 初始水平速度 m/s VY0 -30.0 # 初始垂直速度 m/s向下为负 X0 100.0 # 初始水平位置 m VF 1.0 # 期望触地垂速 m/s DT 0.05 # 仿真步长 s V_BRAKE_THRESH -8.0 # 快速制动段的速度阈值 A_CAP_RATIO 0.7 # 可使用过载比例 THRUST_TAU 0.15 # 推力建立时间常数 s H_SENSOR_NOISE 2.0 # 高度噪声标准差 V_SENSOR_NOISE 0.5 # 速度噪声标准差 FILTER_N 5 # 均值滤波深度 state { x: X0, h: H0, vx: VX0, vy: VY0, m: M_DRY M_FUEL, } thrust_now 0.0 phase brake filter_buf_h [H0] * FILTER_N filter_buf_v [VY0] * FILTER_N success False hist [] for step in range(int(120 / DT)): # 量测噪声 h_meas state[h] np.random.normal(0, H_SENSOR_NOISE) vy_meas state[vy] np.random.normal(0, V_SENSOR_NOISE) filter_buf_h.append(h_meas) filter_buf_v.append(vy_meas) filter_buf_h.pop(0) filter_buf_v.pop(0) h_f np.mean(filter_buf_h) vy_f np.mean(filter_buf_v) # 水平制导PD 归中 ax_cmd -0.05 * state[x] - 0.80 * state[vx] ax_cmd np.clip(ax_cmd, -2.0, 2.0) # 垂直制导分段 a_cap A_CAP_RATIO * T_MAX / state[m] - G_MOON a_req (vy_f**2 - VF**2) / (2 * max(h_f, 0.1)) if a_req 0: a_req 0.0 if (vy_f V_BRAKE_THRESH) or (a_req 0.7 * a_cap): phase brake a_net a_cap else: phase precision a_net min(a_req, a_cap * 0.8) # 期望合力 ay_cmd a_net G_MOON u_target (state[m] * np.sqrt(ax_cmd**2 ay_cmd**2)) / T_MAX u_target np.clip(u_target, 0.0, 1.0) # 推力一阶延迟 thrust_now (u_target * T_MAX - thrust_now) * (DT / THRUST_TAU) # 推力分量近似认为垂直为主 T_x thrust_now * ax_cmd / (np.sqrt(ax_cmd**2 ay_cmd**2) 1e-6) T_y thrust_now * ay_cmd / (np.sqrt(ax_cmd**2 ay_cmd**2) 1e-6) # 状态更新 state[vx] (T_x / state[m]) * DT state[vy] (T_y / state[m] - G_MOON) * DT state[x] state[vx] * DT state[h] state[vy] * DT # 质量更新 m_dot thrust_now / VE state[m] - m_dot * DT state[m] max(state[m], M_DRY) hist.append((step * DT, state[x], state[h], state[vx], state[vy], u_target, phase)) if state[h] 0.0: if abs(state[vy]) VF 0.5 and abs(state[vx]) 1.0: success True break print(success:, success) print(final h: %.2f % state[h]) print(final vy: %.2f % state[vy]) print(final vx: %.2f % state[vx]) print(fuel remaining: %.1f % (state[m] - M_DRY))代码里的核心参数都集中在最前面的配置块里你可以根据需要修改初始速度和高度。比如把初始高度改成500米那快速制动段可能来不及把速度压到理想区间最终就会触地失败。这就引出了下一节要讨论的判据和调参思路。3.3 仿真结果判定与调参方向只看“success”那个布尔值不够。我习惯把重心放在三个量上触地垂直速度、触地水平速度、剩余燃料。这三个量分别对应安全、安全和任务余量。如果触地垂直速度在1.5米每秒以内、水平速度在1米每秒以内基本可以认为着陆动作合格。跑通一次之后我开始扫描初始条件。固定H02000米不变把VY0从-10逐步变到-50观察成功率和剩余燃料曲线。结论很有代表性初始速度小于-20时全油门制动段贡献了主要的速度消减量初始速度大于-30则仍然有大量燃料剩余。如果初始速度比这更极端就需要在设计上增加推力上限或者减少着陆质量否则物理上就不可行。4. 常见问题与调试实录4.1 仿真发散的五个典型原因这个项目调试过程中我至少遇到过五种发散表现每种对应的原因都不同。第一种是数值发散表现为高度变成负数后继续变负、速度突破天际。原因是主循环步长太大或者角速度/加速度积分方式过于粗糙。第二种是振荡发散表现为油门值在0和1之间反复横跳最后状态上下震荡越来越厉害。这通常是水平PD增益过大、微分项放大了传感器噪声。第三种是缓慢漂移轨迹看起来正常但落地时间比预期长了非常多大部分燃料都耗在水平纠偏上多半是初始位置误差太大但水平增益太小。第四种是过早起火制动段过于激进导致触地之前燃料耗尽落地瞬间速度依然很大。第五种是物理参数不一致比如推力上限在计算时用了瞬时质量但执行器延迟导致实际推力跟不上出现长期“刹车不足”。针对发散的排查顺序我建议是先检查动力学公式符号有没有错再检查执行器模型是否稳定最后检查制导增益。很多人一上来就调PID参数结果方向完全反了白白浪费时间。4.2 推力估算与燃料预算推力估算这个环节最容易算错。很多人都直接从目标推力反推油门忽略质量随时间变化。我在项目中单独跑了一组燃料预算实验初始燃料200公斤最大推力5000牛比冲300秒。理论上燃料全部耗尽可以提供大约598千牛的冲量而这次任务整个着陆过程约需要30万牛·秒的冲量所以燃料富余量是很大的。但如果把水平纠偏任务加进去尤其是初始横向速度20米每秒、横向位移100米燃料消耗会比纯垂直着陆高出不少。实际仿真中成功的着陆剩余燃料大约在96到120公斤之间也就是说耗费了80多公斤燃料。这个数量级要心里有数。若你后续要扩展项目比如加障碍物规避、加入着陆腿接触模型燃料预算都会变化需要重新做一遍扫描。4.3 从老代码中学到的三个工程习惯我在做这个项目时特意参考了那个年代制导计算机的设计习惯有三个方面对我影响很大。第一是状态机驱动不会让代码运行到“意想不到”的分支里第二是数据周期刷新传感器数据和控制命令不在同一个循环里乱写而是有固定采样节奏第三是检查表文化控制律里的每个除法运算都加了分母保护每个数组访问都确认长度。其中最值得说的是第三点。自制仿真里经常出现除以h而h在触地后会变成负数甚至零不保护的话轻则NaN重则状态飞掉。我在代码里写max(h_f, 0.1)就是一种最朴素的保护。真实工程里还有更多细分的防御手段比如对速度方向做限制、对油门变化率做限制。这些细节单看都不起眼但叠加起来就是系统是否可靠的分水岭。4.4 噪声过滤与控制时延的平衡有一次仿真结果异常触地垂直速度总是比预期大我以为是控制律参数问题调了很久没有改善。后来把高度和速度的历史曲线调出来对比才意识到是均值滤波的深度太大了。均值滤波会带来群延迟深度5时延迟大约是0.1秒深度10时就变成0.2秒。控制周期只有0.05秒延迟4个周期足以让制导误差被放大。这个问题的典型特征是噪声明显减小但最终落地精度变差。解决办法不是只降低滤波深度而是同时调整期望加速度的上限。如果传感器噪声本身很严重选择降低期望加速度上限比滤波更有效因为控制本身变保守就不会因为噪声产生过大的油门摆动。以下是我整理的常见问题速查表方便你在调试时对照定位现象优先排查项调参建议高度曲线振荡发散步长dt是否过大减小到0.02秒再试油门在0和1之间抖动水平PD增益降低Kp增加微分滤波触地垂速偏大很多快速制动段切换是否过晚提高制动速度阈值燃料过早耗尽初始推力段过于激进降低最大可用过载比例噪声明显但落地很差滤波深度和相位延迟减小滤波深度降低期望加速度剩余燃料还有但触地失败触地判定高度太大把判定高度改到0.2米以下5. 一点个人体会这个项目做完之后我最深的感受是制导算法再漂亮也要在执行器、传感器、时序约束下才有意义。很多人上手做仿真第一反应是堆高级算法但真正让着陆器稳定落地的往往是那些不起眼的细节——符号约定、分母保护、相位延迟、油门限幅。如果你也想动手复现我建议先别急着上扩展卡尔曼滤波或者模型预测控制把最基础的走廊公式跑通再逐步加噪声、加延迟、加饱和约束。在这个基础上引入更多高级算法你才能看懂它们在解决什么具体问题。最后分享一个小技巧所有关键状态量每步都记录下来画成曲线一起看很多玄学问题其实一眼就能定位。相比靠猜和反复试错对着曲线检查状态是效率最高的排查方式。