CN116038702A - 一种七轴机器人逆解方法及七轴机器人 - Google Patents

Abstract

本发明公开了一种七轴机器人逆解方法及七轴机器人，采用标准DH参数方法建立了7轴机器人的DH参数模型，根据机器人的正运动算法得到7轴机器人从基坐标系到法兰坐标系的变换矩阵的表达式，根据输入的期望的坐标变化矩阵的数据和臂型平面法线的表达式，构建了约束方程。通过得到的约束方程，采用迭代算法对上述方程求解，得到7轴机器人的逆解关节角度，实现了7轴机器人的运动学逆解。

Description

一种七轴机器人逆解方法及七轴机器人

技术领域

本发明涉及工业机器人领域，更具体地，本发明涉及一种七轴机器人逆解方法及七轴机器人。

背景技术

7轴机器人又称冗余机器人，与6轴机器人相比，附加轴使机器人能够避开一定的目标，便于末端执行器到达特定的位置，可以更加灵活地适应一些特殊的工作环境。例如在不影响末端作业的条件下实现避障，避关节极限等诸多任务。

运动学逆解是指已知机器人末端位姿,求解各运动关节的位置,它是机器人运动规划和轨迹控制的基础，与常见的6轴机器人相比，7轴机器人多出了一个自由度，对于给定期望的位置和姿态，对应的解有无穷多组。由于其求解过程相对复杂。现有7轴逆解方法相对较少，其中一种方式为，采用锁定某一个关节轴，即设定某个关节角度(如机器人J3轴)为已知的值，然后再调用6轴机器人的反解方法求解其余6个关节角度，该方法虽然得到了有限组解，但是同时也降低7轴的灵活性。

发明内容

本发明克服了现有技术7轴逆解方法的不足，提供了一种七轴机器人逆解方法及七轴机器人，以期望可以解决上述问题。

为解决上述的技术问题，本发明一方面提供了一种七轴机器人逆解方法，包括以下步骤：

S1、指令解析

对示教程序指令解析，得到机器人从起点P⁽¹⁾直线运动或者圆弧运动到终点P⁽²⁾，起点P⁽¹⁾点的关节坐标q⁽¹⁾、基坐标系到法兰坐标系的齐次变换矩阵T⁽¹⁾和机器人的臂型角ψ⁽¹⁾,机器人终点P⁽²⁾的关节坐标q⁽²⁾、齐次变换矩阵T⁽²⁾和臂型角ψ⁽²⁾，其中，q⁽¹⁾和q⁽²⁾为7维矢量，ψ⁽¹⁾和ψ⁽²⁾为标量，T⁽¹⁾和T⁽²⁾为4×4的矩阵；

S2、D-H参数建模

采用标准D-H法建立机器人的运动学模型，得到机器人每个关节D-H参数：d_i、a_i、α_i，其中i＝(1,2...7)；

其中，d_i：连杆偏置，表示将X_i-1沿着Z_i-1轴平移至X_i的距离；

a_i：连杆长度，表示将Z_i-1沿着X_i轴平移至Z_i的距离；

α_i：连杆扭转角，表示将Z_i-1沿着X_i轴旋转至Z_i的角度；

q_i：表示第i个关节的关节角度；

S3、根据期望姿态矩阵构建约束方程

设在某个插补周期时机器人期望位置和姿态矩阵为如公式(3)所示，各个值均已知，机器人的期望臂型角为ψ；

S31、设机器人基坐标系为{0}，第i个关节坐标系为{i-1}，机器人末端法兰坐标系为{7}；

S32、根据坐标变换方法可得到基坐标系{0}到法兰坐标系{7}的齐次变换矩阵：

当前一个插补周期的关节角度已知时为q⁽⁰⁾，如果还未开始运动，前一个插补周期的值取起点的关节角度q⁽⁰⁾＝q⁽¹⁾，当前插补周期的关节角度为待求解变量时，关节角度为q，q为7维矢量；

设机器人基坐标系为{0}，第i个关节坐标系为{i-1}，机器人末端法兰坐标系为{7}；

