關(guān)于MATLAB 中的機(jī)械臂算法分析和介紹

前面，我們講了機(jī)械臂的運(yùn)動(dòng)學(xué)（kinematics）（點(diǎn)擊藍(lán)字查看）：正向運(yùn)動(dòng)學(xué)和反向運(yùn)動(dòng)學(xué)。

正向運(yùn)動(dòng)學(xué)指的是在已知機(jī)械臂各個(gè)關(guān)節(jié)角度的情況下，推算end-effector（機(jī)械臂終端）的位置和方向（合稱pose）。反向運(yùn)動(dòng)學(xué)指的是在已知end-effector的位置和方向，推算各個(gè)機(jī)械臂的關(guān)節(jié)角度。我們主要研究反向運(yùn)動(dòng)學(xué)。

讓我們看一下基于反向運(yùn)動(dòng)學(xué)的控制邏輯圖：

在給出機(jī)械臂的end-effector的pose后，反運(yùn)動(dòng)學(xué)模塊計(jì)算出各個(gè)關(guān)節(jié)所需要的角度，然后通過電機(jī)產(chǎn)生力矩(torque)去執(zhí)行。在此期間，通過反饋(feedback)去消除一些控制誤差。

但是，事實(shí)上會有很多的干擾因素存在。例如：

地球引力（gravity）

慣量（inertia）

摩擦力（friction）

科里奧利力和離心力（Coriolis and centrifugal）

由于連桿之間是通過關(guān)節(jié)（joint）耦合在一起，連桿之間會有反作用力和反向慣量

以及一些人為的要求，例如：

我們想要機(jī)械臂運(yùn)動(dòng)的速度，按照我們預(yù)先的設(shè)定運(yùn)行（velocity and acceleration）

我們想要求機(jī)械臂在觸碰外物時(shí)候做出安全保護(hù)（external force）

這些我們暫且稱之為disturbance（干擾），當(dāng)然有些“干擾”是我們故意要求的，比如說需要按照預(yù)設(shè)軌跡的速度運(yùn)行。加入“干擾”后的示意圖如下：

如果我們能提前計(jì)算出來這些“disturbance”，然后在控制環(huán)路中將它“抵消”（有時(shí)候也叫前饋控制）。這樣，就能使得機(jī)械臂“完美”運(yùn)行了。讓我們再改一下控制邏輯圖：

為了計(jì)算這些“disturbance”。我們需要考慮幾個(gè)因素：

機(jī)械臂各個(gè)關(guān)節(jié)角度，即q

機(jī)械臂各個(gè)關(guān)節(jié)的速度，即qd（對q求導(dǎo)）

機(jī)械臂的各個(gè)關(guān)節(jié)的加速度，即qdd（對qd再次求導(dǎo)）

機(jī)械臂的質(zhì)量和各個(gè)連桿的重心點(diǎn)

有了這些因素，我們可以通過數(shù)學(xué)方法：

u=M(q)qdd + C(q,qd)qd +G(q)

求出所需要的力矩，其中M項(xiàng)代表克服了機(jī)械臂的加速度慣量以及不同連桿之間的慣量影響所需力矩、C項(xiàng)代表了克服科里奧利力和離心力所需力矩、G項(xiàng)代表了克服地球引力力矩。

其中Inverse Dynamics叫做反向動(dòng)力學(xué)，它的作用是輸入想要的關(guān)節(jié)速度(qd)、關(guān)節(jié)加速度(qdd)、關(guān)節(jié)角度(q)，輸出為每個(gè)關(guān)節(jié)所需要的力矩（u）。當(dāng)然也有正向動(dòng)力學(xué)（forward dynamics）,它的作用和反向動(dòng)力學(xué)相反，輸入關(guān)節(jié)角度（q）、關(guān)節(jié)速度（qd）、每個(gè)關(guān)節(jié)的力矩（u），輸出為每個(gè)關(guān)節(jié)的加速度（qdd）。

上述的控制邏輯圖，是一個(gè)“前饋控制”的例子，它的作用是預(yù)先計(jì)算出所需的力矩，輸入給控制器，“反饋”的存在是為了消除一些誤差，例如摩擦力和其他噪音。

總之，我們需要這么一個(gè)“動(dòng)力學(xué)”模型，使得我們可以抵消各種不同的“disturbance”的影響 --- 慣量、地球引力、科里奧利力和離心力等等。另外，實(shí)際電機(jī)的功率是有限的，通過反向動(dòng)力學(xué)我們也可以準(zhǔn)確知道電機(jī)需要提供多少力矩，從而為電機(jī)選型提供參考依據(jù)。

下圖初略表示了end-effector – inverse kinematics – inverse dynamics之間的關(guān)系：

Robotics System Toolbox（RST）中的動(dòng)力學(xué)算法

RST 提供了動(dòng)力學(xué)方面的函數(shù)和Simulink block。

MATLAB函數(shù)：

Simulink blocks：

讓我們看一個(gè)簡單的例子：

