基于CPF-EKF算法的大载荷植保无人机姿态解算方法

吴和龙 白 越 裴信彪 马 萍 彭 程 高慧斌

(1.中国科学院长春光学精密机械与物理研究所， 长春 130033； 2.中国科学院大学， 北京 100039)

0 引言

近年来多旋翼无人机因搭载农药质量较大，在农业低空施药领域受到广泛关注[1-3]，但传统小型消费级无人机的结构和姿态解算方法均无法满足大载荷的需求[3]，故需对无人机结构和姿态解算方法进行优化设计。

为了解决农业植保机的大载荷要求，从提高升力系统的数量入手，对八轴多旋翼无人机进行结构改进，使用共轴双桨和旋翼模块倾斜配置[4]，在有效提高负载能力的同时，增加了系统的冗余性和可靠性。

用于无人机姿态角解算的数据融合方法有互补滤波算法(CPF)、扩展卡尔曼滤波(EKF)、共轭梯度法滤波和无迹卡尔曼滤波(UKF)，其中应用比较广泛的是CPF算法和EKF算法[5]。CPF算法可以看作是一种基于一阶微分系统的数据融合算法，可以有效融合具有低频特性的加速度计、磁力计、GPS信息和具有高频特性的陀螺仪信息，在姿态解算过程中具有良好的稳定性，能够滤去噪声和抑制漂移[6]。EKF算法是一种高精度的在飞行器中应用非常广泛的姿态解算方法[7]，它被应用在飞行器的姿态融合中，具有很高的姿态解算精度。然而EKF存在一个极大的缺点：当线性化假设不成立时，线性化会导致滤波器极度不稳定，在姿态解算过程中容易发散。

根据大载荷植保无人机强振动易导致EKF姿态解算方法发散的特点，本文提出基于四元数的20维状态量CPF-EKF姿态解算方法，利用四元数的旋转特性构造反馈量，有效减少计算量，同时避免姿态解算中的奇异性问题。CPF和EKF同时进行传感器数据融合，解算出姿态角，CPF作为EKF的检测复位模块；当CPF检测到EKF有发散趋势时，进行EKF复位，并用CPF解算的姿态角进行初始对准，以保证复位后的EKF能快速收敛，从而简单高效地解决EKF发散的问题，保证姿态解算的准确性和可靠性。

1 系统设计

1.1 八轴十六旋翼无人机概述

八轴十六旋翼无人机是一种依靠电力驱动的多旋翼无人机[8]，其结构如图1所示，8根等长的杆臂均匀的分布在飞行器中心点。各臂杆的末端垂直安装两组旋翼驱动单元，其中沿逆时针方向旋转的旋翼是1、3、5、7号臂杆的上旋翼和2、4、6、8号臂杆的下旋翼。沿顺时针方向旋转的旋翼是1、3、5、7号臂杆的下旋翼和2、4、6、8号臂杆的上旋翼，同一根臂杆的上下两旋翼转速相等、旋转方向相反。旋翼所在的平面跟机体平面所成的角，即转轴与机体平面的法线方向所呈的角为γ(0°<γ<90°)，且相邻两个转轴的指向相反。

图1 八轴多旋翼无人机原理图Fig.1 Sketch of sixteen-rotor UAV

作为一种新兴的农业现代化机械设备，农用无人机近些年来受到广泛关注，八轴多旋翼无人机除了具备传统多旋翼无人机的优势外，其特有的共轴双桨和旋翼模块的倾斜结构，使一根轴上的上下两旋翼产生的反扭力矩相互抵消，飞行器偏航靠的是升力在水平方向的分力，不是像传统飞行器靠反扭力矩来控制偏航，从而使整个系统的机动性更强[9-10]。此外，共轴双桨的倾斜结构设计，同一根轴上有上下两组升力系统，升力系统较单轴无人机增加一倍，使整个系统载重量更大，满足农业植保机的大载荷需求。

1.2 八轴十六旋翼无人机的姿态解算方法

八轴十六旋翼植保无人机由于机体本身质量大，转动惯量高，额定工作载荷大，共轴双桨的布局，八轴多旋翼无人机16个电动机同时工作，给惯性测量单元造成极大的振动，所以工作时所受到的自身和外界的扰动要高于其他轻载型无人机[11-12]；此外，在药液喷洒过程中，由于药液在药箱内晃动，飞行过程中会给无人机造成额外的扰动力矩[13-14]。因此，洒药系统要实现对农田精准的喷洒作业，对八轴多旋翼无人机的姿态角解算的准确性提出更高的要求。为了克服这个难题，设计了一种CPF辅助EKF的基于20维状态量的CPF-EKF姿态解算方法，并结合无人机真实飞行数据进行了试验验证。

