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

本发明涉及多旋翼飞行器的技术领域，特别是涉及一种多旋翼飞行器的飞行过程中磁力计自动校准算法。

背景技术：

磁力计是多旋翼飞行器中的重要组成部分，主要提供对地球磁场向量的测量，通过与陀螺仪、加速度传感器进行多传感器数据融合，从而得到准确的姿态角信息。但是由于磁力计容易受到外界磁场的干扰，从而导致测量不准确，进而得不到准确的姿态角信息，对飞行稳定性的影响很大。因此，我们需要对磁力计数据进行校准，从而减小外界干扰对测量的影响。

目前，在传统的飞行器领域中，磁力计的校准具有以下特点：首先是只能在飞行前进行校准，需要通过遥控器或地面站设备发送校准命令，飞控接受到校准命令后，通过界面交互或led状态交互的方式指导操作人员进行水平旋转2圈以上以及垂直放置飞行器，旋转2圈以上，从而采集到各个方向的磁力计数据的最大值与最小值；

其次，得到最大值与最小值后，一般采用最小二乘法进行校准系数的计算，该方法需要涉及到大量的矩阵运算，所以对处理器芯片的运算能力有一定的要求。

综上所述，传统的磁力计校准方法需要在飞行前进行大量的旋转操作才能完成校准。这样需要用户进行人为的操作才可以完成，此外，如果是大轴距的飞行器，如轴距1m以上的飞行器，进行垂直旋转操作显然是非常不便的。因此，如何让飞行器在飞行过程中，自动完成磁力计的校准，且校准算法的计算量较小，是本发明需要解决的问题。

技术实现要素：

针对现有技术存在的不足，本发明提供一种使飞行器在飞行过程中自动完成校准的多旋翼飞行器的飞行过程中磁力计自动校准算法。

为实现上述目的，本发明采用的技术方案包括：一种多旋翼飞行器的磁力计自动校准方法，其特征在于，当飞行器进行偏航运动时，在每个控制周期对磁力计采集的数据采用计算得到的磁力计零偏值进行补偿；通过实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与地球标准磁场向量模值的方差是否小于或等于设定的阈值以判定是否完成磁力计校准。

一种多旋翼飞行器的磁力计自动校准方法，当飞行器进行偏航运动时，在每个控制周期对磁力计采集的数据采用计算得到的磁力计零偏值进行补偿；

第一个控制周期的磁力计零偏的计算步骤为：

(1)

表示第一个控制周期计算得到的磁力计零偏，表示控制器参数，表示地球标准磁场向量的模值，表示校准算法第一个控制周期获取的磁场向量模值，表示控制误差的方向；表示第一个控制周期获取的磁力计数据，表示初始获取的磁力计数据；为的模值；

补偿后的第二个控制周期磁力计数据等于减去,表示第二个控制周期获取的磁力计数据；补偿后的第n个控制周期的磁力计数据等于减去，表示第n个控制周期获取的磁力计数据,表示第n-1个控制周期计算得到的磁力计零偏；

第二个控制周期的磁力计零偏的计算步骤为：

(2)

表示第二个控制周期计算得到的磁力计零偏，表示补偿后的第二个控制周期磁力计数据；

进行迭代计算，第n个控制周期的磁力计零偏的计算步骤为：

(3)

表示第n个控制周期计算得到的磁力计零偏，表示补偿后的第n个控制周期磁力计数据；表示补偿后的第n-1个控制周期磁力计数据。

通过实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与地球标准磁场向量模值的方差是否小于或等于设定的阈值以判定是否完成磁力计校准。

具体地，所述的若干采样周期为25个采样周期，方差计算步骤为：

（4）

为第p个控制周期得到补偿后磁力计数据时计算得到的25个采样周期的方差，p为正整数；当p为大于25的正整数，…均为补偿后的某个控制周期的磁力计数据；当p为小于或等于25的正整数且不为1时，…部分为补偿后的某个控制周期的磁力计数据，部分为测得的未经补偿的磁力计数据；当p=1，…均为测得的未经补偿的磁力计数据。在每一个控制周期得到补偿后磁力计数据后，都计算方差以判定是否完成磁力计校准。另外，尽管在第一个控制周期还没有得到补偿后磁力计数据（获得测量值），也计算方差。

一种多旋翼飞行器的磁力计自动校准方法，具体包括如下步骤：

步骤1：实时计算若干采样周期测得的磁场向量与标准磁场向量的方差，记录初始的磁场向量；

