CN108761467A - 一种基于前视声纳的三维水下地图构建方法 - Google Patents

Abstract

本发明涉及一种基于前视声纳的三维水下地图构建方法，包含以下步骤：构建自治水下机器人坐标系统，包含惯性坐标系和载体坐标系；将自治水下机器人所在的球体坐标映射到惯性坐标系中；将惯性坐标系栅格化；构建前视声纳模型，根据模型确定当前时刻的信度函数；利用D‑S规则将当前时刻的信度函数和存储设备中前时刻的信度函数进行融合；将融合后的信度进行判别，确定栅格地图每一个栅格的模式，是障碍物还是自由空间。本发明在不增加声纳传感器数量的前提下，针对前视声纳成像的特点，结合D‑S信息融合算法，连续不断地融合声纳传感器在前、后两个时刻所获取的传感数据，实现高准确度的三维水下地图构建。

Description

一种基于前视声纳的三维水下地图构建方法

技术领域

本发明涉及声纳成像技术领域，具体涉及一种基于前视声纳的三维水下地图构建方法。

背景技术

海洋约占地球表面积的71％，是一个蕴藏丰富资源而未能得到有效开发的广袤区域。随着对海洋的深入研究，越来越迫切的需要对海洋环境构建地图，便于进一步的开发。现在用于水下作业的传感器主要有：微光TV、激光测距仪、红外传感器、声纳传感器等。微光TV拍的图像由于受海水能见度低的影响，导致能辨别的距离较短，在实际探测方面受到了限制。激光探测仪虽然比微光TV的辨别距离远一些，但是受目前科学技术水平的限制，实际应用不多。再者，红外传感器虽然具有很强的方向性，并且探测角度小，但是不提供距离信息。而声纳传感器探测角度过大，方向性差，获取的距离信息有限，直接使用单个声纳传感器的数据构建水下地图的误差较大。

发明内容

有鉴于现有技术的上述缺陷，本发明提供一种基于前视声纳的三维水下地图构建方法，通过自治水下机器人搭载前视声纳完成三维水下地图构建。

本发明在不增加声纳传感器数量的前提下，针对前视声纳成像的特点，结合D-S信息融合算法，连续不断地融合声纳传感器在前、后两个时刻所获取的传感数据，实现高准确度的三维水下地图构建。

本发明提供一种前视声纳模型。动态构建三维水下地图时，本发明的前视声纳模型能准确地将声纳采集的环境信息转换成栅格地图中每个栅格的信度分配值，得到了每个栅格的信度分配值后就可以根据指定的栅格状态判别规则判断出每个栅格的状态，从而达到动态构建地图的目的。对于一个三维环境中使用的声纳，因为它的对称性，本发明中只需在一个二维面上讨论该声纳的探测模型。本发明的模型根据声纳的信度函数不同被分为两个区，第一个区的信度函数分配值为：

第二个区的信度函数为：

本发明提供的一种基于前视声纳的三维水下地图构建方法，至少包含以下步骤：

步骤1、构建自治水下机器人坐标系统；

步骤2、将自治水下机器人所在的球体坐标映射到惯性坐标系中；

步骤3、将惯性坐标系栅格化；

步骤4、构建前视声纳模型，根据模型确定当前时刻的信度函数；

步骤5、利用D-S规则融合将当前时刻的信度函数和存储设备中前时刻的信度函数进行融合；

步骤6、将融合后的信度进行判别，确定栅格地图每一个栅格的模式，是障碍物还是自由空间。

所述步骤1中，将坐标系分为惯性坐标系和载体坐标系。水下机器人的空间位置在惯性坐标系中的3个分量x、y、z以及载体坐标系(水下机器人)对于惯性坐标系的3个姿态角θ、ψ，用矢量表示，载体坐标系的速度矢量V＝[u,v,w,p,q,r]^T，则惯性坐标系和载体坐标系的速度矢量之间的关系为：

从载体坐标系到惯性坐标系的坐标转换矩阵为：

式中：

所述步骤2中的声纳传感器获取的障碍物球体坐标数据与惯性坐标数据之间的相互转化关系如下：

