Robotics Toolbox与Robotics System Toolbox在串联机械臂动力学建模上的应用与比较。

预备知识：

机械臂标准DH建模法。

机器人动力学。

Robotics Toolbox相关函数的使用。

Robotics System Toolbox相关函数的使用。

比较方法（思路）：

以puma560机械臂的标准D-H参数建立机器人模型，分别用Robotics System Toolbox和Robotics Toolbox建模，然后利用两个工具箱的逆动力学函数分别求解，看结果是否一致。

首先用Robotics System Toolbox建模求解，代码如下：

%puma560标准D-H参数
%           a        alpha  d    offset
dhparams = [0   	-pi/2	0   	0;
            0.432	0       0       0
            0.02	-pi/2	0.149	0;
            0   	pi/2	0.433	0;
            0       -pi/2	0   	0;
            0       0       0       0];
%创建刚体树对象以构建机器人。
robot = rigidBodyTree;
robot.Gravity=[0,0,-9.81];%设置重力大小和方向
robot.DataFormat = 'row';%设置机器人数据输入格式，行或者列。
body1 = rigidBody('body1');% 创建第一个刚体并将其添加到机器人。
jnt1 = rigidBodyJoint('jnt1','revolute');% 创建刚体关节对象并为其指定唯一名称。
setFixedTransform(jnt1,dhparams(1,:),'dh');% 使用setFixedTransform指定使用DH参数的体到体转
%换。DH参数的最后一个元素θ被忽略，因为角度取决于关节位置。
body1.Joint = jnt1;
addBody(robot,body1,'base')% 调用addBody将第一个身体关节连接到机器人的基座（base）。
% 创建其他刚体并将其添加到机器人。在调用addBody附加之前的主体名称时，
% 请指定该主体名称。每个固定变换相对于前一个关节坐标系。
body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
body3 = rigidBody('body3');
jnt3 = rigidBodyJoint('jnt3','revolute');
body4 = rigidBody('body4');
jnt4 = rigidBodyJoint('jnt4','revolute');
body5 = rigidBody('body5');
jnt5 = rigidBodyJoint('jnt5','revolute');
body6 = rigidBody('body6');
jnt6 = rigidBodyJoint('jnt6','revolute');
setFixedTransform(jnt2,dhparams(2,:),'dh');
setFixedTransform(jnt3,dhparams(3,:),'dh');
setFixedTransform(jnt4,dhparams(4,:),'dh');
setFixedTransform(jnt5,dhparams(5,:),'dh');
setFixedTransform(jnt6,dhparams(6,:),'dh');
body2.Joint = jnt2;
body3.Joint = jnt3;
body4.Joint = jnt4;
body5.Joint = jnt5;
body6.Joint = jnt6;
addBody(robot,body2,'body1')
addBody(robot,body3,'body2')
addBody(robot,body4,'body3')
addBody(robot,body5,'body4')
addBody(robot,body6,'body5')
%设置各个关节的角度、角速度、角加速度。
q=[2,3,4,2,3,2];
qd=[2,1,1,1,1,2];
qdd=[12,23,22,11,23,45];
%利用inverseDynamics函数，求出该状态下机器人各个关节的力矩。
t1=inverseDynamics(robot,q,qd,qdd);
%运行结果是  87.3066595855199	290.302707321317	205.475760481201	
%           -27.9728413871036	-11.4436733165222	44.1255782451433

 （注意：利用Robotics System Toolbox的rigidbodytree建立串联机械臂的时候，连杆的动力学参数有默认值，质量=1.质心=[0,0,0],惯量矩阵=[1,1,1,0,0,0]即[1,0,0;0,1,0;0,0,1],下面利用Robotics Toolbox建模时，按照这个动力学参数给连杆赋值）

然后，用Robotics Toolbox建模求解，代码如下：

%puma560标准D-H参数
%       theta    d           a        alpha     offset
L1=Link([0      0           0        -pi/2       0     ],'standard');
L2=Link([0      0           0.432     0          0     ],'standard');
L3=Link([0      0.149       0.02    -pi/2        0     ],'standard');
L4=Link([0      0.433       0        pi/2        0     ],'standard');
L5=Link([0      0           0        -pi/2       0     ],'standard');
L6=Link([0      0           0        0           0     ],'standard');
%设置连杆的动力学参数，只设置质量(m)，质心(r)，惯量矩阵(I)和电机惯量(Jm)。
%其中Jm设置为0，因为rigidbodytree里没有关于电机惯量的设置，为保持一致，所以设置为0，
%然后质量(m)，质心(r)，惯量矩阵(I)都是按照rigidbodytree里的默认连杆属性设置。
L1.m=1;L2.m=1;L3.m=1;
L4.m=1;L5.m=1;L6.m=1;
L1.r=[0,0,0];L2.r=[0,0,0];L3.r=[0,0,0];
L4.r=[0,0,0];L5.r=[0,0,0];L6.r=[0,0,0];
L1.I=[1,0,0;0,1,0;0,0,1];L2.I=[1,0,0;0,1,0;0,0,1];L3.I=[1,0,0;0,1,0;0,0,1];
L4.I=[1,0,0;0,1,0;0,0,1];L5.I=[1,0,0;0,1,0;0,0,1];L6.I=[1,0,0;0,1,0;0,0,1];
L1.Jm=0;L2.Jm=0;L3.Jm=0;
L4.Jm=0;L5.Jm=0;L6.Jm=0;
p560=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560');
%设置重力
p560.gravity=[0,0,9.81];
%设置各个关节的角度、角速度、角加速度。
q=[2,3,4,2,3,2];
qd=[2,1,1,1,1,2];
qdd=[12,23,22,11,23,45];
%利用rne函数，求出该状态下机器人各个关节的力矩。
t2=p560.rne(q,qd,qdd);
%运行结果87.3066595855199	290.302707321317	205.475760481201	
%        -27.9728413871036	-11.4436733165222	44.1255782451433

结论：在matlab上运行上诉两个代码后，发现在给定各关节相同的角度、角速度、角加速度后，利用Robotics System Toolbox和Robotics Toolbox的逆动力学函数所求出的各关节力矩一致。