企业官网建设流程全解析

最近在做一个多无人机协同路径规划的项目，踩了不少坑，也沉淀了不少经验。这个方向市面上开源代码不少，但真正能跑通动态环境、多机协同、防撞一整套流程的，还是需要自己把逻辑捋清楚。我基于Matlab从零实现了一套动态环境下多无人机协同路径规划与防撞系统，包含全局路径规划、局部动态避障、多机协同防撞和可视化仿真四大部分，整体在离线仿真中达到了预期的规划精度和实时性目标。

这套实现的价值在于，它不是把几个现成的工具箱函数拼在一起，而是把“协同”真正落到了规划层：每架无人机不仅走自己的路，还要在动态威胁下与其他无人机相互避让。对于正在学习无人机集群算法、需要完成课程设计或仿真验证的开发者，这套代码提供了一个可以直接修改、扩展的工程框架，按自己的场景换地图、换模型、加约束都能快速跑起来。

1. 问题拆解与整体设计思路

1.1 多无人机协同规划解决的是什么

拆开“多无人机协同路径规划与防撞”这句话，核心就三件事：

第一，每架无人机要从起点到终点找到一条可行路径。这个“可行”包含两层意思，一是不能碰到障碍物，二是路径要符合无人机的飞行约束（最小转弯半径、最大速度、飞行高度等）。

第二，多架无人机之间不能互相碰撞。单机路径规划只需要对静态地图负责，多机环境下，每架飞机对别人来说都是一个移动障碍物，而且这个障碍物本身的未来轨迹不确定，因为它也在动态决策。

第三，在动态环境中，原来的路径可能随时失效。比如环境中突然出现新的静态障碍，或者其他飞机切入本机航线，这时候需要局部重规划，而不只是简单走全局路径。

这三件事如果分开做，每件都不难，难的是同时做并且保证计算效率。我最初尝试把全局路径规划结果固定下来，再靠底层速度控制强行避让，结果发现全局路径频繁失效，飞机经常绕大圈。后来换成分层架构，情况才显著改善。

1.2 动态环境带来的三个额外麻烦

静态环境下做多机规划，本质上是在几何空间里做“找路+防撞”，但一旦引入动态障碍，三个麻烦会立刻出现：

第一个是时间维度。静态规划只需要回答“走哪条路”，动态规划必须回答“什么时候在哪个位置”。路径规划空间从XY二维（或XYZ三维）扩展成XYT（或XYZ-T）四维，搜索复杂度成指数级上升，不能再简单地套静态A*或RRT去暴力搜索。

第二个是预测不确定性。动态障碍物的运动状态只能通过一定时间窗口内观测来估计，短期内可以假设匀速或匀加速度，长期预测误差快速累积。规划算法必须把“预测的时间跨度”限制在可控范围内。

第三个是计算实时性。多机协同下，每架飞机的规划结果都会影响其他飞机的输入。如果规划耗时太长，等算出来局势早就变了。所以算法必须在几十毫秒到几百毫秒内完成一次重规划，这就逼着你在最优性和实时性之间做权衡。

1.3 方案选型：集中式还是分布式

协同规划在架构上有两类主流思路：集中式和分布式。

集中式是有一个中枢节点统一接收所有飞机的位置、目标点、状态，算出全部无冲突路径后统一下发。好处是全局最优性好，冲突可以直接在规划阶段消解。坏处是计算量随飞机数量指数上升，而且对通信依赖极高，中枢一旦故障整个集群瘫痪。适合飞机数量少（比如3-5架）、通信条件好的场景。

分布式则是每架飞机独立规划路径，依靠局部感知和机间通信来协调避让，比如优先级机制、协商协议。优点是扩展性好，适合大规模集群；缺点是需要精心设计协调策略，否则会出现“死锁”——两架飞机互相让路让个没完。

我在这个项目中综合考虑了实时性和扩展需求，选择了集中规划与分布式避让结合的混合方案：全局规划阶段用集中式协调生成带时间信息的初始路径，局部防撞阶段用分布式速度障碍法实时避让。这样既能保留全局协调能力，又能应对动态变化，整体结构也更贴合实际工程中的“地面站+机载自主”模式。

