CN112276940A - 一种六自由度非球型手腕机器人逆运动学求解方法 - Google Patents

Abstract

本发明的一种六自由度非球型手腕机器人逆运动学求解方法，计算精度高、运算速度快效率高，同时，逆解物理意义明确直观，多解问题解决方便，有利于算法的工程应用。该逆运动学求解方法基于一种六自由度非球型手腕机器人与6R正交球型手腕机器人间的等效变换，为基于雅可比矩阵的数值迭代法提供了可靠的初始值，计算精度高、运算速度快效率高。所求得的机器人运动学逆解物理意义明确直观，可以利用等效6R正交球型手腕机器人处理多解问题方法解决所述六自由度非球型手腕机器人逆运动学多解问题，有利于算法的工程应用。

Description

一种六自由度非球型手腕机器人逆运动学求解方法

技术领域

本发明涉及一种六自由度非球型手腕机器人。特别是涉及一种六自由度非球型手腕机器人逆运动学求解方法。

背景技术

运动学是机器人运动控制和轨迹规划的基础，涉及正向运动学和逆向运动学两个子问题，对于串联机器人，逆运动学相对复杂，是理论研究的焦点。目前，六自由度串联机器人逆运动学求解方法主要有：解析法和数值法。其中，解析法要求机器人的构型满足Pieper准则，即具有3个相邻关节轴交于一点或者3个相邻关节轴相互平行，例如6R正交球型手腕机器人手腕部分的3个相邻关节轴交于一点，机器人末端位置和姿态可以分别看作由前三关节和后三关节决定，各国学者针对这种机器人提出了大量有效的逆运动学求解方法。6R正交球型手腕机器人也被广泛应用于工业机器人，但是，对于某些特殊的工业应用，例如喷涂，具有非球型手腕的机器人得到了应用。例如，一种具有柔性手腕(专利CN 107379004A)的7回转副6自由度非球型手腕机器人凭借更大的姿态空间和更高的运动柔性，在喷涂机器人上得到了广泛应用。

上述7回转副6自由度非球型手腕机器人由7个依次连接的回转关节组成，为了避免冗余的出现，在机器人的第5和第6回转关节之间设置了运动约束，即θ₆＝-θ₅，这种机器人不满足Pieper准则。对于不满足Pieper准则串联机器人的逆运动学，主要有两种常用的数值迭代算法。第一种基于速度雅可比矩阵，包括牛顿——拉夫森法和阻尼最小二乘法等，第二种算法包括神经网络算法、遗传算法和模拟退火法等智能算法将逆运动学问题转化为极值问题。但是，上述数值迭代算法的缺点是运算效率有待工程验证，算法对初始值和奇异位姿敏感。另外，对于机器人逆运动学的多解问题也需要进一步研究。

发明内容

本发明所要解决的技术问题是，提供一种计算精度高、运算速度快效率高的六自由度非球型手腕机器人逆运动学求解方法。

本发明所采用的技术方案是：一种六自由度非球型手腕机器人逆运动学求解方法，六自由度非球型手腕机器人是由7个依次连接的回转关节组成，为了避免冗余的出现，在六自由度非球型手腕机器的第5回转关节和第6回转关节转角之间设置了运动约束：θ₆＝-θ₅；逆运动学求解方法包括如下步骤：

1)根据D-H参数法建立所述机器人的连杆坐标系{X_i-Y_i-Z_i}；连杆坐标系{X_i-1-Y_i-1-Z_i-1}到连杆坐标系{X_i-Y_i-Z_i}的变换为：

(1)绕Z_i-1轴旋转θ_i，使X_i-1轴与X_i轴平行；

(2)沿Z_i-1轴平移d_i，使X_i-1轴与X_i轴重合；

(3)绕X_i轴旋转α_i，使Z_i-1轴与Z_i轴平行；

(4)沿X_i轴平移a_i，使Z_i-1轴与Z_i轴重合；

其中，θ_i、d_i、α_i、a_i分别称为第i关节的关节转角、关节偏距、连杆扭角和连杆长度，且有θ₆＝-θ₅；

给定六自由度非球型手腕机器的D-H参数，并定义连杆齐次变换矩阵为：

