用于四旋翼无人机的俯仰角的地磁修正方法
【技术领域】
[0001] 本发明设及无人机技术领域，特别设及一种用于四旋翼无人机的俯仰角的地磁修 正方法。
【背景技术】
[0002] 四旋翼无人机使用的自动驾驶仪是比较低端的，其产品价格较低。自动驾驶仪能 够实现自动控制的基础是姿态测量，姿态测量的基础是巧螺仪。然而，四旋翼无人机使用的 巧螺仪由于成本限制，精度不高，再加上巧螺仪本身的漂移，在经过长期累积后会对姿态产 生较大影响。
【发明内容】
[0003] 本发明的目的旨在至少解决所述技术缺陷之一。
[0004] 为此，本发明的目的在于提出一种用于四旋翼无人机的俯仰角的地磁修正方法， 在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精度。
[0005] 为了实现上述目的，本发明的实施例提供一种用于四旋翼无人机的俯仰角的地磁 修正方法，包括如下步骤：
[0006]步骤S1，建立机体坐标系和导航坐标系；
[0007] 步骤S2,控制所述四旋翼无人机根据所述机体坐标系和导航坐标系W预设飞行姿 态角转动；
[0008]步骤S3,根据所述机体转动的飞行姿态角建立旋转矩阵，并根据所述旋转矩阵计 算所述四旋翼无人机的飞机姿态矩阵；
[0009] 步骤S4,设置地磁在参考系中的投影，根据所述地磁在参考系中的投影、旋转矩阵 和飞行姿态矩阵计算地磁俯仰角修正值；
[0010] 步骤S5,根据所述地磁俯仰角修正值修正所述四旋翼无人机的俯仰角W补偿所述 四旋翼无人机的巧螺仪漂移造成的姿态误差。
[0011] 进一步，所述机体坐标系定义为：x轴表示横滚轴，Y轴表示俯仰轴，Z轴表示航向； 所述导航坐标系定义为北东地坐标系。
[0012] 进一步，在所述步骤S2中，所述飞行姿态角为机体按照W下顺序转动时的角度： 绕所述Z轴转动的角度Φ，绕所述Y轴转动的角度Θ，绕所述X轴转动的角度Φ。
[0013]进一步，在所述步骤S3中，所述旋转矩阵为（cl，c2,c3)，
[0014] 根据绕z轴转动Φ角，计算得到 ；
[0015] 根据绕y轴转动Θ角，计算得到
，
[001引根据绕X轴转动Φ角，计算得离 0
[0017] 进一步，在所述步骤S3中，根据所述旋转矩阵计算所述四旋翼无人机的飞机姿态 矩阵，包括如下步骤：
[0018] 根据所述旋转矩阵将参考体系变化至载体系，c"b= c3c2cl;
[0019] 然后计算所述飞机姿态矩阵Cb"为：
[0020]
[00巧]进一步，在所述步骤S4中，根据所述地磁在参考系中的投影、旋转矩阵和飞行姿 态矩阵计算地磁俯仰角修正值Θd为：
[0026]白d=arcsin (yb/xb)，
[0027] 其中，
[0028]
[0029] 其中
赶也磁在参考系中的投影。
[0030] 进一步，所述根据地磁俯仰角修正值修正所述四旋翼无人机的俯仰角，包括如下 步骤：
[00引]Θ1=Θ1-kX(Θg-Θd)，
[0032] 其中，θ1为修正后的俯仰角，θι为上一次计算到的俯仰角θ1，θg为机体绕y 轴转动俯仰角度，0g= (Φ，θ，4)，k为预设系数。
[0033] 进一步，k的取值范围为0.4~0.6。
[0034] 根据本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法，通过地磁修正 俯仰角，来实现对巧螺仪漂移所造成的姿态误差的误差，W现有的传感器为基础，没有增加 额外的传感器，从而在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精 度。
[0035] 本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0036] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变 得明显和容易理解，其中：
[0037] 图1为根据本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法的流程 图。
【具体实施方式】
[0038] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终 相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附 图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[003引如图1所示，本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法，包括 如下步骤：
[0040] 步骤S1，建立机体坐标系和导航坐标系。
[0041] 具体地，坐标系建立如下：机体坐标系定义为：X轴表示横滚轴，对应前方；Y轴表 示俯仰轴，对应右方；Z轴表示航向，对应下方。导航坐标系定义为北东地坐标系。
[0042] 步骤S2,控制四旋翼无人机根据机体坐标系和导航坐标系W预设飞行姿态角转 动。
[0043] 飞行姿态角为机体按照W下顺序转动时的角度：绕Z轴转动的角度Φ，绕Y轴转 动的角度Θ，绕X轴转动的角度Φ。
[0044] 步骤S3,根据机体转动的飞行姿态角建立旋转矩阵，并根据旋转矩阵计算四旋翼 无人机的飞机姿态矩阵。
[0045] 机体转动按照绕航向轴狂轴）转动Φ，俯仰轴灯轴）转动Θ，横滚轴狂轴）转 动Φ的顺序转动时，旋转矩阵为（cl，c2,c3)，
[0046] 根据绕z轴转动Φ角，计算得离
[0047] 根据绕y轴转动θ角，计算得到
;(2)
[0048] 根据绕X轴转动Φ角，计算得至 (3)
[0049] 基于此，根据旋转矩阵计算四旋翼无人机的飞机姿态矩阵，包括如下步骤：
[0050]首先，根据旋转矩阵将参考体系变化至载体系，
[0051]c3c2cl；(4)
[0052] 然后，计算飞机姿态矩阵C为：
[00巧]进一步，可W根据上述飞机姿态矩阵计算飞机姿态角为：
[0059] 步骤S4,设置地磁在参考系中的投影，根据地磁在参考系中的投影、旋转矩阵和飞 行姿态矩阵计算地磁俯仰角修正值Θd。
[0060] 根据地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值 白d。
[0061] 白d=arcsin(yb/xb)，
[006引其中，
[0063]
[0064] 其中：
赶也磁在参考系中的投影。
[0065] 步骤S5,根据地磁俯仰角修正值修正四旋翼无人机的俯仰角W补偿四旋翼无人机 的巧螺仪漂移造成的姿态误差。
[0066]Θ1=Θi-kX(Θg-Θd)，
[0067]其中，θ1为修正后的俯仰角，θι为上一次计算到的俯仰角θ1，θg为机体绕y 轴转动俯仰角度，0g= (Φ，θ，4)，k为预设系数。k的取值范围为0.4~0.6。
[0068] 根据本发明实施例的用于四旋翼无人机的俯仰角的地磁修正方法，通过地磁修正 俯仰角，来实现对巧螺仪漂移所造成的姿态误差的误差，W现有的传感器为基础，没有增加 额外的传感器，从而在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精 度。
[0069] 在本说明书的描述中，参考术语"一个实施例"、"一些实施例"、"示例"、"具体示 例"、或"一些示例"等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特 点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不 一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可W在任何 的一个或多个实施例或示例中W合适的方式结合。
[0070] 尽管上面已经示出和描述了本发明的实施例，可W理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨 的情况下在本发明的范围内可W对上述实施例进行变化、修改、替换和变型。本发明的范围 由所附权利要求极其等同限定。
【主权项】
1. 一种用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，包括如下步骤： 步骤S1，建立机体坐标系和导航坐标系； 步骤S2,控制所述四旋翼无人机根据所述机体坐标系和导航坐标系W预设飞行姿态角 转动； 步骤S3,根据所述机体转动的飞行姿态角建立旋转矩阵，并根据所述旋转矩阵计算所 述四旋翼无人机的飞机姿态矩阵； 步骤S4,设置地磁在参考系中的投影，根据所述地磁在参考系中的投影、旋转矩阵和飞 行姿态矩阵计算地磁俯仰角修正值； 步骤S5,根据所述地磁俯仰角修正值修正所述四旋翼无人机的俯仰角W补偿所述四旋 翼无人机的巧螺仪漂移造成的姿态误差。2. 如权利要求1所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，所 述机体坐标系定义为：X轴表示横滚轴，Y轴表示俯仰轴，Z轴表示航向；所述导航坐标系定 义为北东地坐标系。3. 如权利要求1所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，在 所述步骤S2中，所述飞行姿态角为机体按照W下顺序转动时的角度：绕所述Z轴转动的角 度Φ，绕所述Y轴转动的角度Θ，绕所述X轴转动的角度Φ。4. 如权利要求1所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，在 所述步骤S3中，所述旋转矩阵为（cl，c2,c3)，根据绕z轴转动Φ角，计算得到根据绕y轴转动Θ角，计算得質 ；根据绕X轴转动Φ角，计算得到 〇5. 如权利要求4所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，在 所述步骤S3中，根据所述旋转矩阵计算所述四旋翼无人机的飞机姿态矩阵，包括如下步 骤： 根据所述旋转矩阵将参考体系变化至载体系，c"b=c3c2cl; 然后计算所述飞机姿态矩阵Cb"为：6. 如权利要求5所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，在 所述步骤S4中，根据所述地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰 角修正值Θd为： 白d=arcsin(yb/xb)， 其中，其中为地磁在参考系中的投影。7. 如权利要求6所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，在 所述步骤S5中，所述根据地磁俯仰角修正值修正所述四旋翼无人机的俯仰角，包括如下步 骤： 白1 =白_i_kX(白邑一白d), 其中，Θ1为修正后的俯仰角，θι为上一次计算到的俯仰角Θ1，0g为机体绕y轴转 动俯仰角度，0g= (Φ，Θ，il〇，k为预设系数。8. 如权利要求7所述的用于四旋翼无人机的俯仰角的地磁修正方法，其特征在于，k的 取值范围为0.4~0.6。
【专利摘要】本发明提出了一种用于四旋翼无人机的俯仰角的地磁修正方法，包括：建立机体坐标系和导航坐标系；控制四旋翼无人机根据机体坐标系和导航坐标系以预设飞行姿态角转动；根据机体转动的飞行姿态角建立旋转矩阵，并根据旋转矩阵计算四旋翼无人机的飞机姿态矩阵；设置地磁在参考系中的投影，根据地磁在参考系中的投影、旋转矩阵和飞行姿态矩阵计算地磁俯仰角修正值；根据地磁俯仰角修正值修正四旋翼无人机的俯仰角以补偿四旋翼无人机的陀螺仪漂移造成的姿态误差。本发明在没有提高产品成本的前提下，实现了对姿态误差的补偿，提高了精度。
【IPC分类】G01C21/06, G01C21/18, G01C21/08
【公开号】CN105403218
【申请号】CN201510888686
【发明人】黄磊, 王根勇, 刘聪, 李学风
【申请人】北京健德乾坤导航系统科技有限责任公司
【公开日】2016年3月16日
【申请日】2015年12月8日