根据坐标变换方法可得到基坐标系{0}到法兰坐标系{7}的齐次变换矩阵：

其中，表示从坐标系{i-1}到的坐标系{i}的齐次变换矩阵，i＝1,2...7，的计算公式如下：

S33、构建期望姿态矩阵构建约束方程

由于机器人的DH参数d_i、a_i、α_i均已知，公式(4)中矩阵的前三行均是关于变量q₁至q₇的函数，记为f(q₁,q₂,..q₇)；

联立公式(3)和(4)有：

根据方程组方程(4)理论上可以到12个约束方程，但是由于齐次变化矩阵的前3行和前3列为旋转矩阵，并且满足以下关系：

即当左边矩阵的前3行和前2列与右边矩阵的前3行和前2列对应相等时，则边矩阵的前3行和第3列与右边矩阵的前3行和第3列一定相等。因此基于上述关系，构建关于变量q₁至q₇的9个约束方程：

S4、根据臂型平面法线构建约束方程；

S41、求臂型角的等效旋转轴线K；

S42、求参考平面法线V_ref；

S43、求臂型平面的法线V_arm；

臂型平面是将参考平面绕着等效旋转轴线矢量K旋转臂型角度ψ，根据罗德里格斯公式，将本次旋转描述为3*3的旋转矩阵形式：

R＝I+sin(ψ)M+(1-cos(ψ))MM (8)

其中

则臂型平面法线V_arm的计算公式如下：

S44、根据臂型平面法线矢量构建约束方程；

得到7轴机器人关于期望位姿矩阵和臂型平面法线的12个约束方程，记为f，表示如下：

S5、求解约束方程组。

进一步的技术方案为，所述DH参数建模具体包括以下步骤：

在第i个关节轴线处建立关节坐标系{i-1}，关节坐标系{i-1}的X轴、Y轴、Z轴的构建方法如下：

Z_i-1：第i个关节轴线，正方向满足右手螺旋定则；

X_i-1：第i-1个关节轴线和i个关节轴线的公垂线，正方向从第i-1个关节轴线指向i个关节轴线；

Y_i-1：Z_i-1叉乘X_i-1；

确定第i个关节的DH参数；

得出7轴机器人DH参数。

更进一步的技术方案为，所述求臂型角的等效旋转轴线K具体包括以下步骤：

已知机器人的期望位置和姿态矩阵为根据机器人坐标系变换算法有：

将第7轴的DH参数带入可得，

将当前插补周期的关节角度为待求解变量q(3)、第7轴的DH参数(12)带入机器人坐标系变换算法(11)得：

而则有臂型角的等效旋转轴线：

将单位化，则有：

求解上述公式得到臂型角的等效旋转轴线K。

更进一步的技术方案为，所述根据臂型平面法线矢量构建约束方程具体包括以下步骤：

臂型角平面的法线V_arm与坐标系{3}的Z₃轴方向相反，而

根据臂型角平面的法线V_arm与机器人的坐标系{3}的Z₃轴方向相反，有

Z₃＝-V_arm (17)

将臂型平面法线V_arm的计算公式(9)、公式(16)带入公式(17)得到

S35、联立期望姿态矩阵构建约束方程(7)和公式(18)可得到7轴机器人关于期望位姿矩阵和臂型平面法线的12个约束方程。

更进一步的技术方案为，所述求解约束方程组具体包括以下步骤：

采用数值迭代法对上述约束方程组求解，已知前一个插补周期的关节角度为q⁽⁰⁾，将q＝q⁽⁰⁾；

将q带入期望位姿矩阵和臂型平面法线的约束方程(10)的左边，并减去公式右边，赋值给d_Y；

计算f关于变量q₁至q₇的雅克比矩阵；

计算关节角度增量；

判断||△q||是否在设定的精度范围内

如果不是，则更新关节角度q，回到步骤S41并进行后续步骤；.

q＝q-△q       (22)

如果是，当前q的值即为期望位姿矩阵和臂型平面法线的约束方程(10)的解。

