直线型植保无人机航姿UKF两级估计算法

沈 跃 孙志伟 沈亚运 张大海 钱 鹏 刘 慧

(1.江苏大学电气信息工程学院， 镇江 212013； 2.浙江大学海洋学院， 舟山 310013)

0 引言

植保无人机是近年来新兴的植保机械，适应我国多样性和分散性种植地形[1-2]。直线型植保无人机，相对于常见多轴旋翼植保无人机，其物理喷幅宽，利于提高作业效率。但直线型无人机由于多电机呈直线式紧密分布，飞行控制器与电机间距较小，易受电机旋转产生的磁场干扰，导致航向角估计精度低，同时采用高精度RTK惯导系统会增加无人机飞行质量与制造成本[3]。无人机多传感器精确校准和融合算法，是保证航姿估计准确性的必要条件。

无人机传感器校准主要包含对加速度计、陀螺仪和磁力计的校准，其中加速度计和陀螺仪的校准方法较为成熟[4]，而磁力计容易受电机感应磁场干扰。磁力计校准可分为离线校准和实时校准。孙宏伟等[5]采用带约束的最小二乘法，通过旋转估计椭球参数。WU等[6]利用LM算法先进行球面拟合，再进行椭球拟合计算出静态补偿参数，但对大型无人机校准操作繁琐、动态性能差。蔡浩原等[7]提出一种基于扩展卡尔曼滤波器(Extended Kalman filter，EKF)的动态校准方法，但陀螺仪偏置影响校准参数的估计精度。SLIC等[8]建立了基于单电机油门指令的磁力计模型，但无法适用多电机分布的无人机。目前无人机磁力计的校准算法静态精度较高，但是无法快速补偿无人机机动飞行中时变的电磁场干扰，因此高精度的实时磁力计校准方法还有待进一步研究。

无人机航姿估计的多传感器融合算法主要有互补滤波[9-10]、梯度下降法[11]和扩展卡尔曼滤波[12-14]等。张勇刚等[15]用互补滤波器融合加速度计、陀螺仪估计航姿，抑制了陀螺仪的积分漂移，但磁场干扰直接影响姿态角估计。彭孝东等[16]采用梯度下降法，在无人机运动加速度较小时精度好。蔡安江等[17]推测四元数与欧拉角一一对应相关，但该算法在机动环境下，相关性会降低，导致姿态估计准确性降低。卢艳军等[18]基于EKF分别估计姿态与航向信息，降低了磁场干扰对航向角的影响，但未考虑磁倾角以及软磁场变化。

针对上述问题，结合直线型植保无人机磁场干扰变化快且模值大的特点，本文提出一种应用UKF实现一级融合陀螺仪和加速度计估计姿态、二级融合陀螺仪和和实时校准的磁力计数据求解航向角，进而校正一级四元数的航姿估计算法，大幅减弱磁场干扰对俯仰角和横滚角的影响，提高航向角解算精度。

1 分级融合航姿估计算法

图1 分级融合航姿估计算法流程图Fig.1 Flowchart of hierarchical fusion attitude estimation algorithm

UKF算法流程简述如下：对状态量进行无迹变换(Unscented transform，UT)获得Sigma点集，通过状态方程计算出点集变换后数据，从而获得状态的预测均值及其协方差矩阵；对状态预测进行UT变换，获得Sigma点集，通过量测方程计算出点集变换后数据，从而量测的预测均值及其协方差矩阵；计算卡尔曼增益，更新状态及其协方差[19]。

2 磁力计实时校准

2.1 磁力计量测模型

无人机通常需要考虑地磁场和空间磁场对航姿测量的影响[20-21]。地磁场是航向角估计的重要观测信息，磁力计是测量空间磁场矢量的传感器[22]。但磁力计测量易产生误差：一是生产工艺限制导致的仪表误差，二是外部磁场干扰造成的误差。

