多移动机器人领航追随编队控制Matlab仿真全流程解析

做多移动机器人编队仿真这件事，卡住大多数人的往往不是控制算法本身，而是第一步：面对一堆公式，不知道从哪开始写代码；写完代码，又不知道参数怎么调。领航追随法算是最容易上手的编队方案了，核心思想很朴素——选一个机器人当“头”，其他机器人跟着“屁股”走。但真要把它跑通，里面涉及的坐标变换、队形误差定义、控制律参数整定，全是细节。

这篇文章就围绕“多移动机器人协同编队控制、领航追随法Matlab仿真”这条主线，从思路拆解到代码实现再到参数调优，把整个流程完整过一遍。无论你是刚接触多机器人系统的大四学生，还是正在为课题组搭仿真环境的研究生，这篇都值得收藏参考。文章最后还整理了参考文献列表的用法建议，帮你在论文里把这块交代明白。

1. 领航追随法的思路拆解：为什么它是最容易落地的编队方案

1.1 从“跟车”到“跟机器人”：领航追随法的本质

领航追随法（Leader-Follower）的思想一点都不玄乎。想象一下你在高速上开着定速巡航跟着前车走：前车减速你减速，前车变道你变道，你不需要知道目的地是哪，只需要盯着前车保持安全距离。多移动机器人里的领航追随法就是这个逻辑：编队里选一个领航者（Leader），其余都是追随者（Follower），每个追随者只需要根据领航者的位姿（位置+朝向）计算出自己应该去的位置，然后通过控制器驱赶自己往那个目标点走。

在仿真环境里，这个逻辑被形式化为一组数学表达式。追随者的控制目标通常有两种定义方式：一种是相对距离-相对角度（l-φ控制），追随者与领航者保持固定距离l和固定夹角φ；另一种是直接定义追随者在领航者坐标系下的期望坐标（d_x, d_y）。前者更贴近传感器测量逻辑（激光雷达或者视觉直接给出距离和角度），后者在编程实现上更直观。仿真里我建议直接用第二种，因为代码里少一层三角函数换算，出问题的概率更低。

这里有个很多人容易忽略的点：领航追随法的“领航者”有时候不一定是真实机器人，可以是虚拟点。比如你想让三个机器人排成三角形朝某个方向运动，你可以定义一个虚拟领航者，它按预设轨迹走（直线、圆弧甚至任意曲线），三个真实机器人分别以它为基准计算自己的期望位置。这个做法在仿真里特别实用，因为虚拟领航者的轨迹完全可控，调试队形的时候排除领航者的干扰，问题定位更干净。

1.2 三类主流编队方法的取舍：为什么仿真阶段首选领航追随

除了领航追随法，多机器人编队还有两类常见方案：虚拟结构法和基于行为法。虚拟结构法把整个编队看成一个刚体，每个机器人对应刚体上的一个固定点，队形保持能力很强，但一旦队形需要动态变化就会很僵硬；基于行为法给每个机器人预设若干行为（避障、朝目标走、保持队形），通过加权融合得到最终控制量，灵活是灵活，但数学上很难证明队形稳定性，调权重的过程也容易让人崩溃。

领航追随法正好卡在中间：数学表达清晰，稳定性分析有现成工具（拉普拉斯矩阵、李雅普诺夫方法都能往上套），代码量适中，而且队形变换只需要改期望相对坐标就够了。做课题研究、发论文，审稿人最容易认可的就是这种“思路简单但有理论支撑”的方法。

从我的实际体验来看，领航追随法还有一个隐藏优势：它天然支持分层扩展。一个领航者带三个追随者形成一个小编队，这个小编队整体又可以作为另一个更大编队的“虚拟领航者”。这种嵌套结构在规模化编队控制里很有用，而且Matlab仿真里实现嵌套就是多写几个结构体数组的事，逻辑并不复杂。

1.3 队形保持的核心难点：坐标变换这事绕不过去

领航追随法排兵布阵的时候，最绕不开的就是坐标系变换。每个机器人都有自己的局部坐标系（通常以机器人中心为原点，前进方向为x轴），但队形参考关系是在世界坐标系下算的。追随者的期望位置在世界坐标系下怎么算？简单说三步：

