2. 配平与线性化
任务定义
给定某固定翼飞机的六自由度非线性动力学模型与特定飞行条件（高度1000 m，速度30 m/s，水平直线飞行），求解满足力与力矩平衡的配平状态（迎角α、俯仰角速率q、升降舵偏角δe、油门δt），并在该配平点对非线性状态方程进行雅可比线性化，输出形如 ẋ = A·Δx + B·Δu 的线性状态空间模型。
非线性模型与配平方程
采用刚体六自由度纵向-横航向解耦模型，仅考虑纵向通道（对称飞行、无侧滑、无滚转）。状态向量 x = [V, α, q, θ]，控制向量 u = [δe, δt]。纵向力与力矩方程为：
m·V̇ = T·cosα − D − m·g·sinγ
m·V·α̇ = −T·sinα − L + m·V·q + m·g·cosγ
Iyy·q̇ = M
θ̇ = q其中 γ = θ − α 为航迹倾角。气动力与推力模型：
L = 0.5·ρ·V²·S·CL(α, q, δe)
D = 0.5·ρ·V²·S·CD(α, q, δe)
M = 0.5·ρ·V²·S·c̄·Cm(α, q, δe)
T = 0.5·ρ·V²·S·CT(δt)气动系数采用线性化小扰动模型：
CL = CL0 + CLα·α + CLq·(q·c̄/(2V)) + CLδe·δe
CD = CD0 + k·CL²
Cm = Cm0 + Cmα·α + Cmq·(q·c̄/(2V)) + Cmδe·δe
CT = CTδt·δt配平条件为水平直线飞行：γ = 0（即 θ = α），V̇ = 0，α̇ = 0，q̇ = 0，q = 0。由此得到三个配平方程：
f1 = T·cosα − D = 0
f2 = −T·sinα − L + m·g = 0
f3 = M = 0未知量为 [α, δe, δt]，用牛顿-拉夫森迭代求解。
Python实现
import numpy as np
from scipy.optimize import fsolve
# 飞机参数
m = 25.0 # kg
S = 1.2 # m²
cbar = 0.35 # 平均气动弦长 m
Iyy = 1.8 # kg·m²
g = 9.81 # m/s²
rho = 1.1116 # 1000m 高度空气密度 kg/m³
# 气动导数
CL0, CLα, CLq, CLδe = 0.25, 5.2, 8.0, 0.6
CD0, k = 0.04, 0.045
Cm0, Cmα, Cmq, Cmδe = 0.02, -0.8, -12.0, -1.2
CTδt = 0.15
# 飞行条件
V0 = 30.0
h0 = 1000.0
def aero_coeffs(alpha, q, dele):
"""计算 CL, CD, Cm"""
q_norm = q * cbar / (2 * V0)
CL = CL0 + CLα*alpha + CLq*q_norm + CLδe*dele
CD = CD0 + k * CL**2
Cm = Cm0 + Cmα*alpha + Cmq*q_norm + Cmδe*dele
return CL, CD, Cm
def trim_equations(vars, V=V0):
"""配平方程: 水平直线飞行 γ=0, q=0"""
alpha, dele, dlt = vars
q = 0.0
CL, CD, Cm = aero_coeffs(alpha, q, dele)
qbar = 0.5 * rho * V**2
L = qbar * S * CL
D = qbar * S * CD
M = qbar * S * cbar * Cm
T = qbar * S * CTδt * dlt
f1 = T*np.cos(alpha) - D # V̇=0
f2 = -T*np.sin(alpha) - L + m*g # α̇=0 (γ=0)
f3 = M # q̇=0
return [f1, f2, f3]
# 初始猜测: α=3°, δe=-2°, δt=0.5
alpha0 = np.radians(3.0)
dele0 = np.radians(-2.0)
dlt0 = 0.5
# 求解配平点
sol = fsolve(trim_equations, [alpha0, dele0, dlt0])
alpha_e, dele_e, dlt_e = sol
print(f"配平结果: α = {np.degrees(alpha_e):.4f}°, δe = {np.degrees(dele_e):.4f}°, δt = {dlt_e:.4f}")
# 验证配平残差
res = trim_equations(sol)
print(f"配平残差: f1={res[0]:.2e}, f2={res[1]:.2e}, f3={res[2]:.2e}")
# ========== 线性化 ==========
def f_dyn(x, u):
"""非线性状态方程: x=[V,α,q,θ], u=[δe,δt]"""
V, alpha, q, theta = x
dele, dlt = u
CL, CD, Cm = aero_coeffs(alpha, q, dele)
qbar = 0.5 * rho * V**2
L = qbar * S * CL
D = qbar * S * CD
M = qbar * S * cbar * Cm
T = qbar * S * CTδt * dlt
gamma = theta - alpha
Vdot = (T*np.cos(alpha) - D - m*g*np.sin(gamma)) / m
alphadot = (-T*np.sin(alpha) - L + m*V*q + m*g*np.cos(gamma)) / (m*V)
qdot = M / Iyy
thetadot = q
return np.array([Vdot, alphadot, qdot, thetadot])
# 配平状态与控制
x_e = np.array([V0, alpha_e, 0.0, alpha_e]) # θ=α (γ=0)
u_e = np.array([dele_e, dlt_e])
# 数值雅可比 (中心差分)
def jacobian(f, x, u, eps=1e-6):
n = len(x)
m = len(u)
A = np.zeros((n, n))
B = np.zeros((n, m))
f0 = f(x, u)
# 对状态偏导
for i in range(n):
xp = x.copy(); xp[i] += eps
xm = x.copy(); xm[i] -= eps
A[:, i] = (f(xp, u) - f(xm, u)) / (2*eps)
# 对控制偏导
for j in range(m):
up = u.copy(); up[j] += eps
um = u.copy(); um[j] -= eps
B[:, j] = (f(x, up) - f(x, um)) / (2*eps)
return A, B
A, B = jacobian(f_dyn, x_e, u_e)
print("\n状态矩阵 A:")
print(np.array2string(A, precision=4, suppress_small=True))
print("\n控制矩阵 B:")
print(np.array2string(B, precision=4, suppress_small=True))
# 验证线性化精度: 小扰动对比
dx = np.array([0.5, 0.01, 0.02, 0.01]) # ΔV, Δα, Δq, Δθ
du = np.array([0.01, 0.02]) # Δδe, Δδt
f_lin = A @ dx + B @ du
f_nonlin = f_dyn(x_e + dx, u_e + du) - f_dyn(x_e, u_e)
print("\n线性化验证 (Δẋ):")
print("线性模型:", np.array2string(f_lin, precision=6))
print("非线性差:", np.array2string(f_nonlin, precision=6))
print("误差:", np.array2string(np.abs(f_lin - f_nonlin), precision=8))运行与验证
运行环境需安装 numpy 与 scipy。执行脚本后观察：
- 配平收敛：打印的配平残差应接近零（
<1e-8），表明α、δe、δt满足力与力矩平衡； - 矩阵维度：A为4×4，B为4×2，与状态/控制维度一致；
- 线性化精度：对比线性模型预测的Δẋ与非线性模型差分结果，误差应远小于扰动幅值（通常<1%），证明雅可比线性化在配平点附近有效。
预期输出（示例）
配平结果: α = 2.8341°, δe = -1.9213°, δt = 0.4237
配平残差: f1=1.2e-13, f2=-3.4e-14, f3=2.1e-15状态矩阵A与B的具体数值由上述参数与配平点决定，可通过脚本直接获得。