[发明专利]灵巧手定位方法及控制系统

权利要求书

1.一种灵巧手定位方法，用于三连杆机械手，其特征在于：三连杆机械手包括依次连接的基座、第一连杆、第二连杆和第三连杆；对三连杆机械手的关节做如下设置：

定义三连杆机械手的关节编号i为1、2、3、4；其中第一关节定义为基座所在处，以第一关节为坐标原点建立空间三维坐标系，三个轴为x₁轴、y₁轴、z₁轴；第二关节定义为第一连杆与第二连杆连接处，以第二关节为坐标原点建立空间三维坐标系，三个轴为x₂轴、y₂轴、z₂轴；第三关节定义为第二连杆与第三连杆连接处，以第三关节为坐标原点建立空间三维坐标系，三个轴为x₃轴、y₃轴、z₃轴；第四关节定义为第三连杆的末端，即机械手末端，以机械手末端为坐标原点建立空间三维坐标系，三个轴为x₄轴、y₄轴、z₄轴；以第一关节为坐标原点的空间三维坐标系中，机械手末端坐标(P_x，P_y，P_z)；

关节坐标轴的设置原则如下：经过设置各个关节所在坐标系的零点和各个坐标轴，使得上一个关节所在坐标系的x坐标轴与相邻的下一个坐标系的z坐标轴垂直并且相交；关节轴所在位置与关节所在坐标系的z轴一致；关节轴即为关节的旋转轴；

定义出关节所在处的坐标系相应于下一个相邻坐标系的参数如下：a_i是顺x_i-1坐标轴从z_i-1坐标轴到z_i坐标轴的空间距离，单位为m；α_i是绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度，单位为rad；d_i是顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离，单位为m；Θ_i是绕z_i坐标轴从x_i-1坐标轴到x_i坐标轴的角度，单位为rad；i＝1,2,3,4；

a₁＝0；α₁＝0；d₁＝0；式(1)

a₂＝A₁；d₂＝0；式(2)

a₃＝A₂；α₃＝0；d₃＝0；式(3)

a₄＝A₃；d₃＝D₃；Θ₄＝0；式(4)

A₁为第一关节的旋转轴线与第二关节的旋转轴线之间的距离，单位为m；A₂为第二关节的旋转轴线与第三关节的旋转轴线之间的距离，单位为m；A₃为第三关节的旋转轴线与机械手末端之间的距离，单位为m；D₃为第三关节的x₃轴与机械手末端的x₄轴之间的距离，单位为m；为第二关节的x₂轴与第一关节的x₁轴之间的角度，单位为rad；为第三关节的x3轴与第二关节的x2轴之间的角度，单位为rad；θ₁为第一关节的沿z₁轴旋转角度，单位为rad；θ₂为第二关节的沿z₂轴旋转角度，单位为rad；θ₃为第三关节的沿z₃轴旋转角度，单位为rad；

因绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度α_i，实现相邻关节的转换用下列矩阵表示：

因顺x_i-1坐标轴从z_i-1坐标轴到z坐标轴的空间距离a_i，实现相邻关节的转换用下列矩阵表示：

因绕z_i坐标轴从x_i-1坐标轴到x坐标轴的角度Θ_i，实现相邻关节的转换用下列矩阵表示：

顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离d_i，实现相邻关节的转换用下列矩阵表示：

将式(5)、式(6)、式(7)、式(8)相乘，得出相邻关节四个参数转换的矩阵如下：

将式(1)、式(2)、式(3)、式(4)的数值带入式(9)，然后矩阵相乘，得到

其中

又因三连杆机械手末端用一个4x4矩阵进行如下表述，即

其中(P_x，P_y，P_z)则为机械手末端坐标，且

A＝T₁ 式(16)

两边均左乘⁰T₁的逆，得到

^oT₁^-1A＝^oT₁^-1T₁ 式(17)

其中

其中P_x1＝-P_xsinΘ₁-P_ycosΘ₁ 式(20)

P_y1＝-P_xcosΘ₁-P_ysinΘ₁ 式(21)

P_z1＝P_z 式(22)

接着，计算^oT₁^-1T₁：

从中得到，^oT₁^-1A与⁰T₁^-1T₁的第二行、第四列的元素相等且等于0，得到

由^oT₁^-1A与⁰T₁^-1T₁的第1行、第四列元素相等，以及第三行、第四列元素相等，得到，

其中

由式(24)、式(25)、式(27)得到机械手末端坐标与θ₁，θ₂，θ₃的关系；

当已知机械手末端的目标位置时，控制第一关节、第二关节和第三关节的旋转角度，使得θ₁，θ₂，θ₃到达对应的角度即可。