MATLAB中的運動學(xué)機械臂算法

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

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

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

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

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

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

有時候，對于旋轉(zhuǎn)會有不同的表達方式，例如歐拉角（Euler Angles）、四元素（Quaternion）、旋轉(zhuǎn)矩陣（Rotation Matrix）等等；表達轉(zhuǎn)置，也可使用轉(zhuǎn)置向量（Translation Vector）。有了 RST 這些都可以輕松通過不同的函數(shù)進行互換。下圖為具體的函數(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

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

機械臂關(guān)注的主要是反向運動學(xué)。

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

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

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

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

運動學(xué)

1. Rigid Body Tree （剛體樹）

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

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

我們隨便敲幾行 MATLAB 命令：

robot = importrobot('iiwa14.urdf');

show(robot);

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

q=randomConfiguration(robot);

show(robot,q);

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

showdetails(robot)

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

2. 反向運動學(xué)算法

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

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

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

Inverse Kinematics

Generalized Inverse Kinematics

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

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

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

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

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

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

我們再看一下比較復(fù)雜的 Generalized Inverse Kinematics:

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

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

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

對反向運動學(xué)進行求解

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

3. Simulink 示例

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

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

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

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

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

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

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