人工势场法实现无人机三维编队避障与队形恢复的Matlab仿真解析

最近被问到一个挺有意思的仿真任务：领航者带着四架追随者无人机在三维空间里保持正方形编队飞行，中途遇到障碍物还要紧急避险，躲完之后队形还得自动恢复。项目组直接用人工势场算法把这套跑通了，整个实现放在Matlab里做，过程不算复杂，但里面有几个值得掰开揉碎讲清楚的细节。这篇博文就当这份Matlab代码的导读加复盘，适合正在做编队控制、机器人避障、或者想用势场法但不知道怎么处理避险恢复的同学参考。

整个方案的核心思路不绕弯子：领航者负责沿宏观路径飞行，追随者各自被一个“虚拟的期望编队位置”牵着走，同时被障碍物和邻居无人机推开。编队保持靠引力，紧急避险靠斥力，避险结束后的队形恢复靠状态机切换和渐进式参数回归。这个思路听着简单，实际操作中涉及坐标变换、势场系数分配、避险状态切换阈值、恢复过渡策略一大堆细节，任何一个环节没处理好，仿真画面就会变成四架无人机原地抽搐或者四散逃窜。

1. 领航者带着四架飞机跑正方形，需求到底长什么样

1.1 任务场景拆解

这个Demo要解决的问题可以拆成四句话：

1. 领航者从起点出发，沿一条三维路径飞向目标点。
2. 四架追随者以正方形四个顶点的位置分布，跟着领航者平移并转向。
3. 飞行路线上随机布置多个球状障碍物，所有无人机必须自动避开，但不能把编队彻底甩掉。
4. 整个系统在Matlab里仿真，需要输出三维轨迹、编队状态动画和队形误差曲线。

最经典的配置是正方形边长8米，四架追随者分别位于正方形四个顶点，领航者位于正方形中心正上方3米处。这样做有实际的工程考量：领航者和追随者高度错开后，能减少旋翼下洗气流对编队姿态的干扰，而且在三维可视化里更容易看清楚机群的空间结构。如果你只用2D平面上画一个四机正方形，领航者跟四个顶点重叠在一个平面里，视角稍微转一下就很难分辨谁是谁。

1.2 为什么选人工势场，没有上全局规划器或模型预测控制

做编队控制，解决方案其实不少，但每个方案都有自己的脾气。全局路径规划器（A*、RRT这类）擅长离线规划出一条从起点到终点的无碰撞路径，但遇到动态障碍物就得重新规划，而且要为每一架无人机单独规划，四架追随者加一架领航者，规划次数和通信开销很快就上去了。

模型预测控制（MPC）性能上限很高，可以把编队约束、避障约束、速度约束全部塞进一个优化问题里，但调参周期长，对仿真资源的要求也比较高。对于一个以验证编队控制逻辑为主要目标的仿真Demo来说，这套东西显然有点重。

人工势场法的优势在于它天生就是“反应式”的。每架无人机的控制量只依赖局部信息——自己当前位置、领航者位置、邻居位置、附近障碍物位置，计算量极小，代码实现也不过几十行。更关键的是，编队控制里那个“期望队形位置”天然就是一个虚拟引力点，跟势场法的框架完美契合。把编队保持和避障统一到一个合力模型里，逻辑上顺理成章。

当然，人工势场法局部极小值的问题一直被人诟病，这个我在后面专门的踩坑章节里详细说。现阶段知道它有这个毛病，然后在仿真里通过状态机和逃逸策略兜底就够了。

1.3 领航者-追随者模式下的角色划分

在领航者-追随者架构里，领航者的职责是决定整个编队的宏观路径，追随者的职责是把自己维持在领航者周围某个确定的相对位置上。这个“相对位置”不是死的，它要随着领航者的位置和航向实时变化。

我用一个简单的力模型来区别两类角色：

• 领航者只受两个力：目标点引力和障碍物斥力。目标点引力把它拉向最终目的地，斥力让它绕开障碍物。
• 追随者一共受三个力：期望队形位置引力、障碍物斥力、邻居无人机互斥力。期望队形位置引力让它始终有回到标准队形位置的趋势，斥力让它躲开障碍物和其他无人机。

这三股力量合成一个合力，形成每架无人机每个仿真周期的速度增量。跑出来的直观效果是：正常飞行时，四架追随者稳稳趴在正方形顶点上；遇到障碍物时，谁离得近谁先往外撇，旁边的无人机也给让出空间；绕过障碍后，期望队形位置再次主导，队伍自动收拢回正方形。

2. 三维正方形编队的数学模型与势场设计

