CN111307160B - 一种自主水下航行器长航时航迹的修正方法及装置 - Google Patents

Abstract

本发明提供了一种自主水下航行器(AUV)长航时航迹的修正方法及装置，该方法包括：获取AUV导航系统存储数据；利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及利用搜索算法，对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹。本发明通过分析航迹数据特性，充分利用多种辅助信息，在AUV自身导航系统的精度限制下，能够得到更高精度地修正航迹，提高记录航迹数据的经济价值和实用性。

Description

一种自主水下航行器长航时航迹的修正方法及装置

技术领域

本发明属于无人系统导航技术领域，具体而言，涉及一种自主水下航行器长航时航迹的修正方法。

背景技术

长航时自主水下航行器(Autonomous Underwater Vehicle，AUV)导航系统表现最突出的特征为：随着水下航行时间延长，如果其导航系统不进行校正，定位误差特性将会迅速变差。因此，长航时AUV记录航迹数据不可避免地存在误差，这给基于记录航迹数据的相关应用带来偏差，严重时可能导致数据完全失效。

因此，对长航时AUV记录航迹数据进行离线推演，研究航迹特征提取、数据可用性提升等方法，实际需求迫切。然而，目前AUV航迹修正相关的研究文献公开发表较少。

发明内容

本发明旨在提供一种自主水下航行器长航时航迹的修正方法，以通过分析航迹数据特性，充分利用多种辅助信息，得到更高精度的自主水下航行器长航时修正航迹。

根据本发明的一个方面，一种AUV航迹修正方法，该方法包括：

获取AUV导航系统存储数据；

利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及

利用搜索算法，对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹。

在该方面中，还包括对所述存储数据进行预处理，形成可供离线修正处理的航迹数据，利用该航迹数据构建包括定位误差域的AUV记录航迹。

进一步地，所述预处理包括剔除野值点，以及利用时间戳或空间信息将控制点与记录航迹数据进行匹配。

再进一步地，所述控制点包括与时间无关的空间控制点和与时间有关的控制点。

在该方面中，所述误差域为以记录航迹或推算航迹为对称轴的经度、纬度定位误差的均方误差的若干倍。

在该方面中，利用搜索算法，对由第一航迹集合和第二航迹集合的并集或交集构成的区域进行搜索，获得AUV真实航迹。

在该方面中，所述搜索算法为深度优先搜索算法、广度优先搜索算法、A*算法、蚁群算法、蒙特卡洛树搜索算法中的一种或几种。

在该方面中，搜索算法的信息素更新在确定可行网格点的过程中进行，每选中一个可行网格点，就调用信息素局部更新规则对可行网格点进行信息素更新。

根据本发明的另一个方面，还提供了一种AUV航迹修正装置，该装置包括：

数据获取模块，用于获取AUV导航系统存储数据；

第一航迹构建模块，用于利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

第二航迹构建模块，用于构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及

搜索模块，用于利用搜索算法对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹。

根据本发明的另一个方面，还提供了一种AUV航迹修正装置，该装置包括：处理器；以及

存储单元，存储由所述处理器执行以实现如下操作的指令：

获取AUV导航系统存储数据；

利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及

利用搜索算法，对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹。

本发明通过分析航迹数据特性，充分利用多种辅助信息，在AUV自身导航系统的精度限制下，能够得到更高精度地修正航迹。

附图说明

图1为复杂机动条件下AUV航迹修正示意图；

图2为根据本发明具体实施方式的AUV航迹修正方法示意性流程；

图3为本发明更为具体的AUV航迹修正方法工作流程；

图4为记录航迹与航行区域环境示意图；

图5为定位误差影响的误差域示意图；

图6为推算航迹示意图；

图7为集合Λ与集合Ω航迹示意图；

图8为最优搜索得到的AUV真实航迹示意图；

图9、10为根据本发明实施例的仿真过程及航迹搜索结果示意图；

图11为根据本发明的一种实施方式的AUV航迹修正装置结构示意图；

图12为根据本发明的另一种实施方式的AUV航迹修正装置结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行更详细地描述。

本发明在进行自主水下航行器(以下简称AUV)航迹数据离线分析时，考虑了以下因素：

