六自由度机器人（机械臂）运动学建模及运动规划系列——避障路径规划算法补充：粒子群算法（PSO）

上一篇介绍了遗传算法，本篇接着介绍应用于路径规划的另一种算法——粒子群算法（PSO），主要介绍算法的理论基础以及实现流程等。

1. 算法起源与理论基础

粒子群算法是可应用于路径规划的一种常用算法，最早源于对鸟群捕食行为的研究。当一块区域内只有一块食物时，鸟群不知道食物的具体位置，但知道食物与他们自身的距离有多远。那么，鸟群找到食物的最优策略为：确定距离食物最近的鸟的距离，然后在其附近搜素。

粒子群算法的具体描述为：在空间中首先随机给出一群粒子，每个粒子都有自己的位置与速度属性，根据具体的优化目标，规定粒子的适应度计算函数，通过不断更新粒子的位置与速度属性进行迭代，将整个粒子群的最优适应度逐渐提高，最终得到近似的问题最优解。

2. 算法实现流程

（1）粒子群初始化

首先确定粒子群的粒子个数n，最大迭代次数max，设置粒子的位置和速度范围xlim，vlim，除此以外，需要确定学习因子c_1，c_2以及权重系数w的大小，用于第（3）步中粒子位置与速度属性的更新。

第一步的最后，还需要对种群中每个粒子的速度和位置进行初始化。初始化位置属性时，可采用在问题规定区域内随机生成的方式，初始化速度属性时，在速度限制范围内随机取值，即：

式中，xlim_H与xlim_L分别代表位置范围的上下限，rand为0到1之间的随机数。

（2）粒子群适应度计算

根据问题的优化目标，确定适应度函数。对路径规划问题，可将适应度函数设置为与路径长度负相关的函数，如：

式中，cons为常数影响因子，可自行设置，D为路径长度。
确定适应度函数之后，对粒子群所有个体的适应度进行计算，根据结果对个体历史最优位置X_Pbest进行更新，同时确定当前种群的最佳适应度，并对整个粒子群的历史最优位置X_Gbest 进行更新。

（3）速度与位置属性更新

粒子群算法的速度与位置更新方式是确定的，具体过程如下：

式中，下标为pre，new分别代表更新前，后的粒子属性，c_1，c_2，w，X_Pbest，X_Gbest以及rand同（1），（2）步骤中的定义。
更新后，需要考虑边界条件xlim与vlim，对X_new与V_new进行判断，若更新后的位置与速度属性超出边界，则将边界值作为新的位置/速度属性。
（4）记录最佳适应度值
将本次迭代过程中粒子群的最佳适应度值记录，并对历史最佳适应度值进行更新。
至此，一次迭代过程完成。

整个算法的实现流程如下图：

3. 路径规划应用示例

与上一篇中介绍遗传算法的示例相同，路径规划问题如图：

（1）粒子群初始化

对于此路径规划问题，可以用每个粒子的维度来表示路径中间点的个数，这里设置为dim，同时，规定每个维度由x,y两个坐标构成。
接着，在位置允许范围内，随机生成每个粒子dim个维度的横纵坐标，同时设置粒子的最大速度为V_max。

（2）适应度计算

与上一篇
路径规划算法之——遗传算法
中的流程相似，这里同样考虑到碰撞问题，规定相同的碰撞函数，故适应度函数设置为：

式中：C为碰撞因子，如果碰撞函数检测到路径与障碍物碰撞，则C为0，否则C为1。

（3）速度与位置属性更新

利用上述2.3中的更新式更新每一个粒子的速度与位置属性。

（4）最佳适应度更新

通过计算粒子群中每个个体的适应度值，对个体历史最佳适应度与群体历史最佳适应度进行更新。

最后，判断是否达到最大迭代次数，若不是则返回到步骤（2）继续迭代，若是则计算结束。

4. 遗传算法与粒子群算法

通过上一篇和本篇的介绍，可以发现，遗传算法与粒子群算法这两种算法在某些方面是相通的，这里对他们做一个比较：

可以看出，两者的不同主要体现在个体的迭代方式上，粒子群算法相对遗传算法，迭代方式更简单，因此实现起来更方便。

5. 总结

本篇主要介绍了应用于路径规划中的另外一种算法——粒子群算法，介绍了算法的理论基础，实现流程，应用示例以及与遗传算法的比较。

到这里，算法补充板块就暂告一个段落了，希望对大家有所帮助。之后打算更新一些word与Python方面的内容，或者大家有什么想了解的，我们评论区见呀~

最后，文章原创不易，期待大家的三连哦~
点赞——是你们对菠萝最大的认可和鼓励~
收藏——方便大家之后需要的时候查找~
转发——分享给需要的小伙伴，大家一起进步~