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

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

【技术实现步骤摘要】

本专利技术属于水下机器人
，具体涉及一种水下机器人双冗余姿态检测系统。
技术介绍
水下机器人姿态检测系统可以为复杂未知环境下进行作业的水下机器人提供姿态、航向、速度、深度、高度等信息，这些信息对于水下机器人能否完成海洋环境作业任务至关重要。高可靠性的导航系统将能够有效保障水下机器人安全生存和成功完成作业任务。由于高精度惯性导航系统成本高昂，结构较为复杂。而基于MEMS惯性器件的惯性导航系统相比于高精度惯性导航系统具有成本低廉，结构简单的优势，同时姿态和航向精度指标能够满足作业级水下机器人工作需求。因此在工程应用领域，若考虑成本因素，同时精度指标要求适当，作业级水下机器人可以采用MEMS惯性器件与电子罗盘组合，为水下机器人提供稳定可靠的姿态和航向参考信息。在水下机器人导航系统中，惯性器件的误差对惯导系统精度影响很大。电子罗盘在实际应用中，易受到外界铁磁性物质干扰，导致输出航向信息存在较大误差。由于单一的导航系统都存在其各自的缺点，性能指标难以完全满足作业级水下机器人的导航需求。为了使导航系统的精度和可靠性能够满水下机器人作业需求，需要结合各个单一导航系统的优势，通过合理的数据融合方式，获得精度和可靠性满足需求的水下机器人导航系统。在作业级水下机器人的导航系统中，通过组合导航的方式，可以实现导航原理、输出数据的类型和特点都各不相同的导航系统的数据融合，实现各个导航系统之间的优势互补，满足作业级水下机器人对导航信息精度和高可靠性的需求。
技术实现思路
本专利技术的目的在于提供一种多传感器数据融合的姿态检测系统。本专利技术的目的是这样实现的：一种水下机器人双冗余姿态检测系统，由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成，通过微控制器实现导航计算，输出姿态、速度、深度、高度信息，多普勒计程仪的输出接口接至数据融合处理器的输入接口，电子罗盘的输出接口接至数据融合处理器的输入接口，MEMS姿态传感器的输出接口接至数据融合处理器的输入接口，深度计的输出接口接至数据融合处理器的输入接口，高度计的输出接口接至数据融合处理器的输入接口，数据融合处理器的输出接口接至水下机器人中心控制器的输入接口；数据融合处理器是将多种传感器信息采集后，进行信息融合和处理，通过基于UD分解的EKF滤波和组合导航系统AHRS部分模型修正后，输出姿态、速度、深度、高度信息；MEMS姿态传感器是由三轴MEMS加速度计和三轴MEMS陀螺组成；电子罗盘采用地球磁场信息来确定北极指向的导航设备；电子罗盘采用传感器检测所在位置地磁场强度，经过计算获得载体航向信息，采用非铁磁性金属作为外壳；基于UD分解的EKF滤波的运算步骤如下：步骤1：状态一步预测：式中：Xk,k-1是系统的n×n维状态观测矩阵，是系统的状态转移矩阵，是状态估计；步骤2：UD分解：式中：Pk是估计方差矩阵，Pk,k-1是误差方差矩阵，Γk,k-1是n×p维噪声输入矩阵，Qk,k-1是系统过程噪声方差阵；步骤3：滤波增益矩阵的求解：Gk＝Uk,k-1FkSk＝HkGk+Rk步骤4：状态估计：步骤5：估计方差矩阵：组合导航系统AHRS部分模型修正方案的步骤如下所示：步骤1：采用三轴磁力计和三轴加速度计原始数据，计算出地理坐标系下的航向角，作为导航系统航向的观测信息；对电子罗盘的三轴原始数据进行倾斜补偿：航向角的计算如下所示：步骤2：组合导航系统姿态航向部分的模型构建：令Z轴MEMS陀螺的角速度输入值为陀螺零偏为εZ，陀螺噪声序列为nr,z，Z轴MEMS陀螺的模型如下所示：令k时刻陀螺的零偏为εZ，k-1时刻陀螺的零偏为零偏增量ΔεZ如下所示：航向部分状态方程和观测方程如下所示：系统的状态量由航向角增量Δψ和ΔεZ构成，如下所示：令Z轴陀螺陀螺随机游走噪声序列为nωw，系统的噪声激励序列Wyaw如下所示：Wyaw＝[nωrnωw]T；系统的状态转移矩阵系统噪声驱动阵Γyaw如下所示：系统的观测量由电子罗盘航向角ψMag与上一时刻系统输出航向角ψ差值构成，如下所示：系统的观测转移矩阵Hyaw如下所示：Hyaw＝[10]；系统状态噪声方差阵Qyaw、量测噪声方差阵Ryaw如下所示：步骤3：将旧AHRS模型中的航向修正部分移除，只保留姿态部分，如下所示：步骤4：求取滤波结果后，通过四元数求取横滚和俯仰角，与修正后的航向角求取滤波后得到的四元数，完成本次滤波器观测更新。本专利技术的有益效果在于：(1)采用多种类型导航传感器提供的信息，通过数据融合实现高可靠导航信息输出的方法。(2)采用的传感器都是低成本，但是相互信息具有很强的互补性，能够有效满足水下机器人实际需要，克服单一传感器的缺陷，通过对典型海况下的分析，设计出有效地补偿方法，避免外界干扰和传感器自身缺点对水下机器人姿态信息影响。(3)基于多传感器的姿态检测系统为水下机器人姿态测量提供解决方案，具有一定通用性和实用性。附图说明图1为水下机器人双冗余姿态检测系统结构图；图2为组合导航系统AHRS模型优化处理后信号流程图；图3为组合导航系统AHRS模型优化前后姿态波形图；图4为水下机器人导航系统动态实验姿态波形；图5为水下机器人导航系统动态实验速度波形；图6为水下机器人导航系统动态实验位置波形。具体实施方式下面结合附图对本专利技术做更详细的描述。本专利技术涉及水下作业机器人(简称水下机器人)控制领域，提供了一种具有双冗余的姿态检测系统。本专利技术设计双冗余水下机器人姿态检测系统由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成。通过微控制器实现导航计算，输出姿态、速度、深度、高度信息。本专利技术解决单一形式姿态传感器精度低、易受外界干扰等问题，通过多种传感器信息融合和相互补偿，实现水下机器人姿态检测系统的低成本、高可靠性和高精度，且具有通用性和实用性，满足水下机器人在不同工作环境下对姿态检测要求。该系统克服单一传感器检测精度低，易受外界干扰等问题，提高系统的可靠性和测量精度，满足实际作业需要。本专利技术采用的技术方案是：双冗余水下机器人姿态检测系统由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成。通过微控制器实现导航计算，输出姿态、速度、深度、高度信息。多普勒计程仪的输出接口接至数据融合处理器的输入接口，电子罗盘的输出接口接至数据融合处理器的输入接口，MEMS姿态传感器的输出接口接至数据融合处理器的输入接口，深度计的输出接口接至数据融合处理器的输入接口，高度计的输出接口接至数据融合处理器的输入接口，数据融合处理器的输出接口接至水下机器人中心控制器的输入接口。MEMS姿态传感器是由三轴MEMS加速度计和三轴MEMS陀螺组成，能够测量载体的姿态，该传感器已经为每个传感器的灵敏度、偏置等参数进行工厂校准，并且对传感器的安装误差进行了严格的正交对准，使其能够提供精确的测量信息。同时该传感器针对MEMS器件的温度漂移也进行了校准，使该传感器能够有更宽的温度应用环境。电子罗盘是采用地球磁场信息来确定北极指向的导航设备。电子罗盘采用传感器检测所在位置地磁场强度，经过计算获得载体航向信息。采用非铁磁性金属作为外壳，具有低功耗、小体积和易于安装的优点，但是易受外界磁场干扰。本文档来自技高网...

