基于TOF传感器的堵板拆装机器人水室定位

陈柳妍，王湘江，冯栋彦

（南华大学 机械工程学院，湖南 衡阳 421001）

0 引言

随着科学技术的进步，对电力的需求也在不断增加，国家提出了大力发展核能工业的方针和政策[1]，以满足我国核电产业的迅猛发展和核电市场的扩大。加强核设备的安全性和可靠度，使操作人员受到的辐射剂量最小化，改善操作人员的工作条件，是中国核电工业急需解决的难题。为保障核电站的正常运行，必须对核电站的关键设施进行定期检修，因此开发智能、性能先进、可完全取代人力的核电站机器人是当前的研究热点，期望核电站机器人能实现日常维护、事故处理、核废物处理等工作。

蒸汽发生器的管道堵板工作，是核电站每次大修必须执行的工作项目，每次大修至少需完成3次堵板拆装作业。据统计，每执行一次堵板工作约受到3 mSv剂量的照射，给现场的施工人员辐射量控制带来了一定的压力，因此，目前亟需机器人取代人工作业完成检修的工作模式。

在已设计的十自由度一次侧堵板拆装机器人进行堵板作业时，由于机器臂末端执行机构的尺寸仅略小于蒸汽发生器的人孔孔径，并且机械臂拆装堵板时的末端精度需±0.5 mm，因此对于一次侧堵板拆装机器人的工作位置要求较高。只有确定了机器人执行堵板作业时的工作位置，才能方便后续作业。为此，必须对其进行精确的定位及运动轨迹规划。此外，为适应核电站不同型号尺寸蒸汽发生器的堵板作业，需确定一套便捷准确、泛用性高、功能完备的堵板机器人室外定位方案及设备，并对此种定位方法进行误差分析以保证测量精度。

目前室外定位技术迅速发展，但由于室内环境通常狭小复杂，定位结果往往未能差强人意。用于高精度室内定位的技术一般有：基于WiFi的室内定位技术、惯导传感器室内定位技术[2]、地磁强度室内定位技术、红外线定位技术、超宽带室内定位技术等。张俊龙等[3]利用位置特征信息识别通过WiFi网络进行定位，达到基于每个AP热定覆盖面积100 m2的情况下，精度为3 m；原玉磊等[4]使用红外线和超声波的定位技术，最小误差可以达到4.43 cm；杨潇等[5]提出一种基于超宽带技术的人员定位方法，最终定位精度提高了5%。孙建伟等[6]通过多源信息融合室内定位的方法减小复杂空间结构对室内定位精度的影响，得到室内定位精度的平均偏差为0.601 m。红外测距技术较为成熟，且使用与结果分析较为简便直接，本文直接采用红外测距中的激光位移传感器进行实验研究。

1 激光位移传感器测量原理

当前市面上的机械式激光雷达采用了多种测距方法[7-13]，其主要有两种：激光三角测距和TOF测距。激光三角法[14]的测量原理是将红外激光从雷达测距模块中发射出去，入射到被测对象上后，通过接收透镜将一部分散射光聚集在线阵图像传感器（CCD/CMOS）上。与其相比，TOF激光位移传感器具有更小的外形尺寸、更宽的动态感应范围和适应各种工作环境的能力，不仅如此，TOF激光位移传感器还具有低功耗、小尺寸、高精度、高速度、高安全性和远程测量的优势，是本实验最理想的测距方式。

TOF传感器测距时，驱动激光器发射一束瞬时功率极大[15]但持续时间极短的光脉冲，同时启动传感器内部的计时单元，光脉冲经过发射电路到测量物体表面后四散开来。接收光路通过光电器件将经过接收光路的散射光能量转化为光电流，再经过回波信号处理电路转化为电压信号，经过放大及调理后产生电脉冲，触发计时单元停止计时。计时单元记录的时间间隔即为激光脉冲从发射到返回的时间，与光速相乘再除以2，就是传感器到被测物体的距离。

图1 TOF激光位移传感器测量原理

2 蒸汽发生器与机器人空间模型

2.1 蒸汽发生器与机器人坐标系建立

