本發(fā)明屬于一種擬人七自由度偏置機械臂逆解方法，涉及擬人七自由度機械臂逆解過程中機械臂初始姿態(tài)計算的一種方法，應(yīng)用對象包括空間機械臂和地面機械臂，可應(yīng)用于空間機械臂的任務(wù)規(guī)劃，尤其更適用于障礙物規(guī)避，空間立體感更加形象。

背景技術(shù)：

隨著航天事業(yè)的發(fā)展，空間設(shè)施越來越多，空間機械臂在未來將越來越重要的作用。由于冗余機械臂在障礙回避、奇異處理、關(guān)節(jié)力矩優(yōu)化等方面具有較大的優(yōu)勢，根據(jù)在軌任務(wù)的需求分析采用冗余度機械臂的重要性，然后從奇異消除、工作空間優(yōu)化、運動學(xué)簡化、關(guān)節(jié)運動范圍、機械制造可實現(xiàn)性等方面進行臂型論證，最后采取在最優(yōu)六自由度機械臂基礎(chǔ)上，在肩部增加一個關(guān)節(jié)，構(gòu)成“3+1+3”配置的臂型方案，該機械表的肩部和腕部三個關(guān)節(jié)軸線相較于一點，運動學(xué)方程簡單，先解出非偏置機械臂(srs)的逆解，在利用偏置機械臂與非偏置機械臂(ssrms)逆解之間的對應(yīng)關(guān)系可得到偏置機械臂解析形式的運動學(xué)逆解。機械臂在執(zhí)行笛卡爾空間軌跡的運動時，需要根據(jù)期望的末端位姿計算關(guān)節(jié)角，由于七自由度機械臂的位置逆解存在無窮中情況，為了降低求解難度和存在逆解的可能情況，首先規(guī)定三號坐標系的坐標原點在參考平面上時的姿態(tài)為初始姿態(tài)，并作為參考臂型，此時各量的右上標取為0加以區(qū)分，參考系標識在左上角；并引入壁型角的概念，從而求出期望位姿下的逆解。本發(fā)明是以冗余機械臂為研究對象，為相關(guān)運動學(xué)問題的研究奠定理論基礎(chǔ)，并且該發(fā)明順應(yīng)了我國大力發(fā)展機器人行業(yè)，尤其發(fā)展空間機器人及其在軌服務(wù)技術(shù)的趨勢，研究成果對未來空間機器人在軌服務(wù)的實際應(yīng)用具有一定的理論和實際意義。

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

要解決的技術(shù)問題

為了避免現(xiàn)有技術(shù)的不足之處，本發(fā)明提出一種七自由度偏置機械臂逆解方法，結(jié)合幾何圖像，讓人選擇臂型是更加形象，更有利于臂型設(shè)計和障礙規(guī)避。

技術(shù)方案

一種七自由度偏置機械臂逆解方法，其特征在于步驟如下：

步驟1：建立機械臂末端位姿的4×4的齊次變換矩陣⁰t7

以基座為起始端，在關(guān)節(jié)部位建立0號坐標系，依次編號至末端執(zhí)行器處建立7號坐標系，共建立0、1、2、3、4、5、6和7個關(guān)節(jié)部位的坐標系；所述的坐標系為d-h坐標系；

⁰p3為3號坐標系原點在0號坐標系中的表示，d1d2d3d4d5d6d7分別為1-7號坐標系之間z軸的偏移長度，a1a2a3a4a5a6a7分別為相鄰坐標系z軸的垂直距離，α1α2α3α4α5α6α7分別相鄰坐標系前一個坐標系z軸繞自己x軸旋轉(zhuǎn)α角與后一個坐標系z軸重合，θ1θ2θ3θ4θ5θ6θ7分別為各個關(guān)節(jié)旋轉(zhuǎn)的角度。當d2d6為0時為偏置機械臂對應(yīng)的非偏置機械臂臂型；

