复现这套基于旋转动力学双模型的多旋翼无人机时间最优轨迹规划的MATLAB代码起因其实很朴素我希望让多旋翼从A点飞到B点的用时尽量短。结果点质量模型给出的时间最优解第一拍就要求姿态从0度瞬间偏到40多度——飞控再激进也追不上这种目标。后来我把旋转动力学作为第二个模型耦合进规划器才第一次看到一条算出来就能飞出来的时间最优轨迹。这篇文章就以MATLAB代码复现为主线把背后的公式推导、离散化、求解和验证完整过一遍适合正在复现论文、做无人机运动规划或者在fmincon里被非线性约束折磨的读者。先把这个题目的含义说清楚。多旋翼轨迹规划如果只看位置、速度和加速度那就是一个点质量模型适合航点规划这类宏观任务但一旦要做到时间最优姿态这个看不见的状态立刻变成主角——因为加速度矢量就是推力轴给的而推力轴转多快由旋转动力学决定。双模型的意思就是让规划器同时看到平动和旋转这两套动力学约束而不是把姿态当作瞬时通道。1. 为什么点质量模型算出的最短时间一到真机就变形1.1 外环算轨迹内环飞姿态旋转动力学在哪里介入多旋翼的飞控普遍是分层结构外环位置环输出期望加速度翻译成期望姿态角交给内环姿态环姿态环再输出电机转速指令。轨迹规划器工作在加速度这一层只要加速度变化不疯狂这种翻译就没问题。但时间最优问题不一样——目标是最小化时间优化器会把所有约束全部推到边界加速度的变化率会被逼到极限姿态指令自然出现跳变。旋转动力学在这里第一次登场姿态角速度有上限姿态角不能跳加速度矢量因此不能瞬间转向。我复现时先跑了一个最简单的3D算例从悬停状态飞向正前方2米终点回到悬停。点质量模型假设加速度大小在0.1g到2g之间最优解是典型的bang-bangt0时刻加速度从0一步跳到最大值。水平加速要求的姿态是一个绕y轴的倾转夹角约等于atan(a_x/g)算下来大约45度。点质量模型认为这一跳是零成本的0毫秒完成。但如果机体的最大角速度是5 rad/s约286度每秒这是小型多旋翼一个很常见的手感上限45度倾转至少需要(pi/4)/5约0.157秒。对一条总时长不到1秒的轨迹来说这个看不见的姿态环节占了快两成的时间。1.2 复现代码时要主动绕开瞬时姿态假设把姿态当瞬时通道是这类复现代码最普遍的失真来源。很多论文复现版本在翻译期望加速度时直接反解姿态角然后假设内环瞬间跟上。对于minimum snap这类平滑轨迹这个假设误差不大但在时间最优场景内环的动态会显著拉长实际飞行时间甚至导致起始段的俯仰超调和弧线实测到达时间比规划值长得多。正确做法是承认加速度矢量不能瞬间转向。它的转向速率就是机体旋转角速度而角速度有物理上限。这样旋转动力学就不是外环的事后检查而是规划器本身的一条硬约束。接下来我把这条硬约束的数学形式推出来。2. 双模型的数学核心旋转动力学如何变成加速度侧的硬约束2.1 平动模型与旋转模型各自的数学表达平动模型很简单。取世界系z轴向上e3[0;0;1]机体推力方向单位向量是n机体z轴在世界系里的投影。平动方程是m * p̈ T * n - m * g * e3令ap̈定义比力矢量σ a g*e3。注意这个σ非常重要它的大小就是推重比T/m方向始终与推力轴n共线。换句话说σ就是加速度平面里我们能直接看到的推力向量。旋转模型则负责描述n的转向。有机物运动学关系ṅ ω × n其中ω是机体角速度。真正把力矩和角加速度连起来的是欧拉方程J*ω̇ -ω×J*ω M但做轨迹规划时内部力矩不是我们想要的决策变量通常只保留一层角速度上限约束||ω|| ≤ ω_max。这个上限来自电机转速余量、旋翼气动力矩和姿态环带宽飞控手册或实验标定里都能拿到。2.2 核心恒等式jerk的横向分量就是机体旋转角速度对σ求时间导数得到jerk加速度的变化率j σ̇ (T/m) * n (T/m) * (ω × n)第一项沿推力轴方向只改变推力大小第二项垂直于推力轴来自推力轴的旋转。把j拆成沿n的分量和垂直分量垂直分量的模就是||j_perp|| |σ| * ||ω × n|| ≤ |σ| * ω_max这就是把旋转动力学压缩进规划器的桥。原来要约束的机体角速度变成了对加速度矢量变化率在横向上的约束。物理含义非常直观推力越大同样的角速度能产生更大的横向jerk反过来低推力状态下加速度矢量的转向能力也下降。这个耦合关系纯点质量模型完全看不见。我复现时常用的一组参数是σ1.5g、ω_max5 rad/s横向jerk上限约为1.59.81573.6 m/s3。单独看这个数字很大但对一条总时长1秒、速度要改变好几米每秒的轨迹横向jerk会在整个加速段持续起作用绝不能当摆设。2.3 双模型时间最优问题的完整定义把上述两套模型放进同一个最优控制问题状态取x[p;v;a]共9维控制取jerkj共3维。目标是最小化末端时间T。动力学是三个积分器ṗv, v̇a, ȧj。约束有三类推重比上下限σ_min ≤ ||a g*e3|| ≤ σ_max对应油门上下限旋转动力学约束||j_perp|| ≤ ||a g*e3|| * ω_max端点条件起点和终点都是悬停位置给定速度和加速度为零。严格说j还有沿n的分量对应推力大小的变化率受电机转速变化率限制。工程上要么加一个松一点的jerk模长上限要么直接忽略。我复现时给j_parallel加了一个较宽松的上限避免求解器给出油门疯狂抖动的解。所以说双模型的含义就是这么直白同一个优化问题里同时装着平动模型和旋转模型。平动模型提供状态演化旋转模型提供能力约束。如果你的规划器显式地规划四元数和角速度那是另一种把旋转状态完全展开的做法精度更高但状态维数更大、求解更难。我下文采用的是把旋转动力学做成约束的紧凑形式这也是工程复现里性价比最高的配方。3. 时间最优问题放进fmincon多段打靶、变量打包与约束实现3.1 直接转录 vs 庞特里亚金边值问题为什么选前者时间最优问题的经典解法是庞特里亚金极小值原理能推出bang-bang开关结构然后解两点边值问题。但多旋翼这里不仅有推力大小上下界还有横向jerk的锥形约束开关结构的切换时间和次数不是先验已知的用打靶法猜初始协态非常痛苦。直接转录把状态和控制直接作为优化变量交给SQP这类非线性规划求解器处理。约束复杂一点也就是非线性约束工程上省心很多。fmincon虽然不是最快的但MATLAB原生只要Optimization Toolbox装好就能跑不需要额外依赖。如果想更快可以考虑CasADi加IPOPT但那偏离了附MATLAB代码的主题。复现时我用的是fminconR2022b之后版本的SQP算法数值表现都足够稳定。3.2 时间归一化与优化变量的打包方式末端时间T是未知量直接用变步长离散会让变量网格跟着变化。标准做法是把真实时间写成t T * ττ∈[0,1]每段真实步长hT/NN为段数。状态x_k在节点k上控制u_k在第k段内保持常值。全部优化变量z由节点状态、段控制和时间T组成。N 40; nx 9; nu 3; nz (N1)*nx N*nu 1; % 最后一个变量是 T idxX (k) (k*nx 1 : (k1)*nx); % x_k, k 0..N idxU (k) ((N1)*nx k*nu 1 : (N1)*nx (k1)*nu); % u_k, k 0..N-1 idxT nz; % T 在最后一个位置状态取[p;v;a]控制是jerk。N40时变量总数是4194031490对fmincon来说算中等规模直接在桌面级机器跑是可行的。3.3 约束函数实现动力学、端点、推重比与横向jerk接下来是核心部分。多段打靶的等式约束保证相邻节点的状态通过RK4积分连接同时压住起点和终点条件不等式约束压住推重比和横向jerk。我用一个nlcon函数统一返回不等式c和等式ceqfunction [c, ceq] nlcon(z, prm) nx prm.nx; nu prm.nu; N prm.N; g prm.g; wmax prm.omega_max; smin prm.sigma_min; smax prm.sigma_max; idxX prm.idxX; idxU prm.idxU; idxT prm.idxT; T z(idxT); h T / N; % 等式约束起点 N段打靶连续性 终点 ceq zeros((N2)*nx, 1); ceq(1:nx) z(idxX(0)) - prm.x_start; % 起点 for k 0:N-1 xk z(idxX(k)); uk z(idxU(k)); xk_next rk4(xk, uk, h, prm); ceq((k2)*nx1:(k3)*nx) xk_next - z(idxX(k1)); end ceq((N1)*nx1:(N2)*nx) z(idxX(N)) - prm.x_goal; % 终点 % 不等式推重比上下限 横向jerk约束 c zeros(3*N, 1); for k 0:N-1 xk z(idxX(k)); uk z(idxU(k)); a xk(7:9); sigma a [0;0;g]; sig norm(sigma); nvec sigma / sig; j_perp uk - (uk*nvec) * nvec; c(3*k1) smin - sig; % σ 下限 c(3*k2) sig - smax; % σ 上限 c(3*k3) norm(j_perp) - sig*wmax; % 旋转动力学约束 end endRK4积分器很简单动力学本身就是三重积分器function xn rk4(x, u, h, prm) f (xq) [xq(4:6); xq(7:9); u]; % ṗv, v̇a, ȧju k1 f(x); k2 f(x h/2*k1); k3 f(x h/2*k2); k4 f(x h*k3); xn x h/6*(k1 2*k2 2*k3 k4); end横向jerk约束写成norm(j_perp) - sig*wmax 0等价于说垂直于推力轴的jerk分量不能超过当前推力允许的角速度上限。这个约束是非凸的但作为不等式交给SQP配合好的初始猜测可以稳定收敛。注意ceq的长度是(N2)*nx起点、N个打靶残差、终点各占一块千万别把最后一个动力学残差和终点约束写到同一个索引上否则约束会被覆盖掉。3.4 目标函数就是T主脚本结构与求解调用时间最优的目标函数刻意保持纯粹就是最后一个变量T本身不加任何正则项。加了正则项求出来的就不是严格时间最优而是带平滑折中的时间最优复现论文时对不上号的。主脚本调用fminconz0 buildInitialGuess(prm); % 直线插值初始猜测下面会讲 options optimoptions(fmincon, Algorithm, sqp, ... Display, iter, MaxIterations, 800, ... OptimalityTolerance, 1e-7, ConstraintTolerance, 1e-7, ... FiniteDifferenceStepSize, 1e-6); sol fmincon((z) z(idxT), z0, [], [], [], [], [], [], ... (z) nlcon(z, prm), options);初始猜测函数我这样写function z0 buildInitialGuess(prm) x_start prm.x_start; x_goal prm.x_goal; N prm.N; nx prm.nx; nu prm.nu; nz (N1)*nx N*nu 1; z0 zeros(nz, 1); for k 0:N tau k/N; z0(prm.idxX(k)) x_start tau * (x_goal - x_start); end % T初值按点质量最短时间粗略估计再放大 L norm(x_goal(1:3) - x_start(1:3)); T_lb 2 * sqrt(L / (prm.smax - prm.smin)); z0(prm.idxT) 1.5 * T_lb; end直线插值状态作为初始猜测很粗糙但对多段打靶来说足够给SQP一个不炸的出发点。真正影响收敛的是后面要讲的量纲、同伦和求解器选项。4. 决定fmincon能不能收敛的三个细节量纲、初始化与求解器选项4.1 量纲差异会让SQP的线搜索白跑fmincon的默认容差对梯度范数敏感。这个问题的变量量级跨度很大位置是米级速度是米每秒级加速度是10量级jerk轻松上到几十上百时间又是1秒级。变量之间差三个数量级SQP的线搜索经常把迭代步长浪费在不重要的方向上。我的做法是做无量纲化。先定特征长度L0比如2米和特征时间T0比如1秒所有变量除以对应特征量位置除以L0速度除以L0/T0加速度除以L0/T0^2jerk除以L0/T0^3。这样全部变量基本都在0.1到10之间。求解完成后再把结果按原单位恢复。无量纲不改变最优解但能显著减少fmincon在数值野区的挣扎。4.2 同伦初始化从宽松约束一步步收紧直接拿真实ω_max去跑fmincon经常在初始点就报告不可行。这是时间最优问题的一个典型麻烦靠近边界的地方可行域非常狭窄直线初始猜测根本不在域里。可靠的做法是两阶段同伦。先设ω_max50 rad/s相当于基本不管旋转动力学让问题退化成普通的点质量时间最优先跑出一个可行轨迹然后把它作为ω_max5 rad/s的初值通常一两轮就收敛了。代码就是一层循环w_list [50, 20, 10, 5]; % rad/s从松到紧 z0 buildInitialGuess(prm); for wi w_list prm.omega_max wi; [z, T_opt, exitflag] fmincon((z) z(idxT), z0, [], [], ... [], [], [], [], (z) nlcon(z, prm), options); if exitflag 0 warning(当前 ω_max%.1f 未收敛, wi); end z0 z; % 用上一轮的收敛解继续 end推重比约束同理先放宽到0.5g到3g跑通后再收紧到目标范围。经验是每一步松弛量不要超过上一轮约束值的两倍否则SQP容易从可行域里跳出去。这个技巧花费的代码量很小但对收敛成功率提升极大。4.3 SQP还是interior-point以及要不要给解析雅可比我的默认组合是SQP加有限差分。优点是对中等规模稠密约束SQP在边界上推进快缺点是每次迭代都要重新计算全部约束的数值雅可比N40、3D情况大概几秒一次可以接受。interior-point对大规模稀疏问题更合适但在fmincon里你不太容易显式给出稀疏结构反而容易更慢。关于雅可比如果追求稳定把约束雅可比写成解析形式效果很好代价是公式复杂。一个折中是只给动力学等式约束写解析雅可比不等式继续用有限差分。我复现时写过一次RK4的解析雅可比后来觉得收益一般——这个问题的瓶颈通常在于拉回可行域而不是梯度精度。如果你决定写解析梯度一定记得打开CheckGradients选项检查一遍。我曾经在v和a的索引上写错一个偏移量导致等式约束梯度错误收敛到一个物理上不可能的轨迹光看结果根本发现不了只有梯度检查能抓出来。5. 复现结果的验证方法与四个典型翻车现场5.1 时间最优解应该贴在约束上时间最优的本质是全程把某个约束推到边界。跑完之后把σ曲线和横向jerk松弛量norm(j_perp) - sig*wmax画出来检查。时间最优解的大部分片段应该等于0或非常接近0只有起止段因为端点悬停条件需要滑入。如果解在中间有一大段完全远离约束大概率是初始化太差落进了某个内部局部解或者目标函数被加过东西。另一个容易误判的点加速度曲线在时间最优下通常表现出bang-bang的极限环但加上旋转动力学之后起始段会出现一个平滑的倾转建立过程这是对的不是bug。横向jerk贴着上限的那几段对应真机上快速压杆再回杆的动作这正是旋转动力学参与规划后的自然结果。5.2 离线验算ω_req和ode45全状态仿真规划层只输出p、v、a怎么验证姿态可行性呢我习惯在跑完全状态仿真前先做一层离线验算。对每个时刻由σ算出期望机体z轴n_des σ / ||σ||再用中心差分得到n_des的变化率那么需要的机体角速度就是ω_req cross(n_des, n_des_dot)。sigma a_sol [0;0;9.81]; % a_sol 是各节点的加速度按行排列 nvec sigma ./ vecnorm(sigma, 2, 2); dt T_opt / N; ndot gradient(nvec, dt); w_req cross(nvec, ndot, 2); % 需要的机体角速度 violation max(vecnorm(w_req, 2, 2) - prm.omega_max, [], all);这里有个细节cross(nvec, ndot)算出的ω_req天然垂直于n对应的是俯仰和滚转角速率。如果轨迹还带偏航真实总角速度等于ω_req加上偏航角速度再乘n验证时要把偏航项加回来。很多复现翻车就是漏了这一步。离线验算通过之后再用ode45把完整动力学p̈ (T/m)*n - g*e3和J*ω̇ -ω×J*ω M一起积分姿态环用简单的PD就好。仿真结果只要轨迹误差小于规划网格的离散误差就说明双模型约束没有白加。5.3 复现中我踩过的四个坑现象根因解法第一步就报不可行σ接近零导致nvec sigma/sig除零或推力下限设在悬停点附近导致边界过紧推力下限离g留5%-10%裕量给σ加一个小epsilon防止除零收敛到一个对称但明显慢的解初始猜测给成完全对称的直线SQP停在鞍点附近用点质量bang-bang解或同伦法作为初始猜测轨迹高频抖动或锯齿N太少或者推力上限太紧导致多个局部极值增大N或把上一轮收敛解插值后作为新初值贴边检查不过横向jerk约束实现时把norm(j_perp)写成了norm(j)检查投影实现j_perp j - (j*n)*n我曾经就在这里栽过跟头第四条值得多说两句。横向jerk的约束本质是垂直于推力轴的那部分jerk如果你忘了做投影等于给整段jerk加了上限这会无端地让轨迹变得保守但又看不出来哪里错。用二维平面版本最容易暴露这类问题横向jerk退化成标量投影公式变得非常直观对上了再升三维。6. 从跑通到能用扩展方向和个人体会6.1 从单段时间最优到在线MPC引导轨迹单段时间最优的NLP求解时间即使N40也要数秒直接在线使用不现实。但它最大的价值在于给出可达时间下界和一条高质量的引导轨迹。我后期方案是离线用双模型算出这条时间最优曲线在线用MPC跟踪它遇到障碍物变化时只在局部重规划。这样既保留了时间最优的激进性又获得MPC的鲁棒性。6.2 约束型双模型与全状态展开的取舍横向jerk约束这种压缩形式在推导时默认角速度完全垂直于推力轴忽略了偏航耦合和力矩限制。对大角度机动、横滚倒飞、重载悬停这类工况这种约束会略显乐观。这时可以升级成把四元数和角速度全部放进状态向量直接在旋转动力学层面做约束。代价是状态维数从9涨到13以上fmincon的收敛难度成倍增加。我的建议是先跑通紧凑约束版确认自己的问题域确实需要大机动能力再考虑升级。6.3 复现这类代码的落地建议我自己的习惯是先在2D平面跑通状态只有[x,z,vx,vz,ax,az]控制是二维jerk横向jerk约束退化成标量代码量直接减半。2D版本跑通并画出合理的时间最优轨迹后再升到3D——逻辑完全一样只是n变成三维向量投影用矩阵写。直接上3D的话一旦结果不对你根本分不清是约束写错了还是初始化有问题。另外把贴边检查和ω_req验证写成脚本每次改动约束都跑一遍。这两条检查比调求解器参数管用得多因为它们是直接判断答案在不在物理上。我对这个项目最深的体会是时间最优规划难点不在优化而在把物理写进约束。点质量模型给了一个更小的T但那个T不可信把旋转动力学双模型加进去之后T只变大了一点但这条轨迹第一次变得可信了。复现代码时请一定保留这个视角。