CN108803673A - 定向控制方法和装置 - Google Patents

Abstract

本申请公开了一种定向控制方法和装置。所述方法包括在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。本申请解决了水下机器人的定向方向经常发生错误，需要多次尝试调整定向方向，才能到达目标位置，降低了水下机器人的工作效率的技术问题。

Description

定向控制方法和装置

技术领域

本申请涉及机器人技术领域，具体而言，涉及一种定向控制方法和装置。

背景技术

水下机器人也称无人遥控潜水器，是一种工作于水下的极限作业机器人。水下环境恶劣危险，人的潜水深度有限，所以水下机器人已成为开发海洋的重要工具。因为，水下机器人需要在水下广泛的区域内进行搜索、取样等工作，因此，工作时需要对水下机器人的定向控制，以使水下机器人在水下广泛的区域内运行至目标位置。

现有技术中，水下机器人的定向控制方法，通过本端建立的机器坐标系获取本端的实时姿态数据，再根据目标位置确定定向方向，水下机器人沿着定向方向运行到达目标位置。

在实施本申请实施例的过程中，发明人发现现有技术至少存在以下问题：

现有技术中，获取实时姿态数据都是基于自身建立的机器坐标系，水下机器人的实时姿态的参照数据单一，并不能准确地反映水下机器人的实时姿态，导致水下机器人的定向方向经常发生错误，需要多次尝试调整定向方向，才能到达目标位置，降低了水下机器人的工作效率。

发明内容

本申请的主要目的在于提供一种定向控制方法和装置，以解决水下机器人的定向方向经常发生错误，需要多次尝试调整定向方向，才能到达目标位置，降低了水下机器人的工作效率的问题。

为了实现上述目的，第一方面，本申请实施例提供的一种定向控制方法，用于对水下机器人的运行数据进行数据处理，本定向控制方法包括：

在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；

对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；

根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。

可选地，对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据，包括：

对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第三姿态数据；

获取本端的参考角度数据，并根据参考角度数据对第三姿态数据进行矫正，得到第二姿态数据。

可选地，获取本端的参考角度数据，包括：

通过磁力计得到本端相对大地磁场北的参考角度数据。

可选地，生成因子为本端所受重力时，获取本端的至少两个第一姿态数据包括：

建立大地坐标系和本端的机器坐标系；

根据本端所受重力在大地坐标系中的分量以及在机器坐标系中的分量，获取生成因子为本端所受重力的第一姿态数据。

可选地，生成因子为本端的实时角速度时，获取本端的至少两个第一姿态数据包括：

通过陀螺仪获取本端的实时角速度，得到测算数据；

对测算数据进行数据转换，得到生成因子为实时角速度的第一姿态数据。

第二方面，本申请实施例提供的一种定向控制装置，用于对水下机器人的运行数据进行数据处理，本定向控制装置包括：

姿态获取模块，用于在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；

数据处理模块，用于对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；

定向模块，用于根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。

可选地，数据处理模块，用于：

对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第三姿态数据；

获取本端的参考角度数据，并根据参考角度数据对第三姿态数据进行矫正，得到第二姿态数据。

可选地，数据处理模块，用于：

通过磁力计得到本端相对大地磁场北的参考角度数据。

可选地，生成因子为本端所受重力时，姿态获取模块，用于：

建立大地坐标系和本端的机器坐标系；

根据本端所受重力在大地坐标系中的分量以及在机器坐标系中的分量，获取生成因子为本端所受重力的第一姿态数据。

可选地，生成因子为本端的实时角速度时，姿态获取模块，用于：

通过陀螺仪获取本端的实时角速度，得到测算数据；

对测算数据进行数据转换，得到生成因子为实时角速度的第一姿态数据。