在上述矩陣中變量nx、ny、nz為7號坐標系x軸單位向量在7號坐標系中的分量；sx、sy、sz為7號坐標系y軸的單位向量在0號坐標系中的分量；ax、ay、az為7號坐標系z軸單位向量在0號坐標系的分量；px、py、pz為7號坐標系的原點在0號坐標系中的表示；

步驟2、臂型角ψ的定義與參數(shù)化：臂型角是參考面繞著向量w按右手螺旋定則旋轉(zhuǎn)到與當前臂型面重合時所轉(zhuǎn)的角度；

其中：分別為向量w、p、k單位向量，采用sψ,cψ，si,ci表示sinψ、cosψ、sinθi、cosθi，i＝1,2，3,4,5,6,7；v為與關(guān)節(jié)1的旋轉(zhuǎn)軸平行的單位矢量；

步驟3、偏置機械臂肘部關(guān)節(jié)角的計算θ4：

sw為機械臂的斜邊長；

步驟4、計算偏置機械臂肩部關(guān)節(jié)角和腕部關(guān)節(jié)角：偏置機械臂肩部關(guān)節(jié)角包括θ1θ2θ3，腕部關(guān)節(jié)角包括θ5θ6θ7；

先求非偏置機械臂逆解關(guān)節(jié)角θ′1θ′2θ′3θ′4θ′5θ′6θ′7：

θ1＝θ′1

θ2＝θ′2

θ6＝θ′6

θ7＝θ′7

a＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)

b＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-

d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2

θ3＝artan(a(a3+a4c4)-ba4s4,b(a3+a4c4)+aa4s4)

θ5＝θ′3+θ′4+θ′5-θ3-θ4

再偏置機械臂的逆解為：

θ1＝θ′1

θ2＝θ′2

θ6＝θ′6

θ7＝θ′7

a＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)

b＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-

d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2

θ3＝artan(a(a3+a4c4)-ba4s4,b(a3+a4c4)+aa4s4)

θ5＝θ′3+θ′4+θ′5-θ3-θ4

θ′4、α、θ2θ4θ6分別取兩種情況，因此偏置七自由度機械臂的逆解有32種情況；

步驟5、選擇合理的一種逆解：

設(shè)偏置機械臂初始機械臂姿態(tài)角為q＝[q′1q′2q′3q′4q′5q′6q′7]，根據(jù)2-范數(shù)原理求出距初始姿態(tài)角最小的一組逆解，方法如下：

計算腕部關(guān)節(jié)和肩部關(guān)節(jié)

腕部兩個關(guān)節(jié)的計算：

θ6＝±acos(-(aw23sinψ+bw23cosψ+cw23))

θ7＝atan2((aw33sinψ+bw33cosψ+cw33)s6,(aw13sinψ+bw13cosψ+cw13)s6)

肩部兩個關(guān)節(jié)的計算：

θ1＝atan2((as23sinψ+bs23cosψ+cs23)s2,(as13sinψ+bs13cosψ+cs13)s2)

θ3＝atan2(-(as32sinψ+bs32cosψ+cs32)s2,(as31sinψ+bs31cosψ+cs31)s2)

式中的qij(i＝1、2、3…32，j＝1、2…7)代表32種7個關(guān)節(jié)的解，qij(i＝1、2、3…32，j＝1、2…7)每組逆解關(guān)節(jié)與初始關(guān)節(jié)差的絕對值；

選出δqi最小的對應(yīng)一組解，為所求逆解。

有益效果

本發(fā)明提出的一種七自由度偏置機械臂逆解方法，建立機械臂末端位姿的4×4的齊次變換矩陣⁰t7；定義臂型角和臂型面，計算偏置機械臂肘部關(guān)節(jié)角θ4、肩部關(guān)節(jié)角和腕部關(guān)節(jié)角；根據(jù)偏置機械臂與非偏置機械臂逆解的關(guān)系，可求偏置機械臂的逆解。本方法在障礙回避、奇異處理、關(guān)節(jié)力矩優(yōu)化等方面具有較大的優(yōu)勢；簡化了七自由度關(guān)節(jié)機器人的逆解求解過程，提高了計算效率。采用本方法在機器人工作空間優(yōu)化、關(guān)節(jié)型機器人運動路徑規(guī)劃等方面能提供形象的空間感，能直觀的表達機械臂的臂型。

