粒子群算法路径规划_机器人粒子群轨迹规划

知乎视频​www.zhihu.com

简介：采用粒子群优化算法进行机器人轨迹规划，以3-5-3多项式轨迹形式进行轨迹规划，以最短时间为优化指标进行寻优。

1. 3-5-3轨迹轨迹的形式

其中，j为关节编号，如六自由度编号为1-6。已知关节j的4个插值点(弧度值)：

。3-5-3轨迹系数矩阵的求解

2. 3-5-3轨迹系数矩阵的求解

约束优化条件：

其中，

为适应度函数

3. 约束条件转换

是需要优化的未知量，系数矩阵a是待求解的位置数，若把a当作自变量则需要优化14个维度的数据，计算量大，为此直接将t作为自变量，降低搜索维度为3维。

4.粒子群优化

(1)初始化种群规模NPon=50，迭代次数NGer=50，待求解向量维度NDim=3，随机初始化粒子位置向量theta和粒子速度向量vTheta

(2)给定粒子位置向量的取值范围thetaLim，给定粒子速度的取值范围vThetaLim

(3)给定惯性权重weight，自我学习因子cLearn1和群体学习因子cLearn2

(4)初始化个体的最佳历史位置为thetaHistory = theta，种群最佳历史位置thetaBest = zeros(1, NDim)，个体最佳适应度indTBest = inf+zeros(NPon, 1)，种群历史最佳适应度tBest = inf

注意：由于是求最小值indTBest、tBest应当初始化为一个较大的值，而不应该初始化为0。

(5)调用适应度函数fitnessTrac，计算系数矩阵A,关节速度在各段曲线上的极大值和极小值矩阵V，若A满秩且V绝对值的最大值小于给定的最大关节速度说明当前时间t1、t2、t3有效，否则说明当前值无效，flag返回false。

(6)若flag为true 说明当前时间

有效，进行记录并于个体的历史最佳适应度indTBest进行比较，若indTBest大于当前计算的适应度fxBest则更新indTBest，否则不更新；若flag 为false 说明当前值无效，此时直接将当前适应度fxBest赋值为无穷大。

(7)获得当前种群中最佳适应度min(indTBest)，若其小于种群历史最佳适应度tBest，则更新tBest。

(8)采用粒子群公式进行速度vTheta的更新；

(9)判断每个粒子的速度和位置是否在安全范围之内，若不在则拉回到安全边界值。

5.博客，欢迎交流：

qq_37469992的博客_yy小苗(高级划水工程师)_CSDN博客-设计模式Gof,matlab,C++领域博主​blog.csdn.net