CN105403218B - 用于四旋翼无人机的俯仰角的地磁修正方法 - Google Patents

Abstract

本发明提出了一种用于四旋翼无人机的俯仰角的地磁修正方法，包括：建立机体坐标系和导航坐标系；控制四旋翼无人机根据机体坐标系和导航坐标系以预设飞行姿态角转动；根据机体转动的飞行姿态角建立旋转矩阵，并根据旋转矩阵计算四旋翼无人机的飞机姿态矩阵；设置地磁在参考系中的投影，根据地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值；根据地磁俯仰角修正值修正四旋翼无人机的俯仰角以补偿四旋翼无人机的陀螺仪漂移造成的姿态误差。本发明在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精度。

Description

用于四旋翼无人机的俯仰角的地磁修正方法

技术领域

本发明涉及无人机技术领域，特别涉及一种用于四旋翼无人机的俯仰角的地磁修正方法。

背景技术

四旋翼无人机使用的自动驾驶仪是比较低端的，其产品价格较低。自动驾驶仪能够实现自动控制的基础是姿态测量，姿态测量的基础是陀螺仪。然而，四旋翼无人机使用的陀螺仪由于成本限制，精度不高，再加上陀螺仪本身的漂移，在经过长期累积后会对姿态产生较大影响。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种用于四旋翼无人机的俯仰角的地磁修正方法，在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精度。

为了实现上述目的，本发明的实施例提供一种用于四旋翼无人机的俯仰角的地磁修正方法，包括如下步骤：

步骤S1，建立机体坐标系和导航坐标系；

步骤S2，控制所述四旋翼无人机根据所述机体坐标系和导航坐标系以预设飞行姿态角转动，其中，所述飞行姿态角为机体按照以下顺序转动时的角度：绕所述Z轴转动的角度ψ，绕所述Y轴转动的角度θ，绕所述X轴转动的角度Ф；

步骤S3，根据所述机体转动的飞行姿态角建立旋转矩阵，并根据所述旋转矩阵计算所述四旋翼无人机的飞机姿态矩阵，其中，所述旋转矩阵为(c1,c2,c3),

根据绕z轴转动ψ角，计算得到

根据绕y轴转动θ角，计算得到

根据绕x轴转动Ф角，计算得到

根据所述旋转矩阵计算所述四旋翼无人机的飞机姿态矩阵，包括如下步骤：

根据所述旋转矩阵将参考体系变化至载体系，

然后计算所述飞机姿态矩阵为：

其中，

步骤S4，设置地磁在参考系中的投影，根据所述地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值；

步骤S5，根据所述地磁俯仰角修正值修正所述四旋翼无人机的俯仰角以补偿所述四旋翼无人机的陀螺仪漂移造成的姿态误差。

进一步，所述机体坐标系定义为：X轴表示横滚轴，Y轴表示俯仰轴，Z轴表示航向；所述导航坐标系定义为北东地坐标系。

进一步，在所述步骤S4中，根据所述地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值θd为：

θd＝arcsin(yb/xb)，

其中，

其中，为地磁在参考系中的投影。

进一步，所述根据地磁俯仰角修正值修正所述四旋翼无人机的俯仰角，包括如下步骤：

θ1＝θ_{_1}-k×(θg-θd)，

其中，θ1为修正后的俯仰角，θ_{_1}为上一次计算到的俯仰角θ1，θg为机体绕y轴转动俯仰角度，θg＝(Ф，θ，ψ)，k为预设系数。

进一步，k的取值范围为0.4～0.6。

根据本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法，通过地磁修正俯仰角，来实现对陀螺仪漂移所造成的姿态误差的误差，以现有的传感器为基础，没有增加额外的传感器，从而在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精度。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法的流程图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

如图1所示，本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法，包括如下步骤：

步骤S1，建立机体坐标系和导航坐标系。

具体地，坐标系建立如下：机体坐标系定义为：X轴表示横滚轴，对应前方；Y轴表示俯仰轴，对应右方；Z轴表示航向，对应下方。导航坐标系定义为北东地坐标系。

