粒子群优化算法在MATLAB中实现移动机器人最优路径规划

本文将介绍如何使用粒子群优化（PSO）算法在MATLAB中实现移动机器人的最优路径规划。PSO是一种基于群体智能的优化算法，特别适合解决路径规划这类复杂优化问题。

移动机器人路径规划的目标是在包含障碍物的环境中，找到从起点到终点的最优路径。最优性通常考虑：

1. 路径长度最短
2. 安全性最高（远离障碍物）
3. 路径平滑度

MATLAB实现

主程序：PSO路径规划

%% 粒子群优化算法实现移动机器人路径规划
clear; clc; close all;

% 设置随机数种子保证结果可重现
rng(42);

%% 环境设置
map_size = [0, 100]; % 地图尺寸 [x_min, x_max; y_min, y_max]
start_point = [10, 10]; % 起点坐标
goal_point = [90, 90];  % 终点坐标
num_obstacles = 15;     % 障碍物数量
obstacle_radius = 5;    % 障碍物半径

% 生成随机障碍物
obstacles = generate_obstacles(map_size, num_obstacles, obstacle_radius);

% 可视化环境
figure(1);
plot_environment(start_point, goal_point, obstacles, map_size);
title('环境设置');
xlabel('X坐标'); ylabel('Y坐标');

%% PSO参数设置
n_particles = 50;     % 粒子数量
n_dims = 5;           % 路径点数量（不包括起点终点）
max_iter = 100;       % 最大迭代次数
w = 0.8;              % 惯性权重
c1 = 2.0;             % 个体学习因子
c2 = 2.0;             % 群体学习因子

% 初始化粒子群
particles = initialize_particles(n_particles, n_dims, map_size, start_point, goal_point);

% 初始化全局最优
global_best.position = [];
global_best.cost = inf;

% 存储每个粒子的历史最优位置和成本
particle_best.position = particles;
particle_best.cost = inf(1, n_particles);

% 迭代过程记录
history.best_cost = zeros(1, max_iter);
history.mean_cost = zeros(1, max_iter);

%% PSO主循环
for iter = 1:max_iter
    % 计算每个粒子的适应度
    for i = 1:n_particles
        % 获取当前路径点
        path_points = reshape(particles(i, :), 2, [])';
        full_path = [start_point; path_points; goal_point];
        
        % 计算路径成本
        cost = path_cost(full_path, obstacles);
        
        % 更新个体最优
        if cost < particle_best.cost(i)
            particle_best.position(i, :) = particles(i, :);
            particle_best.cost(i) = cost;
        end
        
        % 更新全局最优
        if cost < global_best.cost
            global_best.position = particles(i, :);
            global_best.cost = cost;
        end
    end
    
    % 记录迭代信息
    history.best_cost(iter) = global_best.cost;
    history.mean_cost(iter) = mean(particle_best.cost);
    
    % 更新粒子速度和位置
    for i = 1:n_particles
        % 计算新速度
        r1 = rand(1, 2*n_dims);
        r2 = rand(1, 2*n_dims);
        
        velocity = w * particles(i, :) + ...
                   c1 * r1 .* (particle_best.position(i, :) - particles(i, :)) + ...
                   c2 * r2 .* (global_best.position - particles(i, :));
        
        % 更新位置
        particles(i, :) = particles(i, :) + velocity;
        
        % 边界处理
        particles(i, :) = max(particles(i, :), repmat([map_size(1), map_size(1)], 1, n_dims));
        particles(i, :) = min(particles(i, :), repmat([map_size(2), map_size(2)], 1, n_dims));
    end
    
    % 每10次迭代显示当前最优路径
    if mod(iter, 10) == 0
        figure(1);
        plot_best_path(global_best.position, start_point, goal_point, obstacles, iter);
    end
end

%% 结果显示
% 提取最优路径
best_path_points = reshape(global_best.position, 2, [])';
best_full_path = [start_point; best_path_points; goal_point];

% 平滑路径
smoothed_path = smooth_path(best_full_path);

