CN107816989A - 水下机器人航向数据处理方法和装置 - Google Patents

Abstract

本发明涉及一种水下机器人航向数据处理方法和装置，该方法包括：获取航向角度；通过角速度传感器获取角度偏移量；根据航向角度和角度偏移量计算获得偏移角度；通过磁场感应角度传感器获取磁场角度；检测偏移角度和磁场角度的差值是否大于预设阈值；当偏移角度和磁场角度的差值大于预设阈值时，以偏移角度为航向角度，返回通过角速度传感器获取角度偏移量的步骤。在偏移角度和磁场角度的差值大于预设阈值时，表明当前水下机器人受环境的磁场影响较大，则以偏移角度为航向角度，根据航向角度控制水下机器人航行，从而能够减小环境磁场对水下机器人的航向的影响，使得水下机器人的航向控制更为稳定，精度更高。

Description

水下机器人航向数据处理方法和装置

技术领域

本发明涉及水下检测机器人控制技术领域，特别是涉及水下机器人航向数据处理方法和装置。

背景技术

水下机器人也称无人遥控潜水器，是一种工作于水下的极限作业机器人。水下环境恶劣危险，人的潜水深度有限，所以水下机器人已成为进行水下作业的重要工具。

水下机器人航向是运动控制的重要参考量，现在主流的航向传感器是基于角速度传感器和磁传感器的卡尔曼数据融合的航向传感器。这样的航向传感器算法复杂，且价格昂贵。

此外，机器人在水下作业的过程中在接近坝体等水工建筑物时，周围的磁场会发生很大的变化，由于水下强磁场变化较为剧烈，在水下的多变和强磁场环境下使用这种航向传感器，将造成水下机器人的航向控制稳定性不佳，另一方面，一般的磁传感器数据更新频率比较低，通常更新频率为1Hz到5Hz，这样的数据更新周期不能满足控制***的实时性需求。

发明内容

基于此，有必要针对传统的水下机器人航向控制受磁场变化影响较大，造成航向控制稳定性不佳，精度不高，实时性较差的缺陷，提供一种水下机器人航向数据处理方法和装置。

一种水下机器人航向数据处理装置，包括：

获取航向角度；

通过角速度传感器获取角度偏移量；

根据所述航向角度和所述角度偏移量计算获得偏移角度；

通过所述磁场感应角度传感器获取磁场角度；

检测所述偏移角度和所述磁场角度的差值是否大于预设阈值；

当所述偏移角度和所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度，返回所述通过角速度传感器获取角度偏移量的步骤。

在其中一个实施例中，所述方法还包括：

当所述偏移角度和所述磁场角度的差值小于或等于所述预设阈值时，以所述磁场角度为所述航向角度，返回所述通过角速度传感器获取角度偏移量的步骤。

在其中一个实施例中，所述根据所述航向角度和所述角度偏移量计算获得偏移角度的步骤之后还包括：

根据所述偏移角度控制所述水下机器人航行。

在其中一个实施例中，所述通过角速度传感器获取角度偏移量的步骤为：

每间隔一个第一周期，通过所获角速度传感器获取一次所述角度偏移量。

在其中一个实施例中，所述根据所述航向角度和所述角度偏移量计算获得偏移角度的步骤为：

根据所述航向角度和多个所述角度偏移量计算获得偏移角度。

在其中一个实施例中，所述获取航向角度的步骤包括：

通过所述磁场感应角度传感器获取所述航向角度。

一种水下机器人航向数据处理装置，包括：磁场感应角度传感器、角速度传感器和航向控制器，所述磁场感应角度传感器以及所述角速度传感器均与所述航向控制器连接；

所述角速度传感器用于在每间隔一个第一周期获取一次角度偏移量；

所述磁场感应角度传感器用于在每间隔一个第二周期获取一次磁场角度；

所述航向控制器用于获取所述角速度传感器的角度偏移量以及所述磁场感应角度传感器的磁场角度，所述航向控制器还用于根据所述角度偏移量和一个第二周期获得的所述磁场角度计算获得偏移角度，在所述偏移角度和下一个第二周期的所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度。

在其中一个实施例中，所述第二周期大于所述第一周期。

在其中一个实施例中，所述磁场感应角度传感器包括电子罗盘。

在其中一个实施例中，所述角速度传感器包括光纤陀螺。