更进一步的技术方案为，在解出q的值后，将将当前关节角度q赋值给q⁽⁰⁾，为下次迭代准备。

本发明还提供了一种七轴机器人，所述机器人至少包括固定机架、第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴、第七关节轴、处理器以及存储器；

所述第一关节轴安装在所述固定机架上，所述第一关节轴至所述第七关节轴依次连接，第一关节轴、第二关节轴、第三关节轴的轴线相较于点P_s，第四关节轴、第五关节轴的轴线相较于点P_e，第五关节轴、第六关节轴、第七关节轴的轴线相交于点P_w；

所述存储器中存储有计算机程序，所述处理器用于执行所述计算机程序以实现上述方法中的步骤。

与现有技术相比，本发明至少具有以下有益效果：本发明采用标准DH参数方法建立了7轴机器人的DH参数模型，根据机器人的正运动算法得到7轴机器人从基坐标系到法兰坐标系的变换矩阵的表达式，根据输入的期望的坐标变化矩阵的数据，建立了9个约束方程，9个方程中的未知变量均是J₁轴至J₇轴的关节角度，同时，引入机器人臂型角，利用臂型平面法线的表达式，构建3个约束方程，3个约束方程中的未知变量为J₁轴至J₃轴的关节角度。

因此，通过机器人的末端位置、姿态、臂型平面法线可得到12个约束方程，将前一插补周期的7个关节角度q⁽⁰⁾作为初始值，采用迭代算法对上述方程求解，得到7轴机器人的逆解关节角度，并将该组解赋值给q⁽⁰⁾，作为下一插补周期迭代算法求解的初值，通过以上方法，实现了7轴机器人的运动学逆解。

附图说明

图1为本发明实施例一的流程示意图。

图2为本发明实施例二的结构示意图及个关节旋转正方向。

图3为本发明实施例二的7轴机器人的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例一

一种七轴机器人逆解方法，参见图1，包括以下步骤：

S1、指令解析

以笛卡尔空间直线运动或者圆弧运动为例，机器人从起点P⁽¹⁾直线运动或者圆弧运动到终点P⁽²⁾，通过对示教程序指令解析，可得到机器人起点P⁽¹⁾点的关节坐标q⁽¹⁾、基坐标系到法兰坐标系的齐次变换矩阵T⁽¹⁾和机器人的臂型角ψ⁽¹⁾,机器人终点P⁽²⁾的关节坐标q⁽²⁾、齐次变换矩阵T⁽²⁾和臂型角ψ⁽²⁾，其中，q⁽¹⁾和q⁽²⁾为7维矢量，ψ⁽¹⁾和ψ⁽²⁾为标量，T⁽¹⁾和T⁽²⁾为4×4的矩阵；

需要说明的是，参见图2，七轴机器人第一关节轴、第二关节轴、第三关节轴的轴线相交于点P_s，第四关节轴、第五关节轴的轴线相交于点P_e，第五关节轴、第六关节轴、第七关节轴的轴线相交于点P_w；

参考平面定义为和机器人1轴轴线确定的平面，臂型平面定义为和确定的平面；

臂型角ψ定义为将参考平面绕着旋转至臂型平面所转过的角度；

S2、DH参数建模

如图2所示，采用标准D-H法建立机器人的运动学模型，得到机器人每个关节D-H参数：d_i、a_i、α_i，其中i＝(1,2...7)，方法如下：

S21、在第i个关节轴线处建立关节坐标系{i-1}，关节坐标系{i-1}的X轴、轴Y、Z轴的构建方法如下：

Z_i-1：第i个关节轴线，正方向满足右手螺旋定则；

X_i-1：第i-1个关节轴线和i个关节轴线的公垂线，正方向从第i-1个关节轴线指向i个关节轴线；

Y_i-1：Z_i-1叉乘X_i-1；

S22、确定第i个关节的DH参数

第i个关节的DH参数含义如下：

d_i：连杆偏置，表示将X_i-1沿着Z_i-1轴平移至X_i的距离

a_i：连杆长度，表示将Z_i-1沿着X_i轴平移至Z_i的距离

