CN110181522B - 一种五自由度首末对称机械臂逆运动学计算的优化方法 - Google Patents

Abstract

本发明提供了一种五自由度首末对称机械臂逆运动学计算的优化方法，增加一个转动角度始终为零的虚拟旋转关节，以此计算机械臂DH参数；计算球型手腕中心的位置坐标，通过求齐次变换矩阵并进行转换，从而求得所求目标转角。本发明对五自由度首末对称机械臂的逆运动学求解过程未经过迭代、分类、试凑等技术手段，而是直接根据机械臂自身参数得到了关节角与关节形位的闭式表达式，因此可以可快速求解五自由度首末对称机械臂的逆运动学问题，提高了计算精度和计算速度，有助于实现机械臂的实时响应。

Description

一种五自由度首末对称机械臂逆运动学计算的优化方法

技术领域

本发明涉及机械臂逆运动学领域，特别涉及五自由度首末对称机械臂的计算方法。

背景技术

五自由度首末对称的机械臂，可在水下和空间等弱重力环境中执行搬运、对接和爬行等任务。由于机械臂的关节数目小于6，所以其运动学不能解耦，因此其逆运动学求解相当困难。当前普遍采用的数值法由于迭代的原因计算精度有限，而计算精度高的解析方法多需要较多的分类讨论，进而影响了计算速度。

发明内容

为了克服现有技术的不足，本发明提供一种五自由度首末对称机械臂逆运动学计算的优化方法，使用本发明可快速求解五自由度首末对称的机械臂关节角与关节形位的闭式表达式。

本发明解决其技术问题所采用的技术方案包括如下步骤：

a)将五自由度首末对称机械臂的各个关节按从基座至执行器的顺序进行标号，基座处的关节标记为第一关节，执行器处的关节标记为第五关节，并在第三和第四关节之间增加一个转动角度始终为零的虚拟旋转关节，以此计算机械臂DH参数；

b)给定目标形位相对于惯性坐标系O₀X₀Y₀Z₀的齐次变换矩阵T_O，取T_O的前3行前3列子矩阵为目标位置O₆相对于惯性坐标系的姿态矩阵，记为R₆，取T_O的第4列前3元素为目标位置O₆相对于惯性坐标系的坐标，记为o₆，将虚拟关节和第四第五关节命名为球型手腕，将虚拟关节和第四第五关节的关节轴延长线的交点命名为球型手腕中心，计算球型手腕中心O_c的位置坐标o_c；球型手腕中心O_c的位置坐标o_c计算方法为：

o_c＝o₆-d₆·R₆·{0,0,1}^T (1)

其中，d₆为目标位置沿Z₅方向相对手腕中心的偏置距离；

c)提取o_c中的三个元素x_c、y_c、z_c，据几何关系求目标转角θ_O1、θ_O2、θ_O3，目标转角θ_O1、θ_O2、θ_O3的计算方法为：

θ_O1＝Atan2(x_c,y_c) (2)

其中，D_O称为球腕点参数，D_O＝[x_c ²+y_c ²+(z_c-l₁)²-l₂ ²-l₃ ²]/(2l₂l₃)；Atan2为双变量反正切函数，l₁、l₂和l₃分别为机械臂的第一段连杆的长度、第二段连杆的长度和第三段连杆的长度；

d)计算目标转角为θ_O1、θ_O2、θ_O3时前三关节的齐次变换矩阵为T₃ ⁰，求虚拟关节和后两关节的目标齐次变换矩阵T₅ ³，虚拟关节和后两关节的目标齐次变化矩阵T₅ ³的计算方法为：

T₅ ³＝(T₃ ⁰)^TT_O (5)

e)记T₅ ³中第i行第j列元素为r_ij，据齐次变换关系，目标转角θ_O4、θ_O5的计算方法为：

θ_O4＝Atan2(r₃₃,-r₁₃) (6)

θ_O5＝Atan2(r₂₂,r₂₁) (7)

用公式(6)和(7)即可求得目标转角θ_O4、θ_O5。

本发明的有益效果在于对五自由度首末对称机械臂的逆运动学求解过程未经过迭代、分类、试凑等技术手段，而是直接根据机械臂自身参数得到了关节角与关节形位的闭式表达式，因此可以可快速求解五自由度首末对称机械臂的逆运动学问题，提高了计算精度和计算速度，有助于实现机械臂的实时响应。

附图说明

图1为本发明首末对称机械臂实际坐标系设置图。

图2为本发明采用本发明所述方法的首末对称机械臂坐标系设置图。

图3为本发明后三关节中心与惯性坐标系的几何关系示意图。

其中，X₀、X₁、X₂、X₃、X₄、X₅分别表示从机械臂第一关节到执行器的所有坐标系的x轴，X_x表示虚拟关节坐标系的x轴，Z₀、Z₁、Z₂、Z₃、Z₄、Z₅分别表示从机械臂第一关节到执行器的所有坐标系的z轴，Z_x表示虚拟关节坐标系的z轴，Y₀表示机械臂第一关节坐标系的y轴，N₁、N₂、N₃、N₄、N₅分别表示机械臂的各个关节，N_x表示虚拟关节，θ₁、θ₂、θ₃、θ₄、θ₅分别表示机械臂的各个关节的关节转角，θ_x表示虚拟关节的转角，l₁、l₂、l₃、l₄分别表示机械臂的各段连杆的长度，d₆为目标位置沿Z₆方向相对手腕中心的偏置距离，O_c是球型手腕中心，xc、yc分别是球型手腕中心的横坐标和纵坐标。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

