六自由度机械手控制系统设计与仿真*

国核电站运行服务技术公司 王一帆 赵琛 袁梦扬 关光

针对常用的反应堆压力容器检测用六自由度机械手进行了运动学和控制研究。对机械手的控制需求进行了系统介绍，通过系统模型导入实现机械手在核压力容器内的三维模型运动仿真和控制，并实现自动检测路径的轨迹规划和姿态规划，通过系统通讯调试和仿真测试，验证了控制系统设计的合理性。

核反应堆压力容器是核电厂中保证核电设备安全的关键设备。在核电厂的检验规范和大纲中，对反应堆压力容器和顶盖的焊缝及其他部位提出了无损检测的强制性要求。受核电环境的影响，检测必须依靠多轴机械手来完成，采用多轴机械手首先是需要考虑安全。压力堆反应容器和顶盖不允许出现碰撞、划伤等，否则会对设备运行带来极大危险。由于多轴机械手受空间限制和碰撞安全等要素影响，不仅需要多轴机械手具有高度灵活性，还需要在检测前对多轴机械手的检测路径进行规划仿真。通过多轴机械手运动学和动力学仿真软件对多轴机械手进行检测过程的运动仿真和路径规划，确保操作过程的高效性、安全性和有效性。

1 机械手系统控制及仿真模拟设计

1.1 系统控制和仿真需求

系统的控制与仿真即在导入反应堆压力容器/顶盖模型、机械手、扫查器三维仿真模型后，根据机械手各个电机位置反馈的信息可使仿真模型与机械手系统在容器中的实际位置相匹配。根据人机界面的参数输入，指定机械手进行相应的位姿变换，通过该六轴机械手运动学分析模块，即可将机械手到达指定位置和姿态，同时人机界面上的仿真模型也会实时显示实际机械手的运动状态。同时，仿真操作和实际运动操作可根据需求进行自由切换。

1.2 总体设计说明

六轴机械手运动学和动力学仿真软件系统适用于Windows7、Windows10等计算机系统，依托于Visual Studio2019为开发平台，采用C#语言编写，通过编程实现对模型的仿真。本文以六轴机械手运动学和动力学仿真软件系统为立足点，以实现核反应堆压力容器的机械手智能化仿真检测。通过建立（或兼容导入）压力容器、顶盖等被检对象三维模型、机械手模型和端部效应器模型，可在虚拟模式下实现机械手的模拟检测并获得可行的检测方案，并可根据实际机械手关节臂长度进行不同机械手的选择，生成机械手三维运动图，模拟机械手控制的各种模式，提供仿真模拟运动轨迹规划、验证。

2 仿真系统功能实现

系统仿真功能的实现通过对各个模块开发来展开，采用UG、CATIA、Pro-E等三维建模软件构件模型，主要包括：机械手、压力容器及检测设备模型。在模型导入前建立模型的XML文档，以此设置模型初始化空间坐标及模型本身位姿。对于机械手模型参数的修改，在XML文件中可以修改机械手的名称、质量等信息，同时可以改变各关节可视化的颜色，设定D-H参数。

2.1 核压力容器模型

核压力容器常见于CAP1400、AP1000和CPR1000堆型。如图1所示，为AP1000压水堆压力容器的大致结构。

图1 核反应堆压力容器结构图Fig.1 Structural diagram of nuclear reactor pressure vessel

2.2 检测机械手模型

如图2所示为所仿真的模型为六自由度机械手，该机械手由六个关节组成。其中，前四个关节决定机械手在空间中的位置，后两关节则决定机械手末端的姿态，在机械手末端的法兰盘与扫查器法兰盘相接，用于安装不同类型的扫查器。

图2 六轴机械手结构图Fig.2 Structure diagram of six axis manipulator

从结构上看，该机械手运用开发式的运动链，通过多关节旋转连杆的方式组成。机械手的每一个关节均装有独立的伺服驱动器，通过关节的相对运动使得机械手以一定的运动轨迹和空间姿态到达预期位置，进而进行检测工作。由于各关节存在线缆拉扯、限位等运动限制，各关节的转角非360°的全周运动，而是通过多关节协调运动的方式进行机械手控制姿态变换。

2.3 三维模型实现

