吳和龍 白 越 裴信彪 馬 萍 彭 程 高慧斌

(1.中國科學(xué)院長(zhǎng)春光學(xué)精密機(jī)械與物理研究所， 長(zhǎng)春 130033； 2.中國科學(xué)院大學(xué)， 北京 100039)

0 引言

近年來多旋翼無人機(jī)因搭載農(nóng)藥質(zhì)量較大，在農(nóng)業(yè)低空施藥領(lǐng)域受到廣泛關(guān)注[1-3]，但傳統(tǒng)小型消費(fèi)級(jí)無人機(jī)的結(jié)構(gòu)和姿態(tài)解算方法均無法滿足大載荷的需求[3]，故需對(duì)無人機(jī)結(jié)構(gòu)和姿態(tài)解算方法進(jìn)行優(yōu)化設(shè)計(jì)。

為了解決農(nóng)業(yè)植保機(jī)的大載荷要求，從提高升力系統(tǒng)的數(shù)量入手，對(duì)八軸多旋翼無人機(jī)進(jìn)行結(jié)構(gòu)改進(jìn)，使用共軸雙槳和旋翼模塊傾斜配置[4]，在有效提高負(fù)載能力的同時(shí)，增加了系統(tǒng)的冗余性和可靠性。

用于無人機(jī)姿態(tài)角解算的數(shù)據(jù)融合方法有互補(bǔ)濾波算法(CPF)、擴(kuò)展卡爾曼濾波(EKF)、共軛梯度法濾波和無跡卡爾曼濾波(UKF)，其中應(yīng)用比較廣泛的是CPF算法和EKF算法[5]。CPF算法可以看作是一種基于一階微分系統(tǒng)的數(shù)據(jù)融合算法，可以有效融合具有低頻特性的加速度計(jì)、磁力計(jì)、GPS信息和具有高頻特性的陀螺儀信息，在姿態(tài)解算過程中具有良好的穩(wěn)定性，能夠?yàn)V去噪聲和抑制漂移[6]。EKF算法是一種高精度的在飛行器中應(yīng)用非常廣泛的姿態(tài)解算方法[7]，它被應(yīng)用在飛行器的姿態(tài)融合中，具有很高的姿態(tài)解算精度。然而EKF存在一個(gè)極大的缺點(diǎn)：當(dāng)線性化假設(shè)不成立時(shí)，線性化會(huì)導(dǎo)致濾波器極度不穩(wěn)定，在姿態(tài)解算過程中容易發(fā)散。

根據(jù)大載荷植保無人機(jī)強(qiáng)振動(dòng)易導(dǎo)致EKF姿態(tài)解算方法發(fā)散的特點(diǎn)，本文提出基于四元數(shù)的20維狀態(tài)量CPF-EKF姿態(tài)解算方法，利用四元數(shù)的旋轉(zhuǎn)特性構(gòu)造反饋量，有效減少計(jì)算量，同時(shí)避免姿態(tài)解算中的奇異性問題。CPF和EKF同時(shí)進(jìn)行傳感器數(shù)據(jù)融合，解算出姿態(tài)角，CPF作為EKF的檢測(cè)復(fù)位模塊；當(dāng)CPF檢測(cè)到EKF有發(fā)散趨勢(shì)時(shí)，進(jìn)行EKF復(fù)位，并用CPF解算的姿態(tài)角進(jìn)行初始對(duì)準(zhǔn)，以保證復(fù)位后的EKF能快速收斂，從而簡(jiǎn)單高效地解決EKF發(fā)散的問題，保證姿態(tài)解算的準(zhǔn)確性和可靠性。

1 系統(tǒng)設(shè)計(jì)

1.1 八軸十六旋翼無人機(jī)概述

