一种基于声呐的时空关联地图实时构建方法

技术领域

本发明涉及水下机器人声呐图像处理领域，具体为一种基于声呐的时空关联地图实时构建方法。

背景技术

在半未知或完全未知环境下，为完成自主航行与作业任务，水下机器人必须具备通过传感器逐步建立环境地图的能力。声学传感器具有感知距离远、环境依赖性低的优良特性，多波束前视声呐更可实时提供图像流，成为水下感知传感器的首选。

与光学图像相比，前视声呐图像质量更低，因此使用含噪声呐图像实时构建环境地图对图像处理算法提出了更高的要求。此外，现有的机器人地图构建方法往往基于静态环境假设，但在水下机器人作业环境中，还可能存在动态物体。

目前，从含噪声呐图中提取环境信息的主流方法有两种：基于迭代计算的阈值分割方法和基于图像建模的图像分割方法。基于迭代计算的方法通过遍历图像中灰度值，选取对噪声抑制较好的阈值，对声呐图像进行阈值分割；但该方法计算分割阈值时需要多次遍历图像各像素点，不适用于实时建图。基于图像建模的方法通过建立描述声呐图像噪声的模型，使用马尔可夫随机场估算目标和背景的模型参数，然后利用该参数进行声呐图像分割；该方法要求所建立的描述模型与实际噪声相匹配，同时模型参数估计的准确性也影响图像分割的效果。

现有机器人地图实时构建方法中，占用栅格法具有简单、直观和可描述不确定性信息等优点，可较好地应用于机器人导航、避障和定位等研究。针对室内半结构化环境，Wolf算法通过处理激光雷达数据分别建立静、动态栅格地图，将二者合并即可得到整个环境的完整描述。但是，Wolf算法中动态物体在全局动态地图标记后，即使未能再次观测到该动态物体，仍然认为该物体存在。此外，Wolf算法未设计选择性更新机制，对地图进行全局更新的时间和空间代价较大。

综上所述，基于声呐图像的实时建图方法还未到完善的地步，对于存在动态物体的水下环境建图，主要存在图像信息提取效率低和动态地图未与时间关联的问题。因此，迫切需要提出一种可实时提取图像信息并将空间栅格与时间相关联的静、动态环境建图方法，以进一步促进水下机器人技术的发展。

发明内容

针对现有方法图像信息提取效率低和动态地图未与时间关联的问题，本发明通过划分灰度区间并取区间均值作为备选分割阈值、为动态栅格地图增加生存时间约束等方法，依据水下声呐图像的特点及其与全局地图的时空联系，提供了一种基于声呐的时空关联地图实时构建方法。

本发明的技术方案为：

所述一种基于声呐的时空关联地图实时构建方法，包括以下步骤：

步骤1：建立地面坐标系，并在地面坐标系下建立静态地图sm和动态地图dm，设定地图栅格数量与大小，并且初始化地图中各栅格占用概率为l

步骤2：获取声呐图像，对于当前帧声呐图像，利用最优分割阈值对声呐图像进行二值化处理；

步骤3：按照步骤1设定的栅格参数对步骤2获得的图像进行栅格化处理，并使用量化模型输出图像中处于声呐视域内的每个栅格对应的量化结果s(d)l

步骤4：利用步骤3得到的当前帧声呐图像的量化模型输出结果，使用静态反演观测模型选择性更新静态地图，并使用动态反演模型选择性更新动态地图；其中更新动态地图时，先校验各栅格的生存时间是否到达预设约束值，若达到，则直接将该栅格占用概率设置为初始值l

进一步的，步骤2中，当采用所述最优分割阈值对声呐图像进行二值化处理后，若得到的图像中，面积小于设定值的连通区域的数量多于预设限制值，则增大所述最优分割阈值，再利用更新后的最优分割阈值对声呐图像进行二值化处理，如此循环直至面积小于设定值的连通区域的数量不多于预设限制值。

进一步的，步骤2中，若得到的图像中，面积小于设定值的连通区域的数量多于预设限制值，则修正最优分割阈值T

进一步的，步骤2中，所述最优分割阈值通过以下过程得到：

对于当前帧声呐图像，获取图像中像素灰度值的最大值T

n

计算R区间灰度加权平均值

T

以k

其中

定义声呐图像中灰度低于阈值的像素点为背景B，不低于阈值的像素点为物体O，然后分别以k

进一步的，步骤2中，使用最大熵法计算阈值T下的背景和物体信息熵之和H为：

其中L

进一步的，所述声呐视域根据声呐的探测距离和水平开角确定。

进一步的，所述占用概率分为l

进一步的，步骤3中量化结果s(d)l

