CN114492030A

CN114492030A - 基于实测数据回放的水下无人机导航算法调试系统

Info

Publication number: CN114492030A
Application number: CN202210089543.XA
Authority: CN
Inventors: 张舸; 伊国兴; 魏振楠
Original assignee: Harbin Institute of Technology
Current assignee: Harbin Institute of Technology
Priority date: 2022-01-25
Filing date: 2022-01-25
Publication date: 2022-05-13
Anticipated expiration: 2042-01-25
Also published as: CN114492030B

Abstract

基于实测数据回放的水下无人机导航算法调试系统，它属于水下无人机导航算法调试技术领域。本发明解决了传统调试方法的调试效率低的问题。本发明提出的基于实测数据回放的水下无人机导航算法调试系统能够在水下试验过程中记录各传感器数据并时标化存储，并在算法调试过程中能够脱离真实水下环境，采用基于实测数据的回放进行算法仿真和数据分析，提高了调试效率，并降低了人力物力成本，为水下组合导航的算法开发和调试工作提供了崭新的思路和高效的工具。本发明方法可以应用于水下无人机导航算法调试。

Description

基于实测数据回放的水下无人机导航算法调试系统

技术领域

本发明属于水下无人机导航算法调试技术领域，具体涉及一种基于实测数据回放的水下无人机导航算法调试系统。

背景技术

占据地球表面积约71％的海洋中蕴含着丰富的生物、矿产资源，而随着人类社会的飞速发展，对海洋资源的开发与利用已经成为各国的迫切需求。水下地形环境的勘探检测、油气矿产的开采挖掘、管线装备的铺设建造以及货物人员的运输转移都对水下导航技术有着极高要求，水下导航技术对于人类探索海洋至关重要。

目前在水下导航领域，主要包括基于加速度和陀螺仪的惯性导航，基于声学信标阵的声学导航，基于地形和地磁的地球物理导航，同时各类水下传感器例如深度计、多普勒测速仪、声纳、水下摄像机等都为水下导航提供了丰富的数据信息。单一的导航方式往往具有局限性，无法稳定而准确的为水下设备提供导航定位信息，引入多种传感器进行水下组合导航，将各类传感器数据进行综合，保留各导航方式的优点，弥补各自缺点，是水下导航技术的关键点。

水下组合导航由于结合了多种传感器设备，各传感器的装配、噪声以及导航算法参数均需要进行不断调试才能达到最佳效果。在传统调试过程中，需要进行多次水下试验并随船不断调试修改，人力物力消耗巨大且算法调试效率极低。

发明内容

本发明的目的是为解决传统调试方法的调试效率低的问题，而提出的一种基于实测数据回放的水下无人机导航算法调试系统。

本发明为解决上述技术问题所采取的技术方案是：

基于实测数据回放的水下无人机导航算法调试系统，所述系统包括嵌入式运行模块、仿真运行模块和数据分析模块，其中：

所述嵌入式运行模块嵌入在水下无人机的导航计算机中，嵌入式运行模块包括导航解算单元、上位机、传感器和存储板；

所述导航解算单元接收传感器数据和上位机发送的操作指令，并向上位机传输反馈响应信息数据；导航解算单元根据导航算法初始参数对接收的传感器数据和上位机数据进行导航解算，并将导航解算结果输出；

存储板用于对传感器数据、上位机数据、导航解算结果数据和对上位机的反馈响应信息数据进行时标化存储，生成实测数据文件；

所述仿真运行模块嵌入在仿真计算机中，仿真运行模块读取嵌入式运行模块中存储板生成的实测数据文件，并对实测数据文件进行解析，获得解析出来的实测数据，并将解析出来的实测数据以传输给数据分析模块；

仿真运行模块根据解析出来的实测数据和导航算法参数生成仿真数据文件，并将仿真数据文件传输给数据分析模块；

所述数据分析模块根据实测数据文件、嵌入式运行模块的导航解算结果、仿真运行模块解析出来的实测数据和仿真数据文件来分析仿真效果是否达到要求，若达到要求，则结束调试过程，否则不断修正仿真运行模块的导航算法参数后，进行重新仿真，直至仿真效果达到要求时调试过程结束。

本发明的有益效果是：

本发明提出的基于实测数据回放的水下无人机导航算法调试系统能够在水下试验过程中记录各传感器数据并时标化存储，并在算法调试过程中能够脱离真实水下环境，采用基于实测数据的回放进行算法仿真和数据分析，提高了调试效率，并降低了人力物力成本，为水下组合导航的算法开发和调试工作提供了崭新的思路和高效的工具。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，并不构成对本发明的不当限定。在附图中：

