基于MEMS室內(nèi)定位的姿態(tài)角解算?

李蓉李哲

（西安郵電大學(xué)計(jì)算機(jī)學(xué)院 西安 710061）

基于MEMS室內(nèi)定位的姿態(tài)角解算?

李蓉李哲

（西安郵電大學(xué)計(jì)算機(jī)學(xué)院 西安 710061）

為提高基于MEMS室內(nèi)定位的精度，將加速度計(jì)和陀螺儀應(yīng)用到室內(nèi)定位中提出了一種新的求解姿態(tài)角的方法。通過將加速度計(jì)和陀螺儀的數(shù)據(jù)相結(jié)合，使用四元數(shù)法求出初級姿態(tài)角，再將初級姿態(tài)角與陀螺儀的角速度相結(jié)合使用卡爾曼融合，得出最終的姿態(tài)角。該方法與現(xiàn)有的方法相比，減小了來自噪聲和磁場的干擾。仿真結(jié)果表明：該解算算法能夠提高姿態(tài)角的準(zhǔn)確度。

加速度計(jì)；陀螺儀；姿態(tài)角；四元數(shù)；卡爾曼融合

1 引言

微機(jī)電系統(tǒng)（Micro Electro Mechanical System，MEMS）傳感器體積小、重量輕、成本低、易于集成和實(shí)現(xiàn)智能化的特點(diǎn)［1］，可用于室內(nèi)導(dǎo)航定位。室內(nèi)導(dǎo)航是利用MEMS的加速度計(jì)、陀螺儀、磁力計(jì)和氣壓傳感器構(gòu)成的慣性測量單元（Inertial Mea?surement Unit，IMU）。結(jié)合導(dǎo)航算法軟件實(shí)現(xiàn)的一種小范圍導(dǎo)航定位系統(tǒng)。MEMS傳感器分別可以測量到：載體三軸的加速度；三軸的角速度；三軸的磁場大小和強(qiáng)度；大氣壓的強(qiáng)度［2］。

MEMS傳感器直接將采集到的數(shù)據(jù)進(jìn)行導(dǎo)航定位，誤差會很大。需要對采集到的數(shù)據(jù)進(jìn)行零偏校正、四元數(shù)以及卡爾曼濾波處理，主要對姿態(tài)角進(jìn)行算法處理。盡管磁力計(jì)可以直接計(jì)算出航向角［3］，但它容易受到周圍磁場的干擾；加速度計(jì)可以直接計(jì)算出俯仰角和翻滾角，而且不會產(chǎn)生累積誤差，但在運(yùn)動時，會產(chǎn)生很大的噪聲；盡管陀螺儀能夠克服磁場的干擾，而且動態(tài)性能良好，但容易受到漂移的影響以及積分產(chǎn)生的累積誤差的影響，結(jié)合各自的優(yōu)缺點(diǎn)，提出了利用加速度計(jì)和陀螺儀相結(jié)合求解姿態(tài)角的方法。

2 系統(tǒng)總體方案設(shè)計(jì)

通過I2C接口實(shí)現(xiàn)對MEMS傳感器數(shù)據(jù)的采集［4］，采集出來的原始數(shù)據(jù)，需要進(jìn)行算法處理，消除運(yùn)動過程中產(chǎn)生的誤差。處理之后，加速度計(jì)的數(shù)據(jù)通過坐標(biāo)轉(zhuǎn)換矩陣，從載體坐標(biāo)系轉(zhuǎn)換到水平坐標(biāo)系，轉(zhuǎn)換后，去掉重力（Z軸）加速度，得到水平面的加速度；加速度兩次積分得到位移［5］，再通過航位推算算法得到導(dǎo)航的位置坐標(biāo)。

由于基于MEMS的慣性導(dǎo)航定位，計(jì)算出的位置是相對位置。所以要實(shí)現(xiàn)精確的導(dǎo)航定位，就需要選則最優(yōu)的初始位置。因此，可以在建筑物內(nèi)部的重要位置貼有二維碼，在二維碼中添加位置信息，通過掃描二維碼得到某個位置的位置信息，然后使用航位推算算法，計(jì)算出某一時刻的位置信息，達(dá)到定位的目的。

3 姿態(tài)角解算

利用加速度傳感器采集的加速度數(shù)據(jù)經(jīng)過一次積分得到速度信息，再次積分得到位移信息的，但在做積分變換前先要對采集到的數(shù)據(jù)進(jìn)行坐標(biāo)轉(zhuǎn)換，即從載體坐標(biāo)系（加速度計(jì)采集數(shù)據(jù)所在坐標(biāo)系）轉(zhuǎn)換為水平坐標(biāo)系［6］，如圖1所示。

圖中O-XnYnZn為水平坐標(biāo)系，O-XYZ為載體坐標(biāo)系；角θ為俯仰角，是載體坐標(biāo)系以Y軸為中心軸旋轉(zhuǎn)得到的，是X軸與水平面之間的夾角；角γ為翻滾角，是載體坐標(biāo)系以X軸為中心軸旋轉(zhuǎn)得到，是Y軸與水平面之間的夾角；繞向上的Z軸旋轉(zhuǎn)會得到航向角。