磁力计仪表误差包括零偏误差和尺度因子误差、非正交误差等[23-24];外部磁场干扰主要包括无刷电机的永磁体、载流导线引起的磁力计偏置和机身金属连接件导致的三轴非正交误差等。对2种误差建立磁力计测量模型，其零位偏移为

(1)

尺度因子矩阵为

(2)

磁力计测量模型的非正交变换矩阵为Kd，磁力计测量模型的三维变换矩阵为

(3)

Mm=KMd+Mb

(4)

Md=K-1(Mm-Mb)

(5)

2.2 LM非线性最小二乘模型

地磁场强度为50～60 μT，植保无人机在某一范围内作业时，地磁矢量变化很小，可视为定值，假定地磁场模长为常数，即

(6)

在仅考虑地磁场的理想环境下

(7)

式中b——机体坐标系(b系)

e——当地地理坐标系(e系)

Mb——理想磁力计输出矢量

Me——地磁场矢量

无人机在空中运动时，磁力计数据点形成一个各轴等长的球面。但在实际环境中，由式(5)可知，球体三轴产生旋转且非正交、各轴长度不等，导致球心偏离坐标原点形成椭球面。

经校准后，磁力计模长为

‖Md‖2=(K-1(Mm-Mb))T(K-1(Mm-Mb))

(8)

将K-1转换为

(9)

并构建代价函数L，通过非线性最小二乘拟合校准参数得

(10)

(11)

其中

(12)

(13)

式中xcali——需要校准的参数

应用列文伯格-马夸特(Levenberg-Marquardt，LM)算法[25]求解使代价函数接近于零的参数。

LM算法初始化：

校准参数向量xcali的初始值xcali|0=[1 0 0 1 0 1mbx0mby0mbz0]，其中[mbx0mby0mbz0]T为在空旷处获得的机载硬磁零偏，加快算法收敛速度。

地磁场模长的参考值是利用WMM2020世界地磁模型[26]和经纬度计算得到。

令阻尼系数初始值μ0=2，阻尼缩放系数α=2，迭代次数初始值k=1。

LM算法迭代更新步骤：

(1) 计算Jacobian矩阵

(14)

式中Mmk——kTs时刻传感器测量值

Ts——迭代周期

(2) 计算Hessian近似矩阵

(15)

(3) 更新阻尼系数μk

μk=Sgyroμk-1

(16)

其中

(17)

式中Sgyro——阻尼系数动态调整因子

‖wgyro‖——陀螺仪矢量模值

(4) 计算迭代步长

dk=-(J(xcali|k-1,Mmk)TJ(xcali|k-1,Mmk)+μkI)-1·
J(xcali|k-1,Mmk)TL(xcali|k-1,Mmk)

(18)

式中I——9维单位矩阵

(5) 确定搜索方向

(6) 输出迭代参数

xcali|k=xcali|k-1+dk

(19)

利用LM算法依据地磁场模长不变的特点，实时求解磁力计校准参数，减小了磁力计的测量噪声，增强了磁力计数据的抗干扰能力。

3 基于UKF的一级航姿估计

无人机航姿变化主要依靠对横滚角、俯仰角和航向角的调节[27]。横滚角和俯仰角的姿态估计依赖加速度计与陀螺仪融合，加速度计可以实时补偿陀螺仪零偏，但是由于重力加速度垂直于水平面，所以无法补偿航向角漂移，需要融合磁力计数据进行补偿。下面建立基于UKF的四元数估计算法模型。

3.1 航姿状态方程

建立姿态预测状态方程

新课标指出，在小学语文教学过程中应该对学生各种能力的培养提出更高的重视，而阅读能力作为一种重要的基础能力，教师更应该给予重视。在实际的教学过程中，教师要重视学生阅读能力的培养，创建有趣的情境激发学生的阅读兴趣，面对小学生，情境教学是一种非常有效的手段，具备特殊的应用价值，尤其值得在小学语文教学中推广和运用。

xk=f(xk-1)+wk-1

(20)

式中f(xk-1)——状态连续的非线性向量函数

xk——系统状态向量

wk-1——系统噪声