图1为本发明的基于实测数据回放的水下无人机导航算法调试系统的结构示意图；

图2为嵌入式运行模块的工作流程图；

图3为嵌入式运行模块的系统结构图；

图4为嵌入式运行模块中实测数据存储流程图；

图5为时标化输入数据的流程图；

图6为仿真运行模块的工作流程图；

图7为调试算法的流程图；

图8为仿真运行模块下数据回放流程图；

图9为数据分析模块的工作流程图；

图10为调试系统的工作流程图；

图11为实测数据文件ASCII显示模式的示意图；

图12为实测数据文件Hex显示模式的示意图；

图13为罗经航向角数据的示意图；

图14为DVL三轴速度数据的示意图；

图15为USBL定位数据的示意图；

图16为嵌入式运行模式轨迹的示意图；

图17为导航算法在第一组参数下运行的结果图；

图18为导航算法在第二组参数下运行的结果图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施方式及实施方式中的特征可以相互组合。

具体实施方式一、结合图1、图2、图3、图6和图7说明本实施方式。本实施方式所述的一种基于实测数据回放的水下无人机导航算法调试系统，所述系统包括嵌入式运行模块、仿真运行模块和数据分析模块，其中：

所述仿真运行模块嵌入在仿真计算机中，仿真运行模块读取嵌入式运行模块中存储板生成的实测数据文件，并对实测数据文件进行解析，获得解析出来的实测数据，并将解析出来的实测数据以txt文件形式传输给数据分析模块；

如图9所示，数据分析模块的输入为仿真运行模式下输出的原始数据文件和仿真数据文件。其中，原始数据文件是对实测数据文件中传感器数据和默认反馈数据的解析，是对嵌入式运行模式的数据复现；仿真数据是依据实测数据文件中传感器数据和指令数据的回放进行模拟仿真后的输出，是算法调试仿真的结果。

在原始数据文件驱动下，数据分析平台主要对嵌入式运行模式下存储的各种信息进行分析并进行图像绘制。包括嵌入式运行时的系统状态、参数等信息，罗经、DVL、USBL和深度计的原始数据和图像，嵌入式运行时航位推算和组合导航输出的位置数据和图像。

在仿真数据文件驱动下，数据分析平台主要对基于实测数据回放仿真输出的各种信息分析并进行图像绘制。包括回放仿真过程中的系统状态、参数等，回放仿真中解析的罗经、DVL、USBL和深度计数据和图像，回放仿真中航位推算和组合导航的位置输出和图像，除此之外还包括回放仿真结果对于组合参数调整的分析反馈。

本发明的调试系统主要由嵌入式运行模块、仿真运行模块和数据分析模块三部分组成。嵌入式运行模式工作于水下无人机的嵌入式导航计算机中，在真实的水下环境中进行导航信息解算和实测数据存储。仿真运行模块工作于仿真计算机中，根据实测数据的回放在模拟环境中进行水下试验复现和导航算法调试仿真。数据分析模块工作于数据分析计算机，为仿真运行模块下输出的数据文件提供可视化分析结果。该系统能够完整记录水下无人机工作环境数据信息并进行时标化存储，生成实测数据文件，进而通过精准复现水下无人机工作环境数据，为水下无人机导航算法的调试提供便捷、一致的实验数据和高效的数据分析手段，是水下无人机工况复现和故障诊断的有利工具，同时极大提高了水下无人机导航算法调试效率。

本发明具备以下3个优点：

(1)能够根据时标准确记录并回放水下无人机的工作环境、系统状态和导航输出，为水下无人机的故障分析提供数据支持；

(2)能够根据实测数据的回放进行调试仿真，在保证外部环境一致的前提下，调试导航算法参数，为水下导航算法优化提供便捷的途径。

(3)能够根据实测数据的复现和调试仿真的输出进行可视化结果分析，直观的显示试验过程数据以及导航算法的调试效果，为水下导航系统提供高效的交互方式。

具体实施方式二：本实施方式与具体实施方式一不同的是，所述传感器数据包括罗经数据、DVL数据、USBL数据深度计数据、GPS数据和INS数据，所述上位机发送的操作指令包括算法复位、模式切换和参数赋值，所述导航算法初始参数包括量测噪声、过程噪声、状态变量和协方差阵初始值。

其中，罗经数据为水下无人机提供量测的姿态角信息，DVL数据为水下无人机提供量测的速度信息，USBL数据为水下无人机提供量测的位置信息，深度计数据为水下无人机提供量测的深度信息。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述导航解算单元通过网络接口以UDP方式接收传感器数据和上位机发送的操作指令。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：结合图4和图5说明本实施方式。本实施方式与具体实施方式一至三之一不同的是，所述存储板进行时标化存储的具体流程为：

