CN113434982B - 一种电力智能仿生攀爬机器人的运动学逆解方法 - Google Patents

Abstract

本发明公开了一种电力智能仿生攀爬机器人的运动学逆解算法，构建机器人的模型，分别建立基坐标系和各个关节坐标系；将机器人的各个关节转角所构成的向量作为天牛的位置x，构建适应度函数F(x)，得到天牛的位置x的适应度值为fx；经天牛须算法的迭代后，得到的天牛的最优位置x_best，即得到机器人的各个关节转角的最优值；改进天牛须算法基于以负指数次幂衰减的变步长step_t进行搜索，且随着迭代次数的增多，变步长step_t逐渐归为基本分辨率step′，天牛的下一次迭代的位置更新参考全局最优的左、右须适应度值，引入了接受概率p，以适当的接受概率p接受一个较劣解。本发明的基于改进天牛须算法的机器人运动学逆解方法的可靠性更强、精度更高、收敛速度更快。

Description

一种电力智能仿生攀爬机器人的运动学逆解方法

技术领域

本发明涉及机器人运动学逆解的技术领域，尤其是一种电力智能仿生攀爬机器人的运动学逆解方法。

背景技术

目前采用工业机器人代替人工完成危险的巡检任务已经成为一大热门，例如，六自由度结构的电力智能仿生攀爬机器人。

工业机器人的运动学逆解是指通过给定的目标机器人末端位姿参数计算出各关节角，对机器人的轨迹规划动态性能分析等有着重要的研究意义。目前，工业机器人的运动学逆解方法，即逆运行方程的求解方法主要是几何法、解析法和数值法。针对几何法，徐文福等提出一种改进的模态方法，使用模式函数描述机器人结构框架表示几何形状，求取应用轨道服务的机械臂逆解，几何法局限于机械构型，一般要求前三轴以几何形式存在解。针对解析法，Yuchuang Tong等提出一种冗余滑动机械臂的逆运动学参数分析方法，通过关节角度参数化方法求取解析解，解析法高度依赖配置，若应用对象为多关节机械臂时，计算量较大且求解过程复杂。针对数值法，韩磊等提出一种使用牛顿迭代算法求机械臂逆解的方法，数值法求解解稳定性无法保障。J.K.Parker等提出使用遗传算法(GA)求逆解，最大程度减少关节位移，但该方法求解精度有限。王宸，向长峰等提出一种变步长的改进天牛须算法在一定程度上可以提高天牛搜索优化速度，但由于并没有设置步长基本分辨率，会导致后期迭代出现无效迭代的情况。卢光辉、滕欢等对天牛须算法引入模拟退火算法中的蒙特卡洛法则，使得算法稳定性提高并应用于分布式电源选址定容问题验证了在应用于电源选址问题求解中的高效性，但是针对处理类似六自由度关节机器人这种高维度、非线性、强耦合的问题时，求解精度有限。

发明内容

为了克服上述现有技术中的缺陷，本发明提供一种电力智能仿生攀爬机器人的运动学逆解方法，可靠性更强、精度更高、收敛速度更快。

为实现上述目的，本发明采用以下技术方案，包括：

一种电力智能仿生攀爬机器人的运动学逆解方法，包括以下步骤：

S1，构建机器人的模型，分别建立基坐标系和各个关节坐标系；其中，坐标系0为基座标系；坐标系i为第i个关节坐标系，i＝1,2,…,I，I为关节总数量；

S2，将机器人的各个关节转角所构成的向量作为天牛的位置x，构建适应度函数F(x)，得到天牛的位置x的适应度值为fx；

S3，经天牛须算法的迭代后，得到的天牛的最优位置x_best，即得到机器人的各个关节转角的最优值。

步骤S3中，具体过程如下所示：

S31，进行初始化，即迭代次数t＝0，将机器人的各个关节转角的当前值所构成的向量作为天牛的初始位置x⁰，天牛的初始左须位置为

天牛的初始右须位置为

将天牛的初始位置x⁰设为最优位置x_best，即x_best＝x⁰，且天牛的左须最优位置

天牛的右须最优位置

天牛的最优位置x_best所对应的最优适应度值为fx_best，天牛的左须最优位置fx_lbest所对应的左须最优适应度值为fx_lbest，天牛的右须最优位置x_rbest所对应的右须最优适应度值为fx_rbest；

