基于人工势场法的无人机集群三维避障Matlab仿真实现与调参

做无人机集群仿真，最难的不是让某一架飞机飞起来，而是让一群飞机在同一个三维空间里既不撞障碍物、又不互相撞，还能整体到达目标区域。我最早接触这个课题时，第一反应是找现成的开源方案，结果翻了一圈，工业级框架太重、学术代码太散，最后还是老老实实回到Matlab把整个逻辑手写了一遍。这篇博文就完整复盘一下我的实现思路和踩坑过程，从问题建模到算法选型，从核心代码到调参经验，尽量一次讲透。

1. 三维集群避障到底难在哪：先把问题边界画清楚

1.1 集群避碰和静态避障是两个不同的问题，别混在一起写

很多初学者一上来就把所有逻辑塞进一个函数里，结果仿真一跑就乱套，根本分不清哪架飞机是因为什么原因机动。我的建议是先在概念上把问题拆成两层。

第一层是集群内部的碰撞避免，也就是飞机和飞机之间要保持安全间距。这本质上是一个分布式约束问题，每架飞机不仅要知道自己在哪，还要实时感知邻居的位置和速度，然后根据相对几何关系调整自身运动。三维环境下，这个调整不只是左右躲，还要考虑上下躲，自由度多了，但判断逻辑也更复杂。

第二层是静态环境障碍物的避让。障碍物位置固定、形状已知，比如大楼、山体、塔架，可以预先建模成球体、长方体或者凸多面体。静态避障的特点是环境信息可以离线准备好，在线计算时只需要判断当前位置与障碍物的几何关系。

把这两层分开的好处是算法维护起来非常清晰：改静障逻辑不影响机群内部的避碰策略，反之亦然。我做的时候直接把每个无人机的受力分成了三股：目标引力、障碍物斥力、邻居斥力，最后叠加在一起去驱动运动。这个思路看起来简单，但实测下来非常稳。

1.2 三维环境和二维仿真在本质上的区别

刚上手时我犯过一个认知错误：觉得三维只是多了一个z轴的坐标，把二维代码改改就行。实际上差别很大。

二维环境下，飞机的朝向可以用一个偏航角表示，避障方向选择通常只有左右两个半平面。到了三维，朝向需要偏航角加俯仰角，避障方向变成了一个球面上的连续区域，算法要决定的是"往哪个空间方向偏"，而不是"往左还是往右"。这一点在人工势场法里体现得尤其明显，斥力方向从二维的平面向量变成了三维向量，计算量没多多少，但直观调试难度上升了一个档次。

另外，三维环境下的可视化调试也比二维麻烦。二维可以直接俯视看到所有轨迹，三维你得不断旋转视角，否则前机遮挡后机、轨迹交错缠绕的问题根本看不出来。这也意味着仿真代码里必须预留良好的轨迹记录和视角控制接口，否则后期的验证工作会非常痛苦。

1.3 先定死坐标系和单位体系

这是我强烈建议所有做无人机仿真的朋友第一件要做的事。Matlab本身没有强制单位，但混用单位会让所有物理公式失真。

我统一使用"米"作为长度单位、"秒"作为时间单位、速度单位是米/秒。无人机的位置用三维坐标向量表示，速度是三维向量，加速度由合力除以质量得到。质量我统一取1（归一化），这样合力数值上直接等于加速度，省去一堆换算。本质上是因为我关注的是运动学层面的避障效果，不关心动力学层面的推力响应，这样的简化是合理的。

坐标系我用的是右手系：x指向东、y指向北、z指向上方。所有障碍物位置、目标点、初始位置都按这个坐标系定义。这样做的目的是为了后续扩展真实地图数据时不需要做坐标变换。

2. 算法选型思路：为什么我选用人工势场法打底

2.1 常见避障算法速览与对比

在动手写代码之前，我列了一张表对比主流算法，反复权衡了很久。

考虑我的目标：在三维环境中快速迭代验证机群避障逻辑，并且把计算量控制在普通家用电脑能流畅运行的范围，APF人工势场法是最合适的。它天然支持分布式计算——每架飞机只需要知道自己的位置、目标点、障碍物信息以及邻居状态，不需要全局规划器统一调度。这一点对集群规模扩展很重要，从5架加到20架，代码框架几乎不用改。

2.2 人工势场法的核心表达式与三维化改造