步骤a、读取系统参数文件，并对系统参数文件进行解析以获取系统参数；

系统参数文件是位于嵌入式系统内部的txt文件，文件内部以字符串形式保存系统工作频率和输出频率，系统接入的上位机、存储板和各传感器的IP地址及端口，系统导航输出的方式和端口，系统接入的传感器数量、类型、序号和所使用的协议编号，系统参数文件更新日期等。该文件在嵌入式系统运行前根据导航系统实际情况进行修改并更新。嵌入式系统在开始运行时先读取该文件，并解析以上内容获取系统相应参数；

步骤b、系统时间更新

系统时间在导航解算单元上电时被初始化为零，并在系统运行周期对系统时间进行累加；其中系统周期ΔT由读取的系统工作频率计算得到。

步骤c、系统输入判断

判断当前时刻导航解算单元的各输入队列有无新的有效数据，当有新的有效数据时，将系统输出标志位置位，跳转至步骤d将新的有效数据进行时标化后，再接着执行步骤e；当无新的有效数据时，则直接跳转至步骤e；

步骤d、时标化输入数据

时标化输入数据的具体流程如图4所示，根据系统参数，依次对各传感器的数据和上位机操作指令数据进行时标化；

步骤e、根据系统参数、传感器数据和上位机操作指令数据进行导航解算，获得导航解算结果；

步骤f、系统输出判断

根据系统输出频率和系统输出标志位置位判断当前周期是否进行数据输出，当有系统输入或者到达系统默认的输出周期时，则进入步骤g对系统状态进行时标化存储；否则，跳转至步骤h；

步骤g、对系统参数、反馈响应信息数据和导航解算结果数据进行时标化，将时标化后的系统参数、传感器数据、上位机操作指令数据、反馈响应信息数据和导航解算结果数据进行组合，并通过UDP端口输出至存储板，由存储板以追加写的形式生成实测数据文件；

步骤h、系统结束标志判断

判断导航解算单元的导航程序是否结束运行，若导航程序正常运行，则重复步骤b到步骤g的过程；若导航程序结束运行，则终止实测数据存储流程。

其它步骤及参数与具体实施方式一至三之一相同。

本实施方式中生成的实测数据文件是一个后缀为.bin的二进制文件，由多种基本时标化数据单元按照时间顺序组合而成，每个基本时标化数据单元之间以0x0A，即ASCII码表对应的换行键相互分隔。

实测数据文件产生于存储板，存储板在每次上电时根据UDP输入端口号产生一个新的文件。导航解算单元每个周期产生数个基本时标化数据单元并通过UDP传输给存储板，存储板将接收到的数据以二进制形式写入该文件中。

基本时标化数据单元的格式如表1所示。

第一项为UTC日期，占用10字节，表示该条数据产生的日期，按照日、月、年的顺序由斜杠相互分隔。

第二项为UTC时间，占用12字节，表示该条数据产生的具体时间，按照时、分、秒的顺序由分号相互分隔，其中，秒保留小数点后3位，即精确到毫秒。

第三项为系统时间，占用10字节，表示该条数据产生时距离系统开始时刻的秒数。小数点前保留6位，小数点后保留3位，即系统时间分辨率为毫秒，且最大系统时间为999999.999s(≈277小时)。

第四项为数据类型，占用6字节，表示该条数据产生的来源和类型，按照#xxxxx的格式进行区分，区分字符右对齐且左端由下划线补齐，可定义或扩展的包括不限于COMP、DVL、USBL、DEP、GPS、INS等。

第五项为原始数据内容，是传感器或上位机传输的未解析的原始数据的拷贝，占用字节数由原始数据量决定，数据内容一般为协议头、数据包、校验位等不同传感器协议数据。

第六项为结束标志，占用5字节，表示该条时标化数据单元的结束位置，是数据回放流程的重要标志内容。

第一项到第五项每项之间由0x09制表符连接，共占用4字节，则每个时标化数据单元占用47+N字节，其中N为原始数据大小。

表1基本时标化数据单元格式

如表2所示，为实测数据文件的结构。表中每行代表一种基本数据单元，多个数据基本单元的组合构成实测数据文件。基本数据单元由时标、标识符和数据内容组成。时标反应数据产生时刻的UTC时间和嵌入式系统内部时间；标识符反应该数据类型；数据内容为该基本数据单元的有效内容。

表2实测数据文件结构

根据数据来源和内容的不同，基本数据单元主要分为传感器数据、指令数据和反馈数据三类。