S32，进行第t次迭代，根据天牛的第t次迭代的位置x^t，得到天牛的第t次迭代的左须位置

天牛的第t次迭代的右须位置

天牛的第t次迭代的适应度值为fx^t，天牛的第t次迭代的右须适应度值为

天牛的第t次迭代的左须适应度值为

其中，

为随机的方向向量，d^t为天牛的第t次迭代的左右须距离，d^t＝step_tc₀，step_t为步长，c₀为比例系数；

S33，比较天牛的第t次迭代的左须适应度值

和天牛的左须最优适应度值之间fx_lbest的大小，若

小于fx_lbest，则对天牛的左须最优位置x_lbest和左须最优适应度值fx_lbest分别进行更新，将天牛的左须最优位置x_lbest的值更新为

的值，将天牛的左须最优适应度值fx_lbest更新为

比较天牛的第t次迭代的右须适应度值

的和天牛的右须最优适应度值fx_rbest之间的大小，若

小于fx_rbest，则对天牛的右须最优位置x_rbest和右须最优适应度值fx_rbest分别进行更新，将天牛的右须最优位置x_rbest的值更新为

的值，将天牛的右须最优适应度值fx_rbest更新为

S34，将步骤S33更新得到的天牛的左须最优位置x_lbest和右须最优位置x_rbest代入如下公式进行计算，得到天牛的第t次迭代的位置偏量G^t：

G^t＝c_lr_d(x_lbest-x^t)+c_rr_d(x_rbest-x^t)；

其中，r_d为0～1之间的随机常数；G^t为天牛的第t次迭代的位置偏量；c_r为右须系数；c_l为左须系数；

λ为设定的常数，λ的取值为0和1之间；

S35，根据天牛的第t次迭代的位置偏量G^t与天牛的第t次迭代的位置x^t，计算天牛的下一次迭代即第t+1次迭代的位置x^t+1，具体如下所示：

根据天牛的下一次迭代即第t+1次迭代的位置x^t+1和适应度函数F(x)，得到对应的第t+1次迭代的适应度值为fx^t+1；

S36，计算接受概率p，并根据接受概率p，判断是否对天牛的最优位置x_best和最优适应度值fx_best进行更新：

若fx^t+1＜fx_best，则p＝1，且将天牛的最优位置x_best更新为天牛的第t+1次迭代的位置x^t+1，将天牛的最优适应度值fx_best更新为天牛的第t+1次迭代的适应度值fx^t+1；

若fx^t+1≥fx_best，则

T的取值为1t，此时，若rand＜p，则将天牛的最优位置x_best更新为天牛的第t+1次迭代的位置x^t+1，将天牛的最优适应度值fx_best更新为天牛的第t+1次迭代的适应度值fx^t+1；若rand≥p，则不对天牛的最优位置x_best和最优适应度值fx_best进行更新；其中，rand为0～1之间的随机常数；

S37，判断第t次迭代是否达到设定条件，若达到，则迭代结束，输出天牛的最优位置x_best和最优适应度值fx_best的值；；若未达到，则跳转步骤S32，将步骤S35计算得到天牛的下一次迭代即第t+1次迭代的位置x^t+1带入步骤S32中，继续进行下一次迭代即第t+1次迭代。

步骤S1中，坐标系0即基坐标系即基座坐标系中包括X₀轴、Y₀轴、Z₀轴；坐标系1到坐标系I依次为从基座到末端执行器的I个旋转关节的关节坐标系；坐标系i即第i个关节坐标系中包括X_i轴、Y_i轴、Z_i轴；第i个关节的关节转角θ_i为坐标系i中的X_i轴与坐标系i-1中的X_i-1轴的夹角。

步骤S2中，包括以下具体步骤：

S21，对坐标系0至坐标系I中相邻的两个坐标系分别建立齐次变换矩阵；其中，坐标系i与坐标系i-1的齐次变换矩阵

其中，连杆扭角α_i为Z_i-1轴和Z_i轴的夹角；连杆长度b_i为坐标系i和坐标系i-1的公垂线的线段距离；连杆距离d_i为坐标系i的原点在坐标系i-1的Z_i-1轴上的投影与坐标系i-1的原点之间的距离；θ_i为第i个关节的关节转角即第i个关节转角；

