蒸汽发生器二次侧检测用爬壁式移动机械臂控制系统设计

张 硕 吴洪明 孙健铨

1 武汉理工大学交通与物流工程学院 武汉 430063 2 广东省机器人与智能系统重点实验室 深圳 518055

3 中国科学院人机智能协同系统重点实验室 深圳 518055

0 引言

蒸汽发生器在核电站发电系统发挥着极其重要的作用。蒸汽发生器的下半部分为二次侧，其中排布的传热管在一回路压力边界处与许多零部件相连接，受到多种恶劣环境因素的影响（包括但不限于高温、高压、振动、应力和水化学等）。因此，易产生零部件的腐蚀损伤（如机械零件脱落和传热管破裂等），成为热交换设备中的薄弱环节。对蒸汽发生器进行预防检查是降低核电站事故发生概率的重要手段[1]。在蒸汽发生器二次侧的结构中，内部容许操作的空间极其有限，应充分考虑限制条件，选择爬壁机器人处理该类工程问题，可具备良好的可操作性。由于工作环境一般不会发生较大差异与变化，设计人员在充分了解作业环境后便可提出可行的机械方案，以解决机构设计问题，而在结构设计基础上延伸出的控制系统设计问题才是爬壁机器人作业的中心问题。

在爬壁机器人的控制系统设计研究中，国内外学者研究的侧重方向不同，提供了不同的研究内容。在移动机器人定位方面，针对机器人的内部传感器存在使用受限的影响，王子润等[2]采用了激光雷达与改进AMCL相结合的全局定位方法；路晨曦等[3]采用视觉信息与车体IMU 的定位方法，提升机器人在运动过程中的定位精度。在轨迹跟踪方面，滕昊等[4]在爬壁机器人运动学模型的基础上构建了双环轨迹跟踪控制器，提升机器人在跟踪过程中的动态品质；Boomeri V 等[5]采用拉格朗日方法对爬壁机器人进行了动力学分析，其中涵盖了负载加速力矩、科里奥利力、离心力、重力等因素；刘佳等[6]利用U-K 方程建立了爬壁机器人在预定轨迹下的解析动力学方程，消除了采用拉格朗日方法引入的拉格朗日算子。在软硬件设计方面，杨东宇等[7]介绍了以ROS 为上层软件控制器、以BeagleBone Black 嵌入式单板计算机为底层驱动控制器的设计方案；宋容[8]则在软硬件控制系统设计的基础之上提出了基于大数据聚类分析的设计方案，以期解决机器人的运动定位控制问题。

目前，国内外针对爬壁机器人的相关研究将爬壁机器人视作移动机器人，并从该角度入手进行定位、轨迹跟踪和软硬件的设计，但未形成从作业角度出发的控制系统细分设计思路。本文将结合蒸汽发生器二次侧检测用爬壁式移动机械臂的作业需求，总结出较全面地爬壁机器人控制系统设计路线，对爬壁式移动机械臂的控制系统设计方案进行介绍，为相关研究提供参考。

1 爬壁式移动机械臂运动学模型

如图1 所示，爬壁式移动机械臂由移动平台和二自由度机械臂组合而成。移动平台通过永磁式车轮吸附于蒸汽发生器二次侧结构的内壁，移动机构采用辅助支撑轮与双驱动轮相结合的方式，需要注意的是2 车轮并不同轴，轮距与轴距之比约等于5。在设计时采用了吸附力十分强劲的永磁铁，车轮与接触平面之间的摩擦极大，运动时产生的轴向滑移量较小。另外，考虑到轮距与轴距之比较大，为了降低分析的复杂度、简化运动模型，将图1 所示移动平台视为受到了图2 所示情况的非完整约束。

图1 爬壁式移动机械臂参数示意图

图2 移动平台非完整约束简化模型

