CN111482969A

CN111482969A - 一种基于bas算法的六自由度偏置机器人逆解方法

Info

Publication number: CN111482969A
Application number: CN202010593995.2A
Authority: CN
Inventors: 甘亚光; 张晓龙; 甘亚辉; 刘元建
Original assignee: Inexbot Technology Co ltd
Current assignee: Inexbot Technology Co ltd
Priority date: 2020-06-28
Filing date: 2020-06-28
Publication date: 2020-08-04

Abstract

本发明属于机器人逆运动学领域，具体涉及一种六自由度偏置机器人的逆运动学通用求解方法。该方法首先将手腕无偏置型机器人运动学逆解作为迭代起点，其后通过齐次坐标变换矩阵构建适应度函数F(X)，最后通过BAS算法，引入步长改进策略，以及天牛个体大小缩变策略，由迭代初始的大步长大天牛粗略搜索大致范围，到迭代中后期的小步长小天牛精细搜索，比较左须函数值和右须函数值的大小，确定下一次飞行方向和位置，同时引入随机因素，提高全局寻优能力，不断迭代直至得到手腕偏置型机器人的逆运动学解。该方法相较于传统的逆解算法，有更快的收敛性，大大减小了计算量，提高实时性。

Description

一种基于BAS算法的六自由度偏置机器人逆解方法

技术领域

本发明属于机器人逆运动学领域，具体涉及一种六自由度偏置机器人的逆运动学通用求解方法。

背景技术

逆运动学算法是机器人运动学的重要组成部分，通过逆解算法求得各任务轨迹点处各关节角度值，据此可建立轨迹函数，进一步找到各关节位置、速度、加速度。为了得到封闭形式的位置逆解，机器人一般采用所谓的球形手腕，其3个转动关节轴线交于一点，恰好满足Pieper给出的具有封闭解的条件，然而，球形手腕结构也有着很大的局限性，并不总能满足要求。一方面是由于机械结构限制中间关节无法实现360°转动，从而限制了机器的灵活性；另一方面是由于三轴相交的限制使其结构强度受限，不能满足操作大负荷的要求。

为了解决这些问题，不得不使用所谓“偏置手腕”的结构形式。限于目前的数学工具，偏置型机器人的逆运动学问题一般没有实用的封闭解，通常采用数值解法。数值法主要有牛顿-拉夫森法、优化算法和迭代搜索类算法等，此外还有学者尝试使用人工神经网络方法、牛顿-拉夫森法及其改进算法。这些算法均存在算法复杂、运算量大的问题。

发明内容

本发明为了克服上述现有技术的不足，故在此提出一种基于BAS算法(天牛须算法)的六自由度偏置机器人逆解方法。本发明的精度高、收敛性快，求解过程更加实时、快速、准确。

为实现上述目的，本发明公开的方法具体包括以下步骤。

S1，构建六自由度偏置机器人的sdh模型：六自由度偏置机器人由基座、末端执行器和5个连杆以及6个旋转关节组成；基于标准的DH参数法建立各关节坐标系，坐标系0为基坐标系，坐标系i为建立在连杆i的末端即旋转关节i+1上的坐标系，坐标系i具有X_i、Y_i、Z_i轴，关节转角θ_i表示X_i轴与X_i-1轴的夹角，i＝1,2,3,4,5,6，坐标系6为末端执行器坐标系。

S2，将六自由度偏置机器人的末端坐标系的目标位姿当作手腕无偏置型机器人的末端坐标系的位姿，求得手腕无偏置型机器人的运动学逆解的解析解即旋转关节的无偏置关节转角θ₁′至θ₆′，作为迭代初始点X₀。

S3，构建适应度函数F(X)：

。

其中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中的笛卡尔坐标。

S4，基于BAS算法求取六自由度偏置机器人的逆运动学解，具体包括以下步骤。

S41，取初始点X₀，置迭代次数k=0。

S42，根据F(X)的2-范数||F(X)||₂是否到达设定阈值或是否达到最大迭代次数，判断是否跳出循环，是则跳出循环，否则进入下一次迭代。

S43，确定第k+1中拟更新的X_k+1，并判断该第k+1次迭代是否有效，具体包括以下步骤。

S431，计算左须位置xl_k和右须位置xr_k：xl_k=X_k+d_k*dir_k/2，xr_k=X_k-d_k*dir_k/2，