第一步，知道领航者的当前位姿（x_L, y_L, θ_L）。第二步，知道队形定义的相对位置，这个相对位置通常定义在领航者局部坐标系下（比如“领航者正前方2米偏右1米”）。第三步，把这个局部相对位置旋转到世界坐标系，加上领航者的世界坐标，得到追随者在世界坐标系下的期望位置。

第三步就是那个旋转变换，公式长这样（假设相对位置是Δx_local, Δy_local）：

Δx_world = Δx_local * cos(θ_L) - Δy_local * sin(θ_L) Δy_world = Δx_local * sin(θ_L) + Δy_local * cos(θ_L)

就这么两行公式，我见过不下五个同学在仿真里栽跟头，表现是队形旋转时追随者全乱跑。原因大同小异：要么忘了乘旋转矩阵，要么旋转方向搞反，要么把θ_L误用了追随者自己的朝向角。这里建议你在调试阶段把“期望队形位置”这个变量打点画出来，跟领航者轨迹叠在一起看，能一眼看出坐标变换对不对。

2. Matlab仿真环境准备与整体框架设计

2.1 基础环境与工具箱选择

Matlab仿真多移动机器人编队，其实不需要额外装什么昂贵的工具箱。基础Matlab环境加一个Simulink可选（我这里主要讲脚本式仿真，因为脚本式对初学者更友好、更好调试），核心工具就三个：普通矩阵运算、ode45求解器、plot绘图系列函数。如果你装了Robotics System Toolbox，里面有一些现成的机器人模型和坐标变换函数，但说实话对这种简单编队仿真来说属于“杀鸡用牛刀”，而且工具箱函数用多了反而容易忽略底层逻辑，对理解算法本身帮助不大。

版本方面，我用的环境是某主流版本（近几年的版本都可以，代码没有用到版本特有函数）。如果电脑配置一般也不用担心，三五个机器人的编队仿真，计算量非常小，普通笔记本几秒钟就能跑完一次仿真。

2.2 顶层架构：六个模块，各干各的

写仿真程序最忌讳的就是“一坨式”代码——所有逻辑全堆在主脚本里，跑完一次想改个队形参数，得在几十行甚至上百行代码里翻找。我习惯把编队仿真拆成六个独立模块，各司其职：

第一个模块是参数初始化脚本，集中放所有可调参数：机器人数量、初始位置、队形相对坐标、控制增益、速度上限、仿真时长。第二个模块是领航者轨迹生成函数（虚拟领航者时用），输出每个时刻领航者的位置和朝向。第三个模块是追随者运动学模型，描述机器人如何响应控制输入（线速度和角速度）。第四个模块是控制器函数，输入追随者当前位姿和目标位姿，输出线速度和角速度指令。第五个模块是主仿真循环，用固定步长或ode45推进时间，按顺序调用上面这些函数。第六个模块是绘图脚本，画轨迹、画队形快照、画误差曲线。

模块化最大的好处是排错效率高。比如仿真结果里追随者轨迹出现异常抖动，你可以先看控制量曲线，如果控制量正常，那问题大概率在运动学模型或数据传递上；如果控制量本身就在高频振荡，那就是控制器增益调得太猛。定位范围缩小一个数量级，不用从头到尾逐行看。

2.3 运动学模型：差速机器人的简化表达

编队仿真里用的机器人运动学模型，绝大多数是差速驱动模型。这种模型在Matlab里长这样：

x_dot = v * cos(θ) y_dot = v * sin(θ) θ_dot = ω

其中v是线速度，ω是角速度，θ是朝向角。输入是(v, ω)，输出是位姿变化率。这个模型有两个隐含假设：机器人是刚体，轮子不打滑。做编队控制仿真这个精度足够了，但如果后面做实物实验，需要在这个模型基础上加轮式滑动的补偿，那就是另一个话题了。

