船舶清理机器人定位基准位姿测量技术研究

杨其雷，郝永志,2

（1.浙江国际海运职业技术学院航海工程学院，浙江舟山 316021；2.上海海事大学商船学院，上海 201306）

船舶海水脱硫装置大多采用湿法喷淋来除去烟气中的硫元素，以天然海水为吸收剂，将碱液以喷雾的形式喷入到海水中，两者混合反应后，来脱除烟气中SO2，从而达到净化的目的[1]。但是，海水脱硫法会在烟囱内壁附着大量盐巴，这些盐巴久而久之会堵塞出烟口从而影响排烟效率，进而影响海水脱硫效果。目前清理盐巴的主要措施是风吹、水洗、人工铲除等，经济代价大，易使设备损坏，且效果不佳。因此，需要采用新型机器人清理烟囱内壁的盐巴以提高海水脱硫效果。

机器人工作时定位基准位姿的测量精度又是盐巴清理机器人研发需解决的关键技术。机器人定位基准位姿测量在国内外已经进行了大量的研究，在20 世纪80 年代就有学者将数学模型应用到了机器人定位基准位姿测量中，通过在空间平面内选取四个以上已知特征点，根据每一个特征点的选取，计算出摄像机图像平面与空间平面之间的单应性矩阵，来推算空间平面内的机器人定位基准位姿信息[2]；国内对于机器人定位基准位姿测量的研究起步较晚，但是在科研人员的努力和国家的大力支持下，机器人定位基准位姿测量研究已经得到了比较深入的研究[3]。王天宇等[4]利用图像匹配方法检测了平面靶标在图像中的具体坐标，采用固定靶标的特性，求取了位姿的中心点，实验通过多幅图像样本验证了图像匹配方法的鲁棒性，将靶标控制点的空间和图像坐标作为输入，获取到摄像机与移动物体之间的三维相对位姿，实验结果表明，该位姿测量系统具有较高的测量精度[4]；黄敏高[5]以工业机器人为研究对象，引入了静态图像收集算法和空间匹配算法，通过工业试验得到机器人的定位数据，实验结果显示，该系统符合加工自动化的信息数据要求，可以实现空间姿态与工业机器人参数之间的匹配，提升了工业生产的自动化程度。

结合船舶海水脱硫装置的特点，本文在前人研究的基础上优化设计了船舶海水脱硫装置中盐巴清理机器人定位基准位姿测量系统，以期提升海水脱硫效果。

1 船舶盐巴清理机器人定位基准位姿测量系统硬件设计

1.1 机器人定位基准位姿跟踪仪优化设计

机器人定位基准位姿测量系统设计硬件部分由位姿跟踪仪、控制器、用户计算机、环境传感器以及靶镜等组成[6]，见图1。

图1 位姿跟踪仪测量系统的组成Fig.1 Composition of the measurement system of pose tracker

设计机器人定位基准位姿跟踪仪是通过位姿跟踪仪发出的光束信号经过靶镜的反射与另一路光束发生干涉，根据光束干涉原理[7]可以得到盐巴清理机器人定位基准位姿跟踪头到反射镜之间的距离。根据盐巴清理机器人定位基准位姿跟踪头2个轴上的角度编码器，可以得到测量位置相对于盐巴清理机器人定位基准位姿跟踪头的俯仰角与放大角，见图2。

图2 机器人定位基准位姿跟踪仪工作原理图Fig.2 Working principle diagram of the positioning datum pose tracker of salt cleaning robot

根据船舶盐巴清理机器人定位基准位姿跟踪仪测头与定位基准位姿之间的距离和角度，计算出机器人定位基准位姿在跟踪仪坐标下的直角坐标。

1.2 机器人定位基准位姿测量传感器设计

机器人定位基准位姿测量传感器的设计是基于对位姿测量光束飞行时间的测量，换句话说就是从发送位姿测量光束到接收光束的时间间隔与位姿测量传感器到机器人之间的距离成正比关系[8]。

