CN111289933B - 一种多旋翼飞行器的磁力计自动校准系统和方法 - Google Patents

一种多旋翼飞行器的磁力计自动校准系统和方法 Download PDF

Info

Publication number
CN111289933B
CN111289933B CN202010374939.XA CN202010374939A CN111289933B CN 111289933 B CN111289933 B CN 111289933B CN 202010374939 A CN202010374939 A CN 202010374939A CN 111289933 B CN111289933 B CN 111289933B
Authority
CN
China
Prior art keywords
magnetometer
control period
data
zero offset
magnetic field
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010374939.XA
Other languages
English (en)
Other versions
CN111289933A (zh
Inventor
胡镇
何巍
罗伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Avic Jincheng Unmanned System Co ltd
Original Assignee
Avic Jincheng Unmanned System Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Avic Jincheng Unmanned System Co ltd filed Critical Avic Jincheng Unmanned System Co ltd
Priority to CN202010374939.XA priority Critical patent/CN111289933B/zh
Publication of CN111289933A publication Critical patent/CN111289933A/zh
Application granted granted Critical
Publication of CN111289933B publication Critical patent/CN111289933B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01R—MEASURING ELECTRIC VARIABLES; MEASURING MAGNETIC VARIABLES
    • G01R35/00—Testing or calibrating of apparatus covered by the other groups of this subclass
    • G01R35/005—Calibrating; Standards or reference devices, e.g. voltage or resistance standards, "golden" references

Abstract

本发明涉及多旋翼飞行器的技术领域，特别是涉及一种多旋翼飞行器的磁力计自动校准系统和方法，当飞行器进行偏航运动的时候，在每个控制周期，通过使用计算得到的磁力计零偏值，对磁力计采集的数据进行补偿；通过实时计算与地球标准磁场向量模值
Figure DEST_PATH_IMAGE002
的方差以判定是否完成校准。综上所述，本发明技术方案的有益效果是：不需要人为进行校准操作，由飞行器在空中飞行时自动完成；算法简单，没有大量的矩阵乘除运算；明确了算法的启动条件与退出条件，减少了故障率。

Description

一种多旋翼飞行器的磁力计自动校准系统和方法
技术领域
本发明涉及多旋翼飞行器的技术领域，特别是涉及一种多旋翼飞行器的飞行过程中磁力计自动校准算法。
背景技术
磁力计是多旋翼飞行器中的重要组成部分，主要提供对地球磁场向量的测量，通过与陀螺仪、加速度传感器进行多传感器数据融合，从而得到准确的姿态角信息。但是由于磁力计容易受到外界磁场的干扰，从而导致测量不准确，进而得不到准确的姿态角信息，对飞行稳定性的影响很大。因此，我们需要对磁力计数据进行校准，从而减小外界干扰对测量的影响。
目前，在传统的飞行器领域中，磁力计的校准具有以下特点：首先是只能在飞行前进行校准，需要通过遥控器或地面站设备发送校准命令，飞控接受到校准命令后，通过界面交互或LED状态交互的方式指导操作人员进行水平旋转2圈以上以及垂直放置飞行器，旋转2圈以上，从而采集到各个方向的磁力计数据的最大值与最小值；
其次，得到最大值与最小值后，一般采用最小二乘法进行校准系数的计算，该方法需要涉及到大量的矩阵运算，所以对处理器芯片的运算能力有一定的要求。
综上所述，传统的磁力计校准方法需要在飞行前进行大量的旋转操作才能完成校准。这样需要用户进行人为的操作才可以完成，此外，如果是大轴距的飞行器，如轴距1m以上的飞行器，进行垂直旋转操作显然是非常不便的。因此，如何让飞行器在飞行过程中，自动完成磁力计的校准，且校准算法的计算量较小，是本发明需要解决的问题。
发明内容
针对现有技术存在的不足，本发明提供一种使飞行器在飞行过程中自动完成校准的多旋翼飞行器的飞行过程中磁力计自动校准算法。
为实现上述目的，本发明采用的技术方案包括：一种多旋翼飞行器的磁力计自动校准方法，其特征在于，当飞行器进行偏航运动时，在每个控制周期对磁力计采集的数据采用计算得到的磁力计零偏值进行补偿；通过实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与地球标准磁场向量模值
Figure 415405DEST_PATH_IMAGE001
的方差是否小于或等于设定的阈值以判定是否完成磁力计校准。
一种多旋翼飞行器的磁力计自动校准方法，当飞行器进行偏航运动时，在每个控制周期对磁力计采集的数据采用计算得到的磁力计零偏值进行补偿；
第一个控制周期的磁力计零偏的计算步骤为：
Figure 332546DEST_PATH_IMAGE002
(1)
Figure 348886DEST_PATH_IMAGE003
表示第一个控制周期计算得到的磁力计零偏，
Figure 275254DEST_PATH_IMAGE004
表示控制器参数，
Figure 89626DEST_PATH_IMAGE001
表示地球标准磁场向量的模值，
Figure 318613DEST_PATH_IMAGE005
表示校准算法第一个控制周期获取的磁场向量模值，
Figure 851225DEST_PATH_IMAGE006
表示控制误差的方向；
Figure 315705DEST_PATH_IMAGE007
表示第一个控制周期获取的磁力计数据，
Figure 125529DEST_PATH_IMAGE008
表示初始获取的磁力计数据；
Figure 650051DEST_PATH_IMAGE005
为
Figure 669960DEST_PATH_IMAGE007
的模值；
补偿后的第二个控制周期磁力计数据
Figure 813496DEST_PATH_IMAGE009
等于
Figure 602461DEST_PATH_IMAGE010
减去
Figure 297884DEST_PATH_IMAGE011
,
Figure 680455DEST_PATH_IMAGE010
表示第二个控制周期获取的磁力计数据；补偿后的第n个控制周期的磁力计数据
Figure 486737DEST_PATH_IMAGE012
等于
Figure 661367DEST_PATH_IMAGE013
减去
Figure 137478DEST_PATH_IMAGE014
，
Figure 397559DEST_PATH_IMAGE013
表示第n个控制周期获取的磁力计数据,
Figure 741952DEST_PATH_IMAGE015
表示第n-1个控制周期计算得到的磁力计零偏；
第二个控制周期的磁力计零偏的计算步骤为：
Figure 380875DEST_PATH_IMAGE016
(2)
Figure 152522DEST_PATH_IMAGE017
表示第二个控制周期计算得到的磁力计零偏，
Figure 899898DEST_PATH_IMAGE009
表示补偿后的第二个控制周期磁力计数据；
进行迭代计算，第n个控制周期的磁力计零偏的计算步骤为：
Figure 923349DEST_PATH_IMAGE018
(3)
Figure 806991DEST_PATH_IMAGE019
表示第n个控制周期计算得到的磁力计零偏，
Figure 15119DEST_PATH_IMAGE012
表示补偿后的第n个控制周期磁力计数据；
Figure 859578DEST_PATH_IMAGE020
表示补偿后的第n-1个控制周期磁力计数据。
通过实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与地球标准磁场向量模值
Figure 811353DEST_PATH_IMAGE001
的方差是否小于或等于设定的阈值以判定是否完成磁力计校准。
具体地，所述的若干采样周期为25个采样周期，方差计算步骤为：
Figure 424869DEST_PATH_IMAGE021
（4）
Figure 803897DEST_PATH_IMAGE022
为第p个控制周期得到补偿后磁力计数据时计算得到的25个采样周期的方差，p为正整数；当p为大于25的正整数，
Figure 135653DEST_PATH_IMAGE023
…
Figure 891119DEST_PATH_IMAGE024
均为补偿后的某个控制周期的磁力计数据；当p为小于或等于25的正整数且不为1时，
Figure 483774DEST_PATH_IMAGE023
…
Figure 174650DEST_PATH_IMAGE024
部分为补偿后的某个控制周期的磁力计数据，部分为测得的未经补偿的磁力计数据；当p=1，
Figure 852756DEST_PATH_IMAGE025
…
Figure 880755DEST_PATH_IMAGE026
均为测得的未经补偿的磁力计数据。在每一个控制周期得到补偿后磁力计数据后，都计算方差以判定是否完成磁力计校准。另外，尽管在第一个控制周期还没有得到补偿后磁力计数据（获得测量值），也计算方差。
一种多旋翼飞行器的磁力计自动校准方法，具体包括如下步骤：
步骤1：实时计算若干采样周期测得的磁场向量与标准磁场向量的方差，记录初始的磁场向量
Figure 468862DEST_PATH_IMAGE027
；
步骤2：实时判定飞行器的偏航角速率数据（该数据与
Figure 189693DEST_PATH_IMAGE027
同时测得）是否大于设定的阈值，若判断为是，则进入步骤3；
步骤3：测量获取当前第一个控制周期读取的磁力计数据
Figure 355095DEST_PATH_IMAGE028
；
步骤4：计算磁力计零偏；
步骤5：将步骤4得到的磁力计零偏
Figure 62151DEST_PATH_IMAGE029
，补偿给下一个控制周期测得的磁力计数据，得到补偿后的第二个控制周期的磁力计数据；
步骤6：将步骤5得到的补偿后的第二个控制周期的磁力计数据
Figure 629399DEST_PATH_IMAGE009
，进行新的一轮零偏计算，从而得到新的磁力计零偏值，新的磁力计零偏值用于补偿给下一个控制周期测得的磁力计数据；经过若干个控制周期的计算后，最终使得零偏值趋向于一个稳定的值；
步骤7，判断当前补偿零偏后的磁力计数据的方差
Figure 662077DEST_PATH_IMAGE022
是否小于或等于设定的阈值，如果小于或等于设定的阈值，则判定磁力计校准已完成，校准结束，退出校准算法。
这里的步骤7实际上是随着步骤3-6的初始计算和迭代计算持续进行的，在每一个控制周期得到补偿后磁力计数据并计算得到零偏值后，都计算方差以判定是否完成磁力计校准。另外，尽管在第一个控制周期还没有得到补偿后磁力计数据（获得测量值），也计算方差以判定是否完成磁力计校准。
在每个控制周期，读取的磁力计数据与零偏补偿后得到补偿零偏后的磁力计数据（例外情况：第一个控制周期采用
Figure 314775DEST_PATH_IMAGE028
)，计算零偏（这个零偏是用于补偿下一个周期的磁力计数据的），然后计算方差，当方差大于0.02，则校准算法进入下一个控制周期；如果方差小于或等于0.02，那么校准算法结束。
步骤1具体为：飞行器上电后，磁力计开始工作，从而得到测量的X、Y、Z三个坐标轴的磁场数据
Figure 950156DEST_PATH_IMAGE030
，
Figure 247276DEST_PATH_IMAGE031
，
Figure 309910DEST_PATH_IMAGE032
，将其进行求模计算；
Figure 449905DEST_PATH_IMAGE033
（5）
根据经验值，建立一个数组变量
Figure 758483DEST_PATH_IMAGE034
，保存连续25个采样周期的
Figure 300323DEST_PATH_IMAGE035
数据；
Figure 533858DEST_PATH_IMAGE035
为磁场向量模值；
设地球标准磁场向量
Figure 770935DEST_PATH_IMAGE001
为0.5高斯，计算每25个采样周期内的方差值
Figure 748119DEST_PATH_IMAGE036
，
Figure 144465DEST_PATH_IMAGE037
（6）
实时更新记录初始磁场向量
Figure 424268DEST_PATH_IMAGE038
，
Figure 273275DEST_PATH_IMAGE039
(7)。
步骤3中的磁力计数据
Figure 54149DEST_PATH_IMAGE040
(8)；
步骤4具体为：
首先计算磁力计向量变化的方向，将校准算法第一个控制周期读取的磁力计数据
Figure 180368DEST_PATH_IMAGE028
与初始磁力计数据
Figure 755706DEST_PATH_IMAGE038
进行做差，即：
Figure 92009DEST_PATH_IMAGE041
（9）
以及当前磁力计数据
Figure 410995DEST_PATH_IMAGE007
的模值与标准向量模值的误差大小，即：
Figure 657300DEST_PATH_IMAGE042
（10）
最后，将式（9）、式（10）与设定的控制参数kp相乘，kp默认值为0.6，参数kp决定了收敛至稳定的零偏的快慢，得到第一个控制周期的零偏估计值
Figure 403539DEST_PATH_IMAGE043
(11)；
Figure 961559DEST_PATH_IMAGE044
（1）。
步骤5具体为：将步骤4得到的零偏
Figure 959602DEST_PATH_IMAGE045
，补偿给第二个控制周期读取的磁力计数据
Figure 919468DEST_PATH_IMAGE046
(12)，得到补偿后的第二个控制周期的磁力计数据
Figure 102188DEST_PATH_IMAGE047
(13)，计算方法如下：
Figure 288450DEST_PATH_IMAGE048
（14）
步骤6具体为：将步骤5得到的补偿后的第二个控制周期的磁力计数据
Figure 949238DEST_PATH_IMAGE009
，进行新的一轮零偏计算，即与步骤4一样，从而得到新的零偏值；
Figure 763610DEST_PATH_IMAGE049
（2）
经过若干个控制周期的计算后，最终使得零偏值趋向于一个稳定的值。值得一提的是，当迭代计算的零偏估计值不稳定时，方差一定是大于设定的阈值的。
本发明还提供一种多旋翼飞行器的磁力计自动校准系统，包括校准算法模块、方差计算模块、偏航判定模块；
所述的偏航判定模块，通过获取陀螺仪传感器测得的偏航角速率，并将偏航角速率与设定阈值相比较，当偏航角速率大于设定阈值，则判定飞行器发生偏航，并启动校准算法模块；
所述的校准算法模块，实时获取磁力计传感器测得的磁力计数据，进行零偏计算，并将计算得到的磁力计零偏再与磁力计传感器测得的磁力计数据进行补偿，并将补偿后的磁力计数据发送给方差计算模块；
方差计算模块负责读取校准算法模块中补偿后的磁力计数据以及磁力计传感器测得的磁力计数据，实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与标准地磁向量模值的方差，并判定该方差大小是否小于或等于设定阈值，作为校准算法结束的判定条件。
所述的校准算法模块，包括零偏计算模块、磁力计数据补偿模块；所述的零偏计算模块，实时获取磁力计传感器测得的磁力计数据和磁力计数据补偿模块计算得到的补偿后磁力计数据，进行零偏计算；所述的磁力计数据补偿模块，将零偏计算模块计算得到的磁力计零偏与磁力计传感器测得的磁力计数据进行补偿，并将补偿后的磁力计数据发送给方差计算模块。
可采用adis16470作为陀螺仪传感器，由陀螺仪传感器测得偏航角速率。ist8308作为磁力计传感器（地磁传感器），由磁力计传感器测得（即采集）磁场向量（即磁力计数据）。
例如，陀螺仪传感器数据读取模块（其为现有技术，软件模块）用于读取陀螺仪传感器测量的偏航角速率，而偏航判定模块则从陀螺仪传感器数据读取模块获取偏航角速率数据。 磁力计传感器数据读取模块（其为现有技术，软件模块）用于读取磁力计传感器测得的实时磁场数据，而校准算法的零偏计算模块与磁力计数据补偿模块则从磁力计传感器数据读取模块获取实时磁场数据，方差计算模块也从磁力计传感器数据读取模块获取实时磁场数据，此外，方差计算模块还从磁力计数据补偿模块获取补偿后的磁力计数据。
综上所述，本发明技术方案的有益效果是：不需要人为的进行校准操作，由飞行器在空中飞行时自动完成；算法简单，没有大量的矩阵乘除运算；明确了算法的启动条件与退出条件，减少了故障率。
附图说明
图1为本发明的一个实施例提供的磁力计自动校准方法的流程示意图；
图2为本发明的一个实施例提供的磁力计自动校准方法的计算结果示意图。
具体实施方式
本发明实施例提供一种飞行器的磁力计校准方法，解决传统校准方法，在飞行前需要进行至少两个平面的旋转操作，提高飞行器的使用便捷性，同时还提出以磁力计数据校准前后的方差作为判定校准算法的完成。为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述：
磁力计校准主要针对的是由于机身造成的固定磁干扰，导致的磁力计数据产生了固定的零偏值。常见的磁力计校准方法主要是在飞行前，通过将飞行前水平旋转以及垂直旋转2圈以上，获取磁力计的最大和最小的磁场数据，从而进入校准算法进行零偏计算。然而，实际应用过程中，如果飞行器机身过大，如轴距大于1m以上的飞行器，要将其水平或垂直旋转会有诸多不便，因此本发明提出一种飞行器在飞行过程中自动完成校准的算法。本发明主要理论依据是，无论飞行器的偏航角为什么角度值，实时测量的地球磁场向量应该都是一样的。因此，通过在飞行过程中，不断的进行偏航运动，校正相邻时间周期的磁场向量为一个固定值。所以下面的等式成立：
Figure 117231DEST_PATH_IMAGE050
（15）
上式中，
Figure 525210DEST_PATH_IMAGE051
表示机体坐标系下，由磁力计传感器测量得到的磁力计数据，
Figure 989689DEST_PATH_IMAGE052
表示机体坐标系下，所需估计的磁力计零偏，即校准算法的结果，
Figure 924147DEST_PATH_IMAGE053
表示地理坐标系下地球的标准磁场向量模值，地球的标准磁场向量模值是一个已知的固定值，约为0.5高斯。在下面的具体实施例中，
Figure 324036DEST_PATH_IMAGE008
、
Figure 78365DEST_PATH_IMAGE007
、
Figure 612115DEST_PATH_IMAGE010
…
Figure 276445DEST_PATH_IMAGE013
均为在不同周期磁力计传感器测量得到的磁力计数据（磁力计数据又称为磁场向量、磁场数据）。
当飞行器正在进行偏航运动，则启动自动校准算法。将校准算法转换成一个控制问题，控制目标是将测量得到的磁场向量能实时跟踪参考地磁向量，也就是说
Figure 971869DEST_PATH_IMAGE054
(16)。这里，我们引入一个比例控制，设控制参数（即控制器参数）为
Figure 479074DEST_PATH_IMAGE055
，则
Figure 160722DEST_PATH_IMAGE056
（1）
式中，
Figure 804193DEST_PATH_IMAGE057
表示第一个控制周期计算得到的零偏，
Figure 670518DEST_PATH_IMAGE055
表示控制器参数，
Figure 540385DEST_PATH_IMAGE053
表示地球标准磁场向量的模值，这里设为0.5高斯，
Figure 150357DEST_PATH_IMAGE007
表示校准算法第一个控制周期获取的磁场向量（即磁力计数据），
Figure 913914DEST_PATH_IMAGE006
表示了第一个控制周期控制误差的方向。
将第一个控制计算周期得到的
Figure 685561DEST_PATH_IMAGE058
补偿到第二个控制周期获取的磁场向量
Figure 42724DEST_PATH_IMAGE010
，得到补偿后的磁场向量
Figure 456388DEST_PATH_IMAGE009
，接着迭代计算：
Figure 74451DEST_PATH_IMAGE059
（2）
得到校准算法第二个控制周期的零偏估计值
Figure 157945DEST_PATH_IMAGE060
，同样补偿给第三个控制周期获取的地磁测量值（即磁场向量、磁力计数据），从而不断的迭代下去。
前两个控制周期相当于是校准算法的初始部分，从第三个控制周期开始直到第N个控制周期的公式如下：
Figure 861459DEST_PATH_IMAGE061
（3）
第n个控制周期的零偏计算，需要第n个控制周期补偿后的磁力计数据，以及第n-1个控制周期补偿后的磁力计数据（n=3、4、5…N，N为校准算法的总控制周期数）。补偿后的磁力计数据即为校准偏移后的磁力计数据。N由方差判定结果决定，例如p=200时
Figure 954180DEST_PATH_IMAGE022
小于0.02而p=1-199时
Figure 426749DEST_PATH_IMAGE022
均大于0.02，那么N即为200。
其中，本发明中的控制周期，指的是校准算法运行的周期，即在第一个控制周期进行零偏计算的时候，需要第一个控制周期的地磁测量值以及初始记录的地磁测量值。然后第一个控制周期计算得到的零偏，代入到第二个控制周期的地磁测量值，得到补偿后的地磁测量值，再与第一个控制周期的地磁测量值，进行第二个控制周期的零偏计算。经过两个控制周期计算以后，就能得到连续两个周期的补偿后的地磁数据，即完成了初始计算，后续则按如下公式进行迭代计算，直至算法完成。
Figure 805778DEST_PATH_IMAGE062
（3）
此外，由于实际飞行过程中，除了机身的固定干扰造成的零点偏移以外，飞行器的磁力计数据还会受到外界的随机干扰，如果飞行过程中一直进行自动校准，则会出现误计算的现象，因此本发明还提出了自动停止校准的方法。
实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与标准地球的磁场向量模值
Figure 137533DEST_PATH_IMAGE001
的方差（这里用标准模值代替了均值，所以这个方差表示与标准模值的幅值波动大小），当方差小于或等于一定的阈值时，则认为校准已经完成，自动退出该算法。所述的测得的磁场向量模值为没有零偏补偿的测得的磁场向量的模值，这是由于若干采样周期中某些采样周期还没有进入零偏计算（因此没有零偏补偿）。
图1为本发明的一个实施例提供的一种磁力计校准方法的流程示意图。如图1所示，该方法包括：
步骤1，实时计算若干采样周期测得的磁场向量（例如25个采样周期）与标准磁场向量的方差（这里校准算法没有启动，则相当于计算的是原始测量数据与标准地磁向量的方差），记录初始的磁场向量
Figure 627420DEST_PATH_IMAGE038
。
首先飞行器上电后，磁力计开始工作，从而得到测量的X、Y、Z三个坐标轴的磁场数据
Figure 220076DEST_PATH_IMAGE030
，
Figure 770006DEST_PATH_IMAGE031
，
Figure 317619DEST_PATH_IMAGE032
，将其进行求模计算。
Figure 611197DEST_PATH_IMAGE033
（5）
建立一个数组变量
Figure 58359DEST_PATH_IMAGE034
，保存连续25个采样周期的
Figure 654556DEST_PATH_IMAGE035
数据。
Figure 819958DEST_PATH_IMAGE035
即磁场向量模值。
设地球标准磁场向量
Figure 917227DEST_PATH_IMAGE001
为0.5高斯，代入下面公式后，计算每25个周期内的方差值
Figure 94262DEST_PATH_IMAGE063
，
Figure 985994DEST_PATH_IMAGE037
（6）
实时更新记录初始磁场向量
Figure 373113DEST_PATH_IMAGE038
，
Figure 149440DEST_PATH_IMAGE039
（7）。
Figure 571194DEST_PATH_IMAGE064
、
Figure 633828DEST_PATH_IMAGE065
、
Figure 649188DEST_PATH_IMAGE066
分别为初始测得的X、Y、Z三个坐标轴的磁场数据。
步骤2，通过陀螺仪传感器测得飞行器的偏航角速率，判定飞行器的偏航角速率数据是否大于设定的阈值，阈值设为0.4rad/s，若判断为是，则正式进入校准算法步骤3；若判断为否，则重新返回步骤1，计算与标准磁场向量的方差，并记录校准算法前的磁场向量（相当于校准算法没有启动的时候，一直在更新
Figure 822680DEST_PATH_IMAGE038
，将当前的磁场数据进行赋值）；
步骤3，测量获取第一个控制周期的磁力计数据
Figure 98941DEST_PATH_IMAGE067
，
Figure 473422DEST_PATH_IMAGE068
（8）；
Figure 835133DEST_PATH_IMAGE069
、
Figure 812316DEST_PATH_IMAGE070
、
Figure 208662DEST_PATH_IMAGE071
分别为第一控制周期获取的X、Y、Z三个坐标轴的磁场数据。
步骤4，计算磁力计零偏。
首先计算磁力计向量变化的方向，即：
Figure 754044DEST_PATH_IMAGE072
（9）
以及第一个控制周期读取的磁力计数据
Figure 337472DEST_PATH_IMAGE067
的模值与标准向量模值的误差大小，即：
Figure 118347DEST_PATH_IMAGE073
（10）
最后，将式（9）、式（10）与设定的控制参数
Figure 244566DEST_PATH_IMAGE004
相乘，
Figure 819903DEST_PATH_IMAGE004
默认值为0.6，参数
Figure 156207DEST_PATH_IMAGE004
决定了收敛至稳定的零偏参数的快慢，得到第一个周期的零偏估计值
Figure 350559DEST_PATH_IMAGE074
（11）。
Figure 455918DEST_PATH_IMAGE075
（1）
Figure 202157DEST_PATH_IMAGE076
、
Figure 166702DEST_PATH_IMAGE077
、
Figure 23800DEST_PATH_IMAGE078
分别为第一控制周期计算得出的X、Y、Z三个坐标轴的磁场数据零偏。
步骤5，将步骤4得到的零偏
Figure 249245DEST_PATH_IMAGE045
，补偿给第二个控制周期读取的磁力计数据
Figure 166385DEST_PATH_IMAGE046
（12），得到补偿后的磁力计数据
Figure 87068DEST_PATH_IMAGE047
（13），计算方法如下：
Figure 747856DEST_PATH_IMAGE079
（14）
Figure 827808DEST_PATH_IMAGE080
、
Figure 56795DEST_PATH_IMAGE081
、
Figure 589408DEST_PATH_IMAGE082
分别为第二控制周期获取的X、Y、Z三个坐标轴的磁场数据。
Figure 319466DEST_PATH_IMAGE083
、
Figure 129290DEST_PATH_IMAGE084
、
Figure 388233DEST_PATH_IMAGE085
分别为补偿后的第二控制周期的X、Y、Z三个坐标轴的磁场数据。
步骤6，将步骤5得到的补偿后的磁力计数据
Figure 408142DEST_PATH_IMAGE009
，进行第二个控制周期的零偏计算，即与步骤4一样，从而得到第二个控制周期的零偏值。
Figure 676312DEST_PATH_IMAGE086
（2）
经过两个控制周期的计算以后，则完成了校准算法的初始化部分，即可以得到连续两个周期的补偿后的磁力计数据，从而迭代计算如下：
Figure 606222DEST_PATH_IMAGE087
（17）
…
经过n个周期的计算后，最终使得零偏估计值趋向于一个稳定的值(实际上，校准算法运行的最后一段时间的零偏值趋向于一个稳定的值)。
Figure 36066DEST_PATH_IMAGE088
（18）
Figure 684217DEST_PATH_IMAGE061
（3）
Figure 490499DEST_PATH_IMAGE089
、
Figure 133969DEST_PATH_IMAGE090
、
Figure 294DEST_PATH_IMAGE091
分别为补偿后的第n个控制周期的X、Y、Z三个坐标轴的磁场数据。
Figure 604582DEST_PATH_IMAGE092
、
Figure 480134DEST_PATH_IMAGE093
、
Figure 978112DEST_PATH_IMAGE094
分别为第n-1个控制周期计算出的X、Y、Z三个坐标轴的零偏。
Figure 884845DEST_PATH_IMAGE095
、
Figure 101062DEST_PATH_IMAGE096
、
Figure 249147DEST_PATH_IMAGE097
为第n个控制周期读取的X、Y、Z三个坐标轴的磁场数据。
第n个控制周期的零偏，需要当前n时刻（即第n个控制周期）的补偿后的磁力计数据与上一时刻n-1时刻（即第n-1个控制周期）的补偿后的磁力计数据。
步骤7，判断当前补偿零偏后的磁力计数据的方差
Figure 8156DEST_PATH_IMAGE022
（即若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与地球标准磁场向量模值
Figure 216283DEST_PATH_IMAGE001
的方差
Figure 919797DEST_PATH_IMAGE022
），是否小于或等于设定的阈值0.02，如果小于或等于设定的阈值，则退出校准算法，认定校准已完成；如果大于设定的阈值，则校准算法仍继续计算。
这里的步骤7实际上是随着步骤3-6的初始计算和迭代计算持续进行的。
例如，在第一个控制周期读取
Figure 871572DEST_PATH_IMAGE067
、再计算得到
Figure 485087DEST_PATH_IMAGE098
，再通过公式（4）计算方差（p=1）并与阈值（0.02）比较，如果方差（p=1）小于或等于阈值0.02，则判定磁力计校准已完成，退出校准算法，校准算法的总控制周期仅为1。如果方差（p=1）大于阈值0.02，则校准算法进入第二个控制周期，在第二个控制周期读取
Figure 864116DEST_PATH_IMAGE010
、再用
Figure 54926DEST_PATH_IMAGE099
补偿得到
Figure 420179DEST_PATH_IMAGE009
、再计算得到
Figure 12835DEST_PATH_IMAGE100
，再通过公式（4）计算方差（p=2）。当方差（p=2）小于或等于阈值0.02，则判定磁力计校准已完成，退出校准算法，校准算法的总控制周期为2。当方差（p=2）大于阈值0.02，则校准算法进入第三个控制周期。在第三个控制周期读取
Figure 562765DEST_PATH_IMAGE101
、再用
Figure 116237DEST_PATH_IMAGE102
补偿得到
Figure 409815DEST_PATH_IMAGE103
、再计算得到
Figure 856977DEST_PATH_IMAGE104
，再通过公式（4）计算方差（p=3）并与阈值（0.02）比较；如果方差（p=3）小于或等于阈值0.02，则判定磁力计校准已完成，退出校准算法，校准算法的总控制周期为3。如果方差（p=3）大于阈值0.02，则校准算法进入第四个控制周期，依此类推。
Figure 577808DEST_PATH_IMAGE021
（4）
Figure 352997DEST_PATH_IMAGE022
为第p个控制周期得到补偿后磁力计数据时计算得到的25个采样周期的方差，p为正整数；当p为大于25的正整数，
Figure 450266DEST_PATH_IMAGE025
…
Figure 17514DEST_PATH_IMAGE105
均为补偿后的某个（由下标p-24,p-23,p-22…p-1或p表示）控制周期的磁力计数据(即某个控制周期的补偿后的磁力计数据，例如
Figure 50192DEST_PATH_IMAGE025
表示补偿后的第p-24个控制周期的磁力计数据，
Figure 437311DEST_PATH_IMAGE105
表示补偿后的第p个控制周期的磁力计数据)；当p为小于或等于25的正整数且不为1时，
Figure 72692DEST_PATH_IMAGE106
…
Figure 369812DEST_PATH_IMAGE105
部分为补偿后的某个控制周期的磁力计数据，部分为测得的未经补偿的磁力计数据；当p=1，
Figure 432446DEST_PATH_IMAGE107
…
Figure 306861DEST_PATH_IMAGE108
均为测得的未经补偿的磁力计数据。
同公式（6）一样，
Figure 621299DEST_PATH_IMAGE025
…
Figure 897559DEST_PATH_IMAGE105
包括25个磁场数据，下标分别为p-24、p-23、p-22…p-2、p-1、p。这里的p与上面的控制周期n等同。
当p=1时，也就是处于第一个控制周期，此时25个磁场数据包括
Figure 131095DEST_PATH_IMAGE067
、
Figure 492806DEST_PATH_IMAGE038
，另外23个磁场数据是测得
Figure 610934DEST_PATH_IMAGE038
之前的23个采样周期分别测得的磁场数据；当p=2时，也就是处于第二个控制周期，此时25个磁场数据包括
Figure 741702DEST_PATH_IMAGE009
、
Figure 146138DEST_PATH_IMAGE067
、
Figure 870512DEST_PATH_IMAGE038
，另外22个磁场数据是测得
Figure 651386DEST_PATH_IMAGE038
之前的22个采样周期分别测得的磁场数据；当p=3时，25个磁场数据包括补偿后的第三个控制周期的磁力计数据、
Figure 43184DEST_PATH_IMAGE009
、
Figure 618522DEST_PATH_IMAGE067
、
Figure 220404DEST_PATH_IMAGE038
，另外21个磁场数据是测得
Figure 414757DEST_PATH_IMAGE038
之前的21个采样周期分别测得的磁场数据；依此类推；当p=25时，25个磁场数据包括补偿后的第2到25个控制周期的磁力计数据以及
Figure 520116DEST_PATH_IMAGE067
。当p=26时，25个磁场数据包括补偿后的第2到26个控制周期的磁力计数据。依此类推。
若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值
Figure 531934DEST_PATH_IMAGE109
…
Figure 230900DEST_PATH_IMAGE110
，在选用时，只要有补偿后的磁场向量的模值，则使用补偿值（即补偿后的磁场向量的模值）；在没有补偿值的情况下，才使用测得的磁场向量的模值。
在通常情况下，而校准算法运行时间约10s，因此，一般来说，结束校准算法时
Figure 353577DEST_PATH_IMAGE106
…
Figure 313442DEST_PATH_IMAGE105
均为补偿后的磁力计数据。
本文中，飞行器上电后，25个采样周期内的磁场数据的模值与地球标准磁场向量模值的方差计算始终持续进行。而控制周期，指的是校准算法运行的周期。采样周期相当于飞行器上电后这个采样程序持续运行，计算零偏的时候控制周期等同于采样周期。
需要解释的是，飞行器上电一开始的时候理论上不存在立刻进入校准的情况，即启动校准算法前一定已历经25个以上的采样周期（能计算得到方差，对应于本实施例），实际25个采样周期大概是0.05s；即便在不满25个采样周期的前提下进入了校准算法，那么可以等到满25个采样周期开始计算方差后再判定是否退出校准算法，由于25个采样周期大概是0.05s，时间很短基本没有影响。
图2为本实施例提供的一个算法结果示意图，图中显示，x轴的磁力计数据具有0.3高斯左右的零偏，经过算法计算后，10s左右，就能估计出这个0.3高斯的零偏，而y轴与z轴则在0附近变化，所以本算法有效。
以上显示描述了本发明的基本原理、主要特征以及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落进要求保护本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (9)