其中，变量s_i＝sinθ_i，c_i＝cosθ_i，c_αi＝cosα_i，s_αi＝sinα_i；基于所述连杆齐次变换矩阵，定义六自由度非球型手腕机器正向运动学为：

其中，

表示六自由度非球型手腕机器人末端在基础坐标系下的位姿矩阵；p＝(p_x,p_y,p_z)^T表示连杆坐标系{X_i-Y_i-Z_i}相对连杆坐标系{X_i-1-Y_i-1-Z_i-1}的位置向量，n＝(n_x,n_y,n_z)^T、o＝(o_x,o_y,o_z)^T、a＝(a_x,a_y,a_z)^T表示连杆坐标系{X_i-Y_i-Z_i}相对连杆坐标系{X_i-1-Y_i-1-Z_i-1}的姿态向量；

2)建立六自由度非球型手腕机器人与6R正交球型手腕机器人之间的等效变换，所述等效变换包括正向变换和逆向变换；

3)根据给定的六自由度非球型手腕机器末端位姿矩阵T_end，求解等效6R正交球型手腕机器人逆运动学解析解；

4)根据步骤3)求得的等效6R正交球型手腕机器人逆运动学解析解和步骤2)建立的六自由度非球型手腕机器人与等效6R正交球型手腕机器人之间的逆向变换，计算所述六自由度非球型手腕机器人逆运动学近似解；

5)以步骤4)求得的六自由度非球型手腕机器人逆运动学近似解为初始值，利用基于运动雅可比矩阵的数值迭代法，计算六自由度非球型手腕机器人逆运动学精确解，所述基于运动雅可比矩阵的数值迭代法的迭代公式为：

δθ＝(J^TJ)^-1J^Te_T，θ_i＝θ_i-1+δθ

其中，

为机器人末端当前位姿

与目标位姿

之间的误差向量,J为运动雅可比矩阵，δθ＝(δθ₁,δθ₂,δθ₃,δθ₄,δθ₅,δθ₇)^T为独立关节转角增向量。当关节转角增量的绝对值小于给定阈值||δθ||≤ε或达到最大迭代次数，迭代结束，并输出最终运动学逆解，ε表示关节转角增量绝对值的阈值。

本发明的一种六自由度非球型手腕机器人逆运动学求解方法，计算精度高、运算速度快效率高，同时，逆解物理意义明确直观，多解问题解决方便，有利于算法的工程应用。具有的优点和积极效果是：

1、该逆运动学求解方法基于一种六自由度非球型手腕机器人与6R正交球型手腕机器人间的等效变换，为基于雅可比矩阵的数值迭代法提供了可靠的初始值，计算精度高、运算速度快效率高。

2、所求得的机器人运动学逆解物理意义明确直观，可以利用等效6R正交球型手腕机器人处理多解问题方法解决所述六自由度非球型手腕机器人逆运动学多解问题，有利于算法的工程应用。

附图说明

图1是本发明一种六自由度非球型手腕机器人逆运动学求解方法的流程示意图；

图2是本发明中六自由度非球型手腕机器人的连杆坐标系；

图3是本发明等效6R正交球型手腕机器人的连杆坐标系。

具体实施方式

下面结合实施例和附图对本发明的一种六自由度非球型手腕机器人逆运动学求解方法做出详细说明。

本发明的一种六自由度非球型手腕机器人逆运动学求解方法，六自由度非球型手腕机器人采用申请号为202010082984.8发明专利所公开的技术方案，是由7个依次连接的回转关节组成，为了避免冗余的出现，在六自由度非球型手腕机器的第5回转关节和第6回转关节转角之间设置了运动约束：θ₆＝-θ₅；如图1所示，逆运动学求解方法包括如下步骤：

1)根据D-H参数法建立所述机器人的连杆坐标系{X_i-Y_i-Z_i}；所述一种六自由度非球型手腕机器人的连杆坐标系如图2所示。连杆坐标系{X_i-1-Y_i-1-Z_i-1}到连杆坐标系{X_i-Y_i-Z_i}的变换为：

(1)绕Z_i-1轴旋转θ_i，使X_i-1轴与X_i轴平行；

(2)沿Z_i-1轴平移d_i，使X_i-1轴与X_i轴重合；