激光器的光束发射后如果遇到障碍物就会被反射回来，随后会被测距仪内部的接收器记录下来。光束被测距仪内部的旋转光镜进行偏转，便于对周围环境形成一个扇面扫描区域[9]。待测机器人定位基准位姿的轮廓线根据脉冲序列来确定[10]，通过串行接口可以测量数据传送给计算机，从而进行下一步处理。

盐巴清理机器人定位基准位姿测量传感器的技术参数设计见表1。

表1 位姿测量传感器的技术参数Tab.1 Technical parameters of pose measurement sensor

通过位姿测量传感器的设计，设置位姿测量传感器的技术参数，实现了系统的硬件设计。

2 船舶盐巴清理机器人定位基准位姿测量系统软件设计

2.1 机器人定位基准位姿解算算法设计

根据先验知识和机器人定位基准位姿特征点图像坐标，通过精确图像处理算法得到机器人定位基准位姿特征点像素坐标(u,v)，从而经过转换得到图像坐标(x,y)，根据空间几何投影约束条件和投影模型[11]，求解得到机器人定位基准位姿特征点的空间三维坐标(xci,yci,zci)。

假设机器人定位基准位姿特征点在标靶坐标系和防爆相机坐标系下的坐标为Pbi(xbi,ybi,zbi) 和Pci(xci,yci,zci)，因此，两个坐标系的转换关系可以表示为：

公式中，旋转矩阵Rbc和平移矩阵Tbc采用对偶四元数[12]可以将其表示为：

此外假设机器人定位基准位姿特征点在摄像机坐标系下对应的实测值为P～ci，由于机器人定位基准位姿特征点安装以及定位机器人基准位姿过程中可能存在误差[13]，因此建立了公式（3）的机器人定位基准位姿解算模型，其中，N 表示机器人定位基准位姿特征点数量。

采用对偶四元数来表示公式（3），得到机器人定位基准位姿特征点的目标方程为：

在公式（4）中G1，G2、G3和的计算公式为：

通过求解机器人定位基准位姿特征点的目标方程，得到对偶四元数的实部r 和对偶部分s，从而在标靶坐标系和防爆相机坐标系下，获得旋转矩阵Rcb和平移向量Tcb。根据机器人定位基准位姿特征点相对于海水脱硫装置的坐标转换关系：

公式中，M40表示机器人定位基准位姿特征点相对于海水脱硫装置的坐标转换关系，Mc0表示机器人定位基准位姿特征点到防爆相机坐标系的转换关系，Mcb表示跟踪器到标靶坐标系的转换关系，M4b表示机器人定位基准位姿特征点到标靶的坐标转换。

根据先验知识和机器人定位基准位姿特征点图像坐标，建立了标靶坐标系和防爆相机坐标系的转换关系，采用对偶四元数建立了机器人定位基准位姿特征点的目标方程，完成盐巴清理机器人定位基准位姿解算模型的建立。

2.2 测量机器人定位基准位姿测量的实现

在机器人定位基准位姿解算模型的基础上，通过开发一个机器人定位基准位姿测量程序，来实现机器人定位基准位姿的测量。主要包括获取机器人定位基准位姿图像的驱动、预处理、识别靶标位姿解算以及数据显示等[14]。为了方便调试机器人定位基准位姿测量程序，将灰度图和深度图像在上位机中显示出来，从而查看机器人定位基准位姿到的深度值。

为了准确描述出靶标的位置信息，先确定并选择坐标系，将其坐标表示为（X,Y,Z）。本文选择盐巴清理机器人定位基准位姿的本体坐标系和观察坐标系来描述靶标的具体位置。在测量机器人定位基准位姿之前，必须先确定相机的畸变参数和内参数，在工作模式下，获取机器人定位基准位姿数据之后，测量机器人定位基准位姿，具体步骤如下：

1)识别靶标

采用畸变校正和降噪的方式来处理相机采集到的机器人定位基准位姿图像，利用阈值算法从处理的图像中筛选靶标，提取靶标的形心坐标，并从深度图像中提取形心的机器人定位基准位姿深度信息。

2)匹配靶标

利用靶标形心的深度值和坐标、相机的内参矩阵，计算出靶标的三维坐标值，从而求解出靶标的相对距离矩阵[15]。计算物体坐标系下机器人定位基准位姿的相对距离矩阵，将计算得到了2 组相对距离矩阵作为测试程序的输出，利用退火算法找到靶标的序号对。