1.一种多旋翼飞行器的磁力计自动校准方法，其特征在于，当飞行器进行偏航运动的时候，在每个控制周期，通过使用计算得到的磁力计零偏值，对磁力计采集的数据进行补偿；
第一个控制周期的磁力计零偏的计算步骤为：
Figure 818885DEST_PATH_IMAGE001
(1)
Figure 641348DEST_PATH_IMAGE002
表示第一个控制周期计算得到的磁力计零偏，
Figure 846064DEST_PATH_IMAGE003
表示控制器参数，
Figure 44964DEST_PATH_IMAGE004
表示地 球标准磁场向量的模值，
Figure 713843DEST_PATH_IMAGE005
表示校准算法第一个控制周期获取的磁场向量的模值，
Figure 894158DEST_PATH_IMAGE006
表示控制误差的方向；
Figure 648487DEST_PATH_IMAGE007
表示第一个控制周期获取的磁力计数据，
Figure 323182DEST_PATH_IMAGE008
表示初始 获取的磁力计数据；
补偿后的第二个控制周期磁力计数据
Figure 846567DEST_PATH_IMAGE009
等于
Figure 463362DEST_PATH_IMAGE010
减去
Figure 704988DEST_PATH_IMAGE011
,
Figure 980111DEST_PATH_IMAGE010
表示第二个控制 周期获取的磁力计数据；补偿后的第n个控制周期的磁力计数据
Figure 561265DEST_PATH_IMAGE012
等于
Figure 896432DEST_PATH_IMAGE013
减去
Figure 815234DEST_PATH_IMAGE014
，
Figure 894048DEST_PATH_IMAGE013
表示第n个控制周期获取的磁力计数据,
Figure 392025DEST_PATH_IMAGE015
表示第n-1个控制周期 计算得到的磁力计零偏；
第二个控制周期的磁力计零偏的计算步骤为：
Figure 101355DEST_PATH_IMAGE016
(2)
Figure 51994DEST_PATH_IMAGE017
表示第二个控制周期计算得到的磁力计零偏，
Figure 121450DEST_PATH_IMAGE009
表示补偿后的第二个控制周 期磁力计数据；
进行迭代计算，第n个控制周期的磁力计零偏的计算步骤为：
Figure 473934DEST_PATH_IMAGE018
(3)
Figure 416482DEST_PATH_IMAGE019
表示第n个控制周期计算得到的磁力计零偏，
Figure 792100DEST_PATH_IMAGE012
表示补偿后的第n个控制周期 磁力计数据；
Figure 212717DEST_PATH_IMAGE020
表示补偿后的第n-1个控制周期磁力计数据。
2.根据权利要求1所述的一种多旋翼飞行器的磁力计自动校准方法，其特征在于，通过 实时计算若干采样周期内经过零偏补偿后的磁场向量的模值与地球标准磁场向量模值
Figure 872237DEST_PATH_IMAGE004
的方差是否小于或等于设定的阈值以判定是否完成磁力计校准；当某采样周期没有零 偏补偿后的磁场向量模值，则使用该采样周期测得的磁场向量模值。
3.根据权利要求2所述的一种多旋翼飞行器的磁力计自动校准方法，其特征在于，所述的若干采样周期为25个采样周期，方差计算步骤为：
Figure 985687DEST_PATH_IMAGE021
(4)
Figure 910917DEST_PATH_IMAGE022
为第p个控制周期得到补偿后磁力计数据时计算得到的25个采样周期的方差， p为正整数；当p为大于25的正整数，
Figure 72908DEST_PATH_IMAGE023
…
Figure 134405DEST_PATH_IMAGE024
均为补偿后的某个控制周期的磁力计数据； 当p为小于或等于25的正整数且不为1时，
Figure 418756DEST_PATH_IMAGE023
…
Figure 18234DEST_PATH_IMAGE024
部分为补偿后的某个控制周期的磁力 计数据，部分为测得的未经补偿的磁力计数据；当p=1，
Figure 46232DEST_PATH_IMAGE025
…
Figure 227815DEST_PATH_IMAGE026
均为测得的未经补偿的 磁力计数据。
4.一种多旋翼飞行器的磁力计自动校准方法，其特征在于，具体包括如下步骤：
步骤1：实时计算若干采样周期测得的磁场向量模值与地球标准磁场向量模值的方差， 记录初始获取的磁力计数据
Figure 355171DEST_PATH_IMAGE027
；
步骤2：实时判定飞行器的偏航角速率数据是否大于设定的阈值，若判断为是，则进入步骤3；
步骤3：测量获取当前第一个控制周期读取的磁力计数据
Figure 254994DEST_PATH_IMAGE028
；
步骤4：计算磁力计零偏；
步骤5：将步骤4得到的磁力计零偏
Figure 5126DEST_PATH_IMAGE029
，补偿给下一个控制周期测得的磁力计数 据，得到补偿后的第二个控制周期的磁力计数据；
步骤6：将步骤5得到的补偿后的第二个控制周期的磁力计数据
Figure 306794DEST_PATH_IMAGE009
，进行新的一轮零 偏计算，从而得到新的磁力计零偏值，新的磁力计零偏值用于补偿给下一个控制周期测得 的磁力计数据；经过若干个控制周期的计算后，最终使得零偏值趋向于一个稳定的值；
步骤7，判断当前若干采样周期内经过零偏补偿后的磁场向量的模值与地球标准磁场 向量模值
Figure 667368DEST_PATH_IMAGE004
的方差
Figure 992170DEST_PATH_IMAGE022
是否小于或等于设定的阈值，如果小于或等于设定的阈值，则 判定磁力计校准已完成；当某采样周期没有零偏补偿后的磁场向量模值，则使用该采样周 期测得的磁场向量模值；
步骤4中，第一个控制周期的磁力计零偏的计算步骤为：
Figure 361972DEST_PATH_IMAGE001
(1)
Figure 252567DEST_PATH_IMAGE002
表示第一个控制周期计算得到的磁力计零偏，
Figure 236573DEST_PATH_IMAGE003
表示控制器参数，
Figure 845408DEST_PATH_IMAGE004
表示地 球标准磁场向量的模值，
Figure 956584DEST_PATH_IMAGE005
表示校准算法第一个控制周期获取的磁场向量的模值，
Figure 967265DEST_PATH_IMAGE006
表示控制误差的方向；
Figure 122172DEST_PATH_IMAGE007
表示第一个控制周期获取的磁力计数据，
Figure 218304DEST_PATH_IMAGE008
表示初始 获取的磁力计数据；
步骤5中，补偿后的第二个控制周期磁力计数据
Figure 929908DEST_PATH_IMAGE009
等于
Figure 732779DEST_PATH_IMAGE010
减去
Figure 871636DEST_PATH_IMAGE011
,
Figure 455064DEST_PATH_IMAGE010
表示第 二个控制周期测得的磁力计数据；
步骤6中，第二个控制周期的磁力计零偏的计算步骤为：
Figure 891731DEST_PATH_IMAGE016
(2)
Figure 877004DEST_PATH_IMAGE017
表示第二个控制周期计算得到的磁力计零偏，
Figure 124446DEST_PATH_IMAGE009
表示补偿后的第二个控制 周期磁力计数据；
进行迭代计算，第n个控制周期的磁力计零偏的计算步骤为：
Figure 195170DEST_PATH_IMAGE018
(3)
Figure 248577DEST_PATH_IMAGE019
表示第n个控制周期计算得到的磁力计零偏，
Figure 278237DEST_PATH_IMAGE012
表示补偿后的第n个控制周 期磁力计数据；
Figure 758897DEST_PATH_IMAGE020
表示补偿后的第n-1个控制周期磁力计数据；补偿后的第n个控制周期 的磁力计数据
Figure 51338DEST_PATH_IMAGE012
等于
Figure 846119DEST_PATH_IMAGE013
减去
Figure 540405DEST_PATH_IMAGE014
，
Figure 378917DEST_PATH_IMAGE013
表示第n个控制周期获取的磁力计数据,
Figure 424234DEST_PATH_IMAGE015
表示第n-1个控制周期计算得到的磁力计零偏。
5.根据权利要求4所述的一种多旋翼飞行器的磁力计自动校准方法，其特征在于，步骤 1具体为：飞行器上电后，磁力计开始工作，从而得到测量的X、Y、Z三个坐标轴的磁场数据
Figure 757126DEST_PATH_IMAGE030
，
Figure 305919DEST_PATH_IMAGE031
，
Figure 393961DEST_PATH_IMAGE032
，将其进行求模计算；
Figure 847945DEST_PATH_IMAGE033
（5）
根据经验值，建立一个数组变量
Figure 46845DEST_PATH_IMAGE034
，保存连续25个采样周期的
Figure 387828DEST_PATH_IMAGE035
数 据；
Figure 646771DEST_PATH_IMAGE035
为磁场向量模值；
设地球标准磁场向量
Figure 401100DEST_PATH_IMAGE004
为0.5高斯，计算每25个采样周期内的方差值
Figure 325063DEST_PATH_IMAGE036
，
Figure 848448DEST_PATH_IMAGE037
（6）
实时更新记录初始获取的磁力计数据
Figure 215975DEST_PATH_IMAGE038
，
Figure 192021DEST_PATH_IMAGE039
(7)。
6.根据权利要求4所述的一种多旋翼飞行器的磁力计自动校准方法，其特征在于，步骤 3中的磁力计数据
Figure 732724DEST_PATH_IMAGE040
(8)；
步骤4具体为：
首先计算磁力计向量变化的方向，将校准算法第一个控制周期读取的磁力计数据
Figure 376195DEST_PATH_IMAGE028
与初始获取的磁力计数据
Figure 731320DEST_PATH_IMAGE038
进行做差，即：
Figure 460242DEST_PATH_IMAGE041
（9）
以及当前磁力计数据
Figure 539056DEST_PATH_IMAGE007
的模值与地球标准磁场向量模值的误差大小，即：
Figure 974717DEST_PATH_IMAGE042
（10）
最后，将式（9）、式（10）与设定的控制参数kp相乘，kp默认值为0.6，参数kp决定了收敛 至稳定的零偏的快慢，得到第一个控制周期的磁力计零偏
Figure 480784DEST_PATH_IMAGE043
(11)；
Figure 697002DEST_PATH_IMAGE044
（1）；
Figure 766458DEST_PATH_IMAGE004
表示地球标准磁场向量的模值。
7.根据权利要求4所述的一种多旋翼飞行器的磁力计自动校准方法，其特征在于，步骤 5具体为：将步骤4得到的磁力计零偏
Figure 118942DEST_PATH_IMAGE045
，补偿给第二个控制周期读取的磁力计数据
Figure 61490DEST_PATH_IMAGE046
(12)，得到补偿后的第二个控制周期的磁力计数据
Figure 437108DEST_PATH_IMAGE047
(13)，计算 方法如下：
Figure 857725DEST_PATH_IMAGE048
（14）
步骤6具体为：将步骤5得到的补偿后的第二个控制周期的磁力计数据
Figure 330295DEST_PATH_IMAGE009
，进行新的 一轮零偏计算，即与步骤4一样，从而得到新的零偏值；
Figure 365116DEST_PATH_IMAGE049
（2）
经过若干个控制周期的计算后，最终使得零偏值趋向于一个稳定的值。
8.一种多旋翼飞行器的磁力计自动校准系统，其特征在于，包括校准算法模块、方差计算模块、偏航判定模块；
所述的偏航判定模块，通过获取陀螺仪传感器测得的偏航角速率，并将偏航角速率与设定阈值相比较，当偏航角速率大于设定阈值，则判定飞行器发生偏航，并启动校准算法模块；
所述的校准算法模块，实时获取磁力计传感器测得的磁力计数据，进行零偏计算，并将计算得到的磁力计零偏再与磁力计传感器测得的磁力计数据进行补偿，并将补偿后的磁力计数据发送给方差计算模块；
第一个控制周期的磁力计零偏的计算步骤为：
Figure 290346DEST_PATH_IMAGE001
(1)
Figure 452337DEST_PATH_IMAGE002
表示第一个控制周期计算得到的磁力计零偏，
Figure 779413DEST_PATH_IMAGE003
表示控制器参数，
Figure 63764DEST_PATH_IMAGE004
表示地 球标准磁场向量的模值，
Figure 663242DEST_PATH_IMAGE005
表示校准算法第一个控制周期获取的磁场向量的模值，
Figure 425661DEST_PATH_IMAGE006
表示控制误差的方向；
Figure 607244DEST_PATH_IMAGE007
表示第一个控制周期获取的磁力计数据，
Figure 179DEST_PATH_IMAGE008
表示初始 获取的磁力计数据；
补偿后的第二个控制周期磁力计数据
Figure 900002DEST_PATH_IMAGE009
等于
Figure 466113DEST_PATH_IMAGE010
减去
Figure 692082DEST_PATH_IMAGE011
,
Figure 318236DEST_PATH_IMAGE010
表示第二个控制 周期获取的磁力计数据；补偿后的第n个控制周期的磁力计数据
Figure 705355DEST_PATH_IMAGE012
等于
Figure 747260DEST_PATH_IMAGE013
减去
Figure 903435DEST_PATH_IMAGE014
，
Figure 700489DEST_PATH_IMAGE013
表示第n个控制周期获取的磁力计数据,
Figure 496276DEST_PATH_IMAGE015
表示第n-1个控制周期计算得到的磁 力计零偏；
第二个控制周期的磁力计零偏的计算步骤为：
Figure 669768DEST_PATH_IMAGE016
(2)
Figure 680450DEST_PATH_IMAGE017
表示第二个控制周期计算得到的磁力计零偏，
Figure 648406DEST_PATH_IMAGE009
表示补偿后的第二个控制 周期磁力计数据；
进行迭代计算，第n个控制周期的磁力计零偏的计算步骤为：
Figure 682221DEST_PATH_IMAGE018
(3)
Figure 128246DEST_PATH_IMAGE019
表示第n个控制周期计算得到的磁力计零偏，
Figure 259013DEST_PATH_IMAGE012
表示补偿后的第n个控制周期 磁力计数据；
Figure 584821DEST_PATH_IMAGE020
表示补偿后的第n-1个控制周期磁力计数据；
方差计算模块负责读取校准算法模块中补偿后的磁力计数据以及磁力计传感器测得 的磁力计数据，实时计算若干采样周期内经过零偏补偿后的磁场向量模值与地球标准磁场 向量模值
Figure 168249DEST_PATH_IMAGE004
的方差，并判定该方差大小是否小于或等于设定阈值，作为校准算法结束的 判定条件；当某采样周期没有零偏补偿后的磁场向量模值，则使用该采样周期测得的磁场 向量模值。
9.根据权利要求8所述的一种多旋翼飞行器的磁力计自动校准系统，其特征在于，所述的校准算法模块，包括零偏计算模块、磁力计数据补偿模块；
所述的零偏计算模块，实时获取磁力计传感器测得的磁力计数据和磁力计数据补偿模块计算得到的补偿后磁力计数据，进行零偏计算；
所述的磁力计数据补偿模块，将零偏计算模块计算得到的磁力计零偏与磁力计传感器测得的磁力计数据进行补偿，并将补偿后的磁力计数据发送给方差计算模块。
CN202010374939.XA 2020-05-07 2020-05-07 一种多旋翼飞行器的磁力计自动校准系统和方法 Active CN111289933B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010374939.XA CN111289933B (zh) 2020-05-07 2020-05-07 一种多旋翼飞行器的磁力计自动校准系统和方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010374939.XA CN111289933B (zh) 2020-05-07 2020-05-07 一种多旋翼飞行器的磁力计自动校准系统和方法

