CN112581610A

CN112581610A - 从多波束声呐数据中建立地图的鲁棒优化方法和系统

Info

Publication number: CN112581610A
Application number: CN202011112700.1A
Authority: CN
Inventors: 陈德山; 刘衡睿
Original assignee: Wuhan University of Technology WUT
Current assignee: Wuhan University of Technology WUT
Priority date: 2020-10-16
Filing date: 2020-10-16
Publication date: 2021-03-30
Anticipated expiration: 2040-10-16
Also published as: CN112581610B

Abstract

本发明公开了一种从多波束声呐数据中建立地图的鲁棒优化方法和系统，应用在声呐图像技术，该方法包括以下步骤：获取一组声呐图像数据以及所述声呐图像数据对应的水下航行器的航行数据；根据水下航行器的航行数据建立水下航行器位姿优化模型；根据所述声呐图像数据，构建地图点云关联模型；根据所述行器位姿优化模型和所述点云关联模型构建位姿优化代价函数；通过最小二乘法对所述位姿优化代价函数进行求解，得到优化量；根据所述优化量和所述声呐图像数据得到优化后的水下环境数据。本方案可以产生更加接近于真实水下场景的。

Description

从多波束声呐数据中建立地图的鲁棒优化方法和系统

技术领域

本发明涉及声呐图像技术，尤其是一种从多波束声呐数据中建立地图的鲁棒优化方法。

背景技术

随着当今科技的飞速进步，自主式水下航行器(Autonomous UnderwaterVehicle，AUV)已经成为水下目标探查、搜救探索以及资源开发等水下环境测绘的重要载体。水下信息感知主要由声、光、磁、电等传感器接收的水下环境和信息和目标信号，但是水下环境相对复杂，视觉、激光等一些在陆地上常用的传感器在水下环境则需要一些类似水域有足够的清晰度、传感器与探测表面距离足够近等局限条件，因此远程信息感知常用的是声场信息，即在水下机器人在获取大范围水下环境数据主要依托声呐完成，相较于其他传感器，声呐更适合水下探测的相关任务。多波束声呐是最常用的声呐设备之一，其不仅测量范围大、速度快，而且具有较高的精确度和检测效率，能够对数字化以及实时绘图进行有效的记录，实现了对上百个水下目标点的形状、大小以及高低变化等的检测。但由于水下机器人在运动过程中由于环境恶劣、可用信息少、水下暗流等一些随机因素影响着水下机器人的运动，从而引起一些不确定性的误差，导致在利用传感器进行航迹以及位姿推算更新时会产生较大的累计误差，使得到的水下机器人的位姿信息精度降低，如何减小水下机器人在运动过程中由传感器带来的测量误差，提高位姿数据精度，逐渐成为移动机器人定位研究的热点问题。

水下机器人的同时定位与地图构建技术(simultaneous localization andmapping,SLAM)是当前实现水下未知环境下导航定位的关键性技术，也是当前水下地图绘制的研究热点，同时定位与制图技术是指搭载特定传感器的主体，在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自身的运动。

在相关技术中，构建的水下场景精度比较差需要进行改进。

发明内容

为解决上述技术问题的至少之一，本发明的目的在于：提供一种可以比较准确地探测出水下场景的从多波束声呐数据中建立地图的鲁棒优化方法和系统。

第一方面，本发明实施例提供了：

一种从多波束声呐数据中建立地图的鲁棒优化方法，包括以下步骤：

获取一组声呐图像数据以及所述声呐图像数据对应的水下航行器的航行数据；

根据水下航行器的航行数据建立水下航行器位姿优化模型；

根据所述声呐图像数据，构建地图点云关联模型；

根据所述航行器位姿优化模型和所述点云关联模型构建位姿优化代价函数；

通过最小二乘法对所述位姿优化代价函数进行求解，得到优化量；

根据所述优化量和所述声呐图像数据得到优化后的水下环境数据。

在部分实施例中，所述航行数据包括时间戳数据、声呐朝向角数据、航行朝向角数据、深度数据和航行速度数据。

在部分实施例中，所述水下航行器位姿优化模型N为：

N＝(Y_t-X_t)^TE*A^-1(Y_t-X_t)+(ΔY_t-ΔX_t)^TB^-1(ΔY_t-ΔX_t)

其中，ΔY_t＝Y_t-Y_t-1，ΔX_t＝X_t-X_t-1；A为高斯分布协方差矩阵；A^-1为信息矩阵，用于表示传感器的误差，E为设定信任矩阵，t为行情器的航行时间，Y_t表示时间为t的待优化数据，X_t表示时间为t的航行数据。

