CN106017510A - 一种无人机地磁传感器的动态校准方法 - Google Patents

Abstract

本发明属于无人机技术领域，提供了一种无人机地磁传感器的动态校准方法，该动态校准方法中包括：S1分别获取无人机中各个旋翼的转速控制量、地磁三轴数据以及无人机电池电压值并建立函数关系；S2基于步骤S1中建立的函数关系确定无人机飞行时单个旋翼对地磁三轴数据的干扰量；S3基于步骤S2中单个旋翼产生的地磁干扰量确定该无人机飞行时地磁三轴方向的实际干扰量；S4基于步骤S3中得出的无人机飞行时地磁三轴方向所受的实际干扰量对无人机地磁传感器进行实时校准，得到校准后的地磁数据。该方法有效排除了电机等机载设备的干扰，使得地磁模块可以直接放置在无人机内部，相比于仅使用静态校准的方法，无需单独设计地磁位置，无人机的整体结构变得更加简单，紧凑。

Description

一种无人机地磁传感器的动态校准方法

技术领域

本发明属于无人机技术领域，尤其涉及一种无人机地磁传感器的动态校准方法。

背景技术

随着多旋翼无人机技术的成熟，用于感知无人机航向角的地磁传感器应用的越来越广泛。然而，由于地球地磁场非常微弱，地磁传感器很容易受到诸如电机，电源线等机载设备以及所处地理环境的磁场干扰，无人机在飞行的过程中，需要及时对地磁数据进行校正。

目前常用的校准方法是静态校准法，其首先将无人机在地面上分别绕Z轴和X轴(或Y轴)旋转一周得到X，Y，Z三轴的数据，随后通过拟合数据曲线得到校准参数，以此无人机在起飞后根据校准参数实时修正地磁数据。

但是，使用该静态校准法校准地磁数据存在诸多缺点，如，其只能修正地磁传感器静态偏差和地理磁场变化导致的偏差，无法排除由电机、电源线等机载设备对传感器造成的干扰，无人机实际飞行时仍会造成地磁数据的干扰，进而影响偏航角的计算，导致无人机飞行方向出现较大偏差；当无人机仅使用静态校准法校准地磁时，为避免地磁受电机等机载设备产生的磁场干扰，需要单独设计地磁模块在无人机中的位置，这使得无人机整体结构变得复杂，庞大。

发明内容

针对上述问题，本发明旨在提供一种无人机地磁传感器的动态校准方法，有效解决了现有静态校准法无法排除无人机机载设备干扰的问题。

本发明提供的技术方案如下：

一种无人机地磁传感器的动态校准方法，包括：

S1分别获取无人机中各个旋翼的转速控制量、地磁三轴数据以及无人机电池电压值并建立函数关系；

S2基于步骤S1中建立的函数关系确定无人机飞行时单个旋翼对地磁三轴数据的干扰量；

S3基于步骤S2中单个旋翼产生的地磁干扰量确定无人机飞行时地磁三轴方向的实际干扰量；

S4基于步骤S3中得出的无人机飞行时地磁三轴方向所受的实际干扰量，对无人机地磁传感器进行实时校准，得到校准后的地磁数据。

进一步优选地，在步骤S1中具体包括：

S11在无人机电池满电量的情况下，依次单独向无人机的各个旋翼发送转速控制量，直到无人机电池出现低电量报警；

S12实时采集该旋翼的转速控制量、地磁三轴数据以及无人机电池电压值；

S13获取各旋翼均静止时地磁三轴数据的平均值，并将其作为地磁数据参考值；

S14建立地磁三轴数据、地磁数据参考值以及无人机电池电压值与转速控制量之间的函数关系。

进一步优选地，在步骤S11中，单独向无人机的各个旋翼发送的转速控制量具体为：从零线性增加至旋翼的最大转速，再由最大转速线性减小至零；且往单个旋翼发送转速控制量时，其他旋翼的转速控制量均为零。

进一步优选地，在步骤S14中，地磁三轴数据、地磁数据参考值以及无人机电池电压值与转速控制量之间的函数关系具体为：

$\frac{mxi-mx0}{vi}=f_{xi}\left(RotorInputi\right)$

$\frac{myi-my0}{vi}=f_{yi}\left(RotorInputi\right)$

$\frac{mzi-mz0}{vi}=f_{zi}\left(RotorInputi\right)$

