CN107807375A - 一种基于多gps接收机的无人机姿态追踪方法及系统 - Google Patents

Abstract

本发明提出了一种基于多GPS接收机的无人机姿态追踪方法，首先，确定各个GPS接收机在ENU坐标系下的初始化位置；当无人机在飞行过程中姿态发生变化时，采用RTK定位的方法，实时计算各个GPS接收机在ECEF坐标系下的坐标位置；然后，将各GPS接收机在ECEF坐标系下的位置转化为ENU坐标系下的位置信息；最后，将得到的位置信息与初始化位置进行比较、计算，得出GPS接收机在ENU坐标系下绕各个轴向旋转的角度。本发明还提出一种的无人机姿态追踪系统。本发明通过比较当前位置姿态与初始位置姿态，能够直接得到角度的变化量，计算结果更加准确，实现无人机姿态的精确追踪。

Description

一种基于多GPS接收机的无人机姿态追踪方法及系统

技术领域

本发明涉及一种无人机姿态追踪方法及系统，属于无人机技术研究领域。

背景技术

随着科技的进步，近几年无人机技术得到了飞速的发展，而惯性测量单元IMU是无人机技术当中重要的一环，它能够测量出无人机三轴姿态角以及加速度的装置。陀螺仪是IMU的主要元件之一，其精度直接影响到惯性系统的精度。

为何陀螺仪会成为无人机姿态导航的首选传感器呢？陀螺仪是利用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一轴或多轴的角运动检测装置。它可感测一轴或多轴的旋转角度，可精准感测自由空间中的复杂移动动作，因此，陀螺仪成为追踪物体移动方位与旋转动作的必要运动传感器，如手机、可穿戴设备、潜艇、飞机、导弹和航天器的惯性导航设备等。

然而，像潜艇、飞机、导弹和航天器等工业级别的设备上，大多采用工业级别的、精度更高的陀螺仪，比如说光纤陀螺仪、环形激光陀螺仪、工业级MEMS陀螺仪，而面对消费级无人机，大多采用普通的陀螺仪。在无人机实际的飞行过程中，普通的陀螺仪容易受各种不可避免的因素干扰，比如外界温度、瞬间大电流、电机电磁干扰等，而导致陀螺仪产生误差，从初始对准开始，其导航误差就随时间而增长，会导致IMU中陀螺仪的精度下降甚至失效，这是惯导系统的主要缺点。尤其是姿态位置误差对无人机来说往往都是致命的，会直接导致无人机炸机。

针对以上的问题，最好的解决办法是利用外部信息进行辅助，实现组合导航，使其有效地减小误差。目前各大无人机厂商大多采用冗余设计来解决，即在飞控上搭载多组IMU。欧洲的3DRobotics公司的pixhawk飞控上搭载了两组IMU；国内CUAV雷迅科技的pixhawk 2,4x飞控上搭载了两组IMU，pixhawkv3飞控上搭载了三组IMU；国内大疆的精灵4的飞控上搭载了两组IMU，A3pro飞控上搭载了三组IMU。飞控系统采用软件算法对各组IMU数据进行判断，当一组IMU数据出现异常时，飞控系统立即切换至另外一组IMU上，以保障无人机可靠的、稳定的飞行。

IMU的冗余设计在一定程度上保障了无人机的正常飞行，但是多组IMU的设计一方面增加了硬件的开销，另一方面增加了软件设计的复杂度。

发明内容

本发明所要解决的技术问题是：针对无人机飞行过程中，由于温度、电流、磁场等因素的影响所带来的惯性测量单元IMU精度低甚至失效的问题，提供了一种无人机姿态追踪的方法及系统，解决无人机内部惯性测量单元IMU受到外部干扰后精度降低或失效的问题。

本发明为解决上述技术问题采用以下技术方案：

一种基于多GPS接收机的无人机姿态追踪方法，包括如下步骤：

S1、在无人机的机臂上，以每个机臂为单位分别安装GPS接收机，并以其中一个GPS接收机为原点，建立ENU坐标系；

S2、将无人机水平放置，确定各个GPS接收机在ENU坐标系下的初始化位置；