【技术保护点】
一种水下机器人双冗余姿态检测系统，由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成，其特征在于：通过微控制器实现导航计算，输出姿态、速度、深度、高度信息，多普勒计程仪的输出接口接至数据融合处理器的输入接口，电子罗盘的输出接口接至数据融合处理器的输入接口，MEMS姿态传感器的输出接口接至数据融合处理器的输入接口，深度计的输出接口接至数据融合处理器的输入接口，高度计的输出接口接至数据融合处理器的输入接口，数据融合处理器的输出接口接至水下机器人中心控制器的输入接口；数据融合处理器是将多种传感器信息采集后，进行信息融合和处理，通过基于UD分解的EKF滤波和组合导航系统AHRS部分模型修正后，输出姿态、速度、深度、高度信息；MEMS姿态传感器是由三轴MEMS加速度计和三轴MEMS陀螺组成；电子罗盘采用地球磁场信息来确定北极指向的导航设备；电子罗盘采用传感器检测所在位置地磁场强度，经过计算获得载体航向信息，采用非铁磁性金属作为外壳；基于UD分解的EKF滤波的运算步骤如下：步骤1：状态一步预测：式中：Xk,k‑1是系统的n×n维状态观测矩阵，是系统的状态转移矩阵，是状态估计；步骤2：UD分解：式中：Pk是估计方差矩阵，Pk,k‑1是误差方差矩阵，Γk,k‑1是n×p维噪声输入矩阵，Qk,k‑1是系统过程噪声方差阵；步骤3：滤波增益矩阵的求解：Fk=Dk,k-1Uk,k-1THkT]]> Gk＝Uk,k‑1Fk Sk＝HkGk+RkKk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1=GkSk-1;]]>步骤4：状态估计：步骤5：估计方差矩阵：Pk=(I-KkHk)Pk,k-1=Uk,k-1(Dk,k-1-FkSk-1FkT)Uk,k-1T;]]>组合导航系统AHRS部分模型修正方案的步骤如下所示：步骤1：采用三轴磁力计和三轴加速度计原始数据，计算出地理坐标系下的航向角，作为导航系统航向的观测信息；对电子罗盘的三轴原始数据进行倾斜补偿：hxhyhz=[Rpitch]T[Rroll]Tmxmymz=1000cosθsinθ0-sinθcosθTcosγ0-sinγ010sinγ0cosγTmxmymz;]]>航向角的计算如下所示：ψMag=arctanhyhx;]]>步骤2：组合导航系统姿态航向部分的模型构建：令Z轴MEMS陀螺的角速度输入值为陀螺零偏为εZ，陀螺噪声序列为nr,z，Z轴MEMS陀螺的模型如下所示：ωZ=ω^Z+ϵZ+nr,z;]]>令k时刻陀螺的零偏为εZ，k‑1时刻陀螺的零偏为零偏增量ΔεZ如下所示：ΔϵZ=ϵZ-ϵ^Z;]]>航向部分状态方程和观测方程如下所示：系统的状态量由航向角增量Δψ和ΔεZ构成，如下所示：X~yaw,k=ΔψΔϵZT;]]>令Z轴陀螺陀螺随机游走噪声序列为nωw，系统的噪声激励序列Wyaw如下所示：Wyaw＝[nωr nωw]T；系统的状态转移矩阵系统噪声驱动阵Γyaw如下所示：系统的观测量由电子罗盘航向角ψMag与上一时刻系统输出航向角ψ差值构成，如下所示：Z~yaw,k=(ψMag-ψ);]]>系统的观测转移矩阵Hyaw如下所示：Hyaw＝[1 0]；系统状态噪声方差阵Qyaw、量测噪声方差阵Ryaw如下所示：Qyaw=σZr2ΔtσZw2Δt2σZw2Δt2σZw2Δt,Ryaw=σMag2;]]>步骤3：将旧AHRS模型中的航向修正部分移除，只保留姿态部分，如下所示：Z~ahrs=ax-a^xay-a^yaz-a^zT]]>Rahrs=σacc2I3x3]]>步骤4：求取滤波结果后，通过四元数求取横滚和俯仰角，与修正后的航向角求取滤波后得到的四元数，完成本次滤波器观测更新。...