四元数微分方程为

(21)

式(21)矩阵形式为

(22)

式(22)进行欧拉积分将四元数更新，得

(23)

(24)

陀螺仪零偏具有低频特性，随时间缓慢波动，用一阶马尔可夫过程建模

(25)

式中τ——相关时间系数

(26)

3.2 重力加速度观测方程

建立姿态的观测方程

yk=h(xk)+vk

(27)

式中h(xk)——状态连续的非线性向量函数

yk——量测信息vk——量测噪声

加速度计测量模型为

(28)

gb——机体重力加速度

ma——机体运动加速度

ab——加速度计三轴零偏

观测量选取机体坐标系下重力加速度的比力，将比力旋转至机体坐标系得

(29)

故观测方程为

(30)

3.3 观测噪声协方差自适应调节

在微机动或静止状态下，加速度计通过测量重力加速度，可以准确补偿陀螺仪零偏，但是载体机动时存在线加速度，从式(28)可得到加速度计测量值不仅是重力加速度，而是重力加速度和运动加速度的矢量和。而陀螺仪受运动加速度影响较小，在大机动的场景下，观测量将偏离预设模型，根据加速度计的测量数据模值，自适应调节测量噪声方差矩阵。

使用分段函数描述测量噪声变化

(31)

式中g——重力加速度模值

磁力计测量噪声自适应调节与加速度计类似，以地磁场强度为基准。

4 基于UKF的二级航姿修正

第2级融合是利用磁力计数据对上一级融合的航向角进行修正，同时避免磁倾角误差、磁场干扰等因素影响横滚角和俯仰角数据。

4.1 航向角状态方程

通过欧拉角微分方程，可得航向角与三轴角速度的关系为

(32)

式中φ——横滚角θ——俯仰角

ψ——航向角

选取航向角为状态量，故航向角状态预测方程为

(33)

4.2 磁力计观测方程

(34)

式中Ry(θ)——绕y轴旋转矩阵

Rx(φ)——绕x轴旋转矩阵

忽略z轴数据，获得磁力计在水平面投影，归一化后作为航向角观测量。采用东北天坐标系，故地磁水平方向为[0 1 0]T，航向角观测方程为

(35)

4.3 航向角误差四元数

四元数的角轴表示法为

(36)

式中ε——旋转角u——空间的旋转轴

Qe=(cos(ψ2-ψ1),0,0,sin(ψ2-ψ1))T

(37)

式中ψ1——第1级融合航向角

ψ2——第2级融合航向角

5 试验平台与试验方法

5.1 硬件平台

直线型植保无人机航姿测量系统(图2)主要由核心处理器、六轴微惯性器件和三轴磁力计构成。其中，核心处理器选用ST公司的STM32F405RGT6芯片。该芯片处理器为Cortex M4内核，最高处理频率168 MHz，可满足直线型植保无人机航姿估计要求。六轴微惯性传感器选用InvenSense公司的ICM20602型微惯性测量芯片，包含三轴陀螺仪和三轴加速度计。磁力计采用PNI传感器公司的RM3100型电子罗盘传感器。微惯性传感器的采样频率设置为500 Hz，磁力计的采样频率为200 Hz，保证无人机在定点悬停、高速飞行作业等复杂工况下航姿的准确测量与磁力计实时校准。

图2 航姿测量系统实物图Fig.2 Heading and attitude measurement system1.RM3100型磁力计 2.STM32F405RGT6单片机 3.ICM20602型六轴微惯性传感器

机体坐标系的坐标原点位于六轴微惯性器件的中心处，Xb轴沿机体纵轴指向前，Yb轴沿机体横轴指向左，Zb轴垂直于OXbYb平面沿机体竖轴向上; 导航坐标系OXnYnZn选用地理坐标系，即东北天坐标系。

