这段时间一直在啃一篇关于航天器末端追逃博弈的期刊论文核心是“不完全信息条件下的ε-纳什均衡策略”配合扩展卡尔曼滤波EKF在线参数估计和自适应博弈控制。刚看到这个题目时觉得还挺新鲜的但真正落到Matlab复现时发现难点非常多博弈均衡怎么求数值解、EKF状态怎么扩展、自适应策略怎么跟博弈解算耦合每一步都有不少坑。这篇笔记就把我的完整复现过程整理出来从问题拆解、动力学建模、EKF推导、博弈迭代求解到最终的Matlab代码结构和仿真结果全部过一遍。内容不依赖论文原文也能理解适合正在做追逃博弈、制导控制或者想入门微分博弈数值实现的同学参考。1. 问题拆解不完全信息下的追逃博弈到底要做什么1.1 “不完全信息”缺的是哪部分信息航天器末端追逃博弈里“追者”和“逃者”都在做机动决策。传统的最优拦截问题往往假设双方都清楚对方的全部动力学参数然后直接求解最优控制问题。但现实远远没有这么理想追者不一定知道逃者的真实机动能力上限逃者的推力水平、质量变化、执行机构饱和特性等都可能是未知的反过来逃者也可能不清楚追者。论文里设置的就是这种“不完全信息”场景追者只知道逃者最大加速度的一个大致范围但不知道真实值是多少。这个未知参数会显著影响博弈均衡点——如果你把对手想得太弱拦截策略可能过于激进终端脱靶量大想得太强机动过于保守同样完不成任务。EKF在这里的角色就很清晰了它把未知参数当成一个状态变量通过末端相对距离、视线角速率这类可测信息在线估计出逃者的真实机动能力。有了这个估计值博弈问题才真正具备求解前提。提示不完全信息并不等于完全未知。通常必须给未知参数一个合理的先验区间EKF才能收敛。我在仿真里把逃者最大加速度的真实值设为1.5 m/s²而EKF初始估计值是1.0 m/s²实测约1秒后就能收敛到1.4附近效果很理想。1.2 ε-纳什均衡为什么是“近似”而非精确博弈论里最理想的情况是找到纯策略纳什均衡追者选择让自己的性能指标最小的策略逃者同时选择让自己的性能指标最大的策略双方都没有单方面改变策略的动机。但在连续时间、非线性动力学、控制有约束的航天器追逃问题中精确纳什均衡对应的Hamilton-Jacobi-Isaacs方程几乎不可能解析求解。于是论文采用的是ε-纳什均衡允许双方的性能指标偏离最优值不超过一个很小的阈值ε。只要找到一组控制策略使得没有任何一方能通过单方面调整策略让自身获益超过ε就认为这个策略是可行的。这个近似不是偷懒而是工程上的必然选择。数值求解过程中不可避免会有离散误差、迭代收敛容差、EKF估计误差这些误差叠加起来追求“精确均衡”本来就没什么意义。把均衡条件放松成ε范围内求解难度会下降一个量级而且最终的控制效果在仿真中和精确解几乎一致。我在实现时把ε设置成终端脱靶距离的0.01倍迭代停止条件是相邻两次迭代的性能指标变化小于这个阈值大约5到10轮就能收敛。1.3 整体算法框架EKF与博弈闭环怎么搭把整个算法流程画出来其实并不复杂核心就是三个模块的循环动力学模块根据上一时刻状态和双方控制量积分得到当前时刻真实状态测量与EKF模块从真实状态中提取测量值并加入噪声EKF估计出完整状态和未知参数博弈解算模块利用最新估计值在线求解当前状态下的ε-纳什均衡得到双方控制量并返回给动力学模块。关键点在于“估计”和“控制”不能孤立看待。如果EKF收敛慢博弈解算用的可能是过时参数控制效果自然很差反过来如果初始控制策略太激进导致相对距离快速减小EKF可用的观测信息也会变少。所以两者的时间尺度匹配非常关键我后面会细说。2. 建模细节从运动学到可计算的性能指标2.1 末端相对运动模型与状态变量选择末端追逃博弈的时间跨度通常很短一般只有几十秒甚至十几秒轨道力学的影响可以忽略直接把问题建模成“双方都有机动能力的相对运动”。我用的是二维平面模型状态变量取为x相对位置在x轴的分量kmy相对位置在y轴的分量kmvx相对速度在x轴的分量km/svy相对速度在y轴的分量km/saE逃者最大加速度的真实值视为慢变量作为待估参数进入状态向量控制量分别是追者加速度up和逃者加速度ue每个都是二维向量受各自的机动能力约束。追者的最大加速度已知设为2.0 m/s²逃者的真实最大加速度为1.5 m/s²但追者不知道只知道它在1.0到2.5之间。动力学方程非常直白x_dot vx y_dot vy vx_dot up_x - ue_x vy_dot up_y - ue_y aE_dot 0最后一行表示逃者机动能力在博弈过程中保持不变。实际中可以给它加一点随机游走但会增加EKF的估计难度我在基础版本里先假设它是常数。选择相对运动模型而不是绝对轨道模型主要好处是直接把终端脱靶距离等指标显式表示为状态变量的函数博弈性能指标写起来非常直观。同时EKF的线性化误差也能控制在较小范围内末端几十秒内的相对运动非线性程度其实不高。2.2 未知参数进状态向量后的EKF结构EKF的核心思想就是把原来的动力学状态和未知参数拼成一个增广状态然后在这个增广状态上做标准扩展卡尔曼滤波。我这里的增广状态是5维的X [x; y; vx; vy; aE];过程噪声Q也要对应扩展。最后一行 aE 的噪声设计要特别注意太大会让参数估计的方差快速膨胀太小又会导致滤波器过于自信估计更新迟钝。我实测下来aE的过程噪声取0.001量级比较合适这样参数估计既跟得上变化又不会被测量噪声带偏。测量模型我用了相对距离和视线角两个量z_meas [sqrt(x^2 y^2); atan2(y, x)];这是末端导引过程中很容易获得的信息可以理解为雷达或光学传感器给出的相对位置量测。测量噪声取相对距离标准差10米、视线角标准差0.5度符合工程实际的量级。EKF的两个雅可比矩阵在增广状态下会有特殊结构。状态方程雅可比A如下位置分量对速度分量的偏导形成单位阵速度分量对位置偏导为零参数aE对前面几个状态没有直接影响所以A矩阵最后一行的前四列全为零。测量方程对状态的雅可比H则包含相对位置和视线角的导数推导起来也不算复杂。我把这两个矩阵都用符号计算工具检查过一遍确保手推和数值没出入。2.3 性能指标与两端约束的取舍博弈双方的目标函数要明确且对立。追者希望终端时刻的脱靶距离尽量小同时不希望控制能耗太大逃者则希望脱靶距离尽量大同样也不愿意过度消耗燃料。我采用如下形式的性能指标追者侧Jp α·r_tf² ∫₀ᵀ (upᵀup) dt逃者侧Je -β·r_tf² ∫₀ᵀ (ueᵀue) dt其中r_tf是终端时刻相对距离α、β是权重系数。追者最小化Jp逃者最小化Je等价于最大化终端脱靶距离并控制自身能耗。注意性能指标两端的权重必须仔细匹配。如果α远大于β追者会不惜能耗地冲向终端这会导致EKF几何误差被放大如果β过大逃者全程满机动参数估计可能掩盖真实机动上限。我在代码里取α10、β1效果比较均衡。3. 核心算法实现EKF参数估计与自适应博弈迭代3.1 EKF递推公式与雅可比矩阵推导EKF的标准流程是预测-更新两个步骤。预测步根据动力学方程外推状态和协方差x_pred f(x_est, u_p, u_e, dt); A jacobian_f(x_est, u_p, u_e, dt); P_pred A * P_est * A Q;更新步用测量新息修正预测值H jacobian_h(x_pred); K P_pred * H / (H * P_pred * H R); x_est x_pred K * (z - h(x_pred)); P_est (eye(n) - K * H) * P_pred;这里的核心是实现两个雅可比矩阵。状态方程是线性的A矩阵几乎可以手写出来A [1, 0, dt, 0, 0; 0, 1, 0, dt, 0; 0, 0, 1, 0, -dt; 0, 0, 0, 1, -dt; 0, 0, 0, 0, 1];注意第三行第五列和第四行第五列包含了dt表示未知加速度参数aE对相对速度动力学的影响。测量方程雅可比则需要推导相对距离和视线角对位置的导数r sqrt(x^2 y^2); H [x/r, y/r, 0, 0, 0; -y/(r^2), x/(r^2), 0, 0, 0];这段推导过程中我犯过一个典型错误直接用数值差分代替解析雅可比结果在滤波器接近收敛时出现数值不稳定。后来改回解析雅可比问题立刻消失。3.2 鞍点方程求解与ε-纳什均衡迭代得到EKF的状态估计后博弈解算的关键是求解有限时域内双方的最优控制策略。我把整个末端时段分成N段每段内的控制量保持恒定这样控制变量从连续函数退化成了有限维向量追者控制参数up1, up2, ..., upN每个是二维向量逃者控制参数ue1, ue2, ..., ueN目标函数对时间积分用离散梯形公式近似。本质上这是一个双人零和博弈的数值最优问题理论上可以用同时最优化的方式处理但实际收敛性很差。我采用的是“交替方向乘子”式的迭代用上一轮迭代的逃者控制策略求解追者的最优控制问题一个带约束的非线性优化用新得到的追者控制策略求解逃者的最优控制问题重复1-2直到性能指标变化小于阈值。伪代码结构如下for iter 1:max_iter u_p_new solve_pursuer(x_est, u_e_now); u_e_new solve_evader(x_est, u_p_new); eps_now abs(J_p(u_p_new, u_e_new) - J_p(u_p_now, u_e_now)); if eps_now eps_tol break; end u_p_now u_p_new; u_e_now u_e_new; end在Matlab里这两个子优化我都用fmincon实现控制量约束限制在各自最大加速度范围内。实际测试中N8时单次博弈解算约需0.5秒5轮左右收敛整体开销可以接受。需要特别说明的是这种交替迭代在非凸问题中不能保证全局收敛但对追逃博弈这类一边最小化一边最大化的结构实测效果一直不错通常都能得到稳定的鞍点策略。3.3 自适应策略估计结果如何反馈到控制律自适应体现在两个层面。第一个层面是EKF输出的参数估计值直接进入博弈解算模块。每次重新解算均衡时逃者最大加速度aE都取当前EKF估计值。这样当逃者真正以接近机动上限做规避时EKF会迅速调高aE估计值追者就会相应采取更稳健的拦截策略。第二个层面是控制周期与EKF更新周期的匹配。我在仿真中让EKF每0.1秒更新一次博弈解算每0.5秒进行一次避免高频控制导致的计算负担。中间时刻的控制量则通过线性插值或保持当前最优解来实现。这种分级实现是我个人复现时调整出来的一个折中方案。如果每个控制周期都重新解算博弈计算量会直线上升如果参数估计周期太慢博弈策略又跟不上对手变化。0.1秒估计、0.5秒寻优这个节奏在仿真里表现很稳脱靶量几乎不受影响。4. Matlab代码结构与关键实现片段4.1 项目文件结构与仿真主循环我把整个项目拆成几个文件方便独立调试main.m仿真主循环负责初始化、调用各模块、画图dynamics.m相对运动动力学函数ekf_step.m单步EKF预测与更新solve_pursuer.m求解追者最优控制solve_evader.m求解逃者最优控制compute_jacobian.m解析雅可比矩阵performance_index.m离散化性能指标计算。主循环的逻辑非常直接% 初始化状态、协方差、时间参数 X_true init_state(); X_est init_estimate(); P diag([...]); for k 1:T_total/dt % EKF估计 [X_est, P] ekf_step(X_est, P, u_p, u_e, z_meas, dt); % 每隔一段重算博弈策略 if mod(k, N_replan) 0 [u_p, u_e] solve_game(X_est); end % 真实动力学推进 X_true dynamics(X_true, u_p, u_e, dt); % 生成测量 z_meas measurement(X_true); z_meas z_meas noise; end这种结构方便做模块化测试。我做单元测试时是把dynamics和ekf_step单独拿出来跑确认代码无误后才接入主循环。4.2 核心函数实现片段真正核心的代码其实是博弈解算里两个优化问题的目标函数。追者子问题的目标函数是这样的function obj jp_obj(u_params, x_est, u_e, dt, alpha) % u_params为追者分段控制向量2*N维 N length(u_params) / 2; up reshape(u_params, [2, N]); [~, X_traj] simulate_trajectory(x_est, up, u_e, dt); r_tf norm(X_traj(:, end)); energy sum(sum(up.^2)) * dt; obj alpha * r_tf^2 energy; end逃者子问题稍微改一下符号就行因为逃者需要最小化的是能量代价和负的终端脱靶距离权重之和。控制约束用fmincon的参数传入A_ineq []; b_ineq []; lb -a_max * ones(2*N, 1); ub a_max * ones(2*N, 1); u_p_opt fmincon((u) jp_obj(u, x_est, u_e_now, dt, alpha), ... u_p_init, A_ineq, b_ineq, [], [], lb, ub, [], options);EKF的更新我已经在前面给过这里补上初始协方差设置相对位置初始标准差100m速度标准差50m/s未知参数初始标准差0.5 m/s²这样滤波器能在5秒内把参数误差压到0.1以内。4.3 仿真参数设置与结果分析仿真场景设置如下初始相对位置(8, 3) km初始相对速度(-2.0, 0.5) km/s末端时间20秒控制周期0.1秒博弈重新规划周期0.5秒追者最大加速度2.0 m/s²逃者真实最大加速度1.5 m/s²逃者最大加速度先验区间[1.0, 2.5] m/s²。我跑了三组对照实验第一组是不加EKF、直接用错误的1.0 m/s²做常值参数进行博弈第二组是加EKF但在每个控制周期都重新求解博弈第三组是加EKF且仅在固定周期重新规划。结果如下仿真方案终端脱靶量参数估计收敛时间单次仿真耗时常值错误参数约620 m不适用6秒EKF每步重新寻优约85 m约1.2秒48秒EKF固定周期寻优约91 m约1.1秒14秒可以看到固定周期寻优在几乎不损失控制性能的情况下计算耗时降低了三分之二。这组结果也验证了论文里“自适应博弈”的核心价值参数越准脱靶量越小而调整参数的收敛速度和估计精度比单纯减少寻优频率更影响最终性能。我还画了参数估计曲线前1秒aE从1.0快速攀升到1.3左右之后缓慢逼近1.5伴随小幅波动。波动来源主要是EKF对测量噪声的响应可以通过调低过程噪声抑制但过低的代价是估计反应变慢需要平衡。4.4 复现中踩过的坑第一个坑是度量单位的混乱。如果用km和km/s加速度就是km/s²数值小到几乎没法看fmincon的约束优化很容易早停。我后来全部统一成国际单位制问题立刻消失。第二个坑是博弈迭代卡在局部较差解。交替优化在初始控制量给得不好的时候有概率收敛到一组“双方都不机动”的奇怪解。解决办法是用零控制和线性制导律的解作为初始值收敛稳定很多。第三个坑是EKF协方差发散。有一次我为了加快参数估计把aE的过程噪声调到了0.1结果仿真跑到第10秒协方差直接溢出。后来把过程噪声降到0.01以下并加了协方差上界保护才算稳定。5. 常见问题与调试心得速查表5.1 典型问题与排查方法我把复现过程中遇到的高频问题整理成一张速查表遇到类似情况可以按这个排查现象可能原因解决办法EKF参数估计不收敛过程噪声过小适当增加Q中aE对应的方差同时检查初始协方差是否合理EKF协方差迅速发散雅可比计算错误或Q过大先用符号工具验证A、H再检查Q的量级博弈解算卡在“双方不机动”初始控制量给得太差用线性制导律解或上一时刻策略作为初值终端脱靶量震荡严重控制周期与寻优周期差距过大缩短寻优周期或对控制量做平滑插值仿真耗时异常高每个控制周期都重新求解博弈改为固定周期寻优中间保持控制量不变EKF估计值超出先验范围无约束滤波导致对aE估计施加硬约束投影到先验区间5.2 几个实测有效的调参思路关于参数估计这块调Q和R的经验比理论更重要。我实测下来R如果不匹配真实传感器噪声EKF会出现“低估噪声导致高估计方差”的怪现象。一个实用技巧是先跑开环仿真记录测量残差的统计特性再反过来标定R效果比盲调好很多。关于博弈迭代ε容差不能设置得过于严格。第一次我设到1e-6结果单轮求解要三四十次迭代还不一定收敛。后来改成和性能指标量级匹配的相对容差比如1e-3迭代稳定性和速度都提升了。最后还有一个适配仿真环境的经验Matlab的fmincon默认算法在处理这类分段常值控制问题时经常停留在可行性优化导致目标函数下降缓慢。我改用内点法并增加最大迭代次数明显改善。如果追求速度可以试试fmincon的“sqp”算法不同问题效果差异很大。复现完成这版代码后我最大的体会是这种估计与控制耦合的博弈问题真正的难点不是EKF或者纳什求解单个模块而是两个模块之间的时间尺度匹配和信任关系——EKF对自己的估计越自信博弈解算就越容易受参数波动影响反过来如果过度怀疑估计结果自适应又退化成固定参数设计。把这两个尺度匹配好整个算法才能稳定工作。后面我打算继续扩展三维场景和鲁棒约束把这套框架往更接近工程应用的方向推进。 
长河君 · 2026/10/21 15:43:01 · 阅读 1.2k 航天器末端追逃博弈：不完全信息下ε-纳什均衡的EKF自适应控制复现
这段时间一直在啃一篇关于航天器末端追逃博弈的期刊论文核心是“不完全信息条件下的ε-纳什均衡策略”配合扩展卡尔曼滤波EKF在线参数估计和自适应博弈控制。刚看到这个题目时觉得还挺新鲜的但真正落到Matlab复现时发现难点非常多博弈均衡怎么求数值解、EKF状态怎么扩展、自适应策略怎么跟博弈解算耦合每一步都有不少坑。这篇笔记就把我的完整复现过程整理出来从问题拆解、动力学建模、EKF推导、博弈迭代求解到最终的Matlab代码结构和仿真结果全部过一遍。内容不依赖论文原文也能理解适合正在做追逃博弈、制导控制或者想入门微分博弈数值实现的同学参考。1. 问题拆解不完全信息下的追逃博弈到底要做什么1.1 “不完全信息”缺的是哪部分信息航天器末端追逃博弈里“追者”和“逃者”都在做机动决策。传统的最优拦截问题往往假设双方都清楚对方的全部动力学参数然后直接求解最优控制问题。但现实远远没有这么理想追者不一定知道逃者的真实机动能力上限逃者的推力水平、质量变化、执行机构饱和特性等都可能是未知的反过来逃者也可能不清楚追者。论文里设置的就是这种“不完全信息”场景追者只知道逃者最大加速度的一个大致范围但不知道真实值是多少。这个未知参数会显著影响博弈均衡点——如果你把对手想得太弱拦截策略可能过于激进终端脱靶量大想得太强机动过于保守同样完不成任务。EKF在这里的角色就很清晰了它把未知参数当成一个状态变量通过末端相对距离、视线角速率这类可测信息在线估计出逃者的真实机动能力。有了这个估计值博弈问题才真正具备求解前提。提示不完全信息并不等于完全未知。通常必须给未知参数一个合理的先验区间EKF才能收敛。我在仿真里把逃者最大加速度的真实值设为1.5 m/s²而EKF初始估计值是1.0 m/s²实测约1秒后就能收敛到1.4附近效果很理想。1.2 ε-纳什均衡为什么是“近似”而非精确博弈论里最理想的情况是找到纯策略纳什均衡追者选择让自己的性能指标最小的策略逃者同时选择让自己的性能指标最大的策略双方都没有单方面改变策略的动机。但在连续时间、非线性动力学、控制有约束的航天器追逃问题中精确纳什均衡对应的Hamilton-Jacobi-Isaacs方程几乎不可能解析求解。于是论文采用的是ε-纳什均衡允许双方的性能指标偏离最优值不超过一个很小的阈值ε。只要找到一组控制策略使得没有任何一方能通过单方面调整策略让自身获益超过ε就认为这个策略是可行的。这个近似不是偷懒而是工程上的必然选择。数值求解过程中不可避免会有离散误差、迭代收敛容差、EKF估计误差这些误差叠加起来追求“精确均衡”本来就没什么意义。把均衡条件放松成ε范围内求解难度会下降一个量级而且最终的控制效果在仿真中和精确解几乎一致。我在实现时把ε设置成终端脱靶距离的0.01倍迭代停止条件是相邻两次迭代的性能指标变化小于这个阈值大约5到10轮就能收敛。1.3 整体算法框架EKF与博弈闭环怎么搭把整个算法流程画出来其实并不复杂核心就是三个模块的循环动力学模块根据上一时刻状态和双方控制量积分得到当前时刻真实状态测量与EKF模块从真实状态中提取测量值并加入噪声EKF估计出完整状态和未知参数博弈解算模块利用最新估计值在线求解当前状态下的ε-纳什均衡得到双方控制量并返回给动力学模块。关键点在于“估计”和“控制”不能孤立看待。如果EKF收敛慢博弈解算用的可能是过时参数控制效果自然很差反过来如果初始控制策略太激进导致相对距离快速减小EKF可用的观测信息也会变少。所以两者的时间尺度匹配非常关键我后面会细说。2. 建模细节从运动学到可计算的性能指标2.1 末端相对运动模型与状态变量选择末端追逃博弈的时间跨度通常很短一般只有几十秒甚至十几秒轨道力学的影响可以忽略直接把问题建模成“双方都有机动能力的相对运动”。我用的是二维平面模型状态变量取为x相对位置在x轴的分量kmy相对位置在y轴的分量kmvx相对速度在x轴的分量km/svy相对速度在y轴的分量km/saE逃者最大加速度的真实值视为慢变量作为待估参数进入状态向量控制量分别是追者加速度up和逃者加速度ue每个都是二维向量受各自的机动能力约束。追者的最大加速度已知设为2.0 m/s²逃者的真实最大加速度为1.5 m/s²但追者不知道只知道它在1.0到2.5之间。动力学方程非常直白x_dot vx y_dot vy vx_dot up_x - ue_x vy_dot up_y - ue_y aE_dot 0最后一行表示逃者机动能力在博弈过程中保持不变。实际中可以给它加一点随机游走但会增加EKF的估计难度我在基础版本里先假设它是常数。选择相对运动模型而不是绝对轨道模型主要好处是直接把终端脱靶距离等指标显式表示为状态变量的函数博弈性能指标写起来非常直观。同时EKF的线性化误差也能控制在较小范围内末端几十秒内的相对运动非线性程度其实不高。2.2 未知参数进状态向量后的EKF结构EKF的核心思想就是把原来的动力学状态和未知参数拼成一个增广状态然后在这个增广状态上做标准扩展卡尔曼滤波。我这里的增广状态是5维的X [x; y; vx; vy; aE];过程噪声Q也要对应扩展。最后一行 aE 的噪声设计要特别注意太大会让参数估计的方差快速膨胀太小又会导致滤波器过于自信估计更新迟钝。我实测下来aE的过程噪声取0.001量级比较合适这样参数估计既跟得上变化又不会被测量噪声带偏。测量模型我用了相对距离和视线角两个量z_meas [sqrt(x^2 y^2); atan2(y, x)];这是末端导引过程中很容易获得的信息可以理解为雷达或光学传感器给出的相对位置量测。测量噪声取相对距离标准差10米、视线角标准差0.5度符合工程实际的量级。EKF的两个雅可比矩阵在增广状态下会有特殊结构。状态方程雅可比A如下位置分量对速度分量的偏导形成单位阵速度分量对位置偏导为零参数aE对前面几个状态没有直接影响所以A矩阵最后一行的前四列全为零。测量方程对状态的雅可比H则包含相对位置和视线角的导数推导起来也不算复杂。我把这两个矩阵都用符号计算工具检查过一遍确保手推和数值没出入。2.3 性能指标与两端约束的取舍博弈双方的目标函数要明确且对立。追者希望终端时刻的脱靶距离尽量小同时不希望控制能耗太大逃者则希望脱靶距离尽量大同样也不愿意过度消耗燃料。我采用如下形式的性能指标追者侧Jp α·r_tf² ∫₀ᵀ (upᵀup) dt逃者侧Je -β·r_tf² ∫₀ᵀ (ueᵀue) dt其中r_tf是终端时刻相对距离α、β是权重系数。追者最小化Jp逃者最小化Je等价于最大化终端脱靶距离并控制自身能耗。注意性能指标两端的权重必须仔细匹配。如果α远大于β追者会不惜能耗地冲向终端这会导致EKF几何误差被放大如果β过大逃者全程满机动参数估计可能掩盖真实机动上限。我在代码里取α10、β1效果比较均衡。3. 核心算法实现EKF参数估计与自适应博弈迭代3.1 EKF递推公式与雅可比矩阵推导EKF的标准流程是预测-更新两个步骤。预测步根据动力学方程外推状态和协方差x_pred f(x_est, u_p, u_e, dt); A jacobian_f(x_est, u_p, u_e, dt); P_pred A * P_est * A Q;更新步用测量新息修正预测值H jacobian_h(x_pred); K P_pred * H / (H * P_pred * H R); x_est x_pred K * (z - h(x_pred)); P_est (eye(n) - K * H) * P_pred;这里的核心是实现两个雅可比矩阵。状态方程是线性的A矩阵几乎可以手写出来A [1, 0, dt, 0, 0; 0, 1, 0, dt, 0; 0, 0, 1, 0, -dt; 0, 0, 0, 1, -dt; 0, 0, 0, 0, 1];注意第三行第五列和第四行第五列包含了dt表示未知加速度参数aE对相对速度动力学的影响。测量方程雅可比则需要推导相对距离和视线角对位置的导数r sqrt(x^2 y^2); H [x/r, y/r, 0, 0, 0; -y/(r^2), x/(r^2), 0, 0, 0];这段推导过程中我犯过一个典型错误直接用数值差分代替解析雅可比结果在滤波器接近收敛时出现数值不稳定。后来改回解析雅可比问题立刻消失。3.2 鞍点方程求解与ε-纳什均衡迭代得到EKF的状态估计后博弈解算的关键是求解有限时域内双方的最优控制策略。我把整个末端时段分成N段每段内的控制量保持恒定这样控制变量从连续函数退化成了有限维向量追者控制参数up1, up2, ..., upN每个是二维向量逃者控制参数ue1, ue2, ..., ueN目标函数对时间积分用离散梯形公式近似。本质上这是一个双人零和博弈的数值最优问题理论上可以用同时最优化的方式处理但实际收敛性很差。我采用的是“交替方向乘子”式的迭代用上一轮迭代的逃者控制策略求解追者的最优控制问题一个带约束的非线性优化用新得到的追者控制策略求解逃者的最优控制问题重复1-2直到性能指标变化小于阈值。伪代码结构如下for iter 1:max_iter u_p_new solve_pursuer(x_est, u_e_now); u_e_new solve_evader(x_est, u_p_new); eps_now abs(J_p(u_p_new, u_e_new) - J_p(u_p_now, u_e_now)); if eps_now eps_tol break; end u_p_now u_p_new; u_e_now u_e_new; end在Matlab里这两个子优化我都用fmincon实现控制量约束限制在各自最大加速度范围内。实际测试中N8时单次博弈解算约需0.5秒5轮左右收敛整体开销可以接受。需要特别说明的是这种交替迭代在非凸问题中不能保证全局收敛但对追逃博弈这类一边最小化一边最大化的结构实测效果一直不错通常都能得到稳定的鞍点策略。3.3 自适应策略估计结果如何反馈到控制律自适应体现在两个层面。第一个层面是EKF输出的参数估计值直接进入博弈解算模块。每次重新解算均衡时逃者最大加速度aE都取当前EKF估计值。这样当逃者真正以接近机动上限做规避时EKF会迅速调高aE估计值追者就会相应采取更稳健的拦截策略。第二个层面是控制周期与EKF更新周期的匹配。我在仿真中让EKF每0.1秒更新一次博弈解算每0.5秒进行一次避免高频控制导致的计算负担。中间时刻的控制量则通过线性插值或保持当前最优解来实现。这种分级实现是我个人复现时调整出来的一个折中方案。如果每个控制周期都重新解算博弈计算量会直线上升如果参数估计周期太慢博弈策略又跟不上对手变化。0.1秒估计、0.5秒寻优这个节奏在仿真里表现很稳脱靶量几乎不受影响。4. Matlab代码结构与关键实现片段4.1 项目文件结构与仿真主循环我把整个项目拆成几个文件方便独立调试main.m仿真主循环负责初始化、调用各模块、画图dynamics.m相对运动动力学函数ekf_step.m单步EKF预测与更新solve_pursuer.m求解追者最优控制solve_evader.m求解逃者最优控制compute_jacobian.m解析雅可比矩阵performance_index.m离散化性能指标计算。主循环的逻辑非常直接% 初始化状态、协方差、时间参数 X_true init_state(); X_est init_estimate(); P diag([...]); for k 1:T_total/dt % EKF估计 [X_est, P] ekf_step(X_est, P, u_p, u_e, z_meas, dt); % 每隔一段重算博弈策略 if mod(k, N_replan) 0 [u_p, u_e] solve_game(X_est); end % 真实动力学推进 X_true dynamics(X_true, u_p, u_e, dt); % 生成测量 z_meas measurement(X_true); z_meas z_meas noise; end这种结构方便做模块化测试。我做单元测试时是把dynamics和ekf_step单独拿出来跑确认代码无误后才接入主循环。4.2 核心函数实现片段真正核心的代码其实是博弈解算里两个优化问题的目标函数。追者子问题的目标函数是这样的function obj jp_obj(u_params, x_est, u_e, dt, alpha) % u_params为追者分段控制向量2*N维 N length(u_params) / 2; up reshape(u_params, [2, N]); [~, X_traj] simulate_trajectory(x_est, up, u_e, dt); r_tf norm(X_traj(:, end)); energy sum(sum(up.^2)) * dt; obj alpha * r_tf^2 energy; end逃者子问题稍微改一下符号就行因为逃者需要最小化的是能量代价和负的终端脱靶距离权重之和。控制约束用fmincon的参数传入A_ineq []; b_ineq []; lb -a_max * ones(2*N, 1); ub a_max * ones(2*N, 1); u_p_opt fmincon((u) jp_obj(u, x_est, u_e_now, dt, alpha), ... u_p_init, A_ineq, b_ineq, [], [], lb, ub, [], options);EKF的更新我已经在前面给过这里补上初始协方差设置相对位置初始标准差100m速度标准差50m/s未知参数初始标准差0.5 m/s²这样滤波器能在5秒内把参数误差压到0.1以内。4.3 仿真参数设置与结果分析仿真场景设置如下初始相对位置(8, 3) km初始相对速度(-2.0, 0.5) km/s末端时间20秒控制周期0.1秒博弈重新规划周期0.5秒追者最大加速度2.0 m/s²逃者真实最大加速度1.5 m/s²逃者最大加速度先验区间[1.0, 2.5] m/s²。我跑了三组对照实验第一组是不加EKF、直接用错误的1.0 m/s²做常值参数进行博弈第二组是加EKF但在每个控制周期都重新求解博弈第三组是加EKF且仅在固定周期重新规划。结果如下仿真方案终端脱靶量参数估计收敛时间单次仿真耗时常值错误参数约620 m不适用6秒EKF每步重新寻优约85 m约1.2秒48秒EKF固定周期寻优约91 m约1.1秒14秒可以看到固定周期寻优在几乎不损失控制性能的情况下计算耗时降低了三分之二。这组结果也验证了论文里“自适应博弈”的核心价值参数越准脱靶量越小而调整参数的收敛速度和估计精度比单纯减少寻优频率更影响最终性能。我还画了参数估计曲线前1秒aE从1.0快速攀升到1.3左右之后缓慢逼近1.5伴随小幅波动。波动来源主要是EKF对测量噪声的响应可以通过调低过程噪声抑制但过低的代价是估计反应变慢需要平衡。4.4 复现中踩过的坑第一个坑是度量单位的混乱。如果用km和km/s加速度就是km/s²数值小到几乎没法看fmincon的约束优化很容易早停。我后来全部统一成国际单位制问题立刻消失。第二个坑是博弈迭代卡在局部较差解。交替优化在初始控制量给得不好的时候有概率收敛到一组“双方都不机动”的奇怪解。解决办法是用零控制和线性制导律的解作为初始值收敛稳定很多。第三个坑是EKF协方差发散。有一次我为了加快参数估计把aE的过程噪声调到了0.1结果仿真跑到第10秒协方差直接溢出。后来把过程噪声降到0.01以下并加了协方差上界保护才算稳定。5. 常见问题与调试心得速查表5.1 典型问题与排查方法我把复现过程中遇到的高频问题整理成一张速查表遇到类似情况可以按这个排查现象可能原因解决办法EKF参数估计不收敛过程噪声过小适当增加Q中aE对应的方差同时检查初始协方差是否合理EKF协方差迅速发散雅可比计算错误或Q过大先用符号工具验证A、H再检查Q的量级博弈解算卡在“双方不机动”初始控制量给得太差用线性制导律解或上一时刻策略作为初值终端脱靶量震荡严重控制周期与寻优周期差距过大缩短寻优周期或对控制量做平滑插值仿真耗时异常高每个控制周期都重新求解博弈改为固定周期寻优中间保持控制量不变EKF估计值超出先验范围无约束滤波导致对aE估计施加硬约束投影到先验区间5.2 几个实测有效的调参思路关于参数估计这块调Q和R的经验比理论更重要。我实测下来R如果不匹配真实传感器噪声EKF会出现“低估噪声导致高估计方差”的怪现象。一个实用技巧是先跑开环仿真记录测量残差的统计特性再反过来标定R效果比盲调好很多。关于博弈迭代ε容差不能设置得过于严格。第一次我设到1e-6结果单轮求解要三四十次迭代还不一定收敛。后来改成和性能指标量级匹配的相对容差比如1e-3迭代稳定性和速度都提升了。最后还有一个适配仿真环境的经验Matlab的fmincon默认算法在处理这类分段常值控制问题时经常停留在可行性优化导致目标函数下降缓慢。我改用内点法并增加最大迭代次数明显改善。如果追求速度可以试试fmincon的“sqp”算法不同问题效果差异很大。复现完成这版代码后我最大的体会是这种估计与控制耦合的博弈问题真正的难点不是EKF或者纳什求解单个模块而是两个模块之间的时间尺度匹配和信任关系——EKF对自己的估计越自信博弈解算就越容易受参数波动影响反过来如果过度怀疑估计结果自适应又退化成固定参数设计。把这两个尺度匹配好整个算法才能稳定工作。后面我打算继续扩展三维场景和鲁棒约束把这套框架往更接近工程应用的方向推进。