S22，适应度函数F(x)为：

其中，x为各个关节转角所构成的向量，即x＝[θ_i|i＝1,2,…,I]；

坐标系I相对于坐标系0的齐次坐标变换矩阵为

根据矩阵连乘求出：

表示坐标系I相对于坐标系0的齐次坐标变换矩阵

中的第p行第q列元素的元素值，p＝1,2,3，q＝1,2,3,4；

nx、ny、nz分别为坐标系I的X_I轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；ox、oy、oz分别为坐标系I的Y_I轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；ax、ay、az分别为坐标系I的Z_I轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；px、py、pz为坐标系I的原点在坐标系0中的笛卡尔坐标值。

步骤S2中，适应度值为fx采用2-范数进行表示，即fx＝||F(x)||₂。

步骤S32中，step_t为变步长，变步长step_t＝step_t-1*e^-kt+step′，step′为基本分辨率，k为衰减率。

步骤S38中，判断第t次迭代是否达到设定条件为：判断迭代次数t是否达到设定的迭代总次数。

本发明的优点在于：

(1)基于以负指数次幂衰减的变步长step_t进行搜索，由于天牛的初始位置往往距离真实解较远，故迭代初期的变步长step_t设置较大，随着天牛的前进，变步长step_t逐渐缩小，由于变步长step_t和天牛个体大小d即为天牛的左右须距离d成正比，在迭代初期变步长step_t设置较大能够提高全局搜索能力，在迭代后期变步长step_t逐渐缩小能够提高局部搜索能力。

(2)随着迭代次数的增多，指数衰减逐渐趋于0，不利于高迭代次数下的局部搜索，因此采用基本分辨率step′，随着迭代次数的增多，变步长step_t逐渐归为基本分辨率step′，保障了高迭代次数下的局部搜索。

(3)借鉴粒子群算法中全局信息的概念，记录天牛运动过程中的天牛左须位置和右须位置的最优适应度值，令天牛的下一次迭代的位置更新参考全局最优的左、右须适应度值，从而加快天牛搜索速度。

(4)引入了接受概率p，以适当的接受概率p接受一个较劣解，从而减小陷入局部最优的概率，且随着迭代次数的不断增加，||F(x^t+1)||₂和||F(x_best)||₂的差值波动较小，随着迭代的进行，接受较劣解的接受概率p会逐步降低。

(5)在MATLAB环境下对本发明进行仿真，将本发明的基于改进天牛须算法即IBAS算法的机器人运动学逆解方法与传统的天牛须算法即BAS算法、遗传算法即MPGA算法、粒子群算法即SAEPSO算法进行对比，对每种算法分别进行独立运算100次，并对各种算法每次最优适应度进行统计，带入位置误差和姿态误差公式，将位置误差和姿态误差的均值作为统计对象，分别统计各种算法运行100次的最优、最差、平均值和标准差，从而可知IBAS算法的四个评价指标均比其他3种算法好，其中，相比于传统BAS算法，IBAS算法的平均值高2个数量级，故IBAS算法的精度高，且IBAS算法的标准差相对较小，因此本发明的基于改进天牛须算法的机器人运动学逆解方法可靠性更强、精度更高、收敛速度更快。

附图说明

图1为本发明的基于改进天牛须算法即IBAS算法的机器人运动学逆解方法的流程图。

图2为本实施例的机器人的模型和坐标系示意图。

图3为IBAS算法、BAS算法、MPGA算法、SAEPSO算法之间对比图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

由图2所示，本实施例的机器人为可用于电力铁塔攀爬的六自由度机器人，包括基座、5个连杆、6个关节即旋转关节。

由图1所示，本实施例的机器人的运动学逆解方法，包括以下步骤：

S1，构建机器人的mdh模型：基于DH参数法分别建立基坐标系和各个关节坐标系。由图2所示，坐标系0为基座标系；坐标系i为第i个关节坐标系，i＝1,2,…,6。

坐标系i即第i个关节坐标系中包括X_i轴、Y_i轴、Z_i轴；坐标系0即基坐标系即基座坐标系中包括X₀轴、Y₀轴、Z₀轴；

第i个关节的关节转角θ_i为坐标系i中的X_i轴与坐标系i-1中的X_i-1轴的夹角；

