CN111412900B - 一种数字式姿态补偿和波浪测量系统及控制方法 - Google Patents

Abstract

本发明公开一种数字式姿态补偿和波浪测量系统及控制方法，包括控制单元，与所述控制单元连接的6轴mems惯导单元、3轴磁力计单元、时钟单元、存储单元、串口电平转换单元以及电源转换单元。本发明的有益效果为：基于同一套硬件实现了姿态测量、姿态补偿和波浪测量这3种海洋调查中常需的观测功能，提高了海洋仪器的多元化观测能力和性能，实现一机多用。通过在系统中增设相应硬件单元，解决了现有姿态测量、姿态补偿技术步骤繁多容易出错的缺陷，使晃动平台的海洋现场的观测姿态能够实时、准确校正，并且数字式系统解决了波浪观测技术所用的机械常平架安装不便、体积大的问题，有效降低了海洋仪器成本、体积及重量。

Description

一种数字式姿态补偿和波浪测量系统及控制方法

技术领域

本发明涉及海洋观测应用领域，尤其涉及一种数字式姿态补偿和波浪测量系统及控制方法。

背景技术

科考船、无人船、浮标、Glider等海上无人观测平台长期工作在海面上，不可避免的受到海洋动力环境影响，运动轨迹和姿态是多变的，可能发生旋转、上下波动、摇摆等运动。当无人平台姿态剧烈变化时，将影响传感器的海面要素观测，导致数据准确度和可靠性降低。所以，掌握无人平台的运动姿态并能实时进行在线的姿态补偿，获取自然地理坐标系下的真实矢量值，对获取高质量的观测数据具有十分重要的意义。对于在晃动平台上的海洋矢量参数测量，传统做法是观测仪器搭配罗经同步使用，后期数据由专业处理人员分析。如果现场观测人员和后期数据处理人员不能很好的对接，则其中的现场安装参数可能会出现交接错误，从而不能准确的进行姿态校正。

基于惯导器件的波浪观测同样需要解决波浪的姿态测量和姿态补偿问题，传统的波浪传感器解决晃动的方法多是IMU单元安装在一个机械式的常平架中，不管外部环境如何晃动，这个机械常平架可以一直保持在水平和垂直状态。但机械常平架体积大、重量大、安装不方便。

发明内容

针对上述问题，本发明提出一种数字式姿态补偿和波浪测量系统及控制方法，在一套系统内集成姿态测量、姿态补偿和波浪测3种功能，主要解决现有姿态测量、姿态补偿技术步骤繁多容易出错，以及波浪观测技术所用的机械常平架安装不方便的问题。

为解决上述技术问题，本发明的技术方案如下：

提供一种数字式姿态补偿和波浪测量系统，包括控制单元，与控制单元连接的6轴mems惯导单元、3轴磁力计单元、时钟单元、存储单元、串口电平转换单元以及电源转换单元；

控制单元用于处理来自外围单元的数据并发送控制命令控制外围单元；

6轴mems惯导单元用于获取角运动信息以及加速度信息；

3轴磁力计单元用于获取地磁信息；

时钟单元用于产生时钟信号；

存储单元用于储存原始采样数据和波浪统计结果；

串口电平转换单元用于输出串口电平；

电源转换单元用于给控制单元、6轴mems惯导单元、3轴磁力计单元、时钟单元、存储单元以及串口电平转换单元输入电源。

在一些实施方式中，控制单元为STM32F446微处理器，6轴mems惯导单元为ADIS16485芯片，3轴磁力计单元为RM3100芯片，时钟单元为DS1302时钟芯片，存储单元为SD卡，串口电平转换单元为MAX3232芯片。

除此之外，还提供一种控制方法，用于上述的数字式姿态补偿和波浪测量系统，包括以下步骤：

控制单元通过预设的交互指令与上位机进行数据交互，交互指令包括姿态补偿工作模式或波浪工作模式切换命令，时钟设置命令、姿态测量频率设置命令以及波浪采样频率设置命令；

