一种基于声呐图像的水下机器人定位方法

1.本发明属于图像处理研究领域。具体涉及对声呐图像进行处理，实现一种新的水下机器人定位方法。


背景技术：

2.目前，对于水下机器人的导航，主要采用惯性导航系统和水声导航定位系统。在惯性导航系统方面，如法国ixblue公司研制的光纤陀螺仪，在无辅助定位的情况下，精度为2min内误差0.3m。为了使定位更加准确，还需要配备gps和多普勒仪进行辅助定位，多用于水面舰艇和水下平台。在水声导航方面，如blueprint subsea公司的seatrac标准系统。seatrac标准系统包含一个x150usbl信标和一个x110应答器信标，可以实时跟踪rov和auv在2000米深度下的位置。其定位精度为0.1m，还需要结合vos的精度。但这两种设备构成的导航系统普遍价格昂贵。
3.声呐技术是利用声波在水中的传播和反射特性，通过电声转换和信息处理进行导航和测距，利用这种技术可以对水下目标进行探测。主要用于搜救打捞、海洋工程、码头检查、环境监测、现场清理和水下管道调查等领域。水下机器人配备声呐主要用于对水下物体进行观测，无法在仅依靠声呐的情况下实现水下机器人定位。如m750d多波束声呐设备，通常会配备buleprint的seatrac系统来实现定位，该定位系统需要两个信标，一个放置于水下的固定位置，一个放于机器人上，可以实现两者相对距离的测算。但依靠信标的定位方法要求两个信标处于同一开阔水域并能进行相互通信，存在一定的局限性。
4.本发明以特定条件下(密闭矩形地下水池)清淤机器人的工作环境为背景研究机载声呐成像及其处理，通过对背景特征的一系列检测和分类，并将像素距离转化为实际距离，最终实现水下机器人定位。


技术实现要素：

