CN109262612A - 一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法 - Google Patents

Abstract

本发明实施例公开了一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法。包括：建立平面三自由度被动‑主动‑主动型(PAA型)欠驱动机械臂动力学模型；然后，针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动‑主动型(PA型)子系统，并明确关节角与机械臂末端位置之间关系；最后，以欠驱动机械臂末端位置与目标位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的Metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角。根据本发明实施例所提出的技术方案，在欠驱动机械臂位置控制时，可以降低寻优算法陷入局部最优的概率并提高求解精度。

Description

一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法

【技术领域】

本发明属于自动化控制技术领域，涉及模型降阶和粒子群算法，具体是一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法。

【背景技术】

在太空中，为了使机械臂完成复杂多样性任务，空间机械臂应具有较轻质量、较低能耗的特性以及一定的冗余度和高度的灵活性。机械臂的能耗与驱动装置数量呈正相关，欠驱动机械臂需要的驱动电机数目少于关节数目，在保证完成任务的前提下，更适合在太空中工作。

在欠驱动机械臂的位置控制中，机械臂动力学具有高度的非线性耦合，利用反演控制的思想，将三自由度欠驱动机械臂降阶为两个子系统进行控制。为了实现精准的位置控制，需要准确的求解出主动关节的目标角度，粒子群算法通过均匀初始化粒子，根据全局最优和历史最优更新粒子，求解到达目标位置所对应的最优关节角，然而粒子群算法在求解机械臂最优关节角的精度上还亟待优化。

【发明内容】

有鉴于此，鉴于上述粒子群优化方法所存在的问题，本发明提出一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法，包括：

建立平面三自由度被动-主动-主动型(PAA型)欠驱动机械臂动力学模型；

针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动-主动型(PA型)子系统，并明确关节角与机械臂末端位置之间关系；

以欠驱动机械臂末端位置与目标位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的Metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角。

上述方法中，建立平面三自由度被动-主动-主动型(PAA型)欠驱动机械臂动力学模型，包括：

建立平面三自由度PAA型欠驱动机械臂动力学模型如下：

其中，分别表示关节角度、关节角速度以及关节角加速度，M(q)∈R^3×3为关节空间中的质量惯性矩阵，为哥氏力和离心力矩阵，τ为关节力矩矢量；

此动力学方程可以化为如下形式：

由式(2)可知，这是一个多输入多输出的非线性方程，其中，M_ij∈R^3×3(i,j＝1,2,3)具体为，

a_k(k＝1,2,...,6)为系统的结构参数，q_i(i＝1,2,3)为机械臂的第i关节角，

m_i为第i杆的质量(i＝1,2,3)，L_i为第i杆的长度(i＝1,2,3)，l_i为第i杆质心到前一关节的长度(i＝1,2,3)，J_i为第i杆的转动惯量(i＝1,2,3)；

H_i∈R^3×1(i＝1,2,3)为哥氏力和离心力项，

τ＝(0,τ₂,τ₃)^T为关节力矩矢量，其中被动关节的输入力矩为0，主动关节输入力矩为τ₂,τ₃。

上述方法中，所述针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动-主动型(PA型)子系统，并明确关节角与机械臂末端位置之间关系，包括：

基于反演控制思想，将三自由度欠驱动机械臂降阶为两个子系统，分成两个控制阶段；

根据式(2)得到：

将哥氏力和离心力矩阵式(5)带入式(6)中，可得：

利用分部积分，式(7)对时间t求定积分，可得：

其中，第一阶段通过锁死第三关节，保证第三杆的角度和角速度不变，将三自由机械臂降阶为两自由度PA型机械臂，可以得到第一关节与第二关节的角度约束，化简为如下形式：

其中，q₁ ¹(0)表示第一阶段时被动杆初始角度，q₂(0)和q₃(0)分别表示第二杆和第三杆的初始角度，D₁,E₁,G₁,g₁为式(9)化简过程的中间项，

