CN108344426B - 一种水面/水下航行器与定位设备之间的航向角偏差估计方法 - Google Patents

Abstract

本发明公开一种水面/水下航行器与定位设备间的航向角偏差估计方法，首先航行器在水面或水下进行定向直线航行，同时通过自身定位和定位设备定位，获得大地东北向坐标下航行器的离散定位点数据；然后采用数据拟合将航行器自身运动轨迹点和定位设备测试数据进行直线拟合，两条直线间的角度差即为航行器和定位设备的航向角偏差值；对同航向进行多次测试，并将多次数据求平均，即可获得该方向下的航向角偏差；然后控制航行器朝别的不同方向进行直线航行，重复上述步骤，获得其它方向下的航向角的偏差，若多个方向下的航向角偏差的相对误差小于5％，则再次求平均。该方法操作便捷，定位设备获取的定位数据更加精准，适用于各种不同的航行器航行环境。

Description

一种水面/水下航行器与定位设备之间的航向角偏差估计 方法

技术领域

本发明涉及水面/水下水下航行器导航仪器校准领域，具体涉及一种水面/水下航行器与定位设备之间的航向角偏差估计方法。

背景技术

水下航行器(以下简称航行器)是有缆或无缆连接的水面/水下无人航行平台(比如AUV、ROV、滑翔机或者混合型水下航行器等)，在装载合适的声纳、水文、化学等传感器后可应用于水上/水下环境监测、近海石油工程作业、水下搜索与测绘等领域。它具有控制灵活、航行面积广阔、价格低廉等特点，是近年来海洋工程领域研究的热点之一。水下导航技术是实现航行器自主航行的关键，但是不同于传统导航系统，水下导航技术具有海洋环境复杂、可获取的外界信息少、系统功耗/体积制约等特点，具有极大的挑战性。航行器搭载的常用的水下导航传感器有多普勒计程仪(Doppler Velocity Log，DVL)、电子罗盘、惯性导航系统、深度传感器、基线定位系统(如长基线LBL、短基线SBL、超短基线USBL等)等。

在幽深的海底，要想知道潜水器所在位置并不容易，因为无线电波在水中的快速衰减使全球定位系统(Global Positioning System，GPS)的定位手段无计可施，而航行器在航行过程中自身导航精度有限，需采用对端定位设备对航行器进行辅助定位以提高精度。声波是目前最有效的水下远距离传播的信息载体，水下对端定位设备一般采用基线定位系统，利用声波进行定位，分为长基线、短基线和超短基线(USBL)三类。超短基线的接收传感器就类似于人类的耳朵，两耳之间的距离就相当于基线。长基线系统的信标之间相距几公里到几十公里，基线越长，精度越高。但潜水器进行水下精确作业时，需要在海底布阵，复杂且受限。故超短基线USBL定位系统是比较常用的手段，应用在水下航行器中时，常使用USBL作为对端定位设备，它由发射换能器、应答器、接收基阵组成。发射换能器和接收基阵安装在母船或固定框架上，应答器固定在航行器上。发射换能器发出一个声脉冲，应答器收到后，回发声脉冲，接收基阵收到后，测量出X、Y两个方向的相位差，并根据声波的到达时间计算出水下装置到基阵的距离，从而计算得到航行器在USBL设备体坐标上的位置，USBL设备采用自身姿态传感器可进一步得到航行器在大地坐标下的位置数据。

在航行器的导航定位中，航行器可靠自身携带的传感器获得的测量信息推算自身的位置，但是仅仅依靠推算精度往往打不到要求，并且误差在不断的积累扩大。在航行器推算位置的基础上，融入对端定位设备的定位信息就能比较快速有效地解决这一问题。另外，在实际应用中经常需要控制航行器回到定位设备附近，比如进行回收或回坞，而在这一过程中对端定位设备对航行器的定位显得尤为重要，并且在近距离更看重相对位置关系，如何获得两者更加准确的相对关系是这一技术的关键。但是对端定位设备(后面将以USBL为例)测得的位置信息是相对于自身定义的仪器体坐标系下的位置，在使用USBL测量的位置进行导航时，必须要经过坐标转换，将其转换到和航行器相同的大地坐标系(即东北天)下，这一过程通常需利用各自自身的姿态传感器测得的其载体姿态信息。若USBL坐标系与航行器坐标系不存在偏角，则坐标转换过程不会导致测量位置的偏差。然而实际应用中，航行器和USBL设备的姿态传感器(特别是航向角)本身传感器之间测量有固定偏差，各自受到周边不同环境影响也会产生偏差，导致两者航向角不一致。另外，USBL本身定位数据产生在体坐标下，其姿态/航向传感器和声线模块之间的安装方向偏差将会导致坐标转换过程中的偏差。这几方面引起的航向角偏差会直接影响系统整体的导航精度，且随着航行时间的增加会产生累计误差，因此有必要对两者之间的航向角的偏差进行估计并校正，以提高航行器的导航精度。