这里有个小细节：仿真里更新位姿，很多人直接用欧拉积分（x = x + x_dot * dt），简单省事。但欧拉积分在dt偏大时会有累积误差，导致轨迹慢慢漂移。我习惯用ode45来积分，Matlab内置求解器自适应步长，精度有保证。代价是代码稍复杂一点，但换来的是可靠性提升，对新手来说，与其去调试那些莫名其妙的漂移问题，不如一开始就用对工具。

3. 核心控制律设计与Matlab代码实现

3.1 跟随控制律的数学形式：从误差到控制量

控制律设计是整个编队仿真最核心的部分。我用的是一种在文献里很常见的形式：先算追随者当前位置与期望位置之间的误差，然后设计一个比例控制器把误差压缩到零。具体来说，定义误差向量为：

e_x = x_des - x_f e_y = y_des - y_f

然后直接设计线速度和角速度：

v = v_leader + k_x * (e_x * cos(θ_f) + e_y * sin(θ_f)) ω = k_θ * (θ_des - θ_f)

其中θ_des是追随者期望朝向角，通常取为指向期望位置的方位角，也就是atan2(e_y, e_x)。注意这个控制律里我加了前馈项v_leader，这是让追随者能跟上领航者速度的关键。如果不加这个前馈，追随者只有在误差为零后才能达到领航者的速度，任何扰动都会产生滞后，表现为编队“被拉长”。

k_x和k_θ是两个比例增益。怎么选？k_x决定追随者追赶目标点的快慢，太大会导致速度饱和甚至超调，太小则响应迟钝；k_θ决定转向的激进程度，太大会导致朝向振荡。我的经验值是k_x取0.5到1.5之间，k_θ取1.0到2.5之间，具体数值跟仿真步长和速度上限有关，后面再细说调参方法。

3.2 完整仿真代码注解：领航追随法Maltab实现

我在这里给出一个完整的可运行框架代码，注释写得比较细。这几个关键函数建议按顺序看完，整个编队仿真的骨架就清楚了。

首先是主脚本，负责参数定义和仿真循环：

% 主脚本：多移动机器人领航追随编队仿真 % 队形：三个机器人，领航者1个，追随者2个 clear; close all; clc; %% 参数初始化 dt = 0.05; % 控制周期（秒） T_total = 20; % 总仿真时长（秒） t = 0:dt:T_total; N = length(t); % 机器人初始位姿 [x; y; theta] pose_leader = [0; 0; pi/4]; pose_f1 = [-1; -1; pi/4]; % 追随者1初始位姿 pose_f2 = [-1; 1; pi/4]; % 追随者2初始位姿 % 队形相对坐标（领航者局部坐标系，前向为x正方向） offset_1 = [-1.5; 1.0]; % 追随者1：左后方 offset_2 = [-1.5; -1.0]; % 追随者2：右后方 % 控制增益 k_v = 0.8; % 线速度增益 k_w = 2.0; % 角速度增益 % 存储记录 hist_leader = zeros(3, N); hist_f1 = zeros(3, N); hist_f2 = zeros(3, N); %% 仿真主循环 for i = 1:N t_current = t(i); % 1. 计算领航者轨迹（简单示例：匀速直线+圆弧） if t_current < 10 v_L = 0.5; w_L = 0.15; else v_L = 0.5; w_L = 0.0; % 后半程直线 end % 2. 更新领航者位姿（这里直接给精确轨迹，模拟理想领航） pose_leader(1) = pose_leader(1) + v_L * cos(pose_leader(3)) * dt; pose_leader(2) = pose_leader(2) + v_L * sin(pose_leader(3)) * dt; pose_leader(3) = pose_leader(3) + w_L * dt; % 3. 计算两个追随者的期望位置（世界坐标系） theta_L = pose_leader(3); R = [cos(theta_L), -sin(theta_L); sin(theta_L), cos(theta_L)]; des_f1 = pose_leader(1:2) + R * offset_1; des_f2 = pose_leader(1:2) + R * offset_2; % 4. 调用控制器，计算追随者控制量 [v_f1, w_f1] = follower_controller(pose_f1, des_f1, v_L, k_v, k_w); [v_f2, w_f2] = follower_controller(pose_f2, des_f2, v_L, k_v, k_w); % 5. 更新追随者位姿（简化运动学模型） pose_f1 = update_pose(pose_f1, v_f1, w_f1, dt); pose_f2 = update_pose(pose_f2, v_f2, w_f2, dt); % 6. 记录数据 hist_leader(:, i) = pose_leader; hist_f1(:, i) = pose_f1; hist_f2(:, i) = pose_f2; end %% 绘图展示 figure(1); plot(hist_leader(1,:), hist_leader(2,:), 'b-', 'LineWidth', 1.5); hold on; plot(hist_f1(1,:), hist_f1(2,:), 'r--', 'LineWidth', 1.5); plot(hist_f2(1,:), hist_f2(2,:), 'g--', 'LineWidth', 1.5); axis equal; grid on; legend('Leader', 'Follower1', 'Follower2'); xlabel('x (m)'); ylabel('y (m)'); title('多移动机器人领航追随编队轨迹');

