CN112428267A - 一种快速求解冗余自由度机器人逆解的方法 - Google Patents

Abstract

本发明公开了一种快速求解冗余自由度机器人逆解的方法，采用D‑H法建立机器人的正运动学模型；确定机器人各关节的角度范围；改进量子粒子群算法；建立机器人逆解评价指标；将机器人逆解评价指标作为优化目标，采用改进量子粒子群算法求解冗余自由度机器人逆解。本发明一种快速求解冗余自由度机器人逆解的方法，解决了冗余自由度机器人逆解求解时速度慢，准确性不高的问题。

Description

一种快速求解冗余自由度机器人逆解的方法

技术领域

本发明属于机器人运动求解技术领域，具体涉及一种快速求解冗余自由度机器人逆解的方法。

背景技术

为了更好地对冗余自由度机器人进行位置控制，需要快速求解冗余自由度机器人逆解。目前，求解冗余自由度机器人逆解的方法主要有几何解析法，伪逆法，智能算法等，但是，几何解析法需要满足特殊构型，伪逆法存在累积误差，传统的智能算法收敛速度不够快。传统的粒子群算法虽然应用广泛，但是存在收敛速度慢、易陷入局部极值的缺陷，所以，研究一种求解机器人运动学逆解的方法是很有必要的。

发明内容

本发明的目的是提供一种快速求解冗余自由度机器人逆解的方法，解决了冗余自由度机器人逆解求解时速度慢，准确性不高的问题。

本发明所采用的技术方案是一种快速求解冗余自由度机器人逆解的方法，具体按照以下步骤实施：

步骤1，采用D-H法建立机器人的正运动学模型；

步骤2，确定机器人各关节的角度范围；

步骤3，改进量子粒子群算法；

步骤4，建立机器人逆解评价指标；

步骤5，将机器人逆解评价指标作为优化目标，采用改进量子粒子群算法求解冗余自由度机器人逆解。

本发明的特点还在于：

步骤1具体为：

机器人第i个关节变换矩阵

为：

式(1)中，

为机器人第i个关节变换矩阵，d_i为连杆长度，α_i为连杆扭转角，a_i为连杆偏移量，θ_i为关节角；

将机器人各关节的变换矩阵依次相乘，得到机器人的末端相对于基坐标的变换矩阵

式(2)中，

为机器人的末端相对于基坐标的变换矩阵；

通过式(2)构建正运动学模型。

步骤2具体为：

根据工作空间的限制和工作任务的要求，确定机器人各关节的角度范围。

步骤3具体为：

步骤3.1，初始化量子粒子群中M个粒子的位置向量，并计算各个粒子的适应度，设置最大迭代次数为T；

步骤3.2，给粒子个体极值pbest及全局极值gbest赋初值；

步骤3.3，设定自适应收缩-扩张系数β，

β＝λ₁×rate+λ₂×ag+λ₃ (3)，

式(3)中，λ₁＜0，λ₂＞0，λ₃＝1，rate＝f(mbestt)/f(mbestt-1)，表示量子粒子群算法在运行过程中全局最优值的变化速率，

表示量子粒子群的当前聚集度，t为当前迭代次数；β是一个小于1.782的常数；

步骤3.4，根据式(4)计算量子粒子群平均最优位置：

式(4)中，mbest_d为量子粒子群平均最优位置，pbest_id为粒子最优位置；

步骤3.5，计算粒子的当前适应度值，并与前一代的适应度值进行比较，如果当前适应度值小于前一代适应度值，则将粒子位置更新为粒子当前的位置；

步骤3.6，计算量子粒子群当前的全局最优位置，比较当前的全局最优位置与前一代全局最优位置，如果当前的全局最优位置适应度值好，则将量子粒子群的最优位置更新为当前的全局最优位置；

步骤3.7，根据式(5)计算粒子的每一维，得到一个随机点的位置；

式(5)中，