传统的卡尔曼滤波直接将三轴姿态角作为状态量，陀螺仪积分作为三轴姿态角的预测值，加速度计和磁力计作为观测传感器，其中加速度计对俯仰角和滚转角进行校正，磁力计对偏航角进行校正；由于陀螺仪和加速度计对振动非常敏感，故采集的原值中叠加了大量振动信息，对姿态角的修正效果产生极大的影响，导致修正后的姿态角与真值的误差较大[15-16]。基于四元数的EKF是将用于姿态解算的四元数q、三轴速度V和位置S作为状态向量，磁力计和GPS作为观测传感器，对状态量进行修正，有效避免了加速度计观测所带来的振动的影响；并将IMU(包括三轴陀螺仪、加速度计和磁力计)、GPS、气压计等传感器的数据进行融合。此外将Z轴加速度计偏置azb、三轴磁力计偏置mb、三轴陀螺仪偏置ωb和三轴大地磁场M也作为状态量，这些状态量不是由状态预测过程直接修改的，而是通过后续的状态观测进行修改；故状态量是一个20维的向量，状态矩阵方程为

(1)

选择机体坐标系的坐标原点O位于惯性测量单元所安装的重心处，xb轴沿机体纵轴指向前，yb轴沿机体横轴指向右，zb轴垂直于Oxbyb平面沿机体竖轴向下；导航坐标系Oxnynzn选用地理坐标系，即北东地坐标系。四元数下表示的机体坐标b系到大地坐标的n系的旋转矩阵为

(2)

则由四元数解算出的姿态角为

(3)

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

γ——偏航角

1.2.1EKF预测过程

首先通过上一状态的最优估计值和将要施加的控制量来预测当前状态，建立系统的预测状态方程

Xk+1|k=Fk+1|kXk+Gk+1|kuk+wk

(4)

其中

式中F——状态转移矩阵

G——控制矩阵

u——本周期施加的控制量

w——系统噪声

要求F矩阵，需要找到Xk+1中每一项对应于Xk的更新方程。

四元数的更新采用龙格-库塔公式

(5)

其中

式中ω——陀螺仪三轴角速率

Δt——上一状态到当前状态的时间增量

速度预测方程为

(6)

式中ax、ay、az——三轴加速度计测量值

位置预测方程为

Sk+1=Sk+VkΔt

(7)

其他状态量的预测值均等于上一时刻的最优估计值。有了各状态量的预测更新方程，得出状态转移矩阵为

(8)

其中

式中I——单位对角阵O——零矩阵

计算协方差预测值P，用于求解卡尔曼增益K，即

(9)

其中

式中Qk——传感器误差的过程噪声

Qs——使滤波器稳定附加的过程噪声

F矩阵在前边已经算得，P和Q的初值可根据不同传感器的特性给定，下面介绍G的计算方法。

要求G矩阵，需要找到Xk当中每一项对于uk的更新方程。控制量是通过控制电动机的转速来改变飞机的姿态、速度和方向的；所以G矩阵中与控制量相关的就是四元数和速度。由式(5)和式(6)的四元数和速度的更新方程计算出G矩阵

(10)

其中

且

式中σω——三轴陀螺仪噪声方差

σa——三轴加速度计噪声方差

从而通过式(9)可计算出P。

1.2.2磁力计数据融合

机体上三轴磁力计的预测值为

(11)

三轴磁力计的观测值为

以X轴为例，进行数据融合，计算新息νk+1。

νmx=mx-Zmx

(12)

其中

2(q1q2+q0q3)ME+2(q1q3+q0q2)MD+mxb

(13)

对式(13)求导得

X轴的观测矩阵为

计算卡尔曼增益

(14)

式中R——观测噪声

其数值由观测传感器确定状态量的最优估计值为

k+1|k+1=Xk+1|k+Kmνmx

(15)

计算当前状态的协方差

k+1|k+1=(I-KHX)Pk+1|k

(16)

