微小型无人机航向姿态解算研究*

郭志杰， 庄哲民， Alex Noel Joseph Raj， 黄慕斌

(1. 汕头大学 电子工程系， 广东 汕头 515063； 2. 汕头市杰泰电子科技有限公司， 广东 汕头 515041)

0 引 言

无人机(Unmanned Aerial Vehicle, UAV)在飞行过程中， 航向系统起着航向角计算的作用[1]. 航向角的获取方法有3种， 分别为： ① 无线电航向角获取； ② 工业陀螺仪航向角获取； ③ 磁感器的航向角获取.

无线电航向角是通过对比机体和信标台的角度而计算出的机体航向角， 其缺点是易于被其他无线电干扰. 工业陀螺仪航向角用陀螺的定轴性获得载体的姿态角度， 此方法得到的角度虽然精度高， 但是成本也很高[2]. 磁感器也叫磁罗盘或磁力计， 利用地磁场来测量航向. 地磁场是在近地空间连续分布的矢量场， 在一定的地面空间内是相同的， 因此可作为很理想的方位导向基准. 并且磁感器的结构简单、 体积小、 功耗低， 是一种常见的航向角获取方法， 在军事、 民用、 商业等方面都应用很广泛.

一般来说， 微小型无人机的航向测量系统中常用陀螺仪输出角速率积分得到航向姿态角[3]， 但此方法得到的航向角精度低、 稳定性差， 从而导致无人机缓慢自旋. 为了克服上述问题， 微小型无人机的航向系统开始使用磁感器定航向. 此方法成本低、 稳定性较高， 但由于磁感器周围存在着磁性材料， 会对测量产生干扰. 最常见的干扰是软磁干扰和硬磁干扰[4]， 前者指的是磁感器附近存在软磁物质， 外部磁场作用在软磁物质上， 使得磁感器附近磁场发生畸变， 从而产生的干扰， 具体表现为磁感器数据拟合出来不再是标准圆球； 后者是由硬磁物质引起的， 硬磁物质会给磁感器带来一个磁场偏移从而产生干扰， 最终效果就是磁力计数据拟合后偏离原心， 因此需要引入磁干扰校正补偿[5].

由于磁感器自身工艺、 安装角度、 测量时的抖动以及上述一系列干扰等因素使得解算出的航向角精度低甚至结果错误. 因此需要一个高精度、 高准确度的磁校准算法对其进行校正， 故本文设计了基于优化的HMC5883L和MPU6050融合磁校准算法.

1 微小型无人机航向角校准系统的组成

普通无人机的航向角通常由单个磁力计数据计算而来， 但是这样计算出的航向角精度低， 稳定性差. 本文设计的微小型四旋翼无人机航向角校准系统运行流程为：

1) 让磁感器进行硬磁干扰补偿；

2) 让陀螺仪数据进行零点校正；

3) 把经过硬磁干扰补偿的陀螺仪3轴数据ωx(t),ωy(t),ωz(t)转换到导航坐标系下；

4) 把补偿后的磁感器数据和陀螺仪数据通过互补滤波融合去掉高频干扰；

5)用文献[6]得到的Roll和Pitch角计算出机体倾角补偿， 进而计算出准确的航向角. 其航向角控制系统方框图如图 1 所示.

图 1 无人机航向角校准系统Fig.1 UAV direction angle calibration system

首先， 将遥控器的校正标志f=1通过2.4GHz无线传输传送到主控芯片STM32F031k6中触发硬磁干扰校正， 其次， 将无人机按一定顺序旋转， 根据主控板采集到的30 s数据计算出磁力计补偿数据， 从而对磁力计的输出进行补偿， 得到校正后的磁力计输出数据[7]； 最后把已经零点校正后的陀螺仪数据通过旋转矩阵转换到导航坐标系下， 利用互补滤波融合磁力计和陀螺仪数据得到准确的航向角.

2 基于优化的HMC5883L和MPU6050融合航向姿态解算算法

外部干扰磁场和磁力计误差是磁力计在测量磁场强度和方向过程中的主要误差来源[8].

图 2 硬磁干扰源示意图Fig.2 Diagram of a hard magnetic interference source

