UR機械臂運動學正逆解方法

最近幾個月由於工做接觸到了機械臂的項目，忽然對機械臂運動方法產生了興趣，也就是如何控制機械臂的位置和姿態。借用一張網上的圖片，應該是ur5的尺寸。我用到的是ur3機械臂，除了尺寸不同，各關節結構和初始位置和ur5是同樣的。

ur機械臂是六自由度機械臂，由D-H參數法肯定它的運動學模型，連桿座標系的創建如上圖所示。我當時在這個地方的理解上走了很多彎路，後來找個一個視頻，我以爲講解地比較容易理解，能夠參考一下Denavit-Hartenberg參數視頻詳解。ur機械臂DH參數表以下，

轉動關節θ_i是關節變量，連桿偏移d_i是常數。

關節編號	α（繞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

由此能夠創建座標系i在座標系i-1的齊次變換矩陣，注意每次無論平移仍是旋轉是相對於當前的運動座標系變換，矩陣右乘

那麼把DH參數代入就能夠獲得全部相鄰座標系的變換矩陣

因此末端座標系6到基座固定座標系0的變換矩陣。那麼求正解就很簡單了，只要輸入六個關節角度θ_i，就獲得末端座標在基座標系的變換矩陣T。ur機械臂的視教板上末端點的座標是用六個值[x, y, z, rx, ry, rz]表示的。前三個值[x, y, z]是三維笛卡爾座標，表示空間位置，後三個值[rx, ry, rz]是座標旋轉向量，表示空間姿態。咱們獲得的變換矩陣T怎麼變成六值座標[x, y, z, rx, ry, rz]呢？設

T的左上角的3x3矩陣是旋轉矩陣，旋轉矩陣和旋轉向量之間能夠經過羅德里格斯（Rodrigues）變換進行轉換。opencv裏有相應的函數調用。算法也比較簡單，不用opencv的函數本身寫代碼也不難。T的右上角3x1就是空間位置[x, y, z]。這樣有變換矩陣T獲得六值座標，完成了正解。

逆解相對要複雜一些，由末端的空間位置和姿態，計算可能的關節角度。逆解的方法有解析法，迭代法和幾何法。其中解析法用數學推導，能夠獲得所有根，可是計算複雜。有的機械臂能夠獲得無窮解，好比7軸機械臂。而ur的6軸機械臂是有有限解的。這裏推導一下ur的逆解。

首先計算求變換矩陣T過程其中的一些中間矩陣。

，其中c₂₃=cos(θ₂+θ₃)，s₂₃=sin(θ₂+θ₃)。

由獲得。計算

，，獲得

等式兩邊矩陣的行列應該分別相等，由第三行第四列獲得，可解得，有兩個解。這裏注意寫程序的時候，求解這裏的反正切是用atan2()這類的函數，返回之在(-π,+π]。而反餘弦的返回值在[0,π]，從而保證在2π範圍每一個解是惟一的。

由第三行第三列得，可解得，兩個解。由第三行第二列獲得，可解得。

接着由

，

計算

，得出等式左邊等於

。

由，兩邊平方，令，。

一樣由，令，。

兩式相加獲得，則，有兩個解。

把θ₃帶入和，得，，其中t₂=tanθ₂。兩式消去c₂，獲得。

最後獲得，從而獲得θ₄。

綜合有兩個解的狀況，ur機械臂逆解總共由2x2x2=8組解。

按照上面的算法，用python寫了兩個程序，一個正解一個逆解驗證一下。工做手邊是ur3的機械臂，上面的圖和表都是ur5的，換成ur3的參數。正解算出來都沒有問題，能夠和實際機械臂的空間位姿對應。但是逆解算出來8組值，好像只有四組值是對的。一直還沒理解究竟是怎麼回事，仔細檢查了算法和程序好像都沒有錯阿，不知道是哪裏出了問題。網上也沒有找到答案，若是哪位大神知道，望不吝賜教！