D₁＝(8a₃a₅-4a₂a₆-4a₁a₆-4a₄a₆)cosq₂(0)-a₁ ²-a₂ ²-4a₆ ²cos²q₂(0)-a₄ ²+4a₅ ²+4a₃ ²-2a₁a₂-2a₁a₄-2a₂a₄

G₁＝(a₁+a₂+a₄+2a₆cosq₂(0)-2a₅cosq₂(0)-2a₃)(cosq₂-1)+2a₅sinq₂(0)sinq₂

其中，第二阶段通过锁死第二关节，保证第二杆的角度和角速度不变，将三自由机械臂降阶为两自由度PA型机械臂，可以得到第一关节与第三关节的角度约束，化简为如下形式：

其中，q₁ ²(0)为第一杆在第二阶段的初始角度，q_2d为第二杆的目标角度，D₂,E₂,G₂,g₂为式(10)化简过程的中间项，

D₂＝(4a₂a₃+4a₁a₃+4a₃a₄-8a₅a₆)cosq_2d+a₁ ²+a₂ ²+4a₃ ²cos²q_2d+a₄ ²-4a₅ ²-4a₆ ²+2a₁a₂+2a₁a₄+2a₂a₄

G₂＝(a₁+a₂+a₄+2a₃cosq_2d-2a₅cosq_2d-2a₆)(cosq₃-1)+2a₅sinq_2d sinq₃

根据运动学，利用机械臂各关节角，可得机械臂末端位置：

其中，X,Y分别为机械臂末端的实际位置横坐标和纵坐标。

上述方法中，所述以欠驱动机械臂末端位置与预期位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的Metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角，包括：

根据式(9)，(10)，(11)可知，机械臂的末端位置由关节角控制，当目标关节角度求解准确时，机械臂与目标位置距离小；相反，距离增大；被动关节角q₁可由主动关节角q₂,q₃求解，因此将主动关节角作为优化目标，以机械臂末端位置与目标位置距离为优化算法的适应度函数：

其中，X_d,Y_d分别为目标位置的横坐标和纵坐标，f为优化算法的适应度函数；

改进粒子群算法的欠驱动机械臂关节角寻优方法，包括以下步骤：

1)在搜索空间内初始化N个粒子，使其均匀分布；

2)设置算法的参数，包括：迭代次数、惯性权重、学习因子；

3)计算每个粒子的适应度f，并在算法第一次迭代时设置Pbest和Gbest，将第一代粒子的适应度记为Pbest，将Pbest中的最小值记为Gbest；

其中，Pbest为N行一列的矩阵，每行对应一个粒子，记录其所经过最好位置的适应度，Gbest为一个值，表示全部粒子在搜索过程中的最优位置所对应的适应度；

4)引入模拟退火的Metropolis接受准则，对每个粒子，将其适应度与粒子所经过最优位置的适应度Pbest比较，如果适应度小于Pbest，则将其作为Pbest；如果适应度大于Pbest，在(0,1)内生成随机数，比较接受概率p和随机数，如果p大于随机数，则将当前粒子适应度记为Pbest，如果p小于随机数，则Pbest不变；

其中，gen表示当前迭代的次数；

5)对于每个粒子，将其适应度与全局最优粒子的适应度Gbest比较，如果适应度小于Gbest，则将其作为Gbest，如果适应度大于Gbest，则Gbest不变；

6)根据式(14)更新粒子速度；

其中，为更新前粒子的速度，为更新后粒子的速度，ω为惯性权重，为更新前粒子的位置，为以为圆心，为半径的超球面，

其中，为Pbest所对应的粒子位置，为Gbest所对应的粒子位置；

7)引入人工鱼群拥挤度的概念，当适应度基本不变时，假定陷入局部最优，以Gbest为圆心，以随迭代次数增加而增加r为半径，将圆内粒子重新初始化；

其中，gen_max为最大迭代次数；

8)终止判断，当达到最大迭代次数或者适应度满足要求时终止算法，否则返回2)；