第1个关节至第6个关节依次为从基座到末端执行器的6个旋转关节；坐标系6即第6个关节坐标系为末端执行器坐标系。

步骤S1中，基坐标系和各个关节坐标系的建立方式，具体如下所示：

S11，建立坐标系0，坐标系0的选择是任意的，可以选择将坐标系0的原点放置在基座关节轴线Z₀轴的任意位置，由图2所示，坐标系0的原点放置于最下方。

S12，从第1个关节至第6个关节，依次建立坐标系i，i＝1,2,…,6；其中，坐标系i的建立方式为：

首先确定坐标系i中的Z_i轴方向，Z_i轴方向与第i个关节的轴线方向保持一致；

然后确立坐标系i中的X_i轴方向，当坐标系i-1中的Z_i-1轴和坐标系i中的Z_i轴不共面时，Z_i-1轴和Z_i轴的公垂线定义为X_i轴，X_i轴与Z_i轴的交点即为坐标系i的原点；当Z_i-1轴平行于Z_i轴时，Z_i-1轴和Z_i轴之间存在无穷多个共同法线，将穿过坐标系i-1的原点的法线定义为X_i轴，坐标系i的原点是该法线与Z_i轴的交点；当Z_i-1轴和Z_i轴相交时，X_i轴为垂直于Z_i-1轴和Z_i轴的交点，且Z_i轴上的任意一点都可作为坐标系i的原点；

最后确立坐标系i中的Y_i轴方向，Y_i轴方向由Z_i轴和X_i轴确定，以Z_i轴为右手拇指，依据右手定则确定。

由图2所示，L₁到L₇依次为坐标系0至坐标系6中依次相邻的两个坐标系的原点的距离。

S2，将机器人的各个关节的关节转角所构成的向量作为天牛的位置x，即x＝[θ₁ θ₂θ₃ θ₄ θ₅ θ₆]，构建适应度函数F(x)。

步骤S2中，包括以下具体步骤：

S21，对坐标系0至坐标系6中依次相邻的两个坐标系建立齐次变换矩阵；其中，坐标系i与坐标系i-1的齐次变换矩阵

如下所示：

其中，连杆扭角α_i为Z_i-1轴和Z_i轴的夹角；连杆长度b_i为坐标系i和坐标系i-1的公垂线的线段距离；连杆距离d_i为坐标系i的原点在坐标系i-1的Z_i-1轴上的投影与坐标系i-1的原点之间的距离；θ_i为第i个关节的关节转角。

本实施例中，所选用的mdh参数如表1所示：

i	θ<sub>i</sub>	α<sub>i-1</sub>	d<sub>i</sub>	b<sub>i-1</sub>	offset<sub>i</sub>
						1	θ<sub>1</sub>(0°)	0°	0	0	0
2	θ<sub>1</sub>(0°)	-90°	0	0	pi/2
						3	θ<sub>1</sub>(0°)	0°	0	0.5	0
4	θ<sub>1</sub>(0°)	0°	0	0.6	0
						5	θ<sub>1</sub>(0°)	0°	0	0.5	pi/2
6	θ<sub>1</sub>(0°)	-90°	0	0	0

表1

offset_i为初始时刻时，机器人的各个关节转角的初始值。

S22，适应度函数F(x)为：

其中，x为各个关节的关节转角所构成的向量，即x＝[θ₁ θ₂ θ₃ θ₄ θ₅ θ₆]；

坐标系6相对于坐标系0的齐次坐标变换矩阵为

根据矩阵连乘求出：

表示坐标系6相对于坐标系0的齐次坐标变换矩阵

的第p行第q列元素的元素值，p＝1,2,3，q＝1,2,3,4；

nx、ny、nz分别为坐标系6的X₆轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；ox、oy、oz分别为坐标系6的Y₆轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；ax、ay、az分别为坐标系6的Z₆轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；px、py、pz为坐标系6的原点在坐标系0中的笛卡尔坐标值。

S3，基于改进天牛须算法对机器人进行运动学逆解，具体如下所示：

S31，定义代次数为t，定义天牛的位置为x，定义天牛的左须位置

定义天牛的右须位置

其中，d为天牛的左须和右须之间的距离即左右须距离，

为天牛个体每次迭代的天牛朝向，为随机的方向向量；