3)计算机器人定位基准位姿

将正确匹配的靶标序号对应的三维坐标值作为机器人定位基准位姿测量程序的输入，利用机器人定位基准位姿解算模型求出旋转矩阵和位移向量。

根据机器人定位基准位姿测量步骤，设计了机器人定位基准位姿测量流程。

综上所述，在盐巴清理机器人定位基准位姿解算模型的基础上，结合机器人定位基准位姿测量程序设计，完成了系统的软件设计，实现了机器人定位基准位姿的测量。

3 系统测试

3.1 试验设计

由于传感器不能识别盐巴清理机器人的运动加速度，因此不能适应机器人的高动态行走情况，在此基础上，通过改进滤波器来增加空速计，从而识别机器人运动加速度的影响。利用Matlab 仿真工具来验证机器人定位基准位姿测量系统的有效性。仿真试验结构图见图3。

图3 仿真试验结构图Fig.3 Structure diagram of simulation experiment

仿真试验的盐巴清理机器人行走参数见表2。

表2 盐巴清理机器人行走轨迹参数Tab.2 Trajectory parameters of salt cleaning robot

3.2 机器人定位基准位姿平移距离测量误差试验

在3.1 试验设计的基础上，分别采用文献[4]的位姿测量系统、文献[5]的位姿测量系统以及本文提出的位姿测量系统来测量船舶海水脱硫装置中盐巴清理机器人定位基准位姿，得到机器人定位基准位姿平移距离测量误差对比结果，见图4。

从图4 的实验结果可以看出，文献[4]的位姿测量系统在测量船舶海水脱硫装置中盐巴清理机器人定位基准位姿时，由于系统硬件的跟踪设备精度不足，使得机器人定位基准位姿测量的结果与实际测量产生的误差达到10%左右，而且还低于实际测量结果；文献[5]的位姿测量系统由于解算船舶海水脱硫装置中盐巴清理机器人定位基准位姿过程中不够精确，使得机器人定位基准位姿测量的结果与实际测量结果也存在一定误差，达到4%左右，而本文提出的位姿测量系统建立了船舶盐巴清理机器人定位基准位姿解算模型，简化了位姿解算步骤，使机器人定位基准位姿测量结果与实际测量结果之间的误差较小，在1%以内，从而提机器人定位基准位姿测量的精度。

图4 机器人定位基准位姿平移距离测量误差对比结果Fig.4 Comparison result of measurement errors for the translation distance of the positioning datum and pose of the salt cleaning robot

3.3 机器人定位基准位姿测量角度误差试验

在3.1 试验设计的基础上，分别采用文献[4]的位姿测量系统、文献[5]的位姿测量系统以及本文提出的位姿测量系统，来测量船舶海水脱硫装置中盐巴清理机器人定位基准位姿，得到盐巴清理机器人定位基准位姿旋转角度测量误差对比结果，见图5。

从图5 的实验结果可以看出，文献[4]和文献[5]的位姿测量系统由于只进行了识别靶标和匹配靶标中的一步，使得到的船舶盐巴清理机器人定位基准位姿测量的结果与实际测量结果之间的旋转角度误差达到了20%左右，而本文提出的位姿测量系统细化了机器人定位基准位姿的测量步骤，使机器人定位基准位姿测量的结果与实际测量结果之间的误差在5%以内，从而提高了机器人定位基准位姿测量精度。

图5 机器人定位基准位姿旋转角度测量误差对比结果Fig.5 Comparison result of measurement errors for the Rotation Angle of the positioning datum and pose of the salt cleaning robot

4 结束语

本文提出了船舶海水脱硫装置中盐巴清理机器人定位基准位姿测量系统研究，通过设计机器人定位基准位姿测量系统的硬件部分和软件部分，完成了机器人定位基准位姿测量系统。仿真结果表明，该位姿测量系统在测量精度方面表现优于传统方法。但本文方法因未考虑噪声干扰，为进一步提高测量精度，还需要在未来研究中着重探究深度识别算法和滤波跟踪算法。