(1)需充分利用典型条件下AUV导航系统的误差特性和其他辅助信息，突显其对AUV导航误差进行校正或对导航输出数据进行推演修正的作用。

(2)长时间水下航行时，AUV根据水声定位信标等可用外部信息提供校正参数，提高导航精度保持时间和抑制在线导航误差。

(3)利用水下固定的水声定位信标或定位浮标信息等作为控制点，可结合该条件下AUV航迹的平滑误差特性，提升相关数据离线修正的有效性。

图1示出了AUV记录航迹、修正航迹以及真实航迹之间的关系。

如图1所示，AUV机动航行条件下，

代表灯塔、路标、岛屿等地理信息，构成了AUV水下航行的不可通行区域或障碍物；

表示AUV航行过程中水声定位信标、定位浮标、上浮卫星定位以及其他手段获得准确定位的位置点；

表示根据AUV导航系统的记录航迹数据，按时间戳(或时间标记)提取的与准确定位信息对应的同时刻AUV水下实际定位结果。

整个航程中，AUV保持水下航行(粗实线航迹)，仅在航行起点、结束点可以获得准确定位信息

。由于没有水下位置校正手段，记录航迹(虚线航迹)仅与真实航迹的轮廓相似(为便于观察，图中尽量放大了二者的差别)。同时，由于AUV自身导航系统的精度限制，更极可能出现记录航迹严重偏离真实航迹的情况，这将对AUV相关应用带来较大影响，甚至导致任务或作业失败。

图2示出了根据本发明实施例的AUV航迹修正方法的示意性流程，图3示出了更为具体的AUV航迹修正方法工作流程。

参照图2，并结合图3，该修正方法包括如下步骤：

步骤1、获取AUV导航系统存储数据，并对存储数据进行预处理，形成可供离线修正处理的航迹数据；

获取AUV导航系统中存储的航迹数据，并对航迹数据进行整理，剔除野值点，并利用时间戳或空间信息将控制点与记录航迹数据进行匹配，形成可供离线修正处理的航迹数据。

控制点资源包括定位浮标、水声定位信标、卫星定位、海图等。AUV航行过程中，受海洋环境条件(如岛礁、深度、人工设施等)限制，海图等数据可以为航迹修正提供丰富资源；水下固定部署的水声定位信标或择机释放的定位浮标，可以为航迹修正提供充足的控制点资源；利用上浮窗口，AUV可以获得卫星定位绝对位置信息，也可以为航迹数据的控制点选择提供可选资源。

通过水声定位信标、定位浮标、上浮卫星定位，以及灯塔、陆标、岛屿等地理信息获得的准确定位信息构成控制点集合Z为：

其中，

表示与时间无关的空间控制点，如岛屿、陆标、水声定位信标、定位浮标等；

表示与时间有关的控制点，如上浮卫星定位。

本发明中，将记录航迹表示为以下模型：

μ_e＝μ_r+Δp₀ (2)

其中，μ_e为记录航迹，Δp₀为记录航迹中定位误差，μ_r为真实航迹。

由此，航迹修正的主要目的可归纳为：根据记录航迹μ_e和控制点集合Z构建航迹修正误差模型，抑制记录航迹中的定位误差Δp₀，实现航迹修正精度满足一定指标的要求。

如图4所示，按所有网格尺度大小一致的原则，将整个航行区域进行等网格划分、，实线曲线为AUV存储设备得到的记录航迹，记录航迹为带有时间戳的路径点的连线，该实线在海图上不可能与障碍物(或对航行安全产生影响的物标)相交。

初始时刻，AUV导航系统可获得准确的经度、纬度位置坐标(图4中的起点A)。从A点开始，AUV进入具有准确起点的导航工作模式。由于已经得到起点A、结束点B的绝对位置控制点，因此，该两点不存在误差。但由于Δp₀的存在，航迹中其他点可能存在误差，甚至可能是无效的。因此，要想修正记录航迹，需要对Δp₀进行离线校正补偿。

根据所构建的记录航迹模型，真实航迹是固定的，而航迹中的定位误差是未知的。若能够根据已有的离线数据，构建两条以上的航迹集合，这些航迹集合均能够以一定误差域包含真实轨迹，则无论每一航迹集合的误差域特征如何，任意两条航迹集合的交集必然也将以一定的误差域包含真实航迹。因此，若两条航迹集合构建恰当，则所获得的航迹集合相对于真实航迹误差将会更小，进而提高航迹恢复(修正)精度。

