粒子群算法matlab路径规划系统

粒子群算法（Particle Swarm Optimization，PSO）是一种基于群体智能的优化算法，常用于路径规划等问题。下面是一个使用Matlab实现的粒子群算法路径规划系统的示例：

% 定义目标点和起始点
goal = [10, 10];
start = [1, 1];

% 定义地图
map = zeros(20, 20);
map(5:15, 5:15) = 1;

% 定义粒子群算法参数
num_particles = 50;
max_iterations = 100;
c1 = 2;
c2 = 2;
w = 0.7;

% 初始化粒子群
particles = repmat(struct('position', [], 'velocity', [], 'cost', [], 'best_position', [], 'best_cost', []), num_particles, 1);
for i = 1:num_particles
    particles(i).position = start + rand(1, 2) * (goal - start);
    particles(i).velocity = rand(1, 2) * 0.1;
    particles(i).cost = path_cost(particles(i).position, goal, map);
    particles(i).best_position = particles(i).position;
    particles(i).best_cost = particles(i).cost;
end

% 初始化全局最优解
global_best_position = particles(1).position;
global_best_cost = particles(1).cost;

% 迭代优化
for iter = 1:max_iterations
    for i = 1:num_particles
        % 更新粒子速度和位置
        particles(i).velocity = w * particles(i).velocity + c1 * rand(1, 2) .* (particles(i).best_position - particles(i).position) + c2 * rand(1, 2) .* (global_best_position - particles(i).position);
        particles(i).position = particles(i).position + particles(i).velocity;
        
        % 限制粒子位置在地图内
        particles(i).position = max(particles(i).position, [1, 1]);
        particles(i).position = min(particles(i).position, [20, 20]);
        
        % 计算粒子代价
        particles(i).cost = path_cost(particles(i).position, goal, map);
        
        % 更新粒子最优解
        if particles(i).cost < particles(i).best_cost
            particles(i).best_position = particles(i).position;
            particles(i).best_cost = particles(i).cost;
        end
        
        % 更新全局最优解
        if particles(i).best_cost < global_best_cost
            global_best_position = particles(i).best_position;
            global_best_cost = particles(i).best_cost;
        end
    end
    
    % 绘制路径
    plot_path(start, goal, global_best_position, map);
end

% 计算路径代价
function cost = path_cost(position, goal, map)
    path = [position; goal];
    cost = 0;
    for i = 1:size(path, 1)-1
        if ~check_collision(path(i,:), path(i+1,:), map)
            cost = cost + norm(path(i,:) - path(i+1,:));
        else
            cost = Inf;
            break;
        end
    end
end

% 检查路径是否与障碍物相交
function collision = check_collision(p1, p2, map)
    collision = false;
    line = linspace(p1, p2, 10);
    for i = 1:size(line, 1)-1
        if map(round(line(i,2)), round(line(i,1))) == 1
            collision = true;
            break;
        end
    end
end

% 绘制路径和地图
function plot_path(start, goal, path, map)
    clf;
    hold on;
    axis equal;
    axis([0 21 0 21]);
    for i = 1:size(map, 1)
        for j = 1:size(map, 2)
            if map(i, j) == 1
                fill([j-1 j j j-1], [i-1 i-1 i i], 'k');
            end
        end
    end
    plot(start(1), start(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
    plot(goal(1), goal(2), 'gx', 'MarkerSize', 10, 'LineWidth', 2);
    plot(path(:,1), path(:,2), 'b-', 'LineWidth', 2);
    drawnow;
end

该示例中，我们首先定义了目标点和起始点，以及一个简单的地图。然后定义了粒子群算法的参数，包括粒子数、最大迭代次数、加速度常数等。接着初始化了粒子群，并进行迭代优化，更新粒子速度和位置，并计算粒子的代价。最后绘制了路径和地图。