至此X轴磁力计数据融合完成。Y轴和Z轴磁力计数据融合与X轴类似。

1.2.3速度和位置的数据融合

磁力计数据融合采用EKF算法，而速度和位置的数据融合直接采用Kalman filter算法。速度和位置的数据融合用的传感器是GPS和气压计；GPS直接测量水平方向的速度和位置，气压计测量垂直方向上的位置，满足Kalman filter的3个前提条件[17-18]。

速度和位置的观测量为

Zvs=[VxVyVzSxSySz]

速度和位置的状态预测如式(6)、(7)。

计算Kvs增益

(17)

计算新息

νvs=X[VxVyVzSnSeSd]-Zvs

(18)

状态量的最优估计值为

k+1|k+1=Xk+1|k+Kvpνvs

(19)

至此，磁力计、速度和位置的数据融合已全部结束。通过式(3)，用四元数的最优估计值可计算出飞行器当前的姿态角。

1.2.4CPF-EKF算法

传统EKF收敛判据采用不等式[19-20]

(20)

式中r——安全系数，r≥1

tr——矩阵的迹

(θ1-θ2)2+(φ1-φ2)2+(γ1-γ2)2>N

(21)

N为设定的复位阈值，当连续3个运行周期满足式(21)时，判定EKF发散，并用CPF解算的姿态角值作为EKF的复位参考值，对EKF重新初始化，从而抑制了EKF的发散，保证三轴姿态角的准确解算。

2 试验分析及算法验证

2.1 试验平台

为了验证算法的有效性和可行性，搭建了基于STMF429的双传感器硬件平台，主要包括八轴多旋翼无人机、惯性测量单元MPU6050(集成了三轴陀螺仪、三轴加速度计、气压高度计)、LSM303D型三轴磁力计和Neo-M8N GPS模块，并集成了CPF-EKF算法。另一组传感器采用荷兰Xsens公司生产的MTi惯性测量单元，其自带卡尔曼滤波算法，能直接输出高精度的姿态角。滚转角、俯仰角动态精度为±0.1°，偏航角动态精度为±0.5°，试验平台见图2。无人机满载质量为24.532 kg，电动机额定工作转速为6 000 r/s；由于加速度计能够直接返回三轴加速度值，对于多旋翼无人机，电动机在额定工作转速下机体的3个坐标轴产生的振动情况用加速度计返回值间接表征振动强度，更能直观反映无人机在大载荷、强振动工况下叠加到机体的振动信息，试验表明，机体坐标系下，X和Y轴的振动强度峰峰值为±0.488g，Z轴的振动强度峰峰值为±0.732g。

图2 八轴十六旋翼试验平台Fig.2 Experimental platform based on sixteen-rotor UAV

为了保证飞行试验的安全性，使用MTi输出的姿态角指导飞行，STMF429系统集成的CPF-EKF算法仅对传感器数据进行姿态解算，不介入飞行过程的指导；在STMF429主控硬件平台上运行20维状态量的CPF-EKF算法，解算周期为15 ms，而无人机姿态控制周期为20 ms，试验分为静态和动态两种验证。

2.2 静态试验及结果分析

把无人机放在水平地面上，偏航角保持在一个非0°的固定的角度(因为在0°附近，偏航角度会在0°～180°之间跳变)，IMU的采样频率为50 Hz，LSM303D型三轴磁力计采样频率为10 Hz，GPS的采样频率为5 Hz，共计18 000个采样点，合约360 s的采集数据，静态测试得到的滚转角、俯仰角、偏航角如图3所示。

图3 静态条件下的三轴姿态角Fig.3 Three axes attitude angle under static condition

静态测试显示，在不引入电动机工作所带来的振动干扰以及外部风干扰的情况下，滚转角和俯仰角精度能达到±0.05°，偏航角精度为±0.2°，由于水平地面不能保持严格水平，故滚转角、俯仰角出现不同程度小角度。该试验结果说明CPF-EKF姿态解算方法对于减小陀螺仪的零漂具有极大的优势，并且能持续保持稳定的高精度。

2.3 动态试验及结果分析

为了验证CPF-EKF姿态解算方法在无人机动态条件下的有效性，采用无人机飞行采集的实测数据进行了分析。采用荷兰Xsens公司生产的MTi惯性测量单元的姿态角输出作为参考对照。试验无人机参数如表1所示。

表1 试验无人机参数Tab.1 Experimental UAV parameters