Publications (2)

Publication Number Publication Date
CN111289933A CN111289933A (zh) 2020-06-16
CN111289933B true CN111289933B (zh) 2020-09-01

Family

ID=71022684

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010374939.XA Active CN111289933B (zh) 2020-05-07 2020-05-07 一种多旋翼飞行器的磁力计自动校准系统和方法

Country Status (1)

Country Link
CN (1) CN111289933B (zh)

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120101766A1 (en) * 2010-10-26 2012-04-26 Research In Motion Limited System and Method for Determining Quality of Calibration Parameters for a Magnetometer
EP2487458A3 (en) * 2011-02-11 2014-11-26 BlackBerry Limited System and method for calibrating a magnetometer with visual affordance
RU2497139C1 (ru) * 2012-05-18 2013-10-27 Закрытое акционерное общество "МЕРА" Способ калибровки трехкомпонентного магнитометра
US20150019159A1 (en) * 2013-07-15 2015-01-15 Honeywell International Inc. System and method for magnetometer calibration and compensation
CN103954286B (zh) * 2014-04-24 2016-08-24 南京航空航天大学 微小卫星磁传感器多误差模型在轨迭代标定方法
CN104613983B (zh) * 2015-02-03 2018-03-02 中国航天时代电子公司 一种应用于微小型无人机的整机磁强计校准方法
CN106842094B (zh) * 2016-12-31 2020-04-24 深圳市优必选科技有限公司 磁力计校准的数据处理方法和装置
CN106997035A (zh) * 2017-03-29 2017-08-01 吉林大学 一种基于磁梯度不变量的磁梯度计校正方法
CN108051761A (zh) * 2017-09-06 2018-05-18 哈尔滨工程大学 一种三轴磁力计自身误差在线校正方法
CN107907142B (zh) * 2017-10-12 2020-09-11 歌尔科技有限公司 一种磁力计的初始校准方法、装置和磁力计