附圖說明

圖1：計算七自由度機械臂逆解流程圖

圖2：臂型面定義

圖3：臂型角定義

圖4：任意狀態(tài)下各向量關(guān)系圖

圖5：初始狀態(tài)下各向量關(guān)系圖

圖6：初始姿態(tài)下機械臂的姿態(tài)圖

圖7：機械臂臂型示意圖

具體實施方式

現(xiàn)結(jié)合實施例、附圖對本發(fā)明作進一步描述：

步驟一：建立機械臂末端位姿的4×4的齊次變換矩陣⁰t7

⁰t7為4×4的齊次變換矩陣，即為機械臂末端位姿，可寫成如下形式：

以基座為起始端，在關(guān)節(jié)部位建立0號坐標系，依次編號至末端執(zhí)行器處建立7號坐標系，共建立0、1、2、3、4、5、6和7個關(guān)節(jié)部位的坐標系；所述的坐標系為d-h坐標系；

在上述矩陣中變量nx、ny、nz為7號坐標系x軸單位向量在7號坐標系中的分量；sx、sy、sz為7號坐標系y軸的單位向量在0號坐標系中的分量；ax、ay、az為7號坐標系z軸單位向量在0號坐標系的分量；px、py、pz為7號坐標系的原點在0號坐標系中的表示；

步驟二：臂型角ψ的定義與參數(shù)化

選取關(guān)節(jié)1和關(guān)節(jié)2旋轉(zhuǎn)軸線的交點作為s點，3號坐標系的坐標原點為e點，關(guān)節(jié)6和關(guān)節(jié)7旋轉(zhuǎn)軸線的交點為w點，各點的位置矢量(從參考坐標系原點指向該點的矢量)分別表示為s、e、w。點s、e、w所組成的平面sew即為當前臂型面。

定義與關(guān)節(jié)1的旋轉(zhuǎn)軸平行的單位矢量為v，即v＝[001]^t，則矢量v與點w所組成的平面為參考平面。令w＝w-s、e＝e-s。則有：

e＝⁰p3-[00d1]^t(3)

⁰p3為3號坐標系原點在0號坐標系中的表示，d1d2d3d4d5d6d7分別為1-7號坐標系之間z軸的偏移長度，a1a2a3a4a5a6a7分別為相鄰坐標系z軸的垂直距離，α1α2α3α4α5α6α7分別相鄰坐標系前一個坐標系z軸繞自己x軸旋轉(zhuǎn)α角與后一個坐標系z軸重合，θ1θ2θ3θ4θ5θ6θ7分別為各個關(guān)節(jié)旋轉(zhuǎn)的角度。當d2d6為0時為偏置機械臂對應(yīng)的非偏置機械臂臂型。

臂型角便是參考面繞著向量w按右手螺旋定則旋轉(zhuǎn)到與當前臂型面重合時，所轉(zhuǎn)的角度，即為臂型角ψ。

矢量e在矢量w上的投影為：

當前臂型面內(nèi)與向量w垂直的單位矢量可表示為：

參考平面內(nèi)與矢量w垂直的單位向量可表示為：

根據(jù)式(5)和式(6)以及矢量點乘和叉乘的性質(zhì)可得：

因此：

分別為向量w、p、k單位向量，以sψ,cψ，si,ci表示sinψcosψsinθicosθii＝1,2，3,4,5,6,7

點s⊥為點s沿關(guān)節(jié)3(z2)軸線延長d3長度的點，與e、w點構(gòu)成一平面。根據(jù)該機械臂的臂型特征可知，直線ss⊥與面s⊥ew垂直。如圖所示，我們規(guī)定三號坐標系的坐標原點在參考平面上時各量的右上標取為0加以區(qū)分，參考系標識在左上角。