图2所示为安装一次侧堵板的蒸汽发生器，由于机械臂实验环境极其狭小，在仿真模拟和试验中，唯有当大电缸经过人孔轴线时，机械臂方能顺利通过人孔进行后续拆装工作，因此，电缸与人孔轴线重合时为最佳实验位置。

图2 蒸汽发生器与机器人空间坐标系

蒸发发生器模型为半径1895 mm球体的一部分，因此建立一个以球心为坐标原点的三维坐标系xyz，然后在xyz三维坐标系基础上平移dx、dy、dz，以大电缸与小车平面交点为原点建立三维坐标系x′y′z′。

2.2 蒸汽发生器球面与人孔面方程

在以球面A球心为坐标原点的三维坐标系下，M1、M2、M3三点的坐标如表1所示。

表1 人孔面上3点坐标

2.3 拟合人孔平面边线及圆心求解

如图3所示，借用Matlab画图工具，根据球面A与平面B的方程绘制三维图形，显然，球面A和平面B相交所得的交线圆C即为人孔平面边线。

图3 模拟人孔平面图

均匀选取12 个圆C 上的点，设圆C方程为A·（x2+y2+z2）+Bx+Cy+Dz+E=0，利用Python工具进行数据拟合得到ABCDE的值如下：

计算得到圆C半径约为435，与实际圆面直径吻合，圆心坐标为（935.9339,728.3192，-1415.1192）。

2.4 机器人与激光位移传感器之间的位置变化

大电缸经过人孔平面圆心时，可认为圆心与大电缸轴线顶点M0（x0，y0，z0）重合，大电缸与小车平面的夹角为α，两激光位移传感器安装在离x′y′z′原点距离d的位置，

表2 圆C数据采集表

由平行关系可得到以下方程组：

图4 计算示意图

2.5 激光位移传感器的理论测量结果

直线AB的方向向量μ=A—→B；

平面B法向量：n=M1M2×M1M3。

由于三维坐标系xyz和x′y′z′之间不存在旋转关系，仅在空间内平移了dx、dy、dz：

所以证得直线AB与平面B平行，故在定位实验中，当测量到两激光位移传感器确定的直线与人孔平面平行位置时（两传感器显示的距离相同），即为机器人面向蒸汽发生器人孔面的最佳工作位置。

2.6 基于激光位移传感器非接触测量模型

激光位移传感器测量定位装置主要由基恩士LRTB5000TOF激光位移传感器、Y200RA200电控旋转工作台、滑轨组成。机械臂为新松机械臂，具有±0.05 mm的重复定位精度，对本实验的误差影响可忽略不计。机器人的移动小车部分装有图5所示的XY方向的移动平台，因此只需确定机器人面向蒸汽发生器的摆放角度，再根据实际情况通过移动平台调整大电缸的前后左右位置即可达到机械臂最佳工作位置。

图5 蒸汽发生器与机器人装配图

实验时，先由激光传感器对蒸汽发生器人孔面进行测量[17]，再将测量数据经计算处理后反馈给底部的工作台，旋转到所求角度后进行后续的测量与定位。

根据计算机器人与蒸汽发生器的空间位置关系，得到了机器人相对蒸汽发生器的理论摆放位置，即在机器人的电缸轴线与人孔轴线重合的基础上，机械臂后续的作业难度会大大降低，借助激光位移传感器进行测量计算即可得到机器人的实际摆放位置。

将激光传感器定位装置放置于面向蒸汽发生器人孔的地面上，激光位移传感器A和B所在位置设定为点a、b，且a、b之间的距离为g，如图6所示，通过调整定位装置的朝向和安装激光位移传感器的转板角度，使得两激光位移传感器发出的射线m、n均在人孔环形平面上，形成光点p、q，设定点a到p的距离为x，点b到q的距离为y。通过点a、b的直线为l1，通过光点p、q的直线为l2，直线l1、l2共面并形成夹角β。读取激光位移传感器上显示的x、y的实时数值，按照式（1）计算夹角β：

图6 基于TOF传感器的测量原理图

然后，根据以上求得的角度β，在电动旋转平台上输入step，转动旋转台，旋转台每1600 Step为1°，使直线l1、l2相互平行，同时，使x=y；当x=y时，即定位完成，故转动角度β后的直线l1方向即为一次侧堵板拆装机器人的摆放角度。