八軸十六旋翼無人機(jī)是一種依靠電力驅(qū)動(dòng)的多旋翼無人機(jī)[8]，其結(jié)構(gòu)如圖1所示，8根等長(zhǎng)的桿臂均勻的分布在飛行器中心點(diǎn)。各臂桿的末端垂直安裝兩組旋翼驅(qū)動(dòng)單元，其中沿逆時(shí)針方向旋轉(zhuǎn)的旋翼是1、3、5、7號(hào)臂桿的上旋翼和2、4、6、8號(hào)臂桿的下旋翼。沿順時(shí)針方向旋轉(zhuǎn)的旋翼是1、3、5、7號(hào)臂桿的下旋翼和2、4、6、8號(hào)臂桿的上旋翼，同一根臂桿的上下兩旋翼轉(zhuǎn)速相等、旋轉(zhuǎn)方向相反。旋翼所在的平面跟機(jī)體平面所成的角，即轉(zhuǎn)軸與機(jī)體平面的法線方向所呈的角為γ(0°<γ<90°)，且相鄰兩個(gè)轉(zhuǎn)軸的指向相反。

圖1 八軸多旋翼無人機(jī)原理圖Fig.1 Sketch of sixteen-rotor UAV

作為一種新興的農(nóng)業(yè)現(xiàn)代化機(jī)械設(shè)備，農(nóng)用無人機(jī)近些年來受到廣泛關(guān)注，八軸多旋翼無人機(jī)除了具備傳統(tǒng)多旋翼無人機(jī)的優(yōu)勢(shì)外，其特有的共軸雙槳和旋翼模塊的傾斜結(jié)構(gòu)，使一根軸上的上下兩旋翼產(chǎn)生的反扭力矩相互抵消，飛行器偏航靠的是升力在水平方向的分力，不是像傳統(tǒng)飛行器靠反扭力矩來控制偏航，從而使整個(gè)系統(tǒng)的機(jī)動(dòng)性更強(qiáng)[9-10]。此外，共軸雙槳的傾斜結(jié)構(gòu)設(shè)計(jì)，同一根軸上有上下兩組升力系統(tǒng)，升力系統(tǒng)較單軸無人機(jī)增加一倍，使整個(gè)系統(tǒng)載重量更大，滿足農(nóng)業(yè)植保機(jī)的大載荷需求。

1.2 八軸十六旋翼無人機(jī)的姿態(tài)解算方法

八軸十六旋翼植保無人機(jī)由于機(jī)體本身質(zhì)量大，轉(zhuǎn)動(dòng)慣量高，額定工作載荷大，共軸雙槳的布局，八軸多旋翼無人機(jī)16個(gè)電動(dòng)機(jī)同時(shí)工作，給慣性測(cè)量單元造成極大的振動(dòng)，所以工作時(shí)所受到的自身和外界的擾動(dòng)要高于其他輕載型無人機(jī)[11-12]；此外，在藥液噴灑過程中，由于藥液在藥箱內(nèi)晃動(dòng)，飛行過程中會(huì)給無人機(jī)造成額外的擾動(dòng)力矩[13-14]。因此，灑藥系統(tǒng)要實(shí)現(xiàn)對(duì)農(nóng)田精準(zhǔn)的噴灑作業(yè)，對(duì)八軸多旋翼無人機(jī)的姿態(tài)角解算的準(zhǔn)確性提出更高的要求。為了克服這個(gè)難題，設(shè)計(jì)了一種CPF輔助EKF的基于20維狀態(tài)量的CPF-EKF姿態(tài)解算方法，并結(jié)合無人機(jī)真實(shí)飛行數(shù)據(jù)進(jìn)行了試驗(yàn)驗(yàn)證。

