MATLAB中的機械臂算法——動力學

2. Robotics System Toolbox(RST)中的動力學算法

前面，我們講了機械臂的運動學(kinematics)：正向運動學和反向運動學。正向運動學指的是在已知機械臂各個關節角度的情況下，推算end-effector(機械臂終端)的位置和方向(合稱pose)。反向運動學指的是在已知end-effector的位置和方向，推算各個機械臂的關節角度。我們主要研究反向運動學。

讓我們看一下基於反向運動學的控制邏輯圖：

在給出機械臂的end-effector的pose後，反運動學模塊計算出各個關節所需要的角度，然後通過電機產生力矩(torque)去執行。在此期間，通過反饋(feedback)去消除一些控制誤差。

這些我們暫且稱之為disturbance(幹擾)，當然有些「幹擾」是我們故意要求的，比如說需要按照預設軌跡的速度運行。加入「幹擾」後的示意圖如下

如果我們能提前計算出來這些「disturbance」，然後在控制環路中將它「抵消」(有時候也叫前饋控制)。這樣，就能使得機械臂「完美」運行了。讓我們再改一下控制邏輯圖：

acceleration

為了計算這些「disturbance」。我們需要考慮幾個因素

有了這些因素，我們可以通過數學方法u=M(q)qdd + C(q,qd)qd + G(q)求出所需要的力矩，其中M項代表克服了機械臂的加速度慣量以及不同連杆之間的慣量影響所需力矩、C項代表了克服科裡奧利力和離心力所需力矩、G項代表了克服地球引力力矩。

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

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

總之，我們需要這麼一個「動力學」模型，使得我們可以抵消各種不同的「disturbance」的影響 --- 慣量、地球引力、科裡奧利力和離心力等等。另外，實際電機的功率是有限的，通過反向動力學我們也可以準確知道電機需要提供多少力矩，從而為電機選型提供參考依據。

下圖初略表示了end-effector – inverse kinematics –inverse dynamics之間的關係：

Robotics System Toolbox(RST)提供了動力學方面的函數和Simulink block。

MATLAB函數

Simulink block

讓我們看一個簡單的例子：這是一個工作在2D空間的機器人。標明了各個結構件的質量、長度和重心。在靜止狀態下，兩個關節分別需要多大的力矩去抵消地球引力並支撐起這個機器人?

我們先一步步地構造這個機器人：

%% 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');

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的情況下，調用inverseDynamics函數算出力矩：

%% 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 =

我們也可以用gravityTorque這個函數試一下，結果是相同的。

gravTorq = gravityTorque(robot,q);

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

dq = [1 1]; %velocity

ddq = [2 2];%acceleration

tau = inverseDynamics(robot, q, dq, ddq);

相應地，所需力矩也增大了。

在MATLAB Central File Exchange上搜索「Designing Robot Manipulator Algorithms」。

這是一個機械臂按運動軌跡抓取物體的例子：

在前面運動學算法中，我們也介紹了這個例子。當時我們用反向運動學(inverse kinematics)去計算end-effector的位置。這次我們用力矩控制的方式來重新看一下這個問題。這裡有兩種方式供參考：

1: Inverse Kinematics + Joint Space Controllers

首先，inverse kinematics根據end-effector的位置，計算出各個關節(電機)所需要的角度位置，然後交給各個關節的電機去執行。由於關節控制的是角度。所以叫Joint Space Control (即關節角度控制)。

前饋部分(feedforward)負責計算出所需力矩(用來抵消重力，按預定軌跡運動等等)。

反饋部分(feedback)用PID去消除誤差。

2: Task Space Controller

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

Computed Torque Control

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

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

computed torque controller的輸入qd，預設的機械臂關節要達到的角度 (desired q)

ddqd，預設的機械臂關節要達到的加速度

computed torque controller的輸出

有了computed torque controller我可以做出比較複雜的控制系統，例如一方面要求機械臂按預定的trajectory(含角度、速度、加速度等信息)運行;一方面又要求機械臂遭受外力幹擾後改變運行軌跡(trajectory)。讀者如果感興趣，可以看MATLAB自帶的「Perform Safe Trajectory Tracking Control Using Robotics Manipulator Blocks」這個例子。機械臂在碰到遮擋的硬物，改變預設的運行軌跡，保證運行安全。

關注MathWorks訂閱號

學習更多MATLAB機械臂算法知識