[发明专利]一种水下机器人双冗余姿态检测系统

摘要

本发明属于水下机器人技术领域，具体涉及一种水下机器人双冗余姿态检测系统。一种水下机器人双冗余姿态检测系统，由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成，通过微控制器实现导航计算，输出姿态、速度、深度、高度信息，多普勒计程仪的输出接口接至数据融合处理器的输入接口，电子罗盘的输出接口接至数据融合处理器的输入接口，MEMS姿态传感器的输出接口接至数据融合处理器的输入接口。本发明采用多种类型导航传感器提供的信息，通过数据融合实现高可靠导航信息输出的方法。通过对典型海况下的分析，设计出有效地补偿方法，避免外界干扰和传感器自身缺点对水下机器人姿态信息影响。

主权项

1.一种水下机器人双冗余姿态检测系统，由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成，其特征在于：通过微控制器实现导航计算，输出姿态、速度、深度、高度信息，多普勒计程仪的输出接口接至数据融合处理器的输入接口，电子罗盘的输出接口接至数据融合处理器的输入接口，MEMS姿态传感器的输出接口接至数据融合处理器的输入接口，深度计的输出接口接至数据融合处理器的输入接口，高度计的输出接口接至数据融合处理器的输入接口，数据融合处理器的输出接口接至水下机器人中心控制器的输入接口；数据融合处理器是将多种传感器信息采集后，进行信息融合和处理，通过基于UD分解的EKF滤波和组合导航系统AHRS部分模型修正后，输出姿态、速度、深度、高度信息；MEMS姿态传感器是由三轴MEMS加速度计和三轴MEMS陀螺组成；电子罗盘采用地球磁场信息来确定北极指向的导航设备；电子罗盘采用传感器检测所在位置地磁场强度，经过计算获得载体航向信息，采用非铁磁性金属作为外壳；基于UD分解的EKF滤波的运算步骤如下：步骤1：状态一步预测：式中：X_k,k‑1是系统的n×n维状态观测矩阵，是系统的状态转移矩阵，是状态估计；步骤2：UD分解：式中：P_k是估计方差矩阵，P_k,k‑1是误差方差矩阵，Γ_k,k‑1是n×p维噪声输入矩阵，Q_k,k‑1是系统过程噪声方差阵；步骤3：滤波增益矩阵的求解：G_k＝U_k,k‑1F_kS_k＝H_kG_k+R_k步骤4：状态估计：步骤5：估计方差矩阵：组合导航系统AHRS部分模型修正方案的步骤如下所示：步骤1：采用三轴电子罗盘和三轴加速度计原始数据，计算出地理坐标系下的航向角，作为导航系统航向的观测信息；对电子罗盘的三轴原始数据进行倾斜补偿：航向角的计算如下所示：步骤2：组合导航系统姿态航向部分的模型构建：令Z轴MEMS陀螺的角速度输入值为陀螺零偏为ε_Z，陀螺噪声序列为n_r,z，Z轴MEMS陀螺的模型如下所示：令k时刻陀螺的零偏为ε_Z，k‑1时刻陀螺的零偏为零偏增量Δε_Z如下所示：航向部分状态方程和观测方程如下所示：系统的状态量由航向角增量Δψ和Δε_Z构成，如下所示：令Z轴陀螺陀螺随机游走噪声序列为n_ωw，系统的噪声激励序列W_yaw如下所示：W_yaw＝[n_ωr n_ωw]^T；系统的状态转移矩阵系统噪声驱动阵Γ_yaw如下所示：系统的观测量由电子罗盘航向角ψ_Mag与上一时刻系统输出航向角ψ差值构成，如下所示：系统的观测转移矩阵H_yaw如下所示：H_yaw＝[1 0]；系统状态噪声方差阵Q_yaw、量测噪声方差阵R_yaw如下所示：步骤3：将旧AHRS模型中的航向修正部分移除，只保留姿态部分，如下所示：步骤4：求取滤波结果后，通过四元数求取横滚和俯仰角，与修正后的航向角求取滤波后得到的四元数，完成本次滤波器观测更新。