CN111844023A - 一种基于天牛须算法的六自由度机器人逆解算法 - Google Patents

Abstract

一种基于天牛须算法的六自由度机器人逆解算法，包括以下步骤：S1、将六自由度关节机器人当前所处的各关节角θ′₁至θ′₆作为迭代初始角；S2、根据六自由度关节机器人的sdh模型下的矩阵连乘计算式的一般表达式求解出

把旋转分量和位置分量分别对应于六自由度关节机器人的末端坐标系的位姿的旋转分量和位置分量；S3、根据天牛须算法，利用符号函数判断下一步的飞行方向，更新X的值，并再次判断F_left＝F(X_l)和F_right＝F(X_r)的大小；S4、重复步骤S3的迭代计算直到达到已设置的循环次数，此时的X即为六自由度关节机器人的逆解。本申请使用的天牛须算法属于智能优化算法的一种，其迭代过程不易受梯度和初始值的影响，从而能够实时快速、准确地得到机器人逆解角度。

Description

一种基于天牛须算法的六自由度机器人逆解算法

技术领域

本发明涉及工业机器人控制技术领域，尤其涉及一种基于天牛须算法的六自由度机器人逆解算法。

背景技术

机器人的逆运动学求解是机器人离线编程、轨迹规划和控制算法设计等其他课题研究的前提和基础。逆运动学求解的实质是完成机器人工作空间到关节空间的映射，逆运动学方程组具有高纬度、非线性的特点，求解复杂且不易求出。许多学者在该领域做出的大量的研究，提出了许多理论和方法。传统的方法有代数法，几何法，和数值法等。代数法主要以消元的方式，将机器人位置反解中的高维方程组转化为低维方程组，从而求得所有运动学逆解。该方法需要进行大量的三角变换，简化过程十分复杂。甚至可以认为这样求解非线性代数方程往往需要靠直觉或者运气。几何法针对机器人的某些特殊结构进行简化，再进行求解。一般无法单独使用，甚至无法使用。

发明内容

为了能够提高机器人逆解角度的精准度、收敛性，为此，本发明提供一种基于天牛须算法的六自由度机器人逆解算法。本发明采用以下技术方案：

一种基于天牛须算法的六自由度机器人逆解算法，包括以下步骤：

S1、确定六自由度机器人的sdh模型，将六自由度关节机器人当前所处的各关节角θ′₁至θ′₆作为迭代初始角；

S2、根据六自由度关节机器人的sdh模型下的矩阵连乘计算式的一般表达式求解出

把旋转分量和位置分量分别对应于六自由度关节机器人的末端坐标系的位姿的旋转分量和位置分量，可得十二个方程，令其为F(X)＝0,其中未知数X为六个关节角；

S3、根据天牛须算法，利用天牛觅食过程简化抽象模型，计算左须函数值F_left＝F(X_l)和右须函数值F_right＝F(X_r)的大小，X_l表示天牛的左须坐标，X_r表示天牛的右须坐标，X表示质心坐标；利用符号函数判断下一步的飞行方向，并更新X的值：X＝X-step*dir*sign(F_left-F_right)；将得到的新的X带入X_l和X_r的表达式中；

S4、重复步骤S3的迭代计算直到达到已设置的循环次数，此时的X即为六自由度关节机器人的逆解。

本发明的优点在于：本申请使用的天牛须算法属于智能优化算法的一种，其迭代过程不易受梯度和初始值的影响，从而能够实时快速、准确地得到机器人逆解角度。

附图说明

图1为本发明涉及的一种六自由度关节机器人建立的空间坐标系示意图。

图2为本发明涉及的算法流程图。

图3为本发明涉及的基于天牛须算法的六自由度工业机器人逆解的迭代结果图。

具体实施方式

如图2所示，一种基于天牛须算法的六自由度机器人逆解算法，包括以下步骤：

S1、确定六自由度机器人的sdh模型，将六自由度关节机器人当前所处的各关节角θ₁′至θ₆′作为迭代初始角；

具体步骤如下：

S11、确定六自由度关节机器人的sdh模型：首先确定基坐标系，基座标系的选择近乎时任意的，可以选择将基坐标系的原点放置在Z₀轴的任意位置，如图1所示，建立0坐标系，确定Z_i轴，其方向与关节轴线方向保持一致。然后确立X_i方向，当轴Z_i-1和轴Z_i不共面时，轴Z_i-1和轴Z_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坐标系的原点。最后Y_i的方向由z轴和x轴确定，以z轴为右手拇指，依据右手定则确定。根据上述规则从基座往机器人末端方向，依次建立7个坐标系，分别为坐标系{0}～坐标系{6}，其中坐标系{0}即所述基座坐标系{0}，坐标系{6}即末端坐标系{6}，如图1所示，L1到L7为各个关节坐标系原点之间的距离。如图1所示，菱形表示转动轴平行于纸面，圆环表示转动轴垂直于直面。