Also Published As

Publication number Publication date
CN111289933A (zh) 2020-06-16

Similar Documents

Publication Publication Date Title
CN109000612A (zh) 设备的角度估算方法、装置、摄像组件及飞行器
JP2013543976A (ja) ジャイロセンサ較正のデバイス及び方法
CN110174123B (zh) 一种磁传感器实时标定方法
CN109813308A (zh) 姿态估计方法、装置及计算机可读存储介质
CN108827301A (zh) 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN110361031B (zh) 一种基于回溯理论的imu全参数误差快速标定方法
CN111780786A (zh) 一种三轴tmr传感器在线校准方法
WO2012068364A2 (en) Apparatuses and methods for dynamic tracking and compensation of magnetic near field
WO2020164206A1 (zh) 一种旋转加速度计重力梯度仪标定方法
CN110954102A (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110579730A (zh) 一种三轴磁传感器的转向差校正方法
CN107860382B (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法
CN104819717B (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN108871323B (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN106468563A (zh) 一种机载磁传感器在线标定方法
CN115046539A (zh) Mems电子罗盘动态校准方法
CN110779553A (zh) 磁力计数据的校准方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN113291493B (zh) 一种卫星多敏感器融合姿态确定方法和系统
CN111289933B (zh) 一种多旋翼飞行器的磁力计自动校准系统和方法
CN112729332B (zh) 一种基于旋转调制的对准方法
CN112033405B (zh) 一种室内环境磁异常实时修正与导航方法及装置
CN112525144B (zh) 一种非线性姿态检测补偿方法及终端
CN110411481B (zh) 陀螺仪不正交误差的校准方法及校准系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant