[發明專利]一種任意三關節的逆運動學求解方法

在審

• 申請號: 201810323427.3
• 申請日: 2018-04-12
• 公開（公告）號: CN108763151A
• 公開（公告）日: 2018-11-06
• 發明（設計）人: 王海霞;盧曉;李玉霞;盛春陽;崔瑋;張志國
• 申請（專利權）人: 山東科技大學
• 主分類號: G06F17/11
• 分類號: G06F17/11;G06F17/16
• 代理公司: 青島智地領創專利代理有限公司 37252
• 代理人: 種艷麗

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)兩邊分別同乘以和可得：

則可得θ₂的值：