单水声信标测距辅助的水下同步定位与构图方法与流程

本发明涉及同步定位与构图技术，特别涉及一种单水声信标测距辅助基于扩展卡尔曼滤波的水下同步定位与构图方法。

背景技术：

在当前的水下导航定位方法中，可供水下航行器使用的导航方式主要有非自主导航和自主导航两大类。非自主导航方式，如罗兰、欧米伽、gps等，仅在接收机能够接收到导航信号时才能完成导航。罗兰、欧米伽相对于gps导航精度要低。但是这些基于无线电的导航方式，由于电磁波在水中很快衰减的原因，在水下航行器上的使用受到很大限制。相对于电磁信号来说，声信号可以在水下传播距离较远，因此声换能器可以作为信标来引导载体的航行。目前采用的声学导航系统主要有长基线导航、短基线导航和超短基线导航三种形式。这三种形式都需事先在航行海域布放换能器或换能器阵，由于声学导航需要位置已知的信标，因此这种方式更加适用于科学研究及其他民用领域。地球物理导航方法目前主要有基于地磁的导航、基于重力场的导航和基于地形、地貌的导航三种方法，但进行这种导航时载体需要将测量到的数据与先验的测绘图或数据库进行匹配，不仅生成这些先验测绘图的成本与难度很大，而且随着空间维数的增加，寻找匹配峰值的计算复杂度呈指数级增长。正是由于这些导航方法的不足，才促使人们探索研究无需先验图的水下导航算法，这也正是同步定位与构图(simultaneouslocalizationandmapping,slam)算法力求解决的问题。

机器人在完全未知的环境中依靠自身携带的传感器来感知周围信息并且同时实现自身的定位和环境地图的创建是其真正实现自主导航的一个关键问题。slam技术的最初目标是使移动机器人在未知的位置和没有环境预知信息的条件下，利用自身携带的传感器来构建地图，同时利用这个地图来估计自身的姿态(位置和方向)。

近年来，众多关于解决室内、室外，甚至空中和水下的同步定位与构图技术应运而生，然而在水下应用中，传感器感知能力减弱，系统噪声增大且噪声统计特性无法预测，最重要的是水下可识别的特征十分贫乏，这使得水下slam问题成为最具挑战性的问题之一。

技术实现要素：

本发明的目的是克服现有技术中的不足，提供一种单水声信标测距辅助的水下同步定位与构图方法，利用单水声信标测距信息对水下同步定位与构图方法进行约束，减少系统噪声的影响，提高定位精度。

本发明所采用的技术方案是：一种单水声信标测距辅助的水下同步定位与构图方法，包括以下步骤：

步骤1，确定航行器运动的状态方程，依据状态方程得到预测方程；

步骤2，在实际航行过程中，航行器对环境特征进行观测，得到观测量，同时对提前布置好的单水声信标进行测距观测，得到增广的观测量；

步骤3，依据步骤2得到的增广的观测量对新观测到的路标进行数据关联，不断对特征地图和状态量进行更新，从而得到实时的定位导航结果。

进一步的，步骤1具体包括以下步骤：

步骤1-1，对侧扫声呐图像进行处理，进行特征提取，步骤1-1中提取的特征为点特征；

步骤1-2，利用ekf-slam系统的模型进行预测，得到预测状态量和预测状态协方差：

记水下航行器在各个时刻的状态为x1,x2,...,xk，其中k是离散的时间下标；在slam系统中，系统的状态为水下航行器的位置和方向，用式(1)来描述状态估计问题：

其中，f(·)表示运动方程，u表示控制输入，w表示运动噪声，z表示观测数据，g(·)表示观测方程，n表示观测噪声；

依据初始信息，得到状态的预测结果，即：

式(2)表示系统的状态向量预测结果，即由k-1时刻的状态量预测得到k时刻的状态量，状态量由水下航行器的位置和方向构成，其中x_x(·)表示水下航行器在二维平面内的横坐标，x_y(·)表示水下航行器在二维平面内的纵坐标，φ(·)表示航向；

得到状态协方差的预测结果，即：

式(3)中p表示状态协方差，表示状态转换函数的雅克比矩阵，qk表示噪声方差。

进一步的，步骤2具体包括以下步骤：

步骤2-1，在运动过程中每间隔相同时间对特征点进行一次观测，得到观测结果：

记每一个特征点的坐标为则航行器在每一运动状态下与特征点之间的距离为：

记dx(i)＝lm_x(i)-x_x(i)，dy(i)＝lm_y(i)-x_y(i)，则观测到的航向为：

