Matlab - Solidworks 机器人建模（6）——使用rigidBodyTree构建机器人模型

前言

本文适用对象：

没有机器人的Solidworks模型自己又懒得画的童鞋
没有机器人URDF模型的童鞋

如果你在Matlab帮助里面搜索rigidBody，你大概率会看到matlab自带的例程链接在这里教你怎么用rigidBody建立机器人模型，里面有一小节告诉我们怎么用自己推导的DH参数表来建立操作臂模型。

它这里用的例程是PUMA560 机械臂

然后建出来的坐标系模型长这样
就怎么说呢，很TM不直观。。。一是因为重叠的坐标系显示不出来，二是我们很难把这个坐标系跟我们的机器人模型对应。图像显示的目的是让复杂模型简化更为直观，而不是让你更懵逼，因此本文讲解怎么不通过导入URDF的方法直接建立rigidBodyTree机器人模型。

正文

考虑如下的结构的机器人（我特意挑了个特殊的结构，检验一下自己的DH建模水平）

该结构有四个旋转关节，再加上tool frame一共有5个坐标系。经过手动DH建系结果如下：

1.其实你根本不需要DH参数表，直接把坐标系放在关节上，定义好不同坐标系之间的转换就可以跑通仿真了

2.要注意的是，这个结构Tool frame之前需要额外增加一个坐标系满足DH默认条件才能无脑代公式得到正运动学矩阵

简单分析一下这个模型，他有4个连杆（连杆0在地面上了，这里不考虑），4个关节，加上Tool frame的话就有5个坐标系。另外，各个连杆的长度分别定义成L1=0.3 L2=0.3 L3=0.3 L4=0.2。额外添加的坐标系只是为了方便我们代入公式求正运动学转换矩阵，实现仿真演示的话只需要坐标系0~4即可。

OK，开始撸代码，rigidBody代码如下

1. 新建一个rigidBodyTree模型，定义各个连杆长度

%%
% Start with a blank rigid body tree model.
robot = rigidBodyTree('DataFormat','column','MaxNumBodies',4);
%%
% Specify arm lengths for the robot arm.
L1 = 0.3;
L2 = 0.3;
L3 = 0.3;
L4 = 0.2;

2. 定义四个连杆和关节

连杆1是依附在base上的，初始条件下0号坐标系与Matlab的基坐标系重合，因此此时没有相对基坐标系的旋转和平移，且有一个旋转关节在Z0轴

%%
% Add |'link1'| body with |'joint1'| joint.
body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
%定义0号坐标系与基坐标系的相对位置（这里没有旋转与平移）
setFixedTransform(joint,trvec2tform([0 0 0]));
%定义旋转轴方向
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');

连杆2依附在连杆1上，有相对转动和平移，为了简化代码这里我们偷了懒（不然需要添加几个旋转矩阵，有兴趣的可以自己尝试），直接换了关节2的方向表达，我们定义了x1为2号关节的转动方向

%%
% Add |'link2'| body with |'joint2'| joint.
body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint, trvec2tform([0,0,L1]));
joint.JointAxis = [1 0 0];
body.Joint = joint;
addBody(robot, body, 'link1');

完成后面的几个连杆关节的定义

%%
% Add |'link3'| body with |'joint3'| joint.
body = rigidBody('link3');
joint = rigidBodyJoint('joint3','revolute');
setFixedTransform(joint, trvec2tform([0,L2,0]));
joint.JointAxis = [1 0 0];
body.Joint = joint;
addBody(robot, body, 'link2');
%%
% Add |'link4'| body with |'joint4'| joint.
body = rigidBody('link4');
joint = rigidBodyJoint('joint4','revolute');
setFixedTransform(joint, trvec2tform([0,L3,0]));
joint.JointAxis = [1 0 0];
body.Joint = joint;
addBody(robot, body, 'link3');
%%
% Add |'tool'| end effector with |'fix1'| fixed joint.
body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([0, L4, 0]));
body.Joint = joint;
addBody(robot, body, 'link4');

至此，建模算是结束了。

3. 检查建模结果

看一下我们的模型

%%
% Show details of the robot
showdetails(robot)
show(robot)

看起来是这么回事，我们试下换不同的关节位置看看。

configuration1 = [0;0.8;-0.6;0];
show(robot,configuration1)

检查无误，建模结束。

4. 完整代码

% Create a |rigidBodyTree| object and rigid bodies with their
% associated joints. Specify the geometric properties of each rigid body
% and add it to the robot.%%
% Start with a blank rigid body tree model.
robot = rigidBodyTree('DataFormat','column','MaxNumBodies',4);
%%
% Specify arm lengths for the robot arm.
L1 = 0.3;
L2 = 0.3;
L3 = 0.3;
L4 = 0.2;
%%
% Add |'link1'| body with |'joint1'| joint.
body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint,trvec2tform([0 0 0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');
%%
% Add |'link2'| body with |'joint2'| joint.
body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint, trvec2tform([0,0,L1]));
joint.JointAxis = [1 0 0];
body.Joint = joint;
addBody(robot, body, 'link1');
%%
% Add |'link3'| body with |'joint3'| joint.
body = rigidBody('link3');
joint = rigidBodyJoint('joint3','revolute');
setFixedTransform(joint, trvec2tform([0,L2,0]));
joint.JointAxis = [1 0 0];
body.Joint = joint;
addBody(robot, body, 'link2');
%%
% Add |'link2'| body with |'joint2'| joint.
body = rigidBody('link4');
joint = rigidBodyJoint('joint4','revolute');
setFixedTransform(joint, trvec2tform([0,L3,0]));
joint.JointAxis = [1 0 0];
body.Joint = joint;
addBody(robot, body, 'link3');
%%
% Add |'tool'| end effector with |'fix1'| fixed joint.
body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([0, L4, 0]));
body.Joint = joint;
addBody(robot, body, 'link4');%%
% Show details of the robot
showdetails(robot)
show(robot)configuration1 = [0;0.8;-0.6;0];
show(robot,configuration1)