基于MEMS室内定位的姿态角解算∗

李蓉李哲

（西安邮电大学计算机学院 西安 710061）

基于MEMS室内定位的姿态角解算∗

李蓉李哲

（西安邮电大学计算机学院 西安 710061）

为提高基于MEMS室内定位的精度，将加速度计和陀螺仪应用到室内定位中提出了一种新的求解姿态角的方法。通过将加速度计和陀螺仪的数据相结合，使用四元数法求出初级姿态角，再将初级姿态角与陀螺仪的角速度相结合使用卡尔曼融合，得出最终的姿态角。该方法与现有的方法相比，减小了来自噪声和磁场的干扰。仿真结果表明：该解算算法能够提高姿态角的准确度。

加速度计；陀螺仪；姿态角；四元数；卡尔曼融合

1 引言

微机电系统（Micro Electro Mechanical System，MEMS）传感器体积小、重量轻、成本低、易于集成和实现智能化的特点［1］，可用于室内导航定位。室内导航是利用MEMS的加速度计、陀螺仪、磁力计和气压传感器构成的惯性测量单元（Inertial Mea⁃surement Unit，IMU）。结合导航算法软件实现的一种小范围导航定位系统。MEMS传感器分别可以测量到：载体三轴的加速度；三轴的角速度；三轴的磁场大小和强度；大气压的强度［2］。

MEMS传感器直接将采集到的数据进行导航定位，误差会很大。需要对采集到的数据进行零偏校正、四元数以及卡尔曼滤波处理，主要对姿态角进行算法处理。尽管磁力计可以直接计算出航向角［3］，但它容易受到周围磁场的干扰；加速度计可以直接计算出俯仰角和翻滚角，而且不会产生累积误差，但在运动时，会产生很大的噪声；尽管陀螺仪能够克服磁场的干扰，而且动态性能良好，但容易受到漂移的影响以及积分产生的累积误差的影响，结合各自的优缺点，提出了利用加速度计和陀螺仪相结合求解姿态角的方法。

2 系统总体方案设计

通过I2C接口实现对MEMS传感器数据的采集［4］，采集出来的原始数据，需要进行算法处理，消除运动过程中产生的误差。处理之后，加速度计的数据通过坐标转换矩阵，从载体坐标系转换到水平坐标系，转换后，去掉重力（Z轴）加速度，得到水平面的加速度；加速度两次积分得到位移［5］，再通过航位推算算法得到导航的位置坐标。

由于基于MEMS的惯性导航定位，计算出的位置是相对位置。所以要实现精确的导航定位，就需要选则最优的初始位置。因此，可以在建筑物内部的重要位置贴有二维码，在二维码中添加位置信息，通过扫描二维码得到某个位置的位置信息，然后使用航位推算算法，计算出某一时刻的位置信息，达到定位的目的。

3 姿态角解算

利用加速度传感器采集的加速度数据经过一次积分得到速度信息，再次积分得到位移信息的，但在做积分变换前先要对采集到的数据进行坐标转换，即从载体坐标系（加速度计采集数据所在坐标系）转换为水平坐标系［6］，如图1所示。

图中O-XnYnZn为水平坐标系，O-XYZ为载体坐标系；角θ为俯仰角，是载体坐标系以Y轴为中心轴旋转得到的，是X轴与水平面之间的夹角；角γ为翻滚角，是载体坐标系以X轴为中心轴旋转得到，是Y轴与水平面之间的夹角；绕向上的Z轴旋转会得到航向角。

先用加速度计和陀螺仪相结合，使用四元数法得到初级的姿态角，此时只是计算得到了姿态角，但未经滤波处理，会有误差产生。因此对计算得到的姿态角和陀螺仪的角速度相结合，使用卡尔曼融合得到终极姿态角，即就是经过滤波处理的姿态角。

3.1 四元数法计算初级姿态角

四元数［7］的表示为：q=[q0，q1，q2，q3]。从导航坐标系n与载体坐标系b之间的姿态变换矩阵［8］，用四元数表示为

图1 载体坐标系与水平坐标系之间的关系

1）初始化四元素

使用四元数之前，先要进行定义并初始化四元数的四个变量。

2）规范化处理

分别用加速度的采集数据，除以a，得到各轴的规范化加速度［9］。

3）重力加速度

旋转得到导航坐标系的重力加速度g0={0，0，1}'带入式（1）可得：

4）叉乘积求出误差

ax，ay，az是测量得到的重力向量，gx，gy，gz是陀螺积分后的姿态来推算出的重力向量，它们都是以MEMS传感器的坐标作为参照系，因此它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。