其中，RotorInput i(i＝1,2,3...N)为各个旋翼的转速控制量；mxi(i＝1,2,3...N)，myi(i＝1,2,3...N)和mzi(i＝1,2,3...N)分别为地磁三轴数据；mx 0,my 0,mz 0分别为地磁数据参考值；vi(i＝1,2,3...N)为各个旋翼旋转时对应的无人机电池电压值；函数f_xi，f_yi以及f_zi为多项式函数。

进一步优选地，在步骤S2中，无人机飞行时单个旋翼对地磁三轴数据的干扰量Δmxi,Δmyi以及Δmzi具体为：

$\begin{array}{c}\mathrm{\Δ}mxi=Voltage\·f_{xi}\left(RotorInputi\right)\\ \mathrm{\Δ}myi=Voltage\·f_{yi}\left(RotorInputi\right)\\ \mathrm{\Δ}mzi=Voltage\·f_{zi}\left(RotorInputi\right)\end{array},$

其中，

$\begin{array}{c}{f}_{xi}\left(RotorInputi\right)=pxi1\·RotorInput{i}^3+pxi2\·RotorInput{i}^2+pxi3\·RotorInputi\\ f_{yi}\left(RotorInputi\right)=pyi1\·RotorInput{i}^3+pyi2\·RotorInput{i}^2+pyi3\·RotorInputi\\ f_{zi}\left(RotorInputi\right)=pzi1\·RotorInput{i}^3+pzi2\·RotorInput{i}^2+pzi3\·RotorInputi\end{array},$

系数pxi1～pxi3、pyi 1～pyi 3和pzi 1～pzi 3由对进行

多项式拟合得到；Δmxi,Δmyi以及Δmzi还可表示为矩阵形式：

$\left[\begin{array}{c}\mathrm{\Δ}mxi\\ \mathrm{\Δ}myi\\ \mathrm{\Δ}mzi\end{array}\right]=Voltage\·(MATRIX{3}_i\·RotorInput{i}^3+MATRIX{2}_i\·RotorInput{i}^2+MATRIX{1}_i\·RotorInputi)$

其中，MATRIX 3_i,MATRIX 2_i,MATRIX 1_i分别为MATRIX 3，MATRIX 2和MATRIX 1的各个列向量；RotorInput i为各个旋翼的转速控制量；Δmxi,Δmyi,Δmzi为各旋翼旋转时对三轴地磁产生的干扰值，N为旋翼数量，Voltage为无人机电池电压值；且

$MATRIX3=\left[\begin{array}{ccccc}px11& px21& px31& ...& pxN1\\ py11& py21& py31& ...& pyN1\\ pz11& pz21& pz31& ...& pzN1\end{array}\right]$

$MATRIX2=\left[\begin{array}{ccccc}px12& px22& px32& ...& pxN2\\ py12& py22& py32& ...& pyN2\\ pz12& pz22& pz32& ...& pzN2\end{array}\right]$

$MATRIX1=\left[\begin{array}{ccccc}px13& px23& px33& ...& pxN3\\ py13& py23& py33& ...& pyN3\\ pz13& pz23& pz33& ...& pzN3\end{array}\right]$

其中，系数矩阵MATRIX 3为各个旋翼拟合数据中3次方多项式系数，MATRIX 2为各个旋翼拟合数据中2次方多项式系数，MATRIX 1为各个旋翼拟合数据中1次方多项式系数，每行分别代表地磁X、Y和Z轴的拟合系数。

进一步优选地，在步骤S3中，无人机飞行时地磁三轴数据的实际干扰量为各旋翼对地磁数据产生的干扰量的叠加，Δmx、Δmy以及Δmz具体为：

$\begin{array}{c}\mathrm{\Δ}mx=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}mxi\\ \mathrm{\Δ}my=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}myi\\ \mathrm{\Δ}mz=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}mzi\end{array},$

其中i为旋翼编号，N为旋翼数量。

进一步优选地，在步骤S4中，校准后的无人机地磁数据mx_cal、my_cal以及mz_cal具体为：

mx_cal＝mx_measure-Δmx

my_cal＝my_measure-Δmy

mz_cal＝mz_measure-Δmz

其中，mx_measure、my_measure以及mz_measure分别为地磁三轴方向的测量值。

本发明提供的无人机地磁传感器的动态校准方法，其有益效果在于：

