水下相关组件单棒转移机械手的轨迹规划与控制

张斌，陈建文，冷慧玲，祝沛桢

（核动力运行研究所，武汉 430200）

0 引言

核电机组的长期运行会产生大量废弃的燃料相关组件，废弃的燃料相关组件存放于乏燃料池中，而乏燃料水池可用的有效空格架数量有限。为了最大限度地减小相关组件的贮存空间，研制并应用水下立式缩容处理机器人对相关组件进行缩容处理[1]。缩容机器人的液压剪切机构在水下剪切相关组件单棒，剪断的单棒需要通过单棒转移机械手进行转移和分类存储。

相关组件具有一定的辐照剂量，缩容工艺需通过水屏蔽，在水下进行缩容作业，单棒转移机械手在操作单棒时会受流体作用力影响。单棒在水下的运动问题属于圆柱绕流[2]问题，一方面绕流阻力会限制单棒的运动速度，另一方面单棒运动可能产生流致振动[3]，会影响单棒转运的稳定性和可靠性。

针对单棒转移机械手在水下操作存在的问题，本文对单棒转移机械手的末端轨迹规划进行研究并设计机械手控制系统确保机器人的平稳运动，实现缩容工艺中单棒转移机器手的稳定搬运和存储。

1 系统结构及参数

单棒转移机械手是水下立式缩容处理机器人的组成部分之一。水下立式缩容处理机器人系统整体位于乏池水下约10 m水深环境中，其中水体温度约为50 ℃。立式缩容处理机器人系统组成包括液压剪切机构、组件旋转机构、存储容器倾斜机构、组件连接柄转移机械手和单棒转移机械手等。液压剪切机构用于剪切相关组件单棒，其刀具位置可调整，组件旋转机构用于旋转燃料组件，容器倾斜机构用于倾斜存储容器，可调整其倾斜角度，连接柄转移机械手用于转运燃料组件上方连接柄，具有2个移动自由度。单棒转移机械手用于夹持并稳定转运单棒，具有3个移动自由度和1个夹持自由度。水下立式缩容处理机器人整体如图1所示。

图1 水下立式缩容处理机器人

单棒转移机械手结构如图2所示。机械手共有4个关节，升降关节用于驱动机械手整体在垂直方向的升降；旋转关节用于驱动机械手在水平面上旋转，伸缩关节驱动机械臂在水平面上进行伸长和收缩，夹持关节为机械手爪，用于护持和夹持单棒。

图2 单棒转移机械手

4个关节均通过直流伺服电动机驱动，机械手升降关节、旋转关节和伸缩关节的运动直接确定了其末端位置。各个关节的相关参数如表1所示。

表1 单棒转移机械手关节参数

2 建模分析

为解决单棒转移机械手在水下操作单棒的问题，需建立单棒转移机械手的正、逆运动学模型和微分运动学模型，用于对机械手进行末端轨迹规划和运动控制。同时需建模分析水流对单棒运动造成的阻力和振动影响，设计的控制器需在水流影响下保持稳定性。

2.1 运动学模型

根据单棒转移机械手的结构参数，对其进行运动学分析。夹爪末端坐标用向量X表示，3个电动机角度用向量q表示，采用几何法，根据单棒转移机械手的结构特点进行分析，则有X=T（q），其中：

2.2 微分运动学模型

将X=T（q）两边微分，单棒转移机械手的微分运动学方程可写成δX=J（q）δq，由T（q），可计算J（q）如下：

J（q）表示单棒转移机械手操作空间与关节空间的速度转换关系，为机械手的速度雅可比矩阵，J（q）T表示单棒转移机械手操作空间与关节空间的力、力矩转换关系，为机械手的力雅可比矩阵。

2.3 单棒绕流阻力模型

单棒转移机械手夹持相关组件单棒在水下运动时，机械臂和单棒均会受到水体作用力的影响。机械臂结构健壮，水体作用力对其干扰作用不大，而单棒整体呈细长棒状，通过一个夹持点被机械臂夹持，受水流影响可能会产生弯曲或夹持脱落，需对单棒受水体作用力模型进行分析。

单棒在乏池的运动过程属于典型的绕流问题，水体与单棒的相对运动会产生绕流阻力，根据绕流公式，计算绕流阻力、绕流升力和流致振动频率：

式中：FD为绕流阻力；FL为绕流升力；f为绕流升力的脉动频率；CD为绕流阻力系数；CL为绕流升力系数；Sr为斯特劳哈数；CD、CL、Sr与雷诺数Re有关；ρ为水流密度；v为单棒相对水池的运动速度；d为单棒直径；A为单棒在垂直于运动方向平面上的投影面积。

计算单棒运动时的雷诺数：

式中：μ为流体动力黏性系数；L为流场特征长度。

3 轨迹规划与控制

3.1 轨迹规划

为使单棒转移机械手在水下操作单棒时具有较好的稳定性和运动平稳性，在轨迹规划时需考虑运动速度与水体绕流阻力的关系，选择合适的运动速度，兼顾转移效率和稳定性。同时为保证夹持的可靠性，转移轨迹的位置、速度、加速度应连续且变化平缓。

