★ FEATURED ARTICLE 做无人机集群仿真最难的不是让某一架飞机飞起来而是让一群飞机在同一个三维空间里既不撞障碍物、又不互相撞还能整体到达目标区域。我最早接触这个课题时第一反应是找现成的开源方案结果翻了一圈工业级框架太重、学术代码太散最后还是老老实实回到Matlab把整个逻辑手写了一遍。这篇博文就完整复盘一下我的实现思路和踩坑过程从问题建模到算法选型从核心代码到调参经验尽量一次讲透。1. 三维集群避障到底难在哪先把问题边界画清楚1.1 集群避碰和静态避障是两个不同的问题别混在一起写很多初学者一上来就把所有逻辑塞进一个函数里结果仿真一跑就乱套根本分不清哪架飞机是因为什么原因机动。我的建议是先在概念上把问题拆成两层。第一层是集群内部的碰撞避免也就是飞机和飞机之间要保持安全间距。这本质上是一个分布式约束问题每架飞机不仅要知道自己在哪还要实时感知邻居的位置和速度然后根据相对几何关系调整自身运动。三维环境下这个调整不只是左右躲还要考虑上下躲自由度多了但判断逻辑也更复杂。第二层是静态环境障碍物的避让。障碍物位置固定、形状已知比如大楼、山体、塔架可以预先建模成球体、长方体或者凸多面体。静态避障的特点是环境信息可以离线准备好在线计算时只需要判断当前位置与障碍物的几何关系。把这两层分开的好处是算法维护起来非常清晰改静障逻辑不影响机群内部的避碰策略反之亦然。我做的时候直接把每个无人机的受力分成了三股目标引力、障碍物斥力、邻居斥力最后叠加在一起去驱动运动。这个思路看起来简单但实测下来非常稳。1.2 三维环境和二维仿真在本质上的区别刚上手时我犯过一个认知错误觉得三维只是多了一个z轴的坐标把二维代码改改就行。实际上差别很大。二维环境下飞机的朝向可以用一个偏航角表示避障方向选择通常只有左右两个半平面。到了三维朝向需要偏航角加俯仰角避障方向变成了一个球面上的连续区域算法要决定的是往哪个空间方向偏而不是往左还是往右。这一点在人工势场法里体现得尤其明显斥力方向从二维的平面向量变成了三维向量计算量没多多少但直观调试难度上升了一个档次。另外三维环境下的可视化调试也比二维麻烦。二维可以直接俯视看到所有轨迹三维你得不断旋转视角否则前机遮挡后机、轨迹交错缠绕的问题根本看不出来。这也意味着仿真代码里必须预留良好的轨迹记录和视角控制接口否则后期的验证工作会非常痛苦。1.3 先定死坐标系和单位体系这是我强烈建议所有做无人机仿真的朋友第一件要做的事。Matlab本身没有强制单位但混用单位会让所有物理公式失真。我统一使用米作为长度单位、秒作为时间单位、速度单位是米/秒。无人机的位置用三维坐标向量表示速度是三维向量加速度由合力除以质量得到。质量我统一取1归一化这样合力数值上直接等于加速度省去一堆换算。本质上是因为我关注的是运动学层面的避障效果不关心动力学层面的推力响应这样的简化是合理的。坐标系我用的是右手系x指向东、y指向北、z指向上方。所有障碍物位置、目标点、初始位置都按这个坐标系定义。这样做的目的是为了后续扩展真实地图数据时不需要做坐标变换。2. 算法选型思路为什么我选用人工势场法打底2.1 常见避障算法速览与对比在动手写代码之前我列了一张表对比主流算法反复权衡了很久。算法基本思路优点缺点三维扩展难度人工势场法目标产生引力障碍物产生斥力原理简单、计算量小、实时性强局部极小值问题可能卡死低速度障碍法VO/RVO在速度空间排除碰撞速度区间避碰效果硬、适合动态障碍多机协作时决策耦合复杂中RRT/RRT*在状态空间随机采样建树概率完备、可处理复杂约束路径不平滑、实时性差中高模型预测控制MPC滚动优化未来一段时间的轨迹约束处理能力强、最优性好计算量巨大集群场景极慢高考虑我的目标在三维环境中快速迭代验证机群避障逻辑并且把计算量控制在普通家用电脑能流畅运行的范围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_{k1} v_k a * dtp_{k1} p_k v_{k1} * 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 i1: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 i1: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 局部极小值问题对称场景下的经典卡死人工势场法最臭名昭著的问题就是局部极小值。我在第一版仿真里就遇到了这样的场景两架无人机、一个球状障碍物、目标点在障碍物正后方当无人机正对着障碍物飞过去时引力方向和斥力方向恰好完全相反合力趋近于零飞机就悬停在障碍物前方不动了。这个问题的物理本质是势场函数在某个位置出现了鞍点或者极小值点梯度为零算法无法区分到了目标和被困住了。解决思路大致有四类施加随机扰动当检测到飞机速度连续N步接近零且没到目标时加一个随机方向的小力改变势场函数形式在斥力中加入切向分量让合力总是有侧向偏移引入子目标点在障碍物侧面预设一个临时航点引导飞机绕行把斥力方向修正为垂直于引力方向的分量牺牲一点最优性换取避障的必然性我实际采用的是第4种思路的变体当障碍物斥力与目标引力的夹角接近180度时我把斥力向量投影到垂直于引力的平面上再施加。这样飞机不会完全悬停而是侧向漂移等到偏离障碍物正面之后引力自然会把飞机拉向正确方向。4.2 引力增益和斥力增益的比例必须要配对调试这个比例关系直接决定飞行稳定性。我一开始用的kAtt2、kRepObs1整个仿真跑下来飞机像喝醉了一样来回晃轨迹绕得乱七八糟。后来分析受力才发现规律如果引力增益相对斥力增益过大飞机会直愣愣地冲向障碍物直到距离很近时斥力才反转方向导致飞机划出一道锐利的急转轨迹轨迹不连续且姿态变化剧烈。如果斥力增益过大飞机会在障碍物很远处就开始转向绕的路径特别宽甚至出现机群被斥力弹开后很难重新聚拢。一个相对靠谱的初始配比是引力增益与斥力增益的比值在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 * dtdamping取0.1到0.3之间。这个改动虽然简单但对整体稳定性贡献非常大强烈建议加上。二是把邻居斥力的作用范围设置得略大于安全距离比如安全距离3米、作用范围4米。这样斥力是渐进的不是到3米才突然爆发飞机提前开始平滑减速抖振明显缓解。4.5 多机同时靠近冲突区域时的优先级问题多架飞机同时冲向一个障碍物附近其实还有一个隐蔽的问题如果不是同一高度层飞行只是水平投影重叠实际三维距离还是够的。这种场景下斥力反应正常轨迹也合理。但如果多架飞机在三维空间里都挤到了一个窄通道就会形成机群内部的级联冲突——飞机A为了避障往右偏撞上了飞机B的控制意图B为躲避A往左偏又撞上了C。这种级联效应在人工势场法框架下很难彻底根除因为每架飞机的决策都是独立做出的。我的缓解手段是对邻居斥力加了权重因子距离越近的邻居斥力权重越大并且让这个权重随距离指数增长。这样冲突最激烈的小范围内飞机会优先处理最近的威胁不至于同时被多股力量撕扯。虽然不能完全避免级联但能把机群的震荡幅度控制在一个可接受的范围内。5. 仿真结果的可视化分析与后续扩展方向5.1 三维轨迹可视化为什么旋转视角比静态图更有用Matlab的plot3可以画出最基础的轨迹线但如果只是把所有飞机的轨迹画在一个图里一旦轨迹密集前后遮挡严重根本看不出谁在什么时候避了什么障碍物。我更推荐的做法是分段着色把轨迹按时间段分为多个段每段用不同颜色的渐变显示并且把障碍物的球面用surf函数绘制成半透明表面。这样旋转视角时可以很清楚地看到飞机是从障碍物的哪个方向绕过去的。另外当前时刻的飞机位置用大号标记点显示过往轨迹则用细线表示层次感会清晰很多。还有一个实用技巧加一个跟随视角模式。选定一架无人机让相机始终跟随它可以观察到它视野内的障碍物和其他飞机的几何关系。这在调试局部极小值问题时有奇效你能直观看到飞机为什么悬停——相机视角下的受力方向对称性非常明显。5.2 量化评估指标不要只看结果要算数据我的习惯是从第二轮仿真开始就逐项记录量化指标不然换一组参数就忘了上一版效果如何。我常用的指标包括最小机间距离、最小障碍物距离、到达目标的时间、路径总长度、速度超限比例等。用表格来追踪记录是一个好方法比如参数组最小机间距离(m)最小障碍物距离(m)到达时间(s)是否发生碰撞A2.71.823.5否B1.91.221.2是C3.12.426.8否通过这样的数据对比参数优劣一目了然。我尤其关注最小障碍物距离因为它直接反映安全冗余量。目标是最小距离远大于无人机物理半径加安全余量同时到达时间又不至于过慢。这两个指标在本质上是矛盾的最终参数选择是一个折中。5.3 从人工势场法出发的四个扩展方向如果你已经跑通了这套仿真后续想往更实用的方向走我认为有四个值得展开的方向。第一是增加速度障碍法模块。APF的实时性好但避障路径不一定最优VO/RVO在速度空间决策更硬把两者结合可以做到既平滑又安全。我尝试过在APF框架下对动态障碍物额外叠加一个RVO约束效果比纯APF好很多。第二是加入编队控制项。目前的逻辑是所有飞机直奔同一目标点到达后会挤在一起。如果加入编队间距约束比如让飞机保持V字队形、菱形队形需要在力场中加入期望距离和期望方位角两个参考量这会显著提升仿真的应用价值。第三是做任意规模参数扫描。把核心参数kAtt、kRep、safeDist、dt作为扫描变量批量跑场景统计成功率和安全指标可以得到一组参数的鲁棒性画像。这个过程在算法论文里几乎是必做的提前准备好代码框架能省很多时间。第四是往实机部署方向走。仿真里的质点运动学模型和真实飞机的动力学模型差别很大真机还要考虑姿态响应、通信延迟、传感器噪声。建议逐步把模型从质点升级到带惯性的一阶延迟模型控制周期也要和真实的飞控频率对齐。5.4 我踩过的最蠢的一个坑最后分享一个让我浪费了两天时间的坑。我在仿真中把障碍物坐标写错了写成了[x, y, z, radius]的格式但读取数据时用了[radius, x, y, z]的顺序结果所有障碍物都偏移到了完全不对的位置飞机频频撞上隐形墙。排查了很久才发现是索引顺序不一致。所以从第一行代码开始就统一数据结构定义并且把读取和使用的代码位置隔离开数据定义集中放使用逻辑集中放。这个代码规范上的小习惯在后续扩展模型、增加障碍物时真的会省下大量排查时间。
阅读完成 · 觉得有帮助？
