图 1 PSO-BFOA-RRT 无人机三维路径规划：文章主线
MATLAB 无人机三维路径规划：RRT + PSO + BFOA 融合优化、避障建模与完整代码
从“先找到可行路径”到“全局优化航路点”再到“局部精修”，给出统一建模、适应度函数、碰撞检测、关键工程修正与可直接运行的完整 MATLAB 脚本。
|
一句话结论 RRT 负责把搜索从“可能无解”推进到“先有一条可行解”；PSO 在固定维度航路点空间做全局优化；BFOA 再围绕 PSO 的优质解进行局部强化。三者共享同一套路径编码、碰撞检测和综合代价，因此融合的关键不是算法数量，而是任务分工。 |
前言：为什么三维无人机路径规划不能只追求“最短”
无人机三维路径规划不仅要让飞行器从起点到达终点，还必须同时处理高度、转弯、安全距离、障碍物边界以及路径可执行性。若目标函数只保留欧氏距离，优化器很容易产生贴障飞行、连续大幅爬升、急转弯甚至线段穿障等“数学上短、工程上不可飞”的路线。
本文采用 RRT、PSO 与 BFOA 的分阶段协同思路：先用快速扩展随机树探索自由空间，得到一条无碰撞种子路径；再把路径重采样为固定数量的三维航路点，交给粒子群进行全局优化；最后用轻量化 BFOA 在优质解附近做局部精修。评价阶段始终使用统一的综合适应度函数，确保不同阶段优化的是同一个目标。
本文示例按 MATLAB R2025b 的基础语法组织，不依赖深度学习工具箱或第三方库。障碍物使用长方体近似，适合课程设计、算法验证、科研原型与静态三维环境的路径规划实验。
一、整体方案：RRT 找到路，PSO 优化路，BFOA 精修路
图 2 RRT → PSO → BFOA 的协同架构与统一评价层
|
阶段 |
输入 |
核心任务 |
主要优势 |
主要风险 |
|
环境建模 |
边界、起终点、障碍物 |
统一几何与安全约束 |
结构清晰、便于扩展 |
长方体模型仍是近似 |
|
RRT |
三维自由空间 |
快速找到无碰撞种子路径 |
探索能力强、适合非凸空间 |
路径折点多、随机性高 |
|
PSO |
固定维度航路点向量 |
大范围优化路径控制点 |
连续空间全局搜索简单高效 |
可能早熟收敛 |
|
BFOA |
PSO 优质解附近 |
局部强化与多样性恢复 |
适合精修局部几何细节 |
函数评估成本增加 |
|
平滑与输出 |
融合最优路径 |
删冗余点、生成可视化 |
提高可读性与可执行性 |
必须防止平滑后代价变差 |
二、问题建模：三维空间、障碍物与安全边界
飞行区域使用三维笛卡尔坐标系描述，范围为 X∈[0,100] m、Y∈[0,100] m、Z∈[0,60] m。起点为 S=(5,8,8)，终点为 G=(94,90,45)。四个障碍物使用六列矩阵 [xmin,xmax,ymin,ymax,zmin,zmax] 表示。
图 3 示例三维飞行空间、起终点与四个长方体障碍物
碰撞判断时，障碍物边界先向外膨胀 `safeRadius=2 m`，相当于把无人机机体尺寸和基础安全裕度吸收到障碍物模型中；随后适应度函数再用 `clearance=5 m` 对距离膨胀后障碍物过近的采样点施加连续惩罚。这样，硬碰撞约束与软安全距离约束可以同时存在。
三、路径编码：把三维航迹转成 PSO/BFOA 能处理的向量
图 4 固定起终点、优化中间航路点的路径编码方式
完整路径设置 12 个航路点，其中起点和终点固定，因此仅优化 10 个中间点。每个中间点包含 x、y、z 三个坐标，优化维度为 D=3×(12-2)=30。RRT 输出的原始折线路径节点数通常不固定，因此先按累计弧长做等距重采样，再编码成 30 维向量。
|
为什么固定维度很重要 RRT 的职责是“发现拓扑可行性”，而 PSO/BFOA 需要固定维度连续变量。弧长重采样把两类算法的表示方式连接起来，使 RRT 产生的任意节点数路径都能成为粒子群的种子解。 |
四、RRT：先确保“有路可走”
RRT 从起点开始建树。每次随机采样一个三维点，找到当前树中距离它最近的节点，并沿该方向扩展固定步长。新线段只有在边界合法且整段无碰撞时才会加入树。为了减少终点附近的无效探索，示例使用 0.18 的目标偏置概率：一部分采样直接取终点。
与只检查“新节点是否在障碍物内”相比，本文对整条候选边进行离散采样。这样可以避免两个端点都在自由空间、但中间线段穿过障碍物的情况。采样步长为 1 m，可根据最小障碍物尺度和计算预算调整。
if rand < goalBias
sample = env.goal;
else
sample = env.bounds(:,1)' + rand(1,3).*(env.bounds(:,2)'-env.bounds(:,1)');
end
[~,nearestIndex] = min(vecnorm(nodes-sample,2,2));
direction = sample - nodes(nearestIndex,:);
newPoint = nodes(nearestIndex,:) + ...
min(stepSize,norm(direction))*direction/max(norm(direction),eps);
if isInsideBounds(newPoint,env) && ...
isSegmentFree(nodes(nearestIndex,:),newPoint,env)
nodes = [nodes;newPoint];
parents = [parents;nearestIndex];
end
工程上不建议在 RRT 失败时直接把 `[start; goal]` 当作备用种子，因为直线可能本身穿障。本文完整代码改为最多重试 5 次；仍失败则显式报错，避免把不可行路径悄悄传入后续优化。
五、PSO：在连续航路点空间做全局优化
PSO 中一个粒子对应一条候选路径的 30 维中间航路点向量。速度更新由惯性项、个体认知项和群体社会项共同组成：
|
PSO 速度更新 vᵢ(t+1) = ω·vᵢ(t) + c₁·r₁·(pbestᵢ−xᵢ) + c₂·r₂·(gbest−xᵢ) |
示例采用动态参数：惯性权重从 0.90 线性下降到约 0.35，个体学习因子 c1 从 1.8 降到 1.2，群体学习因子 c2 从 1.2 升到 2.0。前期更强调探索与个体差异，后期逐步增加向全局优质区域聚合的倾向。每次更新后还会做速度限幅、边界截断，并以 0.12 的概率施加小幅随机扰动以维持种群多样性。
inertia = 0.90 - 0.55 * (iteration-1) / max(psoIterations-1,1);
c1 = 1.8 - 0.6 * (iteration-1) / max(psoIterations-1,1);
c2 = 1.2 + 0.8 * (iteration-1) / max(psoIterations-1,1);
velocities(i,:) = inertia * velocities(i,:) ...
+ c1*r1.*(personalBest(i,:) - positions(i,:)) ...
+ c2*r2.*(globalBest - positions(i,:));
positions(i,:) = positions(i,:) + velocities(i,:);
PSO 结束后必须立即保存 `psoBest`、`psoScore` 和 `psoPath`。如果等到 BFOA 完成后再从 `globalBest` 解码所谓“PSO 路径”，那么全局最优已经被 BFOA 改写，图中的 PSO 对比线就失去意义。完整代码已修正这一点。
六、BFOA：围绕 PSO 优质解做局部精修
BFOA 阶段不再承担大范围全局探索，而是在 `psoBest` 附近初始化一组细菌。每个细菌仍是一条候选路径。趋化时沿随机单位方向移动，若代价下降则接受；随后把较优细菌复制到较差细菌附近，并对当前最差的一小部分个体做随机迁移，以恢复多样性。
趋化步长随迭代由约 2.45 逐步减小到约 0.31，使前期局部搜索范围较大，后期更适合细化航路点位置。繁殖后重新排序，再选择当前最差个体执行迁移，避免使用旧排序导致迁移对象与当前种群质量不一致。
chemotaxisStep = 2.2 * (1 - (iteration-1)/bfoIterations) + 0.25;
direction = randn(1,dimension);
direction = direction / max(norm(direction),eps);
candidate = bacteria(i,:) + chemotaxisStep * direction;
if pathCost(decodePath(candidate,env),env) < bacteriaScore(i)
bacteria(i,:) = candidate;
end
|
实现边界说明 这里使用的是面向路径规划的轻量化 BFOA：保留趋化、繁殖和迁移三类关键机制，用于 PSO 后的局部强化；它并不是对经典 BFOA 全部嵌套循环与 swim 行为的逐字复刻。这样写能把算法定位说清楚，也更符合实际代码。 |
七、统一适应度函数：先可行，再安全，最后比较长度与平滑性
本文把五类目标统一到一个综合代价函数中：
|
综合代价 J(P)=wL·L(P)+wC·Ncollision+wS·Pclearance+wT·Pturn+wH·Paltitude |
|
代价项 |
计算含义 |
示例权重 |
作用 |
|
L(P) |
相邻航路点欧氏距离之和 |
1.0 |
缩短航程 |
|
Ncollision |
线段离散采样后的碰撞点数量 |
1e5 |
强制排除穿障路径 |
|
Pclearance |
安全距离不足的平方惩罚 |
80 |
推开贴障路径 |
|
Pturn |
相邻方向夹角变化惩罚 |
12 |
减少急转弯 |
|
Paltitude |
相邻航路点高度变化绝对值之和 |
1.5 |
抑制频繁爬升/下降 |
图 5 适应度权重的数量级与“先可行、再安全、后优化”设计
权重不是“越大越好”。碰撞项设置为最高数量级，是为了让任何穿障路径都不可能因为距离较短而获得优势；安全距离次之；长度、转弯和高度变化主要用于可行路径之间的细粒度比较。实际项目应根据地图尺度、无人机尺寸、控制精度和任务优先级重新标定。
八、碰撞检测与安全距离：为什么要检查整段线段
对于路径段 A→B，程序按 `sampleStep` 将其离散成多个采样点。每个点先判断是否进入“膨胀后的障碍物”，再计算到最近障碍物边界的距离。碰撞是硬风险，因此按碰撞采样点数量施加高额惩罚；未碰撞但距离过近时，使用 `(clearance - d)^2` 形成连续惩罚。
这种做法的工程含义是：优化器不仅知道“能不能过”，还知道“离危险区域有多近”。相比只有 0/1 碰撞判断，连续安全距离能为 PSO/BFOA 提供更平滑的搜索信号。
|
采样步长的取舍 `sampleStep` 太大，窄障碍物可能被跨过；太小，单次适应度评估会明显变慢。对本文 100×100×60 m 的示例空间，1 m 是偏保守的教学值。工程应用应结合最小障碍物尺寸、地图分辨率和无人机速度共同确定。 |
九、路径平滑：不能只“删点”，还要保证综合代价不变差
原始的贪心直线简化通常会从当前点尝试直接连接更远的航路点，只要不碰撞就删除中间点。这能减少折点，但存在一个容易忽略的问题：新线段虽然不碰撞，却可能更贴近障碍物，或造成更大的高度变化，从而使综合代价变差。
完整代码改为 `smoothPathCostAware`：候选捷径首先必须无碰撞，其次将候选整条路径重新送入 `pathCost`。只有综合代价不高于当前基线时才接受捷径；最终再比较平滑前后的总代价，如果变差则保留 BFOA 原始结果。
十、复杂度与参数敏感性：真正耗时的是适应度评估
设路径包含 M 条线段，每条线段平均采样 S 个检测点，障碍物数量为 O。一次 `pathCost` 的主要开销约为 O(M·S·O)。因此整体运行时间通常不是由 PSO 的向量更新决定，而是由“粒子数量 × 迭代次数 × 碰撞/安全距离评估”累积形成。
|
参数 |
示例值 |
增大后的影响 |
常见调节方向 |
|
numParticles |
45 |
全局搜索更充分，但函数评估更多 |
复杂环境可提高到 60~100 |
|
psoIterations |
100 |
更充分收敛，耗时线性增加 |
观察曲线平台期决定是否继续 |
|
numBacteria |
12 |
局部样本更丰富 |
通常不必和粒子数一样大 |
|
bfoIterations |
35 |
局部精修更充分 |
PSO 已较好时可适当减少 |
|
rrtStep |
8 m |
树扩展更快但可能错过狭窄通道 |
障碍密集时减小 |
|
rrtGoalBias |
0.18 |
更偏向终点，但探索多样性下降 |
通常在 0.05~0.25 内试验 |
|
sampleStep |
1 m |
检测更精细，计算更慢 |
根据最小障碍尺度设定 |
十一、完整 MATLAB 程序（修正版）
下面给出可直接保存为 `main_PSO_BFOA_RRT.m` 的完整脚本。与原始版本相比，主要修正了 PSO 对比路径快照、RRT 失败处理、BFOA 繁殖后的重新排序，以及路径平滑后的综合代价保护。
clear; clc; close all;
rng(2025,'twister');
%% 1. Environment and objective parameters
env.bounds = [0 100; 0 100; 0 60];
env.start = [5 8 8];
env.goal = [94 90 45];
env.safeRadius = 2.0;
env.clearance = 5.0;
env.sampleStep = 1.0;
env.obstacles = [20 35 18 48 0 28;
45 62 8 30 0 38;
68 84 42 70 0 32;
28 48 68 88 0 22];
env.weights.length = 1.0;
env.weights.collision = 1e5;
env.weights.clearance = 80;
env.weights.turn = 12;
env.weights.altitude = 1.5;
%% 2. Algorithm parameters
numWaypoints = 12;
numParticles = 45;
psoIterations = 100;
bfoIterations = 35;
numBacteria = 12;
rrtMaxNodes = 4500;
rrtStep = 8.0;
rrtGoalBias = 0.18;
rrtRetries = 5;
dimension = 3 * (numWaypoints - 2);
lowerBound = repmat(env.bounds(:,1)',1,numWaypoints-2);
upperBound = repmat(env.bounds(:,2)',1,numWaypoints-2);
%% 3. RRT: obtain a feasible seed path
rrtSuccess = false;
for retry = 1:rrtRetries
[rrtPath,rrtSuccess] = buildRRT(env,rrtMaxNodes,rrtStep,rrtGoalBias);
if rrtSuccess
break;
end
end
if ~rrtSuccess
error('RRT failed to find a collision-free seed path after %d retries.',rrtRetries);
end
rrtPath = resamplePath(rrtPath,numWaypoints);
rrtPosition = encodePath(rrtPath);
rrtScore = pathCost(rrtPath,env);
%% 4. PSO: global path optimization
positions = zeros(numParticles,dimension);
velocities = zeros(numParticles,dimension);
for particleIndex = 1:numParticles
if particleIndex == 1
positions(particleIndex,:) = rrtPosition;
else
positions(particleIndex,:) = rrtPosition + 8 * randn(1,dimension);
positions(particleIndex,:) = min(max(positions(particleIndex,:),lowerBound),upperBound);
end
velocities(particleIndex,:) = 0.15 * (upperBound-lowerBound) .* randn(1,dimension);
end
personalBest = positions;
personalScore = inf(numParticles,1);
globalBest = rrtPosition;
globalScore = rrtScore;
psoCurve = zeros(psoIterations,1);
for particleIndex = 1:numParticles
personalScore(particleIndex) = pathCost(decodePath(positions(particleIndex,:),env),env);
if personalScore(particleIndex) < globalScore
globalScore = personalScore(particleIndex);
globalBest = positions(particleIndex,:);
end
end
for iteration = 1:psoIterations
inertia = 0.90 - 0.55 * (iteration-1) / max(psoIterations-1,1);
c1 = 1.8 - 0.6 * (iteration-1) / max(psoIterations-1,1);
c2 = 1.2 + 0.8 * (iteration-1) / max(psoIterations-1,1);
for particleIndex = 1:numParticles
r1 = rand(1,dimension);
r2 = rand(1,dimension);
velocities(particleIndex,:) = inertia * velocities(particleIndex,:) ...
+ c1 * r1 .* (personalBest(particleIndex,:) - positions(particleIndex,:)) ...
+ c2 * r2 .* (globalBest - positions(particleIndex,:));
vmax = 0.25 * (upperBound-lowerBound);
velocities(particleIndex,:) = max(min(velocities(particleIndex,:),vmax),-vmax);
positions(particleIndex,:) = positions(particleIndex,:) + velocities(particleIndex,:);
if rand < 0.12
positions(particleIndex,:) = positions(particleIndex,:) + 2.5 * randn(1,dimension);
end
positions(particleIndex,:) = min(max(positions(particleIndex,:),lowerBound),upperBound);
currentScore = pathCost(decodePath(positions(particleIndex,:),env),env);
if currentScore < personalScore(particleIndex)
personalBest(particleIndex,:) = positions(particleIndex,:);
personalScore(particleIndex) = currentScore;
end
if currentScore < globalScore
globalBest = positions(particleIndex,:);
globalScore = currentScore;
end
end
psoCurve(iteration) = globalScore;
end
% Important: snapshot PSO result BEFORE BFOA updates globalBest.
psoBest = globalBest;
psoScore = globalScore;
psoPath = decodePath(psoBest,env);
%% 5. Lightweight BFOA: local refinement around PSO best
bacteria = repmat(psoBest,numBacteria,1) + 3.0 * randn(numBacteria,dimension);
bacteria(1,:) = psoBest;
bacteria = min(max(bacteria,lowerBound),upperBound);
bacteriaScore = zeros(numBacteria,1);
for bacteriaIndex = 1:numBacteria
bacteriaScore(bacteriaIndex) = pathCost(decodePath(bacteria(bacteriaIndex,:),env),env);
end
[bestBacteriaScore,bestIndex] = min(bacteriaScore);
globalBest = psoBest;
globalScore = psoScore;
if bestBacteriaScore < globalScore
globalScore = bestBacteriaScore;
globalBest = bacteria(bestIndex,:);
end
bfoCurve = zeros(bfoIterations,1);
for iteration = 1:bfoIterations
chemotaxisStep = 2.2 * (1 - (iteration-1)/bfoIterations) + 0.25;
% Chemotaxis
for bacteriaIndex = 1:numBacteria
direction = randn(1,dimension);
direction = direction / max(norm(direction),eps);
candidate = bacteria(bacteriaIndex,:) + chemotaxisStep * direction;
candidate = min(max(candidate,lowerBound),upperBound);
candidateScore = pathCost(decodePath(candidate,env),env);
if candidateScore < bacteriaScore(bacteriaIndex)
bacteria(bacteriaIndex,:) = candidate;
bacteriaScore(bacteriaIndex) = candidateScore;
end
if candidateScore < globalScore
globalBest = candidate;
globalScore = candidateScore;
end
end
% Reproduction
[~,order] = sort(bacteriaScore,'ascend');
halfCount = floor(numBacteria/2);
for rankIndex = 1:halfCount
sourceIndex = order(rankIndex);
targetIndex = order(rankIndex+halfCount);
bacteria(targetIndex,:) = bacteria(sourceIndex,:) ...
+ 0.8 * chemotaxisStep * randn(1,dimension);
bacteria(targetIndex,:) = min(max(bacteria(targetIndex,:),lowerBound),upperBound);
bacteriaScore(targetIndex) = pathCost(decodePath(bacteria(targetIndex,:),env),env);
end
% Re-sort after reproduction, then migrate current worst bacteria.
[~,order] = sort(bacteriaScore,'ascend');
migrateCount = max(1,round(0.15*numBacteria));
for migrateIndex = 1:migrateCount
targetIndex = order(end-migrateIndex+1);
bacteria(targetIndex,:) = lowerBound + rand(1,dimension).*(upperBound-lowerBound);
bacteriaScore(targetIndex) = pathCost(decodePath(bacteria(targetIndex,:),env),env);
end
[currentBestScore,currentBestIndex] = min(bacteriaScore);
if currentBestScore < globalScore
globalScore = currentBestScore;
globalBest = bacteria(currentBestIndex,:);
end
bfoCurve(iteration) = globalScore;
end
%% 6. Cost-aware path simplification and final metrics
bfoPath = decodePath(globalBest,env);
bfoScore = pathCost(bfoPath,env);
finalPath = smoothPathCostAware(bfoPath,env);
finalScore = pathCost(finalPath,env);
if finalScore > bfoScore
finalPath = bfoPath;
finalScore = bfoScore;
end
fprintf('RRT cost: %.3f\n',rrtScore);
fprintf('PSO cost: %.3f\n',psoScore);
fprintf('BFOA-refined cost: %.3f\n',bfoScore);
fprintf('Final cost: %.3f\n',finalScore);
fprintf('Final path length: %.3f m\n',sum(vecnorm(diff(finalPath,1,1),2,2)));
%% 7. 3D path visualization
figure('Color','w','Name','PSO-BFOA-RRT 3D Path Planning');
hold on; grid on; axis equal; view(3);
xlabel('X / m'); ylabel('Y / m'); zlabel('Z / m');
xlim(env.bounds(1,:)); ylim(env.bounds(2,:)); zlim(env.bounds(3,:));
for obstacleIndex = 1:size(env.obstacles,1)
drawCuboid(env.obstacles(obstacleIndex,:));
end
plot3(rrtPath(:,1),rrtPath(:,2),rrtPath(:,3),'--','LineWidth',1.2);
plot3(psoPath(:,1),psoPath(:,2),psoPath(:,3),'-','LineWidth',1.5);
plot3(finalPath(:,1),finalPath(:,2),finalPath(:,3),'-','LineWidth',2.8);
scatter3(env.start(1),env.start(2),env.start(3),90,'filled');
scatter3(env.goal(1),env.goal(2),env.goal(3),90,'filled');
legend('RRT seed','PSO','PSO-BFOA-RRT','Start','Goal','Location','best');
title('PSO-BFOA-RRT UAV 3D Path Planning');
hold off;
%% 8. Convergence curve
figure('Color','w','Name','Convergence Curve');
plot(1:psoIterations,psoCurve,'LineWidth',2);
hold on;
plot(psoIterations+(1:bfoIterations),bfoCurve,'LineWidth',2);
grid on;
xlabel('Iteration'); ylabel('Composite cost');
legend('PSO stage','BFOA stage','Location','best');
title('PSO-BFOA-RRT Convergence');
hold off;
%% ---------------- Local functions ----------------
function [path,success] = buildRRT(env,maxNodes,stepSize,goalBias)
nodes = env.start;
parents = 0;
success = false;
goalIndex = 0;
for nodeCount = 1:maxNodes
if rand < goalBias
sample = env.goal;
else
sample = env.bounds(:,1)' + rand(1,3).*(env.bounds(:,2)'-env.bounds(:,1)');
end
distances = vecnorm(nodes-sample,2,2);
[~,nearestIndex] = min(distances);
direction = sample-nodes(nearestIndex,:);
directionLength = norm(direction);
if directionLength < eps
continue;
end
extensionLength = min(stepSize,directionLength);
newPoint = nodes(nearestIndex,:) + extensionLength*direction/directionLength;
if ~isInsideBounds(newPoint,env)
continue;
end
if ~isSegmentFree(nodes(nearestIndex,:),newPoint,env)
continue;
end
nodes = [nodes;newPoint]; %#ok<AGROW>
parents = [parents;nearestIndex]; %#ok<AGROW>
newIndex = size(nodes,1);
if norm(newPoint-env.goal) <= stepSize && isSegmentFree(newPoint,env.goal,env)
nodes = [nodes;env.goal]; %#ok<AGROW>
parents = [parents;newIndex]; %#ok<AGROW>
goalIndex = size(nodes,1);
success = true;
break;
end
end
if success
reversePath = nodes(goalIndex,:);
currentIndex = goalIndex;
while currentIndex ~= 1
currentIndex = parents(currentIndex);
reversePath = [reversePath;nodes(currentIndex,:)]; %#ok<AGROW>
end
path = flipud(reversePath);
else
path = zeros(0,3);
end
end
function path = resamplePath(rawPath,numWaypoints)
segmentLengths = vecnorm(diff(rawPath,1,1),2,2);
cumulativeLength = [0;cumsum(segmentLengths)];
if cumulativeLength(end) < eps
path = repmat(rawPath(1,:),numWaypoints,1);
return;
end
queryLength = linspace(0,cumulativeLength(end),numWaypoints)';
path = zeros(numWaypoints,3);
for coordinateIndex = 1:3
path(:,coordinateIndex) = interp1(cumulativeLength,rawPath(:,coordinateIndex), ...
queryLength,'linear');
end
end
function vector = encodePath(path)
vector = reshape(path(2:end-1,:)',1,[]);
end
function path = decodePath(vector,env)
middle = reshape(vector,3,[])';
path = [env.start;middle;env.goal];
end
function cost = pathCost(path,env)
totalLength = sum(vecnorm(diff(path,1,1),2,2));
collisionCount = 0;
clearancePenalty = 0;
for segmentIndex = 1:size(path,1)-1
segment = path(segmentIndex+1,:)-path(segmentIndex,:);
segmentLength = norm(segment);
sampleCount = max(2,ceil(segmentLength/env.sampleStep));
for sampleIndex = 0:sampleCount
ratio = sampleIndex/sampleCount;
point = path(segmentIndex,:)+ratio*segment;
[inside,minDistance] = obstacleMetric(point,env);
collisionCount = collisionCount + double(inside);
clearancePenalty = clearancePenalty + max(0,env.clearance-minDistance)^2;
end
end
turnPenalty = 0;
for pointIndex = 2:size(path,1)-1
vectorA = path(pointIndex,:)-path(pointIndex-1,:);
vectorB = path(pointIndex+1,:)-path(pointIndex,:);
cosineValue = dot(vectorA,vectorB) / ...
(max(norm(vectorA),eps)*max(norm(vectorB),eps));
cosineValue = max(-1,min(1,cosineValue));
turnPenalty = turnPenalty + (1-cosineValue)^2;
end
altitudePenalty = sum(abs(diff(path(:,3))));
cost = env.weights.length*totalLength ...
+ env.weights.collision*collisionCount ...
+ env.weights.clearance*clearancePenalty ...
+ env.weights.turn*turnPenalty ...
+ env.weights.altitude*altitudePenalty;
end
function [inside,minDistance] = obstacleMetric(point,env)
inside = false;
minDistance = inf;
for obstacleIndex = 1:size(env.obstacles,1)
obstacle = env.obstacles(obstacleIndex,:);
expanded = obstacle + [-env.safeRadius env.safeRadius ...
-env.safeRadius env.safeRadius -env.safeRadius env.safeRadius];
insideCurrent = point(1)>=expanded(1) && point(1)<=expanded(2) ...
&& point(2)>=expanded(3) && point(2)<=expanded(4) ...
&& point(3)>=expanded(5) && point(3)<=expanded(6);
inside = inside || insideCurrent;
dx = max([expanded(1)-point(1),0,point(1)-expanded(2)]);
dy = max([expanded(3)-point(2),0,point(2)-expanded(4)]);
dz = max([expanded(5)-point(3),0,point(3)-expanded(6)]);
distanceCurrent = sqrt(dx^2+dy^2+dz^2);
if insideCurrent
distanceCurrent = -min([point(1)-expanded(1), expanded(2)-point(1), ...
point(2)-expanded(3), expanded(4)-point(2), ...
point(3)-expanded(5), expanded(6)-point(3)]);
end
minDistance = min(minDistance,distanceCurrent);
end
end
function free = isSegmentFree(pointA,pointB,env)
segmentLength = norm(pointB-pointA);
sampleCount = max(2,ceil(segmentLength/env.sampleStep));
free = true;
for sampleIndex = 0:sampleCount
ratio = sampleIndex/sampleCount;
point = pointA+ratio*(pointB-pointA);
[inside,~] = obstacleMetric(point,env);
if inside
free = false;
return;
end
end
end
function inside = isInsideBounds(point,env)
inside = all(point>=env.bounds(:,1)') && all(point<=env.bounds(:,2)');
end
function pathOut = smoothPathCostAware(path,env)
pathOut = path(1,:);
currentIndex = 1;
while currentIndex < size(path,1)
baseline = [pathOut;path(currentIndex+1:end,:)];
baselineCost = pathCost(baseline,env);
nextIndex = currentIndex + 1;
for candidateIndex = size(path,1):-1:currentIndex+1
if ~isSegmentFree(path(currentIndex,:),path(candidateIndex,:),env)
continue;
end
candidatePath = [pathOut;path(candidateIndex:end,:)];
if pathCost(candidatePath,env) <= baselineCost + 1e-9
nextIndex = candidateIndex;
break;
end
end
pathOut = [pathOut;path(nextIndex,:)]; %#ok<AGROW>
currentIndex = nextIndex;
end
end
function drawCuboid(box)
x = [box(1) box(2)]; y = [box(3) box(4)]; z = [box(5) box(6)];
vertices = [x(1) y(1) z(1); x(2) y(1) z(1); x(2) y(2) z(1); x(1) y(2) z(1); ...
x(1) y(1) z(2); x(2) y(1) z(2); x(2) y(2) z(2); x(1) y(2) z(2)];
faces = [1 2 3 4; 5 6 7 8; 1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8];
patch('Vertices',vertices,'Faces',faces,'FaceColor',[0.65 0.65 0.70], ...
'FaceAlpha',0.45,'EdgeColor',[0.25 0.25 0.25]);
end
十二、运行结果应该怎么看
脚本运行后会生成两幅主要图：三维路径对比图和收敛曲线。三维图中建议重点观察四件事：RRT 是否先给出无碰撞路径；PSO 是否显著调整冗余折点；融合路径是否远离障碍物膨胀边界；最终路径是否存在突兀的高度跳变。
收敛曲线应当整体非增，因为程序记录的是历史全局最优。PSO 阶段通常承担主要的代价下降；BFOA 阶段如果仍有下降，说明 PSO 优质区域附近仍存在可利用的局部改进空间。如果 BFOA 长期完全不改善，也不一定是错误，可能意味着 PSO 已经收敛得较充分，或 BFOA 的步长/种群规模需要调整。
命令窗口会分别输出 RRT、PSO、BFOA 精修和最终路径的综合代价，以及最终路径长度。由于随机优化对初始样本敏感，本文固定 `rng(2025,'twister')` 以便复现实验；如果要做算法性能比较，应更换多个随机种子并统计均值、标准差和成功率，而不是只比较单次运行。
十三、这套模型适合什么场景，不适合什么场景
|
适合 |
原因 |
|
课程设计 / 算法实验 |
结构完整，包含建模、搜索、优化、碰撞检测和绘图 |
|
静态三维障碍物路径规划 |
障碍物与边界在规划期间保持不变 |
|
科研原型与算法融合验证 |
三阶段接口清晰，便于替换 RRT*、QPSO、DE、GWO 等模块 |
|
规则建筑/禁飞区近似 |
长方体障碍物表达简单，计算量可控 |
|
当前模型的边界 |
需要的扩展 |
|
动态障碍物未显式建模 |
加入时间维、速度预测、DWA/MPC 或在线重规划 |
|
未加入无人机完整动力学 |
增加最大速度、加速度、爬升率、最小转弯半径等约束 |
|
障碍物采用长方体近似 |
接入 DEM、点云、体素或多面体几何 |
|
适应度权重依赖人工标定 |
做归一化、多目标 Pareto 优化或自适应权重 |
|
离散碰撞检测仍是近似 |
提高采样密度或改用解析线段-几何体相交检测 |
十四、进一步可以怎么扩展
如果希望继续提高规划质量，可以从三个方向扩展。第一，把 RRT 换成 RRT*、Informed RRT* 或双向 RRT-Connect，提高初始路径质量和搜索效率；第二，把单一加权适应度改成多目标优化，让路径长度、安全裕度、能耗和平滑性形成 Pareto 解集；第三，在静态全局路径基础上增加动态局部规划器，使无人机遇到临时障碍物时能够在线绕行。
若用于真实无人机，还应把航路点几何约束进一步映射到飞行控制约束，例如最大爬升率、横向加速度、航向角变化率、最小航速、续航能耗和通信覆盖。此时“路径最短”只是目标之一，真正的工程目标应是安全、可执行、可复现且计算预算可接受。
总结
PSO-BFOA-RRT 的价值不在于简单叠加三种智能算法，而在于把它们放到最擅长的阶段：RRT 解决复杂自由空间中的可行性，PSO 负责固定维度航路点的全局优化，BFOA 负责优质解附近的局部强化；统一适应度函数则保证三个阶段始终朝同一个工程目标优化。
对三维无人机路径规划而言，真正决定结果可信度的往往不是算法名称，而是路径表示、碰撞检测、安全距离、权重层级、结果快照、失败处理和后处理保护这些实现细节。把这些环节做扎实，融合算法才会从“能跑”提升到“可解释、可复现、可继续扩展”。
参考资料
1. S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning. Technical Report TR 98-11, Iowa State University, 1998.
2. J. Kennedy, R. Eberhart. Particle swarm optimization. Proceedings of ICNN'95, 1995. DOI: 10.1109/ICNN.1995.488968.
3. K. M. Passino. Biomimicry of bacterial foraging for distributed optimization and control. IEEE Control Systems Magazine, 22(3):52-67, 2002. DOI: 10.1109/MCS.2002.1004010.
4. MATLAB 示例实现：本文使用基础矩阵运算、随机数、三维绘图和脚本局部函数组织代码。
转载自 CSDN-专业IT技术社区
原文链接：https://blog.csdn.net/xiaoxingkongyuxi/article/details/164033777