其中，X_k表示第k次迭代时旋转关节的各关节转角，也是天牛个体质心的位置，d_k表示天牛个体大小，初始天牛个体大小d₀为设定值，优选为0.005，dir_k表示天牛飞行方向归一化的向量，dir_k=dir_k′/norm(dir_k′)，dir_k′表示天牛飞行方向的随机向量，其为rands(6,1)，用于产生6*1阶随机向量的函数，norm表示求向量范数的函数；d_k通过调整函数进行控制，其满足d_k=d_k-1*[(n+1-k)/(n+1)]^ω ，其中n为迭代总次数，ω为常数，表示天牛个体大小递减的程度，优选为3。

S432，计算第k+1次迭代中拟更新的X_k+1的值及相应的F(X_k+1)：

X_k+1= X_k-step_k* dir_k *sign(F_left-F_right)

其中，左须函数值F_left=||F(xl_k)||，右须函数值F_right=||F(xr_k)||，即分别对F(xl_k)和F(xr_k)取范数；sign为符号函数；step_k表示步长，每步迭代中步长以负指数次幂衰减，即：

step_k=step_k-1*e^-tk，其中t为第一衰减率，优选t＝-ln0.93；初始步长step₀为设定值，优选为0.5，step₀与初始天牛个体大小d₀的比值为c，c是常数，优选为100。

S433,计算允许概率因子ρ，判断该第k+1次迭代是否有效：

，

其中，||F(X_k)||₂、||F(X_k+1)||₂表示分别对F(X_k)、F(X_k+1)取2-范数。

当||F(X_k+1)||₂≤||F(X_k)||₂，此时ρ为1，第k+1次迭代完全有效，X_k更新为X_k+1，并转入步骤S42；当||F(X_k+1)||₂>||F(X_k)||₂时，比较ρ与用于产生(0,1)之间随机数的随机函数rand()的大小，若ρ大于rand()，则第k+1次迭代有效，X_k更新为X_k+1，并转入步骤S42，否则前述第k+1次迭代无效，X_k不更新，重新进行第k+1次迭代，即转入步骤S431。

其中exp表示自然指数，即以e为底数的指数函数；M为运动系数，随着迭代的进行，天牛个体逐渐找到偏置型机器人运动学逆解，运动幅度减小，M的变化规律为M=2^-uk，其中u为第二衰减率，优选u＝-ln0.63。

该方法相较于传统的逆解算法，有更快的收敛性，大大减小了计算量，提高了实时性。

附图说明

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

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

图3为本发明涉及的迭代结果图。

具体实施方式

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

本发明的机器人包括工业机器人、多关节机械手或多自由度的机器装置。如图1所示，六自由度偏置机器人由基座、末端执行器和5个连杆以及6个旋转关节组成。该六自由度偏置机器人的逆解的具体求法包括以下步骤。

S1，构建六自由度偏置机器人的sdh模型：基于标准的DH参数法建立各关节坐标系，坐标系的具体设置如下。

首先确定基坐标系，基座标系的选择几乎是任意的，可以选择将基坐标系的原点放置在基座关节轴线Z₀轴的任意位置，图1中放置于最下方，建立坐标系0。其次确定Z_i轴，其方向与关节轴线方向保持一致。然后确立X_i方向，当Z_i轴和Z_i+1轴不共面时，Z_i轴和Z_i+1轴的公垂线定义为X_i轴，X_i轴与Z_i轴的交点即为坐标系i的原点；当Z_i轴平行于Z_i+1轴时， Z_i轴和Z_i+1轴之间存在无穷多个共同法线，将穿过坐标系i+1的原点的法线选作X_i轴，坐标系i的原点是该法线与Z_i轴的交点；当Z_i轴和Z_i+1轴相交时，选择X_i轴垂直于Z_i轴和Z_i+1轴的交点，不过Z_i轴上的任意一点都可选做坐标系i的原点。最后Y_i轴的方向由Z_i轴和X_i轴确定，以Z_i轴为右手拇指，依据右手定则确定。根据上述规则从基座往机器人末端方向，依次建立7个坐标系，坐标系0为基坐标系，坐标系i为建立在连杆i的末端即旋转关节i+1上的坐标系，坐标系i具有X_i、Y_i 、Z_i轴，关节转角θ_i表示X_i轴与X_i-1轴的夹角，i为从基座到机器人末端的旋转关节编号，依次为1、2、3、4、5、6，坐标系6为末端执行器坐标系，如图1所示，L1到L7为各个关节坐标系原点之间的距离，菱形表示转动轴平行于纸面，圆环表示转动轴垂直于直面。

