Matlab Robotics Toolbox系列—使用篇（6）

Chapter 6 Inverse Dynamics

% 逆向动力学主要用于计算到达指定关节位置、速度、加速所需要的关节力矩。递归牛顿-欧拉公式是一种高效的矩阵式算法，在RTB中，调用方法为SerialLink.rne()

% 逆向动力学的计算需要具备每个关节的惯量、重量以及运动学参数

% 以Puma560为例，其在零转角状态下，以 角速度：5 rad/s，角加速度：1 rad/s2，计算关节力矩的方法

% 调入模型

>> mdl_puma560

 

% 关节力矩计算函数，rne(臂形，角速度，角加速度)

% ones用于产生数组，ones(a,b)产生a行b列，如ones(1,3)=[1 1 1]

>> tau = p560.rne(qz, 5*ones(1,6), ones(1,6))

 

% 关节坐标系轨迹创建，计算速度、角速度方法

 

% 建立时间向量

>> t = [0:.056:2];

 

% 计算关节坐标轨迹

>> [q,qd,qdd] = jtraj(qz, qr, t);

 

% 计算逆向动力学

>> tau = p560.rne(q, qd, qdd);

 

% 力矩图形

>> plot(t, tau(:,1:3)); xlabel('Time (s)'); ylabel('Joint torque (Nm)')

 

% Puma560的2,3关节的力矩主要是克服重力影响，因此，这两部分也可通过重力计算？

% 采用函数为gravload(臂形)

>> taug = p560.gravload(q);

>> plot(t, taug(:,1:3)); xlabel('Time (s)'); ylabel('Gravity torque (Nm)')

 

% 计算摩擦力

>> subplot(2,1,1); plot(t,[tau(:,2) taug(:,2)]); xlabel('Time (s)'); ylabel('Torque on joint 2 (Nm)');

>> subplot(2,1,2); plot(t,[tau(:,3) taug(:,3)]); xlabel('Time (s)'); ylabel('Torque on joint 3 (Nm)');

 

% 计算关节1 在运动中的惯量变换

>> M = p560.inertia(q);

>> M11 = squeeze(M(1,1,:));

>> clf

>> plot(t, M11); xlabel('Time (s)'); ylabel('Inertia on joint 1 (kgms2)')

 

% 计算变化率

>> max(M11)/min(M11)