张　港，朱勤翔，郭　薇，杨晓辉（.上海交通大学区域光纤通信网与新型光通信系统国家重点实验室，上海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的导航定位方面的研究。