定义step_t为变步长，每步迭代中步长以负指数次幂衰减，变步长step_t＝step_t-1*e^-kt+step′，step′为基本分辨率，优选step′＝0.008；k为衰减率，优选k＝-ln0.93；step₀＝1；

根据适应度函数F(x)可知，由于适应度函数F(x)是非线性方程组，因此定义适应度值采用2-范数的形式。

初始化迭代次数t＝0，将机器人的各个关节的关节转角θ_i的当前值

所构成的向量初始化为天牛的初始位置x⁰，即

天牛的初始左须位置

天牛的初始右须位置

将天牛的初始位置x⁰设为最优位置x_best，即x_best＝x⁰，且天牛的左须最优位置

天牛的右须最优位置

根据适应度函数F(x)可知，天牛的最优位置x_best所对应的最优适应度值为||F(x_best)||₂，天牛的左须最优位置x_lbest所对应的左须最优适应度值为||F(x_lbest)||₂，天牛的右须最优位置x_rbest所对应的右须最优适应度值为||F(x_rbest)||₂；

S32，进行第t次迭代，根据天牛的第t次迭代的位置x^t，得到天牛的第t次迭代的左须位置

天牛的第t次迭代的右须位置

天牛的第t次迭代的适应度值||F(x^t)||₂，天牛的第t次迭代的右须适应度值

天牛的第t次迭代的左须适应度值

其中，d^t为天牛的第t次迭代的左右须距离，d^t＝step_t/c₀，step_t为步长，c₀为比例系数；step₀＝1；c₀＝2；

S33，比较天牛的第t次迭代的左须适应度值

和天牛的左须最优适应度值||F(x_lbest)||₂之间的大小，若

小于||F(x_lbest)||₂，则对天牛的左须最优位置x_lbest和左须最优适应度值||F(x_lbest)||₂的值分别进行更新，将天牛的左须最优位置x_lbest的值更新为

的值，将天牛的左须最优适应度值||F(x_lbest)||₂的值更新为

的值；

比较天牛的第t次迭代的右须适应度值

和天牛的右须最优适应度值||F(x_rbest)||₂之间的大小，若

小于||F(x_rbest)||₂，则对天牛的右须最优位置x_rbest和右须最优适应度值||F(x_rbest)||₂的值分别进行更新，将天牛的右须最优位置x_rbest的值更新为

的值，将天牛的右须最优适应度值||F(x_rbest)||₂更新为

的值；

S34，将步骤S33更新得到的天牛的左须最优位置x_lbest和右须最优位置x_rbest代入如下公式进行计算，得到天牛的第t次迭代的位置偏量G^t：

G^t＝c_lr_d(x_lbest-x^t)+c_rr_d(x_rbest-x^t)；

其中，r_d为0～1之间的随机常数；G^t为天牛的第t次迭代的位置偏量；c_r为右须系数；c_l为左须系数；

λ为设定的常数，λ的取值为0和1之间；本实施例中，λ取值为0.857；

S35，根据天牛的第t次迭代的位置偏量G^t与天牛的第t次迭代的位置x^t，计算天牛的下一次迭代即第t+1次迭代的位置x^t+1，具体如下所示：

根据天牛的下一次迭代即第t+1次迭代的位置x^t+1和适应度函数F(x)，得到对应的第t+1次迭代的适应度值为||F(x^t+1)||₂。

S36，计算接受概率p，并根据接受概率p，判断是否对天牛的最优位置x_best和最优适应度值||F(x_best)||₂的值进行更新：

若||F(x^t+1)||₂＜||F(x_best)||₂，则p＝1，将天牛的最优位置x_best更新为天牛的第t+1次迭代的位置x^t+1，将天牛的最优适应度值||F(x_best)||₂的值更新为天牛的第t+1次迭代的适应度值||F(x^t+1)||₂，输出天牛的最优位置x_best和最优适应度值fx_best的值；

若||F(x^t+1)||₂≥||F(x_best)||₂，则

T的取值为1/t，此时，若rand＜p，则将天牛的最优位置x_best更新为天牛的第t+1次迭代的位置x^t+1，将天牛的最优适应度值||F(x_best)||₂更新为天牛的第t+1次迭代的适应度值||F(x^t+1)||₂，输出天牛的最优位置x_best和最优适应度值fx_best的值；若rand≥p，则不对天牛的最优位置x_best和最优适应度值||F(x_best)||₂的值进行更新，输出天牛的最优位置x_best和最优适应度值fx_best的值；其中，rand为0～1之间的随机常数；