3 实验研究

3.1 激光位移传感器定位实验

本试验目的为通过测量两激光位移传感器到人孔平面上任意两点的距离，求得人孔平面与激光位移传感器所在直线的夹角，通过转动电控旋转台使两者平行，从而确定移动小车初始位置。

实验开始时，先将激光位移传感器、内置双轴心直线运动导轨、电控旋转工作台按照设计方案组装，并接通电源；将定位装置摆放至人孔前方地面，使得激光位移传感器的测量范围在5 m以内即可；调整两个TOF激光位移传感器的角度，使其激光点均在人孔平面上，根据两激光位移传感器测得的距离与传感器原定的安装距离，计算两激光位移传感器所在直线与人孔平面的偏移角度；将所求角度转换为旋转台转动步数并反馈给电控旋转工作台，即此时激光位移传感器所在位置即为机器人摆放角度。

为保证后续实验准确性与可靠性，用此实验方法进行多次定位，实验结果如表3所示。由于定位装置摆放位置随机，因此实验结果中的旋转台转动角度与摆放位置不具有线性特征。

表3 定位实验记录表

3.2 定位结果误差分析

为测试定位方法的可靠性和重复性精度，在第一次利用定位装置确定机器人的作业位置后，录制了一整套机械臂由人孔进入蒸汽发生器并完成安装堵板的程序G，在实验1定位结果的基础上运行程序G并记录机械臂末端位置作为参考标准点。然后将定位装置多次面对人孔方向随机摆放，用上述定位方法进行重复实验后运行程序G，机械臂按原路径完成了一次侧堵板安装，但几次试验的机械臂末端位置存在一定出入，机械臂末端位置记录如表4所示。

表4 重复定位实验记录表

将7次实验的期望值作为标准点A，坐标为（x0，y0，z0），其他7组机械臂末端位置坐标一次记录为B（x1，y1，z1）、C

此定位方法的重复误差为1.183 mm。

一般来说，相对误差更能反映测量的可信度，相对误差设为δ，绝对误差为Δ=1.183，L为整体尺寸，机器人的最大有效工作半径为1400 mm，因此

计算结果表明，基于该定位方法下的重复定位精度较高，在实际实验研究中具有一定的可靠性。

3.3 测量结果不确定性分析

从测量数学模型可以看出，测量误差、激光位移传感器系统误差、机械臂系统误差是影响测量精度的重要因素。

首先，计算引用到的公式等存在近似限制，使用仪器时操作不规范，均会使实际测量与理论结果有所出入；其次，TOF激光位移传感器系统自身存在一定的技术限制[18]。当被测量对象的表面特别明亮且离TOF传感器的距离非常近时，传感器会向接收机散射多余的光线，从而产生伪影和一些不需要的反射。尤其是当TOF传感器的工作环境是晴朗的室外时，强烈的太阳光线会加速传感器的像素饱和，使其不能准确快速地检测到物体反射的实际光。再有，当TOF传感器测量的目标面为转角或者凹部时，会产生多次反射，给测量数据带来很大的误差，即造成测量结果的不确定性。

在验证定位结果准确性时，利用了机械臂走G程序，记录末端位置分析定位误差的方法，误差分析过程中，应考虑机械臂本身存在的系统误差[19]。

4 结论

本文提出了一种基于TOF激光位移传感器的一次侧堵板拆装机器人的水室定位方法，涉及核电站自动化检修技术领域。在已设计的十自由度一次侧堵板拆装机器人进行堵板拆装作业时，由于机械臂尺寸略小于蒸汽发生器的人孔孔径，对于机器人的工作位置要求较高。因此只有确定了机器人执行堵板作业时的工作位置，才能方便机械臂进入人孔并且为后续作业提供相对较大的活动范围和操作空间。首先建立蒸汽发生器与机器人的空间位置模型，得出机器人的移动小车部分必须平行人孔平面的结论；再利用设计的激光位移传感器定位装置对小车的位置进行测量与校正；最后通过测量机械臂末端的空间坐标变化来验证此定位方法的重复性精度和可靠性。经验证，此方法的相对重复定位误差为0.0845%，可以为机器人提供较为理想的工作位置，有效降低机器人的操作难度，提高拆装堵板的工作效率。