步骤2：实时判定飞行器的偏航角速率数据（该数据与同时测得）是否大于设定的阈值，若判断为是，则进入步骤3；

步骤3：测量获取当前第一个控制周期读取的磁力计数据；

步骤4：计算磁力计零偏；

步骤5：将步骤4得到的磁力计零偏，补偿给下一个控制周期测得的磁力计数据，得到补偿后的第二个控制周期的磁力计数据；

步骤6：将步骤5得到的补偿后的第二个控制周期的磁力计数据，进行新的一轮零偏计算，从而得到新的磁力计零偏值，新的磁力计零偏值用于补偿给下一个控制周期测得的磁力计数据；经过若干个控制周期的计算后，最终使得零偏值趋向于一个稳定的值；

步骤7，判断当前补偿零偏后的磁力计数据的方差是否小于或等于设定的阈值，如果小于或等于设定的阈值，则判定磁力计校准已完成，校准结束，退出校准算法。

这里的步骤7实际上是随着步骤3-6的初始计算和迭代计算持续进行的，在每一个控制周期得到补偿后磁力计数据并计算得到零偏值后，都计算方差以判定是否完成磁力计校准。另外，尽管在第一个控制周期还没有得到补偿后磁力计数据（获得测量值），也计算方差以判定是否完成磁力计校准。

在每个控制周期，读取的磁力计数据与零偏补偿后得到补偿零偏后的磁力计数据（例外情况：第一个控制周期采用)，计算零偏（这个零偏是用于补偿下一个周期的磁力计数据的），然后计算方差，当方差大于0.02，则校准算法进入下一个控制周期；如果方差小于或等于0.02，那么校准算法结束。

步骤1具体为：飞行器上电后，磁力计开始工作，从而得到测量的x、y、z三个坐标轴的磁场数据，，，将其进行求模计算；

（5）

根据经验值，建立一个数组变量，保存连续25个采样周期的数据；为磁场向量模值；

设地球标准磁场向量为0.5高斯，计算每25个采样周期内的方差值，

（6）

实时更新记录初始磁场向量，(7)。

步骤3中的磁力计数据(8)；

步骤4具体为：

首先计算磁力计向量变化的方向，将校准算法第一个控制周期读取的磁力计数据与初始磁力计数据进行做差，即：

（9）

以及当前磁力计数据的模值与标准向量模值的误差大小，即：

（10）

最后，将式（9）、式（10）与设定的控制参数kp相乘，kp默认值为0.6，参数kp决定了收敛至稳定的零偏的快慢，得到第一个控制周期的零偏估计值(11)；

（1）。

步骤5具体为：将步骤4得到的零偏，补偿给第二个控制周期读取的磁力计数据(12)，得到补偿后的第二个控制周期的磁力计数据(13)，计算方法如下：

（14）

步骤6具体为：将步骤5得到的补偿后的第二个控制周期的磁力计数据，进行新的一轮零偏计算，即与步骤4一样，从而得到新的零偏值；

（2）

经过若干个控制周期的计算后，最终使得零偏值趋向于一个稳定的值。值得一提的是，当迭代计算的零偏估计值不稳定时，方差一定是大于设定的阈值的。

本发明还提供一种多旋翼飞行器的磁力计自动校准系统，包括校准算法模块、方差计算模块、偏航判定模块；

所述的偏航判定模块，通过获取陀螺仪传感器测得的偏航角速率，并将偏航角速率与设定阈值相比较，当偏航角速率大于设定阈值，则判定飞行器发生偏航，并启动校准算法模块；

所述的校准算法模块，实时获取磁力计传感器测得的磁力计数据，进行零偏计算，并将计算得到的磁力计零偏再与磁力计传感器测得的磁力计数据进行补偿，并将补偿后的磁力计数据发送给方差计算模块；

方差计算模块负责读取校准算法模块中补偿后的磁力计数据以及磁力计传感器测得的磁力计数据，实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与标准地磁向量模值的方差，并判定该方差大小是否小于或等于设定阈值，作为校准算法结束的判定条件。

所述的校准算法模块，包括零偏计算模块、磁力计数据补偿模块；所述的零偏计算模块，实时获取磁力计传感器测得的磁力计数据和磁力计数据补偿模块计算得到的补偿后磁力计数据，进行零偏计算；所述的磁力计数据补偿模块，将零偏计算模块计算得到的磁力计零偏与磁力计传感器测得的磁力计数据进行补偿，并将补偿后的磁力计数据发送给方差计算模块。

