简介一套面向多智能体系统纯方位编队控制算法验证的Matlab仿真程序主要面向计算机、电子信息工程、数学等专业本科生及研究生既可用于课程设计、期末大作业与毕业设计也适合对编队控制算法感兴趣的入门研究者快速验证思路。程序兼容Matlab 2014、2019a、2024a多个版本采用参数化编程方式代码注释清楚。资源包共12个文件核心为6个m脚本另含2张运行结果图、1个txt说明和1个doc说明文档整体仅30KB轻量易用可运行后直接观察不同算法的队形收敛效果。目前已有40人学习下载。通过修改初始坐标、角度约束等参数可以对比多种纯方位编队控制策略在收敛速度与避碰行为上的差异随附案例数据免去环境搭建成本特别适合需要边跑边学、深入理解算法原理的入门与进阶人群。1. 纯方位编队控制的一百个标题背后缺的不是公式而是这一层认知多智能体系统纯方位编队控制这几年在机器人集群、无人机协同、无人艇编队里反复出现核心诉求只有一个不依赖 GPS、不依赖相对测距靠每个智能体对邻居的“视线方向”这一个标量观测把一群个体排成预先设定的几何队形并保持下去。标题里那套 Matlab 仿真程序通常不是某一篇论文的复现而是把几种主流算法——梯度下降、一致性、编队参考向量场、输出反馈观测器——放在同一个坐标系下做横向对比的骨架代码。适合谁读你不需要先啃完控制理论四大本但最好已经会写基本的状态方程和 Simulink 仿真因为这篇文章不是给你讲 Lie 群而是直接把“纯方位”这个前提压到最紧告诉你哪些地方会不可观测、哪些参数一调就崩。看懂了这一层再看任何一套 bearing-only 仿真包都能在十分钟内定位到它到底在解哪个子问题。2. 纯方位编队控制的可观测性前提与两类坐标建模2.1 为什么“只有角度”也能确定相对位置一个智能体只能测得邻居的方位角测不到距离按理说相对位置是欠定的。但编队控制的目标不是追踪某个绝对位置而是把邻居放到“期望方向上”去。二维平面里智能体 i 对智能体 j 的方位角定义为[ \phi_{ij} \operatorname{atan2}(y_j - y_i, x_j - x_i) ]当控制器让 (\phi_{ij}) 收敛到期望值 (\phi^*_{ij}) 时队形并没有被唯一锁定而是一族相似变换下的解——平移、整体旋转、等比缩放都不会改变方位角关系。所以在仿真程序里如果只用角度误差做控制最后往往收敛到“形状对但尺度/整体朝向漂移”的结果这是纯方位控制和距离控制最大的差别也是新手第一个踩坑点。要锁死尺度必须引入至少一个锚点智能体anchor或者额外加一个关于编队尺度/朝向的参考约束否则系统自由度永远多一个。2.2 全局坐标系与局部坐标系下建模的差异Matlab 仿真程序里常见的第一处分岔是测量方程写在哪组坐标系下。全局坐标系直接用 (\operatorname{atan2}) 计算邻居方位忽略本体姿态适合仿真入门如果模型中引入了机载惯性测量单元给出的自转角 (\theta_i)那么测量到的方位角就是它在载体坐标系的投影[ \phi^{\text{body}}{ij} \operatorname{atan2}\left( \sin(\phi{ij} - \theta_i), \cos(\phi_{ij} - \theta_i) \right) ]两种建模不影响控制器的结构但影响噪声注入和控制器重定义参考方位的方式。对于纯方位编队控制算法来说控制量本身往往要和这个载体坐标系相乘写错旋转方向是最常见的实现错误。常见做法是先在全局坐标系里把控制律写出来再乘旋转矩阵转到载体坐标系避免在期望方位角上反复做加减法。2.3 作用拓扑与三角形约束由于单个方位角的信息量有限控制拓扑必须满足一个基本条件每个智能体至少要能观测到两个不共线的邻居。与此同时整个编队的图要满足“周期间无向连通”的性质或者说至少在生成树上保证方位信息能传播到每个节点。下面这张表总结了两种建模的适用场景差异。建模方式度量输出控制器实现难度典型用途全局坐标系(\phi_{ij}\operatorname{atan2}(\Delta y,\Delta x))低直接调梯度算法对比、教学仿真载体坐标系需补偿 (\theta_i) 旋转中带旋转矩阵接入惯性测量单元、模拟真实机器人相机/传感器坐标系有视场角限制高需处理非视场失效视觉导引的编队控制当 agent 与两个邻居近似共线时距离方向不可观测控制量会在这条线附近来回摆动严谨的说法是该状态不可观仿真里表现为队形缓慢漂移或振荡发散。好的 Matlab 仿真程序里都会检查这个条件并在初始化时尽量把智能体摆成三角形、四边形靠在首个 agent 附近保留一个 anchor 来固定编队的绝对位置。3. 在 Matlab 里搭建纯方位仿真环境的最小骨架3.1 状态方程、测量方程与主循环先给出一份结构化的基础脚本把多智能体系统的状态、测量和控制主循环组织起来。这也是后续移植各种算法的统一入口。% bearing_only_setup.m % 多智能体纯方位编队控制 · 最小仿真骨架 % 状态: p [x1 y1 x2 y2 ... xN yN] % 输入: u 每个智能体的期望速度 (2维) N 4; % 智能体数量 dt 0.05; % 步长 5ms ~ 50ms 常见 T 20.0; % 仿真时长 p0 [0 0; 4 0; 0 3; 4 3]; % 初始位置4个点构成矩形 p p0(:); % 拉成列向量 % 期望相对结构: 这里用邻接列表每条边记录期望方位角 edges [1 2; 1 3; 2 4; 3 4; 1 4]; bearing_ref [0 pi/2 0 pi/2 pi/4]; % 弧度示例值 % 主循环 for k 1:round(T/dt) % 1) 计算当前所有边的方位角 phi compute_bearings(p, edges); % 2) 根据算法计算控制输入 u bearing_controller_gradient(p, edges, phi, bearing_ref); % 3) 欧拉积分简单位移模型 p p u * dt; % 4) 记录和绘制略 end上面这段代码里状态变量只保存位置这是最朴素的运动学模型实际工程中会换成更接近真实底盘或无人机动力学的模型再在控制器输出后面接一个饱和或低通环节。参数上dt 的选取要和后面的噪声强度配套后面第 5 章会专门讲这个问题。控制律函数bearing_controller_gradient是一个可替换接口这就是不同算法在同一仿真框架下对比的关键设计。3.2 噪声、测量异常与不可观测场景的处理所有真实传感器都有噪声纯方位仿真程序里最常用的噪声模型是角度加性高斯噪声% 在测量端模拟传感器噪声 phi_measured phi_true normrnd(0, sigma_phi, size(phi_true));但噪声不是唯一的干扰源。视线遮挡会让测量完全失效此时不能把这个丢失的值当成 0 或者上一个时刻的旧值应该是把这条边临时从计算图中移除或者挂起对应的控制器通道。另一个常见问题是角度回绕(\pi) 和 (-\pi) 表示同一个方向如果直接做差误差会从接近 0 跳到 (2\pi)。仿真程序的通用处理是把角度误差约束到 ((-\pi, \pi]) 区间具体的误差项用 (\sin(\phi - \phi^*)) 而不是直接取差这样既能自然处理回绕也能在误差较大时抑制突变。3.3 数据记录与可视化验证做算法对比时只有一条收敛曲线是远远不够的。常见做法是同时记录三个量角度误差的范数、编队几何偏差、控制输入的能量把这三组曲线放在同一个 figure 里对照着看能快速定位问题出在控制器还是观测器还是初始状态上。可视化时建议不要只用粒子点动画也可以把每条边的期望方位角用小箭头标出来让角度误差从几十度的收敛过程直接可见。这个记录结构在你的仿真包里随手就能加却是后续调参最省时间的投资。4. 四种常用算法的 Matlab 实现与参数对比4.1 基于方位误差的梯度下降控制最直接的算法是把编队控制问题转成一个势场优化让每个智能体沿着“使角度误差减小”的方向移动。定义边 (e(i,j)) 的角度误差为 (\sin(\phi_{ij}-\phi^*_{ij}))那么对智能体 i 的位置求梯度得到一个垂直于当前视线方向的修正力。function u bearing_controller_gradient(p, edges, phi, ref) % p: 所有位置列向量; edges: 邻接边表; phi/ref: 当前与期望方位角 N numel(p)/2; u zeros(2*N, 1); Kp 1.2; % 增益超过2.0容易振荡 R [0 -1; 1 0]; % 旋转90度矩阵 for idx 1:size(edges,1) i edges(idx,1); j edges(idx,2); pi_ p(2*i-1:2*i); pj_ p(2*j-1:2*j); d pj_ - pi_; dist norm(d); e sin(phi(idx) - ref(idx)); % 梯度方向: 垂直视线方向指向角度误差减小的那个方向 grad R * d / dist; u(2*i-1:2*i) u(2*i-1:2*i) - Kp * e * grad; u(2*j-1:2*j) u(2*j-1:2*j) Kp * e * grad; end end这里值得注意的一个参数是增益 (K_p)它直接决定了角速度的响应大小。(K_p) 太小会收敛极慢太大则会在奇异配置附近出现高频抖动。由于梯度方向用 (R\cdot d/|d|) 算出来的是一个单位向量(u) 的量纲实际上是角速度误差乘增益调参时可以按这个物理意义来估计初值。另一个容易忽视的点是梯度法只修正角度误差完全没有考虑队形的整体位移如果编队整体正在匀速漂移这个算法只能保证相对方向正确整体位置会一直漂。4.2 基于一致性的分布式方位控制纯方位一致性方法的思想是构造一组“虚拟速度”并让它们保持一致同时约束方位角。最简化的形式是把期望的队形速度 (v_0) 广播给部分节点每个智能体的目标速度向与邻居速度的差值以及方位误差的修正方向加权组合。function u bearing_controller_consensus(p, edges, phi, ref, v0) N numel(p)/2; u_cons zeros(2*N, 1); L zeros(N); % 邻接矩阵 for idx 1:size(edges,1) i edges(idx,1); j edges(idx,2); L(i,j) -1; L(i,i) L(i,i)1; L(j,i) -1; L(j,i) L(j,i)1; end v reshape(reshape(p, 2, N), [], 1); % 简化为速度状态 for i 1:N % 速度一致性项 u_cons(2*i-1:2*i) 0.5 * sum( ... (v(2*i-1:2*i) - v(2*find(L(i,:))-1:2*find(L(i,:)))) ); end u u_cons 0.3 * bearing_controller_gradient(p, edges, phi, ref); end一致性项负责让成员速度趋同梯度项负责维持期望夹角。实际使用中最常见的问题是这两项的权重比例权重太大会丢失编队几何形状太小则编队像刚体一样转不动。经验上先让一致性项系数取梯度项系数的 1/3 到 1/2再根据仿真曲线调整。需要强调的是这种分布式实现只依赖局部通信不需要中央计算单元和实际机器人平台的距离更近。4.3 基于编队参考的向量场控制另一种思路是把编队看成一个虚拟刚体每个智能体相对于编队参考中心的期望位置是固定的。控制器把参考中心的速度映射成每个智能体需要的速度再叠加一个针对位置误差的反馈。这里的位置误差无法直接测量只用方位角反馈构造一个李雅普诺夫函数把速度场设计成对期望方向的指数收敛形式。function u bearing_controller_vectorfield(p, edges, phi, ref, v_order) % 编队参考速度 v_order 由 leader 或虚拟领导者给出 u zeros(size(p)); % 1) 对所有智能体加一致性速度让整体跟随 v_order u u repmat(v_order, size(p,1)/2, 1); % 2) 对每个有参考边的智能体加期望方向的吸引项 lam 0.8; % 收敛速率越大跟随越快 for idx 1:size(edges,1) i edges(idx,1); e sin(phi(idx) - ref(idx)); grad_dir [cos(phi(idx)); sin(phi(idx))]; u(2*i-1:2*i) u(2*i-1:2*i) lam * e * [0 -1; 1 0] * grad_dir; end end这类向量场控制方法的优点是天然支持编队整体轨迹跟踪因为它把参考速度作为一个公共输入注入到每一个智能体不会出现梯度法那种“角对了但队形整体漂移”的问题。但缺点是要额外设定虚拟领导者或者参考轨迹这在纯方位模型下需要领导者存在或者已知参考输入。很多对标论文会在这个基础上加入动态反馈线性化让队形在曲线轨迹上也能保持形状这已经超出了最小仿真包的范畴。4.4 带观测器的输出反馈控制前面三种方法都隐含假设控制器使用的 (\phi) 是真实的角度且没有延迟。在仿真包更接近真实系统时测量值要经过一个非线性观测器才能得到较好的相对位置估计值再把估计位置输给梯度或一致性控制器。一个简化版就是分别对每条边维护一个相对位置的估计用方位角误差做修正项。function p_hat update_bearing_observer(p_hat, ph_measured, edges, dt) % 简单非线性观测器测量残差修正相对位置估计 K_obs 0.6; for idx 1:size(edges,1) i edges(idx,1); j edges(idx,2); pi_hat p_hat(2*i-1:2*i); pj_hat p_hat(2*j-1:2*j); phi_hat atan2(pj_hat(2)-pi_hat(2), pj_hat(1)-pi_hat(1)); resid sin(ph_measured(idx) - phi_hat); % 把残差沿垂直方向投影回去修正相对位置 dir_perp [0 -1; 1 0] * ... (pj_hat - pi_hat) / norm(pj_hat - pi_hat); p_hat(2*i-1:2*i) p_hat(2*i-1:2*i) - K_obs * resid * dir_perp * dt; end end观测器增益 (K_{obs}) 的调节逻辑和控制器增益刚好相反观测器要慢一点避免把测量噪声直接放大成位置估计的抖动但又不能太慢否则系统整体响应滞后控制器会以为是真实误差而继续加大输入最终形成极限环。一个经验法则是让观测器的时间常数约等于控制器时间常数的 1/5 到 1/3。输出反馈控制的整体结构是先用观测器得到 (\hat{p})再把它替代 4.1 或 4.2 中的真实位置送入控制律这一点在所有论文里都是标准做法。4.5 参数对照表与收敛性判定算法主要参数初选范围收敛标志特点梯度下降(K_p)0.5 ~ 2.0角度误差降至噪声底结构最简单队形整体漂移一致性法一致性权重 / 梯度权重0.3 ~ 0.6各节点速度差趋近 0分布式、易拓展向量场法(\lambda)0.5 ~ 1.0角度误差与速度同时收敛可追踪轨迹观测器反馈(K_{obs})0.2 ~ 0.8估计位置误差收敛到零附近抗噪声最好这四个算法的仿真结果放到一起比较时最直观的判定指标不是最终误差而是收敛曲线的形状。常见的失败模式是角度误差剧烈振荡此时检查是不是增益过大或者积分步长过大另一种是误差单调下降但队形缩成一个点此时就是缺少尺度约束的问题。仿真程序里建议在每次运行结束把最终队形单独画出来和期望队形叠在一起看比只看误差数值更有诊断价值。5. 仿真程序的三个高频坑共线、噪声与积分步长5.1 共线配置导致的可观测性崩溃仿真跑着跑着突然某个智能体开始绕圈或者在一条直线上往复震荡大概率不是控制器写错而是某个时刻两个邻居相对于本体的视线方向重合了。这时方位角无法提供关于“该条直线上距离”的任何信息观测器和控制器的梯度方向都失去了对该自由度的修正能力。调试时要把每个采样时刻的最小奇异值或者视线夹角记录下来当某条边的两个观测视线夹角小于 (5^\circ) 时视为进入近奇异区。在初始化阶段要刻意检查每个智能体所在的角度扇区避免让队形开局就处于退化的构型上。5.2 测量噪声与控制增益的上限约束测量噪声以加性角噪声的形式进入系统控制器梯度方向是视线方向的法向噪声造成的修正项随增益线性放大。增益一旦超过某个阈值噪声会被控制回路放大形成高频抖振甚至掩盖真实的收敛趋势。给一个便于仿真的规律如果角度噪声标准差为 (\sigma_\phi)弧度增益上限经验上不超过 (2\pi/\sigma_\phi) 的 1/20。比如 (\sigma_\phi0.02) 弧度时增益上限约在 1.5 附近这和 4.1 里的 (K_p1.2) 基本吻合。实际测试中先在无噪声情况下调好控制器再加噪声降增益是所有仿真包都应该标注的调参顺序。5.3 固定步长与自适应步长的取舍纯方位控制器加观测器之后系统在某些构型下会表现出刚性特征尤其增益偏大时固定步长欧拉积分容易出现数值发散。先用ode45或固定步长 RK4 跑通主流程再把积分步长缩小一半观察收敛曲线是否改变如果曲线形态变化明显说明仿真结果对步长敏感此时应当切换到ode15s或者在主循环里加入步长自适应逻辑。一个最简单的判断标准是把步长从 0.05s 改成 0.01s如果收敛时间和最终误差变化超过 20%那步长还没到合理区间。这个隐性条件在很多人下载的仿真包里不会被自动检查但一旦你开始对比多种算法步长不一致会让对比结论完全失真。6. 把仿真程序改造成后续研究基座的三件事6.1 从二维切换到三维纯方位模型二维的方位角只有一个实数三维则要扩展为方位角与俯仰角两个量或者直接使用视线方向的单位向量。Matlab 的改写量集中在测量方程和控制律中的梯度计算上把旋转矩阵从 2 乘 2 替换为 3×3 旋转群上的李代数映射把 (\operatorname{atan2}) 改成求两个姿态角。更重要的是给三维初始配置检查共线性二维里的一个退化点是一条直线三维里退化条件是一条直线和一个平面检查逻辑要改写成对视线方向向量两两点积的判断点积接近 ±1 时视为近奇异。6.2 给控制器加饱和与避碰约束仿真跑通后的第一件事往往是加饱和现实机器人速度不可能无限大。在控制指令后加句简单的u max(min(u, u_max), -u_max)是最基础的但要防止饱和后积分项继续累计造成超调。避碰约束的常见做法是加入一项短程势场力当两个智能体距离小于安全半径时产生排斥力排斥力作用范围控制在安全半径的 1.2 倍以内即可避免影响远距离下编队控制的形状。6.3 用批量跑数据验证算法稳定性在 Matlab 里用parfor对随机生成的初始队形、不同噪声水平和不同期望队形做蒙特卡洛扫描统计收敛成功率与平均收敛时间是判断一套算法是否可用的最快路径。把初始位置偏差上限、角度噪声标准差、控制器增益作为三个循环维度各取 5 个水平一次批量跑 125 组参数每组仿真 20 秒一晚上就能出一张算法的鲁棒性地图。这张图比任何一条单次仿真曲线都有说服力也是这个仿真包真正能在后续论文里复用为基线工具的地方。本文还有配套的精品资源点击获取