实际上，记录航迹即为一条满足以上条件的航迹集合，因此，为了能够采用上述航迹集合的航迹恢复(修正)方法，还需要至少再构建另一条航迹集合。

步骤2、构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

图4中的记录航迹如式(2)所示。由于定位误差最终表现为AUV位置，即经度、纬度的定位误差，因此本发明采用误差域确定Δp₀。令σ为随机位置的均方误差，则：

其中，f(x)表示关于x的函数表达式，

δL分别为经度、纬度定位误差。

图5示出了该项误差的误差域表示。如图5所示，各曲线以记录航迹对称表示，由内向外依次代表的是1σ(1倍σ)误差、2σ误差、3σ误差。

AUV的真实航迹点必然位于以记录航迹为对称轴的误差域内。由内向外，真实航迹点在1σ误差域内的概率约为68％，在2σ误差域内的概率约为95％，在3σ误差域内的概率为99.7％，接近100％。

从而，可以获得一个包含AUV真实航迹的第一航迹集合，用Λ表示。

步骤3、构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；

AUV携带多种导航设备，能够记录自身的航向信息H_航向以及对水移动速度(例如采用相对计程仪等)，上述信息配合该海域的海流、深度及海图中与记录深度不相符的区域、岛屿、障碍物等，即可以得到AUV的推算航迹，如图6所示。该推算航迹可以表示为：

推算航迹＝f(H_航向,对水移动速度,流向,流速,深度)\不可航行区域(4)

其中，“x\y”表示在x中排除y，在这里表示为从利用各种信息推算得到的AUV航迹中排除AUV不可航行区域包含的航迹点。

由于AUV的航向、对水移动速度以及流向流速等均存在定位误差影响，并且与岛屿、深度以及与深度计记录深度不相符的区域等的比对信息并不是全程都可得到，故图6中的虚线航迹为包含定位误差的推算航迹，因此同样用均方误差1σ′、2σ′、3σ′表示，进而同样可以获得一个包含AUV真实航迹的第二航迹集合，用Ω表示，如图7所示。

步骤4、利用搜索算法，对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹。

由于基于记录航迹得到的第一航迹集合Λ以及基于推算航迹得到的第二航迹集合Ω分别包含AUV真实航迹，因此，AUV真实航迹为两集合相交的集合，即：

μ_r∈Λ∩Ω (5)

由图7可见，如果仅对Λ∪Ω区域构造空间网格，则无需对整个航行区域构造网格，从而可有效提高航迹修正效率，真实航迹位于两集相交处区域的概率较高。另外的，μ_r∈Λ∪Ω且

的概率次之。

本发明采用的搜索算法，例如可以是深度优先搜索算法、广度优先搜索算法、A*算法、蚁群算法、蒙特卡洛树搜索算法等。考虑障碍物规避、各区域真实航迹点概率等因素，可对Λ∪Ω区域构造的空间网格开展全局搜索，进而得出最优的AUV真实航迹，如图8中的虚线所示。

以下以蚁群搜索算法为例，对本发明提供的航迹修正方法进一步说明。

(1)参数初始化：

τ₀(i,j)＝C，C表示常数，Δτ₀(i,j)＝0,记录航迹的纬度均方误差为σ_Le，经度均方误差为

蚂蚁总数为K，循环搜索次数为N。

AUV携带多种导航设备，能够记录自身的航向信息H_航向，以及对水移动速度，配合该海域的海流、深度及海图中与记录深度不相符的区域、岛屿、障碍物等，可得到AUV的推算航迹，如图6所示：

推算航迹＝f(H_航向,对水移动速度,流向,流速,深度)\不可航行区域

确定推算航迹的纬度均方误差为σ_LTS，经度均方误差为

(2)构造网格

将记录航迹的在3σ误差域与推算航迹的在3σ′误差域的并集构造为蚁群搜索的空间网格。网格划分长度为m×n。将空间网格坐标转换为地理坐标WGS

记录航迹

推算位置

最或然位置

则：

(3)每次循环搜索中，每只蚂蚁按以下规则选择下一个网格点：

