目錄
1. 為什么研究機(jī)械臂的動(dòng)力學(xué)
2. Robotics System Toolbox(RST)中的動(dòng)力學(xué)算法
3. Simulink示例
1、為什么研究機(jī)械臂的動(dòng)力學(xué)
前面，我們講了機(jī)械臂的運(yùn)動(dòng)學(xué)(kinematics)：正向運(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é)所需要的角度，然后通過(guò)電機(jī)產(chǎn)生力矩(torque)去執(zhí)行。在此期間，通過(guò)反饋(feedback)去消除一些控制誤差。
但是，事實(shí)上會(huì)有很多的干擾因素存在。例如：
地球引力(gravity)
慣量(inertia)
摩擦力(friction)
科里奧利力和離心力(Coriolis and centrifugal)
由于連桿之間是通過(guò)關(guān)節(jié)(joint)耦合在一起，連桿之間會(huì)有反作用力和反向慣量
以及一些人為的要求，例如：
我們想要機(jī)械臂運(yùn)動(dòng)的速度，按照我們預(yù)先的設(shè)定運(yùn)行(velocity and acceleration)
我們想要求機(jī)械臂在觸碰外物時(shí)候做出安全保護(hù)(external force)
這些我們暫且稱之為disturbance(干擾)，當(dāng)然有些“干擾”是我們故意要求的，比如說(shuō)需要按照預(yù)設(shè)軌跡的速度運(yùn)行。加入“干擾”后的示意圖如下：
如果我們能提前計(jì)算出來(lái)這些“disturbance”，然后在控制環(huán)路中將它“抵消”(有時(shí)候也叫前饋控制)。這樣，就能使得機(jī)械臂“完美”運(yùn)行了。讓我們?cè)俑囊幌驴刂七壿媹D：
velocity
acceleration
為了計(jì)算這些“disturbance”。我們需要考慮幾個(gè)因素
機(jī)械臂各個(gè)關(guān)節(jié)角度，即q
機(jī)械臂各個(gè)關(guān)節(jié)的速度，即qd(對(duì)q求導(dǎo))
機(jī)械臂的各個(gè)關(guān)節(jié)的加速度，即qdd(對(duì)qd再次求導(dǎo))
機(jī)械臂的質(zhì)量和各個(gè)連桿的重心點(diǎn)
有了這些因素，我們可以通過(guò)數(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ī)的功率是有限的，通過(guò)反向動(dòng)力學(xué)我們也可以準(zhǔn)確知道電機(jī)需要提供多少力矩，從而為電機(jī)選型提供參考依據(jù)。
下圖初略表示了end-effector – inverse kinematics –inverse dynamics之間的關(guān)系：
2、RST中的動(dòng)力學(xué)算法
Robotics System Toolbox(RST)提供了動(dòng)力學(xué)方面的函數(shù)和Simulink block。
MATLAB函數(shù)
Simulink block
讓我們看一個(gè)簡(jiǎn)單的例子：這是一個(gè)工作在2D空間的機(jī)器人。標(biāo)明了各個(gè)結(jié)構(gòu)件的質(zhì)量、長(zhǎng)度和重心。在靜止?fàn)顟B(tài)下，兩個(gè)關(guān)節(jié)分別需要多大的力矩去抵消地球引力并支撐起這個(gè)機(jī)器人?
我們先一步步地構(gòu)造這個(gè)機(jī)器人：
%% create rigid body tree
robot = robotics.RigidBodyTree;
%%create body1
body1 = robotics.RigidBody("body1");
joint1 = robotics.Joint('joint1','revolute');
body1.Joint = joint1;
%%create body2
body2 = robotics.RigidBody("body2");
joint2 = robotics.Joint('joint2','revolute');
tform = trvec2tform([1,0,0]) %length of body1
setFixedTransform(joint2,tform);
body2.Joint = joint2;
%%create body3
endEffector = robotics.RigidBody('endEffector');
joint3 = robotics.Joint('joint3','fixed');
tform = trvec2tform([1,0,0]) %length of body2
setFixedTransform(joint3,tform);
endEffector.Joint = joint3;
%%add bodies to the tree
addBody(robot,body1,'base');
addBody(robot,body2,'body1');
addBody(robot,endEffector,'body2');
然后加入重心、質(zhì)量
robot.Gravity = [0,-10,0]; % in y direction
%define inertia properties
%mass
robot.Bodies{1}.Mass = 1; %body1
robot.Bodies{2}.Mass = 1; %body2
robot.Bodies{3}.Mass = 0.5; %end effector
%center of mass
robot.Bodies{1}.CenterOfMass = [0.5 0 0]; %body1
robot.Bodies{2}.CenterOfMass = [0.5 0 0]; %body2
robot.Bodies{3}.CenterOfMass = [0 0 0]; %end effector
%zero inertia assuming point of mass
robot.Bodies{1}.Inertia = [0 0 0 0 0 0]; %body 1
robot.Bodies{2}.Inertia = [0 0 0 0 0 0]; %body 2
robot.Bodies{3}.Inertia = [0 0 0 0 0 0]; %end effector
然后在速度和加速度都是0的情況下，調(diào)用inverseDynamics函數(shù)算出力矩：
%% inverse dynamics
robot.DataFormat='row';
q = [pi/3 -pi/3]; % angles
dq = [0 0]; %velocity
ddq = [0 0];%acceleration
tau = inverseDynamics(robot, q, dq, ddq);
tau =
20 10
很明顯，第一個(gè)關(guān)節(jié)要比第二個(gè)需要更大的力矩。
我們也可以用gravityTorque這個(gè)函數(shù)試一下，結(jié)果是相同的。
gravTorq = gravityTorque(robot,q);
gravTorq =
20 10
如果我們將速度或者加速度改變一下，看看力矩的變化，例如：
dq = [1 1]; %velocity
ddq = [2 2];%acceleration
tau = inverseDynamics(robot, q, dq, ddq);
相應(yīng)地，所需力矩也增大了。
tau =
30.5981 12.1340
3、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的位置。這次我們用力矩控制的方式來(lái)重新看一下這個(gè)問(wèn)題。這里有兩種方式供參考：
1: Inverse Kinematics + Joint Space Controllers
首先，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ì)算出所需力矩(用來(lái)抵消重力，按預(yù)定軌跡運(yùn)動(dòng)等等)。
反饋部分(feedback)用PID去消除誤差。
2: Task Space Controller
這個(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)用的控制方法有很多種。通常來(lái)說(shuō)，反饋是必要的 – 可以消除控制誤差和一些噪音。從力矩計(jì)算的角度來(lái)看，可以做
力矩前饋控制。剛才兩個(gè)例子都是將計(jì)算好的力矩做前饋。由于機(jī)械臂的移動(dòng)速率遠(yuǎn)遠(yuǎn)小于電機(jī)控制速率，所以計(jì)算力矩的頻率并不高。比如說(shuō)，電機(jī)的控制頻率為5K Hz，前饋力矩計(jì)算的頻率大約是50Hz。對(duì)控制器的硬件要求不高。
計(jì)算力矩控制(computed torque control)。computed torque control工作在較高的控制頻率上(例如1K Hz)，要比力矩前饋控制頻率高的多。對(duì)控制器硬件要求較高。
Simulink提供dynamics方面的blocks，可以方便地搭出computed torque controller：
computed torque controller的輸入
q，機(jī)械臂關(guān)節(jié)角度
dq，機(jī)械臂關(guān)節(jié)速度，即對(duì)q求導(dǎo)
qd，預(yù)設(shè)的機(jī)械臂關(guān)節(jié)要達(dá)到的角度 (desired q)
dqd，預(yù)設(shè)的機(jī)械臂關(guān)節(jié)要達(dá)到的速度
ddqd，預(yù)設(shè)的機(jī)械臂關(guān)節(jié)要達(dá)到的加速度
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)行安全。
（轉(zhuǎn)載）