传感器数据根据类别主要包括罗经、DVL、USBL和深度计，数据内容为当前时刻各传感器输出的带协议的原始量测信息。根据各传感器输出频率的不同，该类数据在实测数据文件中出现的频率基本固定。例如罗经的输出频率设置为10HZ，则在实测数据文件中，罗经类基本数据单元每秒出现10条。

指令数据是当前时刻嵌入式运行模块接收到上位机输出的带协议的原始指令信息，只有当上位机进行操作时该类基本数据单元才会出现在实测数据文件中，而出现的频率也与操作次数相关。

反馈数据是包括默认反馈和响应反馈两种，默认反馈是当前时刻嵌入式运行模块内部系统状态、导航系统参数、导航模式以及导航系统的输入数据和输出数据等信息，响应反馈是当收到来自上位机的指令信息后，嵌入式运行模块对该指令做出的响应并向上位机传递的反馈。这两种反馈信息以规定的协议进行编码存储于反馈数据基本单元中。默认反馈每隔10个周期出现一次，响应反馈只有在收到指令数据时才出现，在实测数据文件中默认反馈以固定频率出现，响应反馈与指令数据往往同时出现。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述仿真运行模块与嵌入式运行模块具备相同的时钟基准，仿真运行模块根据该时钟基准对实测数据文件进行解析。

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：结合图8说明本实施方式。本实施方式与具体实施方式一至五之一不同的是，所述仿真运行模块的工作流程为：

实测数据回放是运行在计算机环境中，来模拟、再现嵌入式系统在实际工作环境中接收到的传感器数据和上位机信息，进而对导航算法做出相应调试和改进。实测数据回放过程中，模拟计算机系统根据与嵌入式系统相同的系统参数文件，以及由存储板系统生成的实测数据文件，对嵌入式系统工作环境中接收的各种信息进行时序回放。

步骤1、读取系统参数文件

该系统参数文件是嵌入式系统中读取的系统参数文件的拷贝，是位于模拟计算机系统内部的txt文件。与嵌入式系统的系统参数文件相同，内部以字符串形式保存系统工作频率和输出频率，系统接入的上位机、存储板和各传感器的IP地址及端口，系统导航输出的方式和端口，系统接入的传感器数量、类型、序号和所使用的协议编号，系统参数文件更新日期等。该文件的修改版本始终与嵌入式系统中的系统参数文件保持一致。实测数据回放系统在开始运行时先读取该文件，并解析以上内容获取系统相应参数；

步骤2、打开实测数据文件

该文件即为嵌入式系统运行完毕后由存储板系统生成的实测数据文件，在实测数据回放之前，需要保证将该文件拷贝至相应读取目录，并赋予可读权限。在完成系统参数读取后，会在文件读取目录以只读方式打开该实测数据文件；

步骤3、判断实测数据文件是否成功打开，若实测数据文件打开成功则继续执行步骤4，否则实测数据文件打开未成功则结束流程；

步骤4、系统时间更新

系统时间初始化为零，并在成功打开实测数据文件进入仿真运行模块运行周期后进行更新，即在每个运行周期对系统时间进行ΔT累加，其中，运行周期ΔT由读取的系统工作频率计算得到，且与嵌入式运行模型运行时保持一致；

步骤5、在每个运行周期完成系统时间更新后，判断数据缓存单元内是否存在有效数据，若存在有效数据，则执行步骤6，否则，执行步骤7；

数据缓存单元是数据回放过程中系统读取实测数据文件时存放未来数据的容器，其大小由系统参数文件中传感器最大数据量来确定；

步骤6、读取数据缓存单元的有效数据，再将数据缓存单元内的有效数据置为无效，并利用读取的有效数据执行步骤9；

数据缓存单元一般临时存放着前一个系统运行周期从实测数据文件或缓存单元读取的相对当前时刻的未来数据，即，一条时标信息指向当前时刻之后的基本时标化数据单元；

步骤7、按行读取实测数据文件

当数据缓存单元中不存在有效数据时，根据文件指针向后按字符读取数据，直到结束符[END]出现，读取一条基本时标化数据单元，并将文件指针向后移动一位；

步骤8、判断实测数据文件读取是否结束

在读取实测数据文件过程中，当文件指针指向实测数据文件末尾时，则表示实测数据文件全部读取完毕并结束流程，否则，继续执行步骤9；

步骤9、时标判断

判断读取的数据为历史数据、当前系统时间数据或未来数据，若读取的数据为历史数据，则跳转至步骤7直到读取到当前系统时间数据，若读取的数据为当前系统时间数据，则跳转至步骤10直到读取到未来数据，若读取的数据为未来数据，则跳转至步骤11；