傳統(tǒng)的卡爾曼濾波直接將三軸姿態(tài)角作為狀態(tài)量，陀螺儀積分作為三軸姿態(tài)角的預(yù)測(cè)值，加速度計(jì)和磁力計(jì)作為觀測(cè)傳感器，其中加速度計(jì)對(duì)俯仰角和滾轉(zhuǎn)角進(jìn)行校正，磁力計(jì)對(duì)偏航角進(jìn)行校正；由于陀螺儀和加速度計(jì)對(duì)振動(dòng)非常敏感，故采集的原值中疊加了大量振動(dòng)信息，對(duì)姿態(tài)角的修正效果產(chǎn)生極大的影響，導(dǎo)致修正后的姿態(tài)角與真值的誤差較大[15-16]?；谒脑獢?shù)的EKF是將用于姿態(tài)解算的四元數(shù)q、三軸速度V和位置S作為狀態(tài)向量，磁力計(jì)和GPS作為觀測(cè)傳感器，對(duì)狀態(tài)量進(jìn)行修正，有效避免了加速度計(jì)觀測(cè)所帶來的振動(dòng)的影響；并將IMU(包括三軸陀螺儀、加速度計(jì)和磁力計(jì))、GPS、氣壓計(jì)等傳感器的數(shù)據(jù)進(jìn)行融合。此外將Z軸加速度計(jì)偏置azb、三軸磁力計(jì)偏置mb、三軸陀螺儀偏置ωb和三軸大地磁場(chǎng)M也作為狀態(tài)量，這些狀態(tài)量不是由狀態(tài)預(yù)測(cè)過程直接修改的，而是通過后續(xù)的狀態(tài)觀測(cè)進(jìn)行修改；故狀態(tài)量是一個(gè)20維的向量，狀態(tài)矩陣方程為

(1)

選擇機(jī)體坐標(biāo)系的坐標(biāo)原點(diǎn)O位于慣性測(cè)量單元所安裝的重心處，xb軸沿機(jī)體縱軸指向前，yb軸沿機(jī)體橫軸指向右，zb軸垂直于Oxbyb平面沿機(jī)體豎軸向下；導(dǎo)航坐標(biāo)系Oxnynzn選用地理坐標(biāo)系，即北東地坐標(biāo)系。四元數(shù)下表示的機(jī)體坐標(biāo)b系到大地坐標(biāo)的n系的旋轉(zhuǎn)矩陣為

(2)

則由四元數(shù)解算出的姿態(tài)角為

(3)

式中θ——滾轉(zhuǎn)角φ——俯仰角

γ——偏航角

1.2.1EKF預(yù)測(cè)過程

首先通過上一狀態(tài)的最優(yōu)估計(jì)值和將要施加的控制量來預(yù)測(cè)當(dāng)前狀態(tài)，建立系統(tǒng)的預(yù)測(cè)狀態(tài)方程

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

(4)

其中

式中F——狀態(tài)轉(zhuǎn)移矩陣

G——控制矩陣

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

w——系統(tǒng)噪聲

要求F矩陣，需要找到Xk+1中每一項(xiàng)對(duì)應(yīng)于Xk的更新方程。

四元數(shù)的更新采用龍格-庫塔公式

(5)

其中

式中ω——陀螺儀三軸角速率

Δt——上一狀態(tài)到當(dāng)前狀態(tài)的時(shí)間增量

速度預(yù)測(cè)方程為

(6)

式中ax、ay、az——三軸加速度計(jì)測(cè)量值

位置預(yù)測(cè)方程為

Sk+1=Sk+VkΔt

(7)

其他狀態(tài)量的預(yù)測(cè)值均等于上一時(shí)刻的最優(yōu)估計(jì)值。有了各狀態(tài)量的預(yù)測(cè)更新方程，得出狀態(tài)轉(zhuǎn)移矩陣為

(8)

其中

式中I——單位對(duì)角陣O——零矩陣

計(jì)算協(xié)方差預(yù)測(cè)值P，用于求解卡爾曼增益K，即

(9)

其中

式中Qk——傳感器誤差的過程噪聲

Qs——使濾波器穩(wěn)定附加的過程噪聲

F矩陣在前邊已經(jīng)算得，P和Q的初值可根據(jù)不同傳感器的特性給定，下面介紹G的計(jì)算方法。