α_i:连杆扭转角，表示将Z_i-1沿着X_i轴旋转至Z_i的角度

q_i：表示第i个关节的关节角度

S23、确定7轴机器人DH参数

S3、根据期望姿态矩阵构建约束方程

设在某个插补周期时机器人期望位置和姿态矩阵为如公式(3)，各个值均已知，机器人的期望臂型角为ψ,前一个插补周期的关节角度已知为q⁽⁰⁾,如果还未开始运动，前一个插补周期的值取起点的关节角度q⁽⁰⁾＝q⁽¹⁾，当前插补周期的关节角度为待求解变量为q，q为7维矢量。

在本实施例中，起始点关节角度为：q⁽⁰⁾＝[0 45 0 -45 0 90 0](deg)

期望的位姿矩阵为：

期望的臂型角ψ为10(deg)；

S31、设机器人基坐标系为{0}，第i个关节坐标系为{i-1}，机器人末端法兰坐标系为{7}。

S32、根据坐标变换方法可得到基坐标系{0}到法兰坐标系{7}的齐次变换矩阵：

其中，表示从坐标系{i-1}到的坐标系{i}的齐次变换矩阵，i＝1,2...7，的计算公式如下，

由于机器人的DH参数d_i、a_i、α_i均已知，公式(4)中矩阵的前三行均是关于变量q₁至q₇的函数，记为f(q₁,q₂,..q₇)。

联立公式(3)和(4)有：

根据方程组方程(6)理论上可以到12个约束方程，但是由于齐次变化矩阵的前3行和前3列为旋转矩阵，并且满足以下关系：

即当左边矩阵的前3行和前2列与右边矩阵的前3行和前2列对应相等时，则边矩阵的前3行和第3列与右边矩阵的前3行和第3列一定相等。因此基于上述关系，可构建关于变量q₁至q₇的9个约束方程。

S4、根据臂型平面法线构建约束方程

S41、求臂型角的等效旋转轴线K

已知机器人的期望位置和姿态矩阵为根据机器人坐标系变换算法有：

而根据公式(5)将第7轴的DH参数带入可得，

将公式(3)、(10)带入(9)得

而则有臂型角的等效旋转轴线：

将单位化，则有

S42、求参考平面法线V_ref

根据参考平面的定义知：

S43、臂型平面的法线V_arm

臂型平面是将参考平面绕着等效旋转轴线矢量K旋转臂型角度ψ，根据罗德里格斯公式，将本次旋转描述为3*3的旋转矩阵形式:

R＝I+sin(ψ)M+(1-cos(ψ))MM (15)

其中

则臂型平面法线V_arm的计算公式如下：

因此结合公式(13)(14)(15)可得到臂型平面的法线矢量。

S44、根据臂型平面法线矢量构建约束方程

进一步分析易知，臂型角平面的法线V_arm与坐标系{3}的Z₃轴方向相反，而

根据臂型角平面的法线V_arm与机器人的坐标系{3}的Z₃轴方向相反，有

Z₃＝-V_arm (18)

将公式(16)(17)带入(18)有

S45、联立公式(8)和公式(19)可得到7轴机器人关于期望位姿矩阵和臂型平面法线的12个约束方程，记为f如下：

S5、求解约束方程

采用数值迭代法对上述约束方程组求解，方法如下：

已知前一个插补周期的关节角度为q⁽⁰⁾，将q＝q⁽⁰⁾。

S51、将q带入公式(20)的左边，并减去公式右边，赋值给d_Y。

S52、计算f关于变量q₁至q₇的雅克比矩阵；

S53、计算关节角度增量；

S54、判断||△q||是否在设定的精度范围内；

如果不是，则更新关节角度q，重复S51至S54，

q＝q-△q       (25)

如果是，当前q的值即为方程组(20)的解。

在本实施例中，经过7次迭代后，此时||dY||的值已经小于1e-6，完成了q的求解，7次迭代的情况如下：

迭代次数	误差(||dY||的值)
		1	489.4981091801905
2	531.0351233057955
		3	106.5345462089905