图4是飞行试验的现场图，飞行试验的2D轨迹和3D轨迹如图5和图6所示，以无人机起飞位置作为原点，记录飞行过程中每一时刻的位置，为了验证EKF检测模块的作用效果，让无人机在空中进行热点环绕，飞行一个圆形轨迹，采样时间1 000 s。图7是不带CPF检测模块的EKF算法与MTi解算的三轴姿态角的对比图，图8是带CPF检测模块的EKF算法与MTi的对比图。

图4 飞行试验现场Fig.4 Flight experiment scene

图5 飞行试验2D轨迹Fig.5 2D trajectory in flight experiment

图6 飞行试验3D轨迹Fig.6 3D trajectory in flight experiment

由图7可知，不带CPF复位检测模块的EKF姿态解算方法，在解算过程中三轴姿态角出现发散现象，其中偏航方向上的发散尤为严重，在飞行时间为320 s时，偏航角的解算开始出现滞后现象，并且此现象越来越严重，到700 s时出现发散现象，偏航角完全不能跟踪期望角度，俯仰角和滚转角也出现了不同程度的发散现象，滚转角优于俯仰角，虽基本能跟踪期望角度，但解算精度大大降低。造成这种现象的原因是EKF需要对方程进行线性化假设，无人机的大载荷和强振动的工作条件，使线性化假设不成立，强制线性化会导致滤波器极度不稳定，在姿态解算过程中容易发散。

图7 EKF姿态解算Fig.7 EKF attitude calculation

图8 CPF-EKF姿态解算Fig.8 CPF-EKF attitude calculation

图8是带CPF复位检测的EKF算法解算的三轴姿态角，其中复位值N设定为0.274 16，表示三轴姿态角差值之和超过30°时，CPF-EKF复位，在320 s时，复位模块检测到EKF有发散趋势，并且连续3个运行周期满足式(21)设定的要求，判定EKF发散，于是对EKF进行复位操作，用CPF解算的姿态角作为EKF的当前姿态角，方差和其他状态量进行初始化，重新进入EKF循环，继续进行姿态角的解算。随后分别在780 s和960 s又进行了复位；由图8可知，在整个飞行测试过程中，EKF总共复位3次，三轴姿态角被准确跟踪，证明该算法的有效性。

N值是以CPF算法为参考的EKF算法的复位阈值，它表征的是两种算法的三轴姿态角差值的平方和所允许达到的阈值，一旦超过了这个阈值，就判定EKF发散，对EKF进行初始化操作，由此表明，N值越小，复位条件越苛刻，EKF输出的姿态角就越接近CPF的输出，由于CPF解算姿态角的精度不高，故导致EKF解算的姿态角精度也不高。当N值选择过大，会使EKF发散判定条件过于宽松，无法及时判断EKF发散从而错过复位的恰当时机，也会导致EKF输出的姿态角不准确。故对N值的选择要适当；N值的确定与无人机的载荷大小密切相关，搭载的载荷越大，无人机工作时电动机的额定转速越大，振动越强，EKF越容易发散，此时EKF复位条件应当越苛刻，N值取得越小；此外，无人机飞行的速度和高度以及传感器的减振效果对N值也有影响，速度和高度越大，施加于无人机的控制量就越大，电动机转速和振动会相应增加，N越小；经过大量试验验证，工程上选定N值的范围在0.14～0.28，表征三轴姿态角差值之和在20°～30°之间。

图9和图10给出了CPF-EKF算法所解算出的三轴陀螺仪偏置和三轴磁力计偏置。由图可知，三轴陀螺仪偏置误差和磁力计偏置误差在刚上电时波动较大，说明上电时对陀螺仪和磁力计的影响较大，但上电后短时间内经过滤波器补偿后陀螺仪偏置和磁力计偏置迅速收敛并保持稳定，且在EKF出现发散趋势而迅速复位后依然保持收敛且无大的跳变，从而保证了在EKF计算过程中能够准确地去除陀螺仪和三轴磁力计偏置误差，进而保证姿态角的解算精度。

图9 三轴陀螺仪偏置Fig.9 Gyro bias of three axes

图10 三轴磁力计偏置Fig.10 Magnetometer bias of three axes

3 结论