通过加速度计测得的重力坐标系下的单位向量与上一时刻四元数转换成的单位向量进行叉乘，得到的误差量外积再相减就可以得到姿态误差V_error。

5）调节陀螺仪的误差

利用合适的调节参数，修正陀螺仪积分产生的漂移误差。

V_error_I+=V_error*Ki；

Gyro+=V_error*Kp+V_error_I；

其中Ki和Kp是调节参数。利用互补滤波，将姿态误差补偿到角速度上，可以修正角速度积分漂移。

6）更新并规范四元数的值

四元数微分方程，从一个参考坐标系转换到另一个系，对应的矩阵［10］：

采用一阶龙格库塔尔［11］，求解四元数的微分方程，即：将 q(t+h)=q(t)+0.5hw(t)*q(t)带入式（4），得到更新后四元数的值，然后再做规范化处理。

7）欧拉角的转换

由载体坐标系b到导航坐标系n三次旋转对应的坐标变换矩阵为

3.2 卡尔曼融合计算终级姿态角

将四元数法得到的姿态角陀螺仪相结合，使用卡尔曼融合［12］，求出终极姿态角。

角度的预测值Angle：

其中Angle为角度，Gyro为陀螺仪输出的角速度值，Q_bias为静态漂移值，dt为采样时间。需要注意的是右边角度和角速度必须对应，如果角度为俯仰角，那么角速度就是俯仰角的角速度。

近似认为某一时刻的漂移与其上一时间的漂移是相同的：

1）先验估计

假设现在的状态为k，可以依据上一状态，预测出现在的状态：

由式（6）～（7）得矩阵：

式（8）～（9）中的值相对应，需要注意的是U(k)为Gyro，它是现在状态的控制量，没有控制量时，默认为0。

2）协方差矩阵的预测

系统的结果已经更新，但现在状态的协方差P还没有更新：

P(k|k-1)是 x(k|k-1)的协方差；P(k-1|k-1)为上一时刻的预测方差阵，卡尔曼滤波的目的就是让这个阵最小；Q是系统过程噪声的协方差，这里噪声分别为角度值的噪声和漂移的噪声，即为静止时的角度值和角速度值。

3）计算卡尔曼增益

将式（10）化解后分子为

其中卡尔曼增益Kg(k)设为|k0 k1|T；这里H为|1 0|，它是测量系统参数，单测量时为1；R是系统测量噪声的协方差，这里R为R_angle角度测量噪声。

4）通过卡尔曼增益来修正误差

结合预测值和测量值，可以得到现在状态的最优化估算值：

其中，Z(k)为Accel，它是由计算出来的值。由式（11）可以得到修正后的姿态角、偏移量以及角速度，以X轴为例：

Angle+=k0×Angle_err；

Q_bias+=k1×Angle_err；

Gyro_x=Gyro-Q_bias；

5）更新协方差矩阵

现在状态的最优估算值已经得到，但为了使卡尔曼滤波不断地迭代下去直至结束，还需要更新协方差矩阵：

4 姿态角的仿真结果

图2是没有经过算法处理的姿态角的仿真图，图3是经过四元数法和卡尔曼滤波处理后得到的姿态角的仿真图，分别如下图所示：

图2和图3从上到下依次是对俯仰角、翻滚角和航向角的仿真。理想情况下，在空中绕Z轴旋转若干圈后，俯仰角和翻滚角基本不变且为零，航向角做周期性的变化。实际操作中会存在一些问题，比如，旋转的过程中手会发生稍微的抖动，导致不是完全绕着Z轴在旋转。

图2中的俯仰角、翻滚角和航向角都是未经过算法和滤波处理的，自始至终俯仰角和翻滚角都呈现出杂乱无章的变化。而图3是经过四元数和卡尔曼融合处理后的姿态角，俯仰角和翻滚角在刚开始的时候也是呈现出杂乱无章的变换。但后期变化幅度明显减小且趋近于0，而航向角变化不大。因此，可以得出图3的姿态角变化趋势明显比图2的要准确。

5 结语

未经过算法处理的姿态角容易受到漂移和噪声的影响，会产生较大的误差。但是经过四元数法和卡尔曼融合处理后的俯仰角和翻滚角受到的影响明显减弱，说明四元数法和卡尔曼融合在姿态角解算过程中起到很重要的作用。

［1］吴远星.基于MEMS传感器的嵌入式捷关惯性测量单元及其应用研究［D］.成都：西南交通大学，2014.WU Yuanxing.Embedded Strapdown Inertial Measure⁃ment Unit and Application Based on MEMS Sensors［D］.Chengdu：Southwest Jiaotong University Master Degree Thesis，2014.