此时得到观测矩阵如下：

步骤2-2，进行单水声信标测距，得到测距结果，并将测距结果加入到观测方程当中：

每一次观测的同时，航行器与单水声信标进行一次测距工作，得到航行器与单信标之间的距离和方向角，并将航行器与单信标之间的距离和方向角加入观测方程当中进行约束；

记水下固定单信标的平面位置坐标为(a,b)，对水下固定单信标进行单信标测距得到：

记dx1(i)＝a-x_x(i)，dy1(i)＝b-x_y(i)，则得到航向为：

此时得到对单信标观测矩阵如下：

同时，将对单信标观测矩阵(9)加入到原观测矩阵(6)当中，得到最终的观测矩阵：

z＝(zr)(10)。

进一步的，步骤3具体包括以下步骤：

步骤3-1，对新观测到的路标进行数据关联：利用最邻近数据关联算法进行数据关联，即通过观测矩阵中的距离值与设置的最大关联距离进行判断，看是否与新观测到的路标进行关联，并将新观测到的路标加入到观测地图当中；

步骤3-2，通过步骤3-1数据关联的结果进行状态更新，对系统的状态量与状态协方差进行更新，从而得到新的运动状态：

计算实际观测与预测观测之间的差值：

式(11)中，z(·)表示实际观测量，表示预测观测量；

更新之后的状态向量为：

x(k|k)＝x(k|k-1)+k*ε(k)(12)

式(12)中，k表示卡尔曼增益，即观测值与预测值的一个权重。

本发明的有益效果是：一种单水声信标测距辅助的水下同步定位与构图方法，本发明针对于深海环境中的小型水下移动航行器，提供一种导航效果好、定位准确、且不需要先验测绘图的基于单信标测距辅助的水下同步定位与构图技术，尤其在水下可识别特征物十分贫乏的情况之下可以修正定位结果。相比于传统基于ekf相比于测距辅助的同步定位与构图方法，本方法在定位精度上有一定提高，减弱了环境噪声对slam系统的影响，使其在没有先验测绘图的情况之下实现较高精度的定位导航。而且，基于单信标测距辅助的水下定位算法，只需一个固定的水下单信标，将连续观测时间内水下信标和航行器之间的测距结果和方向加入观测方程，对观测方程进行扩维处理，以相对准确的单信标测距信息作为约束条件，就可对航行器进行位置定位，在航行器的远距离航行过程中具有明显的优势，更加适用于水下观测特征数目较少的情况。更值得一提的是，基于单信标测距辅助的同步定位与构图方法不需要先验测绘图即可进行定位，是在传统定位方法之上的一个创新。

附图说明

图1为slam工作过程示意图；

图2为单水声信标测距辅助的水下同步定位与构图方法流程图；

图3为基于ekf的同步定位与构图方法仿真图；

图4为单水声信标测距辅助的水下同步定位与构图方法仿真图。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹例举以下实施例，并配合附图详细说明如下：

本发明针对深海环境中的水下移动航行器在远距离航行时的定位，主要目的在于解决当前水下定位技术中，航海的先验地图不易获取或先验地图不准确，以及水下环境噪声很大、可识别特征十分贫乏的不足，以及传统水声定位系统基阵布置复杂的不足。搭载本方法所研究导航定位系统的航行器，通过内外部传感器信息以及与水下固定潜标进行连续的距离量测，将扩展卡尔曼滤波(extendkalmanfilter,ekf)应用到slam系统当中，实现水下航行器的实时定位。

一种单水声信标测距辅助的水下同步定位与构图方法：在深海的一个二维平面内，已知运动模型的水下航行器从一个未知的初始点出发，在一个有若干环境特征的环境中移动，航行器通过传感器的数据确定环境的二维坐标，同时确定自身的三维坐标。(1)确定航行器运动的状态方程，依据状态方程得到预测方程；(2)在实际航行过程中，航行器对环境特征进行观测，得到观测量，同时对提前布置好的单水声信标进行测距观测，得到增广的观测量；(3)依据上述增广的观测量对新观测到的路标进行数据关联，不断对特征地图和状态量进行更新，从而得到实时的定位导航结果。

图1、2分别为slam的工作过程示意图以及单水声信标测距辅助的水下同步定位与构图方法流程图，下面结合图1和图2对本发明方法的过程以及原理进行进一步描述。

(1)对侧扫声呐图像进行处理，进行特征提取，在此提取的特征为点特征。