要求G矩陣，需要找到Xk當(dāng)中每一項(xiàng)對(duì)于uk的更新方程?？刂屏渴峭ㄟ^控制電動(dòng)機(jī)的轉(zhuǎn)速來改變飛機(jī)的姿態(tài)、速度和方向的；所以G矩陣中與控制量相關(guān)的就是四元數(shù)和速度。由式(5)和式(6)的四元數(shù)和速度的更新方程計(jì)算出G矩陣

(10)

其中

且

式中σω——三軸陀螺儀噪聲方差

σa——三軸加速度計(jì)噪聲方差

從而通過式(9)可計(jì)算出P。

1.2.2磁力計(jì)數(shù)據(jù)融合

機(jī)體上三軸磁力計(jì)的預(yù)測(cè)值為

(11)

三軸磁力計(jì)的觀測(cè)值為

以X軸為例，進(jìn)行數(shù)據(jù)融合，計(jì)算新息νk+1。

νmx=mx-Zmx

(12)

其中

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

(13)

對(duì)式(13)求導(dǎo)得

X軸的觀測(cè)矩陣為

計(jì)算卡爾曼增益

(14)

式中R——觀測(cè)噪聲

其數(shù)值由觀測(cè)傳感器確定狀態(tài)量的最優(yōu)估計(jì)值為

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

(15)

計(jì)算當(dāng)前狀態(tài)的協(xié)方差

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

(16)

至此X軸磁力計(jì)數(shù)據(jù)融合完成。Y軸和Z軸磁力計(jì)數(shù)據(jù)融合與X軸類似。

1.2.3速度和位置的數(shù)據(jù)融合

磁力計(jì)數(shù)據(jù)融合采用EKF算法，而速度和位置的數(shù)據(jù)融合直接采用Kalman filter算法。速度和位置的數(shù)據(jù)融合用的傳感器是GPS和氣壓計(jì)；GPS直接測(cè)量水平方向的速度和位置，氣壓計(jì)測(cè)量垂直方向上的位置，滿足Kalman filter的3個(gè)前提條件[17-18]。

速度和位置的觀測(cè)量為

Zvs=[VxVyVzSxSySz]

速度和位置的狀態(tài)預(yù)測(cè)如式(6)、(7)。

計(jì)算Kvs增益

(17)

計(jì)算新息

νvs=X[VxVyVzSnSeSd]-Zvs

(18)

狀態(tài)量的最優(yōu)估計(jì)值為

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

(19)

至此，磁力計(jì)、速度和位置的數(shù)據(jù)融合已全部結(jié)束。通過式(3)，用四元數(shù)的最優(yōu)估計(jì)值可計(jì)算出飛行器當(dāng)前的姿態(tài)角。

1.2.4CPF-EKF算法

傳統(tǒng)EKF收斂判據(jù)采用不等式[19-20]

(20)

式中r——安全系數(shù)，r≥1

tr——矩陣的跡

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

(21)

N為設(shè)定的復(fù)位閾值，當(dāng)連續(xù)3個(gè)運(yùn)行周期滿足式(21)時(shí)，判定EKF發(fā)散，并用CPF解算的姿態(tài)角值作為EKF的復(fù)位參考值，對(duì)EKF重新初始化，從而抑制了EKF的發(fā)散，保證三軸姿態(tài)角的準(zhǔn)確解算。

2 試驗(yàn)分析及算法驗(yàn)證

2.1 試驗(yàn)平臺(tái)