制作材料、 制作工艺会造成磁力计测量偏差， 如零点偏移、 灵敏度等误差. HMC5883L中设计了自检电路， 能自动粗校准3轴的输出灵敏度. 器件里还设计了置位/复位H-桥式电路[9]， 可以消除外界磁场引起的磁感器灵敏度降低问题.

软磁干扰和硬磁干扰是磁场干扰的主要来源， 其中软磁干扰的补偿不易实现且补偿效果较差， 因此通常将磁力计安装在离无人机4个电机最远的地方， 以降低磁感器的软磁干扰. 在微小型四旋翼无人机中软磁干扰比较小， 一般只需进行硬磁校正补偿. 磁感器硬磁干扰源示意图如图 2 所示.

对磁感器硬磁干扰补偿时全方位采集磁场数据的过程如图 3 所示： 首先将无人机水平放在地上，z轴朝上， 绕z轴水平匀速旋转无人机360°； 再将机体y轴朝下， 绕y轴水平匀速旋转无人机360°， 进行数据采集.

图 3 无人机磁力计数据采集过程Fig.3 Data acquisition process of UAV magnetometer

在校正微小型四旋翼无人机硬磁干扰过程中， 无人机主控不断比较3轴的磁场强度的大小， 最终得到磁感器的3轴数据的最大值和最小值， 分别为Xmax,Xmin,Ymax,Ymin,Zmax和Zmin. 通过式(1)获得3轴的硬磁干扰偏置值.

在采集数据的过程中， 因磁力计所处的环境磁场并不完全均衡， 导致HMC5883L的3个轴输出灵敏度不会一样， 所以造成同一个器件的3轴数据的输出值不一样， 故需要让输出数据减去不同干扰补偿， 如式(2)，Xcal,Ycal,Zcal为补偿后的磁场3轴数据. 当HMC5883L 3轴输出量中的干扰量得到补偿后， 其3轴输出数据拟合后的重心点在原点.

(1)

(2)

(3)

式中： 设L为SWF的窗口队列长度；wx(t),wy(t),wz(t)分别为加速度计每次输入SWF的3轴陀螺仪数据.

(4)

(5)

(6)

(7)

由式(6， 7)可得展开式

(8)

校正后HMC5883L的数据准确度高但滞后严重. 经过零点校正后的陀螺仪数据反应灵敏但测量的数据随时间漂移量太大， 如图 4 所示， 为分别在初始航向30°, 60°, 90°时测得的航向角漂移静态测试曲线. 从图 4 可以看出航向角随时间的增加而偏移严重， 故需要加入两者互补滤波让当前校正后磁力计数据和陀螺仪数据融合得到经过校正后的航向角3轴数据. 得到校正后的数据[ψ′,φ′,θ′]与[Xcal,Ycal,Zcal]后， 给两部分一个权重a， 得到一个最优的近似结果[X′,Y′,Z′]， 如式(9)所示：

(9)

最终得到经过陀螺仪和磁力计互补滤波后的航向3轴数据， 将此数据输入式(10)计算出航向角. 其中X′,Y′,Z′为从本文设计的无人机航向角校准系统中输出的数据，φ,θ分别为无人机的横滚角和俯仰角， 由论文[6]可得到.Xh,Yh为求航向角ψ产生的中间变量.

(10)

图 4 陀螺仪航向漂移静态测试Fig.4 Static test of gyro heading drift

磁偏角是指地磁北极与高德地图的地理北极所产生的一个角度偏差. 物体的磁偏角大小和方向由物体在地球表面的空间决定. 磁针指北极N向东偏则磁偏角为正， 向西偏则磁偏角为负[13]， 故需要在求出的航向角ψ中减去磁偏角得到准确航向角ψ′， 如式(11)所示.

ψ′=ψ-Magnetic Declinaition.

(11)

考虑到航向角象限的问题， 当ψ′<0时补偿360°， 如式(12). 设计的整个系统采样周期为6 ms， 并且UAV航向角的范围为： 0°～ 360°.

ψ′=ψ′+360 (ψ′<0).

(12)

3 实验结果