本发明提出的动态校准地磁传感器的方法，其根据各个旋翼的转速控制量和无人机电池电压值的变化实时对地磁数据进行补偿，排除了旋翼电机旋转和无人机电池电压值对地磁数据的干扰，补偿数据精确，在出厂前只需获取一次校准参数，用户在使用中无需再次获取。

另外，由于本发明提出的动态校准地磁传感器的方法有效排除了电机等机载设备的干扰，使得地磁模块可以直接放置在无人机内部，相比于仅使用静态校准的方法，无需单独设计地磁位置，无人机的整体结构变得更加简单，紧凑。

附图说明

图1为本发明中无人机地磁传感器的动态校准方法的流程示意图；

图2为本发明中转速控制量随时间的变化曲线；

图3为本发明中1号旋翼的转速与三轴地磁数据以及电池电压的拟合曲线。

具体实施方式

下面结合附图和具体实施方式，对本发明作进一步详细说明。需要说明的是，下面描述的本发明的特定细节仅为说明本发明用，并不构成对本发明的限制。根据所描述的本发明的教导作出的任何修改和变形也在本发明的范围内。

如图1所示为本发明提供的无人机地磁传感器的动态校准方法的流程示意图，包括：S1分别获取无人机中各个旋翼的转速控制量、地磁三轴数据以及无人机电池电压值并建立函数关系；S2基于步骤S1中建立的函数关系确定无人机飞行时单个旋翼对地磁三轴数据的干扰量；S3基于步骤S2中单个旋翼对地磁三轴数据产生的干扰量，确定无人机飞行时地磁三轴方向的实际干扰量；S4基于步骤S3中得出的无人机地磁三轴数据所受实际干扰量，对无人机地磁传感器进行实时校准，得到校准后的地磁数据。

我们知道，多旋翼无人机包括多个旋翼，可分别记为1号旋翼、2号旋翼、......、N号旋翼，其中N为旋翼数量。具体来说，在步骤S1中，首先，将多旋翼无人机静止固定在一平地上，且使无人机电池处于满电量状态。在动态校准的过程中，依次单独向无人机的各个旋翼发送转速控制量直到无人机电池出现低电量报警，在这个过程中，实时采集该旋翼的转速控制量、地磁三轴数据以及无人机电池电压值并以此建立函数关系。更具体来说，上述单独向无人机的各个旋翼发送的转速控制量具体为：从零线性增加至旋翼的最大转速，再由最大转速线性减小至零；且往单个旋翼发送转速控制量时，其他旋翼的转速控制量均为零。

在一个具体实施例中，仅对1号旋翼给转速值指令，此时除1号旋翼以外的其他旋翼的转速控制量均为零；且如图2所示，1号旋翼给的转速值(转速控制量)变化规律为从零缓慢增至最大(本实施例中最大值为65535)，再由最大值缓慢降为零；之后，依次对其他旋翼重复上述对1号旋翼的控制并循环操作下去，直至电池电量报警。在此过程中，飞控系统实时采集各个旋翼转速数据RotorInput i(i＝1,2,3...N)和地磁三轴数据(X轴，Y轴和Z轴)(mxi(i＝1,2,3...N)、myi(i＝1,2,3...N)和mzi(i＝1,2,3...N))以及无人机电池电压值vi(i＝1,2,3...N)，采样频率为50Hz(赫兹)。

之后，对各旋翼均静止时的地磁三轴数据取平均(本实施例中转速控制量小于2000时旋翼静止)，记为mx 0、my 0以及mz 0，作为不受干扰的地磁数据，即地磁数据参考值。

将测试1号旋翼时的数据提取出来(其他旋翼方法类似)，分别记为RotorInput 1、mx 1、my 1、mz 1以及v1。取RotorInput 1为自变量，(mx 1-mx 0)/v1、(my 1-my 0)/v1以及(mz 1-mz 0)/v1为因变量，分别对

RotorInput 1和(mx 1-mx 0)/v1，

RotorInput 1和(my 1-my 0)/v1，

RotorInput 1和(mz 1-mz 0)/v1，

进行3阶多项式拟合，如图3所示，得到三组拟合系数：

px 1＝[px 11 px 12 px 13 px 14]，

py 1＝[py 11 py 12 py 13 py 14]，