上位机选择姿态补偿工作模式或波浪工作模式；在姿态补偿工作模式下，获取姿态解算结果；在波浪工作模式下，获取波浪特征。

在一些实施方式中，交互命令格式包括头码-1、头码-2、命令标志位、用户设置内容以及校验和，其中，头码-1和头码-2设为固定值，命令标志位处设置5个值，分别对应姿态补偿工作模式、波浪工作模式、姿态测量频率设置模式、波浪采样频率设置模式和时间设置模式。

在一些实施方式中，姿态补偿工作模式包括如下步骤：

运行参数初始化，硬件初始化，进入无限循环主程序，在主程序中按照预置的频率依次进行加速度数据采集、磁力计数据采集、姿态角解算、姿态补偿矩阵计算以及计算结果输出。

在一些实施方式中，姿态解算以及姿态补偿矩阵计算前还包括，判断姿态输出心跳间隔数OutputNum是否等于预设值，判断定时时间是否大于或等于预设值，若均是，则读取6轴mems惯导单元和3轴磁力计单元的数据，否则继续执行判断动作，根据返回的陀螺仪数据和加速度数据，动态选择PID误差控制中的比例环节系数Kp。

在一些实施方式中，比例环节系数Kp选择后还包括，加速度数据归一化，获取重力方向和变迁的评估参数，计算评估参数与加速度数据的误差，对误差进行误差积分，误差积分通过PI控制后补偿到陀螺仪，然后进行四元数微分方程解算，四元数归一化，通过四元数归一化计算姿态角，通过姿态角计算姿态补偿矩阵，通过姿态补偿矩阵计算北西天坐标系的加速度值。

在一些实施方式中，6轴mems惯导单元的陀螺仪在10ms内转速超过5°时选取Kp＝0.001，否则选取Kp＝1。

在一些实施方式中，波浪工作模式包括如下步骤：

步骤一，运行参数初始化，硬件初始化，步骤二，进行惯性数据采样，加速度比力变换，步骤三：对波浪特征进行计算和统计，计算结果输出，波浪特征包括波高、波周期和波向，波浪特征进行计算和统计包括：对垂直方向的加速度进行频域变换，步骤四，通过跨零法计算波高和波周期，通过三角合成计算波向；步骤五，波浪特征计算完成后，输出当前一组的波浪特征计算结果，同步存储波浪特征计算结果。

在一些实施方式中，频域积分变换具体包括，将波浪特征依次进行动态分配内存池、圆频率的离散化、生成FFT数组、进行FFT计算、时域变换为频域、进行2次积分得到频域位移信号、清除频域中的高频/低频截止频率信号、进行IFFT计算，将上述位频域移信号从频域变换为时域。

本发明的有益效果为：

1.基于同一套硬件实现了姿态测量、姿态补偿和波浪测量这3种海洋调查中常需的观测功能，提高了海洋仪器的多元化观测能力和性能，实现一机多用。通过在系统中增设相应硬件单元，解决了现有姿态测量、姿态补偿技术步骤繁多容易出错的缺陷，使晃动平台的海洋现场的观测姿态能够实时、准确校正，并且数字式系统解决了波浪观测技术所用的机械常平架安装不便、体积大的问题，有效降低了海洋仪器成本、体积及重量。

2.对低频波浪加速度信号，频域积分可有效解决时域积分所产生的位移趋势漂移问题。而且频域高低频截止频率的滤波参数设置，相对于传统的时域积分的滤波性能具有更好的自适应性。

附图说明

图1为本发明一种数字式姿态补偿和波浪测量系统的硬件原理图；

图2为本发明控制方法的交互命令集示意图；

图3为本发明控制方法的姿态补偿工作模式的流程图；

图4为本发明控制方法的姿态补偿工作模式的输出格式示意图；

图5为本发明控制方法的波浪工作模式的流程图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚、明确，下面结合附图和具体实施方式对本发明的内容做进一步详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。

实施例一

如图1所示,本实施例提出了一种数字式姿态补偿和波浪测量系统，包括控制单元，与控制单元连接的6轴mems惯导单元、3轴磁力计单元、时钟单元、存储单元、串口电平转换单元以及电源转换单元；

