基于INS/前视声纳组合的输水隧洞AUV巡检定位方法

杨文果，吴文启，张 鹭，王茂松

(国防科技大学智能科学学院，长沙 410073)

0 引言

南水北调工程对于提高水资源的配置效率，促进可持续发展具有重要意义。输水隧洞常年处于满负荷状态，随着运行时间的延长，会出现局部破损等缺陷，使隧洞的安全运行受到严重威胁[1]。因此，定期进行输水隧洞的问题排查和检修是十分必要的。目前，国内对于输水隧洞的检测主要有人工检测和水下机器人检测两类[2]。水下环境的隐秘与恶劣，会对潜水员的人身安全构成威胁，且人工检测效率较低，因此，利用自主水下潜航器(Autonomous Underwater Vehicle，AUV)进行自动巡检是未来输水隧洞检测的主流发展趋势。

确定输水隧洞的故障位置是AUV巡检的主要任务，这就对AUV的环境感知能力和导航定位能力提出了很高的要求。巡检时，AUV用于隧洞环境成像的传感器通常是前视声纳，利用前视声纳对隧洞内结构破损和泥垢堆积等故障进行感知。当前视声纳检测到故障部位时，确定AUV的位置也即确定了故障部位的位置，对AUV进行定位的最终目的是对隧洞故障进行定位。输水隧洞位于地下，不能接收到卫星导航信号，目前，AUV导航定位主要采用惯性导航系统/多普勒测速仪(Inertial Navigation System/Doppler Velocity Log，INS/DVL)组合导航的方式。但前视声纳和DVL都需要发射和接收声波，且二者含有相同的频率成分，因此DVL声波会对声纳成像产生干扰，使得前视声纳对隧洞故障的感知变得困难。一旦前视声纳失去了对隧洞故障的成像能力，AUV对故障处的定位便无从谈起。

为了解决AUV对隧洞故障的成像和定位相矛盾的问题，本文提出了一种基于惯性测量单元(Inertial Measurement Unit，IMU)和前视声纳组合导航的方法，即舍弃DVL，直接根据前视声纳成像原理恢复出载体速度，而后和惯性器件相组合，使前视声纳除了用于成像之外，也能发挥和DVL相接近的作用。目前，利用前视声纳实现水下组合导航的研究仍处于起步阶段，研究热点集中在同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)这一方向。张洁等使用最邻近数据关联算法(Nearest Neighbour Filter，NNF)实现了前视声纳图像间的数据关联，应用EKF-SLAM进行水下机器人的位姿估计与环境中障碍物特征的地图构建[3]，但是仅在仿真环境中验证了其算法的可行性，没有在实际AUV平台上进行测试。T.A.Huang等仿照光学相机的运动恢复结构(Structure from Motion，SfM)算法，推导了适用于前视声纳的SfM算法(Acoustic SfM, ASfM)，从声纳图像帧间恢复出载体的位姿变化[4]，但是跳过了不同声纳图像帧间特征自动匹配这一关键步骤，而是通过手动匹配特征点实现数据关联。E.Westman等对ASfM算法进行了改进，讨论了欠约束特征点的处理方法及其对位姿估计的影响[5]，但是仅在水池中进行了实验，而且提前在水池壁上粘贴了磁铁以便在声纳图像中形成特征点，缺乏实际在未知水域中的实验验证。总之，利用前视声纳进行SLAM算法的设计会遇到各种各样的挑战。目前，利用前视声纳提取载体速度的研究尚未得到深入开展。

本文提出的IMU和前视声纳组合导航的方法，通过在相邻声纳图像帧间进行特征提取和匹配，根据惯导信息约束以及随机抽样一致(Random Sample Consensus，RANSAC)算法，剔除明显的误匹配点对，而后从剩下的配对点中解算出载体速度并与惯导信息进行组合。通过实际实验，以隧洞内的接缝线作为定位参考基准，对INS/前视声纳组合结果的准确性进行了验证。

1 DVL对前视声纳图像的干扰