步骤S2，控制四旋翼无人机根据机体坐标系和导航坐标系以预设飞行姿态角转动。

飞行姿态角为机体按照以下顺序转动时的角度：绕Z轴转动的角度ψ，绕Y轴转动的角度θ，绕X轴转动的角度Ф。

步骤S3，根据机体转动的飞行姿态角建立旋转矩阵，并根据旋转矩阵计算四旋翼无人机的飞机姿态矩阵。

机体转动按照绕航向轴(Z轴)转动ψ，俯仰轴(Y轴)转动θ，横滚轴(X轴)转动Ф的顺序转动时，旋转矩阵为(c1,c2,c3),

根据绕z轴转动ψ角，计算得到

根据绕y轴转动θ角，计算得到

根据绕x轴转动Ф角，计算得到

基于此，根据旋转矩阵计算四旋翼无人机的飞机姿态矩阵，包括如下步骤：

首先，根据旋转矩阵将参考体系变化至载体系，

然后，计算飞机姿态矩阵为：

进一步，可以根据上述飞机姿态矩阵计算飞机姿态角为：

步骤S4，设置地磁在参考系中的投影，根据地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值θd。

根据地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值θd。

θd＝arcsin(yb/xb)，

其中，

其中，为地磁在参考系中的投影。

步骤S5，根据地磁俯仰角修正值修正四旋翼无人机的俯仰角以补偿四旋翼无人机的陀螺仪漂移造成的姿态误差。

θ1＝θ_{_1}-k×(θg-θd)，

其中，θ1为修正后的俯仰角，θ_{_1}为上一次计算到的俯仰角θ1，θg为机体绕y轴转动俯仰角度，θg＝(Ф，θ，ψ)，k为预设系数。k的取值范围为0.4～0.6。

根据本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法，通过地磁修正俯仰角，来实现对陀螺仪漂移所造成的姿态误差的误差，以现有的传感器为基础，没有增加额外的传感器，从而在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精度。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求极其等同限定。

Claims (5)

1.一种用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，包括如下步骤：

步骤S1，建立机体坐标系和导航坐标系；

步骤S2，控制所述四旋翼无人机根据所述机体坐标系和导航坐标系以预设飞行姿态角转动，其中，所述飞行姿态角为机体按照以下顺序转动时的角度：绕所述Z轴转动的角度ψ，绕所述Y轴转动的角度θ，绕所述X轴转动的角度Ф；

步骤S3，根据所述机体转动的飞行姿态角建立旋转矩阵，并根据所述旋转矩阵计算所述四旋翼无人机的飞机姿态矩阵，其中，所述旋转矩阵为(c1,c2,c3),

根据绕z轴转动ψ角，计算得到

根据绕y轴转动θ角，计算得到

根据绕x轴转动Ф角，计算得到

根据所述旋转矩阵计算所述四旋翼无人机的飞机姿态矩阵，包括如下步骤：

根据所述旋转矩阵将参考体系变化至载体系，

然后计算所述飞机姿态矩阵为：

其中，

步骤S4，设置地磁在参考系中的投影，根据所述地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值；

步骤S5，根据所述地磁俯仰角修正值修正所述四旋翼无人机的俯仰角以补偿所述四旋翼无人机的陀螺仪漂移造成的姿态误差。

2.如权利要求1所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，所述机体坐标系定义为：X轴表示横滚轴，Y轴表示俯仰轴，Z轴表示航向；所述导航坐标系定义为北东地坐标系。

3.如权利要求1所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，在所述步骤S4中，根据所述地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值θd为：

θd＝arcsin(yb/xb)，

其中，

其中，为地磁在参考系中的投影。

4.如权利要求3所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，在所述步骤S5中，所述根据地磁俯仰角修正值修正所述四旋翼无人机的俯仰角，包括如下步骤：

θ1＝θ_{_1}-k×(θg-θd)，

其中，θ1为修正后的俯仰角，θ_{_1}为上一次计算到的俯仰角θ1，θg为机体绕y轴转动俯仰角度，θg＝(Ф，θ，ψ)，k为预设系数。

5.如权利要求4所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，k的取值范围为0.4～0.6。