上述水下机器人航向数据处理方法和装置，在获取航向角度后，根据角速度传感器的角度偏移量对航向角度进行计算获得偏移角度，实时获得水下机器人的航向角度，通过磁场感应角度传感器获取磁场角度，在偏移角度和磁场角度的差值大于预设阈值时，表明当前水下机器人受环境的磁场影响较大，则以偏移角度为所述航向角度，根据航向角度控制水下机器人航行，从而能够减小环境磁场对水下机器人的航向的影响，使得水下机器人的航向控制更为稳定，精度更高。

附图说明

图1为一个实施例的水下机器人航向数据处理方法的流程示意图；

图2为一个实施例的水下机器人航向数据处理装置的模块示意图；

图3为一个实施例的水下机器人航向数据处理装置的各元件的工作周期示意图。

具体实施方式

为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的较佳实施方式。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施方式。相反地，提供这些实施方式的目的是使对本发明的公开内容理解的更加透彻全面。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

例如，一种水下机器人航向数据处理方法，包括：获取航向角度；通过角速度传感器获取角度偏移量；根据所述航向角度和所述角度偏移量计算获得偏移角度；通过所述磁场感应角度传感器获取磁场角度；检测所述偏移角度和所述磁场角度的差值是否大于预设阈值；当所述偏移角度和所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度，控制所述水下机器人航行，返回所述通过角速度传感器获取角度偏移量的步骤。

例如，一种水下机器人航向数据处理装置，包括：磁场感应角度传感器、角速度传感器和航向控制器，所述磁场感应角度传感器以及所述角速度传感器均与所述航向控制器连接；所述角速度传感器用于在每间隔一个第一周期获取一次角度偏移量；所述磁场感应角度传感器用于在每间隔一个第二周期获取一次磁场角度；所述航向控制器用于获取所述角速度传感器的角度偏移量以及所述磁场感应角度传感器的磁场角度，所述航向控制器还用于根据所述角度偏移量和一个第二周期获得的所述磁场角度计算获得偏移角度，在所述偏移角度和下一个第二周期的所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度，控制所述水下机器人航行。

本实施例中，在获取航向角度后，根据角速度传感器的角度偏移量对航向角度进行计算获得偏移角度，实时获得水下机器人的航向角度，通过磁场感应角度传感器获取磁场角度，在偏移角度和磁场角度的差值大于预设阈值时，表明当前水下机器人受环境的磁场影响较大，则以偏移角度为所述航向角度，根据航向角度控制水下机器人航行，从而能够减小环境磁场对水下机器人的航向的影响，使得水下机器人的航向控制更为稳定，精度更高。

具体地，该磁场感应角度传感器用于感应磁场的北极方向，从而获得水下机器人的角度，例如，所述磁场感应角度传感器包括电子罗盘。所述角速度传感器用于检测水下机器人的角速率和角偏差，例如，所述角速度传感器包括陀螺仪，例如，所述角速度传感器包括光纤陀螺。下面实施例中，将以电子罗盘和光纤陀螺为例子作进一步阐述。

在一个实施例中，如图1所示，提供一种水下机器人航向数据处理方法，包括：

步骤102，获取航向角度。

具体地，航向角度为水下机器人的当前航向的实际角度，航向控制器以航向角度为航行角度，根据该航向角度控制水下机器人航行。

值得一提的是，该航向角度可以是初始状态下，通过磁场感应角度传感器获取到的磁场角度，也可以是通过角速度传感器的角度偏移量对磁场角度进行纠正后的偏移角度。

步骤104，通过角速度传感器获取角度偏移量。

本实施例中，角速度传感器为光纤陀螺，光纤陀螺实时检测水下机器人的角速度，光纤陀螺对角速度的检测是根据水下机器人的自身运动状态获取的，而不是基于磁场获得的，因此光纤陀螺的角速度的获取不受磁场的影响。

本步骤中，通过角速度传感器检测水下机器人即时航向的角速度，通过时间积分获得角度的变化量，也就是角度偏移量，该角度偏移量为相对值，体现了水下机器人在一段时间内的航向的角度的变化量。

步骤106，根据所述航向角度和所述角度偏移量计算获得偏移角度。

