3. 视场角限制下的最优制导律推导

任务定义

针对视场角受限的终端制导问题，推导使脱靶量最小且满足视场角约束的最优制导律。给定导弹-目标相对运动模型，目标函数为终端脱靶量平方与控制能量加权和，约束为导引头视场角（即弹目视线角与导弹速度方向夹角）不超过允许最大值。要求给出最优制导指令的解析表达式，并分析其最优性条件。

问题建模

采用线性化相对运动方程。设y为弹目相对位移的垂直分量（垂直于初始视线方向），v_y为相对速度垂直分量，a_M为导弹法向加速度（制导指令），t_go为剩余飞行时间。状态方程为：

ẏ = v<sub>y</sub>
v̇<sub>y</sub> = a<sub>M</sub>

视场角约束近似为视线角速率限制。设λ为视线角，σ为导弹速度方向角，视场角ε = λ − σ。在小角度假设下，视线角速率与相对运动关系为λ̇ ≈ v_y/R，其中R为弹目距离。视场角约束|ε| ≤ ε_max可转化为对状态v_y的约束：

|v<sub>y</sub>| ≤ v<sub>max</sub>(t) = ε<sub>max</sub> · V<sub>M</sub>

其中V_M为导弹速度（假设常值），该约束在剩余飞行时间内有效。

最优制导律推导

定义零控脱靶量ZEM = y + v_y·t_go。性能指标为：

J = ½·ZEM(t<sub>f</sub>)² + ½·∫<sub>0</sub><sup>t<sub>f</sub></sup> a<sub>M</sub>²(τ) dτ

约束为|v_y(t)| ≤ v_max。应用Pontryagin极小值原理，哈密顿函数为：

H = ½·a<sub>M</sub>² + p<sub>1</sub>·v<sub>y</sub> + p<sub>2</sub>·a<sub>M</sub>

协态方程为ṗ₁ = 0，ṗ₂ = −p₁。终端条件p₁(t_f) = ZEM(t_f)，p₂(t_f) = ZEM(t_f)·t_go(t_f) = 0。求解得最优控制为：

a*<sub>M</sub>(t) = −p<sub>2</sub>(t) = −[ZEM(t<sub>f</sub>)·(t<sub>f</sub> − t)]

当视场角约束激活时（即|v_y|达到边界），最优控制切换为边界控制，使状态保持在约束边界上。综合得到最优制导律的分段形式：

a*<sub>M</sub>(t) = ⎧ −3·ZEM(t)/t<sub>go</sub>²,  当 |v<sub>y</sub>| < v<sub>max</sub> 且 |a*<sub>M</sub>| ≤ a<sub>max</sub>
           ⎨ −v<sub>max</sub>/t<sub>go</sub>,       当 v<sub>y</sub> = v<sub>max</sub>（边界保持）
           ⎩ +v<sub>max</sub>/t<sub>go</sub>,       当 v<sub>y</sub> = −v<sub>max</sub>（边界保持）

其中比例导引系数3对应终端脱靶量最小化的最优增益。当状态远离约束边界时，制导律退化为经典最优比例导引；当状态到达边界时，制导指令切换为边界保持形式，防止视场角进一步增大。

最优性条件分析

由极小值原理，最优性条件包括：

正则方程：协态变量满足ṗ₁=0，ṗ₂=−p₁，且p₁为常数；
横截条件：p₁(t_f) = ∂Φ/∂y = ZEM(t_f)，其中Φ为终端代价；
约束互补条件：当|v_y| < v_max时，约束非激活，最优控制由无约束解给出；当|v_y| = v_max时，约束激活，控制需满足边界保持条件且拉格朗日乘子非负；
切换条件：在约束边界上，控制切换需保证哈密顿函数连续。

由于性能指标对a_M为凸函数，约束集为凸集，该最优解满足充分性条件，即为全局最优。

代码实现


import numpy as np
from scipy.integrate import solve_ivp