pz 1＝[pz 11 pz 12 pz 13 pz 14]，

具体，在上述系数pxij中，i代表旋翼编号，j代表系数序号；pyij、pzij同理。

重复上述拟合步骤，分别计算出其他编号旋翼对应的拟合系数，记为：

px 2,px 3,...pxN，

py 2,py 3,...pyN，

pz 2,pz 3,...pzN，

其中，N为旋翼数量。

基于上述拟合步骤得到的拟合系数，构建系数矩阵，分别为：

$MATRIX3=\left[\begin{array}{ccccc}px11& px21& px31& ...& pxN1\\ py11& py21& py31& ...& pyN1\\ pz11& pz21& pz31& ...& pzN1\end{array}\right]$

$MATRIX2=\left[\begin{array}{ccccc}px12& px22& px32& ...& pxN2\\ py12& py22& py32& ...& pyN2\\ pz12& pz22& pz32& ...& pzN2\end{array}\right]$

$MATRIX1=\left[\begin{array}{ccccc}px13& px23& px33& ...& pxN3\\ py13& py23& py33& ...& pyN3\\ pz13& pz23& pz33& ...& pzN3\end{array}\right]$

具体，上述系数矩阵MATRIX 3为3次方多项式系数，MATRIX 2为2次方多项式系数，MATRIX 1为1次方多项式系数，由于0次方系数接近零，对结果影响几乎为零，为了减少计算量，省去0次方系数矩阵。更具体来说，上述MATRIX 3各列代表每个旋翼得出的拟合系数中的3次方项系数；MATRIX 2各列代表每个旋翼的拟合系数中的2次方项系数；MATRIX 1各列代表每个旋翼的拟合系数中的1次方项系数，每行分别代表地磁X、Y和Z轴的拟合系数。

基于上述拟合结果，无人机飞行时地磁三轴数据所受单个旋翼的干扰量Δmxi,Δmyi以及Δmzi具体为：

Δmxi＝Voltage·f_xi(RotorInput i)

Δmyi＝Voltage·f_yi(RotorInput i)

Δmzi＝Voltage·f_zi(RotorInput i)

其中，

$\begin{array}{c}{f}_{xi}\left(RotorInputi\right)=pxi1\·RotorInput{i}^3+pxi2\·RotorInput{i}^2+pxi3\·RotorInputi\\ f_{yi}\left(RotorInputi\right)=pyi1\·RotorInput{i}^3+pyi2\·RotorInput{i}^2+pyi3\·RotorInputi\\ f_{zi}\left(RotorInputi\right)=pzi1\·RotorInput{i}^3+pzi2\·RotorInput{i}^2+pzi3\·RotorInputi\end{array},$

Δmxi,Δmyi以及Δmzi还可表示为矩阵形式：即：

$\left[\begin{array}{c}\mathrm{\Δ}mxi\\ \mathrm{\Δ}myi\\ \mathrm{\Δ}mzi\end{array}\right]=Voltage\·(MATRIX{3}_i\·RotorInput{i}^3+MATRIX{2}_i\·RotorInput{i}^2+MATRIX{1}_i\·RotorInputi)$

其中，MATRIX 3_i,MATRIX 2_i,MATRIX 1_i分别为MATRIX 3，MATRIX 2和MATRIX 1的各个列向量；RotorInput i为各个旋翼的转速控制量；Δmxi,Δmyi,Δmzi为各旋翼旋转时对三轴地磁产生的干扰值，N为旋翼数量，Voltage为无人机电池电压值。

将每个旋翼对三轴数据的干扰量进行叠加，得出地磁各个轴数据的实时偏移量，分别为：

$\begin{array}{c}\mathrm{\Δ}mx=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}mxi\\ \mathrm{\Δ}my=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}myi\\ \mathrm{\Δ}mz=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}mzi\end{array},$

其中_i为旋翼编号，N为旋翼数量。

以此得到地磁校准后的数据分别为：

mx_cal＝mx_measure-Δmx

my_cal＝my_measure-Δmy

mz_cal＝mz_measure-Δmz

其中，mx_measure、my_measure以及mz_measure为地磁传感器的测量值，mx_cal、my_cal以及mz_cal为地磁经动态校准后的三轴数据。

以上通过分别描述每个过程的实施场景案例，详细描述了本发明，本领域的技术人员应能理解。在不脱离本发明实质的范围内，可以作修改和变形。

