一种基于激光矩阵的水下网衣巡检机器人定位方法

秦 浩，杨仁友，杨 靓

(南方海洋科学与工程广东省实验室(湛江)，广东 湛江，524002)

近年来，中国海水渔业养殖业发展迅猛，内湾与近海水域分布共计有100多万只网箱[1-2]，为获得更好的养殖质量[3]，开始尝试将养殖水域向深远海拓展[4-6]。网衣作为深水网箱的重要组成部分，受污损生物的攀附影响，堵塞部分网孔，降低网箱内溶氧含量[7-8]，增加环境载荷，降低网衣使用寿命[9]，破坏网衣结构，增加鱼群逃逸风险[10-11]。因此，网衣巡检成为深海网箱的关键性技术和推广中需要解决的重要难点之一[12]。目前，深水网箱网衣巡检依赖于专业潜水员下潜作业[13]。网衣受海浪等随机性因素影响，会出现不可控的摆动及变形[14-15]，而基于静态参量设置的导航程序难以适应不断变化的网衣情况，无法做到高精度定位与导航，可能造成机器人在巡检过程中定位丢失、遗漏检测区域等问题，使得最终检测结果与实际情况存在较大误差。因此面向随机运动网衣的动态定位是实现机器人水下高精度巡检作业的关键技术问题。

现有的水下定位技术，主要包括船位推算、惯性导航(INS)/声学组合导航、INS/多普勒计程仪(DVL)组合导航、视觉导航、地球物理导航[16-17]等。航位推算[17-18]主要依赖测速仪、姿态仪及深度传感器等，算法简单且实现方便，但其推算精度不高，养殖网箱中，网衣变形不易测量，故搭载机器人难以实现跟随网衣的定位。惯导定位是一种根据载体的加速度和角速度值推算载体相对位置的导航方法[21]，自主性好、短时精度高，但是存在长期累积误差，常用超短基线设备或DVL抑制长期漂移。超短基线等安装位姿要没入一定水深深度，难以获取近水面的定位数据，且近水面受波浪背景噪声及多途干扰，而且声学设备对鱼类养殖可能产生干扰，故养殖网箱不宜使用INS/声学组合导航。类似地，DVL设备由于养殖鱼群干扰会频繁出现短时失效，同时受网箱运动干扰，很难获取对底速度，无法消除惯导设备的累积误差，因网衣变形难以测量，不适用于搭载机器人对网衣的相对定线航行和抵近检测。在内使用声学系统进行水下机器人的定位。视觉系统[22-24]通常与激光或惯导结合使用，但由于光在水中传播损耗较大，养殖网箱随海浪周期性和随机性运动，视觉里程计会引入背景误差，以及网衣受流变形导致视觉里程计难以实现定位。地球物理导航[25-26]主要依靠自身携带的传感器对地球物理参数，如地形匹配、重力场、地磁场等，与预先勘探测绘的地球物理参考图进行对比获得水下定位，其无源性，精度高，不受时间限制，隐蔽性强，在军用中有着重要前景，但也因其传感器精度要求高，技术尚不成熟，不适用于养殖网箱尺度的水下环境。

本研究利用相位式激光在水下近距离测距的高精度、低干扰优势，组成一套基于十字激光测距矩阵传感器的水下机器人定位系统，并提出一种基于激光矩阵的水下网衣巡检机器人定位方法实现养殖网箱系统内的相对定位，以及巡航过程中的相对航行姿态计算。

1 基于激光矩阵的水下网衣巡检机器人定位系统

如图1所示，该水下网衣巡检机器人的定位系统[27]包括：处理器1001，例如CPU，网络接口1004，用户接口1003，存储器1005，通信总线1002。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003包括显示屏(Display)、输入单元比如键盘(Keyboard)，还包括标准的有线接口、无线接口。网络接口1004包括标准的有线接口、无线接口(如WI-FI接口)。存储器1005可以是高速RAM存储器，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器1005可以是独立于前述处理器1001的存储装置。

图1 水下网衣巡检机器人硬件运行环境的系统结构示意图

如图2所示，在水下巡检机器人的航行过程中，使水下巡检机器人上安装的多个激光传感器发射激光至目标网衣上，并通过多个呈十字型排布的激光传感器持续接收激光反射信号，以获取初始回波信号图。激光传感器如图2所示的方式间隔均匀排布并朝向目标网衣表面，数量可根据实际需要设置；网衣由网线编织而成，其包括多个网目，激光传感器发射的激光若打网线上，将被网线反射回同一激光传感器上，而若打在网目位置，激光将穿过网目而不发生反射。激光传感器接收到反射激光的时间点即为回波信号点，随着水下巡检机器人绕目标网衣表面不断航行，激光传感器不断接收到目标网衣反射的反射激光。将每个激光传感器的回波信号点在时间轴上逐一标示出来，即形成了矩阵激光回波信号图。通过计算分析信号图，可以获得水下网衣巡检机器人的相对定位。