(1)针对植保无人机大载荷、强机动性的需求，对八轴多旋翼无人机结构进行改进，提出共轴双桨和旋翼的倾斜结构。共轴双桨结构满足了植保无人机大载荷的需求，旋翼的倾斜配置解决了传统无人机依靠反扭力矩控制偏航而导致偏航方向控制力矩不足的问题，提高了无人机的机动性。

(2)针对大载荷植保无人机的姿态解算方法，提出了基于20维状态量的CPF-EKF姿态解算方法，并引入CPF算法作为复位检测模块，对EKF算法进行发散时的复位操作。静态试验表明，该算法的滚转角、俯仰角解算精度为±0.05°，偏航角精度为±0.2°。动态试验中，在大载荷、强振动的试验条件下，姿态角能准确跟踪、解算精度高，并且在EKF出现发散趋势时CPF对其及时复位，保证了姿态角的解算精度。试验采用MTi惯性测量单元的输出作为参考，对比结果表明，CPF-EKF算法的动态解算精度能达到滚转角和俯仰角精度为±0.1°，偏航角精度为±0.5°。在STMF429主控硬件平台上运行20维状态量的CPF-EKF算法，解算周期为15 ms，而无人机姿态控制周期为20 ms，EKF解算周期小于控制周期，EKF输出实时性良好，适用于无人机动态性能要求较高的导航信息解算。

1 张东彦，兰玉彬，陈立平，等.中国农业航空施药技术研究进展与展望[J/OL].农业机械学报，2014，45(10): 53-59．http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20141009&journal_id=jcsam.DOI：10.6041/j.issn.1000-1298.2014.10.009.