由于微小型四旋翼无人机需要考虑成本和功耗， 故硬件上用HoneyWell公司的磁感器HMC5883L进行磁场测量， 传感器电路图如图 5 所示， 其中2.2 K两个电阻为I2C总线的上拉电阻； C1， C3两个电容为滤波电容， 起保护芯片的作用.

图 5 HMC5883L电路图Fig.5 HMC5883L circuit diagram

角度测量使用InvenSense公司的低功耗6轴传感器MPU6050， 使用低功耗MCU STM32f031k6作为磁感器、 陀螺仪3轴数据采集、 加速度计和滤波等的主控芯片. 用Beken公司的2.4 GHz传输芯片 BK2423进行遥控数据传输， 其功耗低(供电1.9～2.6V)， 可提供最大8 MHz的时钟速率的4脚SPI接口， 短距离稳定性强、 抗干扰能力强且通讯速率高， 满足微小型四旋翼无人机实时传输的要求. 设计的无人机所用电池为800 mAh， 满电电压为4.2 V.

从图 6 可以看出： 微小型无人机飞行控制的硬件平台包括低成本、 低功耗的6轴传感器MPU6050、 微控制单元(Microprogrammed Control Unit， MCU)STM32f0， 2.4 GHz模块、 磁力计模块HMC5883L和气压计模块SPL06-001等.

图 6 无人机飞行控制系统硬件图Fig.6 Hardware diagram of UAV flight control system

在没有金属钢架结构的地方进行实验， 图 7 为采集到的7 000个3轴磁场数据， 由上述数据拟合而成的半球体， 球的中心不在(0， 0， 0)点. 根据式(1)计算补偿数据， 再根据式(2)对原始数据进行补偿， 采集干扰补偿后的数据， 通过Matlab拟合3轴数据可得到图 8. 从图 8 中可以看出： 拟合出的曲线为一个球形， 球体球心与坐标原点(0， 0， 0)重合[14]， 此时磁场的补偿工作顺利完成.

图 7 原始磁场数据三维图Fig.7 The three-dimensional map of the original magnetic field data

图 8 补偿后磁场数据三维图Fig.8 The three-dimensional map of magnetic field data after compensation

当磁力计补偿完成后， 让无人机飞起来， 在空中悬停， 用串口USART通过蓝牙将数据传送到上位机， 然后将数据保存下来. 采集3组8 000个数据， 如图 9 所示， 陀螺仪计算出的航向角漂移太严重， 大约漂移速率为0.187 5 °/s， 这会让无人机在定高时自旋、 由磁力计测出的数据精度低. 本文算法得到的航向角较磁力计测出的航向角精度提升50%.

让无人机继续定于空中悬停， 用遥控器操控， 让其先向负方向自旋转30°左右， 一段时间后， 将其航向回归初始位置. 同上， 将数据传送到上位机然后将数据保存下来， 采集3组5 800个数据， 如图 10 所示， 陀螺仪计算出的航向角随着自旋转角度继续漂移， 且存在少量角度超调； 由磁力计测出的数据存在较大超调量， 这将直接导致无人机航向抖动； 本文算法得到的航向角超调较小， 能保证微小型无人机航向稳定.

综上所述， 采用本文方法对微小型无人机航向定航时， 算出的航向角数据精度高、 稳定性强， 在玩具无人机定航中具有较好的优势. 其原因在于：

1) 通过“最大最小值”法去除HMC5883L的硬磁干扰；

2) 采用互补滤波将两者数据进行融合， 把陀螺仪数据的高频性、 实时性与磁力计的准确性、 稳定性通过一个权重a相结合， 从而保证了航向角度具有很好的稳定性、 实时性和准确性.

图 9 微小型无人机悬停时航向角对比曲线Fig.9 Comparison curves of heading angle for micro UAV hovering

4 结 论

本文针对微小型四旋翼无人机在定航时由于计算出的航向角准确度低导致机体微自旋的缺点， 设计了基于优化的HMC5883L和MPU6050融合航向姿态解算新方法. 实验结果表明： 本文提出的磁校准算法能准确去除硬磁干扰且能结合陀螺仪和磁力计两者的优点， 能输出实时性高、 误差小、 稳定性强的航向角， 有效保证了微小型四旋翼无人机的航向稳定.