可采用adis16470作为陀螺仪传感器，由陀螺仪传感器测得偏航角速率。ist8308作为磁力计传感器（地磁传感器），由磁力计传感器测得（即采集）磁场向量（即磁力计数据）。

例如，陀螺仪传感器数据读取模块（其为现有技术，软件模块）用于读取陀螺仪传感器测量的偏航角速率，而偏航判定模块则从陀螺仪传感器数据读取模块获取偏航角速率数据。磁力计传感器数据读取模块（其为现有技术，软件模块）用于读取磁力计传感器测得的实时磁场数据，而校准算法的零偏计算模块与磁力计数据补偿模块则从磁力计传感器数据读取模块获取实时磁场数据，方差计算模块也从磁力计传感器数据读取模块获取实时磁场数据，此外，方差计算模块还从磁力计数据补偿模块获取补偿后的磁力计数据。

综上所述，本发明技术方案的有益效果是：不需要人为的进行校准操作，由飞行器在空中飞行时自动完成；算法简单，没有大量的矩阵乘除运算；明确了算法的启动条件与退出条件，减少了故障率。

附图说明

图1为本发明的一个实施例提供的磁力计自动校准方法的流程示意图；

图2为本发明的一个实施例提供的磁力计自动校准方法的计算结果示意图。

具体实施方式

本发明实施例提供一种飞行器的磁力计校准方法，解决传统校准方法，在飞行前需要进行至少两个平面的旋转操作，提高飞行器的使用便捷性，同时还提出以磁力计数据校准前后的方差作为判定校准算法的完成。为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述：

磁力计校准主要针对的是由于机身造成的固定磁干扰，导致的磁力计数据产生了固定的零偏值。常见的磁力计校准方法主要是在飞行前，通过将飞行前水平旋转以及垂直旋转2圈以上，获取磁力计的最大和最小的磁场数据，从而进入校准算法进行零偏计算。然而，实际应用过程中，如果飞行器机身过大，如轴距大于1m以上的飞行器，要将其水平或垂直旋转会有诸多不便，因此本发明提出一种飞行器在飞行过程中自动完成校准的算法。本发明主要理论依据是，无论飞行器的偏航角为什么角度值，实时测量的地球磁场向量应该都是一样的。因此，通过在飞行过程中，不断的进行偏航运动，校正相邻时间周期的磁场向量为一个固定值。所以下面的等式成立：

（15）

上式中，表示机体坐标系下，由磁力计传感器测量得到的磁力计数据，表示机体坐标系下，所需估计的磁力计零偏，即校准算法的结果，表示地理坐标系下地球的标准磁场向量模值，地球的标准磁场向量模值是一个已知的固定值，约为0.5高斯。在下面的具体实施例中，、、…均为在不同周期磁力计传感器测量得到的磁力计数据（磁力计数据又称为磁场向量、磁场数据）。

当飞行器正在进行偏航运动，则启动自动校准算法。将校准算法转换成一个控制问题，控制目标是将测量得到的磁场向量能实时跟踪参考地磁向量，也就是说(16)。这里，我们引入一个比例控制，设控制参数（即控制器参数）为，则

（1）

式中，表示第一个控制周期计算得到的零偏，表示控制器参数，表示地球标准磁场向量的模值，这里设为0.5高斯，表示校准算法第一个控制周期获取的磁场向量（即磁力计数据），表示了第一个控制周期控制误差的方向。

将第一个控制计算周期得到的补偿到第二个控制周期获取的磁场向量，得到补偿后的磁场向量，接着迭代计算：

（2）

得到校准算法第二个控制周期的零偏估计值，同样补偿给第三个控制周期获取的地磁测量值（即磁场向量、磁力计数据），从而不断的迭代下去。

前两个控制周期相当于是校准算法的初始部分，从第三个控制周期开始直到第n个控制周期的公式如下：

（3）

第n个控制周期的零偏计算，需要第n个控制周期补偿后的磁力计数据，以及第n-1个控制周期补偿后的磁力计数据（n=3、4、5…n，n为校准算法的总控制周期数）。补偿后的磁力计数据即为校准偏移后的磁力计数据。n由方差判定结果决定，例如p=200时小于0.02而p=1-199时均大于0.02，那么n即为200。

