[发明专利]基于实测数据回放的水下无人机导航算法调试系统

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

步骤b、系统时间更新

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

步骤c、系统输入判断

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

步骤d、时标化输入数据

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

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

步骤f、系统输出判断

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

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

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

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

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