先用加速度計(jì)和陀螺儀相結(jié)合，使用四元數(shù)法得到初級的姿態(tài)角，此時只是計(jì)算得到了姿態(tài)角，但未經(jīng)濾波處理，會有誤差產(chǎn)生。因此對計(jì)算得到的姿態(tài)角和陀螺儀的角速度相結(jié)合，使用卡爾曼融合得到終極姿態(tài)角，即就是經(jīng)過濾波處理的姿態(tài)角。

3.1 四元數(shù)法計(jì)算初級姿態(tài)角

四元數(shù)［7］的表示為：q=[q0，q1，q2，q3]。從導(dǎo)航坐標(biāo)系n與載體坐標(biāo)系b之間的姿態(tài)變換矩陣［8］，用四元數(shù)表示為

圖1 載體坐標(biāo)系與水平坐標(biāo)系之間的關(guān)系

1）初始化四元素

使用四元數(shù)之前，先要進(jìn)行定義并初始化四元數(shù)的四個變量。

2）規(guī)范化處理

分別用加速度的采集數(shù)據(jù)，除以a，得到各軸的規(guī)范化加速度［9］。

3）重力加速度

旋轉(zhuǎn)得到導(dǎo)航坐標(biāo)系的重力加速度g0={0，0，1}'帶入式（1）可得：

4）叉乘積求出誤差

ax，ay，az是測量得到的重力向量，gx，gy，gz是陀螺積分后的姿態(tài)來推算出的重力向量，它們都是以MEMS傳感器的坐標(biāo)作為參照系，因此它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加計(jì)測出來的姿態(tài)之間的誤差。

通過加速度計(jì)測得的重力坐標(biāo)系下的單位向量與上一時刻四元數(shù)轉(zhuǎn)換成的單位向量進(jìn)行叉乘，得到的誤差量外積再相減就可以得到姿態(tài)誤差V_error。

5）調(diào)節(jié)陀螺儀的誤差

利用合適的調(diào)節(jié)參數(shù)，修正陀螺儀積分產(chǎn)生的漂移誤差。

V_error_I+=V_error*Ki；

Gyro+=V_error*Kp+V_error_I；

其中Ki和Kp是調(diào)節(jié)參數(shù)。利用互補(bǔ)濾波，將姿態(tài)誤差補(bǔ)償?shù)浇撬俣壬希梢孕拚撬俣确e分漂移。

6）更新并規(guī)范四元數(shù)的值

四元數(shù)微分方程，從一個參考坐標(biāo)系轉(zhuǎn)換到另一個系，對應(yīng)的矩陣［10］：

采用一階龍格庫塔爾［11］，求解四元數(shù)的微分方程，即：將 q(t+h)=q(t)+0.5hw(t)*q(t)帶入式（4），得到更新后四元數(shù)的值，然后再做規(guī)范化處理。

7）歐拉角的轉(zhuǎn)換

由載體坐標(biāo)系b到導(dǎo)航坐標(biāo)系n三次旋轉(zhuǎn)對應(yīng)的坐標(biāo)變換矩陣為

3.2 卡爾曼融合計(jì)算終級姿態(tài)角

將四元數(shù)法得到的姿態(tài)角陀螺儀相結(jié)合，使用卡爾曼融合［12］，求出終極姿態(tài)角。

角度的預(yù)測值A(chǔ)ngle：

其中Angle為角度，Gyro為陀螺儀輸出的角速度值，Q_bias為靜態(tài)漂移值，dt為采樣時間。需要注意的是右邊角度和角速度必須對應(yīng)，如果角度為俯仰角，那么角速度就是俯仰角的角速度。

近似認(rèn)為某一時刻的漂移與其上一時間的漂移是相同的：

1）先驗(yàn)估計(jì)

假設(shè)現(xiàn)在的狀態(tài)為k，可以依據(jù)上一狀態(tài)，預(yù)測出現(xiàn)在的狀態(tài)：

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

式（8）～（9）中的值相對應(yīng)，需要注意的是U(k)為Gyro，它是現(xiàn)在狀態(tài)的控制量，沒有控制量時，默認(rèn)為0。

2）協(xié)方差矩陣的預(yù)測

系統(tǒng)的結(jié)果已經(jīng)更新，但現(xiàn)在狀態(tài)的協(xié)方差P還沒有更新：

P(k|k-1)是 x(k|k-1)的協(xié)方差；P(k-1|k-1)為上一時刻的預(yù)測方差陣，卡爾曼濾波的目的就是讓這個陣最小；Q是系統(tǒng)過程噪聲的協(xié)方差，這里噪聲分別為角度值的噪聲和漂移的噪聲，即為靜止時的角度值和角速度值。

3）計(jì)算卡爾曼增益

將式（10）化解后分子為

其中卡爾曼增益Kg(k)設(shè)為|k0 k1|T；這里H為|1 0|，它是測量系統(tǒng)參數(shù)，單測量時為1；R是系統(tǒng)測量噪聲的協(xié)方差，這里R為R_angle角度測量噪聲。

