張　港，朱勤翔，郭　薇，楊曉輝（.上海交通大學(xué)區(qū)域光纖通信網(wǎng)與新型光通信系統(tǒng)國家重點實驗室，上海000；.國網(wǎng)河南省電力公司電力科學(xué)研究院，河南鄭州45005）

基于九軸MEMS定位系統(tǒng)誤差分析與補償算法研究*

張港1，朱勤翔1，郭薇1，楊曉輝2
（1.上海交通大學(xué)區(qū)域光纖通信網(wǎng)與新型光通信系統(tǒng)國家重點實驗室，上海201100；2.國網(wǎng)河南省電力公司電力科學(xué)研究院，河南鄭州450052）

基于九軸MEMS定位系統(tǒng)，對由陀螺儀零偏造成的位移平面偏轉(zhuǎn)誤差進行分析與補償。通過實驗與推導(dǎo)，首次給出偏差的定量分析和誤差傳播的數(shù)學(xué)模型。為估計偏差，設(shè)計了一種新的狀態(tài)向量中僅包含陀螺儀零偏的、簡化擴展卡爾曼濾波（SEKF）技術(shù)，克服了無加速度計輔助條件下的偏差估計問題，降低了計算量，能夠很好地滿足位移實時估計需求。硬件實驗證明：經(jīng)過偏差補償?shù)木泡SMEMS定位系統(tǒng)，能夠準(zhǔn)確估計有限范圍內(nèi)周期性運動的位移，幅值定位精確度達(dá)到90%。該系統(tǒng)可用于無人機、機器人定位等領(lǐng)域。

九軸MEMS定位系統(tǒng)；誤差分析；陀螺儀偏差估計；簡化擴展卡爾曼濾波器

0　引言

不依賴全球定位系統(tǒng)（GPS）的慣性導(dǎo)航定位是公認(rèn)的世界難題。一是由于加速度信號本身的直流偏差的存在，經(jīng)過二次積分之后位移軌跡會迅速發(fā)散；二是加速度計容易受扭轉(zhuǎn)干擾，必須去除扭轉(zhuǎn)分量才能對其信號進行積分。Woodman O［1］分析了MEMS加速度計信號二次積分后，位移誤差與時間平方呈正比；Foxlin［2］將MEMS應(yīng)用于行人位置追蹤，通過在擴展卡爾曼濾波（EKF）中周期性將速度置零來打破誤差隨時間積累；邵穎彪等人［3］設(shè)計了一種測量導(dǎo)線舞動的九軸MEMS定位系統(tǒng)，考慮了扭轉(zhuǎn)運動對加速度信號造成的影響（舞動中通常同時包含平動和扭轉(zhuǎn)運動），由于舞動中真實平動加速度信號具有周期性，其直流偏差可以通過濾波予以消除，從而避免軌跡快速發(fā)散。

本文基于九軸MEMS系統(tǒng)，研究有限空間內(nèi)的周期性運動的定位追蹤，重點考察陀螺儀零偏的誤差傳播過程和對估測位移造成的影響，創(chuàng)新性地提出一種簡化的基于EKF的新算法—SEKF來估計偏差，實驗證明：該算法能夠很好地消除還原位移中因陀螺儀零偏造成的時變偏差，適用于無人機、機器人定位等領(lǐng)域。

1　九軸系統(tǒng)的位移平面旋轉(zhuǎn)偏差

九軸MEMS系統(tǒng)［3］如圖1所示。通過仿真，觀測因陀螺儀零偏造成的偏差。仿真中各傳感器讀數(shù)模型為

式中na，t，nm，t，nω，t為白噪聲；ra，t，rm，t，rω，t為抖動噪聲；ba，bm，bω為常量偏差；各噪聲方差用阿倫方差技術(shù)［4］求出。加速度at和角速度ωt由預(yù)先設(shè)定好的軌跡給出。

圖1　九軸系統(tǒng)還原流程Fig 1　Reduction flowchart of nine-axis system

如圖2，運動軌跡設(shè)定為導(dǎo)線截面YZ平面內(nèi)的橢圓形，位移運動頻率為1 Hz，測量點同時繞傳感器坐標(biāo)系X軸扭轉(zhuǎn)，扭轉(zhuǎn)角度按正弦規(guī)律變化。

圖2　仿真軌跡示意圖Fig 2　Diagram of simulated trajectory

九軸系統(tǒng)還原軌跡如圖3所示，從圖中可見還原軌跡有明顯變形和偏移，且具有時變的特點。仿真發(fā)現(xiàn)，在不添加陀螺儀偏差bω的情況下，九軸系統(tǒng)的還原軌跡與真實軌跡十分接近，僅有小幅度偏移，如圖4，而去除其他噪聲對結(jié)果影響不明顯。這說明，導(dǎo)致九軸系統(tǒng)產(chǎn)生圖3的時變偏差的主要誤差源是陀螺儀偏差bω。

