CN106500721B - 一种水下机器人双冗余姿态检测系统 - Google Patents

Abstract

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

Description

一种水下机器人双冗余姿态检测系统

技术领域

本发明属于水下机器人技术领域，具体涉及一种水下机器人双冗余姿态检测系统。

背景技术

水下机器人姿态检测系统可以为复杂未知环境下进行作业的水下机器人提供姿态、航向、速度、深度、高度等信息，这些信息对于水下机器人能否完成海洋环境作业任务至关重要。高可靠性的导航系统将能够有效保障水下机器人安全生存和成功完成作业任务。

由于高精度惯性导航系统成本高昂，结构较为复杂。而基于MEMS惯性器件的惯性导航系统相比于高精度惯性导航系统具有成本低廉，结构简单的优势，同时姿态和航向精度指标能够满足作业级水下机器人工作需求。因此在工程应用领域，若考虑成本因素，同时精度指标要求适当，作业级水下机器人可以采用MEMS惯性器件与电子罗盘组合，为水下机器人提供稳定可靠的姿态和航向参考信息。

在水下机器人导航系统中，惯性器件的误差对惯导系统精度影响很大。电子罗盘在实际应用中，易受到外界铁磁性物质干扰，导致输出航向信息存在较大误差。由于单一的导航系统都存在其各自的缺点，性能指标难以完全满足作业级水下机器人的导航需求。为了使导航系统的精度和可靠性能够满水下机器人作业需求，需要结合各个单一导航系统的优势，通过合理的数据融合方式，获得精度和可靠性满足需求的水下机器人导航系统。

在作业级水下机器人的导航系统中，通过组合导航的方式，可以实现导航原理、输出数据的类型和特点都各不相同的导航系统的数据融合，实现各个导航系统之间的优势互补，满足作业级水下机器人对导航信息精度和高可靠性的需求。

发明内容

本发明的目的在于提供一种多传感器数据融合的姿态检测系统。

本发明的目的是这样实现的：

一种水下机器人双冗余姿态检测系统，由数据融合处理器、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_k

S_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：求取滤波结果后，通过四元数求取横滚和俯仰角，与修正后的航向角求取滤波后得到的四元数，完成本次滤波器观测更新。

本发明的有益效果在于：

(1)采用多种类型导航传感器提供的信息，通过数据融合实现高可靠导航信息输出的方法。

(2)采用的传感器都是低成本，但是相互信息具有很强的互补性，能够有效满足水下机器人实际需要，克服单一传感器的缺陷，通过对典型海况下的分析，设计出有效地补偿方法，避免外界干扰和传感器自身缺点对水下机器人姿态信息影响。

(3)基于多传感器的姿态检测系统为水下机器人姿态测量提供解决方案，具有一定通用性和实用性。

附图说明

图1为水下机器人双冗余姿态检测系统结构图；

图2为组合导航系统AHRS模型优化处理后信号流程图；

图3为组合导航系统AHRS模型优化前后姿态波形图；

图4为水下机器人导航系统动态实验姿态波形；

图5为水下机器人导航系统动态实验速度波形；

图6为水下机器人导航系统动态实验位置波形。

具体实施方式

下面结合附图对本发明做更详细的描述。

本发明涉及水下作业机器人(简称水下机器人)控制领域，提供了一种具有双冗余的姿态检测系统。本发明设计双冗余水下机器人姿态检测系统由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成。通过微控制器实现导航计算，输出姿态、速度、深度、高度信息。本发明解决单一形式姿态传感器精度低、易受外界干扰等问题，通过多种传感器信息融合和相互补偿，实现水下机器人姿态检测系统的低成本、高可靠性和高精度，且具有通用性和实用性，满足水下机器人在不同工作环境下对姿态检测要求。该系统克服单一传感器检测精度低，易受外界干扰等问题，提高系统的可靠性和测量精度，满足实际作业需要。

本发明采用的技术方案是：

双冗余水下机器人姿态检测系统由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成。通过微控制器实现导航计算，输出姿态、速度、深度、高度信息。多普勒计程仪的输出接口接至数据融合处理器的输入接口，电子罗盘的输出接口接至数据融合处理器的输入接口，MEMS姿态传感器的输出接口接至数据融合处理器的输入接口，深度计的输出接口接至数据融合处理器的输入接口，高度计的输出接口接至数据融合处理器的输入接口，数据融合处理器的输出接口接至水下机器人中心控制器的输入接口。

MEMS姿态传感器是由三轴MEMS加速度计和三轴MEMS陀螺组成，能够测量载体的姿态，该传感器已经为每个传感器的灵敏度、偏置等参数进行工厂校准，并且对传感器的安装误差进行了严格的正交对准，使其能够提供精确的测量信息。同时该传感器针对MEMS器件的温度漂移也进行了校准，使该传感器能够有更宽的温度应用环境。

电子罗盘是采用地球磁场信息来确定北极指向的导航设备。电子罗盘采用传感器检测所在位置地磁场强度，经过计算获得载体航向信息。采用非铁磁性金属作为外壳，具有低功耗、小体积和易于安装的优点，但是易受外界磁场干扰。

