[发明专利]一种5关节机器人末端工具位姿控制方法

权利要求书

1.5旋转关节机器人末端工具位姿控制方法，其特征在于：将图1所示的5R（5旋转关节）机器人看做是图2所示的并且具有表1所示D-H参数的6R（6旋转关节）机器人中令关节6角度θ₆固定为0的特例，在其直角坐标工作空间内任意给定末端工具工作点位置（p_x、p_y、p_z）和姿态角（α、β、γ），这种5R机器人的各关节角度的控制值计算过程如下：

表1：6关节机器人的D-H参数表

（1）根据任意给定的机器人末端工具工作点位置（p_x、p_y、p_z）和姿态角（α、β、γ），按照已知的公式其位姿矩阵T可按下式算出：

T=r11r12r13pxr21r22r23pyr31r32r33pz0001=cαcβcαsβsγ-sαcγcαsβcγ+sαsγpxsαcβsαsβsγ+cαcγsαsβcγ-cαsγpy-sβcβsγcβcγpz0001---(1)]]>

式中：γ、β、α——机器人末端工具分别为绕固连于机器人底座的直角固定坐标轴X、Y、Z旋转的末端工具方位角；

p_x、p_y、p_z——机器人末端工具工作点在X、Y、Z轴上的坐标值。

c_α、c_β、c_γ——分别代表α、β、γ的余弦；

s_α、s_β、s_γ——分别代表α、β、γ的正弦；

（2）求出T后，根据已知公式计算图2所示6R机器人关节角度θ₁、θ₂、θ₃如下：

θ1=atan(py/px)θ3=atan(a3/d4)-atan(Ka32+d42-K2)θ2=atan(t1/t2)-θ3---(2)]]>

式中s_i、c_i——分别代表sin(θ_i)、cos(θ_i)，i=1～6；

atan——反正切函数。

式（2）中的其他参数见下式：

K=px2+py2+pz2-a22-a32-d422a2t1=(c1px+s1py)(a2s3-d4)-(a3+a2c3)pzt2=(a2s3-d4)pz+(a3+a2c3)(c1px+s1py)---(3)]]>

（3）求出T、θ₁、θ₂、θ₃后，按下式计算θ₄：

θ4=atan(r11s1-r21c1r11c1c23+r21s1c23-r31s23)---(4)]]>

式中：s_ij、c_ij——分别代表sin(θ_i+θ_j)、cos(θ_i+θ_j)，i,j=1～6；

r_ij——位姿矩阵元素，i,j=1～3，见式（1）；

（4）求出θ₄后，再按下式计算γ′：

γ′=atan(c1sαsβ-s1cαsβ-(cβs23-cαsβc1c23-s1c23sαsβ)tanθ4sαs1+cαc1+(cαs1c23-sαc1c23)tanθ4)---(5)]]>

（5）用求出的γ′，与给定的（p_x、p_y、p_z）和姿态角α、β组成新的位姿（α、β、γ′、p_x、p_y、p_z），这是使图2所示的6R机器人中关节角θ₆等于0的位姿；

（6）根据新的位姿参数（α、β、γ′、p_x、p_y、p_z），按照下式重新计算机器人位姿矩阵T′：

T′=r11r12′r13′pxr21r22′r23′pyr31r32′r33′pz0001=cαcβcαsβsγ′-sαcγ′cαsβcγ′+sαsγ′pxsαcβsαsβsγ′+cαcγ′sαsβcγ′-cαsγ′py-sβcβsγ′cβcγ′pz0001---(6)]]>

式中，由于位姿矩阵元素r₁₁、r₂₁、r₃₁只和姿态角α、β相关，因此在α、β不变的前提下，r₁₁、r₂₁、r₃₁也不变，见式（1）。

（7）根据上面得出的T′，利用6R机器人逆运动学公式，按下式计算相应的关节角θ₅、θ₆的正弦和余弦值：

s5=r33′s23c4-r13′(c1c23c4+s1s4)-r23′(s1c23c4-c1s4)c5=-r13′c1s23-r23′s1s23-r33′c23s6=r31s23s4-r11(c1c23s4-s1c4)-r21(s1c23s4+c1c4)c6=r11((c1c23c4+s1s4)c5-c1s23s5)+r21((s1c23c4-c1s4)c5-s1s23s5)-r31(s23c4c5+c23s5)---(7)]]>

进而用下式求出节角θ₅、θ₆：

θ5=atan(s5/c5)θ6=atan(s6/c6)---(8)]]>

其中θ₆的计算结果将始终为0。

（8）将上面步骤（2）、步骤（3）、步骤（7）计算出的θ₁、θ₂、θ₃、θ₄、θ₅作为目标值，分别应用于图1所示5R机器人关节1～关节5的角度控制，得到的末端工具姿态和位置即为（α、β、γ′、p_x、p_y、p_z），与初始给定的5R机器人末端工具姿态和位置（α、β、γ、p_x、p_y、p_z）相比，工作点位置（p_x、p_y、p_z）没有变化，工作姿态中只有γ角发生了变化。