由于水下光线昏暗以及水质混浊等问题，基于光学原理的相机难以在水下进行远距离成像，目前用于水下成像的设备通常是各类声纳产品[6]。其中，前视声纳主要用于水下机器人前方范围的感知和成像。图1所示为前视声纳在未受干扰时所记录的某一输水隧洞中的图像(蓝线框出的黑色弧线为隧洞内的接缝线)。

图1 前视声纳在输水隧洞中拍摄到的图像

DVL和前视声纳都是AUV上常用的传感器，当两者在输水隧洞内同时开启，前视声纳受干扰严重时采集到的图像如图2所示。

图2 前视声纳受到DVL干扰时形成的图像

输水隧洞内空间狭小，DVL发射的声波经隧洞壁来回反射后，有不少会被声纳换能器接收，在图像中形成噪声干扰。对比图1和图2可知，虽然声纳图像的清晰度不如陆地上普遍使用的光学相机，但在图1未受DVL干扰的情况下，可以分辨出隧洞壁上的凹槽和接缝线等特征。而在受干扰严重的图2中，除了隧洞壁边缘(两条竖直亮斑)可以辨析出来，其他的图像细节大都湮没在噪声之中。图中蓝线框出的区域为接缝线所在位置，由于噪声污染已变得非常模糊。而且在不同的图像帧之间，噪声斑千变万化，时大时小，时而出现时而消失，没有规律可循。因此，如果在AUV上同时使用DVL和前视声纳，则通过前视声纳感知隧洞故障会变得困难，影响巡检任务的完成。

2 由前视声纳图像提取载体速度

2.1 前视声纳成像原理

针对上述问题，本文提出的解决方案是从AUV上卸载DVL，直接根据声纳图像的成像原理以及AUV在隧洞中的运动特点，从声纳图像中提取出载体速度。

前视声纳工作时，换能器基阵周期性地向前方一定角度范围发射声波，而后接收障碍物反射的回波。回波方位对应于障碍物相对声纳的方位，接收回波的时间对应于障碍物相对声纳的距离，回波强度对应于障碍物在图像上的像素值大小[7]。前视声纳的视域为前方的扇形空间，如图3所示。

图3 前视声纳三维视域范围

图3中，前视声纳位于原点O处，以O点指向声纳正前方为x轴，指向右侧为y轴，指向下侧为z轴建立声纳坐标系。描述声纳视域的参数为[8]：发射波束水平开角θs，垂直开角φs，最大探测距离rmax(O到曲面M1M2M3M4上任一点的距离)，最小探测距离rmin(rmin一般接近于0，图中没有表示出来)。如果以x轴正向为基准建立球坐标系，则对于坐标系内任何障碍点，只要其距离r、方位角θ、仰角φ这3个参数满足

则其位于声纳的视域范围内，如果该点与O点的连线上没有其他障碍点，则可以投射到声纳图像上。

前视声纳图像是二维图像，视域中的三维坐标点投射到二维图像上时，会有一个维度的信息丢失，具体的投射关系如图4所示。声纳图像的成像面为xOy平面与视域范围相交所形成的二维扇形。对于空间中的一个障碍点，假设其位于声纳视域中[r,θ,φ]这一位置，通过该点反射的回波可以得到r和θ这2个参数，则其沿着图中由r和θ所确定的红色圆弧投射到圆弧与成像扇面的交点位置，其像素大小由回波强度决定。也就是说，投影到成像平面内的点，其距离和方位角这2个参数得以保留，而仰角信息丢失，根据声纳图像无法确定成像点的仰角大小[9]。前视声纳和光学相机在成像时，都会有一个维度的信息丢失。不同的是，光学相机成像于与图4中yOz平面平行的归一化平面上，而前视声纳成像于xOy平面；光学相机丢失了空间点的距离信息，而前视声纳丢失的是空间点的仰角信息。

图4 前视声纳图像投影模型