為了驗(yàn)證算法的有效性和可行性，搭建了基于STMF429的雙傳感器硬件平臺(tái)，主要包括八軸多旋翼無人機(jī)、慣性測(cè)量單元MPU6050(集成了三軸陀螺儀、三軸加速度計(jì)、氣壓高度計(jì))、LSM303D型三軸磁力計(jì)和Neo-M8N GPS模塊，并集成了CPF-EKF算法。另一組傳感器采用荷蘭Xsens公司生產(chǎn)的MTi慣性測(cè)量單元，其自帶卡爾曼濾波算法，能直接輸出高精度的姿態(tài)角。滾轉(zhuǎn)角、俯仰角動(dòng)態(tài)精度為±0.1°，偏航角動(dòng)態(tài)精度為±0.5°，試驗(yàn)平臺(tái)見圖2。無人機(jī)滿載質(zhì)量為24.532 kg，電動(dòng)機(jī)額定工作轉(zhuǎn)速為6 000 r/s；由于加速度計(jì)能夠直接返回三軸加速度值，對(duì)于多旋翼無人機(jī)，電動(dòng)機(jī)在額定工作轉(zhuǎn)速下機(jī)體的3個(gè)坐標(biāo)軸產(chǎn)生的振動(dòng)情況用加速度計(jì)返回值間接表征振動(dòng)強(qiáng)度，更能直觀反映無人機(jī)在大載荷、強(qiáng)振動(dòng)工況下疊加到機(jī)體的振動(dòng)信息，試驗(yàn)表明，機(jī)體坐標(biāo)系下，X和Y軸的振動(dòng)強(qiáng)度峰峰值為±0.488g，Z軸的振動(dòng)強(qiáng)度峰峰值為±0.732g。

圖2 八軸十六旋翼試驗(yàn)平臺(tái)Fig.2 Experimental platform based on sixteen-rotor UAV

為了保證飛行試驗(yàn)的安全性，使用MTi輸出的姿態(tài)角指導(dǎo)飛行，STMF429系統(tǒng)集成的CPF-EKF算法僅對(duì)傳感器數(shù)據(jù)進(jìn)行姿態(tài)解算，不介入飛行過程的指導(dǎo)；在STMF429主控硬件平臺(tái)上運(yùn)行20維狀態(tài)量的CPF-EKF算法，解算周期為15 ms，而無人機(jī)姿態(tài)控制周期為20 ms，試驗(yàn)分為靜態(tài)和動(dòng)態(tài)兩種驗(yàn)證。

2.2 靜態(tài)試驗(yàn)及結(jié)果分析

把無人機(jī)放在水平地面上，偏航角保持在一個(gè)非0°的固定的角度(因?yàn)樵?°附近，偏航角度會(huì)在0°～180°之間跳變)，IMU的采樣頻率為50 Hz，LSM303D型三軸磁力計(jì)采樣頻率為10 Hz，GPS的采樣頻率為5 Hz，共計(jì)18 000個(gè)采樣點(diǎn)，合約360 s的采集數(shù)據(jù)，靜態(tài)測(cè)試得到的滾轉(zhuǎn)角、俯仰角、偏航角如圖3所示。

圖3 靜態(tài)條件下的三軸姿態(tài)角Fig.3 Three axes attitude angle under static condition

靜態(tài)測(cè)試顯示，在不引入電動(dòng)機(jī)工作所帶來的振動(dòng)干擾以及外部風(fēng)干擾的情況下，滾轉(zhuǎn)角和俯仰角精度能達(dá)到±0.05°，偏航角精度為±0.2°，由于水平地面不能保持嚴(yán)格水平，故滾轉(zhuǎn)角、俯仰角出現(xiàn)不同程度小角度。該試驗(yàn)結(jié)果說明CPF-EKF姿態(tài)解算方法對(duì)于減小陀螺儀的零漂具有極大的優(yōu)勢(shì)，并且能持續(xù)保持穩(wěn)定的高精度。

2.3 動(dòng)態(tài)試驗(yàn)及結(jié)果分析

為了驗(yàn)證CPF-EKF姿態(tài)解算方法在無人機(jī)動(dòng)態(tài)條件下的有效性，采用無人機(jī)飛行采集的實(shí)測(cè)數(shù)據(jù)進(jìn)行了分析。采用荷蘭Xsens公司生產(chǎn)的MTi慣性測(cè)量單元的姿態(tài)角輸出作為參考對(duì)照。試驗(yàn)無人機(jī)參數(shù)如表1所示。

表1 試驗(yàn)無人機(jī)參數(shù)Tab.1 Experimental UAV parameters