发明内容

本发明的目的在于针对现有技术的不足，提供一种水面/水下航行器与定位设备之间的航向角偏差估计方法，该估计方法操作便捷，使定位设备获取的定位数据更加精准。具体技术方案如下：

一种水面/水下航行器与定位设备之间的航向角偏差估计方法，其特征在于，该方法包括如下步骤：

(1)数据采集：保持航行器的航向不变，使航行器先在水面或水下进行定向直线航行，航行器记录本次航行过程中的大地东北向坐标下的位置信息，同时用定位设备对航行器每隔设定的时间段进行一次定位，获得大地东北向坐标下相应的航行器本次航行过程中的离散定位点数据；

(2)数据处理：采用数据拟合方法将所述的定位设备记录的本次航行离散定位点数据进行拟合，拟合出一条直线；同时将航行器自身记录的本次航行的运动轨迹点采用同样的数据拟合方法拟合得到另一条直线；这两条直线之间的角度差即为航行器和定位设备之间的航向角偏差值；

(3)控制航行器朝相同的方向进行水面或水下定向直线航行，多次重复步骤(1)(2)，将获得的多个该方向下的航向角偏差值进行平均，作为该航向上航行器与定位设备之间的航向角偏差；

(4)根据应用场景的需要，控制航行器朝别的不同方向进行水面或水下定向直线航行，并重复步骤(1)(2)(3)，获得对应方向上航行器与定位设备之间的航向角偏差；如果多个方向下的航向角偏差之间的相对误差不超过5％，则对多个方向下的航向角偏差进行再次平均，作为航行器与定位设备的航向角偏差值；如果多个方向下的航向角偏差之间的相对误差大于5％，则不再做平均，仅选取对应方向上的航向角偏差作为航行器与定位设备的航向角偏差值。

进一步地，所述的航行器记录的航行位置信息通过卫星定位系统或自载的惯性导航设备做航位推算获取或融合卫星定位和航位推算获取。

进一步地，所述的数据拟合方法为最小二乘法。

进一步地，所述的定位设备为对端定位设备。

进一步地，所述的航向角通过姿态传感器获取。

与现有技术相比，本发明的有益效果：

本发明的航向角偏差估计方法操作便捷，使定位设备获取的定位数据更加精准，并且适用于各种不同的航行器航行环境。

附图说明

图1为航行器导航坐标系和对端定位设备坐标系的航向偏差示意图；

图2为本发明的偏差估计方法流程图；

图3为航行器运动轨迹与USBL定位点拟合示意图；

图4为修正后的USBL定位示意图。

具体实施方式

本实施例以水下航行器和超短基线定位系统(USBL)为例，采用最小二乘法进行曲线拟合，进行航向角偏差估计。

首先对本发明采用的坐标系进行说明，如图1所示，图中OXnYn代表航行器大地东北向坐标系，OXpYp为对端定位设备大地东北向坐标系，可以看出两者由于自身各自的航向测量传感器或受各自工作的周边环境影响导致两者航向角存在偏差，故在本应重合的大地东北向坐标系内有一个偏差角度。

如图2所示，介绍了本发明的航向角偏差的估计方法的流程，具体如下：

一、数据采集

图3中黑色实线为航行器航行的轨迹，该轨迹实际上是由很多离散的点组成，由航行器通过卫星定位系统或自载的惯性导航设备做航位推算获取或融合卫星定位和航位推算获取。，五角星型轨迹点为对端定位设备(本例中为USBL)对航行器位置的定位点，可以看出两者的趋势线呈明显分叉状，这是由于对端定位设备系统与航行器大地东北向坐标系存在固定的航向角偏差角导致的。如果不加以补偿偏角，直接将对端定位设备定位的数据应用的航行器导航里，则会产生很大的导航误差，故需要对定位设备的定位点进行直线拟合，然后算出与航行器运动轨迹的角度差值，即为两者之间航向的偏差角。

二、数据处理

采用曲线拟合方法将定位设备的离散定位点数据进行拟合；本例中对USBL定位点以及航行器航行轨迹均采用最小二乘法进行拟合，具体如下：

航行器位置点集合{(x_i，y_i)}(i＝0,1,2,...,m)，求y＝p(X)，使误差的平方和最小，即∑[p(X)-y_i]²最小，所求得的函数y＝p(X)即为航行器位置曲线拟合的结果，获得一条直线；

用同样的方法对定位设备获得的离散定位点进行处理，拟合出另一条直线；

这两条直线之间的角度差即为航行器和定位设备之间的航向角偏差值；