控制单元用于处理来自外围单元的数据并发送控制命令控制外围单元；

6轴mems惯导单元用于获取角运动信息以及加速度信息；

3轴磁力计单元用于获取地磁信息；

时钟单元用于产生时钟信号；

存储单元用于储存原始采样数据和波浪统计结果；

串口电平转换单元用于输出串口电平；

电源转换单元用于给控制单元、6轴mems惯导单元、3轴磁力计单元、时钟单元、存储单元以及串口电平转换单元输入电源。

基于同一套硬件实现了姿态测量、姿态补偿和波浪测量这3种海洋调查中常需的观测功能，提高了海洋仪器的多元化观测能力和性能，实现一机多用。通过在系统中增设相应硬件单元，解决了现有姿态测量、姿态补偿技术步骤繁多容易出错的缺陷，使晃动平台的海洋现场的观测姿态能够实时、准确校正，并且数字式系统解决了波浪观测技术所用的机械常平架安装不便、体积大的问题，有效降低了海洋仪器成本、体积及重量。

通过以上外围单元的协同作用，使到原本需要两台仪器测量的工作集成到一台海洋仪器上。

实施例二

同样基于实施例一所述的数字式姿态补偿和波浪测量系统，图1仅给出主要器件的引脚连接方式，省略各器件自身的电容、电阻、电感、电源等连接配置。控制单元为STM32F446微处理器，6轴mems惯导单元为ADIS16485芯片，3轴磁力计单元为RM3100芯片，时钟单元为DS1302时钟芯片，存储单元为SD卡，串口电平转换单元为MAX3232芯片。更进一步的，本发明采用核心处理器型号为STM32F446RC(U1)，其中战术级6轴惯导单元ADIS16485(U2)通过SPI总线连接到U1的SPI1通道上，具体为ADIS16485的Pin3，Pin4，Pin5，Pin6，Pin9分别对应连接到STM32F446RC的Pin21，Pin22，Pin23，Pin20，Pin24。3轴磁力计RM3100(U3)通过SPI总线连接到STM32F446RC的SPI2通道上，具体为RM3100的Pin1，Pin3，Pin23，Pin27和Pin28分别连接到U1的Pin36，Pin33，Pin37，Pin34和Pin35。SD卡接口槽(U4)通过SDIO总线连接到U1的对应引脚上，具体为SD卡的Pin1，Pin2，Pin3，Pin5，Pin7，Pin8和Pin9分别连接到U1的Pin51，Pin52，Pin54，Pin53，Pin39，Pin40和Pin41。时钟芯片DS1302(U5)通过IIC总线方式连接到STM32F446RC，具体为DS1302的Pin5，Pin6和Pin7分别连接到U1的Pin27，Pin26和Pin25。串口电平转换芯片MAX3232(U6)的Pin11和Pin12分别连接到STM32F446RC的Pin42和Pin43。

本发明中的芯片选型组合目前具有无可替代性，芯片的选型综合了姿态测量和波浪测量这两种需求后，按照芯片测量精度性能、芯片可靠性(战术级)、芯片价格以及芯片功耗等顺序选取。

实施例三

一种控制方法，用于的数字式姿态补偿和波浪测量系统，包括以下步骤：

控制单元通过预设的交互指令与上位机进行数据交互，交互指令包括姿态补偿工作模式或波浪工作模式切换命令，时钟设置命令、姿态测量频率设置命令以及波浪采样频率设置命令；

上位机选择姿态补偿工作模式或波浪工作模式；在姿态补偿工作模式下，获取姿态解算结果；在波浪工作模式下，获取波浪特征。

