4、制导律模型实现：比例导引律、追踪法、三点法、制导指令生成与限幅

各位同学，欢迎来到第四章。前面我们聊完了导弹的动力学模型和传感器仿真，现在终于到了最核心的部分——制导律。

说白了，制导律就是导弹的「大脑」。它告诉导弹：「目标在那边，你该怎么飞过去。」我做了这么多年半实物仿真，见过太多因为制导律参数没调好，导致仿真结果「飞偏」的案例。今天咱们就把三种最经典的制导律讲透。

4.1 比例导引律（PN）—— 最常用的「老大哥」

比例导引律，简称PN。为什么它最常用？因为简单、可靠、效果好。我在项目中，80%的场景都用它。

它的核心思想就一句话：导弹的转弯角速度，与视线角速度成正比。

数学表达式很简单：

a_cmd = N * V_m * λ_dot

其中：

• a_cmd —— 指令加速度（垂直于视线方向）
• N —— 导航比，通常取3~5
• V_m —— 导弹速度
• λ_dot —— 视线角速率

嗯，这里要注意：λ_dot 是视线角速率，不是角度本身。我见过有人直接把视线角当输入，结果导弹像喝醉了一样乱晃。

在Python里实现PN，核心代码就几行：

def proportional_navigation(lambda_dot, V_m, N=4):
    """
    比例导引律计算
    :param lambda_dot: 视线角速率 (rad/s)
    :param V_m: 导弹速度 (m/s)
    :param N: 导航比
    :return: 指令加速度 (m/s^2)
    """
    a_cmd = N * V_m * lambda_dot
    return a_cmd

你想想看，这个函数是不是很简单？但实际工程中，lambda_dot 的获取才是难点。视线角速率通常来自导引头或雷达测量，会有噪声和延迟。我在做硬件在环仿真时，必须给 lambda_dot 加上低通滤波，否则指令会抖得厉害。

4.2 追踪法 —— 最直观的「狗追兔子」

追踪法，也叫纯追踪法。它的逻辑非常直观：导弹的头部始终指向目标当前位置。

就像你拿手电筒照一个移动的物体，手电筒始终对准它。导弹的加速度方向，就是指向目标的视线方向。

数学表达式：

a_cmd = K * (λ - θ_m)

其中：

• λ —— 视线角
• θ_m —— 导弹弹道角
• K —— 比例系数

代码实现：

def pure_pursuit(lambda_angle, theta_m, K=2.0):
    """
    追踪法制导律
    :param lambda_angle: 视线角 (rad)
    :param theta_m: 导弹弹道角 (rad)
    :param K: 比例系数
    :return: 指令加速度 (m/s^2)
    """
    error = lambda_angle - theta_m
    # 角度误差归一化到 [-pi, pi]
    error = (error + np.pi) % (2 * np.pi) - np.pi
    a_cmd = K * error
    return a_cmd

注意那个角度归一化。我刚开始做的时候忘了这步，结果角度误差从179度变成-179度时，指令会跳变，导弹直接「抽风」。嗯，这个小坑我踩过。

4.3 三点法 —— 适合「打固定靶」

三点法，也叫「瞄准线法」。它的原理是：导弹、目标、制导站（或雷达）始终保持在一条直线上。

说白了，就是制导站一直盯着目标，导弹必须飞在制导站和目标之间的连线上。这种方法常用于早期的指令制导系统，比如一些反坦克导弹。

数学上，三点法要求导弹的视线角等于制导站到目标的视线角：

λ_m = λ_t

其中 λ_m 是制导站-导弹视线角，λ_t 是制导站-目标视线角。

制导指令就是让导弹的视线角误差趋于零：

a_cmd = K_p * (λ_t - λ_m) + K_d * (λ_t_dot - λ_m_dot)

代码示例：

def three_point_method(lambda_t, lambda_m, lambda_t_dot, lambda_m_dot, Kp=3.0, Kd=1.0):
    """
    三点法制导律
    :param lambda_t: 目标视线角 (rad)
    :param lambda_m: 导弹视线角 (rad)
    :param lambda_t_dot: 目标视线角速率 (rad/s)
    :param lambda_m_dot: 导弹视线角速率 (rad/s)
    :param Kp: 比例增益
    :param Kd: 微分增益
    :return: 指令加速度 (m/s^2)
    """
    error = lambda_t - lambda_m
    error_dot = lambda_t_dot - lambda_m_dot
    a_cmd = Kp * error + Kd * error_dot
    return a_cmd

4.4 制导指令生成与限幅 —— 别让导弹「飞过头」

好了，现在三种制导律都讲完了。但有个问题：计算出来的指令加速度，导弹真的能执行吗？

不能。导弹的舵面偏转有限，发动机推力有限，过载也有限。你算出一个100g的指令，导弹实际只能拉20g，那指令就白算了。

所以，限幅是必须的。

我一般做三件事：

1. 加速度限幅：根据导弹的最大可用过载，限制指令加速度的幅值。
2. 变化率限幅：防止指令变化太快，导致舵机响应不过来。
3. 积分饱和处理：如果用了PID控制，要防止积分项无限累积。

代码实现：

def limit_command(a_cmd, max_accel=200.0, max_rate=500.0, dt=0.001, prev_cmd=0.0):
    """
    制导指令限幅
    :param a_cmd: 原始指令加速度 (m/s^2)
    :param max_accel: 最大允许加速度 (m/s^2)
    :param max_rate: 最大变化率 (m/s^3)
    :param dt: 仿真步长 (s)
    :param prev_cmd: 上一时刻指令
    :return: 限幅后的指令
    """
    # 1. 变化率限幅
    rate = (a_cmd - prev_cmd) / dt
    if abs(rate) > max_rate:
        a_cmd = prev_cmd + np.sign(rate) * max_rate * dt
    
    # 2. 幅值限幅
    a_cmd = np.clip(a_cmd, -max_accel, max_accel)
    
    return a_cmd

另外，在实际硬件在环仿真中，我还会加一个指令平滑滤波器。比如一阶低通滤波：

a_filtered = alpha * a_cmd + (1 - alpha) * a_prev

其中 alpha 是滤波系数，通常取0.1~0.3。这样可以让指令更平滑，减少舵机的抖动。

4.5 三种制导律的对比与选型建议

最后，我整理了一个表格，方便你对比：

制导律	优点	缺点	适用场景
比例导引律	实现简单，脱靶量小	对视线角速率测量精度要求高	中远程空空导弹、地空导弹
追踪法	直观，容易理解	对机动目标效果差，过载需求大	低速目标、固定目标
三点法	适合指令制导，抗干扰强	需要制导站持续参与，弹道弯曲	反坦克导弹、早期指令制导系统

我个人建议：如果拿不准，先用比例导引律。它是最成熟、最稳定的。等你把PN调好了，再尝试其他方法。

好了，这一章的内容就到这里。下一章我们会把这些制导律放到硬件在环仿真平台里跑起来，看看实际效果。到时候你会发现，仿真和理论计算之间，还有不少「坑」等着你去填。