步驟三：偏置機械臂肘部關(guān)節(jié)角的計算θ4

上面定義了臂型面及臂型角，下面將推導(dǎo)給定臂型角下的各關(guān)節(jié)角的計算公式。

在直角三角形s⊥ws中由勾股定理得：

三角形s⊥ew另兩邊的長度s⊥e＝a3、ew＝a4。根據(jù)余弦定律有：

因此：

由此可見，軸關(guān)節(jié)角θ4與臂型角ψ沒有關(guān)系，對于給定的末端位姿，有兩組取值。

步驟四：計算偏置機械臂肩部關(guān)節(jié)角和腕部關(guān)節(jié)角

偏置機械臂肩部關(guān)節(jié)角包括θ1θ2θ3，腕部關(guān)節(jié)角包括θ5θ6θ7，欲求偏置機械臂逆解關(guān)節(jié)角必須先求非偏置機械臂逆解關(guān)節(jié)角，非偏置機械臂關(guān)節(jié)角用θ′1θ′2θ′3θ′4θ′5θ′6θ′7表示。

θ1＝θ′1

θ2＝θ′2

θ6＝θ′6

θ7＝θ′7

a＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)

b＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-

d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2

θ3＝artan(a(a3+a4c4)-ba4s4,b(a3+a4c4)+aa4s4)

θ5＝θ′3+θ′4+θ′5-θ3-θ4

1)：計算非偏置初始姿態(tài)

其中

規(guī)定，三號坐標系的坐標原點在參考平面上時各量的右上標取為0加以區(qū)分，參考系標示在左上角，式(12)中為當臂型角ψ為0時3號坐標系在0號坐標系的方向余璇矩陣，分別為當3號坐標系原點在參考平面上時其的xyz軸的單位矢量在0號坐標系的表示，式(13)中為3號坐標系原點在參考平面時矢量e和4號坐標系x軸的單位矢量在0號坐標系的表達

由dh坐標系建系規(guī)則可知：

d3z3+a3x3＝e(14)

z3x3為第3號坐標系的zx軸的單位矢量。

由式(14)可得：

式(15)中⁰e⁰為已知量，證明如下：

向量e可以看作是由向量w繞向量l(次向量是v與ω叉乘所得)旋轉(zhuǎn)α角度所得。此時則有：

⁰e＝r(l,α)·w(16)

其中⁰e為當?shù)?號坐標系原點在參考平面上時矢量e的表示，其中l(wèi)為：

l＝v×w(17)

r(l,α)＝i3+[ul×]sin(α)+[ul×]²(1-cos(α))(18)

[ul×]表示向量l單位化后的叉乘因子，i3為三階單位矩陣。

由式(16)可得：

⁰e⁰＝⁰e＝r(l,α)·w(19)

式(19)中只有α是未知的，α的求解過程如下：

在直角三角形ss⊥e中，由勾股定理可得：

在δse0w中由余弦定理可得:

將式(20)帶入式(21)可得：

在δse0w中，由于即，邊e0w不是δse0w中的最長邊，所以α≤90°，故有：

至此，α求解完畢，α角可取兩種情況，即⁰e⁰為已知量證明完畢。

由dh坐標系建系規(guī)則可知：

w-e＝a4x4(24)

式(24)中x4為第4號坐標系x軸的單位矢量。

由方向余弦表示的姿態(tài)變換關(guān)系可知：

³x4＝[c4s40]^t(25)

⁰x4＝⁰r3·³x4＝[⁰x3⁰y3⁰z3]³x4(26)

式(25)中³x4為4號坐標系的x軸的單位矢量在3號坐標系的表示，式⁰x4為4號坐標系的x軸的單位矢量在0號坐標系的表示，⁰r3為3號坐標系相對于0號坐標系的方向余弦矩陣，⁰x3⁰y3⁰z3分別為3號坐標系的xyz軸的單位矢量在0號坐標系的表示。由式(24)、式(25)和式(26)可得：