在缓存单元或数据文件中读取到一条基本时标化数据单元后，依据基本时标化数据单元存储格式，读取第25到35字节数据并以double型表征从而获取该条数据的系统时间。再通过与当前系统时间进行对比，判断读取的数据为历史数据、当前系统时间数据或未来数据中的哪种；

步骤10、配置系统输入

根据读取数据的存储格式，读取第36字节至第41字节数据并以字符串形式表征数据类型，读取第43字节至结束字符数据作为当前时刻的输出，并将当前时刻的输出以unsignedchar数组形式传递给导航算法；

当读取的基本时标化数据单元为当前数据时，数据回放系统将根据基本时标化数据单元存储格式，读取第36到41字节数据并以字符串形式表征从而获得该条数据的数据类型，然后从第43字节开始读取到结束字符之前则为该数据类型代表的传感器当前时刻的输出，将该数据以unsignedchar数组形式传递给导航系统主体算法相应传感器数据的输入缓存单元，则完成了一项系统输入的配置。

步骤11、数据存入数据缓存单元

将读取出的未来数据拷贝至数据缓存单元，并置位相应有效位，供下一个运行周期进行数据读取；

当读取的基本时标化数据单元为未来数据时，代表着数据回放系统已经完成了当前系统时间的全部数据读取，系统将执行步骤12，在这之前需要将已经读出的未来数据拷贝至缓存单元并置位相应有效位，为下个系统运行周期的数据读取。

步骤12、基于导航算法进行仿真；

当前系统时间的所有输入均配置完毕后，数据回放系统进入导航系统的主题算法部分，进行基于实测数据回放的导航算法仿真；

步骤13、仿真输出

当导航算法执行完毕后，输出解析的实测数据文件信息和导航仿真解算信息，并跳转至步骤4，进行下一个系统运行周期。

其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是，所述数据缓存单元的大小由传感器最大数据量确定。

其它步骤及参数与具体实施方式一至六之一相同。

具体实施方式八：本实施方式与具体实施方式一至七之一不同的是，所述判断读取的数据为历史数据、当前时间数据或未来数据，其具体过程为：

若数据时间小于当前系统时间且两者之差的绝对值大于0.2ΔT，则表示读取的数据为历史数据；

若数据时间与当前系统时间之差的绝对值小于等于0.2ΔT时，则表示读取的数据为当前系统时间数据；

若数据时间大于当前系统时间且两者之差的绝对值大于0.2ΔT，则表示读取的数据为未来数据。

其它步骤及参数与具体实施方式一至七之一相同。

具体实施方式九：本实施方式与具体实施方式一至八之一不同的是，所述数据分析模块用于对实测数据文件和嵌入式运行模块的导航解算结果进行分析和图像绘制，用于对仿真运行模块解析出来的实测数据和仿真数据文件进行分析和图像绘制。

其它步骤及参数与具体实施方式一至八之一相同。

具体实施方式十：本实施方式与具体实施方式一至九之一不同的是，所述导航解算结果包括航位推算和组合导航输出的位置数据。

其它步骤及参数与具体实施方式一至九之一相同。

为了更好的理解基于实测数据回放的导航系统，如图10所示为嵌入式和仿真运行模式工作的流程示意图。两种运行模式均由c语言编写，共享主体导航算法，只在运行平台、数据来源和数据输出上有所不同。嵌入式运行模式适配搭载Linux系统的水下无人设备，在相同硬件环境的虚拟机下将c工程编译为二进制文件并拷贝至该平台，则可以使无人设备运行水下组合导航算法，在真实条件下进行水下导航等任务。仿真运行模式适配Windows10系统计算机，在该平台下将c工程编译为可执行文件，配合实测数据文件，则可以复现水下无人设备在实际任务环境中的工作状况和数据往来。

本发明中，嵌入式运行模块和仿真运行模块的导航算法具体包括以下步骤：

步骤一：基于罗经的姿态角量测信息以及DVL的速度量测信息进行运载体航位推算，所述运载体航位推算包括坐标变换和位置解算；

所述步骤一的具体步骤为；

步骤一一：获取来自罗经的姿态角量测信息、来自DVL的速度量测信息以及来自KF的估计结果标度因数和安装角度误差；

步骤一二：利用来自罗经的姿态角量测信息、来自DVL的速度量测信息以及来自KF的估计结果标度因数和安装角度误差进行航位推算，得到包含DVL误差变量的位置递推信息。

步骤二：以X＝[δk″ θ′_dz δL δλ δh]^T作为状态变量、以LBL的位置量测信息和深度计的深度量测信息为观测值进行滤波；

其中，