(3)绕X_i轴旋转α_i，使Z_i-1轴与Z_i轴平行；

(4)沿X_i轴平移a_i，使Z_i-1轴与Z_i轴重合；

其中，θ_i、d_i、α_i、a_i分别称为第i关节的关节转角、关节偏距、连杆扭角和连杆长度，且有θ₆＝-θ₅；

给定六自由度非球型手腕机器的D-H参数，并定义连杆齐次变换矩阵为：

其中，变量s_i＝sinθ_i，c_i＝cosθ_i，c_αi＝cosα_i，s_αi＝sinα_i；基于所述连杆齐次变换矩阵，定义六自由度非球型手腕机器正向运动学为：

其中，

表示六自由度非球型手腕机器人末端在基础坐标系下的位姿矩阵；p＝(p_x,p_y,p_z)^T表示连杆坐标系{X_i-Y_i-Z_i}相对连杆坐标系{X_i-1-Y_i-1-Z_i-1}的位置向量，n＝(n_x,n_y,n_z)^T、o＝(o_x,o_y,o_z)^T、a＝(a_x,a_y,a_z)^T表示连杆坐标系{X_i-Y_i-Z_i}相对连杆坐标系{X_i-1-Y_i-1-Z_i-1}的姿态向量；

2)建立六自由度非球型手腕机器人与6R正交球型手腕机器人之间的等效变换，所述等效变换包括正向变换和逆向变换；其中，

(1)所述的正向变换包括：

忽略六自由度非球型手腕机器人手腕中心点位置的变化，得到等效6R正交球型手腕机器人的D-H参数为：

其中，β＝|α₄|＝|α₆|表示机器人手腕轴交角，

分别称为等效6R正交球型手腕机器人第i关节的关节转角、关节偏距和连杆长度，所述等效6R正交球型手腕机器人的连杆坐标系如图3所示。根据机器人末端位姿矩阵对应元素相等，得到由所述六自由度非球型手腕机器人关节转角计算等效6R正交球型手腕机器人关节转角的表达式如下：

其中，

C＝c_α6s_α5s₅-s_α6(c₅s₅-c_α5c₅s₅)；

当两种机器人关节转角正负符号相同时，正向变换为一对一变换。

(2)所述的逆向变换包括：

根据六自由度非球型手腕机器人末端位姿矩阵对应元素相等，求得由等效6R正交球型手腕机器人关节转角计算六自由度非球型手腕机器人关节转角的表达式如下：

其中，ξ⁺表示式(W-U+V)ξ²+2(V-W)ξ+W+U+V＝0的唯一非负数解，W＝s_α4s_α6-s_α6c_{α 5}s_α4，U＝-s_α5(c_α6s_α4+c_α4s_α6)，V＝c_α4c_α5c_α6-s_α4s_α6-c_5eq，

当两种机器人关节转角正负符号相同时，逆向变换为一对一变换。

3)根据给定的六自由度非球型手腕机器末端位姿矩阵T_end，求解等效6R正交球型手腕机器人逆运动学解析解；所述的求解等效6R正交球型手腕机器人逆运动学解析解的具体表达式如下：

其中，

根据六自由度非球型手腕机器人末端位姿矩阵T_end以及求解等效6R正交球型手腕机器人逆运动学解析解的表达式，得到等效6R正交球型手腕机器人逆运动学的8组解，分别对应

和

具有两组解，

和

具有一组解；

等效6R正交球型手腕机器人属于工业机器人最典型的构型方式，根据机器人各关节转角的取值范围分别从

和

的两组解中选择一组合理的解，进而解决机器人逆运动学多解问题。

4)根据步骤3)求得的等效6R正交球型手腕机器人逆运动学解析解和步骤2)建立的六自由度非球型手腕机器人与等效6R正交球型手腕机器人之间的逆向变换，计算所述六自由度非球型手腕机器人逆运动学近似解；

5)以步骤4)求得的六自由度非球型手腕机器人逆运动学近似解为初始值，利用基于运动雅可比矩阵的数值迭代法，计算六自由度非球型手腕机器人逆运动学精确解，所述基于运动雅可比矩阵的数值迭代法的迭代公式为：