由于w、a4和⁰e⁰都是已知量，故也是已知量，其中表示當臂型角為0時，4號坐標系的x軸單位矢量在0號坐標系的表示。

式(15)和式(27)左右兩邊分別叉乘且結(jié)合三軸單位正交矢量的關(guān)系即，x×y＝z、y×z＝x、z×x＝y(tǒng)可得：

解由式(15)、式(27)和式(29)組成的方程組：

可得：

故：

即初始姿態(tài)有上述算法得到。

2)非偏置機械臂腕部關(guān)節(jié)和肩部關(guān)節(jié)計算方法

非偏置機械臂的肩部關(guān)節(jié)包括θ′1θ′2θ′3，腕部關(guān)節(jié)角包括θ′5θ′6θ′7，肘部關(guān)節(jié)為θ′4

腕部關(guān)節(jié)角

θ′2＝±acos(-(as33sinψ+bs33cosψ+cs33))

θ′1＝atan2((as23sinψ+bs23cosψ+cs23)s2,(as13sinψ+bs13cosψ+cs13)s2)

θ′3＝atan2(-(as32sinψ+bs32cosψ+cs32)s2,(as31sinψ+bs31cosψ+cs31)s2)

上公式由下面所得

三號坐標系在零號坐標系中繞w旋轉(zhuǎn)ψ角后的姿態(tài)變換矩陣為：

⁰rψ＝i3+[⁰uw×]sinψ+[⁰uw×]²(1-cosψ)(33)

其中，i3為3×3的單位陣，[⁰uw×]為向量w的斜對稱陣。由式(33)知三號坐標系在零號坐標下的姿態(tài)可表示為：

將方程(33)帶入方程(34)可得：

⁰r3＝assinψ+bscosψ+cs(35)

其中，

根據(jù)姿態(tài)變換矩陣的定義，可得：

由式(35)和式(36)可得：

c2＝-(as33sinψ+bs33cosψ+cs33)(37)

根據(jù)式(37)，可求解θ2，有兩組值：

θ′2＝±acos(-(as33sinψ+bs33cosψ+cs33))(38)

相應(yīng)于θ2的取值結(jié)合式(35)和式(36)，可求解出θ1和θ3：

θ′1＝atan2((as23sinψ+bs23cosψ+cs23)/s2,(as13sinψ+bs13cosψ+cs13)/s2)(39)

θ′3＝atan2(-(as32sinψ+bs32cosψ+cs32)/s2,(as31sinψ+bs31cosψ+cs31)/s2)(40)

式(37)—式(40)中asij(i、j＝1、2、3)分別為as矩陣中的元素，bsij(i、j＝1、2、3)分別為bs中的元素，csij(i、j＝1、2、3)分別為cs矩陣中的元素。

需要說明的是，計算θ1、θ3的式(39)和(40)中，每一項均除以相同的系數(shù)s2′，但這是不能隨便約掉的，因為它的符號影響了相應(yīng)角度所處的象限位置，但為了避免由于s2＝0導(dǎo)致計算中出現(xiàn)無窮大的數(shù)而無法正常運算，可將處于s2改造成乘以s2而不影響計算結(jié)果，即：

θ′1＝atan2((as23sinψ+bs23cosψ+cs23)s2,(as13sinψ+bs13cosψ+cs13)s2)(41)

θ′3＝atan2(-(as32sinψ+bs32cosψ+cs32)s2,(as31sinψ+bs31cosψ+cs31)s2)(42)

肩部關(guān)節(jié)角與肘部關(guān)節(jié)：

θ′6＝±acos(-(aw23sinψ+bw23cosψ+cw23))

θ′4＝atan2((aw22sinψ+bw22cosψ+cw22)s6,(aw21sinψ+bw21cosψ+cw21)s6)