這是一個(gè)工作在2D空間的機(jī)器人。標(biāo)明了各個(gè)結(jié)構(gòu)件的質(zhì)量、長度和重心。在靜止?fàn)顟B(tài)下，兩個(gè)關(guān)節(jié)分別需要多大的力矩去抵消地球引力并支撐起這個(gè)機(jī)器人？

我們先一步步地構(gòu)造這個(gè)機(jī)器人：

然后加入重心、質(zhì)量：

然后在速度和加速度都是0的情況下，調(diào)用inverse Dynamics函數(shù)算出力矩：

tau =

20 10

很明顯，第一個(gè)關(guān)節(jié)要比第二個(gè)需要更大的力矩。

我們也可以用gravityTorque這個(gè)函數(shù)試一下，結(jié)果是相同的。

gravTorq =

20 10

如果我們將速度或者加速度改變一下，看看力矩的變化，例如：

相應(yīng)地，所需力矩也增大了。

tau =

30.5981 12.1340

Simulink 示例

在MATLAB Central File Exchange上搜索“Designing Robot Manipulator Algorithms”。

這是一個(gè)機(jī)械臂按運(yùn)動(dòng)軌跡抓取物體的例子：

在前面運(yùn)動(dòng)學(xué)算法中，我們也介紹了這個(gè)例子。當(dāng)時(shí)我們用反向運(yùn)動(dòng)學(xué)（inverse kinematics）去計(jì)算end-effector的位置。這次我們用力矩控制的方式來重新看一下這個(gè)問題。這里有兩種方式供參考：

首先，inverse kinematics根據(jù)end-effector的位置，計(jì)算出各個(gè)關(guān)節(jié)（電機(jī)）所需要的角度位置，然后交給各個(gè)關(guān)節(jié)的電機(jī)去執(zhí)行。由于關(guān)節(jié)控制的是角度。所以叫Joint Space Control （即關(guān)節(jié)角度控制）。

前饋部分（feedforward）負(fù)責(zé)計(jì)算出所需力矩（用來抵消重力，按預(yù)定軌跡運(yùn)動(dòng)等等）。

反饋部分（feedback）用PID去消除誤差。

這個(gè)做法是直接控制end-effector的位置，并用PID去消除end-effector的位置誤差。也叫做task space control，即直接控制在XYZ空間（也叫笛卡爾坐標(biāo)的位置）。

前饋部分只做一件事：計(jì)算抵消地球引力的力矩。

反饋部分用PID去控制end-effector的位置。然后用Jacobian矩陣將end-effector（在笛卡爾坐標(biāo)）的力矩，轉(zhuǎn)化成各個(gè)關(guān)節(jié)的力矩。

Computed Torque Control

有了機(jī)械臂的動(dòng)力學(xué)模型(dynamic model)，我們可以應(yīng)用的控制方法有很多種。通常來說，反饋是必要的 – 可以消除控制誤差和一些噪音。從力矩計(jì)算的角度來看，可以做：

力矩前饋控制。剛才兩個(gè)例子都是將計(jì)算好的力矩做前饋。由于機(jī)械臂的移動(dòng)速率遠(yuǎn)遠(yuǎn)小于電機(jī)控制速率，所以計(jì)算力矩的頻率并不高。比如說，電機(jī)的控制頻率為5K Hz，前饋力矩計(jì)算的頻率大約是50Hz。對控制器的硬件要求不高。

計(jì)算力矩控制（computed torque control）。computed torque control工作在較高的控制頻率上（例如1K Hz），要比力矩前饋控制頻率高的多。對控制器硬件要求較高。

Simulink提供dynamics方面的blocks，可以方便地搭出computed torque controller：

computed torque controller的輸入：

q，機(jī)械臂關(guān)節(jié)角度

qd，預(yù)設(shè)的機(jī)械臂關(guān)節(jié)要達(dá)到的角度 (desired q)

dq，機(jī)械臂關(guān)節(jié)速度，即對q求導(dǎo)

dqd，預(yù)設(shè)的機(jī)械臂關(guān)節(jié)要達(dá)到的速度 （desired）

ddqd，預(yù)設(shè)的機(jī)械臂關(guān)節(jié)要達(dá)到的加速度 （desired ）

computed torque controller的輸出：

u，機(jī)械臂各個(gè)關(guān)節(jié)的力矩

有了computed torque controller我可以做出比較復(fù)雜的控制系統(tǒng)，例如一方面要求機(jī)械臂按預(yù)定的trajectory（含角度、速度、加速度等信息）運(yùn)行；一方面又要求機(jī)械臂遭受外力干擾后改變運(yùn)行軌跡（trajectory）。

如果感興趣，可以看MATLAB自帶的“Perform Safe Trajectory Tracking Control Using Robotics Manipulator Blocks”這個(gè)例子。機(jī)械臂在碰到遮擋的硬物，改變預(yù)設(shè)的運(yùn)行軌跡，保證運(yùn)行安全。