圖4是飛行試驗(yàn)的現(xiàn)場(chǎng)圖，飛行試驗(yàn)的2D軌跡和3D軌跡如圖5和圖6所示，以無人機(jī)起飛位置作為原點(diǎn)，記錄飛行過程中每一時(shí)刻的位置，為了驗(yàn)證EKF檢測(cè)模塊的作用效果，讓無人機(jī)在空中進(jìn)行熱點(diǎn)環(huán)繞，飛行一個(gè)圓形軌跡，采樣時(shí)間1 000 s。圖7是不帶CPF檢測(cè)模塊的EKF算法與MTi解算的三軸姿態(tài)角的對(duì)比圖，圖8是帶CPF檢測(cè)模塊的EKF算法與MTi的對(duì)比圖。

圖4 飛行試驗(yàn)現(xiàn)場(chǎng)Fig.4 Flight experiment scene

圖5 飛行試驗(yàn)2D軌跡Fig.5 2D trajectory in flight experiment

圖6 飛行試驗(yàn)3D軌跡Fig.6 3D trajectory in flight experiment

由圖7可知，不帶CPF復(fù)位檢測(cè)模塊的EKF姿態(tài)解算方法，在解算過程中三軸姿態(tài)角出現(xiàn)發(fā)散現(xiàn)象，其中偏航方向上的發(fā)散尤為嚴(yán)重，在飛行時(shí)間為320 s時(shí)，偏航角的解算開始出現(xiàn)滯后現(xiàn)象，并且此現(xiàn)象越來越嚴(yán)重，到700 s時(shí)出現(xiàn)發(fā)散現(xiàn)象，偏航角完全不能跟蹤期望角度，俯仰角和滾轉(zhuǎn)角也出現(xiàn)了不同程度的發(fā)散現(xiàn)象，滾轉(zhuǎn)角優(yōu)于俯仰角，雖基本能跟蹤期望角度，但解算精度大大降低。造成這種現(xiàn)象的原因是EKF需要對(duì)方程進(jìn)行線性化假設(shè)，無人機(jī)的大載荷和強(qiáng)振動(dòng)的工作條件，使線性化假設(shè)不成立，強(qiáng)制線性化會(huì)導(dǎo)致濾波器極度不穩(wěn)定，在姿態(tài)解算過程中容易發(fā)散。

圖7 EKF姿態(tài)解算Fig.7 EKF attitude calculation

圖8 CPF-EKF姿態(tài)解算Fig.8 CPF-EKF attitude calculation

圖8是帶CPF復(fù)位檢測(cè)的EKF算法解算的三軸姿態(tài)角，其中復(fù)位值N設(shè)定為0.274 16，表示三軸姿態(tài)角差值之和超過30°時(shí)，CPF-EKF復(fù)位，在320 s時(shí)，復(fù)位模塊檢測(cè)到EKF有發(fā)散趨勢(shì)，并且連續(xù)3個(gè)運(yùn)行周期滿足式(21)設(shè)定的要求，判定EKF發(fā)散，于是對(duì)EKF進(jìn)行復(fù)位操作，用CPF解算的姿態(tài)角作為EKF的當(dāng)前姿態(tài)角，方差和其他狀態(tài)量進(jìn)行初始化，重新進(jìn)入EKF循環(huán)，繼續(xù)進(jìn)行姿態(tài)角的解算。隨后分別在780 s和960 s又進(jìn)行了復(fù)位；由圖8可知，在整個(gè)飛行測(cè)試過程中，EKF總共復(fù)位3次，三軸姿態(tài)角被準(zhǔn)確跟蹤，證明該算法的有效性。