在本申请实施例中提供的定向控制方法，通过在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。这样，通过引入不同的生成因子，得到至少两个第一姿态数据，再将全部的第一姿态数据进行数据融合，从而提高了获取的水下机器人的实时姿态数据的准确率，实现了更加精准地对水下机器人的运行方向进行定向控制的技术效果，进而解决了水下机器人的定向方向经常发生错误，需要多次尝试调整定向方向，才能到达目标位置，降低了水下机器人的工作效率的技术问题。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，使得本申请的其它特征、目的和优点变得更明显。本申请的示意性实施例附图及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是根据本申请实施例的一种定向控制方法的流程图；

图2是根据本申请实施例的一种步骤S200的流程图；

图3是根据本申请实施例的一种步骤S100的流程图；

图4是根据本申请实施例的一种步骤S100的流程图；

图5是根据本申请实施例的一种定向控制装置的结构示意图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

本申请实施例提供了一种定向控制方法，如图1所示，该方法包括如下的步骤S100和步骤S300：

S100，在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同。

在本实施例中，可以通过多个感应装置获取本端的每个第一姿态数据对应的生成因子，并根据每个生成因子得到每个第一姿态数据。其中，该第一姿态数据包括本端对应的滚动角、俯仰角和偏航角中的任意一种或多种。另外，多个感应装置至少包括重力加速度计、三轴加速度传感器、陀螺仪、磁力计或者其他可以用于感应角度、角速度、重力、惯性等属性的装置。

在实施中，因为每个生成因子不同，因此获取的第一姿态数据各不相同，获取的第一姿态数据更具多样性。

S200，对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据。

在本实施例中，对全部的第一姿态数据卡尔曼滤波进行数据融合处理，可以将多个第一姿态数据融合形成一个更为精准地第二姿态数据。其中，卡尔曼滤波是一个最优化自回归数据处理算法，通过平均加权处理，能够将多组数据计算得到一组最准确的数据。这样，通过至少两种上述姿态数据进行卡尔曼滤波数据融合，得到的实时姿态数据更为准确，解决了目前实时姿态数据由单一数据决定，导致实时姿态数据不准确的问题，例如，现有技术中通过陀螺仪获取的实时姿态数据，会受到许多因素干扰(例如噪声)，导致单一由陀螺仪获取的实时姿态数据不准确。此外，该第二姿态数据至少包括本端对应的滚动角、俯仰角和偏航角中的任意一种或多种。

S300，根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。

在本实施例中，通过目标位置和本端位置可以确定定向方向数据，根据定向方向数据与第二姿态数据确定定向方向与本端当前运行方向之间的角度，该角度数据即为定向转动数据。

在实施中，通过引入不同的生成因子，得到至少两个第一姿态数据，再将全部的第一姿态数据进行数据融合，从而提高了获取的水下机器人的实时姿态数据的准确率，实现了更加精准地对水下机器人的运行方向进行定向控制的技术效果，使得水下机器人的定向方向准确，降低了水下机器人调整定向方向的次数，能够尽快到达目标位置，提高了水下机器人的工作效率。

如图2所示，可选地，步骤S200，对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据，包括如下的步骤S210和步骤S220：

S210，对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第三姿态数据。

在本实施例中，对全部的第一姿态数据卡尔曼滤波进行数据融合处理，可以将多个第一姿态数据融合形成一个更为精准地第三姿态数据。其中，该第三姿态数据至少包括本端对应的滚动角、俯仰角和偏航角中的任意一种或多种。

S220，获取本端的参考角度数据，并根据参考角度数据对第三姿态数据进行矫正，得到第二姿态数据。

在本实施例中，采用感应装置可以得到本端相对大地磁场一预设方向的角度，将该角度作为参考角度，通过参考角度对应第三实时姿态数据包括的数据进行矫正，具体地，通过参考角度对第四实时姿态数据包括的每个轴向上的角度进行矫正。

可选地，获取本端的参考角度数据，包括：

通过磁力计得到本端相对大地磁场北的参考角度数据。

在本实施例中，采用磁力计检测出本端相对大地磁场北的参考角度数据，其中，以大地磁场北作为本端相对大地磁场一预设方向。