pbest_id为粒子最优位置，gbest_d为量子粒子群的最优位置；

步骤3.8，根据式(6)得到粒子的新位置；

式(6)中，u＝rand(0,1)，x_id(t+1)为粒子的新位置，g_id为一个随机点的位置，mbest_d为量子粒子群平均最优位置，t为当前迭代次数；

步骤3.9，若没有达到最大迭代次数T，则重复步骤3.4～3.8，否则结束。

机器人逆解评价指标的适应度函数为：

θ∈Rⁿ|θ_k,min≤θ_k≤θ_k,max,k＝1,2,3...n (8)，

式(7)、(8)中，(x₁,y₁,z₁)为机器人末端的位置，(x₂,y₂,z₂)为机器人末端的实际位置，θ_k为机器人的关节角度，θ_k,min、θ_k,max分别为机器人的关节所能到达的最小、最大角度，R为任意实数。

步骤5具体为：

步骤5.1，初始化量子粒子群

初始化机器人中M个关节的位置向量，在机器人各关节的角度范围内随机产生关节角度；

步骤5.2，将M自由度的机器人代入式(4)中求解机器人各关节的平均角度；

步骤5.3，计算适应度值

根据正运动学模型求解出期待位置，计算各关节的位置，代入式(7)求解出各关节的位置误差；

步骤5.4，如果机器人各关节转角超过受限的关节范围，淘汰这个解，并用上一代的解代替；

步骤5.5，利用改进量子粒子群算法迭代产生新的解，更新机器人各关节角度，达到最大迭代次数T后结束，否则返回步骤5.2。

本发明的有益效果是：

本发明一种快速求解冗余自由度机器人逆解的方法，提高了全局优化能力及粒子群算法的收敛速度，提升了粒子群算法的性能；本发明一种快速求解冗余自由度机器人逆解的方法，将改进后的粒子群算法应用到了冗余自由度机器人逆解的求解，解决了冗余自由度机器人逆解求解时速度慢，准确性不高的问题。

附图说明

图1是本发明一种快速求解冗余自由度机器人逆解的方法的流程图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明一种快速求解冗余自由度机器人逆解的方法，如图1所示，具体按照以下步骤实施：

步骤1，采用D-H法建立机器人的正运动学模型；

步骤2，确定机器人各关节的角度范围；

步骤3，改进量子粒子群算法；

步骤4，建立机器人逆解评价指标；

步骤5，将机器人逆解评价指标作为优化目标，采用改进量子粒子群算法求解冗余自由度机器人逆解。

步骤1具体为：

机器人第i个关节变换矩阵

为：

式(1)中，

为机器人第i个关节变换矩阵，d_i为连杆长度，α_i为连杆扭转角，a_i为连杆偏移量，θ_i为关节角；

将机器人各关节的变换矩阵依次相乘，得到机器人的末端相对于基坐标的变换矩阵

式(2)中，

为机器人的末端相对于基坐标的变换矩阵；

通过式(2)构建正运动学模型。

步骤2具体为：

根据工作空间的限制和工作任务的要求，确定机器人各关节的角度范围。

步骤3具体为：

步骤3.1，初始化量子粒子群中M个粒子的位置向量，并计算各个粒子的适应度，设置最大迭代次数为T；

步骤3.2，给粒子个体极值pbest及全局极值gbest赋初值；

步骤3.3，设定自适应收缩-扩张系数β，

β＝λ₁×rate+λ₂×ag+λ₃ (3)，

式(3)中，λ₁＜0，λ2＞0，λ₃＝1，rate＝f(mbestt)/f(mbestt-1)，表示量子粒子群算法在运行过程中全局最优值的变化速率，

表示量子粒子群的当前聚集度，t为当前迭代次数；β是一个小于1.782的常数；

