粒子群算法路径规划c++

粒子群算法（Particle Swarm Optimization，PSO）是一种优化算法，它模拟鸟群觅食的过程，通过不断地调整个体位置，寻找最优解。在路径规划问题中，可以将起点和终点看作粒子群的初始位置和目标位置，通过不断地调整位置，寻找最短的路径。

下面是一个简单的粒子群算法路径规划的 C++ 实现，以二维平面上的路径规划为例：

#include <iostream>
#include <vector>
#include <cmath>
#include <ctime>

using namespace std;

const int MAX_ITERATION = 100; // 最大迭代次数
const int N = 30; // 粒子数量
const double C1 = 2.0; // 学习因子1
const double C2 = 2.0; // 学习因子2
const double W_MAX = 0.9; // 惯性权重上限
const double W_MIN = 0.4; // 惯性权重下限
const double V_MAX = 5.0; // 最大速度
const double V_MIN = -5.0; // 最小速度
const double ERROR_THRESHOLD = 1e-5; // 误差阈值

// 定义一个粒子的结构体
struct Particle {
    vector<double> position; // 当前位置
    vector<double> velocity; // 当前速度
    vector<double> pbest; // 个体最优解
    double pbest_fitness; // 个体最优解对应的适应度值
};

// 计算两点之间的距离
double distance(double x1, double y1, double x2, double y2) {
    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

// 计算适应度值，即路径长度
double fitness(vector<double> position) {
    double length = 0.0;
    for (int i = 0; i < position.size() - 1; i += 2) {
        length += distance(position[i], position[i+1], position[i+2], position[i+3]);
    }
    return length;
}

// 初始化粒子群
void initialize(vector<Particle>& swarm, vector<double>& start, vector<double>& end) {
    srand((unsigned)time(NULL));
    for (int i = 0; i < N; ++i) {
        Particle p;
        for (int j = 0; j < start.size(); ++j) {
            p.position.push_back(start[j] + (end[j] - start[j]) * rand() / (RAND_MAX + 1.0));
            p.velocity.push_back((V_MAX - V_MIN) * rand() / (RAND_MAX + 1.0) + V_MIN);
        }
        p.pbest = p.position;
        p.pbest_fitness = fitness(p.position);
        swarm.push_back(p);
    }
}

// 更新粒子位置和速度
void update(vector<Particle>& swarm, vector<double>& gbest) {
    double w = W_MAX - (W_MAX - W_MIN) * t / MAX_ITERATION; // 计算当前惯性权重
    for (int i = 0; i < N; ++i) {
        for (int j = 0; j < swarm[i].position.size(); ++j) {
            swarm[i].velocity[j] = w * swarm[i].velocity[j] + C1 * rand() / (RAND_MAX + 1.0) * (swarm[i].pbest[j] - swarm[i].position[j]) + C2 * rand() / (RAND_MAX + 1.0) * (gbest[j] - swarm[i].position[j]); // 更新速度
            if (swarm[i].velocity[j] > V_MAX) swarm[i].velocity[j] = V_MAX; // 限制速度范围
            if (swarm[i].velocity[j] < V_MIN) swarm[i].velocity[j] = V_MIN;
            swarm[i].position[j] += swarm[i].velocity[j]; // 更新位置
        }
        double fitness_value = fitness(swarm[i].position);
        if (fitness_value < swarm[i].pbest_fitness) { // 更新个体最优解
            swarm[i].pbest = swarm[i].position;
            swarm[i].pbest_fitness = fitness_value;
        }
        if (fitness_value < fitness(gbest)) { // 更新全局最优解
            gbest = swarm[i].position;
        }
    }
}

int main() {
    vector<double> start = {0, 0};
    vector<double> end = {10, 10};
    vector<Particle> swarm;
    vector<double> gbest = start; // 全局最优解
    double error = ERROR_THRESHOLD + 1;
    int t = 0;
    initialize(swarm, start, end);
    while (t < MAX_ITERATION && error > ERROR_THRESHOLD) {
        update(swarm, gbest);
        error = abs(fitness(gbest) - fitness(end));
        ++t;
    }
    cout << "Shortest path length: " << fitness(gbest) << endl;
    return 0;
}

在上面的代码中，`Particle` 结构体定义了一个粒子的属性，包括当前位置、当前速度、个体最优解和个体最优解对应的适应度值。`initialize` 函数用于初始化粒子群，随机生成粒子群的初始位置和速度，并计算个体最优解和全局最优解。`update` 函数用于更新粒子群的位置和速度，根据粒子的个体最优解和全局最优解，调整粒子的速度和位置。`fitness` 函数用于计算粒子的适应度值，即路径长度。在主函数中，通过循环迭代，不断调整粒子的位置和速度，直到达到最大迭代次数或误差阈值。

需要注意的是，在实际应用中，粒子群算法需要根据具体的问题进行调整和优化，例如学习因子、惯性权重、速度范围等参数的设置。此外，粒子群算法也存在着局部最优解问题，需要通过多次运行或采用其他优化算法进行改进。