贝塞尔曲线[4]轨迹一定经过起始点P0和终点，轨迹处处连续，且其导数仍为贝塞尔曲线，用贝塞尔曲线规划的轨迹，其位置、速度、加速度连续且变化平缓。采用贝塞尔曲线规划运动轨迹，通过标定与示教设置控制点，确保机械手运动轨迹符合要求。

设轨迹规划时给定了n个控制点P0、P1、P2…Pn，其贝塞尔曲线的一般表达式可写为

式中：CP0为初始状态时机械臂末端坐标；CP5为机械臂的目标位置。根据各控制点，生成的机械臂末端轨迹如图3 所示，机械臂升降、旋转、伸缩关节的位置和速度规划 如 图4、图5、图6所示。

图3 单棒转移机械手臂末端运动轨迹

图4 升降关节规划位置和规划速度

图5 旋转关节规划位置和规划速度

图6 伸缩关节规划位置和规划速度

3.2 控制系统结构

为避免绕流阻力对单棒转移机械手臂的运动产生不利影响，采用前馈控制的思想，通过前馈补偿绕流阻力[5]，在较大程度上消除机械手臂运动过程中的绕流阻力的影响。同时为提高控制系统抵抗外部干扰的能力，采用PD控制算法[6]对机械手臂末端位置进行控制。

设计带前馈控制的控制率u计算公式为

控制系统框图如图7所示。

图7 控制系统框图

4 仿真分析

为确保单棒转移机械手轨迹规划和控制算法的有效性，采用仿真分析对机械手的运动轨迹和控制算法进行验证并优化。

仿真计算平台为Matlab 2019，采用Simulink完成规划和控制算法，采用Simscape模块进行物理仿真和三维可视化验证。

4.1 仿真模型

搭建Simscape仿真模型如图8所示，三维可视化效果如图9所示，模型的输入为各关节电动机力/转矩，模型输出为各关节当前位置和速度，且添加了外部绕流阻力和绕流升力作用。

图8 Simscape单棒转移机械手仿真模型

图9 Simscape单棒转移机械手三维可视化

根据Simscape模型，在Simulink中构件控制系统如图10所示。其中，PD位置控制器模块的输入为贝塞尔轨迹，输出为各关节力/力矩。绕流力补偿模块根据规划速度计算了绕流力对应的力/力矩补偿值，流体作用力计算模块根据单棒运动速度计算了实时的绕流阻力和绕流升力，并将其作用于Simscape模型中。

图10 Simulink单棒转移机械手控制系统

4.2 运动控制仿真

为验证单棒转移机械手运动控制系统的有效性，对机械手的各关节进行了关节位置跟踪仿真，跟踪信号为正弦信号，其升降、旋转、伸缩关节的跟踪效果如图11、图12、图13所示。由运动控制仿真结果可知，位置跟踪精度较好，具有轻微的滞后，滞后时间约为0.05 s。

图11 升降关节正弦跟踪

图12 旋转关节正弦跟踪

图13 伸缩关节正弦跟踪

4.3 绕流作用力仿真

单棒在水下运动速度的不同，水体对单棒的作用力也不同，为选取合适的单棒运动速度，对不同速度下的单棒绕流阻力、绕流升力和流致振动频率进行了仿真研究，取单棒的运动速度范围为0～5 m/s，仿真结果如图14、图15、图16所示。可以看到在低速时绕流阻力和绕流升力很小，随着速度增大绕流阻力和绕流升力呈平方关系迅速增大，振动频率呈比例关系逐渐增大。

图14 绕流阻力-速度

图15 绕流升力-速度图

图16 流致振动频率-速度图

4.4 综合仿真

根据规划的机械手末端轨迹，采用带前馈补偿的PD控制器对单棒转移机械手进行控制，计算单棒运动过程中绕流作用力，并将作用力施加在单棒上，进行单棒转移机械手运动的综合仿真，机械手各关节轨迹跟踪效果如图17、图18、图19所示。单棒运动过程中绕流阻力、升力和流致振动频率如图20、图21、图22所示。

图17 升降关节规划轨迹跟踪图

图18 旋转关节规划轨迹跟踪图

图19 伸缩关节规划轨迹跟踪图

图20 单棒扰流阻力图

图21 单棒扰流升力图

图22 单棒流致振动频率图

根据仿真结果，带前馈补偿的PD控制器对机械手的控制具有较好的效果，跟踪误差和时间滞后都很小，验证了带补偿的PD控制器的控制效果。机械手在按照规划的轨迹运动时，其受到的绕流阻力最大为0.3 N，最大绕流升力为1.7×10-2N，最大振动频率为2.8 Hz。表明规划的轨迹没有引起明显绕流作用力，对单棒的转运具有较好的效果。

5 结论

本文针对相关组件水下立式缩容工艺中单棒水下转移问题，提出一种采用贝塞尔曲线进行轨迹规划的方法，并设计了带绕流阻力前馈的PD控制算法，用于机械手末端的运动控制。通过机械手运动仿真验证了规划的轨迹方法在位置、速度、加速度上均连续，单棒运转平稳，同时也验证了控制算法可有效补偿绕流作用力，具有较好的稳定性和可靠性。

在后续的研究工作中，可进一步优化单棒的运动速度，在转运效率和稳定性的约束条件下研究机械手的最佳转运速度，同时可进一步研究单棒运转中的流致振动问题，设计稳定性、鲁棒性更佳的控制系统，并通过试验研究进行验证和优化。