如图3所示，可选地，生成因子为本端所受重力时，步骤S100，获取本端的至少两个第一姿态数据包括如下的步骤S110和步骤S120：

S110，建立大地坐标系和本端的机器坐标系。

在本实施例中，根据实际参照需要设置一个大地坐标系，再根据本端的外形结构或者功能结构位置设置本端的机器坐标系。其中，大地坐标系是大地测量中以参考椭球面为基准面建立起来的坐标系，地面点的位置用大地经度、大地纬度和大地高度表示，大地坐标系的确立包括选择一个椭球、对椭球进行定位和确定大地起算数据。

S120，根据本端所受重力在大地坐标系中的分量以及在机器坐标系中的分量，获取生成因子为本端所受重力的第一姿态数据。

在本实施例中，通过重力传感器或者加速度计获取本端实时所受的重力，根据根据机器坐标系对本端实时所受的重力进行分解得到本端所受重力在机器坐标系中的分量，再根据本端实时所受的重力指向大地坐标系中的原点，生成本端所受重力在大地坐标系中的分量，对这两组分量进行数据转换，可以得到机器坐标系与大地坐标系的对应关系，从而得到本端相对于大地坐标系的第一姿态数据。

如图4所示，可选地，生成因子为本端的实时角速度时，步骤S100，获取本端的至少两个第一姿态数据包括如下的步骤S130和步骤S140：

S130，通过陀螺仪获取本端的实时角速度，得到测算数据。

在本实施例中，通过陀螺仪获取本端的实时角速度，再根据机器坐标系对实时角速度进行分解，得到该实时角速度对应机器坐标系的各轴向上的分量，再对每个分量进行积分，得到对应机器坐标系的各轴向上的角度，其中，测算数据包括对应机器坐标系的各轴向上的角度。

步骤S140，对测算数据进行数据转换，得到生成因子为实时角速度的第一姿态数据。

在本实施例中，基于陀螺仪的数据转换方式，对测算数据进行数据转换，将对应机器坐标系的各轴向上的角度换算为对应大地坐标系的各轴向上的角度，该各轴向上的角度即为对应生成因子为本端的实时角速度的第一实时姿态数据。

在本申请实施例中提供的定向控制方法，通过S100，在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；S200，对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；S300，根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。这样，通过引入不同的生成因子，得到至少两个第一姿态数据，再将全部的第一姿态数据进行数据融合，从而提高了获取的水下机器人的实时姿态数据的准确率，实现了更加精准地对水下机器人的运行方向进行定向控制的技术效果，进而解决了水下机器人的定向方向经常发生错误，需要多次尝试调整定向方向，才能到达目标位置，降低了水下机器人的工作效率的技术问题。

基于与上述定向控制方法相同的技术构思，本申请实施例还提供了一种定向控制装置，用于对水下机器人的运行数据进行数据处理，如图5所示，本定向控制装置包括：

姿态获取模块10，用于在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；

数据处理模块20，用于对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；

定向模块30，用于根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。

可选地，数据处理模块20，用于：

对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第三姿态数据；

获取本端的参考角度数据，并根据参考角度数据对第三姿态数据进行矫正，得到第二姿态数据。

可选地，数据处理模块20，用于：

通过磁力计得到本端相对大地磁场北的参考角度数据。

可选地，生成因子为本端所受重力时，姿态获取模块10，用于：

建立大地坐标系和本端的机器坐标系；

根据本端所受重力在大地坐标系中的分量以及在机器坐标系中的分量，获取生成因子为本端所受重力的第一姿态数据。

可选地，生成因子为本端的实时角速度时，姿态获取模块10，用于：

通过陀螺仪获取本端的实时角速度，得到测算数据；

对测算数据进行数据转换，得到生成因子为实时角速度的第一姿态数据。

