中正平和的機器人學筆記——2. 機械臂逆運動學（附MATAB代碼）

0. 預備知識（別跳過呀）

上一篇我們講了正運動學，也就是通過齊次變換矩陣，知曉關節角後求得機械臂末端的位姿，並表示在基座標系下。這一篇我們就來講一講逆運動學問題——已知工具座標系{T}相對於固定座標系{S}的期望位姿，如何計算一系列滿足期望要求的關節角？

0.1 求解的難度

這是一個難度更大的問題，因爲求解操作臂運動學方程是一個非線性問題。譬如六自由度的機械臂Puma560，回顧最後求得的運動學方程，

這裏我們一共有12個方程，6個未知數。旋轉部分3×3的矩陣由於每一列都是單位矢量，兩兩正交，因此這9個方程只有3個是獨立的。再加上第四列前三行位置矢量的三個方程，6個方程對應6個未知數，應當是有解的。但是其中含有sin和cos，是非線性超越方程，求解難度很大，應到考慮到解是否存在，多解以及求解方法的問題。

0.2 解的存在性

這裏引入三個概念，
工作空間（workspace）：操作臂末端的執行器所能達到的範圍。
靈巧工作空間（dexterous workspace）：機器人末端執行器能從各個方向達到的空間。
可達工作空間（reachable workspace）：機器人至少從一個方向上可以道德的空間。

從上述概念可以分析得到，如果要求解存在，那麼期望點一定在工作空間內纔行。
我們以兩連桿機械臂爲例，如果l1 = l2 = r，且關節旋轉角能達到360°，那麼其可達工作空間爲半徑爲2r的圓，靈巧工作空間爲原點；如果l1 ≠ l2，那麼可達工作空間爲外徑l1 + l2，內徑爲|l1 - l2|的圓環，不存在靈巧工作空間。
一般的，當機械臂自由度小於6時，其不能到達三維空間內一般的目標位姿。

0.3 多解問題

多解問題也很好理解，例如Puma560機械臂，對已確定目標可有8個不同解，但往往由於關節運動範圍的限制，這8個解中的某些解不能實現。如下圖所示：

0.4 解法

由於非線性方程組沒有通用的求解算法，針對解法問題，最好對一直操作臂的解的構成形式加以定義。
我們把操作臂的全部求解方法分爲兩類：封閉解（closed solutions）和數值解（numerical solutions）。
本篇主要講述封閉解形式，也可分爲兩類，代數法和幾何法。
同時我們給出一個結論：所有包含轉動關節和移動關節的串聯型6自由度機械臂均是可解的。 當然一般是數值解，只有在特殊情況下才存在封閉解（解析解），而且這種情況往往具有一些特性（存在幾個正交關節軸或者有多個α_i爲0或者90°）。根據這一特性，機械臂設計者們在設計時也往往會考慮到這一點，儘可能將工業機械臂設計得足夠簡單，從而能夠得到封閉解。

1. 代數解法

我們以三連桿平面機械臂爲例，看的小夥伴可以一起拿出紙筆算一下，首先按照上一篇正運動學的方法求解出三連桿的運動學方程：

因爲是平面機械臂，確定期望的目標位姿需要三個參數（x,y,Φ）

將上面兩個矩陣聯立，可得如下方程：

我們把第三個和第四個式子求平方再相加，利用三角函數公式可以求出cosθ₂


假定目標位置在工作空間內，那麼sinθ₂的表達式爲

應用雙變量反正切公式¹ 就可以計算出θ₂

接下來我們求θ₁，我們將原來x和y的式子改寫成如下形式
式中k₁和k₂爲

這種變換在求解逆運動學時是很常用的形式，大家可以留意。
假設

且

則有

那麼上面改寫後的x和y的式子就可以寫成如下形式

上面就是很明顯的正餘弦公式了，整理可得

那麼我們再用雙變量反正切公式就可以求出
求出θ₁和θ₂後，我們也可以由下式求出θ₃了。

2. 幾何解法

以下圖的平面機械臂爲例

由余弦定理可得

整理後可以求出cosθ₂

應用雙變量反正切公式就可以求出θ₂了。
接下來再由幾何分析可以得到，


我們就可以求出θ₁

最後由如下公式求得θ₃

3.逆運動學實例——Puma560機械臂

寫起來太麻煩啦，貼上自己計算手稿又太羞恥，字也不好看。csdn上有現成的求解步驟，大家去看吧！
如果嫌麻煩不想找就看這裏我貼的書中的求解步驟吧！





基本步驟和代數法是差不多的，所以大家可以感受到，逆運動學的求解實在是太枯燥啦（嘔）！

5. 逆運動學MATLAB的實現

mdl_puma560
%載入工具箱提供的example，建立好的puma560機械臂模型

qn = [0	0.7854	3.1416	0	0.7854	0]  %期望的關節角

T = transl(0.5, 0.5, 0.7) * rpy2tr(0, 3*pi/4, 0)
T = p560.fkine(qn)
%上面兩句都是求出正運動學的位姿，也可以直接自己給出一個期望的位姿T

qi = p560.ikine(T)
qi = p560.ikine6s(T)
%這兩句都是求解逆運動學，但是第二句適用於6自由度機械臂

%由於存在多解情況，我們可以人爲的限定期望的位型解
qi = p560.ikine6s(T, 'ru')
%'l','r'   左手/右手
%'u','d'   肘部在上/肘部在下
%'f','n'    手腕翻轉/手腕不翻轉

p560.plot(qi)

這只是最簡單的一些代碼，還有很多函數的使用方式大家可以參考《Robotics, Vision and Control》這本書。

6. 總結

以上就是機械臂的逆運動學部分講解，實際上如果你也跟着每一個例子計算以後會發現如果手算逆運動學會很麻煩（我都懶得敲公式直接貼圖了能不麻煩嗎Or2），但是得益於計算機的發展，得益於大佬們工具包做的好，我們可以看到逆運動學用matlab求解非常簡單（這裏希望大家能夠試着不用工具箱自己寫一下逆運動學的求解），但是我們不能滿足於躺在前人的成就上洋洋自得，幾行代碼寫起來簡單，但是對於我們而言，爲了真正學好機器人學，理解代碼背後的原理纔是最爲重要的，各個學科都是如此。所以也希望大家不論是學什麼，都不要眼高手低，拿出紙筆自己多寫寫畫畫，譬如BP神經網絡的原理，直接看一坨公式自然不容易理解，那麼畫一個簡單的網絡結構，自己按照公式推導一下慢慢理解就好了。

---

1. http://blog.sina.com.cn/s/blog_131fa47b20102wftq.html ↩︎