S21，建立相邻坐标系的各齐次变换矩阵：

其中连杆扭角α_i表示Z_i-1轴和Z_i轴的夹角，连杆长度a_i表示坐标系i和坐标系i-1的公垂线线段距离，连杆距离d_i表示坐标系i的原点在坐标系i-1的Z_i-1轴上的投影距离坐标系i-1的原点的距离；偏移角offset表示机器人的初始时刻θ_i的角度值。本专利具体选用的sdh参数如表1所示。

表1 机器人的参数设置

计算1-3旋转关节的无偏置关节转角θ₁′至θ₃′，其满足

。

其中[X_d Y_d Z_d 1]^T表示无偏置手腕末端位置点的向量表达式，解方程组可以求出1-3旋转关节的无偏置关节转角θ₁′至θ₃′。

计算4-6旋转关节的无偏置关节转角θ₄′至θ₆′，其满足：

。

其中

为θ₄′=0时，坐标系6相对于坐标系4的旋转矩阵，

为 θ₄′=0时，坐标系4相对于基坐标系的旋转矩阵的逆矩阵，

为未引入偏置参数的坐标系6相对于坐标系0的旋转矩阵，其可通过常规计算得出。通过解方程组可以求出4-6旋转关节的无偏置关节转角θ₄′至θ₆′。最后三个关节通常有两种解，因此这种机器人的解的总数就是前三个关节解的数量的2倍。

S3，构建适应度函数F(X)：

。

其中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中的笛卡尔坐标。

其中引入偏置参数的坐标系6相对于坐标系0的齐次坐标变换矩阵可根据矩阵连乘求出：

。

其中，

。

S4，基于改进BAS算法求取六自由度偏置机器人的逆运动学解，具体包括以下步骤：

S41，取初始点X₀，置迭代次数k=0。

S43，确定第k+1次迭代中拟更新的X_k+1，并判断该第k+1次迭代是否有效，具体包括以下步骤。

S431，计算左须位置xl_k和右须位置xr_k：xl_k=X_k+d_k*dir_k/2，xr_k=X_k-d_k*dir_k/2，其中，X_k表示第k次迭代时旋转关节的各关节转角，也是天牛个体质心的位置，d_k表示天牛个体大小，初始天牛个体大小d₀为设定值，优选为0.005，dir_k表示天牛飞行方向归一化的向量，dir_k=dir_k′/norm(dir_k′)，dir_k′表示天牛飞行方向的随机向量，其为rands(6,1)，rands(m,n)为用于产生m*n阶随机向量的函数，norm表示求向量范数的函数；d_k通过调整函数进行控制，其满足d_k=d_k-1*[(n+1-k)/(n+1)]^ω ，其中n为迭代总次数，ω为常数，表示天牛个体大小递减的程度，优选为3。

S432，计算第k+1次迭代中拟更新的X_k+1的值及相应的F(X_k+1)：

X_k+1= X_k-step_k* dir_k *sign(F_left-F_right)

S433,计算允许概率因子ρ，判断该第k+1次迭代是否有效：

，

当||F(X_k+1)||₂≤||F(X_k)||₂，此时ρ为1，第k+1次迭代完全有效，X_k更新为X_k+1，并转入步骤S42；当||F(X_k+1)||₂>||F(X_k)||₂时，比较ρ与用于产生(0,1)之间随机数的随机函数rand()的大小,若ρ大于rand()，则第k+1次迭代有效，X_k更新为X_k+1，并转入步骤S42，否则前述第k+1次迭代无效，X_k不更新,重新进行第k+1次迭代，即转入步骤S431。

在本发明公开的BAS算法中，步长选取是一个至关重要的问题，对搜索效率的提升具有重要作用。本专利中，为了避免优化陷入局部极小，初始步长一般取得较大，随着优化过程越来越接近目标值。

实验采用matlab软件进行仿真，取t＝-ln0.93，c=100，维数为6，取最大迭代次数为300，因为此处使用目标位姿带入无偏置逆解函数，求出的迭代起点，和目标解很近，故使初始步长设置为0.5。图3为迭代结果图，其中横轴为迭代次数k，纵轴为适应度函数的范数||F(X_k)||₂，前50次||F(X_k)||₂能较快下降，最终在终止条件内，迭代结果趋于稳定。将迭代得到的结果带入手腕偏置型自动操作机器的正解函数，求得的位姿与目标位姿相减，旋转分量和位置分量差值小于0.01，可认为一致。

