本發(fā)明屬于機(jī)器人逆運動學(xué)領(lǐng)域，具體涉及一種任意關(guān)系的二階子問題逆運動學(xué)求解方法。

背景技術(shù)：

paden-kanhan子問題在機(jī)器人逆運動學(xué)應(yīng)用非常廣泛，因為它具有幾何意義和數(shù)值穩(wěn)定性，能夠靈活的為多種機(jī)器人提供封閉解。paden-kanhan子問題主要分為三類：一階子問題，二階子問題，三階子問題。其中一階子問題是針對單關(guān)節(jié)的轉(zhuǎn)動r或平移t運動的逆解問題；二階子問題是針對兩個關(guān)節(jié)逆解問題，包含了3種情況：rr，tt，rt/tr，其中rr又分為相交、平行、異面垂直等不同的類型；三階子問題是針對三個關(guān)節(jié)的逆解問題，包含了6種情況。在實際中，由于加工、裝配很多幾何關(guān)系很難保證，比如：相交、平行，而且不同的結(jié)構(gòu)需要選擇不同的公式，這為實際應(yīng)用帶來很多不便。

技術(shù)實現(xiàn)要素：

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

為了實現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：

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

步驟1：求θ1

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

其中，是p,q的齊次坐標(biāo)，由第i關(guān)節(jié)軸的軸方向向量ωi和軸上一點ri組成，這些參數(shù)均已知。根據(jù)旋量理論的距離相等原則可知：

||c-r2||＝||p-r2||(5)；

將帶入上式，并利用的rodrigues旋轉(zhuǎn)公式將其化簡成關(guān)于θ1的三角函數(shù)方程：

x1sinθ1+y1cosθ1＝z1(9)；

其中為已知參數(shù)，從上式可解得θ1的表達(dá)式：

上式中需要通過調(diào)整r1和r2來保證

步驟2：求θ2

根據(jù)已知的θ1可得c的值，而c還可表示為：

將的rodrigues旋轉(zhuǎn)公式帶入上式整理可得：

x2sinθ2+y2cosθ2＝z2(14)；

其中均為已知參數(shù)，從上式中可解的θ2的表達(dá)式：

θ2角度的具體象限由和的符號決定，需注意的是當(dāng)相鄰兩關(guān)節(jié)相交的時候，兩關(guān)節(jié)軸上的點r1和r2，必須滿足r1≠r2≠r0，其中r0是兩條軸線的交點。

本發(fā)明所帶來的有益技術(shù)效果：

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

附圖說明

圖1為任意關(guān)系的rr結(jié)構(gòu)圖。

具體實施方式

下面結(jié)合附圖以及具體實施方式對本發(fā)明作進(jìn)一步詳細(xì)說明：

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

步驟1：求θ1

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

其中，和是空間點p和q的齊次坐標(biāo)表示，且點為初始點，繞軸ω2轉(zhuǎn)θ2到點c，c點繞ω1旋轉(zhuǎn)θ1到點q，為運動旋量，由關(guān)節(jié)軸的單位方向向量和軸上的任意一點構(gòu)成，是剛體變換的指數(shù)表達(dá)，對于轉(zhuǎn)動關(guān)節(jié)其表達(dá)式為：

其中，i3×3為3×3的單位矩陣，是旋轉(zhuǎn)矩陣，可用rodrigues表示為：

其中，是單位方向向量ω＝[ωx,ωy,ωz]^t的反對稱矩陣，可表示為：

根據(jù)旋量理論的距離相等原則可知：

||c-r2||＝||p-r2||(5)；

根據(jù)旋量理論的基本原理可知：

上述兩式相減可得：

將指數(shù)積公式的表達(dá)式(2)帶入式(6)可得：

將公式(7)帶入公式(5)可得：

再將的rodrigues表達(dá)(3)帶入式(8)，兩邊平方后，整理可得：

x1sinθ1+y1cosθ1＝z1(9)；

其中

設(shè)x1＝ρcosφ，y1＝ρsinφ，則利用三角函數(shù)的積化和差公式，公式(9)可變?yōu)椋?/p>

其中，同理可以得到：

則關(guān)節(jié)角度θ1可表示為：

上式中需要通過調(diào)整r1和r2來保證

步驟2：求θ2

將θ1的值帶入公式(7)中可得c的值，而c還可表示為：

將的rodrigues表達(dá)(3)帶入式(13)，整理可得：

x2sinθ2+y2cosθ2＝z2(14)；

其中

由于則在公式(14)兩邊分別同乘以和可得：

則θ2可表示為：

θ2角度的具體象限由和的符號決定，需要注意的是當(dāng)相鄰兩關(guān)節(jié)相交的時候，兩關(guān)節(jié)軸上的點r1和r2，必須滿足r1≠r2≠r0，其中r0是兩條軸線的交點。

當(dāng)然，上述說明并非是對本發(fā)明的限制，本發(fā)明也并不僅限于上述舉例，本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明的實質(zhì)范圍內(nèi)所做出的變化、改型、添加或替換，也應(yīng)屬于本發(fā)明的保護(hù)范圍。