人工势场法的经典形式，目标引力场定义如下，U_att = (1/2) * k_att * ||p_goal - p||^2，对应的引力是负梯度方向，F_att = k_att * (p_goal - p)。

障碍物斥力场我选用的是具有"影响距离"截断的形式：当无人机与障碍物表面距离大于rho_0时，斥力为零；只有进入影响范围才开始作用。三维球形障碍物的斥力表达式为：

F_rep = k_rep * (1/rho - 1/rho_0) * (1/rho^2) * n_rep

其中rho是无人机到障碍物表面的距离，n_rep是从障碍物指向无人机的单位向量。这个公式经典的来源是Khatib在1986年提出的势场法，虽然年头久，但到今天依然好用，关键是它天然支持三维向量运算，改造起来毫无成本。

邻居间的互斥力我用了一种更温和的形式：不采用障碍物那种截断式斥力，而是设定一个安全距离d_min，当两机距离小于d_min时施加线性斥力，大于d_min时完全不作用。这样机群内部不会因为斥力场重叠而剧烈抖动。

我还额外加了一个边界约束：如果无人机飞出了设定空域，给它一个指向空域内部的强制力。这个在仿真里很有用，否则有些调参失败的场景飞机一骑绝尘飞上天，轨迹图根本没法看。

2.3 为什么速度层面的控制比位置层面的控制更实用

另一个关键设计是，我选择在速度层面做控制，而不是直接修改位置。具体来说，每架无人机维护一个速度向量，每帧根据加速度更新速度，再根据速度更新位置。公式就是最基础的质点运动学：v_{k+1} = v_k + a * dt，p_{k+1} = p_k + v_{k+1} * dt。

这个做法的好处特别实际：能够自然地限制最大飞行速度。无人机的物理特性决定了速度不能无限大，如果直接在位置上施加偏移量，可能出现跳变式移动，这在仿真里虽然能跑通，但严重失真。我在每帧更新后加了一个限速操作，速度模长超过最大值时归一化到最大速度。这样既保证了运动看起来真实，也避免了大步长下把碰撞判定穿过去。

还有一点，速度层面的控制让后续扩展动态障碍物避让顺手很多。动态障碍物的核心变量就是速度和位置，你的控制量和它处于同一个空间，判断会更直观。

3. Matlab仿真的模块化落地：状态更新、力场计算与碰撞判定

3.1 整体代码框架：一个主循环、三个核心函数、一个可视化模块

我代码的整体结构如下：

%% 无人机集群三维避障仿真主脚本 % 文件: SwarmAvoidance3D_Main.m clear; clc; close all; %% 1. 参数配置 n = 6; % 无人机数量 dt = 0.1; % 仿真步长，单位秒 simSteps = 1000; % 总仿真步数 maxSpeed = 8; % 最大飞行速度，m/s kAtt = 1.5; % 目标引力增益 kRepObs = 0.8; % 障碍物斥力增益 kRepNei = 1.2; % 邻居斥力增益 safeDistNei = 3.0; % 机间安全距离，m inflDistObs = 6.0; % 障碍物影响半径，m % 目标位置（所有无人机相同目标） goal = [60, 50, 25]; % 静态障碍物: [x, y, z, radius, inflRadius] obstacles = [ 20, 15, 8, 3, 6; 35, 30, 15, 4, 7; 50, 20, 12, 3, 6; ]; % 无人机初始位置（错开，避免起点处拥挤） initPos = [ 0, 0, 0; 2, 1, 1; 1, 3, 0; 3, 2, 2; 0, 4, 1; 4, 0, 2 ]; pos = initPos; vel = zeros(n, 3); %% 2. 主仿真循环 traj = zeros(simSteps, n, 3); collisionFlag = zeros(simSteps, 1); for t = 1:simSteps totalForce = zeros(n, 3); % 目标引力 totalForce = totalForce + calcAttraction(pos, goal, kAtt); % 静态障碍物斥力 totalForce = totalForce + calcObsRepulsion(pos, obstacles, kRepObs); % 邻居互斥力 totalForce = totalForce + calcNeighborRepulsion(pos, kRepNei, safeDistNei); % 更新速度与位置 vel = vel + totalForce * dt; % 限速 speed = vecnorm(vel, 2, 2); overLimit = speed > maxSpeed; if any(overLimit) vel(overLimit, :) = vel(overLimit, :) ./ speed(overLimit, 1) * maxSpeed; end pos = pos + vel * dt; % 碰撞检测：机间和障碍物 collisionFlag(t) = detectCollision(pos, obstacles, safeDistNei); traj(t, :, :) = pos; end %% 3. 可视化 visualizeTrajectory(traj, obstacles, goal, initPos);