多普勒计程仪是一种利用多普勒效应，实现航行器在运动中绝对或相对水层速度测量的传感器，深度计是测量载体到水面深度的传感器，高度计是利用声学测量载体到海底的高度传感器。

本发明解决了单一传感器检测精度低，易受外界干扰等问题，通过多种传感器信息融合实现信息的融合和互补，克服MEMS姿态传感器长时间的累积误差，以及电子罗盘容受外界电磁干扰的缺点，提高系统的可靠性和测量精度。

本发明提供的是一种水下机器人双冗余姿态检测系统，该系统解决单一形式姿态传感器精度低、易受外界干扰等问题，通过多种传感器信息融合和相互补偿，实现水下机器人姿态检测系统的低成本、高可靠性和高精度，满足控制系统需要和实际工程需要，具有通用性和实用性，满足水下机器人在不同工作环境下对姿态检测要求。

实施1，结合附图1，本发明的双冗余水下机器人姿态检测系统由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成。通过微控制器实现导航计算，输出姿态、速度、深度、高度信息。多普勒计程仪的输出接口1接至数据融合处理器的输入接口2，电子罗盘的输出接口3接至数据融合处理器的输入接口4，MEMS姿态传感器的输出接口5接至数据融合处理器的输入接口6，深度计的输出接口7接至数据融合处理器的输入接口8，高度计的输出接口9接至数据融合处理器的输入接口10，数据融合处理器的输出接口11接至水下机器人中心控制器的输入接口12。

实施2，基于UD分解的EKF滤波的运算步骤如下：

步骤1：状态一步预测：

步骤2：UD分解：

步骤3：滤波增益矩阵的求解：

由式(3)可得：

步骤4：状态估计：

步骤5：估计方差矩阵：

通过基于UD分解的EKF滤波算法，能够避免EKF的运算发散，有效提高了导航系统数据融合算法的稳定性。

实施3，结合附图2，组合导航系统AHRS部分模型修正方案的步骤如下所示：

步骤1：采用三轴磁力计和三轴加速度计原始数据，计算出地理坐标系下的航向角，作为导航系统航向的观测信息；

对电子罗盘的三轴原始数据进行倾斜补偿，如式(7)所示：

航向角的计算如式(8)所示：

步骤2：组合导航系统姿态航向部分的模型构建

令Z轴MEMS陀螺的角速度输入值为陀螺零偏为ε_Z，陀螺噪声序列为n_r,z。Z轴MEMS陀螺的模型如式(9)所示：

令k时刻陀螺的零偏为ε_Z，k-1时刻陀螺的零偏为零偏增量Δε_Z如式(10)所示：

航向部分状态方程和观测方程如式(11)所示：

系统的状态量由航向角增量Δψ和Δε_Z构成，如式(12)所示：

令Z轴陀螺陀螺随机游走噪声序列为n_ωw，系统的噪声激励序列W_yaw如式(13)所示：

W_yaw＝[n_ωr n_ωw]^T (13)

系统的状态转移矩阵系统噪声驱动阵Γ_yaw如式(14)所示：

系统的观测量由电子罗盘航向角ψ_Mag与上一时刻系统输出航向角ψ差值构成，如式(15)所示：

系统的观测转移矩阵H_yaw如式(16)所示：

H_yaw＝[1 0] (16)

系统状态噪声方差阵Q_yaw、量测噪声方差阵R_yaw如式(17)所示：

步骤3：将旧AHRS模型中的航向修正部分移除，只保留姿态部分，如式(18)所示：

步骤4：求取滤波结果后，通过四元数求取横滚和俯仰角，与修正后的航向角求取滤波后得到的四元数，完成本次滤波器观测更新。

实施4，验证试验设计上，将导航系统静止放置，在1.1秒至4.6秒、对电子罗盘进行铁磁干扰，记录模型优化前后导航系统的姿态和航向信息，对方案的有效性进行验证。模型优化前后的系统欧拉角波形附图3所示。由附图3可知，在电子罗盘受到铁磁干扰时，模型未优化前系统得横滚角、俯仰角在磁干扰稳定后分别产生了-13°和28°的稳态误差。模型优化后，系统横滚角、俯仰角在系统在电子罗盘受到铁磁干扰时，仍然维持初始角度。通过该实验，验证了AHRS模型优化后方案的有效性。

实施5，水下机器人在进行水下作业时，主要有定高、定深、定航向、匀速巡检等模式，需要水下机器人导航系统提供满足水下机器人控制精度需求的姿态、航向、速度、深度和高度信息。水下机器人在水中移动速度较慢，液压系统和推进器运转也会使水下机器人存在一定程度震动。考虑到测试环境限制，在本实验中，将水下机器人导航系统安装至运动载体，进行导航系统测试。采用GPS导航系统为组合导航系统的速度和水平位置提供参考，利用气压计传感器提供高度参考。系统姿态、速度、位置数据输出附图4、附图5、附图6所示。

Claims (1)

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_k

S_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：求取滤波结果后，通过四元数求取横滚和俯仰角，与修正后的航向角求取滤波后得到的四元数，完成本次滤波器观测更新。