[发明专利]一种六自由度串联机器人运动学反解的求解方法

说明书

技术领域

本发明属于机器人运动学求解方法领域，更具体地，涉及一种六自由度串联机器人运动学反解的求解方法。

背景技术

机器人运动学的反解是机器人控制中非常重要的一环。目前，六自由度串联机器人运动学求解的方法主要有：代数法、几何法和数值解解法等。代数法是在已知机器人DH参数的情况下，对于机器人运动学方程，将一个未知数由矩阵方程的右边移向左边，使其与其他未知数分开，解出这个未知数，再把下一个未知数移至左边，重复进行，直到解出所有未知数；几何法是在分析机器人几何结构的基础上，将其在三维空间的几何问题分解成若干个容易求解的平面几何问题，然后在二维平面内，分析各个连杆之间的几何关系，而不用建立机器人的运动学方程；数值解解法是通过采用智能算法如神经网络等，通过逼近的方法来求解非线性方程的解。

然而，上述现有的六自由度串联机器人运动学求解方法存在以下问题：代数法作为最常用的机器人运动学反解的求解方法，求解过程需要进行高达6次的矩阵求逆运算，推导时运算量较大，且需要进行大量的重复性工作；而几何法对于结构简单的串联机器人求解快速，但是它会随着机构复杂程度的增加，算法的求解也会变得复杂；数值解解法则因为稳定性和精度难以得到保证，因此很少在实际机器人反解中得到运用。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种六自由度串联机器人运动学反解的求解方法，其采用将机器人各关节处位置和姿态进行分离求解的方式，实现六自由度串联机器人运动学的反求解，解决目前代数法运算量大，几何法对于高自由度机器人算法求解复杂以及数值解解法不稳定和精度低的缺陷。

为实现上述目的，本发明提出了一种六自由度串联机器人运动学反解的求解方法，其特征在于，包括如下步骤：

1)将待求解的六自由度串联机器人的第一、第二和第三关节依次简化为点A(X_A,Y_A,Z_A)、B和C，第四、第五和第六关节轴线交于点E(X_E,Y_E,Z_E)，所述六自由度串联机器人的基座则简化为线段HA，连杆一和连杆二则依次简化为线段AB和BC，连杆三和连杆四则简化为线段DE，其中，A(0,0,0),H(0,0,-L₀),AH＝L₀,AB＝d₂,BC＝a₂,CD＝a₃,DE＝d₄；建立所述各连杆的坐标系为X_iY_iZ_i，将各连杆坐标系中X_i-1到X_i绕Z_i旋转的角度设为关节变量θ_i，其中1≤i≤6；

2)过所述点B做线引入虚点Q(X_B,Y_B,-L₀)，使得根据已知的点E(X_E,Y_E,Z_E)的位置和姿态，建立位置约束方程，求得所述点B的位置坐标其中k＝1、2；

3)根据所述E(X_E,Y_E,Z_E)、建立位置约束方程，求得所述点C的位置坐标其中s＝1、2、3、4；

4)根据步骤1)-3)中所述的点A、B、C、E的位置坐标建立位置约束方程，求得所述点D的位置坐标其中m为正整数，且1≤m≤8；再根据从所述中选取四组解；

5)建立各连杆的坐标系的姿态约束方程，代入与各关节对应的各点的位置坐标，求解各关节变量的中间值S_jθ_i，其中1≤j≤8；依次判断每组各关节旋转是否为正：若为正，则S_jθ_i′＝S_jθ_i；否则S_jθ_i′＝-S_jθ_i；最后从S_jθ_i′中选取各关节变量的最佳解；以此方式，完成机器人运动学反解的求解。

作为进一步优选的，所述步骤2)中的位置约束方程具体如下：

作为进一步优选的，所述步骤3)中的位置约束方程具体如下：