四旋翼飛行器航姿參考系統(tǒng)的誤差補(bǔ)償方法研究

楊志永，畢德學(xué)，孟　強(qiáng)，王濟(jì)陽

(1.天津科技大學(xué) 機(jī)械工程學(xué)院，天津　300222; 2.獨(dú)山子石化公司 乙烯廠橡膠聯(lián)合車間，新疆 克拉瑪依　833600)

?

四旋翼飛行器航姿參考系統(tǒng)的誤差補(bǔ)償方法研究

楊志永1，畢德學(xué)1，孟強(qiáng)2，王濟(jì)陽1

(1.天津科技大學(xué) 機(jī)械工程學(xué)院，天津300222; 2.獨(dú)山子石化公司 乙烯廠橡膠聯(lián)合車間，新疆 克拉瑪依833600)

摘要：航姿參考系統(tǒng)是四旋翼飛行器姿態(tài)控制和慣性測量的關(guān)鍵，基于MEMS的慣性導(dǎo)航系統(tǒng)長期穩(wěn)定性較差，陀螺儀傳感器和加速度計(jì)存在明顯的零漂現(xiàn)象，因此在使用前必須對陀螺儀傳感器和加速度計(jì)的誤差系數(shù)進(jìn)行標(biāo)定；論文首先以航姿參考系統(tǒng)的陀螺儀和加速度計(jì)實(shí)時(shí)數(shù)據(jù)的采集與處理，并通過對陀螺儀和加速度計(jì)的誤差產(chǎn)生原理的分析，提出了陀螺儀和加速度計(jì)的新型誤差補(bǔ)償模型，并推導(dǎo)了基于高斯牛頓法的誤差補(bǔ)償與標(biāo)定方法，由此簡化了標(biāo)定過程，得到了標(biāo)定結(jié)果； 實(shí)驗(yàn)結(jié)果表明了在不提高成本和降低精度的情況下該基于高斯牛頓法的誤差補(bǔ)償與標(biāo)定方法的有效性。

關(guān)鍵詞：航姿參考系統(tǒng)；高斯牛頓法；誤差補(bǔ)償與標(biāo)定

0引言

航姿參考系統(tǒng)是四旋翼飛行器姿態(tài)控制和慣性測量的關(guān)鍵，它主要由加速度計(jì)、陀螺儀和磁力計(jì)構(gòu)成，各傳感器的數(shù)據(jù)通過控制器的解算之后得到飛行器的姿態(tài)角、加速度等信息，為飛行器的控制邏輯提供重要信息。隨著現(xiàn)代科技的不斷發(fā)展，特別是微機(jī)電技術(shù)(MEMS)的不斷進(jìn)步，許多高精度微機(jī)電系統(tǒng)得以廣泛應(yīng)用。由于基于MEMS的慣性導(dǎo)航系統(tǒng)具有體積小、重量輕、成本低、不受外界干擾等優(yōu)點(diǎn)[1]，近年來在軍事及民用領(lǐng)域正得到越來越廣泛的關(guān)注[2]。

1MPU-6000傳感器概述

本文四旋翼飛行器航姿參考系統(tǒng)實(shí)驗(yàn)平臺采用了占用空間較小的InvenSense公司的MPU-6000，如圖1所示。MPU-6000是全球首例整合性6軸運(yùn)動處理組件，它整合三軸陀螺儀與三軸加速器于同一硅芯片上，內(nèi)建的數(shù)字運(yùn)動感測處理器(digital motion processor,DMP)可處理復(fù)雜的九軸運(yùn)動感測融合算法。通過I2C接口可以連接額外的數(shù)字傳感器進(jìn)行拓展，如磁力計(jì)等，擴(kuò)展之后就可以通過其I2C或SPI接口輸出9軸信號，向應(yīng)用端輸出完整的9軸融合演算技術(shù)[3]。

圖1　MPU-6000模塊

2陀螺儀零點(diǎn)漂移補(bǔ)償

陀螺儀具有良好的動態(tài)響應(yīng)性能，但其靜態(tài)響應(yīng)性能不足，根據(jù)誤差產(chǎn)生機(jī)制不同，MPU6000傳感器陀螺儀誤差主要有：零點(diǎn)漂移誤差、標(biāo)度因數(shù)誤差、安裝誤差及隨機(jī)噪聲等[4]。當(dāng)應(yīng)用陀螺儀傳感器時(shí)必須首先對誤差進(jìn)行標(biāo)定，以提高測量的精度。

