[发明专利]一种求解非球腕6R工业机器人逆运动学的数值算法

权利要求书

1.一种求解非球腕6R工业机器人逆运动学的数值算法，其特征在于，包括以下步骤：

S1、首先采用改进的断开-重连方法和DIXON合成法，推导出具有连续性且只包含θ₆的非线性方程，以及其它关节变量的逆解公式；

机器人末端相对于基坐标系的正向运动学公式可由各连杆齐次变换矩阵相乘得到：

^baseT＝T₁(θ₁)T₂(θ₂)T₃(θ₃)T₄(θ₄)T₅(θ₅)T₆(θ₆) (1)

建立正运动学方程后，可将上式变形为：

式(2)表示将原机器人分解为两个子链，子链L的末端位姿为T_L，它由关节2的原点至关节4和关节5的轴线交点部分组成，是一个平面结构；其余部分组成子链R，其末端位姿为T_R，其关节1和关节6之间的原点由T表示的虚拟连杆连接；在子链末端处，只要两子链的位姿满足以下条件即可重连：

p_L＝p_R (3)

式(3)实际为三个方程，当满足上述6个方程的6个关节变量都被求解出来时，两子链将在断开连接的位置处重新结合为原机器人，而这些得到的关节变量就是机器人的逆解；式(3)-(4)用于求θ₁、θ₂、θ₃和θ₆，式(5)可求得θ₄，式(6)可求得θ₅；

S2、然后证明相邻位姿点的逆解在相同唯一域中距离最小，并根据唯一域的判断，给出了逆解公式中正负符号确定的方法：

首先由式l₄c₃-l₃s₃＝0(c₃表示cosθ_3，s₃表示sinθ₃)可直接得到θ₃的边界值：从而因此，θ₃的逆解值域与唯一域是一致的，k₂的值可由下式确定：

k₁可由下式确定

由于机器人末端跟踪的是连续轨迹，k₁和k₂可由初始点代入上式(7)和(8)中确定；

S3、最后利用黄金分割法对非线性方程搜索求解，得到θ₁、θ₂、θ₃、θ₄、θ₅的求解公式；

S4、仿真验证本方法的有效性。

2.根据权利要求2所述的一种求解非球腕6R工业机器人逆运动学的数值算法，其特征在于，在步骤S1中相邻连杆间的齐次变换公式为：

其中

θ_i——x_i-1轴和x_i轴之间关于z_i的夹角；

d_i——{i-1}坐标系原点沿着z_i-1轴到x_i轴的距离；

a_i——沿着x_i轴，z_i-1轴和z_i轴之间的距离；

α_i——z_i-1轴和z_i轴之间关于x_i的夹角。