θ′5＝(θ′4+θ′5)-θ′4

θ′7＝atan2((aw33sinψ+bw33cosψ+cw33)s6,(aw13sinψ+bw13cosψ+cw13)s6)

結(jié)合式⁰t7＝⁰t1¹t2…⁶t7＝fkine(θ1,θ2,…,θ7)和式⁰r3＝assinψ+bscosψ+cs可知四號坐標系在七號坐標系下的表示為：

其中：

由姿態(tài)變換方式得到的四號坐標系在七號坐標系下的表示為：

由方程(43)和方程(44)并采用類似于計算肩部關(guān)節(jié)角的處理方式，有：

θ′6＝±acos(-(aw23sinψ+bw23cosψ+cw23))(45)

θ′4＝atan2((aw22sinψ+bw22cosψ+cw22)s6,(aw21sinψ+bw21cosψ+cw21)s6)(46)

θ′4+θ′5＝arctan((aw22sψ+bw22cψ+cw22)s6,(aw21sψ+bw21cψ+cw21)s6)(45)

θ′5＝(θ′4+θ′5)-θ′4(48)

θ′7＝atan2((aw33sinψ+bw33cosψ+cw33)s6,(aw13sinψ+bw13cosψ+cw13)s6)(49)

式(46)—式(50)中awij(i、j＝1、2、3)分別為aw矩陣中的元素，bwij(i、j＝1、2、3)分別為bw中的元素，cwij(i、j＝1、2、3)分別為cw矩陣中的元素。

綜上非偏置七自由度機械臂的逆解共有16種結(jié)果。

根據(jù)偏置機械臂與非偏置機械臂逆解的關(guān)系，可求偏置機械臂的逆解，對應(yīng)關(guān)系如下表所示。

表1偏置機械臂型與非偏置機械臂型關(guān)節(jié)解析表達式關(guān)系

綜上可得偏置機械臂的逆解為：

θ1＝θ′1

θ2＝θ′2

θ6＝θ′6

θ7＝θ′7

a＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)

b＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-

d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2

θ3＝artan(a(a3+a4c4)-ba4s4,b(a3+a4c4)+aa4s4)

θ5＝θ′3+θ′4+θ′5-θ3-θ4

θ′4、α、θ2θ4θ6分別可取兩種情況，因此偏置七自由度機械臂的逆解有32種情況。

步驟五：選擇合理的一種逆解

設(shè)偏置機械臂初始機械臂姿態(tài)角為q＝[q′1q′2q′3q′4q′5q′6q′7]，根據(jù)2-范數(shù)原理求出距初始姿態(tài)角最小的一組逆解，方法如下

式(50)中的qij(i＝1、2、3…32，j＝1、2…7)代表32種7個關(guān)節(jié)的解，qij(i＝1、2、3…32，j＝1、2…7)每組逆解關(guān)節(jié)與初始關(guān)節(jié)差的絕對值。

選出δqi最小的對應(yīng)一組解，即為所求逆解。

具體實施方法

假設(shè)七自由度機械臂的dh參數(shù)為如下表所示，機械臂初始姿態(tài)如圖6所示

表2ssrms型機械臂的d-h參數(shù)

當機械臂末端位姿為

當求逆解時應(yīng)當考慮障礙物規(guī)避問題，在末端位姿為上式的情況下，如圖7所示。

當壁型角為60°時可能有障礙物擋住機械臂，此時我們可以找到合適的壁型角規(guī)避機械臂在運動時碰觸到障礙物。即取壁型角為30°，即根據(jù)上述算法編程求解，可得到32種逆解，這時取在當前臂型變化最小的一組逆解

表332種機械臂逆解

經(jīng)編程正運動學(xué)驗證，上表32種逆解是正確的。

根據(jù)2-范數(shù)原理求得目標位姿與初始姿態(tài)變化最小的一組逆解，結(jié)果如下表所示。

表4相對于初始姿態(tài)變化最小的逆解

即逆解求出。