辛巴1995多旋翼无人机的时间最优轨迹规划，说白了就是在电机不烧、姿态不失控的前提下，把飞机从A点送到B点并且总时间最短。接触过飞控的人都知道，位置环和姿态环是一个强耦合的整体，但很多轨迹规划教程喜欢把位置轨迹单独拎出来设计，最后生成的轨迹看似平滑，一放到旋转动力学里就暴露问题。我自己在复现“基于旋转动力学双模型的多旋翼无人机时间最优轨迹规划”时，最大的收获是把旋转动力学拆成“规划用简化模型”和“校核用完整模型”两条路线。这篇文章会把建模、离散化、Matlab代码以及调参时的坑都摊开讲，适合正在做无人机轨迹规划、最优控制复现，或者相关毕业设计的读者。
1. 整体设计思路：为什么旋转动力学双模型能治病
1.1 位置轨迹与旋转动力学之间的天然鸿沟
多旋翼的微分平坦模型非常漂亮：把位置轨迹和偏航角当作平坦输出，位置求导得到期望加速度，再逆推出期望姿态和推力方向。这个思路在轨迹跟踪里已经很成熟，但它对“姿态响应”做了很强的理想化假设——认为无人机能瞬间到达期望姿态，或者姿态回路的带宽远大于规划带宽。真实情况下，姿态变化要受到刚体动力学约束，旋转通道的加速度取决于力矩，力矩又取决于电机转速和螺旋桨气动力。如果规划器完全无视这些限制，生成的轨迹可能在几何上没有毛病，真机跑起来却会突然出现掉高、侧滑甚至姿态超调。
我见过不少同学做路径规划时只在二维平面上用B样条或者多项式拟合成一条平滑曲线，后期再加一个时间缩放比例就完事。这种做法的核心问题是“几何路径”和“时间分配”分离得太过粗糙。无人机飞行需要同时满足平移加速度和角加速度，二者通过姿态角耦合在一起。旋转动力学双模型的核心思路，就是把这种耦合放到规划阶段显式建模，而不是等轨迹生成后再靠飞控硬扛。
1.2 双模型的定位：规划模型与校核模型明确分工
这里说的双模型，不是简单地把同一个方程写两遍，而是两套复杂度和用途都不同的模型。第一套我称为规划模型，它把俯仰角、角速度、角加速度看成一条二阶积分链：θ' = ω，ω' = M / I_y。输入是期望力矩M，输出是绕俯仰轴的角加速度。这个模型没有惯量耦合，没有电机延迟，非常适合放进非线性优化器里反复迭代，因为它的约束是线性且解耦的，数值上非常友好。
第二套模型是校核模型，它保留刚体动力学和电机执行器的真实关系。对于三维大机动，绕俯仰轴的力矩并不完全等于I_y乘以角加速度，还包含其他角速度分量的交叉项ω × (Jω)。也就是说，当无人机同时快速滚转和偏航时，俯仰轴实际需要的力矩会偏离简化模型的计算结果。用校核模型可以在仿真里提前发现这个问题，然后把安全系数反馈给规划模型，形成一次迭代闭环。
1.3 适用人群与前置知识
如果你正在做旋翼无人机时间最优轨迹规划、最优控制课程项目，或者单纯想用Matlab复现某个轨迹规划论文，这篇文章至少能帮你少踩三个坑：第一，理解为什么时间最优轨迹不能只用位置多项式拟合；第二，知道fmincon这类通用非线性优化器怎么解最优控制问题；第三，看到一套可以直接复现的离散化与约束写法。前置知识只需要你了解多旋翼刚体运动方程，会Matlab基础数值计算和最优化工具箱，能看懂状态空间模型即可。全文以二维纵向模型作为主算例，但最后会解释如何向三维扩展。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 时间最优问题的数学建模与约束处理
2.1 坐标约定与运动状态
为了把问题写清楚，又不让代码绕进三维欧拉角的万向节死锁，这套复现采用二维纵向模型。多旋翼在x-z平面内运动，机体只绕y轴俯仰，推力方向始终沿机体z轴。状态向量定义为：
x = [px, pz, vx, vz, θ, ω]^T
其中px、pz是惯性系下的位置，vx、vz是速度，θ是俯仰角，ω是俯仰角速度。控制输入取u = [T, M]^T，T是总推力，M是绕俯仰轴的合成力矩。
位置动力学可以直接写出来：px'' = (T/m)sinθ，pz'' = (T/m)cosθ - g。这里没有把推力在机体坐标和世界坐标之间做四元数变换，而是直接投影到x-z平面。这种简化在二维问题里完全合法。如果你要扩展到三维，就必须把sinθ和cosθ对应项替换成旋转矩阵R，并加上偏航角运动学。在二维算例中，θ的绝对值一般不会超过90度，所以用欧拉角也不会遇到奇异问题。
2.2 旋转动力学双模型的数学表达
规划模型写为：
θ' = ω
ω' = M / I_y
约束条件可以整理为：
- θ_min ≤ θ ≤ θ_max
- ω_min ≤ ω ≤ ω_max
- T_min ≤ T ≤ T_max
- M_min ≤ M ≤ M_max
校核模型则使用三维刚体欧拉方程：
J * ω_b_dot + ω_b × (J * ω_b) = M_b
其中ω_b是机体角速度向量，J是惯量对角阵，M_b是施加在机体轴上的外力矩。将该方程投影到俯仰轴后，会得到一个比规划模型多出交叉项的表达式。若机体滚转和偏航角速度不为零，则俯仰轴所需力矩并不仅仅是I_y·ω_y_dot，还要加上J_x与J_z之间的差乘以滚转角速度和偏航角速度的乘积项。校核模型的作用，就是在轨迹生成后把期望角速度、期望角加速度代进去，看这一项会不会让总力矩越限。
可能有人会问：双模型在校核后发现力矩超限怎么办？我的处理方式是迭代。把校核出的多余力矩量折算成规划模型中俯仰角加速度上限的收缩系数，重新跑一次时间最优，再校核。几次迭代后，规划模型和校核模型的结果会收敛到一个工程可接受的边界内。对于本文的二维模型，交叉项天然为零，所以这个校核过程更多体现在三维扩展场景；但在二维复现时把校核函数一起写上，会让你后续扩维少走很多弯路。
2.3 时间最优目标函数与路径约束整理
时间最优的目标函数不是能量，也不是加加速度，而是总飞行时间T。采用直接配点法时，把时间轴离散成N段，每段长度设为优化变量Δt。如果共有N段，总时间T = N·Δt。优化器不断调整Δt和控制序列，使总时间最小。
代价函数写成：
J = N·Δt
为了防止时间最优引起的控制量频繁抖振，我通常会在后面加一个很小的惩罚项：
J = N·Δt + ε * Σ(M_i - M_{i-1})^2
ε一般取1e-4到1e-3。这个正则项不会明显改变最优时间，但能避免几乎不可实现的开关控制。求解时，约束可以分成三类：第一类是边界约束，集中在控制量和角度角速度的上下限；第二类是初末状态约束，起点和终点必须严格等于预设的悬停状态；第三类是动力学等式约束，把离散状态节点用积分关系串起来。这三类约束会全部写进fmincon中。
3. Matlab代码复现：完整流程与关键实现
3.1 代码结构与参数初始化
我建议把整个复现分成三个文件：drone2D.m负责规划模型的动力学，solveTimeOptimal.m负责调用fmincon求解最优控制，plotResult.m负责画图。每部分单独调试会非常方便。下面先给出参数初始化部分，这部分界定了你想要的飞行任务和物理边界。
matlab复制%% solveTimeOptimal.m 主脚本
clear; clc; close all;
% ---------- 物理参数 ----------
params.g = 9.81;
params.m = 1.0;
params.Iy = 0.05;
% ---------- 执行器/姿态约束 ----------
params.Tmin = 0.5 * params.m * params.g; % 最小总推力，能提供下降加速度
params.Tmax = 2.0 * params.m * params.g; % 最大总推力
params.Mmin = -2.0;
params.Mmax = 2.0;
params.theta_max = deg2rad(45);
params.omega_max = deg2rad(120);
% ---------- 初始与终端状态 ----------
p0 = [0; 1];
pf = [5; 1];
v0 = [0; 0];
vf = [0; 0];
x0 = [p0; v0; 0; 0];
xf = [pf; vf; 0; 0];
% ---------- 离散参数 ----------
N = 40; % 时间段数，越多越精确，但优化变量也越多
这里有几个参数值得解释。Tmin设成0.5倍重力，Tmax设成2倍重力，意味着无人机可以输出比悬停推力小一半的推力，从而获得向下的加速度，也能输出两倍推力做急加速。M范围±2牛米对应一个质量1千克、俯仰惯量0.05千克·平方米的小型无人机，最大角加速度约40 rad/s²。如果机型不同，M范围需要根据电机和螺旋桨能力重新标定，不能照搬。
3.2 决策变量与初值设计
这一步是很多人最头疼的地方。直接配点法把状态序列、控制序列和时间步长一起塞进一个大向量z里。令状态维度nx=6，控制维度nu=2，则z的长度是 (N+1)nx + Nnu + 1。前(N+1)nx个元素放状态，接下来Nnu个元素放控制，最后一个放Δt。
我把初始z0设计成位置线性插值、速度零向量、姿态水平、控制取悬停推力的形式：
matlab复制% ---------- 初值构造 ----------
% 状态初值：位置线性插值，速度和姿态均为0
p_lin_x = linspace(p0(1), pf(1), N+1);
p_lin_z = linspace(p0(2), pf(2), N+1);
x_guess = zeros(6, N+1);
x_guess(1,:) = p_lin_x;
x_guess(2,:) = p_lin_z;
% 控制初值：悬停推力，零力矩
u_guess = repmat([params.m * params.g; 0], 1, N);
dt_guess = 0.05;
% 拼接成优化变量
nx = 6; nu = 2;
z0 = [x_guess(:); u_guess(:); dt_guess];
初值速度全为0，意味着优化器要从“几乎悬停”的状态出发去寻找可行轨迹。这个初值在动力学上不够精确，但非常安全，fmincon在最初的几步迭代会快速把它拉向可行域。如果你一开始就给一个全速飞行的轨迹，目标函数虽然更小，但等式约束残差很大，求解器很容易发散。另一个需要注意的点是dt_guess不能给太大，否则初始节点间的位置变化会超过物理可能。0.05到0.1秒通常是一个比较稳妥的开始。
接下来设置变量的上下界。状态边界限定了飞行高度和姿态角速度范围，控制边界限定了推力和力矩范围。dt的下界要设一个很小的正数，避免积分步长退化到零。
matlab复制% ---------- 决策变量边界 ----------
lb_state = [-inf; 0.2; -inf; -inf; -params.theta_max; -params.omega_max];
ub_state = [ inf; inf; inf; inf; params.theta_max; params.omega_max];
lb_state = repmat(lb_state(:), N+1, 1);
ub_state = repmat(ub_state(:), N+1, 1);
lb_ctrl = repmat([params.Tmin; params.Mmin], N, 1);
ub_ctrl = repmat([params.Tmax; params.Mmax], N, 1);
lb = [lb_state; lb_ctrl; 1e-4];
ub = [ub_state; ub_ctrl; 5.0];
这里把z方向位置下限设成0.2米，是为了避免优化器生成穿过地面的轨迹。x方向没有设上限，因为终点在5米处，飞行范围不需要额外限制。如果你希望轨迹不超出某个走廊，可以继续修改lb_state和ub_state。注意，状态边界是box constraint，直接传给fmincon会让求解速度更快，比写成非线性不等式约束更高效。
3.3 动力学函数与非线性约束实现
规划模型动力学函数drone2D.m非常简洁。它只做一件事：根据当前状态和控制输入算状态导数。
matlab复制%% drone2D.m
function xdot = drone2D(x, u, params)
% 状态 x = [px; pz; vx; vz; theta; omega]
% 输入 u = [T; M]
th = x(5); om = x(6);
ax = (u(1) / params.m) * sin(th);
az = (u(1) / params.m) * cos(th) - params.g;
xdot = [x(3); x(4); ax; az; om; u(2) / params.Iy];
end
这个函数就是规划模型中的旋转动力学部分。它把角加速度直接写成M/I_y，完全忽略了角速度耦合项。在二维纵向模型里这样做是精确的，因为唯一的角速度是俯仰角速度，而绕俯仰轴的力矩与角速度本身没有交叉耦合。如果你做三维扩展，就需要在这个函数里加入ω × (Jω)项，但那样会让优化问题非线性显著增强，求解难度会上升。
非线性约束函数nonlcon需要同时返回等式约束ceq和不等式约束c。这里主要用等式约束做动力学配点，不等式约束暂时为空。初始状态和终端状态也作为等式约束的一部分写进去。
matlab复制function [c, ceq] = nonlcon(z, N, params, x0, xf)
nx = 6; nu = 2;
% 拆出状态与控制
xv = reshape(z(1:nx*(N+1)), nx, []);
uv = reshape(z(nx*(N+1)+1 : nx*(N+1)+nu*N), nu, []);
dt = z(end);
ceq = zeros(N*nx, 1);
% 每个离散时间节点的动力学约束
for k = 1:N
fk = drone2D(xv(:,k), uv(:,k), params);
ceq((k-1)*nx+1 : k*nx) = xv(:,k+1) - xv(:,k) - dt * fk;
end
% 初始状态和终端状态固定
ceq = [ceq; xv(:,1) - x0; xv(:,N+1) - xf];
c = []; % 本版本无非线性不等式约束
end
这段代码最核心的是欧拉积分约束 x_{k+1} = x_k + Δt·f_k。它把动力学等式严格放进优化里。如果想让积分更准确，可以把差分换成梯形法，但代价是下一时刻状态和控制都会出现在约束中，雅可比矩阵更稠密。对于N=40、Δt大约0.1秒的离散，显式欧拉的精度已经足够。实际复现时，我建议先在N=20下跑通，再把N提高到40到60。
3.4 调用fmincon求解与结果显示
主脚本里把目标函数、非线性约束、优化选项串起来。目标函数需要从z里取出Δt和控制序列，计算总时间加控制变化惩罚。
matlab复制% ---------- 目标函数 ----------
obj = @(z) z(end) * N + 1e-4 * sum(diff(reshape(...
z(nx*(N+1)+1 : nx*(N+1)+nu*N), nu, []), 1, 2).^2, 'all');
% ---------- 求解 ----------
options = optimoptions('fmincon', ...
'Algorithm', 'sqp', ...
'Display', 'iter', ...
'MaxIterations', 800, ...
'ConstraintTolerance', 1e-5, ...
'OptimalityTolerance', 1e-6, ...
'FiniteDifferenceStepSize', 1e-6);
[zopt, fval, exitflag] = fmincon(...
obj, z0, [], [], [], [], lb, ub, ...
@(z) nonlcon(z, N, params, x0, xf), options);
几个选项的解释：Algorithm选sqp，适合中等规模带非线性等式约束的问题，比内点法在迭代后期更不容易卡死；FiniteDifferenceStepSize设成1e-6，避免数值微分噪声过大；ConstraintTolerance设到1e-5已经足够。如果你发现求解收敛很慢，可以把MaxIterations先提高，同时把OptimalityTolerance放宽到1e-5，先得到一个参考解再说。
求解完成后，把zopt拆回状态和控制，并生成时间轴。画图时重点看三个信号：轨迹是否光滑、推力是否频繁穿越上下限、俯仰角和角速度是否触及边界。如果推力在大部分时间贴在上下限上，说明确实在做时间最优的bang-bang飞行，这是期望结果。
matlab复制xv = reshape(zopt(1:nx*(N+1)), nx, []);
uv = reshape(zopt(nx*(N+1)+1 : nx*(N+1)+nu*N), nu, []);
dt_opt = zopt(end);
t_axis = (0:N) * dt_opt;
figure;
subplot(2,2,1);
plot(xv(1,:), xv(2,:), 'LineWidth', 1.5);
xlabel('x (m)'); ylabel('z (m)'); title('Trajectory'); grid on;
axis equal;
subplot(2,2,2);
plot(t_axis, rad2deg(xv(5,:)), 'LineWidth', 1.5);
xlabel('t (s)'); ylabel('pitch (deg)'); title('Pitch Angle'); grid on;
subplot(2,2,3);
stairs(t_axis(1:end-1), uv(1,:), 'LineWidth', 1.5);
xlabel('t (s)'); ylabel('Thrust (N)'); title('Thrust'); grid on;
subplot(2,2,4);
stairs(t_axis(1:end-1), uv(2,:), 'LineWidth', 1.5);
xlabel('t (s)'); ylabel('Moment (Nm)'); title('Pitch Moment'); grid on;
用stairs画控制量可以清楚看到每段常值控制的效果。如果你发现控制量末尾出现异常跳动，通常是终端约束和边界约束打架，需要检查终点的姿态是否依然保持在水平状态。
4. 仿真实验与调参经验
4.1 典型算例与结果解读
我用的算例是从(0,1)米悬停飞到(5,1)米悬停，飞行高度不变，物理参数如上。设置Tmin=0.5mg，Tmax=2.0mg，最大俯仰角45度，最大俯仰角速度120度/秒，N=40。实际运行后，最优时间大约在2.4到2.8秒之间，具体数值依赖推力上限和姿态约束。轨迹不是简单的匀加速再匀减速，因为旋转动力学也限制了加速度方向变化的速率。
对比传统用三次多项式规划的轨迹，时间最优轨迹在初始阶段会以最大俯仰角快速获得水平加速度，中间段推力接近上限，接近终点时通过反向俯仰或减小推力来减速。这里有个容易忽略的点：如果不加旋转角速度约束，优化器会生成一个理论上时间更短但姿态角速度接近无穷大的尖峰轨迹。仿真里无人机看起来在平移但姿态几乎瞬间翻转，实际飞控早就饱和。所以θ和ω的边界必须同时在配点法里作为状态边界显式给出，而不是只靠力矩上限去隐式限制。
4.2 约束边界对结果的影响
参数扫描非常值得做。我做了三组对比：第一组把俯仰角放宽到90度，角速度上限提高到180度/秒，总时间会明显下降，但轨迹后半段出现更大的倒转趋势，飞行姿态比较激进。第二组把推力上限从2mg提高到3.5mg，总时间下降约20%，但角速度约束很快变成新的瓶颈，轨迹不再更短。第三组只提高Tmin，允许更大的下降推力，优化器会更倾向用主动下降来做高度切换，这在水平穿越问题里并不明显，但在跨高度起降问题里影响很大。
这种参数化扫描不仅是调参，还能帮你判断当前问题到底被哪个约束卡住。如果总时间随某个界限几乎不变，说明瓶颈在其他约束上；如果总时间对某个界限非常敏感，那就是主导约束。在我这个算例里，最敏感的是俯仰角上限和角速度上限，因为水平方向加速必须靠倾斜姿态，一旦倾斜受限，水平加速度就无法快速建立，时间自然被拖长。
4.3 初值设定与求解器收敛技巧
fmincon对初值非常敏感。一个实用技巧是先解一个“放松约束”版本：把角度和角速度边界放宽到很大，推力上下限也放宽，快速得到一个可行的粗轨迹，然后把该轨迹作为初值重新跑严格约束版本。我管这叫两阶段求解。第一阶段的目的是进入可行域，第二阶段的目的是在严格约束下优化目标。实际经验是，直接跑严格约束很容易报“无可行解”，但先松后紧几乎总能收敛。
另一个技巧是检查所有状态和控制是否做了单位标准化。常见问题是位置是几米，角速度是几弧度每秒，力矩是几牛米，这些量同时在z向量中，fmincon的有限差分步长对所有变量用同一个尺度，很容易出现梯度误差。解决办法是设置FiniteDifferenceStepSize为相对值，或者对目标函数乘以一个缩放系数。我通常把目标函数设成时间秒数，不额外缩放；但如果状态量范围从1e-3到100，就必须考虑预缩放。对于文中这个算例，各个量都保持在1e-1到1e1范围，直接跑没有问题。
5. 常见问题与排查技巧实录
5.1 fmincon提示“No feasible solution found”怎么办
这是复现过程中最常碰到的问题。原因通常不是算法不行，而是约束本身自相矛盾。最常见的矛盾是：初始状态到终端状态之间的距离太长，但推力上限和姿态角上限组合下，无人机在最大时间步长内根本飞不到。此时fmincon会说找不到可行解。
排查顺序可以这样走：先检查初末状态是否正确拼进了ceq。再检查推力上下限是否覆盖悬停推力mg，如果Tmin > mg而Tmax < mg，任何小扰动都无法维持飞行，问题必然无解。然后把dt的上限从5秒调到20秒甚至更大，给优化器更大的可行空间。最后临时关闭角度和角速度约束，只保留推力边界，看是否能找到忽略姿态约束下的可行解。如果可以，说明姿态限制才是瓶颈，再去逐步收紧。
5.2 时间步长与离散点数怎么选
N太小，轨迹被离散得粗糙，优化结果看似时间很短，但还原成连续时间后动力学误差极大，甚至出现某个节点上角速度略微越界。N太大，状态加控制变量快速膨胀，fmincon在Matlab里迭代一次就要几十秒。我在二维问题里的经验是N取40到60，三维完整模型用N=50到80。判断N是否够用有一个标准：把优化得到的轨迹用更小步长重新积分，对比离散节点和积分轨迹的差。如果位置误差超过1厘米，说明N不够；如果超过5厘米，直接加大N。另外，如果你用显式欧拉积分，Δt最好不要超过0.05秒。对于总时间2.5秒、N=50，Δt=0.05秒刚合格。
5.3 双模型校核时常见的陀螺力矩误判
我在用完整旋转动力学校核时间最优轨迹时，发现一个很有意思的现象：规划模型认为力矩没超限，但完整模型一算，俯仰力矩超限了15%。原因不是模型写错，而是规划模型忽略了横滚和偏航通道的角速度耦合。如果轨迹里同时有快速偏航和俯仰机动，机体角速度在机体轴上的交叉项会产生一个额外的表观力矩。严格处理需要把完整欧拉方程放进优化里，但那样求解效率会大幅下降。
我的工程折中方案是：用完整模型离线校核一批典型轨迹，统计出“规划模型角加速度上限”和“完整模型实际可用角加速度上限”之间的比例系数，通常在0.75到0.9之间，然后把规划模型的M上限乘以这个保守系数。这个方法不优雅但非常实用。双模型的价值就在这里：简化模型负责快，完整模型负责准。代码里虽然只写了二维规划模型，但你可以另外写一个rotationVerify.m，把三维欧拉方程拿出来做离线校验。
5.4 代码层面的其他坑
reshape拆分状态和控制时，务必注意状态是按列存储还是按行存储。本文所有状态都是“每列一个节点”，拆矩阵后第一维是状态量，第二维是时间节点。索引错位会直接导致动力学约束出现奇怪残差，排查起来很费时间。fmincon的匿名函数会捕获工作区变量，不要再循环里反复定义nonlcon，否则变量散落一地，调试很痛苦。正则项的权重不要超过1e-2，否则目标函数被平滑项主导，时间不再是主要优化指标。最后画轨迹时，最好用interp1把离散节点加密到1000个点再画，这样可以避免折线拐角误导你判断轨迹是否光滑。
另一个容易被忽略的问题是：直接配点法得到的控制是分段常值序列，如果后续要作为飞控参考输入，必须先做低通滤波或者转换成样条曲线。我在试飞时发现，直接把阶梯状推力指令发给飞控，电调会因指令跳变产生明显的延迟，轨迹跟踪误差在换向时刻会突然增大。所以从优化器输出的原始控制序列，更适合拿去分析问题，而不是直接当真机指令。