【技术特征摘要】
1.一种水下机器人双冗余姿态检测系统，由数据融合处理器、MEMS姿态传感器、电子罗盘、多普勒计程仪、深度计、高度计传感器构成，其特征在于：通过微控制器实现导航计算，输出姿态、速度、深度、高度信息，多普勒计程仪的输出接口接至数据融合处理器的输入接口，电子罗盘的输出接口接至数据融合处理器的输入接口，MEMS姿态传感器的输出接口接至数据融合处理器的输入接口，深度计的输出接口接至数据融合处理器的输入接口，高度计的输出接口接至数据融合处理器的输入接口，数据融合处理器的输出接口接至水下机器人中心控制器的输入接口；数据融合处理器是将多种传感器信息采集后，进行信息融合和处理，通过基于UD分解的EKF滤波和组合导航系统AHRS部分模型修正后，输出姿态、速度、深度、高度信息；MEMS姿态传感器是由三轴MEMS加速度计和三轴MEMS陀螺组成；电子罗盘采用地球磁场信息来确定北极指向的导航设备；电子罗盘采用传感器检测所在位置地磁场强度，经过计算获得载体航向信息，采用非铁磁性金属作为外壳；基于UD分解的EKF滤波的运算步骤如下：步骤1：状态一步预测：式中：Xk,k-1是系统的n×n维状态观测矩阵，是系统的状态转移矩阵，是状态估计；步骤2：UD分解：式中：Pk是估计方差矩阵，Pk,k-1是误差方差矩阵，Γk,k-1是n×p维噪声输入矩阵，Qk,k-1是系统过程噪声方差阵；步骤3：滤波增益矩阵的求解：Fk=Dk,k-1Uk,k-1THkT]]>Gk＝Uk,k-1FkSk＝HkGk+RkKk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1=GkSk-1;]]>步骤4：状态估计：步骤5：估计方差矩阵：Pk=(I-KkHk)Pk,k-1=Uk,k-1(Dk,k-1-FkSk-1FkT)Uk,k-1T;]]>组合导航系统AHRS部分模型修正方案的步骤如下所示：步骤1：采用三轴磁力计和三轴加速度计原始数据，计算出地理坐标系下的航向角，作为导航系统航向的观测信息；对电子罗盘的三轴原始数据进行倾斜补偿：hxhyhz=[Rpitch]T&lsqb...

【专利技术属性】
技术研发人员：魏延辉，高延滨，张皓渊，郭锐，姚贵鹏，洪国庆，乔金鹤，罗姗姗，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23