[发明专利]一种水下机器人位姿数据采集系统及其数据融合方法

权利要求书

1.一种水下机器人位姿数据采集系统，其特征在于：包括ARM处理器模块、GPS模块、深度计及其滤波模块、九轴传感器模块、CAN数据收发模块以及电源模块，所述的ARM处理器模块与GPS模块、深度计及其滤波模块、九轴传感器模块、CAN数据收发模块相连；所述GPS模块、深度计及其滤波模块和九轴传感器模块分别通过总线方式与ARM处理器模块相连接；所述电源模块与GPS模块、深度计及其滤波模块、ARM处理器模块以及CAN数据收发模块相连接，且CAN数据收发模块通过CAN总线方式与水下机器人控制系统连接；

其中，所述的ARM处理器模块包括数据收发处理模块、互补滤波算法模块以及CAN数据包处理模块；所述互补滤波算法模块与数据收发模块和CAN数据包处理模块连接；所述数据收发模块分别采用串口通信、RS485总线通信、SPI总线通讯方式与GPS模块、深度计及其滤波模块、九轴传感器模块连接，CAN数据包处理模块与CAN数据收发模块连接。

2.根据权利要求1所述的一种水下机器人位姿数据采集系统，其特征在于：所述数据采集系统还包括其他辅助模块，所述其他辅助模块通过总线通信与ARM处理器模块连接，所述其他辅助模块包括运行指示模块、电源管理模块、温湿度检测模块和漏水模块。

3.根据权利要求1所述的一种水下机器人位姿数据采集系统，其特征在于：所述GPS模块包括微处理芯片、天线和串口数据收发芯片。

4.根据权利要求1所述的一种水下机器人位姿数据采集系统，其特征在于：所述深度计及其滤波模块包块传感器芯片、RS485收发芯片和滤波芯片。

5.根据权利要求1所述的一种水下机器人位姿数据采集系统，其特征在于：所述九轴传感器模块包括三轴陀螺仪芯片、三轴加速度计芯片、三轴磁力计芯片和数据收发芯片。

6.根据权利要求1所述的一种水下机器人位姿数据采集系统，其特征在于：所述ARM处理器模块包括ARM处理器芯片。

7.根据权利要求1所述的一种水下机器人位姿数据采集系统，其特征在于：所述CAN数据收发模块包括CAN数据收发芯片和数据隔离芯片。

8.根据权利要求1所述的一种水下机器人位姿数据采集系统，其特征在于：所述电源模块包括线性稳压电源、DC/DC电源和滤波电路，所述滤波电路在电源输入/出端并联不同容值的电解电容和钽电容。

9.一种权利要求1所述的水下机器人位姿数据采集系统的数据融合方法，其特征在于：包括传感器数据采集、数据处理、互补滤波算法融合和数据打包发送，具体包括以下步骤：

(1)通过深度计及其滤波模块实时采集深度计模块的数据，并滤除噪声，将得到的滤波处理后的水下机器人深度信息存储到ARM处理器的固定存储地址内；

(2)通过九轴传感器模块实时采集三轴陀螺仪数据、三轴加速度计数据和三轴磁力计数据，并滤除噪声，将得到的滤波处理后的九轴传感器原始数据存储到ARM处理器的固定存储地址内；

(3)通过互补滤波算法将处理后的数据进行融合，得到水下机器人位姿数据；

(4)通过GPS模块采集GPS数据，将得到的处理后的GPS数据存储到ARM处理器的固定存储地址内，在GPS模块有效时，通过GPS数据校正水下机器人位姿数据；

(5)CAN数据收发模块将水下机器人位姿等数据上传至下位机主控制系统，并接收下位机主控制系统对水下机器人位姿的控制指令。

10.根据权利要求9所述的一种数据融合方法，其特征在于：具体包括以下步骤：

(1)通过中位值平均滤波算法对采集的三轴加速度原始数据(a_x、a_y、a_z)、三轴磁力计原始数据(m_x、m_y、m_z)进行滤波去噪；

(2)归一化加速度计和磁力计数据，即

(3)计算地理坐标系转换到载体坐标系的重力分量、磁场分量；

(4)通过向量叉乘计算出位姿误差的校正补偿值；陀螺仪的校正补偿值可通过载体坐标系下的加速度数据和重力分量做向量叉乘运算，即同理可得磁力计补偿陀螺仪的误差向量表示成矩阵形式：

式中，为加速度计实际测量值，为加速度计理论预测值，为磁力计实际测量值，为磁力计理论预测值。e_a、e_m为实际测量值与理论预测值之间的误差。

(5)通过比例-积分法修正误差；将姿态误差补偿到角速度上，补偿值计算公式如下所示：

λ＝K_p(e_a+e_m)+K_i∫₀^t(e_a+e_m)dt

对陀螺仪数据补偿后的角速度更新公式为：ω_t＝ω+λ；

式中，K_p、K_i分别为调节的比例积分系数，λ为补偿值，ω_t为补偿后的角速度值，ω＝(ω_x、ω_y、ω_z)为陀螺仪的实际测量值。

(6)补偿后的ω_t已知，通过四元数微分方程求解出四元数：

根据公式求解出姿态角，即：

式中，Q＝(q₀、q₁、q₂、q₃)为四元数，负责数据解算融合的整个过程，θ、γ、ψ为欧拉角，负责描述着水下机器人的姿态。

(7)在GPS数据有效时，通过GPS数据校正姿态角偏移值。