所述方法引入模拟退火的Metropolis接受准则和拥挤度因子的概念改进粒子群算法，能够加强算法的全局搜索能力、减少陷入局部最优的概率和避免算法过早收敛的情况，减小由机械臂关节角度不精确导致的误差，最终可以较快的寻找到目标位置所对应的最优角度，实现平面三自由度PAA型欠驱动机械臂精确的位置控制。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1表示本发明实例的设计流程图；

图2表示机械臂末端位置与关节角的关系；

图3表示本发明实例优化算法流程图；

图4表示Metropolis接受准则概率P的函数；

图5表示粒子速度v和位置x更新函数；

图6表示机械臂末端位置与目标位置的差值曲线。

【具体实施方式】

为了更好地理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法，具体包括步骤如下：

1.建立平面三自由度被动-主动-主动型(PAA型)欠驱动机械臂动力学模型；2.针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动-主动型(PA型)子系统，并明确关节角与机械臂末端位置之间关系；3.以欠驱动机械臂末端位置与目标位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的Metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角。

本发明实施例给出基于粒子群优化的欠驱动机械臂关节角寻优方法，请参考图1，其为本发明实施例所提出的一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法的流程示意图，该方法包括以下步骤：

步骤101，建立平面三自由度被动-主动-主动型(PAA型)机械臂动力学模型；

具体的，建立平面三自由度PAA型欠驱动机械臂动力学模型如下：

其中，分别表示关节角度、关节角速度以及关节角加速度，M(q)∈R^3×3为关节空间中的质量惯性矩阵，为哥氏力和离心力矩阵，τ为关节力矩矢量；

此动力学方程可以化为如下形式：

由式(2)可知，这是一个多输入多输出的非线性方程，其中，M_ij∈R^3×3(i,j＝1,2,3)具体为，

a_k(k＝1,2,...,6)为系统的结构参数，q_i(i＝1,2,3)为机械臂的第i关节角，

m_i为第i杆的质量(i＝1,2,3)，L_i为第i杆的长度(i＝1,2,3)，l_i为第i杆质心到前一关节的长度(i＝1,2,3)，J_i为第i杆的转动惯量(i＝1,2,3)；

H_i∈R^3×1(i＝1,2,3)为哥氏力和离心力项，

τ＝(0,τ₂,τ₃)^T为关节力矩矢量，其中被动关节的输入力矩为0，主动关节输入力矩为τ₂,τ₃。

步骤102，针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动-主动型(PA型子系统)，并明确关节角与机械臂末端位置之间关系；

具体的，基于反演控制思想，将三自由度欠驱动机械臂降阶为两个子系统，分成两个控制阶段；

根据式(2)得到：

将哥氏力和离心力矩阵式(5)带入式(6)中，可得：

利用分部积分，式(7)对时间t求定积分，可得：

其中，第一阶段通过锁死第三关节，保证第三杆的角度和角速度不变，将三自由机械臂降阶为两自由度PA型机械臂，可以得到第一关节与第二关节的角度约束，化简为如下形式：

其中，q₁ ¹(0)表示第一阶段时被动杆初始角度，q₂(0)和q₃(0)分别表示第二杆和第三杆的初始角度，D₁,E₁,G₁,g₁为式(9)化简过程的中间项，

D₁＝(8a₃a₅-4a₂a₆-4a₁a₆-4a₄a₆)cosq₂(0)-a₁ ²-a₂ ²-4a₆ ²cos²q₂(0)-a₄ ²+4a₅ ²+4a₃ ²-2a₁a₂-2a₁a₄-2a₂a₄

G₁＝(a₁+a₂+a₄+2a₆cosq₂(0)-2a₅cosq₂(0)-2a₃)(cosq₂-1)+2a₅sinq₂(0)sinq₂

其中，第二阶段通过锁死第二关节，保证第二杆的角度和角速度不变，将三自由机械臂降阶为两自由度PA型机械臂，可以得到第一关节与第三关节的角度约束，化简为如下形式：

