利用PSO算法解决无碰撞路径问题的MATLAB代码

下面是一个利用PSO算法解决无碰撞路径问题的MATLAB代码示例：

% 初始化障碍物、起点和终点
obs = [1 3; 2 5; 3 4; 4 6; 5 2]; % 障碍物坐标
start_pos = [0 0]; % 起点坐标
end_pos = [6 6]; % 终点坐标

% 初始化PSO参数
num_particles = 50; % 粒子数量
max_iters = 100; % 迭代次数
w = 0.8; % 惯性权重
c1 = 2; % 自我认知因子
c2 = 2; % 社会认知因子
v_max = 1; % 最大速度限制

% 初始化粒子位置和速度
positions = rand(num_particles, 2) * 6; % 随机初始化粒子位置
velocities = zeros(num_particles, 2); % 初始化粒子速度

% 计算粒子适应度函数值
fitness_values = zeros(num_particles, 1);
for i = 1:num_particles
    fitness_values(i) = fitness_func(positions(i,:), obs, start_pos, end_pos);
end

% 初始化全局最优解
global_best_pos = positions(1,:);
global_best_fitness = fitness_values(1);

% 迭代PSO算法
for iter = 1:max_iters
    % 更新粒子速度和位置
    for i = 1:num_particles
        % 计算新速度
        new_velocity = w * velocities(i,:) + ...
            c1 * rand(1,2) .* (best_positions(i,:) - positions(i,:)) + ...
            c2 * rand(1,2) .* (global_best_pos - positions(i,:));
        
        % 限制速度范围
        new_velocity = min(new_velocity, v_max);
        new_velocity = max(new_velocity, -v_max);
        
        % 更新粒子位置
        new_position = positions(i,:) + new_velocity;
        
        % 检查新位置是否在边界内
        if new_position(1) < 0 || new_position(1) > 6 || ...
                new_position(2) < 0 || new_position(2) > 6
            new_position = positions(i,:);
        end
        
        % 检查新位置是否与障碍物碰撞
        if check_collision(new_position, obs)
            new_position = positions(i,:);
        end
        
        % 更新粒子位置和速度
        positions(i,:) = new_position;
        velocities(i,:) = new_velocity;
        
        % 更新个体最优解
        if fitness_func(new_position, obs, start_pos, end_pos) < fitness_values(i)
            best_positions(i,:) = new_position;
            fitness_values(i) = fitness_func(new_position, obs, start_pos, end_pos);
        end
    end
    
    % 更新全局最优解
    [min_fitness, min_index] = min(fitness_values);
    if min_fitness < global_best_fitness
        global_best_pos = best_positions(min_index,:);
        global_best_fitness = min_fitness;
    end
end

% 输出无碰撞路径
path = reconstruct_path(global_best_pos, best_positions, obs, start_pos, end_pos);
disp(path);

% --------------------
% 辅助函数
% --------------------

function fitness = fitness_func(position, obs, start_pos, end_pos)
    % 计算路径长度和碰撞次数
    path = reconstruct_path(position, [], obs, start_pos, end_pos);
    path_length = path_cost(path);
    collision_count = collision_check(path, obs);
    
    % 计算适应度函数值
    fitness = path_length + collision_count;
end

function path = reconstruct_path(position, best_positions, obs, start_pos, end_pos)
    % 重建路径
    if isempty(best_positions)
        path = [start_pos; position; end_pos];
    else
        path = [start_pos; position];
        [~, index] = min(sum((best_positions - repmat(position, size(best_positions,1), 1)).^2, 2));
        path = [path; best_positions(index:end,:); end_pos];
    end
    
    % 检查路径是否与障碍物碰撞
    if collision_check(path, obs)
        path = [];
    end
end

function cost = path_cost(path)
    % 计算路径长度
    cost = sum(sqrt(sum(diff(path).^2, 2)));
end

function count = collision_check(path, obs)
    % 计算路径与障碍物的碰撞次数
    count = 0;
    for i = 2:size(path,1)-1
        for j = 1:size(obs,1)
            if check_line_intersection(path(i,:), path(i-1,:), obs(j,:), j < size(obs,1))
                count = count + 1;
                break;
            end
        end
    end
end

function flag = check_collision(position, obs)
    % 检查点是否与障碍物碰撞
    flag = false;
    for i = 1:size(obs,1)
        if norm(position - obs(i,:)) < 0.5
            flag = true;
            break;
        end
    end
end

function flag = check_line_intersection(p1, p2, q1, is_last)
    % 检查线段是否与障碍物相交
    flag = false;
    for i = 1:2
        if i == 1
            p = p1;
            q = q1;
        else
            p = p2;
            q = q1;
        end
        
        if ~is_last || i == 1
            for j = 1:size(q,1)-1
                if check_line_intersection_helper(p, q(j,:), q(j+1,:))
                    flag = true;
                    break;
                end
            end
        end
        
        if flag
            break;
        end
    end
end

function flag = check_line_intersection_helper(p, q1, q2)
    % 检查两条线段是否相交
    flag = false;
    a = q2 - q1;
    b = p - q1;
    c = [0 -a(1)];
    d = [b(1)*a(2)-b(2)*a(1) b(1)*a(1)+b(2)*a(2)];
    s = d(1) / norm(c);
    t = d(2) / dot(a,a);
    if s >= 0 && s <= 1 && t >= 0 && t <= 1
        flag = true;
    end
end

该代码包括了PSO算法的核心逻辑和一些辅助函数。其中，fitness_func函数计算粒子的适应度函数值，reconstruct_path函数根据粒子位置和个体最优解重建路径，collision_check函数计算路径与障碍物的碰撞次数，check_collision函数检查点是否与障碍物碰撞，check_line_intersection函数检查两条线段是否相交。