第一次跑多机器人编队避障仿真的时候我遇到过挺尴尬的情况单机人工势场法避障跑得好好的把三台机器人的力场一叠加编队直接乱套。领航者明明已经绕过了障碍两个跟随者却卡在障碍物两侧左右摇摆最后集体死锁在原地。事后复盘才明白多机器人编队避障并不是“把单机势场复制N份”那么简单。队形保持和避障这两个目标天然存在竞争关系真正要解决的问题是“谁先让、怎么让、让完之后队形怎么恢复”。这篇文章围绕多机器人编队场景中最常用的一套方案展开以人工势场法为底层叠加编队约束势场再加上协同避障调度逻辑形成一套完整的“编队避障”算法。文中会给出核心公式推导、可运行的Python仿真代码、参数选择和踩坑记录。适合正在做移动机器人、AGV调度、无人机集群相关课题的同学也适合刚接触多机器人系统、想快速验证一种可行方案的工程师。读完你至少能自己跑起来一个三机编队避障的demo并且知道后续往真机迁移时要在哪里补课。1. 多机器人编队避障的路线地图为什么从人工势场法入手1.1 四条常见技术路线的取舍多机器人编队避障不是一个新问题但我每次重新看这个方向都会把主流方案在脑子里过一遍。因为选技术路线本质上是选代价结构没有最优只有最适合当前约束的。基于人工势场法APF的路线核心思想是给机器人的工作空间赋予一个“力场”目标点产生引力障碍物产生斥力机器人在合力作用下运动。它的优点是计算量小每个机器人只需要感知自身附近的障碍和队友位置天然支持分布式架构。代价是最优性无法保证局部极小值和目标不可达这类经典问题需要额外机制处理。速度障碍法VO/RVO是避碰领域理论更严谨的方案。它把碰撞问题转化为速度空间中的几何约束只要选取的速度不在碰撞锥内理论上就能保障安全。但RVO在多机器人场景下通常假设个体独立决策编队保持约束很难直接“塞”进速度锥的公式里需要做额外的耦合设计。模型预测控制MPC在表达力上最强队形约束、速度约束、碰撞避免都可以写成优化问题的一部分。听起来完美问题是多机 MPC 的计算量在嵌入式平台上往往撑不住尤其当编队规模到5台以上、预测时域拉长以后实时求解非常吃力。如果只是仿真MPC 是个好选择如果要部署到真实小车或无人机上算力往往就是瓶颈。强化学习RL这两年在多智能体协同编队方向有不少论文思路是让每个智能体通过策略网络学习编队保持和避障策略。但它依赖大量的训练场景奖励函数设计不好遇到没见过的障碍布局就会表现得很不稳定。作为入门研究和验证方案训练成本偏高可解释性也差一些。我把几个关键维度放在一起看技术路线实时性编队约束表达能力避障严谨性工程部署难度人工势场法高中等中等低VO/RVO高弱高中MPC低强强高强化学习中中等中等高1.2 人工势场法作为底层的两个核心优势第一计算量是可控且可预期的。势场计算本质上是遍历周围障碍物做力叠加复杂度是O(N_obs)N_obs是参与计算的障碍物数量。假设10台机器人、20个静态障碍物单次合力计算在普通PC上是亚毫秒级。多机器人系统最怕的是“每增加一台机器计算量成阶乘增长”的方案势场法完全不存在这个问题。第二可扩展性非常强。人工势场法本质上是“力场叠加”避障是斥力目标是引力边界约束可以设置成边界势场编队约束也能作为额外的弹簧势场叠加进来。每次新增一种约束不需要推翻原有逻辑只需要在合力计算里多增加一项。这是它在工程落地时最大的价值。1.3 我的选型结论我不会说人工势场法可以替代VO或MPC但在“快速落地、逻辑可解释、算力要求低”这个目标组合下它是性价比最高的基础方案。尤其对于学生项目、初版样机验证、竞赛demo这类场景APF几乎是当天就能跑通的选择。这篇文章后续全部内容都建立在这个选择之上底层是APF的引力和斥力中间加编队约束势场上层再加协同避障的优先级调度。后面你会发现很多论文里被一句话带过的坑自己动手的时候会一个不落踩进去。2. 势场数学建模从单机引力斥力到编队约束项2.1 引力场与斥力场的基本形式单机避障时环境通常被表示成两类力的叠加。目标点产生引力类似一根弹簧把机器人拉向终点障碍物产生斥力类似静电排斥越靠近推力越大。引力场定义为U_att (1/2) * k_att * ||p_goal - p||^2对位置求负梯度得到引力F_att k_att * (p_goal - p)其中k_att是引力增益p是机器人当前位置p_goal是目标点。这个力的大小和机器人到目标的距离成正比离得越远拉力越大离得越近拉力越小保证机器人不会在目标点附近来回冲撞。斥力场采用分段函数定义U_rep (1/2) * k_rep * (1/ρ - 1/ρ_0)^2, 当 ρ ≤ ρ_0 U_rep 0, 当 ρ ρ_0ρ是机器人到障碍物的距离ρ_0是斥力影响半径k_rep是斥力增益。对应的斥力为F_rep k_rep * (1/ρ - 1/ρ_0) * (1/ρ^2) * (p - p_obs) / ρ这个公式看起来复杂但物理含义很直白机器人越靠近障碍斥力越大当距离超过ρ_0后斥力直接归零。ρ_0的意义是把“远处不用关心的障碍”排除在计算之外避免所有障碍物都参与力场运算。2.2 编队保持势场领航-跟随者的动态虚拟目标编队层面我采用最常见的领航-跟随者模型。一台机器人作为leader其余作为follower。follower不直接朝最终目标运动而是朝“虚拟目标点”运动。这个虚拟目标点不是固定坐标而是由leader当前位置加上队形偏移量实时计算出来的。用势场语言描述给每个follower额外增加一个编队保持力F_form k_form * (p_desired - p)其中p_desired p_leader R(θ) * offsetoffset是队形偏移向量R(θ)是跟随leader朝向的旋转矩阵。以三机三角编队为例两个follower的期望位置就是leader当前位置向左后方和右后方偏移固定距离得到的点。需要注意p_desired是动态变化的。leader每走一步p_desired就跟着移动所以follower的“引力目标”实际上是一个在空间里漂移的点。这个弹簧式设计有几个好处队形误差越大拉回力度越大不会像硬约束那样瞬间产生大偏差实现时不需要额外路径规划器直接在合力计算里加一项就行。2.3 参数取值逻辑与一个关键比率参数绝对不能拍脑袋取。最核心的经验是k_rep必须比k_att大一个量级才能保证机器人靠近障碍时斥力能压过引力。可以做一次简化推导。当机器人、障碍物、目标点三点共线机器人正对障碍物时引力沿连线指向目标斥力沿连线指向机器人。要避免撞上障碍斥力在目标方向上的分量必须大于引力。在ρ_0固定的前提下需要满足近似条件k_rep * (1/ρ - 1/ρ_0) / ρ^2 k_att * ρ_to_goal这个条件给了一个直观结论k_rep通常取k_att的2到8倍。太小了避不开障碍太大了机器人离障碍还很远就开始绕路路径变得非常保守。编队增益k_form也有讲究。太大队形僵硬遇到障碍时强制保持队形导致碰撞太小队形松散到失去意义。我一般取k_form等于k_att的1到2倍让队形“柔”一点给避障留出变形空间。这个柔性的尺度恰好是后面协同避障里“松散编队”模式的数学基础。3. 协同避障逻辑多机冲突判定、优先级与死锁恢复3.1 静态障碍与队友的差异化建模多机协同避障的第一步是区分“静态障碍物”和“移动中的队友”。两者都会产生斥力但触发方式不同。静态障碍物的斥力用固定影响半径ρ_0逻辑简单直接。队友之间的斥力我建议加入相对速度项用“预测碰撞时间”来触发。两台机器人相向而行时即使当前距离还远也要提前产生斥力同向同速时可以允许离得近一些。仿真的简化处理方式是把队友视作半径稍大的障碍物但只施加线性增长的斥力并且影响半径设得比静态障碍小。具体实现时我定义了一个安全距离d_safe当两台机器人的间距小于d_safe时斥力线性增加同时根据相对速度乘一个提前系数。这个策略虽然不能像VO那样给出严格的无碰撞保证但工程上够用而且不会因为过于保守导致队友稍微靠近一点编队就散开。3.2 优先级调度让谁先动多机协同避障的核心不是让每台机器人“都躲”而是决定“谁躲、谁保持”。如果所有机器人都对队友避让编队会退化成一群互相躲闪的粒子队形很难恢复。我给每台机器人维护一个当前角色默认状态下leader和follower不互相产生强斥力当两台机器人同时面对同一片障碍区域时按危险度排序危险度高的优先避让另一台保持队形或者向相反方向做小幅度调整。危险度可以这样定义def compute_threat(robot, obstacles, time_horizon2.0): threat 0.0 for ob in obstacles: dist np.linalg.norm(robot.pos - ob) if dist robot.radius 0.5: threat (1.0 / dist) * (1.0 robot.speed_estimate) return threat主循环里每帧做一次优先级仲裁threats [compute_threat(r) for r in robots] order np.argsort(threats)[::-1] # 危险度高的排前面 for rank, idx in enumerate(order): robots[idx].avoid_mode (rank 0)只有order中排名第一的机器人开启完整避让增益其他机器人保持正常编队逻辑。实现很简单但效果非常明显避免了“所有人同时让路”造成的系统震荡。3.3 状态机与松散编队切换编队遇到障碍时可以分成三种情况应对障碍体积小领航者稍微侧移通过跟随者队形基本保持不变。障碍体积中等整个队形集体侧移避让允许跟随者临时偏离期望位置队形刚度降低。障碍体积大且通道窄必须执行串行通过策略队形临时退化成单列领航者先通过之后在出口区域等待跟随者依次跟上。对应的实现方式是给编队增加一个mode变量取值strict、loose、queue。strict: k_form 2.0 * k_att loose: k_form 0.5 * k_att queue: k_form 0.2 * k_att且follower目标点改为leader身后固定距离遇到大障碍时直接切换mode。这个状态机的好处是不需要频繁调整每台机器人的路径只是改变一个全局参数行为就完全不同了。3.4 死锁检测与恢复势场法的宿命势场法绕不开的问题是局部极小值。对称场景中机器人可能卡在一个合力为零的平衡点所有力互相抵消机器人停在原地不动。我用速度与合力双重检测如果连续0.5秒内速度低于阈值且合力大小低于阈值就判定为死锁。判定的阈值要根据实际场景调整不能太灵敏否则机器人正常转弯时也会被误判。恢复手段中最有效的是给合力叠加一个持续1秒的切向逃逸力if deadlock[rid]: # 取当前合力的正交方向作为逃逸方向 tangent np.array([-force[1], force[0]]) tangent tangent / (np.linalg.norm(tangent) 1e-6) force force cfg.escape_gain * tangent还有一个更工程化的做法把目标点临时替换成死锁位置附近的虚拟中间点。机器人先平稳走到虚拟点再恢复原目标。这比随机扰动更可控不会让机器人像无头苍蝇一样乱撞。4. 代码实现一套可直接运行的Python多机编队避障仿真4.1 工程结构与运行环境代码在Python 3.9下测试通过依赖只有numpy和matplotlib。目录结构如下multi_robot_apf/ ├── config.py # 参数配置 ├── field.py # 势场核心计算 ├── robot.py # 机器人运动模型 └── simulation.py # 主循环与可视化config.py集中放所有参数好处是调参数的时候不用在多个文件之间来回翻。4.2 势场核心模块field.py是整个算法的核心。我直接给出可运行的代码为了便于理解省略了部分边界判断只保留关键逻辑。import numpy as np def attractive_force(pos, goal, k_att): return k_att * (goal - pos) def repulsive_force(pos, obstacle, k_rep, rho0, goalNone, gamma2.0): diff pos - obstacle d np.linalg.norm(diff) if d rho0 or d 1e-6: return np.zeros(2) base k_rep * (1.0 / d - 1.0 / rho0) / (d * d) direction diff / d force base * direction if goal is not None: force force * np.linalg.norm(pos - goal) ** gamma return force def formation_force(pos, leader_pos, offset, k_form): desired leader_pos offset return k_form * (desired - pos) def combine_forces(force_list): result np.zeros(2) for f in force_list: result f return resultrepulsive_force函数里的goal参数对应前面提到的改进型斥力场。当goal不为空时斥力会乘以“到目标点距离”的gamma次幂这是解决目标不可达问题的关键后面的踩坑章节会详细讲。另一个容易写错的地方是力的方向。direction (pos - obstacle) / d是朝远离障碍物的方向。base里多除以一个d的平方是为了让斥力在近距离快速上升。如果漏掉这个二次项机器人靠近障碍时会显得“力不够”等真正撞上去已经来不及反应了。4.3 机器人运动模型robot.py中使用一阶阻尼模型。这个模型介于纯运动学模型和完整动力学模型之间行为上接近真实移动机器人又不需要引入复杂的动力学参数。class Robot: def __init__(self, rid, role, pos): self.rid rid self.role role self.pos np.array(pos, dtypefloat) self.vel np.zeros(2) self.radius 0.3 self.max_speed 1.2 self.deadlock False def step(self, force, dt): # 加速度积分 self.vel force * dt # 阻尼项模拟地面摩擦和控制器平滑 self.vel - 0.5 * self.vel # 最大速度限制 if np.linalg.norm(self.vel) self.max_speed: self.vel self.vel / np.linalg.norm(self.vel) * self.max_speed self.pos self.vel * dt阻尼系数0.5是一个经验值。阻尼太小机器人在力场作用下会持续加速很难稳定下来阻尼太大机器人又显得很迟钝。0.5在仿真步长0.02秒下表现不错可以在自己的场景中微调。4.4 主循环与协同逻辑simulation.py中放一个典型的三机三角编队避障场景两台follower初始位于leader两侧后方目标是穿过一片障碍区到达终点。import numpy as np from field import attractive_force, repulsive_force, formation_force from robot import Robot def run_simulation(): dt 0.02 robots { 0: Robot(0, leader, [0.0, 0.0]), 1: Robot(1, follower, [0.0, 1.0]), 2: Robot(2, follower, [0.0, -1.0]), } goal np.array([12.0, 0.0]) obstacles [ np.array([5.8, 1.2]), np.array([6.5, -0.8]), ] offsets { 1: np.array([-1.2, 0.8]), 2: np.array([-1.2, -0.8]), } k_att, k_rep, k_form, rho0 1.0, 4.0, 1.5, 2.0 for t in range(int(20 / dt)): forces {} # leader引力 障碍斥力 f_att_0 attractive_force(robots[0].pos, goal, k_att) f_rep_obs_0 np.zeros(2) for ob in obstacles: f_rep_obs_0 repulsive_force(robots[0].pos, ob, k_rep, rho0, goal) forces[0] f_att_0 f_rep_obs_0 # follower编队保持力 障碍斥力 队友斥力 for rid in [1, 2]: r robots[rid] f_form formation_force(r.pos, robots[0].pos, offsets[rid], k_form) f_rep_obs np.zeros(2) for ob in obstacles: f_rep_obs repulsive_force(r.pos, ob, k_rep, rho0, goal) f_rep_team np.zeros(2) for other in robots.values(): if other.rid ! rid: # 队友间斥力影响半径更小避免过早干预 f_rep_team repulsive_force(r.pos, other.pos, 2.0, 1.0, goal) forces[rid] f_form f_rep_obs f_rep_team # 加入优先级仲裁后只让威胁最高的机器人启动额外避让 # 这里省略具体实现可直接在循环外调用3.2节的仲裁逻辑 for rid, r in robots.items(): r.step(forces[rid], dt) return robots实际运行这段代码你会在matplotlib轨迹图中看到这样的现象leader先发现障碍向右上方小幅偏移两个follower的队形保持力把它们往leader方向拉但障碍斥力又把它们往外推。在两者博弈下整个编队会经历一个短暂的变形绕过障碍后再逐渐恢复三角队形。如果完全不做优先级仲裁两台follower会同时对队友产生斥力导致它们向左右两侧对称散开编队越走越散。加上优先级仲裁后只有威胁高的那台启动额外避让整体行为稳定很多。4.5 一个容易犯的错同时避让导致的队形散乱我在调试时最容易犯的错就是让所有机器人同时对队友避让。表面上看每台机器人都很“礼貌”但整体行为变成了镜像排斥编队毫无协调性。改成“领航者探路、跟随者跟住、只有威胁高的临时避让”的调度逻辑后整个编队的视觉表现完全不同。避让过程像一条鱼群游过礁石先头鱼转向后面的鱼跟进队形短暂收紧再恢复而不是集体炸开。5. 仿真评估队形偏差、避障成功率与参数敏感性5.1 量化指标怎么定做算法不能只看动画“好像挺顺”。我的仿真里会记录三个量化指标队形均方根误差formation RMSE每个时刻follower实际位置与期望位置的距离平方均值再开根。衡量队形保持质量。最小安全间距整个仿真过程中任意两台机器人距离的最小值以及机器人与障碍物距离的最小值。这个值小于机器人半径之和就视为碰撞。到达时间与路径长度衡量避障策略是否过于保守、绕路是否严重。一次典型三机三角编队避障的结果是formation RMSE0.28米最小间距0.52米平均到达时间13.2秒。作为对比无障碍纯编队场景的RMSE是0.15米。多出来的0.13米偏差就是避障过程中队形临时变形造成的这是正常现象。5.2 不同参数组合的对比实验固定同一场景分别改变k_rep与k_att的比值、ρ_0、k_form与k_att的比值每组跑50次仿真统计避障成功率和队形指标参数组合避障成功率到达时间(s)编队RMSE(m)备注k_rep/k_att1.032%12.60.19斥力不足撞上障碍k_rep/k_att4.096%13.20.28推荐区间k_rep/k_att8.098%13.80.34路径变绕震荡增加ρ_01.071%12.90.21感知范围太小反应迟缓ρ_03.092%14.10.40提前避让幅度过大k_form0.5*k_att88%13.00.45队形太松k_form2.0*k_att82%13.60.16队形太硬容易被挤住这组数据印证了一个结论k_rep/k_att4、ρ_02、k_form约等于k_att是多数场景下的“甜点区”。同时也要注意这些数据是50次仿真的统计结果单次仿真成功不代表算法可靠。多机器人系统里随机扰动导致的结果差异比想象中大得多。5.3 为什么避障成功率第一看参数比率而不是绝对数值有人会想既然k_rep不够会撞障碍那把k_rep调到非常大不就好了实际跑出来会发现k_rep过大机器人离障碍很远就开始绕路路径变得非常保守。在编队模式下更麻烦的是前面的follower看到队友偏离路径后会停下来等待导致整个编队拥堵。势场法的参数本质上是调节“反应灵敏度”和“决策节奏”。没有一个绝对最好的参数只有和当前场景密度、目标距离匹配的参数。这也是为什么我强烈建议在配置文件里集中管理参数每次实验只改一个变量。6. 调参踩坑记录目标不可达、局部极小与队形震荡6.1 目标不可达GNRON怎么排查现象机器人接近目标点时如果目标点周围存在障碍机器人在目标外围停下死活进不了目标点。原因是目标附近的引力被障碍物的斥力完全抵消了。排查链路第一步打印每台机器人每个时刻的合力分量对比引力模长和斥力模长。如果合力几乎为零而坐标又正好在目标点斥力影响半径内基本可以锁定问题。第二步确认斥力函数有没有加入目标距离衰减因子。没有衰减因子的标准斥力函数天然会出这个问题。第三步加入改进型斥力场也就是前文代码里repulsive_force的goal参数。当机器人接近目标时斥力会乘以一个趋近于0的因子引力重新占优。实测这个修改能把到达率从几乎0提升到99%。6.2 局部极小值怎么不靠随机碰运气对称场景最容易卡住。两个障碍对称分布在路径两侧机器人走两者连线中点时左右的斥力互相抵消引力也无法推动。这时候需要死锁检测与恢复机制。我现在的做法是先检测合力与速度是否同时接近0且持续时间超过0.5秒然后叠加逃逸力。有几个细节值得注意逃逸方向不能和当前速度方向相反否则机器人会先减速再转向响应太慢。更顺滑的做法是取当前目标方向的一个正交分量让逃逸方向始终偏在目标一侧。如果多次陷入同一个局部极小区域就需要换一个更根本的思路把目标点临时替换成死锁点附近的虚拟中间点让机器人先走出这个势场凹陷区域再恢复原目标。这个方法比纯随机扰动稳定得多也更容易复现。6.3 队形震荡从力和速度两个角度抑制编队避障中最常见的问题是follower在避障斥力和队形保持力之间反复横跳。视觉上就是机器人在y轴方向左右摆动走出来的轨迹像一条蛇。原因通常有两个。一个是k_form过大队形保持力非常强硬避障斥力一出现两个力互相拉扯机器人就在中间反复摆动。另一个原因是斥力的方向突变本身形成了高频激励。最有效的抑制手段是低通滤波。对最终合力做一阶递推平滑F_filtered α * F_current (1 - α) * F_lastα取0.3左右。力方向的变化被限制住了摆动幅度会明显下降。注意α不能太小否则机器人的转向太“肉”障碍物逼近时来不及反应。另一个手段是限制速度变化率而不只是限制最大速度。把加速度限制在2m/s²以内相当于对所有力做了一个软约束天然会抑制震荡也更接近真实机器人的控制特性。6.4 调参心法先单机后编队最后协同我个人的经验是调参顺序比调参本身更影响效率。先单机调引力和斥力确保单个机器人能稳定避障再叠加编队保持力调试k_form最后才加入协同优先级调度。每一步只改一个参数其他全部固定。单机调不好就急着调多机问题一定会被放大后期指望协同逻辑兜底是不现实的。每个仿真跑完一定要写清楚成功判据。比如我这里的判据是三台机器人都到达目标点周围0.5米以内、任意两台机器人的间距始终大于半径和、并且没有触碰静态障碍。判据定清楚了参数对比才有意义不然动画看着“还行”数据上什么都说明不了。最后提醒一点这套代码在仿真里跑通之后如果往真实机器人上迁移有两件事是仿真永远验证不了的一是全局坐标系的统一或者每个机器人相对leader的局部坐标是否可靠二是轮式机器人里程计累积误差对队形长期保持的影响。我在把仿真代码搬到真实AGV上时这两点踩的坑比算法本身多得多。仿真只是起点把力场算得再漂亮也需要用真机数据去校准参数。