4）通過卡爾曼增益來修正誤差

結(jié)合預(yù)測值和測量值，可以得到現(xiàn)在狀態(tài)的最優(yōu)化估算值：

其中，Z(k)為Accel，它是由計(jì)算出來的值。由式（11）可以得到修正后的姿態(tài)角、偏移量以及角速度，以X軸為例：

Angle+=k0×Angle_err；

Q_bias+=k1×Angle_err；

Gyro_x=Gyro-Q_bias；

5）更新協(xié)方差矩陣

現(xiàn)在狀態(tài)的最優(yōu)估算值已經(jīng)得到，但為了使卡爾曼濾波不斷地迭代下去直至結(jié)束，還需要更新協(xié)方差矩陣：

4 姿態(tài)角的仿真結(jié)果

圖2是沒有經(jīng)過算法處理的姿態(tài)角的仿真圖，圖3是經(jīng)過四元數(shù)法和卡爾曼濾波處理后得到的姿態(tài)角的仿真圖，分別如下圖所示：

圖2和圖3從上到下依次是對俯仰角、翻滾角和航向角的仿真。理想情況下，在空中繞Z軸旋轉(zhuǎn)若干圈后，俯仰角和翻滾角基本不變且為零，航向角做周期性的變化。實(shí)際操作中會存在一些問題，比如，旋轉(zhuǎn)的過程中手會發(fā)生稍微的抖動，導(dǎo)致不是完全繞著Z軸在旋轉(zhuǎn)。

圖2中的俯仰角、翻滾角和航向角都是未經(jīng)過算法和濾波處理的，自始至終俯仰角和翻滾角都呈現(xiàn)出雜亂無章的變化。而圖3是經(jīng)過四元數(shù)和卡爾曼融合處理后的姿態(tài)角，俯仰角和翻滾角在剛開始的時候也是呈現(xiàn)出雜亂無章的變換。但后期變化幅度明顯減小且趨近于0，而航向角變化不大。因此，可以得出圖3的姿態(tài)角變化趨勢明顯比圖2的要準(zhǔn)確。

5 結(jié)語

未經(jīng)過算法處理的姿態(tài)角容易受到漂移和噪聲的影響，會產(chǎn)生較大的誤差。但是經(jīng)過四元數(shù)法和卡爾曼融合處理后的俯仰角和翻滾角受到的影響明顯減弱，說明四元數(shù)法和卡爾曼融合在姿態(tài)角解算過程中起到很重要的作用。

［1］吳遠(yuǎn)星.基于MEMS傳感器的嵌入式捷關(guān)慣性測量單元及其應(yīng)用研究［D］.成都：西南交通大學(xué)，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慣性傳感器的高精度姿態(tài)測量關(guān)鍵技術(shù)研究［D］.杭州：浙江大學(xué)，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傳感器室內(nèi)導(dǎo)航的系統(tǒng)設(shè)計(jì)［D］.西安：西安郵電大學(xué)，2014.HAN Yingdang.Indoor Navigation System Based on MEMS Sensors［D］.Xi'an：Xi'an University of Posts and Telecommunications，2014.

［4］何亞軍，鄧飛其.嵌入式Linux中I2C總線驅(qū)動程序設(shè)計(jì)［J］.計(jì)算機(jī)工程與設(shè)計(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ì)誤差模型補(bǔ)償在方位捷聯(lián)平臺慣性導(dǎo)航系統(tǒng)中的應(yīng)用［J］.中國慣性技術(shù)學(xué)報(bào)，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］施聞明，徐彬，陳利敏.捷聯(lián)式航姿系統(tǒng)中四元素算法Kalman濾波器的實(shí)現(xiàn)研究［J］.自動化技術(shù)與應(yīng)用，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］秦永元.慣性導(dǎo)航［M］.北京：科學(xué)出版社，2014：288.QIN Yongyuan.Inertial Navigation［M］.Beijing：Science Press，2014：288.

［9］衡霞，王忠民.基于手機(jī)加速度傳感器的人體行為識別［J］.西安郵電大學(xué)學(xué)報(bào)，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ǎo)航算法研究［D］.廈門：廈門大學(xué)，2014.YANG Hui.High Precision Pedestrian Navigation Sys?tem Based on MEMS sensor［D］.Xiamen：Xiamen Uni?versity，2014.

［11］譚珍珍.大型民機(jī)飛行控制系統(tǒng)建模仿真研究［D］.南京：南京航空航天大學(xué)，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］周樹道，金永奇，衛(wèi)克晶，等.采用卡爾曼濾波算法的MEMS器件姿態(tài)測量［J］.實(shí)驗(yàn)室研究與探索，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日

李蓉，女，碩士研究生，研究方向：嵌入式應(yīng)用軟件的設(shè)計(jì)與實(shí)現(xiàn)。李哲，男，碩士，碩士生導(dǎo)師，研究方向：電路與系統(tǒng)、嵌入式系統(tǒng)、電子與通信工程。