4	58.2082607472319
		5	2.4668108544417
6	0.0119173130670
		7	0.0000002789942

S6、将当前关节角度q赋值给q⁽⁰⁾，为下次迭代准备。

q⁽⁰⁾＝q    (26)

实施例二

一种七轴机器人，参见图2、图3，所述机器人至少包括固定机架、第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴、第七关节轴、处理器以及存储器；

所述第一关节轴安装在所述固定机架上，所述第一关节轴至所述第七关节轴依次连接，第一关节轴、第二关节轴、第三关节轴的轴线相较于点P_s，第四关节轴、第五关节轴的轴线相较于点P_e，第五关节轴、第六关节轴、第七关节轴的轴线相交于点P_w；

所述存储器中存储有计算机程序，所述处理器用于执行所述计算机程序以实现上述方法中的步骤。

尽管这里参照本发明的解释性实施例对本发明进行了描述，但是，应该理解，本领域技术人员可以设计出很多其他的修改和实施方式，这些修改和实施方式将落在本申请公开的原则范围和精神之内。更具体地说，在本申请公开的范围内，可以对主题组合布局的组成部件和/或布局进行多种变型和改进。除了对组成部件和/或布局进行的变型和改进外，对于本领域技术人员来说，其他的用途也将是明显的。

Claims (7)

1.一种七轴机器人逆解方法，其特征在于，包括以下步骤：

S1、指令解析

对示教程序指令解析，得到机器人从起点P⁽¹⁾直线运动或者圆弧运动到终点P⁽²⁾，起点P⁽¹⁾点的关节坐标q⁽¹⁾、基坐标系到法兰坐标系的齐次变换矩阵T⁽¹⁾和机器人的臂型角ψ⁽¹⁾,机器人终点P⁽²⁾的关节坐标q⁽²⁾、齐次变换矩阵T⁽²⁾和臂型角ψ⁽²⁾，其中，q⁽¹⁾和q⁽²⁾为7维矢量，ψ⁽¹⁾和ψ⁽²⁾为标量，T⁽¹⁾和T⁽²⁾为4×4的矩阵；

S2、D-H参数建模

采用标准D-H法建立机器人的运动学模型，得到机器人每个关节D-H参数：d_i、a_i、α_i，其中i＝(1,2...7)；

其中，d_i：连杆偏置，表示将X_i-1沿着Z_i-1轴平移至X_i的距离；

a_i：连杆长度，表示将Z_i-1沿着X_i轴平移至Z_i的距离；

α_i：连杆扭转角，表示将Z_i-1沿着X_i轴旋转至Z_i的角度；

q_i：表示第i个关节的关节角度；

S3、根据期望姿态矩阵构建约束方程

设在某个插补周期时机器人期望位置和姿态矩阵为

如公式(3)所示，各个值均已知，机器人的期望臂型角为ψ；

S31、设机器人基坐标系为{0}，第i个关节坐标系为{i-1}，机器人末端法兰坐标系为{7}；

S32、根据坐标变换方法可得到基坐标系{0}到法兰坐标系{7}的齐次变换矩阵：

当前一个插补周期的关节角度已知时为q⁽⁰⁾，如果还未开始运动，前一个插补周期的值取起点的关节角度q⁽⁰⁾＝q⁽¹⁾，当前插补周期的关节角度为待求解变量时，关节角度为q，q为7维矢量；

设机器人基坐标系为{0}，第i个关节坐标系为{i-1}，机器人末端法兰坐标系为{7}；

根据坐标变换方法可得到基坐标系{0}到法兰坐标系{7}的齐次变换矩阵：

其中，

表示从坐标系{i-1}到的坐标系{i}的齐次变换矩阵，i＝1,2...7，

的计算公式如下：

S33、构建期望姿态矩阵构建约束方程

由于机器人的DH参数d_i、a_i、α_i均已知，公式(4)中矩阵的前三行均是关于变量q₁至q₇的函数，记为f(q₁,q₂,..q₇)；

联立公式(3)和(4)有：

构建关于变量q₁至q₇的9个约束方程：