% 绘制最终结果
figure(2);
subplot(2,1,1);
plot_environment(start_point, goal_point, obstacles, map_size);
plot_path(best_full_path, '初始最优路径', 'b-o');
title('PSO优化路径');

subplot(2,1,2);
plot_environment(start_point, goal_point, obstacles, map_size);
plot_path(smoothed_path, '平滑后路径', 'r-s');
title('路径平滑处理');

% 绘制收敛曲线
figure(3);
plot(1:max_iter, history.best_cost, 'b-', 'LineWidth', 1.5);
hold on;
plot(1:max_iter, history.mean_cost, 'r--', 'LineWidth', 1.5);
xlabel('迭代次数');
ylabel('路径成本');
title('PSO收敛曲线');
legend('全局最优成本', '粒子平均最优成本');
grid on;

%% 输出路径信息
fprintf('路径规划完成！\n');
fprintf('最终路径成本: %.2f\n', global_best.cost);
fprintf('路径长度: %.2f 单位\n', path_length(best_full_path));
fprintf('平滑后路径长度: %.2f 单位\n', path_length(smoothed_path));

%% 辅助函数定义
function obstacles = generate_obstacles(map_size, num_obstacles, radius)
    % 生成随机障碍物
    obstacles = zeros(num_obstacles, 3); % [x, y, radius]
    for i = 1:num_obstacles
        valid = false;
        while ~valid
            x = map_size(1) + (map_size(2)-map_size(1)) * rand();
            y = map_size(1) + (map_size(2)-map_size(1)) * rand();
            r = radius * (0.8 + 0.4*rand()); % 随机半径
            
            % 确保障碍物不重叠
            overlap = false;
            for j = 1:i-1
                dist = sqrt((x - obstacles(j,1))^2 + (y - obstacles(j,2))^2);
                if dist < (r + obstacles(j,3))
                    overlap = true;
                    break;
                end
            end
            
            if ~overlap
                obstacles(i, :) = [x, y, r];
                valid = true;
            end
        end
    end
end