然后是追随者控制器函数：

function [v, w] = follower_controller(pose, des_pos, v_leader, k_v, k_w) % 领航追随控制器 % 输入：pose - 当前位姿 [x; y; theta] % des_pos - 期望位置 [x_des; y_des] % v_leader - 领航者当前线速度（前馈） % k_v, k_w - 控制增益 % 输出：v - 线速度指令， w - 角速度指令 x = pose(1); y = pose(2); th = pose(3); x_des = des_pos(1); y_des = des_pos(2); % 位置误差 e_x = x_des - x; e_y = y_des - y; % 期望朝向（指向目标点） th_des = atan2(e_y, e_x); % 朝向误差（注意角度归一化到[-pi, pi]） e_th = wrapToPi(th_des - th); % 控制律 v = v_leader + k_v * (e_x * cos(th) + e_y * sin(th)); w = k_w * e_th; end

再是运动学更新函数：

function pose_new = update_pose(pose, v, w, dt) % 差速机器人运动学更新 % 输入：pose - 当前位姿 [x; y; theta] % v - 线速度， w - 角速度 % dt - 积分步长 % 输出：pose_new - 更新后的位姿 x = pose(1); y = pose(2); th = pose(3); x_new = x + v * cos(th) * dt; y_new = y + v * sin(th) * dt; th_new = th + w * dt; pose_new = [x_new; y_new; th_new]; end

上面这套代码跑通之后，你就拥有了一个最基础的领航追随编队仿真环境。三个机器人能保持倒三角队形沿轨迹运动，如果不出意外，轨迹图画出来应该是三条平行且错位有致的曲线。

3.3 加入队形误差评估指标：别只盯着动画看

很多人仿真跑完，看一眼轨迹觉得“队形好像没散”，就完事了。这不行，做研究不能凭感觉。你需要量化评估队形保持效果，最常用的指标是队形误差——每个时刻追随者实际位置与期望位置的偏差的范数。定义如下：

e_form(t) = sqrt((x_f - x_des)^2 + (y_f - y_des)^2)

把所有时刻的队形误差画成曲线，你就能直观看到：初始阶段有一个误差收敛的瞬态过程，稳态阶段误差应该在零附近小幅波动。如果这个稳态误差长期不归零，说明控制律里有静差，得检查是不是漏了前馈项，或者增益太小。

我建议在主脚本里加一个记录队形误差的数组，仿真结束后单独画一张图。这张图在论文里很有用，直接就能当“编队保持效果分析”这一节的插图。误差曲线从大变小到稳定的过程，比你写三百字文字描述有说服力得多。

3.4 队形变换的扩展：换队友还是换位置

基础仿真跑通后，你可以试着让编队做队形变换。最简单的方式：在仿真中途改offset变量的值。比如前10秒保持三角形队形，第10秒切换成横队（一字排开），只需要在循环里加一个判断，第10秒后把offset_1和offset_2改成新的值。

