多旋翼无人机磁罗盘校准方法

(1.江苏省宿迁经贸高等职业技术学校，江苏 宿迁 223600;2.南京信息工程大学 信息与控制学院，南京 210044)

0 引言

多旋翼无人机凭借其垂直起降、定点悬停、成本低廉、使用方便、无人员伤亡等优点，在民用与军事领域得到了广泛的应用。随着应用的推广，用户对无人机的飞行性能和安全性能要求也日益提升。多旋翼无人机的自主飞行时，导航坐标系与机体坐标系之间的转换以航向角为基础。航向角偏差会导致飞行时航线变斜、原地画圈等问题，甚至出现炸机的危险，严重影响无人机的飞行性能。因此，如何提高航向角的准确性便显得尤为重要[1-2]。

目前，消费级和低端行业应用级飞控系统普遍采用磁罗盘作为航向测量设备。传感器自身的准确性以及周边环境的磁干扰会对航向角推算产生非常大的影响。针对如何校准磁罗盘，获取准确的航向信息，国内外机构都进行了大量的研究[3-6]。

传统的多旋翼无人机磁罗盘校准方法大都采用三维旋转，并对采集到的磁场数据椭圆拟合校准实现[7]。然而，在具体应用时，用户往往会忽略这个步骤，只有在发现角度误差较大的情况下才会进行校准地磁。而在飞行过程中发现角度偏差较大，存在非常大的安全隐患。随着无人机行业应用发展，无人机轴距越来越大，转圈椭圆拟合校准的方法已经越来越困难。此外，该校准方法仅对磁罗盘零偏和机体上的固定干扰有效，而忽略了机体产生的动态干扰以及周边环境的磁场干扰对磁罗盘产生的影响，具有一定的局限性。

本文结合多旋翼无人机的具体应用，从分析磁罗盘干扰的来源着手，进行机体坐标系的动态与静态校准。并结合多旋翼的应用背景，采用GNSS模块输出的速度方向信息对航向角进行补偿，得到准确的航向角，保障无人机的飞行性能。

1 磁罗盘工作原理与测量误差分析

1.1 磁罗盘工作原理

磁罗盘是一种可测量环境磁场强度的传感器件，在多旋翼无人机领域应用非常广泛。因为地球表面存在地磁场，且同一地理位置的地磁场的大小和方向基本保持恒定的。地磁场方向与地理北极的夹角称之为磁偏角。通过测量地磁场向量在磁罗盘内部三个相互垂直的坐标轴的磁场分量即可确定航向角。

磁罗盘一般与多旋翼机体固连，为方便讨论，建立机体坐标系与导航坐标系如下[8]：

1)导航坐标系:导航坐标系可选取当地便于导航计算的点为原点On，Zn沿重力方向竖直向下，Xn指向正北方向，Yn指向正东方向。

2)机体坐标系:机体坐标系以机体的质心为原点Ob，Zb垂直于机体平面垂直向下，Xb指向飞行器前方，Yb指向飞行器右方。

机体坐标系与导航坐标系间的夹角即为机体的姿态角。机体坐标系Xb轴与导航坐标系OnYnZn平面的夹角定义为俯仰角θ；机体坐标系Zb轴与通过机体坐标系Xb轴的铅垂面间的夹角定义为滚转角φ；机体坐标系Xb轴在导航坐标系OnXnYn平面的投影与正北方向的夹角定义航向角ψ，坐标系与姿态角示意图如图1所示。

图1 坐标系与姿态角定义

导航坐标系下地磁场向量bNE与机体坐标系下地磁场向量bBE可表示为:

(1)

(2)

式中，bnex、bney、bnez分别为地磁场在导航坐标系下三轴的磁场强度分量。Bbex、bbey、bbez分别为地磁场在机体坐标系下三轴的磁场强度分量。假设载体三维空间运动时，其滚转、俯仰、航向角分别为φ、θ、ψ，则有关系式:

(3)

(4)

不考虑磁偏角前提下，地磁场方向与地理正北方向重合，其Y轴分量bney恒等于零，结合式(3)、式(4)则可求得航向角:

(5)