在执行分析时，按照图3 所示方法建立机器人的运动坐标系，坐标系的原点建立在移动平台的运动中心处。具体的运动控制参数见图1，其运动控制变量有航向角θ、末端姿态φ、机械臂1θ1、机械臂2θ2、车轮角速度θl和θr等；操作空间坐标系下的特征点坐标有移动平台运动中心坐标为（x0，0，z0）、机械臂2 终点坐标为（x2，y2，z2）。另外，为了简化模型，将二自由度机械臂中关节2 的最远端点视为机械臂控制末端。

图3 爬壁式移动机械臂坐标系示意图

1）移动平台的非完整约束

移动平台的运动中心受到的非完整约束条件可表示为

由式（1）可知，在非完整约束下，移动平台的受控变量仅为2 个。将驱动车轮的控制角度θl和θr转化后，即可得到沿移动平台前进方向的线速度v和改变前进方向的航向角θ。

2）二自由度机械臂的完整约束

在二自由度机械臂中，转动关节仅具备单一可运动的自由度，故属于完整约束。机械臂末端相对于运动中心的坐标为

2 机器人系统总体控制框架设计

爬壁式移动机械臂是复合型机器人，由移动平台和二自由度机械臂复合而成，且增设了吸附功能。相较于传统的平面移动机器人、爬壁机器人、二自由度机械臂，爬壁式移动机械臂的控制系统的设计难度更大。为了降低系统设计难度，本文设计了图4 所示机器人控制系统，从物理连接的角度分为移动平台和二自由度机械臂2 个子系统；从控制层级的角度划分为上位机控制软件、定位系统和轨迹跟踪控制系统3 部分。上位机控制软件向机器人发送通信控制指令，部署于机器人的定位系统为机器人提供运动定位信息、轨迹跟踪控制系统，在运动定位信息的基础上进行闭环控制，实现了未知狭窄环境下的高精度作业。

图4 爬壁式移动机械臂控制系统总体框架

本文以单片机控制器为基础，搭建了爬壁式移动机械臂的基础功能。机器人控制系统采用分层设计方法，上层系统以机器人的运动学模型为基础设计上位机软件，底层控制子系统以机器人的闭环控制和定位功能为目标进行设计。二自由度机械臂和移动平台均选用STM32F103VET6 型单片机作为主控制器，采用串口通信方式与上位机软件进行实时通信。各子系统的单片机控制均采用中断处理方式，高频定时处理子系统内的传感器对应信息和闭环驱动控制的输出更新。

3 机器人定位系统设计

定位系统内对应的传感器安装示意图以及对应的测量参数如图5 所示。二自由度机械臂关节定位系统受限于机械结构的设计，在不同关节处采用不同的传感器定位方案。在车体一侧的机械臂关节1 处，采用电位器进行角度反馈；在靠近机器人末端的一侧，采用磁环＋ES49ESUA 的组合形式，以霍尔传感器检测磁环的磁场变化反馈实际电压输出。

图5 传感器布置与参数示意图

移动类机器人的运动定位方法有内部信息定位、外部信息定位、内外部信息结合定位3 种。考虑到永磁式车轮对车轮电动机编码器磁轮的影响、内部信息定位方法在运动距离较长时会场生较大的累计误差等因素，爬壁式移动机械臂选用外部信息定位方法处理。移动平台定位系统使用PAT9125EL 型光流传感器，通过对不锈钢材质的光滑工作平面进行表面图像信息采集，测量移动平台的运动信息；采用VL53-400S 型激光测距传感器测量移动平台两侧到容器的上下底面的距离；选用HWT31型传感器对移动平台的实时运动位姿进行检测。其中，光流传感器的测量数据受到诸多因素的影响，如传感器距离工作平面的距离、工作平面的表面特性等。经过多次数据采集实验发现，在运动一定距离后会产生误差变化，需要进行补足。

移动平台的工作平面上的坐标计算式为

式中：Δl为运动中心到激光测距传感器在机器人运动方向上的距离，取值为42 mm。

4 机器人轨迹跟踪控制系统设计

机器人的轨迹跟踪控制是实现机器人在作业空间内精准作业的核心内容，需将驱动关节的电动机与定位系统的反馈信息进行组合，从而实现机器人的位姿闭环控制。移动平台和机械臂关节1 的驱动电动机均采用Maxon 电动机，机械臂关节2 则采用微型直流减速电动机。

