做过多旋翼轨迹规划的人应该都有这种体会位置环的minimum snap轨迹人人都会给但一旦把“时间最优”三个字加进来问题立马变味。因为时间最优不只要让路径平滑更要在每一刻都踩在物理极限上——油门给到多大、机体倾斜多快、姿态能不能跟得上。这个问题的核心瓶颈就是旋转动力学。推力方向不是你说转就能立刻转过去的机体有转动惯量、角速度有上限、力矩有饱和姿态的响应速度直接决定了轨迹在空间里能拐多快。这篇文章我围绕“旋转动力学双模型”来拆解多旋翼无人机的时间最优轨迹规划一套完整旋转动力学模型负责描述真实物理一套简化模型负责给出快速解两者配合迭代逼近全局最优。最后给出Matlab复现的核心框架和我在实际调试中踩过的一堆坑适合正在做无人机轨迹优化、论文复现或者竞赛时间最优科目的朋友参考。1. 难点根源旋转动力学把“轨迹规划”这碗水端平了1.1 位置动力学好解但加速度方向被姿态锁死先说一个经常被新手忽略的事实多旋翼的位置动力学其实很简单本质上就是双积分器。速度是位置的导数加速度是速度的导数你给定一条加速度曲线位置曲线就出来了。如果只做位置层面的平滑minimum snap、minimum jerk这些方法已经非常成熟五阶多项式一拟合轨迹又平滑又好看。但无人机不是一辆可以在平面上自由加减速的小车。它的加速度只有一个来源——螺旋桨推力沿着机体坐标系的Z轴方向产生。你想让飞机向右加速不能直接“向右给油门”必须先把整个机身向右倾斜让推力方向指向右侧才能产生水平方向的加速度分量。也就是说位置轨迹上需要的加速度方向必须由机体的姿态矩阵去承载。姿态方向和加速度方向不是两个独立变量它们之间隔着一层刚体旋转动力学。这也解释了为什么很多纯路径规划算法生成的轨迹“看起来很美飞上去就废”路径上某个弯要求机体在极短时间内从正飞状态倾斜到60度位置环算得再漂亮也没有用姿态根本转不过去。1.2 推力方向不能瞬变带来的连锁反应旋转动力学的本质是一层“积分延迟”。姿态矩阵R的更新由角速度ω决定而角速度的变化又由力矩决定位置层面p → v → a推力直接产生加速度姿态层面R → ω → τ角速度是姿态的导数力矩是角速度的导数。从“想改变推力方向”到“推力方向真的改变”中间隔着角速度的一次积分。当机体转动惯量较大、电机力矩余量不足时这一层积分延迟会非常明显。我用开车的例子类比过很多次位置规划相当于决定“车头朝哪个方向走”旋转动力学相当于方向盘本身还有惯性你打死方向车头不是立刻转过去而是有一个转动的过程。如果规划的时候无视这个过程生成的轨迹就要求机体在0.1秒内横滚60度实际电机根本给不出那么大力矩飞控一限幅整条轨迹立刻失真。1.3 时间最优天然要求“每一刻都踩在极限上”时间最优和普通平滑轨迹最大的区别在于它不追求“留余量”而是追求“不浪费”。最短时间意味着每一段都至少有一个约束被激活要么油门推到最大要么姿态角速度到达上限要么力矩饱和。这时候旋转动力学的地位就变得极其关键。如果忽略姿态动态时间最优解会给你一条极其激进的方向切换轨迹——上一秒还在朝左全力加速下一秒要求推力方向瞬间翻到右侧。真实飞行器做不到因为机体需要在两个方向之间完成一次完整的“倾斜—回正”机动这个机动本身要消耗时间。所以多旋翼时间最优轨迹规划的第一个核心认知就是这不是一个纯位置问题而是一个“姿态先行”的耦合问题。你要规划的不只是路径和时间还有机体姿态怎么转、以多快的角速度转、在哪个时间段转。这就是旋转动力学进入轨迹规划建模的直接原因。2. 双模型到底怎么定义完整模型与简化模型的数学差异2.1 完整旋转动力学模型模型A的状态与控制完整的旋转动力学模型把机体当作刚体状态向量包含位置、速度、姿态和角速度。用四元数表示姿态状态一共13维x [p_x, p_y, p_z, v_x, v_y, v_z, q_w, q_x, q_y, q_z, ω_x, ω_y, ω_z]动力学方程写成连续形式p_dot v m * v_dot f * R * e3 - m * g * e3 q_dot 0.5 * q ⊗ [0, ω] J * ω_dot τ - ω × (J * ω)这里的输入是总推力f和体轴系下的力矩τ。实际轨迹规划里很多人为了方便直接拿体轴角加速度ω_dot当输入因为力矩约束可以换算成角加速度约束等价于假设内环力矩响应足够快。工程上这么处理是合理的只要把ω_dot的限幅选得保守一点给内环留出响应余量。完整模型的约束条件总推力0 ≤ f ≤ f_max体轴角加速度|ω̇_i| ≤ ω̇_max各轴独立限制机体姿态隐含在四元数单位约束里q^T q 1。2.2 简化旋转动力学模型模型B去掉哪一层简化模型模型B的出发点是既然完整模型难解那把“角速度积分”这一层去掉直接假设推力方向的变化率可控。也就是让角速度本身变成控制量相当于认为内环角速度响应无限快——你要求机身以多大角速度转动它立刻就能以多大角速度转动。模型B的状态只需要位置、速度和姿态共10维x [p_x, p_y, p_z, v_x, v_y, v_z, q_w, q_x, q_y, q_z]动力学方程p_dot v m * v_dot f * R * e3 - m * g * e3 q_dot 0.5 * q ⊗ [0, ω]这里的输入是总推力f和角速度ω约束变成总推力0 ≤ f ≤ f_max角速度幅值|ω_i| ≤ ω_max。对比一下就很清楚模型B把“角速度的导数”这一层积分去掉了控制变量从ω̇降到了ω系统的相对阶降低了一阶非线性程度也下降了一阶。代价是结果偏乐观——它假设姿态变化不需要建立角速度的时间相当于忽略机体转动惯量。2.3 两套模型的时间最优值之间的关系两套模型解出来的最短时间T满足一个非常重要的偏序关系T_B ≤ T_A*其中T_B是简化模型求出的最短时间T_A*是真实完整模型的最优时间。这个关系成立是因为模型B的约束集比模型A更宽松模型B能做到的机动模型A不一定能做到模型A的最优轨迹在模型B里一定可行所以模型B的可行域包含了模型A的可行域最小化时间一定只小不大。这个性质是双模型迭代算法的理论基石。它告诉你简化模型的结果不是瞎猜它是一条严格的下界。有了下界你就可以衡量完整模型的解到底还有多少优化空间如果完整模型当前可行解的时间等于T_B那它一定就是全局最优。我在实际项目里把这两个模型分别封装成两套不同的代码接口因为它们的输入维度、约束形式、求解策略完全不同。一个解决策变量的“形态”一个解决策变量的“精度”合在一起才是完整的双模型思路。3. 时间最优的数学本质为什么最优解是“地板油满舵”的方波3.1 极小值原理下的bang-bang结构时间最优的最优控制问题有一个非常著名、也非常符合直觉的结论最优控制量要么在最大值要么在最小值很少待在中间。这个结论来自庞特里亚金极小值原理——当你写出哈密顿函数控制量在哈密顿函数里是以线性形式出现的线性函数在闭区间上的最小值只能落在端点除非前面的系数恰好为零。落到多旋翼上就是总推力f几乎一直在f_max和f_min之间切换中间值只在非常特殊的奇异弧上出现角加速度ω̇也是类似要么给到正向极限要么给到负向极限。这看起来和赛车开法一模一样最省时间的开法就是油门踩到底、刹车踩到底、方向打满因为任何“留余量”的操作都是在浪费时间只有把物理约束全部激活才能把时间压到极限。我在第一次看到数值解的时候也吓了一跳——最优推力曲线根本不是平滑的而是方波。起飞段油门拉满中段收油减速末段再补一脚油门修正姿态整条曲线像脉冲信号一样。这不是求解器抽风而是时间最优的本征结构。3.2 奇异弧什么时候油门不能推到顶当然bang-bang不是时间最优的全部。极小值原理里还有一种特殊情况叫奇异弧控制量前面的系数恰好等于零这时哈密顿函数对控制量的依赖消失了控制量可以取中间值而不影响最优性。放在多旋翼时间最优里奇异弧对应的情况大致是推力大小恰好与重力沿机体某个方向的分量保持平衡同时速度方向沿着一条特定的最优流形滑行。这时候油门位置既不在最大也不在最小而是维持在一个中间平衡点让飞行器以恒定的加速度状态滑行。奇异弧在理论上很有研究价值但在数值求解里经常是以“切换点附近的抖动”呈现的。你会在最优解的推力曲线上看到bang-bang段之间有一段高频震荡的过渡那就是数值解在逼近奇异弧。实际处理时我建议不要刻意去追奇异弧而是给目标函数加一个很小的正则项比如ε倍的推力平方积分让求解器在奇异段附近自然平滑。这样虽然严格最优性差一点点但工程上避免了求解器在切换点附近反复震荡导致不收敛的问题。3.3 旋转动力学耦合下的方向切换动作旋转动力学进入时间最优后方向切换的形态和固定翼或者小车完全不同。小车转向可以瞬间改变速度方向无人机不行——它必须先让机身倾斜推力方向偏转然后水平加速度才开始建立。要换方向就必须经历“先朝一个方向倾再朝另一个方向倾”的完整过程。在最优轨迹上的体现就是你经常能看到一个大S弯或者回环动作。机体先朝一侧倾斜积累一个反向角速度然后用力矩把角速度压回来推力方向才能掉头。整个过程角速度曲线是典型的梯形或三角形前半段正向饱和后半段负向饱和。这个阶段就是完整旋转动力学模型和简化模型差异最大的地方。简化模型会说“角速度直接给到最大就行”完整模型却要说“你角速度从0建立到最大也需要时间”。双模型迭代的时候这个差异会直接体现为力矩约束违和——简化模型的轨迹给到完整模型里校验往往就是这一段不满足力矩上限。4. 双模型迭代求解流程下界先行、逐轮修正4.1 为什么不能直接硬解完整模型可能有人会问既然完整模型最准确为什么不直接在完整模型上做时间最优一步到位我在最早的时候也是这样想的但实际算下来发现两个致命问题。第一完整模型的状态包含旋转矩阵或四元数姿态流形SO(3)是一个非凸流形加上ω̇这个输入的非线性耦合项整个问题是一个高维非线性最优控制问题全局求解基本不可能。用数值优化器硬解极度依赖初始猜测初值给不好收敛出来的就是一个局部最优甚至压根收敛不了。第二完整模型的最优解是“姿态层和位置层的强耦合解”需要同时调整姿态轨迹和时间分配。直接在全状态空间里搜索求解器的大部分算力都浪费在维护姿态约束一致性上收敛速度极慢。双模型思路的本质是用简化模型跑全局用完整模型做修正。简化模型结构好适合快速给出一个全局近优解完整模型精度高适合在局部做精确修正。两者迭代各取所长。4.2 算法主循环乐观解、可行性校验、割平面更新我在项目里实现的迭代主循环可以写成下面这样的流程初始化T_lb 0T_ub ∞额外约束集合C为空求解简化模型B附加约束集合C得到最短时间T_B和对应轨迹ξ_B将ξ_B的前馈轨迹代入完整模型A做仿真校验检测力矩约束是否越界并统计最大违和量violation如果violation小于容差说明ξ_B在完整模型里可行此时T_B就是精确最优直接输出否则在越界最严重的时间片段附近添加“割平面约束”——限制该时段内姿态变化的剧烈程度或角速度变化率的上限带着新增约束重新回到第二步重复直到收敛。这个循环里最关键的工程细节是步骤5的“割平面”怎么加。我在实际代码里不是简单粗暴地缩小ω̇_max而是检测到哪个时间段的姿态变化斜率过高就把该时段相邻节点之间的姿态变化量设一个显式上限相当于告诉优化器“这段你们想法激进过头了完整模型做不到。”每迭代一轮T_B会单调上升约束越加越多可行域越小T_ub会单调下降不断用更合理的轨迹去做完整模型的局部优化两者之间的间隙gap T_ub - T_B也在缩小。当gap小于设定阈值比如0.05秒就可以确认当前解已经足够接近全局最优。4.3 迭代收敛判据与工程终止条件收敛判据我用两个指标同时把关可行性完整模型仿真的全轨迹力矩违和量接近零最优间隙T_ub与T_B的差值小于某个阈值。实际项目中我见过不少只查可行性就收手的做法——轨迹在完整模型里跑得通但时间可能比最优值大不少。倒也不是不能用只是如果你真的在做“时间最优”那至少要有意识地看一下T_b的下界知道自己离理论最优还有多远。工程上还有一个更务实的终止条件如果连续三轮迭代的时间改善小于0.2%直接停。因为再迭代下去收益微乎其微浪费算力。我一般会把迭代上限设在5轮左右大部分问题3到4轮就能收敛只有姿态要求特别苛刻的任务才需要跑到第5轮。5. Matlab复现代码框架状态定义、时间归一化与求解器5.1 状态向量与动力学函数Matlab复现的第一步是把状态向量和动力学函数写清楚。我惯用的状态排序是位置、速度、四元数、角速度% 状态 x [px py pz vx vy vz qw qx qy qz wx wy wz] function dx dynamics_full(x, u, params) p x(1:3); v x(4:6); q x(7:10); omega x(11:13); f u(1); % 总推力 omega_dot u(2:4); % 体轴角加速度 R quat2rotm(q); % 四元数转旋转矩阵 e3 [0; 0; 1]; % 位置与速度更新 p_dot v; v_dot f * R * e3 / params.m - params.g * e3; % 四元数运动学q_dot 0.5 * q ⊗ [0, omega] q_dot 0.5 * quatmultiply(q, [0, omega]); % 角速度更新简化模型没有这一项 omega_dot_real omega_dot; % 直接用限幅后的角加速度 dx [p_dot; v_dot; q_dot; omega_dot_real]; end这里有个很容易踩的坑四元数转旋转矩阵之后要确认你的旋转矩阵乘的是列向量还是行向量。MATLAB的quat2rotm返回的矩阵约定是R * v而有些工具箱是v * R搞反了推力方向直接指向天上轨迹完全不对。我在项目里踩过一次花了半天才通过对比重力项方向发现是这个问题。5.2 时间归一化把T变成优化变量而不是网格边界时间最优问题里总时间T是未知数。如果直接在真实时间轴上离散网格间隔Δt本身也是变量处理起来很麻烦。标准做法是时间归一化。令s ∈ [0, 1]为归一化时间真实时间t T * s。对s求导要用链式法则dx/ds T * f(x, u)这样处理后网格节点固定为s_10, s_21/(N-1), ..., s_N1优化变量里多出一个T目标函数直接写成minimize T时间归一化有几个好处。第一T作为标量变量直接进入目标函数求解器的梯度计算很简单第二网格固定之后所有节点上的约束布置一次就能复用第三给不同猜测时间T0的初值优化器可以通过调整T来缩放整条轨迹的时间尺度初值给到1.5倍最优值也能慢慢拉回来。代价也很明显动力学方程右侧全部乘了T数值敏感度提高尤其是T初值给得离谱时导数会很大导致求解器早期迭代直接发散。建议初始化T时用一个保守估计比如按位置距离除以最大速度的2倍给个初值。5.3 离散化与约束组装我用的是多点打靶法multiple shootingN个节点每个节点上有状态x_i和输入u_i相邻节点之间用积分器连接。离散化之后约束分三类动力学连接约束x_{i1} integrate(x_i, u_i, T/N)写成等式约束输入饱和约束f_min ≤ f_i ≤ f_max|ω̇_i| ≤ ω̇_max终端约束p_N p_goalv_N 0q_N q_hoverω_N 0。约束组装的Matlab骨架如下function [c, ceq] constraints(z, params) % 从z中拆出状态序列、输入序列和T [X, U, T] unpack_z(z, params); ceq []; % 动力学连接约束 for i 1:params.N-1 x_next rk4(dynamics_full, X(:,i), U(:,i), T/(params.N-1), params); ceq [ceq; X(:,i1) - x_next]; end % 终端约束 ceq [ceq; X(1:3,end) - params.p_goal]; ceq [ceq; X(4:6,end)]; % 终端速度为零 ceq [ceq; X(11:13,end)]; % 终端角速度为零 % 不等式约束输入限幅 c []; for i 1:params.N c [c; params.f_min - U(1,i); U(1,i) - params.f_max]; c [c; abs(U(2:4,i)) - params.omega_dot_max]; end end连接约束我强烈建议用四阶龙格库塔RK4而不是欧拉法。欧拉法在这种高动态时间最优问题上会引入数值耗散导致最优轨迹出现不自然的振荡RK4虽然计算量大一点但约束一致性明显更好。四元数单位约束q_i^T q_i 1也要加到等式约束里否则优化器可能偷偷让四元数缩放来“作弊”满足动力学得到的结果无法转回旋转矩阵。5.4 求解器选择与目标函数细节求解器我用过两条路线纯Matlab路线fmincon配interior-point或sqp算法好处是不用装额外工具包坏处是速度和收敛性都一般适合N20左右的小规模问题CasADi IPOPT路线CasADi自动求导加IPOPT求解非线性规划效率和稳定性都远超fminconN50都能在几十秒内收敛。强烈推荐。目标函数除了T之外建议加一个非常小的正则项J T 1e-3 * sum(U(:).^2)正则项不是为了改变最优解而是为了给奇异弧附近的自由度提供一点曲率帮助求解器避免在切换点附近来回震荡。量级要控制好太大会把bang-bang结构磨平太小则起不到稳定作用。我实际调试时通常从1e-4开始试观察推力曲线如果出现无意义的高频抖动就加大到1e-3。6. 复现过程中真正容易翻车的五个地方6.1 四元数漂移与SO(3)约束被破坏四元数做姿态表示虽然无奇点但有一个烦人的问题优化迭代过程中四元数的模会漂离1。一旦模长偏离旋转矩阵就不再是严格的正交矩阵推力方向计算就会出错整个轨迹的物理含义跟着崩。我遇到过一次特别隐蔽的约束只在终端加了四元数单位约束中间节点全靠动力学方程连接结果中间节点的四元数模长慢慢缩到0.7等发现的时候整条轨迹已经变成“半平面上的幻想解”。解决方法是每个节点都加单位约束不要只加首末两端。每轮迭代结束后再做一次四元数归一化并重新初始化把漂移控制在极小范围内。6.2 初始猜测不好时间最优会变成“时间次优”非线性规划对初值极其敏感时间最优问题尤其严重。我试过给一个线性插值的初始轨迹fmincon直接陷在局部最优里出不来求解出来的轨迹比双模型下界T_B大了接近一倍而且形状很怪像是在故意绕路。后来我把初始猜测改成两段式先用简化模型B求一个粗糙的全局解把其中的姿态轨迹和推力曲线拿出来作为完整模型A的初值。这样完整模型的局部优化起点已经非常接近最优流形收敛速度和解的质量都有质的提升。这算是双模型框架带来的一个“额外红利”——它本身就是最好的初值生成器。6.3 节点数、容差和计算时间的三角平衡节点数N是时间最优复现里最需要权衡的参数。N太小轨迹的时间分辨率不够最优解里的bang-bang切换点对不齐网格节点推力曲线看起来像缺了一块的方波N太大约束维度爆炸求解时间翻着倍往上走。我的经验值普通点对点机动N取31到41带障碍物的场景取51到61。判断N是否够用的简单办法是看推力曲线切换点附近是否出现阶梯状——如果出现说明网格不够密切换点的精确位置没有被网格捕获。求解器容差也不要一上来就设到1e-8。我在调试阶段用1e-4的约束容差等轨迹基本成型再收紧到1e-6效率能差好几倍。6.4 单位制和量纲混乱这个坑说出来有点丢人但必须单独写一段。我曾经在复现一个公开算法时发现解出来的时间T总是比别人大5倍查了半天发现是角速度把rad/s写成了deg/s而推力、质量、重力全部用的国际单位。单位不统一导致惯性项和重力项的量纲尺度差出两个数量级优化器为了平衡量纲把时间拉长了一倍多。做轨迹规划类代码我建议在文件开头统一做一次单位定义所有物理量一律进入求解器前转成SI制输出可视化时再转回习惯单位。角速度统一用rad/s注意2π≈6.28这个换算系数很容易在姿态约束和力矩约束之间埋下隐患。6.5 电机延迟与力矩饱和的建模层次仿真里如果把ω̇直接当控制输入等价于假设电机和内环可以瞬间产生所需的角加速度。真实无人机上电机有一阶延迟电调有力矩饱和内环的角速度环也有限带宽。时间最优轨迹正是把系统推到极限的轨迹这些延迟和饱和在和极限轨迹叠加时会产生明显的跟踪误差。我在往真机上移植之前都会先把ω̇_max的取值打一个折扣——比如理论算出来是8 rad/s^2我实际设成6 rad/s^2给内环留出30%左右的响应余量。这样整条轨迹虽然在数学上不是“绝对时间最优”但在真实飞行器上反而往往更快因为跟踪误差减小了不会出现飞出预期路径后被迫减速修正的情况。这个经验也和双模型的思想一致模型B保留乐观估计模型A提供物理约束的真实边界而当你对接真实飞控时还需要再加一层“工程余量模型”把飞控内环的能力纳入进来。我个人在多次复现和实飞验证之后的体会是双模型迭代真正的价值不只是数学上的“下界—修正”框架而是它让你的算法流程里始终有一条物理可信的轨迹兜底。简化模型的乐观解给方向完整模型的校验给底线两者之间的gap就是你理解这个系统的时间。如果你正在复现类似论文我建议你这几个部分都保留迭代过程里的中间轨迹也画出来看一眼——当你看到第一轮的乐观解如何逐轮被“按回”物理可行域的时候你会对多旋翼的时间最优问题有比公式推导深刻得多的理解。