其中，q₁ ²(0)为第一杆在第二阶段的初始角度，q_2d为第二杆的目标角度，D₂,E₂,G₂,f₂为式(10)化简过程的中间项，

D₂＝(4a₂a₃+4a₁a₃+4a₃a₄-8a₅a₆)cosq_2d+a₁ ²+a₂ ²+4a₃ ²cos²q_2d+a₄ ²-4a₅ ²-4a₆ ²+2a₁a₂+2a₁a₄+2a₂a₄

G₂＝(a₁+a₂+a₄+2a₃cosq_2d-2a₅cosq_2d-2a₆)(cosq₃-1)+2a₅sinq_2d sinq₃

根据运动学，如图2所示，利用机械臂各关节角，可得机械臂末端位置：

其中，X,Y分别为机械臂末端的实际位置横坐标和纵坐标。

步骤103，以欠驱动机械臂末端位置与预期位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的Metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角；

根据式(9)，(10)，(11)可知，机械臂的末端位置由关节角控制，当目标关节角度求解准确时，机械臂与目标位置距离小；相反，距离增大；被动关节角q₁可由主动关节角q₂,q₃求解，因此将主动关节角作为优化目标，以机械臂末端位置与目标位置距离为优化算法的适应度函数：

其中，X_d,Y_d分别为目标位置的横坐标和纵坐标，f为优化算法的适应度函数；

改进粒子群算法的欠驱动机械臂关节角寻优方法，如图3所示，包括以下步骤：

1)在搜索空间内初始化N个粒子，使其均匀分布；

2)设置算法的参数，包括：迭代次数、惯性权重、学习因子；

3)计算每个粒子的适应度f，并在算法第一次迭代时设置Pbest和Gbest，将第一代粒子的适应度记为Pbest，将Pbest中的最小值记为Gbest；

其中，Pbest为N行一列的矩阵，每行对应一个粒子，记录其所经过最好位置的适应度，Gbest为一个值，表示全部粒子在搜索过程中的最优位置所对应的适应度；

4)引入模拟退火的Metropolis接受准则，对每个粒子，将其适应度与粒子所经过最优位置的适应度Pbest比较，如果适应度小于Pbest，则将其作为Pbest；如果适应度大于Pbest，在(0,1)内生成随机数，比较接受概率p和随机数，如果p大于随机数，则将当前粒子适应度记为Pbest，如果p小于随机数，则Pbest不变，概率p如图4所示，具体为：

其中，gen表示当前迭代的次数；

5)对于每个粒子，将其适应度与全局最优粒子的适应度Gbest比较，如果适应度小于Gbest，则将其作为Gbest，如果适应度大于Gbest，则Gbest不变；

6)根据式(14)更新粒子速度，如图5所示，

其中，为更新前粒子的速度，为更新后粒子的速度，ω为惯性权重，为更新前粒子的位置，为以为圆心，为半径的超球面，

其中，为Pbest所对应的粒子位置，为Gbest所对应的粒子位置；

7)引入人工鱼群拥挤度的概念，当适应度基本不变时，假定陷入局部最优，以Gbest为圆心，以随迭代次数增加而增加r为半径，将圆内粒子重新初始化；

其中，gen_max为最大迭代次数；

8)终止判断，当达到最大迭代次数或者适应度满足要求时终止算法，否则返回2)；

所述方法引入模拟退火的Metropolis接受准则和拥挤度因子的概念改进粒子群算法，能够加强算法的全局搜索能力、减少陷入局部最优的概率和避免算法过早收敛的情况，减小由机械臂关节角度不精确导致的误差，最终可以较快的寻找到目标位置所对应的最优角度，实现平面三自由度PAA型欠驱动机械臂精确的位置控制。

具体实施中，其动力学参数及控制参数如下表所示：

a<sub>1</sub>	a<sub>2</sub>	a<sub>3</sub>	a<sub>4</sub>	a<sub>5</sub>	a<sub>6</sub>	L<sub>1</sub>	L<sub>2</sub>	L<sub>3</sub>
									1.57867	2.016	1.536	0.33333	0.4	0.6	0.8	1.2	1.0