其中：(x'_e,y'_e,z'_e)为该障碍物在惯性坐标系中的坐标；(x_r,y_r,z_r)为水下机器人在惯性坐标系的坐标；r为该被测障碍物与AUV(自治水下机器人)之间的距离；是该障碍物在AUV球体坐标系中的矢量角。

所述步骤3中惯性坐标系格式化的转换关系如下：

其中：(x_e,y_e,z_e)为该障碍物在惯性栅格坐标系中的坐标；w为单元栅格宽度，在本发明中设置为1；为取整运算。

所述步骤4中信度函数的确定为：

其中：m_O(i,j,k)是栅格占有障碍物的信度函数分配值；而m_E(i,j,k)为栅格非障碍物的信度函数分配值；该栅格不确定状态模式的信度函数分配值是m_{O、E}(i,j,k)。

所述步骤5中D-S信息融合规则为：

其中：和分别表示单元栅格Y(i,j,k)在t时刻占有障碍物与非障碍物的信度函数分配值。

所述步骤6中的判别规则为：

其中：Q₀表示判定状态模式的阈值，Q₁表示判定的状态模式和其它状态模式的信度函数分配值之差的门限值，Q₂表示不确定状态模式的信度函数分配值的阈值，它们都为常数。

本发明还提供一种自治水下机器人搭载前视声纳构建三维水下地图的方法，由自治水下机器人搭载前视声纳在待构建地图的区域全覆盖航行，使用上述基于前视声纳的三维水下地图构建方法来构建三维水下地图；

其中，在判断未完成全覆盖时，对声纳传感器采集的数据进行以下的循环处理：前视声纳根据声纳模型构建信度函数，将第一时刻构建的信度函数存入存储器中；继续构建第二时刻的信度函数，并利用D-S规则将第一时刻和第二时刻的信度函数进行融合，形成新的信度函数，来更新存储器中的第一时刻的信度函数；对新的信度函数进行判别，判别声纳采集的地图信息的模式，并判断声纳探测到的区域是障碍物还是自由空间。

本发明与现有技术相比具有以下优点：1、声纳传感器在水中能辨别的距离远，分辨率较高，同时处理信息的过程简单、实时性强和价格低廉。2、采用D-S信息融合算法。考虑到水下机器人搭载能力有限，使用D-S信息融合算法能够在不增加声纳传感器的基础上，通过对声纳传感器不同时刻采集的信息进行融合，提高了构建地图的准确度。

附图说明

图1为本发明中水下机器人坐标系示意图。

图2为本发明中前视声纳模型。

图3为本发明中D-S信息融合技术原理图。

图4为本发明利用自治水下机器人构建三维水下地图原理图。

具体实施方式

以下结合附图，通过详细说明一个较佳的具体施例，对本发明做进一步阐述。

水下机器人在水下主要承受重力、浮力、推力、水动力、干扰力以及与这些力有关的各种力矩的作用。在这些力和力矩形成的合力和合力矩的作用下水下机器人产生6个自由度的空间运动。为了确定水下机器人的位置，坐标系分为惯性坐标系和载体坐标系两种。

如图1所示，惯性坐标系的原点E可以取海面或海中的任何一点。规定坐标轴EZ的正向指向地心，另外两个方向可以任意选取，如机器人的位置和方向可描述为其中x、y、z分别为水下机器人在惯性坐标系中的位置；θ、ψ分别为水下机器人对惯性坐标系的横倾角、纵倾角和艏向角。

载体坐标系是与水下机器人载体固定在一起的。一般来说，其原点与水下机器人的重心重合，OX₀与水下机器人的主对称轴一致，指向艏部为正；OY₀平行于基线面，指向右舷为正；OZ₀位于水下机器人主体中纵剖面内，指向底部为正；机器人的线速度，角速度及力、力矩在载体坐标系中描述为：速度矢量V＝[u,v,w,p,q,r]^T，力和力矩矢量τ＝[X,Y,Z,K,M,N]^T，其中：u、v、w分别为水下机器人线速度矢量在载体坐标系中的3个分量；p、q、r分别为水下机器人角速度矢量在载体坐标系中的3个分量；X、Y、Z分别为水下机器人的轴向、倾向、垂向水动力；K、M、N分别为水下机器人横摇、纵倾、转艏水动力矩。

动态构建三维水下地图时，前视声纳传感器模型能准确地把将声纳采集的环境信息转换成栅格地图中每个栅格的信度分配值，得到了每个栅格的信度分配值后就可以根据指定的栅格状态判别规则判断出每个栅格的状态，从而达到动态构建地图的目的。对于一个三维环境中使用的声纳，因为它的对称性，本发明中只在一个二维面上讨论该声纳的探测模型。如图2所示，设声波辐射半径量程R，辐射误差d，辐射角度α。图中：β是声纳的探测角，可根据前面坐标转换得到的对应障碍物的惯性坐标求得，如某一障碍物的惯性坐标是B＝(x'_e-x_r,y'_e-yr,z'_e-z_r)，同时在声纳传感器的中心轴线上的某一点是A＝(x'_e-x_r,0,0)，于是可求得：

对于区间Ⅰ，有R-d≤r≤R+d:

对于区间Ⅱ，有R_min≤r≤R-d:

其中：m_O(i,j,k)是栅格占有障碍物的信度函数分配值；而m_E(i,j,k)为栅格非障碍物的信度函数分配值；该栅格不确定状态模式的信度函数分配值是m_{O、E}(i,j,k)。

如图3所示，一种基于前视声纳的三维水下地图构建方法，至少包含以下步骤：

步骤1、构建自治水下机器人坐标系统；

步骤2、将自治水下机器人所在的球体坐标映射到惯性坐标系中；

步骤3、将惯性坐标系栅格化；

步骤4、构建前视声纳模型，根据模型确定当前时刻的信度函数；

步骤5、利用D-S规则将当前时刻的信度函数和存储设备中前时刻的信度函数进行融合；

步骤6、将融合后的信度进行判别，确定栅格地图每一个栅格的模式，是障碍物还是自由空间。

所述步骤1中，坐标系分为惯性坐标系和载体坐标系。

水下机器人的空间位置在惯性坐标系中的3个分量x、y、z以及载体坐标系(水下机器人)对于惯性坐标系的3个姿态角θ、ψ，用矢量表示，载体坐标系的速度矢量V＝[u,v,w,p,q,r]^T，则惯性坐标系和载体坐标系的速度矢量之间的关系为：

从载体坐标系到惯性坐标系的坐标转换矩阵为：

式中：

所述步骤2中的声纳传感器获取的障碍物球体坐标数据与惯性坐标数据之间的相互转化关系如下：

其中：(x'_e,y'_e,z'_e)为该障碍物在惯性坐标系中的坐标；(x_r,y_r,z_r)为水下机器人在惯性坐标系的坐标；r为该被测障碍物与AUV之间的距离；是该障碍物在AUV球体坐标系中的矢量角。

所述步骤3中惯性坐标系格式化的转换关系如下：

其中：(x_e,y_e,z_e)为该障碍物在惯性栅格坐标系中的坐标；w为单元栅格宽度，在本发明中设置为1；为取整运算。

所述步骤4中信度函数的确定为：

其中：m_O(i,j,k)是栅格占有障碍物的信度函数分配值；而m_E(i,j,k)为栅格非障碍物的信度函数分配值；该栅格不确定状态模式的信度函数分配值是m_{O、E}(i,j,k)。

所述步骤5中D-S的D-S信息融合规则为：

其中：和分别表示单元栅格Y(i,j,k)在t时刻占有障碍物与非障碍物的信度函数分配值。

所述步骤6中的判别规则为：

其中：Q₀表示判定状态模式的阈值，Q₁表示判定的状态模式和其它状态模式的信度函数分配值之差的门限值，Q₂表示不确定状态模式的信度函数分配值的阈值，它们都为常数。

如图4所示，自治水下机器人搭载一个前视声纳构建三维水下地图的过程。首先向自治水下机器人下达航行指令，确定待构建地图的区域，完成自治水下机器人的起始位置等初始化设置。得到指令后，自治水下机器人搭载前视声纳开始在待构建地图的区域全覆盖航行。当自治水下机器人航行开始时，前视声纳根据声纳模型构建信度函数，将第一时刻构建的信度函数存入存储器中。然后继续构建第二时刻的信度函数。当第二时刻的信度函数构建完成后，根据D-S信息融合技术将第一时刻和第二时刻的信度函数进行融合，形成新的信度函数。新的信度函数用于更新存储器中的第一时刻的信度函数。同时对新的信度函数进行判别，判别声纳采集的地图信息的模式，及判断声纳探测到的区域是障碍物还是自由空间。接下来判断自治水下机器人是否完成全覆盖，如果没有完成，那么重复上述过程，D-S信息融合方法将当前时刻的信度函数和存储设备中前时刻的信度函数进行融合，继续构建三维水下地图。直至自治水下机器人完成待构建地图区域的全覆盖航行。本发明通过自治水下机器人的全覆盖航行，基于前视声纳采集的信息，根据D-S信息融合方法，完成三维水下地图的构建。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (9)

1.一种基于前视声纳的三维水下地图构建方法，其特征在于，包含以下步骤：

步骤1、构建自治水下机器人坐标系统，包含惯性坐标系和载体坐标系；

步骤2、将自治水下机器人所在的球体坐标映射到惯性坐标系中；

步骤3、将惯性坐标系栅格化；

步骤4、根据构建的前视声纳模型，确定当前时刻的信度函数；

步骤5、利用D-S规则将当前时刻的信度函数和存储设备中前一时刻的信度函数进行融合；

步骤6、将融合后的信度进行判别，确定栅格地图每一个栅格的模式是障碍物还是自由空间。

2.如权利要求1所述基于前视声纳的三维水下地图构建方法，其特征在于，

所述步骤1中，惯性坐标系的原点E取海面或海中的任何一点；坐标轴EZ的正向指向地心，另外两个方向任意选取；机器人的位置和方向描述为其中x、y、z分别为自治水下机器人在惯性坐标系中的位置；θ、ψ分别为自治水下机器人对惯性坐标系的横倾角、纵倾角和艏向角；

载体坐标系的原点O与自治水下机器人的重心重合，OX₀与自治水下机器人的主对称轴一致，指向艏部为正；OY₀平行于基线面，指向右舷为正；OZ₀位于水下机器人主体中纵剖面内，指向底部为正；

机器人的线速度、角速度及力、力矩在载体坐标系中描述为：速度矢量V＝[u,v,w,p,q,r]^T，力和力矩矢量τ＝[X,Y,Z,K,M,N]^T，其中：u、v、w分别为自治水下机器人线速度矢量在载体坐标系中的3个分量；p、q、r分别为水下机器人角速度矢量在载体坐标系中的3个分量；X、Y、Z分别为水下机器人的轴向、倾向、垂向水动力；K、M、N分别为水下机器人横摇、纵倾、转艏水动力矩。

3.如权利要求2所述基于前视声纳的三维水下地图构建方法，其特征在于，

惯性坐标系和载体坐标系的速度矢量之间的关系为：

从载体坐标系到惯性坐标系的坐标转换矩阵为：

式中：

4.如权利要求3所述基于前视声纳的三维水下地图构建方法，其特征在于，

所述步骤2中的声纳传感器获取的障碍物球体坐标数据，与惯性坐标数据之间的相互转化关系如下：

其中：(x'_e,y'_e,z'_e)为该障碍物在惯性坐标系中的坐标；(x_r,y_r,z_r)为自治水下机器人在惯性坐标系的坐标；r为该被测障碍物与自治水下机器人之间的距离；是该障碍物在自治水下机器人球体坐标系中的矢量角。

5.如权利要求4所述基于前视声纳的三维水下地图构建方法，其特征在于，

所述步骤3中对惯性坐标系进行格式化，转换关系如下：

其中：(x_e,y_e,z_e)为该障碍物在惯性栅格坐标系中的坐标；w为单元栅格宽度，为取整运算。

6.如权利要求5所述基于前视声纳的三维水下地图构建方法，其特征在于，

所述步骤4中信度函数为：

其中：m_O(i,j,k)是栅格占有障碍物的信度函数分配值；而m_E(i,j,k)为栅格非障碍物的信度函数分配值；该栅格不确定状态模式的信度函数分配值是m_{O、E}(i,j,k)；前视声纳模型中，设声波辐射半径量程R，辐射误差d，辐射角度α，声纳的探测角β。

7.如权利要求6所述基于前视声纳的三维水下地图构建方法，其特征在于，

所述步骤5中D-S信息融合规则为：

其中：和分别表示单元栅格Y(i,j,k)在t时刻占有障碍物与非障碍物的信度函数分配值。

8.如权利要求7所述基于前视声纳的三维水下地图构建方法，其特征在于，

所述步骤6中的判别规则为：

其中：Q₀表示判定状态模式的阈值，Q₁表示判定的状态模式和其它状态模式的信度函数分配值之差的门限值，Q₂表示不确定状态模式的信度函数分配值的阈值，它们都为常数。

9.一种自治水下机器人搭载前视声纳构建三维水下地图的方法，其特征在于，自治水下机器人搭载前视声纳在待构建地图的区域全覆盖航行，使用权利要求1-8中任意一项所述基于前视声纳的三维水下地图构建方法来构建三维水下地图；

其中，在判断未完成全覆盖时，对声纳传感器采集的数据进行以下的循环处理：前视声纳根据声纳模型构建信度函数，将第一时刻构建的信度函数存入存储器中；继续构建第二时刻的信度函数，并利用D-S规则将第一时刻和第二时刻的信度函数进行融合，形成新的信度函数，来更新存储器中的第一时刻的信度函数；对新的信度函数进行判别，判别声纳采集的地图信息的模式，并判断声纳探测到的区域是障碍物还是自由空间。