a)AUV操纵约束

AUV作为水下航行体，受其自身操纵性能的约束，仅考虑其转向性能，L表示AUV所处的纬度，

表示AUV所处的经度；设AUV由

运动至

为方位角，则：

d_Lij＝L_j-L_i

自身操纵约束模型表示为：

b)不可航区域

若空间网格点与障碍物中心距离小于一定距离

则认为是不可航区域。构建模型如下：

其中，D(待选网格点,障碍物)表示待选网格点与障碍物之间距离。

c)综合约束模型

综合约束模型为：H(i,j)＝f₁(i,j)·f₂(i,j)。

d)网格点选择概率

可得网格点的选择概率模型：

根据各网格点选择概率，可确定下一个网格点，更新蚂蚁网格点禁忌表。

e)信息素局部更新

搜索算法的信息素更新可以在确定可行网格点的过程中进行，每选中一个可行网格点

就可以调用信息素局部更新规则对可行网格点进行信息素更新。这里，局部更新模型确定为：

式中，

为更新参数，

τ₀(i,j)为可行网格点信息素的初始值且为一较小的正常数。通过

引入了负反馈机制，每当一只蚂蚁由一个网格移动到另一个网格时，该航迹上的信息素都按照以上公式被相应地消除一部分，从而实现一种信息素的局部调整，以减小已选择过的航迹再次被选择的概率。

f)寻找最近空间网格点集合

该实施例仅将网格点与记录航迹和推算航迹的关系作为评价的主要原则，而网格点如何选择则决定于记录航迹和推算航迹的精度。在所有蚂蚁均构建完一条航迹后，令第k只蚂蚁搜索得到的航迹表为

选择与最或然位置最近的一条航迹即为最大概率航迹，真均方根值形式的评估可表示为：

则有评价函数：

F^*＝min(E^k(t))

g)信息素全局更新

在所有蚂蚁均构建完一条航迹后，即可进行搜索算法的信息素全局更新。该实施例采用多指标评价函数进行航迹优劣评价。全局更新模型为：

τ(i,j)＝(1-ρ)·τ(i,j)+ρ·Δτ(i,j)

式中，ρ为挥发因子，0<ρ<1；ρ·Δτ(i,j)为航迹多指标评价函数的最优值。Δτ(i,j)＝1/S且初始值Δτ₀＝0，S为本次迭代全局最优航迹的长度，从而增强属于全局最优航迹上的信息素。

(4)检查终止条件：是否达到循环搜索次数N。

(5)根据最近空间网格点集合，输出搜索得到的最优航迹。

该实施例的仿真参数设置为：坐标轴单位为nmile(海里)，起始点(10，10)，航向分别为30°、60°、75°，每段航行3h，航速10kn，流向150°，流速1kn；记录航迹偏差的1倍σ为1nmile。

考虑到搜索算法效率，仅对图9中主要航线区域进行网格划分。网格划分长度0.5nmile×0.5nmile。图中圆形区域为模拟障碍物，坐标分别为(15，29)、(51，57)、(90，56)，

即对AUV的威胁半径设为2nmile。根据网格点选择概率，采用轮盘赌法选择确定下一个网格点。

图10示出了该实施例的航迹搜索结果。图10中，最上侧粗实线为AUV记录航迹，最下侧粗实线为推算航迹，两航迹上下两侧的细实线为各自误差域，从上往下第三条粗实线为真实航迹，从上往下第二条粗实线为经搜索后的修正航迹。

由此可见，在充分利用各种约束信息条件下，仿真结果显示本发明所提出的复杂航迹搜索方法是有效的，能够在一定程度上抑制记录航迹数据的相关应用偏差，提高数据的可用性。

根据本发明另外的实施方式，还提供了一种用于执行上述方法的AUV航迹修正装置。如图11所示，该装置包括存储单元10，用于存储计算机指令；处理器20，与所述存储单元10通信耦合，用于执行所述计算机指令。当处理器20执行所述计算机指令时，执行前述AUV航迹修正方法。

根据本发明另外的实施方式，还提供了一种AUV航迹修正装置。如图12所示，该装置包括：

数据获取模块100，用于获取AUV导航系统存储数据；

第一航迹构建模块200，用于利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

第二航迹构建模块300，用于构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及