为验证航姿测量系统的精确性与系统的适用性，本文采用MTi-680G型 GNSS/惯性导航系统(INS)模块数据作为基准，将测量系统的硬件单元搭载在自主设计的直线型植保无人机(图3)进行试验。MTi-680G型模块集成GNSS/惯性导航系统(INS)，配备RTK GNSS接收器，可提供同步的3D姿态和航向输出。MTi-680G型姿态测量精度0.2°，航向角测量精度1.0°。

图3 直线型植保无人机试验平台Fig.3 Linear plant protection UAV test platform

图3所示的直线型植保无人机试验平台，主升电机为4个飓风U3515型电机，采用竖直杆为姿态调整杆，姿态调整杆上为2个对称的TMOTOR乘风2207型电机，通过串级控制完成飞行任务。

5.2 试验设计与结果分析

植保无人机作业时，一般不会出现大幅度机动，因此本文进行直线型植保无人机小幅度运动模式的磁力计校准试验。选取周围3 m内无明显磁场干扰源的空旷场地，人为旋转图2所示的航姿测量系统，以测量模块小幅度运动(磁场矢量在单位球面一个小区域内运动，俯仰角、横滚角和航向角运动范围均小于90°)的数据。试验共采集7 000个数据点，并对旋转过程中LM实时校准算法、LS椭球拟合法2种方法的局部采样曲线和局部采样误差曲线进行对比分析。图4为试验过程中2种算法在磁场球面的轨迹曲线；图5为2种算法校准过程中产生的误差曲线。

图4 局部采样磁力计校准对比Fig.4 Comparison diagram of local sampling magnetometer calibration

图5 局部采样磁力计校准误差对比Fig.5 Comparison diagram of calibration error of local sampling magnetometer

由图4、5可知，LS椭球拟合法在磁力计原始数据集中在小区域内时，校准后数据多数偏离理想球面(理想磁场矢量活动面)，校准数据发散严重，校准效果较差；而本文提出的LM实时校准算法在相同情况下，校准后数据与原始数据区域重合率更高、误差更小，校准效果更好。

如表1所示，在上述试验环节，LS椭球拟合法最大误差为236.22 μT、均方根误差为51.19 μT，说明LS椭球拟合法已经发散，无法完成此场景下的磁力计校准；本文提出的LM实时校准法最大误差为4.50 μT、均方根误差为0.58 μT，在测量模块小幅度运动时，可以完成磁力计实时校准并快速收敛。

表1 局部采样磁场校准试验结果Tab.1 Magnetic field interference test results μT

飞行过程中，电机旋转会产生磁场干扰，为测试LM实时校准算法在动态飞行过程中的鲁棒性，将电机作为干扰源，设计磁场干扰试验。选取周围3 m内无明显磁场干扰源的空旷场地，旋转测量系统。由于旋转前期无法快速满足LM实时校准法和LS椭球拟合法收敛条件，因此在旋转测量系统8 s后(俯仰角、横滚角和航向角运动范围均大于90°)加入磁场干扰源，将电机在距航姿测量系统15 cm处做钟摆运动，采集20 000个数据点，进行LM实时校准算法和LS椭球拟合法的对比试验。图6为受电机磁干扰后两种算法的校准曲线；图7为受电机磁干扰后两种算法校准过程中的误差曲线。

图6 磁场干扰试验磁力计校准对比Fig.6 Calibration comparison diagram of magnetometer in magnetic field interference test

图7 磁场干扰试验误差对比Fig.7 Comparison diagram of magnetic field interference test error

由图6、7可知，凭借LS椭球拟合提供的实时校准算法初始值，LM实时校准法快速收敛。在8s出现磁场干扰后，LM实时校准法依旧快速估计出了磁力计校准参数。如表2所示，LS椭球拟合法最大误差为29.33 μT、均方根误差为3.78 μT；LM实时校准法最大误差为6.17 μT、均方根误差为 0.59 μT，均小于LS椭球拟合法校准产生的误差。因此，本文提出的基于LM实时校准算法的航姿测量系统对于时变的磁场干扰，能够快速收敛，保持高稳定性。

表2 磁场干扰试验结果Tab.2 Magnetic field interference test results μT

