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

這些名詞聽(tīng)起來(lái)都比較深?yuàn)W，但是在機(jī)械臂的世界里，這些都非常有用。

讓我們看一個(gè)簡(jiǎn)單的例子。下圖是一個(gè)簡(jiǎn)單的機(jī)械臂示意：機(jī)械臂的 end-effector（末端機(jī)構(gòu)）受到 4 個(gè)旋轉(zhuǎn)關(guān)節(jié)和 3 個(gè)連桿的共同作用，可以到達(dá)不同的作業(yè)地點(diǎn)，也可以處于不同的旋轉(zhuǎn)角度。

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

它相對(duì)底座開(kāi)始，做了 4 次旋轉(zhuǎn)（rotation）和 3 次轉(zhuǎn)置（translation）。那這 4 此旋轉(zhuǎn)和 3 此轉(zhuǎn)置的總和，我們可以用一個(gè)矩陣來(lái)表示：

這個(gè)矩陣也叫 Homogeneous Transformation（齊次變換）。

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

例如：將歐拉角轉(zhuǎn)為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

由于機(jī)械臂的連桿長(zhǎng)度是已知的，只要確定了各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的角度，我們就可以確定 end-effector 的最終位置和方向。這個(gè)我們稱(chēng)之為 forward kinematics（正向運(yùn)動(dòng)學(xué)）。反過(guò)來(lái)，如果我們知道了 end-effector 的最終位置和方向，我們也可以推導(dǎo)各個(gè)關(guān)節(jié)的角度，這個(gè)我們稱(chēng)之為 inverse kinematics（反向運(yùn)動(dòng)學(xué)）。

機(jī)械臂關(guān)注的主要是反向運(yùn)動(dòng)學(xué)。

如果 end-effector，需要走一段比較長(zhǎng)的路程(path)，從甲點(diǎn)運(yùn)行到乙點(diǎn)。我們?yōu)榱耸沟脵C(jī)械臂的 end-effector 的路徑平滑，需要規(guī)劃一系列的路徑點(diǎn)（waypoints），這個(gè)我們叫做路徑規(guī)劃（trajectory planning）或者叫運(yùn)動(dòng)插補(bǔ)（interpolation）。例如下圖，藍(lán)色的曲線(xiàn)叫 path，而各個(gè)時(shí)間經(jīng)過(guò)的路徑點(diǎn)叫 trajectory。如何設(shè)計(jì)經(jīng)過(guò)這些路徑點(diǎn)的 trajectory，比較顯而易見(jiàn)的指標(biāo)是 “平滑” 。那什么是“平滑” ，它可能意味著 “速度連續(xù)” 、“加速度連續(xù)” 、 “沒(méi)有頓挫” 等等。這些指標(biāo)，都會(huì)轉(zhuǎn)化成數(shù)學(xué)算法。 RST 也會(huì)有相應(yīng)的算法支持，作者將另外寫(xiě)文章描述。

機(jī)械臂的關(guān)節(jié)位置我們一般用電機(jī)來(lái)驅(qū)動(dòng)。電機(jī)通過(guò)產(chǎn)生力矩來(lái)轉(zhuǎn)動(dòng)機(jī)械裝置，驅(qū)動(dòng)機(jī)械臂。不同場(chǎng)合或者時(shí)機(jī)，需要的力矩不盡相同。

例如，機(jī)械臂水平放置的時(shí)候需要關(guān)節(jié)電機(jī)產(chǎn)生力矩來(lái)抵消地球引力；當(dāng)機(jī)械臂需要迅速移動(dòng)的時(shí)候，需要的力矩比緩慢移動(dòng)的要大，當(dāng)機(jī)械臂彎曲或者平展時(shí)候，重心發(fā)生變化，由于慣量（I = mr2）的不同，需要的關(guān)節(jié)力矩也不相同；另外，在很多場(chǎng)合，機(jī)械臂需要和人交互（collaborative robots），在碰到人體的時(shí)候，需要做出安全的保護(hù)動(dòng)作，并對(duì)力矩進(jìn)行調(diào)整。

這些需要考慮力矩的因素，我們稱(chēng)之為動(dòng)力學(xué)（dynamics）。和運(yùn)動(dòng)學(xué)類(lèi)似，動(dòng)力學(xué)分為正向動(dòng)力學(xué)（forward dynamics）和反向運(yùn)動(dòng)學(xué)(inverse dynamics)。 RST 里支持兩種都有相應(yīng)的 MATLAB 函數(shù)和Simulink block。作者也會(huì)另外寫(xiě)文章詳細(xì)介紹 RST 關(guān)于動(dòng)力學(xué)的部分。

運(yùn)動(dòng)學(xué)

1. Rigid Body Tree （剛體樹(shù)）

我們說(shuō)研究運(yùn)動(dòng)學(xué)（主要是反向運(yùn)動(dòng)學(xué)），就是研究 end-effector 的位置改變會(huì)帶動(dòng)各個(gè)關(guān)節(jié)的角度如何改變。RST 用 Rigid Body Tree 這樣一個(gè)對(duì)象，在這個(gè)對(duì)象上可以使運(yùn)動(dòng)學(xué)設(shè)計(jì)易用且可視化。下圖展示了機(jī)械臂的剛體樹(shù)樣例，可以在 MATLAB 界面中展示各個(gè) body 的詳細(xì)參數(shù)。