其中，本发明中的控制周期，指的是校准算法运行的周期，即在第一个控制周期进行零偏计算的时候，需要第一个控制周期的地磁测量值以及初始记录的地磁测量值。然后第一个控制周期计算得到的零偏，代入到第二个控制周期的地磁测量值，得到补偿后的地磁测量值，再与第一个控制周期的地磁测量值，进行第二个控制周期的零偏计算。经过两个控制周期计算以后，就能得到连续两个周期的补偿后的地磁数据，即完成了初始计算，后续则按如下公式进行迭代计算，直至算法完成。

（3）

此外，由于实际飞行过程中，除了机身的固定干扰造成的零点偏移以外，飞行器的磁力计数据还会受到外界的随机干扰，如果飞行过程中一直进行自动校准，则会出现误计算的现象，因此本发明还提出了自动停止校准的方法。

实时计算若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与标准地球的磁场向量模值的方差（这里用标准模值代替了均值，所以这个方差表示与标准模值的幅值波动大小），当方差小于或等于一定的阈值时，则认为校准已经完成，自动退出该算法。所述的测得的磁场向量模值为没有零偏补偿的测得的磁场向量的模值，这是由于若干采样周期中某些采样周期还没有进入零偏计算（因此没有零偏补偿）。

图1为本发明的一个实施例提供的一种磁力计校准方法的流程示意图。如图1所示，该方法包括：

步骤1，实时计算若干采样周期测得的磁场向量（例如25个采样周期）与标准磁场向量的方差（这里校准算法没有启动，则相当于计算的是原始测量数据与标准地磁向量的方差），记录初始的磁场向量。

首先飞行器上电后，磁力计开始工作，从而得到测量的x、y、z三个坐标轴的磁场数据，，，将其进行求模计算。

（5）

建立一个数组变量，保存连续25个采样周期的数据。即磁场向量模值。

设地球标准磁场向量为0.5高斯，代入下面公式后，计算每25个周期内的方差值，

（6）

实时更新记录初始磁场向量，（7）。

、、分别为初始测得的x、y、z三个坐标轴的磁场数据。

步骤2，通过陀螺仪传感器测得飞行器的偏航角速率，判定飞行器的偏航角速率数据是否大于设定的阈值，阈值设为0.4rad/s，若判断为是，则正式进入校准算法步骤3；若判断为否，则重新返回步骤1，计算与标准磁场向量的方差，并记录校准算法前的磁场向量（相当于校准算法没有启动的时候，一直在更新，将当前的磁场数据进行赋值）；

步骤3，测量获取第一个控制周期的磁力计数据，（8）；

、、分别为第一控制周期获取的x、y、z三个坐标轴的磁场数据。

步骤4，计算磁力计零偏。

首先计算磁力计向量变化的方向，即：

（9）

以及第一个控制周期读取的磁力计数据的模值与标准向量模值的误差大小，即：

（10）

最后，将式（9）、式（10）与设定的控制参数相乘，默认值为0.6，参数决定了收敛至稳定的零偏参数的快慢，得到第一个周期的零偏估计值（11）。

（1）

、、分别为第一控制周期计算得出的x、y、z三个坐标轴的磁场数据零偏。

步骤5，将步骤4得到的零偏，补偿给第二个控制周期读取的磁力计数据（12），得到补偿后的磁力计数据（13），计算方法如下：

（14）

、、分别为第二控制周期获取的x、y、z三个坐标轴的磁场数据。、、分别为补偿后的第二控制周期的x、y、z三个坐标轴的磁场数据。

步骤6，将步骤5得到的补偿后的磁力计数据，进行第二个控制周期的零偏计算，即与步骤4一样，从而得到第二个控制周期的零偏值。

（2）

经过两个控制周期的计算以后，则完成了校准算法的初始化部分，即可以得到连续两个周期的补偿后的磁力计数据，从而迭代计算如下：

（17）

…

经过n个周期的计算后，最终使得零偏估计值趋向于一个稳定的值(实际上，校准算法运行的最后一段时间的零偏值趋向于一个稳定的值)。

（18）

（3）

、、分别为补偿后的第n个控制周期的x、y、z三个坐标轴的磁场数据。、、分别为第n-1个控制周期计算出的x、y、z三个坐标轴的零偏。、、为第n个控制周期读取的x、y、z三个坐标轴的磁场数据。

第n个控制周期的零偏，需要当前n时刻（即第n个控制周期）的补偿后的磁力计数据与上一时刻n-1时刻（即第n-1个控制周期）的补偿后的磁力计数据。

