简介这是一款基于MATLAB的领航跟随法多机器人编队控制实现面向机器人控制、多智能体协同方向的研究者与学生。算法通过设定领航者运动轨迹使跟随者保持期望的相对距离与角度进而形成稳定编队可应用于多机器人编队行进、路径跟踪与协同避障等场景。资源压缩包共1个文件为单个m脚本体积仅816B代码精简但逻辑集中便于读者快速理解领航-跟随控制律的构建思路。当前已有1073人学习下载说明该实现具备一定的参考价值。脚本中清晰呈现了期望队形计算、位姿误差求解与控制量输出等关键环节读者可直接运行也可通过修改参数快速测试不同队形和轨迹适合需要快速上手编队算法、完成课程设计或进行算法对比验证的MATLAB开发者使用。1. 领航跟随不是“跟车”是多机器人分散式控制的第一课把“编队”做成MATLAB仿真很多人的第一步是给每台机器人单独写轨迹然后祈祷它们别撞上。领航-跟随Leader-Follower的思路完全不同只有领航者Leader按预设路径走跟随者Follower只盯着领航者的相对距离和相对方位做闭环控制。这个范式在无人机、AGV、水下机器人里都是最常用的编队骨架也是多机器人MATLAB仿真里最容易落地的一类。l_f.rar 这类压缩包在CSDN、GitHub上反复出现核心内容基本就是一套基于距离-角度l-φ控制的MATLAB脚本或Simulink模型。这套东西的价值不在于代码本身多复杂而在于它把编队问题拆成了“相对位姿解算”和“跟踪控制律”两块你搞清楚这两块改改参数就能跑出三角形、直线、楔形编队。适合刚接触多机器人控制、想用MATLAB快速验证编队算法的工程师和学生。2. 领航-跟随的数学模型从几何关系到控制律2.1 为什么选 l-φ 控制而不是纯位置跟踪领航-跟随法的常见实现有两种一种是直接跟踪领航者的全局坐标另一种是维持期望的相对距离 l 和相对角度 φ。前者在高斯噪声下容易飘因为领航者坐标一旦跳变跟随者会猛打方向。后者只关心“我在你左后方 1.5 米、偏角 60 度”天然带一个几何约束调参更直观。用以下公式描述跟随者的期望位姿参照系定义领航者位姿为 (x_L, y_L, θ_L)跟随者期望相对距离为 l_d期望相对角度为 φ_d在领航者坐标系下定义。跟随者期望位置为x_F_des x_L l_d * cos(θ_L φ_d) y_F_des y_L l_d * sin(θ_L φ_d)这里的 φ_d 不是全局角度是相对领航者朝向的角度这是初学者最容易搞混的地方。如果你直接写成 x_F_des x_L l_d * cos(φ_d)那编队会随着领航者转弯而整体漂移看起来就像“编队散了”。2.2 用 MATLAB 写相对位姿解算在 MATLAB 里位姿通常用列向量 [x; y; theta] 表示。写一个函数计算领航者到跟随者期望点的变换function [xF_des, yF_des] leaderToFollower(xL, yL, thetaL, l_d, phi_d) % 输入领航者位姿(xL,yL,thetaL)期望距离l_d期望相对角phi_d(弧度) % 输出跟随者期望位置(xF_des, yF_des) xF_des xL l_d * cos(thetaL phi_d); yF_des yL l_d * sin(thetaL phi_d); end逻辑说明thetaL 是领航者的航向角phi_d 是相对角偏移。当 phi_d 为正时跟随者位于领航者左侧为负时在右侧。这个函数只做坐标变换不涉及控制你可以单独测试它给一组领航者位姿和期望编队参数画出来确认几何关系正确再做闭环。参数说明里l_d 的典型取值范围是 0.5~3 米视机器人尺寸phi_d 常用 ±pi/3 到 ±pi/4 之间这样编队不会太扁也不会太挤。2.3 跟随者的运动学模型与误差定义跟随者用差速驱动机器人两轮建模运动学方程为x_F_k1 x_F_k v_F * cos(theta_F_k) * dt y_F_k1 y_F_k v_F * sin(theta_F_k) * dt theta_F_k1 theta_F_k omega_F * dt控制量是线速度 v 和角速度 omega。误差定义在领航者坐标系里e_x (x_F_des - x_F) * cos(theta_L) (y_F_des - y_F) * sin(theta_L) e_y -(x_F_des - x_F) * sin(theta_L) (y_F_des - y_F) * cos(theta_L)注意 e_x 是沿领航者前进方向的误差e_y 是横向误差。控制律用比例反馈v_F v_L * cos(e_y) k_x * e_x omega_F omega_L k_y * e_y k_theta * (theta_des - theta_F)theta_des 可以由 l_d 和 phi_d 反解出来但在实际工程里很多人直接省略 theta_des 项只用 e_y 产生角速度修正效果也够用。关键是 k_x 和 k_y 的比例关系k_x 太大容易前后震荡k_y 太大容易横向甩尾。领航-跟随的稳定性分析通常用李雅普诺夫方法做但工程上你只需要记住一个结论k_x 和 k_y 都为正且 k_y 略大于 k_x 时误差指数收敛超调量在可接受范围内。这个结论可以在仿真里验证后面第 3 章会给具体参数范围。3. 用 MATLAB 跑通领航-跟随最小脚本与参数整定3.1 写一个两极机器人编队的最小仿真脚本这一步直接给能跑的最小实现。假设领航者按圆形轨迹运动跟随者用上一节的控制律跟踪。% leader_follower_min.m % 最小领航-跟随编队仿真两轮差速模型 clear; clc; close all; %% 参数初始化 dt 0.05; % 仿真步长秒 T 20; % 仿真总时长秒 N T / dt; t 0:dt:T-dt; % 领航者初始位姿和速度 xL0 0; yL0 0; thetaL0 0; vL 0.8; % 领航者线速度m/s omegaL 0.3; % 领航者角速度rad/s % 跟随者初始位姿 xF0 -1.0; yF0 -0.5; thetaF0 0; % 期望编队参数 l_d 1.5; % 期望距离米 phi_d deg2rad(60); % 期望相对角60度 % 控制增益 k_x 1.2; k_y 1.8; %% 状态初始化 xL xL0; yL yL0; thetaL thetaL0; xF xF0; yF yF0; thetaF thetaF0; positionsL zeros(N, 3); positionsF zeros(N, 3); %% 主循环 for i 1:N % 记录状态 positionsL(i, :) [xL, yL, thetaL]; positionsF(i, :) [xF, yF, thetaF]; % 领航者更新圆形轨迹 thetaL thetaL omegaL * dt; xL xL vL * cos(thetaL) * dt; yL yL vL * sin(thetaL) * dt; % 跟随者期望位置 xF_des xL l_d * cos(thetaL phi_d); yF_des yL l_d * sin(thetaL phi_d); % 领航者坐标系下的误差 ex (xF_des - xF) * cos(thetaL) (yF_des - yF) * sin(thetaL); ey -(xF_des - xF) * sin(thetaL) (yF_des - yF) * cos(thetaL); % 控制律 vF vL * cos(ey) k_x * ex; omegaF omegaL k_y * ey; % 跟随者更新 thetaF thetaF omegaF * dt; xF xF vF * cos(thetaF) * dt; yF yF vF * sin(thetaF) * dt; end %% 绘图 figure; plot(positionsL(:,1), positionsL(:,2), b-, LineWidth, 1.5); hold on; plot(positionsF(:,1), positionsF(:,2), r--, LineWidth, 1.5); axis equal; grid on; xlabel(x (m)); ylabel(y (m)); legend(领航者轨迹, 跟随者轨迹); title(领航-跟随编队圆形轨迹); % 画编队连线每2秒画一次 for i 1:100:N plot([positionsL(i,1), positionsF(i,1)], [positionsL(i,2), positionsF(i,2)], k:); end逻辑说明主循环里先更新领航者状态再计算跟随者期望位置然后求误差、算控制量、更新跟随者。关键点是误差计算必须转换到领航者坐标系否则负反馈不成立。vL * cos(ey) 这项目的是让跟随者跟上前进方向的运动防止累计距离误差。参数说明dt 取 0.05 秒对仿真精度足够如果机器人实际控制频率是 20 Hz这正好对应一个控制周期。k_x1.2, k_y1.8 是一组比较保守的值响应时间约 2~3 秒不会明显超调。如果你想编队更“紧”可以加大 k_x 到 2.0但超过 3.0 会出现前后震荡。运行这段代码你会看到跟随者的红色轨迹先绕一小段然后慢慢并到领航者轨迹的内侧或外侧取决于 phi_d 的符号最终形成一个稳定的弧形编队。如果跟随者的轨迹发散或画圈先检查 ey 的符号phi_d 为正时跟随者应该在左侧ey 应该趋近于 0 而不是趋近于一个直流偏置。3.2 领航者轨迹切换直线、圆、Bezier 曲线圆形轨迹的验证有一个问题角速度恒定控制律里的 omegaL 项始终在补偿掩盖了跟随性能的不足。更严格的测试是让领航者走一段直线然后突然转弯看跟随者的误差峰值有多大。% 简单实现前10秒直线后10秒圆 if t(i) 10 omegaL 0; else omegaL 0.4; end逻辑说明把 omegaL 从常量改成随时间变化的分段函数就可以模拟直线转圆弧的过渡。这时候你会发现在转折点附近ey 会出现一个短暂的尖峰然后被控制律压回来。这个尖峰的高度和恢复时间是评价编队控制器性能的核心指标。参数说明恢复时间定义为 ey 从峰值衰减到峰值的 63% 所需的时间工程上要求小于 2 秒否则编队会被拉断距离超过传感器量程。3.3 关键参数表与整定顺序参数含义推荐范围过大后果过小后果k_x纵向误差增益0.5~2.5前后震荡跟随者点头跟不上加速距离拉大k_y横向误差增益1.0~3.0横向甩尾轨迹外冲转向迟钝转弯时内侧偏移大l_d期望距离0.5~3.0 米编队松散传感器易丢失目标容易碰撞避障空间小phi_d期望相对角-70°~70°编队过于扁平转弯扫掠面积大跟随者遮挡领航者视野dt仿真步长0.01~0.1 秒离散误差大仿真不稳计算量大无意义整定顺序我一般这样走先把 k_x 和 k_y 从 1.0 起步跑直线轨迹确认无震荡再加入转弯段观察 ey 的峰值逐步加大 k_y 减小横向误差最后再调 l_d 和 phi_d 改变队形。不要上来就改 phi_d 到 80 度转弯时离心力会让误差计算困难。4. Simulink 仿真与数据可视化编队收敛过程怎么看4.1 用 Simulink 搭领航-跟随模型的结构脚本仿真的好改、好复制但 Simulink 有它不可替代的价值你可以直接把领航者和跟随者封装成子系统然后拖着滑杆调增益实时看编队形态变化。搭模型的结构是一个领航者运动学子系统一个跟随者控制子系统一个跟随者运动学子系统再加一个误差输出。子系统内部核心是 S-Function 或 MATLAB Function 模块。如果你不想写 C 语言用 MATLAB Function 模块就够了function [vF, omegaF] followerControl(xL, yL, thetaL, xF, yF, thetaF, vL, omegaL, l_d, phi_d, k_x, k_y) % 领航者坐标系下的误差 xF_des xL l_d * cos(thetaL phi_d); yF_des yL l_d * sin(thetaL phi_d); ex (xF_des - xF) * cos(thetaL) (yF_des - yF) * sin(thetaL); ey -(xF_des - xF) * sin(thetaL) (yF_des - yF) * cos(thetaL); % 控制律 vF vL * cos(ey) k_x * ex; omegaF omegaL k_y * ey; end在 Simulink 里把这个模块接入一个积分器链注意 theta 是角度积分前要加“角度归一化”处理防止角度超过 [-pi, pi] 后突然跳变。这个跳变是仿真中出现轨迹尖刺的头号原因比控制律本身的问题更常见。用 Simulink 的另一个理由是方便加噪声和延迟给传感器测量值加一个高斯白噪声模块或给控制信号加一个 Transport Delay就能模拟实际通信和感知的不确定性。这是脚本仿真很难做自然的事情。4.2 误差可视化与收敛性判断只看轨迹图会让收尾很慢。你要把误差绘制在同一个图里% 假设误差已经存到 errors 矩阵里 figure; subplot(2,1,1); plot(t, errors(:,1), b-, LineWidth, 1.2); % ex ylabel(ex (m)); title(纵向误差 vs 横向误差); grid on; subplot(2,1,2); plot(t, errors(:,2), r-, LineWidth, 1.2); % ey ylabel(ey (m)); xlabel(时间 (s)); grid on;逻辑说明ex 和 ey 都在 0 附近波动是理想状态。实际中 ex 会因为领航者加速出现一个台阶ey 在转弯时出现一个类似正弦的波动。如果你的 ey 曲线在转弯段结束时仍有残留误差不归零原因是 k_y 不够大或者领航者角速度 omegaL 变化太快控制器跟手不及时。参数说明判断收敛性的经验标准是稳态误差小于 0.05 米超调量小于 0.2 米调节时间小于 4 秒。达不到就按第 3.3 节表格调增益。4.3 常见仿真失败模式与排错方向故障现象可能原因排查方法跟随者画圆圈ey 符号反了控制律变成正反馈打印 ey 数值看是否符合几何关系轨迹发散但误差不大dt 太大离散化误差累积把 dt 缩小一半看是否恢复编队形态正确但整体漂移phi_d 用的是全局角而不是相对角检查期望位置公式里的 thetaL phi_d转弯时跟随者切内角k_y 太小omega 补偿不足增大 k_y 到 2.5 并观察起始阶段巨大超调初始位置离期望点太远比例控制饱和把初始位置放到期望点附近或加限幅提示排错时比看轨迹更快的方法是把几个中间量打印出来。尤其是 thetaF 的变化如果它持续朝一个方向转说明角度更新或归一化没做对。5. 领航-跟随的 3 个进阶技巧参考轨迹切换、避障、跟随性能验证5.1 领航者轨迹切换时的平滑过渡用分段函数切换轨迹会让跟随者产生明显冲击因为航线角速度跳变了。常见做法是给 omegaL 加一个斜坡过渡而不是直接跳到目标值。用 MATLAB 写就是omegaL_cmd 0; if t 10 omegaL_cmd 0.4; end % 一阶低通平滑 omegaL omegaL (omegaL_cmd - omegaL) * min(1, dt / 0.3);这样领航者的转弯是渐进的跟随者不会看到突变的速度指令。这个技巧在实物机器人上更关键因为电机响应不了阶跃角速度。5.2 跟随者局部避障的一个简单手段领航-跟随本身不解决避障问题。工程上最简单可靠的方式是在控制律输出 vF 和 omegaF 之后加一个优先级更高的避障单元用势场法或DWA算出避障速度然后做加权融合。给一个极简的斥力势场实现思路% 假设障碍物在 obs_x, obs_y dist_obs sqrt((xF - obs_x)^2 (yF - obs_y)^2); if dist_obs 0.8 rep_x (xF - obs_x) / dist_obs * 0.5 * (0.8 / dist_obs)^2; rep_y (yF - obs_y) / dist_obs * 0.5 * (0.8 / dist_obs)^2; vF vF 0.3 * (rep_x * cos(thetaF) rep_y * sin(thetaF)); end避障优先级高于编队只有当障碍物距离大于安全阈值时才恢复纯编队控制。5.3 跟随性能的量化验证距离误差 RMS 与最大偏差最后给一个可量化验证的方法。跑完成形实验后计算整个过程中跟随者实际位置与期望位置的偏差dist_err sqrt((positionsF(:,1) - xF_des_hist) * (positionsF(:,1) - xF_des_hist) ... (positionsF(:,2) - yF_des_hist) * (positionsF(:,2) - yF_des_hist)); rms_err sqrt(mean(dist_err.^2)); max_err max(dist_err); fprintf(RMS误差: %.3f m, 最大误差: %.3f m\n, rms_err, max_err);用这两个指标对比不同控制增益、不同编队参数的仿真结果你就能找到“参数太快导致抖、太慢导致跟不上”的平衡点。RMS误差小于 0.1 米、最大误差小于 0.3 米是一个比较实用的验收线超过这个数值就说明控制器动态响应不够优先增大 k_y再考虑加大 k_x。这套验证流程能从仿真直接迁移到实物测试是领航-跟随落地前最有性价比的一步。本文还有配套的精品资源点击获取