在本申请实施例中提供的定向控制方法，通过姿态获取模块10，用于在同一时刻，获取本端的至少两个第一姿态数据，其中，至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；数据处理模块20，用于对本端全部的第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；定向模块30，用于根据目标位置和本端位置确定定向方向数据，并根据定向方向数据与第二姿态数据确定定向转动数据。这样，通过引入不同的生成因子，得到至少两个第一姿态数据，再将全部的第一姿态数据进行数据融合，从而提高了获取的水下机器人的实时姿态数据的准确率，实现了更加精准地对水下机器人的运行方向进行定向控制的技术效果，进而解决了水下机器人的定向方向经常发生错误，需要多次尝试调整定向方向，才能到达目标位置，降低了水下机器人的工作效率的技术问题。

显然，本领域的技术人员应该明白，上述的本申请的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本申请不限制于任何特定的硬件和软件结合。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种定向控制方法，其特征在于，用于对水下机器人的运行数据进行数据处理，所述方法包括：

在同一时刻，获取本端的至少两个第一姿态数据，其中，所述至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；

对本端全部的所述第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；

根据目标位置和本端位置确定定向方向数据，并根据所述定向方向数据与所述第二姿态数据确定定向转动数据。

2.根据权利要求1所述的定向控制方法，其特征在于，所述对本端全部的所述第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据，包括：

对本端全部的所述第一姿态数据进行卡尔曼滤波处理，得到本端的第三姿态数据；

获取本端的参考角度数据，并根据所述参考角度数据对所述第三姿态数据进行矫正，得到第二姿态数据。

3.根据权利要求1所述的定向控制方法，其特征在于，所述获取本端的参考角度数据，包括：

通过磁力计得到本端相对大地磁场北的所述参考角度数据。

4.根据权利要求1所述的定向控制方法，其特征在于，所述生成因子为本端所受重力时，所述获取本端的至少两个第一姿态数据包括：

建立大地坐标系和本端的机器坐标系；

根据本端所受重力在所述大地坐标系中的分量以及在所述机器坐标系中的分量，获取生成因子为本端所受重力的第一姿态数据。

5.根据权利要求1所述的定向控制方法，其特征在于，所述生成因子为本端的实时角速度时，所述获取本端的至少两个第一姿态数据包括：

通过陀螺仪获取本端的实时角速度，得到测算数据；

对所述测算数据进行数据转换，得到生成因子为实时角速度的第一姿态数据。

6.一种定向控制装置，其特征在于，用于对水下机器人的运行数据进行数据处理，所述装置包括：

姿态获取模块，用于在同一时刻，获取本端的至少两个第一姿态数据，其中，所述至少两个第一姿态数据中每个第一姿态数据对应的生成因子均不同；

数据处理模块，用于对本端全部的所述第一姿态数据进行卡尔曼滤波处理，得到本端的第二姿态数据；

定向模块，用于根据目标位置和本端位置确定定向方向数据，并根据所述定向方向数据与所述第二姿态数据确定定向转动数据。

7.根据权利要求6所述的定向控制装置，其特征在于，所述数据处理模块，用于：

对本端全部的所述第一姿态数据进行卡尔曼滤波处理，得到本端的第三姿态数据；

获取本端的参考角度数据，并根据所述参考角度数据对所述第三姿态数据进行矫正，得到第二姿态数据。

8.根据权利要求6所述的定向控制装置，其特征在于，所述数据处理模块，用于：

通过磁力计得到本端相对大地磁场北的所述参考角度数据。

9.根据权利要求6所述的定向控制装置，其特征在于，所述生成因子为本端所受重力时，所述姿态获取模块，用于：

建立大地坐标系和本端的机器坐标系；

根据本端所受重力在所述大地坐标系中的分量以及在所述机器坐标系中的分量，获取生成因子为本端所受重力的第一姿态数据。

10.根据权利要求6所述的定向控制装置，其特征在于，所述生成因子为本端的实时角速度时，所述姿态获取模块，用于：

通过陀螺仪获取本端的实时角速度，得到测算数据；

对所述测算数据进行数据转换，得到生成因子为实时角速度的第一姿态数据。