这个主循环的写法比较接近我实际使用的版本，去掉了大量冗余注释，保留了核心逻辑。注意几个细节：所有力用totalForce累加，这样架构上便于调试单一力项；限速操作放在速度更新之后、位置更新之前；碰撞检测独立成一个函数，方便统计安全指标。

3.2 三个力场的Matlab实现要点

第一个是目标引力函数，实现非常简单：

function F = calcAttraction(pos, goal, kAtt) diff = goal - pos; F = kAtt * diff; end

这里有个细节：目标引力的大小不随时间变化，始终正比于距离。距离远时引力大，靠近目标时引力自然减小。很多教程会在公式里加个距离上限截断，但我实测发现，对于机群编队到达的场景，不截断反而更平稳，因为当飞机接近目标时，速度会自然降下来，不会产生来回震荡。

第二个是障碍物斥力函数，这里要注意三维斥力方向计算：

function F = calcObsRepulsion(pos, obstacles, kRep) n = size(pos, 1); F = zeros(n, 3); for i = 1:n p = pos(i, :); for j = 1:size(obstacles, 1) obsPos = obstacles(j, 1:3); obsRad = obstacles(j, 4); inflRad = obstacles(j, 5); dirVec = p - obsPos; dist = norm(dirVec); rho = dist - obsRad; % 到球面的距离 if rho < inflRad && rho > 1e-6 % 斥力大小随距离减小而增大 magnitude = kRep * (1/rho - 1/inflRad) / (rho^2); F(i, :) = F(i, :) + dirVec / dist * magnitude; elseif rho <= 1e-6 % 极端情况下无人机已经穿进障碍物 % 用大力度直接把飞机推出去 F(i, :) = F(i, :) + dirVec / dist * kRep * 10; end end end end

第三个是邻居斥力函数，我优化过的一种写法：

function F = calcNeighborRepulsion(pos, kRepNei, safeDist) n = size(pos, 1); F = zeros(n, 3); for i = 1:n for j = i+1:n diff = pos(i, :) - pos(j, :); dist = norm(diff); if dist < safeDist && dist > 1e-6 forceMag = kRepNei * (safeDist - dist) / safeDist; F(i, :) = F(i, :) + diff / dist * forceMag; F(j, :) = F(j, :) - diff / dist * forceMag; end end end end

注意，邻居斥力是成对出现的。i对j的力和j对i的力大小相等方向相反，这个写法天然满足牛顿第三定律，机群内部不会因为斥力计算产生"动量不守恒"的异常运动。

3.3 静态障碍物建模：为什么用球体而不是长方体

我做障碍物建模时统一用的球体，原因很务实：球体的表面法向量方向计算最简单，只有"球心指向当前点"这一个方向，任何三维位置的斥力方向都能唯一确定。而长方体在边和角附近的法向量不连续，斥力方向容易发生突变，导致无人机出现抖动或卡在棱边。

真实场景中很多障碍物也确实可以用多个球体叠加来近似，尤其是一些大体量建筑、山体，用七八个球体排布就能获得不错的碰撞近似效果。如果你需要对更复杂的形状做避让，建议继续用凸多面体加分离轴判定，但Matlab实现成本会上一个台阶，先用球体把整个仿真链路跑通更重要。

3.4 碰撞检测的判定逻辑

碰撞检测函数我用了两层逻辑：机间碰撞和障碍物碰撞。

function isCollision = detectCollision(pos, obstacles, safeDist) n = size(pos, 1); isCollision = false; % 机间碰撞：距离小于安全距离 for i = 1:n for j = i+1:n if norm(pos(i,:) - pos(j,:)) < safeDist * 0.8 isCollision = true; return; end end end % 障碍物碰撞：距离小于障碍物半径（可加安全余量） for i = 1:n for j = 1:size(obstacles, 1) if norm(pos(i,:) - obstacles(j,1:3)) < obstacles(j,4) + 0.5 isCollision = true; return; end end end end

这里安全距离乘了0.8作为碰撞判定阈值，目的是给避碰控制留出余量。也就是说，控制层面要求保持3米间距，实际判定时距离小于2.4米才算碰撞。这样即便控制有一点超调，也不会立刻判成碰撞，仿真结果更符合"安全冗余"的工程习惯。