如图2所示,交互命令格式包括头码-1(1字节)、头码-2(1字节)、命令标志位(1字节)、用户设置内容(频率设置1字节/时间设置6字节)以及校验和(1字节)，其中，头码-1和头码-2设为固定值，分别为0x55和0xaa，命令标志位处设置5个值，0x10，0x11，0x72，0x73,0xf2，分别对应姿态补偿工作模式、波浪工作模式、姿态测量频率设置模式、波浪采样频率设置模式和时间设置模式，其第4字节为用户需要设置的姿态测量频率值(16进制写入)，允许写入的频率值范围为1-100Hz之间。对于波浪采样频率设置命令来说，其第4字节为用户输入的波浪采样频率值(16进制写入)，允许写入的频率值为2Hz或4Hz。对于时钟设置命令来说，其第4-9字节分别为用户需要写入的设置时间值(16进制写入)。第4字节为年，允许写入的范围为(0-100，表示2000年-2100年)。第5字节为月，允许写入的范围为(1-12，表示1月-12月)。第6字节为日，允许写入的范围为(1-31，表示1号-31号)。第7字节为小时，允许写入的范围为(1-24，表示1小时-24小时)。第8字节为分，允许写入的范围为(1-60，表示1分-60分)。第9字节为秒，允许写入的范围为(1-60，表示1秒-60秒)。

如图3所示，姿态补偿工作模式基于STM32F446的姿态补偿工作模式的板载源程序，包括如下步骤：

运行参数初始化，硬件初始化，进入无限循环主程序，在主程序中按照预置的频率依次进行加速度数据采集、磁力计数据采集、姿态角解算、姿态补偿矩阵计算以及计算结果输出。运行参数和硬件的初始化主要包括：首先读取片上Flash中的Mode_Flag工作模式标志位、Fre_Flag测量频率值和系统时间，并赋值相应的运行变量。进一步，初始化PID误差控制中的比例环节系数Kp和姿态输出心跳间隔数OutputNum。最后进行硬件初始化，依次初始化STM32F446的TIMER3、UART4、GPIO、SPI1通道和SPI2通道等片上硬件、还包括6轴mems惯导单元以及3轴磁力计单元。

姿态解算以及姿态补偿矩阵计算前还包括，判断姿态输出心跳间隔数OutputNum是否等于预设值，判断定时时间是否大于或等于预设值，若均是，则读取6轴mems惯导单元和3轴磁力计单元的数据，否则继续执行判断动作，根据返回的陀螺仪数据和加速度数据，动态选择PID误差控制中的比例环节系数Kp。本发明中根据陀螺仪的转动速率设置了两个不同的Kp值，以适应在不同的晃动速率下进行更好的数据融合。根据长期室内试验调试，实验人员发现，当ADIS16485的陀螺仪在10ms内转速超过5°时选取Kp＝0.001，否则选取Kp＝1，这种参数设置最为理想。

比例环节系数Kp选择后还包括，加速度数据归一化，获取重力方向和变迁的评估参数，计算评估参数与加速度数据的误差，对误差进行误差积分，误差积分通过PI控制后补偿到陀螺仪，然后进行四元数微分方程解算，四元数归一化，通过四元数归一化计算姿态角，通过姿态角计算姿态补偿矩阵，通过姿态补偿矩阵计算北西天坐标系的加速度值。

如图4所示，姿态角解算以及姿态补偿矩阵计算的结果按照指定格式通过串口依次输出，输出参数包括：横滚角(roll)、俯仰角(pitch)、航向角(yaw)、四元数(q₀、q₁、q₂、q₃),载体坐标系->北西天坐标系的姿态转换矩阵C_bn、载体系下的三轴加速度(ax_b,ay_b,az_b),北西天坐标系下的三轴加速度(ax_n,ay_n,az_n)。每次输出固定的92字节数据，其中头码占据2字节为0x55,0xaa，校验码占据2个字节，每个参数均占4个字节的float类型，且按照高位在前低位在后的格式发送。其中姿态补偿矩阵C_bn为3*3的矩阵，其数字式姿态补偿的主要功能可用来对载体系上的矢量测量进行姿态校正，将载体系的矢量测量值校正到北西天坐标系，获取自然地理坐标系下的矢量真实值。可应用在对晃动平台，如无人机、无人艇、Glider、海洋浮标等晃动平台上对三维矢量测量的姿态校正。如对浮标晃动平台上的三维风速校正，假设锚定浮标是只有晃动而没有运动发生，浮标上搭载的三维风速测量值为V_b＝[U_b,V_b,W_b]^T,则校正到北西天系后的三维风速测量值为V_n＝V_b*C_bn。