图2 激光传感器的排布示意图

2 水下网衣巡检机器人定位方法

如图3所示，提出了一种基于激光测距矩阵的水下网衣巡检机器人定位方法，该方法包括以下步骤：基于网箱物理模型，利用地址编码给网箱结构、网衣结点、网线、网孔建立编码地址索引库；水下巡检机器人上安装的多个激光传感器发射激光至目标网衣上并接收；通过激光信号接收器，获取回波信号图；通过回波信号图得到机器人航行的网目位置；通过公式计算出航行距离、竖直偏移距离、横向偏移距离和横向偏移角度；从而确定水下巡检机器人在水下网衣的定位地址。深远海养殖网箱中，由于机器人在水下巡检，水下环境的复杂，水体浊度对水下相位式激光测距仪精度影响较小，且在近距离测距中测量精度理想[28]，所以采用相位式激光传感器的相位式激光进行定位。

图3 水下网衣巡检机器人定位方法流程图

2.1 相位式激光测距原理

相位式激光测距的基本原理是对激光强度进行幅度正弦波调制，并将传播后的飞行时间(Time of flight)转换为相位差[29]。可以通过式(1)得到相位差Δφ：

(1)

式中：ω为调制光的角频率，rad/s；τ为激光传输时间，s；f0为调制光的频率，Hz；d为激光发射器到目标的距离，m；c为激光传播速度，m/s。

采用基于光外差相位检测技术进行相位测量时，具有良好的分辨率是非常重要的[30]。利用光外差技术，相位差又可以写成Δφ=2πTX/TIF，其中TIF为中频信号的周期，Tx为相位检测器电路输出处得到的矩形波列高脉冲宽度所对应的时间。所以激光发射器到目标的距离d可以由式(2)计算得到:

(2)

式中：λ0为激光波长，m；λ0=c/f0。

相位差测量的精度受到串扰效应、激光二极管驱动器信号频率的精度和稳定性、相位检测器计数器的局部频率和时钟脉冲频率等因素的影响。而在水下测量时，水的折射率决定了光波长、密度、温度以及水的压力。同时，水的盐分、温度、压力及速率分布会引起折射率的非均匀分布，这会很大程度上影响激光测距仪的结果。光的吸收损耗会导致光束能量的减少，也会导致传输距离的减少。光的散射作用会影响光束的方向，并且会极大地降低测距精度。而使用的相位式激光测距能一定程度上避免水体浊度对精度的影响，但测距动态范围随着水体浊度的增大呈现指数衰减现象。经定标校正后，水下相位式激光测距仪在水下3.5 m范围内测距误差平均值不超过3 mm，可用于水下目标近距离的精确测距[28]，本研究中渔网巡检机器人的贴网距离设定为0.5 m，理论测距误差满足定位需求。

2.2 激光测距传感器网格计数方法

在理想状况下，水下巡检机器人始终平行于横向网线航行(具体为呈十字型的激光传感器始终平行于横向网线)，由于每个网目的尺寸相同，因此每个激光传感器接收到竖向网线反射的反射光的时间节点是基本相同的，即此时回波信号图中每个激光传感器的每个回波信号点在时间轴上的位置是基本相同的，在此情况下，当各个激光传感器的回波信号点在某一时间节点上同时出现，且在下一时间节点上再次同时出现时，则可认为水下巡检机器人在两个时间节点之间走过了一个网目的距离，因此在已知网目尺寸的情况下，通过统计总共走过的网目数量，可方便地获知水下巡检机器人相对目标网衣表面的总航行距离。

具体计数步骤如下：

①将与竖向部上的激光传感器相对应的校正回波时间轴筛选出来，以作为竖向回波时间轴；

②设定时间阈值Δt(s)，在目标航行时间段内，统计多个竖向回波时间轴在一个Δt范围内均出现回波信号点的总次数，以作为水下巡检机器人在目标航行时间段内的经历网目数；

③基于经历网目数，计算出校正航行距离ΔY。

时间阈值Δt具体可根据式(3)计算：

Δt=l/2v

(3)

式中：l为目标网衣上单个网目的边长，m；v为水下巡检机器人的航行速度，m/s。

如图4所示的情况，在目标航行时间段内，多个竖向回波时间轴在一个时间阈值范围内均出现回波信号点的总次数为5次，则可认为水下巡检机器人在目标航行时间段内等效走过了5个网目的距离。

图4 矩阵激光传感器信号接收示意图

由此可根据公式(4)计算出水下巡检机器人的航行网目距离Δy。