ZHANG Dongyan, LAN Yubin, CHEN Liping，et al. Research progress and prospect of agricultural aviation application technology in China[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(10): 53-59.(in Chinese)

2 薛新宇，兰玉彬.美国农业航空技术现状和发展趋势分析[J/OL].农业机械学报，2013，44(5):194-201．http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20130534&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2013.05.034.

XUE Xinyu, LAN Yubin. Analysis of status and development trend of American agriculture aviation technology[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2013,44(5): 194-201.(in Chinese)

3 王玲, 兰玉彬,HOFFMANN W，等. 微型无人机低空变量喷药系统设计与雾滴沉积规律研究[J/OL]. 农业机械学报, 2016, 47(1):15-22.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20160103&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2016.01.003.

WANG Ling, LAN Yubin, HOFFMANN W, et al. Design of variable spraying system and influencing factors on droplets deposition of small UAV[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(1):15-22. (in Chinese)

4 徐东甫,白越,宫勋,等. 基于六轴多旋翼飞行器的赤眼蜂投放系统设计与试验[J/OL]. 农业机械学报,2016,47(1):1-7.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20160101&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2016.01.001.

XU Dongfu, BAI Yue, GONG Xun, et al. Design of trichogramma delivering system based on hex-rotor UAV[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(1):1-7. (in Chinese)

5 张栋，焦嵩鸣，刘延泉.互补滤波和卡尔曼滤波的融合姿态解算方法[J].传感器与微系统，2017,36(3):62-69.

ZHANG Dong, JIAO Songming, LIU Yanquan. Fused attitude estimation algorithm based on complementary filtering and Kalman filtering[J].Transducer and Mircrosystem Technologies,2017,36(3):62-69. (in Chinese)

6 陈孟元，谢义建，陈跃东.基于四元数改进型互补滤波的MEMS姿态解算[J].电子测量与仪器学报，2015,29(9)：1391-1397.

CHEN Mengyuan, XIE Yijian, CHEN Yuedong. Attitude estimation of MEMS based on improved quaternion complementert filter[J].Journal of Electronic Measurement and Instrumentation,2015,29(9):1391-1397. (in Chinese)

7 张欣，白越，赵常均，等.多旋翼姿态解算中的改进自适应扩展Kalman算法[J].光学精密工程，2014，22(12):3384-3390.

ZHANG Xin, BAI Yue, ZHAO Changjun, et al. Improved adaptive extended Kalman algorithm for attitude estimation of multi-rotor UAV[J]. Optics and Precise Engineering, 2014, 22(12)：3384-3390.(in Chinese)

8 彭程.共轴八旋翼飞行器姿态与航迹跟踪控制研究[D].长春：吉林大学，2015.

PENG Cheng. On the attitude and trajectory tracking control for a coaxial eight-rotor unmanned aircraft[D]. Changchun: Jilin University, 2015.(in Chinese)

9 XU Dongfu,PEI Xinbiao, BAI Yue, et al. Altitude information fusion method for unmanned aerial vehicles[J].High Technology Letters, 2017, 23(2):165-172.

10 张常云. 自适应滤波方法研究[J]. 航空学报，1998，19(增刊1)：97-100.

ZHANG C Y. Approach to adaptive filtering algorithm [J]. Acta Aeronauticaet Astronautica Sinica, 1998,19(Supp.1):97-100.(in Chinese)

11 茹煜，贾志成，范庆妮，等．无人直升机远程控制喷雾系统[J/OL].农业机械学报，2012， 43(6): 47-52．http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20120609&journal_id=jcsam.DOI：10.6041/j.issn.1000-1298.2012.06.009.

RU Yu，JIA Zhicheng，FAN Qingni，et al．Remote control spraying system based on unmanned helicopter[J/OL]．Transactions of the Chinese Society for Agricultural Machinery，2012，43(6): 47-52．(in Chinese)

12 SAGE A P, HUSA G W. Adaptive filtering with unknown prior statist [C]∥Proceeding of the Joint Automatic Control Conference, 1969: 760-769.

13 窦景欣，孔祥希，闻邦椿.四旋翼姿态的反步滑模自抗扰控制及稳定性[J].东北大学学报，2016,37(10):1415-1420.

DOU Jingxin, KONG Xiangxi, WEN Bangchun. Backstepping sliding mode active disturbance rejection control of quadrotor attitude and its stability[J].Journal of Northeastern University,2016,37(10):1415-1420.(in Chinese)

14 李继宇，张铁民，彭孝东，等．四旋翼飞行器农田位置信息采集平台设计与实验[J/OL]．农业机械学报，2013，44(5): 202-206．http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20130535&journal_id=jcsam.DOI：10.6041/j.issn.1000-1298.2013.05.035.

LI Jiyu，ZHANG Tiemin，PENG Xiaodong，et al． The field location information collection platform design based on the four-rotor aircraft[J]．Transactions of the Chinese Society for Agricultural Machinery，2013，44(5): 202-206．(in Chinese)

15 缪存孝，楚焕鑫，孙志辉，等.基于双GNSS天线及单陀螺的车轮转角测量系统[J/OL].农业机械学报,2017,48(9):17-23.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20170902&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2017.09.002.

MIAO Cunxiao, CHU Huanxin, SUN Zhihui, et al. Wheel turning angle measurement system based on double GNSS antennas and single gyro[J].Transactions of the Chinese Society for Agricultural Machinery，2017，48(9)：17-23.(in Chinese)

16 宫勋，白越，赵常均，等.新型Hex-rotor无人飞行器及其飞行控制系统设计[J].光学精密工程，2012,20(11)：1995-2002.

GONG Xun, BAI Yue, ZHAO Changjun, et al. Novel Hex-rotor aircraft and its autonomous flight control system[J]. Optics and Precise Engineering, 2012,20(11)：1995-2002.(in Chinese)

17 彭丁聪.卡尔曼滤波的基本原理及应用[J].软件导刊，2009,20(11)：32-34.

18 张文，孙瑞胜.EKF与UKF的性能比较及应用[J].南京理工大学学报，2015，39(5)：614-618.

ZHANG Wen, SUN Ruisheng.Research on performance comparison of EKF and UKF and their application[J]. Journal of Nanjing University of Science and Technology,2015,39(5):614-618.(in Chinese)

19 赵德安，贾伟宽，张云,等.农业机器人自主导航改进自适应滤波控制器研究[J/OL].农业机械学报，2015,46(5):1-6. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20150501&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2015.05.001.

ZHAO Dean, JIA Weikuan, ZHANG Yun, et al. Design of agriculture robot autonomous navigation control based on improved self-adaptive filter[J/OL].Transactions of the Chinese Society for Agricultural Machinery，2015,46(5)：1-6. (in Chinese)

20 许亚朝，何秋生，王少江，等.一种改进的自适应卡尔曼滤波算法[J].太原科技大学学报，2016,37(3)：163-168.

XU Yazhao, HE Qiusheng, WANG Shaojiang, et al. An improved adaptive Kalman filtering algorithm[J].Journal of Taiyuan University of Science and Technology,2016,37(3):163-168.(in Chinese)