[发明专利]一种任意三关节的逆运动学求解方法

摘要

本发明公开了一种任意三关节的逆运动学求解方法，属于机器人逆运动学领域，本发明在指数积模型的基础上，利用简单的几何约束方程、旋量理论的基本性质以及旋转矩阵的Rodrigues表达，将问题转化成关于三角函数的线性方程进行求解，实现了任意三个关节轴线的逆解问题，使机器人逆解的求解不再局限于相交、平行、垂直的约束关系中，可根据需求设计结构，安装或加工中存在的误差也不会影响最终的计算结果。本发明是一种灵活、方便、实用的机器人逆解方法，为机器人在实际中的应用提供了方便。

主权项

1.一种任意三关节的逆运动学求解方法，其特征在于：包括以下步骤：步骤1：求解θ1和θ3空间点p绕轴ω3旋转角度θ3到点p1，再绕轴ω2旋转角度θ2到点p2，最后点p2绕轴ω1旋转角度θ1到q点，这一过程可表示为：其中，是p,q的齐次坐标，为第i关节的运动旋量，包括关节轴的轴方向向量和轴上一点ω_i和r_i被称为旋量参数，的表达形式如下：其中，是ω_i的反对称矩阵，如果ω_i＝[ω_ix,ω_iy,ω_iz]^T，则可表示成：其中，和i＝1,3均已知；是刚体变换的指数表达，对于转动关节其表达式为：其中，I_3×3为3×3的单位矩阵，是旋转矩阵，可用Rodrigues表示为：根据旋量理论的基本性质可得：其中，r₂₁和r₂₂分别为第二个轴上的两个点，将以及和的Rodrigues公式，带入式(4)整理得：a1sinθ1+b1cosθ1+c1sinθ3+d1cosθ3＝k1    (6)；a2sinθ1+b2cosθ1+c2sinθ3+d2cosθ3＝k2   (7)；其中，a1,b1,c1,d1,a2,b2,c2,d2,k1,k2均为已知参数；当a1b2‑b1a2≠0，对式(6)、(7)进行化简可得：其中，当c1d2‑d1c2≠0时，公式(6)和(7)可整理为：其中，公式(10)中的系数可根据公式(9)中的a,b分别与c,d互换，下标不变得到：根据三角函数性质，将式(8)带入sin2θ1+cos2θ1＝1中，整理可得：(fs1‑us1sinθ3‑vs1cosθ3)2+(fc1‑uc1sinθ3‑vc1cosθ3)2＝1   (11)；设则将其带入sin²θ₃+cos²θ₃＝1中，整理可得：m1t4+m2t3+m3t2+m4t+m5＝0    (12)；其中，m1＝(fs1+vs1)2+(fc1+vc1)2‑1m2＝‑4[(fs1+vs1)us1+(fc1+vc1)uc1]m4＝‑4[(fs1‑vs1)us1+(fc1‑vc1)uc1]m5＝(fs1‑vs1)2+(fc1‑vc1)2‑1根据费拉里法求解式(12)一元四次方程可得t的解，根据角度取值范围，可进一步确定θ3的值：θ3＝2arctan(t)     (13)；将θ3的值带入公式(8)，可得θ1：θ1＝atan2(fs1‑us1sinθ3‑vs1cosθ3,fc1‑uc1sinθ3‑vc1cosθ3)     (14)；步骤2：求解θ2当θ₁和θ₃已知时，由和可获得p₁和p₂，而p₂和p₁之间有：将旋转矩阵的Rodrigues公式带入公式(15)可得：x2sinθ2+y2cosθ2＝z2     (16)；其中，由于在公式(16)两边分别同乘以和可得：则可得θ2的值：