📚 课程目录第4章带视场约束的滑模制导律设计

4. 带视场约束的滑模制导律设计

任务定义

针对导弹拦截机动目标场景，设计一种带视场约束的滑模制导律。要求：视线角速率在有限时间内收敛至零附近，同时导弹速度矢量与视线之间的夹角（视场角）始终不超过给定约束值。本案例采用滑模控制方法，通过构造包含视场角约束的滑模面与自适应切换增益实现上述目标。

问题建模

考虑二维拦截几何，定义视线角为 q，弹目相对距离为 r，导弹速度 V_m，目标速度 V_t。视线角速率动态方程为：

dq/dt = q_dot
dq_dot/dt = -(2 * r_dot * q_dot) / r - (a_m * cos(eta_m)) / r + (a_t * cos(eta_t)) / r

其中 a_m 为导弹法向加速度（制导指令），a_t 为目标法向加速度（未知有界扰动），eta_m 为导弹前置角，eta_t 为目标前置角。视场角约束定义为 |eta_m| ≤ η_max，其中 eta_m = q - θ_m，θ_m 为导弹弹道倾角。

滑模面与制导律设计

定义滑模面为：

s = q_dot + λ * q

其中 λ > 0 为设计参数。为保证视场角约束，引入屏障函数 B(eta_m) = 1 / (η_max² - eta_m²)，构造增广滑模面：

s_aug = s + k_b * B(eta_m) * q_dot

其中 k_b > 0 为约束权重。制导律设计为：

a_m = (r / cos(eta_m)) * [ (2 * r_dot * q_dot) / r + λ * q_dot + k_b * d(B)/dt * q_dot + k_b * B * (-(2*r_dot*q_dot)/r) + ρ * sign(s_aug) + k_s * s_aug ]

其中 ρ 为切换增益（需大于目标扰动上界），k_s > 0 为趋近律系数。当 |eta_m| → η_max 时，B → ∞，s_aug 迅速增大，产生强修正指令将视场角拉回约束内。

代码实现

import numpy as np
from scipy.integrate import solve_ivp

# 拦截场景参数
V_m = 300.0          # 导弹速度 m/s
V_t = 150.0          # 目标速度 m/s
eta_max = np.deg2rad(30.0)  # 视场角约束 30°
lambda_ = 2.0        # 滑模面系数
k_b = 0.5            # 屏障函数权重
k_s = 3.0            # 趋近律系数
rho = 20.0           # 切换增益（大于目标机动上界）
a_t_max = 15.0       # 目标最大机动 m/s²

# 初始状态: [r, r_dot, q, q_dot, theta_m, theta_t]
# r=3000m, 接近速度-250m/s, q=10°, q_dot=0.02rad/s
init_state = [3000.0, -250.0, np.deg2rad(10.0), 0.02, np.deg2rad(5.0), np.deg2rad(15.0)]

def guidance_dynamics(t, state):
    r, r_dot, q, q_dot, theta_m, theta_t = state
    
    # 视场角（前置角）
    eta_m = q - theta_m
    eta_t = q - theta_t
    
    # 目标机动（未知但有界，模拟阶跃机动）
    a_t = a_t_max * (1 if t > 1.0 else 0)
    
    # 屏障函数及其导数
    B = 1.0 / (eta_max**2 - eta_m**2)
    dB_deta = 2 * eta_m / (eta_max**2 - eta_m**2)**2
    # d(eta_m)/dt = q_dot - theta_m_dot, theta_m_dot = a_m / V_m
    # 此处先计算不含a_m的项，a_m在下面求解
    
    # 滑模面
    s = q_dot + lambda_ * q
    s_aug = s + k_b * B * q_dot
    
    # 求解制导指令 a_m（隐式，因theta_m_dot依赖a_m）
    # 采用一次迭代近似：先用当前theta_m_dot=0计算a_m，再更新
    theta_m_dot = 0.0  # 初始猜测
    for _ in range(3):  # 固定点迭代
        d_eta_m_dt = q_dot - theta_m_dot
        dB_dt = dB_deta * d_eta_m_dt
        
        # 制导律（不含目标项，目标视为扰动）
        term1 = (2 * r_dot * q_dot) / r
        term2 = lambda_ * q_dot
        term3 = k_b * dB_dt * q_dot
        term4 = k_b * B * (-term1)
        term5 = rho * np.sign(s_aug) + k_s * s_aug
        
        a_m = (r / np.cos(eta_m)) * (term1 + term2 + term3 + term4 + term5)
        
        # 更新theta_m_dot
        theta_m_dot = a_m / V_m
    
    # 状态导数
    dr_dt = r_dot
    dr_dot_dt = (V_t**2 - V_m**2 + r_dot**2) / r  # 简化的相对运动学
    dq_dt = q_dot
    dq_dot_dt = -(2 * r_dot * q_dot) / r - (a_m * np.cos(eta_m)) / r + (a_t * np.cos(eta_t)) / r
    dtheta_m_dt = a_m / V_m
    dtheta_t_dt = a_t / V_t
    
    return [dr_dt, dr_dot_dt, dq_dt, dq_dot_dt, dtheta_m_dt, dtheta_t_dt]

# 仿真
t_span = (0, 5.0)
t_eval = np.linspace(0, 5.0, 500)
sol = solve_ivp(guidance_dynamics, t_span, init_state, t_eval=t_eval, rtol=1e-6, atol=1e-8)

# 提取结果
r = sol.y[0]
q = sol.y[2]
q_dot = sol.y[3]
theta_m = sol.y[4]
eta_m = q - theta_m

# 验证指标
max_eta = np.max(np.abs(eta_m))
final_q_dot = np.abs(q_dot[-1])
print(f"最大视场角: {np.rad2deg(max_eta):.2f}° (约束: {np.rad2deg(eta_max):.0f}°)")
print(f"终端视线角速率: {final_q_dot:.4f} rad/s")
print(f"视场角约束满足: {'是' if max_eta <= eta_max else '否'}")
print(f"视线角速率收敛: {'是' if final_q_dot < 0.005 else '否'}")

验证方法

运行上述仿真代码，观察输出指标。判定标准为：

视场角约束：全程 |eta_m| ≤ 30°，即输出中“最大视场角”不超过30°；
收敛性：终端时刻视线角速率值小于0.005 rad/s；
动态过程：若绘制 eta_m 时间曲线，可见其接近但不超过约束边界，且 q_dot 单调衰减。

结果判定

代码执行后，若打印的“最大视场角”不超过30°且“终端视线角速率”小于0.005 rad/s，则判定制导律设计满足任务要求。仿真过程中视场角曲线应始终位于±30°边界内，视线角速率曲线单调衰减至零附近。若视场角超限，可增大 k_b 或减小 lambda_；若收敛过慢，可增大 k_s 或 rho。