针对航迹规划（Trajectory Planning）与控制研究，我们不需要过于复杂的 6-DOF 全量非线性模型（计算量大，不利于优化算法迭代）。最有效的方案是使用3-DOF 质点模型 + 固定翼运动学约束。
这种模型既保留了固定翼不能悬停、有最小转弯半径的物理特性，又足够简洁，可以直接嵌入 MPC（模型预测控制） 或 轨迹优化算法 中。
一、 数学模型定义
我们定义状态量为位置 \((x, y, h)\) 和速度 \(V\)。控制量为切向加速度 \(a_t\) 和航点变化率 \(\dot{\psi}\)。
1. 状态空间方程
\[\begin{aligned}
\dot{x} &= V \cos\psi \cos\gamma \\
\dot{y} &= V \sin\psi \cos\gamma \\
\dot{h} &= V \sin\gamma \\
\dot{V} &= a_t \\
\dot{\psi} &= \frac{g \tan\phi}{V} \quad (\text{或直接使用} \dot{\psi} \text{作为控制量})
\end{aligned}
\]
2. 固定翼核心约束（关键）
在航迹规划中，必须加入以下物理可达性约束：
| 约束项 | 物理意义 | 典型值 |
|---|---|---|
| \(V_{min} \le V \le V_{max}\) | 失速速度与极速 | 10 ~ 30 m/s |
| $ | a_t | \le a_{max}$ |
| $ | \dot | \le \dot{\psi}_{max}$ |
二、 MATLAB 仿真代码
这段代码实现了带约束的固定翼无人机模型，并模拟了一个“加速 -> 左转爬升 -> 平飞”的航迹。你可以直接将 ref_traj 替换为你规划的轨迹进行跟踪控制研究。
%% 小型固定翼无人机模型 (用于航迹规划与控制研究)
clear; clc; close all;%% 1. 无人机参数 (小型固定翼, 如 Skywalker X8)
params.g = 9.81; % 重力加速度 (m/s^2)
params.V_min = 12; % 最小速度 (失速)
params.V_max = 28; % 最大速度
params.a_max = 4.0; % 最大切向加速度
params.phi_max = deg2rad(35); % 最大滚转角 (决定最小转弯半径)%% 2. 仿真设置
dt = 0.05; % 仿真步长 (s)
T = 30; % 总时长 (s)
steps = T / dt;% 状态向量: [x, y, h, V, psi, gamma]
% x,y: 位置; h: 高度; V: 速度; psi: 航向角; gamma: 航迹倾斜角
state = zeros(steps, 6);
state(1, :) = [0, 0, 100, 15, 0, 0]; % 初始: 位置(0,0,100), 速度15m/s, 平飞%% 3. 模拟规划层给出的控制指令 (参考轨迹生成)
% 这里我们手动构造一个指令序列，实际研究中这里应是优化算法的输出
t = (0:steps-1)' * dt;
a_t_ref = 2 * tanh(t - 5); % 5秒后加速
psi_ref = deg2rad(45) * (1 - exp(-0.2*t)); % 逐渐转向45度
gamma_ref = deg2rad(10) * (t > 10 & t < 20); % 10-20秒爬升10度%% 4. 动力学仿真循环 (带约束)
for k = 1:steps-1% 当前状态x = state(k, 1); y = state(k, 2); h = state(k, 3);V = state(k, 4); psi = state(k, 5); gamma = state(k, 6);% ---- 控制律 (这里模拟轨迹跟踪控制器) ----% 实际研究中，这里可以是 PID 或 MPC 控制器a_t = max(-params.a_max, min(params.a_max, a_t_ref(k) - (V - params.V_min)));% 航点率限制 (物理约束: 最小转弯半径)psi_dot_max = params.g * tan(params.phi_max) / V;psi_dot = max(-psi_dot_max, min(psi_dot_max, psi_ref(k) - psi));% 航迹倾斜角 (通常由升降舵控制, 这里简化为直接跟踪)gamma_dot = 0.1 * (gamma_ref(k) - gamma);% ---- 状态更新 (运动学) ----x_new = x + V * cos(psi) * cos(gamma) * dt;y_new = y + V * sin(psi) * cos(gamma) * dt;h_new = h + V * sin(gamma) * dt;V_new = max(params.V_min, min(params.V_max, V + a_t * dt));psi_new = psi + psi_dot * dt;gamma_new = gamma + gamma_dot * dt;state(k+1, :) = [x_new, y_new, h_new, V_new, psi_new, gamma_new];
end%% 5. 结果可视化
figure('Color', 'white', 'Position', [100 100 1200 400]);% 3D 航迹
subplot(1, 3, 1);
plot3(state(:,1), state(:,2), state(:,3), 'b-', 'LineWidth', 2); hold on;
scatter3(state(1,1), state(1,2), state(1,3), 100, 'g', 'filled');
scatter3(state(end,1), state(end,2), state(end,3), 100, 'r', 'filled');
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Height (m)');
title('3D Flight Trajectory');
grid on; axis equal;% 速度响应
subplot(1, 3, 2);
plot(t, state(:,4), 'k-', 'LineWidth', 2); hold on;
plot(t, params.V_min*ones(size(t)), 'r--', 'LineWidth', 1.5);
plot(t, params.V_max*ones(size(t)), 'r--', 'LineWidth', 1.5);
ylabel('Velocity (m/s)'); xlabel('Time (s)');
title('Speed Profile (with Constraints)');
grid on;% 航向角
subplot(1, 3, 3);
plot(t, rad2deg(state(:,5)), 'm-', 'LineWidth', 2);
ylabel('Heading (deg)'); xlabel('Time (s)');
title('Heading Angle');
grid on;sgtitle('Fixed-Wing UAV Model for Trajectory Planning');
三、 用法
1. 嵌入 MPC 控制器
将上述模型离散化为标准形式 \(x_{k+1} = f(x_k, u_k)\)，然后代入 MPC 的优化问题中：
\[\min_{u} \sum (x_k - x_{ref})^T Q (x_k - x_{ref}) + u_k^T R u_k
\]
Subject to: \(x_{min} \le x_k \le x_{max}\) (即上面的物理约束)。
2. 轨迹生成（Trajectory Generation）
如果你想生成一条光滑的轨迹，可以将状态量表示为多项式（如 Minimum Snap），并利用上述模型推导出的约束作为优化问题的边界条件。
3. 线性化模型（Linerization）
在配平点（Trim Point，如 \(V=20m/s, \gamma=0\)）对上述模型求雅可比矩阵，得到线性状态空间模型 \((A, B)\) 用于 LQR 设计：
% 在配平点计算 A, B 矩阵 (符号法或数值摄动法)
syms x y h V psi gamma a_t psi_dot
% ... 定义状态方程 ...
A = jacobian(f, [x, y, h, V, psi, gamma]);
B = jacobian(f, [a_t, psi_dot]);
参考代码 小型固定翼无人机数学模型 www.youwenfan.com/contentcnu/64229.html
四、 进阶建议
- 风场干扰：在 \(\dot{x}, \dot{y}\) 方程中加入风速分量 \(W_x, W_y\)，研究抗风能力。
- 地形约束：在优化问题中加入 \(h \ge h_{terrain}(x,y)\)，避免撞山。
- 能耗模型：加入电池消耗模型 \(\dot{E} = f(V, a_t)\)，用于研究续航最优轨迹。