一种首末对称机械臂逆运动学计算的优化方法，其具体步骤如下：

a)如图1、图2所示，将五自由度首末对称机械臂的各个关节按从基座至执行器的顺序进行标号，基座处的关节标记为第一关节，执行器处的关节标记为第五关节，并在第三和第四关节之间增加一个转动角度始终为零的虚拟旋转关节，以此计算机械臂DH参数；

b)给定目标形位相对于惯性坐标系O₀X₀Y₀Z₀的齐次变换矩阵T_O，取T_O的前3行前3列子矩阵为目标位置O₆相对于惯性坐标系的姿态矩阵，记为R₆，取T_O的第4列前3元素为目标位置O₆相对于惯性坐标系的坐标，记为o₆，将虚拟关节和第四第五关节命名为球型手腕，将虚拟关节和第四第五关节的关节轴延长线的交点命名为球型手腕中心，计算球型手腕中心O_c的位置坐标o_c；球型手腕中心O_c的位置坐标o_c计算方法为：

o_c＝o₆-d₆·R₆·{0,0,1}^T (1)

其中，d₆为目标位置沿Z₅方向相对手腕中心的偏置距离；

c)提取o_c中的三个元素x_c、y_c、z_c，据如图3所示的几何关系，据几何关系求目标转角θ_O1、θ_O2、θ_O3，目标转角θ_O1、θ_O2、θ_O3的计算方法为：

θ_O1＝Atan2(x_c,y_c) (2)

其中，D_O称为球腕点参数，D_O＝[x_c ²+y_c ²+(z_c-l₁)²-l₂ ²-l₃ ²]/(2l₂l₃)；Atan2为双变量反正切函数，l₁、l₂、l₃分别为机械臂的第一段连杆的长度(即：机械臂基座端面到第一个与该端面平行的关节轴的距离)、第二段连杆的长度(即：第一个与基座端面平行的关节轴到第二个与基座端面平行的关节轴的距离)和第三段连杆的长度(即：第二个与基座端面平行的关节轴到第三个与基座端面平行的关节轴的距离)；

d)计算目标转角为θ_O1、θ_O2、θ_O3时前三关节的齐次变换矩阵为T₃ ⁰，求虚拟关节和后两关节的目标齐次变换矩阵T₅ ³，虚拟关节和后两关节的目标齐次变化矩阵T₅ ³的计算方法为：

T₅ ³＝(T₃ ⁰)^TT_O (5)

e)记T₅ ³中第i行第j列元素为r_ij，据齐次变换关系，目标转角θ_O4、θ_O5的计算方法为：

θ_O4＝Atan2(r₃₃,-r₁₃) (6)

θ_O5＝Atan2(r₂₂,r₂₁) (7)

用公式(6)和(7)即可求得目标转角θ_O4、θ_O5。

本实施例机械臂逆运动学求解过程直接根据机械臂自身参数得到了关节角与关节形位的闭式表达式，未经过迭代、分类、试凑等过程，可快速求解机械臂逆运动学问题，提高了计算精度和计算速度，有助于实现机械臂的实时响应。

Claims (1)

1.一种五自由度首末对称机械臂逆运动学计算的优化方法，其特征在于包括下述步骤：

a)将五自由度首末对称机械臂的各个关节按从基座至执行器的顺序进行标号，基座处的关节标记为第一关节，执行器处的关节标记为第五关节，并在第三和第四关节之间增加一个转动角度始终为零的虚拟旋转关节，以此计算机械臂DH参数；

b)给定目标形位相对于惯性坐标系O₀X₀Y₀Z₀的齐次变换矩阵T_O，取T_O的前3行前3列子矩阵为目标位置O₆相对于惯性坐标系的姿态矩阵，记为R₆，取T_O的第4列前3元素为目标位置O₆相对于惯性坐标系的坐标，记为o₆，将虚拟旋转关节 和第四第五关节命名为球型手腕，将虚拟旋转关节 和第四第五关节的关节轴延长线的交点命名为球型手腕中心，计算球型手腕中心O_c的位置坐标o_c；球型手腕中心O_c的位置坐标o_c计算方法为：

o_c＝o₆-d₆·R₆·{0,0,1}^T (1)

其中，d₆为目标位置沿Z₅方向相对手腕中心的偏置距离；

c)提取o_c中的三个元素x_c、y_c、z_c，据几何关系求目标转角θ_O1、θ_O2、θ_O3，目标转角θ_O1、θ_O2、θ_O3的计算方法为：

θ_O1＝Atan2(x_c,y_c) (2)

其中，D_O称为球腕点参数，D_O＝[x_c ²+y_c ²+(z_c-l₁)²-l₂ ²-l₃ ²]/(2l₂l₃)；Atan2为双变量反正切函数，l₁、l₂和l₃分别为机械臂的第一段连杆的长度、第二段连杆的长度和第三段连杆的长度；

d)计算目标转角为θ_O1、θ_O2、θ_O3时前三关节的齐次变换矩阵为T₃ ⁰，求虚拟旋转关节 和后两关节的目标齐次变换矩阵

虚拟旋转关节 和后两关节的目标齐次变化矩阵T₅ ³的计算方法为：

T₅ ³＝(T₃ ⁰)^TT_O (5)

e)记T₅ ³中第i行第j列元素为r_ij，据齐次变换关系，目标转角θ_O4、θ_O5的计算方法为：

θ_O4＝Atan2(r₃₃,-r₁₃) (6)

θ_O5＝Atan2(r₂₂,r₂₁) (7)

用公式(6)和(7)即可求得目标转角θ_O4、θ_O5。

Images