2.1 编队坐标系与顶点偏移

编队最核心的数据结构是“期望队形位置的偏移量”。在正方形编队中，我先建立领航者机体坐标系：原点在领航者当前位置，X轴指向机头方向，Y轴指向右翼，Z轴指向下。正方形的四个顶点在机体坐标系里的偏移量定义为：

bias_1 = [ 4, 4, 3] % 右前下顶点 bias_2 = [-4, 4, 3] % 左前下顶点 bias_3 = [-4, -4, 3] % 左后下顶点 bias_4 = [ 4, -4, 3] % 右后下顶点

这里Z轴偏移取3，表示追随者整体比领航者低3米。正方形边长为8米，对角线到中心距离为4米。实际飞行中，追随者的期望位置不是“领航者位置加偏移量”这么简单，还要把机体坐标系的偏移旋转到世界坐标系。公式如下：

p_i_des = p_leader + R(yaw_leader) * bias_i

其中R是绕世界坐标系Z轴的旋转矩阵：

R = [cos(yaw) -sin(yaw) 0 sin(yaw) cos(yaw) 0 0 0 1]

为什么要做这一步旋转，我试过不加旋转矩阵直接加偏移的方案。领航者一旦转弯，正方形的四个顶点还保持原来的世界方向，整个编队就像一扇门被硬生生拧开了，队形瞬间变形。加上旋转矩阵后，四个顶点跟着领航者机头方向同步旋转，编队外形在转弯时才不会散。

2.2 引力场、斥力场与合力

人工势场法里的“力”其实都是从势函数求梯度得来的。我做了一个工程上的简化，直接用线性弹簧模型替代严格的二次势场，效果差别不大，代码更好写。摘抄核心公式：

编队引力

F_att_formation = k_att_formation * (p_i_des - p_i)

这个式子可以理解为：每架追随者和它对应的期望位置之间拴了一根虚拟弹簧，偏离越远，拉力越大，方向指向期望位置。

障碍物斥力

经典斥力公式是：

U_rep = 0.5 * k_rep_obs * (1/rho - 1/rho_0)^2, rho < rho_0 F_rep_obs = k_rep_obs * (1/rho - 1/rho_0) / (rho^2) * (p_i - p_obs) / rho

其中rho是无人机到障碍物中心的距离，rho_0是斥力影响半径。当rho大于等于rho_0时，斥力为零。这个公式的特点是：离障碍物越近，斥力越趋近无穷大，理论上无人机永远不会真正撞进去，前提是数值积分别出差错。

无人机互斥力

机间互斥力和障碍物斥力用的是同一个斥力模型，只是影响半径rho_0_uv取得比较小。正方形编队正常飞行时，相邻机间距有8米，互斥力完全不触发；避险散开时，机间距压缩到两三米，互斥力才会介入，防止两架无人机在躲避同一个障碍物时相互挤压。

最终控制量是这些力的线性叠加：

F_i = F_att_formation_i + F_rep_obs_i + F_rep_uv_i

2.3 三维势场计算要额外注意什么

从2D升级到3D，不是简单把二维向量换成三维向量就完事了，有三个方面必须额外处理。

第一，欧氏距离必须用三维范数。所有距离计算包括无人机到障碍物、无人机到期望位置、无人机到邻居，全部用三项平方和的平方根，不能只算XY平面距离再忽略高度差。尤其是在障碍物球体上方绕行时，高度方向的斥力分量必须参与计算，否则无人机会从障碍物顶部擦过。

第二，速度必须限幅。势场法算出来的是“力”，不是“位置”。在仿真中，我把它当成加速度增量来用，乘以仿真步长加进速度。如果某架无人机恰好陷入一个局部势场极值附近，合力可能会因为斥力项的突变而变得非常大，然后无人机在一步之内平移好几米，直接穿模飞过障碍球。所以每步更新完速度后，我都会检查一下速度模长，超过上限就等比截断。

第三，障碍物半径要留安全余量。斥力模型里说的rho是到球心距离，所以障碍物的真实半径R_obs要算进影响半径里。通常的做法是让rho_0 = R_obs + safety_margin，其中safety_margin设为无人机安全飞行半径，我取2米。这样即使斥力计算以球心为准，无人机也不会真的贴到障碍物表面。

3. 紧急避险：状态切换与队形恢复策略

3.1 检测与状态机

编队控制在正常状态下当然怎么调都顺，真正考验控制逻辑的是“紧急避险”这一下子。最开始的版本我搞得很简单——所有力全程按固定权重算，障碍物斥力始终开着。结果发现一旦某个障碍物恰好位于领航者路径与追随者期望位置之间，斥力会与编队引力反复拉扯，无人机在障碍物边界上来回穿行，机群整个变成一个在跳舞的爆米花队伍。