本步骤中，在步骤102中获取的当前时间和角度偏移量计算获得水下机器人的偏移角度，即该偏移角度为根据水下机器人的角速度计算获得的水下机器人的实时的航向角度。可以理解的是，由于水下机器人持续在运动或者航行，其航向角度则将实时改变，而该偏移角度是根据水下机器人的实际角度和角度偏移量计算得出，则该偏移角度则为水下机器人当前的实际航向角度。

例如，根据航向角度与角度偏移量之和计算获得偏移角度。

值得一提的是，该偏移角度可以作为航向角度，航向控制器根据该偏移角度对水下机器人进行航向控制。

步骤108，通过所述磁场感应角度传感器获取磁场角度。

本实施例中，通过电子罗盘获取磁场角度，电子罗盘又称数字罗盘。该电子罗盘感应磁场，进而检测到磁场角度。例如，该电子罗盘以正北方向为零度，且获得的磁场角度范围为0～360°。电子罗盘根据磁场检测到水下机器人的角度，该角度即为磁场角度，且该磁场角度为绝对值。

步骤110，检测所述偏移角度和所述磁场角度的差值是否大于预设阈值。

本步骤中，将偏移角度和磁场角度进行对比，检测两者之间的差值是否大于预设阈值，当两者之间的差值大于预设阈值时，表明根据光纤陀螺计算出的实时航向角度与电子罗盘检测到的磁场角度之间偏差较大，表明当前电子罗盘受环境磁场影响较大。

值得一提的是，该环境磁场为水下环境中强磁物体发出的磁场，而并不是地球的磁场。

步骤112，当所述偏移角度和所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度，返回步骤104。

具体地，当偏移角度和磁场角度的差值大于所述预设阈值时，表明根据光纤陀螺计算出的实时航向角度与电子罗盘检测到的磁场角度的差值较大，表明当前电子罗盘受环境磁场影响较大，电子罗盘获取到的磁场角度不准确。因此，本步骤中，以光纤陀螺实时获取的角度偏移量计算得出偏移角度为航向角度，根据该航向角度控制所述水下机器人航行。

本实施例中，在获取航向角度后，根据角速度传感器的角度偏移量对航向角度进行计算获得偏移角度，实时获得水下机器人的航向角度，通过磁场感应角度传感器获取磁场角度，在偏移角度和磁场角度的差值大于预设阈值时，表明当前水下机器人受环境的磁场影响较大，则以偏移角度为所述航向角度，根据航向角度控制水下机器人航行，从而能够减小环境磁场对水下机器人的航向的影响，使得水下机器人的航向控制更为稳定，精度更高。

应该理解的是，各实施例中，根据航向角度控制水下机器人航行可以是直接控制水下机器人按照航向角度进行航行，也可以是以该航向角度为基准，根据控制指令控制水下机器人进行航行。具体地，航向控制器根据航向角度控制水下机器人航行，可通过控制X轴驱动器、Y轴驱动器以及Z轴驱动器的工作来进行，该航行控制方式可采用现有技术实现，本实施例中不累赘描述。

在一个实施例中，所述方法还包括：当所述偏移角度和所述磁场角度的差值小于或等于所述预设阈值时，以所述磁场角度为所述航向角度，返回所述通过角速度传感器获取角度偏移量的步骤。

具体地，当偏移角度和磁场角度之间的差值小于或等于预设阈值时，表明根据光纤陀螺计算出的实时航向角度与电子罗盘检测到的磁场角度较为接近，表明当前电子罗盘受环境磁场影响较小。因此，以所述磁场角度为所述航向角度，根据所述航向角度控制水下机器人航行。

值得一提的是，由于光纤陀螺检测获取的角度偏移量不受磁场影响，因此，能够根据该角度偏移量，实时计算得出精确的当前的航向角度，在较短时间内，由该光纤陀螺的角度偏移量计算得到的偏移角度较为精确，而随着时间的增加，对角速度积分获得的角度偏移量的误差将随之增大，因此，通过电子罗盘检测磁场角度，并以该电子罗盘检测到的磁场角度为航向角度，能够减小光纤陀螺在长时间对角度偏移量的检测误差。

上述实施例中，当偏移角度和磁场角度之间的差值小于或等于预设阈值时，以所述磁场角度为所述航向角度，当偏移角度和磁场角度之间的差值大于预设阈值时，以偏移角度为所述航向角度，也就是说，水下机器人在环境磁场影响较大的情况下，以光纤陀螺检测的角度偏移量对航向角度进行更新，而在环境磁场影响较小的情况下，电子罗盘检测到的磁场角度对航向角度进行更新，光纤陀螺的检测结果与电子罗盘的检测结果相互结合，能够有效减小环境磁场对水下机器人的航向角度的影响，且能够减小光纤陀螺长时间检测所带来偏差。

