1. 项目概述与核心价值最近几年无人机集群协同作业已经从概念验证走向了实际应用尤其是在物流运输、应急救援和大型构件吊装等领域。但当我们把目光从单机或松散编队转向“多无人机刚性负载级联运输系统”时整个问题的复杂度就呈指数级上升了。这不再是简单的“多架飞机一起飞”而是要求多架无人机像一个整体一样协同搬运一个刚性、不可变形的负载并且它们之间还可能存在物理连接或级联关系。我花了相当长的时间和团队一起啃这个硬骨头核心就聚焦在两件事上轨迹规划和避障算法。前者决定了这个“空中运输队”能否平稳、高效地从A点到达B点后者则是在充满不确定性的真实环境中保障整个系统安全、不“撞车”的生命线。简单来说这个系统要解决的核心矛盾是既要保证多机协同的“一致性”和负载的“刚性约束”又要应对动态环境的“灵活性”和“实时性”。比如想象一下用三架无人机吊起一块大型玻璃幕墙穿过建筑工地。任何一架无人机轨迹的微小偏差都可能导致玻璃受力不均而破裂同时工地上的塔吊、突然出现的工人或车辆都需要整个编队能像“老司机”一样流畅地集体规避。这背后的算法就是我们要研究的核心。无论你是从事机器人学、控制理论的研究者还是正在开发工业级无人机协同应用的一线工程师理解这套系统的设计思路和实现难点都能为你打开一扇新的大门。2. 系统核心架构与约束分析在动手写代码或推导公式之前我们必须先把整个系统的“骨架”和“规矩”搞清楚。一个典型的多无人机刚性负载级联运输系统其物理和逻辑架构决定了我们算法设计的边界。2.1 系统物理模型与级联关系首先“刚性负载”意味着负载本身在运输过程中被视为一个刚体其内部各点之间的相对位置保持不变。这直接引入了运动学约束所有无人机的运动必须使得负载的位姿位置和姿态变化符合刚体运动规律。例如如果我们用三架无人机通过刚性杆连接一个平台那么这三架无人机在三维空间中的位置必须时刻满足平台保持水平或特定角度的几何关系。“级联运输”则描述了无人机之间的连接或依赖关系。常见的级联形式有两种物理级联无人机之间通过刚性或柔性连接件如连杆、绳索直接相连形成链式或网状结构。一架无人机的运动会通过连接件将力和运动传递到相邻无人机。控制级联无人机之间没有物理连接但在控制逻辑上存在主从或层级关系。例如指定一架为“领航者”其规划出的轨迹作为期望输入其他“跟随者”的轨迹则根据刚体约束和相对位置关系实时计算得出。在我们的研究中主要聚焦于控制级联的刚性负载系统因为它更通用也更容易通过算法实现。系统的状态可以用一个高维向量来描述包含了每架无人机的位置、速度、姿态角、角速度以及负载的中心位置和欧拉角。它们之间的关系通过一组非线性代数方程刚体约束方程联系起来。注意在实际建模时我们通常会对无人机进行简化例如将其视为质点或考虑其动力学为一阶或二阶积分器模型。但对于高精度、大负载场景必须引入更精确的动力学模型包括电机响应、空气动力学效应等这会让问题变得极其复杂通常需要基于模型预测控制MPC等高级方法。2.2 核心约束条件拆解规划算法必须严格遵守以下几类约束否则规划出的轨迹毫无意义刚体运动学约束这是最核心的等式约束。假设负载为刚体其上任一点P在世界坐标系中的位置r_P可以表示为r_P r_G R * p其中r_G是负载质心位置R是负载的姿态旋转矩阵p是点P在负载体坐标系中的固定坐标。对于每架与负载固定连接的无人机其位置r_i必须时刻满足此公式。这意味着无人机的自由度被大大缩减。无人机动力学约束每架无人机都有其物理极限包括最大速度/加速度决定了轨迹的平滑度和机动能力。最大倾角/角速度无人机不能无限快地改变姿态这限制了其横向机动能力。执行器饱和电机推力有上下限。避障约束这是不等式约束。对于静态障碍物要求无人机和负载的外包络与障碍物之间距离d大于安全阈值d_safed d_safe。对于动态障碍物此约束是时变的需要预测障碍物未来轨迹。队形保持与连通性约束在多机系统中为了防止无人机之间发生碰撞或丢失通信需要保持最小和最大距离约束。对于级联系统还需要保证控制链路的连通性。任务约束包括起始点和目标点的位姿、指定的通过路径点、任务完成时间等。将这些约束统一建模轨迹规划问题就转化为了一个高维、非线性、非凸、带硬约束的优化问题。我们的目标就是找到一个满足所有约束的、时间最优或能量最优的轨迹。3. 轨迹规划算法从集中式到分布式面对上述复杂问题没有一种“银弹”算法。我们需要根据任务场景、实时性要求和计算资源选择合适的规划策略。我将其分为三个层次全局路径规划、局部轨迹生成和实时轨迹优化。3.1 全局路径搜索与拓扑规划在已知环境地图包括静态障碍物的情况下我们首先需要为整个运输系统通常以负载质心为代表规划一条从起点到终点的无碰撞通道。由于负载有尺寸我们不能简单将其视为一个点。常用方法基于采样的路径规划对于这类高维空间规划问题基于采样的方法如RRT快速探索随机树星* 和PRM概率路图非常有效。但直接应用于我们的系统需要巧妙处理刚体约束。状态空间采样我们不能直接对每架无人机的位置独立采样那样几乎不可能满足刚体约束。一个实用的技巧是对负载的位姿进行采样。具体来说我们在负载的位姿空间SE(3)三维空间中的位置和姿态进行采样。每采样一个负载位姿(r_G, R)根据每架无人机在负载体坐标系中的固定连接点p_i利用公式r_i r_G R * p_i直接计算出所有无人机的位置。这样任何一个采样点都自动满足刚体约束。碰撞检测采样后需要对整个系统进行碰撞检测。这包括检测负载本身 approximated by a bounding box or convex hull是否与环境障碍物碰撞。检测每架无人机通常简化为一个球体是否与障碍物碰撞。可选检测连接无人机与负载的虚拟杆或绳索是否与障碍物碰撞。 这个过程计算量较大是性能瓶颈之一。我们通常使用层次包围盒如AABB、OBB和空间划分数据结构如Octree、KD-Tree来加速。通过RRT*等算法我们可以得到一条由一系列满足约束的负载位姿节点构成的初始路径。这条路径是“可行”的但可能不平滑不适合直接跟踪。3.2 局部轨迹生成与优化全局路径给出了一个“走廊”我们需要在其中生成一条光滑、动态可行的轨迹。这里多项式轨迹和B样条轨迹是主流选择。1. 最小抖动轨迹Minimum Snap Trajectory这是无人机领域非常经典的方法由Daniel Mellinger等人推广。其核心思想是生成一条分段多项式轨迹通常是四阶或五阶对应位置、速度、加速度连续并最小化控制量如推力的导数近似于加加速度Jerk或加加加速度Snap的平方积分。对于单机目标函数是min ∫(p^(4)(t))^2 dt最小化Snap 其中p(t)是位置的多项式函数。对于我们的多机刚性负载系统问题变成了协同最小化。我们不能独立优化每架无人机的轨迹因为它们通过刚体约束耦合。一个有效的策略是优化负载质心的轨迹。步骤将全局路径离散为一系列关键帧包括时间分配。以负载质心的位置r_G(t)和姿态R(t)或用四元数q(t)表示为优化变量构造分段多项式。约束条件边界条件起止点的位姿、速度通常为零。连续性条件各段多项式在连接点处的位置、速度、加速度连续。动态可行性将负载质心的加速度、角加速度映射回各无人机的推力需求确保不超过各无人机的最大推力。这引入了非线性约束。避障确保整条轨迹上由r_G(t)和R(t)决定的整个系统包络不与障碍物相交。这通常通过在高散时间点添加约束来实现。目标函数最小化负载质心轨迹的Snap或Jerk积分这能使运动更加平滑减少能量消耗和振荡。这个问题最终被形式化为一个带约束的二次规划QP问题如果姿态用多项式近似且避障约束线性化或更一般的非线性规划NLP问题。可以使用OSQP、IPOPT等求解器求解。2. B样条轨迹的优越性在实际项目中我们更倾向于使用均匀B样条。相比多项式B样条有几个致命优势凸包性质轨迹完全位于其控制点构成的凸包内。这意味着只要确保所有控制点是无碰撞的那么整条轨迹就是无碰撞的。这极大地简化了避障约束的处理。局部支撑性修改一个控制点只影响轨迹的局部段落这非常利于实时在线重规划。导数易于计算B样条的导数仍然是B样条且控制点有明确的物理意义与速度、加速度相关。对于多机系统我们可以为负载质心的位置和姿态分别生成一条B样条曲线。控制点的位置决定了轨迹的形状。优化过程就是调整这些控制点在满足动力学约束和凸包避障约束的前提下优化平滑性指标。3.3 分布式实时重规划策略集中式规划虽然能给出全局最优解但计算负担重难以应对未知动态障碍物。因此一个更鲁棒的架构是分层规划分布式反应。顶层一个中心节点或领航无人机运行全局规划器基于已知静态地图为负载质心生成一条粗略的、时间较长的B样条参考轨迹T_ref。中层各架无人机或分组基于共享的T_ref利用其局部传感器激光雷达、深度相机感知周围动态障碍物。当预测到可能与障碍物碰撞时触发局部重规划。底层局部重规划不再重新求解一个复杂的NLP问题而是采用弹性带Elastic Band或模型预测控制MPC的思想。基于MPC的分布式避障是我们目前验证最有效的实时策略每个控制周期如50ms每架无人机根据当前状态和感知信息独立求解一个短时域如2秒的最优控制问题。问题的成本函数包括跟踪T_ref的误差、控制量大小节能、以及与障碍物的距离惩罚项。关键耦合在每架无人机的优化问题中必须包含其他无人机预测状态的约束。也就是说无人机A在规划自己的未来2秒轨迹时会把无人机B、C根据上一周期规划结果预测的未来位置作为“移动障碍物”来避让。同时它自己的规划结果也会通过通信广播给其他无人机。通过这种分布式但带耦合约束的MPC无人机们能在极短时间内协商出一条满足刚体约束的、局部避障的协同轨迹。这本质是一个分布式优化问题常用交替方向乘子法ADMM来高效求解。实操心得分布式MPC的通信延迟和一致性是最大挑战。我们采用了同步规划周期和轨迹缓冲机制。所有无人机在同一时钟信号下开始规划规划用时必须小于周期时间。规划出的轨迹并非立即执行而是延迟一个周期执行这样本周期规划时已知其他无人机上一周期的轨迹保证了预测信息的一致性有效避免了“对向而行”的决策冲突。4. 避障算法从几何到势场避障不是简单地在规划好的轨迹上“贴”一个反应层而是需要深度融合进规划优化的过程中。针对静态和动态障碍物我们采用了不同的策略组合。4.1 静态障碍物基于ESDF的梯度引导对于已知的静态环境构建欧几里得符号距离场ESDF是提升规划效率和质量的黄金标准。什么是ESDF它是一个三维网格地图每个网格存储着该位置到最近障碍物表面的有符号距离。在障碍物内部为负外部为正表面为零。如何用于规划在优化轨迹的控制点时我们可以将ESDF值作为成本项或约束项。成本项在目标函数中添加一项∑ exp(-d_i / d0)其中d_i是第i个控制点处的ESDF值d0是衰减系数。这会“推离”轨迹使其远离障碍物。约束项更严格的方式是要求所有控制点的ESDF值大于安全距离d_safe。得益于B样条的凸包性这能保证整条轨迹安全。梯度信息ESDF的梯度方向指向障碍物增加最快的方向即远离障碍物的方向。在优化迭代中我们可以利用梯度信息快速将轨迹“拉”出碰撞区域大大加快收敛速度。我们使用FIESTA或Voxblox等开源库来增量式构建和更新ESDF效率很高。4.2 动态障碍物速度障碍法与ORCA对于环境中突然出现的行人、车辆或其他无人机我们需要在线实时避碰。速度障碍法Velocity Obstacle, VO及其优化版本最优互惠避碰ORCA是分布式、反应式避障的利器。核心思想以VO为例假设障碍物和自身都以当前速度匀速运动。在速度空间中存在一个“碰撞锥”VO区域如果选择落入该区域的速度向量未来一定会发生碰撞。因此避障就是在速度空间中选择一个不属于任何障碍物VO区域的速度并且尽可能接近期望速度来自轨迹跟踪。在多机刚性负载系统中的应用这里有个关键问题——每架无人机不能独立选择速度因为它们的速度通过刚体约束相关联。负载的角速度ω和质心线速度v_G共同决定了每架无人机的速度v_i v_G ω × (r_i - r_G)。我们的解决方案将整个运输系统视为一个“扩展刚体”计算其外包络一个包含所有无人机和负载的凸多面体。为这个扩展刚体计算其相对于每个动态障碍物的VO区域。但这个VO是在系统速度空间v_G, ω中定义的这是一个6维空间3维线速度3维角速度。求解一个优化问题在系统速度空间中寻找一个点v_G, ω使其避开所有VO区域同时最小化与期望系统速度来自参考轨迹的偏差。求解出的安全系统速度v_G_safe, ω_safe再通过运动学关系分解为各无人机的期望速度下发给底层控制器跟踪。这种方法将多机协同避障统一到了一个刚体避障的框架下非常优雅且有效。ORCA则在此基础上通过线性规划为每个智能体这里是我们整个系统提供互惠的、无震荡的速度选择保证。4.3 混合障碍物场景下的分层融合真实环境是静态与动态障碍物并存。我们的算法框架如下全局层基于静态地图和ESDF规划出无碰撞的B样条参考轨迹T_ref。预测层利用感知模块跟踪动态障碍物预测其未来数秒的轨迹简化为匀速或CV/CA模型。局部重规划层MPC将T_ref在未来局部时域内的段落作为跟踪目标。将静态障碍物的ESDF距离作为优化约束或惩罚项。将动态障碍物的预测轨迹通过VO/ORCA方法转化为对系统速度v_G, ω的线性约束加入到MPC的约束条件中。执行层MPC求解出最优的系统速度指令和无人机控制序列下发给底层飞控。这个框架实现了全局最优性与局部 reactivity的结合静态避障的精确性与动态避障的实时性的互补。5. 仿真验证与核心参数调优算法设计得再精妙也需要在仿真中反复锤炼。我们主要基于ROS (Robot Operating System)Gazebo搭建仿真环境并使用MATLAB/Simulink进行快速算法原型验证和参数整定。5.1 仿真环境搭建要点无人机模型我们使用PX4 SITL (Software In The Loop)或更简化的rotors_simulator插件。对于算法研究关键是模型保真度与计算效率的平衡。我们采用刚体动力学模型并考虑了电机的一阶延迟和推力-力矩系数。负载与连接建模在Gazebo中通过gazebo_ros_pkgs创建刚性负载模型并使用Fixed Joint将无人机与负载连接起来模拟刚性连接。对于级联系统则通过定义无人机之间的相对位姿约束在控制层面实现而非物理关节。传感器模拟为每架无人机添加模拟的激光雷达如gazebo_ros_ray_sensor或深度相机如gazebo_ros_depth_camera用于感知障碍物。点云数据通过ROS话题发布。障碍物环境构建包含复杂结构如窗户、管道的静态场景并添加按预设路径移动的动态模型如行人、小车。5.2 核心算法参数调优实录参数调优是算法落地中最耗时、最需要经验的部分。以下是几个关键参数及其调试心得B样条轨迹参数阶数 (k)通常选择4阶三次B样条。它保证位置、速度、加速度连续且计算复杂度适中。阶数越高越平滑但控制点影响范围越大局部调整不灵活。控制点间隔 (Δt)这是最影响轨迹“分辨率”和优化自由度的参数。间隔越小控制点越多轨迹越能贴合复杂路径但优化变量也越多计算越慢。我们的经验公式Δt ≈ 期望平均速度 / (2~3 * 控制频率)。例如期望速度1m/s控制频率10Hz则Δt取0.05s到0.1s。优化目标权重在最小化Snap/Snap的代价函数中我们通常还会加入控制点距离原始参考点的惩罚项防止轨迹变形过大和轨迹时间惩罚项。平滑性权重与时间权重的比值需要仔细调整。我们的策略是先给一个很大的平滑性权重得到一条非常平滑但可能绕远的轨迹然后逐步增加时间权重观察轨迹如何“拉直”直到在动态约束下达到时间最短。MPC参数预测时域 (N)和控制周期 (dt)N * dt决定了“看多远”。太短1s反应迟钝太长3s计算量大且预测不准。我们固定dt0.1sN取20即预测2秒在大多数场景下取得了良好平衡。状态/控制量权重矩阵 (Q, R)Q矩阵惩罚状态误差位置、速度R矩阵惩罚控制量推力、力矩。调试口诀先调R再调Q。先给R一个较大的值限制控制量的剧烈变化保证系统基本稳定然后逐步增大Q中对位置误差的惩罚使跟踪更紧密。对于多机系统负载质心跟踪的权重应远大于单机姿态跟踪的权重。避障约束安全距离 (d_safe)这不是一个固定值。我们采用动态安全距离d_safe d_min k * v_rel其中d_min是静态安全距离如0.5米v_rel是相对速度大小k是反应时间系数如0.5s。这样相对速度越大预留的安全距离就越大更符合实际。VO/ORCA参数时间窗口 (τ)在VO中它定义了考虑多长时间的匀速运动。通常取τ 2~5 s。τ 越大避障动作越提前但也可能过于保守。我们根据系统最大减速度来设定τ ≥ v_max / a_max确保在时间窗口内能够刹停。机器人半径在VO计算中需要将自身和障碍物“膨胀”一个半径。对于我们的扩展刚体我们取其外包络球的外接球半径。一个更精确但复杂的方法是计算两个凸多面体之间的Minkowski和。踩坑记录初期我们将所有无人机的感知数据集中融合成一个全局地图然后做规划。这带来了巨大的通信和计算开销且单点故障风险高。后来改为分布式感知局部地图共享。每架无人机只处理自己的局部点云生成一个小范围的局部ESDF然后通过通信只共享其边界处的距离场信息或共享关键障碍物位置极大地降低了带宽需求并实现了去中心化系统鲁棒性显著提升。6. 从仿真到实飞的挑战与解决方案仿真通过后真正的挑战才刚刚开始。将算法部署到真实的无人机硬件上会遇到无数仿真中未曾出现的问题。6.1 通信延迟与丢包多机协同的核心是信息同步。我们使用ROS2的DDS (Data Distribution Service)中间件它提供了更好的实时性和QoS服务质量控制。但无线网络如Wi-Fi 5Ghz固有的延迟20-100ms和偶尔丢包是无法避免的。解决方案状态估计与预测每架无人机不仅广播自己的当前状态还广播一个短时间的未来轨迹预测例如未来0.5秒以0.05s为间隔的一组状态点。其他无人机在收到信息后如果检测到延迟可以根据对方过去的轨迹和动力学模型外推其当前最可能的状态而不是直接使用过时的数据。一致性协议对于关键指令如紧急悬停、任务切换我们采用简单的两阶段提交。领航者发出指令后需要收到所有跟随者的确认才认为指令生效。同时设置超时机制超时未确认则触发安全策略如原地悬停。数据压缩与选择性发送不是所有数据都需要高频发送。例如局部ESDF地图可以只发送更新了的体素或关键障碍物信息。轨迹信息可以用B样条控制点表示远比发送密集的路径点节省带宽。6.2 状态估计与传感器误差仿真中我们有完美的全局定位。现实中无人机依赖视觉惯性里程计 (VIO)或GPS-IMU融合。这些都有误差尤其是VIO在快速运动或纹理缺失区域容易漂移。对刚性约束的破坏各无人机独立估计的位姿即使精度很高也会存在微小的不一致性。这些不一致性在算法中会被解读为“刚体约束被破坏”导致控制器产生不必要的修正力甚至引发振荡。解决方案相对定位辅助在负载上安装UWB (超宽带)或激光测距模块直接测量无人机之间的相对距离。将这些相对测量值融合进各无人机的状态估计器可以有效“锚定”它们之间的几何关系减少协同误差。基于负载的协同定位如果负载上安装了一个主传感器如激光SLAM或视觉标记它可以提供一个相对准确的负载位姿估计。各无人机则主要估计自己相对于这个负载的位姿通过机载相机识别负载上的标记或通过测量与负载固定点的距离。这样所有无人机的位姿都统一到一个共同的参考系下一致性极大提高。控制器鲁棒性设计在底层控制器中对刚体约束的误差容忍度设置一个死区。只要位姿误差在死区内就不产生修正力。这避免了系统对微小噪声的过度反应。6.3 执行器不一致性与外部扰动即使同一批次的电机和螺旋桨其推力系数也存在细微差异。风扰是对室外飞行最大的挑战。问题这会导致即使给出相同的控制指令各无人机的实际推力也不同从而破坏力的平衡导致负载倾斜或编队形变。解决方案前馈-反馈复合控制在底层控制器中不仅包含基于位姿误差的反馈控制如PID还加入基于模型的前馈控制。前馈部分计算为保持当前运动状态所需的理想推力反馈部分则补偿模型误差和扰动。负载力矩分配优化上层规划器给出的是负载所需的合力和合力矩。需要将其分配到各无人机上。这是一个控制分配问题。我们采用加权伪逆法在满足各无人机推力上下限的前提下最小化所有无人机推力的方差。这样可以让推力更均匀同时为某些电机故障提供一定的冗余能力。自适应与扰动观测器设计扰动观测器DOB来实时估计并补偿持续的风扰。对于执行器差异可以在起飞前进行简单的“悬停标定”让每架无人机独立悬停记录其达到悬停所需的油门指令后续控制分配时以此作为基准进行补偿。从仿真到实飞是一个不断发现并解决问题的循环。我们的经验是仿真中要将模型尽可能贴近现实加入噪声、延迟、通信丢包模型实飞则要从最简单、最安全的场景开始如室内无风环境、低速、低高度逐步增加复杂度。每一次失败的飞行日志都比十次成功的仿真更有价值。
多无人机刚性负载协同运输：轨迹规划与避障算法全解析
发布时间：2026/6/21 4:22:46
1. 项目概述与核心价值最近几年无人机集群协同作业已经从概念验证走向了实际应用尤其是在物流运输、应急救援和大型构件吊装等领域。但当我们把目光从单机或松散编队转向“多无人机刚性负载级联运输系统”时整个问题的复杂度就呈指数级上升了。这不再是简单的“多架飞机一起飞”而是要求多架无人机像一个整体一样协同搬运一个刚性、不可变形的负载并且它们之间还可能存在物理连接或级联关系。我花了相当长的时间和团队一起啃这个硬骨头核心就聚焦在两件事上轨迹规划和避障算法。前者决定了这个“空中运输队”能否平稳、高效地从A点到达B点后者则是在充满不确定性的真实环境中保障整个系统安全、不“撞车”的生命线。简单来说这个系统要解决的核心矛盾是既要保证多机协同的“一致性”和负载的“刚性约束”又要应对动态环境的“灵活性”和“实时性”。比如想象一下用三架无人机吊起一块大型玻璃幕墙穿过建筑工地。任何一架无人机轨迹的微小偏差都可能导致玻璃受力不均而破裂同时工地上的塔吊、突然出现的工人或车辆都需要整个编队能像“老司机”一样流畅地集体规避。这背后的算法就是我们要研究的核心。无论你是从事机器人学、控制理论的研究者还是正在开发工业级无人机协同应用的一线工程师理解这套系统的设计思路和实现难点都能为你打开一扇新的大门。2. 系统核心架构与约束分析在动手写代码或推导公式之前我们必须先把整个系统的“骨架”和“规矩”搞清楚。一个典型的多无人机刚性负载级联运输系统其物理和逻辑架构决定了我们算法设计的边界。2.1 系统物理模型与级联关系首先“刚性负载”意味着负载本身在运输过程中被视为一个刚体其内部各点之间的相对位置保持不变。这直接引入了运动学约束所有无人机的运动必须使得负载的位姿位置和姿态变化符合刚体运动规律。例如如果我们用三架无人机通过刚性杆连接一个平台那么这三架无人机在三维空间中的位置必须时刻满足平台保持水平或特定角度的几何关系。“级联运输”则描述了无人机之间的连接或依赖关系。常见的级联形式有两种物理级联无人机之间通过刚性或柔性连接件如连杆、绳索直接相连形成链式或网状结构。一架无人机的运动会通过连接件将力和运动传递到相邻无人机。控制级联无人机之间没有物理连接但在控制逻辑上存在主从或层级关系。例如指定一架为“领航者”其规划出的轨迹作为期望输入其他“跟随者”的轨迹则根据刚体约束和相对位置关系实时计算得出。在我们的研究中主要聚焦于控制级联的刚性负载系统因为它更通用也更容易通过算法实现。系统的状态可以用一个高维向量来描述包含了每架无人机的位置、速度、姿态角、角速度以及负载的中心位置和欧拉角。它们之间的关系通过一组非线性代数方程刚体约束方程联系起来。注意在实际建模时我们通常会对无人机进行简化例如将其视为质点或考虑其动力学为一阶或二阶积分器模型。但对于高精度、大负载场景必须引入更精确的动力学模型包括电机响应、空气动力学效应等这会让问题变得极其复杂通常需要基于模型预测控制MPC等高级方法。2.2 核心约束条件拆解规划算法必须严格遵守以下几类约束否则规划出的轨迹毫无意义刚体运动学约束这是最核心的等式约束。假设负载为刚体其上任一点P在世界坐标系中的位置r_P可以表示为r_P r_G R * p其中r_G是负载质心位置R是负载的姿态旋转矩阵p是点P在负载体坐标系中的固定坐标。对于每架与负载固定连接的无人机其位置r_i必须时刻满足此公式。这意味着无人机的自由度被大大缩减。无人机动力学约束每架无人机都有其物理极限包括最大速度/加速度决定了轨迹的平滑度和机动能力。最大倾角/角速度无人机不能无限快地改变姿态这限制了其横向机动能力。执行器饱和电机推力有上下限。避障约束这是不等式约束。对于静态障碍物要求无人机和负载的外包络与障碍物之间距离d大于安全阈值d_safed d_safe。对于动态障碍物此约束是时变的需要预测障碍物未来轨迹。队形保持与连通性约束在多机系统中为了防止无人机之间发生碰撞或丢失通信需要保持最小和最大距离约束。对于级联系统还需要保证控制链路的连通性。任务约束包括起始点和目标点的位姿、指定的通过路径点、任务完成时间等。将这些约束统一建模轨迹规划问题就转化为了一个高维、非线性、非凸、带硬约束的优化问题。我们的目标就是找到一个满足所有约束的、时间最优或能量最优的轨迹。3. 轨迹规划算法从集中式到分布式面对上述复杂问题没有一种“银弹”算法。我们需要根据任务场景、实时性要求和计算资源选择合适的规划策略。我将其分为三个层次全局路径规划、局部轨迹生成和实时轨迹优化。3.1 全局路径搜索与拓扑规划在已知环境地图包括静态障碍物的情况下我们首先需要为整个运输系统通常以负载质心为代表规划一条从起点到终点的无碰撞通道。由于负载有尺寸我们不能简单将其视为一个点。常用方法基于采样的路径规划对于这类高维空间规划问题基于采样的方法如RRT快速探索随机树星* 和PRM概率路图非常有效。但直接应用于我们的系统需要巧妙处理刚体约束。状态空间采样我们不能直接对每架无人机的位置独立采样那样几乎不可能满足刚体约束。一个实用的技巧是对负载的位姿进行采样。具体来说我们在负载的位姿空间SE(3)三维空间中的位置和姿态进行采样。每采样一个负载位姿(r_G, R)根据每架无人机在负载体坐标系中的固定连接点p_i利用公式r_i r_G R * p_i直接计算出所有无人机的位置。这样任何一个采样点都自动满足刚体约束。碰撞检测采样后需要对整个系统进行碰撞检测。这包括检测负载本身 approximated by a bounding box or convex hull是否与环境障碍物碰撞。检测每架无人机通常简化为一个球体是否与障碍物碰撞。可选检测连接无人机与负载的虚拟杆或绳索是否与障碍物碰撞。 这个过程计算量较大是性能瓶颈之一。我们通常使用层次包围盒如AABB、OBB和空间划分数据结构如Octree、KD-Tree来加速。通过RRT*等算法我们可以得到一条由一系列满足约束的负载位姿节点构成的初始路径。这条路径是“可行”的但可能不平滑不适合直接跟踪。3.2 局部轨迹生成与优化全局路径给出了一个“走廊”我们需要在其中生成一条光滑、动态可行的轨迹。这里多项式轨迹和B样条轨迹是主流选择。1. 最小抖动轨迹Minimum Snap Trajectory这是无人机领域非常经典的方法由Daniel Mellinger等人推广。其核心思想是生成一条分段多项式轨迹通常是四阶或五阶对应位置、速度、加速度连续并最小化控制量如推力的导数近似于加加速度Jerk或加加加速度Snap的平方积分。对于单机目标函数是min ∫(p^(4)(t))^2 dt最小化Snap 其中p(t)是位置的多项式函数。对于我们的多机刚性负载系统问题变成了协同最小化。我们不能独立优化每架无人机的轨迹因为它们通过刚体约束耦合。一个有效的策略是优化负载质心的轨迹。步骤将全局路径离散为一系列关键帧包括时间分配。以负载质心的位置r_G(t)和姿态R(t)或用四元数q(t)表示为优化变量构造分段多项式。约束条件边界条件起止点的位姿、速度通常为零。连续性条件各段多项式在连接点处的位置、速度、加速度连续。动态可行性将负载质心的加速度、角加速度映射回各无人机的推力需求确保不超过各无人机的最大推力。这引入了非线性约束。避障确保整条轨迹上由r_G(t)和R(t)决定的整个系统包络不与障碍物相交。这通常通过在高散时间点添加约束来实现。目标函数最小化负载质心轨迹的Snap或Jerk积分这能使运动更加平滑减少能量消耗和振荡。这个问题最终被形式化为一个带约束的二次规划QP问题如果姿态用多项式近似且避障约束线性化或更一般的非线性规划NLP问题。可以使用OSQP、IPOPT等求解器求解。2. B样条轨迹的优越性在实际项目中我们更倾向于使用均匀B样条。相比多项式B样条有几个致命优势凸包性质轨迹完全位于其控制点构成的凸包内。这意味着只要确保所有控制点是无碰撞的那么整条轨迹就是无碰撞的。这极大地简化了避障约束的处理。局部支撑性修改一个控制点只影响轨迹的局部段落这非常利于实时在线重规划。导数易于计算B样条的导数仍然是B样条且控制点有明确的物理意义与速度、加速度相关。对于多机系统我们可以为负载质心的位置和姿态分别生成一条B样条曲线。控制点的位置决定了轨迹的形状。优化过程就是调整这些控制点在满足动力学约束和凸包避障约束的前提下优化平滑性指标。3.3 分布式实时重规划策略集中式规划虽然能给出全局最优解但计算负担重难以应对未知动态障碍物。因此一个更鲁棒的架构是分层规划分布式反应。顶层一个中心节点或领航无人机运行全局规划器基于已知静态地图为负载质心生成一条粗略的、时间较长的B样条参考轨迹T_ref。中层各架无人机或分组基于共享的T_ref利用其局部传感器激光雷达、深度相机感知周围动态障碍物。当预测到可能与障碍物碰撞时触发局部重规划。底层局部重规划不再重新求解一个复杂的NLP问题而是采用弹性带Elastic Band或模型预测控制MPC的思想。基于MPC的分布式避障是我们目前验证最有效的实时策略每个控制周期如50ms每架无人机根据当前状态和感知信息独立求解一个短时域如2秒的最优控制问题。问题的成本函数包括跟踪T_ref的误差、控制量大小节能、以及与障碍物的距离惩罚项。关键耦合在每架无人机的优化问题中必须包含其他无人机预测状态的约束。也就是说无人机A在规划自己的未来2秒轨迹时会把无人机B、C根据上一周期规划结果预测的未来位置作为“移动障碍物”来避让。同时它自己的规划结果也会通过通信广播给其他无人机。通过这种分布式但带耦合约束的MPC无人机们能在极短时间内协商出一条满足刚体约束的、局部避障的协同轨迹。这本质是一个分布式优化问题常用交替方向乘子法ADMM来高效求解。实操心得分布式MPC的通信延迟和一致性是最大挑战。我们采用了同步规划周期和轨迹缓冲机制。所有无人机在同一时钟信号下开始规划规划用时必须小于周期时间。规划出的轨迹并非立即执行而是延迟一个周期执行这样本周期规划时已知其他无人机上一周期的轨迹保证了预测信息的一致性有效避免了“对向而行”的决策冲突。4. 避障算法从几何到势场避障不是简单地在规划好的轨迹上“贴”一个反应层而是需要深度融合进规划优化的过程中。针对静态和动态障碍物我们采用了不同的策略组合。4.1 静态障碍物基于ESDF的梯度引导对于已知的静态环境构建欧几里得符号距离场ESDF是提升规划效率和质量的黄金标准。什么是ESDF它是一个三维网格地图每个网格存储着该位置到最近障碍物表面的有符号距离。在障碍物内部为负外部为正表面为零。如何用于规划在优化轨迹的控制点时我们可以将ESDF值作为成本项或约束项。成本项在目标函数中添加一项∑ exp(-d_i / d0)其中d_i是第i个控制点处的ESDF值d0是衰减系数。这会“推离”轨迹使其远离障碍物。约束项更严格的方式是要求所有控制点的ESDF值大于安全距离d_safe。得益于B样条的凸包性这能保证整条轨迹安全。梯度信息ESDF的梯度方向指向障碍物增加最快的方向即远离障碍物的方向。在优化迭代中我们可以利用梯度信息快速将轨迹“拉”出碰撞区域大大加快收敛速度。我们使用FIESTA或Voxblox等开源库来增量式构建和更新ESDF效率很高。4.2 动态障碍物速度障碍法与ORCA对于环境中突然出现的行人、车辆或其他无人机我们需要在线实时避碰。速度障碍法Velocity Obstacle, VO及其优化版本最优互惠避碰ORCA是分布式、反应式避障的利器。核心思想以VO为例假设障碍物和自身都以当前速度匀速运动。在速度空间中存在一个“碰撞锥”VO区域如果选择落入该区域的速度向量未来一定会发生碰撞。因此避障就是在速度空间中选择一个不属于任何障碍物VO区域的速度并且尽可能接近期望速度来自轨迹跟踪。在多机刚性负载系统中的应用这里有个关键问题——每架无人机不能独立选择速度因为它们的速度通过刚体约束相关联。负载的角速度ω和质心线速度v_G共同决定了每架无人机的速度v_i v_G ω × (r_i - r_G)。我们的解决方案将整个运输系统视为一个“扩展刚体”计算其外包络一个包含所有无人机和负载的凸多面体。为这个扩展刚体计算其相对于每个动态障碍物的VO区域。但这个VO是在系统速度空间v_G, ω中定义的这是一个6维空间3维线速度3维角速度。求解一个优化问题在系统速度空间中寻找一个点v_G, ω使其避开所有VO区域同时最小化与期望系统速度来自参考轨迹的偏差。求解出的安全系统速度v_G_safe, ω_safe再通过运动学关系分解为各无人机的期望速度下发给底层控制器跟踪。这种方法将多机协同避障统一到了一个刚体避障的框架下非常优雅且有效。ORCA则在此基础上通过线性规划为每个智能体这里是我们整个系统提供互惠的、无震荡的速度选择保证。4.3 混合障碍物场景下的分层融合真实环境是静态与动态障碍物并存。我们的算法框架如下全局层基于静态地图和ESDF规划出无碰撞的B样条参考轨迹T_ref。预测层利用感知模块跟踪动态障碍物预测其未来数秒的轨迹简化为匀速或CV/CA模型。局部重规划层MPC将T_ref在未来局部时域内的段落作为跟踪目标。将静态障碍物的ESDF距离作为优化约束或惩罚项。将动态障碍物的预测轨迹通过VO/ORCA方法转化为对系统速度v_G, ω的线性约束加入到MPC的约束条件中。执行层MPC求解出最优的系统速度指令和无人机控制序列下发给底层飞控。这个框架实现了全局最优性与局部 reactivity的结合静态避障的精确性与动态避障的实时性的互补。5. 仿真验证与核心参数调优算法设计得再精妙也需要在仿真中反复锤炼。我们主要基于ROS (Robot Operating System)Gazebo搭建仿真环境并使用MATLAB/Simulink进行快速算法原型验证和参数整定。5.1 仿真环境搭建要点无人机模型我们使用PX4 SITL (Software In The Loop)或更简化的rotors_simulator插件。对于算法研究关键是模型保真度与计算效率的平衡。我们采用刚体动力学模型并考虑了电机的一阶延迟和推力-力矩系数。负载与连接建模在Gazebo中通过gazebo_ros_pkgs创建刚性负载模型并使用Fixed Joint将无人机与负载连接起来模拟刚性连接。对于级联系统则通过定义无人机之间的相对位姿约束在控制层面实现而非物理关节。传感器模拟为每架无人机添加模拟的激光雷达如gazebo_ros_ray_sensor或深度相机如gazebo_ros_depth_camera用于感知障碍物。点云数据通过ROS话题发布。障碍物环境构建包含复杂结构如窗户、管道的静态场景并添加按预设路径移动的动态模型如行人、小车。5.2 核心算法参数调优实录参数调优是算法落地中最耗时、最需要经验的部分。以下是几个关键参数及其调试心得B样条轨迹参数阶数 (k)通常选择4阶三次B样条。它保证位置、速度、加速度连续且计算复杂度适中。阶数越高越平滑但控制点影响范围越大局部调整不灵活。控制点间隔 (Δt)这是最影响轨迹“分辨率”和优化自由度的参数。间隔越小控制点越多轨迹越能贴合复杂路径但优化变量也越多计算越慢。我们的经验公式Δt ≈ 期望平均速度 / (2~3 * 控制频率)。例如期望速度1m/s控制频率10Hz则Δt取0.05s到0.1s。优化目标权重在最小化Snap/Snap的代价函数中我们通常还会加入控制点距离原始参考点的惩罚项防止轨迹变形过大和轨迹时间惩罚项。平滑性权重与时间权重的比值需要仔细调整。我们的策略是先给一个很大的平滑性权重得到一条非常平滑但可能绕远的轨迹然后逐步增加时间权重观察轨迹如何“拉直”直到在动态约束下达到时间最短。MPC参数预测时域 (N)和控制周期 (dt)N * dt决定了“看多远”。太短1s反应迟钝太长3s计算量大且预测不准。我们固定dt0.1sN取20即预测2秒在大多数场景下取得了良好平衡。状态/控制量权重矩阵 (Q, R)Q矩阵惩罚状态误差位置、速度R矩阵惩罚控制量推力、力矩。调试口诀先调R再调Q。先给R一个较大的值限制控制量的剧烈变化保证系统基本稳定然后逐步增大Q中对位置误差的惩罚使跟踪更紧密。对于多机系统负载质心跟踪的权重应远大于单机姿态跟踪的权重。避障约束安全距离 (d_safe)这不是一个固定值。我们采用动态安全距离d_safe d_min k * v_rel其中d_min是静态安全距离如0.5米v_rel是相对速度大小k是反应时间系数如0.5s。这样相对速度越大预留的安全距离就越大更符合实际。VO/ORCA参数时间窗口 (τ)在VO中它定义了考虑多长时间的匀速运动。通常取τ 2~5 s。τ 越大避障动作越提前但也可能过于保守。我们根据系统最大减速度来设定τ ≥ v_max / a_max确保在时间窗口内能够刹停。机器人半径在VO计算中需要将自身和障碍物“膨胀”一个半径。对于我们的扩展刚体我们取其外包络球的外接球半径。一个更精确但复杂的方法是计算两个凸多面体之间的Minkowski和。踩坑记录初期我们将所有无人机的感知数据集中融合成一个全局地图然后做规划。这带来了巨大的通信和计算开销且单点故障风险高。后来改为分布式感知局部地图共享。每架无人机只处理自己的局部点云生成一个小范围的局部ESDF然后通过通信只共享其边界处的距离场信息或共享关键障碍物位置极大地降低了带宽需求并实现了去中心化系统鲁棒性显著提升。6. 从仿真到实飞的挑战与解决方案仿真通过后真正的挑战才刚刚开始。将算法部署到真实的无人机硬件上会遇到无数仿真中未曾出现的问题。6.1 通信延迟与丢包多机协同的核心是信息同步。我们使用ROS2的DDS (Data Distribution Service)中间件它提供了更好的实时性和QoS服务质量控制。但无线网络如Wi-Fi 5Ghz固有的延迟20-100ms和偶尔丢包是无法避免的。解决方案状态估计与预测每架无人机不仅广播自己的当前状态还广播一个短时间的未来轨迹预测例如未来0.5秒以0.05s为间隔的一组状态点。其他无人机在收到信息后如果检测到延迟可以根据对方过去的轨迹和动力学模型外推其当前最可能的状态而不是直接使用过时的数据。一致性协议对于关键指令如紧急悬停、任务切换我们采用简单的两阶段提交。领航者发出指令后需要收到所有跟随者的确认才认为指令生效。同时设置超时机制超时未确认则触发安全策略如原地悬停。数据压缩与选择性发送不是所有数据都需要高频发送。例如局部ESDF地图可以只发送更新了的体素或关键障碍物信息。轨迹信息可以用B样条控制点表示远比发送密集的路径点节省带宽。6.2 状态估计与传感器误差仿真中我们有完美的全局定位。现实中无人机依赖视觉惯性里程计 (VIO)或GPS-IMU融合。这些都有误差尤其是VIO在快速运动或纹理缺失区域容易漂移。对刚性约束的破坏各无人机独立估计的位姿即使精度很高也会存在微小的不一致性。这些不一致性在算法中会被解读为“刚体约束被破坏”导致控制器产生不必要的修正力甚至引发振荡。解决方案相对定位辅助在负载上安装UWB (超宽带)或激光测距模块直接测量无人机之间的相对距离。将这些相对测量值融合进各无人机的状态估计器可以有效“锚定”它们之间的几何关系减少协同误差。基于负载的协同定位如果负载上安装了一个主传感器如激光SLAM或视觉标记它可以提供一个相对准确的负载位姿估计。各无人机则主要估计自己相对于这个负载的位姿通过机载相机识别负载上的标记或通过测量与负载固定点的距离。这样所有无人机的位姿都统一到一个共同的参考系下一致性极大提高。控制器鲁棒性设计在底层控制器中对刚体约束的误差容忍度设置一个死区。只要位姿误差在死区内就不产生修正力。这避免了系统对微小噪声的过度反应。6.3 执行器不一致性与外部扰动即使同一批次的电机和螺旋桨其推力系数也存在细微差异。风扰是对室外飞行最大的挑战。问题这会导致即使给出相同的控制指令各无人机的实际推力也不同从而破坏力的平衡导致负载倾斜或编队形变。解决方案前馈-反馈复合控制在底层控制器中不仅包含基于位姿误差的反馈控制如PID还加入基于模型的前馈控制。前馈部分计算为保持当前运动状态所需的理想推力反馈部分则补偿模型误差和扰动。负载力矩分配优化上层规划器给出的是负载所需的合力和合力矩。需要将其分配到各无人机上。这是一个控制分配问题。我们采用加权伪逆法在满足各无人机推力上下限的前提下最小化所有无人机推力的方差。这样可以让推力更均匀同时为某些电机故障提供一定的冗余能力。自适应与扰动观测器设计扰动观测器DOB来实时估计并补偿持续的风扰。对于执行器差异可以在起飞前进行简单的“悬停标定”让每架无人机独立悬停记录其达到悬停所需的油门指令后续控制分配时以此作为基准进行补偿。从仿真到实飞是一个不断发现并解决问题的循环。我们的经验是仿真中要将模型尽可能贴近现实加入噪声、延迟、通信丢包模型实飞则要从最简单、最安全的场景开始如室内无风环境、低速、低高度逐步增加复杂度。每一次失败的飞行日志都比十次成功的仿真更有价值。
