机械手模拟仿真实验文档

机械手模拟仿真实验文档
实验简介
问题描述：应用仿真技术，建立一个具有四自由度的（虚拟）机械手，可完成在任意给定的两个三维空间点之间画一条直线的任务
解决方案：首先使用VRML语言建模机械手模型，再使用Matlab中的Simulink仿真环境模拟机械手四个关节的运动状态，然后在此基础之上编写Matlab程序完成
所需功能。

重点难点：如何建立世界坐标系，并得到每个坐标点与四个关节旋转角度的对应关系？
如何计算机械手臂沿两点直线移动过程中四个关节的运动姿态(即旋转角
度)？
一、机械手建模
为了实现对机械手的仿真实现，我们首先建立一个模拟四个关节（四个自由度）可运动的机械手，效果如下图1.1所示。

图 1.1 四个自由度的机械手模型
该机械手的模型分为底座，四个关节以及顶端的画笔，我们重点来弄清楚四个关节的运
动情况，地下的红色小立方体可以在水平面的方向左右(即顺时针或逆时针)旋转正负90度，其余的三个颜色分别标记为绿色、蓝色和灰色的三个立方体，它们可以绕着各自的底平面心前后旋转运动，其运动幅度均为正负90度。

建立机械手模型时，采用了MA TLAB支持的虚拟现实建模语言VRML。

1.1 虚拟现实建模语言VRML
VRML（Virtual Reality Modeling Language）即虚拟现实建模语言，是一种用于建立真实世界的场景模型或者人们虚构的三维世界的场景建模语言，是一种面向Web面向对象的三维造型语言，其实质为一种解释性语言，常用的编辑环境有VrmlPad，VRML文件的后缀名为.wrl, 使用浏览器浏览时需要相应插件的支持，可下载cortona3d.msi安装即可由于VRML在互联网和可视化的广泛应用，Matlab对于虚拟现实也进行了有力的支持，Matlab提供了Simulink接口和Matlab接口来与虚拟现实进行交互，相应的详细说明文档请查看Matlab帮助文档中的Virtual Reality Toolbox内容。

1.2 机械手模型的概述
下面两幅图1.2和1.3显示的分别为初始机械手模型和旋转一定角度的机械手模型。

图1.2 机械手初始状态图1.3 旋转后的机械手状态
实验室中实物机械手总共有六个关节，也就是说该机械手有六个自由度，每个关节由一个电机来控制，每个关节在空间的运动范围为-90度到+90度，在试验模拟中为了简化系统，
我们使用了四个自由度的模型系统，在Matlab Simulink仿真实验中，只需要实时给出每个关节的旋转角度就可以动态地控制机械手的运动状态。

从上面的模型示意图中我们可以很清楚的看到，该机械手的模型分为底座，四个关节以及顶端的画笔，我们重点来弄清楚四个关节的运动情况，地下的红色小立方体可以在水平面的方向左右(即顺时针或逆时针)旋转正负90度，其余的三个颜色分别标记为绿色、蓝色和灰色的三个立方体，它们可以绕着各自的底平面中心点前后旋转运动（视线正视方向），其运动幅度均为正负90度。

1.3 机械手模型的建立
机械手模型的建立需要VRML语言支持，具体的设计请参考《VRML虚拟现实应用技术》张德丰著，电子工业出版社，在这部分将简要的介绍有关VRML编程的基本知识。

VRML语言其实非常简单，是一种描述性的解释语言，VRML的对象称之为节点，子节点的集合可以构成复杂的景物，节点可以通过实例得到复用，通过路由或者script脚本语言的交互可以生成更加复杂逼真的动态虚拟世界。

例如底盘模型是一个圆锥体和圆柱体的组合，相关的VRML代码如下所示。

############################################################################# DEF Hand Transform #定义一个形变
{
children #并列的子节点
[
#Base : a cone and a cylinder
Shape #定义一个形体
{
appearance Appearance #外观节点
{
material Material # 材质节点
{
#颜色属性设置
diffuseColor 1.0 0.0 0.0
emissiveColor 0.1 0.1 0.1
}
}
geometry Cone #定义一个圆锥体
{
#圆锥体的属性设置
bottomRadius 0.5
height 0.3
}
}
Shape #定义一个形体
{
appearance Appearance #外观节点
{
material DEF White Material # 材质节点
{
#颜色属性设置
diffuseColor 1.0 1.0 1.0
emissiveColor 0.1 0.2 0.2
}
}
geometry Cylinder #定义一个圆柱体
{
#圆柱体的属性设置
radius 0.4
height 0.29
}
}
]
}
############################################################################### 从以上的代码片段来看，我们很容易使用VRML建模语言快速高效地得到我们所需要的模型，是一种优秀的所见即所得方式。