N值是以CPF算法為參考的EKF算法的復(fù)位閾值，它表征的是兩種算法的三軸姿態(tài)角差值的平方和所允許達(dá)到的閾值，一旦超過了這個(gè)閾值，就判定EKF發(fā)散，對(duì)EKF進(jìn)行初始化操作，由此表明，N值越小，復(fù)位條件越苛刻，EKF輸出的姿態(tài)角就越接近CPF的輸出，由于CPF解算姿態(tài)角的精度不高，故導(dǎo)致EKF解算的姿態(tài)角精度也不高。當(dāng)N值選擇過大，會(huì)使EKF發(fā)散判定條件過于寬松，無法及時(shí)判斷EKF發(fā)散從而錯(cuò)過復(fù)位的恰當(dāng)時(shí)機(jī)，也會(huì)導(dǎo)致EKF輸出的姿態(tài)角不準(zhǔn)確。故對(duì)N值的選擇要適當(dāng)；N值的確定與無人機(jī)的載荷大小密切相關(guān)，搭載的載荷越大，無人機(jī)工作時(shí)電動(dòng)機(jī)的額定轉(zhuǎn)速越大，振動(dòng)越強(qiáng)，EKF越容易發(fā)散，此時(shí)EKF復(fù)位條件應(yīng)當(dāng)越苛刻，N值取得越小；此外，無人機(jī)飛行的速度和高度以及傳感器的減振效果對(duì)N值也有影響，速度和高度越大，施加于無人機(jī)的控制量就越大，電動(dòng)機(jī)轉(zhuǎn)速和振動(dòng)會(huì)相應(yīng)增加，N越?。唤?jīng)過大量試驗(yàn)驗(yàn)證，工程上選定N值的范圍在0.14～0.28，表征三軸姿態(tài)角差值之和在20°～30°之間。

圖9和圖10給出了CPF-EKF算法所解算出的三軸陀螺儀偏置和三軸磁力計(jì)偏置。由圖可知，三軸陀螺儀偏置誤差和磁力計(jì)偏置誤差在剛上電時(shí)波動(dòng)較大，說明上電時(shí)對(duì)陀螺儀和磁力計(jì)的影響較大，但上電后短時(shí)間內(nèi)經(jīng)過濾波器補(bǔ)償后陀螺儀偏置和磁力計(jì)偏置迅速收斂并保持穩(wěn)定，且在EKF出現(xiàn)發(fā)散趨勢(shì)而迅速復(fù)位后依然保持收斂且無大的跳變，從而保證了在EKF計(jì)算過程中能夠準(zhǔn)確地去除陀螺儀和三軸磁力計(jì)偏置誤差，進(jìn)而保證姿態(tài)角的解算精度。

圖9 三軸陀螺儀偏置Fig.9 Gyro bias of three axes

圖10 三軸磁力計(jì)偏置Fig.10 Magnetometer bias of three axes

3 結(jié)論

(1)針對(duì)植保無人機(jī)大載荷、強(qiáng)機(jī)動(dòng)性的需求，對(duì)八軸多旋翼無人機(jī)結(jié)構(gòu)進(jìn)行改進(jìn)，提出共軸雙槳和旋翼的傾斜結(jié)構(gòu)。共軸雙槳結(jié)構(gòu)滿足了植保無人機(jī)大載荷的需求，旋翼的傾斜配置解決了傳統(tǒng)無人機(jī)依靠反扭力矩控制偏航而導(dǎo)致偏航方向控制力矩不足的問題，提高了無人機(jī)的機(jī)動(dòng)性。

(2)針對(duì)大載荷植保無人機(jī)的姿態(tài)解算方法，提出了基于20維狀態(tài)量的CPF-EKF姿態(tài)解算方法，并引入CPF算法作為復(fù)位檢測(cè)模塊，對(duì)EKF算法進(jìn)行發(fā)散時(shí)的復(fù)位操作。靜態(tài)試驗(yàn)表明，該算法的滾轉(zhuǎn)角、俯仰角解算精度為±0.05°，偏航角精度為±0.2°。動(dòng)態(tài)試驗(yàn)中，在大載荷、強(qiáng)振動(dòng)的試驗(yàn)條件下，姿態(tài)角能準(zhǔn)確跟蹤、解算精度高，并且在EKF出現(xiàn)發(fā)散趨勢(shì)時(shí)CPF對(duì)其及時(shí)復(fù)位，保證了姿態(tài)角的解算精度。試驗(yàn)采用MTi慣性測(cè)量單元的輸出作為參考，對(duì)比結(jié)果表明，CPF-EKF算法的動(dòng)態(tài)解算精度能達(dá)到滾轉(zhuǎn)角和俯仰角精度為±0.1°，偏航角精度為±0.5°。在STMF429主控硬件平臺(tái)上運(yùn)行20維狀態(tài)量的CPF-EKF算法，解算周期為15 ms，而無人機(jī)姿態(tài)控制周期為20 ms，EKF解算周期小于控制周期，EKF輸出實(shí)時(shí)性良好，適用于無人機(jī)動(dòng)態(tài)性能要求較高的導(dǎo)航信息解算。