一般來(lái)說(shuō)，Rigid Body Tree 都是直接從機(jī)械臂的 CAD 文件或者 URDF（Unified Robot Description Format）文件導(dǎo)入。不過(guò)，也支持每個(gè) body 的逐步添加。

我們隨便敲幾行 MATLAB 命令：

robot = importrobot('iiwa14.urdf');

show(robot);

讓我們來(lái)改變一下機(jī)器人的各個(gè)關(guān)節(jié)角度（configuration），比如讓MATLAB自動(dòng)給一個(gè)隨機(jī)角度配置，再看一下結(jié)果。顯然各個(gè)角度發(fā)生了變化。

q=randomConfiguration(robot);

show(robot,q);

我們看看這個(gè)機(jī)械臂最末端的 end-effector 是什么？

showdetails(robot)

我們?cè)倏纯?end-effector 相對(duì)機(jī)器人底座(base)的 Homogeneous Transformation(相對(duì)位置和角度)。

2. 反向運(yùn)動(dòng)學(xué)算法

反向運(yùn)動(dòng)學(xué)算法求解分兩種：一種是分析解法（Analytic solutions）；一種是數(shù)值解法（Numerical solutions）。

MATLAB 用的是數(shù)值解法，可以理解為迭代尋優(yōu)，或者近似解。

MATLAB 里面的反向運(yùn)動(dòng)學(xué)求解器(solver)有兩個(gè)：

Inverse Kinematics

Generalized Inverse Kinematics

兩者的區(qū)別是，后者比前者多了很多限制(constraints) 。例如end-effector的方向限制、機(jī)械臂各個(gè)關(guān)節(jié)的角度限制、位置限制等等。

我們先看一下比較簡(jiǎn)單的 Inverse Kinematics：

這是一個(gè) 6 軸機(jī)器人，end-effector 是 L6。

我們想要的最終結(jié)果就是下圖：

tform 是 L6 相對(duì) base 的位置和方向（合稱(chēng) pose）。

下面的 MATLAB 代碼是計(jì)算出最終的各個(gè)關(guān)節(jié)的角度（configSoln），由于是用了迭代的數(shù)值解法，weights 為權(quán)重，initialguess 為給出一個(gè)初始估計(jì)。

我們?cè)倏匆幌卤容^復(fù)雜的 Generalized Inverse Kinematics:

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

導(dǎo)入了一個(gè) 7 自由度的 rethink 機(jī)械臂— sawyer

設(shè)定反向運(yùn)動(dòng)學(xué)的求解限制— 例如機(jī)械臂的 end-effector 永遠(yuǎn)指向地面的一個(gè)物體

對(duì)反向運(yùn)動(dòng)學(xué)進(jìn)行求解

如果我們加一段 end-effector 位置變化后，調(diào)用這段代碼的動(dòng)畫(huà)效果，你會(huì)發(fā)現(xiàn) end-effector 的指向沒(méi)有變化 – 帶限制的反向動(dòng)力學(xué)求解成功了：

3. Simulink 示例

在安裝 RST 之后，Simulink 的 library 里就會(huì)出現(xiàn)幾個(gè)和機(jī)械臂（manipulator）相關(guān)的 block：

其中 Inverse Kinematics 就是反向運(yùn)動(dòng)學(xué) block，其他的一些模塊顧名思義和動(dòng)力學(xué)有關(guān)，在下一篇文章我會(huì)重點(diǎn)介紹。

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

這個(gè)例子展現(xiàn)了機(jī)械臂的 end-effector 抓了紅色物體，沿著規(guī)劃好的紫色 trajectory，進(jìn)行運(yùn)動(dòng)。

下圖的 stateflow 狀態(tài)機(jī)是一個(gè) trajectory tracking 的算法，它的作用是確保 end-effctor 沿著預(yù)設(shè)的 trajectory 運(yùn)行。

狀態(tài)機(jī)下面的是運(yùn)動(dòng)控制部分和環(huán)境和物理模型。運(yùn)動(dòng)控制很簡(jiǎn)單 – 直接計(jì)算反向運(yùn)動(dòng)學(xué)，將算好的關(guān)節(jié)角度交給物理模型去展現(xiàn)。物理模型構(gòu)建也很簡(jiǎn)單—— 用 SimScape 中的 SimMultibody 直接導(dǎo)入機(jī)械臂的 URDF 文件即可。

這里可以看到物理模型并沒(méi)有包含伺服電機(jī)，而是“透明傳輸”— 反向運(yùn)動(dòng)學(xué)的結(jié)果直接發(fā)給了機(jī)械模型去展現(xiàn)。實(shí)際上真實(shí)的運(yùn)動(dòng)控制器會(huì)將位置、速度、力矩指令通過(guò)伺服總線(xiàn)（例如 EtherCAT）發(fā)給每個(gè)關(guān)節(jié)的電機(jī)去執(zhí)行，電機(jī)通過(guò)減速器去帶動(dòng)機(jī)械結(jié)構(gòu)。例如，一個(gè) 6 軸機(jī)械臂會(huì)有 6 個(gè)伺服電機(jī)，運(yùn)動(dòng)控制器會(huì)將運(yùn)動(dòng)過(guò)程解析為 6 個(gè)電機(jī)可以理解的位置、速度、力矩指令。