S4、根据臂型平面法线构建约束方程；

S41、求臂型角的等效旋转轴线K；

S42、求参考平面法线V_ref；

S43、求臂型平面的法线V_arm；

臂型平面是将参考平面绕着等效旋转轴线矢量K旋转臂型角度ψ，根据罗德里格斯公式，将本次旋转描述为3*3的旋转矩阵形式：

R＝I+sin(ψ)M+(1-cos(ψ))MM (8)

其

则臂型平面法线V_arm的计算公式如下：

S44、根据臂型平面法线矢量构建约束方程；

得到7轴机器人关于期望位姿矩阵和臂型平面法线的12个约束方程，记为f，表示如下：

S5、求解约束方程组。

2.如权利要求1所述的一种七轴机器人逆解方法，其特征在于，所述DH参数建模具体包括以下步骤：

在第i个关节轴线处建立关节坐标系{i-1}，关节坐标系{i-1}的X轴、Y轴、Z轴的构建方法如下：

Z_i-1：第i个关节轴线，正方向满足右手螺旋定则；

X_i-1：第i-1个关节轴线和i个关节轴线的公垂线，正方向从第i-1个关节轴线指向i个关节轴线；

Y_i-1：Z_i-1叉乘X_i-1；

确定第i个关节的DH参数；

得出7轴机器人DH参数。

3.如权利要求1所述的一种七轴机器人逆解方法，其特征在于，所述求臂型角的等效旋转轴线K具体包括以下步骤：

已知机器人的期望位置和姿态矩阵为

根据机器人坐标系变换算法有：

将第7轴的DH参数带入可得，

将当前插补周期的关节角度为待求解变量q(3)、第7轴的DH参数(12)带入机器人坐标系变换算法(11)得：

而

则有臂型角的等效旋转轴线：

将

单位化，则有：

求解上述公式得到臂型角的等效旋转轴线K。

4.如权利要求1所述的一种七轴机器人逆解方法，其特征在于，所述根据臂型平面法线矢量构建约束方程具体包括以下步骤：

臂型角平面的法线V_arm与坐标系{3}的Z₃轴方向相反，而

根据臂型角平面的法线V_arm与机器人的坐标系{3}的Z₃轴方向相反，有

Z₃＝-V_arm (17)

将臂型平面法线V_arm的计算公式(9)、公式(16)带入(17)得到

S35、联立期望姿态矩阵构建约束方程(7)和公式(18)可得到7轴机器人关于期望位姿矩阵和臂型平面法线的12个约束方程。

5.如权利要求1所述的一种七轴机器人逆解方法，其特征在于，所述求解约束方程组具体包括以下步骤：

采用数值迭代法对上述约束方程组求解，已知前一个插补周期的关节角度为q⁽⁰⁾，将q＝q⁽⁰⁾；

将q带入期望位姿矩阵和臂型平面法线的约束方程(10)的左边，并减去公式右边，赋值给d_Y；

计算f关于变量q₁至q₇的雅克比矩阵；

计算关节角度增量；

判断△q是否在设定的精度范围内

如果不是，则更新关节角度q，回到步骤S41并进行后续步骤；.

q＝q-△q       (22)

如果是，当前q的值即为期望位姿矩阵和臂型平面法线的约束方程(10)的解。

6.如权利要求1所述的一种七轴机器人逆解方法，其特征在于，在解出q的值后，将将当前关节角度q赋值给q⁽⁰⁾，为下次迭代准备。

7.一种七轴机器人，其特征在于，所述机器人至少包括固定机架、第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴、第七关节轴、处理器以及存储器；

所述第一关节轴安装在所述固定机架上，所述第一关节轴至所述第七关节轴依次连接，第一关节轴、第二关节轴、第三关节轴的轴线相较于点P_s，第四关节轴、第五关节轴的轴线相较于点P_e，第五关节轴、第六关节轴、第七关节轴的轴线相交于点P_w；

所述存储器中存储有计算机程序，所述处理器用于执行所述计算机程序以实现权利要求1-6任一项所述方法中的步骤。

Images