如图5所示，波浪工作模式基于STM32F446的波浪工作模式的板载源程序，包括如下步骤：

步骤一，运行参数初始化，硬件初始化，步骤二，进行惯性数据采样，加速度比力变换，步骤三：对波浪特征进行计算和统计，计算结果输出，波浪特征包括波高、波周期和波向，波浪特征进行计算和统计包括：对垂直方向的加速度进行频域变换，步骤四，通过跨零法计算波高和波周期，通过三角合成计算波向；步骤五，波浪特征计算完成后，通过串口输出当前一组的波浪特征计算结果，同步存储波浪特征计算结果。更进一步地，该惯性数据采样包括获取角运动信息、加速度信息以及地磁信息；所述加速度比力变换包括姿态角解算、姿态转换矩阵计算、载体系至北西天坐标系的加速度变换。

步骤一，初始化相应的中间运行变量数组(高通Fh和低通截止频率Fl)，读取片上Flash中的Fre_Flag_W波浪频率值、系统时间等并赋值给相应的运行变量。其它运行参数和硬件的初始化方法与上述姿态补偿工作模式的运行参数初始化步骤相同。完成初始化之后，程序进入90秒的定时等待并以1秒的间隔不断输‘*’字符，等待外部输入命令，如设置时钟时间、波浪采样频率(2Hz和4Hz)，用户如果需要对时间设置、波浪采样频率这两个参数进行设置的话，需要在开机后的90秒内完成。

90秒等待命令输入结束后，程序进入数据采样阶段，波浪采样时间长度为17分30秒。如果采样频率为2Hz则全部采样点数为2100组，如果采样频率为4Hz，则全部采样点数为4200组。采样完成后，对采样数据的首尾高污染区域进行数据剔除，剔除方法为：舍弃采样数据首部和尾部的固定长度的数据。如果是2Hz采样频率则舍弃首部和尾部各26组采样点(13秒)数据，保留中间的2048(2¹¹)组数据。如果是4Hz采样频率则舍弃首部和尾部各52组采样点(13秒)数据，保留中间的4096(2¹²)组数据。

步骤二，假设采样频率为4Hz，采样后进入该部分的数据包括4096组经过姿态补偿后在北西天坐标系下的三轴加速度值ax_n,ay_n和az_n。对垂直方向的加速度az_n进行2次积分即可得到位移sz_n，其核心积分算法采用频域数值积分，对低频波浪加速度信号的积分来说，频域积分可有效解决时域积分所产生的位移趋势漂移问题。而且频域高低频截止频率的滤波参数设置，相对于传统的时域积分的滤波性能具有更好的自适应性。

步骤三，频域积分变换具体包括，将波浪特征依次进行动态分配内存池、圆频率的离散化、生成FFT数组、调用STM32F446RC的DSP库函数进行FFT计算、时域变换为频域、进行2次积分得到频域位移信号、清除频域中的高频/低频截止频率信号、调用STM32F4的DSP库函数进行IFFT计算，将上述位频域移信号从频域变换为时域。

步骤四，所述波向的计算方法为：首先把0-360度划分为以22.5度为间隔的16个方向区，统计水平2轴加速度ax_n,ay_n合成角度在跨零点处落在各个方向区的数量，其中数量最多的方向区作为波浪的主方向。具体实现方法为：首先计算4096组合成角度，因坐标补偿后的坐标系为北西天坐标系，即ax_n的正方向为北向,ay_n的正方向为西向(-ay_n的正方向为动向)，两轴加速度的合成角度θ＝arctan(-ay_n/ax_n)。进一步，统计4096组角度数据中跨零点处的角度数据分别分布在16个方向区内的数量，其中数量最多的方向区作为主波向。

所述波浪特征计算结果的输出格式为：$WAVE,YYYY-MM-DD HH:MM:SS,H_m,T_m,H_1/10,T_1/10,H_1/3,T_1/3,H_a,T_a,D_m<CR><LF>

