先说我第一次看到微分平坦Differential Flatness时的真实反应。当时我在看一套无人机轨迹规划的开源代码优化变量竟然只有位置p [x, y, z]和偏航角ψ外加它们的各阶导数完整动力学方程根本没出现在优化变量里。我当时心想这怕不是在偷懒动力学约束去哪了后来把微分平坦这个概念啃明白才发现这还真不是偷懒而是把动力学的隐性约束用代数手段“预消掉”了。这篇作为“轨迹规划理论”系列的第一篇我就把微分平坦的老底讲清楚它解决什么问题、定义怎么理解、在无人机/小车/机械臂轨迹规划里到底怎么用以及哪些地方特别容易翻车。适合正在啃运动规划源码、或者想用理论解释“为什么能只在输出空间画线”的读者。1. 轨迹规划里的两难为什么需要微分平坦这种“捷径”1.1 轨迹规划的原问题状态和输入互相纠缠轨迹规划最原始的问题其实是一个最优控制问题给定系统的状态方程ẋ f(x, u)找到一组输入时间历程u(t)让系统从起点走到终点同时满足状态约束、输入约束、避障约束顺便使某个性能指标最小比如时间最短、能耗最低、抖动最小。这个问题的难点在于状态变量和输入变量永远是纠缠在一起的。你想让无人机飞得快推力就得大推力又决定姿态姿态又和角速度耦合角速度又和电机转速耦合。每一步决策都牵一发动全身。如果直接在状态空间里离散化求解优化变量的规模会非常吓人——一段几秒钟的轨迹光采样点就有几十上百个每个点都要带上全部状态量和输入量。1.2 传统数值方法的代价打靶法和直接配点法在没有微分平坦这类理论武器的时候工程上最常用的是两类数值方法。一类叫打靶法Shooting Method。思路是猜一组输入u(t)然后积分状态方程看终端状态对不对不对就修正初始猜测反复迭代。这种方法变量少但积分过程不能免而且对初始猜测极其敏感。我见过不少人在做机械臂时间最优轨迹时用打靶法结果迭代几百次还不收敛原因就是初始猜测离真实解太远。另一类叫直接配点法Direct Collocation。把整个时间域离散成N段状态量和输入量都当作优化变量再把动力学方程作为等式约束配上。好处是全局性好一些缺点是变量数量爆炸。无人机一个 5 秒的轨迹哪怕采样率降到 50Hz状态加输入也有上千个变量再加上避障约束的非凸性求解器经常在可行域边缘挣扎。1.3 微分平坦做的事把微分方程约束变成代数表达式微分平坦的核心思想很反直觉存在一组特殊的“平坦输出”z(t)它的维数等于系统的输入维数并且系统的所有状态量和输入量都能表示为z及其有限阶导数的代数函数。这句话翻译成人话就是你只要在z(t)空间里画出一条足够光滑的曲线那么系统的状态轨迹x(t)和输入轨迹u(t)就自动跟着确定了完全不需要求解微分方程。微分方程约束被“解耦”掉了变成了纯粹的代数映射。这在轨迹规划里是降维打击。以前需要同时优化状态和输入现在只需要优化一条输出曲线以前要处理复杂的动力学微分约束现在只需要处理代数映射以前变量上千个现在可能只需要几十个多项式系数。这也是为什么微分平坦在无人机、移动机器人、部分机械臂轨迹规划里几乎成了标配理论。2. 微分平坦的定义别被满屏学术符号吓退2.1 用一个流水线类比理解平坦输出我后来找到一个比较顺的类比把整个动态系统想象成一条流水线平坦输出相当于流水线末端的成品。你只需要盯着成品做质检规划成品曲线中间每一道工序的状态零件位置、加工参数都能由成品要求倒推出来。如果某个系统不具备平坦性那就相当于中间有“自由发挥”的工序——你控制了成品也不能保证中间过程确定必须额外盯住那些隐藏的动态。机械臂就是典型的对比。刚性机械臂的关节角q本身可以作为平坦输出因为给定q(t)关节速度q̇(t)是它的导数关节力矩τ又能由逆动力学公式M(q)q̈ C(q,q̇)q̇ g(q)直接算出来。整条动力学链条是确定的。但如果机械臂关节带有明显的柔性情况就复杂了电机转角和连杆转角之间存在隐藏的弹性动态这时随意把某个输出当成平坦输出就不一定成立。2.2 形式化定义状态和输入都能被代数表达从控制理论角度一个系统ẋ f(x, u) x ∈ Rⁿ u ∈ Rᵐ如果存在一个输出向量z ∈ Rᵐ维数等于输入个数以及有限的非负整数α、β使得下面的关系成立x Φ(z, ż, z̈, ..., z^(α))u Ψ(z, ż, z̈, ..., z^(β))并且z本身也能用x和u及其有限阶导数表达那么我们就说这个系统是微分平坦的z叫平坦输出。注意几个关键点z必须有m个分量和输入个数一样。因为系统有m个自由度可以独立控制平坦输出就把这m个自由度全部占满。表达式中只允许有限阶导数不允许出现积分项。一旦有积分残留说明存在无法由输出直接确定的内部动态。映射关系要求是光滑的代数关系一般还希望它是可逆可微的这样我们才能把约束来回映射。2.3 判定一个系统是否平坦实际要看这三样理论上有复杂的光滑性和秩条件判据但工程上我会先快速检查三件事。第一输出维数是否正确。平坦输出的分量数必须等于输入维数多了少了都不行。四旋翼有 4 个输入总推力 三轴力矩所以平坦输出是 4 维的通常取位置 3 维加偏航角 1 维。差速小车有 2 个输入线速度和角速度平坦输出就是 2 维的取位置坐标。很多系统之所以“不平坦”本质上是因为候选输出维数对不上或者导数的阶数要求无法同时满足多个通道。第二系统是否存在“隐藏内动态”。如果输入能完全决定输出的未来演化同时状态又能被输出代数表达那基本可以认为没有隐藏动态。反过来如果一个系统“看得到状态却猜不出输入”比如部分欠驱动系统就要高度怀疑它不具备平坦性。第三物理上映射是否有定义域限制。很多教科书例题里的平坦性是全局的但工程系统的映射往往只在某个开集上成立。比如后面会提到的小车模型速度为零的位置处角速度映射会退化。这种局部奇异性不属于定义问题但属于使用问题我放到第 5 节专门讲。3. 三个经典模型的平坦输出无人机、差速小车、机械臂3.1 四旋翼位置加偏航角的四维平坦输出四旋翼是一般读者最先接触的平坦系统。它的状态包含位置p、速度v、旋转矩阵R和角速度ω输入包含总推力T和三轴力矩τ。严格写出来状态空间是 12 维往上的但大家做轨迹规划时几乎都只用σ (p, ψ)这四个量。原因是这样的。四旋翼的平动方程可以写成m p̈ T R e₃ - m g e₃这里e₃是机体坐标系的 z 轴单位向量。给定期望加速度p̈_des和期望偏航角ψ_desT R e₃这个向量就被唯一确定了——它的方向就是期望加速度补偿重力后的方向长度就是推力大小。在偏航角固定的前提下垂直于推力方向的“前向朝哪”也确定了于是整个旋转矩阵R都能由(p̈_des, ψ_des)代数重构出来。接下来角速度ω从Ṙ R [ω]ₓ得到角加速度又和力矩τ通过欧拉方程挂钩。所以四旋翼飞行器的位置动力学、姿态动力学、电机执行器模型可以被完整地打包成一个代数映射给定平坦输出(p(t), ψ(t))的各阶导数就能算出力、力矩、电机转速。这就是为什么很多优秀的开源轨迹规划器敢把核心优化变量只设为x, y, z, ψ四个通道的多项式系数。3.2 差速小车和单轮车模型位置坐标本身就是平坦输出差速小车和单轮车模型是另一个特别适合用来“找感觉”的例子。它的模型是ẋ v cosθẏ v sinθθ̇ ω这里(x, y, θ)是小车位姿输入(v, ω)是线速度和角速度。大多数资料都会告诉你取平坦输出σ (x, y)然后可以反推v ±√(ẋ² ẏ²)θ atan2(ẏ, ẋ)ω (ẋÿ - ẏẍ) / (ẋ² ẏ²)细心的人已经发现了这个映射里只用到了位置输出的一阶导和二阶导。也就是说只要你画出一条 C² 连续的位置曲线那么小车每个时刻的速度、朝向、角速度全都被定死了。这里面包含了一个非常重要的直觉对差速小车来说“轨迹的形状”本身就是对系统动态最简洁的描述。轮子怎么转完全取决于这条线画得弯不弯、跑得快不快。这个例子也顺带解释了为什么移动机器人绕桩路径总被设计得处处光滑——因为转弯半径为零的路径在这里是奇异的角速度公式分母会爆炸。3.3 机械臂关节空间平凡的平坦输出不平凡的工程意义刚性机械臂的平坦性从定义出发有点“无聊”取平坦输出为关节角向量z q则状态x (q, q̇)显然可由z和ż表示输入则由逆动力学给出τ M(q)q̈ C(q,q̇)q̇ g(q)每个关节角自己就是平坦输出这看起来什么都没说但在轨迹规划里的意义非同小可。它意味着我们可以把机械臂轨迹规划拆成两步走第一步先在关节空间里规划足够光滑的关节曲线q(t)第二步再由逆动力学计算出实际需要的关节力矩然后检查力矩是否超过电机能力。整个过程不需要在高维状态空间里做配点也不需要对微分方程反复积分。我在实际做机械臂轨迹规划时这个“平凡的平坦输出”带来的工程价值非常大。比如做带约束的点到点运动我可以直接规划一条五次多项式保证速度、加速度连续再通过逆动力学验证力矩峰值。如果峰值超限只需要把时间拉长或调整中间路径点迭代一轮就能出结果比直接上直接配点法省很多事。当然机械臂也有不平凡的地方。比如带弹性关节的机械臂、柔性臂或者机械臂末端负载未知的情况关节角就不再是平坦输出必须重新找候选输出甚至可能根本不存在。这也是为什么移动机械臂这类复杂系统做规划经常会借鉴欠驱动系统的平坦性分析方法。4. 用微分平坦把轨迹规划问题“降维”完整推导与流程4.1 规划总流程参数化、映射、约束回代、优化迭代把微分平坦真正用进轨迹规划标准流程实际上只有四步。第一步选定平坦输出并确定你关心的各阶导数阶数。无人机一般关心位置的三阶导jerk和四阶导snap因为它直接影响推力和姿态角速度小车关心加速度因为它决定角速度和向心力机械臂关节规划关心加加速度因为它和冲击、振动相关。第二步在平坦输出空间里做参数化。最常用的是多项式、贝塞尔曲线、B样条曲线。这一选择的理由是光滑性和线性性更精确地说是因为这些参数化形式下约束条件对参数往往是线性的至少是凸的求解器才处理得动。第三步用平坦性映射把轨迹映射回状态和输入空间逐点检查约束是否满足。比如无人机最大推力、最大角速度小车最大转弯半径机械臂最大关节力矩。这一检查通常放在优化目标函数里作为惩罚项或者作为约束条件。第四步迭代求解。如果约束不满足调整时间分配、航点位置、参数数量或者改变性能指标的权重重新求解直到满足需求。4.2 四旋翼最小加加速度轨迹一个可以直接抄的模型我以四旋翼在两点之间飞行、要求最小化 jerk 积分为例把这条流水线走一遍。先定义平坦输出σ(t) [r(t); ψ(t)]其中r是三维位置。为了视觉直观先只看一维比如 x 方向。用五次多项式连接起点终点保证位置、速度、加速度边界连续x(t) c₀ c₁t c₂t² c₃t³ c₄t⁴ c₅t⁵jerk 是x‴(t) 6c₃ 24c₄t 60c₅t²性能指标取∫ x‴(t)² dt。由于这个积分对系数c是二次型再加上两端边界条件给出的线性等式约束这实际上是个带等式约束的二次规划解是闭式可求的。三维版本就是把 x、y、z 三个方向的二次规划问题拼起来每个方向独立求解后再合成空间轨迹。到这里为止我们还没有遇到任何动力学问题。存在高度方向的轨迹稍加调整把重力补偿写进去期望加速度a_des r̈ g e₃期望推力方向z_b a_des / |a_des|推力大小T m |a_des|。如果 a_des 在某处接近零相当于垂直坠落的近似自由落体推力方向就会不稳定这里就是第一个需要小心的地方。实际工程不会直接闭式求这么多约束下的解。更常见的是把整个问题塞进一个基于多项式和贝塞尔曲线的 QP 求解器约束写成对控制点的不等式约束。利用贝塞尔曲线的凸包性质控制点多边形包含了整条曲线因此只需要约束控制点就相当于约束了整条曲线的上下界。这个性质很实用我在实际写代码时会优先用 B 样条而不是普通多项式就是为了约束处理的鲁棒性。4.3 输入约束如何映射成输出导数约束很多初学者做到这步就停了以为规划出光滑的位置曲线就完事了。实际上必须把执行器约束回代到输出导数上否则会出现在仿真里飞得好、真机上一给就是NaN的惨剧。对于四旋翼推力饱和约束变成T_min ≤ m |a_des| ≤ T_max也就是加速度幅值有上下界角速度约束需要看z_b方向的变化率。直观理解z_b变化越快机体转动越快。所以位置的三阶导 jerk 越大角速度越大电机转速上限还要把四个电机的混控方程考虑进来。这部分一般放到后端做校验不在规划主循环里实时算。对于差速小车输入约束就变得更直观了。线速度约束变成|ẋ² ẏ²|的幅值约束角速度约束变成曲率约束因为ω近似等于速度和曲率的乘积。所以“路径不能太弯”这句话在平坦输出空间里就是位置二阶梯度的幅值约束。我自己做项目时有个习惯规划完成之后一定会写一个独立的仿真校验脚本把平坦输出的各阶导数映射回真实的动力学模型检查每个采样点上的输入是否落在执行器限幅内。这一步看似多余但在真实项目中救了我很多次。5. 微分平坦的三个“温柔陷阱”5.1 奇异点速度为零、转向、垂直自由落体第一个坑是奇异点。平坦性的代数映射往往只在某个开集上有效一旦轨迹经过奇异区域映射就会崩溃。差速小车模型里ω (ẋÿ - ẏẍ) / (ẋ² ẏ²)分母是速度的平方。如果轨迹某个点上速度为 0角度就变成不定式原地转向这种操作在输出空间里根本表达不出来。这说明规划时必须保证参考轨迹的最小速度大于某个阈值否则映射回真实系统时方向会乱跳。四旋翼对应的奇异情况是期望加速度恰好抵消重力也就是推力为零的自由落体状态。此时推力方向完全不确定姿态角会剧烈翻转这会让角速度约束爆炸。如果你的任务场景中无人机可能做“坠落”动作需要在平坦输出规划阶段就加入最小加速度约束不要指望后端能兜住。机械臂对应的奇异情况是逆动力学里的惯性矩阵接近奇异也就是接近奇异位形。关节角作为平坦输出时映射本身没问题但算出来的关节力矩会随着接近奇异而急剧增大表现为关节角轨迹挺光滑、力矩轨迹却像炸了一样。5.2 输入约束映射到输出空间后往往是非凸的第二个坑听起来更“数学”但工程后果很直接即使系统是平坦的把原来的凸约束映射到平坦输出空间后也大概率变成非凸约束。比如四旋翼的推力上限约束本来是输入空间的简单区间约束T ≤ T_max映射到平坦输出空间变成|r̈ g e₃| ≤ T_max/m这是个二阶锥约束还是凸的比较好办。但如果你同时约束角速度和推力大小或者加入避障约束问题就变成非凸了。这种情况下很多教程里“直接用凸优化求解”的承诺就不成立了。实际项目中常见做法有几种把非凸约束线性化或做凸包近似代价是结果保守可能找不到别的可行解把约束放进代价函数做惩罚项迭代调整惩罚权重工程上最常用用样条曲线和采样法离散化约束把问题转成非线性优化或序列二次规划。我个人的经验是面对非凸约束时不要一上来就追求“全局最优”先把问题转成一个可以稳定求解的局部优化问题再在这个基础上做航点插值和初始解热启动。很多论文里漂亮的凸优化写法到了工程现场都是要降级成更朴素的求解方式的。5.3 障碍物和动力学约束不会因为平坦性自动变简单第三个坑和避障直接相关。微分平坦处理的是系统动力学约束它不会帮你处理空间避障。避障约束本质上是在工作空间里描述“轨迹不能穿过障碍物”当你把它映射到平坦输出空间时本来很简单的“圆与线段不相交”判断会变成输出空间里的一个复杂隐式约束。四旋翼还好因为平坦输出本身就是位置坐标避障约束可以直接加在位置曲线上。但对于某些人体模型或带负载的无人机系统平坦输出可能是负载位置、绳长等组合量这时候障碍物在输出空间里的形状会扭曲得很厉害。机械臂更明显你可以选关节角做平坦输出但障碍物在工作空间里是矩形或圆柱映射到关节空间就变成严重非凸的曲面离线规划时极难预先描述。所以我的建议是不要试图把“用平坦性解决动力学”和“用采样或搜索解决避障”混在一起。现在主流的混合做法是用 RRT/RRT* 或概率路线图在前端生成一条几何可行的路径然后用微分平坦模型把这条路径细化成一个满足动力学约束的平滑轨迹。前端的路径可以很粗糙后端的平坦性负责把它修正成可执行的曲线。6. 一点个人实践体会学了微分平坦之后我对轨迹规划整个工作流的理解改变很大。以前总觉得一步到位地把动力学的每个细节都塞进优化里才算严谨现在反而认为在平坦系统里把“几何形状”和“动力学可行性”拆成两段处理既高效又鲁棒。这个理念也直接影响了我的代码结构前段负责空间避障后段负责时间和动力学可行性中间用平坦映射作为接口两类问题各自独立调试出问题时不会糊成一团。还有一点不得不提就是一定要亲自动手把四旋翼那个从期望加速度到期望姿态、再到角速度的映射推一遍。盯着论文看十遍不如自己在纸上把旋转矩阵的插值、so(3) 的小扰动公式写一遍来得通透。我第一次推的时候卡在Ṙ R[ω]ₓ这个符号上后来才发现一旦掌握了这个式子后面所有和姿态相关的映射都变得顺理成章了。微分平坦这条线后面还能延伸出的内容包括最小加加速度/最小加加加速度轨迹的闭式解、时间最优轨迹规划、以及把平坦性和模型预测控制结合起来的做法。下一篇我打算直接从“如何把最小加加速度轨迹规划写成可运行的代码”开始尽量少讲空理论多放能跑的例程。你要是已经把这篇里的映射关系亲手推过一遍那下篇读起来应该会非常轻松。