5.为了在不依靠惯性导航系统和水声通信定位系统，在水下机器人只配备声呐的情况下实现定位，本发明提供了一种基于声呐图像的高精度水下定位方法。
6.经过对声呐图像中池壁的成像和噪声特点的分析，选择使用高斯滤波、阈值分割、中值滤波以及canny边缘检测四个步骤完成池壁特征提取，再通过lsd线段检测返回池壁的像素坐标信息，最终通过线段分类以即像素距离到实际距离的转换实现水下机器人定位。
7.具体实现步骤如下；
8.s1，通过高斯滤波对声呐图像进行处理；
9.s2，通过对经过步骤s1处理后的图像灰度值设置阈值，对像素点进行分类，将检测目标与背景分离；
10.s3，通过中值滤波去除经过步骤s2处理后的图像底边两侧的强噪声；
11.s4，对经过步骤s1-s3处理后的图像进行canny边缘检测；
12.s5，采用lsd线段检测算法对步骤s4处理后的声呐图像进行线段提取；
13.s6，对检测的线段长度设置阈值，排除较短的线段，利用检测到线段的斜率和声呐图像原点对线段进行分类以此来确定池壁的位置信息；
14.s7，计算声呐图像原点到被检测线段的像素距离，利用声呐图像的像素高度和当前声呐最大量程的对应关系，换算出像素距离转化为实际距离的比例尺；
15.s8，以步骤s7得到的比例尺将像素距离转化为实际距离，得到距离参数；
16.s9，以步骤s8得到实际距离确定机器人位置坐标，对产生的坐标进行筛选，将产生的坐标每m个分为一组，按升序排序，计算中间n个坐标的平均值得到最终坐标，将坐标传入可视化界面进行显示，记录运动轨迹，若水池的宽度为已知量，可以利用水池宽度对机器人进行辅助定位；
17.s10，设置危险距离报警，当机器人距离墙壁小于一定距离时提醒机器人停止前行。
18.进一步的，s1中高斯滤波器的模板系数随着距离模板中心的距离增大而减小，服从二维高斯分布规律，其核心函数如下：
[0019][0020]
其中f(x,y)为图像所对应的灰度值，σ为方差，μ为均值，由于计算均值是中心点为原点，因此均值为0，可以得到：
[0021][0022]
进一步的，s2中实现目标与背景分离的方式如下；
[0023]
阈值分割的阈值是一个变量，具体操作是将输入的灰度图转化为数组形式，再对数组中的所有灰度值按升序进行排序，取数组后30％数量的灰度值的均值作为阈值，所取的30％是以池壁的成像面积所占整张图像的比例大小为依据；
[0024]
按阈值将灰度值设置为0或者255，以此来实现图像的二值化，其函数如下：
[0025][0026]
其中，dst(x,y)为阈值分割后像素坐标对应的灰度值，src(x,y)为未处理前像素坐标对应的灰度值，tresh为阈值分割的阈值，maxvel为最大灰度值255。
[0027]
进一步的，s6包括以下子步骤；
[0028]
s6.1，对线段进行分类确定池壁位置信息，若声呐垂直于水池底边进行探测，成像环境中位于机器人正前方的水池底边的池壁成像接近水平，位于水池两侧池壁成像接近两条垂直线段。但机器人在水下空间作业时，由于机器人的旋转和晃动，声呐设备的探测方向难以一直保持垂直于底边。为此提出利用检测线段的斜率和声呐图像原点(x0,y0)进行池壁三边的区分，即使声呐在一定程度的旋转范围内，也能实现线段的检测和分类；
[0029]
s6.2，在机器人刚放入水下时，声呐垂直于水池底边进行探测，经过lsd线段检测返回得到线段两个端点(x1,y1)与(x2,y2)，并结合已知的声呐图像原点(x0,y0)，可以通过以下公式对线段进行分类；
[0030][0031][0032]
当检测到的线段是水平线时，那么该线段属于水池的底边；当检测到的线段垂直时，那么该线段属于水池两条竖边中的一条，需要借助声呐图像原点的横坐标x0进一步判断，当检测到线段的中点横坐标小于x0时，该线段属于左竖边；当检测到线段呢的中点横坐标大于x0时，该线段属于右竖边。但声呐发生旋转时，若左竖边线段中点横坐标大于x0，或者右竖边线段中点横坐标小于x0，就很难保证左右竖边的区分正确，这时就必须对声呐实现线段检测的旋转角度进行计算。
[0033]
s6.3计算声呐实现线段检测的最小旋转角度，在机器人在刚进入水池且距离左竖边刚好等于安全工作距离sd，这时机器人与底边的距离bd最长，那么声呐旋转角度θ的最小值为，
[0034][0035]
若声呐以垂直底边为基准向左旋转角度小于θ，左竖边不会与声呐中轴线相交，此时线段分类正确。若声呐向左旋转角度超过θ，声呐中轴线与左竖边相交，此时线段分类出现错误。同理，当声呐在起点与右竖边距离等于机器人安全工作距离，声呐向右旋转角度小于θ，右竖边也不会与声呐中心轴相交造成线段分类错误。
[0036]
s6.4，由s6.3可知声呐以垂直于底边为基准，旋转角度在-θ到θ范围内旋转时，左右竖边的区分不会出现错误。之后可以利用检测到线段的斜率对竖边和底边进行区分；
[0037][0038]
tan(-θ)《slope《tan(θ)，底边
[0039]
(slope》tan(90
°‑
θ))∪(slope《tan(90
°
+θ))，竖边
[0040]
上述底边和竖边的斜率范围不会出现交集，所以底边与竖边的区分不会出现错误。对于左竖边与右竖边的区分，依旧需要依靠声呐图像原点的横坐标x0，其区分依据如下：
[0041][0042]
进一步的，s7所述包括以下子步骤；
[0043]
s7.1，当声呐只发生旋转不发生位移时，声呐原点与检测到线段的垂直距离不会发生改变，当检测到线段斜率不存在时，即x1等于x2。则声呐图像原点到被检测线段的像素距离公式如下：
[0044][0045]
s7.2，当检测到线段的斜率存在时，即x1不等于x2时，利用两个端点(x1,y1)和(x2,y2)，可以得到被检测直线的方程，并变换为:
[0046]
ax+by+c＝0
[0047]
其中a、b、c均为直线方程表达式的系数，通过计算可以得到，
[0048][0049]
利用点到直线的距离公式，
[0050][0051]
将声呐图像原点坐标代入，
[0052][0053]
其中距离d也是水下机器人到墙壁的像素距离，将声呐图像原点与左竖边的距离记为x
l
，与右竖边的距离记为xr，与底边的距离记为y。
[0054]
s7.3，利用声呐图像的像素高度和声呐当前最大量程的对应关系换算出像素距离到实际距离的比例尺，公式如下：
[0055][0056]
式中s为像素距离到实际距离转化的比例尺，range为当前声呐的最大量程，height为声呐的像素高度。
[0057]
进一步的，步骤s8中根据s7.3得到的比例尺s，将像素距离转化为实际距离，公式如下：
[0058][0059]
其中x
l
是声呐图像原点与左竖边的实际距离，xr是声呐图像原点与右竖边的实际距离，y是声呐图像原点与底边的实际距离，当机器人位于水池起点时，可以得到：
[0060][0061]
其中width为水池的宽度，length为水池的长度。
[0062]
进一步的，步骤s9中已知水池的宽度为w，按下面的方式进行辅助定位提高精度，
[0063]
w-0.1《x
l
+xr《w+0.1
[0064]
其中x
l
与xr分别为声呐与水池左右竖边的距离。
[0065]
进一步的，还包括步骤s11，机器人定位精度分析；具体实现方式如下，
[0066]
将声呐放置于机器人上，机器人在水池中移动所产生的坐标为(x
l
,y)，人为测量的真实坐标为用坐标的偏移量来表示定位精度；
[0067][0068]
与现有技术相比，本发明的优点和有益效果：惯性导航系统的导航信息是通过积分产生，其定位误差随着时间而增大，机器人的水下作业通常可达数小时，惯性导航系统难以保证在长时间工作下提供较高精度的定位信息。如法国ixblue的光纤陀螺仪phins compact c7在无辅助定位下，在工作时间为120s时的定位误差可达30cm,且在每次使用之前必须经过较长的初始对准时间。对于水声导航系统，如超短基线定位需要两个信标，一个信标放置在水下固定位置，另外一个信标放置于机器人上，对于密闭的水下空间而言，固定信标的安装存在一定的局限性。本发明的定位方法是对每一帧的声呐图像进行预处理后，经过lsd线段检测和线段分类，最后将像素距离转化为实际距离给机器人提供位置信息。位置信息并不通过积分产生，不会因为长时间工作导致定位精度降低，在使用之前无需进行较长时间的校准。实验场景下定位精度可达20cm以内，满足水下机器人的定位需求。利用该发明，配备声呐的水下机器人不仅可以实现水下场景的成像，而且可以在无其他的导航定位系统的情况下实现定位功能，一定程度上降低了经济成本。
附图说明
[0069]
图1为声呐设备所传输的原图。
[0070]
图2总体流程图。
[0071]
图3为声呐图像预处理过程。
[0072]
图4为声呐图像灰度图经过高斯滤波后的前后对比图。
[0073]
图5为声呐图像进行阈值分割后的结果。
[0074]
图6为声呐图像经过中值滤波后的结果。
[0075]
图7为声呐图像经过canny边缘检测后的结果。
[0076]
图8为声呐图像经过lsd线段检测后所返回的坐标，其中检测到线段的两个端点为(x1,y1)和(x2,y2)，声呐图像原点为(x0,y0)。
[0077]
图9为经过lsd线段检测后的图像与检测前的灰度图对比，图中将检测到线段的两个端点与声呐图像原点相互连接，方框处为线段误检测。
[0078]
图10声呐刚进入水池且与左竖边保持安全距离，垂直于底边探测。
[0079]
图11声呐刚进入水池且与左竖边保持安全距离，声呐向左侧旋转θ。
[0080]
图12声呐刚进入水池且与左竖边保持安全距离，声呐向左侧旋转超过θ。
[0081]
图13声呐刚进入水池且与右竖边保持安全距离，垂直于底边探测。
[0082]
图14声呐刚进入水池且与右竖边保持安全距离，声呐向右侧旋转θ。
[0083]
图15声呐刚进入水池且与右竖边保持安全距离，声呐向右侧旋转超过θ。
[0084]
图16经过池壁三边区分后的结果(水池底边标注为“p”，水池右竖边标注为“r”，水池左竖边标注为“l”，其中方框中出现的误检线段未被检测到。)
[0085]
图17左图为声呐当前的最大的量程，右为声呐图像的像素高度(箭头标注处)。
[0086]
图18为显示坐标的更新流程。
[0087]
图19为水下机器人位置坐标的可视化。
具体实施方式
[0088]
下面结合附图对本发明的技术方案作进一步阐述。
[0089]
本发明提供一种基于声呐图像的水下机器人定位方法，包括如下步骤：
[0090]
s1：通过高斯滤波对声呐图像进行处理，可以去除声呐图像中的斑点噪声，同时降低其中池壁线段出现的边缘不平整和内部间断等不需要的图像细节；
[0091]
s2：由于在声呐图像中，检测的目标(池壁)灰度值与背景存在差异，通过对灰度值设置阈值，对像素点进行分类，从而将目标与背景分离。
[0092]
s3：为了处理底边两侧的强噪声，本发明使用了中值滤波，中值滤波是一种非线性平滑技术，它将每一像素点的灰度值设置为该点邻域窗口内的所有像素点的灰度值的中值。其利用了二维滑动模板，将板内的像素点的灰度值进行排序，生成单调的按像素点大小进行排列的二维数组，并用该二维数组的中值取代中心像素点的灰度值，这样就可以去掉孤立的噪声点。
[0093]
s4：采用canny边缘检测算法对阈值分割后的图像进行边缘检测。由于在进行步骤s3之后，图中的灰度值的梯度变化较大，这样目标边缘提取变得比较容易，且具有很好的效果。
[0094]
s5：采用lsd线段检测对预处理后的声呐图像进行线段提取。
[0095]
s6：以s5检测的线段设置阈值，排除较短的线段。利用检测到线段的斜率和声呐图像原点对线段进行分类，以此来确定池壁的位置信息。
[0096]
s7：计算声呐图像原点到检测到的线段的像素距离，利用声呐图像的像素高度和声呐当前最大量程的对应关系，换算出像素距离转化为实际距离的比例尺。
[0097]
s8：以步骤s7得到的比例尺将像素距离转化为实际距离，得到三个距离参数x
l
,xr,y，用(x
l
,y)作为显示坐标。
[0098]
s9：以步骤s8得到实际距离确定机器人位置坐标，对产生的坐标进行筛选，将产生的坐标每12个分为一组，按升序排序，计算中间6个坐标的平均值得到最终坐标。将坐标传入可视化界面进行显示，并记录运动轨迹；若矩形水下空间的宽度是已知的，那么可以利用已知的水池宽度进行辅助定位。
[0099]
s10：设置危险距离报警，当机器人距离墙壁小于一定距离时提醒小车停止前行。本发明中设置安全距离为0.5m，则三个距离参数其中之一小于0.5m，该系统就会对小车发出报警，让小车停止前行。
[0100]
s11：机器人定位精度分析。
[0101]
步骤s1包含子步骤如下：
[0102]
s1.1，高斯滤波器的模板系数随着距离模板中心的距离增大而减小，服从二维高斯分布规律，其核心函数如下：
[0103]
[0104]
其中f(x,y)为图像所对应的灰度值，σ为方差，μ为均值，由于计算均值的中心点为原点，因此均值为0。可以得到：
[0105][0106]
步骤s2包含子步骤如下：
[0107]
s2.1，阈值分割的阈值是一个变量，具体操作是将输入的灰度图转化为数组形式，再对数组中的所有灰度值按升序进行排序，取数组后30％数量的灰度值的均值作为阈值，所取的30％是以池壁的成像面积所占整张图像的比例大小为依据；
[0108]
s2.2，按阈值将灰度值设置为0或者255，以此来将灰度图像转化为二值化图像，函数如下：
[0109][0110]
其中，dst(x,y)为阈值分割后像素坐标对应的灰度值，src(x,y)表示未处理前像素坐标对应的灰度值，tresh为阈值分割的阈值，maxvel为最大灰度值255。
[0111]
步骤s6包含子步骤如下：
[0112]
s6.1，将得到的线段长度进行排序后的中值作为阈值。经过实验这样的阈值选取方式是十分有效的，可以极大程度上避免检测到一些表征为线段的噪声。
[0113]
s6.2，对线段进行分类确定池壁位置信息，若声呐垂直于水池底边进行探测，成像环境中位于机器人正前方的水池底边的池壁成像接近水平，位于水池两侧池壁成像接近两条垂直线段。但机器人在水下空间作业时，由于机器人的旋转和晃动，声呐设备的探测方向难以一直保持垂直于底边。为此本发明提出利用检测线段的斜率和声呐图像原点(x0,y0)进行池壁三边的区分，即使声呐在一定程度的旋转范围内，也能实现线段的检测和分类；
[0114]
s6.3，在机器人刚放入水下时，声呐垂直于水池底边进行探测，经过lsd线段检测返回得到线段两个端点(x1,y1)与(x2,y2)，并结合已知的声呐图像的原点(x0,y0)，可以通过以下公式对线段进行分类；
[0115][0116][0117]
当检测到的线段是水平线时，那么该线段属于水池的底边；当检测到的线段垂直时，那么该线段属于水池两条竖边中的一条，需要借助声呐图像原点的横坐标x0进一步判断，当检测到线段的中点横坐标小于x0时，该线段属于左竖边；当检测到线段呢的中点横坐标大于x0时，该线段属于右竖边。此时也可以通过上述的线段分类方式确定水池的尺寸信息(后文步骤s8.1将提到如何计算)。但声呐发生旋转时，若左竖边线段中点横坐标大于x0，或者右竖边线段中点横坐标小于x0，就很难保证左右竖边的区分正确，这时就必须对声呐实现线段检测的旋转角度进行计算。
[0118]
s6.4，由s6.3得到水池的尺寸信息后，结合机器人在作业时的安全距离，可以计算声呐实现线段检测的最小旋转角度。如图10所示，在机器人在刚进入水池，且距离竖边刚好等于安全工作距离sd，这时小车与底边距离bd最长(后文s8.1将会提到如何计算机器人与池壁的距离)。如图11所示，若声呐向左旋转θ，那么水池的左竖边刚好与声呐中轴线相交，这时声呐实现线段检测的最小旋转角度θ，
[0119][0120]
若声呐以垂直底边为基准向左旋转角度小于θ，左竖边不会与声呐中轴线相交，此时线段分类正确。如图12所示，若声呐向左旋转角度超过θ，声呐中轴线与左竖边相交，此时线段分类出现错误。同理，当声呐在起点也与右竖边的距离等于机器人安全工作距离，声呐向右旋转角度小于θ，右竖边也不会与声呐中心轴相交造成线段分类错误。
[0121]
s6.5，由s6.4可知声呐可以在以垂直于底边为基准-θ到θ范围内旋转且不会导致左右竖边的区分出现错误，之后可以利用检测到线段的斜率对线段竖边和底边进行区分；
[0122][0123]
tan(-θ)《slope《tan(θ),底边
[0124]
(slope》tan(90
°‑
θ))∪(slope《tan(90
°
+θ)),竖边
[0125]
上述底边和竖边的斜率范围不会出现交集，所以底边与竖边的区分不会出现错误。对于左竖边与右竖边的区分，依旧需要依靠声呐图像原点的横坐标x0，其区分依据如下：
[0126][0127]
步骤s7包含子步骤如下：
[0128]
s7.1，当声呐只发生旋转不发生位移时，声呐原点与检测到线段的垂直距离不会发生改变，当检测到线段斜率不存在时，即x1等于x2。则声呐图像原点到被检测线段的像素距离公式如下：
[0129][0130]
s7.2，当检测到线段的斜率存在时，即x1不等于x2时，利用两个端点(x1,y1)和(x2,y2)，可以得到被检测直线的方程，并转化为：
[0131]
ax+by+c＝0
[0132]
其中a、b、c均为线段方程表达式的系数。
[0133]
通过计算可以得到，
[0134][0135]
我们可以利用点到直线的距离公式，
[0136][0137]
将声呐图像原点(x0,y0)坐标代入，
[0138][0139]
其中距离d就是水下机器人到池壁的像素距离，我们将声呐图像原点与左竖边的距离记为x
l
，与右竖边的距离记为xr，与底边的距离记为y。
[0140]
s7.3，利用声呐图像的像素高度(像素值)和声呐当前最大量程(实际值)的对应关系换算出像素距离到实际距离的比例尺公式如下：
[0141][0142]
式中s为像素距离到实际距离转化的比例尺，range为当前声呐的最大量程，height为当前声呐图像的像素高度，如图17所示两者是对应关系。
[0143]
步骤s8包含子步骤如下：
[0144]
s8.1，根据s7.3得到的比例尺s，将像素距离转化为实际距离，公式如下：
[0145][0146]
其中x
l
是声呐图像原点与左竖边的实际距离，xr是声呐图像原点与右竖边的实际距离，y是声呐图像原点与底边的实际距离。当机器人位于水池起点时，可以得到：
[0147][0148]
其中witdth为水池的宽度，length为水池的长度。
[0149]
步骤s9，包含子步骤如下：
[0150]
s9.1若已知水池的宽度为w，我们可以按下面的方式进行辅助定位提高精度，
[0151]
w-0.1《x
l
+xr《w+0.1
[0152]
其中x
l
与xr分别为声呐与水池左右竖边的距离，理论在x
l
与xr的和应该等于水池的宽度，但在实际检测中会存在误差，所以我们对其检测到的水池宽度设置
±
0.1m的阈值，当检测到水池的宽度处于阈值范围内时，机器人的定位较为准确。
[0153]
步骤s11包含子步骤如下：
[0154]
将声呐放置于机器人上，机器人在水池中移动检测所产生的坐标为(x
l
,y)，人为
测量的真实坐标为用坐标的偏移量来表示定位精度。
[0155][0156]
以下是定位精度表：
[0157][0158]
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