后来我改成带滞回的状态机。三个状态如下：

这里的关键是滞回设计：进入避障状态的阈值是8米，退出避障状态的阈值是10米，中间有个2米的缓冲区。如果进出都用同一个阈值，无人机会在高频抖动中反复切换状态，队形永远稳定不下来。加了滞回之后，至少要飞出障碍影响区相当距离才会触发恢复流程，过渡过程一下子干净了。

整个状态机的代码逻辑也很简单，就是一个基于最小距离的if-else链，每个仿真步都执行一次。判断距离时，领航者和四架追随者都要计算，只要有任意一架无人机接近障碍物，整个编队就同时切换到避险状态，由领航者和各追随者根据自身受力情况决定往哪个方向闪。这种“全局状态、局部动作”的策略，比每一架无人机独立判据要稳定得多，不会出现领航者还在没事人一样继续往前飞、追随者已经吓得四处乱窜的情况。

3.2 恢复队形为什么不能直接拉回

避险结束后把追随者拉回标准编队位置，这个问题乍一看很简单——把编队引力系数从0.4调回1.0不就行了？我一开始真就是这么干的，结果跑出来的画面很尴尬：避险期间四架无人机为了躲障碍物，有的绕左边的目标位置，有的绕右边，位置偏移达到了五米以上。恢复信号一触发，每架无人机的编队引力按1.0全量算出来，指向期望位置，速度直接飙升到上限，四架飞机像被磁铁吸住一样往队形位置猛冲。其中有几架的恢复路径恰好穿过障碍物附近，又一次触发了避障斥力，于是队伍陷入“冲回去——被弹开——再冲回去”的死循环。

后来我改成两步恢复策略。

第一步是“临时汇合”。避险结束判定之后，我不让无人机直接飞回标准正方形顶点，而是先在领航者后方两三米处的某个安全点汇合。这个汇合点临时充当所有追随者的期望位置，它的坐标离障碍物有一定距离，避障条件安全。

第二步才是“标准队形恢复”。等四架无人机基本到达汇合点附近，再让期望位置平滑切换到标准正方形顶点。切换过程用斜坡函数控制编队引力系数从0.4到1.0线性增长，时长约1.5秒。这样做的好处是，每架无人机的速度在这个阶段已经降下来了，引力再一点一点加回去，就不会出现猛拉猛拽的过冲。

最后还有一个恢复期的细节：避险状态退出后，障碍物斥力系数并没有立刻从3.0掉回1.0，而是先维持在1.5，等标准队形完全恢复后再回到1.0。这批斥力始终在，无人机就不会在恢复路径上再次贴近障碍物。

4. 仿真场景搭建与调参过程

4.1 模拟任务场景与代码参数初始化

搭建场景时，我设置领航者从世界坐标原点附近起飞，目标点放在(60, 60, 20)，任务跑道水平方向60米，高度上升10米。途中布置了三个球形障碍物，用来模拟楼宇、山包或者无人机飞行表演中的禁飞区域：

obs_1: 球心坐标 (25, 18, 8) 半径 5m obs_2: 球心坐标 (40, 42, 12) 半径 6m obs_3: 球心坐标 (50, 55, 15) 半径 4m

初始状态中，领航者在(0, 0, 10)，四架追随者按照正方形布局分布在下方：领航者基本在起点上方，四条飞机在下方排开，起降阶段不会重叠。仿真时长设为40秒，步长0.1秒。对于这个量级的仿真，400步跑完也就眨眼的工夫，可视化动画倒是要花一点时间渲染。

4.2 一组能直接跑的初始参数

我把调好的参数整理成一张表，方便直接复制到代码里跑第一版：

4.3 调参顺序

参数不要一口气全上，不然出了问题根本不知道是哪组参数引起的。我强烈建议按下面这个顺序推进：

第一步，关掉所有障碍物和追随者，只保留领航者飞向目标点的引力模型。确认领航者的轨迹是一条平滑的曲线，没有抖动和偏航。

第二步，加入追随者的编队引力，关掉机间斥力和障碍物斥力。观察四架追随者能不能跟上领航者，在转弯时正方形是否变形，记录最大队形误差。

第三步，打开机间互斥力。正常编队飞行时，机间距8米，互斥力不应该触发。如果触发了，说明某个时刻队形误差已经超过安全范围，先回头看领航者路径和编队参数。