4. 那些一跑就翻车的调参坑：从局部极小值到碰撞半径

4.1 局部极小值问题：对称场景下的经典卡死

人工势场法最臭名昭著的问题就是局部极小值。我在第一版仿真里就遇到了这样的场景：两架无人机、一个球状障碍物、目标点在障碍物正后方，当无人机正对着障碍物飞过去时，引力方向和斥力方向恰好完全相反，合力趋近于零，飞机就悬停在障碍物前方不动了。

这个问题的物理本质是，势场函数在某个位置出现了鞍点或者极小值点，梯度为零，算法无法区分"到了目标"和"被困住了"。解决思路大致有四类：

1. 施加随机扰动，当检测到飞机速度连续N步接近零且没到目标时，加一个随机方向的小力
2. 改变势场函数形式，在斥力中加入切向分量，让合力总是有侧向偏移
3. 引入子目标点，在障碍物侧面预设一个临时航点，引导飞机绕行
4. 把斥力方向修正为垂直于引力方向的分量，牺牲一点最优性换取避障的必然性

我实际采用的是第4种思路的变体：当障碍物斥力与目标引力的夹角接近180度时，我把斥力向量投影到垂直于引力的平面上再施加。这样飞机不会完全悬停，而是侧向漂移，等到偏离障碍物正面之后，引力自然会把飞机拉向正确方向。

4.2 引力增益和斥力增益的比例必须要配对调试

这个比例关系直接决定飞行稳定性。我一开始用的kAtt=2、kRepObs=1，整个仿真跑下来飞机像喝醉了一样来回晃，轨迹绕得乱七八糟。后来分析受力才发现规律：

如果引力增益相对斥力增益过大，飞机会直愣愣地冲向障碍物，直到距离很近时斥力才反转方向，导致飞机划出一道锐利的急转轨迹，轨迹不连续且姿态变化剧烈。如果斥力增益过大，飞机会在障碍物很远处就开始转向，绕的路径特别宽，甚至出现机群被斥力"弹开"后很难重新聚拢。

一个相对靠谱的初始配比是引力增益与斥力增益的比值在1.2到1.8之间，同时保证斥力影响半径内最大斥力大于当时的引力。这个最大斥力估算公式很简单：F_rep_max = kRepObs * (1/rho_min - 1/inflRad) / rho_min^2，其中rho_min是允许的最小表面距离，一般取0.5米。你用这个反推一下，就能估算出当前参数下飞机是否会撞上障碍物。

4.3 仿真步长的选择：一个容易被忽略的隐性问题

步长dt是决定仿真可信度的隐形变量。dt太大，反应跟不上，力更新滞后严重，碰撞检测都可能"穿透"——飞机在一个步长内跨过了障碍物表面。dt太小，计算量大增，而这个增长在以无人机数量n的平方量级增长时非常明显。

我的实际经验是：最大速度8米/秒时，dt取0.05到0.1秒是合理的范围。这时候每个步长内飞机最多移动0.8米，远小于障碍物半径3米，碰撞穿透风险很小。如果你的场景里无人机速度更快，或者障碍物尺寸更小，务必相应缩小dt，保持"单步位移小于障碍物最小截面尺寸"这个基本原则。

还有一个细节是，仿真步长和控制更新频率要统一。如果你在代码里用了离散事件触发控制，最好先用固定步长跑通，不要一开始就尝试变步长求解器，否则受力时序错乱，排查效率极低。

4.4 邻居斥力阈值设置不当导致的机群抖振

邻居斥力如果设置过高，机群内部会出现持续抖振：两架飞机相距略小于安全距离，斥力把它们推开，之后因为没有耗散，惯性让它们继续飞远，接着又因为引力作用重新靠近，再一次触发斥力。如此往复，轨迹呈锯齿状，速度曲线强烈震荡。

我当时的解决方案有两个：一是给速度引入一个阻尼项，让飞机在没有外力时慢慢减速。我的更新式子改成了vel = vel * (1 - damping) + totalForce * dt，damping取0.1到0.3之间。这个改动虽然简单，但对整体稳定性贡献非常大，强烈建议加上。

二是把邻居斥力的作用范围设置得略大于安全距离，比如安全距离3米、作用范围4米。这样斥力是渐进的，不是到3米才突然爆发，飞机提前开始平滑减速，抖振明显缓解。

4.5 多机同时靠近冲突区域时的优先级问题