模型建立最核心的部分是，我们需要在相关关节适当的地方放置一个CylinderSensor感知器，然后通过ROUTE的方式将感知到的旋转角度路由给相应的关节，这样就可以控制不同关节的旋转运动状态。

具体的例子如下面代码片段所示。

#########################################################
ROUTE MoveFirstArm.rotation_changed TO BaseJoint.rotation
ROUTE MoveSecondArm.rotation_changed TO SecondJoint.rotation
ROUTE MoveThirdArm.rotation_changed TO ThirdJoint.rotation
ROUTE MoveForthArm.rotation_changed TO ForthJoint.rotation
ROUTE MoveHand.translation_changed TO Hand.translation
在VrmlPad环境中，我们可以对相应的代码进行编辑调试及运行，使用VrmlPad软件可以大大简化我们的编程工作量，所以强烈建议使用该开发环境而非简单的文本编辑器。

二、机械手运动仿真
Matlab中的Simulink软件模块可以对动态系统进行仿真分析，可以使用该模块来对现实中真实系统进行建模，用虚拟的方法实现功能上等价的系统。

有关Simulink模块的详细介绍请查看Matlab自带的帮助文档。

2.1 机械手仿真模拟图
机械手仿真模拟模块图m.mdl如图2.2所示。

该模拟图的绘制是在Matlab环境下实现的,在Matlab命令行输入simulink即可打开仿真模块,然后选择相应的控件,设置参数以及连线即可以实现简单的仿真模拟图的绘制,具体请参考Matlab模拟仿真工具箱帮助文档。

图2.2 机械手仿真模拟图m.mdl
下面我们分别来介绍有四个输出分别为Out1到Out4的Pose模块，以及有四个输入的VR Sink模拟仿真模块。

2.2.1 Pose模块
Pose模块实际上是一个子系统模块, 其功能是从工作区间获得输入，然后格式化输出四个关节节点旋转的角度值, Pose子系统的模块如图2.3所示。

图2.3 Pose子系统图示
具体来讲，左边OrderSignal为命令信号, 命令信号的来源为控制台的workspace, 经过一个四路分解为四个角度值angle1, angle2, angle3和angle4并写入保存到当前workspace, 然后再经过一个简单的复合为相应的四个角度输出值Out1，Out2，Out3和Out4。

右边的[0 1 0]指的是相应的旋转轴，而[0 1 0]下面的求和符号代表的是绕相应旋转轴旋转的角度值
2.2.2 VR Sink模块
VR Sink模块是一个Simulink仿真模块, 该模块的四个输入端分别连到VRML仿真文件Machine_hand.wrl中定义的四个关节点的旋转角度，右键打开VR Sink参数设置，可以看到如图2.4所示的对话框。

图 2.4 VR Sink参数设置
参数设置说明如下：在Fields Written 下面是四个参数的设置，必须与VRML文件中的相应关节点一致，World File Name下的Machine_hand.wrl是相应的VRML仿真文件名。

这样左边的Pose子系统将工作区的命令信号分解为相应的角度值分别连到仿真文件相应的关节点，这样一个命令序列随着时间的变化就可以达到仿真实验的目的效果。

2.3 主程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
add_path; %添加相应的文件夹路径
unit = 100;
temp = fromfile('cmd.txt',unit); %将预先得到的命令文件解析为相应的角度值
leng = size(temp,2);
stoptime = unit*leng/1000;
OrderSignal.signals.values = temp'; %设置OrderSinal的信号源
OrderSignal.time = [0:unit/1000:stoptime-unit/1000]'; %设置时间
load_system('m'); % 装载m.mdl仿真图
set_param('m','StopTime',num2str(stoptime)); %设置仿真时间
open_system('m/VR Sink'); %打开仿真环境，开始模拟仿真实验%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
接下来介绍一下命令文件cmd.txt中的命令格式，从文件中取出任意一条命令格式如下：
T4800#1P1500S300#2P1500S300#4P1500S300#3P1500S300#6P1500S300#5P1000S300
每一条命令对应着机械手的各个关节在某一时刻的状态，从上面的命令格式我们可以看出，红色标记的T4800指的是上一个状态到这条指令的状态的变化时间，本命令为4800毫秒
绿色标记的#1P1500指的是关节1的状态值为1500，状态的变化范围为500-2500，其中500对应-90度而2500对应+90度，1500为中间状态，对应0度角，其他关节类似。

蓝色标记的S300为机械手臂关节1的步进电机的速度调节值，一般默认设置为300，在此不做过多解释。