sdh模型连杆参数α_i表示坐标系i-1的z轴和坐标系z轴夹角i

a_i表示坐标系i和坐标系i-1的公垂线线段距离；

d_i表示i坐标系的原点在i-1坐标系z轴上的投影，距离i-1坐标系原点的距离；

θ_i表示i-1坐标系的x轴与i-2坐标系的夹角；

offset表示偏移角，指机器人的初始时刻，各θ_i的角度值。本专利具体选用的sdh参数如表1所示。

i	α<sub>i</sub>	a<sub>i</sub>	d<sub>i</sub>	θ<sub>i</sub>′	offset
						1	pi/2	0.050	0.3215	θ<sub>1</sub>′	0
2	0	0.270	0	θ<sub>2</sub>′	pi/2
						3	pi/2	0.07	0	θ<sub>3</sub>′	0
4	pi/2	0	0.299	θ<sub>4</sub>′	0
						5	pi/2	0	0	θ<sub>5</sub>′	pi/2
6	0	0	0.0785	θ<sub>6</sub>′	0

表1

S12、取六自由度关节机器人当前所处各关节角θ′₁至θ′₆作为迭代初始角。

S2、根据六自由度关节机器人的sdh模型下的矩阵连乘计算式的一般表达式求解出

把旋转分量和位置分量分别对应于六自由度关节机器人的末端坐标系的位姿的旋转分量和位置分量，可得十二个方程，令其为F(X)＝0,其中未知数X为六个关节角。

步骤S2的具体操作步骤包括：

S21、sdh矩阵变换的一般表达式：

根据上述sdh矩阵变换的一般表达式及表1可得：

S22、根据矩阵连乘求出:

S23、根据矩阵连乘求出

使得：

可得十二个方程，建立适应度函数F(X)令其为：

令F(X)＝0，其中未知数X为六个关节角；,

表示坐标系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、已知F(X)＝0为十二个方程其中每个方程含有六个未知数，根据天牛须算法，利用天牛觅食过程简化抽象模型，判断F_left＝F(Xl)和F_right＝F(X_r)的大小，所述X_l表示天牛的左须坐标，X_r表示天牛的右须坐标，X表示质心坐标；利用符号函数判断下一步的飞行方向，并更新X的值：X＝X-step*dir*sign(F_lef_t-F_right)；将得到的新的X带入X_l和X_r的表达式中，并再次判断F_lef_t＝F(X_l)和F_right＝F(X_r)的大小；

具体步骤为：

S31、根据天牛觅食原理中，天牛头朝向任意，得到天牛右须指向左须的向量的朝向的随机向量dir＝rands(n,1)；其中n指未知数的个数，所述d_ir表示表示朝向，rands为随机函数，此处作用是随机生成关于n*1的向量，表示天牛右须指向左须的向量；

S32、对天牛右须指向左须的向量的朝向进行归一化，表示为：dir＝dir/norm(dir)，norm表示求向量范数的函数，继而得到X_l-X_r＝d₀*dir，X_l，X_r可表示成质心的表达式，即左须位置为X_l＝X+d₀*dir/2；右须位置为X_r＝X-d₀*dir/2，其中d₀表示左右两须之间距离；

S33、计算左须函数值F_left＝F(X_l)和右须函数值F_right＝F(X_r)的大小；

S34、利用符号函数sign判断下一步的飞行方向，并更新X的值：X＝X-step*dir*sign(F_left-F_right)，step表示步长，将得到的新的X带入X_l和X_r的表达式中，并再次判断F_lef_t＝F(X_l)和F_right＝F(X_r)的大小。step＝d₀*c，其中c是常数，每步迭代中采用step＝step*eta，其中eta在0和1之间且靠近1，该方案中eta＝0.95。

重复步骤S3的迭代计算直到达到已设置的循环次数，此时的X即为六自由度关节机器人的逆解。

