1. 先想清楚为什么要做“攻击时间控制的动态逆三维制导律”搞过制导控制仿真的人应该都有体会攻击时间控制这个需求一旦落到三维空间里麻烦程度不是二维加一个z通道那么简单。我之前按照二维思路先做俯仰通道、再做偏航通道结果误差收敛到一半轨迹绕着目标转圈攻击时间怎么都压不齐。后来重新回到动态逆这条路上把三维相对运动统一到一个框架里反解过载指令代码才算真正跑通。这篇文章就是把这套攻击时间控制的动态逆三维制导律的建模思路、控制律推导、源代码结构和调试经验完整梳理出来。源码不是那种只给一个空壳的伪代码而是能在Python里直接跑出三维轨迹、时间误差曲线和过载曲线的完整示例。适合正在做协同制导、飞行器末端导引或者对非线性控制在制导场景落地感兴趣的人。哪怕你只是想把动态逆方法用在自己的控制对象上这里面关于“期望误差动态设计”和“反解控制量”的思路也值得参考。1.1 三维制导不能简单拆成两个二维通道很多人在三维制导里习惯性地把问题分解成水平面和垂直面两个独立平面每个平面各用一个二维制导律。这个思路在比例导引这类线性化程度高的问题里勉强能用因为两个通道的视线角速率之间存在弱耦合。但一旦引入攻击时间约束问题就变了攻击时间是一个全局量它同时受到两个通道机动的影响你没法说“水平通道管时间、垂直通道管角度”。我踩过的坑就是两个通道各自设计了一个攻击时间误差反馈项结果两个反馈项在一个方向上打架指令发出去以后不是叠加而是抵消时间误差半天不收敛。正确做法是把三维视线几何写成一个整体。弹目相对位置矢量、相对速度矢量、视线方向这三个量本身就包含了所有通道信息。控制量是飞行器速度方向上的法向过载它在三维空间里是一个垂直于速度方向的二维向量。这个二维向量既要保证视线角速率尽快归零又要让剩余飞行时间跟随期望值变化。两个约束条件刚好对应两个控制自由度这才是三维问题真正好处理的地方。1.2 动态逆在这类问题里解决的是“如何反解控制量”传统PID在设计制导律时大多是直接对误差做比例或比例微分运算然后人为调增益。这种方式对模型不敏感但问题是要调的参数太多而且很难解释“为什么这个增益能同时满足时间约束和角度约束”。动态逆的思路是反过来的先明确你想要的误差动态是什么比如“误差以一阶指数形式收敛到零”然后直接对这个误差动态求导通过模型关系反解出控制量。放在攻击时间控制这个场景里误差就是当前剩余飞行时间与期望剩余飞行时间之差。我们希望这个误差按一阶线性动态收敛于是对误差求导误差导数里会自然出现法向过载在视线方向上的投影。把期望动态代进去就能解出这个投影应该等于多少。这个过程本质上就是动态逆不需要反复调PID增益只需要保证模型关系正确。当然动态逆的前提是模型关系足够准确。如果目标机动模型完全未知反解出来的控制量会有偏差。所以我在代码里先假设目标静止、飞行器速率恒定把问题核心放在方法验证上。等你把动态逆的整个链路跑通了再去引入目标机动估计或者加入干扰观测器思路是一样的只是模型要重推一遍。1.3 这套源码是什么、适合什么人看这套源码不是某个工具箱的封装函数而是一个完整的仿真主程序。你把文件拿过去设置好初始位置、初始速度方向、期望攻击时间Run一下就能看到三维轨迹、攻击时间误差曲线、视线角速率曲线和法向过载曲线。核心制导律部分只有二十几行剩下的都是数值积分、坐标变换和画图。适合看的人有两类。一类是做制导控制研究的学生或工程师可以把这套代码作为基线替换成自己的模型或者在上面加入落角约束、目标机动估计。另一类是刚接触动态逆控制的读者可以通过这个具体例子理解“期望动态设计”到底是怎么落地的而不是停留在“非线性系统线性化”这个抽象概念上。2. 建模视线角、剩余飞行时间与法向过载之间的关系要写出能跑的代码第一步不是写制导律而是把仿真模型的状态量和运动方程理清楚。我在这套代码里没有用复杂的球坐标系方程而是直接在惯性坐标系下用向量运算效果一样但代码可读性高很多也方便后续扩展到变速度或者带目标机动的场景。2.1 坐标系和状态量定义我用的惯性坐标系是北-天-东记为x、y、z。飞行器位置是 (P_m [x_m, y_m, z_m])速度是 (V_m V s)其中 (s) 是速度方向单位向量(V) 是速率大小。目标位置固定记为 (P_t)。弹目相对位置向量是[ R P_t - P_m ]相对距离 (r |R|)。从飞行器指向目标的视线单位向量是[ e R / r ]速度方向与视线方向之间的夹角记为 (\eta)满足[ \cos\eta s \cdot e ]这就是整个建模的核心几何量。注意这里的 (e) 是“从导弹指向目标”的单位向量不是从目标指向导弹很多人在做向量运算时容易搞反导致视线角速率符号出错。后面所有误差推导都基于这个方向约定。动态逆需要知道控制量如何影响状态。飞行器速率不变控制量是法向过载向量 (a_p)它始终垂直于速度方向。速度方向单位向量的导数为[ \dot{s} a_p / V ]这个式子把过载指令和速度方向变化联系起来了是后面推导误差动态的关键。2.2 剩余飞行时间的几何估计剩余飞行时间 (t_{go}) 不能直接用 (r/V)那只在飞行器速度方向正对目标时成立。更通用的估计是[ t_{go} \frac{r}{V \cos\eta} ]这个公式的物理含义是假设飞行器保持当前速度方向不变那么视线距离 (r) 以 (V \cos\eta) 的速率缩短。考虑到制导律会持续调整速度方向这个估计在实际控制过程中仍然足够稳定而且它给出了一条非常干净的误差导数关系后面能直接导出动态逆控制律。期望剩余飞行时间由攻击时间决定[ t_{go,d} t_d - t ]其中 (t_d) 是期望攻击时间也就是从仿真开始到命中的期望总时长。攻击时间误差定义为[ e_t t_{go} - t_{go,d} ]控制目标就是让 (e_t) 尽快收敛到零。注意这里没有直接用误差的积分项原因是一阶误差动态就能满足大部分仿真需求而且参数少便于分析。2.3 控制输入为什么只能是法向过载现实中飞行器的推力方向和大小都有限制但在制导律设计这条线上我们通常只考虑法向过载作为控制输入因为它直接改变速度方向而速度大小由推进系统单独控制。法向过载 (a_p) 是一个垂直于速度方向的三维向量但在每个时刻它其实只有两个自由度。为什么只有两个自由度因为约束条件是[ a_p \cdot s 0 ]所以 (a_p) 只能落在以速度方向为法线的平面内。这个平面内的任意一个向量都能分解成“在视线方向上的投影”和“与视线垂直的方向上的分量”。攻击时间控制主要用的是第一个投影视线角速率控制用的是第二个分量。后面设计控制律时就是用这两个约束分别去解这两个控制自由度。3. 动态逆思路把误差动态设计成一阶线性系统再反解控制量动态逆这个名词听起来很高端其实放到这个例子里非常直观我们不直接设计“过载应该等于多少”而是先规定“攻击时间误差应该怎么收敛”再根据模型反推过载。整个设计过程只涉及一个一阶微分方程和一次二维线性代数求解。3.1 为什么不用PID直接调攻击时间误差和过载指令之间的关系并不是“误差大就猛打舵”那么简单。过载指令会同时改变视线角速率和剩余飞行时间而且这种关系是状态相关的。如果你用PID误差大时输出一个很大的横向过载可能反而让飞行器绕一个大圈攻击时间误差不仅没减小还增大了。这就是反馈极性随状态变化的问题PID在这种非线性、耦合强的问题上非常容易调出震荡。动态逆绕开了这个问题。它先建立一个误差导数与过载之间的解析关系然后直接求解“为了让误差导数等于 (-k e_t)过载应该取多少”。只要模型关系没写错整个闭环的动态在理论上就是干净的一阶惯性系统不存在极性随状态翻转的问题。3.2 期望误差动态与动态逆表达式我们期望的攻击时间误差动态是[ \dot{e}_t -k e_t ]其中 (k 0) 是误差收敛速率。这个式子意味着误差会指数衰减不会超调也不会震荡。k越大收敛越快但要求过载越大。接下来的关键是把 (\dot{e}_t) 展开成包含控制量 (a_p) 的表达式。从剩余飞行时间估计式出发通过链式求导可以得到[ \dot{e}_t \tan^2\eta - \frac{a_p \cdot e}{V \cos^2\eta} ]这个式子是整个动态逆制导律的支点。它告诉我们攻击时间误差的导数由两部分组成一部分是当前几何状态带来的自然趋势另一部分才是控制量贡献。把期望误差动态代进来[ \tan^2\eta - \frac{a_p \cdot e}{V \cos^2\eta} -k e_t ]反解得到 (a_p) 在视线方向上的投影必须满足[ a_p \cdot e V \sin^2\eta k V \cos^2\eta \cdot e_t ]这个公式看着有点复杂但代码里就一行。它本质上是在告诉你当飞行器当前偏离目标方向越明显自然趋势越会把误差搞大你需要用多大的过载去抵消当攻击时间误差不为零时你需要额外附加多少过载去修正。3.3 与比例导引如何协同动态逆项只规定了 (a_p) 在视线方向上的投影没有规定垂直于视线的分量。如果不加其他约束飞行器可能在时间上收敛了但视线角速率一直很大最后轨迹是一条奇怪的弧线。所以需要把比例导引项也纳入进来。比例导引的经典形式是让法向过载与视线角速率成正比方向垂直于视线。在三维空间里我们可以提取视线单位向量的转动角速度向量 (\omega)然后让过载在“垂直于视线”的平面内沿视线转动方向产生阻尼。把这个约束和动态逆解出来的投影约束放到一起就是一个二维线性方程组[ \begin{bmatrix} e_{\perp,x} e_{\perp,y} e_{\perp,z}\ n_x n_y n_z \end{bmatrix} a_p \begin{bmatrix} A \ B \end{bmatrix} ]其中 (e_{\perp}) 是视线方向在过载平面内的投影方向(n) 是比例导引要求的响应方向(A) 是动态逆解出的投影值(B) 是比例导引项生成的附加过载系数。这个方程组是欠定还是恰好定因为 (a_p) 本身就是二维向量在过载平面内而这里有两个方程所以刚好确定唯一解。代码里用最小二乘或者直接消元都能解。4. 核心控制律推导从误差动态到加速度指令这一章把上面的推导再细化一下把每一步的数学来源说清楚。因为即使你把源代码拿走了不理解这个推导过程一旦想改约束或者换模型还是会无从下手。4.1 误差微分方程的推导回到剩余飞行时间估计[ t_{go} \frac{r}{V \cos\eta} ]对它求时间导数。目标静止时相对距离变化率为[ \dot{r} -V \cos\eta ]所以第一项贡献是[ \frac{d}{dt}\left(\frac{r}{V\cos\eta}\right)_{\text{不考虑}\eta变化} \frac{-V\cos\eta}{V\cos\eta} -1 ]这里的 (-1) 非常重要它代表每过一秒剩余飞行时间自然少一秒。期望剩余飞行时间 (t_{go,d}t_d-t) 的导数也是 (-1)所以原生态误差导数里这部分互相抵消只剩与 (\dot{\eta}) 有关的项。接下来求 (\dot{\eta}) 与控制量 (a_p) 的关系。速度方向导数是 (a_p/V)视线方向 (e) 的变化由相对运动和视线转动共同决定。经过向量运算得到[ \dot{\eta} \frac{V\sin\eta}{r} - \frac{a_p \cdot e}{V\sin\eta} ]代入 (t_{go}) 的导数并化简就得到前面那个误差微分方程。整个过程如果自己推一次会花点时间但推完以后对“为什么动态逆项长这样”会非常清楚。4.2 两个控制通道的约束在过载平面里两个约束分别是第一个约束攻击时间误差按期望动态收敛[ a_p \cdot e_{\perp} V \sin^2\eta k V \cos^2\eta , e_t ]注意这里的 (e_{\perp}) 是视线方向在过载平面内的投影向量。因为 (a_p) 垂直于速度方向所以 (a_p \cdot e) 实际上等于 (a_p \cdot e_{\perp})如果不做这个投影直接写 (a_p \cdot e) 会混入速度方向的分量导致无解。第二个约束视线角速率按比例导引衰减[ a_p \cdot n N V |\omega| ]其中 (N) 是导航增益(\omega) 是视线方向转动角速度向量(n) 是垂直于视线的转动响应方向。实际操作中我会用当前时刻视线方向的差分来估计 (\omega)然后取它归一化后的方向作为 (n)。这样即使视线角速率很小也能保持方向一致性。这两个约束写在矩阵里就是完整的控制律核心。代码里我直接构造一个 (2 \times 3) 矩阵然后用最小二乘法求解 (a_p)。因为约束本身保证了 (a_p) 在过载平面内虽然矩阵是 (2 \times 3)但最小二乘结果自动满足垂直于速度方向的约束并且是唯一解。4.3 指令限幅与退化处理动态逆反解出的过载指令理论上可能非常大尤其是攻击时间误差初始值很大、或者飞行器初始指向几乎背对目标时。如果没有限幅仿真里的飞行器速度方向会疯狂翻转数值积分直接发散。我在这里加了两层保护。第一层是对过载模长限幅把 (a_p) 的模长限制到预设最大值比如 (20g)。限幅之后继续保持原方向这比简单切断某一维要平滑得多。第二层是对 (e_t) 的归一化处理当剩余飞行时间误差很大时用 (\tanh) 函数做一个软限幅避免过载指令直接冲到上百g。另外一个容易忽略的退化情况是 (\eta) 接近0或者接近 (\pi/2)。(\eta0) 时飞行器正对目标此时攻击时间控制项自然为0只保留比例导引项(\eta) 接近 (\pi/2) 时飞行器航向几乎垂直于视线此时 (t_{go}) 估计式里的余弦项接近0误差导数会剧烈变化需要把过载限幅压低防止指令震荡。5. 源代码结构与核心函数实现源码整体结构并不复杂核心就是一个时间步进循环里面包含状态更新、制导律计算、数据记录三部分。我用Python写numpy负责向量运算matplotlib负责画图。下面把关键代码展开讲一下。5.1 仿真框架与数据结构我先把飞行器状态放到一个类里class MissileState: def __init__(self, pos, vel, speed): self.pos np.array(pos, dtypefloat) # 惯性系位置 self.vel np.array(vel, dtypefloat) # 速度向量 self.speed speed # 速率大小恒定 self.target np.array([0, 0, 0], dtypefloat) # 目标位置 def velocity_dir(self): return self.vel / np.linalg.norm(self.vel) def line_of_sight(self): r_vec self.target - self.pos r np.linalg.norm(r_vec) return r_vec / r, r用向量表达的好处是直观后续任何坐标变换都不需要手动写一堆三角函数。缺点是向量运算比标量积分稍微慢一点但在这个仿真里完全不是瓶颈。仿真主循环如下dt 0.001 t 0.0 tdes 12.0 # 期望攻击时间 state MissileState([0, 0, 0], [270, 30, 0], 300) history [] while t tdes 0.5: # 计算视线和剩余时间误差 e_los, r state.line_of_sight() s state.velocity_dir() cos_eta np.clip(np.dot(s, e_los), 0.1, 1.0) eta np.arccos(cos_eta) tgo r / (state.speed * cos_eta) et tgo - (tdes - t) # 计算制导指令 a_p compute_guidance(state, e_los, s, eta, cos_eta, et, tgo, t) # 更新速度方向 state.vel state.vel a_p * dt state.vel state.vel / np.linalg.norm(state.vel) * state.speed state.pos state.pos state.vel * dt history.append([t, r, tgo, et, np.linalg.norm(a_p), *state.pos]) t dt注意这里更新速度时先加上速度方向的变化量再归一化回到恒定速率。这个“投影回到原速率”的方式等价于严格在切向量方向做旋转二阶精度足够。5.2 制导循环核心代码完整的制导指令函数是下面这个compute_guidancedef compute_guidance(state, e_los, s, eta, cos_eta, et, tgo, t): V state.speed # 动态逆时间误差期望动态为 e_dot -k * e k 1.5 A_des V * (np.sin(eta) ** 2 k * cos_eta ** 2 * et) # 视线方向在过载平面内的投影 e_perp e_los - cos_eta * s e_perp_norm np.linalg.norm(e_perp) if e_perp_norm 1e-6: e_perp np.zeros(3) else: e_perp e_perp / e_perp_norm # 比例导引项用视线方向差分估计视线角速度 omega_los estimate_los_rate(state, e_los, dt) B_des 3.0 * V * np.linalg.norm(omega_los) if np.linalg.norm(omega_los) 1e-9: n_dir omega_los / np.linalg.norm(omega_los) else: n_dir np.cross(s, e_los) n_dir n_dir / (np.linalg.norm(n_dir) 1e-9) # 构造约束矩阵并求解 A np.array([e_perp, n_dir]) b np.array([A_des, B_des]) a_p, _, _, _ np.linalg.lstsq(A, b, rcondNone) # 法向过载保证垂直于速度 a_p a_p - np.dot(a_p, s) * s # 限幅 max_a 60.0 norm_a np.linalg.norm(a_p) if norm_a max_a: a_p a_p / norm_a * max_a return a_p这个函数里有两个细节需要注意。第一我用最小二乘求两个约束下的三维向量但解出来的向量可能带有微小速度方向分量所以又在后面做了一次投影确保 (a_p \cdot s0)。第二比例导引方向 (n_dir) 在视线角速率为零时用速度方向叉乘视线方向的备选值防止矩阵退化。estimate_los_rate我直接在上一个控制周期保存的视线方向基础上做中心差分再乘以一个平滑系数。更严谨的做法是引入卡尔曼滤波但在这个教学代码里中心差分加滤波足够稳定。以下是视线角速率估计函数def estimate_los_rate(state, e_los, dt): global e_los_prev if e_los_prev is None: e_los_prev e_los.copy() omega np.cross(e_los_prev, e_los) / dt omega omega / (np.linalg.norm(e_los) * np.linalg.norm(e_los_prev) 1e-9) e_los_prev e_los.copy() return omega这里用两个时刻视线向量的叉积除以时间步长来近似角速度向量。注意叉积向量方向正好是视线转动轴垂直于两个视线向量符合比例导引需要的“视线转动方向”。5.3 记录曲线与三维轨迹输出数据记录我直接放到主循环的history列表里每个时间步存一行。仿真结束后一次性转换成 numpy 数组然后分别画图。三维轨迹图画最简单的方式是matplotlib的Axes3D.plot把所有位置点连成线。为了看清最终命中点我在目标位置画一个红色五角星。这个图能直观看到飞行器是否绕圈、是否从侧向接近目标是判断制导律行为最直接的线索。时间误差曲线我画两个一个是 (t_{go}) 与期望剩余时间 (t_d - t) 的对比另一个是两者之差即 (e_t)。前者能看出误差是从哪个方向逼近的后者能看出收敛速度是否满足动态逆设计参数 (k)。理想情况下(e_t) 会从初始误差指数据衰减到0。过载曲线画的是法向过载模长随时间变化。这个曲线能直观反映动态逆有没有在某些时刻输出过大指令。如果看到过载频繁顶到限幅值说明 (k) 设得太大或者初始攻击时间误差给得太离谱。6. 仿真结果解读攻击时间误差、轨迹与过载仿真代码跑通以后不能只看“最后命中了”就结束。制导律设计得好不好要从误差收敛过程、过载特性、轨迹几何三个角度一起看。6.1 典型算例参数我设置的典型算例是这样的飞行器初始位置在 ([0, 0, 0])初始速度方向为 ([270, 30, 0]) 对应的单位向量乘以速率300也就是偏北偏上飞。目标位置在 ([4000, 2000, 1500])。直线飞行距离约4700米匀速直线飞行时间大概15.7秒。我把期望攻击时间设为12秒比直线飞行时间短逼着飞行器走一条更直的路线如果想看攻击时间延长的效果也可以设成18秒那飞行器就会绕路。初始速度方向并不是正对着目标所以初始 (\eta) 有大约30度。这给了动态逆一个很典型的初始误差场景时间误差存在视线角速率也存在两个约束同时要满足。6.2 结果曲线分析在我这组参数下攻击时间误差 (e_t) 从最初的负值快速收敛到零附近大概1.5秒之后进入稳态。这个收敛速度跟动态逆参数 (k1.5) 对应的时间常数是吻合的。动态逆的优点在这里体现得很明显误差收敛过程干净利落没有超调没有振荡。三维轨迹从图上看是一条先平滑转弯、再直线逼近的曲线。因为期望攻击时间小于直线飞行时间所以飞行器开始时会先把速度方向往目标方向拉尽量少绕路等视线角速率衰减到接近零后基本就是比例导引段在起主要作用。过载曲线在初始阶段有一个短暂的峰值大约30多没有碰到限幅值60。这是因为初始 (\eta) 大、视线角速率也大动态逆需要同时做两件事一是快速减小时间误差二是把视线角速率压下来。过了初始段之后过载迅速回落到个位数末端接近零。这说明控制律没有在后半段反复折腾轨迹过程很平稳。6.3 参数改动后的敏感性我把 (k) 从1.5改成3.0以后攻击时间误差收敛速度明显加快但初始过载峰值直接翻倍接近50。这说明 (k) 不能无脑调大它直接控制的是你愿意花多大过载去修正时间误差。如果你的飞行器过载限制只有20g那 (k) 可能要降到0.5左右代价是攻击时间误差收敛慢初始阶段飞行器会走一段较长的过渡轨迹。把最大过载从60改成30后仿真仍然能收敛但收敛时间变长攻击时间误差会有一个明显的平台期。这个现象也解释了为什么很多文献里强调“攻击时间控制律必须考虑过载约束”因为动态逆给出的是无约束解而实际系统一定有饱和饱和本质上会改变期望误差动态需要单独做抗饱和处理。7. 调试过程中最容易被坑的三个地方代码能跑通是第一步但我在调试这套三维制导律时踩了很多不起眼的坑。有些坑光看公式根本发现不了必须把仿真曲线调出来才能定位。这里把最重要的三个坑记录一下希望能帮你少走点弯路。7.1 视线角速率差分噪声大第一次跑的时候比例导引项完全是在抖过载曲线像梳子一样。定位了半天问题出在estimate_los_rate里直接用相邻两帧视线方向做叉积差分。视线方向本身变化非常平滑但叉积结果受浮点误差影响很大尤其在视线方向几乎不变时噪声会被除法放大。解决办法有两个一是把时间步长适当加大不要用0.0001这种过小的步长否则差分噪声反而更大二是对估计出的角速度做一阶低通滤波把高频抖掉。我最后用的是一阶滤波系数0.15左右既不引入明显延迟又能把抖动静音。如果你想更精确可以在视线角速度估计里加卡尔曼滤波但对这个教学仿真来说低通滤波足够了。7.2 剩余飞行时间估计在末端发散攻击时间误差曲线在快要命中目标时突然跳上去这是一个非常隐蔽的问题。原因是 (r) 趋向于0时剩余飞行时间估计 (t_{go}r/(V\cos\eta)) 在分母有限的情况下也会趋向于0但在数值上弹目相对距离小于某个阈值时视线方向向量会跳变(\cos\eta) 的计算不稳定导致 (t_{go}) 出现毛刺。我在代码里加了一个末端判定当 (t_{go} 0.2) 秒时直接把动态逆攻击时间误差项置零只保留比例导引项。这样既不影响末端命中精度又能避免时间误差在收敛到零附近时继续产生过载指令从而避免末端振荡。7.3 坐标系绕序与向量方向不一致三维制导的坑有相当一部分出在坐标系的定义和方向约定上。我在最初实现时视线方向用的是“由目标指向弹体”的方向然后剩余飞行时间估计里的 (\cos\eta) 也按这个方向算结果动态逆反解出来的过载方向正好反了攻击时间误差不但没有收敛反而越来越大。后来我把所有公式统一成“由弹体指向目标”的视线方向再重新推了一遍误差导数代码行为立刻正常。这个教训是写代码之前一定要把坐标系、向量方向写在注释第一行。不要相信自己的记忆力三周后回头看你真的会忘了到底哪边是正方向。如果你打算在这套代码上做扩展比如加入落角约束或者目标机动建议先把第4章的误差微分方程重新推一遍特别是目标速度不为零时( \dot{r} ) 里要多出目标速度项整个动态逆补偿项都会变化。模型一变动态逆的补偿项就必须跟着变这是这套方法的核心也是它比盲目调PID更可控的原因。 攻击时间控制的动态逆三维制导律：建模、推导与Python实现
1. 先想清楚为什么要做“攻击时间控制的动态逆三维制导律”搞过制导控制仿真的人应该都有体会攻击时间控制这个需求一旦落到三维空间里麻烦程度不是二维加一个z通道那么简单。我之前按照二维思路先做俯仰通道、再做偏航通道结果误差收敛到一半轨迹绕着目标转圈攻击时间怎么都压不齐。后来重新回到动态逆这条路上把三维相对运动统一到一个框架里反解过载指令代码才算真正跑通。这篇文章就是把这套攻击时间控制的动态逆三维制导律的建模思路、控制律推导、源代码结构和调试经验完整梳理出来。源码不是那种只给一个空壳的伪代码而是能在Python里直接跑出三维轨迹、时间误差曲线和过载曲线的完整示例。适合正在做协同制导、飞行器末端导引或者对非线性控制在制导场景落地感兴趣的人。哪怕你只是想把动态逆方法用在自己的控制对象上这里面关于“期望误差动态设计”和“反解控制量”的思路也值得参考。1.1 三维制导不能简单拆成两个二维通道很多人在三维制导里习惯性地把问题分解成水平面和垂直面两个独立平面每个平面各用一个二维制导律。这个思路在比例导引这类线性化程度高的问题里勉强能用因为两个通道的视线角速率之间存在弱耦合。但一旦引入攻击时间约束问题就变了攻击时间是一个全局量它同时受到两个通道机动的影响你没法说“水平通道管时间、垂直通道管角度”。我踩过的坑就是两个通道各自设计了一个攻击时间误差反馈项结果两个反馈项在一个方向上打架指令发出去以后不是叠加而是抵消时间误差半天不收敛。正确做法是把三维视线几何写成一个整体。弹目相对位置矢量、相对速度矢量、视线方向这三个量本身就包含了所有通道信息。控制量是飞行器速度方向上的法向过载它在三维空间里是一个垂直于速度方向的二维向量。这个二维向量既要保证视线角速率尽快归零又要让剩余飞行时间跟随期望值变化。两个约束条件刚好对应两个控制自由度这才是三维问题真正好处理的地方。1.2 动态逆在这类问题里解决的是“如何反解控制量”传统PID在设计制导律时大多是直接对误差做比例或比例微分运算然后人为调增益。这种方式对模型不敏感但问题是要调的参数太多而且很难解释“为什么这个增益能同时满足时间约束和角度约束”。动态逆的思路是反过来的先明确你想要的误差动态是什么比如“误差以一阶指数形式收敛到零”然后直接对这个误差动态求导通过模型关系反解出控制量。放在攻击时间控制这个场景里误差就是当前剩余飞行时间与期望剩余飞行时间之差。我们希望这个误差按一阶线性动态收敛于是对误差求导误差导数里会自然出现法向过载在视线方向上的投影。把期望动态代进去就能解出这个投影应该等于多少。这个过程本质上就是动态逆不需要反复调PID增益只需要保证模型关系正确。当然动态逆的前提是模型关系足够准确。如果目标机动模型完全未知反解出来的控制量会有偏差。所以我在代码里先假设目标静止、飞行器速率恒定把问题核心放在方法验证上。等你把动态逆的整个链路跑通了再去引入目标机动估计或者加入干扰观测器思路是一样的只是模型要重推一遍。1.3 这套源码是什么、适合什么人看这套源码不是某个工具箱的封装函数而是一个完整的仿真主程序。你把文件拿过去设置好初始位置、初始速度方向、期望攻击时间Run一下就能看到三维轨迹、攻击时间误差曲线、视线角速率曲线和法向过载曲线。核心制导律部分只有二十几行剩下的都是数值积分、坐标变换和画图。适合看的人有两类。一类是做制导控制研究的学生或工程师可以把这套代码作为基线替换成自己的模型或者在上面加入落角约束、目标机动估计。另一类是刚接触动态逆控制的读者可以通过这个具体例子理解“期望动态设计”到底是怎么落地的而不是停留在“非线性系统线性化”这个抽象概念上。2. 建模视线角、剩余飞行时间与法向过载之间的关系要写出能跑的代码第一步不是写制导律而是把仿真模型的状态量和运动方程理清楚。我在这套代码里没有用复杂的球坐标系方程而是直接在惯性坐标系下用向量运算效果一样但代码可读性高很多也方便后续扩展到变速度或者带目标机动的场景。2.1 坐标系和状态量定义我用的惯性坐标系是北-天-东记为x、y、z。飞行器位置是 (P_m [x_m, y_m, z_m])速度是 (V_m V s)其中 (s) 是速度方向单位向量(V) 是速率大小。目标位置固定记为 (P_t)。弹目相对位置向量是[ R P_t - P_m ]相对距离 (r |R|)。从飞行器指向目标的视线单位向量是[ e R / r ]速度方向与视线方向之间的夹角记为 (\eta)满足[ \cos\eta s \cdot e ]这就是整个建模的核心几何量。注意这里的 (e) 是“从导弹指向目标”的单位向量不是从目标指向导弹很多人在做向量运算时容易搞反导致视线角速率符号出错。后面所有误差推导都基于这个方向约定。动态逆需要知道控制量如何影响状态。飞行器速率不变控制量是法向过载向量 (a_p)它始终垂直于速度方向。速度方向单位向量的导数为[ \dot{s} a_p / V ]这个式子把过载指令和速度方向变化联系起来了是后面推导误差动态的关键。2.2 剩余飞行时间的几何估计剩余飞行时间 (t_{go}) 不能直接用 (r/V)那只在飞行器速度方向正对目标时成立。更通用的估计是[ t_{go} \frac{r}{V \cos\eta} ]这个公式的物理含义是假设飞行器保持当前速度方向不变那么视线距离 (r) 以 (V \cos\eta) 的速率缩短。考虑到制导律会持续调整速度方向这个估计在实际控制过程中仍然足够稳定而且它给出了一条非常干净的误差导数关系后面能直接导出动态逆控制律。期望剩余飞行时间由攻击时间决定[ t_{go,d} t_d - t ]其中 (t_d) 是期望攻击时间也就是从仿真开始到命中的期望总时长。攻击时间误差定义为[ e_t t_{go} - t_{go,d} ]控制目标就是让 (e_t) 尽快收敛到零。注意这里没有直接用误差的积分项原因是一阶误差动态就能满足大部分仿真需求而且参数少便于分析。2.3 控制输入为什么只能是法向过载现实中飞行器的推力方向和大小都有限制但在制导律设计这条线上我们通常只考虑法向过载作为控制输入因为它直接改变速度方向而速度大小由推进系统单独控制。法向过载 (a_p) 是一个垂直于速度方向的三维向量但在每个时刻它其实只有两个自由度。为什么只有两个自由度因为约束条件是[ a_p \cdot s 0 ]所以 (a_p) 只能落在以速度方向为法线的平面内。这个平面内的任意一个向量都能分解成“在视线方向上的投影”和“与视线垂直的方向上的分量”。攻击时间控制主要用的是第一个投影视线角速率控制用的是第二个分量。后面设计控制律时就是用这两个约束分别去解这两个控制自由度。3. 动态逆思路把误差动态设计成一阶线性系统再反解控制量动态逆这个名词听起来很高端其实放到这个例子里非常直观我们不直接设计“过载应该等于多少”而是先规定“攻击时间误差应该怎么收敛”再根据模型反推过载。整个设计过程只涉及一个一阶微分方程和一次二维线性代数求解。3.1 为什么不用PID直接调攻击时间误差和过载指令之间的关系并不是“误差大就猛打舵”那么简单。过载指令会同时改变视线角速率和剩余飞行时间而且这种关系是状态相关的。如果你用PID误差大时输出一个很大的横向过载可能反而让飞行器绕一个大圈攻击时间误差不仅没减小还增大了。这就是反馈极性随状态变化的问题PID在这种非线性、耦合强的问题上非常容易调出震荡。动态逆绕开了这个问题。它先建立一个误差导数与过载之间的解析关系然后直接求解“为了让误差导数等于 (-k e_t)过载应该取多少”。只要模型关系没写错整个闭环的动态在理论上就是干净的一阶惯性系统不存在极性随状态翻转的问题。3.2 期望误差动态与动态逆表达式我们期望的攻击时间误差动态是[ \dot{e}_t -k e_t ]其中 (k 0) 是误差收敛速率。这个式子意味着误差会指数衰减不会超调也不会震荡。k越大收敛越快但要求过载越大。接下来的关键是把 (\dot{e}_t) 展开成包含控制量 (a_p) 的表达式。从剩余飞行时间估计式出发通过链式求导可以得到[ \dot{e}_t \tan^2\eta - \frac{a_p \cdot e}{V \cos^2\eta} ]这个式子是整个动态逆制导律的支点。它告诉我们攻击时间误差的导数由两部分组成一部分是当前几何状态带来的自然趋势另一部分才是控制量贡献。把期望误差动态代进来[ \tan^2\eta - \frac{a_p \cdot e}{V \cos^2\eta} -k e_t ]反解得到 (a_p) 在视线方向上的投影必须满足[ a_p \cdot e V \sin^2\eta k V \cos^2\eta \cdot e_t ]这个公式看着有点复杂但代码里就一行。它本质上是在告诉你当飞行器当前偏离目标方向越明显自然趋势越会把误差搞大你需要用多大的过载去抵消当攻击时间误差不为零时你需要额外附加多少过载去修正。3.3 与比例导引如何协同动态逆项只规定了 (a_p) 在视线方向上的投影没有规定垂直于视线的分量。如果不加其他约束飞行器可能在时间上收敛了但视线角速率一直很大最后轨迹是一条奇怪的弧线。所以需要把比例导引项也纳入进来。比例导引的经典形式是让法向过载与视线角速率成正比方向垂直于视线。在三维空间里我们可以提取视线单位向量的转动角速度向量 (\omega)然后让过载在“垂直于视线”的平面内沿视线转动方向产生阻尼。把这个约束和动态逆解出来的投影约束放到一起就是一个二维线性方程组[ \begin{bmatrix} e_{\perp,x} e_{\perp,y} e_{\perp,z}\ n_x n_y n_z \end{bmatrix} a_p \begin{bmatrix} A \ B \end{bmatrix} ]其中 (e_{\perp}) 是视线方向在过载平面内的投影方向(n) 是比例导引要求的响应方向(A) 是动态逆解出的投影值(B) 是比例导引项生成的附加过载系数。这个方程组是欠定还是恰好定因为 (a_p) 本身就是二维向量在过载平面内而这里有两个方程所以刚好确定唯一解。代码里用最小二乘或者直接消元都能解。4. 核心控制律推导从误差动态到加速度指令这一章把上面的推导再细化一下把每一步的数学来源说清楚。因为即使你把源代码拿走了不理解这个推导过程一旦想改约束或者换模型还是会无从下手。4.1 误差微分方程的推导回到剩余飞行时间估计[ t_{go} \frac{r}{V \cos\eta} ]对它求时间导数。目标静止时相对距离变化率为[ \dot{r} -V \cos\eta ]所以第一项贡献是[ \frac{d}{dt}\left(\frac{r}{V\cos\eta}\right)_{\text{不考虑}\eta变化} \frac{-V\cos\eta}{V\cos\eta} -1 ]这里的 (-1) 非常重要它代表每过一秒剩余飞行时间自然少一秒。期望剩余飞行时间 (t_{go,d}t_d-t) 的导数也是 (-1)所以原生态误差导数里这部分互相抵消只剩与 (\dot{\eta}) 有关的项。接下来求 (\dot{\eta}) 与控制量 (a_p) 的关系。速度方向导数是 (a_p/V)视线方向 (e) 的变化由相对运动和视线转动共同决定。经过向量运算得到[ \dot{\eta} \frac{V\sin\eta}{r} - \frac{a_p \cdot e}{V\sin\eta} ]代入 (t_{go}) 的导数并化简就得到前面那个误差微分方程。整个过程如果自己推一次会花点时间但推完以后对“为什么动态逆项长这样”会非常清楚。4.2 两个控制通道的约束在过载平面里两个约束分别是第一个约束攻击时间误差按期望动态收敛[ a_p \cdot e_{\perp} V \sin^2\eta k V \cos^2\eta , e_t ]注意这里的 (e_{\perp}) 是视线方向在过载平面内的投影向量。因为 (a_p) 垂直于速度方向所以 (a_p \cdot e) 实际上等于 (a_p \cdot e_{\perp})如果不做这个投影直接写 (a_p \cdot e) 会混入速度方向的分量导致无解。第二个约束视线角速率按比例导引衰减[ a_p \cdot n N V |\omega| ]其中 (N) 是导航增益(\omega) 是视线方向转动角速度向量(n) 是垂直于视线的转动响应方向。实际操作中我会用当前时刻视线方向的差分来估计 (\omega)然后取它归一化后的方向作为 (n)。这样即使视线角速率很小也能保持方向一致性。这两个约束写在矩阵里就是完整的控制律核心。代码里我直接构造一个 (2 \times 3) 矩阵然后用最小二乘法求解 (a_p)。因为约束本身保证了 (a_p) 在过载平面内虽然矩阵是 (2 \times 3)但最小二乘结果自动满足垂直于速度方向的约束并且是唯一解。4.3 指令限幅与退化处理动态逆反解出的过载指令理论上可能非常大尤其是攻击时间误差初始值很大、或者飞行器初始指向几乎背对目标时。如果没有限幅仿真里的飞行器速度方向会疯狂翻转数值积分直接发散。我在这里加了两层保护。第一层是对过载模长限幅把 (a_p) 的模长限制到预设最大值比如 (20g)。限幅之后继续保持原方向这比简单切断某一维要平滑得多。第二层是对 (e_t) 的归一化处理当剩余飞行时间误差很大时用 (\tanh) 函数做一个软限幅避免过载指令直接冲到上百g。另外一个容易忽略的退化情况是 (\eta) 接近0或者接近 (\pi/2)。(\eta0) 时飞行器正对目标此时攻击时间控制项自然为0只保留比例导引项(\eta) 接近 (\pi/2) 时飞行器航向几乎垂直于视线此时 (t_{go}) 估计式里的余弦项接近0误差导数会剧烈变化需要把过载限幅压低防止指令震荡。5. 源代码结构与核心函数实现源码整体结构并不复杂核心就是一个时间步进循环里面包含状态更新、制导律计算、数据记录三部分。我用Python写numpy负责向量运算matplotlib负责画图。下面把关键代码展开讲一下。5.1 仿真框架与数据结构我先把飞行器状态放到一个类里class MissileState: def __init__(self, pos, vel, speed): self.pos np.array(pos, dtypefloat) # 惯性系位置 self.vel np.array(vel, dtypefloat) # 速度向量 self.speed speed # 速率大小恒定 self.target np.array([0, 0, 0], dtypefloat) # 目标位置 def velocity_dir(self): return self.vel / np.linalg.norm(self.vel) def line_of_sight(self): r_vec self.target - self.pos r np.linalg.norm(r_vec) return r_vec / r, r用向量表达的好处是直观后续任何坐标变换都不需要手动写一堆三角函数。缺点是向量运算比标量积分稍微慢一点但在这个仿真里完全不是瓶颈。仿真主循环如下dt 0.001 t 0.0 tdes 12.0 # 期望攻击时间 state MissileState([0, 0, 0], [270, 30, 0], 300) history [] while t tdes 0.5: # 计算视线和剩余时间误差 e_los, r state.line_of_sight() s state.velocity_dir() cos_eta np.clip(np.dot(s, e_los), 0.1, 1.0) eta np.arccos(cos_eta) tgo r / (state.speed * cos_eta) et tgo - (tdes - t) # 计算制导指令 a_p compute_guidance(state, e_los, s, eta, cos_eta, et, tgo, t) # 更新速度方向 state.vel state.vel a_p * dt state.vel state.vel / np.linalg.norm(state.vel) * state.speed state.pos state.pos state.vel * dt history.append([t, r, tgo, et, np.linalg.norm(a_p), *state.pos]) t dt注意这里更新速度时先加上速度方向的变化量再归一化回到恒定速率。这个“投影回到原速率”的方式等价于严格在切向量方向做旋转二阶精度足够。5.2 制导循环核心代码完整的制导指令函数是下面这个compute_guidancedef compute_guidance(state, e_los, s, eta, cos_eta, et, tgo, t): V state.speed # 动态逆时间误差期望动态为 e_dot -k * e k 1.5 A_des V * (np.sin(eta) ** 2 k * cos_eta ** 2 * et) # 视线方向在过载平面内的投影 e_perp e_los - cos_eta * s e_perp_norm np.linalg.norm(e_perp) if e_perp_norm 1e-6: e_perp np.zeros(3) else: e_perp e_perp / e_perp_norm # 比例导引项用视线方向差分估计视线角速度 omega_los estimate_los_rate(state, e_los, dt) B_des 3.0 * V * np.linalg.norm(omega_los) if np.linalg.norm(omega_los) 1e-9: n_dir omega_los / np.linalg.norm(omega_los) else: n_dir np.cross(s, e_los) n_dir n_dir / (np.linalg.norm(n_dir) 1e-9) # 构造约束矩阵并求解 A np.array([e_perp, n_dir]) b np.array([A_des, B_des]) a_p, _, _, _ np.linalg.lstsq(A, b, rcondNone) # 法向过载保证垂直于速度 a_p a_p - np.dot(a_p, s) * s # 限幅 max_a 60.0 norm_a np.linalg.norm(a_p) if norm_a max_a: a_p a_p / norm_a * max_a return a_p这个函数里有两个细节需要注意。第一我用最小二乘求两个约束下的三维向量但解出来的向量可能带有微小速度方向分量所以又在后面做了一次投影确保 (a_p \cdot s0)。第二比例导引方向 (n_dir) 在视线角速率为零时用速度方向叉乘视线方向的备选值防止矩阵退化。estimate_los_rate我直接在上一个控制周期保存的视线方向基础上做中心差分再乘以一个平滑系数。更严谨的做法是引入卡尔曼滤波但在这个教学代码里中心差分加滤波足够稳定。以下是视线角速率估计函数def estimate_los_rate(state, e_los, dt): global e_los_prev if e_los_prev is None: e_los_prev e_los.copy() omega np.cross(e_los_prev, e_los) / dt omega omega / (np.linalg.norm(e_los) * np.linalg.norm(e_los_prev) 1e-9) e_los_prev e_los.copy() return omega这里用两个时刻视线向量的叉积除以时间步长来近似角速度向量。注意叉积向量方向正好是视线转动轴垂直于两个视线向量符合比例导引需要的“视线转动方向”。5.3 记录曲线与三维轨迹输出数据记录我直接放到主循环的history列表里每个时间步存一行。仿真结束后一次性转换成 numpy 数组然后分别画图。三维轨迹图画最简单的方式是matplotlib的Axes3D.plot把所有位置点连成线。为了看清最终命中点我在目标位置画一个红色五角星。这个图能直观看到飞行器是否绕圈、是否从侧向接近目标是判断制导律行为最直接的线索。时间误差曲线我画两个一个是 (t_{go}) 与期望剩余时间 (t_d - t) 的对比另一个是两者之差即 (e_t)。前者能看出误差是从哪个方向逼近的后者能看出收敛速度是否满足动态逆设计参数 (k)。理想情况下(e_t) 会从初始误差指数据衰减到0。过载曲线画的是法向过载模长随时间变化。这个曲线能直观反映动态逆有没有在某些时刻输出过大指令。如果看到过载频繁顶到限幅值说明 (k) 设得太大或者初始攻击时间误差给得太离谱。6. 仿真结果解读攻击时间误差、轨迹与过载仿真代码跑通以后不能只看“最后命中了”就结束。制导律设计得好不好要从误差收敛过程、过载特性、轨迹几何三个角度一起看。6.1 典型算例参数我设置的典型算例是这样的飞行器初始位置在 ([0, 0, 0])初始速度方向为 ([270, 30, 0]) 对应的单位向量乘以速率300也就是偏北偏上飞。目标位置在 ([4000, 2000, 1500])。直线飞行距离约4700米匀速直线飞行时间大概15.7秒。我把期望攻击时间设为12秒比直线飞行时间短逼着飞行器走一条更直的路线如果想看攻击时间延长的效果也可以设成18秒那飞行器就会绕路。初始速度方向并不是正对着目标所以初始 (\eta) 有大约30度。这给了动态逆一个很典型的初始误差场景时间误差存在视线角速率也存在两个约束同时要满足。6.2 结果曲线分析在我这组参数下攻击时间误差 (e_t) 从最初的负值快速收敛到零附近大概1.5秒之后进入稳态。这个收敛速度跟动态逆参数 (k1.5) 对应的时间常数是吻合的。动态逆的优点在这里体现得很明显误差收敛过程干净利落没有超调没有振荡。三维轨迹从图上看是一条先平滑转弯、再直线逼近的曲线。因为期望攻击时间小于直线飞行时间所以飞行器开始时会先把速度方向往目标方向拉尽量少绕路等视线角速率衰减到接近零后基本就是比例导引段在起主要作用。过载曲线在初始阶段有一个短暂的峰值大约30多没有碰到限幅值60。这是因为初始 (\eta) 大、视线角速率也大动态逆需要同时做两件事一是快速减小时间误差二是把视线角速率压下来。过了初始段之后过载迅速回落到个位数末端接近零。这说明控制律没有在后半段反复折腾轨迹过程很平稳。6.3 参数改动后的敏感性我把 (k) 从1.5改成3.0以后攻击时间误差收敛速度明显加快但初始过载峰值直接翻倍接近50。这说明 (k) 不能无脑调大它直接控制的是你愿意花多大过载去修正时间误差。如果你的飞行器过载限制只有20g那 (k) 可能要降到0.5左右代价是攻击时间误差收敛慢初始阶段飞行器会走一段较长的过渡轨迹。把最大过载从60改成30后仿真仍然能收敛但收敛时间变长攻击时间误差会有一个明显的平台期。这个现象也解释了为什么很多文献里强调“攻击时间控制律必须考虑过载约束”因为动态逆给出的是无约束解而实际系统一定有饱和饱和本质上会改变期望误差动态需要单独做抗饱和处理。7. 调试过程中最容易被坑的三个地方代码能跑通是第一步但我在调试这套三维制导律时踩了很多不起眼的坑。有些坑光看公式根本发现不了必须把仿真曲线调出来才能定位。这里把最重要的三个坑记录一下希望能帮你少走点弯路。7.1 视线角速率差分噪声大第一次跑的时候比例导引项完全是在抖过载曲线像梳子一样。定位了半天问题出在estimate_los_rate里直接用相邻两帧视线方向做叉积差分。视线方向本身变化非常平滑但叉积结果受浮点误差影响很大尤其在视线方向几乎不变时噪声会被除法放大。解决办法有两个一是把时间步长适当加大不要用0.0001这种过小的步长否则差分噪声反而更大二是对估计出的角速度做一阶低通滤波把高频抖掉。我最后用的是一阶滤波系数0.15左右既不引入明显延迟又能把抖动静音。如果你想更精确可以在视线角速度估计里加卡尔曼滤波但对这个教学仿真来说低通滤波足够了。7.2 剩余飞行时间估计在末端发散攻击时间误差曲线在快要命中目标时突然跳上去这是一个非常隐蔽的问题。原因是 (r) 趋向于0时剩余飞行时间估计 (t_{go}r/(V\cos\eta)) 在分母有限的情况下也会趋向于0但在数值上弹目相对距离小于某个阈值时视线方向向量会跳变(\cos\eta) 的计算不稳定导致 (t_{go}) 出现毛刺。我在代码里加了一个末端判定当 (t_{go} 0.2) 秒时直接把动态逆攻击时间误差项置零只保留比例导引项。这样既不影响末端命中精度又能避免时间误差在收敛到零附近时继续产生过载指令从而避免末端振荡。7.3 坐标系绕序与向量方向不一致三维制导的坑有相当一部分出在坐标系的定义和方向约定上。我在最初实现时视线方向用的是“由目标指向弹体”的方向然后剩余飞行时间估计里的 (\cos\eta) 也按这个方向算结果动态逆反解出来的过载方向正好反了攻击时间误差不但没有收敛反而越来越大。后来我把所有公式统一成“由弹体指向目标”的视线方向再重新推了一遍误差导数代码行为立刻正常。这个教训是写代码之前一定要把坐标系、向量方向写在注释第一行。不要相信自己的记忆力三周后回头看你真的会忘了到底哪边是正方向。如果你打算在这套代码上做扩展比如加入落角约束或者目标机动建议先把第4章的误差微分方程重新推一遍特别是目标速度不为零时( \dot{r} ) 里要多出目标速度项整个动态逆补偿项都会变化。模型一变动态逆的补偿项就必须跟着变这是这套方法的核心也是它比盲目调PID更可控的原因。