三、使虚拟机械手沿任意两点画直线
3.1 程序描述
VRML三维虚拟手臂模型及Simulink仿真环境建立后，整个matlab程序在此基础之上完成的功能可以用一句话总结：输入起始和终止两点，使仿真环境中的虚拟手臂沿着两点确定的直线，从起点移动到终点。

3.2 实现步骤
3.2 .1 建立世界坐标系
构建出机械手臂的三维虚拟模型后，首先最基础的工作是建立这样一个三维世界坐标系，以机械手底座的中心为坐标原点，整个手臂竖立时正好处在Z轴正方向上，而X轴正方向指向外，Y轴正方向指向右。

在坐标系中，整个手臂竖立时，从下往上三截部分长度分别为0.5，0.5，0.625个单位（最后一截包含了手臂头，所以略长）。

手臂顶端为我们需要关注的目标点，按照建立的坐标系，整个手臂竖立时，手臂顶端的坐标点为（0，0，1.625）,而此时四个关节对应的旋转角度均为90°，即该坐标点对应的关节角度向量为（90，90，90，90）。

下图3.1为我们建立的世界坐标系的示意图。

图 3.1 世界坐标系示意图
3.2.2建立坐标点与关节姿态的对应关系（formAng函数）
世界坐标系中每一个坐标对应一个四维的关节角度参数向量，我们首先需要解决的问题是根据已知的三维坐标点，求得对应的关节姿态，这正是basic/目录下formAng函数所完成的功能，formAng函数也是最基础、最重要的一个函数。

算法的基本思想是：首先构建一个标准用以衡量给定坐标点与任意一个关节姿态的匹配程度，然后对于给定坐标点，参考前一个点确定一个范围，搜索该范围内的所有关节姿态位置，优化的找出其中与给定点最匹配的位置，得出此时四个关节的旋转角度，这里主要调用了matlab自带的lsqnonlin函数。

更详细的内容可以参看下面的一些代码清单。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [angle,resnorm,flag] = formAng(P,K0)
函数根据给定点P，通过优化方法找到对应的四个关节旋转角度。

输入参数P 为：给定的世界坐标系中的一点。

输入参数K0为：进行优化方法估计时的搜索起点，默认为[0 0 0 0]，表示四个关节的
旋转角度均为0°。

输出参数angle为：得到的四维关节旋转角度向量，形如[90 90 80 65]，向量中的四个值
依次表示四个关节的旋转角度。

输出参数resnorm为：进行优化方法估计的误差。

输出参数flag 为：函数运行成功与否的标识，值为1时表示运行成功，值为0时表示
运行失败。

[angle ,resnorm] = lsqnonlin(@(K0)formEnd(K0,P),K0);
代码在上面一条语句中调用了核心的lsqnonlin函数，这是matlab自带的进行优化搜索数据拟合的一个函数，构建的formEnd函数用来衡量每个搜索点与目标点之间的差距，K0为搜索起点，更详细的信息可以参考matlab中help文档中关于lsqnonlin的注释。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
下面我们重点看看formAng中构建的子函数formEmd：%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function gap = formEnd(theta,G)
函数用于计算一个给定四维关节旋转角度向量与一个给定坐标点之间的差距，用以衡量他们是否相匹配。

输入参数theta为：给定的四维关节旋转角度向量，向量的四个值依次为四个关节的旋转
角度。

输入参数G为：给定的世界坐标系中的一点。

输出参数gap：输出的差距度量，用于衡量给定关节向量与给定坐标点之间的匹配对应度，数值越小，表示对应程度越高。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 这是formEnd中主要的代码，作用是根据给定的四个关节角度（theta_A，theta_B，theta_C，% theta_D）计算手臂顶端在世界坐标系中的坐标（pos_E）。

pos_C_in_B = [0 0 0.5]';
pos_D_in_C = [0 0 0.5]';
pos_E_in_D = [0 0 0.645]';
% 上面的语句对于机械手顶端，及从上往下的第一第二关节做了一个坐标映射，得到新的% 相对坐标，以配合下面构建的旋转矩阵进行计算。

% the Rotate matrix;
% x->y
Rotate_A = [cos(theta_A) -sin(theta_A) 0;
sin(theta_A) cos(theta_A) 0;
0 0 1];
% z->x
Rotate_B = [cos(theta_B) 0 sin(theta_B);
0 1 0;
-sin(theta_B) 0 cos(theta_B)];
% z->-x
Rotate_C = [cos(theta_C) 0 -sin(theta_C);
0 1 0;
sin(theta_C) 0 cos(theta_C)];
% z->x
Rotate_D = [cos(theta_D) 0 sin(theta_D);
0 1 0;
-sin(theta_D) 0 cos(theta_D)];
% 上面的语句由四个关节的旋转角度分别构建了四个旋转矩阵，用每个关节的顶端坐标乘% 以相应的旋转矩阵，即可得到旋转相应角度后该关节顶端的新坐标。