q<sub>1</sub>(0)	q<sub>2</sub>(0)	q<sub>3</sub>(0)	xd	yd	NP	Gen_max	c1	c2
									0	0	0	1.7	-1.2	15	300	2	1.8

假定初始角度为q₁₀＝0,q₂₀＝0,q₃₀＝0,初始角速度为算法迭代300次,要求机械臂末端达到的期望位置为x_d＝1.7,y_d＝-1.2,。图6表示机械臂末端位置与目标位置的差值曲线。图中可以看出适应度函数逐渐衰减，意味着算法不断更新迭代找到适应度更小的关节角，即末端位置较小的关节角。当算法迭代一定次数时，收敛速度减慢，直至适应度不变。假定陷入局部最优，在可行域内以当前最好值为圆心，随迭代次数增加而增加的r为半径，重新初始化圆内粒子，粒子只在圆外搜索。若找到更好的解，则作为当前最优解；若找不到更好的解，则当前为最优解。由仿真结果可以看出，本发明所提方法可以降低陷入局部最优的可能性并提高求解目标角度的精度。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (4)

1.一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法，其特征在于，所述方法步骤包括：

(1)建立平面三自由度被动-主动-主动型(PAA型)欠驱动机械臂动力学模型；

(2)针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动-主动型(PA型)子系统，并明确关节角与机械臂末端位置之间关系；

(3)以欠驱动机械臂末端位置与目标位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的Metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角。

2.根据权利要求1所述的方法，其特征在于，建立平面三自由度被动-主动-主动型(PAA型)欠驱动机械臂动力学模型；

建立平面三自由度PAA型欠驱动机械臂动力学模型如下：

其中，q,分别表示关节角度、关节角速度以及关节角加速度，M(q)∈R^3×3为关节空间中的质量惯性矩阵，为哥氏力和离心力矩阵，τ为关节力矩矢量；

此动力学方程可以化为如下形式：

由式(2)可知，这是一个多输入多输出的非线性方程，其中，M_ij∈R^3×3(i,j＝1,2,3)具体为，

a_k(k＝1,2,...,6)为系统的结构参数，q_i(i＝1,2,3)为机械臂的第i关节角，

m_i为第i杆的质量(i＝1,2,3)，L_i为第i杆的长度(i＝1,2,3)，l_i为第i杆质心到前一关节的长度(i＝1,2,3)，J_i为第i杆的转动惯量(i＝1,2,3)；

H_i∈R^3×1(i＝1,2,3)为哥氏力和离心力项，

τ＝(0,τ₂,τ₃)^T为关节力矩矢量，其中被动关节的输入力矩为0，主动关节输入力矩为τ₂,τ₃。

3.根据权利要求1所述的方法，其特征在于，针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动-主动型(PA型)子系统，并明确关节角与机械臂末端位置之间关系；

根据式(2)得：

将哥氏力和离心力矩阵式(5)带入式(6)中，可得：

利用分部积分，式(7)对时间t求定积分，可得：

其中，第一阶段通过锁死第三关节，保证第三关节角和角速度不变，将三自由机械臂降阶为两自由度PA型机械臂，可以得到第一关节与第二关节的角度约束，化简为如下形式：

其中，q₁ ¹(0)表示第一阶段时被动杆初始角度，q₂(0)和q₃(0)分别表示第二杆和第三杆的初始角度，D₁,E₁,G₁,g₁为式(9)化简过程的中间项，

D₁＝(8a₃a₅-4a₂a₆-4a₁a₆-4a₄a₆)cosq₂(0)-a₁ ²-a₂ ²-4a₆ ²cos²q₂(0)-a₄ ²+4a₅ ²+4a₃ ²-2a₁a₂-2a₁a₄-2a₂a₄

G₁＝(a₁+a₂+a₄+2a₆cosq₂(0)-2a₅cosq₂(0)-2a₃)(cosq₂-1)+2a₅sinq₂(0)sinq₂