前视声纳图像是对其正前方扇形成像区域的等比例缩放，图中亮度越大的地方，其反射回波越强，相邻2个像素点间所对应的实际物理距离用于衡量图像的分辨率。例如，声纳的最大探测距离设置为10m，若图像中原点与圆弧边缘间隔500个像素点，则图像分辨率为0.02m/pixel。

2.2 载体速度提取模型

图5 前视声纳在相邻两帧图像间的运动

(1)

(2)

特征点在导航系中的坐标变化，与AUV在导航系中的位移存在对应关系

(3)

则在t1和t2这2个时刻之间，根据特征点匹配解算出的AUV在导航系中的速度为

(4)

式(4)是在特征点坐标可测量的情况下建立起的方程，但是特征点的仰角是未知的，因此需要先确定特征点在声纳坐标系中的仰角值才能提取载体速度。特征点仰角的计算是前视声纳用于导航的一大障碍，在前视声纳垂直开角不大的情况下，可以直接假设所有特征点仰角为固定值，此时引入的误差并不是很大。但是为了实现更高精度的INS/前视声纳组合导航，就必须对特征点的仰角进行计算。

本文根据AUV巡检时的运动特点，提出了一种针对水底为局部平坦区域的特征点仰角估算方法。通常，IMU会严格平行于AUV主轴安装，即b系x轴指向AUV正前方，y轴指向AUV右侧。为了确保前视声纳能扫描到隧洞底部，声纳基阵会斜向下发射声波，即声纳坐标系的x轴是指向AUV的前下方，s系和b系之间通过绕y轴旋转φs角能使三坐标轴平行，如图6所示。

图6 特征点仰角计算几何示意图

AUV巡检时，通常将其设置为巡航模式，将深度传感器和罗盘、IMU测得的信息融入控制回路，使AUV所处深度基本保持不变，姿态维持稳定，因此俯仰角和横滚角维持在0°左右，变化幅度不大。图6中，水底的AB区域被声纳视域所覆盖，可以反射声波，形成图像，设A点距坐标原点S的距离为RSA，在声纳视域中比RSA更近的区域内没有障碍物，不会形成图像。因此，声纳图像中会出现一条较明显的分界线，该线为声波面SA与实际环境相交形成的，近似为弧线，如图7所示。分界线以内区域不能形成回波，因此像素值很小，图1中原点附近即有一条较明显的弧形分界线。

图7 声纳图像分界线示意图

当水底为起伏不大的平坦区域时，设声纳坐标系原点与分界线顶端距离为RSA，则在AUV维持水平姿态的情况下，SA与竖直方向的夹角是固定的，因此可以计算出AUV距水底的高度

(5)

对于水底成像视域内的一点P，其与坐标原点的距离是已知的，设为RSP，SP与水平面的夹角为

(6)

则P点在声纳坐标系中的仰角为

φP=φSP-φs

(7)

对于图像中的分界线，可以通过图像二值化和梯度检测等手段提取出来。由于AUV设置为定深模式，如果提前获取水深信息，在估算仰角过程中也可以将AUV距水底高度设为常值。当输水隧洞底部为圆弧状时，不能大范围运用水平面假设，只能在底部小区域内进行水平面近似。但是当水底区域起伏较大或者AUV自稳能力变差时，上述方法求解特征点仰角的误差会增大。

与仰角计算相比，更能影响速度解算结果的便是特征点的匹配精度。目前没有专门针对前视声纳图像的特征提取和匹配方法，设计导航系统时只能使用针对光学图像的ORB、SIFT和KAZE等算法，由于声纳图像的低分辨率，提取的配对点精度普遍不高，而且含有大量的误匹配。如果误匹配特征点出现在方程(4)中，则最后求解的载体速度和实际情况偏差较大。针对这个问题，本文设计的解决方案如下。

步骤一：