技术特征：
1.一种基于声呐图像的水下机器人定位方法，其特征在于，包括以下步骤：s1，通过高斯滤波对声呐图像进行处理；s2，通过对经过步骤s1处理后的图像灰度值设置阈值，对像素点进行分类，将检测目标与背景分离；s3，通过中值滤波去除经过步骤s2处理后的图像底边两侧的强噪声；s4，对经过步骤s1-s3处理后的图像进行边缘检测；s5，采用线段检测算法对步骤s4处理后的声呐图像进行线段提取；s6，对检测的线段长度设置阈值，排除较短的线段，利用检测到线段的斜率和声呐图像原点对线段进行分类以此来确定池壁的位置信息；s7，计算声呐图像原点到被检测线段的像素距离，利用声呐图像的像素高度和当前声呐最大量程的对应关系，换算出像素距离转化为实际距离的比例尺；s8，以步骤s7得到的比例尺将像素距离转化为实际距离，得到距离参数；s9，以步骤s8得到实际距离确定机器人位置坐标，对产生的坐标进行筛选，将产生的坐标每m个分为一组，按升序排序，计算中间n个坐标的平均值得到最终坐标，将坐标传入可视化界面进行显示，记录运动轨迹，若水池的宽度为已知量，可以利用水池宽度对机器人进行辅助定位；s10，设置危险距离报警，当机器人距离池壁小于一定距离时提醒机器人停止前行。2.根据权利要求1所述的一种基于声呐图像的水下机器人定位方法，其特征在于：s1中高斯滤波器的模板系数随着与模板中心的距离增大而减小，服从二维高斯分布规律，其核心函数如下：其中f(x,y)为图像所对应的灰度值，σ为方差，μ为均值，由于计算均值的中心点为原点，因此均值为0，可以得到：3.根据权利要求1所述的一种基于声呐图像的水下机器人定位方法，其特征在于：s2中实现目标与背景分离的方式如下；阈值分割的阈值是一个变量，具体操作是将输入的灰度图转化为数组形式，对数组中的所有灰度值按升序进行排序，取数组后30％数量的灰度值的均值作为阈值，所取的30％是以池壁的成像面积所占整张图像的比例大小为依据；按阈值将灰度值设置为0或者255，以此来实现图像的二值化，其函数如下：其中，dst(x,y)为阈值分割后像素坐标对应的灰度值，src(x,y)为未处理前像素坐标对应的灰度值，tresh为阈值分割的阈值，maxvel为最大灰度值255。4.根据权利要求1所述的一种基于声呐图像的水下机器人定位方法，其特征在于：s6包括以下子步骤；
s6.1，对线段进行分类确定池壁位置信息，若声呐垂直于水池底边进行探测，成像环境中位于机器人正前方的水池底边池壁成像接近水平，位于水池两侧池壁成像接近两条垂直线段；但机器人在水下空间作业时，由于机器人的旋转和晃动，声呐设备的探测方向难以一直保持垂直于底边；为此提出利用检测线段的斜率和声呐图像原点(x0,y0)进行池壁三边的区分，即使声呐在一定程度的旋转范围内，也能实现线段的检测和分类；s6.2，在机器人刚放入水下时，声呐垂直于水池底边进行探测，经过lsd线段检测返回得到线段两个端点(x1,y1)与(x2,y2)，并结合已知的声呐图像原点(x0,y0)，可以通过以下公式对线段进行分类；式对线段进行分类；当检测到的线段是水平线时，那么该线段属于水池的底边；当检测到的线段垂直时，那么该线段属于水池两条竖边中的一条，需要借助声呐图像原点的横坐标x0进一步判断，当检测到线段的中点横坐标小于x0时，该线段属于左竖边；当检测到线段呢的中点横坐标大于x0时，该线段属于右竖边；但声呐发生旋转时，若左竖边线段中点横坐标大于x0，或者右竖边线段中点横坐标小于x0，就很难保证左右竖边的区分正确，这时就必须对声呐实现线段检测的旋转角度进行计算；s6.3，计算声呐实现线段检测的最小旋转角度，在机器人刚进入水池且距离左竖边刚好等于安全工作距离sd，这时机器人与底边的距离bd最长，那么声呐旋转角度θ的最小值为，若声呐以垂直底边为基准向左旋转角度小于θ，左竖边不会与声呐中轴线相交，此时线段分类正确；若声呐向左旋转角度超过θ，声呐中轴线与左竖边相交，此时线段分类出现错误；同理，当声呐在起点也与右竖边距离等于机器人安全工作距离，声呐向右旋转角度小于θ，右竖边也不会与声呐中心轴相交造成线段分类错误；s6.4，由s6.3可知声呐以垂直于底边为基准，旋转角度在-θ到θ范围内旋转时，左右竖边的区分不会出现错误，之后可以利用检测到线段的斜率对竖边和底边进行区分；tan(-θ)<slope<tan(θ)，底边(slope>tan(90
°‑
θ))∪(slope<tan(90
°
+θ))，竖边上述底边和竖边的斜率范围不会出现交集，所以底边与竖边的区分不会出现错误；对于左竖边与右竖边的区分，依旧需要依靠声呐图像原点的横坐标x0，其区分依据如下：
5.根据权利要求1所述的一种基于声呐图像的水下机器人定位方法，其特征在于：s7所述包括以下子步骤；s7.1，当声呐只发生旋转不发生位移时，声呐原点与检测到线段的垂直距离不会发生改变，当检测到线段斜率不存在时，即x1等于x2；则声呐图像原点到被检测线段的像素距离公式如下：s7.2，当检测到线段的斜率存在时，即x1不等于x2时，利用两个端点(x1,y1)和(x2,y2)，可以得到被检测直线的方程，并变换为：ax+by+c＝0其中a、b、c均为直线方程表达式的系数，通过计算可以得到，利用点到直线的距离公式，将声呐图像原点坐标代入，其中距离d也就是水下机器人到池壁的像素距离，将声呐图像原点与左竖边的距离记为x
l
，与右竖边的距离记为x
r
，与底边的距离记为y；s7.3，利用声呐图像的像素高度和声呐当前最大量程的对应关系换算出像素距离到实际距离的比例尺，公式如下：式中s为像素距离到实际距离转化的比例尺，range为当前声呐的最大量程，height为声呐图像的像素高度。6.根据权利要求5所述的一种基于声呐图像的水下机器人定位方法，其特征在于：步骤s8中根据s7.3得到的比例尺s，将像素距离转化为实际距离，公式如下：
其中x
l
是声呐图像原点与左竖边的实际距离，x
r
是声呐图像原点与右竖边的实际距离，y是声呐图像原点与底边的实际距离，当机器人位于水池起点时，可以得到：其中width为水池的宽度，length为水池的长度。7.根据权利要求1所述的一种基于声呐图像的水下机器人定位方法，其特征在于：步骤s9中已知水池的宽度为w，按下面的方式进行辅助定位提高精度，w-0.1<x
l
+x
r
<w+0.1其中x
l
与x
r
分别为声呐与水池左右竖边的距离。8.根据权利要求1所述的一种基于声呐图像的水下机器人定位方法，其特征在于：还包括步骤s11，机器人定位精度分析；具体实现方式如下，将声呐放置于机器人上，机器人在水池中移动检测所产生的坐标为(x
l
,y)，人为测量的真实坐标为用坐标的偏移量来表示定位精度；9.根据权利要求1所述的一种基于声呐图像的水下机器人定位方法，其特征在于：步骤s4中采用canny边缘检测算法；步骤s5中采用lsd线段检测算法。

技术总结
本发明提出了一种基于声呐图像的水下机器人定位方法，主要用于水下机器人定位，使机器人在密闭水下空间作业时能够获得实时位置信息，避免与空间壁发生碰撞。首先，利用多波束声呐对水下空间进行探测，发现水下空间壁的声呐图像具有线段特征。再对声呐图像进行复合去噪、阈值分割和Canny边缘检测处理，初步提取出空间壁轮廓。然后利用LSD线段检测算法精确检测出空间壁特征线段。最后进行线段分类，本发明提出了一种利用声呐图像的原点和检测线段的斜率对线段进行有效分类的方法，在特定矩形水下空间下实验精度可达20cm以内。若增加更多检测线段所对应的斜率变化范围，该发明还可以应用于非矩形空间的水下定位。应用于非矩形空间的水下定位。应用于非矩形空间的水下定位。


技术研发人员：李利荣 梅冰 陈鹏 丁江 张云良 吴方祥 巩朋成
受保护的技术使用者：湖北工业大学
技术研发日：2022.07.22
技术公布日：2022/11/1