MATLAB 在 2016 年就推出了 Robotics System Toolbox(RST)，其中有很多關于機械臂方面的算法。而且隨著客戶需求的增加，也在加入一些新的功能。為了試圖讓讀者了解更多 RST 在機械臂方面的支持，讓我們來看一下機械臂方面的算法概貌。

這些名詞聽起來都比較深奧，但是在機械臂的世界里，這些都非常有用。

讓我們看一個簡單的例子。下圖是一個簡單的機械臂示意：機械臂的 end-effector（末端機構）受到 4 個旋轉關節和 3 個連桿的共同作用，可以到達不同的作業地點，也可以處于不同的旋轉角度。

為了分析 end-effector 的具體位置和角度，我們看到：

它相對底座開始，做了 4 次旋轉（rotation）和 3 次轉置（translation）。那這 4 此旋轉和 3 此轉置的總和，我們可以用一個矩陣來表示：

這個矩陣也叫 Homogeneous Transformation（齊次變換）。

有時候，對于旋轉會有不同的表達方式，例如歐拉角（Euler Angles）、四元素（Quaternion）、旋轉矩陣（Rotation Matrix）等等；表達轉置，也可使用轉置向量（Translation Vector）。有了 RST 這些都可以輕松通過不同的函數進行互換。下圖為具體的函數列表：

例如：將歐拉角轉為Homogeneous Transformation：

>> eul = [0 pi/2 0];

tformZYX = eul2tform(eul)

tformZYX =

0.0000 0 1.0000 0

0 1.0000 0 0

-1.0000 0 0.0000 0

0 0 0 1.0000

由于機械臂的連桿長度是已知的，只要確定了各個關節轉動的角度，我們就可以確定 end-effector 的最終位置和方向。這個我們稱之為 forward kinematics（正向運動學）。反過來，如果我們知道了 end-effector 的最終位置和方向，我們也可以推導各個關節的角度，這個我們稱之為 inverse kinematics（反向運動學）。

機械臂關注的主要是反向運動學。

如果 end-effector，需要走一段比較長的路程(path)，從甲點運行到乙點。我們為了使得機械臂的 end-effector 的路徑平滑，需要規劃一系列的路徑點（waypoints），這個我們叫做路徑規劃（trajectory planning）或者叫運動插補（interpolation）。例如下圖，藍色的曲線叫 path，而各個時間經過的路徑點叫 trajectory。如何設計經過這些路徑點的 trajectory，比較顯而易見的指標是 “平滑” 。那什么是“平滑” ，它可能意味著 “速度連續” 、“加速度連續” 、 “沒有頓挫” 等等。這些指標，都會轉化成數學算法。 RST 也會有相應的算法支持，作者將另外寫文章描述。

機械臂的關節位置我們一般用電機來驅動。電機通過產生力矩來轉動機械裝置，驅動機械臂。不同場合或者時機，需要的力矩不盡相同。

例如，機械臂水平放置的時候需要關節電機產生力矩來抵消地球引力；當機械臂需要迅速移動的時候，需要的力矩比緩慢移動的要大，當機械臂彎曲或者平展時候，重心發生變化，由于慣量（I = mr2）的不同，需要的關節力矩也不相同；另外，在很多場合，機械臂需要和人交互（collaborative robots），在碰到人體的時候，需要做出安全的保護動作，并對力矩進行調整。

這些需要考慮力矩的因素，我們稱之為動力學（dynamics）。和運動學類似，動力學分為正向動力學（forward dynamics）和反向運動學(inverse dynamics)。 RST 里支持兩種都有相應的 MATLAB 函數和Simulink block。作者也會另外寫文章詳細介紹 RST 關于動力學的部分。

運動學

1. Rigid Body Tree （剛體樹）