S3、当无人机在飞行过程中姿态发生变化时，实时计算出各个GPS接收机在ECEF坐标系下的坐标位置；

S4、将步骤S3中得到的各个GPS接收机在ECEF坐标系下的坐标位置，转化为各个GPS接收机在ENU坐标系下的位置信息；

S5、通过比较步骤S4和步骤S2中各个GPS接收机的位置变化，计算出无人机各个轴向的旋转角度。

进一步的，本发明的无人机姿态追踪方法，在步骤S2中，将作为原点的GPS接收机的坐标位置设为(0,0,0)，将各个GPS接收机在ENU坐标系下相对于原点GPS接收机的坐标位置，作为各个GPS接收机在ENU坐标系下的初始化的位置。

进一步的，本发明的无人机姿态追踪方法，步骤S3是采用RTK定位算法实时计算出各个GPS接收机在ECEF坐标系下的坐标位置。

进一步的，本发明的无人机姿态追踪方法，在步骤S4中是应用三维坐标系旋转、空间向量运算的理论，将各个GPS接收机在ECEF坐标系下的坐标位置转化为在ENU坐标系下的位置信息，具体是：

首先进行三维坐标系的旋转，使得旋转后的ECEF坐标系转化为以地球地心为原点的ENU坐标系；然后，再应用空间向量运算，得到位置发生变化的GPS接收机在ENU坐标系下的位置信息。

进一步的，本发明的无人机姿态追踪方法，在步骤S5中，通过比较位置发生变化后的GPS接收机的位置信息与初始化位置信息，根据坐标系旋转变换的理论，计算得出GPS接收机在ENU坐标系下绕各个轴向旋转的角度，即无人机横滚、俯仰、偏航的角度变化。

本发明还提出一种基于多GPS接收机的无人机姿态追踪系统，包括：安装在无人机的各个机臂上GPS接收机，以及姿态追踪模块；其中，姿态追踪模块包括：

初始位置计算单元，用于将一个GPS接收机作为原点接收机，计算其余GPS接收机在ENU坐标系下相对于原点GPS接收机的坐标位置，得到各个GPS接收机在ENU坐标系下的初始化位置；

实时姿态计算单元，用于无人机在飞行过程中姿态发生变化时，实时计算出各个GPS接收机在ECEF坐标系下的坐标位置；

坐标信息转换单元，用于将实时姿态计算单元得到的各个GPS接收机在ECEF坐标系下的坐标位置，应用三维坐标系旋转、空间向量运算的理论，转化为各个GPS接收机在ENU坐标系下的位置信息；

位置变化计算单元，用于通过比较各GPS接收机在ENU坐标系下的实时位置与初始化位置，计算出无人机各个轴向的旋转角度。

本发明采用以上技术方案，在不采用传统的惯性传感器的情况下，摆脱了温度、电流、磁场等因素的干扰，提高了无人机的安全可靠性，与现有技术相比，具有以下技术效果：

(1)本系统在建立坐标系的时候直接采用ENU坐标系，与无人机采用相同的坐标系，优点在于方便后期与飞控数据的融合。

(2)陀螺仪是在测得角速度大小的基础上，通过对时间的积分得到角度的变化量，存在累积误差，而本发明的方法是通过比较当前位置姿态与初始位置姿态，能够直接得到角度的变化量，计算结果更加准确。

(3)本发明的使用是在RTK精准定位的基础上实现的，定位精度属于厘米级别甚至毫米级别，转化为轴向旋转的精度大约为2度，与陀螺仪的精度相当，但是不存在外部环境的干扰情况，也不存在累积的误差，更加的安全可靠。

(4)本发明不仅可以为无人机提供高精度的姿态追踪方法，还可以为无人机提供实时的高精度定位结果。

附图说明

图1是本发明方法的原理图。

图2是本发明方法流程图。

图3是本发明方法应用场景示意图。

图4是ECEF坐标系和ENU坐标系关系图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

ENU：站心坐标系，指用户所在位置点P为坐标原点，三个坐标轴分别是相互垂直的东向、北向和天向；比如以站心(如GPS接收天线中心)为坐标系原点O，Z轴与椭球法线重合，向上为正(天向)，y轴与椭球短半轴重合(北向)，x轴与地球椭球的长半轴重合(东向)所构成的直角坐标系，又称为当地东北天坐标系(ENU)。

