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

摘要

本发明公开了一种六自由度串联机器人运动学反解的求解方法，该方法包括：读入连杆参数建立机器人连杆坐标系模型；已知连杆末端关节位置，建立关节位置约束方程；根据各关节位置约束方程，确定各关节位置；建立机器人各关节坐标系的姿态约束方程；将之前求得的关节位置坐标解分别代入姿态约束方程中，根据姿态约束方程，求解各组关节变量中间值；对关节变量中间值进行分析处理，选取最佳关节变量解。本发明采用空间几何理论将机器人运动学反解中位置和姿态进行分离求解，大大降低了几何法运动学反解运算的复杂性，并能够应用于机器人空间避障领域。

主权项

一种六自由度串联机器人运动学反解的求解方法，其特征在于，包括如下步骤：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′中选取各关节变量的最佳解；以此方式，完成机器人运动学反解的求解。