实验采用matlab软件进行仿真。取eta＝0.96,c＝100,维数k＝6；取循环次数n＝500,因为此处使用垂直六自由度关节机器人各关节的当前角作为迭代起点，和目标解很近，故使起始步长设置为step＝0.5，且此处宜使用小天牛即使得d₀变小,同时，此处还引入变步长的方法提高天牛须收敛算法的速度和精度。temp₀＝0.05；temp₁＝temp₁*eta；step＝temp₁+temp₀，图3中横轴为迭代次数，纵轴为F(X)的2范数，如图3结果表明，前50次F(X)能较快下降，最终在终止条件内，迭代结果趋于稳定。将迭代得到的结果x带入六自由关节机器人正解函数，求得的位姿与目标位姿相减，旋转分量和位置分量差值小于0.01，可认为一致。

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

Claims (6)

1.一种基于天牛须算法的六自由度机器人逆解算法，其特征在于，包括以下步骤：

S1、确定六自由度机器人的sdh模型，将六自由度关节机器人当前所处的各关节角θ′₁至θ′₆作为迭代初始角；

S2、根据六自由度关节机器人的sdh模型下的矩阵连乘计算式的一般表达式求解出

把旋转分量和位置分量分别对应于六自由度关节机器人的末端坐标系的位姿的旋转分量和位置分量，可得十二个方程，令其为F(X)＝0,其中未知数X为六个关节角；

S3、根据天牛须算法，利用天牛觅食过程简化抽象模型，计算左须函数值F_left＝F(X_l)和右须函数值F_right＝F(X_r)的大小，X_l表示天牛的左须坐标，X_r表示天牛的右须坐标，X表示质心坐标；利用符号函数判断下一步的飞行方向，并更新X的值：X＝X-step*dir*sign(F_left-F_right)；将得到的新的X带入X_l和X_r的表达式中；

S4、重复步骤S3的迭代计算直到达到已设置的循环次数，此时的X即为六自由度关节机器人的逆解。

2.根据权利要求1所述的一种基于天牛须算法的六自由度机器人逆解算法，其特征在于，所述步骤S1的具体步骤如下：

S11、确定六自由度关节机器人的sdh模型：以六自由度串联机器人的关节1旋转轴与关节2旋转轴交点为原点，关节1旋转轴所在直线为坐标系Z轴，关节2旋转轴所在直线为坐标系Y轴，建立基座坐标系；从基座往机器人末端方向，根据六个自由度依次建立6个坐标系，分别为坐标系{0}～坐标系{6}，其中坐标系{0}即所述基座坐标系{0}，坐标系{6}即末端坐标系{6}；

S12、取六自由度关节机器人当前所处各关节角θ′₁至θ′₆作为迭代初始角。

3.根据权利要求2所述的一种基于天牛须算法的六自由度机器人逆解算法，其特征在于，步骤S2的具体操作步骤包括：

S21、sdh矩阵变换的一般表达式：

其中连杆扭角α_i表示Z_i-1轴和Z_i轴的夹角；连杆长度a_i表示坐标系i和坐标系i-1的公垂线线段距离；连杆距离d_i表示i坐标系的原点在Z_i-1轴上的投影，距离i-1坐标系原点的距离；θ_i表示X_i-1轴与X_i-2坐标系的夹角；

S22、根据矩阵连乘求出:

S23、根据矩阵连乘求出

使得：

可得十二个方程，令其为F(X)＝0,其中未知数X为六个关节角。

4.根据权利要求3所述的一种基于天牛须算法的六自由度机器人逆解算法，其特征在于，步骤S3的具体步骤如下：

S31、根据天牛觅食原理中，天牛头朝向任意，得到天牛右须指向左须的向量的朝向的随机向量dir＝rands(n,1)；其中n指未知数的个数，所述d_ir表示表示朝向，rands(n，1)为用于产生n*1阶随机向量的函数；

S32、对天牛右须指向左须的向量的朝向进行归一化，表示为：dir＝dir/norm(dir)，norm表示求向量范数的函数，继而得到X_left-X_right＝d₀*dir，左须位置X_l＝X+d₀*dir/2；右须位置X_r＝X-d₀*dir/2，其中d₀表示左右两须之间距离；

S33、计算左须函数值F_left＝F(X_l)和右须函数值F_right＝F(X_r)的大小；

S34、利用符号函数sign判断下一步的飞行方向，并更新X的值：X＝X-step*dir*sign(F_left-F_right)，step表示步长，将得到的新的X带入X_l和X_r的表达式中，并再次判断F_left＝F(X_l)和F_right＝F(X_r)的大小。

5.根据权利要求4所述的一种基于天牛须算法的六自由度机器人逆解算法，其特征在于，step为变步长，step＝d₀*c，其中c是常数，每步迭代中采用step＝step*eta，其中eta在0和1之间且靠近1。

6.根据权利要求5所述的一种基于天牛须算法的六自由度机器人逆解算法，其特征在于，所述eta取值0.95。

Images