δk″为DVL的标度因数误差的变体，θ′_dz为DVL的z轴安装角度误差的变体，δL、δλ、δh分别为运载体的纬度误差、运载体的经度误差和运载体的高度误差，δk为DVL的标度因数误差，θ_dz为DVL的z轴安装角度误差；

所述滤波的具体步骤为；

步骤二一：获取来自LBL的位置量测信息、来自深度计的深度量测信息以及来自运载体航位推算的包含DVL误差变量的位置递推信息；

步骤二二：将来自LBL的位置量测信息、来自深度计的深度量测信息以及来自航位推算的包含DVL误差变量的位置递推信息输入卡尔曼滤波器中，得到DVL标度因数、安装角度误差以及位置误差信息。

所述卡尔曼滤波的具体步骤为：

步骤1：将状态变量X和协方差矩阵P的初值进行设定，状态变量X和协方差矩阵P表示为：

X(0)＝[δk″(0) θ′_dz(0) δL(0) δλ(0) δh(0)]^T＝0_5×1

P(0)＝I_5×5

其中，I_5×5为5维单位矩阵；

步骤2：获取上一时刻的状态变量，上一时刻的状态变量表示为：

X(k-1)＝[δk″(k-1) θ′_dz(k-1) δL(k-1) δλ(k-1) δh(k-1)]^T

同时获取上一时刻的协方差矩阵P(k-1)；

步骤3：获取上一时刻的DVL的安装误差角和标度因数误差，并根据上一时刻的DVL的安装误差角和标度因数误差得到DVL误差矩阵，DVL误差矩阵表示为：

步骤4：由DVL得到当前的量测速度，当前的量测速度表示为：

由当前的量测速度和DVL误差矩阵得到当前的载体坐标系速度，当前的载体坐标系速度表示为：

其中，

分别为k时刻载体坐标系下x轴速度、y轴速度和z轴速度；

步骤5：根据当前的载体坐标系速度以及由机体坐标系到导航坐标系的坐标转换矩阵得到运载体在导航坐标系下速度，运载体在导航坐标系下速度表示为

步骤6：对当前的量测速度V^d(k)进行矩阵化，表示为：

然后结合当前罗经量测结果得到速度误差转移矩阵，速度误差转移矩阵表示为：

步骤7：根据速度误差转移矩阵得到当前系统矩阵F_k/k-1，当前系统矩阵F_k/k-1表示为：

其中，L为纬度；

步骤8：根据当前系统矩阵F_k/k-1进行状态一步预测，得到状态变量的估计值，状态变量的估计值表示为：

步骤9：根据运载体上一时刻所处的经度信息得到当前噪声矩阵G_k-1，当前噪声矩阵G_k-1表示为：

其中，

为导航坐标系速度误差转换矩阵，

N₁矩阵表示为：

矩阵表示为：

步骤10：根据上一时刻的协方差矩阵P(k-1)、当前噪声矩阵G_k-1以及当前系统矩阵F_k/k-1得到当前时刻的协方差矩阵估计值P_k/k-1，当前时刻的协方差矩阵估计值P_k/k-1表示为：

其中Q为DVL量测噪声协方差矩阵，σ_DVLx、σ_DVLy、σ_DVLz分别为DVL速度测量过程中的噪声标准差；

步骤11：获取当前时刻的长基线和深度计的测量值，并根据当前时刻的长基线和深度计的测量值得到当前时刻的位置量测值，当前时刻的位置量测值表示为：

其中，

分别为LBL量测的纬度、LBL量测的经度和深度计量测的深度，

根据当前时刻的位置量测值和航位推算结果得到观测值，观测值表示为：

步骤12：根据当前时刻的协方差矩阵估计值P_k/k-1得到滤波增益K_k，滤波增益K_k表示为：

其中，量测矩阵H_k表示为：

量测噪声方差矩阵R表示为：

其中σ_LBLL、σ_LBLλ和σ_DEP分别为长基线的纬度量测噪声标准差、长基线的经度量测噪声标准差和深度计量测标准差；

步骤13：根据状态变量的估计值、滤波增益K_k和观测值进行状态修正，得到当前时刻的状态变量，当前时刻的状态变量表示为：

步骤14：根据滤波增益K_k、量测矩阵H_k以及当前时刻的协方差矩阵估计值P_k/k-1进行协方差阵修正，得到当前时刻的协方差矩阵，当前时刻的协方差矩阵表示为：

P_k＝(I-K_k·H_k)P_k/k-1。

步骤三：利用滤波结果矫正航位推算，进而得到运载体的精确定位以及DVL误差参数。

所述步骤三的具体步骤为；