值得一提的是，上述实施例中的返回步骤可以视为循环，则一个循环可视为一个周期，在一个循环的周期内，通过电子罗盘获取一次磁场角度，而在一个循环的周期内，有多个小周期，每个小周期通过光纤陀螺获取一次角度偏移量，并且在若干个小周期后，根据角度偏移量对航向角度进行更新。

在一个实施例中，步骤106之后还包括：根据所述偏移角度控制所述水下机器人航行。

本实施例中，在光纤陀螺获取角度偏移量，并根据航向角度和角度偏移量计算获得偏移角度后，根据该偏移角度控制水下机器人航行，该偏移角度为当前实时的航向角度，这样，能够使得水下机器人能够根据实时的航向角度进行航行，能够有效提高水下机器人的航向控制精度。

在一个实施例中，步骤104为每间隔一个第一周期，通过所获角速度传感器获取一次所述角度偏移量。

具体地，光纤陀螺的对角度偏移量的更新周期为第一周期，例如，光纤陀螺每间隔一个第一周期检测一次角速度，并根据角速度对角度偏移量进行更新。例如，该光纤陀螺的角速度的更新周期为0.005秒，即该光纤陀螺的更新频率为200Hz，应该理解的是，光纤陀螺的更新频率越高，相邻两次获取得的角速度的值约为接近，因此，对水下机器人的角度偏移量的检测精度更高。

在一个实施例中，步骤106为：根据所述航向角度和多个所述角度偏移量计算获得偏移角度。

具体地，对偏移角度的计算周期大于第一周期，也就是说，在多个第一周期内分别获得角度偏移量后，达到一个计算周期时，根据航向角度和多个角度偏移量计算得出实时的航向角度，对航向角度进行更新，使得航向角度更为精确。

在一个实施例中，步骤102包括：通过所述磁场感应角度传感器获取所述航向角度。

具体地，初始的航向角度为电子罗盘检测到的磁场角度。例如，在水下机器人初始阶段，通过该电子罗盘检测水下机器人的初始的航向角度。

本实施例中，以该电子罗盘检测到的磁场角度为航向角度，例如，在水下机器人的初始化阶段，以该电子罗盘检测到的磁场角度为航向角度。

在一个实施例中，水下机器人航向数据处理方法还包括：检测偏移角度和磁场角度的差值大于预设阈值的次数，当偏移角度和磁场角度的差值大于预设阈值的次数大于预设次数时，增大所述预设阈值。例如，当偏移角度和磁场角度的差值大于预设阈值的连续出现的次数大于预设次数时，增大所述预设阈值，其中，偏移角度和磁场角度的差值大于预设阈值的连续出现的次数越大，所述预设阈值越大。

具体地，本实施例中，每当执行至执行步骤112后，则返回步骤104，这样会在多个循环周期内进行多次的偏移角度和磁场角度的差值与预设阈值的对比，当差值与预设阈值的对比的结果为偏移角度和磁场角度的差值大于预设阈值时，其该结果连续出现次数大于预设次数时，则对预设阈值进行调整，且该结果连续出现次数越多，预设阈值越大。具体地，电子罗盘获取的磁场角度和光纤陀螺积分获得的偏差角度之间差值所对比的预设阈值是动态调整的，该预设阈值根据受到环境磁场影响的电子罗盘的周期数进行动态调整，连续受到影响的周期数越多，设定的阈值越大。应该理解的是，受到磁场影响的周期越长，则光纤陀螺积分获得的累计偏差越大，如果航向控制器持续以光纤陀螺的偏移角度为航向角度，则使得光纤陀螺的积分的累计偏差不断增大，如果不及时对航向角度纠正，则使得误差越来越大，使得航向角度无法根据电子罗盘的磁场角度进行更新，因此，本实施例中，当检测偏移角度与磁场角度的差值大于预设阈值的的次数大于预设次数时，则增大预设阈值，使得在下一次的偏移角度与磁场角度差值与预设阈值进行对比时，能够使得偏移角度与磁场角度差值小于或等于预设阈值，这样，能够使得航向角度根据磁场角度进行更新，能够有效避免光纤陀螺的累计偏差。