ECEF(Earth-Centered，Earth-Fixed)，是一个笛卡尔坐标系，以地球为中心，(0，0，0)表示地球的质心(Earth-Centered)，它的坐标轴以国际参考极点和国际参考子午线为准，遵照地表所确定(Earth-Fixed)。

本发明以四轴X型无人机为例，但不局限于四轴无人机，任何多旋翼、固定翼无人机均可适用。

参考图2所示，本发明的方法流程举例说明如下：

步骤1)：如图3所示，在四旋翼无人机的各个机臂上，分别安装GPS接收机，然后按照图1所示的原理图，建立ENU坐标系，分别命名各个GPS接收机为GPS1、GPS2、GPS3、GPS4，参考图1中的(a)、(b)所示。

步骤2)：建立了ENU坐标系后，每个的GPS接收机的坐标便可表示出来：GPS1(0,0,0)、GPS2(L1，0，0)、GPS3(0，L2，0)、GPS4(L1，L2，0)。为方便计算，使用空间向量进行表示，以GPS1为起始点，方向分别指向其他GPS接收机，则GPS1指向GPS2的向量表示为(L1，0，0)，GPS1指向GPS3的向量表示为(0，L2，0)，GPS1指向GPS4的向量表示为(L1，L2，0)，并把这三个向量作为本系统的初始化位置P0：

其中L1、L2分别是东向、北向接收机距离原点接收机的距离。

步骤3)：在无人机飞行的过程中，无人机的姿态不断地发生着变化，如：横滚、俯仰、偏航角度不断变化。此步骤属于实时的检测阶段，采用RTK定位算法，实时计算出各个GPS接收机在ECEF坐标系下的精准位置：GPS1(x₁，y₁，z₁)、GPS2(x₂，y₂，z₂)、GPS3(x₃，y₃，z₃)、GPS4(x₄，y₄，z₄)。

RTK(Real-time kinematic，载波相位差分技术)，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标，这是一种常用的GPS测量方法，本发明在此不再赘述。

步骤4)：将上一步骤的各个GPS接收机的ECEF坐标，应用三维坐标系旋转、空间向量运算的理论，从而求解出变化后各个GPS接收机在ENU坐标系下的坐标信息P。

其中，ECEF坐标系和ENU坐标系的关系如图4所示。

首先进行坐标系的旋转，使得旋转后的ECEF坐标系转化为以地球地心为原点的ENU坐标系：

其中：是过GPS接收机的基准椭球面法线与赤道面的夹角，λ是过GPS接收机所在的子午面与本初子午面之间的夹角。

然后，再应用空间向量的知识，得到位置发生变化的GPS接收机在ENU坐标系下的位置P。详细说明就是，ECEF坐标系转化为ENU坐标系后，是以地心为坐标原点的，而GPS接收机的ENU坐标系是以GPS1为坐标原点的，虽然说是两个ENU坐标系，但是通过空间向量计算的结果是一致的，例如，向量(GPS1，GPS2)是通过GPS2的坐标减去GPS1的坐标得到的，而其值在两个ENU坐标系下结果相同。也就是通过以地心为原点的ENU坐标系下的向量位置P就等于以GPS1接收机为原点的ENU坐标系下的位置P，二者相等，求出前者即可。

步骤5)：通过比较变化后的GPS接收机的位置信息P与初始化时刻的位置信息P0，根据坐标系旋转变换的理论，见如下公式，计算得出GPS接收机在ENU坐标系下绕各个轴向旋转的角度α、β、θ，即无人机横滚、俯仰、偏航的角度变化。

P＝R3(θ)·R2(β)·R1(α)·P0；

其中，

绕E轴旋转α；

绕N轴旋转β；

绕U轴旋转θ。

本发明在不失精度的情况下，不采用传统的惯性传感器，摆脱了温度、电流、磁场等因素的干扰，提高了无人机的安全可靠性。

本发明还提出一种基于多GPS接收机的无人机姿态追踪系统，包括：安装在无人机的各个机臂上GPS接收机，以及姿态追踪模块；其中，姿态追踪模块包括：