在k-1时刻，经卡尔曼滤波得到了载体的姿态、速度和位置，到了下一个滤波时刻k时，首先根据惯导解算出的未经滤波修正的速度，对每对特征点进行初步判定。由于2个滤波时刻之间的间隔较短，纯惯导解算出的速度在其间不会出现大的误差累积，有一定的可信度。图8所示为水下机器人搭载的某型光纤惯导系统在静止状态下的水平速度误差绝对值曲线。第100s之前，以零速为观测量进行卡尔曼滤波，解算出的水平速度维持在0附近；100s之后不再进行滤波修正，纯惯导解算出的水平速度误差绝对值会逐渐偏离0，但在断开滤波后的短时间内，惯导解算值与真值偏离得并不远，因此可用于对特征点数据进行初步筛选，剔除明显的误匹配点。

图8 纯惯导水平速度误差绝对值曲线

则先将该对特征点保留，否则舍弃。其中，Δd是声纳图像中的距离分辨率，本文实验部分所用前视声纳的分辨率为0.013m/pixel，T为时间间隔，k为一个筛选参数，与匹配噪声和惯导精度有关，若惯导系统精度较差，可将k值增大，防止原本正确的配对点被剔除。

步骤二：

经步骤一剔除了明显的野值之后，对于剩下的误差较大的配对点，本文仿照纯视觉导航中RANSAC算法的思路进行剔除，具体操作如下：

输入:连续两帧特征点集,最大迭代次数kmax,速度误差阈值Δvmax,nmax=0,k=0输出:特征内点集当knmax,则nmax=n,记录此时的内点集k=k+1,并重复执行以上过程结束返回最后记录的内点集,即为筛选出的特征内点

步骤三：

对于筛选出的N对特征点，通过取均值的方式求解最后的载体速度，计算公式为

(8)

求出载体速度后，便可将其作为观测量进行卡尔曼滤波。

3 INS/前视声纳组合导航模型

在INS/前视声纳组合导航系统中，取系统状态变量为

X=[φNφEφDδvNδvEδvD

δLδλδh]T

(9)

其中，φN、φE、φD分别代表沿北向、东向和地向的姿态失准角；δvN、δvE、δvD分别为北向速度误差、东向速度误差和地向速度误差；δL为纬度误差；δλ为经度误差；δh为高度误差。

姿态失准角方程为

(10)

速度误差方程为

(11)

位置误差方程为

(12)

依据北向和东向速度误差建立的观测方程为

(13)

(14)

另外，由定深模式下设定的深度值，可对状态变量中的高度误差建立观测

(15)

其中，υdepth是高度观测噪声。

联立方程(10)、(11)、(12)、(14)和(15)，便能得到连续时间状态下的卡尔曼滤波模型。将连续时间模型离散化后，在滤波周期内，经时间更新、量测更新、增益系数计算等过程后，便能得到系统状态变量的最优估计，而后对姿态、速度和位置进行修正。

4 实验验证

4.1 实验设计

用于实验验证的数据来源于南水北调工程中穿黄隧洞内的AUV测试数据，该隧洞位于河南焦作境内，地图上位置如图9所示。

图9 实验地点图

实验时所用的惯导系统为某型光纤惯性导航系统，其技术指标如表1所示。

表1 惯导系统技术指标

巡航模式下，AUV航向为沿着隧洞出口方向，根据IMU和前视声纳采集的数据，对INS/前视声纳组合算法性能进行验证和评估。在实际工程中，隧洞故障在河段上的具体位置是迫切需要获取的信息，因此AUV需要精确测量故障处离隧洞入口的距离，即需要精确测量检测到故障时自身在隧洞前向上运行了多长距离。输水隧洞中接收不到全球定位系统(Global Positioning System，GPS)信号，因此不能利用卫星定位信息对INS/前视声纳组合定位算法进行精度评估，对此，本文将隧洞内接缝线作为位置基准。实验隧洞中的接缝线是均匀分布的，每2条接缝线之间间距为10m，可在事后经过对声纳图像数据的甄别，将其中的隧洞接缝线标注出来，并作为定位基准，对组合算法精度进行评估。以AUV经过的第一条接缝线为起始线，而后经过每条接缝线时，AUV的位置便依次为距起始线10m、20m、30m……以此为评估基准。

