0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 从鸟群迁徙说起

粒子群算法(Particle Swarm Optimization, PSO)的发展历史可以追溯到上世纪90年代初，由美国印第安纳大学的Eberhart和肯塔基大学的Kennedy教授提出，其灵感来源于对鸟群等生物群体行为的观察和研究。鸟群在自然界中展现了许多集体智慧的行为特征，比如鸟群在觅食、迁徙或逃避天敌时表现出协同行动和集体智慧。这些行为背后蕴含着群体成员之间的合作、信息交流和相互影响，从而使整个群体能够以高效的方式完成各种任务。

在PSO算法中，许多算法原理的设计都与鸟群行为密切相关。举例而言：

• 群体协作和信息共享：在鸟群中，鸟类之间会相互沟通、协作以及分享信息，以便更好地寻找食物或者栖息地。类似地，在PSO算法中，每个“粒子”代表了待优化问题的一个潜在解，这些粒子通过相互影响和信息共享来共同搜索最优解。粒子之间不断地交换自身的位置和速度信息，以便通过群体智慧的方式来逐步靠近最佳解。

• 个体适应度和集体最优解：在鸟群中，每只鸟都会根据自身感知到的环境信息和个体经验来调整自己的飞行方向和速度。类似地，PSO算法中的每个粒子也具有自身的适应度值（即目标函数值），它会根据自身的经验和当前群体的最优解来调整自己的位置和速度，以期望更好地逼近最优解。
• 群体智能的搜索行为：鸟群在搜索食物或栖息地时会展现出一种集体智能的搜索行为，即通过相互协作和信息共享来快速找到最佳路径。同样地，PSO算法通过模拟群体的协作和信息共享，使得整个粒子群体能够在解空间中高效地搜索到最优解。

2 粒子群算法基本概念

接下来，我们形式化地描述上面的概念

粒子群算法的主要概念有：

• $M$：粒子群个体数量
• ${\mathbf{x}}_i$：粒子$i$在问题解空间中的广义位置，即${\mathbf{x}}_i={\left[\begin{array}{cccc}{x}_i^1& x_i^2& \cdots & x_i^d\end{array}\right]}^T\left(i=1,2,\cdots ,M\right)$， $x_i^j\in [x_{\min }^j,x_{\max }^j]$，其中$d$为解空间维度，种群初始化方程为
  $$x_i^j=x_{\min }^j+r_0^1\left(x_{\max }^j-x_{\min }^j\right)j=1,2,\cdots ,d$$
  其中$r_0^1$为$[0,1]$上的随机数。
• ${\mathbf{v}}_i$：粒子$i$在问题解空间中的速度向量，即${\mathbf{v}}_i={\left[\begin{array}{cccc}{v}_i^1& v_i^2& \cdots & v_i^d\end{array}\right]}^T\left(i=1,2,\cdots ,M\right)$，速度更新公式为
  $${\mathbf{v}}_i^{\ast }={\mathbf{v}}_i+c_1{r}_0^1\left({\mathbf{p}}_i-{\mathbf{x}}_i\right)+c_2{r}_0^1\left({\mathbf{p}}_g-{\mathbf{x}}_i\right)$$
  其中${\mathbf{p}}_i$为粒子$i$的历史最优位置，按
  $${\mathbf{p}}_i^{\ast }=\{\begin{array}{l}{\mathbf{p}}_i,fit\left({\mathbf{p}}_i\right)⩾fit\left({\mathbf{x}}_i^{\ast }\right)\\ {\mathbf{x}}_i^{\ast },fit\left({\mathbf{p}}_i\right)<fit\left({\mathbf{x}}_i^{\ast }\right)\end{array}$$
  更新；${\mathbf{p}}_g$为粒子群的历史最优位置，按
  p g ∗ = a r g max ⁡ p ∈ { p 1 ∗ , p 2 ∗ , ⋯   , p M ∗ }    f i t ( p ) \boldsymbol{p}_{g}^{*}=\underset{\boldsymbol{p}\in \left\{ \boldsymbol{p}_{1}^{*},\boldsymbol{p}_{2}^{*},\cdots ,\boldsymbol{p}_{M}^{*} \right\}}{\mathrm{arg}\max}\,\,fit\left( \boldsymbol{p} \right) pg∗​=p∈{p1∗​,p2∗​,⋯,pM∗​}argmax​fit(p)
  更新；$c_1$、$c_2$为学习因子，表征粒子个体向其个体最优与群体最优位置移动的加速权重。得到速度向量后即可迭代粒子位置
  p g ∗ = a r g max ⁡ p ∈ { p 1 ∗ , p 2 ∗ , ⋯   , p M ∗ }    f i t ( p ) \boldsymbol{p}_{g}^{*}=\underset{\boldsymbol{p}\in \left\{ \boldsymbol{p}_{1}^{*},\boldsymbol{p}_{2}^{*},\cdots ,\boldsymbol{p}_{M}^{*} \right\}}{\mathrm{arg}\max}\,\,fit\left( \boldsymbol{p} \right) pg∗​=p∈{p1∗​,p2∗​,⋯,pM∗​}argmax​fit(p)
• $fit\left(\cdot \right)$：位置适应度函数；
• ${\mathbf{v}}_{\max }$：速度极限，又称粒子群动态系统的lipschitz条件，对于不符合lipschitz条件的粒子需要进行处理确保算法运行在可行域

3 粒子群算法流程

粒子群算法基本原理如下所示

4 粒子群算法ROS实现

核心代码如下所示

bool PSO::plan(const unsigned char* global_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
               std::vector<Node>& expand)
{
  start_ = std::pair<double, double>(static_cast<double>(start.x_), static_cast<double>(start.y_));
  goal_ = std::pair<double, double>(static_cast<double>(goal.x_), static_cast<double>(goal.y_));
  costmap_ = global_costmap;
  expand.clear();

  // variable initialization
  double init_fitness;
  Particle best_particle;
  PositionSequence init_positions;
  std::vector<Particle> particles;

  // Particle initialization
  for (int i = 0; i < n_particles_; ++i)
  {
    std::vector<std::pair<int, int>> init_position;
    if ((i < n_inherited_) && (inherited_particles_.size() == n_inherited_))
      init_position = inherited_particles_[i].best_pos;
    else
      init_position = init_positions[i];

    std::vector<std::pair<int, int>> init_velocity(point_num_, std::make_pair(0, 0));

    // Calculate fitness
    init_fitness = calFitnessValue(init_position);

    if ((i == 0) || (init_fitness > best_particle.fitness))
    {
      best_particle.fitness = init_fitness;
      best_particle.position = init_position;
    }
    // Create and add particle objects to containers
    particles.emplace_back(init_position, init_velocity, init_fitness);
  }

  // random data
  std::random_device rd;
  std::mt19937 gen(rd());

  // Iterative optimization
  for (size_t iter = 0; iter < max_iter_; iter++)
  {
    std::vector<std::thread> particle_list = std::vector<std::thread>(n_particles_);
    for (size_t i = 0; i < n_particles_; ++i)
      particle_list[i] = std::thread(&PSO::optimizeParticle, this, std::ref(particles[i]), std::ref(best_particle),
                                     std::ref(gen), std::ref(expand));
    for (size_t i = 0; i < n_particles_; ++i)
      particle_list[i].join();
  }

  // Generating Paths from Optimal Particles
 ...

  return !path.empty();
}

上面的绿色符号就是粒子群

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