队形变换时会出现一个有意思的现象：两个追随者会各自规划路径去新位置，可能会出现互相穿越的情况。如果你在意这个问题（在实物场景中很重要，涉及避碰），就需要引入避碰算法，比如人工势场法。但在仿真初期，先把队形变换跑通就够了，避碰作为后续扩展章节去补充。

4. 常见问题排查与参数整定心得

4.1 仿真发散：误差越滚越大怎么办

这是最常遇到的问题。表现是机器人轨迹越来越离谱，误差曲线像过山车，最终某个值变成无穷大。原因有这么几类，按出现频率排序：

第一类，增益过大。k_v或k_w调得太高，控制量输出超出执行器能力范围，形成振荡发散。典型特征是误差曲线先收敛后发散，或从小幅振荡变成大幅振荡。解决方法简单粗暴：把增益降到原来的三分之一，看是否稳定，再逐步往上加。我习惯的找临界值方法：k_w从1.0开始，每次加0.5，直到出现振荡再退回上一个值。

第二类，期望朝向角计算错误。atan2的返回范围是(-pi, pi]，但机器人的朝向角可能因为长时间累积超出了这个范围。比如θ_f=3π/2，θ_des=-π/4，直接相减得到巨大误差。这就是典型的“角度卷绕”问题。解决：用wrapToPi函数（Matlab内置）把所有角度差归一化到(-pi, pi]。我在这上面栽过一次，当时怎么都想不通为什么机器人在某几个角度区域突然疯狂自旋，检查了两天才发现是角度没归一化。

第三类，时间步长太大。响应速度快的控制器对dt敏感，dt太大时离散化误差淹没稳定裕度。把dt从0.05改成0.01，问题通常直接消失。注意如果用了ode45，dt只是输出步长，不影响积分精度，问题相对小一些。

4.2 队形收敛太慢或“拖尾”明显：前馈增益在作怪

如果你发现队形整体是稳的，但追随者总是慢半拍，领航者转弯时追随者的轨迹大幅向外飘逸，那基本是前馈项的问题。我上面给的代码里控制律是v = v_leader + k_v * (...) 的形式，前馈v_leader是必要项。如果你把前馈去掉，仿真还能稳，但队形误差会在转弯时明显增大。

前馈不一定非要等于领航者线速度，你还可以加更高级的前馈：期望速度不仅考虑领航者的速度，还要加上由于领航者转弯带来的“牵连速度”。这个用公式表示很复杂，但效果很直观——转弯时队形误差显著减小。做高精度编队控制的论文，这一步是加分项。

4.3 代码没错但队形就是不对称：检查初始位置

带有对称偏移量的编队（比如两个追随者左右对称），跑出来的轨迹和队形却不对称，这个问题排查起来最坑。我遇到过一次，代码逻辑完全正确，最后发现是追随者1的初始位置设置有问题——两个追随者的初始朝向角不一致，一个正朝前一个偏了5度。虽然5度不大，但编队收敛后两个追随者的误差特性就明显不同了，一个过冲一个不过冲。

解决方案：初始位姿尽量设置得精确对称，或者先让仿真跑一段“预收敛”时间，从某个中间时刻开始正式统计分析。第一种方法适合强迫症，第二种方法适合实证研究，一般情况下两种配合着来。

4.4 参数整定速查：一套能用的起点参数

调试参数是有捷径的——先让系统稳定，再调性能。这里直接给一套经过验证的起点参数，适合机器人速度范围0到1.0 m/s、角速度范围-1.5到1.5 rad/s的场景：

特别提醒：无论k_v和k_w算出来多大的控制量，最后给运动学模型之前，务必要做限幅处理。这个我管它叫“执行器饱和”。代码里加几行：

v_max = 1.0; w_max = 1.5; v = max(min(v, v_max), -v_max); w = max(min(w, w_max), -w_max);

不加限幅，一旦仿真中途有扰动（哪怕是数值计算的舍入误差），控制量就可能巨大，直接让仿真结果失去意义。加了限幅，仿真行为才接近真实的物理机器人。

5. 参考文献与论文写作衔接技巧

5.1 这个仿真对应哪些经典文献