δθ＝(J^TJ)^-1J^Te_T，θ_i＝θ_i-1+δθ

其中，

为机器人末端当前位姿

与目标位姿

之间的误差向量,J为运动雅可比矩阵，δθ＝(δθ₁,δθ₂,δθ₃,δθ₄,δθ₅,δθ₇)^T为独立关节转角增向量。当关节转角增量的绝对值小于给定阈值||δθ||≤ε或达到最大迭代次数，迭代结束，并输出最终运动学逆解，ε表示关节转角增量绝对值的阈值。

下面给出具体实例：

首先，根据所述逆运动学求解方法，建立一种六自由度非球型手腕机器人的连杆坐标系，如图1所示；给定机器人相应D-H参数，如表1所示，其中θ₆＝-θ₅；给定目标关节转角为θ＝(θ₁,θ₂,θ₃,θ₄,θ₅,θ₇)^T＝(60°,30°,-60°,-30°,60°,30°)^T，并根据前面步骤1)所述的机器人正向运动学公式计算末端目标位姿矩阵为：

表1所述一种六自由度非球型手腕机器人的D-H参数

其次，建立一种六自由度非球型手腕机器人与6R正交球型手腕机器人之间等效变换的正向变换，计算等效6R正交球型手腕机器人的D-H参数，如表2所示。

表2等效6R正交球型手腕机器人的D-H参数

再次，求解等效6R正交球型手腕机器人逆运动学解析解，并建立一种六自由度非球型手腕机器人与6R正交球型手腕机器人之间等效变换的逆向变换，计算所述一种六自由度非球型手腕机器人逆运动学近似解，如表3所示。

最后，求解所述一种六自由度非球型手腕机器人逆运动学精确解，如表4所示。由表4可知，求得第一组解相较给定目标关节转角的误差小于0.0001°，证明所提出一种六自由度非球型手腕机器人逆运动学求解方法具有较高计算精度。通过对比表3和表4可知，基于一种六自由度非球型手腕机器人与6R正交球型手腕机器人间的等效变换，求得机器人逆运动学近似解较精确解误差小于5°，近似解为后续基于雅可比矩阵的数值迭代法提供了可靠的初始值，有利于提高其计算精度和计算效率。另外，算法求得一种六自由度非球型手腕机器人运动学逆解与等效6R正交球型手腕机器人运动学逆解一一对应，物理意义明确直观，可以利用等效6R正交球型手腕机器人处理多解问题方法解决所述一种六自由度非球型手腕机器人逆运动学多解问题，有利于算法的工程应用。

表3所述一种六自由度非球型手腕机器人逆运动学近似解

表4所述一种六自由度非球型手腕机器人逆运动学精确解

Claims (4)

1.一种六自由度非球型手腕机器人逆运动学求解方法，六自由度非球型手腕机器人是由7个依次连接的回转关节组成，为了避免冗余的出现，在六自由度非球型手腕机器的第5回转关节和第6回转关节转角之间设置了运动约束：θ₆＝-θ₅；其特征在于，逆运动学求解方法包括如下步骤：

1)根据D-H参数法建立所述机器人的连杆坐标系{X_i-Y_i-Z_i}；连杆坐标系{X_i-1-Y_i-1-Z_i-1}到连杆坐标系{X_i-Y_i-Z_i}的变换为：

(1)绕Z_i-1轴旋转θ_i，使X_i-1轴与X_i轴平行；

(2)沿Z_i-1轴平移d_i，使X_i-1轴与X_i轴重合；

(3)绕X_i轴旋转α_i，使Z_i-1轴与Z_i轴平行；

(4)沿X_i轴平移a_i，使Z_i-1轴与Z_i轴重合；

其中，θ_i、d_i、α_i、a_i分别称为第i关节的关节转角、关节偏距、连杆扭角和连杆长度，且有θ₆＝-θ₅；

给定六自由度非球型手腕机器的D-H参数，并定义连杆齐次变换矩阵为：

其中，变量s_i＝sinθ_i，c_i＝cosθ_i，c_αi＝cosα_i，s_αi＝sinα_i；基于所述连杆齐次变换矩阵，定义六自由度非球型手腕机器正向运动学为：

其中，