θ＝arctan((y

其中，(x

进一步的，步骤4中，仅使用步骤3量化结果中不等于l

进一步的，步骤4中，若声呐载体当前时刻的位姿为x

其中，sl

动态地图的更新方程为

其中，dl

静态反演测量模型为：

动态反演测量模型为：

有益效果

1.本发明利用最优分割阈值对声呐图像进行二值化处理，并优选使用灰度区间灰度加权均值为备选阈值，评估分割效果并迭代缩小灰度区间的方法选取最佳分割阈值，可减少阈值选取时迭代计算次数；

2.本发明使用时空关联的方法分别构建实时动静态栅格地图，并对动态物体栅格加以生存时间约束，可更为准确地描述存在动态物体的水下环境。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1：算法执行流程图；

图2：声呐安装示意图；

图3：图像坐标系与声呐坐标系关系图；

图4：阈值分割结果图；

图5：实时建图效果图；

图6：ROS运行算法时节点与话题图。

具体实施方式

本实施例为用于水下机器人的一种基于前视声呐的时空关联地图实时构建方法，算法执行流程图如图1所示，在实施例中，包括以下步骤：

步骤1：建立地面坐标系，设定地面坐标系的原点为机器人下水点，x轴指向正北，y轴指向正东；并在地面坐标系下建立静态地图sm和动态地图dm，设定地图栅格数量为200×200个，栅格大小为dx＝dy＝0.2m，并且初始化地图中各栅格占用概率为l

静态地图sm和动态地图dm中表示为有限多个栅格单元sm

而为实现单帧声呐图像到全局地图的增量式融合，根据机器人在地面坐标系中的位姿和声呐传感器在机器人上的安装位置与角度，结合声呐成像原理，使用坐标转换矩阵的形式，建立单帧声呐图像像素点到全局地图栅格之间的转换关系：

如图2所示，设此时水下机器人的欧拉角为(φ,θ,ψ)，声呐安装于载体坐标系中的(x′

其中，

步骤2：获取声呐图像，对于当前帧声呐图像，利用最优分割阈值对声呐图像进行二值化处理。

所述最优分割阈值通过以下过程得到：

对于当前帧声呐图像，获取图像中像素灰度值的最大值T

n

计算R区间灰度加权平均值

T

以k

其中

定义声呐图像中灰度低于阈值的像素点为背景B，不低于阈值的像素点为物体O，然后分别以k

这里使用最大熵法计算阈值T下的背景和物体信息熵之和H为：

其中L

当采用所述最优分割阈值对声呐图像进行二值化处理后，若得到的图像中，面积小于设定值的连通区域的数量多于预设限制值，则令T

本实施例中，声呐选用BluePrint公司Oculus M750d声呐，工作模式设置为高频f＝1.2MHz，声呐的探测距离为l＝20m，水平开角θ

图4是所选取的3帧图像进行处理后的效果，由此可知进行小连通区域数量判断后，分割后图像中存在的噪声点数目明显降低，障碍物与噪声之间具有更好的区分度。但是，障碍物的明显程度也有一定的下降，这是算法选取了更高的灰度分割阈值导致的。

步骤3：按照步骤1设定的栅格参数对步骤2获得的图像进行栅格化处理，并使用量化模型输出图像中处于声呐视域内的每个栅格对应的量化结果s(d)l

θ＝arctan((y

其中，(x

步骤4：利用步骤3得到的当前帧声呐图像的量化模型输出结果，过滤动态物体，使用静态反演观测模型选择性更新静态地图，并对照静态地图，使用动态反演模型选择性更新动态地图；其中更新动态地图时，先校验各栅格的生存时间是否到达预设约束值，若达到，则直接将该栅格占用概率设置为初始值l

此时，静、动态全局地图选择性更新策略为，仅使用步骤3量化结果中不等于l

其中，sl

动态地图的更新方程为

其中，dl

反演测量模型根据当前时刻的静、动态地图与量测数据，给出该栅格此时刻被占用的概率值。在反演测量模型中，通过对比静、动态地图，区分静态物体与动态物体。具体来说，一方面，环境中的物体不可能即是动态又是静态的，故静态与动态地图在某一栅格的取值不能相同；另一方面，静态物体可作为运动物体的参照物，故使用同一时刻该栅格的静态占用值作为更新动态地图的参考。

例如，当某一栅格的状态从未知变为被占用时，将其视为静态物体；但当某一栅格的状态从空闲变为被占用时，并不将其视为静态物体，而是将其视为动态物体。根据上述原则，设计静态反演测量模型如表1所示。

表1：静态反演测量模型

该建图方法的时空关联性表现为，一方面，随着不同时刻量测的融合，静、动态全局地图都是时变的；另一方面动态物体的位置与时间紧密关联，故动态地图栅格的取值仅在其被观测后的一定时间内有意义。

因此，将动态地图栅格与时间相关联，对其设置生存时间约束，超过指定时间t

表2：动态反演测量模型

在实施例中，利用所获取的声呐图像流，实时进行环境建图，其结果如图5所示。图5(a)、(c)、(e)分别表示t＝1s,40s,60s时获取的静态地图；(b)、(d)、(f)则为t＝1s,40s,60s时获取的动态地图，图中红色箭头表示机器人此时所在位置。

图5(a)、(c)、(e)中有部分动态噪声未能消除，主要是因为定位误差未被完全滤除。最终使用1405帧图像完成建图，所设计的算法可以很好地区分环境中的动态实验人员、BlueROV2和静态石阶，构建的地图精度较高。同时，仅考虑局部视野中有效栅格的更新机制使地图更新时间复杂度从O(n

图6为本发明所提出的声呐图像处理与建图算法在ROS平台上运行时的节点与话题图，其中/sonar_oculus_node为读取声呐原始数据节点，/sonar_oculus_viewer为声呐图像预处理节点，/cv_process为本文所提出的阈值分割与实时建图算法节点；/ping话题为原始声呐数据，/image为处理后OpenCV格式声呐二值图像，/auv_states为经滤波处理后的机器人当前状态；/occu_map发布此时刻的实时占用地图。

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作任何的简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。