1. 从“能飞”到“飞得好”为什么四旋翼需要轨迹优化大家好我是老张在无人机和机器人领域摸爬滚打了十几年。今天想和大家聊聊一个听起来有点“学术”但实际开发中天天要打交道的核心问题四旋翼无人机的轨迹优化。很多刚入行的朋友可能会觉得轨迹规划嘛不就是给无人机一串目标点让它飞过去就行了吗我刚开始做项目时也是这么想的结果飞机要么飞得“磕磕绊绊”要么在复杂环境里直接“炸机”教训惨痛。其实从“能飞”到“飞得好、飞得稳、飞得安全”中间隔着一道巨大的鸿沟这道鸿沟的名字就叫“动力学约束”和“环境不确定性”。想象一下你让一个顶级体操运动员去走钢丝他身体素质再好动力强如果钢丝路径规划得歪歪扭扭、忽高忽低轨迹不平滑他也得摔下来。四旋翼也一样它有最大速度、最大加速度、最大加加速度Jerk的限制。前端路径搜索比如A*、RRT给出的往往是一条由离散点连成的、锯齿状的“通道”就像一条布满棱角的铁丝。你让无人机硬着头皮去跟踪这条“铁丝”结果就是电机疯狂加减速产生剧烈抖动耗电量激增飞行体验极差在狭窄空间里更是容易撞上“铁丝”的尖角。所以我们需要一个“后端优化”的工序把这条生硬的“铁丝”熔炼、打磨成一条光滑、流畅、且完全处在安全通道内的“钢管”让无人机可以优雅地沿着“钢管”内部飞行。这就是轨迹优化的核心任务。而伯恩斯坦多项式就是我近年来发现的一把打造这根“安全钢管”的绝佳“模具”。它不像传统的最小挠度Minimum Snap多项式那样需要反复试探、添加约束来保证安全而是天生就具备一种“收敛”和“包络”的特性能让我们从一开始就把轨迹牢牢“锁”在安全的飞行走廊里。接下来我就结合自己的实战经验带你看看这把“模具”到底怎么用。2. 伯恩斯坦多项式你的轨迹“安全模具”2.1 什么是伯恩斯坦多项式一个直观的类比提到多项式大家可能想到的是a0 a1*t a2*t^2 ...这种标准形式。但伯恩斯坦多项式长得不太一样它是用一组叫做“伯恩斯坦基函数”的东西组合起来的。别被名字吓到我们可以用一个非常形象的“拉扯橡皮筋”的类比来理解。假设我们要规划一条从A点到B点的轨迹。我们在这两点之间设定几个“控制点”就像在A、B之间钉下几颗钉子。伯恩斯坦多项式描述的轨迹就像一根有弹性的橡皮筋被这些钉子“吸引”和“拉扯”。关键来了这条橡皮筋轨迹永远只会在这群钉子所围成的“凸包”内部活动。所谓“凸包”你可以理解为用一根橡皮圈套住所有钉子后绷紧形成的形状。这就带来了一个巨大的好处安全性保障变得极其直观。如果我们能事先规划出一个安全的空间比如一个长方体飞行走廊然后我们只需要确保所有的“控制点”钉子都落在这个长方体内部那么整条伯恩斯坦多项式轨迹橡皮筋就100% guaranteed会被限制在这个长方体内部这相当于我们把一个复杂的、对整条轨迹的连续约束简化成了对少数几个离散控制点的简单约束。在数学优化里这能把一个非凸的、难解的问题转化为一个凸优化问题求解起来又快又稳。# 一个简化的示意如何用伯恩斯坦基函数计算曲线上的点 import numpy as np def bernstein_poly(n, i, t): 计算第i个n阶伯恩斯坦基函数在t处的值 from math import comb return comb(n, i) * (t**i) * ((1-t)**(n-i)) # 假设我们有4个控制点 (2维x,y) control_points np.array([[0, 0], [1, 2], [3, 3], [4, 0]]) n len(control_points) - 1 # 多项式阶数 def bezier_curve(t): 计算伯恩斯坦多项式贝塞尔曲线上对应参数t的点 point np.zeros(2) for i in range(n1): point bernstein_poly(n, i, t) * control_points[i] return point # 这样当t从0变化到1时bezier_curve(t)就会走出一条光滑曲线 # 且这条曲线完全位于control_points的凸包内。2.2 为何它特别适合四旋翼动力学可行性的“打包”解决四旋翼的模型比较复杂但我们最关心的往往是高阶导数位置、速度、加速度、加加速度Jerk。轨迹是否“可行”就看这些量是否超过电机的物理极限。伯恩斯坦多项式另一个开挂的特性是其导数的控制点可以由原始控制点的简单线性组合得到。这意味着什么假设我们设计了一条满足位置约束在走廊内的伯恩斯坦轨迹。我们不需要重新去优化速度、加速度只需要用一套固定的公式从位置控制点就能直接算出速度的控制点、加速度的控制点。然后我们检查这些速度、加速度的控制点是否超过了最大限值。因为导数轨迹本身也是伯恩斯坦多项式所以只要这些导数控制点不超限整条导数轨迹就一定不会超限。这相当于把动力学约束也“打包”成了对控制点的约束。在实际的优化问题中我们可以把位置安全约束控制点在走廊内、速度约束、加速度约束全部写成控制点的线性不等式。这样一来整个轨迹优化问题就变成了一个**二次规划QP**问题市面上有大量成熟、高效的求解器比如OSQP、qpOASES可以秒解。我实测下来在机载计算机如Jetson Xavier上生成一条几秒长的复杂轨迹优化时间通常在毫秒级完全能满足在线重规划的需求。3. 构建安全舞台飞行走廊的生成与膨胀有了伯恩斯坦多项式这把好“模具”我们还得为它打造一个合适的“舞台”这个舞台就是飞行走廊。你不能让无人机在充满障碍物的杂乱仓库里随意飞必须给它划出明确的、无碰撞的通道。这就是飞行走廊的意义——它是一系列在三维空间中连接起来的、不重叠的凸多面体通常是长方体或六面体序列。3.1 从地图到通道如何生成飞行走廊在我的项目实践中流程一般是这样的这和原始文章的思路类似但我会补充更多细节和踩过的坑感知与建图无人机通过激光雷达或深度相机实时构建局部或全局的欧几里得符号距离场ESDF。简单说ESDF就是一个三维网格地图每个格子不仅告诉你这里有没有障碍物还告诉你这个位置到最近障碍物的精确距离。这个“距离”信息是生成走廊的基石。获取初始路径前端路径规划器如A*、快速行进法FMM在ESDF上从起点到终点找出一条全局的、尽可能远离障碍物的“草稿路径”。这条路径可能很靠近障碍物或者转弯很急。初始化安全球与立方体沿着这条“草稿路径”每隔一段距离取一个点节点。对于每个节点查询ESDF得到它到最近障碍物的距离d_safe。以这个节点为球心d_safe为半径可以画出一个绝对安全的球体空间。然后我们取这个球体的内接立方体作为该点处初始的飞行走廊单元。为什么用立方体因为立方体的边界是轴对齐的平面用x_min, x_max, y_min, y_max, z_min, z_max六个参数就能描述后续做约束时非常方便。走廊膨胀——榨取最大自由空间初始立方体通常比较保守。为了给优化留出更多余地让轨迹更平滑我们需要膨胀它。膨胀的逻辑不是简单地把立方体等比例放大那样会撞上障碍物。正确的做法是沿着每个坐标轴±x, ±y, ±z方向去“推”立方体的六个面。具体操作是检查当前立方体边界外的下一个网格单元在ESDF中的距离是否仍然安全大于无人机半径余量如果安全就把边界向外移动一个网格重复这个过程直到边界触碰到“安全距离”的阈值。这个过程就像吹气球但只在空旷的方向膨胀。走廊修剪与合并膨胀后相邻节点的立方体可能会大量重叠甚至完全一样。这些冗余的立方体会增加后端优化问题的规模却不会带来更大的解空间。因此需要一个修剪步骤将连续的、相同或包含关系的立方体合并最终得到一串紧凑的、无缝衔接的凸多面体序列这就是最终的飞行走廊。// 伪代码示意走廊膨胀的核心逻辑 for (每个走廊单元Cube) { for (每个扩张方向 dir: {X, -X, Y, -Y, Z, -Z}) { while (true) { 计算当前边界在dir方向上的下一个网格坐标 next_cell; 查询 next_cell 在ESDF中的距离 dist_to_obs; if (dist_to_obs SAFE_THRESHOLD) { 将Cube的边界向dir方向扩展一个网格; } else { break; // 碰到“墙壁”停止在这个方向膨胀 } } } }3.2 踩坑经验走廊质量决定优化上限这里我分享一个真实案例。有一次我们在室内测试无人机需要穿过一个门框。前端路径给出了一条穿门而过的直线。但由于传感器噪声和ESDF的离散化生成的飞行走廊在门框位置的那个立方体在垂直方向Z轴上膨胀得不够充分高度只比无人机本身高一点点。当我们用伯恩斯坦多项式去优化轨迹时虽然轨迹被严格限制在了走廊内没有碰撞风险但因为走廊本身在垂直方向“压”得太扁优化出的轨迹为了满足高阶平滑特别是加速度连续不得不“委曲求全”在穿过门框时速度降得非常低显得很犹豫。后来我们改进了走廊生成算法在类似狭窄通道处主动给予更大的安全余量甚至允许走廊在非关键方向比如水平方向很宽时牺牲一点宽度来换取关键方向如高度的扩展。优化后的轨迹明显更果断、更流畅。所以飞行走廊的生成不是一成不变的它需要与你的任务需求、无人机动力学特性紧密结合。一个好的走廊应该在保证安全的前提下尽可能为轨迹优化提供“舒展筋骨”的空间。4. 实战将问题“套进”凸优化的框架现在我们有了安全的飞行走廊一系列立方体也有了伯恩斯坦多项式这个利器。如何把它们组装起来变成一个可以求解的数学问题呢这才是工程实现的核心。4.1 问题建模目标函数与约束我们的目标是生成一条时间最优、且尽可能平滑的轨迹。通常我们会最小化加加速度Jerk或加速度的积分平方因为这直接关系到电机能耗和飞行平稳性。用伯恩斯坦多项式表示轨迹后这个目标函数可以写成关于控制点坐标的二次型。假设我们将轨迹分成M段对应M个走廊单元每段用一个N阶伯恩斯坦多项式表示。那么整条轨迹的控制点集合就是我们的优化变量。约束主要来自以下几方面安全性约束硬约束对于第k段轨迹它的所有控制点必须落在第k个飞行走廊立方体内。这是一个线性不等式约束每个控制点的x,y,z坐标分别大于立方体最小值小于最大值。动力学可行性约束硬约束根据2.2节提到的特性我们可以计算出速度、加速度对应的控制点。要求这些控制点的每个分量x,y,z方向的速度、加速度的绝对值小于无人机的最大速度v_max和最大加速度a_max。这同样是线性不等式约束。连续性约束硬约束段与段之间的轨迹在连接点处必须满足位置、速度、加速度的连续甚至加加速度连续取决于阶数N。由于伯恩斯坦多项式在端点处的性质这个约束可以表达为相邻段控制点之间的线性等式关系。起点终点约束硬约束轨迹的起点和终点的位置、速度通常设为零是给定的这也可以转化为对首尾控制点的线性等式约束。看所有的约束都是线性的目标函数是二次的。所以整个轨迹优化问题就是一个标准的**凸二次规划QP**问题。它的形式非常干净Minimize: (1/2) * x^T * Q * x c^T * x Subject to: A_eq * x b_eq A_ineq * x b_ineq lb x ub其中x就是所有控制点坐标排成的一个长向量。Q和c由最小化Jerk的目标决定A_eq, b_eq对应连续性约束和起终点约束A_ineq, b_ineq对应安全性和动力学约束lb, ub可以直接是走廊和动力学的边界。4.2 求解与代码实现片段凸QP问题的求解已经非常成熟。在C中我常用osqp-eigen这个库它是OSQP求解器的Eigen封装接口友好求解速度快。下面是一个高度简化的代码框架展示了如何设置问题#include OsqpEigen/OsqpEigen.h #include Eigen/Sparse // ... 假设已经计算好目标函数矩阵H即Q向量g即c // ... 以及约束矩阵A下界lb上界ub包含了等式和不等式 OsqpEigen::Solver solver; solver.settings()-setVerbosity(false); solver.settings()-setWarmStart(true); solver.data()-setNumberOfVariables(num_variables); // 变量数 控制点总数 * 3 (x,y,z) solver.data()-setNumberOfConstraints(num_constraints); // 将Eigen稀疏矩阵转换为CSC格式OSQP所需 Eigen::SparseMatrixdouble H_sparse H.sparseView(); Eigen::SparseMatrixdouble A_sparse A.sparseView(); if (!solver.data()-setHessianMatrix(H_sparse)) return false; if (!solver.data()-setGradient(g)) return false; if (!solver.data()-setLinearConstraintsMatrix(A_sparse)) return false; if (!solver.data()-setLowerBound(lb)) return false; if (!solver.data()-setUpperBound(ub)) return false; if (!solver.initSolver()) return false; if (solver.solveProblem() ! OsqpEigen::ErrorExitFlag::NoError) return false; Eigen::VectorXd solution solver.getSolution(); // solution 里存储的就是优化后的控制点坐标求解完成后我们就得到了一组最优的控制点。将这组控制点代入伯恩斯坦多项式公式就能得到一条从起点到终点、全程在飞行走廊内、速度加速度不超限、且非常平滑的时空轨迹。5. 在未知环境中自主导航系统集成与实测效果理论再好也得落地飞起来看。基于伯恩斯坦多项式和飞行走廊的轨迹优化框架非常适合与未知环境探索系统集成实现真正的自主导航。我们的系统工作流是一个闭环局部感知与ESDF更新无人机利用机载传感器如Livox激光雷达实时扫描前方环境增量式地更新局部ESDF地图。全局重规划触发当在当前的飞行走廊末端感知到新的障碍物侵占了走廊空间或者有更优的路径出现时触发全局重规划。前端快速搜索使用**快速行进法FMM**在更新的ESDF上从当前位置到目标点计算出一条新的、基于距离场的初始路径。FMM的好处是它能自然地考虑“距离”成本生成的路径会倾向于在宽阔地带走远离障碍物。生成新走廊沿着这条新路径用第3章的方法快速生成新的飞行走廊序列。后端凸优化以当前无人机的状态位置、速度为起点以目标状态为终点在新的走廊约束下利用第4章的方法求解一个新的伯恩斯坦多项式轨迹。由于是凸优化求解成功率极高速度极快通常在10-50ms内。轨迹执行与循环将新轨迹发送给底层的飞行控制器如PX4的Offboard模式控制器会跟踪这条光滑的轨迹。同时系统回到步骤1持续感知准备下一次重规划。实测效果在杂乱无章的室内仓库、森林般的管道区域等场景测试中这套方案表现非常鲁棒。无人机能够以数米每秒的速度在密集障碍物间敏捷穿行。即使突然出现未知障碍比如测试人员挥手无人机也能在几十毫秒内规划出一条绕行的平滑新轨迹动作没有急停或剧烈抖动过渡非常自然。这完全得益于伯恩斯坦多项式带来的轨迹内在平滑性和凸优化求解的可靠性。相比之下传统基于采样或数值优化的方法在遇到紧急重规划时可能会陷入局部最优或者因为迭代次数不够而产生不平滑甚至不可行的轨迹。而我们的方法因为把安全性和动力学的硬约束都转化为了线性形式并利用了伯恩斯坦的凸包性质使得每次求解都像是在一个形状规则的“盒子”里找最优解又快又稳。6. 进阶讨论参数选择与性能调优在实际部署中有几个关键参数会直接影响飞行性能和计算效率这里分享一下我的调参经验。伯恩斯坦多项式的阶数N阶数越高轨迹越光滑可导次数越多优化自由度也越大但控制点数量也越多优化问题规模变大。对于四旋翼通常保证加速度连续Jerk有界就足够了这要求位置轨迹至少是5阶多项式因为加速度是位置的二阶导需要多项式至少2阶可导但为了优化目标通常取更高。我常用的选择是N7即8个控制点一段这样可以优化加加速度Snap让飞行更柔和同时问题规模也可接受。每段轨迹的时间分配这是影响轨迹“节奏”的关键。时间给得太短会导致速度、加速度轻易触达极限优化问题可能无解时间给得太长则轨迹缓慢不高效。一个实用的方法是根据路径长度和平均期望速度来初估总时间然后按每段走廊的“狭窄程度”进行比例缩放。在狭窄的走廊段给予更多的时间允许飞慢点在宽阔的走廊段给予较少的时间可以飞快点。这可以通过在优化问题中将时间也作为优化变量来实现变成非线性问题但更简单有效的方法是采用启发式分配然后固定时间进行QP求解如果求解失败如动力学约束无法满足再整体增加时间重新求解。飞行走廊的粒度走廊单元的大小和数量需要权衡。单元太大、数量太少则自由空间利用不充分可能在某些狭窄处限制过死单元太小、数量太多则优化变量增多计算量增大且可能因为分段过多导致轨迹不必要的弯曲。我的经验是让走廊单元的尺寸与环境的复杂度自适应。在空旷区域可以生成较大的走廊在复杂区域则生成较密、较小的走廊来精确描述通道形状。同时一定要做好第3章提到的走廊合并修剪这是降低问题维度的有效手段。最后再提一个工程上的细节数值稳定性。伯恩斯坦多项式在参数t∈[0,1]上定义良好但当阶数较高时直接计算幂次可能会带来数值误差。在实际代码中我推荐使用德卡斯特里奥De Casteljau算法来递归计算伯恩斯坦多项式及其导数值这种方法数值上更稳定。优化求解器如OSQP对问题数据的缩放也很敏感最好将位置、速度、加速度等物理量归一化到相近的数量级比如位置用米速度用米/秒但可以适当缩放让Hessian矩阵Q的条件数不要太大这样求解器收敛更快、更稳定。这套基于伯恩斯坦多项式和飞行走廊的方法我已经在多个实际项目和比赛中应用过从室内竞速无人机到大型物流巡检无人机它都提供了一个坚实、可靠、高效的轨迹生成核心。它可能不是学术界最前沿的但绝对是工程实践中最经得起考验的方案之一。希望这些分享能帮你少走些弯路更快地让你的无人机飞得既聪明又稳健。