粒子群算法在机器人路径规划中的MATLAB实现与优化

1. 粒子群算法在机器人路径规划中的实战应用

最近在实验室折腾移动机器人导航系统时，我发现传统A*算法在处理复杂动态环境时存在局限性。经过一番研究，发现粒子群优化（PSO）算法在路径规划领域展现出独特优势。于是我用MATLAB开发了一套完整的解决方案，不仅实现了算法核心，还构建了可视化交互界面，让路径规划过程变得直观可见。

这个项目最大的特点是可以实时调整环境参数：通过鼠标点击就能设置障碍物、起点和终点，算法会立即生成最优路径。这种交互方式特别适合教学演示和算法调试，我在实验室用这套系统给学弟学妹讲解PSO原理时，效果出奇地好。

2. 环境搭建与地图建模

2.1 地图矩阵的巧妙设计

在机器人路径规划中，环境建模是基础工作。我采用20×20的二维矩阵表示地图空间，其中0代表自由区域，1代表障碍物。这种表示方法计算效率高，且与MATLAB的矩阵运算完美契合。

matlab复制map_size = [20,20];
obstacle_density = 0.2;
obs_matrix = randi([0 1], map_size).*(rand(map_size)<obstacle_density);

这段代码的精妙之处在于：

1. randi([0 1], map_size)生成随机0/1矩阵
2. rand(map_size)<obstacle_density生成概率过滤矩阵
3. 通过矩阵乘法实现密度控制，保证障碍物既随机分布又符合预设密度

提示：实际应用中，建议将地图尺寸参数设为变量，方便后续扩展。我在项目中就预留了地图缩放功能，可以随时调整规划空间大小。

2.2 障碍物的可视化处理

为了让障碍物显示更直观，我采用了patch函数进行绘制：

matlab复制function plot_obstacles(obs_matrix)
    [row,col] = find(obs_matrix == 1);
    for i = 1:length(row)
        rectangle('Position',[col(i)-0.5,row(i)-0.5,1,1],...
                 'FaceColor',[0.3 0.3 0.3],'EdgeColor','none');
    end
    axis equal
    grid on
    xlim([0.5 map_size(2)+0.5]);
    ylim([0.5 map_size(1)+0.5]);
end

这种表示方式不仅美观，而且与矩阵坐标一一对应，方便后续的碰撞检测。

3. 粒子群算法核心实现

3.1 粒子类的面向对象设计

采用面向对象方式封装粒子属性，使代码更易维护：

matlab复制classdef Particle < handle
    properties
        position   % 当前控制点坐标
        velocity   % 当前速度
        best_pos   % 个体最优位置
        best_cost  % 个体最优适应度
    end
    
    methods
        function obj = Particle(n_ctrl_points)
            % 初始化控制点坐标（均匀分布在起点和终点之间）
            x = linspace(start_pos(