步骤三一：利用DVL标度因数、安装角度误差以及位置误差信息对包含DVL误差变量的位置递推信息矫正运载体航位推算，进而得到运载体的精确定位以及DVL误差参数。

所述运载体航位推算具体包括以下步骤：

步骤1：获取运载体上一时刻所处的经度信息、纬度信息以及高度信息；

步骤2：由罗经得到当前的量测姿态角，表示为：

其中，

θ(k)、γ(k)分别为k时刻的航向角、俯仰角和滚转角，

根据当前的量测姿态角得到由机体坐标系到导航坐标系的坐标转换矩阵，由机体坐标系到导航坐标系的坐标转换矩阵表示为：

其中，γ为滚转角、k为k时刻、θ为俯仰角、

为航向角；

由DVL得到当前的量测速度，表示为：

其中，

分别为k时刻DVL量测坐标系下x轴速度、y轴速度和z轴速度，

根据当前的量测速度以及由机体坐标系到导航坐标系的坐标转换矩阵得到运载体在导航坐标系下速度，运载体在导航坐标系下速度表示为：

V^nd(k)＝[v_E(k) v_N(k) v_U(k)]^T＝C(k)·V^d(k)

其中，v_E(k)、v_N(k)、v_U(k)分别为k时刻导航坐标系下的东向速度、北向速度和天向速度；

步骤3：根据运载体在导航坐标系下速度以及经纬高数据的转化矩阵得到当前时刻导航系下位置增量，

所述经纬高数据的转化矩阵表示为：

其中，T_s为Ts采样时间、R_n为子午圈曲率半径、R_m为卯酉圈曲率半径、secL为正割运算、L(k-1)为k-1时刻纬度，

当前时刻导航系下位置增量表示为：

d_Pose(k)＝[d_L(k) d_λ(k) d_h(k)]^T＝M·V^nd(k)

其中，d_L(k)、d_λ(k)、d_h(k)分别为k时刻的纬度增量、经度增量和高度增量；

步骤4：由运载体上一时刻所处的经度、纬度、高度和当前时刻导航系下位置增量得到当前的航位推算结果，当前的航位推算结果表示为：

其中，

分别为k时刻航位推算纬度、经度和高度。

所述步骤三中利用滤波结果矫正航位推算表示为：

其中δ_Pose(k)表示为：

δ_Pose(k)＝[δL(k) δλ(k) δh(k)]^T＝X(k)_3:5

其中，L(k)、λ(k)、h(k)分别为k时刻校正后的航位纬度、经度和高度，δL(k)、δλ(k)、δh(k)分别为k时刻的纬度误差、经度误差和高度误差。

所述DVL误差参数表示为：

实验部分

下面结合某次ROV试验过程，对本发明中实测数据存储和回放方案进行详细描述。

导航系统配置：

系统使用罗经、DVL和USBL三种传感器进行水下组合导航试验，在导航及数据存储系统配置过程中，将嵌入式系统、各传感器、上位机和存储板系统通过网线相互连接。

嵌入式系统的系统参数文件路径为work/navsim/data/nvmdata.txt。定义工作频率为100HZ，输出频率为10HZ。上位机地址192.168.0.50，端口号为8020，传感器地址192.168.0.20，其中，罗经端口号为8011、DVL端口号为8003、USBL端口号为8004，存储板系统地址为192.168.0.17，端口号为8019，导航输出地址为192.168.0.18，端口号为8021。定义系统接入的传感器数量为3，其中，1号为罗经，采用$HEHDT协议传输方位角，$PHTRO协议传输姿态角，$--ZDA协议传输UTC日期和时间；2号为DVL，采用PD6协议传输三轴速度；3号为USBL，采用$GPGGA传输经纬高位置信息。

数据存储：

嵌入式系统上电后自启动运行导航系统，读取系统参数后进入导航与数据存储流程，在运行过程中将每个时刻的传感器输入、上位机输入和系统反馈、指令反馈都通过网络端口传输至192.168.0.17的8019端口，并被存储板系统保存为SENSLOG.bin文件，文件内容如图11所示，该文件由基本时标化数据单元组成，其中，$FDB为自定义的Hex上位机协议，在ACSII模式下显示为乱码，其Hex数据如图12所示。

数据回放：

将嵌入式系统中的nvmdata.txt文件和存储板系统中的SENSLOG.bin文件拷贝至仿真计算机的data\data_sim目录下，然后运行数据回放程序，则可以完整复现水下试验的数据和算法输出，若导航输出效果不满意，则调整导航算法的参数并反复运行数据回放程序，则可便捷的完成导航算法调试工作，截取其中360s试验数据进行该算法调试系统验证。