在进行特征点提取和匹配时，本文的设计是在声纳图像的正前方区域截取一个固定的小窗口，从小窗口中提取特征进行数据关联，将小窗口所对应的实际水底区域近似为水平面，便于进行特征点仰角估计，图像其他区域的信息统统舍弃。

用于光学图像特征提取和匹配的算法有很多，经实际测试，用于前视声纳图像时每一种都会出现许多明显的误匹配点，目前没有具体数据能够衡量各种算法在匹配精度上的优劣。从算法的复杂性和实现实时系统的可行性的角度考虑，本文采用了一种快速特征点提取和描述(Oriented FAST and Rotated BRIEF，ORB)算法。ORB在平移、旋转、缩放的变换下仍有良好的表现，广泛应用于基于光学相机的实时SLAM系统中[10]，但目前将其用于声纳图像的尝试还比较少。

4.2 实验结果

整个实验过程中，AUV的状态可分为“缓慢前进-静止-缓慢前进”3个阶段，共进行了约1800s的IMU和前视声纳数据采集，前进过程中平均速度约为0.3m/s。除了进行INS/前视声纳组合的数据采集外，为便于对比，本文实验也在相同区域进行了INS/DVL组合的数据采集，运用两种组合定位方法分别对同一区域的接缝线进行定位，比较不同算法的定位效果。AUV经过第n条接缝线时，其相对于第一条接缝线的参考位置为10(n-1)m，图10所示分别为两种算法对各条接缝线的定位相对于参考位置的误差，表2列出了两种算法的均方根误差(RMS)、最大误差(MAX)和终点误差(END)。

(a)INS/DVL组合算法对各条接缝线定位误差

表2 两种算法的误差统计

实验中，INS/前视声纳组合的最大定位误差为3.0m，小于总行程的1%。从图表中可以看出，输水隧洞内，INS/前视声纳组合与INS/DVL组合的性能非常相似，两种算法的定位精度没有明显的优劣之分。但是在实际巡检工作中，DVL声波会对前视声纳图像产生干扰，使得声纳图像损失大量的成像细节，从图像中辨析出隧洞故障的难度会增大。因此，在巡检时卸载DVL、运用INS/前视声纳组合定位具有以下优势：1)从根本上消除DVL声波的干扰，提高声纳图像质量，确保故障定位任务的完成；2)降低巡检成本，卸载DVL能减小AUV功耗，节省蓄电池电能，进而延长AUV在水下持续工作的时间。

本文所提出的INS/前视声纳组合算法能为解决AUV导航定位问题提供新的思路，除了在输水隧洞巡检时可以替代INS/DVL组合之外，在其他诸如水下堤坝检修和水下地形探测等一些对声纳图像质量要求高的任务中，需要规避DVL干扰时，也可以采取INS/前视声纳组合替代INS/DVL组合。但是当AUV姿态自稳能力变差，或者前视声纳成像区域起伏较大，不能进行水平面近似时，本文提出的方法在估算特征点仰角时会存在一定误差，当AUV进行长航时、远距离的运动时，这种简化方式会使导航定位精度有所降低。

5 结论

本文针对AUV在输水隧洞巡检过程中存在的DVL对前视声纳图像产生干扰的问题，提出了一种从前视声纳图像提取载体速度，进而和IMU进行组合导航的方法。经实际的输水隧洞测试，验证了其具有较高精度，能发挥和INS/DVL组合相接近的作用，具有实际应用价值。在其他需要隔绝DVL对声纳图像干扰的应用场合，也可以选择卸载DVL，采用INS/前视声纳组合定位的方法。但其也存在需进一步解决的问题，例如本文在估算特征点仰角时，对水底进行了局部平面假设，如果隧洞横截面为弧形，这种假设只对弧形底部小范围内的特征点近似成立，如果大范围使用，需要进一步引入弧形底面的几何约束。