当粒子群进化速度较小的时候，应减小β值，使得量子粒子群算法能够在小范围内寻优，以便尽快逼近全局最优点；当粒子群聚集程度过大时，此时，量子粒子群算法容易陷入局部最优，应该增大β，使得量子粒子群算法能够跳出局部最优点，保持粒子群的多样性；综上所述，β值与变化速率rate呈负相关关系，与聚集度ag呈正相关关系；

步骤3.4，根据式(4)计算量子粒子群平均最优位置：

式(4)中，mbestd为量子粒子群平均最优位置，pbestid为粒子最优位置；

步骤3.5，计算粒子的当前适应度值，并与前一代的适应度值进行比较，如果当前适应度值小于前一代适应度值，则将粒子位置更新为粒子当前的位置；

步骤3.6，计算量子粒子群当前的全局最优位置，比较当前的全局最优位置与前一代全局最优位置，如果当前的全局最优位置适应度值好，则将量子粒子群的最优位置更新为当前的全局最优位置；

步骤3.7，根据式(5)计算粒子的每一维，得到一个随机点的位置；

式(5)中，

pbestid为粒子最优位置，gbest_d为量子粒子群的最优位置；

步骤3.8，根据式(6)得到粒子的新位置；

式(6)中，u＝rand(0,1)，x_id(t+1)为粒子的新位置，g_id为一个随机点的位置，mbest_d为量子粒子群平均最优位置，t为当前迭代次数；

步骤3.9，若没有达到最大迭代次数T，则重复步骤3.4～3.8，否则结束。

机器人逆解评价指标的适应度函数为：

θ∈Rⁿ|θ_k,min≤θ_k≤θ_k,max,k＝1,2,3...n (8)，

式(7)、(8)中，(x₁,y₁,z₁)为机器人末端的位置，(x₂,y₂,z₂)为机器人末端的实际位置，θ_k为机器人的关节角度，θk,min、θk,max分别为机器人的关节所能到达的最小、最大角度，R为任意实数。

步骤5具体为：

步骤5.1，初始化量子粒子群

初始化机器人中M个关节的位置向量，在机器人各关节的角度范围内随机产生关节角度；

步骤5.2，将M自由度的机器人代入式(4)中求解机器人各关节的平均角度；

步骤5.3，计算适应度值

根据正运动学模型求解出期待位置，计算各关节的位置，代入式(7)求解出各关节的位置误差；

步骤5.4，如果机器人各关节转角超过受限的关节范围，淘汰这个解，并用上一代的解代替；

步骤5.5，利用改进量子粒子群算法迭代产生新的解，更新机器人各关节角度，达到最大迭代次数T后结束，否则返回步骤5.2。

本发明一种快速求解冗余自由度机器人逆解的方法，其优点在于：

采用混合量子行为的量子粒子群方法来求解逆解，并且通过自适应收缩-扩张系数替换原有线性递减的收缩-扩张系数的方式使粒子群算法的收敛速度和全局最优性提高；与传统的粒子群算法相比，基于量子行为的粒子群算法取消了粒子的移动方向属性，粒子位置的更新和该粒子之前的运动没有任何关系，这样就增加了粒子位置的随机性，由于量子系统是一个多态、不确定性系统，且粒子没有一个确定的轨迹，因此QPSO能够克服一般粒子群算法在收敛性能上的缺陷。

Claims (6)

1.一种快速求解冗余自由度机器人逆解的方法，其特征在于，具体按照以下步骤实施：

步骤1，采用D-H法建立机器人的正运动学模型；

步骤2，确定机器人各关节的角度范围；

步骤3，改进量子粒子群算法；

步骤4，建立机器人逆解评价指标；

步骤5，将所述机器人逆解评价指标作为优化目标，采用改进量子粒子群算法求解冗余自由度机器人逆解。

2.如权利要求1所述的一种快速求解冗余自由度机器人逆解的方法，其特征在于，所述步骤1具体为：

机器人第i个关节变换矩阵