机械手在实际使用过程中，末端扫查器根据不同扫查对象会进行更换，故在三维模型的显示中，末端扫查器也需要随着使用场景的变化进行动态替换。采取方案为：在主界面左侧菜单栏中点击“模型选择”，调出不同的机械手模型和扫查器模型，选择所采用的模型，并设置相应的参数，点击“加载机械手模型”/“加载末端扫查器”即可完成模型更换，加载单元如图3所示。对模型的选择包含了图3中的单元模型选择列表，导入所需的模型单元(cell)，步骤如下：（1）在“模型选择”＞“选择单元模型”模块选择需要加载的单元；（2）选择好单元后点击“加载单元”实现单元导入。

图3 加载单元Fig.3 Loading unit

2.4 机械手仿真模型运动控制

六轴机械手仿真模型的运动选择有单轴点动和多轴联动两种。单轴点动，即各关节轴可相对于其他关节独立运动控制，单一选择关节以不同的速度进行角度旋转，实现机械手安装、多步骤手动调节机械手到达目标位置等功能；多轴联动，即机械手的多个关节在到达预设位姿时需采用笛卡尔坐标，同时驱动机械手的多个关节来运行控制，如图4所示。在多轴联动时，全局坐标分别对应机械手的整体坐标和压力容器的整体坐标，通过上下、前后、左右等全局坐标方式实现位置和角度的任意控制，多轴联动的主界面采用的“笛卡尔”选项控制界面，具备手动操作和数值输入，可同时输入速度和加速度来设置多轴联动的运行方式，具体步骤为：（1）打开笛卡尔控制面板；（2）在控制面板中选择平移维度“X Y Z”或者旋转维度“A B C”中的任意一个，选中之后的按钮颜色变暗；（3）“正向运动”或“负向运动”键中选择其中一个。同时，在运行控制时，鼠标点击不松时为连续运动方式，鼠标单击时则为单点运动方式。运动完毕后，六个关节轴的角度值将显示在运动控制界面的下方供操作人员观察，此外，为使机械手以到达指定位置并以指定姿态呈现，可通过数值输入功能设置运行参数精确控制，具体操作为：运动界面中选择相对位置、绝对位置和从文件选项，在“X Y Z”“A B C”六个数值的输入框中输入数据，键盘按下Enter键开始运动。

图4 多轴控制Fig.4 Multi axis control

3 自动检测路径规划及控制模块

3.1 自动检测路径规划及控制模块

根据实际检查需要，通过相应参数选择和设定实现压力容器各检查部位的扫查路径生成。并可实现扫查路径的单步、连续、中断续检等过程控制。自动扫查的路径是在输入关键参数之后自动生成的。机械手自动扫查过程是自动化控制中关键的一项内容。其中，自动扫查的路径是在输入关键参数之后自动生成的。（1）点击“Input parameters”按钮，弹出路径类型选择项；（2）选择需要生成路径的类型，例如：在这里想要生成圆形路径，点击“Circular path”，此时会在一边弹出提示输入关键参数的输入框；（3）输入关键参数信息，包括“Radgap”“Gap”“Dmin”“Dmax”“Height”“Rs” “Re”“Rotate”“Startp”“Endp”。最终，将会在仿真界面上自动显示出所绘制的圆形路径。如图5所示，在路径规划中，通过输入起始坐标数据、离散点数、圆半径，终点坐标数据、圆半径和中间间隔即可画出路径。其中圆半径是指需要检测的孔半径，中间间隔是指起始环与终点环之间需要添加的路径环数量。

图5 不同孔形状规划路径Fig.5 Planning paths for different hole shapes

3.2 轨迹规划与运动控制模块

轨迹规划的目的是为了让机械手以一定的方式进行末端直线和圆弧运动，以满足不同检测需求，可有效保证检测质量。在运行时，机械手的末端始终处于规划轨迹上，且末端扫查器也始终与前进的发现方向垂直，可防止机械手位姿混乱，同时节省大量的运动控制时间。因此，需要对机械手进行六自由度路径规划，保证位置和姿态满足任务需求。

3.2.1 直线路径规划

机械手的末端扫查器需要进行精细的扫查运动，需要保证机械手末端运动的平滑性，在该运动系统中，末端运动特点为如图6所示的梯形速度曲线方式进行。

图6 梯形速度曲线Fig.6 Trapezoidal velocity curve

（1）加速时间

t

确定：

式中，

vel

是运动前预设的末端速度，，

acc

是运动前预设的末端加速度。

（2）插补运动直线长度确定：

假设

P

和

P

分别代表起始点和终止点，两点间的距离则为：

（3）速度轨迹曲线的修订：

（4）每次运动步长的确定：

采用插补周期

T

作为定时插补，插补次数则为

