李蓉李哲

（西安郵電大學計算機學院 西安 710061）

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

李蓉李哲

（西安郵電大學計算機學院 西安 710061）

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

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

1 引言

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

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

2 系統(tǒng)總體方案設計

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

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

3 姿態(tài)角解算

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

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

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

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

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

圖1 載體坐標系與水平坐標系之間的關系

1）初始化四元素

使用四元數之前，先要進行定義并初始化四元數的四個變量。

2）規(guī)范化處理

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

3）重力加速度

旋轉得到導航坐標系的重力加速度g0={0，0，1}'帶入式（1）可得：

4）叉乘積求出誤差

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

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

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

利用合適的調節(jié)參數，修正陀螺儀積分產生的漂移誤差。

V_error_I+=V_error*Ki；

Gyro+=V_error*Kp+V_error_I；

其中Ki和Kp是調節(jié)參數。利用互補濾波，將姿態(tài)誤差補償到角速度上，可以修正角速度積分漂移。

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

四元數微分方程，從一個參考坐標系轉換到另一個系，對應的矩陣［10］：

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

7）歐拉角的轉換

由載體坐標系b到導航坐標系n三次旋轉對應的坐標變換矩陣為

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

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

角度的預測值Angle：

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

近似認為某一時刻的漂移與其上一時間的漂移是相同的：

1）先驗估計

假設現在的狀態(tài)為k，可以依據上一狀態(tài)，預測出現在的狀態(tài)：

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

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

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

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

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

3）計算卡爾曼增益

將式（10）化解后分子為

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

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

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

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

Angle+=k0×Angle_err；

Q_bias+=k1×Angle_err；

Gyro_x=Gyro-Q_bias；

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

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

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

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

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

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

5 結語

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

［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慣性傳感器的高精度姿態(tài)測量關鍵技術研究［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傳感器室內導航的系統(tǒng)設計［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］劉西河，胡恒章，宋有山，等.加速度計誤差模型補償在方位捷聯平臺慣性導航系統(tǒng)中的應用［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］施聞明，徐彬，陳利敏.捷聯式航姿系統(tǒng)中四元素算法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］譚珍珍.大型民機飛行控制系統(tǒng)建模仿真研究［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］周樹道，金永奇，衛(wèi)克晶，等.采用卡爾曼濾波算法的MEMS器件姿態(tài)測量［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日

李蓉，女，碩士研究生，研究方向：嵌入式應用軟件的設計與實現。李哲，男，碩士，碩士生導師，研究方向：電路與系統(tǒng)、嵌入式系統(tǒng)、電子與通信工程。