第四步，加入障碍物斥力。先只让领航者避障，确认它能绕开所有球体，再开放追随者的障碍物斥力。

第五步，调避险状态机的系数和滞回阈值。这步要结合可视化动画一遍遍跑，看队伍避障是否从容、恢复是否平滑。

我调参时发现一个比较反直觉的现象：编队引力不要太大。很多人一看到队形误差想把k_att_formation调大，结果编队是紧了，但遇到任何干扰，整支队伍会像一块僵硬的铁板一样剧烈反应，恢复时过冲特别明显。把引力调小一点，允许一定的跟随滞后，编队的整体韧性反而更好。

4.4 参数过冲的典型表现与排查

调参过程中常见三种故障画面，我总结成表格方便对照排查：

5. Matlab实现与核心代码导读

5.1 代码文件结构

这份仿真代码我按模块拆成五个文件，每个文件职责单一，调试的时候能少走很多弯路：

setup_formation.m 参数初始化、队形偏移、障碍物定义 formation_force.m 核心力计算：编队引力+障碍物斥力+机间互斥 avoid_state.m 避险状态机：输入最小距离，输出状态系数 main_simulate.m 主循环：状态更新、速度限幅、可视化调度 draw_frame.m 帧绘制：无人机位置、正方形面片、历史轨迹

5.2 核心函数代码：formation_force.m

这个函数是整个仿真最核心的部分，每架追随者的所有力和编队期望位置都在这里算出来。简化后代码如下：

function F = formation_force_matrix(P, leader_pos, yawL, obstacles, mode, p) % P: 四架追随者当前位置, 4x3矩阵 % mode: 结构体, 包含att_scale与rep_scale, 由状态机输出 n = size(P, 1); F = zeros(n, 3); % 领航者机体坐标到世界坐标的旋转矩阵 R = [cos(yawL), -sin(yawL), 0; sin(yawL), cos(yawL), 0; 0, 0, 1]; ka = p.k_att_formation * mode.att_scale; kp = p.k_rep_obs * mode.rep_scale; % 每个追随者的编队引力 for i = 1:n des = leader_pos(:)' + (R * p.bias(:, i)).'; F(i, :) = F(i, :) + ka * (des - P(i, :)); end % 障碍物斥力 for i = 1:n for j = 1:size(obstacles, 1) diff = P(i, :) - obstacles(j, 1:3); d = norm(diff); if (d < p.rho0_obs && d > 1e-6) rep = kp * (1/d - 1/p.rho0_obs) / d^2; F(i, :) = F(i, :) + rep * diff / d; end end end % 无人机器间斥力 for i = 1:n for j = i+1:n diff = P(i, :) - P(j, :); d = norm(diff); if (d < p.rho0_uv && d > 1e-6) rep = p.k_rep_uv * (1/d - 1/p.rho0_uv) / d^2; F(i, :) = F(i, :) + rep * diff / d; F(j, :) = F(j, :) - rep * diff / d; end end end end

这里有个容易忽略的点：障碍物数据格式里除了球心坐标和半径，还要在计算rho0_obs时把半径加进去。我习惯把障碍物存成[x, y, z, R]四列，计算rho0_obs时直接读第四列，这样换场景只需要改数据，不需要改代码。

速度更新主循环放在main_simulate.m里，核心逻辑如下：

for t = 1:p.T_steps obs = p.obstacles; % 静态障碍物, 动态场景的话在此更新 % 计算领航者与所有追随者的最近障碍距离 all_pos = [P_L; P]; dmin = min_dis_to_obstacles(all_pos, obs); mode = update_mode(mode, dmin); % 追随者状态更新 F_f = formation_force_matrix(P, P_L, yawL, obs, mode, p); V = V + (F_f - p.damping * V) * p.dt; speed = max(sqrt(sum(V.^2, 2)), 1e-4); V = V .* min(1, mode.vmax_scale * p.vmax ./ speed); P = P + V * p.dt; % 领航者状态更新 F_L = leader_force(P_L, p.goal, obs, p); V_L = V_L + (F_L - p.damping * V_L) * p.dt; vL = norm(V_L); if vL > p.vmax_leader V_L = V_L / vL * p.vmax_leader; end P_L = P_L + V_L * p.dt; yawL = atan2(V_L(2), V_L(1)); draw_frame(P_L, P, t, p); end

这个主循环用的是最基础的欧拉法数值积分。可能有人觉得一阶欧拉精度不够，想上四阶龙格库塔。我在这个仿真里实测下来，势场力本身就是非连续的（斥力在影响边界突然到零），更高阶积分器带来的精度优势微乎其微，反而把代码复杂度抬高不少。只要你把步长控制在0.05到0.1秒，加上速度限幅，欧拉法完全够用。