Claims (7)

1.一种无人机地磁传感器的动态校准方法，其特征在于，所述动态校准方法中包括：

S1分别获取无人机中各个旋翼的转速控制量、地磁三轴数据以及无人机电池电压值并建立函数关系；

S2基于步骤S1中建立的函数关系确定无人机飞行时单个旋翼对地磁三轴数据的干扰量；

S3基于步骤S2中单个旋翼产生的地磁干扰量确定无人机飞行时地磁三轴方向的实际干扰量；

S4基于步骤S3中得出的无人机飞行时地磁三轴方向所受的实际干扰量，对无人机地磁传感器进行实时校准，得到校准后的地磁数据。

2.如权利要求1所述的动态校准方法，其特征在于，在步骤S1中具体包括：

S11在无人机电池满电量的情况下，依次单独向无人机的各个旋翼发送转速控制量，直到无人机电池出现低电量报警；

S12实时采集该旋翼的转速控制量、地磁三轴数据以及无人机电池电压值；

S13获取各旋翼静止时地磁三轴数据的平均值，并将其作为地磁数据参考值；

S14建立地磁三轴数据、地磁数据参考值以及无人机电池电压值与转速控制量之间的函数关系。

3.如权利要求2所述的动态校准方法，其特征在于，在步骤S11中，单独向无人机的各个旋翼发送的转速控制量具体为：从零线性增加至旋翼的最大转速，再由最大转速线性减小至零；且往单个旋翼发送转速控制量时，其他旋翼的转速控制量均为零。

4.如权利要求2或3所述的动态校准方法，其特征在于，在步骤S14中，地磁三轴数据、地磁数据参考值以及无人机电池电压值与转速控制量之间的函数关系具体为：

$\frac{mxi-mx0}{vi}=f_{xi}\left(RotorInputi\right)$

$\frac{myi-my0}{vi}=f_{yi}\left(RotorInputi\right)$

$\frac{mzi-mz0}{vi}=f_{zi}\left(RotorInputi\right)$

其中，RotorInput i(i＝1,2,3...N)为各个旋翼的转速控制量；mxi(i＝1,2,3...N)，myi(i＝1,2,3...N)和mzi(i＝1,2,3...N)分别为地磁三轴数据；mx 0,my 0,mz 0分别为地磁数据参考值；vi(i＝1,2,3...N)为各个旋翼旋转时对应的无人机电池电压值；函数f_xi，f_yi以及f_zi为多项式函数。

5.如权利要求4所述的动态校准方法，其特征在于，在步骤S2中，无人机飞行时单个旋翼对地磁三轴数据的干扰量Δmxi,Δmyi以及Δmzi具体为：

Δmxi＝Voltage·f_xi(RotorInput i)

Δmyi＝Voltage·f_yi(RotorInput i)，

Δmzi＝Voltage·f_zi(RotorInput i)

其中，

f_xi(RotorInput i)＝pxi 1·RotorInput i³+pxi 2·RotorInput i²+pxi 3·RotorInput i

f_yi(RotorInput i)＝pyi 1·RotorInput i³+pyi 2·RotorInput i²+pyi 3·RotorInput i，

f_zi(RotorInput i)＝pzi 1·RotorInput i³+pzi 2·RotorInput i²+pzi 3·RotorInput i系数pxi 1～pxi 3,pyi 1～pyi 3和pzi 1～pzi 3由对进行多项式拟合得到，Voltage为无人机电池电压值。

6.如权利要求5所述的动态校准方法，其特征在于，在步骤S3中，无人机飞行时地磁三轴数据的实际干扰量为各旋翼对地磁数据产生的干扰量的叠加，Δmx、Δmy以及Δmz具体为：

$\mathrm{\Δ}mx=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}mxi$

$\mathrm{\Δ}my=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}myi,$

$\mathrm{\Δ}mz=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\Δ}mzi$

其中i为旋翼编号，N为旋翼数量。

7.如权利要求5所述的动态校准方法，其特征在于，在步骤S4中，校准后的无人机地磁数据mx_cal、my_cal以及mz_cal具体为：

mx_cal＝mx_measure-Δmx

my_cal＝my_measure-Δmy

mz_cal＝mz_measure-Δmz

其中，mx_measure、my_measure以及mz_measure分别为地磁三轴方向的测量值。