S37，判断第t次迭代是否达到设定条件，即判断当前迭代次数t是否达到设定的迭代总次数，若达到，则迭代结束；若未达到，则跳转步骤S32，将步骤S35计算得到天牛的下一次迭代即第t+1次迭代的位置x^t+1带入步骤S32中，继续进行下一次迭代即第t+1次迭代。

在本发明的改进天牛须算法中：

基于以负指数次幂衰减的变步长step_t进行搜索，由于天牛的初始位置往往距离真实解较远，故迭代初期的变步长step_t设置较大，随着天牛的前进，变步长step_t逐渐缩小，由于变步长step_t和天牛个体大小d即为天牛的左右须距离d成正比，在迭代初期变步长step_t设置较大能够提高全局搜索能力，在迭代后期变步长step_t逐渐缩小能够提高局部搜索能力。

随着迭代次数的增多，指数衰减逐渐趋于0，不利于高迭代次数下的局部搜索，因此采用基本分辨率step′，随着迭代次数的增多，变步长step_t逐渐归为基本分辨率step′，保障了高迭代次数下的局部搜索。

借鉴粒子群算法中全局信息的概念，记录天牛运动过程中的天牛左须位置和右须位置的最优适应度值，令天牛的下一次迭代的位置更新参考全局最优的左、右须适应度值，从而加快天牛搜索速度。

引入了接受概率p，以适当的接受概率p接受一个较劣解，从而减小陷入局部最优的概率，且随着迭代次数的不断增加，||F(x^t+1)||₂和||F(x_best)||₂的差值波动较小，随着迭代的进行，接受较劣解的接受概率p会逐步降低。

在MATLAB环境下对本发明进行仿真，将本发明的基于改进天牛须算法的机器人运动学逆解方法与传统的天牛须算法、遗传算法、粒子群算法进行对比；根据实际设计需要，L₂＝0.5m，L₃＝0.6m，L₄＝0.5m；设置各个算法的参数，

本发明的改进天牛须算法即IBAS算法中，天牛步长和天牛个体大小d成正比，d＝step/c₀，c₀为比例系数，c₀＝2，变步长step_t＝step_t-1*e^-kt+step₀，衰减率k＝-ln0.93，初始步长step₀＝1，基本分辨率step′＝0.006，最大迭代次数为140次；传统的天牛须算法即BAS算法中，令步长δ＝1；IBAS算法和BAS算法中，天牛的初始位置x⁰初始角设置为机器人的各个关节的关节转角θ_i的当前值

所构成的向量，

初始时刻时，机器人的各个关节转角的初始值依次为0、pi/2、0、0、pi/2、0。

传统的遗传算法即MPGA算法中，种群规模取值为30，交叉概率取值为0.8，变异概率取值为0.001到0.05之间的随机数；传统的粒子群算法即SAEPSO算法中，学习因子取值为1.7、1.5，近似度取值为10^-6，弹射概率取值为0.2，弹射选择系数取值为0.2，惯性权重取值为0.3，种群规模取值为30；传统的遗传算法和粒子群算法中初始值均采用混沌初始化的算法进行初始化各种群个体初值，给定每种算法的最大迭代次数为140。

由图3所示，迭代前期IBAS算法由于按照一定的接受概率接受坏值和大步长参数的设置，所以IBAS算法的前40次迭代过程中出现最优适应度值大于BAS算法的现象，有利于天牛个体在机器人逆解寻解过程中跳出局部最优值。MPGA算法和SAEPSO算法也分别以基于动态变异率的变异算子和弹射概率的参数设置进行全局寻优，但相比IBAS算法，SAEPSO算法和MPGA算法前期下降速度较快，不利于全局寻优，因此IBAS算法的跳出全局的局部搜索能力强。在经过40次迭代后，SAEPSO算法少部分点的适应度值和IBAS算法相同或者更好，但SAEPSO算法适应度值波动较大，稳定性差，IBAS算法后期由于和目标解较近，所以设置步长的衰减使得天牛逐渐变为小天牛进行精细搜索快速收敛，从而找到最优解。