對MPU6000三軸陀螺儀傳感器輸出影響最大的是零點(diǎn)漂移和標(biāo)度因數(shù)誤差，此外，由于制造工藝的限制，陀螺儀3個(gè)敏感軸還不能做到完全正交，這樣就會產(chǎn)生非正交誤差，同時(shí)陀螺儀傳感器在安裝焊接過程中也會引入安裝誤差，它和軸間非正交誤差作用效果相似，可按安裝誤差一并處理。

綜合考慮三軸陀螺儀各種誤差，依據(jù)誤差補(bǔ)償原理[5]可以得到如下公式：

(1)

以上就是對陀螺儀的完全標(biāo)定，由于ATMEGA芯片有限的內(nèi)存和計(jì)算能力，并且測試時(shí)陀螺儀標(biāo)度因數(shù)和交叉耦合項(xiàng)影響較小，這里只標(biāo)記陀螺儀的零值偏移。由于忽略了標(biāo)度因數(shù)和交叉耦合項(xiàng)的影響，可以大大簡化標(biāo)定方法。

為評估零點(diǎn)漂移的大小，使陀螺儀保持靜止?fàn)顟B(tài)，此時(shí)陀螺儀的輸出均應(yīng)為零，但是受溫度變化、電源電壓不穩(wěn)等因素的影響，靜態(tài)工作點(diǎn)發(fā)生了變化，產(chǎn)生了零點(diǎn)漂移，輸出結(jié)果和期望值并不一致，此時(shí)的誤差記為零點(diǎn)漂移誤差，MPU6000傳感器陀螺儀漂移一般不超過0.04 dps。

為更準(zhǔn)確地評估陀螺儀的零點(diǎn)漂移，陀螺儀靜止后進(jìn)行200次采樣，將200次采樣的均值作為零點(diǎn)漂移誤差，當(dāng)然，零點(diǎn)漂移誤差不能超過0.04 dps，為此可以重復(fù)以上工作，找到更好的評估值作為零點(diǎn)漂移誤差。

陀螺儀的零點(diǎn)漂移評估流程如圖2所示。

圖2陀螺儀標(biāo)定流程

3基于高斯牛頓法的加速度計(jì)誤差補(bǔ)償與標(biāo)定

基于MEMS的慣性導(dǎo)航系統(tǒng)長期穩(wěn)定性較差，存在明顯的零漂現(xiàn)象，因此在使用前必須對傳感器的誤差系數(shù)進(jìn)行標(biāo)定。國內(nèi)有很多加速度計(jì)方面的研究，加速度計(jì)標(biāo)定的方法也比較多[6-7]。

3.1高斯牛頓法誤差補(bǔ)償與標(biāo)定原理

一般的標(biāo)定方法包括靜態(tài)實(shí)驗(yàn)方法和動態(tài)實(shí)驗(yàn)方法兩種。這兩種方法或者精度較低，或者對轉(zhuǎn)臺等硬件精度需求較高，加大的成本投入，都存在一定的弊端[8]。為簡化加速度計(jì)的標(biāo)定過程，本文采用了新的高斯牛頓方法進(jìn)行標(biāo)定。

(2)

如果參數(shù)是正確的，且測量中沒有噪聲，那么無論加速度計(jì)傳感器姿態(tài)如何，上式應(yīng)滿足：

(3)

即：

(4)

(5)

如果用xi，yi，zi表示第i次采樣的讀數(shù)值，那么第i次的采樣誤差可以寫為：

(6)

現(xiàn)在可以正式描述本節(jié)所要解決的問題：

給定采樣值(xi,yi,zi)(其中i=1,…,N)，找到合適的參數(shù)mx,my,mz,δx,δy,δz，以使罰函數(shù)：

(7)

盡可能地小。

這是典型的非線性最小二乘問題，為便于求解，通常把非線性最小二乘問題轉(zhuǎn)化為一系列線性最小二乘問題求解，設(shè)x(k)是解的第k次近似，在x(k)將函數(shù)εi(x)線性化，這樣就把非線性最小二乘問題轉(zhuǎn)化為了線性最小二乘問題[9]。

在式(7)中，假設(shè)

(8)