1.2 磁罗盘误差分析

磁罗盘测量误差来源因素很多, 包括硬磁干扰、软磁干扰、传感器仪表误差等[9]。根据干扰磁场源的相对位置以及干扰磁场强度是否恒定，可将干扰划分为机体坐标系下的静态磁干扰、机体坐标系下的动态磁干扰、导航坐标系下的静态磁干扰、导航坐标系下的动态磁干扰四类。则机体坐标系下磁罗盘示数bB可表示为:

bB=bBE+bBs+bBd+RBNTbNs+RBNTbNd

(6)

bB=bBE+bBs+bBd+RBNTbNs

(7)

2 机体坐标系动态干扰校准

多旋翼无人机机体坐标系动态干扰校准的核心思想为：飞控系统安装完毕后，磁罗盘位置相对固定。磁罗盘的动态干扰主要为飞行中电机与动力线产生的干扰磁场，其磁场强度应与电机转速有直接对应关系。因此，可通过事先逐个测试电机在不同转速指令下产生的干扰磁场，拟合出对应关系式。后续飞行过程中，根据每个电机的控制指令求得其产生的干扰磁场，叠加后即为合成的动态干扰磁场。将磁罗盘原始数据减去合成的动态干扰磁场，即可得到准确的磁场数据。

以六轴植保机为例，首先逐个缓慢变化各电机PWM指令，其它电机保持静止。采集该电机转动时对三轴磁场数据的影响。测试过程中，电机的转速指令需缓慢变化，如图2所示。电机转动引起的X、Y、Z轴的磁场强度变化曲线如图3所示。

图2 电机转速指令

图3 磁场强度变化曲线

图3可以发现，电机转动时，三轴磁场强度有较为明显变化。为获取其对应关系，采用三阶多项式拟合电机指令到三轴磁场强度变化量的关系式。以1号电机为例，X、Y、Z轴的三阶多项式可表示为:

(8)

式中,bBd1为1号电机产生的动态干扰，Δmx1、Δmy1、Δmz1电机转动引起的三轴磁场的变化量，a、b、c为三阶多项式系数，u为电机指令。拟合曲线如图4所示。

图4 一号电机拟合曲线图

同样方法，可以得到2～6号电机指令与产生的干扰磁场bBd2-bBd6关系式，则全部电机的动态干扰磁场bBd可表示为:

bBd=bBd1+bBd2+bBd3+bBd4+bBd5+bBd6

(9)

动态干扰校准后，系统再次按照图1的转速指令工作时，三轴磁场强度如图5所示。对比与图2，其磁场强度变化幅度明显减小，并趋于一条稳定直线。因此，该校准方法对电机产生的动态干扰有了明显的改善。实际应用时，根据飞行过程中的实时电机指令，在线求得其产生的动态干扰并校准，以获得更为准确的磁场强度信息。

图5 动态干扰校准后磁场强度变化曲线

3 机体坐标系静态干扰校准

机体坐标系静态干扰主要包括机体磁感材料产生的恒定磁干扰以及传感器零偏。这类干扰的主要特点是，其干扰磁场向量在机体坐标系下恒定不变。为方便校准，研究一种新型的静态干扰磁场校准方法。该方法利用的测量的磁场向量的变化趋势，实现飞行过程中的实时校准。

假设机体坐标系动态干扰校准完成，式(7)可表示为:

bB=bBE+bBs+RBNTbNs

(10)

已知旋转矩阵的转置等于矩阵的逆:

RBNT=RBN- 1

(11)

结合式(3)、式(10)可得:

(12)

因为bNE与bNs在导航坐标系下恒定不变，令:

bNE′=bNE+bNs

(13)

矢量大小的平方可由其自身的点积求得，用矩阵表示法表示即为向量的转置乘以本身，则有:

|bB-bBs|2= (RBNTbE′)T(RBNTbE′) = |bE′|2

(14)

将上式展开，得到:

(15)

选取任意两个时刻点状态，分别为:

(16)

(17)

将式(16)与式(17)相减得:

(18)