(2)利用ekf-slam系统的模型进行预测，得到预测状态量和预测状态协方差。

记水下航行器在各个时刻的状态为x1,x2,...,xk，其中k是离散的时间下标。在slam系统中，我们通常要估计水下航行器的位置，那么系统的状态就指的是水下航行器的位姿(位置和方向)。用两个方程来描述状态估计问题：

其中，f(·)表示运动方程，u表示控制输入，w表示运动噪声，z表示观测数据，g(·)表示观测方程，n表示观测噪声。

依据初始信息，我们可以得到状态的预测结果，即：

式(2)表示系统的状态向量预测结果，即由k-1时刻的状态量预测得到k时刻的状态量，其状态量由水下航行器的位置和方向构成，其中x_x(·)表示水下航行器在二维平面内的横坐标，x_y(·)表示水下航行器在二维平面内的纵坐标，φ(·)表示航向。

同样，在预测阶段我们还可以得到状态协方差的预测结果，即：

式(3)中p表示状态协方差，表示状态转换函数的雅克比矩阵，qk表示噪声方差。

(3)在运动过程中每间隔相同时间对特征点进行一次观测，得到观测结果。

记每一个特征点的坐标为则航行器在每一运动状态下与特征点之间的距离为：

记dx(i)＝lm_x(i)-x_x(i)，dy(i)＝lm_y(i)-x_y(i)，则观测到的航向为：

故此时可以得到观测矩阵如下：

(4)进行单水声信标测距，得到测距结果，并将测距结果加入到观测方程当中。

每一次观测的同时，航行器与单水声信标进行一次测距工作，得到航行器与单信标之间的距离和方向角，并将航行器与单信标之间的距离和方向角加入观测方程当中进行约束。

在此记水下固定单信标的平面位置坐标为(a,b)，对水下固定单信标进行单信标测距可以得到：

记dx1(i)＝a-x_x(i)，dy1(i)＝b-x_y(i)，则可以得到航向为：

故此时可以得到对单信标观测矩阵如下：

同时，将对单信标的观测矩阵加入到原观测矩阵当中，得到该方法的观测矩阵：

z＝(zr)(10)

(5)对新观测到的路标进行数据关联。在此利用最邻近数据关联算法进行数据关联，即通过观测矩阵中的距离值与设置的最大关联距离进行判断，看是否与新观测到的路标进行关联，并将新观测到的路标加入到观测地图当中。

(6)通过上述数据关联的结果进行状态更新，对系统的状态量与状态协方差进行更新，从而得到新的运动状态。

在此过程中我们需要计算新息，即实际观测与预测观测之间的差值：

式(11)中，z(·)表示实际观测量，表示预测观测量。

则更新之后的状态向量为：

x(k|k)＝x(k|k-1)+k*ε(k)(12)

式(12)中，k表示卡尔曼增益，即观测值与预测值的一个权重。

如图1所示，x0表示初始时刻水下航行器的实际位置，x1表示水下航行器在下一时刻的实际运动位置，而x1',x1”则分表表示水下航行器依据本发明的方法在下一时刻估计得到的位置，z0,z1分别表示水下航行器在初始时刻及下一时刻的实际运动过程中可以观测到的特征物，而m则表示在此过程中水下可是别的特征物，在同步定位与构图过程中，我们用u表示其更新估计的过程，即通过预测——观测之后得到的新的结果，在图1中用虚线表示。

如图3、4所示，“*”表示实际特征点的位置，“+”表示估计的特征点位置，椭圆表示误差椭圆，误差越大，椭圆的面积越大，“——”表示水下航行器的实际位置，表示水下航行器上的估计位置，而图4中“△”表示水下单信标的位置。通过3、4可以看出，传统的基于ekf的同步定位与构图方法受环境噪声的干扰较大，并且当观测点较为稀疏时，水下航行器就按照推位算法进行运动，定位精度较差。而采用该发明的方法可以在一定程度上减弱环境噪声的影响，同时针对水下观测特征点较少的实际情况时，依据单信标测距信息可以进行位置校正，一定程度上提高了定位精度。所以本发明可以很好地应用于水下航行器的高精度定位工作。

本发明主要应用于航行器在没有先验地图的深海中作业时的定位与导航，当水下可识别特征物十分贫乏时，依据同步定位与构图方法进行实时定位与导航，同时利用单水声信标测距结果进行约束，以提高同步定位与构图方法的定位精度。

尽管上面结合附图对本发明的优选实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，并不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可以做出很多形式，这些均属于本发明的保护范围之内。