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

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

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

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

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

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

0　引言

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

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

1　九軸系統的位移平面旋轉偏差

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

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

圖1　九軸系統還原流程Fig 1　Reduction flowchart of nine-axis system

如圖2，運動軌跡設定為導線截面YZ平面內的橢圓形，位移運動頻率為1 Hz，測量點同時繞傳感器坐標系X軸扭轉，扭轉角度按正弦規律變化。

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

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

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

2　九軸系統的誤差傳播

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

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

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

下面推導ΔRt關于bω的表達式。旋轉矩陣與角速度的關系如式（6）［5］

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

假設ωt在tk～tk+1區間內為常數ωk+1，則由式（9）可得零階積分因子式（11）

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

式中Rt1=I3×3。考慮陀螺儀讀數為真實值加上偏差bω，將其代入可得扭轉矩陣的估計誤差式（13）

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

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

3　bω的估計與補償

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

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

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

式中N為常數。

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

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

3.2基于SEKF的偏差估計

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

定義從k-1到k時刻扭轉四元數增量

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

對于磁力計有

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

對于陀螺儀有

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

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

4　硬件實驗與結果分析

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

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

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

5　結論

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

表1　九軸系統幅值與頻率誤差對比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］邵穎彪，楊曉輝，郭薇，等.基于九軸傳感器監測導線舞動的研究方法［J］.智能電網，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］敬澤，薛方正，李祖樞.基于單目視覺的空間目標位置測量［J］.傳感器與微系統，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

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

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