其中：$WAVE为标志字符串，YYYY-MM-DDHH:MM:SS为当前一组的波浪测量时间(测量结束时刻)，H_m,T_m,H_1/10,T_1/10,H_1/3,T_1/3,H_a,T_a,D_m分别表示最大波高、最大波周期、1/10大波高、1/10大波周期、1/3大波高、1/3大波周期、平均波高、平均波周期、主波向。每个参数之间以‘，’为分隔符，结果以<CR><LF>为截止符，分隔符和截止符清晰，方便上位机进行数据的解析。其中每个波浪特征参数部分占据固定的5个字符，小数点前后分别包含2个字符，精度可达到小数点后2位数。如一组实际波浪测量的输出结果为：

$WAVE,2020-02-1211:55:30,01.01,05.02,00.85,04.51,00.72,04.31,00.54,03.92,67.5

表示2020年2月12日上午11:38:00-11:55:30这17分30秒期间的波浪特征为：

最大波高1.01米、最大波周期5.02秒、1/10大波高0.85米、1/10大波周期4.51秒、1/3大波高0.72米、1/3大波周期4.31秒、平均波高0.54米、平均波周期3.92秒、主波向67.5度。

步骤五，波浪工作模式中，在波浪特征计算完成后，除了向上位机发送计算结果外，也同步存储在系统的SD卡中，存储的数据包括4096组原始采样数据和波浪统计结果。其中，每次17分30秒采样的4096组原始采样数据都存储为一个以当前日期为文件名的.txt文件中，如02121138.txt表示2020年2月12日上午11:38开始采样的4096组原始数据，其每组原始数据的存数格式为：

YYYY-MM-DDHH:MM:SS ax_b ay_b az_bpitch roll yaw ax_n ay_n az_n<CR><LF>

如一组实际波浪测量的原始数据存储在板载SD卡的根目录下02121138.txt中，文件中的4096组原始数据为，仅给出其中8组为例：

2020-02-1211:38:03 0.01 0.56 1.28 19.8 02.2 46.5 0.07 0.08 1.40

2020-02-1211:38:03 -0.07 0.11 1.38 07.1 07.8 46.1 0.10 -0.06 1.39

2020-02-1211:38:03 -0.05 -0.29 0.45-18.9 11.1 45.2 0.02 -0.13 0.52

2020-02-1211:38:03 0.00 -0.14 0.50 -33.5 00.0 45.1 0.04 0.15 0.49

2020-02-1211:38:04 -0.04 -0.30 0.58 -27.8 -01.8 43.2 -0.06 0.01 0.65

2020-02-1211:38:04 0.13 -0.21 0.96 -07.3 -02.7 43.6 0.07 -0.10 0.98

2020-02-1211:38:04 0.02 0.14 1.15 11.0 02.0 45.6 0.05 -0.08 1.16

2020-02-1211:38:04 -0.36 0.44 1.07 25.9 14.3 45.8 -0.06 -0.06 1.21每组波浪统计结果全部存储在板载SD卡的根目录下wave.txt中,波浪特征的存储格式和步骤46的波浪输出格式相同，如3组实际波浪测量的统计数据存储格式为：

$WAVE,2020-02-1211:55:30,01.01,05.02,00.85,04.51,00.72,04.31,00.54,03.92,67.5

$WAVE,2020-02-1212:25:30,01.13,05.18,00.92,04.63,00.76,04.36,00.59,03.96,45.0

$WAVE,2020-02-1212:55:30,01.06,05.10,00.88,04.55,00.73,04.33,00.56,04.12,67.5

上述实施例只是为了说明本发明的技术构思及特点，其目的是在于让本领域内的普通技术人员能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡是根据本发明内容的实质所做出的等效的变化或修饰，都应涵盖在本发明的保护范围内。

Claims (6)

1.一种数字式姿态补偿和波浪测量系统控制方法，用于以下的数字式姿态补偿和波浪测量系统，所述系统包括控制单元，与所述控制单元连接的6轴mems惯导单元、3轴磁力计单元、时钟单元、存储单元、串口电平转换单元以及电源转换单元；

