[發明專利]一種任意關系的二階子問題逆運動學求解方法

有效

• 申請號: 201710168532.X
• 申請日: 2017-03-21
• 公開（公告）號: CN106991277B
• 公開（公告）日: 2018-03-20
• 發明（設計）人: 王海霞;盧曉;李玉霞;樊炳輝;朱延正;江浩
• 申請（專利權）人: 山東科技大學
• 主分類號: G06F19/00
• 分類號: G06F19/00
• 代理公司: 青島智地領創專利代理有限公司37252
• 代理人: 陳海濱

技術領域

本發明屬于機器人逆運動學領域，具體涉及一種任意關系的二階子問題逆運動學求解方法。

背景技術

Paden-Kanhan子問題在機器人逆運動學應用非常廣泛，因為它具有幾何意義和數值穩定性，能夠靈活的為多種機器人提供封閉解。Paden-Kanhan子問題主要分為三類：一階子問題，二階子問題，三階子問題。其中一階子問題是針對單關節的轉動R或平移T運動的逆解問題；二階子問題是針對兩個關節逆解問題，包含了3種情況：RR，TT，RT/TR，其中RR又分為相交、平行、異面垂直等不同的類型；三階子問題是針對三個關節的逆解問題，包含了6種情況。在實際中，由于加工、裝配很多幾何關系很難保證，比如：相交、平行，而且不同的結構需要選擇不同的公式，這為實際應用帶來很多不便。

發明內容

針對現有技術中存在的上述技術問題，本發明提出了一種任意關系的二階子問題逆運動學求解方法，設計合理，克服了現有技術的不足，具有良好的效果。

為了實現上述目的，本發明采用如下技術方案：

一種任意關系的二階子問題逆運動學求解方法，包括如下步驟：

步驟1：求θ₁

二階子問題RR可用公式表示為

其中，是p,q的齊次坐標，由第i關節軸的軸方向向量ω_i和軸上一點r_i組成，這些參數均已知。根據旋量理論的距離相等原則可知：

||c-r₂||＝||p-r₂||(5)；

將帶入上式，并利用的Rodrigues旋轉公式將其化簡成關于θ₁的三角函數方程：

x₁sinθ₁+y₁cosθ₁＝z₁ (9)；

其中為已知參數，從上式可解得θ₁的表達式：

上式中需要通過調整r₁和r₂來保證

步驟2：求θ₂

根據已知的θ₁可得c的值，而c還可表示為：

將的Rodrigues旋轉公式帶入上式整理可得：

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

其中均為已知參數，從上式中可解的θ₂的表達式：

θ₂角度的具體象限由和的符號決定，需注意的是當相鄰兩關節相交的時候，兩關節軸上的點r₁和r₂，必須滿足r₁≠r₂≠r₀，其中r₀是兩條軸線的交點。

本發明所帶來的有益技術效果：

1、計算效率高，給出了關節角度的封閉解，可利反三角函數直接求出，具有很高的計算效率；2、實現簡單，每個關節的表達形式非常簡單易懂，只需求解一次反三角函數即可；3、應用范圍廣，可應用于任意2R機器人中，不需要考慮其軸線之間的幾何關系。

附圖說明

圖1為任意關系的RR結構圖。

具體實施方式

下面結合附圖以及具體實施方式對本發明作進一步詳細說明：

一種任意關系的二階子問題逆運動學求解方法，包括如下步驟：

步驟1：求θ₁

如圖1所示，二階子問題RR可用公式表示為

其中，和是空間點p和q的齊次坐標表示，且點為初始點，繞軸ω₂轉θ₂到點c，c點繞ω₁旋轉θ₁到點q，為運動旋量，由關節軸的單位方向向量和軸上的任意一點構成，是剛體變換的指數表達，對于轉動關節其表達式為：

其中，I_3×3為3×3的單位矩陣，是旋轉矩陣，可用Rodrigues表示為：

其中，是單位方向向量ω＝[ω_x,ω_y,ω_z]^T的反對稱矩陣，可表示為：