步骤7，判断当前补偿零偏后的磁力计数据的方差（即若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值与地球标准磁场向量模值的方差），是否小于或等于设定的阈值0.02，如果小于或等于设定的阈值，则退出校准算法，认定校准已完成；如果大于设定的阈值，则校准算法仍继续计算。

这里的步骤7实际上是随着步骤3-6的初始计算和迭代计算持续进行的。

例如，在第一个控制周期读取、再计算得到，再通过公式（4）计算方差（p=1）并与阈值（0.02）比较，如果方差（p=1）小于或等于阈值0.02，则判定磁力计校准已完成，退出校准算法，校准算法的总控制周期仅为1。如果方差（p=1）大于阈值0.02，则校准算法进入第二个控制周期，在第二个控制周期读取、再用补偿得到、再计算得到，再通过公式（4）计算方差（p=2）。当方差（p=2）小于或等于阈值0.02，则判定磁力计校准已完成，退出校准算法，校准算法的总控制周期为2。当方差（p=2）大于阈值0.02，则校准算法进入第三个控制周期。在第三个控制周期读取、再用补偿得到、再计算得到，再通过公式（4）计算方差（p=3）并与阈值（0.02）比较；如果方差（p=3）小于或等于阈值0.02，则判定磁力计校准已完成，退出校准算法，校准算法的总控制周期为3。如果方差（p=3）大于阈值0.02，则校准算法进入第四个控制周期，依此类推。

（4）

为第p个控制周期得到补偿后磁力计数据时计算得到的25个采样周期的方差，p为正整数；当p为大于25的正整数，…均为补偿后的某个（由下标p-24,p-23,p-22…p-1或p表示）控制周期的磁力计数据(即某个控制周期的补偿后的磁力计数据，例如表示补偿后的第p-24个控制周期的磁力计数据，表示补偿后的第p个控制周期的磁力计数据)；当p为小于或等于25的正整数且不为1时，…部分为补偿后的某个控制周期的磁力计数据，部分为测得的未经补偿的磁力计数据；当p=1，…均为测得的未经补偿的磁力计数据。

同公式（6）一样，…包括25个磁场数据，下标分别为p-24、p-23、p-22…p-2、p-1、p。这里的p与上面的控制周期n等同。

当p=1时，也就是处于第一个控制周期，此时25个磁场数据包括、，另外23个磁场数据是测得之前的23个采样周期分别测得的磁场数据；当p=2时，也就是处于第二个控制周期，此时25个磁场数据包括、、，另外22个磁场数据是测得之前的22个采样周期分别测得的磁场数据；当p=3时，25个磁场数据包括补偿后的第三个控制周期的磁力计数据、、、，另外21个磁场数据是测得之前的21个采样周期分别测得的磁场数据；依此类推；当p=25时，25个磁场数据包括补偿后的第2到25个控制周期的磁力计数据以及。当p=26时，25个磁场数据包括补偿后的第2到26个控制周期的磁力计数据。依此类推。

若干采样周期内经过零偏补偿后的磁场向量模值或测得的磁场向量模值…，在选用时，只要有补偿后的磁场向量的模值，则使用补偿值（即补偿后的磁场向量的模值）；在没有补偿值的情况下，才使用测得的磁场向量的模值。

在通常情况下，而校准算法运行时间约10s，因此，一般来说，结束校准算法时…均为补偿后的磁力计数据。

本文中，飞行器上电后，25个采样周期内的磁场数据的模值与地球标准磁场向量模值的方差计算始终持续进行。而控制周期，指的是校准算法运行的周期。采样周期相当于飞行器上电后这个采样程序持续运行，计算零偏的时候控制周期等同于采样周期。

需要解释的是，飞行器上电一开始的时候理论上不存在立刻进入校准的情况，即启动校准算法前一定已历经25个以上的采样周期（能计算得到方差，对应于本实施例），实际25个采样周期大概是0.05s；即便在不满25个采样周期的前提下进入了校准算法，那么可以等到满25个采样周期开始计算方差后再判定是否退出校准算法，由于25个采样周期大概是0.05s，时间很短基本没有影响。

图2为本实施例提供的一个算法结果示意图，图中显示，x轴的磁力计数据具有0.3高斯左右的零偏，经过算法计算后，10s左右，就能估计出这个0.3高斯的零偏，而y轴与z轴则在0附近变化，所以本算法有效。

以上显示描述了本发明的基本原理、主要特征以及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落进要求保护本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。