2. 核心算法选型与技术原理

2.1 全局路径规划：栅格地图+RRT*的选型逻辑

全局规划层我用了基于栅格地图的RRT算法。选它的理由很朴素：A在稠密栅格地图上虽然能给出较优路径，但地图尺寸一大、维度一高，内存占用和计算时间膨胀得厉害。RRT系列作为采样规划算法，在高维空间里尤其灵活，而且RRT*相比基础RRT增加了重连（rewire）步骤，能渐进优化出接近最短的路径。

具体做法是先把环境栅格化成二值地图，再用map = binaryOccupancyMap(width, height, resolution)建立占据栅格场。RRT*在这个栅格场里采样节点，同时用stateSpaceDubins约束两点之间的连接曲线，让规划出的路径天生满足最小转弯半径约束，不至于产生折线型的不合理航线。

不过RRT*有一个用户要特别注意的问题：随机采样导致每次规划的路径都不完全一样，在多机协同中路径初始化不稳定会带来额外的协调麻烦。我的处理办法是在目标点附近增加采样偏置，同时规划完成后对路径做剪枝和插值平滑。剪枝能去掉冗余航点，插值则保证路径足够密，给下层控制器提供连续参考轨迹。

2.2 局部动态避障：速度障碍区的判定逻辑

动态环境下最核心的局部避障算法，我选的是速度障碍法，也就是常说的VO。它处理“如何避开移动障碍物”的思路非常直观：把所有移动障碍物在速度空间里“膨胀”成一个危险区域，如果本机当前速度落入这个区域，就意味着按这个速度飞下去迟早会撞上，需要立刻寻找区域外的速度。

举一个简化的计算例子。本机位置在A点，速度Va，一个动态障碍物在B点，速度Vb。把相对速度Vr = Va - Vb画出来，再根据两机半径和相对位置判断相对速度是否指向碰撞圆锥内部。如果指向内部，就把Vr朝圆锥边缘旋转一个安全裕度角度，再换算回本机速度。在Matlab中构造一个几何函数collisionCone(self, obstacle, safetyMargin)，输入两机状态，输出当前速度是否在碰撞锥内以及候选逃逸速度方向。

这个方法在工程上的最大优点是计算量极低，而且不依赖障碍物的长期轨迹预测——只需要短时状态估计，非常适合实时决策。但它的缺点也明显：只考虑瞬时状态，没有考虑本机动力学约束，可能给出一个无人机物理上飞不出来的速度指令。所以在代码里我把VO的输出接入了一个低通滤波和加速度限幅环节，模拟真实控制器的响应特性。

2.3 协同机制：优先级和时间窗怎么配合

多机之间不只是避障碍物，还要互相避让。速度障碍法处理动态障碍没问题，但两架都在自主规划的飞机会出现“双向互让”的振荡。解决这个问题的常见思路是给每架飞机分配一个固定的优先级，或者实时协商动态优先级。

我采用的方法是固定编号优先级加时间窗约束。全局规划时，先给每架飞机分配编号，编号小的优先级高。规划器按优先级从高到低依次规划各机路径，低优先级飞机在规划时，把高优先级飞机的轨迹离散成一连串带时间的占位点，作为动态障碍物输入。这样规划出来的所有路径天然无冲突。

局部防撞层也用到优先级：当VO检测到两机速度冲突时，低优先级飞机主动避让，高优先级飞机维持原速。这样虽然损失了一点对称性，但换来的是决策结果的确定性，在仿真中从未出现死锁或震荡。

时间窗约束的作用则是保证路径的时间一致性。全局规划器输出每条航路点时，会附带一个期望到达时间，所有飞机按这个时间表飞行；一旦某架飞机因为躲避动态障碍延误超过阈值，就触发全区域重规划，把时间基准重新对齐。

3. Matlab代码实现与核心环节解析

3.1 环境建模与无人机运动学模型

环境建模这一步在所有规划算法之前，它的质量直接决定后续规划结果是否可信。我使用Matlab Navigation Toolbox中的功能函数搭建占据栅格地图，核心代码如下：