为：

式(1)中，

为机器人第i个关节变换矩阵，d_i为连杆长度，α_i为连杆扭转角，a_i为连杆偏移量，θ_i为关节角；

将机器人各关节的变换矩阵依次相乘，得到机器人的末端相对于基坐标的变换矩阵

式(2)中，

为机器人的末端相对于基坐标的变换矩阵；

通过式(2)构建正运动学模型。

3.如权利要求2所述的一种快速求解冗余自由度机器人逆解的方法，其特征在于，步骤2具体为：

根据工作空间的限制和工作任务的要求，确定机器人各关节的角度范围。

4.如权利要求3所述的一种快速求解冗余自由度机器人逆解的方法，其特征在于，步骤3具体为：

步骤3.1，初始化量子粒子群中M个粒子的位置向量，并计算各个粒子的适应度，设置最大迭代次数为T；

步骤3.2，给粒子个体极值pbest及全局极值gbest赋初值；

步骤3.3，设定自适应收缩-扩张系数β，

β＝λ₁×rate+λ₂×ag+λ₃ (3)，

式(3)中，λ₁＜0，λ₂＞0，λ₃＝1，rate＝f(mbest_t)/f(mbest_t-1)，表示量子粒子群算法在运行过程中全局最优值的变化速率，

表示量子粒子群的当前聚集度，t为当前迭代次数；β是一个小于1.782的常数；

步骤3.4，根据式(4)计算量子粒子群平均最优位置：

式(4)中，mbest_d为量子粒子群平均最优位置，pbest_id为粒子最优位置；

步骤3.5，计算粒子的当前适应度值，并与前一代的适应度值进行比较，如果当前适应度值小于前一代适应度值，则将粒子位置更新为粒子当前的位置；

步骤3.6，计算量子粒子群当前的全局最优位置，比较当前的全局最优位置与前一代全局最优位置，如果当前的全局最优位置适应度值好，则将量子粒子群的最优位置更新为当前的全局最优位置；

步骤3.7，根据式(5)计算粒子的每一维，得到一个随机点的位置；

式(5)中，

pbest_id为粒子最优位置，gbest_d为量子粒子群的最优位置；

步骤3.8，根据式(6)得到粒子的新位置；

式(6)中，u＝rand(0,1)，x_id(t+1)为粒子的新位置，g_id为一个随机点的位置，mbest_d为量子粒子群平均最优位置，t为当前迭代次数；

步骤3.9，若没有达到最大迭代次数T，则重复步骤3.4～3.8，否则结束。

5.如权利要求4所述的一种快速求解冗余自由度机器人逆解的方法，其特征在于，所述机器人逆解评价指标的适应度函数为：

θ∈Rⁿ|θ_k,min≤θ_k≤θ_k,max,k＝1,2,3...n (8)，

式(7)、(8)中，(x₁,y₁,z₁)为机器人末端的位置，(x₂,y₂,z₂)为机器人末端的实际位置，θ_k为机器人的关节角度，θ_k,min、θ_k,max分别为机器人的关节所能到达的最小、最大角度，R为任意实数。

6.如权利要求5所述的一种快速求解冗余自由度机器人逆解的方法，其特征在于，所述步骤5具体为：

步骤5.1，初始化量子粒子群

初始化机器人中M个关节的位置向量，在机器人各关节的角度范围内随机产生关节角度；

步骤5.2，将M自由度的机器人代入式(4)中求解机器人各关节的平均角度；

步骤5.3，计算适应度值

根据正运动学模型求解出期待位置，计算各关节的位置，代入式(7)求解出各关节的位置误差；

步骤5.4，如果机器人各关节转角超过受限的关节范围，淘汰这个解，并用上一代的解代替；

步骤5.5，利用改进量子粒子群算法迭代产生新的解，更新机器人各关节角度，达到最大迭代次数T后结束，否则返回步骤5.2。

Images