% caculate the pos of the point E.
pos_E =
Rotate_A*Rotate_B*(Rotate_C*(Rotate_D*pos_E_in_D+pos_D_in_C)+pos_ C_in_B);
% 最后即可计算得到按给定四个关节角度旋转后得到的手臂顶端坐标。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
延伸的几点想法：
(1) 能否直接根据坐标点，求出关节的旋转角度，而不用搜索？
设空间坐标点为(x,y,z), 则arccos(/x是否为底盘平面转动的角度？
对于由2根刚性的棍组成的系统，在一个平面内的状态，是否有如下图示状态：
即：以原点为圆心，绘制的圆是第一根棍的端点所能到达的位置；以目标点为圆心的第二个圆，是要求的第二根棍的一端点应处的位置，两个圆的交点，即为两根棍的共同点。

下面是三根棍的一种情况。

虽然可能有多种答案，但我们只要给出其中一种即可。

（2）即使使用搜索技术，也可以将所有可能的情况存储起来，即不同角度组合情况下，目标点的坐标（相当于四维数组），然后从数组中找与给定点最近的目标。

使用该方法可避免对不同目标点重复计算的问题；此外还可以使用索引等技术，避免在整个数组中搜索。

3.2.3由给定两点求移动手臂所需的指令参数文件（create_order_txt 函数）
此外basic/目录下的另一个函数create_order_txt完成的函数功能为，根据给定的起始点与终止点，插值找出其确定的直线上所有点，进一步求出各个点对应的关节角度向量，并转化为规定的指令格式依次写到txt文件中，即为根目录下的cmd.txt文件。

函数主要就是调用了上一部分介绍的formAng函数。

另外，我们需要将关节角度转化为关节指令参数，在机械手程序中，关节角度的参数范围为500至2500，500对应0°，2500对应180°，依次递增，例如90°即对应为参数1500。

最后在cmd.txt文件中，我们写入形如
“#1P1201S300#2P1366S300#4P1101S300#3P2038S300#6P1500S300#5P1000S3 00”的指令格式，前面四个#1P，#2P，#4P，#3P后面所接即为我们所求得的指令参数，最后两个#6P，#5P后面所接为不变的默认值。

3.2.4从指令参数文件（cmd.txt）提取指令信息并输入到仿真系统中
完成实验
根目录下的formfile和PraseOrder函数，则是在我们之前工作基础上打开cmd.txt
文件，并解析其中如上所示的指令。

PraseOrder函数的输入即为一行行的指令，函数输出为从每行指令中提取出来的角度，时间等各个参数。

Fromfile函数的输入即为已经建立的cmd.txt文件，针对每一行调用PraseOrder函数，获得其指令参数，并按照仿真系统的参数格式构建参数变量。

接下来在主干函数demo_drawline函数中调用formfile打开cmd.txt文件并提取其中信息生成解析为角度，时间等，进一步设置仿真环境中的信号源，时间等参数，装载好m.mdl 仿真图（load_system('m')），最后打开虚拟仿真环境（open_system('m/VR Sink')）,就能在虚拟仿真环境中开始模拟仿真实验，使虚拟机械手臂从我们指定的起点沿直线移动到指定终点了。

3.2.4从指令参数文件（cmd.txt）提取指令信息并输入到仿真系统中完成实验
了解整个matlab程序的框架及功能后，控制程序的运行就非常简单。

当根目录的cmd.txt 文件已经写好存在时，运行根目录下的demo_drawline就可以打开解析cmd.txt文件，自动进行一些参数设置后，进一步打开三维虚拟仿真环境，在系统中就能控制虚拟手臂从给定起点按直线移动到给定终点了。

关于cmd.txt文件，只需运行basic/目录下的create_order_txt 函数，输入符合要求的起始两点，即可得到使机械手沿起止两点直线移动的指令集，并写入到生成的cmd.txt文件中。

四、实物机械手
实验室的实物机械手由六个伺服电机驱动，在空间上即为六个自由度，下面图4.1给出的是实物机械手原型。

图4.1 实验室机械手实物图
除了上图所示六个自由度的机械手外，还有一个控制电路板，如图4.2所示，具体的接线方式请查看机械手相应的帮助文档。

图 4.2 机械手控制电路板
图4.3 显示的是机械手配套的调试软件界面示意图，具体操作及功能详见相关说明书。

图 4.3 机械手调试软件界面
图4.4显示的是机械手配套的上位机控制软件界面，具体操作及功能详见相关说明书。

图4.4 机械手控制软件界面。