在一个实施例中，提供一种水下机器人航向数据处理装置，包括：磁场感应角度传感器210、角速度传感器220和航向控制器230，所述磁场感应角度传感器210以及所述角速度传感器220均与所述航向控制器230连接。所述角速度传感器220用于在每间隔一个第一周期获取一次角度偏移量。所述磁场感应角度传感器210用于在每间隔一个第二周期获取一次磁场角度。

所述航向控制器230用于获取所述角速度传感器220的角度偏移量以及所述磁场感应角度传感器210的磁场角度，所述航向控制器230还用于根据所述角度偏移量和一个第二周期获得的所述磁场角度计算获得偏移角度，在所述偏移角度和下一个第二周期的所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度。

所述航向控制器230还用于在所述偏移角度和下一个第二周期的所述磁场角度的差值小于或等于所述预设阈值时，以所述磁场角度为所述航向角度。本实施例中，所述第二周期大于所述第一周期。

具体地，航向控制器根据所述航向角度控制水下机器人航行。例如，所述磁场感应角度传感器为电子罗盘，例如，所述角速度传感器为光纤陀螺。

本实施例中，水下机器人航向数据处理装置通过电子罗盘感应磁场从而获得磁场角度，根据该磁场角度对航向角度进行更行。该电子罗盘在每间隔一个第二周期获取一次磁场角度，例如，该第二周期为0.2秒，即该电子罗盘对磁场角度的获取频率为5Hz。

水下机器人航向数据处理装置通过光纤陀螺获得角度偏移量，根据角度偏移量对当前的航向角度进行更新。该第一周期为0.005秒，即该光纤陀螺获取角度偏移量的频率为200Hz。在一个第二周期内，可以在多个第一周期分别获取多个角度偏移量，并且根据多个角度偏移量对航向角度进行更新，比如，航向角度为这一个第二周期获得的磁场角度，则更新后的航向角度为磁场角度与角度偏移量之和，该磁场角度与角度偏移量之和即为偏移角度。

在下一个第二周期到来时，电子罗盘再次获取到磁场角度，则比较偏移角度与下一个第二周期获得的磁场角度之间的差值，当两者之间的差值大于预设阈值时，表明电子罗盘受环境磁场影响较大，导致电子罗盘的检测结果与光纤陀螺的检测结果之间的偏差较大，而由于第二周期时间较短，光纤陀螺在一个第二周期内持续获得的角度偏移量的累积误差较小，因此，由光纤陀螺的检测的角度偏移量计算获得的偏移角度较为精确，因此，航向控制器以该偏移角度为航向角度，则相当于对航向角度进行了更新，根据该航向角度控制水下机器人航行。而当偏移角度与下一个第二周期获得的磁场角度之间的差值小于或等于预设阈值时，表明电子罗盘受到环境磁场的影响较小，因此，则航向控制器以该电子罗盘获得的磁场角度为航向角度，则相当于对航向角度进行了更新，根据更新后的航向角度控制水下机器人航行。

值得一提的是，在一个第二周期内，光纤陀螺将在多个第一周期内分别获得多个角度偏移量，并根据多个角度偏移量分别计算获得多个偏移角度，在一个第二周期内，航线控制器分别以多个偏移角度为航向角度，即对航向角度进行更新。也就是说，在下一个第二周期没有来到之前，电子罗盘没有获得下一个磁场角度前，则根据偏移角度对水下机器人的航向角度进行微调，进而使得水下机器人的航行角度更为精确。

在一个实施例中，所述航向控制器还用于在偏移角度和下一个第二周期的磁场角度的差值大于所述预设阈值的连续次数大于预设次数时，增大所述预设阈值，其中，偏移角度和下一个第二周期的磁场角度的差值大于所述预设阈值的连续出现的次数越多，预设阈值越大。具体地，电子罗盘获取的磁场角度和光纤陀螺积分获得的偏差角度之间差值所对比的预设阈值是动态调整的，该预设阈值根据受到电磁影响的电子罗盘的周期数进行动态调整，连续受到影响的周期数越多，设定的阈值越大。应该理解的是，受到磁场影响的周期越长，则光纤陀螺积分获得的累计偏差越大，如果航向控制器持续以光纤陀螺的偏移角度为航向角度，则使得光纤陀螺的积分的累计偏差不断增大，如果不及时对航向角度纠正，则使得误差越来越大，使得航向角度无法根据电子罗盘的磁场角度进行更新，因此，本实施例中，当检测偏移角度与磁场角度的差值大于预设阈值的的次数大于预设次数时，则增大预设阈值，使得在下一次的偏移角度与磁场角度差值与预设阈值进行对比时，能够使得偏移角度与磁场角度差值小于或等于预设阈值，这样，能够使得航向角度根据磁场角度进行更新，能够有效避免光纤陀螺的累计偏差。