Δy=N×l

(4)

式中：N为水下巡检机器人经历的网目数，个。

2.3 网目内相对定位方法

通过激光测距传感器网格计数方法仅能得到水下巡检机器人经历过的网衣格数以及精度为l的距离信息，巡检机器人以“网目”为单位进行的定位，即网衣格栅定位，不能确定具体的相对位置信息，故需通过以下方法对巡检机器人再次定位，提高定位精度。

如图5所示，巡检机器人P在巡检过程中某时刻，十字矩阵激光中心P’扫过网目内，通过第一激光线AB可以确定P在绕第二激光线OO’旋转面上的旋转角pitch(rad)，即γ，以及第一激光线AB上的相对位移ΔY，即AP’(BP’)。

图5 水下网衣巡检机器人网目内定位方法示意图

同理，通过第二激光线OO’可以确定P在绕第一激光线AB旋转面上的旋转角yaw(rad)，以及第二激光线AB上的相对位移ΔX。

根据示意图中的结构信息，可以根据式(5)、(6)计算出γ及AP’(BP’)。

(5)

(6)

由此可以计算出巡检机器人相对网衣的航行位置X、Y，以及航向角pitch、yaw，从而提高巡检机器人的相对定位精度。

在实际中激光传感单元的排列、角度、传感噪声、机器运动都会导致误差量Δl1、Δl2、Δα、Δβ的产生，下面通过误差计算来分析误差量对最终计算结果的影响。

(7)

(8)

由于在每次试验在定标矫正后才开始试验，故Δα→0、Δβ→0，在仿真试验中Δα=0、Δβ=0，只有距离测量值会在传感器噪声以及运动下产生误差值Δl1、Δl2。

故式(7)、(8)，可化简成：

(9)

(10)

式中：l′=l1/l2，Δl′=(l1+Δl1)/(l2+Δl2)。

由式(9)、(10)可见，Δγ～Δ(tanγ)～Δl′，计算中航向角误差主要来源于测量值的比值Δl′，而航行位置误差主要由各自的测量值Δl1、Δl2产生。

2.4 仿真环境搭建

2.4.1 养殖网箱编码先验地图建立

根据深远海养殖网箱建立网箱的物理模型，如图6所示。基于网箱物理模型，利用地址编码给网箱结构、网衣结点、网线、网孔建立编码地址索引库。网衣结点定义点编码地址索引库，网线定义线编码地址索引库，网孔(网目)建立面编码地址索引库，同时利用网箱结构定义编码地址边界条件。例如，某一网目编码如图7所示。

图6 深远海养殖网箱物理模型

图7 基于网箱模型的编码示意图

图7编码L0203H0102X06Y20中L后接数字表示结构立柱02与03，H后接数字表示结构桁架01与02，X、Y后接数字表示在该结构立柱与结构桁架为边界条件的情况下，该平面区域中，网目的二维编码位置。

2.4.2 仿真模型构建

通过Gazebo仿真建模，利用水下机器人搭载本研究提出的十字矩阵激光传感器，进行该区域水下网衣巡检的仿真试验，仿真主机的硬件配置如表1所示。

表1 仿真试验环境

仿真过程中，某时刻Gazebo的显示界面如图8所示。

图8 Gazebo仿真环境中水下网衣巡检机器人作业图

如图9所示，十字激光矩阵激光传感器在横向及竖向上分别设置了32个激光器，激光器间隔角度为0.033 45 rad，从带编码标记立柱的确定位置开始，通过控制仿真机器人在规划路径上进行适网航行巡检作业，水下环境有水雾设定以模拟光在水下的衰减情况，以及通过给矩阵激光传感器加入高斯噪声达到传感器的噪声模拟，噪声参数基于公布的Hokuyo激光器规范，在小于10 m的范围内达到“+30mm”的精度。本研究中设置均值为0.0 m，标准差为0.01 m。

图9 水下网衣巡检机器人巡航定位示意图

3 结果与分析

3.1 激光矩阵信号分析

如图10、图11所示，水下网衣巡检机器人从基准点开始，依照规划路径作业巡航17.5 s，采集到的激光矩阵回波信号图。其中图10为竖向部上部分激光信号回波图，图11为横向部上部分激光信号回波图。

图10 竖向部激光传感器部分信号回波图

图11 横向部激光传感器部分信号回波图

根据网目统计方法，从竖向部激光信号回波图可以统计得到在17.5 s内，巡检机器人沿X方向经历了8个网目；从横向部激光信号回波图统计，时间阈值Δt内并未同时出现回波信号，而是依序出现，故巡检机器人沿Y方向经历了0个网目。值得注意的是，从竖向部激光信号回波图中的第六激光器回波图中，可以观察到一段连续的回波点，这是因为巡检机器人在巡航过程中，路径偏离了规划路径，激光发射器发射的激光正好打在了网衣的网线上。