表示六自由度非球型手腕机器人末端在基础坐标系下的位姿矩阵；p＝(p_x,p_y,p_z)^T表示连杆坐标系{X_i-Y_i-Z_i}相对连杆坐标系{X_i-1-Y_i-1-Z_i-1}的位置向量，n＝(n_x,n_y,n_z)^T、o＝(o_x,o_y,o_z)^T、a＝(a_x,a_y,a_z)^T表示连杆坐标系{X_i-Y_i-Z_i}相对连杆坐标系{X_i-1-Y_i-1-Z_i-1}的姿态向量；

2)建立六自由度非球型手腕机器人与6R正交球型手腕机器人之间的等效变换，所述等效变换包括正向变换和逆向变换；

3)根据给定的六自由度非球型手腕机器末端位姿矩阵T_end，求解等效6R正交球型手腕机器人逆运动学解析解；

4)根据步骤3)求得的等效6R正交球型手腕机器人逆运动学解析解和步骤2)建立的六自由度非球型手腕机器人与等效6R正交球型手腕机器人之间的逆向变换，计算所述六自由度非球型手腕机器人逆运动学近似解；

5)以步骤4)求得的六自由度非球型手腕机器人逆运动学近似解为初始值，利用基于运动雅可比矩阵的数值迭代法，计算六自由度非球型手腕机器人逆运动学精确解，所述基于运动雅可比矩阵的数值迭代法的迭代公式为：

δθ＝(J^TJ)^-1J^Te_T，θ_i＝θ_i-1+δθ

其中，

为机器人末端当前位姿

与目标位姿

之间的误差向量,J为运动雅可比矩阵，δθ＝(δθ₁,δθ₂,δθ₃,δθ₄,δθ₅,δθ₇)^T为独立关节转角增向量。当关节转角增量的绝对值小于给定阈值||δθ||≤ε或达到最大迭代次数，迭代结束，并输出最终运动学逆解，ε表示关节转角增量绝对值的阈值。

2.根据权利要求1所述的一种六自由度非球型手腕机器人逆运动学求解方法，其特征在于，步骤2)所述的正向变换包括：

忽略六自由度非球型手腕机器人手腕中心点位置的变化，得到等效6R正交球型手腕机器人的D-H参数为：

其中，β＝|α₄|＝|α₆|表示机器人手腕轴交角，

分别称为等效6R正交球型手腕机器人第i关节的关节转角、关节偏距和连杆长度，根据机器人末端位姿矩阵对应元素相等，得到由所述六自由度非球型手腕机器人关节转角计算等效6R正交球型手腕机器人关节转角的表达式如下：

其中，

C＝c_α6s_α5s₅-s_α6(c₅s₅-c_α5c₅s₅)；

当两种机器人关节转角正负符号相同时，正向变换为一对一变换。

3.根据权利要求1所述的一种六自由度非球型手腕机器人逆运动学求解方法，其特征在于，步骤2)所述的逆向变换包括：

根据六自由度非球型手腕机器人末端位姿矩阵对应元素相等，求得由等效6R正交球型手腕机器人关节转角计算六自由度非球型手腕机器人关节转角的表达式如下：

其中，ξ⁺表示式(W-U+V)ξ²+2(V-W)ξ+W+U+V＝0的唯一非负数解，W＝s_α4s_α6-s_α6c_α5s_α4，U＝-s_α5(c_α6s_α4+c_α4s_α6)，V＝c_α4c_α5c_α6-s_α4s_α6-c_5eq，

当两种机器人关节转角正负符号相同时，逆向变换为一对一变换。

4.根据权利要求1所述的一种六自由度非球型手腕机器人逆运动学求解方法，其特征在于，步骤3)求解等效6R正交球型手腕机器人逆运动学解析解的具体表达式如下：

-sign(s₅)(a_xc₁c₂₃-a_zs₂₃+a_ys₁c₂₃))

sign(s₅)(o_zc₂₃+o_xc₁+o_ys₁s₂₃))

其中，

根据六自由度非球型手腕机器人末端位姿矩阵T_end以及求解等效6R正交球型手腕机器人逆运动学解析解的表达式，得到等效6R正交球型手腕机器人逆运动学的8组解，分别对应

和

具有两组解，

和

具有一组解。

Images