Claims

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

S1，构建手腕偏置型自动操作机器的sdh模型：手腕偏置型自动操作机器由基座、末端执行器和5个连杆以及6个旋转关节组成；基于标准的DH参数法建立各关节坐标系，坐标系0为基坐标系，坐标系i为建立在连杆i的末端即旋转关节i+1上的坐标系，坐标系i具有X_i、Y_i、Z_i轴，关节转角θ_i表示X_i轴与X_i-1轴的夹角，i＝1,2,3,4,5,6，坐标系6为末端执行器坐标系；

S2，将手腕偏置型自动操作机器的末端坐标系的目标位姿当作手腕无偏置型自动操作机器的末端坐标系的位姿，求得手腕无偏置型自动操作机器的运动学逆解的解析解即旋转关节的无偏置关节转角θ₁′至θ₆′，作为迭代初始点X₀；

S3，构建适应度函数F(X)：

；

其中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中的笛卡尔坐标；

S4，基于BAS算法求取手腕偏置型自动操作机器的逆运动学解，具体包括以下步骤：

S41，取初始点X₀，置迭代次数k=0；

S42，根据F(X)的2-范数||F(X)||₂是否到达设定阈值或是否达到最大迭代次数，判断是否跳出循环，是则跳出循环，否则进入下一次迭代；

S43，确定第k+1中拟更新的X_k+1，并判断该第k+1次迭代是否有效，具体包括以下步骤：

其中，X_k表示第k次迭代时旋转关节的各关节转角，也是天牛个体质心的位置，d_k表示天牛个体大小，dir_k表示天牛飞行方向归一化的向量，dir_k=dir_k′/norm(dir_k′)，dir_k′表示天牛飞行方向的随机向量，其为rands(6,1)，用于产生6*1阶随机向量的函数，norm表示求向量范数的函数；d_k通过调整函数进行控制，其满足d_k=d_k-1*[(n+1-k)/(n+1)]^ω ，其中n为迭代总次数，ω为常数，表示天牛个体大小递减的程度；

S432，计算第k+1次迭代中拟更新的X_k+1的值及相应的F(X_k+1)：

X_k+1= X_k-step_k* dir_k *sign(F_left-F_right)

step_k=step_k-1*e^-tk，其中t为第一衰减率；

S433，计算允许概率因子ρ，判断该第k+1次迭代是否有效：

，

其中||F(X_k)||₂、||F(X_k+1)||₂表示分别对F(X_k)、F(X_k+1)取2-范数；

当||F(X_k+1)||₂≤||F(X_k)||₂，此时ρ为1，第k+1次迭代完全有效，X_k更新为X_k+1，并转入步骤S42；当||F(X_k+1)||₂>||F(X_k)||₂时，比较ρ与用于产生(0,1)之间随机数的随机函数rand()的大小，若ρ大于rand()，则第k+1次迭代有效，X_k更新为X_k+1，并转入步骤S42，否则前述第k+1次迭代无效，X_k不更新，重新进行第k+1次迭代，即转入步骤S431；

其中exp表示自然指数，即以e为底数的指数函数；M为运动系数，其满足M=2^-uk，其中u为第二衰减率。

2.如权利要求1所述的一种基于BAS算法的六自由度偏置机器人逆解方法，其特征在于，步骤S2的具体操作步骤包括：

S21，建立相邻坐标系的各齐次变换矩阵：

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

S22，计算1-3旋转关节的无偏置关节转角θ₁′至θ₃′，其满足：

其中[X_d Y_d Z_d 1]^T表示无偏置手腕末端位置点的向量表达式，解方程组求出1-3旋转关节的无偏置关节转角θ₁′至θ₃′；

S23，计算4-6旋转关节的无偏置关节转角θ₄′至θ₆′，其满足：

；

其中

为θ₄′=0时，坐标系6相对于坐标系4的旋转矩阵，

为未引入偏置参数的坐标系 6相对于坐标系0的旋转矩阵，通过解方程组可以求出4-6旋转关节的无偏置关节转角θ₄′至 θ₆′。

3.如权利要求1或2所述的一种基于BAS算法的六自由度偏置机器人逆解方法，其特征在于，所述初始步长step₀与所述初始天牛个体大小d₀的比值为c，c是常数，step₀为0.5。