5.3 可视化与结果判读

可视化我是这么写的：

• 用plot3给每架无人机画历史轨迹，领航者用红线，四架追随者分别用四种颜色。
• 用patch把四个追随者当前位置连成一个半透明的正方形面片，这样队形是否保持、何时变形，一眼就能看出来。
• 用scatter3画障碍物球体表面采样点，或者直接用sphere生成网格再缩放位移。
• 用quiver3画合力向量，平时调试开着，跑正式实验时关掉，否则画面太乱。

仿真跑完，重点看两个指标。第一个是编队误差曲线，即每架追随者理想位置与实际位置的距离。正常飞行时应该稳定在0.5米以内，避险阶段浮动到几米是正常的，关键是恢复阶段能不能回到0.5米以内。第二个是轨迹平滑度，如果某架飞机的轨迹出现V字急折，说明阻尼不够或者参数过冲，要去调damping和速度上限。

6. 折腾两周后总结出的几个坑

6.1 局部极小值：无人机卡在障碍和目标中间

人工势场法最经典的毛病就是局部极小值。我实际遇到的场景是这样：领航者的目标点在障碍球正后方，领航者飞近障碍球后，目标引力方向和斥力方向恰好接近共线反向，合力变得极小，领航者就在障碍物前方两三米处悬停，既不前进也不绕开。四架追随者一看领航者停了，自己也跟着停在半空，整个编队原地僵住。

解决思路不复杂，加一个逃逸扰动。具体做法是：实时监测领航者速度，如果连续20个仿真步速度都小于0.05米每秒，同时距离目标还超过某个阈值，就临时给领航者施加一个垂直方向的恒定推力，持续1秒，破坏受力平衡。实测下来，这个扰动能让领航者从局部极小值里挣脱出来，代价是有时飞行轨迹会多出一小段锯齿形迂回。

想要更精致一点，可以引入“中间目标点”策略——在障碍物侧向几米处临时插入一个虚拟目标，引导无人机绕过障碍后再切回原目标。这个方案更可控，但我偷懒用了扰动法，效果也够。

6.2 避险时队伍被扯成天女散花

之前提到避险期间我把障碍物斥力系数提到了原来的三倍，同时把编队引力砍到0.4倍。这个组合在大多数场景下是有效的，但有一次场景里两个障碍物距离很近，四架追随者同时收到强斥力，每架飞机都朝不同方向猛闪，加上机间互斥力也在起效，队伍一瞬间从紧凑的正方形变成一条长蛇阵，个别追随者的速度甚至冲到了限幅值还拉不住。

排查之后发现问题是：k_rep_obs的3倍系数偏大，而且k_rep_uv在避险时仍然是5.0。当追随者已经四散开来，它们彼此之间的距离迅速变小，机间斥力开始火上浇油，把本已向外飞的无人机推得更远。解决办法是避险状态下把机间互斥系数也降一半，毕竟避险时大家的目标都是“离开障碍物”，方向总体一致，不需要那么强的互斥保护。调整后，四散程度小了很多，恢复时间缩短了快一半。

6.3 高频振荡与穿模

最后一个坑是数值稳定性。为了把画面帧率拉高，我把仿真步长从0.1秒调大到0.2秒，结果无人机在接近障碍物时开始高频抖动，位置曲线呈现出细密的锯齿形。原因是斥力函数在接近影响半径边界时对距离变化非常敏感，0.2秒的积分步长下，无人机一步跨过的空间区域已经跨越了斥力的有效范围，导致每一步的受力方向剧烈变化。

同时速度限幅如果设成跟随者最大值5米每秒、步长0.2秒，一步最大能移动1米，而障碍物影响半径可能就三四米，完全可能一步从斥力区外直接跃迁到斥力区中心甚至穿越过去。所以数值稳定性上必须两条腿走路：步长尽量小于0.1秒，同时速度上限控制在每步位移不超过斥力影响范围的三分之一。卡完之后，画面立竿见影地平顺了。

这套Demo跑通之后，我最大的感受是：人工势场算法在编队控制里真正的价值不在于“完美规划轨迹”，而在于它把编队保持和紧急避险统一成了一个极简的力合成框架，状态机在顶层做协调，底层完全靠加减乘除就能出效果。希望这篇笔记里关于数学模型、避险恢复策略和调参顺序的经验，能让你少走点弯路。代码目录里那份完整的Matlab源码，配合这篇文字逐个函数对照着看，基本半天就能把整套逻辑吃透。