def optimal_guidance_with_fov(y0, vy0, t_go_total, v_max, dt=0.01):
    """
    视场角约束下的最优制导律仿真
    
    参数:
        y0: 初始相对位移垂直分量 (m)
        vy0: 初始相对速度垂直分量 (m/s)
        t_go_total: 总剩余飞行时间 (s)
        v_max: 视场角约束对应的最大垂直速度 (m/s)
        dt: 仿真步长 (s)
    
    返回:
        t_array: 时间数组
        y_array: 相对位移
        vy_array: 相对速度
        a_array: 制导指令
        zem_array: 零控脱靶量
    """
    # 状态初始化
    y = y0
    vy = vy0
    t_go = t_go_total
    
    # 存储结果
    t_array = [0.0]
    y_array = [y]
    vy_array = [vy]
    a_array = [0.0]
    zem_array = [y + vy * t_go]
    
    t = 0.0
    while t_go > dt:
        # 计算零控脱靶量
        zem = y + vy * t_go
        
        # 无约束最优指令 (比例导引形式)
        a_unconstrained = -3.0 * zem / (t_go ** 2)
        
        # 检查视场角约束是否激活
        # 预测下一时刻的vy
        vy_next_unconstrained = vy + a_unconstrained * dt
        
        # 约束处理
        if abs(vy) >= v_max - 1e-6:
            # 约束激活，边界保持控制
            if vy > 0:
                a_cmd = -v_max / t_go  # 保持在上边界
            else:
                a_cmd = v_max / t_go   # 保持在下边界
        else:
            # 检查无约束控制是否会导致越界
            if abs(vy_next_unconstrained) > v_max:
                # 需要限制，计算到达边界所需控制
                if vy_next_unconstrained > v_max:
                    # 将到达上边界
                    a_cmd = (v_max - vy) / dt
                else:
                    a_cmd = (-v_max - vy) / dt
            else:
                # 无约束控制可行
                a_cmd = a_unconstrained
        
        # 更新状态
        vy_new = vy + a_cmd * dt
        y_new = y + vy * dt
        
        # 更新时间
        t += dt
        t_go -= dt
        
        # 存储
        t_array.append(t)
        y_array.append(y_new)
        vy_array.append(vy_new)
        a_array.append(a_cmd)
        zem_array.append(y_new + vy_new * t_go)
        
        # 更新状态
        y = y_new
        vy = vy_new
    
    return (np.array(t_array), np.array(y_array), 
            np.array(vy_array), np.array(a_array), 
            np.array(zem_array))

# 仿真参数
V_M = 300.0          # 导弹速度 (m/s)
epsilon_max = 0.2    # 最大视场角 (rad, 约11.5度)
v_max = epsilon_max * V_M  # 最大垂直速度 (m/s)

# 初始条件
y0 = 50.0            # 初始相对位移 (m)
vy0 = 80.0           # 初始垂直速度 (m/s) - 超过约束
t_go_total = 3.0     # 剩余飞行时间 (s)

# 运行仿真
t, y, vy, a, zem = optimal_guidance_with_fov(y0, vy0, t_go_total, v_max)

# 输出关键结果
print("=== 视场角约束最优制导仿真结果 ===")
print(f"初始条件: y0={y0}m, vy0={vy0}m/s, t_go={t_go_total}s")
print(f"约束条件: v_max={v_max:.1f}m/s (对应视场角{epsilon_max:.2f}rad)")
print(f"初始ZEM: {y0 + vy0 * t_go_total:.2f}m")
print(f"终端ZEM: {zem[-1]:.4f}m")
print(f"最大|vy|: {np.max(np.abs(vy)):.2f}m/s")
print(f"约束满足: {'是' if np.max(np.abs(vy)) <= v_max + 1e-6 else '否'}")
print(f"终端脱靶量: {abs(y[-1]):.4f}m")

运行与验证

运行方式：将上述代码保存为fov_guidance.py，在安装了NumPy和SciPy的Python环境中执行python fov_guidance.py。

验证方法：

检查输出中“约束满足”是否为“是”，确认整个飞行过程中|v_y|未超过v_max；
对比终端ZEM与初始ZEM，验证制导律有效降低了脱靶量；
修改vy0为小于v_max的值（如30m/s），重新运行，观察制导律退化为无约束比例导引，终端ZEM趋近于零。

判定标准：若约束满足标志为“是”且终端ZEM显著小于初始ZEM（至少降低一个数量级），则验证通过。预期输出中终端ZEM应接近零（受步长影响存在数值误差），且全程|v_y|不超过v_max。

📚 课程目录