根据图6中所呈现的机械手末端梯形速度曲线，可以得出插补运动至第k次是所得出的位移：

则下一步的位置为：

3.2.2 圆弧路径规划

在圆弧路径规划中，设定圆弧路径的起始点

P

、终止点

P

和路经点

P

，其中

P

与

P

和

P

不共线，通过这些点即可确定在仿真空间中的圆弧。设定以圆弧的圆心为原点，圆心相对起始点

P

的射线线为

X

轴，圆心相对终止点

P

的射线为

Y

轴，圆弧所在的

OXY

平面的法线为

Z

轴建立坐标系

O

-

X

Y

Z

，如图7所示。

图7 圆弧路径规划Fig.7 Arc path planning

图8 平面上的圆弧Fig.8 Arc on plane

则圆心角

θ

为：

图8作为平面上的圆弧，圆心角

θ

的梯形速度曲线也将按此圆弧进行变化。在此条件下，第k次插补时的圆心角即为

θ

，则在

O

-

X

Y

Z

坐标系中，六轴机械手的末端坐标为：

六轴机械手在世界坐标系中的坐标还需进行转换，转换后的坐标为：

3.2.3 姿态规划

六轴机械手姿态规划的进行采用单位四元数来表征，单位四元数的形式为

Q

={

ɛ

，

η

}，其中：

式中，

θ

为转动角度，

ε

为转轴方向。其对应的旋转矩阵为：

若给定旋转矩阵：

其对应的四元数为：

对于任意2个姿态，可以绕一轴线

ε

转动

θ

角从一个姿态

Q

移动到另一个姿态

Q

。转动角度

θ

和转轴方向

ε

满足：

由上式可以很方便地求出转动角度和转轴方向，然后进行插补。

自动检测的路径采用自主开发的平台来实现，机械手根据RRT、PRM以及他们的各种改进算法形成姿态规划的轨迹点，根据轨迹点即可生成一系列待执行的运动指令，以发送至机械手驱动控制器按照指定路径点运行，这些点明确了机械手在空间坐标系中的一些位置或关节角度，同时在自主开发平台轨迹规划中确定每个关节轴运行的速度和加速度，则该挑路径的时间信息也相对确定。自主开发平台的轨迹规划以能量最小等原则规划求解，通过单轴运动和多轴联动相互配合的方式，提供轨迹运动时各轴实时位置、角度、扭矩等相关参数的显示。

4 系统调试与仿真测试

4.1 系统通讯调试

在自主开发系统中，为方便后续进一步开发，对于下位机通讯系统，采用TCP/IP实现与下位机连接，在IP处输入IP地址，Port处输入端口号。该功能模块包括校零功能、数据同步功能以及上下位机数据传输可视化功能。实时提供各信号的传输状态，提供通讯及通讯保障，TCP通讯如图9所示。点击发送数据可以发送下位机所需各项数据，发送数据时间间隔通过定时器调节，最大程度实现虚拟与现实的实时运动，同时上位机可以接受下位机发送过来的指令，断开/重新连接功能按钮实现与下位机通讯断开与连接。

图9 TCP调试Fig.9 TCP debugging

4.2 系统仿真测试

本次测试覆盖的范围包含开发需求各大功能。测试方案：在UG、CATIA、Pro-E等三维建模软件构建三维几何模型，设置用于自由切换的单元模型共12个（对应于12个不同的组合：机械手本体+14种不同的末端扫查器+筒体）。设置模型导入的测试组主要为单元模型导入和机械手模型导入，并记录模型导入的空间位姿坐标变化情况以及响应时间等数据。

通过测试结果发现，且系统在任何情况下都能够快速地实现模型的导入，较少出现系统奔溃不成功等现象，对于不同的测试用模型，都能够具有100%的切换成功率，且在在高负荷运行等多种情况下，都能够实现正常的模型切换过程。如图10所示，机械手模型导切换延迟1s左右，单元模型切换延迟1min左右，符合操作运行状态下切换的要求。

图10 模型切换响应时间Fig.10 Model switching response time

5 结语

本文根据核电站换料大修期间反应堆压力容器无损检测六轴机械手的控制需求进行了系统总体设计，通过对核电站压力容器的导入和运用运动学和动力学的方法推导出的六轴机械手模型，并运动学和动力学方程的理论解析，规划自动检测路径的姿态和轨迹，以此设计出机械自动检测手直线、弧形规划路径和姿态规划策略。最后采用系统通讯和仿真测试的方式验证了系统设计的合理性，满足系统仿真运行要求。