3.2 定位计算结果分析

通过对激光器的回波信号图中数据的处理，依据上文计算方法，可以计算得到巡检机器人的具体偏航信息，如图12所示为巡检机器人在17.5 s内，XY方向上位移计算值与实际位移的对比。由图12可见，本研究提出的算法计算出的位移，基本符合实际移动趋势以及位移量，可以分析出，巡检机器人在网衣巡检作业过程中，在y方向上偏离了约0.008 m。

图12 XY方向位移计算值与实际位移值对比图

依据本研究提出的偏航角γ值计算pitch以及yaw的值，实际计算中，由于巡检机器人在pitch上偏转较小，数值趋近于0，如图14所示，且由于yaw上变化量依旧很小，对数据进行了简单的均值滤波处理后，得到了如图13的yaw实际值与真实值的对比图。

由图13可见，yaw上计算值的变化趋势较符合yaw的真实值，经过17.5 s后巡检机器人在yaw方向上偏转了约0.14 rad。

图13 计算偏航角度与实际值对比图

在具有网衣编码的先验地图中模拟一组定位试验，如图14所示。

图14 仿真试验下水下网衣巡检机器人巡检路径

本次仿真试验在8×6网格中进行网目计数定位算法验证，网目大小为250 mm×250 mm，由定位结果可见，格栅定位路径能准确反映了巡检机器人在网目中的相对巡检路径， 即在该巡检时段中，巡检机器人贴网航行的方向，以及航行的网格数目，且在开始一段定线巡检中，激光矩阵中心传感器如图10中第6传感器所示，维持了较长一段定线巡航稳定性。

在穿越网线时也能检测出巡线路径的改变，在本研究方法中，只有当网目计数偏离半个网目，才会出现偏航计数误差，故本次仿真网衣格栅定位误差为0个网目。如表2所示，依照基于编码地址的先验地图信息，巡检路径能以编码信息输出，在巡线航行中，编码以两面编码表示面-面相交处网线编码，在编码信息中同时记录养殖网箱系统中网衣的实际情况。

表2 巡检路径编码

3.3 误差分析

本仿真试验中，误差主要来源为由于水下环境造成的建模网衣的轻微振动以及巡检机器人在航行过程中运动稳定性受环境干扰，导致的激光矩阵在测距上出现的测量误差，以及人为设定的传感器高斯噪声产生的误差。由图15可见，在X位移上误差较小，而在Y位移上误差较大，误差均呈现由大到小的分布趋势，X位移上由初始的0～2 s内平均误差7.52%提高到14.5～16.5 s内的3.72%，Y位移上0～2 s内平均误差由51.35%提高到14.5～16.5 s内的8.15%，这是因为在仿真试验中，矩阵激光传感器中加入了高斯噪声以及机器人在水环境中运动测量的干扰，在位移量较小时，高斯噪声造成的传感器测量误差远大于运动造成的误差，随着位移量的增加，传感器测量误差占比逐渐小于运动误差，而本研究提出的计算方法计算出的相对定位值在运动误差影响下符合理想预期，故整体位移误差均随位移量增加呈下降趋势，其中X方向上位移量增加比Y方向上要大，X位移误差下降比Y方向快。

图15 XY方向位移计算误差图

如图16所示，通过计算误差进行进一步分析。由yaw误差曲线图可见，yaw初始时段2～4 s内平均误差由62.48%提高到14.5～16.5 s内的19.18%，yaw的误差如前文所述，在偏转角度较小时，受传感器噪声误差影响较大，故误差异常偏高，随着偏转角度的增加，误差影响逐渐变小，主要呈现运动误差，而计算的转角值符合理想情况，运动误差远小于传感器噪声误差，所以总误差随着偏转角的增大而出现下降趋势。

图16 yaw上计算偏航角度误差图

4 结论

提出的水下网衣巡检机器人定位方法能够准确定位出巡检机器人相对网衣巡航的网目定位，并根据网目内相对定位算法计算出的巡检机器人相对位移值与相对偏航角度，0.4 m距离处计算平均误差3.72%，0.08 m距离处计算平均误差8.15%，0.14 rad偏转计算平均误差19.18%，并在位移量及偏转量增大时误差逐步降低，符合实际位移及偏转情况，在网衣格栅定位中误差为0个网目，达到了水下巡检机器人在非声非惯性系下相对养殖网箱定位要求，巡检路径以编码信息输出，并记录养殖网箱巡检情况。本研究下一步将探索多源异构传感器在深远海养殖网箱系统中的定位，并实现水下网衣巡检机器人的污损网衣识别检测与定位。