在matlab里用粒子群优化算法去优化三次均匀B样条构成的轨迹

以下是一个简单的示例，用粒子群优化算法来优化三次均匀B样条构成的轨迹。

首先，定义一个三次均匀B样条，它由一个控制点向量和一个节点向量组成。控制点向量包含每个控制点的坐标，节点向量包含每个节点的位置。

% 定义控制点向量和节点向量
P = [0, 0; 1, 2; 3, 1; 4, 3; 5, 1; 6, 2; 7, 0];
t = linspace(0, 1, size(P, 1) + 2);
t = t(2:end-1);

% 定义三次均匀B样条函数
B = @(t, i, x) (x >= t(i) & x < t(i+1)) .* ((x - t(i)).^3/(t(i+1)-t(i))^3) ...
    + (x >= t(i+1) & x < t(i+2)) .* ((t(i+2)-x).^3/(t(i+2)-t(i+1))^3);

% 绘制原始轨迹
x = linspace(0, 1, 100);
y = zeros(size(x));
for i = 1:length(x)
    y(i) = sum(P(:,1) .* arrayfun(@(j) B(t, j, x(i)), 1:4));
end
plot(x, y, '-k', P(:,1), P(:,2), 'or');

接下来，定义粒子群优化算法的参数，包括粒子数、最大迭代次数、惯性权重等等。

% 定义粒子群优化算法的参数
n_particles = 50;
n_iterations = 500;
w = 0.5;
c1 = 1.5;
c2 = 1.5;

然后，定义目标函数，它将评估每个粒子的轨迹，并返回其适应度值。在这个例子中，我们将使用均方误差作为适应度函数。

% 定义目标函数
target = @(x) sum((x - y).^2);

接下来，初始化粒子的位置和速度。在这个例子中，每个粒子的位置是一个长度为7的向量，其中前4个元素表示B样条的控制点，后3个元素表示节点。

% 初始化粒子的位置和速度
particles = zeros(n_particles, 7);
velocities = zeros(n_particles, 7);
for i = 1:n_particles
    particles(i,:) = [rand(1,4)*7, sort(rand(1,3))];
    velocities(i,:) = [rand(1,4)*2-1, rand(1,3)*2-1];
end

然后，对于每次迭代，计算每个粒子的适应度函数，并更新全局最优粒子和每个粒子的最优位置。

% 迭代粒子群算法
global_best = particles(1,:);
global_best_fitness = inf;
best_particles = particles;
best_particles_fitness = inf(n_particles, 1);
for i = 1:n_iterations
    % 计算每个粒子的适应度函数
    for j = 1:n_particles
        x = particles(j,:);
        y = zeros(size(x));
        for k = 1:length(x)
            y(k) = sum(P(:,1) .* arrayfun(@(l) B(x(5:7), l, x(k)), 1:4));
        end
        fitness = target(y);
        
        % 更新最优位置
        if fitness < best_particles_fitness(j)
            best_particles(j,:) = particles(j,:);
            best_particles_fitness(j) = fitness;
        end
        
        % 更新全局最优位置
        if fitness < global_best_fitness
            global_best = particles(j,:);
            global_best_fitness = fitness;
        end
    end
    
    % 更新粒子的速度和位置
    for j = 1:n_particles
        r1 = rand(1,7);
        r2 = rand(1,7);
        velocities(j,:) = w * velocities(j,:) ...
            + c1 * r1 .* (best_particles(j,:) - particles(j,:)) ...
            + c2 * r2 .* (global_best - particles(j,:));
        particles(j,:) = particles(j,:) + velocities(j,:);
    end
end

最后，绘制优化后的轨迹。

% 绘制优化后的轨迹
x = linspace(0, 1, 100);
y = zeros(size(x));
for i = 1:length(x)
    y(i) = sum(P(:,1) .* arrayfun(@(j) B(global_best(5:7), j, x(i)), 1:4));
end
figure;
plot(x, y, '-k', P(:,1), P(:,2), 'or');

完整的代码如下：

% 定义控制点向量和节点向量
P = [0, 0; 1, 2; 3, 1; 4, 3; 5, 1; 6, 2; 7, 0];
t = linspace(0, 1, size(P, 1) + 2);
t = t(2:end-1);

% 定义三次均匀B样条函数
B = @(t, i, x) (x >= t(i) & x < t(i+1)) .* ((x - t(i)).^3/(t(i+1)-t(i))^3) ...
    + (x >= t(i+1) & x < t(i+2)) .* ((t(i+2)-x).^3/(t(i+2)-t(i+1))^3);

% 绘制原始轨迹
x = linspace(0, 1, 100);
y = zeros(size(x));
for i = 1:length(x)
    y(i) = sum(P(:,1) .* arrayfun(@(j) B(t, j, x(i)), 1:4));
end
plot(x, y, '-k', P(:,1), P(:,2), 'or');

% 定义粒子群优化算法的参数
n_particles = 50;
n_iterations = 500;
w = 0.5;
c1 = 1.5;
c2 = 1.5;

% 定义目标函数
target = @(x) sum((x - y).^2);

% 初始化粒子的位置和速度
particles = zeros(n_particles, 7);
velocities = zeros(n_particles, 7);
for i = 1:n_particles
    particles(i,:) = [rand(1,4)*7, sort(rand(1,3))];
    velocities(i,:) = [rand(1,4)*2-1, rand(1,3)*2-1];
end

% 迭代粒子群算法
global_best = particles(1,:);
global_best_fitness = inf;
best_particles = particles;
best_particles_fitness = inf(n_particles, 1);
for i = 1:n_iterations
    % 计算每个粒子的适应度函数
    for j = 1:n_particles
        x = particles(j,:);
        y = zeros(size(x));
        for k = 1:length(x)
            y(k) = sum(P(:,1) .* arrayfun(@(l) B(x(5:7), l, x(k)), 1:4));
        end
        fitness = target(y);
        
        % 更新最优位置
        if fitness < best_particles_fitness(j)
            best_particles(j,:) = particles(j,:);
            best_particles_fitness(j) = fitness;
        end
        
        % 更新全局最优位置
        if fitness < global_best_fitness
            global_best = particles(j,:);
            global_best_fitness = fitness;
        end
    end
    
    % 更新粒子的速度和位置
    for j = 1:n_particles
        r1 = rand(1,7);
        r2 = rand(1,7);
        velocities(j,:) = w * velocities(j,:) ...
            + c1 * r1 .* (best_particles(j,:) - particles(j,:)) ...
            + c2 * r2 .* (global_best - particles(j,:));
        particles(j,:) = particles(j,:) + velocities(j,:);
    end
end

% 绘制优化后的轨迹
x = linspace(0, 1, 100);
y = zeros(size(x));
for i = 1:length(x)
    y(i) = sum(P(:,1) .* arrayfun(@(j) B(global_best(5:7), j, x(i)), 1:4));
end
figure;
plot(x, y, '-k', P(:,1), P(:,2), 'or');