［2］陈曦.基于MEMS惯性传感器的高精度姿态测量关键技术研究［D］.杭州：浙江大学，2014.CHEN Xi.The High-precision Attitude Measurement Sys⁃tem Based on MEMS Inertial Sensers Research on Key Technologies of Quantity［D］.Hongzhou：Zhejiang Univer⁃sity 2014.

［3］韩盈党.基于MEMS传感器室内导航的系统设计［D］.西安：西安邮电大学，2014.HAN Yingdang.Indoor Navigation System Based on MEMS Sensors［D］.Xi'an：Xi'an University of Posts and Telecommunications，2014.

［4］何亚军，邓飞其.嵌入式Linux中I2C总线驱动程序设计［J］.计算机工程与设计，2008，29（10）：2517-2519.HE Yajun，DENG Feiqi.Design of I2C Bus Driver Under Embedded Linux System［J］.Computer Engineering and Design，2008，29（10）：2517-2519.

［5］刘西河，胡恒章，宋有山，等.加速度计误差模型补偿在方位捷联平台惯性导航系统中的应用［J］.中国惯性技术学报，1995，6（20）：48-52.LIU Xihe，HU Hengzhang，SONG Youshan，et al.Applica⁃tion of Acceleromenters Error Model Compensation in Azi⁃muth Stradown Platform INS［J］.Journal of Chinese Iner⁃tial Technology，1995，6（20）：48-52.

［6］Kannan B，Meneguzzi F，Dias M B，et al.Predictive indoor navigation using commercial smart-phones［C］.Coimbra，Portugal：ACM symposium on Applied computing.2013：519-525.

［7］施闻明，徐彬，陈利敏.捷联式航姿系统中四元素算法Kalman滤波器的实现研究［J］.自动化技术与应用，2005，24（11）：6-8.SHI Wenming，XU Bin，CHEN Limin.Realization of the Quaternion-Based Kalman Filter for Strapdown AHRS［J］.Control Theory and Application，2005，24（11）：6-8.

［8］秦永元.惯性导航［M］.北京：科学出版社，2014：288.QIN Yongyuan.Inertial Navigation［M］.Beijing：Science Press，2014：288.

［9］衡霞，王忠民.基于手机加速度传感器的人体行为识别［J］.西安邮电大学学报，2014，19（6）：76-79.HENG Xia，WANG Zhongmin.Human Activity Recogni⁃tion Based on Accelerometer Data from a Mobile Phone［J］.Journal of Xi'an University of Posts and Telecommuni⁃cations，2014，19（6）：76-79.

［10］杨辉.基于MEMS传感器的高精度行人导航算法研究［D］.厦门：厦门大学，2014.YANG Hui.High Precision Pedestrian Navigation Sys⁃tem Based on MEMS sensor［D］.Xiamen：Xiamen Uni⁃versity，2014.

［11］谭珍珍.大型民机飞行控制系统建模仿真研究［D］.南京：南京航空航天大学，2014.TAN Zhenzhen.Research on Modeling and Simulation for Flight Control System of Large Civil Aircraft［D］.Nan⁃jing：Nanjing University of Aeronautics and Astronautics，2014.

［12］周树道，金永奇，卫克晶，等.采用卡尔曼滤波算法的MEMS器件姿态测量［J］.实验室研究与探索，2015，34（2）：38-42.ZHOU Shudao，JIN Yongqi，WEI Kejing，et al.Attitude Measurement of MEMS Devices by Using the Kalman Fil⁃ter Algorithm［J］.Research and Exploration in Laborato⁃ry，2015，34（2）：38-42.

Calculation of Attitude Angle Based on MEMS Indoor Positioning

LI Rong LI Zhe
（School of Computer，Xi'an University of Posts and Telecommunications，Xi'an 710061）

In order to improve the accuracy of indoor positioning based on MEMS a new method is proposed to solve the atti⁃tude angle，which is to use accelerometer and gyroscope in indoor positioning.Combining data from the accelerometer and gyroscope to calculate the primary attitude angle by using quaternions，then the primary attitude angle is combined with the angular velocity of the gyroscope to calculate he final attitude angle by using Kalman fusion，compared with the existing method，the method can re⁃duce the interference from noise and magnetic field.Simulation results show that the solution algorithm can improve the accuracy of attitude angle.

accelerometer，gyroscope，attitude angle，quaternions，Kalman fusion

Class Number TP212.9

TP212.9

10.3969/j.issn.1672-9722.2017.12.012

2017年6月9日，

2017年7月30日

李蓉，女，硕士研究生，研究方向：嵌入式应用软件的设计与实现。李哲，男，硕士，硕士生导师，研究方向：电路与系统、嵌入式系统、电子与通信工程。