[发明专利]一种关节型机械臂逆运动学解析解的求取方法

摘要

本发明一种关节型机械臂逆运动学解析解求取方法属于工业机器人制造技术领域，涉及工业机器人领域快速获取一种肩关节朝前偏置的关节型六自由度机械臂的唯一逆运动学解析解求解方法。该方法按照D‑H参数法建立关节机械臂连杆坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并求其逆矩阵；求解机械臂六个关节的旋转角度表达式，利用旋转角度表达式进行逆运动学求解。该方法坐标系建模简单易懂，并且解析解能够保证该类机械臂逆运动学解要求。方法具有求解精度高、求解速度快、求解过程更加简单明了的特点。

主权项

一种关节型机械臂逆运动学解析解的求取方法，其特征是，该方法按照D‑H参数法建立关节型机械臂连杆坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并求其逆矩阵；求解机械臂六个关节的旋转角度表达式，利用旋转角度表达式进行逆运动学求解；方法具体步骤如下：步骤一，关节型机械臂由基座(A)、末端执行器(G)、5个连杆(B、C、D、E、F)和6个旋转关节(1、2、3、4、5、6)组成；按照D‑H参数法建立机械臂连杆坐标系，坐标系包括：XOZ平面，机械臂六个旋转关节对应坐标系O0～O5以及机械臂末端执行器的坐标系O6；各关节坐标系具体为：zi轴沿i+1关节的轴线，xi沿zi轴和zi‑1轴的公垂线，指向背离zi‑1轴方向，yi轴由右手直角坐标系规则确定，其中i＝1,2,3,4,5,6；将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O0:x0,y0,z0}重合，基坐标系始终保持不变；步骤二，根据机械臂相邻关节之间的4个结构几何参数：连杆转角θi、连杆扭角αi、连杆长度ai、连杆距离di计算相邻两坐标系的齐次坐标变换矩阵i‑1Ti,i＝1,2,...6；各几何参数的定义：相邻两连杆之间的连杆转角θi为xi轴与xi‑1轴之间的夹角，绕zi‑1轴从xi‑1轴到xi轴，符合右手规则时为正，对于转动关节，θi为变量；连杆扭角αi为zi轴与zi‑1轴之间的夹角，绕xi轴从zi‑1轴到zi轴，符合右手规则时为正，当两关节轴线平行时，αi＝0，当两关节轴线垂直时，αi＝‑90°或90°；连杆长度ai为zi轴与zi‑1轴的公垂线长度，沿xi轴方向测量，当两关节轴线平行时，ai＝li，li为连杆的长度，当两关节轴线垂直时，ai＝0；相邻两连杆之间的连杆距离di为xi轴与xi‑1轴之间的距离，在zi‑1轴上测量，对于转动关节，di为常数；按照连杆坐标系之间的齐次变换规则计算出相邻坐标系的各齐次变换矩阵i‑1Ti；机械臂的相邻关节坐标系间的齐次坐标变换矩阵i‑1Ti满足公式(1)：0T6＝0T11T22T33T44T55T6 (1)其中，等式左边矩阵为末端执行器的坐标系相对于基坐标系的齐次坐标变换矩阵，为已知条件；其中，nx,ny,nz分别为末端执行器坐标系{O6:x6,y6,z6}的x6轴与基坐标系的x0,y0,z0轴的夹角余弦值；ox,oy,oz分别为末端执行器坐标系的y6轴与基坐标系的x0,y0,z0轴的夹角余弦值；ax,ay,az分别为末端执行器坐标系的z6轴与基坐标系的x0,y0,z0轴的夹角余弦值；px,py,pz为末端执行器坐标系原点O6在基坐标系中的笛卡尔坐标；等式右边矩阵：0T1、1T2、2T3、3T4、4T5、5T6—分别为第一、第二、第三、第四、第五、末端执行器关节坐标系相对于基坐标系的齐次坐标变换矩阵；考虑末端执行器工具长度d6，末端执行器坐标系与第六个关节的坐标系之间的变换是：沿z5平移d6距离之后绕z5旋转θ6；步骤三，计算机械臂的各关节旋转角度理论表达式θi，首先在等式(1)的两边同时左乘矩阵再在等式两边同时右乘矩阵得：T2-11T1-10T06T6-15=T23T34T45---(2)]]>根据矩阵相等的定义，由等式(2)两边矩阵的第3行第4列的元素相等建立方程:pycosθ1‑pxsinθ1+axd6sinθ1‑ayd6cosθ1＝0 (3)由式(3)解得θ1，θ1由双参数反正切函数表示的理论表达式：θ1＝arctan2(px‑axd6,py‑ayd6)+180·N1 (4)其中，N1—由三角函数周期性产生的整数；d6—连杆5与末端执行器之间的距离；ax,ay,px,py—末端执行器坐标系在基坐标系中的位置与姿态的参数；然后，由等式(2)两边的矩阵的第1行第4列元素和第2行第4列元素分别相等，建立方程组①②：①：②：将两方程左右两边同时平方再相加，消去θ2得到由双参数反正切函数表示的θ3理论表达式：θ3=arctan2(a3,d4)-arctan2(k,±a32+d42-k2)+180·N3---(5)]]>其中，k为中间代换参数，N3—由三角函数周期性产生的整数；将方程组①②两边分别相加，由于θ3已知，得到由双参数反正切函数表示的θ2理论表达式：θ2=arctan2(n+m,n-m)-arctan2(v,±(n+m)2+(n-m)2-v2)+180·N2---(6)]]>其中，m，n，v均为中间代换参数；m＝d1‑pz+azd6；n＝pxcosθ1+pysinθ1‑axd6cosθ1‑ayd6sinθ1‑a1；v＝(a3+d4)cosθ3+(a3‑d4)sinθ3+a2；N2—由三角函数周期性产生的整数；在等式(1)的两边同时左乘矩阵得到：T4-13T3-12T2-11T1-10T600=T45T56---(7)]]>由等式(7)两边矩阵的第3行第3列元素相等，有方程：aycosθ1cosθ4-axcosθ4sinθ1-azcosθ2sinθ3sinθ4-azsinθ2cosθ3sinθ4+axcosθ2cosθ1cosθ3sinθ4+aycosθ2cosθ3sinθ1sinθ4-axsinθ2cosθ1sinθ3sinθ4-aysinθ2sinθ1sinθ3sinθ4=0---(8)]]>由(8)式计算得到由双参数反正切函数表示的θ4理论表达式：θ4=arctan2(aycosθ1-axsinθ1,azsin(θ2+θ3)-(axcosθ1+aysinθ1)cos(θ2+θ3))+180·N4---(9)]]>其中，N4—由三角函数周期性产生的整数；由等式(7)两边矩阵的第1行第3列元素与第2行第3列元素分别相等，有方程组③④：③：④：由方程组③④计算得到由双参数反正切函数表示的θ5理论表达式：θ5=arctan2((axsinθ1-aycosθ1)sinθ4-azcosθ4sin(θ2+θ3)+(axcosθ1+aysinθ1)cosθ4cos(θ2+θ3),(axcosθ1+aysinθ1)sin(θ2+θ3)+azcos(θ2+θ3))+180·N5---(10)]]>其中，N5—由三角函数周期性产生的整数；由等式(7)两边矩阵的第3行第1列元素与第3行第2列元素分别相等，有方程组⑤⑥：⑤：⑥：由方程组⑤⑥计算得到由双参数反正切函数表示的θ6理论表达式：θ6=arctan2((nycosθ1-nxsinθ1)cosθ4-nzsinθ4sin(θ2+θ3)+(nxcosθ1+nysinθ1)sinθ4cos(θ2+θ3),(oycosθ1-oxsinθ1)cosθ4-ozsinθ4sin(θ2+θ3)+(oxcosθ1+oysinθ1)sinθ4cos(θ2+θ3))+180·N6---(11)]]>其中，N6—由三角函数周期性产生的整数；步骤四，针对所述机械臂各关节的旋转角度表达式的可能多解结果，根据机械臂的实际工作范围，给定一组各个关节的转动角度值θi1，由正运动学解的唯一性，得到机械臂第六个关节的坐标系相对于基坐标系的由位置和姿态组成的位姿矩阵0T6，将此矩阵作为已知条件，利用所述旋转角度理论表达式θi进行逆运动学求解得到θi2；步骤五，比较给定的转动角度值θi1与逆运动学求解得到θi2值是否满足计算精度要求，若满足计算结束；若相差较大，利用所求θ1～θ6理论表达式进行逆运动学求解；通过调整θ2、θ3角度表达式中根号前面的正负号以及各表达式是否因反三角函数的周期性需要加180°·Ni，Ni为整数，使得由所述旋转角度表达式计算出的各旋转角度值与正向运动的输入相等，即逆运动学求解正确，从而确定θ2、θ3关节角度表达式中的正负号和所有表达式中的Ni值，即得到唯一的一组关节旋转角度表达式。