写论文的时候，“多移动机器人协同编队控制、领航追随法”这块文献引用是有套路的。基础必引的是领航追随法开山鼻祖那几篇，通常可以在IEEE Transactions on Robotics、Automatica、Systems & Control Letters这几个期刊里找到。引用时不要只引一篇，最好引三篇：一篇是编队控制综述，一篇是领航追随法原始提出或系统化分析的文章，一篇是近年改进方案。如果你的方法里有自己的改进（比如增加了避障、改进了控制器），再补一篇对比文献。

这里有个小建议：下载文献的时候注意看它的仿真部分，很多经典文献直接在仿真里复现了编队过程，跟你的仿真结果可以做同条件对比。论文里如果放一张“与某文献编队误差对比图”，工作量不大，但能显著提升说服力。

5.2 仿真结果怎么在论文里呈现

跑完仿真拿到图和误差曲线之后，论文撰写阶段按这个结构组织最稳妥：

第一节，问题描述：说明编队控制的目标，几个机器人、什么队形、有没有障碍物。第二节，控制器设计：给出控制律公式，注意公式中的每一个变量都要在正文或附录里定义清楚，并说明增益怎么选的。第三节，仿真设置：列出机器人参数表（初始位置、队形偏移、控制增益）、仿真时长、轨迹设置，方便别人复现。第四节，仿真结果与分析：放轨迹图、队形误差图，针对每个图写一两句分析——“从图中可以看出，收敛时间约为X秒，稳态误差小于Y米，满足控制要求”。

这套结构是主流写法，也最不容易被审稿人挑毛病。如果你做的是课程作业或毕设，完全可以直接套用这个框架，把“审稿人”换成“答辩老师”，效果一样。

5.3 参考文献格式与标注规范

Matlab仿真用的核心方法参考了哪些文献，理论上在论文的引言和控制器设计部分要引用标注。不同期刊和学校的格式要求不一样，但做的论文多的人会有几个通行的习惯：正文标注用数字上标，参考文献列表按出现顺序编号；每条文献包含完整的作者、年份、题目、期刊/会议名、卷期页码。具体到某个格式（比如IEEE、APA、GB/T 7714），用文献管理工具导出就好，别手动排版，浪费时间还容易出错。

6. 后续扩展方向与个人实操总结

基础的三机编队跑通后，你有好几个方向可以继续深入，按性价比排序：

第一个方向是多机扩展。从3个机器人扩展到5个、10个、20个，看看控制律和代码框架是否依然有效。这里会冒出一个问题：如果所有追随者都直接跟着领航者，距离太远时跟踪误差会变大，这时就要引入“嵌套领航”——离得近的追远的不如追近的，形成级联结构。代码层面，把追随者的期望位置改成参考另一个追随者而不是领航者，改动量很小，但效果和理论复杂度都上一个台阶。

第二个方向是加入障碍物和避碰。最简单的方式是人工势场法，在控制律输出上叠加一个斥力项。Matlab里实现大概几十行代码，但能把编队控制从“理想环境”推向“有限环境”，这个能力在学校项目和实际课题里更有竞争力。

第三个方向是算法对比。拿你自己的领航追随仿真结果，去跟虚拟结构法或模型预测控制方法做对比，画误差对比图。横向对比做得越扎实，论文的含金量越高。

我在实际调试中有一个体会：编队仿真这东西，代码能力和理论理解缺一不可。但如果你先跑通一套基础代码，再回头补理论细节，效率和理解深度都会更好。这套仿真代码我建议你跑通之后，故意改几个参数试试（比如把前馈去掉、把增益调大），观察误差曲线的变化，亲手“搞坏”几次，比只看正确代码学到的东西多得多。最后再分享一个小技巧：做多机编队仿真，一定要把绘图脚本放在循环外部单独写，不要把绘图塞进仿真主循环。仿真归仿真，绘图归绘图，两者分离之后，你会发现调试效率至少提升一倍，而且后期改图样式（线宽、颜色、图例）不用重新跑仿真，改完直接执行绘图脚本就行。这个习惯我从写第一个编队仿真开始坚持到现在，每次都省下大把时间。