对每种算法分别进行独立运算100次，并对各种算法每次最优适应度进行统计，带入位置误差和姿态误差公式，将位置误差和姿态误差的均值作为统计对象，分别统计各种算法运行100次的最优、最差、平均值和标准差，统计结果如下表2所示，从而可知IBAS算法的四个评价指标均比其他3种算法好，其中，相比于传统BAS算法，IBAS算法的平均值高2个数量级，故IBAS算法的精度高，且IBAS算法的标准差相对较小，因此本发明的基于改进天牛须算法的机器人运动学逆解方法可靠性更强、精度更高、收敛速度更快。

算法	BAS	IBAS	MPGA	SAEPSO
					最差	8.346×10<sup>-2</sup>	4.154×10<sup>-3</sup>	9.147×10<sup>-1</sup>	2.133×10<sup>-2</sup>
最优	2.812×10<sup>-3</sup>	6.915×10<sup>-5</sup>	3.766×10<sup>-3</sup>	7.315×10<sup>-4</sup>
					平均值	3.538×10<sup>-2</sup>	2.336×10<sup>-4</sup>	5.288×10<sup>-2</sup>	5.699×10<sup>-3</sup>
标准差	6.127×10<sup>-2</sup>	5.461×10<sup>-4</sup>	2.439×10<sup>-1</sup>	4.521×10<sup>-3</sup>

表2

以上仅为本发明创造的较佳实施例而已，并不用以限制本发明创造，凡在本发明创造的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明创造的保护范围之内。

Claims (5)

1.一种电力智能仿生攀爬机器人的运动学逆解方法，其特征在于，包括以下步骤：

S1，构建机器人的模型，分别建立基坐标系和各个关节坐标系；其中，坐标系0为基座标系；坐标系i为第i个关节坐标系，i＝1,2,…,I，I为关节总数量；

S2，将机器人的各个关节转角所构成的向量作为天牛的位置x，构建适应度函数F(x)，得到天牛的位置x的适应度值为fx；

S3，经天牛须算法的迭代后，得到的天牛的最优位置x_best，即得到机器人的各个关节转角的最优值；

步骤S3中，具体过程如下所示：

S31，进行初始化，即迭代次数t＝0，将机器人的各个关节转角的当前值所构成的向量作为天牛的初始位置x⁰，天牛的初始左须位置为

天牛的初始右须位置为

将天牛的初始位置x⁰设为最优位置x_best，即x_best＝x⁰，且天牛的左须最优位置

天牛的右须最优位置

天牛的最优位置x_best所对应的最优适应度值为fx_best，天牛的左须最优位置fx_lbest所对应的左须最优适应度值为fx_lbest，天牛的右须最优位置x_rbest所对应的右须最优适应度值为fx_rbest；

S32，进行第t次迭代，根据天牛的第t次迭代的位置x^t，得到天牛的第t次迭代的左须位置

天牛的第t次迭代的右须位置

天牛的第t次迭代的适应度值为fx^t，天牛的第t次迭代的右须适应度值为

天牛的第t次迭代的左须适应度值为

其中，

为随机的方向向量，d^t为天牛的第t次迭代的左右须距离，d^t＝step_t/c₀，step_t为步长，c₀为比例系数；

S33，比较天牛的第t次迭代的左须适应度值

和天牛的左须最优适应度值之间fx_lbest的大小，若

小于fx_lbest，则对天牛的左须最优位置x_lbest和左须最优适应度值fx_lbest分别进行更新，将天牛的左须最优位置x_lbest的值更新为

的值，将天牛的左须最优适应度值fx_lbest更新为

比较天牛的第t次迭代的右须适应度值

的和天牛的右须最优适应度值fx_rbest之间的大小，若

小于fx_rbest，则对天牛的右须最优位置x_rbest和右须最优适应度值fx_rbest分别进行更新，将天牛的右须最优位置x_rbest的值更新为

的值，将天牛的右须最优适应度值fx_rbest更新为

S34，将步骤S33更新得到的天牛的左须最优位置x_lbest和右须最优位置x_rbest代入如下公式进行计算，得到天牛的第t次迭代的位置偏量G^t：

G^t＝c_lr_d(x_lbest-x^t)+c_rr_d(x_rbest-x^t)；

