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

以下是利用PSO和A*算法解决无碰撞路径问题的MATLAB代码：

% 无碰撞路径规划
clear;clc;

% 初始化参数
nPop = 10; % 粒子群数量
MaxIt = 50; % 迭代次数
w = 1; % 惯性权重
wdamp = 0.99; % 惯性权重衰减系数
c1 = 2; % 个体加速系数
c2 = 2; % 全局加速系数
alpha = 0.1; % 距离因子

% 地图参数设置
map = zeros(30, 30);
map(3, 2:28) = 1;
map(28, 2:28) = 1;
map(4:27, 2) = 1;
map(4:27, 28) = 1;
map(15:20, 6:25) = 1;

% 起点和终点设置
Start = [5, 5];
End = [25, 25];

% A*算法搜索
[path, cost] = AStarSearch(map, Start, End);

% 显示地图和路径
figure(1);
imshow(map);
hold on;
plot(path(:, 2), path(:, 1), 'r', 'LineWidth', 2);

% 初始化粒子群
empty_particle.Position = [];
empty_particle.Velocity = [];
empty_particle.Cost = [];
empty_particle.Best.Position = [];
empty_particle.Best.Cost = [];
particle = repmat(empty_particle, nPop, 1);
GlobalBest.Cost = inf;

for i = 1:nPop
    % 随机初始化粒子位置
    particle(i).Position = [Start; randi([2, 29], 28, 2); End];
    % 随机初始化粒子速度
    particle(i).Velocity = zeros(size(particle(i).Position));
    % 计算粒子成本函数值
    particle(i).Cost = PathCost(particle(i).Position, map, path, alpha);
    % 更新个体历史最优解
    particle(i).Best.Position = particle(i).Position;
    particle(i).Best.Cost = particle(i).Cost;
    % 更新全局历史最优解
    if particle(i).Best.Cost < GlobalBest.Cost
        GlobalBest = particle(i).Best;
    end
end

% 初始化最优解记录变量
BestCosts = zeros(MaxIt, 1);

% 迭代优化
for it = 1:MaxIt
    for i = 1:nPop
        % 计算新速度和位置
        particle(i).Velocity = w*particle(i).Velocity ...
            + c1*rand(size(particle(i).Velocity)).*(particle(i).Best.Position - particle(i).Position) ...
            + c2*rand(size(particle(i).Velocity)).*(GlobalBest.Position - particle(i).Position);
        particle(i).Position = particle(i).Position + particle(i).Velocity;
        % 边界处理
        particle(i).Position = max(particle(i).Position, repmat([2, 2], size(particle(i).Position, 1), 1));
        particle(i).Position = min(particle(i).Position, repmat([29, 29], size(particle(i).Position, 1), 1));
        % 计算新成本函数值
        particle(i).Cost = PathCost(particle(i).Position, map, path, alpha);
        % 更新个体历史最优解
        if particle(i).Cost < particle(i).Best.Cost
            particle(i).Best.Position = particle(i).Position;
            particle(i).Best.Cost = particle(i).Cost;
            % 更新全局历史最优解
            if particle(i).Best.Cost < GlobalBest.Cost
                GlobalBest = particle(i).Best;
            end
        end
    end
    % 记录当前迭代的最优解
    BestCosts(it) = GlobalBest.Cost;
    % 输出迭代信息
    disp(['Iteration ' num2str(it) ': Best Cost = ' num2str(BestCosts(it))]);
    % 更新惯性权重
    w = w * wdamp;
end

% 显示优化后的路径
figure(2);
imshow(map);
hold on;
plot(GlobalBest.Position(:, 2), GlobalBest.Position(:, 1), 'r', 'LineWidth', 2);

% 绘制成本函数值变化曲线
figure(3);
plot(BestCosts, 'LineWidth', 2);
xlabel('Iteration');
ylabel('Best Cost');
grid on;

% 成本函数计算
function cost = PathCost(Pos, map, path, alpha)
    n = size(Pos, 1);
    cost = 0;
    for i = 1:n-1
        % 计算两点距离
        d = sqrt(sum((Pos(i+1, :) - Pos(i, :)).^2));
        % 计算两点连线与障碍物的交点
        [x, y] = bresenham(Pos(i, 2), Pos(i, 1), Pos(i+1, 2), Pos(i+1, 1));
        k = sub2ind(size(map), x, y);
        % 判断路径是否穿过障碍物
        if any(map(k))
            cost = cost + inf;
            return;
        end
        % 计算路径与理想路径的差距
        d_path = min(sqrt(sum((path - repmat(Pos(i, :), size(path, 1), 1)).^2, 2)));
        cost = cost + alpha*d + (1-alpha)*d_path;
    end
end

代码中首先利用A*算法搜索得到无碰撞路径，然后利用PSO算法对路径进行优化。粒子的位置即为路径经过的点，速度即为路径的变化量，成本函数是路径长度与路径与理想路径的差距的加权和，其中权重由参数alpha控制。代码中还包含了成本函数计算和边界处理等函数。