其中：pi是n維列向量，bi為實(shí)數(shù)，x=(mx,my,mz,δx/G,δy/G,δz/G)T。令

(9)

A為m×n矩陣，b是m維列向量，則式(7)可以寫成矩陣乘積形式：

(10)

E(x)的平穩(wěn)點(diǎn)滿足：

(11)

即：

(12)

對于線性問題，A列滿秩，ATA為n階對稱正定矩陣，目標(biāo)函數(shù)E(x)的平穩(wěn)點(diǎn)：

(13)

而此處為非線性最小二乘問題，不能直接按上式計(jì)算。

將函數(shù)εi在點(diǎn)x(k)處進(jìn)行一階泰勒展開得：

(14)

令

(15)

用φ(x)近似E(x)，用φ(x)的極小點(diǎn)作為函數(shù)E(x)的極小點(diǎn)估計(jì)，求解線性最小二乘：

(16)

記Ak，ε(k)，b分別為：

則式(15)寫成：

(17)

將Ak，b帶入式(12)，得：

(18)

整理得：

(19)

(20)

其中:x(k+1)為罰函數(shù)E(x)極小點(diǎn)的第k+1次近似。式(18)就是高斯牛頓公式。d(k)為點(diǎn)x(k)處的高斯牛頓方向。為保證迭代之后函數(shù)值下降，求得方向d(k)后，從點(diǎn)x(k)起沿這個(gè)方向進(jìn)行一維搜索：

(21)

求出步長λk后，令x(k+1)=x(k)+λkd(k)作為第k+1次近似。通過不斷迭代，直到滿足迭代次數(shù)或兩次迭代差值滿足要求，求得x值。

3.2高斯牛頓法標(biāo)定加速度計(jì)步驟

1)給定初值x(1)=(0,0,0,1/G,1/G,1/G)T，允許誤差σ>0，置k=1。

2)計(jì)算函數(shù)值εi(x(k))，i=1,2,…,6得到向量

(22)

再計(jì)算一階偏導(dǎo)數(shù)

(23)

得到6×6階矩陣：

(24)

3)解方程組

(25)

求得牛頓高斯方向d(k)。

4)從點(diǎn)x(k)起，沿d(k)進(jìn)行一維搜索。求得步長λk，使得

(26)

令x(k+1)=x(k)+λkd(k)。

3.3加速度計(jì)標(biāo)定及實(shí)驗(yàn)結(jié)果

為標(biāo)定模型參數(shù)(公式5)，只需要找到6個(gè)待定參數(shù)：3個(gè)mi和3個(gè)δi。

對于1個(gè)觀察點(diǎn)(x, y, z)得到一個(gè)含6個(gè)未知數(shù)的方程，6個(gè)相互獨(dú)立的觀察點(diǎn)得到含6個(gè)未知數(shù)的6個(gè)方程，并能確定唯一解。但是由于噪聲的存在，這種方法也不準(zhǔn)確。但是可以選擇參數(shù)值，使誤差盡可能地小。為確定這6個(gè)數(shù)，至少需要6次測量，假定采用6點(diǎn)標(biāo)定法，依次測量平放、左90°放置、右90°放置、前傾90°放置、后仰90°放置和倒置6種位置(如圖3所示)的數(shù)據(jù)。實(shí)驗(yàn)步驟如下：

1)將加速度計(jì)按圖3中第一張圖片Z上位置靜止放好，測量獲得此時(shí)零點(diǎn)的20組漂移數(shù)據(jù)。

2)按照1)，順序按圖3 中的加速度計(jì)的位置方式依次放好，測量獲得每種姿態(tài)此時(shí)零點(diǎn)的20組漂移數(shù)據(jù)。直到六種姿態(tài)全部采集。在每個(gè)位置加速度計(jì)保持靜止足夠長的時(shí)間來收集樣本，盡可能多地讀取新數(shù)據(jù)，通過采樣值求平均的(實(shí)驗(yàn)選20個(gè)點(diǎn))方法減弱傳感器在讀數(shù)據(jù)時(shí)的噪聲影響。

3)重復(fù)1)和210次以上，獲得足夠的零漂數(shù)據(jù)。其中一組數(shù)據(jù)如表1 所示。

4)實(shí)驗(yàn)根據(jù)數(shù)據(jù)，利用高斯牛頓法，標(biāo)定出6個(gè)參數(shù)的值，如表1 所示。

