★ FEATURED ARTICLE 最近在倒腾多智能体集群运动的仿真有个需求特别典型让一群智能体围着一个虚拟领航者转互相之间保持期望距离既要聚集起来又不能在运动过程中撞上。这不就是编队控制里最经典的领航者-跟随者问题嘛。我把整套逻辑用MATLAB完整跑了一遍包括控制律设计、避碰势场、轨迹可视化和调参经验都整理在这篇文章里。适合正在做课程设计、本科毕业设计或者刚开始接触集群控制、多智能体系统仿真的同学参考。先说结论这个项目本质上是在做“虚拟领航者 聚集项 避碰项”三部分控制输入的叠加。代码并不复杂但有几个坑很容易踩比如增益不匹配导致振荡、避碰项除零、绘图时维度搞混等等。下面从头到尾拆给你看。1. 为什么选虚拟领航者方案多智能体编队控制里“领航者-跟随者”结构是最容易上手的一种。但在实际设计中我更推荐用虚拟领航者而不是真实领航者。原因很简单真实领航者的轨迹不可控一旦它运动过快或者绕弯太急跟随者很容易被甩开更麻烦的是如果真实领航者那个节点出问题整个编队就崩了。虚拟领航者则完全不存在这个问题。它不需要是系统中的某个实体只要在代码里定义一条参考轨迹就行。所有智能体都朝这条参考轨迹上的某个期望位置运动。领航者的位置、速度都是我们自己规划出来的想让它走圆就走圆想走8字就走8字完全可控。1.1 期望距离与聚集是两个不同层面的目标标题里“保持期望距离”和“聚集”看着好像是两回事但其实它们在你写的控制律里是同一个力的两个侧面。聚集的意思是每个智能体都要向虚拟领航者靠拢直到两者距离达到某个设定值。一旦距离小于设定值这个力就会反向推开防止靠得太近。所以这个“吸引-排斥”的组合本质上就是在维持一个期望距离。那避碰呢避碰管的是智能体与智能体之间的距离约束。两个智能体如果靠得太近就要互相推一把避免重叠。注意智能体之间虽然也要保持距离但这个距离并不要求精确等于某个固定值而是只要大于安全阈值就行。这个区别很关键否则你会把问题想复杂了。1.2 控制律的结构三个力叠加我在仿真里用的控制输入是三个分量的叠加聚集/期望距离项把每个智能体往虚拟领航者周围的期望位置拉避碰项当智能体之间距离过近时产生排斥力速度对齐项让智能体速度向领航者速度看齐同时提供阻尼。这三个力叠加成一个总控制输入驱动每个智能体的加速度。整体思路就是把复杂任务拆成单目标然后线性叠加。这个方法在多智能体编队里非常成熟工程实现也最简单。2. 控制律设计与数学建模这一节把控制律的数学形式写清楚。搞清楚公式再写代码比直接抄代码有效得多。2.1 动力学模型一阶积分器还是二阶模型先要决定智能体用什么模型。如果只是做简单的集群效果一阶积分器就够了p_dot u;也就是说位置直接由控制输入驱动。这个模型简单但有一个问题它没有速度状态所以你没法让智能体带“惯性”也没法做速度对齐。我这次用的是二阶积分器p_dot v; v_dot u;这样每个智能体有位置和速度两个状态控制输入是加速度。更接近真实轮式机器人或无人机质点的运动特性也方便你后续扩展更复杂的动力学模型。离散化更新的时候就是两行代码v v dt * u; p p dt * v;注意更新顺序先更新速度再更新位置。2.2 聚集与期望距离项设第 i 个智能体的位置是 p_i虚拟领航者的位置是 p_0期望距离是 d_exp。那么智能体到领航者的距离为d_i norm(p_i - p_0);期望距离项的控制力方向是从智能体指向领航者方向或反方向大小由距离差决定u_lead -k_att * (d_i - d_exp) * (p_i - p_0) / d_i;这里(p_i - p_0)/d_i是单位方向向量。整个公式的意思是距离远了就往领航者方向拉距离近了就往外推。k_att 控制这个力的强度。有一个细节必须注意当 p_i 和 p_0 完全重合时d_i 是 0单位向量没法算。代码里要加一个判断或者加上一个很小的 eps 防止除零。2.3 避碰项智能体之间避碰的常用做法是排斥势场。设两个智能体位置分别为 p_i 和 p_j距离为 d_ij安全距离为 d_safe作用半径为 r_infl。当 d_ij 小于 r_infl 时触发排斥力u_rep k_rep * (r_infl - d_ij) / (d_ij^2) * (p_i - p_j) / d_ij;这个公式里多了个1/d_ij^2的衰减项。物理直觉是距离越近排斥力越强而且是急剧增强这样才能真正避免碰撞。如果只是用(r_infl - d_ij)的线性形式距离很近的时候力反而不够大很容易撞上。另一个关键点避碰项只在距离小于 r_infl 时触发。r_infl 一般要比期望距离 d_exp 小一个量级但不能太小否则智能体已经撞上才触发就晚了。我经常用的组合是d_exp 1.2、r_infl 2.2、d_safe 0.5。2.4 速度对齐项速度对齐项看起来不起眼但没有它系统很容易发散u_align -k_vel * (v_i - v_lead);它做了两件事让智能体速度逐步向领航者速度靠拢充当阻尼项抑制整个系统的振荡。可以把它想成“虚拟弹簧阻尼器”。如果 k_vel 太小智能体在被吸引项拉向当前位置时会反复“飞过头”产生持续振荡如果 k_vel 太大智能体又太“黏”反应迟钝跟不上领航者的轨迹。3. MATLAB仿真代码逐段拆解下面给出完整的代码结构和关键段说明。你可以直接复制整合到自己的项目里。3.1 参数初始化这一步把所有仿真参数集中定义调参时只改这一块就行。clear; clc; close all; %% 参数设置 N 6; % 智能体数量 dt 0.01; % 仿真步长单位秒 T_total 30; % 仿真总时长单位秒 steps round(T_total / dt); % 虚拟领航者轨迹参数圆形 R_leader 3; % 轨迹圆半径 omega 0.4; % 角速度rad/s d_exp 1.2; % 智能体与领航者的期望距离 d_safe 0.5; % 安全距离 r_infl 2.2; % 避碰作用半径 k_att 2.5; % 聚集/期望距离增益 k_rep 8.0; % 避碰排斥增益 k_vel 1.5; % 速度对齐/阻尼增益 %% 初始状态 p0_leader [R_leader; 0]; % 初始位置围绕领航者起点随机散布 p p0_leader 3 * randn(2, N); v zeros(2, N); %% 保存轨迹 p_traj zeros(2, N, steps); v_traj zeros(2, N, steps); min_dist_hist zeros(1, steps);3.2 虚拟领航者轨迹生成虚拟领航者走一个圆形轨迹% 循环中取当前时刻的位置和速度 t (k - 1) * dt; p_lead [R_leader * cos(omega * t); R_leader * sin(omega * t)]; v_lead [-R_leader * omega * sin(omega * t); R_leader * omega * cos(omega * t)];如果你想换轨迹比如改成8字形只需要把 p_lead 的计算换掉% 八字轨迹示例 p_lead [R_leader * sin(omega * t); ... R_leader * sin(2 * omega * t)]; v_lead [R_leader * omega * cos(omega * t); ... 2 * R_leader * omega * cos(2 * omega * t)];代码结构不用动。3.3 主循环与控制律实现核心循环是下面这段。注意我用了“先算全部控制输入再统一更新状态”的方式这在离散仿真里是个好习惯。如果一边算一边更新位置后面的智能体看到的邻居位置已经是迭代后的会引入不对称偏差。for k 1 : steps - 1 t (k - 1) * dt; % 当前领航者位置/速度 p_lead [R_leader * cos(omega * t); R_leader * sin(omega * t)]; v_lead [-R_leader * omega * sin(omega * t); R_leader * omega * cos(omega * t)]; % 存放当前步所有控制输入 u_all zeros(2, N); for i 1 : N % 1. 聚集与期望距离 diff_to_lead p(:, i) - p_lead; d_li norm(diff_to_lead); u_lead zeros(2, 1); if d_li 1e-6 u_lead -k_att * (d_li - d_exp) * diff_to_lead / d_li; end % 2. 智能体间避碰 u_rep zeros(2, 1); for j 1 : N if j i continue; end diff_ij p(:, i) - p(:, j); d_ij norm(diff_ij); if d_ij r_infl d_ij 1e-6 u_rep u_rep k_rep * (r_infl - d_ij) / (d_ij^2) * diff_ij / d_ij; end end % 3. 速度对齐 u_align -k_vel * (v(:, i) - v_lead); % 总控制输入加速度 u_all(:, i) u_lead u_rep u_align; end % 统一更新状态先算完控制量再更新速度和位置 v v dt * u_all; p p dt * v; % 保存轨迹 p_traj(:, :, k 1) p; v_traj(:, :, k 1) v; % 记录当前最小间距后面用来画图 temp_min inf; for i 1 : N for j i 1 : N temp_min min(temp_min, norm(p(:, i) - p(:, j))); end end min_dist_hist(k 1) temp_min; end这段代码里每个智能体的控制量都分成了三块结构非常清晰。你可以在u_lead、u_rep、u_align后面加断点观察任意时刻某个智能体收到的三个力分别是多少调参时特别有用。3.4 绘图与可视化可视化分成两部分一是画最终轨迹和最小间距二是做动态动画。静态结果图%% 静态结果 theta_plot linspace(0, 2*pi, 300); circle_x R_leader * cos(theta_plot); circle_y R_leader * sin(theta_plot); figure(Position, [80, 80, 1200, 450]); subplot(1, 2, 1); plot(circle_x, circle_y, r--, LineWidth, 1.2); hold on; for i 1 : N xi squeeze(p_traj(1, i, :)); yi squeeze(p_traj(2, i, :)); plot(xi, yi, LineWidth, 1); plot(xi(end), yi(end), o, MarkerSize, 6); end hold off; axis equal; grid on; title(智能体轨迹与虚拟领航者轨迹); xlabel(x); ylabel(y); subplot(1, 2, 2); plot((0:steps-1) * dt, min_dist_hist, LineWidth, 1.5); grid on; title(智能体最小间距随时间变化); xlabel(时间/s); ylabel(最小间距);动态动画%% 动态动画 figure(Position, [80, 80, 600, 600]); hold on; axis equal; grid on; xlim([-R_leader - d_exp - 1, R_leader d_exp 1]); ylim([-R_leader - d_exp - 1, R_leader d_exp 1]); for k 1 : 40 : steps cla; t (k - 1) * dt; p_lead [R_leader * cos(omega * t); R_leader * sin(omega * t)]; % 领航者轨迹参考圆 plot(circle_x, circle_y, k--); % 领航者 plot(p_lead(1), p_lead(2), rp, MarkerSize, 18, LineWidth, 2); % 各智能体位置 for i 1 : N xi squeeze(p_traj(1, i, k)); yi squeeze(p_traj(2, i, k)); plot(xi, yi, bo, MarkerSize, 10, LineWidth, 1.5); % 画一段最近轨迹拖尾 idx_start max(1, k - 80); x_hist squeeze(p_traj(1, i, idx_start:k)); y_hist squeeze(p_traj(2, i, idx_start:k)); plot(x_hist, y_hist, -, Color, [0.6 0.6 0.6]); end title(sprintf(时间 %.1f s, t)); drawnow; pause(0.02); endsqueeze 在这里非常关键。p_traj 是三维数组p_traj(1, i, :)会拿到一个三维的单维度切片直接画图经常报尺寸错误或画出奇怪的结果。用 squeeze 压掉单一维度变成普通向量再喂给 plot 就正常了。4. 实验效果调参与结果判读代码能跑通不等于结果正确。下面是我跑出来的经典现象和判断标准。4.1 如何判断“聚集”和“避碰”是否达标两个硬指标智能体到领航者的距离应该收敛到 d_exp 附近误差不超过 0.1 左右任意两个智能体的最小间距始终大于 d_safe即使初始位置很近也不能低于这个值。第一个指标可以通过统计平均距离误差来看。把每时刻所有智能体到领航者距离的平均值和 d_exp 的误差算出来dist_err_hist zeros(1, steps); for k 1 : steps t (k - 1) * dt; p_lead [R_leader * cos(omega * t); R_leader * sin(omega * t)]; err_sum 0; for i 1 : N err_sum err_sum abs(norm(p_traj(:, i, k)) - norm(p_lead) - 0); end % 注意这里直接用两点距离更准确不要像我写的那样去比范数 end上面这个写法有问题实际上应该直接算两点距离dist_err_hist zeros(1, steps); for k 1 : steps t (k - 1) * dt; p_lead [R_leader * cos(omega * t); R_leader * sin(omega * t)]; err_sum 0; for i 1 : N d_i norm(squeeze(p_traj(:, i, k)) - p_lead); err_sum err_sum abs(d_i - d_exp); end dist_err_hist(k) err_sum / N; end稳定后如果这个平均误差在 0.1 以内说明聚集和期望距离保持项工作正常。第二个指标就是前面代码里存下来的min_dist_hist。稳定后它应该一直大于等于 d_safe并且在避碰项触发时被“弹开”而不是继续逼近。4.2 调参经验与速查表我踩过几个比较典型的坑总结成一张表现象原因调整方向智能体发散越来越远吸引项符号反了或 k_att 过小检查 u_lead 公式把负号补上适当增大 k_att持续振荡来回“抖动”k_vel 太小阻尼不足增大 k_vel一般 1~3 比较合适能聚集但过冲严重k_att 过大k_vel 没跟上降低 k_att同时提高 k_vel两个智能体重叠k_rep 太小或 r_infl 太小增大 k_rep适当增大 r_infl编队太松散避碰项太强排斥力把智能体推得过开降低 k_rep或缩小 r_infl跟随领航者有滞后k_att 不够增大 k_att但注意配合增大 k_vel从我这边的经验看一次比较省心的调参流程是先固定d_exp 1.2、r_infl 2.2、d_safe 0.5把k_vel设置在 1.5 左右然后从小到大加k_att看着轨迹稳定了再从小到大加k_rep。每改一次参数就重新看两个指标图很快就能找到顺手的组合。5. 常见问题排查与扩展方向5.1 高频问题速查表问题原因解决办法报错“Subscript indices must either be real positive integers”k 从 0 开始或循环计数写错检查循环范围MATLAB 下标从 1 开始plot 报维度错误p_traj 三维切片没 squeeze用 squeeze 压成向量智能体全部“跑飞”控制增益匹配失败或符号问题检查 u_lead 负号确认所有增益为正相聚时直接穿过领航者奇点处没处理 d_li 接近 0加 if d_li 1e-6 判断用了 8 字轨迹后不稳定领航者法向加速度太大降低 omega 或增大 k_att避碰项不触发r_infl 太小还没触发就撞上了增大 r_infl避碰在 dt 较大时失效离散化步长太大斥力来不及作用减小 dt比如从 0.05 调到 0.01动画卡顿每步都 drawnow 太频繁隔几帧刷新一次比如步长跳到 405.2 往下还能扩展的方向这套代码的骨架很稳扩展空间也大。我可以给你几个方向固定相位编队现在每个智能体只要保持到领航者期望距离即可。如果你想指定它在哪个方位就把它变成位置偏差反馈u_att -k_att * (p_i - p_des)其中p_des p_lead d_exp * [cos(theta_i); sin(theta_i)]theta_i 是第 i 个智能体的期望相位角。加入静态障碍物把障碍物也视作“虚拟智能体”给障碍物位置加一个更大的排斥势场就能实现避障。通信拓扑限制只允许相邻编号的智能体之间交换位置信息从全连接改成局部通信更接近实际无线网络场景。异构模型部分智能体用一阶模型部分用二阶模型模拟地面机器人和无人机的混合编队。输出 GIF在动画循环里用exportgraphics或者getframe imwrite把每帧存下来最后拼成 GIF放到汇报 PPT 里很直观。我个人在复现这个项目的时候最大的体会是这类多智能体代码真的不难难的是你搞清楚每个力到底在干什么。你随便抄一段代码可能跑出几个点绕圈圈但如果你能把控制律拆成“聚集 避碰 对齐”三个逻辑来想那不管是圆的轨迹、8 字轨迹还是换成三维空间都只是换几个矩阵维度的事。最后再分享一个小技巧调参的时候别盯着动画看先看两个指标曲线——距离误差曲线和最小间距曲线绝大多数问题一眼就能定位。
阅读完成 · 觉得有帮助？
