最近有個任務：求解UR機械臂正逆運動學，在網上參考了一下大家的求解辦法，眾說紛紜，其中有些朋友求解過程非常常規，但是最後求解的8組解，只有4組可用。在這裡我介紹一個可以求解8組解析解的方法，供大家參考。

       以UR5機械臂結構和尺寸引數為例進行正逆運動學求解，下圖分別是UR5結構圖和標準DH系引數：

1. 正運動學求解

正運動學是已知關節六個角度求變換矩陣T

其中：

整理得：

帶入DH引數，求解：

        

最終變換矩陣：

正運動學求解完畢。

2. 逆運動學求解

逆運動學是已知變換矩陣T，求六個關節角度 。逆運動學求解有解析法，幾何法，迭代法，這裡採用解析法求解。

2.1 兩個簡單的數學方法

2.1.1 求角度

 這個逆運動學演算法求解的角度範圍是

因為標準的反正切arctan的值域是

所以不能使用，這裡介紹一個改進的反正切求法 Atan2(y, x)（Matlab裡有這個函式）,它的值域可以滿足要求。

2.1.2 解方程

首先進行三角恆等變換，令

其中：

然後帶入原方程：

則

2.2 約定

為了簡化書寫，約定：

2.3 求解1，5，6關節角度

已知：

則

其中：

等式左邊：

等式右邊：

2.3.1 求關節角1

利用等式左右兩邊第3行，第4列對應相等求關節角1。

整理得：

設：

則  

根據前面介紹的解方程的方法：

2.3.2 求關節角5

利用等式左右兩邊第3行，第3列對應相等求關節角5。

解得：

2.3.3 求關節角6

利用等式左右兩邊第3行，第1列對應相等求關節角6。

設：

則

根據前面介紹的方法：

其實可以通過化簡得到式中

則

2.4 求解2，3，4關節角度

已知：

則

其中：

等式左邊等於

等式右邊等於

2.4.1 求解關節角3

利用等式左右兩邊第1行，第4列對應相等，第2行，第4列對應相等，求關節角3。

為了簡化，設：

將m,n帶入上式得

式子③④平方和為

因為

所以

2.4.2  求解關節角2

將③④展開得：

將關節角3帶入⑤⑥，求關節角2得

則

2.4.3  求解關節角4

用的第2行第2列，第1行第2列求 

則

2.5 總結

2.5.1 求解公式

2.5.2 奇異位置

1.肩關節奇異位置

此時末端執行器參考點O6位於軸線z1和z2構成的平面內，關節角1無法求解。

2.肘關節奇異位置

此時關節角2無法求解。

3.腕關節奇異位置

此時軸線z4和z6平行，關節角6無法求解。

2.6 例項

利用Matlab機器人庫 ur5 DH引數：

alpha1 = pi/2;          a1=0;                 d1=89.459;

alpha2 = 0;              a2=-425;           d2=0;

alpha3 = 0;              a3=-392.25;      d3=0;

alpha4 = pi/2;           a4=0;               d4=109.15;

alpha5 = -pi/2;          a5=0;               d5=94.65;

alpha6 = 0;               a6=0;               d6=82.3;

取 theta1 = 1;  theta2 = 1;  theta3 = 1;  theta4 = 1;  theta5 = 1;  theta6 = 1;  (不要糾結theta選這6個數值是否有實際意義，這裡只驗證演算法的有效性)

• 將theta帶入正運動學公式，求T:

• 將T帶入逆運動學公式, 反求theta

theta = 

• 再將8個theta帶入正運動學公式，反求8個T:

8個T均等於

驗證了演算法的有效性