如图3所示，以虚线和点划线分别做出两条直线，其中黑色实线所代表的航行器轨迹实际上是非常密集的点集，拟合为点划线；黑色五角星型点为USBL定位点集，虚线代表的是它们的拟合直线。

图中，用usbl_deg表示USBL定位点拟合直线与北向(图中y轴，顺时针为正)的夹角，nav_deg表示航行器航行轨迹所拟合直线与北向的夹角，θ_mismatch表示的是两者的差值，即本发明所提到的偏差，

θ_mismatch＝nav_deg-usbl_deg

将它运用到USBL的实际定位点中，即可以提高导航的精度。本举例中计算得航行器航行轨迹拟合直线的倾斜角nav_deg＝-39.4943，USBL拟合直线的倾斜角(X轴逆时针方向为正)usbl_deg＝-23.9467°，θ_mismatch＝-15.5476

为了进一步提高获得的航向偏差角的精度，可以控制航行器朝相同的航向进行水面或水下定向直线航行，多次重复上述三个步骤，获得多组航向偏差角，然后对获得的这些结果取均值或滤波，最终得到更为可信、精度更高的该航向上的航向偏差角。另外，由于航行器朝不同的航向航行时，受环境的影响会发生变化，航向角的偏差值也可能随之发生改变，故根据实际应用的需要，可控制航行器朝不同的方向进行水面或水下定向直线航行，并重复上述步骤，所获得的结果即为对应航向上航行器与定位设备之间的航向角偏差；如果多个方向下的航向角偏差之间的相对误差不超过5％，则对多个方向下的航向角偏差进行再次平均，作为航行器与定位设备的航向角偏差值；如果多个方向下的航向角偏差之间的相对误差大于5％，则不再做平均，仅选取对应方向上的航向角偏差作为航行器与定位设备的航向角偏差值，将进一步提高航向偏差估计精度。

将本发明的航向角估计方法应用到实际的航行器的定位中，可以采用下面的公式对原始定位数据进行修正：

x_usbl＝p*cos(θ’+θ_mismatch)

y_usbl＝p*sin(θ’+θ_mismatch)

其中

是由USBL对航向器定位的原始值，x_usbl、y_usbl是利用估计的航向角偏差值进行修正后更新后的定位值，将更新后的定位数据运用到航行器的导航中从而提高精度。在本例中，利用获得的航向偏差角对USBL定位信息进行修正后的结果如图4所示。如图例所示，*型点为利用获得的航向偏差角修正后的USBL定位点，可以看到，修正后的定位信息才能够使用到航行器的导航中。

Claims (5)

1.一种水面/水下航行器与定位设备之间的航向角偏差估计方法，其特征在于，该方法包括如下步骤：

(1)数据采集：保持航行器的航向不变，使航行器先在水面或水下进行定向直线航行，航行器记录本次航行过程中的大地东北向坐标下的位置信息，同时用定位设备对航行器每隔设定的时间段进行一次定位，获得大地东北向坐标下相应的航行器本次航行过程中的离散定位点数据；

(2)数据处理：采用数据拟合方法将所述的定位设备记录的本次航行离散定位点数据进行拟合，拟合出一条直线；同时将航行器自身记录的本次航行的运动轨迹点采用同样的数据拟合方法拟合得到另一条直线；这两条直线之间的角度差即为航行器和定位设备之间的航向角偏差值；

(3)控制航行器朝相同的方向进行水面或水下定向直线航行，多次重复步骤(1)(2)，将获得的多个该方向下的航向角偏差值进行平均，作为该航向上航行器与定位设备之间的航向角偏差；

(4)根据应用场景的需要，控制航行器朝别的不同方向进行水面或水下定向直线航行，并重复步骤(1)(2)(3)，获得对应方向上航行器与定位设备之间的航向角偏差；如果多个方向下的航向角偏差之间的相对误差不超过5％，则对多个方向下的航向角偏差进行再次平均，作为航行器与定位设备的航向角偏差值；如果多个方向下的航向角偏差之间的相对误差大于5％，则不再做平均，仅选取对应方向上的航向角偏差作为航行器与定位设备的航向角偏差值。

2.根据权利要求1所述的水面/水下航行器与定位设备之间的航向角偏差估计方法，所述的航行器记录的航行位置信息通过卫星定位系统或自载的惯性导航设备做航位推算获取或融合卫星定位和航位推算获取。

3.根据权利要求1所述的水面/水下航行器与定位设备之间的航向角偏差估计方法，所述的数据拟合方法为最小二乘法。

4.根据权利要求1所述的水面/水下航行器与定位设备之间的航向角偏差估计方法，所述的定位设备为对端定位设备。

5.根据权利要求1所述的水面/水下航行器与定位设备之间的航向角偏差估计方法，所述的航向角通过姿态传感器获取。

Images