做机器人编队、分布式控制或者群体系统仿真的人对“多智能体”和“一致性算法”这两个词肯定不陌生。这是多智能体系统里最基础、也最常用的一套协议每个机器人只跟有限几个邻居交换位置信息每个个体都按同一个局部规则更新自己的运动一段时间之后所有机器人会停在同一组坐标上实现位置一致。标题里那句“交流多个机器人的位置信息最终实现所有机器人位置的一致”说的就是这件事。这个主题看着学术实际上非常好上手。你只要有 Matlab 基础、会矩阵运算照着这篇文章的思路走半小时就能跑出一个带轨迹动画的多智能体一致性 Demo。无论你是正在写课程报告的研究生还是刚接触多智能体方向、想搭第一个仿真环境的开发者这套最小可运行示例都能当你的起跳板。我写这篇文章时会把数学原理、参数怎么定、Matlab 代码怎么拆、以及我调代码时踩过的坑一次说清楚。1. 一致性算法是什么位置同步靠什么实现1.1 平均一致性靠的不是“喊口号”而是局部邻居关系先看最常用的一阶一致性协议。假设第 i 个机器人的位置是 (x_i)它有一个连续时间模型[ \dot{x}_i(t) u_i(t) ]控制输入 (u_i) 只依赖自己和邻居之间的相对位置[ u_i(t) -\sum_{j \in \mathcal{N}i} a{ij} \big(x_i(t) - x_j(t)\big) ]这里 (a_{ij}) 表示两个机器人之间有没有通信连接有就为 1没有就为 0。这个公式翻译成人话就是每个机器人把自己跟每个邻居的位置差加起来再取个相反数当成当前的速度指令。如果某个邻居在我左边我就向右靠如果邻居在我右边我就向左靠。所有机器人同时做这件事整个群体的位置就会慢慢聚拢。为什么能收敛一个很直观的原因是“总量守恒”。把系统里所有 (\dot{x}_i) 加起来由于无向图中每条边两端的差值 (\big(x_i - x_j\big)) 和 (\big(x_j - x_i\big)) 会成对抵消所以位置平均值是不变的。也就是说尽管没有一个中心节点指挥大家“去哪个坐标集合”系统收敛后的位置一定是初始位置的平均值。这一点在调试时非常关键你想让机器人最终落在哪儿其实在初始分布里就已经决定了。1.2 换一个视角一致性本质上是在做一个分布式梯度下降我最初看这个协议时总觉得它像某种拍脑袋规则后来发现它其实可以解释成优化问题。定义整个系统的不一致程度[ V(x) \frac{1}{2}\sum_{i1}^{N}\sum_{j1}^{N} a_{ij} |x_i - x_j|^2 ]这个 (V) 把所有通信边上的“位置误差平方”加在一起。如果所有机器人位置相同(V) 就是 0。上面那个控制协议其实就是让每个机器人沿着 (V) 的负梯度方向运动。每个智能体只需要自己的位置和邻居的位置就能算清这条“局部梯度”于是分布式优化和一致性就打通了。这个视角的好处是以后你想给协议加东西思路会很清楚。比如想让机器人在一条线上排成等间距的队形只要把目标函数从“位置相同”改成“位置差等于期望间距”控制律里多减一个期望值就行。标题里说的是位置一致但把这一层想透编队控制、包含领导者的一致性、时变拓扑这些问题都能顺着同一套框架做下去。2. 仿真参数怎么定拓扑、步长、初值一个都不能马虎2.1 通信拓扑决定了收敛速度和实际意义跑仿真之前先想清楚让机器人怎么通信。常见的选择有全连通图、环形图、链式图、随机图等。我实际写代码时默认用环形拓扑因为它在“通信受限”和“能收敛”之间比较平衡。5 个机器人围成一圈每个机器人只和左右两个邻居通信既能体现分布式特点又不会像全连通那样看起来太“中央集权”。不同拓扑的收敛速度差别很大核心指标是拉普拉斯矩阵的第二小特征值 (\lambda_2)也叫代数连通度。代数连通度越大信息在群体里扩散越快收敛就越快。全连通图的 (\lambda_2) 很大所以瞬间就聚合了链式图两端信息要经过中间节点一个个传收敛慢得多。表里面几类常见拓扑的对比可以帮你做选择拓扑类型通信边数代数连通度特征收敛速度适用场景全连通很多高最快理论验证不考虑通信成本环形中等中等较快模拟通信受限但有一定冗余链式少低慢模拟接力式通信、串行拓扑随机不固定不确定取决于连通性模拟真实无线网络丢边你不需要把代数连通度手工算出来Matlab 一行eig(L)就能看到。但如果拓扑不连通(\lambda_2) 会恰好等于 0这也是后面排查“为什么不收敛”最重要的指标。2.2 步长怎么选欧拉离散不是越小越好但更不能太大一致性协议本身是连续时间系统写 Matlab 仿真时必须离散化。最直白的做法是[ x_i(k1) x_i(k) \Delta t \cdot u_i(k) ]这就是一阶欧拉法。这里有一个数值稳定性条件需要重视(\Delta t) 必须小于等于拉普拉斯矩阵最大特征值的倒数乘以 2。我给的环形拓扑(L) 的最大特征值是 4所以理论上 (\Delta t) 最大不能超过 0.5。实际仿真我不会顶到边界一般取 0.01 到 0.05 之间。太小会让仿真变慢太大则可能数值发散模拟出“机器人越靠越远”的假象。你可能要问为什么不直接取 0.01如果你只跑 10 秒0.01 要 1000 步0.02 只要 500 步差别不大。但如果你后面要做几十上百个机器人的编队步长太小的仿真耗时会被放大很多倍。所以我的习惯是先看max(abs(eig(L)))再取一个比理论上限小一两个数量级的步长既稳又不浪费算力。环形 5 节点用 0.02 是很舒服的配置。2.3 初始位置怎么撒对收敛结果影响很大这类一致性算法最终收敛到初始平均值所以初始位置的分布会直接决定终点坐标。仿真时我喜欢用固定随机种子生成初始位置这样每次跑出来的轨迹一致方便对比参数变化。如果你想让机器人最后停在某个指定区域有两条路一是把初始位置的平均值落在那儿二是在协议里加一个参考速度项让跟踪目标参与收敛。另一个容易忽略的细节是初始位置不要撒得太开。如果机器人散布在 100m 乘 100m 的区域只靠相对位置差驱动的控制力又不大收敛时间会明显变长。一般课程 Demo 用 10m 乘 8m 左右的范围就够了视觉上轨迹也更好看。3. Matlab 代码逐段拆解从拉普拉斯矩阵到收敛曲线3.1 初始化、邻接矩阵与拉普拉斯矩阵代码的第一步是构造系统参数和通信拓扑。这里我用的是无向环状拓扑5 个机器人排成一个环每个节点和前后两个邻居相连。邻接矩阵A的第 i 行第 j 列等于 1表示 i 能收到 j 的信息。% 参数设置 N 5; % 机器人数量 dt 0.02; % 离散步长 T 10; % 仿真总时长 steps ceil(T / dt) 1; % 环形拓扑的邻接矩阵 A zeros(N); for i 1:N j mod(i, N) 1; A(i, j) 1; A(j, i) 1; end % 度矩阵与拉普拉斯矩阵 D diag(sum(A, 2)); % 每行求和得到度数 L D - A; % 初始位置固定随机种子保证结果可复现 rng(7); pos0 [rand(1, N) * 10; rand(1, N) * 8]; % 2 x N每列是一个机器人的坐标这段代码里最值得留意的是拉普拉斯矩阵的构造。在无向图里度矩阵D是对角阵每个对角元等于该节点邻居数量邻接矩阵A是对称矩阵减出来之后每行每列的和都是 0。这保证了后面算出来的更新方向总是指向群体中心一侧同时不会改变全局平均位置。3.2 主循环一条向量化语句更新整个群体很多初学者喜欢用双层循环逐个机器人更新比如先遍历i再遍历j。这样写当然没错但完全没有必要。因为状态矩阵每一列是一个机器人矩阵乘法天然把“每个机器人和所有邻居的位置差求和”这件事算好了。核心更新语句只有一行% 预分配轨迹存储 pos_hist zeros(2, N, steps); pos_hist(:, :, 1) pos0; % 主循环 pos pos0; for k 1:steps - 1 pos pos - dt * (pos * L); % 一阶欧拉离散 pos_hist(:, :, k 1) pos; end解释一下pos * L在做什么。pos是 2 乘 N 矩阵每一列是一个机器人坐标乘以L之后每一列得到的是该机器人与邻居的位置差加权和。由于无向拉普拉斯矩阵是对称的右边乘L和左边乘L在效果上等价。这一行等价于所有机器人同步执行了pos(:, i) pos(:, i) - dt * sum(A(i, :) .* (pos(:, i) - pos));但向量化之后不仅代码短执行效率也高得多。如果你后面把 N 从 5 改成 50 甚至 200这种写法依然能跑得动。3.3 画轨迹和曲线亲眼看到多个机器人聚成一个点仿真跑完最有成就感的一步就是把轨迹画出来。我习惯画两张图一张是平面上各机器人从星形分散到聚合的轨迹另一张是“最大位置偏差随时间下降”的收敛误差曲线。第一张图注重直觉第二张图才能定量判断系统是否真的收敛。% 轨迹图 figure; colors lines(N); for i 1:N px squeeze(pos_hist(1, i, :)); py squeeze(pos_hist(2, i, :)); plot(px, py, Color, colors(i, :), LineWidth, 1.2); hold on; plot(pos_hist(1, i, 1), pos_hist(2, i, 1), o, ... Color, colors(i, :), MarkerFaceColor, colors(i, :)); plot(pos_hist(1, i, end), pos_hist(2, i, end), x, ... Color, colors(i, :), MarkerSize, 10); end axis equal; grid on; xlabel(x 坐标); ylabel(y 坐标); title(多智能体位置一致性轨迹);每个机器人的起点我用空心圆标记终点用叉号标记。跑完这个仿真你会看到 5 个机器人从不同位置画出弧线最后聚集在同一个点附近。由于误差是按指数衰减的轨迹末端会有一点“慢悠悠”地靠拢而不是猛地撞在一起。收敛误差曲线的代码也不复杂。每个时刻计算所有机器人到群体中心的距离最大值只要这个值越来越小就说明一致性协议在起作用。% 收敛误差曲线 consensusErr zeros(steps, 1); for k 1:steps P pos_hist(:, :, k); Pcenter P - mean(P, 2); consensusErr(k) max(vecnorm(Pcenter)); end figure; plot(0:dt:T, consensusErr, LineWidth, 1.6); grid on; xlabel(时间 t / s); ylabel(max || p_i - p_{avg} ||); title(一致性误差收敛曲线);如果你的 Matlab 版本比较老vecnorm不可用可以把max(vecnorm(Pcenter))换成max(sqrt(sum(Pcenter.^2, 1)))效果完全一样。3.4 完整可运行脚本一键复现整个流程把上面的代码块拼到一个.m文件里就能直接运行。为了阅读方便我这里给一个精简但完整的版本%% 多智能体位置一致性仿真 clear; close all; clc; N 5; dt 0.02; T 10; steps ceil(T / dt) 1; A zeros(N); for i 1:N j mod(i, N) 1; A(i, j) 1; A(j, i) 1; end L diag(sum(A, 2)) - A; rng(7); pos [rand(1, N) * 10; rand(1, N) * 8]; pos_hist zeros(2, N, steps); pos_hist(:, :, 1) pos; for k 1:steps - 1 pos pos - dt * (pos * L); pos_hist(:, :, k 1) pos; end %% 绘图 figure; colors lines(N); for i 1:N px squeeze(pos_hist(1, i, :)); py squeeze(pos_hist(2, i, :)); plot(px, py, Color, colors(i, :), LineWidth, 1.2); hold on; plot(pos_hist(1, i, 1), pos_hist(2, i, 1), o, ... Color, colors(i, :), MarkerFaceColor, colors(i, :)); plot(pos_hist(1, i, end), pos_hist(2, i, end), x, ... Color, colors(i, :), MarkerSize, 10); end axis equal; grid on; xlabel(x 坐标); ylabel(y 坐标); title(多智能体位置一致性轨迹); figure; consensusErr zeros(steps, 1); for k 1:steps P pos_hist(:, :, k); Pcenter P - mean(P, 2); consensusErr(k) max(vecnorm(Pcenter)); end plot(0:dt:T, consensusErr, LineWidth, 1.6); grid on; xlabel(时间 t / s); ylabel(max || p_i - p_{avg} ||); title(一致性误差收敛曲线);我把这段代码在几个常见的 Matlab 版本上都跑过没有依赖额外工具箱纯基础函数就能运行。你复制粘贴之后只需要关注最终机器人的终点是不是落在初始位置的平均值附近。4. 常见坑与排查心法不收敛、发散、扩展方向4.1 不收敛先检查拓扑是不是连通的我自己调试时遇到最多的问题是仿真跑完发现机器人们分成两三个小团体各聚各的就是不整体同步。这种情况十有八九是通信拓扑不连通。比如你生成了随机邻接矩阵结果有几个机器人只和自己人通信整个图断了信息就永远无法从一个连通分量传到另一个分量。排查方法很直接。第一步看eig(L)如果第二小特征值等于 0说明图不连通。第二步把邻接矩阵画出来用gplot(A, pos0)可视化肉眼看谁和谁连不上。还有一种隐蔽情况是单向通信A 矩阵不对称i 能收到 j 的信息但 j 收不到 i 的信息。有向图下一致性仍然可能收敛但条件不一样最简单省事的做法是保证邻接矩阵对称也就是通信是双向的。提示在一致性算法里拉普拉斯矩阵的第二小特征值就是系统的“健康指标”。它大于 0基本不用太担心收敛问题它等于 0就得回去改拓扑。4.2 发散先怀疑步长再怀疑初始坐标另一个高频问题是机器人位置发散轨迹在图上直接飞出屏幕。遇到这种情况第一反应不是算法错了而是数值仿真不稳定。之前说过欧拉法的稳定上限是 (2 / \lambda_{\max}(L))。环形 5 节点的最大特征值是 4步长超过 0.5 就有发散风险而如果拓扑是全连通最大特征值变成 5理论上限更小你原来设定的步长可能就不安全了。我踩过一次比较隐蔽的坑把机器人数量从 5 改成 20 后随机生成了一个近似全连通图最大特征值突然涨到 14 左右原来的 0.1 步长直接导致发散。排查了半天才发现问题不在控制律而在离散化参数。所以当你改了拓扑或节点数之后先用max(abs(eig(L)))看一眼特征值谱再决定要不要调小步长。初始坐标太夸张也可能造成数值问题。比如坐标数量级在 1e4 以上而步长又取得比较大位置更新时容易产生明显的截断误差。我的建议是先把坐标控制在百以内等算法跑通后再去贴近实际尺度。4.3 一致性只是第一步怎么往编队和领导者扩展跑通位置一致后很多人会问下一步能玩什么。这里我给你几个顺手就能改的扩展方向。第一个方向是编队控制。想让最终队形不再是同一个点而是保持固定间距只需要把控制输入改成[ u_i -\sum_{j} a_{ij} \left[ (x_i - x_j) - d_{ij} \right] ]其中 (d_{ij}) 是期望的相对位置差。比如想让机器人排成一条直线就让相邻节点的期望间隔等于固定值。这个改动在代码里几乎是一行的事。第二个方向是加领导者。选定一个机器人当作领导它不去跟随别人只按自己的轨迹运动其余机器人还是执行一致性协议但只跟邻居中的领导或普通节点比较位置。这样所有机器人最终会跟踪到领导者的轨迹上。代码上的改动是在更新循环里加一个判断领导节点的速度直接指定不需要用pos * L那一行。第三个方向是通信时延或切换拓扑。真实无线通信不可能保证每一时刻拓扑都不变你可以每隔一段时间随机断开几条边看看系统还能不能收敛。这一般需要用到时滞系统的分析工具但作为仿真实验写起来并不复杂只需要把A矩阵在循环内动态更新。我自己复现这个项目的体会是一致性算法最大的价值不是“让机器人聚在一起”这个结果而是提供了一套从局部信息到全局行为的搭建范式。你把这个最简代码吃透后面无论是做分布式估计、时钟同步还是无人机编队底层思路都会非常清晰。先跑通最小例子再逐步加约束这是我最推荐的学习路径。
做机器人编队、分布式控制或者群体系统仿真的人对“多智能体”和“一致性算法”这两个词肯定不陌生。这是多智能体系统里最基础、也最常用的一套协议每个机器人只跟有限几个邻居交换位置信息每个个体都按同一个局部规则更新自己的运动一段时间之后所有机器人会停在同一组坐标上实现位置一致。标题里那句“交流多个机器人的位置信息最终实现所有机器人位置的一致”说的就是这件事。这个主题看着学术实际上非常好上手。你只要有 Matlab 基础、会矩阵运算照着这篇文章的思路走半小时就能跑出一个带轨迹动画的多智能体一致性 Demo。无论你是正在写课程报告的研究生还是刚接触多智能体方向、想搭第一个仿真环境的开发者这套最小可运行示例都能当你的起跳板。我写这篇文章时会把数学原理、参数怎么定、Matlab 代码怎么拆、以及我调代码时踩过的坑一次说清楚。1. 一致性算法是什么位置同步靠什么实现1.1 平均一致性靠的不是“喊口号”而是局部邻居关系先看最常用的一阶一致性协议。假设第 i 个机器人的位置是 (x_i)它有一个连续时间模型[ \dot{x}_i(t) u_i(t) ]控制输入 (u_i) 只依赖自己和邻居之间的相对位置[ u_i(t) -\sum_{j \in \mathcal{N}i} a{ij} \big(x_i(t) - x_j(t)\big) ]这里 (a_{ij}) 表示两个机器人之间有没有通信连接有就为 1没有就为 0。这个公式翻译成人话就是每个机器人把自己跟每个邻居的位置差加起来再取个相反数当成当前的速度指令。如果某个邻居在我左边我就向右靠如果邻居在我右边我就向左靠。所有机器人同时做这件事整个群体的位置就会慢慢聚拢。为什么能收敛一个很直观的原因是“总量守恒”。把系统里所有 (\dot{x}_i) 加起来由于无向图中每条边两端的差值 (\big(x_i - x_j\big)) 和 (\big(x_j - x_i\big)) 会成对抵消所以位置平均值是不变的。也就是说尽管没有一个中心节点指挥大家“去哪个坐标集合”系统收敛后的位置一定是初始位置的平均值。这一点在调试时非常关键你想让机器人最终落在哪儿其实在初始分布里就已经决定了。1.2 换一个视角一致性本质上是在做一个分布式梯度下降我最初看这个协议时总觉得它像某种拍脑袋规则后来发现它其实可以解释成优化问题。定义整个系统的不一致程度[ V(x) \frac{1}{2}\sum_{i1}^{N}\sum_{j1}^{N} a_{ij} |x_i - x_j|^2 ]这个 (V) 把所有通信边上的“位置误差平方”加在一起。如果所有机器人位置相同(V) 就是 0。上面那个控制协议其实就是让每个机器人沿着 (V) 的负梯度方向运动。每个智能体只需要自己的位置和邻居的位置就能算清这条“局部梯度”于是分布式优化和一致性就打通了。这个视角的好处是以后你想给协议加东西思路会很清楚。比如想让机器人在一条线上排成等间距的队形只要把目标函数从“位置相同”改成“位置差等于期望间距”控制律里多减一个期望值就行。标题里说的是位置一致但把这一层想透编队控制、包含领导者的一致性、时变拓扑这些问题都能顺着同一套框架做下去。2. 仿真参数怎么定拓扑、步长、初值一个都不能马虎2.1 通信拓扑决定了收敛速度和实际意义跑仿真之前先想清楚让机器人怎么通信。常见的选择有全连通图、环形图、链式图、随机图等。我实际写代码时默认用环形拓扑因为它在“通信受限”和“能收敛”之间比较平衡。5 个机器人围成一圈每个机器人只和左右两个邻居通信既能体现分布式特点又不会像全连通那样看起来太“中央集权”。不同拓扑的收敛速度差别很大核心指标是拉普拉斯矩阵的第二小特征值 (\lambda_2)也叫代数连通度。代数连通度越大信息在群体里扩散越快收敛就越快。全连通图的 (\lambda_2) 很大所以瞬间就聚合了链式图两端信息要经过中间节点一个个传收敛慢得多。表里面几类常见拓扑的对比可以帮你做选择拓扑类型通信边数代数连通度特征收敛速度适用场景全连通很多高最快理论验证不考虑通信成本环形中等中等较快模拟通信受限但有一定冗余链式少低慢模拟接力式通信、串行拓扑随机不固定不确定取决于连通性模拟真实无线网络丢边你不需要把代数连通度手工算出来Matlab 一行eig(L)就能看到。但如果拓扑不连通(\lambda_2) 会恰好等于 0这也是后面排查“为什么不收敛”最重要的指标。2.2 步长怎么选欧拉离散不是越小越好但更不能太大一致性协议本身是连续时间系统写 Matlab 仿真时必须离散化。最直白的做法是[ x_i(k1) x_i(k) \Delta t \cdot u_i(k) ]这就是一阶欧拉法。这里有一个数值稳定性条件需要重视(\Delta t) 必须小于等于拉普拉斯矩阵最大特征值的倒数乘以 2。我给的环形拓扑(L) 的最大特征值是 4所以理论上 (\Delta t) 最大不能超过 0.5。实际仿真我不会顶到边界一般取 0.01 到 0.05 之间。太小会让仿真变慢太大则可能数值发散模拟出“机器人越靠越远”的假象。你可能要问为什么不直接取 0.01如果你只跑 10 秒0.01 要 1000 步0.02 只要 500 步差别不大。但如果你后面要做几十上百个机器人的编队步长太小的仿真耗时会被放大很多倍。所以我的习惯是先看max(abs(eig(L)))再取一个比理论上限小一两个数量级的步长既稳又不浪费算力。环形 5 节点用 0.02 是很舒服的配置。2.3 初始位置怎么撒对收敛结果影响很大这类一致性算法最终收敛到初始平均值所以初始位置的分布会直接决定终点坐标。仿真时我喜欢用固定随机种子生成初始位置这样每次跑出来的轨迹一致方便对比参数变化。如果你想让机器人最后停在某个指定区域有两条路一是把初始位置的平均值落在那儿二是在协议里加一个参考速度项让跟踪目标参与收敛。另一个容易忽略的细节是初始位置不要撒得太开。如果机器人散布在 100m 乘 100m 的区域只靠相对位置差驱动的控制力又不大收敛时间会明显变长。一般课程 Demo 用 10m 乘 8m 左右的范围就够了视觉上轨迹也更好看。3. Matlab 代码逐段拆解从拉普拉斯矩阵到收敛曲线3.1 初始化、邻接矩阵与拉普拉斯矩阵代码的第一步是构造系统参数和通信拓扑。这里我用的是无向环状拓扑5 个机器人排成一个环每个节点和前后两个邻居相连。邻接矩阵A的第 i 行第 j 列等于 1表示 i 能收到 j 的信息。% 参数设置 N 5; % 机器人数量 dt 0.02; % 离散步长 T 10; % 仿真总时长 steps ceil(T / dt) 1; % 环形拓扑的邻接矩阵 A zeros(N); for i 1:N j mod(i, N) 1; A(i, j) 1; A(j, i) 1; end % 度矩阵与拉普拉斯矩阵 D diag(sum(A, 2)); % 每行求和得到度数 L D - A; % 初始位置固定随机种子保证结果可复现 rng(7); pos0 [rand(1, N) * 10; rand(1, N) * 8]; % 2 x N每列是一个机器人的坐标这段代码里最值得留意的是拉普拉斯矩阵的构造。在无向图里度矩阵D是对角阵每个对角元等于该节点邻居数量邻接矩阵A是对称矩阵减出来之后每行每列的和都是 0。这保证了后面算出来的更新方向总是指向群体中心一侧同时不会改变全局平均位置。3.2 主循环一条向量化语句更新整个群体很多初学者喜欢用双层循环逐个机器人更新比如先遍历i再遍历j。这样写当然没错但完全没有必要。因为状态矩阵每一列是一个机器人矩阵乘法天然把“每个机器人和所有邻居的位置差求和”这件事算好了。核心更新语句只有一行% 预分配轨迹存储 pos_hist zeros(2, N, steps); pos_hist(:, :, 1) pos0; % 主循环 pos pos0; for k 1:steps - 1 pos pos - dt * (pos * L); % 一阶欧拉离散 pos_hist(:, :, k 1) pos; end解释一下pos * L在做什么。pos是 2 乘 N 矩阵每一列是一个机器人坐标乘以L之后每一列得到的是该机器人与邻居的位置差加权和。由于无向拉普拉斯矩阵是对称的右边乘L和左边乘L在效果上等价。这一行等价于所有机器人同步执行了pos(:, i) pos(:, i) - dt * sum(A(i, :) .* (pos(:, i) - pos));但向量化之后不仅代码短执行效率也高得多。如果你后面把 N 从 5 改成 50 甚至 200这种写法依然能跑得动。3.3 画轨迹和曲线亲眼看到多个机器人聚成一个点仿真跑完最有成就感的一步就是把轨迹画出来。我习惯画两张图一张是平面上各机器人从星形分散到聚合的轨迹另一张是“最大位置偏差随时间下降”的收敛误差曲线。第一张图注重直觉第二张图才能定量判断系统是否真的收敛。% 轨迹图 figure; colors lines(N); for i 1:N px squeeze(pos_hist(1, i, :)); py squeeze(pos_hist(2, i, :)); plot(px, py, Color, colors(i, :), LineWidth, 1.2); hold on; plot(pos_hist(1, i, 1), pos_hist(2, i, 1), o, ... Color, colors(i, :), MarkerFaceColor, colors(i, :)); plot(pos_hist(1, i, end), pos_hist(2, i, end), x, ... Color, colors(i, :), MarkerSize, 10); end axis equal; grid on; xlabel(x 坐标); ylabel(y 坐标); title(多智能体位置一致性轨迹);每个机器人的起点我用空心圆标记终点用叉号标记。跑完这个仿真你会看到 5 个机器人从不同位置画出弧线最后聚集在同一个点附近。由于误差是按指数衰减的轨迹末端会有一点“慢悠悠”地靠拢而不是猛地撞在一起。收敛误差曲线的代码也不复杂。每个时刻计算所有机器人到群体中心的距离最大值只要这个值越来越小就说明一致性协议在起作用。% 收敛误差曲线 consensusErr zeros(steps, 1); for k 1:steps P pos_hist(:, :, k); Pcenter P - mean(P, 2); consensusErr(k) max(vecnorm(Pcenter)); end figure; plot(0:dt:T, consensusErr, LineWidth, 1.6); grid on; xlabel(时间 t / s); ylabel(max || p_i - p_{avg} ||); title(一致性误差收敛曲线);如果你的 Matlab 版本比较老vecnorm不可用可以把max(vecnorm(Pcenter))换成max(sqrt(sum(Pcenter.^2, 1)))效果完全一样。3.4 完整可运行脚本一键复现整个流程把上面的代码块拼到一个.m文件里就能直接运行。为了阅读方便我这里给一个精简但完整的版本%% 多智能体位置一致性仿真 clear; close all; clc; N 5; dt 0.02; T 10; steps ceil(T / dt) 1; A zeros(N); for i 1:N j mod(i, N) 1; A(i, j) 1; A(j, i) 1; end L diag(sum(A, 2)) - A; rng(7); pos [rand(1, N) * 10; rand(1, N) * 8]; pos_hist zeros(2, N, steps); pos_hist(:, :, 1) pos; for k 1:steps - 1 pos pos - dt * (pos * L); pos_hist(:, :, k 1) pos; end %% 绘图 figure; colors lines(N); for i 1:N px squeeze(pos_hist(1, i, :)); py squeeze(pos_hist(2, i, :)); plot(px, py, Color, colors(i, :), LineWidth, 1.2); hold on; plot(pos_hist(1, i, 1), pos_hist(2, i, 1), o, ... Color, colors(i, :), MarkerFaceColor, colors(i, :)); plot(pos_hist(1, i, end), pos_hist(2, i, end), x, ... Color, colors(i, :), MarkerSize, 10); end axis equal; grid on; xlabel(x 坐标); ylabel(y 坐标); title(多智能体位置一致性轨迹); figure; consensusErr zeros(steps, 1); for k 1:steps P pos_hist(:, :, k); Pcenter P - mean(P, 2); consensusErr(k) max(vecnorm(Pcenter)); end plot(0:dt:T, consensusErr, LineWidth, 1.6); grid on; xlabel(时间 t / s); ylabel(max || p_i - p_{avg} ||); title(一致性误差收敛曲线);我把这段代码在几个常见的 Matlab 版本上都跑过没有依赖额外工具箱纯基础函数就能运行。你复制粘贴之后只需要关注最终机器人的终点是不是落在初始位置的平均值附近。4. 常见坑与排查心法不收敛、发散、扩展方向4.1 不收敛先检查拓扑是不是连通的我自己调试时遇到最多的问题是仿真跑完发现机器人们分成两三个小团体各聚各的就是不整体同步。这种情况十有八九是通信拓扑不连通。比如你生成了随机邻接矩阵结果有几个机器人只和自己人通信整个图断了信息就永远无法从一个连通分量传到另一个分量。排查方法很直接。第一步看eig(L)如果第二小特征值等于 0说明图不连通。第二步把邻接矩阵画出来用gplot(A, pos0)可视化肉眼看谁和谁连不上。还有一种隐蔽情况是单向通信A 矩阵不对称i 能收到 j 的信息但 j 收不到 i 的信息。有向图下一致性仍然可能收敛但条件不一样最简单省事的做法是保证邻接矩阵对称也就是通信是双向的。提示在一致性算法里拉普拉斯矩阵的第二小特征值就是系统的“健康指标”。它大于 0基本不用太担心收敛问题它等于 0就得回去改拓扑。4.2 发散先怀疑步长再怀疑初始坐标另一个高频问题是机器人位置发散轨迹在图上直接飞出屏幕。遇到这种情况第一反应不是算法错了而是数值仿真不稳定。之前说过欧拉法的稳定上限是 (2 / \lambda_{\max}(L))。环形 5 节点的最大特征值是 4步长超过 0.5 就有发散风险而如果拓扑是全连通最大特征值变成 5理论上限更小你原来设定的步长可能就不安全了。我踩过一次比较隐蔽的坑把机器人数量从 5 改成 20 后随机生成了一个近似全连通图最大特征值突然涨到 14 左右原来的 0.1 步长直接导致发散。排查了半天才发现问题不在控制律而在离散化参数。所以当你改了拓扑或节点数之后先用max(abs(eig(L)))看一眼特征值谱再决定要不要调小步长。初始坐标太夸张也可能造成数值问题。比如坐标数量级在 1e4 以上而步长又取得比较大位置更新时容易产生明显的截断误差。我的建议是先把坐标控制在百以内等算法跑通后再去贴近实际尺度。4.3 一致性只是第一步怎么往编队和领导者扩展跑通位置一致后很多人会问下一步能玩什么。这里我给你几个顺手就能改的扩展方向。第一个方向是编队控制。想让最终队形不再是同一个点而是保持固定间距只需要把控制输入改成[ u_i -\sum_{j} a_{ij} \left[ (x_i - x_j) - d_{ij} \right] ]其中 (d_{ij}) 是期望的相对位置差。比如想让机器人排成一条直线就让相邻节点的期望间隔等于固定值。这个改动在代码里几乎是一行的事。第二个方向是加领导者。选定一个机器人当作领导它不去跟随别人只按自己的轨迹运动其余机器人还是执行一致性协议但只跟邻居中的领导或普通节点比较位置。这样所有机器人最终会跟踪到领导者的轨迹上。代码上的改动是在更新循环里加一个判断领导节点的速度直接指定不需要用pos * L那一行。第三个方向是通信时延或切换拓扑。真实无线通信不可能保证每一时刻拓扑都不变你可以每隔一段时间随机断开几条边看看系统还能不能收敛。这一般需要用到时滞系统的分析工具但作为仿真实验写起来并不复杂只需要把A矩阵在循环内动态更新。我自己复现这个项目的体会是一致性算法最大的价值不是“让机器人聚在一起”这个结果而是提供了一套从局部信息到全局行为的搭建范式。你把这个最简代码吃透后面无论是做分布式估计、时钟同步还是无人机编队底层思路都会非常清晰。先跑通最小例子再逐步加约束这是我最推荐的学习路径。
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
