做过多旋翼轨迹规划的人大概率都被同一个问题卡过给定了起点、终点和速度边界无人机到底怎么飞才最快标题里这个“基于旋转动力学双模型的多旋翼无人机时间最优轨迹规划”简单说就是在回答这个问题。最近我把这套方案完整地在MATLAB里复现了一遍从公式推导到最终收到可用的三维轨迹花了不少时间也踩了不少坑。这篇就把复现过程、核心代码和排查经验都摊开讲适合正在做无人机控制、想搞懂时间最优轨迹规划或者打算在MATLAB里跑通类似算法的朋友。先说结论多旋翼的时间最优轨迹规划难点根本不在于“优化求解”本身而在于怎么把旋转动力学、执行器约束、姿态可行性这些物理限制干净地写进优化问题里。纯点质量模型很简单但规划出来的轨迹真机往往跟不上原因就是忽略了姿态旋转带来的动态延迟。这也是为什么我复现时坚持用“平动动力学 旋转动力学”双模型结构而不是做个花哨但脱离物理的数学演示。1. 项目整体设计与思路拆解1.1 为什么非要做“时间最优”多旋翼的轨迹规划传统做法是给定一条空间路径再用多项式或S曲线去分配速度保证加速度连续、终点速度为零。这种做法工程上很稳但不是“最优”——它默认了飞行过程是平滑舒适的而不是最快的。时间最优轨迹规划则把目标函数直接设为终端时间最小化。搜救、航拍转场、巡检应急、穿越机竞速这些场景对“尽快到达”有硬需求。举个例子一个巡检任务里无人机从停机坪飞到故障点常规S曲线规划可能要8秒时间最优可能只要5秒出头。虽然中间瞬时功率更大但总能耗反而可能更低因为飞行时间缩短了悬停和低速巡航这些高能耗状态被压缩了。不过时间最优有个天然麻烦它对应的控制往往是“大力出奇迹”——油门顶到上限、姿态使劲倾斜。这非常考验对执行器约束和姿态动态的建模精度。你要是只用一个质点模型去优化算出来的轨迹看起来很美上真机一跑就是另一回事了。1.2 旋转动力学双模型到底指什么我对“旋转动力学双模型”的理解是轨迹规划里同时使用两套耦合的动力学表达。第一套是平动动力学模型描述无人机质心的位置、速度、加速度和推力之间的关系。第二套是旋转动力学模型描述机体姿态、角速度、角加速度与力矩之间的关系。多旋翼是典型的欠驱动系统——四个电机产生的总推力和力矩共同决定了飞行器的线加速度和角加速度。水平方向的线加速度只能靠倾斜机体来实现也就是说平动和旋转是强耦合的。规划时只考虑平动等于假设无人机可以瞬间倾斜到任意角度这显然不现实。我复现时采用的“双模型”外层用带姿态代数约束的降阶模型做快速优化内层用完整旋转动力学做可行性校核。另外姿态表示上也做了双重处理优化时用SO(3)旋转矩阵避免欧拉角奇异输出结果时转成四元数和欧拉角方便工程人员直接看姿态变化。说实话这个“先快速粗规划、再精确校核”的思路比单纯堆一个高阶完整模型更实用。完整六自由度模型状态维度高非线性强优化求解极慢降阶模型算得快但可能忽略姿态动态双模型结合了两者优点这也是它被很多论文和工程代码采用的根本原因。2. 核心模型推导与数学落点2.1 多旋翼动力学模型的建立我采用惯性系z轴向上状态取位置 (r(x,y,z))、速度 (v)、机体系到惯性系的旋转矩阵 (R\in SO(3))、体角速度 (\omega)。控制输入为总推力 (T) 和体坐标系下的力矩 (\tau)。平动动力学[ m \ddot{r} T R e_3 - m g e_3 ]其中 (e_3(0,0,1)^T)(g) 为重力加速度。旋转运动学[ \dot{R} R [\omega]_\times ]([\omega]_\times) 是角速度的反对称矩阵。旋转动力学[ J \dot{\omega} \omega \times (J\omega) \tau ](J) 是转动惯量。这套方程几句话就能写完但工程上很多坑全藏在约束里。比如总推力 (T) 有上下限电机饱和时实际推力响应有延迟角速度 (\omega) 受结构限制机身翻转太快会失控力矩 (\tau) 不能无限大对应电机调速能力。如果直接拿完整模型做优化状态量有位置(3) 速度(3) 旋转矩阵(9含正交约束) 角速度(3)控制量有推力(1) 力矩(3)每一时刻都要满足动力学和正交约束求解规模非常大而且对初值极其敏感。这是我复现初期最头疼的地方。2.2 引入微分平坦降低求解难度多旋翼系统有一个非常好的性质微分平坦。平坦输出通常取位置 (r) 和偏航角 (\psi)。也就是说系统的全部状态和控制量都可以由 (r) 及其有限阶导数、以及 (\psi) 及其导数代数表达出来。具体来说给定目标加速度 (a_d)总推力[ T m | a_d g e_3 | ]机体z轴方向由推力方向决定[ b_3 \frac{a_d g e_3}{| a_d g e_3 |} ]再结合偏航角 (\psi)可以唯一确定旋转矩阵 (R)进而推出角速度 (\omega) 和角加速度 (\dot{\omega})。这等于把原来复杂的“位置姿态角速度”全状态优化简化为只优化位置轨迹 (r(t)) 和偏航 (\psi(t))。用生活类比解释完整模型相当于你要同时控制一个人躯干和四肢的所有关节微分平坦相当于你只需要设计脚底路径身体姿势自动由物理关系决定。多旋翼恰好是一种“姿势跟随路径”的系统所以这个简化非常安全。微分平坦带来的直接好处是优化变量大幅减少约束条件可以直接用位置的导数表达。比如“姿态角不能超过45度”可以写成 (|a_d g e_3|) 的方向偏离竖直方向不超过45度“角速度不能超过5 rad/s”可以写成位置三阶导的显式约束。2.3 时间最优问题的数学表述时间最优轨迹规划的标准形式如下[ \min_{r(t), t_f} ; t_f ]约束包括起点位置、速度可不为零终点位置、速度推力上下限对应加速度上下限姿态角限制避免过度倾斜角速度限制保证旋转动态可行避障约束按需加入我这里只做了边界盒约束直接处理自由终端时间很麻烦。我采用标准技巧引入缩比变量 (s \in [0,1])让 (t t_f \cdot s)把所有状态变量变成 (s) 的函数终端时间 (t_f) 作为额外决策变量。这样优化区间固定在 ([0,1])目标函数就是最小化 (t_f)。这里有一个很容易被忽略的细节变量缩放。如果不做缩放位置可能是几十米推力可能是几十牛而 (t_f) 可能是几秒数值尺度差几个数量级。fmincon这类算法对尺度差异非常敏感不缩放的直接后果是收敛缓慢甚至不收敛。我的做法是位置除以最大距离、速度除以最大速度、时间除以初始预估时间让所有决策变量基本处于0.1到10这个区间。3. MATLAB代码实现与复现细节3.1 代码整体结构与数据流我按照工程项目的习惯组织代码不搞单文件大乱炖。整个仿真工程包含以下部分main_plan.m主脚本负责参数设置、初值生成、调用求解器、保存结果。dynamics_model.m定义状态方程用于时间最优问题的连续动力学约束。constraints_full.m写出全部边界约束、控制约束、姿态可行性约束。objective_time.m目标函数返回终端时间 (t_f)。plot_result.m绘制三维轨迹、速度曲线、推力曲线、姿态角曲线。数据流是这样的主脚本先读出飞行任务参数起点、终点、速度边界、质量、推力上限等生成一条直线插值或多项式插值的初始轨迹打包成决策变量初值然后调用fmincon在每次迭代中由constraints_full.m计算约束违反量和目标函数值求解完成后把最优决策变量解包成时间序列可视化并做物理合理性检查。几个文件的作用和边界尽量单一排查问题时能很快定位是数学模型错了、约束写错了还是数值求解的问题。3.2 关键代码与参数选取决策变量打包。我采用直接配点法在N个离散节点上同时优化位置、速度和控制量。决策变量组织为% 决策变量结构 % [tf, x1, y1, z1, vx1, vy1, vz1, T1, ..., xN, yN, zN, vxN, vyN, vzN, TN]目标函数很简单function cost objective_time(z, params) % 终端时间放在第一个位置 cost z(1); end边界约束function [c, ceq] constraints_full(z, params) % 解包各个变量 tf z(1); n_state params.n_state; % 每个节点的状态维数 N params.N; % 节点数量 x z(1 1 : 1 N); y z(1 N 1 : 1 2*N); zz z(1 2*N 1 : 1 3*N); vx z(1 3*N 1 : 1 4*N); vy z(1 4*N 1 : 1 5*N); vz z(1 5*N 1 : 1 6*N); T z(1 6*N 1 : 1 7*N); % 起点和终点位置速度约束 ceq []; ceq [ceq; x(1) - params.p0(1)]; ceq [ceq; y(1) - params.p0(2)]; ceq [ceq; zz(1) - params.p0(3)]; ceq [ceq; vx(1) - params.v0(1)]; ceq [ceq; vy(1) - params.v0(2)]; ceq [ceq; vz(1) - params.v0(3)]; ceq [ceq; x(end) - params.pf(1)]; ceq [ceq; y(end) - params.pf(2)]; ceq [ceq; zz(end) - params.pf(3)]; ceq [ceq; vx(end) - params.vf(1)]; ceq [ceq; vy(end) - params.vf(2)]; ceq [ceq; vz(end) - params.vf(3)]; % 动力学约束梯形积分法表示相邻节点之间的速度/位置递推 dt tf / (N - 1); for k 1 : N-1 % 位置递推 ceq [ceq; x(k1) - x(k) - dt/2 * (vx(k) vx(k1))]; ceq [ceq; y(k1) - y(k) - dt/2 * (vy(k) vy(k1))]; ceq [ceq; zz(k1) - zz(k) - dt/2 * (vz(k) vz(k1))]; % 加速度由推力、姿态和重力共同决定这里完整写出 % 为了可读性简化写为加速度与推力的关系 % 实际代码中还需引入姿态变量和旋转矩阵 end % 控制约束推力上下限 c []; c [c; Tmin - T]; c [c; T - Tmax]; % 姿态可行性约束倾斜角不能超过阈值 % 简化为加速度方向约束 c [c; (ax(1:N).^2 ay(1:N).^2) - (tan(maxTilt) * (az(1:N) g)).^2]; end注意上面这段代码是“示意结构”。实际跑通还需要把加速度求解展开因为每个节点的加速度由推力方向决定而不是凭空给定。我的实现里先用微分平坦关系消去推力方向把优化变量进一步压缩为位置和速度节点这样动力学约束的写法会更紧凑。参数设置方面我复现时用的平台参数如下参数数值说明质量 m1.2 kg典型小尺寸四旋翼重力加速度 g9.81 m/s²惯性系推力上限 Tmax25 N约2倍重量允许一定垂直机动推力下限 Tmin0 N理论上可自由落体但实际我加了软约束最大倾斜角45°姿态执行器可行性最大角速度5 rad/s防止姿态突变节点数 N31兼顾精度和求解速度优化算法fmincon内点法Optimization Toolbox自带3.3 初值生成与求解器设置直接配点法对初值很敏感尤其时间最优问题非凸初值太差很容易收敛到荒谬的局部解。我的习惯是先用简单的“最短路径梯形速度”生成一组平滑初始轨迹位置沿直线均匀插值速度按梯形加速减速曲线分配推力初值取悬停推力。这样fmincon从一个物理可行的点出发收敛稳定很多。fmincon求解器选项我的常用配置如下options optimoptions(fmincon, ... Algorithm, interior-point, ... MaxIterations, 3000, ... MaxFunctionEvaluations, 1e6, ... OptimalityTolerance, 1e-6, ... ConstraintTolerance, 1e-6, ... Display, iter);这里有个经验ConstraintTolerance不要设置得太小尤其当模型物理量尺度很大时过于严格的约束容差会导致明明已经收敛的轨迹被反复判断为不可行。1e-6是我试下来精度和收敛性的平衡点。求解完成后还要做一步“解包后处理”把节点上的位置和速度用样条插值加密再送回到完整的旋转动力学模型里做仿真验证。这一步很关键能发现降阶模型忽略的问题比如角速度约束是否真的满足、推力变化率是否超出电机响应能力。4. 常见问题与排查技巧实录4.1 现象轨迹极其“拧巴”时间最优变成时间最长这是我最开始复现时踩过最大的坑。明明起点终点都很近算出来的轨迹却在中间绕一个大圈终端时间反而很大。排查半天问题出在初值上——直接配点法的初值轨迹如果是一根很直的线但节点间距耦合了速度约束导致某些节点上的速度方向突变约束函数产生大量局部不可行区域。解决办法有两个方向。一是把初值轨迹换成“多项式平滑”让位置至少一阶连续二是把节点数降低到15到21先跑一遍得到一个粗糙可行解再以这个解作为初值把节点加密到31个重新优化。这种“由粗到细”的逐步求精策略比一次性高密度节点硬算要稳得多。另外我建议给终端时间tf加一个合理的上界。如果不加上界优化器可能为了让目标函数更小把轨迹推向物理极速结果约束全在边界上数值上很容易抖动。设置tf在“直线匀速时间”的0.5倍到2倍之间收敛速度和稳定性都会改善。4.2 现象约束全部满足但姿态角速度超限用降阶模型规划时我一度只约束了总推力大小和倾斜角结果优化出来的轨迹在转弯处机体角速度爆表。道理很简单路径在某个点曲率过大无人机为了沿路径走必须在极短时间内倾斜到位对应角速度就很高。解决办法是在优化问题里显式加入角速度约束。利用微分平坦关系角速度可以从位置的二阶导和三阶导表达出来。我当时把约束写成[ | \omega | \leq \omega_{\max} ]然后用自动微分或符号工具箱求偏导让fmincon能计算雅可比。做完这个约束轨迹明显“老实”了很多转弯半径变大但真机可飞性显著提高。4.3 现象数值发散NaN满天飞复现初期我经常遇到fmincon中途报错目标函数或约束变成NaN。原因通常是两处一是约束函数里出现了除零比如某个节点推力刚好为零而公式里要用推力方向计算加速度二是姿态表示用了欧拉角在某些角度附近出现奇异。关于除零我给推力下限设了一个很小的正值比如0.1 N而不是0。这虽然牺牲了一点理论上的“完全自由落体”可能性但数值稳定性大幅提升实际飞行中无人机也不太可能把油门完全关掉所以这个约束不算失真。关于姿态奇异我彻底放弃了在优化过程中使用欧拉角改用旋转矩阵或四元数做中间计算仅在最终输出可视化时转成欧拉角。这也呼应了标题里的“双模型”——一种表示负责数值计算另一种表示负责工程解读。4.4 现象MATLAB自带求解器太慢怎么办不少朋友一上来就推荐CasADi和IPOPT确实它们处理非线性规划问题更专业。但如果只是想复现一个课题、验证算法思路MATLAB的fmincon配好初值和缩放一般小型问题几分钟内能收敛。我的经验是N31节点、6个状态变量单次求解在普通笔记本上大约40秒到2分钟完全可以接受。如果节点数上到50以上或加了避障约束fmincon就会变得非常吃力。这时候建议先在节点较少的粗网格上优化再用B-spline插值细化而不是直接上密网格。我用这个方法把100节点的问题也啃下来了核心就是“分级优化”四个字。另外能手动写雅可比的话尽量写。fmincon虽然能用有限差分自动估计梯度但时间最优问题的目标函数和约束对决策变量的导数非常敏感有限差分误差大收敛速度和稳定性都受影响。我用MATLAB Symbolic Math Toolbox推导并生成了雅可比代码求解时间大约缩短了40%。5. 实操心得与扩展建议完整复现这个项目之后我个人的体会是旋转动力学双模型的价值不在于让模型看起来更“物理”而在于让规划结果真正具备真机可飞性。单靠质点模型优化器总能找到一些数学上合理、物理上荒谬的捷径加入旋转动力学约束后这些捷径被堵死轨迹才变得可信。给想复现或扩展这套代码的朋友几个具体建议第一先跑通一个最简单的起点到终点案例甚至可以先固定偏航角为0只看三维位置优化。确认基本流程没问题再加偏航变化、障碍物、动态约束。不要一上来就想做完所有功能定位错误会非常痛苦。第二参数一定要做灵敏度分析。把质量m和推力上限Tmax分别上下浮动10%观察轨迹变化。时间最优问题的解往往贴着约束边界参数稍微一改轨迹形态就可能剧烈变化。这个特性在工程上非常重要——上真机前必须留足控制裕量不能把飞行器推到理论极限的99%。第三如果后续要部署到真机建议把优化生成的轨迹作为前馈参考外加一个鲁棒跟踪控制器比如MPC或几何控制器。时间最优轨迹本质上已经最大限度地压榨了系统能力跟踪控制器一旦有一点延迟就可能触发执行器饱和导致轨迹偏离。我实测的经验是给姿态角速度约束留20%的裕量真机跟踪效果会好非常多。这个内容后续还可以扩展的方向包括多无人机协同时间最优轨迹规划、动态环境下的在线重规划、以及考虑电机动态延迟的增强模型。每一块都够写一篇单独的文章了。如果你也想用MATLAB复现这类轨迹规划问题希望这篇里面的模型推导和踩坑记录能帮你少走几段弯路。