所述控制单元用于处理来自外围单元的数据并发送控制命令控制外围单元；

所述6轴mems惯导单元用于获取角运动信息以及加速度信息；

所述3轴磁力计单元用于获取地磁信息；

所述时钟单元用于产生时钟信号；

所述存储单元用于储存原始采样数据和波浪统计结果；

所述串口电平转换单元用于输出串口电平；

所述电源转换单元用于给所述控制单元、6轴mems惯导单元、3轴磁力计单元、时钟单元、存储单元以及串口电平转换单元输入电源；

所述控制单元为STM32F446微处理器，所述6轴mems惯导单元为ADIS16485芯片，所述3轴磁力计单元为RM3100芯片，所述时钟单元为DS1302时钟芯片，所述存储单元为SD卡，所述串口电平转换单元为MAX3232芯片；

其特征在于，包括以下步骤：

所述控制单元通过预设的交互指令与上位机进行数据交互，所述交互指令包括姿态补偿工作模式或波浪工作模式切换命令，时钟设置命令、姿态测量频率设置命令以及波浪采样频率设置命令；

所述上位机选择姿态补偿工作模式或波浪工作模式；在所述姿态补偿工作模式下，获取姿态解算结果；在所述波浪工作模式下，获取波浪特征；

所述波浪工作模式包括如下步骤：

步骤一，运行参数初始化，硬件初始化，步骤二，进行惯性数据采样，加速度比力变换，步骤三：对波浪特征进行计算和统计，计算结果输出，所述波浪特征包括波高、波周期和波向，所述波浪特征进行计算和统计包括：对垂直方向的加速度进行频域积分变换，步骤四，通过跨零法计算波高和波周期；步骤五，波浪特征计算完成后，输出当前一组的波浪特征计算结果，同步存储所述波浪特征计算结果；

所述频域积分变换包括，将所述波浪特征依次进行动态分配内存池、圆频率的离散化、生成FFT数组、进行FFT计算、时域变换为频域、进行2次积分得到频域位移信号、清除频域中的高频/低频截止频率信号、进行IFFT计算，将所述频域位移信号从频域变换为时域。

2.如权利要求1所述的控制方法，其特征在于，所述交互指令的格式包括头码-1、头码-2、命令标志位、用户设置内容以及校验和，其中，所述头码-1和头码-2设为固定值，所述命令标志位处设置5个值，分别对应姿态补偿工作模式、波浪工作模式、姿态测量频率设置模式、波浪采样频率设置模式和时间设置模式。

3.如权利要求1所述的控制方法，其特征在于，所述姿态补偿工作模式包括如下步骤：

运行参数初始化，硬件初始化，进入无限循环主程序，在所述主程序中按照预置的频率依次进行加速度数据采集、磁力计数据采集、姿态角解算、姿态补偿矩阵计算以及计算结果输出。

4.如权利要求3所述的控制方法，其特征在于，所述姿态角解算以及所述姿态补偿矩阵计算前还包括，判断姿态输出心跳间隔数OutputNum是否等于预设值，判断定时时间是否大于或等于预设值，若均是，则读取所述6轴mems惯导单元和3轴磁力计单元的数据，否则继续执行判断动作，根据返回的陀螺仪数据和加速度数据，动态选择PID误差控制中的比例环节系数Kp。

5.如权利要求4所述的控制方法，其特征在于，所述比例环节系数Kp选择后还包括，加速度数据归一化，获取重力方向和变迁的评估参数，计算所述评估参数与加速度数据的误差，对所述误差进行误差积分，所述误差积分通过PI控制后补偿到陀螺仪，然后进行四元数微分方程解算，四元数归一化，通过所述四元数归一化计算姿态角，通过所述姿态角计算姿态补偿矩阵，通过所述姿态补偿矩阵计算北西天坐标系的加速度值。

6.如权利要求5所述的控制方法，其特征在于，所述6轴mems惯导单元的陀螺仪在10ms内转速超过5°时选取Kp＝0.001，否则选取Kp＝1。

Images