在部分实施例中，其中，X_t和Y_t均为五维向量，X_t和Y_t中的参数均包括俯仰角数据pitch、偏航角数据roll、翻滚角数据yaw、航行朝向角数据cog和深度数据depth。

在部分实施例中，所述地图点云关联模型M为：

M＝∑min[α,min^j{(f(Y_t,Z_t)ⁱ-f(Y_t-1,Z_t-1)^j)^T*C^-1*(f(Y_t,Z_t)ⁱ-f(Y_t-1,Z_t-1)^j)}]

其中，C为高斯协方差矩阵，C^-1为信息矩阵；Z_t表示在t时刻下的声呐图像数据在本地坐标系中的坐标值；j表示在t-1时刻下的声呐图像数据映射到三维空间中的第j个点云；内层min表示在t-1时刻下的空间映射函数前三维数据与t时刻下的空间映射函数前三位数据中最近的点；f(Y_t,Z_t)ⁱ为空间映射函数，其结果为五维向量。

在部分实施例中，所述f(Y_t,Z_t)ⁱ＝[R(pitch,roll,yaw,cog)*Z_t,cog,depth]

其中，R为旋转矩阵函数；i表示在t时刻下三维空间中的第i个点云，即函数结果的前三维向量。α为最大距离系数；其中，

在部分实施例中，所述位姿优化代价函数F为：

第二方面，本发明实施例提供了：

一种从多波束声呐数据中建立地图的鲁棒优化系统，包括：

获取单元，用于获取一组声呐图像数据以及所述声呐图像数据对应的水下航行器的航行数据；

第一模型构建单元，用于根据水下航行器的航行数据建立水下航行器位姿优化模型；

第二模型构建单元，用于根据所述声呐图像数据，构建地图点云关联模型；

函数构建单元，用于根据所述行器位姿优化模型和所述点云关联模型构建位姿优化代价函数；

计算单元，用于通过最小二乘法对所述位姿优化代价函数进行求解，得到优化量；

优化单元，用于根据所述优化量和所述声呐图像数据得到优化后的水下环境数据。

第三方面，本发明实施例提供了：

一种从多波束声呐数据中建立地图的鲁棒优化系统，包括：

存储器，用于存储程序；

处理器，用于加载所述程序以执行从多波束声呐数据中建立地图的鲁棒优化方法。

本发明实施例的有益效果是：利用水下航行器的位姿数据和多波束声呐的图像数据，建立航行器位姿模型和地图点云关联模型，利用这两个模型构建水下航行器位姿的代价函数，利用捆集调整的思想把所有探测数据中水下航行器位姿数据放到一起进行优化，以最小二乘的方法进行最优化求解，得到更加准确的水下真实场景。

附图说明

图1为根据本发明实施例提供的一种从多波束声呐数据中建立地图的鲁棒优化方法的流程图；

图2为根据本发明实施例提供的另一种从多波束声呐数据中建立地图的鲁棒优化方法的流程图。

具体实施方式

下面结合说明书附图和具体的实施例对本发明进行进一步的说明。

水下机器人的同时定位与地图构建技术(simultaneous localization andmapping,SLAM)是当前实现水下未知环境下导航定位的关键性技术，也是当前水下地图绘制的研究热点，同时定位与制图技术是指搭载特定传感器的主体，在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自身的运动。其关键核心思想是根据传感器所获得的地图信息和水下航行器的位姿信息构建代价函数，利用捆集调整(BundleAdjustment,BA)的方法对水下航行器的位姿和所获得地图信息进行优化调整，且用最小二乘方法对所构建的代价函数进行求解，最终得到优化后的水下航行器位姿和水下场景地图。

本申请利用水下航行器的位姿数据和多波束声呐的图像数据，建立航行器位姿模型和声呐距离传感器模型，利用这两个模型构建水下航行器位姿的代价函数，利用捆集调整的思想把所有探测数据中水下航行器位姿数据放到一起进行优化，以最小二乘的方法进行最优化求解，得到优化后的水下真实场景。

参照图1，一种从多波束声呐数据中建立地图的鲁棒优化方法，包括以下步骤：

步骤110、获取一组声呐图像数据以及所述声呐图像数据对应的水下航行器的航行数据。

需要理解的是，一组声呐图像数据包括多帧声呐图像，由于图像帧的采样周期不一定是固定的，因此相邻帧之间的时间差并不一定相同。本实施所指的水下航行器包括潜艇和鱼雷等。航行数据包括时间戳数据T、声呐朝向角数据

航行朝向角数据cog、深度数据depth、航行速度数据v。在本步骤中可以对采集的数据进行预处理，一方面对声呐图像数据进行处理，得到目标轮廓单线图像，另一方面对航行数据进行处理，通过时间戳数据T计算出采集声呐图像数据的时间间隔T¹，计算公式如下所示：