图13为航向角量测数据，图像表明水下无人机的航向角从开始的206°逐渐增加至220°，并在其附近波动。

图14为速度量测信息，图像表明水下无人机的右向速度和上向速度在0附近波动，前向速度大致在0.2m/s附近波动。

图15为USBL定位量测信息，图像表明水下无人机在试验过程中经度和纬度均匀速减小，即匀速向西南方向行驶。

图16为嵌入式运行模式下组合导航轨迹输出，图像表明USBL虽然具有不发散特性，但其精度由概率圆表征，定位轨迹不够平滑，往往存在剧烈跳变；航位推算虽然动态性能较好，足够平滑，但长时间运行会产生累计误差，使定位精度越来越差；基于卡尔曼滤波的组合导航在参数适当的条件下可以在保证数据平滑的基础上，获得较高精度的定位。

组合导航轨迹与航位推算轨迹相互重合，这是由于嵌入式运行时将USBL的量测噪声设置过大造成的，适当修改参数可以达到较为理想的效果。

为了获得该型ROV及其有效载荷合适的导航参数，需要提取嵌入式运行模式下存储的实测数据文件并基于该数据文件进行导航参数调整并以仿真模式运行。图17和图18是导航算法在两组不同参数下运行的结果。

由于嵌入式运行时USBL量测噪声过大，在第一仿真运行时，如图17所示，第一组参数将USBL的量测噪声调小。发现组合导航轨迹与USBL完全重合，这意味着组合导航对USBL输出依赖较高，即USBL的量测噪声过小。

经过数次计算和调试，如图18所示，第二组参数条件下的组合导航轨迹与USBL有较好重合度，同时轨迹较为平滑，既保留了USBL稳态精度高的优点，又保留了航位推算动态性能好的优点。

实验表明，本发明提出的基于实测数据回放的水下无人机导航算法调试系统能够在规避下水试验的条件下，将该型ROV的导航算法参数调试到最佳状态，极大的提高了算法调试效率。

本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims

1.一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述系统包括嵌入式运行模块、仿真运行模块和数据分析模块，其中：

所述仿真运行模块嵌入在仿真计算机中，仿真运行模块读取嵌入式运行模块中存储板生成的实测数据文件，并对实测数据文件进行解析，获得解析出来的实测数据，并将解析出来的实测数据传输给数据分析模块；

2.根据权利要求1所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述传感器数据包括罗经数据、DVL数据、USBL数据深度计数据、GPS数据和INS数据，所述上位机发送的操作指令包括算法复位、模式切换和参数赋值，所述导航算法初始参数包括量测噪声、过程噪声、状态变量和协方差阵初始值。

3.根据权利要求2所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述导航解算单元通过网络接口以UDP方式接收传感器数据和上位机发送的操作指令。

4.根据权利要求3所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述存储板进行时标化存储的具体流程为：

步骤b、系统时间更新

系统时间在导航解算单元上电时被初始化为零，并在系统运行周期对系统时间进行累加；

步骤c、系统输入判断

步骤d、时标化输入数据

根据系统参数，依次对各传感器的数据和上位机操作指令数据进行时标化；

步骤f、系统输出判断

步骤h、系统结束标志判断

5.根据权利要求4所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述仿真运行模块与嵌入式运行模块具备相同的时钟基准，仿真运行模块根据该时钟基准对实测数据文件进行解析。

6.根据权利要求5所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述仿真运行模块的工作流程为：

步骤1、读取系统参数文件；

步骤2、打开实测数据文件；

步骤4、系统时间更新

系统时间初始化为零，并在成功打开实测数据文件进入仿真运行模块运行周期后进行更新，即在每个运行周期对系统时间进行ΔT累加，其中，运行周期ΔT由读取的系统工作频率计算得到；

步骤7、按行读取实测数据文件

步骤8、判断实测数据文件读取是否结束

步骤9、时标判断

步骤10、配置系统输入

步骤11、数据存入数据缓存单元

步骤12、基于导航算法进行仿真；

步骤13、仿真输出

7.根据权利要求6所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述数据缓存单元的大小由传感器最大数据量确定。

8.根据权利要求7所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述判断读取的数据为历史数据、当前时间数据或未来数据，其具体过程为：

9.根据权利要求8所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述数据分析模块用于对实测数据文件和嵌入式运行模块的导航解算结果进行分析和图像绘制，用于对仿真运行模块解析出来的实测数据和仿真数据文件进行分析和图像绘制。

10.根据权利要求9所述的一种基于实测数据回放的水下无人机导航算法调试系统，其特征在于，所述导航解算结果包括航位推算和组合导航输出的位置数据。