初始位置计算单元，用于将一个GPS接收机作为原点接收机，计算其余GPS接收机在ENU坐标系下相对于原点GPS接收机的坐标位置，得到各个GPS接收机在ENU坐标系下的初始化位置；

实时姿态计算单元，用于无人机在飞行过程中姿态发生变化时，实时计算出各个GPS接收机在ECEF坐标系下的坐标位置；

坐标信息转换单元，用于将实时姿态计算单元得到的各个GPS接收机在ECEF坐标系下的坐标位置，应用三维坐标系旋转、空间向量运算的理论，转化为各个GPS接收机在ENU坐标系下的位置信息；

位置变化计算单元，用于通过比较各GPS接收机在ENU坐标系下的实时位置与初始化位置，计算出无人机各个轴向的旋转角度。

上述无人机姿态追踪系统的工作原理在无人机姿态追踪方法中已经全部一一对应进行了描述，在此不再赘述。

以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种基于多GPS接收机的无人机姿态追踪方法，其特征在于，包括如下步骤：

S1、在无人机的机臂上，以每个机臂为单位分别安装GPS接收机，并以其中一个GPS接收机为原点，建立ENU坐标系；

S2、将无人机水平放置，确定各个GPS接收机在ENU坐标系下的初始化位置；

S3、当无人机在飞行过程中姿态发生变化时，实时计算出各个GPS接收机在ECEF坐标系下的坐标位置；

S4、将步骤S3中得到的各个GPS接收机在ECEF坐标系下的坐标位置，转化为各个GPS接收机在ENU坐标系下的位置信息；

S5、通过比较步骤S4和步骤S2中各个GPS接收机的位置变化，计算出无人机各个轴向的旋转角度。

2.根据权利要求1所述的无人机姿态追踪方法，其特征在于，在步骤S2中，将作为原点的GPS接收机的坐标位置设为(0,0,0)，将各个GPS接收机在ENU坐标系下相对于原点GPS接收机的坐标位置，作为各个GPS接收机在ENU坐标系下的初始化的位置。

3.根据权利要求1所述的无人机姿态追踪方法，其特征在于，步骤S3是采用RTK定位算法实时计算出各个GPS接收机在ECEF坐标系下的坐标位置。

4.根据权利要求1所述的无人机姿态追踪方法，其特征在于，在步骤S4中是应用三维坐标系旋转、空间向量运算的理论，将各个GPS接收机在ECEF坐标系下的坐标位置转化为在ENU坐标系下的位置信息，具体是：

首先进行三维坐标系的旋转，使得旋转后的ECEF坐标系转化为以地球地心为原点的ENU坐标系；然后，再应用空间向量运算，得到位置发生变化的GPS接收机在ENU坐标系下的位置信息。

5.根据权利要求1所述的无人机姿态追踪方法，其特征在于，在步骤S5中，通过比较位置发生变化后的GPS接收机的位置信息与初始化位置信息，根据坐标系旋转变换的理论，计算得出GPS接收机在ENU坐标系下绕各个轴向旋转的角度，即无人机横滚、俯仰、偏航的角度变化。

6.一种基于多GPS接收机的无人机姿态追踪系统，其特征在于，包括：安装在无人机的各个机臂上GPS接收机，以及姿态追踪模块；其中，姿态追踪模块包括：

初始位置计算单元，用于将一个GPS接收机作为原点接收机，计算其余GPS接收机在ENU坐标系下相对于原点GPS接收机的坐标位置，得到各个GPS接收机在ENU坐标系下的初始化位置；

实时姿态计算单元，用于无人机在飞行过程中姿态发生变化时，实时计算出各个GPS接收机在ECEF坐标系下的坐标位置；

坐标信息转换单元，用于将实时姿态计算单元得到的各个GPS接收机在ECEF坐标系下的坐标位置，应用三维坐标系旋转、空间向量运算的理论，转化为各个GPS接收机在ENU坐标系下的位置信息；

位置变化计算单元，用于通过比较各GPS接收机在ENU坐标系下的实时位置与初始化位置，计算出无人机各个轴向的旋转角度。