为了增强电动机力矩的响应特性，采用无需电动机模型的增量式（Proportional Integral Derivative，PID）控制算法，设计得出机器人驱动电动机的电流闭环控制控制器（见图6）。本文将各电动机的电流环驱动视为驱动控制系统，结合前述建立的2 个机器人定位子系统，综合成为2 个采用级联形式的底层轨迹跟踪控制子系统。

图6 各电动机的电流环控制框图

对于二自由度机械臂而言，在执行作业任务时需进行准确的位姿控制，选用位置式PID 控制算法设计位置环，并将其与电流环直接进行级联处理，如图7所示。

图7 二自由度机械臂位置环控制框图

移动平台的作业应从移动机器人的角度出发，由于非完整约束的存在，机器人的位形空间可达性将由移动平台的线速度和航向角共同完成。基于本研究对象，移动平台在执行巡检任务时的轨迹跟踪控制器需要对线速度和航向角进行分别的闭环控制，并综合成为一个控制器。其控制框图如图8 所示。

图8 移动平台巡检轨迹跟踪控制框图

爬壁式移动机械臂在进行特殊作业和检测任务时需对位姿进行微调，此时移动平台围绕运动中心进行旋转运动，改变机器人的航向角。本文直接选择在电流环控制的基础上进行航向角的位置环控制，控制框图如图9所示。

图9 移动平台航向角轨迹跟踪控制框图

5 基于爬壁式移动机械臂控制状态划分的上位机软件设计

前述分析给出了爬壁式移动机械臂在轨迹跟踪控制方面的控制算法设计思路。为了提升机器人的控制水平，增强在未知环境下的自动化控制适应能力，采用基于有限状态机的控制状态切换方法，对机器人进行控制状态划分。机器人在进入蒸汽发生器二次侧过程中有5 个状态。

状态1（Torque/Current Control Mode）：机器人在由手孔进入时，由于机械结构的限制，机械臂关节1 需要根据操作人员对关节施加力的方向进行随动，遇到极限位置即自动停止；机器人在进入蒸汽发生器二次侧并稳定吸附在内壁上后、等待作业的状态：

状态2（Manipulator Position Control Mode）：机械臂的位置控制；

状态3（Mobile Platform Position Control Mode）：移动平台的巡检控制；

状态4（Mobile Platform Angle Control Mode）：移动平台的航向角调整；

状态5（Stop Mode）：停止状态，等待外部通信指令。

状态1 执行结束后由外部通信指令触发后转至状态5，其余状态在自动执行控制指令后自动跳转至状态5，接受外部通信指令的触发。其中，外部通信指令优先级最高。

在使用上位机软件时，可进行机器人控制测试，可选择机器人的控制模式分别为自动巡检和手动测试。其中，选择手动测试时机器人执行过所设置参数的对应功能后将自动进入状态5。上位机软件与轨迹跟踪控制系统间采用RS485 通讯方式进行实时通信，以完成指令传达和传感器信息反馈。上位机软件通过传感器信息反馈实时监测机器人的位姿，结合运动学模型对机器人的进行状态监督，避免与工作环境之间发生碰撞。

6 总结

爬壁式移动机械臂控制系统的组成涵盖了大量软硬件设计。本文针对蒸汽发生器二次侧视频检查的需要，对爬壁式移动机械臂进行了运动定位、轨迹跟踪控制系统的设计，设计过程可归纳为爬壁类机器人的一般设计方法，包括运动学模型分析、控制系统总体框架设计、定位系统设计、轨迹跟踪控制系统设计等，并结合机器人的实际工况进行控制状态划分，设计了上位机控制软件。机器人定位技术、轨迹跟踪控制技术等机器人相关技术经过融合后，为爬壁式移动机械臂在狭小空间内的准确运动控制提供了有力保障，有助于扩大机器人的安全运行范围，提升综合作业效率，并为今后无人化自动视频检查建立实现基础。