★ FEATURED ARTICLE 多四旋翼编队控制在这些年确实是个热门方向无人机集群表演、物流配送、灾后搜索背后都离不开“多机协同”这四个字。但真要把一套完整的多无人机系统跑起来你会发现事情远没想象中那么简单——机与机之间怎么排布、任务怎么分、飞之前怎么规划出一条安全路线每一环都是独立的坑。这篇文章就围绕一个我最近完整调通的Matlab仿真项目展开它覆盖了阵形编队跟随控制、目标分配、全局路径规划三个核心模块适合正在做无人机集群控制方向课题、或者是想用仿真验证编队算法的人参考我会把设计思路、关键原理、代码结构和踩过的坑一次性讲清楚。1. 项目整体设计与思路拆解1.1 三个核心模块联动逻辑刚开始接触多四旋翼编队的人容易把问题想简单让每架飞机都往目标点飞不就是编队了实际上不是。一个能落地的多无人机系统至少要解决三个连续的子问题第一多个目标点怎么分配给多架飞机让总飞行代价最小第二每一架飞机从当前位置到目标点之间的路径要怎么规划且能避开障碍物第三飞行途中怎么维持阵形让编队作为一个整体前进而不是各飞各的。这三个模块在仿真中的关系是串行加并行目标分配模块先行执行把任务“排好顺序”全局路径规划为编队的参考轨迹通常是编队中心的轨迹提供无碰撞路径编队跟随控制模块则以路径规划的输出作为期望运动指令结合编队内部的一致性约束驱动每一架四旋翼协同飞行。三者的输出最终汇入一个统一的仿真循环中。一个错误的做法是把路径规划和编队控制割裂开先规划好每条路再让编队沿着路飞。这样看似合理但一旦遇到动态障碍物或者需要整体绕行编队内部间距保持会非常困难。我的做法是规划一条编队参考点路径让整个编队跟随这条路径单机层面的避障由编队形变来吸收这样模块间耦合小、稳定性好。1.2 为什么指定Matlab作为仿真平台Matlab在无人机编队仿真领域基本属于“标配”选择原因有几个一是控制算法原型的迭代效率极高矩阵运算、控制系统工具箱、优化工具箱开箱即用二是可视化能力足够强三维轨迹绘制、动画演示都只需要几行代码这对于验证编队效果非常重要毕竟光看数据很难直观判断队形是否维持得好三是学校实验室里Matlab的普及率极高后续接硬件或者和Simulink模型做HIL仿真都很方便。在仿真方案设计上我更倾向于用脚本函数模块化结构而不是把所有代码堆到一个大文件里。这样每个模块可以独立调试后续替换算法比如把A换成RRT只动一个函数不影响整体框架。1.3 坐标系与运动模型四旋翼的完整动力学模型涉及惯性坐标系和机体坐标系之间的转换、姿态角横滚、俯仰、偏航的耦合、螺旋桨气动力矩等非常复杂。但在编队控制这个尺度上我们最关心的是位置和速度姿态控制通常由下层控制器处理。因此在仿真中我采用了简化质点模型内环PD姿态快速收敛假设也就是每个四旋翼被视为一个可控加速度的三维质点控制输出是期望加速度通过坐标变换映射到机体轴的推力和姿态角指令位置环控制周期取 0.02s姿态内环假设带宽高于外环5倍以上。这样的简化在集群编队验证阶段完全够用。非要接入复杂动力学反而容易让控制参数调试变得混乱编队误差的来源会分不清是控制算法的问题还是模型精度的问题。模型方程写出来就是x_dot v_x vx_dot u_x y_dot v_y vy_dot u_y z_dot v_z vz_dot u_z其中[u_x, u_y, u_z]是期望加速度控制量也即位置控制器的输出。编队控制算法要算的就是这个控制量。2. 目标分配模块设计与实现2.1 问题建模多对多的指派问题目标分配本质上是已知N架无人机的位置和M个目标位置的组合优化问题。如果N M就是标准的线性指派问题如果N M则需要考虑哪些无人机不参与任务或者允许多架飞机服务一个目标。当N M但要求一架飞机负责一个目标点时可以引入虚拟目标节点把问题转换为N x N的方阵再求解。代价矩阵的构造是第一步也是决定分配质量的关键。我用的是欧氏距离代价C(i, j) sqrt((x_i - x_j)^2 (y_i - y_j)^2 (z_i - z_j)^2)但单纯用距离有个问题——它没有考虑飞行环境比如某两个目标之间隔着障碍物距离近不代表代价小。这时候可以在代价矩阵中叠加一个障碍惩罚项例如把穿越禁飞区的路径代价放大数倍。项目初期为了逻辑清晰我只用了距离代价后续扩展时再引入A*路径长度作为代价效果更好。2.2 匈牙利算法原理与Matlab实现关于指派问题的求解方法最简单的方案是贪心按距离远近逐个分配距离近的优先选择。这个方案的问题在于贪心可能把一架飞机分配给了一个局部最近的目标反而导致整体总代价变很大。一个典型的反例是两架飞机距离目标A都非常近距离目标B都非常远贪心会把飞机1分配给A飞机2不得不飞很长的路去B而最优解恰是飞机1去B、飞机2去A。匈牙利算法能保证得到全局最优匹配时间复杂度O(N^3)在无人机数量几十架的规模下完全可接受。Matlab内置的matchpairs函数可以直接求解但如果想深入理解算法流程可以用经典的行列归约实现function assignment hungarian_assign(costMatrix) % 经典匈牙利算法实现返回匹配索引 assignment % costMatrix: N x N 代价矩阵 n size(costMatrix, 1); u zeros(1, n); v zeros(1, n); p zeros(1, n); way zeros(1, n); for i 1:n p(1:n) 0; way(1:n) 0; minv inf(1, n); visited false(1, n); j0 0; p(0) i; % 注意Matlab索引从1开始实际代码需要偏移处理 % 此处省略标准增广路径更新逻辑 % 核心逐行找最小邻接边反复增广直到匹配完备 end assignment zeros(1, n); for j 1:n assignment(p(j)) j; end end实际项目中我更推荐直接用内置函数M matchpairs(C, 1e6); % C为NxN代价矩阵第二参数为代价上限matchpairs的第二个参数用于设置“无匹配代价”阈值当某架飞机到所有目标的代价都超过该上限时允许它不参与匹配这个特性在处理N M时很实用。2.3 分配结果的衔接逻辑目标分配完成后得到的是一个映射关系第i架无人机对应的目标编号j。这个输出需要在主仿真循环中传递给路径规划模块作为路径的终点坐标。这里有一个容易忽略的细节当编队是一个整体时到底是给每一架飞机都单独规划路径还是给编队规划一条路径我的方案是后者。因为编队阵形控制的前提是所有飞机共享一个运动参考点如果各自规划路径路径的起点不同会导致编队瞬间撕裂。实际操作中先计算所有无人机的几何中心作为编队参考点目标分配的结果也以编队中心到某个任务区域中心为终点再通过路径规划模块生成参考中心的全局路径。3. 全局路径规划模块3.1 路径规划方法选型全局路径规划的方法很多大体上可以分成三类基于图搜索的A*、Dijkstra、JPS、基于采样的RRT、RRT*、PRM和基于智能优化的遗传、蚁群。在二维网格地图中A算法是最经典且效果稳定的选择。它不仅保证找到最短路径在启发函数可采纳的前提下而且实现逻辑简单、调试容易。RRT在处理高维连续空间时有优势但生成路径不稳定、耗时长而且需要后处理平滑反而增加了工作量。这个项目中地图被栅格化为二维网格禁飞区障碍物在网格中标记为1可行区域标记为0。A*搜索使用八邻域扩展允许斜向45度运动但需要额外检查两个轴向相邻格子是否同时为障碍避免路径“穿墙角”。3.2 A*算法实现关键点核心实现逻辑如下function path a_star_grid(map, start, goal) % map: 二维逻辑矩阵1表示障碍0表示可行 % start, goal: [x, y] 栅格坐标 [rows, cols] size(map); openList containers.Map(KeyType,char,ValueType,any); closedSet false(rows, cols); gScore inf(rows, cols); fScore inf(rows, cols); cameFrom zeros(rows, cols); % 存储父节点索引 % 将起点放入openList初始化g和f gScore(start(1), start(2)) 0; fScore(start(1), start(2)) heuristic(start, goal); openList(start_key(start)) [start, 0]; % 存储节点坐标和f值 while ~isempty(openList) % 找出f值最小的节点 [fMin, key] minOpen(openList); current openList(key); openList.remove(key); closedSet(current(1), current(2)) true; if isequal(current(1:2), goal) path reconstruct_path(cameFrom, goal); return; end % 遍历8个邻居 for dx -1:1 for dy -1:1 if dx 0 dy 0, continue; end neighbor current(1:2) [dx, dy]; if neighbor(1)1 || neighbor(1)rows || ... neighbor(2)1 || neighbor(2)cols continue; end if map(neighbor(1), neighbor(2)) 1, continue; end if closedSet(neighbor(1), neighbor(2)), continue; end % 斜向移动时检查穿墙角情况 if dx ~ 0 dy ~ 0 if map(current(1)dx, current(2)) 1 || ... map(current(1), current(2)dy) 1 continue; % 两个轴向方向任一被堵则禁止斜穿 end end % 计算g值直线代价为1斜线为sqrt(2) moveCost (dx ~ 0 dy ~ 0) * sqrt(2) ... (dx ~ 0 dy 0) * 1 ... (dx 0 dy ~ 0) * 1; tentativeG gScore(current(1), current(2)) moveCost; if tentativeG gScore(neighbor(1), neighbor(2)) cameFrom(neighbor(1), neighbor(2)) sub2ind([rows, cols], current(1), current(2)); gScore(neighbor(1), neighbor(2)) tentativeG; fScore(neighbor(1), neighbor(2)) tentativeG heuristic(neighbor, goal); openList(start_key(neighbor)) [neighbor, fScore(neighbor(1), neighbor(2))]; end end end end path []; % 无路径返回空 end启发函数我选的是欧几里得距离function h heuristic(node, goal) h sqrt((node(1)-goal(1))^2 (node(2)-goal(2))^2); end这里有个细节如果选用曼哈顿距离作为启发函数由于允许斜向移动启发值可能高估实际代价导致找不到最短路径。统一用欧氏距离在八邻域搜索时最稳妥。3.3 路径平滑与轨迹生成A*输出的路径是栅格中心点序列直接作为控制参考输入飞机会出现“点与点之间急转弯”的问题编队队形也会受到冲击。因此必须做路径平滑处理。我用的是三次B样条曲线拟合好处是可以保证曲线通过控制点附近但允许平滑变形并且B样条曲线是连续可导的速度指令不会出现跳变。实现可以借助Matlab的曲线拟合工具箱也可以手写function smoothPath splineSmooth(path, density) % 对路径点序列做B样条插值 % path: Nx2 原始路径点 % density: 插值密度 t 1:size(path,1); tt 1:1/density:size(path,1); % 对x、y坐标分别做三次样条插值 sx spline(t, path(:,1), tt); sy spline(t, path(:,2), tt); smoothPath [sx, sy]; end注意spline函数是三次样条插值它会精确穿过所有原始路径点。如果希望更好地权衡路径长度和安全距离推荐用spap2做B样条近似拟合程度通过节点数调节。平滑之后还有一个关键步骤时间分配。根据编队巡航速度比如2 m/s和规划加速度限制把路径长度折算成时间序列生成期望轨迹xd(t), yd(t), zd(t)及其导数。这部分在代码中体现为轨迹生成函数会输出每一时刻的期望位置和速度给编队跟随控制模块使用。4. 编队跟随控制模块4.1 编队控制方案对比编队控制的主流方案有虚拟结构法、领导者-跟随者法和一致性法。虚拟结构法把整个编队看作一个刚体每个无人机锚定在刚体上的固定位置优点是控制精度高缺点是队形变换不够灵活领导者-跟随者法直观且易于实现但存在误差传播问题并且一旦leader失效编队随之瘫痪一致性法基于图论和分布式控制理论上更有扩展性但参数调起来比较麻烦。我在项目中采用了一致性法作为理论内核 领导者参考轨迹作为外部输入的混合结构。具体来说编队中指定一个虚拟领导者不一定是真实无人机可以是编队参考点所有成员跟踪虚拟领导者的运动状态同时通过一致性协议同步编队内部的位置偏差。这样既保留了领导者-跟随者的参照清晰性又具备了分布式控制的鲁棒性。4.2 一致性协议的理论推导一致性协议的核心思想是如果编队中每个节点的控制输入只依赖自身与其邻居节点的相对状态那么在图连通且持续激励的条件下所有节点的状态会收敛到一致。对于三维空间中的编队假设编号为1的节点是虚拟领导者2到N是跟随者。定义跟随者i的跟踪误差为ei pi - (p1 delta_i)其中pi是无人机i的位置p1是编队参考点的位置通常取所有无人机中心的期望轨迹delta_i是无人机i在编队阵形中的期望相对位置比如菱形编队的顶点偏移。采用二阶一致性协议加速度控制量为u_i Kp * ei Kd * (v_i - v1_desired) sum_{j in N_i} a_ij * [ (pi - pj) - (delta_i - delta_j) ]第一项是位置误差的比例反馈第二项是速度跟踪第三项是编队内部一致性协同项。Kp和Kd是控制器增益a_ij是通信拓扑的邻接矩阵元素。这里还有一件事值得注意通信拓扑的连通性。如果图不连通一致性项就无法起效整个编队会散掉。默认我采用的是一棵以跟随着互连的树形拓扑加上虚拟领导者的一跳连接保证所有节点在通信范围内。4.3 期望编队位置生成与坐标变换编队阵形定义在编队参考坐标系中飞行过程中需要转换到世界坐标系。以水平面编队举例假设阵形是菱形delta_1 [0, 0, 0] % 中心参考点 delta_2 [L, 0, 0] % 右翼 delta_3 [-L, 0, 0] % 左翼 delta_4 [0, L, 0] % 后方 delta_5 [0, -L, 0] % 前方其中L是相邻无人机的期望间距。飞行中编队会整体转向这时候delta_i必须绕编队参考点旋转。转换公式为yaw atan2(v1_desired(2), v1_desired(1)); % 编队前进方向 R [cos(yaw), -sin(yaw), 0; sin(yaw), cos(yaw), 0; 0, 0, 1]; delta_world R * delta_body;如果不做旋转编队在转弯时会出现明显的“漂移”现象——队形整体本应朝向前进方向转弯但内部相对位置仍然是世界坐标系下的固定值导致转弯时某些成员拖尾严重。4.4 控制器参数整定与实现控制器增益Kp和Kd的整定我的经验是先调单个跟随者不考虑编队一致性项。把Kp设为2 ~ 4Kd设为2*sqrt(Kp)左右验证单机响应不振荡、超调小于10%。然后接入编队一致性项逐步增大一致性权重观察编队误差变化。如果出现高频振荡大概率是Kd偏小或者仿真步长过大如果收敛太慢可以适当增大Kp。核心控制函数如下function [acc, error_state] formation_controller(state_all, target_state, deltas, Kp, Kd, G) % state_all: 所有无人机状态结构体包含位置和速度 % target_state: 虚拟领导者期望状态位置、速度 % deltas: Nx3 期望编队偏移机体坐标系 % G: 通信拓扑邻接矩阵 N size(state_all, 1); acc zeros(N, 3); yaw atan2(target_state.v(2), target_state.v(1)); R [cos(yaw), -sin(yaw), 0; sin(yaw), cos(yaw), 0; 0, 0, 1]; for i 1:N delta_world (R * deltas(i,:)); % 期望位置 领导者位置 旋转后的偏移 p_desired target_state.p delta_world; v_desired target_state.v; % 比例微分项 err_pos state_all(i).p - p_desired; err_vel state_all(i).v - v_desired; acc(i,:) -Kp * err_pos - Kd * err_vel; % 一致性协同项 for j 1:N if G(i,j) 0 i ~ j delta_j_world (R * deltas(j,:)); acc(i,:) acc(i,:) - G(i,j) * ( ... (state_all(i).p - state_all(j).p) - ... (delta_world - delta_j_world) ); end end end error_state compute_formation_error(state_all, target_state, deltas, yaw); end至于位置环输出如何映射到四旋翼的推力指令我用了一个简单的映射期望加速度[ux, uy, uz]先折算到期望姿态角再通过推力公式换算为电机指令。这部分在仿真中通常不展开直接给无人机模型注入加速度保持控制律与运动学模型的层级清晰。5. 仿真系统集成与完整流程5.1 仿真场景设定我设置了这样一个场景5架四旋翼初始位置散布在出发区域阵形设定为菱形编队翼展距离L 3m地图是50m x 50m的栅格地图内部随机分布5个圆形障碍物和几个矩形禁飞区任务目标点3个位于地图角落区域第一阶段所有无人机起飞并形成菱形编队第二阶段编队整体沿路径规划模块生成的安全路径前进第三阶段到达任务区域边缘后目标分配模块把3个精确目标点分配给5架无人机中的3架剩余2架进入空中悬停等待第四阶段被分配任务的无人机脱离编队各自飞往目标点。这个场景基本还原了一个“区域巡查-任务分配-定点执行”的完整流程。5.2 代码架构与仿真主循环整个项目的文件结构如下multi_uav_sim/ ├── main_simulation.m % 主入口控制仿真流程 ├── initialize_agents.m % 初始化无人机位置速度参数 ├── target_assignment.m % 目标分配模块匈牙利算法 ├── a_star_planning.m % 全局路径规划 ├── path_smoothing.m % 路径平滑 ├── trajectory_generation.m % 轨迹生成与时间分配 ├── formation_control.m % 编队控制器 ├── single_uav_dynamics.m % 单机质点模型更新 ├── collision_check.m % 机间与障碍物碰撞检测 └── plot_results.m % 三维可视化和数据分析主循环流程为% 初始化 [agents, map, targets] initialize_agents(); % 第一阶段编队形成 for t 0:dt:T_form target_state.p [0, 0, 10]; % 悬停参考点 target_state.v [0, 0, 0]; acc formation_controller(agents, target_state, deltas, Kp, Kd, G); agents single_uav_dynamics(agents, acc, dt); plot_results(agents, map); end % 第二阶段全局路径规划离线阶段不参与实时循环 start_center mean([agents.p], 1); path_raw a_star_planning(map, start_center, goal_center); path_smooth path_smoothing(path_raw); traj trajectory_generation(path_smooth, v_cruise); % 编队跟随规划路径 for k 1:length(traj.time) target_state.p traj.pos(k,:); target_state.v traj.vel(k,:); acc formation_controller(agents, target_state, deltas, Kp, Kd, G); agents single_uav_dynamics(agents, acc, dt); % 碰撞检测若间距小于阈值则报警 collision_check(agents, map); end % 第三阶段任务分配与单机飞向目标 assignment target_assignment(agents, targets); for i 1:N if assignment(i) 0 agents(i).task targets(assignment(i)); end end仿真循环中有一个经常被忽略的问题离散化步长。位置控制器输出加速度通过double integrator积分得到速度和位置步长过大大于0.1s会导致加速度反馈出现明显延迟编队一致性发散。我这个项目统一用dt 0.02s实时性完全够用。5.3 编队误差评估指标光看动画效果不足以说明算法好坏我定义了三个量化指标编队位置误差E_pos max_i |pi - (p_lead delta_i_world)|编队速度同步误差E_vel max_i |vi - v_lead|队形保持误差任意两架无人机之间的实际距离与期望距离的偏差指标的计算放在每次控制更新之后最后打印出最大误差和均方根误差。这样调整参数时候才有据可依。在仿真收敛稳定后编队位置误差通常会收敛到0.05m以下速度误差在0.02m/s以内。5.4 可视化与结果分析可视化我用了Matlab的plot3patch画四旋翼简图。主要呈现三类信息全局地图、障碍物、A*规划路径和B样条平滑后的路径每架无人机的三维飞行轨迹颜色区分某个时刻的编队阵形快照以及期望队形轮廓。结果分析部分我会输出编队最大位置误差: 0.086 m 编队均方根误差: 0.041 m 路径规划耗时: 0.18 s 编队规划总长度: 57.3 m 编队巡航时间: 34.5 s这些数据对后续写论文或者做性能评估非常关键。也方便你对比不同的控制参数、不同的编队拓扑、不同路径规划策略的差异。6. 常见问题与排查技巧实录6.1 编队无法收敛甚至发散出现这个现象的时候第一步不是推翻算法而是排查控制器增益是否匹配。Kp过大而Kd不足系统会像弹簧一样来回振荡Kp过小收敛极其缓慢。我的经验是保持Kd ≈ 2*sqrt(Kp)这个临界阻尼关系再逐步调大。第二个高频原因是通信拓扑不连通。比如你想当然地认为所有飞机之间都能通信但建图时用了稀疏矩阵某个节点实际没有邻居一致性项等于零它只能靠领导者项维持。在代码启动时加一句连通性检查if ~all(sum(G, 2) 0) error(通信拓扑不连通存在孤立节点); end能在调试阶段帮你省下大量时间。6.2 目标分配结果不符合直观预期有一类典型情况两架飞机到同一个目标点的距离都非常近匈牙利算法把其中一架分配给远处目标看起来“不合理”。实际上总代价最小就是这样只是你直觉上觉得某架飞机应该去近处。如果业务上有额外要求比如某些目标点有负载能力限制需要在代价矩阵里加约束条件而不是事后改匹配。另外代价矩阵出现Inf时某些目标对某飞机不可达matchpairs的第二个参数要设置合适的代价上限否则内置函数可能报错或返回空匹配。我在代码中做了保护if any(isinf(C(:))) C(isinf(C)) 1e6; end6.3 路径规划“穿墙”或路径锯齿状严重穿墙问题几乎都出在八邻域扩展时没有检查斜向穿越条件。A*在网格地图中斜向移动如果只检查目标格子的障碍属性就会穿过两个相邻障碍物的角点。修复方式我在前面代码中已经展示移动之前检查两个相关的轴相邻格子。锯齿路径则是平滑参数的问题。B样条插值的控制点太密曲线会像折线一样控制点太少会偏离原始路径太远。我的建议是采样间隔取原始路径折点间距的1/3并通过可视化对比平滑前后路径与障碍物的相对位置确保平滑后路径不切进障碍物。6.4 编队穿越窄通道时阻塞这是编队控制项目中比较容易忽略的问题队形是固定的当路径规划出的通道宽度小于编队展宽时飞机会直接撞上障碍物边界。两种解决思路一是在路径规划阶段就检查通道宽度在A*代价中增加编队半径约束避免规划出无法通过的路径二是引入队形动态调整检测到通道变窄时自动收缩编队间距通过后再恢复原始阵形。我项目中采用的是简易版预计算通道宽度如果过窄则强制切换为纵向一字队形。6.5 仿真运行速度慢的优化无人机数量在10架以内Matlab仿真实时性问题不大。如果扩充到30架以上逐秒调用plot3重绘会非常卡。优化思路每10个仿真步长绘制一次画面用set修改已有图形对象的XData/YData/ZData而不是反复调用plot3新建图形对象障碍物地图预先绘制成背景层不参与逐帧更新编队控制模块用向量化计算替代for循环避免逐节点调用。实测下来30架无人机、50m地图的仿真帧率能从0.5fps提升到10fps以上动画流畅度完全不同。6.6 编队转弯时队形扭转最常见又最容易被忽视的问题。我在4.3节里把编队偏移量从机体坐标系转换到世界坐标系如果在控制律中忘了旋转这一步转弯时队形看起来就像“拧麻花”。排查方法是在转弯段把每架飞机的delta_world单独打印出来看是否随偏航角变化。正常情况是队形跟随航向同步旋转而不是保持初始的世界系偏移。这个项目调通之后最大的收获就是搞清楚了“仿真能稳定跑通”和“算法真正可用”之间的距离。目标分配、路径规划、编队控制每一块单拎出来都不难难的是把它们捏合成一个能协同工作的系统。你在实现过程中大概率也会遇到和我一样的坑——队形控制发散、分配结果反直觉、路径穿墙、转弯拧麻花——但只要按照模块化思路一步步排查这些问题都有明确的解法。后续如果想继续扩展可以加入动态障碍物避碰、通信时延补偿、甚至分布式队形切换协议这些都在当前框架的扩展范围内。
阅读完成 · 觉得有帮助？