function plot_environment(start, goal, obstacles, map_size)
    % 绘制环境
    cla; hold on;
    
    % 绘制起点和终点
    plot(start(1), start(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
    plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
    text(start(1), start(2)-5, '起点', 'FontSize', 10);
    text(goal(1), goal(2)+5, '终点', 'FontSize', 10);
    
    % 绘制障碍物
    for i = 1:size(obstacles, 1)
        viscircles([obstacles(i,1), obstacles(i,2)], obstacles(i,3), 'Color', 'k');
    end
    
    % 设置图形属性
    axis equal;
    axis([map_size(1), map_size(2), map_size(1), map_size(2)]);
    grid on;
    box on;
end

function particles = initialize_particles(n_particles, n_dims, map_size, start, goal)
    % 初始化粒子群
    particles = zeros(n_particles, 2*n_dims);
    
    for i = 1:n_particles
        % 在起点和终点之间生成路径点
        for j = 1:n_dims
            % 在起点和终点之间的直线上生成点
            t = j/(n_dims+1);
            base_x = start(1) + t*(goal(1)-start(1));
            base_y = start(2) + t*(goal(2)-start(2));
            
            % 添加随机扰动
            particles(i, 2*j-1) = base_x + (rand()-0.5)*20;
            particles(i, 2*j) = base_y + (rand()-0.5)*20;
        end
    end
end

function cost = path_cost(path, obstacles)
    % 计算路径成本
    alpha = 1.0; % 路径长度权重
    beta = 0.5;  % 安全性权重
    gamma = 0.3; % 平滑度权重
    
    % 计算路径长度
    length_cost = path_length(path);
    
    % 计算安全性成本（与障碍物的最小距离）
    safety_cost = 0;
    min_distance = inf;
    for i = 1:(size(path,1)-1)
        for j = 1:size(obstacles,1)
            dist = point_to_line_distance(obstacles(j,1:2), path(i,:), path(i+1,:));
            if dist < min_distance
                min_distance = dist;
            end
            
            % 如果路径穿过障碍物，增加惩罚
            if dist < obstacles(j,3)
                safety_cost = safety_cost + 100 * (obstacles(j,3) - dist);
            end
        end
    end
    safety_cost = safety_cost + 1/(min_distance + 0.01); % 避免除以0
    
    % 计算路径平滑度（角度变化总和）
    smooth_cost = 0;
    for i = 2:(size(path,1)-1)
        v1 = path(i,:) - path(i-1,:);
        v2 = path(i+1,:) - path(i,:);
        angle = acos(dot(v1,v2)/(norm(v1)*norm(v2)+eps));
        smooth_cost = smooth_cost + angle;
    end
    
    % 总成本
    cost = alpha * length_cost + beta * safety_cost + gamma * smooth_cost;
end

function len = path_length(path)
    % 计算路径长度
    len = 0;
    for i = 1:(size(path,1)-1)
        len = len + norm(path(i,:) - path(i+1,:));
    end
end

function dist = point_to_line_distance(point, line_start, line_end)
    % 计算点到线段的距离
    v = line_end - line_start;
    w = point - line_start;
    
    c1 = dot(w,v);
    if c1 <= 0
        dist = norm(point - line_start);
        return;
    end
    
    c2 = dot(v,v);
    if c2 <= c1
        dist = norm(point - line_end);
        return;
    end
    
    b = c1 / c2;
    pb = line_start + b * v;
    dist = norm(point - pb);
end

function plot_path(path, label, style)
    % 绘制路径
    plot(path(:,1), path(:,2), style, 'LineWidth', 1.5, 'MarkerSize', 6, 'DisplayName', label);
    legend show;
end

function plot_best_path(position, start, goal, obstacles, iter)
    % 绘制当前最优路径
    path_points = reshape(position, 2, [])';
    full_path = [start; path_points; goal];
    
    plot_environment(start, goal, obstacles, [0, 100]);
    plot_path(full_path, sprintf('迭代 %d', iter), 'b-o');
    title(sprintf('PSO路径规划 (迭代 %d/%d)', iter, 100));
    drawnow;
end

function smoothed_path = smooth_path(path)
    % 使用样条插值平滑路径
    n_points = size(path,1);
    original_length = linspace(0,1,n_points);
    new_length = linspace(0,1,3*n_points); % 增加路径点使路径更平滑
    
    % 对x和y坐标分别进行样条插值
    smoothed_x = spline(original_length, path(:,1), new_length);
    smoothed_y = spline(original_length, path(:,2), new_length);
    
    smoothed_path = [smoothed_x', smoothed_y'];
end

算法详解

1. 环境建模

环境建模包括：

• 定义地图边界（0-100单位）
• 设置起点和终点
• 随机生成障碍物（确保不重叠）

% 环境设置
map_size = [0, 100]; % 地图尺寸 [x_min, x_max; y_min, y_max]
start_point = [10, 10]; % 起点坐标
goal_point = [90, 90];  % 终点坐标
num_obstacles = 15;     % 障碍物数量
obstacle_radius = 5;    % 障碍物半径

% 生成随机障碍物
obstacles = generate_obstacles(map_size, num_obstacles, obstacle_radius);

2. 粒子表示

每个粒子代表一条可能的路径：

• 路径由起点、多个中间点和终点组成
• 粒子位置编码为中间点的坐标序列

% 初始化粒子群
particles = zeros(n_particles, 2*n_dims);

for i = 1:n_particles
    for j = 1:n_dims
        % 在起点和终点之间的直线上生成点
        t = j/(n_dims+1);
        base_x = start(1) + t*(goal(1)-start(1));
        base_y = start(2) + t*(goal(2)-start(2));
        
        % 添加随机扰动
        particles(i, 2*j-1) = base_x + (rand()-0.5)*20;
        particles(i, 2*j) = base_y + (rand()-0.5)*20;
    end
end

3. 适应度函数

适应度函数综合考虑三个因素：

1. 路径长度（越短越好）
2. 路径安全性（离障碍物越远越好）
3. 路径平滑度（角度变化越小越好）

function cost = path_cost(path, obstacles)
    alpha = 1.0; % 路径长度权重
    beta = 0.5;  % 安全性权重
    gamma = 0.3; % 平滑度权重
    
    % 计算路径长度
    length_cost = path_length(path);
    
    % 计算安全性成本
    safety_cost = 0;
    min_distance = inf;
    for i = 1:(size(path,1)-1)
        for j = 1:size(obstacles,1)
            dist = point_to_line_distance(obstacles(j,1:2), path(i,:), path(i+1,:));
            if dist < min_distance
                min_distance = dist;
            end
            
            % 如果路径穿过障碍物，增加惩罚
            if dist < obstacles(j,3)
                safety_cost = safety_cost + 100 * (obstacles(j,3) - dist);
            end
        end
    end
    safety_cost = safety_cost + 1/(min_distance + 0.01);
    
    % 计算路径平滑度
    smooth_cost = 0;
    for i = 2:(size(path,1)-1)
        v1 = path(i,:) - path(i-1,:);
        v2 = path(i+1,:) - path(i,:);
        angle = acos(dot(v1,v2)/(norm(v1)*norm(v2)+eps));
        smooth_cost = smooth_cost + angle;
    end
    
    % 总成本
    cost = alpha * length_cost + beta * safety_cost + gamma * smooth_cost;
end

4. PSO优化过程

PSO算法通过迭代更新粒子位置：

1. 评估每个粒子的适应度
2. 更新个体最优和全局最优
3. 根据PSO公式更新速度和位置

% PSO主循环
for iter = 1:max_iter
    % 评估适应度
    for i = 1:n_particles
        path_points = reshape(particles(i, :), 2, [])';
        full_path = [start_point; path_points; goal_point];
        cost = path_cost(full_path, obstacles);
        
        % 更新个体最优
        if cost < particle_best.cost(i)
            particle_best.position(i, :) = particles(i, :);
            particle_best.cost(i) = cost;
        end
        
        % 更新全局最优
        if cost < global_best.cost
            global_best.position = particles(i, :);
            global_best.cost = cost;
        end
    end
    
    % 更新粒子速度和位置
    for i = 1:n_particles
        r1 = rand(1, 2*n_dims);
        r2 = rand(1, 2*n_dims);
        
        velocity = w * particles(i, :) + ...
                   c1 * r1 .* (particle_best.position(i, :) - particles(i, :)) + ...
                   c2 * r2 .* (global_best.position - particles(i, :));
        
        particles(i, :) = particles(i, :) + velocity;
        
        % 边界处理
        particles(i, :) = max(particles(i, :), repmat([map_size(1), map_size(1)], 1, n_dims));
        particles(i, :) = min(particles(i, :), repmat([map_size(2), map_size(2)], 1, n_dims));
    end
end

5. 路径平滑处理

使用样条插值对优化后的路径进行平滑处理：

function smoothed_path = smooth_path(path)
    n_points = size(path,1);
    original_length = linspace(0,1,n_points);
    new_length = linspace(0,1,3*n_points);
    
    smoothed_x = spline(original_length, path(:,1), new_length);
    smoothed_y = spline(original_length, path(:,2), new_length);
    
    smoothed_path = [smoothed_x', smoothed_y'];
end

参考代码 粒子群优化算法在MATLAB中实现移动机器人最优路径规划 youwenfan.com/contentcnc/83786.html

结果分析

程序运行后会产生三个图形：

1. 环境设置图：显示起点、终点和障碍物分布

2. 路径规划结果：

   • 上方子图：PSO优化的初始路径
   • 下方子图：平滑处理后的最终路径

3. 收敛曲线：显示PSO算法的收敛过程

   • 蓝色线：全局最优路径成本
   • 红色虚线：粒子平均最优成本

粒子群优化算法为移动机器人路径规划提供了高效灵活的解决方案，结合MATLAB强大的数值计算和可视化能力，可以快速实现和验证各种路径规划策略。