其中，第二阶段通过锁死第二关节，保证第二杆的角度和角速度不变，将三自由机械臂降阶为两自由度PA型机械臂，可以得到第一关节与第三关节的角度约束，化简为如下形式：

其中，q₁ ²(0)为第一杆在第二阶段的初始角度，q_2d为第二杆的目标角度，D₂,E₂,G₂,g₂为式(10)化简过程的中间项，

D₂＝(4a₂a₃+4a₁a₃+4a₃a₄-8a₅a₆)cosq_2d+a₁ ²+a₂ ²+4a₃ ²cos²q_2d+a₄ ²-4a₅ ²-4a₆ ²+2a₁a₂+2a₁a₄+2a₂a₄

G₂＝(a₁+a₂+a₄+2a₃cosq_2d-2a₅cosq_2d-2a₆)(cosq₃-1)+2a₅sinq_2d sinq₃

根据运动学，利用机械臂各关节角，可得机械臂末端位置：

其中，X,Y分别为机械臂末端的实际位置横坐标和纵坐标。

4.根据权利要求1所述的方法，其特征在于，以欠驱动机械臂末端位置与目标位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的Metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角；

根据式(9)，(10)，(11)可知，机械臂的末端位置由关节角控制，当目标关节角度求解准确时，机械臂与目标位置距离小；相反，距离增大；被动关节角q₁可由主动关节角q₂,q₃求解，因此将主动关节角作为优化目标，以机械臂末端位置与目标位置距离为优化算法的适应度函数：

其中，X_d,Y_d分别为目标位置的横坐标和纵坐标，f为优化算法的适应度函数；

改进粒子群算法的欠驱动机械臂关节角寻优方法，包括以下步骤：

1)在搜索空间内初始化N个粒子，使其均匀分布；

2)设置算法的参数，包括：迭代次数、惯性权重、学习因子；

3)计算每个粒子的适应度f，并在算法第一次迭代时设置Pbest和Gbest，将第一代粒子的适应度记为Pbest，将Pbest中的最小值记为Gbest；

其中，Pbest为N行一列的矩阵，每行对应一个粒子，表示其所经过最好位置的适应度，Gbest为一个值，表示全部粒子在搜索过程中的最优位置所对应的适应度；

4)引入模拟退火的Metropolis接受准则，对每个粒子，将其适应度与粒子所经过最优位置的适应度Pbest比较，如果适应度小于Pbest，则将其作为Pbest；如果适应度大于Pbest，在(0,1)内生成随机数，比较接受概率p和随机数，如果p大于随机数，则将当前粒子适应度记为Pbest，如果p小于随机数，则Pbest不变；

其中，gen表示当前迭代的次数；

5)对于每个粒子，将其适应度与全局最优粒子的适应度Gbest比较，如果适应度小于Gbest，则将其作为Gbest，如果适应度大于Gbest，则Gbest不变；

6)根据式(14)更新粒子速度；

其中，为更新前粒子的速度，为更新后粒子的速度，ω为惯性权重，为更新前粒子的位置，为以为圆心，为半径的超球面，

其中，为Pbest所对应的粒子位置，为Gbest所对应的粒子位置；

7)引入人工鱼群拥挤度的概念，当适应度基本不变时，假定陷入局部最优，以Gbest为圆心，以随迭代次数增加而增加r为半径，将圆内粒子重新初始化；

其中，gen_max为最大迭代次数；

8)终止判断，当达到最大迭代次数或者适应度满足要求时终止算法，否则返回2)；

所述方法引入模拟退火的Metropolis接受准则和拥挤度因子的概念改进粒子群算法，能够加强算法的全局搜索能力、减少陷入局部最优的概率和避免算法过早收敛的情况，减小由机械臂关节角度不精确导致的误差，最终可以较快的寻找到目标位置所对应的最优角度，实现平面三自由度PAA型欠驱动机械臂精确的位置控制。