尧图网站建设 
简介AFSA_Robots.zip是一套面向智能化算法研究与机器人路径规划场景的MATLAB源码包基于人工鱼群算法AFSA解决加工路径的全局优化搜索问题适用于学习群智能优化方法或需要部署轻量级路径规划方案的研究者和工程师。包内共有9个文件以8个.m源文件为主覆盖主程序、鱼群行为控制与辅助函数另有1个.asv自动保存文件。整体仅6KB代码精简模块划分清晰便于快速定位和调试。目前已有179人学习下载。资源提供了完整的AFSA路径规划实现框架从觅食、聚群、追尾、随机游动等鱼群行为建模到目标函数设置、位置更新和最优路径输出均有代码对应通过运行并调整参数可直观观察不同参数对路径搜索效果的影响也能替换适应度函数以适配不同加工约束环境。对于新手是一次快速上手的好机会对研究者也可作为算法改进与对比实验的基石。1. 人工鱼群算法在机器人路径上为什么值得再捡起来人工鱼群AFSA在今天看来不算新但它在中小尺度地图的路径规划场景里依然是一个能出活且不依赖梯度的算法。相比 A* 依赖栅格拓扑、RRT 依赖随机采样AFSA 用鱼群的觅食、聚群、追尾行为去搜索一条从起点到终点的可行路径实现时不需要对地图做复杂的预处理也不需要把障碍物边界拟合成解析式。对初值不敏感、全局搜索能力好是它的两个突出优点所以在无人机路径规划、泊车路径规划、动态避障小车路径规划这类任务里经常能看到把“人工鱼群”和“路径规划”写在同一个标题下的代码包。AFSA_Robots 这类命名规范的工程内部通常就包含地图生成、适应度函数、鱼群行为函数、主循环四块。下面按同样的结构把这套方案讲清楚并直接给出可运行的 MATLAB 实现。2. 人工鱼群算法的行为模型与关键参数2.1 一条人工鱼就是一个候选路径解在路径规划语境里人工鱼的位置就是一组路径点的坐标。给定起点和终点后把两端点之间的路径均匀切分成若干段中间经过的路径点坐标合起来就构成一条鱼的编码向量。举个例子地图大小为 20×20起点[1,1]终点[20,20]。如果中间取 9 个控制点那么每条鱼的位置向量维度是 18对应 9 个(x_i, y_i)。鱼群中的每一条鱼都代表一条从起点到终点的完整路径鱼的位置越优代表这条路径的整体代价越小。这里有几个实现时容易忽略的细节。一是边界修复鱼在移动之后很可能超出地图范围最常见做法是直接截断到地图边界保证所有坐标都落在可行空间内。二是起点和终点不能参与编码它们始终固定只有中间点参与搜索否则优化过程可能把起点终点也推走路径就不连续了。三是中间点数量要适中8~15 个点对 20×20 的地图通常够用点太少转弯能力弱点太多搜索维度膨胀、收敛变慢。2.2 觅食、聚群、追尾和随机四种行为人工鱼群算法的核心是四种行为分别对应四种不同的搜索策略。觅食是局部搜索。鱼在视野visual范围内随机尝试try_number次一旦发现比当前位置代价更低的位置就向那个方向移动一步如果几次尝试都没找到更优位置则随机游走一步。计算公式可以写成X_next X_i (X_j - X_i) / ||X_j - X_i|| * step * rand其中X_j是随机试探到的更优位置step是最大移动步长rand是[0,1]均匀随机数用来给移动加入随机性。聚群是保持群体收敛方向的行为。鱼会观察视野范围内的邻居计算这些邻居的中心位置X_c。如果中心位置的代价比当前鱼更好且邻居数量不拥挤就向中心移动。这样保证鱼群整体朝有希望的区域收拢。追尾是向最优个体学习。鱼在视野内找到代价最低的邻居X_best如果它的位置确实更好且不拥挤就向它移动。这条行为负责加快收敛让好解迅速传播。随机行为是最简单的一条在原地附近随机走一小步。它的价值在于跳出局部最优避免整个鱼群长期卡在同一个局部路径上无法翻身。拥挤度判断是很多人容易漏掉的一环。设当前视野内的伙伴数为n_f鱼群总数为N拥挤度定义为n_f / N。只有拥挤度小于预设的拥挤度因子delta时才允许执行聚群或追尾。否则即使局部很好也要放弃防止所有鱼堆在同一条路径上。求代价极小值时delta通常从 0.618 起步。2.3 与粒子群、遗传算法的差异及选型理由在路径规划里目标函数通常是路径长度和威胁代价的组合函数是非凸、不可导的。遗传算法做交叉变异时需要额外保证子代路径不越界、不产生断点粒子群算法速度更新公式里的全局最优容易让种群过早收敛。人工鱼群的优势在于它同时拥有局部搜索觅食和全局聚集聚群、追尾两条通道参数也相对少。参数含义典型取值N鱼群规模30~50MaxIter最大迭代次数100~300visual视野半径建议取地图宽度或高度的 10%~15%2~4step移动步长一般为 visual 的 0.3~0.5 倍1~2delta拥挤度因子求极小值时取 0.618 附近0.618try_number觅食行为中的试探次数3~5这意味着调试 AFSA 时第一个动作是先把visual和step调到与地图尺寸匹配而不是盲目增大迭代次数。后面的实战部分会专门讲调整顺序。3. 人工鱼群路径规划的目标函数设计3.1 障碍物建模与地图表达要把路径规划问题交给鱼群优化第一步是把地图变成数值可计算的形式。这里使用圆形障碍物表达每条障碍物记录三列数据圆心横坐标、圆心纵坐标、半径。MATLAB 里直接用矩阵表示即可obs [x1, y1, r1; x2, y2, r2];如果地图本身是栅格地图可以先把障碍格膨胀一至两格再用圆近似每个障碍簇。路径点是连续坐标因此检测碰撞时直接计算路径点到圆心的欧氏距离。相比逐格扫描这种方式计算量更小而且能给出“距离障碍还有多远”的连续信号方便鱼群感知危险方向。3.2 路径长度与威胁代价的加权组合鱼群需要一个统一的适应度函数来衡量路径好坏。路径规划里最常见的代价是路径长度和威胁代价的加权和cost w_length * len / L_ref w_threat * threat / T_ref其中len是所有相邻路径点连线长度之和L_ref是起点到终点的直线距离用于归一化长度代价。threat是对障碍物的穿越惩罚T_ref是威胁代价的参考值起到平衡量纲的作用。威胁代价的具体计算方法是对每一条路径线段在线段上均匀采样 10 个点计算每个采样点到每个障碍圆心的距离d如果d小于障碍半径r就累计惩罚值r - d。为了避免鱼群贴着障碍边缘走还可以引入安全半径r_safe r clearance只要距离小于r_safe就产生惩罚。这样路径会自动与障碍物保持一段安全距离实际工程里更实用。完整路径点序列由起点、中间控制点、终点拼接而成。每个中间点都是优化变量鱼群移动时实际修改的是中间点在平面上的位置起点和终点不参与更新。提示威胁代价只惩罚“穿入障碍”或“进入安全半径”不惩罚“离障碍很远”否则鱼群会为了降低威胁代价而绕巨大的弯路路径长度逐渐失控。威胁权重不能设置得过大。3.3 权重取值参考权重直接决定最终路径的性格。长度权重高路径就短但是容易贴着障碍边缘走威胁权重高路径就安全但是总长度会增加。具体取多少要看地图密度。场景w_lengthw_threat效果倾向空旷地图0.850.15短路径优先密集障碍地图0.550.45安全通道优先不确定时0.70.3综合折中实际运行时要留意代价量级。如果地图很大路径长度可能是几十而威胁代价通常只是个位数不归一化会导致长度项完全压过威胁项。所以要先用直线长度L_ref对长度项归一化再用一个经验常数对威胁项缩放。下面代码里直接用固定比值方便复现。4. 用 MATLAB 实现 AFSA 机器人路径规划的核心代码4.1 地图、鱼群和公告板的初始化clear; clc; rng(1); % 固定随机种子便于复现实验 % 地图与端点设置 mapSize 20; startPos [1, 1]; goalPos [20, 20]; % 圆形障碍物 [x, y, r] obs [ 4, 15, 1.5; 14, 6, 1.8; 10, 12, 1.2; 16, 16, 1.0; ]; % 鱼群参数 N 30; % 鱼群规模 MaxIter 150; % 迭代次数 visual 3; % 视野半径 step 1.2; % 移动步长 delta 0.618; % 拥挤度因子 tryNum 4; % 觅食尝试次数 wLength 0.7; wThreat 0.3; nMid 9; % 中间控制点数量 dim nMid * 2; % 每条鱼的位置向量维度 lb ones(1, dim); ub mapSize * ones(1, dim); % 初始化鱼群 for i 1:N fish(i).pos lb (ub - lb) .* rand(1, dim); fish(i).cost computeCost(fish(i).pos, startPos, goalPos, obs, ... wLength, wThreat); end % 公告板 bestCost inf; for i 1:N if fish(i).cost bestCost bestCost fish(i).cost; bestPos fish(i).pos; end end这段代码的初始化逻辑是把所有鱼随机撒在地图范围内每条鱼计算一次初始代价。computeCost是为整个算法服务的核心函数下一节单独说明。公告板保存全局历史最优路径迭代完成后的输出就是bestPos。参数里visual3、step1.2是针对 20×20 地图比较稳妥的选择。如果地图换成 100×100visual至少要放到 10 以上step按比例跟上去。4.2 适应度函数computeCost 的实现function cost computeCost(pos, startPos, goalPos, obs, wLength, wThreat) nMid numel(pos) / 2; midPts reshape(pos, 2, nMid).; pathPts [startPos; midPts; goalPos]; % 计算路径总长度 segLen sum(vecnorm(diff(pathPts, 1, 1), 2, 2)); Lref vecnorm(goalPos - startPos, 2); lenCost segLen / Lref; % 计算威胁代价对每条线段采样 10 个点 threatCost 0; sampleNum 10; for k 1:size(pathPts, 1) - 1 p1 pathPts(k, :); p2 pathPts(k 1, :); % 在当前线段上生成均匀采样点 t linspace(0, 1, sampleNum); samplePts p1 (p2 - p1) .* t; % 检查每个采样点到每个障碍圆心的距离 for s 1:size(obs, 1) d sqrt((samplePts(:,1) - obs(s,1)).^2 ... (samplePts(:,2) - obs(s,2)).^2); % 只惩罚穿入障碍内部的部分r - d 为正时才有威胁 threatCost threatCost sum(max(0, obs(s,3) - d)); end end cost wLength * lenCost wThreat * threatCost; end逻辑说明reshape(pos, 2, nMid).将鱼的位置向量还原成nMid x 2的中间路径点矩阵diff(pathPts, 1, 1)计算每条相邻线段的方向向量vecnorm(..., 2, 2)求出每条线段的长度采样时用linspace(0,1,sampleNum)生成比例系数再线性插值得到线段上的离散点。威胁代价只取max(0, r-d)意味着没有穿入障碍时惩罚为零微分特性也更利于搜索。4.3 四种行为的 MATLAB 函数骨架行为函数的核心是“从当前位置产出一个新位置”。以觅食和聚群两个最常用的为例function newPos forage(pos, visual, step, tryNum, startPos, goalPos, obs, ... lb, ub, wLength, wThreat) bestCost computeCost(pos, startPos, goalPos, obs, wLength, wThreat); newPos pos; for k 1:tryNum % 在视野范围内随机试探 trial pos visual * (2 * rand(size(pos)) - 1); trial max(min(trial, ub), lb); trialCost computeCost(trial, startPos, goalPos, obs, wLength, wThreat); % 找到更优位置则向该方向移动一步 if trialCost bestCost dirVec trial - pos; dirVec dirVec / norm(dirVec); newPos pos dirVec * step * rand; return; end end % 多次试探失败随机游走一步 newPos pos step * (2 * rand(size(pos)) - 1); end代码中的trial是鱼在视野范围里的一次随机猜测试探后得到比当前位置更好的代价鱼就朝试探点方向迈出一步。这里注意“迈一步”和“直接跳到试探点”的区别前者保留鱼的移动惯性有利于维持种群的探索能力后者会退化成随机重采样导致收敛曲线抖动很大。聚群行为的差异在于试探点换成邻居中心function newPos swarm(fish, idx, N, visual, step, delta, startPos, goalPos, ... obs, lb, ub, wLength, wThreat) center zeros(1, numel(fish(idx).pos)); nf 0; for j 1:N if j ~ idx norm(fish(j).pos - fish(idx).pos) visual center center fish(j).pos; nf nf 1; end end newPos fish(idx).pos; if nf 0 return; end center center / nf; centerCost computeCost(center, startPos, goalPos, obs, wLength, wThreat); % 中心更优且不拥挤时才向中心移动 if centerCost fish(idx).cost (nf / N) delta dirVec center - fish(idx).pos; dirVec dirVec / norm(dirVec); newPos fish(idx).pos dirVec * step * rand; end end拥挤度判断就发生在(nf / N) delta这一行。如果没有这个限制鱼群在早期会全部涌向当前最优解附近视野内的伙伴数量迅速变大追尾和聚群行为同时失效算法退化成随机局部搜索。追尾行为结构与聚群类似只是把center替换为视野范围内代价最低的邻居这里不再重复贴完整函数。主循环里可以把三种行为并列执行或随机选一执行。4.4 主循环、路径输出与平滑history zeros(1, MaxIter); for iter 1:MaxIter for i 1:N choice randi(3); if choice 1 newPos forage(fish(i).pos, visual, step, tryNum, ... startPos, goalPos, obs, lb, ub, wLength, wThreat); elseif choice 2 newPos swarm(fish, i, N, visual, step, delta, ... startPos, goalPos, obs, lb, ub, wLength, wThreat); else newPos follow(fish, i, N, visual, step, delta, ... startPos, goalPos, obs, lb, ub, wLength, wThreat); end newPos max(min(newPos, ub), lb); newCost computeCost(newPos, startPos, goalPos, obs, wLength, wThreat); if newCost fish(i).cost fish(i).pos newPos; fish(i).cost newCost; end if fish(i).cost bestCost bestCost fish(i).cost; bestPos fish(i).pos; end end history(iter) bestCost; end % 从 bestPos 还原完整路径并做平滑 midPts reshape(bestPos, 2, nMid).; rawPath [startPos; midPts; goalPos]; t 1:size(rawPath, 1); tt linspace(1, size(rawPath, 1), 200); smoothX pchip(t, rawPath(:, 1), tt); smoothY pchip(t, rawPath(:, 2), tt);主循环里每代对每条鱼只随机执行一种行为这样能让鱼群中不同个体承担不同搜索分工。相比每步执行全部行为这种做法的计算开销小而且不容易让鱼群频繁抖动。history数组记录每代最优代价用来判断算法是否收敛完整。路径平滑用的是pchip分段三次插值它对密集控制点恢复出的路径不会像样条那样产生大范围过冲。平滑后的smoothX和smoothY可以直接画图也可以作为后续局部优化的初始轨迹。5. 调参顺序、收敛验证与局部精修5.1 先调 visual 和 step其他参数靠后AFSA 参数不少但真正左右搜索结果的是visual和step。visual决定鱼能看到多远在地图 20×20、起点到终点距离约 27 的场景下visual3是合理的起点。如果你发现迭代很久路径还是贴着边界或者频繁穿越障碍先检查这两个参数而不是急着把MaxIter改成 500。判断方法输出前 20 代history曲线如果下降非常缓慢且路径在障碍物附近徘徊说明visual太小鱼群走出局部区域的能力太弱。step过大则会让鱼在最优路径附近来回震荡收敛曲线末端出现锯齿状。5.2 用重复实验验证路径质量鱼群算法是随机算法跑一次的结果不可信。完整验证流程至少跑 20 次for r 1:20 [bestPos, bestCost, history] runAFSA(); % 封装主循环 [len, threat] evaluatePath(bestPos); lenList(r) len; costList(r) bestCost; end fprintf(平均路径代价: %.3f\n, mean(costList)); fprintf(路径长度均值: %.3f, 最差: %.3f\n, mean(lenList), max(lenList));如果 20 次结果里代价方差很大说明种群初始化不够均匀或者visual偏小如果每次都能找到近似相同的代价说明地图场景对 AFSA 来说偏简单可以考虑换用更快的确定性算法。5.3 用 patternsearch 做局部精修AFSA 的短板是收敛后期精度不高。一个标准的“两步走”方案是先让 AFSA 搜索全局再把bestPos作为初值交给 MATLAB 自带的patternsearch做局部精修options optimoptions(patternsearch, UseCompletePoll, true); costFun (x) computeCost(x, startPos, goalPos, obs, wLength, wThreat); [posFinal, finalCost] patternsearch(costFun, bestPos, [], [], [], [], ... lb, ub, [], options);patternsearch不需要梯度擅长在已有解附近做精细搜索。AFSA 负责“找到大致对的区域”patternsearch负责把路径拉得更短、离障碍更远。多数情况下最终路径长度和威胁代价都会比单跑 AFSA 有明显的进一步改善尤其适合障碍稀疏、通道较宽的地图场景。本文还有配套的精品资源点击获取 人工鱼群算法在机器人路径规划中的MATLAB实现与调参
简介AFSA_Robots.zip是一套面向智能化算法研究与机器人路径规划场景的MATLAB源码包基于人工鱼群算法AFSA解决加工路径的全局优化搜索问题适用于学习群智能优化方法或需要部署轻量级路径规划方案的研究者和工程师。包内共有9个文件以8个.m源文件为主覆盖主程序、鱼群行为控制与辅助函数另有1个.asv自动保存文件。整体仅6KB代码精简模块划分清晰便于快速定位和调试。目前已有179人学习下载。资源提供了完整的AFSA路径规划实现框架从觅食、聚群、追尾、随机游动等鱼群行为建模到目标函数设置、位置更新和最优路径输出均有代码对应通过运行并调整参数可直观观察不同参数对路径搜索效果的影响也能替换适应度函数以适配不同加工约束环境。对于新手是一次快速上手的好机会对研究者也可作为算法改进与对比实验的基石。1. 人工鱼群算法在机器人路径上为什么值得再捡起来人工鱼群AFSA在今天看来不算新但它在中小尺度地图的路径规划场景里依然是一个能出活且不依赖梯度的算法。相比 A* 依赖栅格拓扑、RRT 依赖随机采样AFSA 用鱼群的觅食、聚群、追尾行为去搜索一条从起点到终点的可行路径实现时不需要对地图做复杂的预处理也不需要把障碍物边界拟合成解析式。对初值不敏感、全局搜索能力好是它的两个突出优点所以在无人机路径规划、泊车路径规划、动态避障小车路径规划这类任务里经常能看到把“人工鱼群”和“路径规划”写在同一个标题下的代码包。AFSA_Robots 这类命名规范的工程内部通常就包含地图生成、适应度函数、鱼群行为函数、主循环四块。下面按同样的结构把这套方案讲清楚并直接给出可运行的 MATLAB 实现。2. 人工鱼群算法的行为模型与关键参数2.1 一条人工鱼就是一个候选路径解在路径规划语境里人工鱼的位置就是一组路径点的坐标。给定起点和终点后把两端点之间的路径均匀切分成若干段中间经过的路径点坐标合起来就构成一条鱼的编码向量。举个例子地图大小为 20×20起点[1,1]终点[20,20]。如果中间取 9 个控制点那么每条鱼的位置向量维度是 18对应 9 个(x_i, y_i)。鱼群中的每一条鱼都代表一条从起点到终点的完整路径鱼的位置越优代表这条路径的整体代价越小。这里有几个实现时容易忽略的细节。一是边界修复鱼在移动之后很可能超出地图范围最常见做法是直接截断到地图边界保证所有坐标都落在可行空间内。二是起点和终点不能参与编码它们始终固定只有中间点参与搜索否则优化过程可能把起点终点也推走路径就不连续了。三是中间点数量要适中8~15 个点对 20×20 的地图通常够用点太少转弯能力弱点太多搜索维度膨胀、收敛变慢。2.2 觅食、聚群、追尾和随机四种行为人工鱼群算法的核心是四种行为分别对应四种不同的搜索策略。觅食是局部搜索。鱼在视野visual范围内随机尝试try_number次一旦发现比当前位置代价更低的位置就向那个方向移动一步如果几次尝试都没找到更优位置则随机游走一步。计算公式可以写成X_next X_i (X_j - X_i) / ||X_j - X_i|| * step * rand其中X_j是随机试探到的更优位置step是最大移动步长rand是[0,1]均匀随机数用来给移动加入随机性。聚群是保持群体收敛方向的行为。鱼会观察视野范围内的邻居计算这些邻居的中心位置X_c。如果中心位置的代价比当前鱼更好且邻居数量不拥挤就向中心移动。这样保证鱼群整体朝有希望的区域收拢。追尾是向最优个体学习。鱼在视野内找到代价最低的邻居X_best如果它的位置确实更好且不拥挤就向它移动。这条行为负责加快收敛让好解迅速传播。随机行为是最简单的一条在原地附近随机走一小步。它的价值在于跳出局部最优避免整个鱼群长期卡在同一个局部路径上无法翻身。拥挤度判断是很多人容易漏掉的一环。设当前视野内的伙伴数为n_f鱼群总数为N拥挤度定义为n_f / N。只有拥挤度小于预设的拥挤度因子delta时才允许执行聚群或追尾。否则即使局部很好也要放弃防止所有鱼堆在同一条路径上。求代价极小值时delta通常从 0.618 起步。2.3 与粒子群、遗传算法的差异及选型理由在路径规划里目标函数通常是路径长度和威胁代价的组合函数是非凸、不可导的。遗传算法做交叉变异时需要额外保证子代路径不越界、不产生断点粒子群算法速度更新公式里的全局最优容易让种群过早收敛。人工鱼群的优势在于它同时拥有局部搜索觅食和全局聚集聚群、追尾两条通道参数也相对少。参数含义典型取值N鱼群规模30~50MaxIter最大迭代次数100~300visual视野半径建议取地图宽度或高度的 10%~15%2~4step移动步长一般为 visual 的 0.3~0.5 倍1~2delta拥挤度因子求极小值时取 0.618 附近0.618try_number觅食行为中的试探次数3~5这意味着调试 AFSA 时第一个动作是先把visual和step调到与地图尺寸匹配而不是盲目增大迭代次数。后面的实战部分会专门讲调整顺序。3. 人工鱼群路径规划的目标函数设计3.1 障碍物建模与地图表达要把路径规划问题交给鱼群优化第一步是把地图变成数值可计算的形式。这里使用圆形障碍物表达每条障碍物记录三列数据圆心横坐标、圆心纵坐标、半径。MATLAB 里直接用矩阵表示即可obs [x1, y1, r1; x2, y2, r2];如果地图本身是栅格地图可以先把障碍格膨胀一至两格再用圆近似每个障碍簇。路径点是连续坐标因此检测碰撞时直接计算路径点到圆心的欧氏距离。相比逐格扫描这种方式计算量更小而且能给出“距离障碍还有多远”的连续信号方便鱼群感知危险方向。3.2 路径长度与威胁代价的加权组合鱼群需要一个统一的适应度函数来衡量路径好坏。路径规划里最常见的代价是路径长度和威胁代价的加权和cost w_length * len / L_ref w_threat * threat / T_ref其中len是所有相邻路径点连线长度之和L_ref是起点到终点的直线距离用于归一化长度代价。threat是对障碍物的穿越惩罚T_ref是威胁代价的参考值起到平衡量纲的作用。威胁代价的具体计算方法是对每一条路径线段在线段上均匀采样 10 个点计算每个采样点到每个障碍圆心的距离d如果d小于障碍半径r就累计惩罚值r - d。为了避免鱼群贴着障碍边缘走还可以引入安全半径r_safe r clearance只要距离小于r_safe就产生惩罚。这样路径会自动与障碍物保持一段安全距离实际工程里更实用。完整路径点序列由起点、中间控制点、终点拼接而成。每个中间点都是优化变量鱼群移动时实际修改的是中间点在平面上的位置起点和终点不参与更新。提示威胁代价只惩罚“穿入障碍”或“进入安全半径”不惩罚“离障碍很远”否则鱼群会为了降低威胁代价而绕巨大的弯路路径长度逐渐失控。威胁权重不能设置得过大。3.3 权重取值参考权重直接决定最终路径的性格。长度权重高路径就短但是容易贴着障碍边缘走威胁权重高路径就安全但是总长度会增加。具体取多少要看地图密度。场景w_lengthw_threat效果倾向空旷地图0.850.15短路径优先密集障碍地图0.550.45安全通道优先不确定时0.70.3综合折中实际运行时要留意代价量级。如果地图很大路径长度可能是几十而威胁代价通常只是个位数不归一化会导致长度项完全压过威胁项。所以要先用直线长度L_ref对长度项归一化再用一个经验常数对威胁项缩放。下面代码里直接用固定比值方便复现。4. 用 MATLAB 实现 AFSA 机器人路径规划的核心代码4.1 地图、鱼群和公告板的初始化clear; clc; rng(1); % 固定随机种子便于复现实验 % 地图与端点设置 mapSize 20; startPos [1, 1]; goalPos [20, 20]; % 圆形障碍物 [x, y, r] obs [ 4, 15, 1.5; 14, 6, 1.8; 10, 12, 1.2; 16, 16, 1.0; ]; % 鱼群参数 N 30; % 鱼群规模 MaxIter 150; % 迭代次数 visual 3; % 视野半径 step 1.2; % 移动步长 delta 0.618; % 拥挤度因子 tryNum 4; % 觅食尝试次数 wLength 0.7; wThreat 0.3; nMid 9; % 中间控制点数量 dim nMid * 2; % 每条鱼的位置向量维度 lb ones(1, dim); ub mapSize * ones(1, dim); % 初始化鱼群 for i 1:N fish(i).pos lb (ub - lb) .* rand(1, dim); fish(i).cost computeCost(fish(i).pos, startPos, goalPos, obs, ... wLength, wThreat); end % 公告板 bestCost inf; for i 1:N if fish(i).cost bestCost bestCost fish(i).cost; bestPos fish(i).pos; end end这段代码的初始化逻辑是把所有鱼随机撒在地图范围内每条鱼计算一次初始代价。computeCost是为整个算法服务的核心函数下一节单独说明。公告板保存全局历史最优路径迭代完成后的输出就是bestPos。参数里visual3、step1.2是针对 20×20 地图比较稳妥的选择。如果地图换成 100×100visual至少要放到 10 以上step按比例跟上去。4.2 适应度函数computeCost 的实现function cost computeCost(pos, startPos, goalPos, obs, wLength, wThreat) nMid numel(pos) / 2; midPts reshape(pos, 2, nMid).; pathPts [startPos; midPts; goalPos]; % 计算路径总长度 segLen sum(vecnorm(diff(pathPts, 1, 1), 2, 2)); Lref vecnorm(goalPos - startPos, 2); lenCost segLen / Lref; % 计算威胁代价对每条线段采样 10 个点 threatCost 0; sampleNum 10; for k 1:size(pathPts, 1) - 1 p1 pathPts(k, :); p2 pathPts(k 1, :); % 在当前线段上生成均匀采样点 t linspace(0, 1, sampleNum); samplePts p1 (p2 - p1) .* t; % 检查每个采样点到每个障碍圆心的距离 for s 1:size(obs, 1) d sqrt((samplePts(:,1) - obs(s,1)).^2 ... (samplePts(:,2) - obs(s,2)).^2); % 只惩罚穿入障碍内部的部分r - d 为正时才有威胁 threatCost threatCost sum(max(0, obs(s,3) - d)); end end cost wLength * lenCost wThreat * threatCost; end逻辑说明reshape(pos, 2, nMid).将鱼的位置向量还原成nMid x 2的中间路径点矩阵diff(pathPts, 1, 1)计算每条相邻线段的方向向量vecnorm(..., 2, 2)求出每条线段的长度采样时用linspace(0,1,sampleNum)生成比例系数再线性插值得到线段上的离散点。威胁代价只取max(0, r-d)意味着没有穿入障碍时惩罚为零微分特性也更利于搜索。4.3 四种行为的 MATLAB 函数骨架行为函数的核心是“从当前位置产出一个新位置”。以觅食和聚群两个最常用的为例function newPos forage(pos, visual, step, tryNum, startPos, goalPos, obs, ... lb, ub, wLength, wThreat) bestCost computeCost(pos, startPos, goalPos, obs, wLength, wThreat); newPos pos; for k 1:tryNum % 在视野范围内随机试探 trial pos visual * (2 * rand(size(pos)) - 1); trial max(min(trial, ub), lb); trialCost computeCost(trial, startPos, goalPos, obs, wLength, wThreat); % 找到更优位置则向该方向移动一步 if trialCost bestCost dirVec trial - pos; dirVec dirVec / norm(dirVec); newPos pos dirVec * step * rand; return; end end % 多次试探失败随机游走一步 newPos pos step * (2 * rand(size(pos)) - 1); end代码中的trial是鱼在视野范围里的一次随机猜测试探后得到比当前位置更好的代价鱼就朝试探点方向迈出一步。这里注意“迈一步”和“直接跳到试探点”的区别前者保留鱼的移动惯性有利于维持种群的探索能力后者会退化成随机重采样导致收敛曲线抖动很大。聚群行为的差异在于试探点换成邻居中心function newPos swarm(fish, idx, N, visual, step, delta, startPos, goalPos, ... obs, lb, ub, wLength, wThreat) center zeros(1, numel(fish(idx).pos)); nf 0; for j 1:N if j ~ idx norm(fish(j).pos - fish(idx).pos) visual center center fish(j).pos; nf nf 1; end end newPos fish(idx).pos; if nf 0 return; end center center / nf; centerCost computeCost(center, startPos, goalPos, obs, wLength, wThreat); % 中心更优且不拥挤时才向中心移动 if centerCost fish(idx).cost (nf / N) delta dirVec center - fish(idx).pos; dirVec dirVec / norm(dirVec); newPos fish(idx).pos dirVec * step * rand; end end拥挤度判断就发生在(nf / N) delta这一行。如果没有这个限制鱼群在早期会全部涌向当前最优解附近视野内的伙伴数量迅速变大追尾和聚群行为同时失效算法退化成随机局部搜索。追尾行为结构与聚群类似只是把center替换为视野范围内代价最低的邻居这里不再重复贴完整函数。主循环里可以把三种行为并列执行或随机选一执行。4.4 主循环、路径输出与平滑history zeros(1, MaxIter); for iter 1:MaxIter for i 1:N choice randi(3); if choice 1 newPos forage(fish(i).pos, visual, step, tryNum, ... startPos, goalPos, obs, lb, ub, wLength, wThreat); elseif choice 2 newPos swarm(fish, i, N, visual, step, delta, ... startPos, goalPos, obs, lb, ub, wLength, wThreat); else newPos follow(fish, i, N, visual, step, delta, ... startPos, goalPos, obs, lb, ub, wLength, wThreat); end newPos max(min(newPos, ub), lb); newCost computeCost(newPos, startPos, goalPos, obs, wLength, wThreat); if newCost fish(i).cost fish(i).pos newPos; fish(i).cost newCost; end if fish(i).cost bestCost bestCost fish(i).cost; bestPos fish(i).pos; end end history(iter) bestCost; end % 从 bestPos 还原完整路径并做平滑 midPts reshape(bestPos, 2, nMid).; rawPath [startPos; midPts; goalPos]; t 1:size(rawPath, 1); tt linspace(1, size(rawPath, 1), 200); smoothX pchip(t, rawPath(:, 1), tt); smoothY pchip(t, rawPath(:, 2), tt);主循环里每代对每条鱼只随机执行一种行为这样能让鱼群中不同个体承担不同搜索分工。相比每步执行全部行为这种做法的计算开销小而且不容易让鱼群频繁抖动。history数组记录每代最优代价用来判断算法是否收敛完整。路径平滑用的是pchip分段三次插值它对密集控制点恢复出的路径不会像样条那样产生大范围过冲。平滑后的smoothX和smoothY可以直接画图也可以作为后续局部优化的初始轨迹。5. 调参顺序、收敛验证与局部精修5.1 先调 visual 和 step其他参数靠后AFSA 参数不少但真正左右搜索结果的是visual和step。visual决定鱼能看到多远在地图 20×20、起点到终点距离约 27 的场景下visual3是合理的起点。如果你发现迭代很久路径还是贴着边界或者频繁穿越障碍先检查这两个参数而不是急着把MaxIter改成 500。判断方法输出前 20 代history曲线如果下降非常缓慢且路径在障碍物附近徘徊说明visual太小鱼群走出局部区域的能力太弱。step过大则会让鱼在最优路径附近来回震荡收敛曲线末端出现锯齿状。5.2 用重复实验验证路径质量鱼群算法是随机算法跑一次的结果不可信。完整验证流程至少跑 20 次for r 1:20 [bestPos, bestCost, history] runAFSA(); % 封装主循环 [len, threat] evaluatePath(bestPos); lenList(r) len; costList(r) bestCost; end fprintf(平均路径代价: %.3f\n, mean(costList)); fprintf(路径长度均值: %.3f, 最差: %.3f\n, mean(lenList), max(lenList));如果 20 次结果里代价方差很大说明种群初始化不够均匀或者visual偏小如果每次都能找到近似相同的代价说明地图场景对 AFSA 来说偏简单可以考虑换用更快的确定性算法。5.3 用 patternsearch 做局部精修AFSA 的短板是收敛后期精度不高。一个标准的“两步走”方案是先让 AFSA 搜索全局再把bestPos作为初值交给 MATLAB 自带的patternsearch做局部精修options optimoptions(patternsearch, UseCompletePoll, true); costFun (x) computeCost(x, startPos, goalPos, obs, wLength, wThreat); [posFinal, finalCost] patternsearch(costFun, bestPos, [], [], [], [], ... lb, ub, [], options);patternsearch不需要梯度擅长在已有解附近做精细搜索。AFSA 负责“找到大致对的区域”patternsearch负责把路径拉得更短、离障碍更远。多数情况下最终路径长度和威胁代价都会比单跑 AFSA 有明显的进一步改善尤其适合障碍稀疏、通道较宽的地图场景。本文还有配套的精品资源点击获取