圖3　九軸系統(tǒng)還原軌跡與真實值對比Fig 3　Comparison of nine-axis system reduction trajectory with true trajectory

2　九軸系統(tǒng)的誤差傳播

為了定量地分析九軸系統(tǒng)的誤差傳播過程，假設(shè)t時刻，平動加速度為at，扭轉(zhuǎn)為Rt則在不考慮向心加速度的情況，加速度計的讀數(shù)為

圖4　九軸系統(tǒng)還原軌跡與真實值對比（bω=0）Fig 4　Comparison of nine-axis reduction trajectorywith true trajectory（bω=0）

式中g(shù)=［0 0 9.8］T；Rt為t時刻測量點的扭轉(zhuǎn)。由于傳感器噪聲和偏差的存在，從六軸算法得到的扭轉(zhuǎn)矩陣Rest，t和真實值Rt有一定差距。從式（5）可以看到，得到的加速度aest，t由兩項構(gòu)成，ΔRt（g+at）代表扭轉(zhuǎn)矩陣的的估計誤差對加速度造成的傳播誤差。

下面推導(dǎo)ΔRt關(guān)于bω的表達(dá)式。旋轉(zhuǎn)矩陣與角速度的關(guān)系如式（6）［5］

式中ωt×為叉乘算子。式（6）有解析解［6］式（8），其中，Θ（t，tk）滿足式（9）、式（10）

假設(shè)ωt在tk～tk+1區(qū)間內(nèi)為常數(shù)ωk+1，則由式（9）可得零階積分因子式（11）

Θ（tk+1，tk）=exp（Ω（ωk+1）Δt）（11）代入到式（6）中得到式（12）

式中Rt1=I3×3?？紤]陀螺儀讀數(shù)為真實值加上偏差bω，將其代入可得扭轉(zhuǎn)矩陣的估計誤差式（13）

類比式（14）和式（6）可以看出，Rerr，t相當(dāng)于是以常角速-bω在做旋轉(zhuǎn)得到的扭轉(zhuǎn)矩陣，與實際扭轉(zhuǎn)角速度無關(guān)。進一步可得到ΔRt與bω的關(guān)系式（15）

從式（16）看出，當(dāng)t比較小時，ΔRt是bω關(guān)于時間的正比例函數(shù)，這是圖3中時變偏差呈線性趨勢的主要原因

3　bω的估計與補償

結(jié)合前面的分析可知，只要能夠補償bω，九軸系統(tǒng)得到的加速度就能逼近真實值，進而得到逼近真值的位移軌跡。本節(jié)給出兩種bω估計方法。

3.1基于低通濾波的偏差估計

考慮扭轉(zhuǎn)頻率在0.1～3 Hz（比如監(jiān)測導(dǎo)線舞動時，九軸單元隨測量點周期性扭轉(zhuǎn)），可直接計算陀螺儀測量值的均值予以補償。然而隨著時間的增加，當(dāng)前讀數(shù)對偏差的更新貢獻(xiàn)越來越小，難以滿足時變特點。針對于此，提出式（18）的基于低通濾波的改進方法

式中N為常數(shù)。

圖5給出了偏差估計的響應(yīng)曲線，初始估計偏差設(shè)為0，經(jīng)過一段時間后各組均收斂到真實值。

圖5　偏差估計響應(yīng)曲線（Z軸）Fig 5　Bias estimation response curve（Z axis）

3.2基于SEKF的偏差估計

當(dāng)實際扭轉(zhuǎn)為非周期性時，不能通過低通濾波估計零偏。在平動加速度近似為零的情況下，常見的估計陀螺儀偏差方法有融合加速度計、磁力計構(gòu)建非線性EKF［7］、無跡卡爾曼濾波（UKF）［8］，補償濾波（CF）［9］等。由于平動加速度近似為零的假設(shè)與定位需求矛盾，只能考慮融合磁力計；另一方面，基于卡爾曼的算法中，陀螺儀偏差僅僅是狀態(tài)向量的一部分，為了估計偏差須進行大量的計算，為此，本節(jié)推導(dǎo)一種新的狀態(tài)向量中只包含陀螺儀偏差的SEKF。

定義從k-1到k時刻扭轉(zhuǎn)四元數(shù)增量

式中δq=［δq2δq3δq4］T，

對于磁力計有

將式（23）展開，忽略δq的二階小量

對于陀螺儀有

結(jié)合式（26）、式（27）得到bω的觀測方程（觀測噪聲未顯式地給出）

再結(jié)合bω的動力學(xué)方程即可搭建KF來估計陀螺儀偏差（具體更新過程文獻(xiàn)［7］）。

4　硬件實驗與結(jié)果分析

九軸 MEMS單元：加速度計 MMA8451Q，磁力計MAG3110和陀螺儀FXAS21002C；Zig Bee CC2530；以單目測量系統(tǒng)［10］為參照（最大誤差1cm）。在舞動試驗機上進行的三組試驗：第一組未進行陀螺儀偏差補償；二組和三組采用式（18）和基于SEKF進行偏差補償。