下面是一个具体实施例：

本实施例中，采用电子罗盘和光纤陀螺组合方式来对水下机器人的航向数据进行融合，获取稳定的水下机器人航向数据，电子罗盘和光纤陀螺的数据更新周期和水下机器人控制周期如图3所示，其中，电子罗盘更新周期为5Hz，光纤陀螺更新周期为200Hz，航向控制器的控制周期为50Hz。图3中T0至T1为电子罗盘的一个更新周期，T0至T1划分40个周期，即T01、T02、T03……T039，每一周期为光纤陀螺的更新周期，而每四个周期为航向控制器的一个控制周期。

电子罗盘：更新周期为5Hz，数据输出频率较慢，数据输出为当时的磁场角度，角度为绝对值，输出范围0-360°，正北方向为零度。该磁场角度只与所测量点的磁场环境有关，与使用时间无关，不会随着时间的推移产生偏差，长时间的稳定性较好，当该磁场角度受环境磁场影响较大，当接近强磁物体使得磁场角度会产生较大的偏差，瞬时稳定性不好。

光纤陀螺：更新周期为200Hz，数据输出频率较快，数据输出为当时的航向的角速度，通过对该角速度进行时间积分得到角度的变化量，也就是角度偏移量，该角度偏移量为相对值。该光纤陀螺的输出角度不受周围磁场的影响，瞬时稳定性较好，在对角速度进行积分获取角度偏移量的过程中会产生累积偏差，而累积偏差随着时间的增大而增大，因此，光纤陀螺数据的长期稳定性较差，其精度随着时间的增长而降低。

航向控制器的控制周期为50Hz，航向角度是航向控制器的重要反馈控制量，在每个控制周期内需要有新的航向角度进行更新，否则控制循环无效。

本实施例中通过电子罗盘和光纤陀螺相互结合，对航向角度进行更新，长期稳定性以电子罗盘为主，瞬时动态稳定性以光纤陀螺为主，主要完成以下两个功能：

(1)消除工作环境下的磁场对航向数据的干扰。

(2)在每个电子罗盘更新周期内，用光纤陀螺的数据对电子罗盘的更新数据进行插补，以满足控制***的要求。

基于电子罗盘和光纤陀螺的航向角度更新方法如下：

1、通过电子罗盘获取初始的磁场角度，以初始的磁场角度为航向角度。

初始化：在开阔无强磁环境下，获取当前的电子罗盘值为此时的航向角度的真值，以及以此值作为光纤陀螺的积分起点。

2、在一个第二周期内，在每一个第一周期通过光纤陀螺获取多个角度偏移量，根据初始的航向角度以及多个角度偏移量分别计算获得偏移角度，以该偏移角度为航向角度，对航向角度更新。

在每一个电子罗盘更新周期内，如图在T0到T1周期内，对每一个光纤陀螺更新时刻T01到T039的角速度进行积分得到相对角度d(T0m)，即偏移角度，则在每个光纤陀螺数据更新时刻航向的真值为D(T0m)＝D(T0)+d(T0m)；每四个光纤陀螺更新周期为一个控制周期，即D(T04)、D(T08)、D(T12)…为航向控制时刻，以光纤陀螺的更新时刻来同步控制时刻，航向控制器在每一个控制周期以更新后的航向角度控制水下机器人航行。

3、在每一个第二周期到来时，根据电子罗盘的磁场角度以及光纤陀螺的偏移角度对航向角度进行更新。

在每个电子罗盘更新时刻进行电子罗盘和光纤陀螺数据相互修正：在此时刻得到两个航向角度的值，一个是电子罗盘读取的数据(磁场角度)，另一个是光纤陀螺基于上一个电子罗盘更新时刻的积分角度(偏移角度)。如果两个航向角度之间的偏差在设定的阈值之内，则认为此时电子罗盘没有受到强磁场的影响，此时以电子罗盘的磁场角度作为该时刻的航向角度的真值。如果两个航向角数据之间的偏差超过设定的阈值，则认为此时电子罗盘受到强磁场的影响，此时以光纤陀螺积分得到的偏移角度作为该时刻的航向角度的真值。无论是以电子罗盘的磁场角度作为该时刻的航向角度的真值，还是以光纤陀螺积分得到的偏移角度作为该时刻的航向角度的真值，下一个第二周期内光纤陀螺的积分以此时刻的航向角度的真值为基准进行积分，即下一个第二周期内光纤陀螺的积分更新后的航向角度为基准进行积分。

通过电子罗盘和光纤陀螺结合对航向角度进行更新，使得航向角度更为精确，能够有效避免环境磁场对航向角度的影响，此外还避免了光纤陀螺长时间对一个航向角度的积分造成的累计偏差偏大的情况。

本实施例中，罗盘值和光纤陀螺积分值偏差的设定阈值根据受到电磁影响的电子罗盘周期数进行动态调整，连续受到影响的周期数越多，设定的阈值越大。

本实施例中，光纤陀螺的积分仅在一个电子罗盘的更新周期内进行，使得光纤陀螺的累计偏差控制在较小范围内，可以保证积分值的准确性。

此外，在水下机器人作业的过程中，受到强磁场环境影响时间不会太长，大部分时间是正常的，采用电子罗盘的值保证了航向数据的长时间稳定性。

应该说明的是，上述***实施例中，所包括的各个模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种水下机器人航向数据处理方法，其特征在于，包括：

获取航向角度；

通过角速度传感器获取角度偏移量；

根据所述航向角度和所述角度偏移量计算获得偏移角度；

通过所述磁场感应角度传感器获取磁场角度；

检测所述偏移角度和所述磁场角度的差值是否大于预设阈值；

当所述偏移角度和所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度，返回所述通过角速度传感器获取角度偏移量的步骤。

2.根据权利要求1所述的水下机器人航向数据处理方法，其特征在于，所述方法还包括：

当所述偏移角度和所述磁场角度的差值小于或等于所述预设阈值时，以所述磁场角度为所述航向角度，返回所述通过角速度传感器获取角度偏移量的步骤。

3.根据权利要求1所述的水下机器人航向数据处理方法，其特征在于，所述根据所述航向角度和所述角度偏移量计算获得偏移角度的步骤之后还包括：

根据所述偏移角度控制所述水下机器人航行。

4.根据权利要求1所述的水下机器人航向数据处理方法，其特征在于，所述通过角速度传感器获取角度偏移量的步骤为：

每间隔一个第一周期，通过所获角速度传感器获取一次所述角度偏移量。

5.根据权利要求4所述的水下机器人航向数据处理方法，其特征在于，所述根据所述航向角度和所述角度偏移量计算获得偏移角度的步骤为：

根据所述航向角度和多个所述角度偏移量计算获得偏移角度。

6.根据权利要求1所述的水下机器人航向数据处理方法，其特征在于，所述获取航向角度的步骤包括：

通过所述磁场感应角度传感器获取所述航向角度。

7.一种水下机器人航向数据处理装置，其特征在于，包括：磁场感应角度传感器、角速度传感器和航向控制器，所述磁场感应角度传感器以及所述角速度传感器均与所述航向控制器连接；

所述角速度传感器用于在每间隔一个第一周期获取一次角度偏移量；

所述磁场感应角度传感器用于在每间隔一个第二周期获取一次磁场角度；

所述航向控制器用于获取所述角速度传感器的角度偏移量以及所述磁场感应角度传感器的磁场角度，所述航向控制器还用于根据所述角度偏移量和一个第二周期获得的所述磁场角度计算获得偏移角度，在所述偏移角度和下一个第二周期的所述磁场角度的差值大于所述预设阈值时，以所述偏移角度为所述航向角度。

8.根据权利要求7所述的水下机器人航向数据处理装置，其特征在于，所述第二周期大于所述第一周期。

9.根据权利要求7所述的水下机器人航向数据处理装置，其特征在于，所述磁场感应角度传感器包括电子罗盘。

10.根据权利要求7所述的水下机器人航向数据处理装置，其特征在于，所述角速度传感器包括光纤陀螺。