基于Matlab机器人系统工具箱(Robotics System Toolbox)对库卡LBR Med 7 R800进行建模分析（一）

本文主要讲解利用机器人系统工具箱(Robotics System Toolbox)对库卡LBR Med 7 R800进行建模，以及利用模型对其进行正、逆解运算分析，库卡LBR Med 7 R800本体图如下：

 首先利用Matlab机器人系统工具箱(Robotics System Toolbox)中的刚体树(rigidbodytree)对机器人进行建模，代码如下：

%创建一个机器人刚体树对象，取名为robot
robot=robotics.RigidBodyTree("MaxNumBodies",9,"DataFormat","row");
%创建九个关节和九个刚体
body0=robotics.RigidBody("body0");
jnt0=robotics.Joint("jnt0","fixed");
body1=robotics.RigidBody("body1");
jnt1=robotics.Joint("jnt1","revolute");
body2=robotics.RigidBody("body2");
jnt2=robotics.Joint("jnt2","revolute");
body3=robotics.RigidBody("body3");
jnt3=robotics.Joint("jnt3","revolute");
body4=robotics.RigidBody("body4");
jnt4=robotics.Joint("jnt4","revolute");
body5=robotics.RigidBody("body5");
jnt5=robotics.Joint("jnt5","revolute");
body6=robotics.RigidBody("body6");
jnt6=robotics.Joint("jnt6","revolute");
body7=robotics.RigidBody("body7");
jnt7=robotics.Joint("jnt7","revolute");
body8=robotics.RigidBody("body8");
jnt8=robotics.Joint("jnt8","fixed");
%对七个旋转关节做角度限制
jnt1.PositionLimits=[-170*((2*pi)/360) 170*((2*pi)/360)];
jnt2.PositionLimits=[-120*((2*pi)/360) 120*((2*pi)/360)];
jnt3.PositionLimits=[-170*((2*pi)/360) 170*((2*pi)/360)];
jnt4.PositionLimits=[-120*((2*pi)/360) 120*((2*pi)/360)];
jnt5.PositionLimits=[-170*((2*pi)/360) 170*((2*pi)/360)];
jnt6.PositionLimits=[-120*((2*pi)/360) 120*((2*pi)/360)];
jnt7.PositionLimits=[-175*((2*pi)/360) 175*((2*pi)/360)];
%定义各个关节之间的旋转和平移
setFixedTransform(jnt1,[1 0 0 0;0 1 0 0;0 0 1 0.157;0 0 0 1]);
setFixedTransform(jnt2,[-1 0 0 0;0 0 1 0;0 1 0 0.183;0 0 0 1]);
setFixedTransform(jnt3,[-1 0 0 0;0 0 1 0.217;0 1 0 0;0 0 0 1]);
setFixedTransform(jnt4,[1 0 0 0;0 0 -1 0;0 1 0 0.183;0 0 0 1]);
setFixedTransform(jnt5,[-1 0 0 0;0 0 1 0.217;0 1 0 0;0 0 0 1]);
setFixedTransform(jnt6,[1 0 0 0;0 0 -1 0;0 1 0 0.183;0 0 0 1]);
setFixedTransform(jnt7,[-1 0 0 0;0 0 1 0.091;0 1 0 0;0 0 0 1]);
setFixedTransform(jnt8,[1 0 0 0;0 1 0 0;0 0 1 0.035;0 0 0 1]);
%把各个关节附着到各个刚体上
body0.Joint=jnt0;
body1.Joint=jnt1;
body2.Joint=jnt2;
body3.Joint=jnt3;
body4.Joint=jnt4;
body5.Joint=jnt5;
body6.Joint=jnt6;
body7.Joint=jnt7;
body8.Joint=jnt8;
%把各个刚体"安装"到刚体树(robot)上
addBody(robot,body0,"base");
addBody(robot,body1,"body0");
addBody(robot,body2,"body1");
addBody(robot,body3,"body2");
addBody(robot,body4,"body3");
addBody(robot,body5,"body4");
addBody(robot,body6,"body5");
addBody(robot,body7,"body6");
addBody(robot,body8,"body7");
show(robot);
showdetails(robot);

运行上述代码，可以看到：图一所示的机器人模型图(命令：show(robot))，图二中在命令行窗口显示的该机器人的详细组成信息(命令：showdetails(robot))，图三中工作区显示的机器人（robot）、关节（joint）、连杆（body）的属性数值。

图一 

 图二

图三 

然后，我们做机器人的逆解运算，代码如下：

%给定末端的位置和欧拉角姿态
weizhi=[500 200 300];%单位 mm
zitai=[0 0 0];%单位 °
tform=trvec2tform(weizhi./1000)*eul2tform(zitai.*(pi/180));%根据位置和姿态，将其转换为齐次矩阵
%定义一个逆解求解器
ik=robotics.InverseKinematics("RigidBodyTree",robot);
%设置求解各个量的权重，前三个是欧拉角、后三个是位置
weights=[0.25 0.25 0.25 1 1 1];
%定义求解器的初始参考位置
initialguess=robot.homeConfiguration;
%开始求解，其中逆解求解结果存放在configSoln中
[configSoln,solnInfo]=ik("body8",tform,weights,initialguess);
%判断求解是否成功，如果成功，打印显示逆解数值（单位是°）
if(solnInfo.Status=="success")
    fprintf("逆解是：")
    nijie=configSoln.*(180/pi);
    disp(nijie)
else
    fprintf("所求无解")
end

运行上诉代码后，在命令行窗口可以看到如图四所示的逆解结果（注意，逆解数值不唯一）。

 图四

最后，我们可以对机器人求解正解，代码如下：

%给定各个关节的角度数值
q=[30 30 30 30 30 30 30];%单位是°
transform=getTransform(robot,q.*(pi/180),"body8","body0");
moduanweizhi=tform2eul(transform).*(180/pi);%末端角度数值
moduanzitai=tform2trvec(transform).*1000;%位置数值
fprintf("\n位置是：")
disp(moduanweizhi)
fprintf("\n姿态是：")
disp(moduanzitai)

 运行上诉代码后，可以在命令行窗口中查看其结果，如图五所示。

 图五

此模型已经利用库卡LBR Med 7 R800真机对其正逆解进行了准确性验证，完全ok！

我们如果想看机器人在特定关节角度下机器人的位姿，可以运行如下代码：

q=[50 30 30 30 40 21 34];%输入各个关节角度，单位式°
q=q*(pi/180);%角度转弧度
show(robot,q);

运行上诉代码后，可看到特定关节角度下机器人的位姿，如图六所示。

 图六