我們說研究運動學（主要是反向運動學），就是研究 end-effector 的位置改變會帶動各個關節的角度如何改變。RST 用 Rigid Body Tree 這樣一個對象，在這個對象上可以使運動學設計易用且可視化。下圖展示了機械臂的剛體樹樣例，可以在 MATLAB 界面中展示各個 body 的詳細參數。

一般來說，Rigid Body Tree 都是直接從機械臂的 CAD 文件或者 URDF（Unified Robot Description Format）文件導入。不過，也支持每個 body 的逐步添加。

我們隨便敲幾行 MATLAB 命令：

robot = importrobot('iiwa14.urdf');

show(robot);

讓我們來改變一下機器人的各個關節角度（configuration），比如讓MATLAB自動給一個隨機角度配置，再看一下結果。顯然各個角度發生了變化。

q=randomConfiguration(robot);

show(robot,q);

我們看看這個機械臂最末端的 end-effector 是什么？

showdetails(robot)

我們再看看 end-effector 相對機器人底座(base)的 Homogeneous Transformation(相對位置和角度)。

2. 反向運動學算法

反向運動學算法求解分兩種：一種是分析解法（Analytic solutions）；一種是數值解法（Numerical solutions）。

MATLAB 用的是數值解法，可以理解為迭代尋優，或者近似解。

MATLAB 里面的反向運動學求解器(solver)有兩個：

Inverse Kinematics

Generalized Inverse Kinematics

兩者的區別是，后者比前者多了很多限制(constraints) 。例如end-effector的方向限制、機械臂各個關節的角度限制、位置限制等等。

我們先看一下比較簡單的 Inverse Kinematics：

這是一個 6 軸機器人，end-effector 是 L6。

我們想要的最終結果就是下圖：

tform 是 L6 相對 base 的位置和方向（合稱 pose）。

下面的 MATLAB 代碼是計算出最終的各個關節的角度（configSoln），由于是用了迭代的數值解法，weights 為權重，initialguess 為給出一個初始估計。

我們再看一下比較復雜的 Generalized Inverse Kinematics:

下面的代碼，做了這么幾件事情：

導入了一個 7 自由度的 rethink 機械臂— sawyer

設定反向運動學的求解限制— 例如機械臂的 end-effector 永遠指向地面的一個物體

對反向運動學進行求解

如果我們加一段 end-effector 位置變化后，調用這段代碼的動畫效果，你會發現 end-effector 的指向沒有變化 – 帶限制的反向動力學求解成功了：

3. Simulink 示例

在安裝 RST 之后，Simulink 的 library 里就會出現幾個和機械臂（manipulator）相關的 block：

其中 Inverse Kinematics 就是反向運動學 block，其他的一些模塊顧名思義和動力學有關，在下一篇文章我會重點介紹。

在 MATLAB Central File Exchange 上搜索 “Designing Robot Manipulator Algorithms”，這是一個基于 Simulink 和 Stateflow 的例子。我們先看一下運行結果：

這個例子展現了機械臂的 end-effector 抓了紅色物體，沿著規劃好的紫色 trajectory，進行運動。

下圖的 stateflow 狀態機是一個 trajectory tracking 的算法，它的作用是確保 end-effctor 沿著預設的 trajectory 運行。

狀態機下面的是運動控制部分和環境和物理模型。運動控制很簡單 – 直接計算反向運動學，將算好的關節角度交給物理模型去展現。物理模型構建也很簡單—— 用 SimScape 中的 SimMultibody 直接導入機械臂的 URDF 文件即可。

這里可以看到物理模型并沒有包含伺服電機，而是“透明傳輸”— 反向運動學的結果直接發給了機械模型去展現。實際上真實的運動控制器會將位置、速度、力矩指令通過伺服總線（例如 EtherCAT）發給每個關節的電機去執行，電機通過減速器去帶動機械結構。例如，一個 6 軸機械臂會有 6 個伺服電機，運動控制器會將運動過程解析為 6 個電機可以理解的位置、速度、力矩指令。