最近幾個月因?yàn)楣ぷ鹘佑|到了機(jī)械臂的項(xiàng)目，突然對機(jī)械臂運(yùn)動方法產(chǎn)生了興趣，也就是如何控制機(jī)械臂的位置和姿態(tài)。借用一張網(wǎng)上的圖片，應(yīng)該是ur5的尺寸。我用到的是ur3機(jī)械臂，除了尺寸不一樣，各關(guān)節(jié)結(jié)構(gòu)和初始位置和ur5是一樣的。
ur機(jī)械臂是六自由度機(jī)械臂，由D-H參數(shù)法確定它的運(yùn)動學(xué)模型，連桿坐標(biāo)系的建立如上圖所示。我當(dāng)時在這個地方的理解上走了不少彎路，后來找個一個視頻，我覺得講解地比較容易理解，可以參考一下Denavit-Hartenberg參數(shù)視頻詳解。ur機(jī)械臂DH參數(shù)表如下，
轉(zhuǎn)動關(guān)節(jié)θi是關(guān)節(jié)變量，連桿偏移di是常數(shù)。
關(guān)節(jié)編號 | α（繞x軸） | a（沿x軸） | θ（繞z軸） | d（沿z軸） |
1 | α1=90 | 0 | θ1 | d1=89.2 |
2 | 0 | a2=-425 | θ2 | 0 |
3 | 0 | a3=-392 | θ3 | 0 |
4 | α4=90 | 0 | θ4 | d4=109.3 |
5 | α5=-90 | 0 | θ5 | d5=94.75 |
6 | 0 | 0 | θ6 | d6=82.5 |
由此可以建立坐標(biāo)系i在坐標(biāo)系i-1的齊次變換矩陣，注意每次不管平移還是旋轉(zhuǎn)是相對于當(dāng)前的運(yùn)動坐標(biāo)系變換，矩陣右乘
那么把DH參數(shù)代入就可以得到所有相鄰坐標(biāo)系的變換矩陣
所以末端坐標(biāo)系6到基座固定坐標(biāo)系0的變換矩陣。那么求正解就很簡單了，只要輸入六個關(guān)節(jié)角度θi，就得到末端坐標(biāo)在基坐標(biāo)系的變換矩陣T。ur機(jī)械臂的視教板上末端點(diǎn)的坐標(biāo)是用六個值[x, y, z, rx, ry, rz]表示的。前三個值[x, y, z]是三維笛卡爾坐標(biāo)，表示空間位置，后三個值[rx, ry, rz]是坐標(biāo)旋轉(zhuǎn)向量，表示空間姿態(tài)。我們得到的變換矩陣T怎么變成六值坐標(biāo)[x, y, z, rx, ry, rz]呢？設(shè)
T的左上角的3x3矩陣是旋轉(zhuǎn)矩陣，旋轉(zhuǎn)矩陣和旋轉(zhuǎn)向量之間可以通過羅德里格斯（Rodrigues）變換進(jìn)行轉(zhuǎn)換。opencv里有相應(yīng)的函數(shù)調(diào)用。算法也比較簡單，不用opencv的函數(shù)自己寫代碼也不難。T的右上角3x1就是空間位置[x, y, z]。這樣有變換矩陣T得到六值坐標(biāo)，完成了正解。
逆解相對要復(fù)雜一些，由末端的空間位置和姿態(tài)，計(jì)算可能的關(guān)節(jié)角度。逆解的方法有解析法，迭代法和幾何法。其中解析法用數(shù)學(xué)推導(dǎo)，可以得到全部根，但是計(jì)算復(fù)雜。有的機(jī)械臂可以得到無窮解，比如7軸機(jī)械臂。而ur的6軸機(jī)械臂是有有限解的。這里推導(dǎo)一下ur的逆解。
首先計(jì)算求變換矩陣T過程其中的一些中間矩陣。
，其中c23=cos(θ2+θ3)，s23=sin(θ2+θ3)。
由得到。計(jì)算
，，得到
等式兩邊矩陣的行列應(yīng)該分別相等，由第三行第四列得到，可解得，有兩個解。這里注意寫程序的時候，求解這里的反正切是用atan2()這類的函數(shù)，返回之在(-π,+π]。而反余弦的返回值在[0,π]，從而保證在2π范圍每個解是唯一的。
由第三行第三列得，可解得，兩個解。由第三行第二列得到，可解得。
接著由
，
計(jì)算
，得出等式左邊等于
。
由，兩邊平方，令，。
同樣由，令，。
兩式相加得到，則，有兩個解。
把θ3帶入和，得，，其中t2=tanθ2。兩式消去c2，得到。
最后得到，從而得到θ4。
綜合有兩個解的情況，ur機(jī)械臂逆解總共由2x2x2=8組解。
按照上面的算法，用python寫了兩個程序，一個正解一個逆解驗(yàn)證一下。工作手邊是ur3的機(jī)械臂，上面的圖和表都是ur5的，換成ur3的參數(shù)。正解算出來都沒有問題，可以和實(shí)際機(jī)械臂的空間位姿對應(yīng)?？墒悄娼馑愠鰜?組值，好像只有四組值是對的。一直還沒理解到底是怎么回事，仔細(xì)檢查了算法和程序好像都沒有錯阿，不知道是哪里出了問題。網(wǎng)上也沒有找到答案，如果哪位大神知道，望不吝賜教！