搜索模块400，用于利用搜索算法对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹。

总而言之，本发明通过分析航迹数据特性，充分利用多种辅助信息研究基于记录航迹修正(或称为航迹恢复)方法，在AUV自身导航系统的精度限制下，能够得到更高精度地修正航迹，提高记录航迹数据的经济价值和实用性。

上述实例仅用以说明本发明的技术方案而非对其进行限制，尽管参照最佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。

Claims (9)

1.一种AUV航迹修正方法，其特征在于，该方法包括：

获取AUV导航系统存储数据；

利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及

利用搜索算法，对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹；

所述误差域为以记录航迹或推算航迹为对称轴的经度、纬度定位误差的均方误差的若干倍；

所述记录航迹表示为：

μ _e =μ _r +∆p₀

其中，μ _e 为记录航迹，∆p₀为记录航迹中定位误差，μ _r 为真实航迹；

所述推算航迹表示为：

推算航迹=f(H_航向,对水移动速度,流向,流速,深度)\不可航行区域

其中，\表示从利用各种信息推算得到的AUV航迹中排除AUV不可航行区域包含的航迹点。

2.根据权利要求1所述的AUV航迹修正方法，其特征在于，还包括对所述存储数据进行预处理，形成可供离线修正处理的航迹数据，利用该航迹数据构建包括定位误差域的AUV记录航迹。

3.根据权利要求2所述的AUV航迹修正方法，其特征在于，所述预处理包括剔除野值点，以及利用时间戳或空间信息将控制点与记录航迹数据进行匹配。

4.根据权利要求3所述的AUV航迹修正方法，其特征在于，所述控制点包括与时间无关的空间控制点和与时间有关的控制点。

5.根据权利要求1所述的AUV航迹修正方法，其特征在于，利用搜索算法，对由第一航迹集合和第二航迹集合的并集或交集构成的区域进行搜索，获得AUV真实航迹。

6.根据权利要求1所述的AUV航迹修正方法，其特征在于，所述搜索算法为深度优先搜索算法、广度优先搜索算法、A*算法、蚁群算法、蒙特卡洛树搜索算法中的一种或几种。

7.根据权利要求1所述的AUV航迹修正方法，其特征在于，搜索算法的信息素更新在确定可行网格点的过程中进行，每选中一个可行网格点，就调用信息素局部更新规则对可行网格点进行信息素更新。

8.一种AUV航迹修正装置，其特征在于，该装置包括：

数据获取模块，用于获取AUV导航系统存储数据；

第一航迹构建模块，用于利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

第二航迹构建模块，用于构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及

搜索模块，用于利用搜索算法对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹；

所述误差域为以记录航迹或推算航迹为对称轴的经度、纬度定位误差的均方误差的若干倍；

所述记录航迹表示为：

μ _e =μ _r +∆p₀

其中，μ _e 为记录航迹，∆p₀为记录航迹中定位误差，μ _r 为真实航迹；

所述推算航迹表示为：

推算航迹=f(H_航向,对水移动速度,流向,流速,深度)\不可航行区域

其中，\表示从利用各种信息推算得到的AUV航迹中排除AUV不可航行区域包含的航迹点。

9.一种AUV航迹修正装置，其特征在于，该装置包括：

处理器；以及

存储单元，存储由所述处理器执行以实现如下操作的指令：

获取AUV导航系统存储数据；

利用所述存储数据构建包括定位误差域的AUV记录航迹，获得包含AUV真实航迹的第一航迹集合；

构建包括定位误差域的AUV推算航迹，获得包含AUV真实航迹的第二航迹集合；以及

利用搜索算法，对由第一航迹集合和第二航迹集合构成的区域进行搜索，获得AUV真实航迹；

所述误差域为以记录航迹或推算航迹为对称轴的经度、纬度定位误差的均方误差的若干倍；

所述记录航迹表示为：

μ _e =μ _r +∆p₀

其中，μ _e 为记录航迹，∆p₀为记录航迹中定位误差，μ _r 为真实航迹；

所述推算航迹表示为：

推算航迹=f(H_航向,对水移动速度,流向,流速,深度)\不可航行区域

其中，\表示从利用各种信息推算得到的AUV航迹中排除AUV不可航行区域包含的航迹点。

Images