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

权利要求书

1.一种任意三关节的逆运动学求解方法，其特征在于：包括以下步骤：

步骤1：求解θ₁和θ₃

空间点p绕轴ω₃旋转角度θ₃到点p₁，再绕轴ω₂旋转角度θ₂到点p₂，最后点p₂绕轴ω₁旋转角度θ₁到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)整理得：

a₁sinθ₁+b₁cosθ₁+c₁sinθ₃+d₁cosθ₃＝k₁ (6)；

a₂sinθ₁+b₂cosθ₁+c₂sinθ₃+d₂cosθ₃＝k₂ (7)；

其中，a₁,b₁,c₁,d₁,a₂,b₂,c₂,d₂,k₁,k₂均为已知参数；

当a₁b₂-b₁a₂≠0，对式(6)、(7)进行化简可得：

其中，

当c₁d₂-d₁c₂≠0时，公式(6)和(7)可整理为：

其中，公式(10)中的系数可根据公式(9)中的a,b分别与c,d互换，下标不变得到：

根据三角函数性质，将式(8)带入sin²θ₁+cos²θ₁＝1中，整理可得：

(f_s1-u_s1sinθ₃-v_s1cosθ₃)²+(f_c1-u_c1sinθ₃-v_c1cosθ₃)²＝1 (11)；

设则将其带入sin²θ₃+cos²θ₃＝1中，整理可得：

m₁t⁴+m₂t³+m₃t²+m₄t+m₅＝0 (12)；

其中，

m₁＝(f_s1+v_s1)²+(f_c1+v_c1)²-1

m₂＝-4[(f_s1+v_s1)u_s1+(f_c1+v_c1)u_c1]

m₄＝-4[(f_s1-v_s1)u_s1+(f_c1-v_c1)u_c1]

m₅＝(f_s1-v_s1)²+(f_c1-v_c1)²-1

根据费拉里法求解式(12)一元四次方程可得t的解，根据角度取值范围，可进一步确定θ₃的值：

θ₃＝2arctan(t) (13)；

将θ₃的值带入公式(8)，可得θ₁：

θ₁＝atan2(f_s1-u_s1sinθ₃-v_s1cosθ₃,f_c1-u_c1sinθ₃-v_c1cosθ₃) (14)；

步骤2：求解θ₂

当θ₁和θ₃已知时，由和可获得p₁和p₂，而p₂和p₁之间有：

将旋转矩阵的Rodrigues公式带入公式(15)可得：

x₂sinθ₂+y₂cosθ₂＝z₂ (16)；

其中，

由于在公式(16)两边分别同乘以和可得：

则可得θ₂的值：