[发明专利]一种数字式姿态补偿和波浪测量系统及控制方法

权利要求书

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。