多旋翼无人机时间最优轨迹规划：旋转动力学双模型与Matlab复现

多旋翼无人机的时间最优轨迹规划，说白了就是在电机不烧、姿态不失控的前提下，把飞机从A点送到B点并且总时间最短。接触过飞控的人都知道，位置环和姿态环是一个强耦合的整体，但很多轨迹规划教程喜欢把位置轨迹单独拎出来设计，最后生成的轨迹看似平滑，一放到旋转动力学里就暴露问题。我自己在复现“基于旋转动力学双模型的多旋翼无人机时间最优轨迹规划”时，最大的收获是把旋转动力学拆成“规划用简化模型”和“校核用完整模型”两条路线。这篇文章会把建模、离散化、Matlab代码以及调参时的坑都摊开讲，适合正在做无人机轨迹规划、最优控制复现，或者相关毕业设计的读者。

1. 整体设计思路：为什么旋转动力学双模型能治病

1.1 位置轨迹与旋转动力学之间的天然鸿沟

多旋翼的微分平坦模型非常漂亮：把位置轨迹和偏航角当作平坦输出，位置求导得到期望加速度，再逆推出期望姿态和推力方向。这个思路在轨迹跟踪里已经很成熟，但它对“姿态响应”做了很强的理想化假设——认为无人机能瞬间到达期望姿态，或者姿态回路的带宽远大于规划带宽。真实情况下，姿态变化要受到刚体动力学约束，旋转通道的加速度取决于力矩，力矩又取决于电机转速和螺旋桨气动力。如果规划器完全无视这些限制，生成的轨迹可能在几何上没有毛病，真机跑起来却会突然出现掉高、侧滑甚至姿态超调。

我见过不少同学做路径规划时只在二维平面上用B样条或者多项式拟合成一条平滑曲线，后期再加一个时间缩放比例就完事。这种做法的核心问题是“几何路径”和“时间分配”分离得太过粗糙。无人机飞行需要同时满足平移加速度和角加速度，二者通过姿态角耦合在一起。旋转动力学双模型的核心思路，就是把这种耦合放到规划阶段显式建模，而不是等轨迹生成后再靠飞控硬扛。

1.2 双模型的定位：规划模型与校核模型明确分工

这里说的双模型，不是简单地把同一个方程写两遍，而是两套复杂度和用途都不同的模型。第一套我称为规划模型，它把俯仰角、角速度、角加速度看成一条二阶积分链：θ' = ω，ω' = M / I_y。输入是期望力矩M，输出是绕俯仰轴的角加速度。这个模型没有惯量耦合，没有电机延迟，非常适合放进非线性优化器里反复迭代，因为它的约束是线性且解耦的，数值上非常友好。

第二套模型是校核模型，它保留刚体动力学和电机执行器的真实关系。对于三维大机动，绕俯仰轴的力矩并不完全等于I_y乘以角加速度，还包含其他角速度分量的交叉项ω × (Jω)。也就是说，当无人机同时快速滚转和偏航时，俯仰轴实际需要的力矩会偏离简化模型的计算结果。用校核模型可以在仿真里提前发现这个问题，然后把安全系数反馈给规划模型，形成一次迭代闭环。

1.3 适用人群与前置知识

如果你正在做旋翼无人机时间最优轨迹规划、最优控制课程项目，或者单纯想用Matlab复现某个轨迹规划论文，这篇文章至少能帮你少踩三个坑：第一，理解为什么时间最优轨迹不能只用位置多项式拟合；第二，知道fmincon这类通用非线性优化器怎么解最优控制问题；第三，看到一套可以直接复现的离散化与约束写法。前置知识只需要你了解多旋翼刚体运动方程，会Matlab基础数值计算和最优化工具箱，能看懂状态空间模型即可。全文以二维纵向模型作为主算例，但最后会解释如何向三维扩展。

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之间的差乘以滚转角速度和偏航角速度的乘积项。校核模型的作用，就是在轨迹生成后把期望角速度、期望角加速度代进去，看这一项会不