一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法
【專利摘要】本發(fā)明公開了一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法，包括如下步驟：S1、獲得基座至執(zhí)行末端的全部位姿變換矩陣Mi；S2、根據(jù)所述位姿變換矩陣Mi建立各個(gè)關(guān)節(jié)的位姿方程Wi；S3、根據(jù)所述Wi得到各個(gè)關(guān)節(jié)的位姿矩陣的微變換dWi；S4、根據(jù)所述dWi得到各個(gè)關(guān)節(jié)位姿矩陣的微分平移變量di與微分旋轉(zhuǎn)變量δi；其中：di＝dxi+dyj+dzk；δi＝δxi+δyj+δzk；(i，j，k)為沿坐標(biāo)系x，y，z軸方向的單位方向向量；S5、根據(jù)所述δx、δy和δz判斷各個(gè)關(guān)節(jié)相對(duì)于x，y，z軸的旋轉(zhuǎn)方向。上述方法，可以方便地選取符合要求的關(guān)節(jié)角度，一定程度上節(jié)省了時(shí)間，簡(jiǎn)化了計(jì)算過(guò)程。
【專利說(shuō)明】
一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域，特別涉及一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解 方法。
【背景技術(shù)】
[0002] 對(duì)應(yīng)于機(jī)械手末端在空間的某一位置，其各關(guān)節(jié)在空間中的位置變換可以有多組 不同的狀態(tài)，實(shí)際上機(jī)械手在運(yùn)動(dòng)過(guò)程中只能選取一個(gè)位置，其受到關(guān)節(jié)角轉(zhuǎn)角有范圍的 限制，同時(shí)在機(jī)械手跟蹤折彎?rùn)C(jī)板材運(yùn)動(dòng)軌跡時(shí)，解的選擇要符合關(guān)節(jié)角變化平緩以及減 少機(jī)器人在工作過(guò)程中角度突變的情況，更要減少關(guān)節(jié)運(yùn)動(dòng)的時(shí)間，滿足目標(biāo)關(guān)節(jié)角與當(dāng) 前關(guān)節(jié)角盡可能接近的原則。
[0003] 然而在實(shí)際情況中，由于解的復(fù)雜性，并不一定能夠很容易的選取合適的解，滿足 上述要求的限制。比如，有些情況是可能存在有兩個(gè)相同大小，但是正負(fù)（方向）不同的逆 解，雖然從數(shù)值角度符合要求，但是不能隨機(jī)確定一組合適的關(guān)節(jié)角的解，所以此時(shí)需要一 種能夠易于直接判斷選取關(guān)節(jié)角的方法。
[0004] 機(jī)器人的控制需要唯一的確切解，若不考慮避障要求，可以按照以下步驟獲取唯 一的解，首先:若關(guān)節(jié)角的求解表達(dá)式的值域不能完全包含該關(guān)節(jié)運(yùn)動(dòng)范圍，則在必要的條 件下可能需要將求得的關(guān)節(jié)角加減360度，補(bǔ)出關(guān)節(jié)運(yùn)動(dòng)范圍以外的其他可能解;其次，剔 除關(guān)節(jié)運(yùn)動(dòng)范圍以外的可能解;最后，有運(yùn)動(dòng)的連續(xù)性，選取于該關(guān)節(jié)角上一時(shí)刻的值距離 最近的可能解為唯一的最優(yōu)解。因此，各關(guān)節(jié)角的求解順序?yàn)?先確定第一角度的唯一最優(yōu) 解，再利用第一角度的逆解求取和多解處理步驟求取第二角度的最優(yōu)解，依次類推求解其 余關(guān)節(jié)角，最后取得的一組解是目標(biāo)關(guān)節(jié)角。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的是提供一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法，該方法可以 解決求解運(yùn)動(dòng)角度較為復(fù)雜的問題。
[0006] 為實(shí)現(xiàn)上述目的，本發(fā)明提供一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法，包 括如下步驟：
[0007] S1、獲得基座至執(zhí)行末端的全部位姿變換矩陣Mi;
[0008] S2、根據(jù)所述位姿變換矩陣Mi建立各個(gè)關(guān)節(jié)的位姿方程Wi;
[0009] S3、根據(jù)所述Wi得到各個(gè)關(guān)節(jié)的位姿矩陣的微變換dWi;
[0010] S4、根據(jù)所述dWi得到各個(gè)關(guān)節(jié)位姿矩陣的微分平移變量di與微分旋轉(zhuǎn)變量Si;
[0011] 其中：di = dxi+dyj+dzk; 5i = 5xi+5yj+5zk;
[0012] (1，」，1〇為沿坐標(biāo)系1，7,2軸方向的單位方向向量；
[0013 ] S5、根據(jù)所述5X、和5Z判斷各個(gè)關(guān)節(jié)相對(duì)于x，y，z軸的旋轉(zhuǎn)方向。
[0014]相對(duì)于上述【背景技術(shù)】，本發(fā)明提供的運(yùn)動(dòng)關(guān)節(jié)角求解方法，針對(duì)折彎?rùn)C(jī)在進(jìn)行板 材折彎的過(guò)程中，機(jī)械手末端是緩慢變化這一特征，引用微分平移變量cU與微分旋轉(zhuǎn)變量心 來(lái)描述各關(guān)節(jié)的位姿變換矩陣Mi，計(jì)算得到的位姿方程Wi以及微變換dWi能更加直觀的表達(dá) 各個(gè)關(guān)節(jié)直接所對(duì)應(yīng)的運(yùn)動(dòng)關(guān)系，并通過(guò)各關(guān)節(jié)的位姿變換矩陣1判斷所應(yīng)選取的折彎?rùn)C(jī) 機(jī)械手運(yùn)動(dòng)關(guān)節(jié)角的值。采用本發(fā)明涉及的求解方法，能夠直觀的體現(xiàn)出折彎?rùn)C(jī)機(jī)械手折 彎板材運(yùn)動(dòng)過(guò)程中各運(yùn)動(dòng)關(guān)節(jié)角運(yùn)動(dòng)變化的趨勢(shì)和轉(zhuǎn)動(dòng)的方向，從而方便地選取符合要求 的關(guān)節(jié)角度，一定程度上節(jié)省了時(shí)間，簡(jiǎn)化了計(jì)算過(guò)程。
[0015] 優(yōu)選地，所述步驟S1中，根據(jù)D-H法獲得所述位姿變換矩陣I。
[0016] 優(yōu)選地，所述步驟S2中，建立所述各個(gè)關(guān)節(jié)的位姿方程1的方式具體包括：
[0017] ffi=ff=MiXM2X---Mi；
[0018] Wi-^ffiXMr1；
[0019] 2=1-jMi-i-S
[0020] Wi-3=ffi-2XMiV1；
[0021] 1
[0022] Wi-n=Wi-n+1 X Mi-柯―1;其中，n = l、2、……i_l;
[0023] 主
[0024] ffi=ff2XM2_1〇
[0025]優(yōu)選地，所述步驟S3中，所述各個(gè)關(guān)節(jié)的位姿矩陣的微變換dWi具體為：dWi = WiX [_I+T(dx，dy，dz) XR(f，d9)]，
[0026]其中 7V/x，c(v'，c/z) 為三階單位矩陣，達(dá)，辦
[0028] 優(yōu)選地，所述步驟S4中，根據(jù)所述dWi得到各個(gè)關(guān)節(jié)位姿矩陣的微分平移變量cU與 微分旋轉(zhuǎn)變量~的具體步驟包括：
[0029] 設(shè)定f xd9 = 5x，f yd9 = 5y，f zd9 = 5z; rr: 勒 _〇]得到-i+7Vfc辦，必）x=|言g；
[0031] 其中：
[0032] 優(yōu)選地，所述步驟S5中，根據(jù)所述Sx、Sy和Sz判斷各個(gè)關(guān)節(jié)分別朝向 x，y，z軸的哪一 方向旋轉(zhuǎn)的具體步驟包括：
[0033]當(dāng)Sx>〇時(shí)，則該關(guān)節(jié)繞X軸方向旋轉(zhuǎn)；
[0034]當(dāng)Sx<〇時(shí)，則該關(guān)節(jié)繞X軸相反方向旋轉(zhuǎn)；
[0035]當(dāng)Sy > 0時(shí)，則該關(guān)節(jié)繞Y軸方向旋轉(zhuǎn)；
[0036] 當(dāng)Sy<〇時(shí)，則該關(guān)節(jié)繞Y軸相反方向旋轉(zhuǎn)；
[0037] 當(dāng)^>〇時(shí)，則該關(guān)節(jié)繞Z軸方向旋轉(zhuǎn)；
[0038]當(dāng)Sz<〇時(shí)，則該關(guān)節(jié)繞Z軸相反方向旋轉(zhuǎn)。
【附圖說(shuō)明】
[0039] 為了更清楚地說(shuō)明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn) 有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本 發(fā)明的實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù) 提供的附圖獲得其他的附圖。
[0040] 圖1為本發(fā)明實(shí)施例所提供的機(jī)器人D-H表示法參考坐標(biāo)系的示意圖；
[0041 ]圖2為本發(fā)明實(shí)施例所提供的運(yùn)動(dòng)關(guān)節(jié)角求解方法的流程圖。
【具體實(shí)施方式】
[0042]下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完 整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；?本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他 實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。
[0043]為了使本技術(shù)領(lǐng)域的技術(shù)人員更好地理解本發(fā)明方案，下面結(jié)合附圖和具體實(shí)施 方式對(duì)本發(fā)明作進(jìn)一步的詳細(xì)說(shuō)明。
[0044] 請(qǐng)參考圖1和圖2,圖1為本發(fā)明實(shí)施例所提供的機(jī)器人D-H表示法參考坐標(biāo)系的示 意圖；圖2為本發(fā)明實(shí)施例所提供的運(yùn)動(dòng)關(guān)節(jié)角求解方法的流程圖。
[0045] 本發(fā)明提高的一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法，主要包括如下步 驟：
[0046] S1、獲得基座至執(zhí)行末端的全部位姿變換矩陣M1;
[0047] S2、根據(jù)所述位姿變換矩陣Mi建立各個(gè)關(guān)節(jié)的位姿方程Wi;
[0048] S3、根據(jù)所述Wi得到各個(gè)關(guān)節(jié)的位姿矩陣的微變換dWi;
[0049] S4、根據(jù)所述dWi得到各個(gè)關(guān)節(jié)位姿矩陣的微分平移變量di與微分旋轉(zhuǎn)變量心；
[0050] 其中：di = dxi+dyj+dzk; 5i = 5xi+5yj+5zk;
[0051 ] (i，j，k)為沿坐標(biāo)系x，y，z軸方向的單位方向向量；
[0052 ] S5、根據(jù)所述5X、和心判斷各個(gè)關(guān)節(jié)分別朝向x，y，z軸的哪一方向旋轉(zhuǎn)。
[0053]本發(fā)明可以先設(shè)定折彎?rùn)C(jī)機(jī)械手末端在折彎開始點(diǎn)時(shí)處于折彎坐標(biāo)系下的位姿 值Wq(xq，yo，zo，rao，rbQ，rcQ);再設(shè)定折彎?rùn)C(jī)機(jī)械手末端在折彎跟隨過(guò)程當(dāng)中處于折彎坐標(biāo) 系下的實(shí)時(shí)位置姿態(tài)值W(x，y，z，ra，rb，rc)，其中：x，y，z為設(shè)定坐標(biāo)系下的沿各方向的直 線距離，ra，rb，rc為繞坐標(biāo)系旋轉(zhuǎn)的各個(gè)角度值。
[0054]根據(jù)齊次變換矩陣D-H坐標(biāo)法，對(duì)機(jī)器人建立如說(shuō)明書附圖1所示的坐標(biāo)系，它是 以6個(gè)關(guān)節(jié)軸為原點(diǎn)建立的坐標(biāo)系。依照D-H表示法原理，可以得到相鄰坐標(biāo)系間的變換矩 陣，分別記為:11屬，1? 4^6。當(dāng)然，根據(jù)實(shí)際情況，關(guān)節(jié)軸的個(gè)數(shù)可以根據(jù)實(shí)際需要而 定，并不限于本文所述。
[0055]基于說(shuō)明書附圖1所示，步驟Sl;Mi為第一連桿對(duì)基座標(biāo)的位姿變換矩陣;M2為第二 連桿對(duì)第一連桿的位姿變換矩陣，M3為第三連桿對(duì)第二連桿的位姿變換矩陣，M4為第四連桿 對(duì)第三連桿的位姿變換矩陣，M 5為第五連桿對(duì)第四連桿的位姿變換矩陣，M6為第六連桿對(duì)第 五連桿的位姿變換矩陣。
[0056] 然后進(jìn)行步驟S2;根據(jù)所述位姿變換矩陣姐建立各個(gè)關(guān)節(jié)的位姿方程I，并根據(jù)所 述1得到各個(gè)關(guān)節(jié)的位姿矩陣的微變換dWi(步驟S3)，而后在步驟S4中，求解各個(gè)關(guān)節(jié)位姿 矩陣的微分平移變量cU與微分旋轉(zhuǎn)變量5 1;將微分平移變量cU與微分旋轉(zhuǎn)變量~分別表示 為如下形式；
[0057] di = dxi+dyj+dzk；8i = 8xi+8yj+8zk；
[0058] (i，j，k)為沿坐標(biāo)系x，y，z軸方向的單位方向向量；
[0059] 通過(guò)cU能夠得知各個(gè)關(guān)節(jié)角度的平移量，而通過(guò)\獲得各個(gè)關(guān)節(jié)角度的旋轉(zhuǎn)量。
[0060] 通過(guò)步驟S5、根據(jù)5x、5y和^判斷各個(gè)關(guān)節(jié)分別朝向x，y，z軸的哪一方向旋轉(zhuǎn)。這樣 便能夠在獲知所需旋轉(zhuǎn)角度的前提下，還能夠明確得知關(guān)節(jié)角度的旋轉(zhuǎn)方向，從而方便地 選取符合要求的關(guān)節(jié)角度，一定程度上節(jié)省了時(shí)間，簡(jiǎn)化了計(jì)算過(guò)程。
[0061] 針對(duì)各個(gè)關(guān)節(jié)的位姿方程I以及各個(gè)關(guān)節(jié)的位姿矩陣的微變換dWi的計(jì)算過(guò)程，本 發(fā)明給出如下【具體實(shí)施方式】。
[0062] 為了得到折彎?rùn)C(jī)機(jī)械手各個(gè)關(guān)節(jié)位姿方程％~％，可以利用矩陣W6左乘MrT1，如下 所示：
[0063] ffi=ff=MiXM2X---Mi；
[0064] Wi-^ffiXMr1；
[0065] I-2=1-jMi-1-1;
[0066] Wi-3=ffi-2XMiV1；
[0067] i
[0068] Wi-n=Wi-n+1 X Mi-柯―1;其中，n = 1、2、……i -1;
[0069] \
[0070] ffi=ff2XM2_1〇
[0071] 本發(fā)明中，i = 6,則有：
[0072] ff6=ff=MiXM2X---M6；
[0073] W5=W6 X M6-1=Mi X M2 X M3 X M4 X Ms;
[0074] ff4=ff5XM5_1=MiXM2XM3XM4；
[0075] ff3=ff4XM4_1=MiXM2XM3；
[0076] ff2=ff3XM3_1=MiXM2
[0077] Wi=W2 XM2-1=Mi。
[0078]通過(guò)以上6個(gè)公式可以建立各關(guān)節(jié)的坐標(biāo)微分平移和微分旋轉(zhuǎn)矩陣關(guān)系表達(dá)式；
[0079] dffi=ffiX [-I+T(dx,dy,dz) XR(f ,d9)],
[0080] 其中，可設(shè)A = -I+T(dx,dy ,dz) XR(f,d9)，則dWi = A XWi，
[0081] A = -I+T(dX，dy，dz)XR(f，d0)為折彎?rùn)C(jī)機(jī)械手各個(gè)關(guān)節(jié)位姿矩陣的微變換。
[0082] 1為與矩陣1'((11，(17，(12)\1?(:^(10)同階的單位矩陣，1?(;^(10)表示繞某一關(guān)節(jié)軸線 f旋轉(zhuǎn)變化d0的矩陣;T(dx，dy，dz)表示沿x，y，z平移的距離的變換矩陣。
[0083] 針對(duì) A = -I+T(dx，dy，dz)XR(f，d9);
[0084] 微分平移矩陣為;敘辦y;
[0085] 其中，E3為三階單位矩陣，淺=(辦,辦,￡/2) 7；： I I 祕(mì)恭邏1
[0086] % i 二g&\ i j
[0087] 設(shè)定 fxd0 = 5x，fydQ = 5y，fzdQ = 5z; (產(chǎn). Hj; I § -：& % I
[0089]其中：Cs=i丨龜 0 ~~：&!!, ^^dxydy,dz)T〇 1% & § I
[0090]采用上述方式得到 di = dxi+dy j+dzk; 5i = 5xi+5y j+5zk;
[0091 ]且di為微分平移變量與\為微分旋轉(zhuǎn)變量；
[0092] (i，j，k)為沿坐標(biāo)系x，y，z軸方向的單位方向向量；
[0093 ]可以根據(jù)所述5X、和^判斷各個(gè)關(guān)節(jié)分別朝向x，y，z軸的哪一方向旋轉(zhuǎn)的具體步 驟包括：
[0094]當(dāng)Sx>〇時(shí)，則該關(guān)節(jié)繞X軸方向旋轉(zhuǎn)；
[0095]當(dāng)Sx<〇時(shí)，則該關(guān)節(jié)繞X軸相反方向旋轉(zhuǎn)；
[0096]當(dāng)Sy>〇時(shí)，則該關(guān)節(jié)繞Y軸方向旋轉(zhuǎn)；
[0097]當(dāng)Sy<〇時(shí)，則該關(guān)節(jié)繞Y軸相反方向旋轉(zhuǎn)；
[0098]當(dāng)^>〇時(shí)，則該關(guān)節(jié)繞Z軸方向旋轉(zhuǎn)；
[0099]當(dāng)心<〇時(shí)，則該關(guān)節(jié)繞Z軸相反方向旋轉(zhuǎn)。
[0100]通過(guò)上述可知，根據(jù)折彎?rùn)C(jī)機(jī)械手板材折彎的變化過(guò)程，各個(gè)關(guān)節(jié)位姿矩陣Wi，w2， ^4，15和16變化前后的關(guān)系可以表示為12=1"1 +八\1"，其中12為關(guān)節(jié)微分變化后的矩 陣，Wml為關(guān)節(jié)微分變化前的矩陣；
[0101] A XWm = Wm2_Wmi=Wmi X [_I+T(dx，dy，dz) XR(f，d9)]，并上述根據(jù) 7X敘辦心）求得各個(gè)關(guān)節(jié)所對(duì)應(yīng)的A值。
[0102]根據(jù)上文所述，可以得到各關(guān)節(jié)對(duì)應(yīng)的坐標(biāo)變換和角度微分變換：相應(yīng)的值的正 負(fù)情況，從而判斷每一關(guān)節(jié)在微分運(yùn)動(dòng)變化的時(shí)候的旋轉(zhuǎn)方向，繼而求得各關(guān)節(jié)角度變化 程度(在微分旋轉(zhuǎn)之前的角度的基礎(chǔ)上減還是加一定的角度），這樣子在解各運(yùn)動(dòng)關(guān)節(jié)多重 解的過(guò)程中可以有助于判斷和選取理想的最優(yōu)解，減少了排除所有逆解都要比較選取的情 況，提尚效率。
[0103] 利用di = dxi+dyj+dzk和5i = 5xi+5yj+5z相應(yīng)值的正負(fù)情況，從而判斷每一關(guān)節(jié)在微 分運(yùn)動(dòng)變化的時(shí)候的旋轉(zhuǎn)方向，從而繼而求得各關(guān)節(jié)角度變化程度(在微分旋轉(zhuǎn)之前的角 度的基礎(chǔ)上減還是加一定的角度），這樣便能夠在解各運(yùn)動(dòng)關(guān)節(jié)多重解的過(guò)程中可以有助 于判斷和選取理想的最優(yōu)解。
[0104]需要說(shuō)明的是，在本說(shuō)明書中，諸如第一和第二之類的關(guān)系術(shù)語(yǔ)僅僅用來(lái)將一個(gè) 實(shí)體或步驟與另外幾個(gè)實(shí)體或步驟區(qū)分開來(lái)，而不一定要求或者暗示這些實(shí)體或步驟之間 存在任何這種實(shí)際的關(guān)系或者順序。
[0105]以上對(duì)本發(fā)明所提供的基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法進(jìn)行了詳細(xì)介 紹。本文中應(yīng)用了具體個(gè)例對(duì)本發(fā)明的原理及實(shí)施方式進(jìn)行了闡述，以上實(shí)施例的說(shuō)明只 是用于幫助理解本發(fā)明的方法及其核心思想。應(yīng)當(dāng)指出，對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員 來(lái)說(shuō)，在不脫離本發(fā)明原理的前提下，還可以對(duì)本發(fā)明進(jìn)行若干改進(jìn)和修飾，這些改進(jìn)和修 飾也落入本發(fā)明權(quán)利要求的保護(hù)范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于折彎?rùn)C(jī)機(jī)械手的運(yùn)動(dòng)關(guān)節(jié)角求解方法，其特征在于，包括如下步驟： 51、 獲得基座至執(zhí)行末端的全部位姿變換矩陣M1; 52、 根據(jù)所述位姿變換矩陣姐建立各個(gè)關(guān)節(jié)的位姿方程W1; 53、 根據(jù)所述^得到各個(gè)關(guān)節(jié)的位姿矩陣的微變換dW1; 54、 根據(jù)所述dWi得到各個(gè)關(guān)節(jié)位姿矩陣的微分平移變量cU與微分旋轉(zhuǎn)變量δ?; 其中：di = dxi+dyj+dzk;5i = 5xi+5yj+5zk; (1」_，1〇為沿坐標(biāo)系1，7,2軸方向的單位方向向量； 55、 根據(jù)所述53^、5\和52判斷各個(gè)關(guān)節(jié)相對(duì)于x，y，z軸的旋轉(zhuǎn)方向。2. 根據(jù)權(quán)利要求1所述的運(yùn)動(dòng)關(guān)節(jié)角求解方法，其特征在于，所述步驟Sl中，根據(jù)D-H法 獲得所述位姿變換矩陣M1。3. 根據(jù)權(quán)利要求1所述的運(yùn)動(dòng)關(guān)節(jié)角求解方法，其特征在于，所述步驟S2中，建立所述 各個(gè)關(guān)節(jié)的位姿方程^的方式具體包括： Wi=W=Mi X M2 X - --Mi; Wi-I=WiXMr1; Wi-2=Wi-i X Mi-1-S ffi-3 = ffi-2XMi-2_1； .V Λ>· Wi-η=Wi-η+ι X Mi-η+Γ1;其中，η=1、2、……i-1； :#.· ：Λ· Α· Wi=W2XM2'4. 根據(jù)權(quán)利要求1至3任意一項(xiàng)所述的運(yùn)動(dòng)關(guān)節(jié)角求解方法，其特征在于，所述步驟S3 中，所述各個(gè)關(guān)節(jié)的位姿矩陣的微變換dWi具體為:dWi=W i X [-I+T(dx，dy，dz) X R(f，d0)]，5. 根據(jù)權(quán)利要求4所述的運(yùn)動(dòng)關(guān)節(jié)角求解方法，其特征在于，所述步驟S4中，根據(jù)所述 dWi得到各個(gè)關(guān)節(jié)位姿矩陣的微分平移變量di與微分旋轉(zhuǎn)變量31的具體步驟包括：6. 根據(jù)權(quán)利要求5所述的運(yùn)動(dòng)關(guān)節(jié)角求解方法，其特征在于，所述步驟S5中，根據(jù)所述 δχ、Sy和52判斷各個(gè)關(guān)節(jié)分別朝向X，y，Z軸的哪一方向旋轉(zhuǎn)的具體步驟包括： 當(dāng)δχ > O時(shí)，則該關(guān)節(jié)繞X軸方向旋轉(zhuǎn)； i3x<0時(shí)，則該關(guān)節(jié)繞X軸相反方向旋轉(zhuǎn)； 當(dāng)δγ > O時(shí)，則該關(guān)節(jié)繞Y軸方向旋轉(zhuǎn)； 當(dāng)3y<0時(shí)，則該關(guān)節(jié)繞Y軸相反方向旋轉(zhuǎn)； 當(dāng)32> O時(shí)，則該關(guān)節(jié)繞Z軸方向旋轉(zhuǎn)； 當(dāng)32<〇時(shí)，則該關(guān)節(jié)繞Z軸相反方向旋轉(zhuǎn)。
【文檔編號(hào)】B25J9/16GK106003055SQ201610653080
【公開日】2016年10月12日
【申請(qǐng)日】2016年8月10日
【發(fā)明人】陸嘯天, 王欽若
【申請(qǐng)人】廣東工業(yè)大學(xué)