为进一步验证航姿测量系统在实际飞行作业过程中的测量精度，于2021年4月在江苏大学(32°12′15″N,119°30′43″E)进行飞行试验。以直线型植保无人机为飞行试验平台，并以MTi-680G型输出数据作为航姿测量参考值。图3为飞行试验的现场图，航姿测量系统安装于直线型植保无人机机架中心处，与飞行控制系统在同一平面。无人机在空中分别完成俯仰、横滚、偏航等动作，采集航姿变化数据，同时采用本文提出的分级融合算法与互补滤波分别进行估计，比对航姿解算鲁棒性。飞行试验结果如图8所示。

由图8a和图8b可知，直线型植保无人机飞行阶段，俯仰角和横滚角动态跟随MTi-680G型姿态变化波形效果较好，没有因磁场扰动而导致姿态估计精度降低。同时，航姿估计算法调节了加速度计的测量方差，降低运动加速度、机身振动对姿态解算的影响，UKF滤波器的姿态解算精度优于互补滤波。

图8 估计对比局部图Fig.8 Estimation and comparison local map

直线型植保无人机飞行时，磁场扰动不断变化，若按照离线校准参数进行航向角解算，会因受变化的软磁场干扰产生误差。而利用本文提出的LM实时校准法，可以减小磁场扰动对于航向角的观测信息的影响，如图8c和图9所示。

图9 飞行试验磁力计模长误差对比曲线Fig.9 Comparison diagram of mode length error of flight test magnetometer

由图8c可知，利用LM法实时校准磁罗盘数据，结合UKF分级融合算法，能够有效降低磁场扰动影响，提高航向角估计精度。由图9可知，在动态飞行试验中，磁场扰动不断变化，LS椭球拟合法模长误差数据不稳定，而利用LM法实时校准磁罗盘数据，比LS椭球拟合法校准的数据，模长误差更小。对采用互补滤波和UKF分级融合算法的飞行测试数据进一步分析，试验数据如表3、4所示。

表3 互补滤波算法飞行测试结果Tab.3 Flight test results of complementary filter (°)

表4 分级融合算法飞行测试结果Tab.4 Flight test results of hierarchical fusion algorithm (°)

由表3、4可知，本文研制的基于UKF分级融合算法的直线型植保无人机航姿测量系统姿态角均方根误差不大于0.75°、航向角均方根误差为1.40°，与互补滤波算法相比，横滚角与俯仰角精度提高了约0.6°、航向角精度提高1.38°，并且航向角最大误差也减小了2.98°。

6 结论

(1)针对直线型植保无人机飞行过程中，磁场干扰影响航姿解算精度的问题，设计了一种基于磁力计实时校准的航姿测量系统。利用地磁场矢量变化缓慢的特点，使用 LM法实时计算磁力计校准参数；采用自适应无迹卡尔曼滤波器，结合六轴微惯性和三轴磁力器件设计了低成本硬件方案。

(2)采用电机作为干扰源，旋转航姿测量模块进行试验。试验结果表明，在外部磁场干扰高达30.97 μT时，实时校准算法依然可以快速计算出磁力计校准参数，模长均方根误差为0.59 μT，最大误差为6.17 μT，提高了航向角观测信息精度。

(3)以自主设计的直线型植保无人机为飞行平台，采用无迹卡尔曼滤波器融合陀螺仪和加速度计数据估计姿态，利用实时校准后磁力计数据与角速度信息融合修正航向角，增强了航向跟踪能力。飞行试验中，本文研制的直线型植保无人机航姿测量系统与互补滤波算法相比，横滚角与俯仰角精度提高0.6°、航向角精度提高1.38°。算法有效抑制了磁力计测量中的干扰，并且干扰没有耦合到姿态估计，保证了飞机的稳定飞行，提高了航向角估计精度。

(4)设计的直线型植保无人机航姿测量系统仍需进行飞行植保作业进一步验证，并根据飞行数据进一步探索融合算法优化方向。