T＝{t₀,t₁,t₂,t₃……t_n-2,t_n-1,t_n} (1)

T¹＝{0,t₁-t₀,t₂-t₀……t_n-1-t₀,t_n-t₀} (2)

步骤120、根据水下航行器的航行数据建立水下航行器位姿优化模型。

在本步骤中，首先设定Y_t为待优化变量，具体形式如下：

Y_t＝(Pitch,Roll,Yaw,cog,depth) (3)

上述Y_t表示在t时刻下的待优化数据值，其为五维向量。设定X_t为传感器所测得的数据，其具体形式如下：

X_t＝(Pitch,Roll,Yaw,cog,depth) (4)

上述X_t表示在t时刻下的传感器所测得的数据，为五维向量。其中，X_t和Y_t均为五维向量，X_t和Y_t中的参数均包括俯仰角数据pitch、偏航角数据roll、翻滚角数据yaw、航行朝向角数据cog和深度数据depth。

根据上述设定的Y_t、X_t建立条件概率分布，并利用贝叶斯法则得到后验概率如公式(5)所示，但是在此问题中无法得知先验概率P(Y_t)，因此利用求解最大似然估计的方法得到公式(6)，然后由于这里本实施例假定由传感器探测数据的噪声服从高斯分布，因此通过最小化负对数的方法求解高斯分布的最大似然估计，从而得到公式(7)：

P(Y_t|X_t)∝argmaxP(X_t|Y_t) (6)

P(Y_t|X_t)∝argmin((Y_t-X_t)^TE*A^-1(Y_t-X_t)) (7)

其中：

A为高斯分布协方差矩阵，在本方法中可表示为5*5的对角阵；

A^-1为信息矩阵，在表达式中为5*5矩阵，可以理解为传感器的误差率。

E为信任系数矩阵，在X_t中，由于前四维是利用惯性传感器测得数据，因此随着测量时间的增加，其偏差越来越大，为相对测量数据。最后一维depth为传感器通过水的压力测得数据，为绝对测量数据，偏差量不随时间改变。因此在此设定信任系数矩阵E为：

其中：

t为航行器的航行时间；

λ为经验系数。

设定ΔY_t＝Y_t-Y_t-1，ΔX_t＝X_t-X_t-1建立条件概率分布如下：

P(ΔY_t|ΔX_t)∝P(ΔX_t|ΔY_t)*P(ΔY_t) (8)

P(ΔY_t|ΔX_t)∝argmin((ΔY_t-ΔX_t)^TB^-1(ΔY_t-ΔX_t)) (9)

其中：

B为高斯分布协方差矩阵，在本方法中可表示为5*5的对角阵。

B^-1为信息矩阵，在表达式中为5*5矩阵。

结合公式(7)、(9)可以得到建立的水下航行器位姿优化模型，如公式(10)所示：

N＝(Y_t-X_t)^TE*A^-1(Y_t-X_t)+(ΔY_t-ΔX_t)^TB^-1(ΔY_t-ΔX_t) (10)

其中：

N为建立的模型。

步骤130、根据所述声呐图像数据，构建地图点云关联模型。

仅仅根据上述的航行器模型是无法进行求解的，因此在本实施例中加入了地图点云关联模型，其根据相邻两帧预处理后的声呐图像即目标轮廓单线图像与航行器的航行数据结合，计算出目标点世界坐标系中的位置信息作为依据，建立相邻两帧目标点的联系。可以理解为由任意相邻两帧声呐图像数据得到的三维点云数据进行迭代最近邻点的方法，从而优化航行器位姿数据，最终建立地图点云关联模型如公式10所示：

M＝∑min[α,min^j{(f(Y_t,Z_t)ⁱ-f(Y_t-1,Z_t-1)^j)^T*C^-1*(f(Y_t,Z_t)ⁱ-f(Y_t-1,Z_t-1)^j)}](11)其中：

M为建立的模型。

C为高斯协方差矩阵，在本方法中可表示为5*5的对角阵。

C^-1为信息矩阵。

Z_t表示在t时刻下的声呐图像数据在本地坐标系中的坐标值。

j表示在t-1时刻下的声呐图像数据映射到三维空间中的第j个点云。

内层min表示在t-1时刻下的空间映射函数前三维数据与t时刻下的空间映射函数前三位数据中最近的点。

f(Y_t,Z_t)ⁱ为空间映射函数，其结果为五维向量，其具体表达式为：

f(Y_t,Z_t)ⁱ＝[R(pitch,roll,yaw,cog)*Z_t,cog,depth] (12)

其中：R为旋转矩阵函数；i表示在t时刻下三维空间中的第i个点云，即函数结果的前三维向量；α为最大距离系数，其表达式为：