% 建立地图尺寸10km x 10km，分辨率50m/格 width = 10000; height = 10000; resolution = 50; map = binaryOccupancyMap(width, height, resolution); % 手动添加静态障碍物（矩形区域） obs1 = [2000, 1500, 800, 600]; % [x, y, width, height] setOccupancy(map, [obs1(1):obs1(3):obs1(1)+obs1(3); ... obs1(2):obs1(4):obs1(2)+obs1(4)]', 1); % 查看地图 show(map)

这段代码里几个细节值得注意。resolution取50米意味着每个栅格代表50m×50m的区域，分辨率过高会拖慢RRT的碰撞检测，过低又会让小型障碍物的边缘失真。我试过10米和50米两档，10米分辨率下RRT单次规划耗时接近一秒，在动态环境下很难接受，50米下基本在两百毫秒内可以完成，对于仿真验证足够。

无人机运动学模型我做了简化，使用Dubins模型描述平面运动。它把飞机看作一个只能在二维平面内以固定最小转弯半径运动的质点，路径由直线段和圆弧组成。这个模型简单但抓住了固定翼无人机路径规划的核心约束——最小转弯半径，而且能直接配合MATLAB的stateSpaceDubins使用。

% 创建Dubins状态空间，设置最小转弯半径 ss = stateSpaceDubins; ss.MinTurningRadius = 500; % 单位：米

转弯半径的取值需要参考实际机型参数。我按一般小型固定翼无人机的飞行性能估算了500米，如果你换用四旋翼平台，可以直接把半径缩小到100米甚至更低，代码不用改结构。

3.2 协同规划主流程

整体协同规划流程的骨架并不复杂，核心是“按优先级依次规划+轨迹离散化作为后续约束”的循环。我把它整理成如下的主循环伪代码：

% 输入：地图map、各机起点starts、各机终点goals、无人机数量N % 输出：每架飞机的航路点序列paths paths = cell(1, N); for i = 1:N % 1. 把已规划飞机的航路点按速度离散化为动态障碍轨迹 dynObs = []; for j = 1:i-1 traj = discreteTrajectory(paths{j}, speeds(j), dt); dynObs = [dynObs; traj]; end % 2. 构造状态验证器：融合静态地图 + 动态轨迹占位 validator = validatorDynamicObstacles(map, dynObs); % 3. 在规划空间里搜索无冲突路径 path = planSingleUAV(ss, validator, starts{i}, goals{i}); % 4. 剪枝 + 平滑 paths{i} = smoothPath(path); end

第1步的discreteTrajectory是协同的关键。它把上一架飞机的路径按期望速度和固定时间步长，离散成一系列“位置+时刻”的集合。这些位置在后续规划中会被当作占据区域处理——某时刻某位置是占用状态，路径规划器就必须绕开。

第3步的planSingleUAV内部调用RRT搜索，但验证器变成动态验证器。实现时我写了一个自定义的validatorDynamicObstacles类，继承自nav.StateValidator，在isStateValid方法里同时检查静态栅格占用和动态轨迹占位。这样RRT每扩展一个节点，都会检查该节点对应的时刻是否与其他飞机轨迹有重叠。

3.3 防撞逻辑实现：距离检测与速度避让

防撞逻辑在每一控制周期执行，输入是当前所有飞机位置、速度，以及地图信息，输出是每架飞机的期望速度修正。核心是两个环节：碰撞提前检测和速度避让计算。

碰撞提前检测主要计算两架飞机的最短相遇时间。如果任意两架飞机在预测时间内距离小于安全半径，就判定为有碰撞风险，进入避让流程：

function [risk, ttc] = checkCollisionRisk(pos1, vel1, pos2, vel2, safeDist) relPos = pos1 - pos2; relVel = vel1 - vel2; % 相对位置在相对速度方向上的投影 a = dot(relVel, relVel); b = 2 * dot(relPos, relVel); c = dot(relPos, relPos) - safeDist^2; % 求解最小相遇时间 disc = b^2 - 4*a*c; if disc < 0 || a <= 0 risk = false; ttc = inf; return; end ttc = max(0, (-b - sqrt(disc)) / (2*a)); risk = ttc < 10; % 10秒为预警门限 end

这个函数的数学本质是求解二次方程，判断相对运动轨迹是否进入以安全距离为半径的“保护圆”。代码里ttc < 10的预警门限可以根据场景调整，越小越激进，越大越保守。我实测下来10秒门限对本次仿真中等密度场景比较合适，超过15秒会出现过度避让，飞机会走很多不必要的弯路。

如果检测到风险，低优先级飞机会触发速度障碍避让，调整速度方向避开碰撞圆锥：

function newVel = velocityObstacleAvoidance(posSelf, velSelf, posObs, velObs, maxSpeed) % 计算相对位置和相对速度 relPos = posSelf - posObs; relVel = velSelf - velObs; % 计算到碰撞圆锥边界的角度偏移 relDist = norm(relPos); coneAngle = asin(safeRadius / relDist); refAngle = atan2(relVel(2), relVel(1)); avoidAngle = refAngle + coneAngle + deg2rad(10); % 额外裕量10度 newDir = [cos(avoidAngle), sin(avoidAngle)]; newVel = newDir * maxSpeed; end

这段代码是我实现中反复调试最多的部分。+ deg2rad(10)这个额外裕量并不是来自理论推导，而是仿真经验——如果没有这10度裕量，避让轨迹会贴着碰撞圆锥边缘走，一旦障碍物有微小非线性运动，立刻又会重新触发避让，造成频繁换向。

3.4 仿真可视化与数据记录

可视化看似是锦上添花，实际上在协同规划开发中是刚需。没有逐帧动画，你很难看出两架飞机到底是在“主动协同”还是“碰巧没撞上”。Matlab中我用figure + plot加定时器驱动的方式，把每一时刻所有飞机的位置、历史和目标点都画在同一张图上。核心结构如下：

% 初始化图形 figure; hold on; grid on; axis([0 10000 0 10000]); plotStaticObstacles(map); % 为每架飞机创建轨迹句柄 h_traj = cell(1, N); for i = 1:N h_traj{i} = plot(nan, nan, '-', 'LineWidth', 1.5); end % 每帧刷新飞机位置与轨迹 for t = 1:numSteps % 更新每个飞机的真实位置（由控制逻辑产生） for i = 1:N set(h_traj{i}, 'XData', trajX{i}(1:t), 'YData', trajY{i}(1:t)); end drawnow; pause(0.05); end

除了图形输出，我把每一时刻各机位置、速度、碰撞检测结果都记录到结构体变量中，仿真结束后统一导出，方便后续量化分析“最小间距”、“平均绕行距离”等指标。这些指标在写实验报告或论文时非常重要，千万别只留动画不留数据。

4. 参数标定与实验效果分析

4.1 各模块关键参数速查表

参数标定是整个项目中花费时间最多的环节，没有之一。我把关键参数整理为一张速查表，方便后续调试时对照修改：

这张表里最容易被忽略的是“优先级时间间隔”。它表示低优先级飞机规划时，高优先级飞机轨迹被离散化时使用的时间粒度。2秒意味着每隔2秒采一个占位点，点与点之间通过线性插值判断占用。间隔太大，低优先级飞机会“钻空档”；间隔太小，计算量又大，实时性变差。我测试了0.5秒到5秒的区间，最终落在2秒是比较平衡的选择。

4.2 实验场景与量化结果

我用一个典型场景做了验证：5架无人机，起点分布在地图四周，目标点是中心附近的5个不同位置，环境中预先放置了3个静态障碍区，另外设置了两条移动障碍轨迹模拟动态威胁。

量化结果上，5架飞机全部在规划时间内到达目标点，全程任意两机间距保持在安全半径以上，最小间距记录出现在第3架和第4架飞机交汇时，为276米（安全半径250米），没有触发实际碰撞。整体路径长度相比单机最短路径总和增加了约18%，动态避让造成的额外绕行在合理范围内。

从实时性看，全局协同规划总耗时约3.1秒，每架飞机平均240毫秒完成路径搜索；局部速度障碍避让单次计算均在毫秒级，控制周期20Hz下完全跑得动。这说明混合架构的动态负载是可接受的，具备实际嵌入式平台部署的潜力。

4.3 与单机规划的对比思考

把协同结果和单纯做5次单机规划对比，能更清楚看到协同层到底付出了多少代价、换来了什么。单机独立规划时每架飞机都会选择对自己最短的路径，但两条最短路径很可能会在某些区域交叉甚至重合。在我这个场景中，5条独立最短路径在地图中心形成了一个明显的汇聚区域，如果不加协同，这个区域就是潜在的“碰撞雷区”。

协同规划本质上是在每个交汇点附近增加时间错开。代价是多飞了18%的距离，换来的是全程无冲突且不需要任何急停急转。这个“时间换空间”的思路在无人机集群里非常关键——硬碰硬的空间分离往往成本过高，合理的时间错位是更经济的方案。

5. 常见问题与调试避坑记录

5.1 运行报警与解决方案对照

实际调试过程中必然遇到一堆报错和异常现象，我把最常见的几类整理成对照表：

最后一条是最容易被忽略的。理论检测的安全距离经常在真实动态仿真中被“突破”一点，原因就是速度改变需要时间，飞机会在加速阶段向前“滑”一段距离。这个“滑行距离”在小速度下不明显，但在高速或大惯量场景中很危险。最简单有效的做法就是给安全半径加上一个冗余系数，而不是追求理论上的紧致界。

5.2 调试方法：从单机到多机的分步推进

多机协同系统的调试复杂度远大于单机，如果直接把5架飞机一起怼进去，出了问题根本没法定位。我的调试习惯是先分层次推进：

第一步，单机静态环境调试。先关掉所有动态障碍和其他飞机，只验证单机全局规划路径是否平滑、是否满足转弯半径约束。此阶段解决的是RRT*本身的问题。

第二步，单机动态环境调试。加入移动障碍物，验证速度障碍法能否响应动态威胁。此阶段需要重点观察避让路径是否有抖动或反复横跳，有就调避让角度裕量和预警门限。

第三步，双机协同调试。只保留两架飞机，验证优先级机制、时间窗配合是否正常，观察交汇时的避让行为。此阶段最容易发现问题，因为两机交互模式简单，一眼就能看出谁在让谁。

第四步，逐步扩展至5架。每加一架都观察是否引入新的冲突模式，尤其关注狭窄通道、多机交汇等高难度场景。

这套分层调试的方法在项目后期帮我节省了大量定位时间。不要嫌前期流程长，越急越容易在后期被一个隐蔽问题拖住。

5.3 实际执行过程中的几个易错点

有几个细节我一定要专门提醒。第一个是动态障碍物轨迹的坐标系必须与地图坐标系完全一致，这听起来是废话，但我在代码里就犯过把局部坐标当成全局坐标用的低级错误，结果飞机绕着空转。第二是RRT*的samples数量不能盲目加大，采样数加倍虽然可能找到更优解，但耗时增长更明显，在动态场景里得不偿失。第三是固定优先级虽然稳定，但要定期检查低优先级飞机是否长期处于避让状态，如果多次仿真中某一架飞机总是绕行，说明优先级分配不合理，需要基于起点到终点的路径交叉度动态调整。

6. 扩展方向与后续思路

这套Matlab实现最大的优势是扩展性。如果后续要做三维环境，只需要把状态空间换为stateSpaceSE3或基于Dubins的三维扩展，同时在地图中增加高度维度栅格；如果要做真实飞行器接入，可以把当前的速度避让输出换成MAVLink指令，云端协同保持离线规划，全球路径下发给机载飞控。

我还在尝试把局部避让层从几何方法换成模型预测控制，用滚动时域优化同时考虑速度约束和加速度约束，可能进一步降低多余绕行，但对计算性能的要求会提高不少。另外，把强化学习引入协同避让决策也是当前一个很有意思的方向，不过就目前的工程落地而言，基于几何和优化结合的方法稳定性和可解释性仍然是最强的。

回看这个项目的全过程，我最深的体会是：多无人机协同路径规划的关键瓶颈从来不是单个算法不够聪明，而是整套系统的环节太多、耦合太紧。把每个环节的参数都理解透，并且建立一种分步调试、层层验证的工作习惯，才是真正把代码跑好、把系统做稳的核心能力。这套思路你现在复制过去，换成自己的地图和参数，也能较快拿到可用的结果。