實驗結(jié)果如圖6所示。從圖6（a）可見未進行陀螺儀偏差補償時，九軸系統(tǒng)還原得到的位移有明顯的時變偏差，與仿真結(jié)果和理論推導(dǎo)一致。而二組和三組（圖6（b）和圖6（c）），由陀螺儀偏差造成的時變誤差明顯消除了。從表1中可以看到，基于低通的校正以后位移幅值誤差最小7.74%左右，最大13.84%，基于SEKF的最小9.51%，最大9.69%；各組頻率誤差均在1%以內(nèi)。

圖6　還原軌跡對比Fig 6　Reduction trajectory constrast

5　結(jié)論

本文基于九軸系統(tǒng)，通過仿真實驗和推導(dǎo)，證明了影響位移還原精度的主要噪聲源為陀螺儀零偏，零偏的存在會使還原軌跡產(chǎn)生嚴(yán)重的時變誤差，短時間內(nèi)為位移平面的線性旋轉(zhuǎn)偏差。SEKF算法能夠較好地估計陀螺儀偏差，從而消除位移中的時變誤差，使位移幅值還原精度提高到90%。

表1　九軸系統(tǒng)幅值與頻率誤差對比Tab 1　Amplitude and frequency error contrast of nine-axis system

［1］Woodman O.An introduction to inertial navigation［R］.Cambridge：University of Cambridge，2007.

［2］Foxlin E.Pedestrian tracking with shoe-mounted inertial sensors［J］.IEEE Computer Graphics and Applications，2005，25（6）：38-46.

［3］邵穎彪，楊曉輝，郭薇，等.基于九軸傳感器監(jiān)測導(dǎo)線舞動的研究方法［J］.智能電網(wǎng)，2015，5（1）：1-8.

［4］El-Sheimy N，Hou Haiying，Niu Xiaoji.Analysis and modeling of inertial sensors using Allan Variance［J］.IEEE Trans on Instrumentation and Measurement，2008，50（1）：140-149.

［5］Kuipers J B.Quaternions and rotation sequences［M］.Princeton，NJ：Princeton Univ Press，1999：276-277.

［6］Trawny N，Roumeliotis S.Indirect Kalman filter for 3D attitude estimation［R］.Minneapolis：University of Minnesota，2005.

［7］Lefferts E J，Markley F L，Shuster M D.Kalman filtering for spacecraft attitude estimation［J］.Journal of Guidance，Con-trol，and Dynamics，1982，5（5）：417-429.

［8］Crassidis J L，Markley F L.Unscented filtering for spacecraft attitude estima-tion［J］.Journal of Guidance，Control，and Dynamics，2003，26（4）：536-542.

［9］Foxlin E.Inertial head-tracker sensors fusion by a complementary separate-bias Kalman filter［C］∥Proceedings of VRAIS'96，IEEE，1996：185-194.

［10］敬澤，薛方正，李祖樞.基于單目視覺的空間目標(biāo)位置測量［J］.傳感器與微系統(tǒng)，2011，30（3）：125-130.

Study on error analysis and compensation algorithm based on nine-axis MEMS positioning system*

ZHANG Gang1，ZHU Qin-xiang1，GUO Wei1，YANG Xiao-hui2
（1.State Key Laboratory of Advanced Optical Communication Systems and Networks，Shanghai Jiao Tong University，Shanghai 201100，China；2.Electric Power Research Institute，State Grid HAEPC，Zhengzhou 450052，China）

Based on nine-axis micro-electro-mechanical system（MEMS）positoning system，analyze and compensate on displacement plane deflect error caused by gyroscope zero-bias.By experiment and formula derivation，quantitative analysis on deviation is firstly given，and establish a mathematic model for error propagation；to estimate deviation，design a new simplified extended Kalman filtering（SEKF）technology，which has state vector containing gyroscope zero-bias only，overcome the estimation deviation problem without accelerometer，reduce calculations amount，this algorithm can nicely meet the online estimation demand for displacement.By hardware experiment，nine-axis MEMS system can accurately estimate the displacement of cyclical movement in limited range，after bias compensation，the accuracy of magnitude positioning reaches 90%.The system can be used for unmanned aerial vehicle（UAV），robot localization，and other fields.

nine-axis MEMS tracking system；error analysis；gyroscope bias estimation；simplified extended Kalman filter

TP212

A

1000—9787（2016）06—0013—04

10.13873/J.1000—9787（2016）06—0013—04

2015—10—08

國網(wǎng)公司重大科技資助項目（17KJ160101F1002920100）；國家重點實驗室自主課題資助項目

張港（1989-），男，河北秦皇島人，碩士，主要從事基于MEMS的導(dǎo)航定位方面的研究。