其中，r_d为0～1之间的随机常数；G^t为天牛的第t次迭代的位置偏量；c_r为右须系数；c_l为左须系数；

λ为设定的常数，λ的取值为0和1之间；

S35，根据天牛的第t次迭代的位置偏量G^t与天牛的第t次迭代的位置x^t，计算天牛的下一次迭代即第t+1次迭代的位置x^t+1，具体如下所示：

根据天牛的下一次迭代即第t+1次迭代的位置x^t+1和适应度函数F(x)，得到对应的第t+1次迭代的适应度值为fx^t+1；

S36，计算接受概率p，并根据接受概率p，判断是否对天牛的最优位置x_best和最优适应度值fx_best进行更新：

若fx^t+1＜fx_best，则p＝1，且将天牛的最优位置x_best更新为天牛的第t+1次迭代的位置x^{t +1}，将天牛的最优适应度值fx_best更新为天牛的第t+1次迭代的适应度值fx^t+1；

若fx^t+1≥fx_best，则

T的取值为1/t，此时，若rand＜p，则将天牛的最优位置x_best更新为天牛的第t+1次迭代的位置x^t+1，将天牛的最优适应度值fx_best更新为天牛的第t+1次迭代的适应度值fx^t+1；若rand≥p，则不对天牛的最优位置x_best和最优适应度值fx_best进行更新；其中，rand为0～1之间的随机常数；

S37，判断第t次迭代是否达到设定条件，若达到，则迭代结束，输出天牛的最优位置x_best和最优适应度值fx_best的值；；若未达到，则跳转步骤S32，将步骤S35计算得到天牛的下一次迭代即第t+1次迭代的位置x^t+1带入步骤S32中，继续进行下一次迭代即第t+1次迭代；

步骤S32中，step_t为变步长，变步长step_t＝step_t-1*e^-kt+step′，step′为基本分辨率，k为衰减率。

2.根据权利要求1所述的一种电力智能仿生攀爬机器人的运动学逆解方法，其特征在于，步骤S1中，坐标系0即基坐标系即基座坐标系中包括X₀轴、Y₀轴、Z₀轴；坐标系1到坐标系I依次为从基座到末端执行器的I个旋转关节的关节坐标系；坐标系i即第i个关节坐标系中包括X_i轴、Y_i轴、Z_i轴；第i个关节的关节转角θ_i为坐标系i中的X_i轴与坐标系i-1中的X_i-1轴的夹角。

3.根据权利要求2所述的一种电力智能仿生攀爬机器人的运动学逆解方法，其特征在于，步骤S2中，包括以下具体步骤：

S21，对坐标系0至坐标系I中相邻的两个坐标系分别建立齐次变换矩阵；其中，坐标系i与坐标系i-1的齐次变换矩阵

其中，连杆扭角α_i为Z_i-1轴和Z_i轴的夹角；连杆长度b_i为坐标系i和坐标系i-1的公垂线的线段距离；连杆距离d_i为坐标系i的原点在坐标系i-1的Z_i-1轴上的投影与坐标系i-1的原点之间的距离；θ_i为第i个关节的关节转角即第i个关节转角；

S22，适应度函数F(x)为：

其中，x为各个关节转角所构成的向量，即x＝[θ_i|i＝1,2,…,I]；

坐标系I相对于坐标系0的齐次坐标变换矩阵为

根据矩阵连乘求出：

表示坐标系I相对于坐标系0的齐次坐标变换矩阵

中的第p行第q列元素的元素值，p＝1,2,3，q＝1,2,3,4；

nx、ny、nz分别为坐标系I的X_I轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；ox、oy、oz分别为坐标系I的Y_I轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；ax、ay、az分别为坐标系I的Z_I轴与坐标系0的X₀轴、Y₀轴、Z₀轴的夹角余弦值；px、py、pz为坐标系I的原点在坐标系0中的笛卡尔坐标值。

4.根据权利要求3所述的一种电力智能仿生攀爬机器人的运动学逆解方法，其特征在于，步骤S2中，适应度值为fx采用2-范数进行表示，即fx＝||F(x)||₂。

5.根据权利要求1所述的一种电力智能仿生攀爬机器人的运动学逆解方法，其特征在于，步骤S38中，判断第t次迭代是否达到设定条件为：判断迭代次数t是否达到设定的迭代总次数。

Images