圖3　加速度計(jì)的6種不同位置

采樣值x/m·s2y/m·s2z/m·s21-0.14530.1354-9.82042-9.78040.46820.817239.7537-0.51110.051840.30279.8224-0.01525-0.6015-9.7593-0.582160.1186-0.14300.4206

運(yùn)用高斯牛頓標(biāo)定方法，并采用Matlab程序進(jìn)行標(biāo)定，標(biāo)定結(jié)果如表2所示。

4結(jié)論

本文主要以航姿參考系統(tǒng)的陀螺儀和加速度計(jì)數(shù)據(jù)采集與應(yīng)用為基礎(chǔ)，分析了陀螺儀和加速度計(jì)的誤差產(chǎn)生原理，針對加速度計(jì)的標(biāo)定提出了新型的高斯牛頓法標(biāo)定法，簡化了標(biāo)定過程，得到了標(biāo)定結(jié)果。

參考文獻(xiàn)：

[1] 陳劍，孫金海,等．慣性系統(tǒng)中加速度計(jì)標(biāo)定方法研究[J]．微電子學(xué)與計(jì)算機(jī)，2012，29(8)：130．

[2] 陸辰，李榮冰，劉建業(yè),等．微型航姿系統(tǒng)中三軸MEMS加速度計(jì)組合誤差建模方法[J]．測試技術(shù)學(xué)報(bào)，2011，25(1)：29．

[3]DataSheet.MPU-6000andMPU-6050RegisterMapandDescription[EB/OL].http：//www.Invensense．com，2011，11．

[4] 彭孝東，陳瑜,等．MEMS三軸數(shù)字陀螺儀標(biāo)定方法研究[J]．傳感器與微系統(tǒng)，2013，32(6)：63-64．

[5] 宋麗君，秦永元．MEMS陀螺儀的一種實(shí)用標(biāo)定法[J]．壓力與聲光，2010，32(3)：372-373．

[6]SyedZF,AggarwalP,GoodallC,etal．Anewmulti-positioncalibrationmethodforMEMSinertialnavigationsystems[J]．MeasurementScienceandTechnology，2007，18(7)：1987-1907．

[7] 張紅宇，葉新生．一種改進(jìn)的IMU加表標(biāo)定模型及快速標(biāo)定方法[J]．理論與實(shí)踐，2009(3)：11-13．

[8] 劉百奇，房建成．一種改進(jìn)的IMU無定向動靜混合高精度標(biāo)定方法[J]．儀器儀表學(xué)報(bào)，2008(6):1250-1254．

[9] 張廣軍．機(jī)器視覺[M]．北京：科學(xué)出版社，2005．

Error Compensation and Calibration for Attitude Heading Reference System with Four-rotor Aircraft

Yang Zhiyong1，Bi Dexue1，Meng Qiang2，Wang Jiyang1

(1.School of Mechanical Engineering,Tianjin University of Science and Technology,Tianjin300222,China；2.Dushanzi Petrochemical Company, Ethylene&rubber Workshop,Kelamayi833600,China)

Abstract：Attitude heading reference system(AHRS)is the key part for four-rotor aircraft to its posecontrol and inertial measurement. The long-term stability of initial navigation system based on MEMS is poor, there exists obvious zero drift phenomenon, so error compensation and calibration of Gyro and Accelerometer must be completed before using it. Based on real time data collection and processing, and error producing analysis, this paper proposes a novel error compensation model for Gyro and Accelerometer, and this new error compensation and parameter calibration method based on Gauss-Newton algorithm is proposed. The method simplifies calibration procedure and obtains calibrated parameter value. Experimental results verify the effectiveness of the proposed method on the condition of no additional cost and accuracy loss.

Keywords：AHRS；Gauss-Newton algorithm；error compensation and calibration

文章編號：1671-4598(2016)02-0267-04

DOI：10.16526/j.cnki.11-4762/tp.2016.02.074

中圖分類號：TH824

文獻(xiàn)標(biāo)識碼：A

作者簡介：楊志永(1987-)，男，河北唐山人，碩士研究生，主要從事嵌入式系統(tǒng)方向的研究。畢德學(xué)(1969-)，男，山東濟(jì)南人，教授，碩士研究生導(dǎo)師，主要從事機(jī)器人方向的研究。

收稿日期：2015-09-01；修回日期：2015-10-15。