由式(18)无法直接求得准确的静态磁场干扰，但给出干扰的方向信息。即bB2与bB1模值的相对大小决定了向量(bB2-bB1)与bBs之间的夹角为钝角还是锐角，因此可以根据bBs的方向逐步累积补偿其偏差。

(19)

实验时，事先人为给定静态偏差:

(20)

飞行校准过程中，不断改变机头方向，算法推测的静态干扰的变化曲线如图6所示。图中可发现推测量经过约7 s可收敛到真实偏差附近，验证了该静态校准方法的有效性。

图6 推测的静态干扰变化曲线

4 导航坐标系静态干扰的罗差补偿

经机体坐标系静态补偿校准后，式(10)可更新为:

(21)

根据式(5)求得的航向角与真实角度之间的误差即为(bNE+bNs)合成向量在水平面投影与地理正北的夹角。因bNE与bNs恒定不变，航向角误差亦恒定不变，假设ρ为推算的航向角ψm和真实的航向角ψ0之间的误差，则有:

ψm=ψ0+ρ

(22)

因为ρ恒定，可通过高精度仪器校准补偿[10]。但是该方法依赖于高精度仪器，不适应于户外作业。因此，本文研究一种飞行过程中的罗差补偿方法。

多旋翼无人机速度飞行模式一般基于导航坐标系。以前飞为例，其速度控制器运行过程可描述为：遥控器操作产生前向速度目标值；测量GNSS模块速度反馈值；控制器运算生成姿态角控制量并将姿态角控制量分解到机体坐标系。下面对该过程进行详细推导。

遥控器前向操作时，对应的前向速度目标值为rvx，则北向和东向的速度目标值rvn、rve可表示为:

(23)

初始悬停状态下，反馈速度为零，PID控制器积分量为零，生成的控制量uvn、uve可近似表示为:

(24)

式中，kp为比例系数，将uvn、uve转换到机体坐标系:

(25)

由式(23)、式(24)、式(25)可得:

(26)

因此，无人机初期运动方向即为机头朝向。后续因速度反馈以及控制器积分项作用，飞机的运动方向会逐渐收敛并稳定与测量航向角ψm方向。

算法利用初期阶段飞机运动方向与测量的航向角方向的偏差即用于航向角罗差补偿。补偿算法具体操作流程图如图7所示，并可分为以下几个步骤。

图7 航向角补偿算法流程图

步骤1：根据推算误差ρk-1和当前测量航向角ψmk更新航向角ψk。

ψk=ψmk-ρk-1

(27)

步骤2：速度飞行模式下，仅操作遥控器Pitch通道，并根据误ψk进行控制分解。采集GNSS模块东向和北向的实时速度vek和vnk，计算ψvk。

(28)

步骤3：计算无人机实际运动方向ψvk与该时刻航向角ψk的误差ek。

ek=ψvk-ψk

(29)

步骤4：利用ek对航向角误差ρ进行更新，积分补偿系数为kψ。

(30)

测试时，人为给定航向角30°的偏移量，为测试方便，机头指向正北方向，经两次飞机前后运动，航向角误差ρ估计结果如图8所示。图中可以发现航向角误差ρ可快速收敛到30°，验证了该在线航向角纠正方法的有效性。

图8 航向角误差推算结果图

5 结束语

本文分析多旋翼无人机磁罗盘罗差产生的原因，将磁罗盘干扰分为机体坐标系静态干扰、机体坐标系动态干扰、导航坐标系静态干扰、导航坐标系静态干扰四种类型。针对不同类型干扰进行了动态校准、静态校准、磁航向补偿等研究。得到以下结论。

1)机体坐标系动态干扰校准方法可有效补偿无人机飞行时，电机与动力线产生的动态变化磁场对磁罗盘的影响；

2)机体坐标系静态干扰校准方法可实现飞行过程中实时校准，解决静态固定干扰和磁罗盘零偏的影响；

3)针对外部恒定磁场干扰，采用GNSS模块速度方向信息实现在线补偿。

因此，通过论文研究的磁罗盘系统化校准与补偿方法，可解决现阶段绝大部分的磁罗盘干扰问题，提高航向角推算精度，保障多旋翼无人机的飞行性能。