多架飞机同时冲向一个障碍物附近，其实还有一个隐蔽的问题：如果不是同一高度层飞行，只是水平投影重叠，实际三维距离还是够的。这种场景下，斥力反应正常，轨迹也合理。但如果多架飞机在三维空间里都挤到了一个窄通道，就会形成机群内部的级联冲突——飞机A为了避障往右偏，撞上了飞机B的控制意图，B为躲避A往左偏，又撞上了C。

这种级联效应在人工势场法框架下很难彻底根除，因为每架飞机的决策都是独立做出的。我的缓解手段是对邻居斥力加了权重因子：距离越近的邻居，斥力权重越大，并且让这个权重随距离指数增长。这样冲突最激烈的小范围内，飞机会优先处理最近的威胁，不至于同时被多股力量撕扯。虽然不能完全避免级联，但能把机群的震荡幅度控制在一个可接受的范围内。

5. 仿真结果的可视化分析与后续扩展方向

5.1 三维轨迹可视化：为什么旋转视角比静态图更有用

Matlab的plot3可以画出最基础的轨迹线，但如果只是把所有飞机的轨迹画在一个图里，一旦轨迹密集，前后遮挡严重，根本看不出谁在什么时候避了什么障碍物。

我更推荐的做法是分段着色：把轨迹按时间段分为多个段，每段用不同颜色的渐变显示，并且把障碍物的球面用surf函数绘制成半透明表面。这样旋转视角时可以很清楚地看到飞机是从障碍物的哪个方向绕过去的。另外，当前时刻的飞机位置用大号标记点显示，过往轨迹则用细线表示，层次感会清晰很多。

还有一个实用技巧：加一个跟随视角模式。选定一架无人机，让相机始终跟随它，可以观察到它视野内的障碍物和其他飞机的几何关系。这在调试局部极小值问题时有奇效，你能直观看到飞机为什么悬停——相机视角下的受力方向对称性非常明显。

5.2 量化评估指标：不要只"看"结果，要算数据

我的习惯是从第二轮仿真开始就逐项记录量化指标，不然换一组参数就忘了上一版效果如何。我常用的指标包括最小机间距离、最小障碍物距离、到达目标的时间、路径总长度、速度超限比例等。

用表格来追踪记录是一个好方法，比如：

通过这样的数据对比，参数优劣一目了然。我尤其关注最小障碍物距离，因为它直接反映安全冗余量。目标是最小距离远大于无人机物理半径加安全余量，同时到达时间又不至于过慢。这两个指标在本质上是矛盾的，最终参数选择是一个折中。

5.3 从人工势场法出发的四个扩展方向

如果你已经跑通了这套仿真，后续想往更实用的方向走，我认为有四个值得展开的方向。

第一是增加速度障碍法模块。APF的实时性好但避障路径不一定最优，VO/RVO在速度空间决策更"硬"，把两者结合可以做到既平滑又安全。我尝试过在APF框架下，对动态障碍物额外叠加一个RVO约束，效果比纯APF好很多。

第二是加入编队控制项。目前的逻辑是所有飞机直奔同一目标点，到达后会挤在一起。如果加入编队间距约束，比如让飞机保持V字队形、菱形队形，需要在力场中加入期望距离和期望方位角两个参考量，这会显著提升仿真的应用价值。

第三是做任意规模参数扫描。把核心参数kAtt、kRep、safeDist、dt作为扫描变量，批量跑场景，统计成功率和安全指标，可以得到一组参数的鲁棒性画像。这个过程在算法论文里几乎是必做的，提前准备好代码框架能省很多时间。

第四是往实机部署方向走。仿真里的质点运动学模型和真实飞机的动力学模型差别很大，真机还要考虑姿态响应、通信延迟、传感器噪声。建议逐步把模型从质点升级到带惯性的一阶延迟模型，控制周期也要和真实的飞控频率对齐。

5.4 我踩过的最蠢的一个坑

最后分享一个让我浪费了两天时间的坑。我在仿真中把障碍物坐标写错了，写成了[x, y, z, radius]的格式，但读取数据时用了[radius, x, y, z]的顺序，结果所有障碍物都偏移到了完全不对的位置，飞机频频撞上"隐形墙"。排查了很久才发现是索引顺序不一致。

所以从第一行代码开始就统一数据结构定义，并且把读取和使用的代码位置隔离开，数据定义集中放，使用逻辑集中放。这个代码规范上的小习惯，在后续扩展模型、增加障碍物时真的会省下大量排查时间。