步骤140、根据所述行器位姿优化模型和所述点云关联模型构建位姿优化代价函数。

结合地图点云关联模型与航行器位姿优化模型，得到在某一时刻t时的位姿优化代价函数。利用捆集调整的思想，将时间域与优化函数进行结合得到最终的代价函数，可其具体表达式如公式(14)所示：

F为建立的模型。

步骤150、通过最小二乘法对所述位姿优化代价函数进行求解，得到优化量。通过对上述代价函数进行最优化最小二乘迭代求解得到最终的优化量Y。

步骤160、根据所述优化量和所述声呐图像数据及其他航行数据综合计算得到优化后的水下环境数据。首先利用航行数据中的航行速度数据v与预处理过的时间数据T¹以及通过上述优化方法优化的航行角cog数据与航行深度数据depth计算出航行器的航行三维轨迹。运算方法如下：

其中Xtra_t、Xtra_t、Ztra_t表示航行器在第t次采集数据时以出发点为原点所记录的所处的三维空间位置。v_t表示在第t次采集数据时的速度。T¹ _t表示第t次采集数据的时间间隔。cog_t表示在第t次采集数据并通过上述方法优化的航行角。

随后通过优化好航行器位姿角度建立旋转矩阵R，如下所示：

上述R_t为在第t次采集数据时的旋转矩阵，pitch_t、yaw_t、yaw_t、cog_t表示在第t次采集数据并通过上述方法优化的量。

然后通过采集声呐图像数据所使用的BlueView设备自带的SDK得出图像数据中每列坐标点与声呐发生器的距离，并得声呐发生器为原点建立三维坐标，得到每列反射点所占的三维空间位置，即得到了局部三维坐标P_local。并利用如下公式得出全局三维坐标P_world：

P_world＝R*P_local (16)

最后结合全局三维坐标P_world与上述求出的航行器位置坐标Xtra_t、Xtra_t、Ztra_t，最终还原出水下真实场景。

本实施例公开了一种从多波束声呐数据中建立地图的鲁棒优化系统，包括：

存储器，用于存储程序；

处理器，用于加载所述程序以执行所述的从多波束声呐数据中建立地图的鲁棒优化方法。

对于上述方法实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

以上是对本发明的较佳实施进行了具体说明，但本发明并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims

1.一种从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，包括以下步骤：

根据水下航行器的航行数据建立水下航行器位姿优化模型；

根据所述声呐图像数据，构建地图点云关联模型；

根据所述行器位姿优化模型和所述点云关联模型构建位姿优化代价函数；

2.根据权利要求1所述的从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，所述航行数据包括时间戳数据、声呐朝向角数据、航行朝向角数据、深度数据和航行速度数据。

3.根据权利要求2所述的从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，所述水下航行器位姿优化模型N为：

N＝(Y_t-X_t)^TE*A^-1(Y_t-X_t)+(ΔY_t-ΔX_t)^TB^-1(ΔY_t-ΔX_t)；

4.根据权利要求3所述的从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，其中，X_t和Y_t均为五维向量，X_t和Y_t中的参数均包括俯仰角数据pitch、偏航角数据roll、翻滚角数据yaw、航行朝向角数据cog和深度数据depth。

5.根据权利要求3所述的从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，所述地图点云关联模型M为：

M＝∑min[α，min^j{(f(Y_t，Z_t)ⁱ-f(Y_t-1，Z_t-1)^j)^T*C^-1*(f(Y_t，Z_t)ⁱ-f(Y_t-1，Z_t-1)^j)}]；

其中，C为高斯协方差矩阵，C^-1为信息矩阵；Z_t表示在t时刻下的声呐图像数据在本地坐标系中的坐标值；j表示在t-1时刻下的声呐图像数据映射到三维空间中的第j个点云；内层min表示在t-1时刻下的空间映射函数前三维数据与t时刻下的空间映射函数前三位数据中最近的点；f(Y_t，Z_t)ⁱ为空间映射函数，其结果为五维向量。

6.根据权利要求5所述的从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，所述f(Y_t，Z_t)ⁱ＝[R(pitch，roll，yaw，cog)*Z_t，cog，depth]；

7.根据权利要求5所述的从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，所述位姿优化代价函数F为：

8.根据权利要求1所述的从多波束声呐数据中建立地图的鲁棒优化方法，其特征在于，所述声呐图像数据和所述航行数据从水下航行器中获取。

9.一种从多波束声呐数据中建立地图的鲁棒优化系统，其特征在于，包括：

10.一种从多波束声呐数据中建立地图的鲁棒优化系统，其特征在于，包括：

存储器，用于存储程序；

处理器，用于加载所述程序以执行如权利要求1-8任一项所述的方法。