1 張東彥，蘭玉彬，陳立平，等.中國農(nóng)業(yè)航空施藥技術(shù)研究進(jìn)展與展望[J/OL].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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 薛新宇，蘭玉彬.美國農(nóng)業(yè)航空技術(shù)現(xiàn)狀和發(fā)展趨勢(shì)分析[J/OL].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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ī)低空變量噴藥系統(tǒng)設(shè)計(jì)與霧滴沉積規(guī)律研究[J/OL]. 農(nóng)業(yè)機(jī)械學(xué)報(bào), 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 徐東甫,白越,宮勛,等. 基于六軸多旋翼飛行器的赤眼蜂投放系統(tǒng)設(shè)計(jì)與試驗(yàn)[J/OL]. 農(nóng)業(yè)機(jī)械學(xué)報(bào),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 張棟，焦嵩鳴，劉延泉.互補(bǔ)濾波和卡爾曼濾波的融合姿態(tài)解算方法[J].傳感器與微系統(tǒng)，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 陳孟元，謝義建，陳躍東.基于四元數(shù)改進(jìn)型互補(bǔ)濾波的MEMS姿態(tài)解算[J].電子測(cè)量與儀器學(xué)報(bào)，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 張欣，白越，趙常均，等.多旋翼姿態(tài)解算中的改進(jìn)自適應(yīng)擴(kuò)展Kalman算法[J].光學(xué)精密工程，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 彭程.共軸八旋翼飛行器姿態(tài)與航跡跟蹤控制研究[D].長(zhǎng)春：吉林大學(xué)，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 張常云. 自適應(yīng)濾波方法研究[J]. 航空學(xué)報(bào)，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ī)遠(yuǎn)程控制噴霧系統(tǒng)[J/OL].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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，F(xiàn)AN 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 竇景欣，孔祥希，聞邦椿.四旋翼姿態(tài)的反步滑模自抗擾控制及穩(wěn)定性[J].東北大學(xué)學(xué)報(bào)，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 李繼宇，張鐵民，彭孝東，等．四旋翼飛行器農(nóng)田位置信息采集平臺(tái)設(shè)計(jì)與實(shí)驗(yàn)[J/OL]．農(nóng)業(yè)機(jī)械學(xué)報(bào)，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天線及單陀螺的車輪轉(zhuǎn)角測(cè)量系統(tǒng)[J/OL].農(nóng)業(yè)機(jī)械學(xué)報(bào),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無人飛行器及其飛行控制系統(tǒng)設(shè)計(jì)[J].光學(xué)精密工程，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 彭丁聰.卡爾曼濾波的基本原理及應(yīng)用[J].軟件導(dǎo)刊，2009,20(11)：32-34.

18 張文，孫瑞勝.EKF與UKF的性能比較及應(yīng)用[J].南京理工大學(xué)學(xué)報(bào)，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 趙德安，賈偉寬，張?jiān)?等.農(nóng)業(yè)機(jī)器人自主導(dǎo)航改進(jìn)自適應(yīng)濾波控制器研究[J/OL].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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ìn)的自適應(yīng)卡爾曼濾波算法[J].太原科技大學(xué)學(xué)報(bào)，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)