STM32的多傳感器融合姿態(tài)檢測

黃志偉，徐蘇楠，韋一，唐瑩（中國計量學(xué)院光學(xué)與電子科技學(xué)院，浙江杭州310018）

?

STM32的多傳感器融合姿態(tài)檢測

黃志偉，徐蘇楠，韋一，唐瑩
（中國計量學(xué)院光學(xué)與電子科技學(xué)院，浙江杭州310018）

摘要：采用STM32微控制芯片作為主控芯片，搭載MPU6050傳感器、HMC5883L磁場傳感器，以四旋翼飛行器為平臺進(jìn)行載體姿態(tài)檢測的實驗.對比單一傳感器和多傳感器姿態(tài)檢測的實驗結(jié)果，結(jié)果表明：基于多傳感器融合的姿態(tài)檢測能彌補單一傳感器檢測姿態(tài)的不足，提高姿態(tài)信息的精確性.

關(guān)鍵詞：姿態(tài)檢測；STM32；MPU6050；HMC5883L；傳感器

姿態(tài)檢測在醫(yī)療儀器、機器人、慣性測量單元、汽車電子、導(dǎo)航控制等領(lǐng)域應(yīng)用廣泛［1－3］.國內(nèi)外眾多學(xué)者對基于傳感器的姿態(tài)檢測進(jìn)行了大量的研究和應(yīng)用.浙江大學(xué)利用加速度傳感器對人體姿態(tài)進(jìn)行檢測來監(jiān)測人體跌倒［4］.哈爾濱工業(yè)大學(xué)機器人研究所利用慣性傳感器來檢測機器人的姿態(tài)，并取得良好效果［5］.國防科技大學(xué)機電工程與自動化利用慣性傳感器對機器人進(jìn)行水平姿態(tài)的檢測［6］.美國密歇根大學(xué)機械工程系通過在棒球和壘球內(nèi)部安裝姿態(tài)檢測單元，研究棒球壘球在投擲、飛行、下落過程中運動軌跡和特點［7］.姿態(tài)檢測作為慣性測量的內(nèi)容，還可應(yīng)用于飛行器的起飛、降落、導(dǎo)航等［8－13］領(lǐng)域.本文討論了多傳感器融合進(jìn)行姿態(tài)檢測的硬件設(shè)計，并搭建了四旋翼飛行器的平臺.

1　硬件設(shè)計

基于Cortex－M3型內(nèi)核的STM32型系列芯片性能優(yōu)越、性價比極高，專為低功耗領(lǐng)域設(shè)計等優(yōu)點［14－15］，選擇STM32f103c8t6型芯片作為主控芯片，該芯片是一款32位ARM微控制器，最高支持72 MHz時鐘頻率，處理速度高，集成了很多常用外設(shè)大量的通用輸入輸出接口，非常適用于多傳感器的控制處理.陀螺儀和加速度傳感器選用Invensence公司的MPU6050型芯片，該芯片將三軸陀螺儀和三軸加速度傳感器集成，能同時輸出6軸信息，消除了由于安裝和焊接帶來的軸間誤差，減少了軟件的補償，保證實驗的準(zhǔn)確性.通過IIC接口，擴展外部傳感器輸出9軸信息.磁場傳感器采用美國Honeywell公司生產(chǎn)的HMC5883L型高精度磁場傳感器，分辨率達(dá)0.002G最大輸出頻率達(dá)160 Hz.高度測量采用的是BMP085型氣壓傳感器，測量范圍為300～110kPa，在超低功耗模式和超高效模式下，測量精度分別為0.06，0.03hPa高度分別精確至0.5，0.25m.陀螺儀和加速度傳感器模塊與磁場傳感器相整合，可以完成九軸姿態(tài)的解算其電路如圖1所示.

圖1　九軸姿態(tài)測量模塊Fig.1　Test module of nine axes attitude detection

高度傳感器直接與主控模塊通過IIC總線相連，實現(xiàn)快速通信，用以檢測的實驗平臺為四旋翼飛行

器，整體硬件模塊示意圖及實物飛控電路板，如圖2所示.

圖2　四旋翼飛行器的實驗平臺Fig.2　Test platform of quadrotor helicopter

2　四旋翼飛行器的姿態(tài)檢測

陀螺儀是姿態(tài)檢測中的核心部件，眾多關(guān)于姿態(tài)求解的理論也都是基于陀螺儀［16－17］.三軸陀螺儀能輸出載體繞3個坐標(biāo)旋轉(zhuǎn)的角速度量，通過對時間積分，得到載體繞旋轉(zhuǎn)軸的角度.加速度傳感器也是姿態(tài)檢測中的重要慣性器件，它測量載體相對慣性空間的絕對加速度和引力加速度之差，稱為比力.在靜止的狀態(tài)下，三軸加速度讀傳感器能測量重力在載體在3個軸上的重力分量.磁場傳感器靈敏度高，能夠測量地球弱磁場，通過磁場的分布，能夠計算出載體當(dāng)前偏離北向的角度.高度測量通過氣壓傳感器測量大氣壓強度，間接測量高度的變化.實驗以四旋翼飛行器為測試平臺，測試在振動干擾情況下姿態(tài)檢測的準(zhǔn)確性.

2.1 陀螺儀測量姿態(tài)

陀螺儀通過對時間積分求得載體姿態(tài)，但是有嚴(yán)重的漂移現(xiàn)象，包括靜止時零點的偏移和溫度引起的漂移.這些偏移在積分過程中會形成累積誤差，使最終計算得到的角度會隨時間漂移而不準(zhǔn)確.首先，對陀螺儀數(shù)據(jù)進(jìn)行零偏校正.陀螺儀靜止時，三軸輸出數(shù)據(jù)在理想情況下應(yīng)該為0，根據(jù)這一特性，在每次硬件初始化工作時，取靜止下一組陀螺儀輸出數(shù)據(jù)求平均值作為陀螺儀的零位偏移，以后讀取的數(shù)據(jù)每次都要減去這個偏移值.然后，對校正偏移后的數(shù)據(jù)進(jìn)行滑動平均濾波，完成對陀螺儀數(shù)據(jù)的處理.用陀螺儀輸出角速度對時間積分來測量姿態(tài)角.陀螺儀檢測靜止條件下的角度（φ），如圖3所示.

圖3　陀螺儀測量角度變化Fig.3　Variation of gyroscope measurement angle

姿態(tài)角的計算周期是2ms，由圖3可知：在水平靜止120s情況下，陀螺儀測量到的角度漂移了2.5°左右，并且角度的漂移是持續(xù)的；更長時間后，角度誤差會積累到更大.因此，僅靠陀螺儀測量姿態(tài)會產(chǎn)生較大的測量誤差，要進(jìn)一步提高精度，必須融合其他種類的傳感器.

2.2 姿態(tài)的測量

加速度能測量載體受到的比力大小，無其他外力的情況下，載體只受到來自地球的引力，根據(jù)引力在3個坐標(biāo)軸上的分量，計算得到載體當(dāng)前的水平姿態(tài)信息.根據(jù)加速度傳感器特性，彌補陀螺儀在靜態(tài)情況下的不足，提高姿態(tài)檢測的精確度［18－20］.按照文獻(xiàn)［16］的方法對三軸加速度傳感器進(jìn)行校準(zhǔn)，由于三軸加速度傳感器是由一個雙軸傳感器和一個單軸傳感器組合而成，所以，可以省略軸間差校準(zhǔn)這一步驟.實際上，加速度傳感器不僅對重力，而且對外力產(chǎn)生的加速度都非常敏感.換言之，它不能區(qū)分加速度是重力引起的，還是除了重力之外的力引起的.因此，加速度傳感器測量到的信號是重力加速度和外力加速度的總和.在數(shù)據(jù)融合時，采用互補濾波器加入閉環(huán)控制進(jìn)行水平姿態(tài)的測量.

互補濾波器利用加速度傳感器測量重力矢量＾gb和陀螺儀測量到的角速度矢量Ωb，分別對重力矢量和角速度矢量進(jìn)行低通濾波和高通濾波.將重力矢量＾gb做歸一化處理，得到一個單位向量珋v.然后，將單位向量與觀測值＾v進(jìn)行叉積.根據(jù)叉積概念，假設(shè)有a，b兩個向量，這兩個向量叉積得到一個向量，其方向與這兩個向量形成的平面垂直，新的向量模的大小為兩個向量模的積乘上兩個向量夾角的正弦值sinα.根據(jù)小角近似，sinα≈α.將珋v與＾v叉積，便會得到一個近似的角度偏差值e，這個偏差代表了真實姿態(tài)與測量姿態(tài)之間的誤差大小，將該誤差進(jìn)行比例和積分環(huán)節(jié)，用來修正陀螺儀角速度矢量的偏移.通過四元數(shù)算法，得到最終的四元數(shù)姿態(tài)矩陣.對校準(zhǔn)過的加速度進(jìn)行形如陀螺儀數(shù)據(jù)的滑動平均濾波得到可靠的數(shù)據(jù).在四旋翼飛行器電機開啟情況下進(jìn)行姿態(tài)檢測測試，得到的俯仰角（θ）和橫滾角（γ），如圖4，5所示.

圖4　四旋翼飛行器姿態(tài)檢測時的俯仰角角度變化Fig.4　Pitch angle variation of quadrotor ahelicopter attitude detection

圖5　四旋翼飛行器姿態(tài)檢測時的橫滾角度變化Fig.5　Roll angle variation of quadrotor ahelicopter attitude detection

開啟四旋翼飛行器的4個電機，檢測到俯仰角和橫滾角能保持在水平位置，只在±1°的范圍內(nèi)漂移.當(dāng)將飛行器姿態(tài)調(diào)整到22.5°時，系統(tǒng)能夠迅速響應(yīng)，并保持新的位置姿態(tài)，檢測角度誤差在±1°范圍內(nèi).

2.3 偏航角

偏航角的融合采用了磁場傳感器，變化的電流會產(chǎn)生磁場.因此，磁場傳感器會受到各種外界磁場干擾，而使測量結(jié)果不準(zhǔn)確，必須注意選取合適的磁場傳感器安裝環(huán)境.HMC5883L型三軸磁場傳感器能夠?qū)Φ厍虼艌鲞M(jìn)行測量，通過磁場在x軸與y軸的分布，確定載體當(dāng)前偏離北向的角度.然而，磁場傳感器必須嚴(yán)格保持水平才能測得準(zhǔn)確的偏航角.在三維條件下，需要加速度傳感器和陀螺儀融合獲得的水平方向橫滾角和俯仰角對磁場傳感器進(jìn)行傾斜補償，以獲得動態(tài)傾斜狀態(tài)下的偏航角.

當(dāng)磁場傳感器水平放置時，偏航角的計算公式為

式（1）中：Mx和My為磁場傳感器測量到的x軸和y軸磁場大小.當(dāng)磁場傳感器傾斜時，設(shè)磁場傳感器輸出為Mγ，θ，φ＝［Mx，1，My，1，Mz，1］T，相同偏航角水平放置的傳感器輸出為M0，0，φ＝［Mx，0，My，0，Mz，0］T.由歐拉旋轉(zhuǎn)公式有

式（2）中：Rγ和Rθ是繞y軸旋轉(zhuǎn)的橫滾角γ和繞x軸旋轉(zhuǎn)的俯仰角θ的旋轉(zhuǎn)矩陣.

代入式（2）可得

由式（4）可求得Mx，0和My，0，由式（1），可求得當(dāng)前載體偏航角.

在實際環(huán)境中，由于磁場硬鐵效應(yīng)和軟鐵效應(yīng)的存在，導(dǎo)致磁場傳感器旋轉(zhuǎn)一周得到的并不是一個標(biāo)準(zhǔn)的圓，而是一個橢圓.對于羅盤誤差的修正，采用最小二乘法對橢圓進(jìn)行矯正，將偏差補償?shù)皆紨?shù)據(jù)上.實驗方法是將傳感器在水平方向上旋轉(zhuǎn)一周，得到x軸和y軸數(shù)據(jù)，并繪制出二維坐標(biāo)圖.在Matlab中，用最小二乘法對原始數(shù)據(jù)進(jìn)行擬合，得到橢圓的長短半軸以及偏離中心點（0，0）的坐標(biāo).補償后的偏航角（φ）實驗結(jié)果，如圖6所示.

圖6　補償后的偏航角變化Fig.6　Yaw angle variationafter compensation

磁場正北方向通過指南針測得，將磁北規(guī)定為0°方向，北偏東為正方向.將磁場傳感器放置于正北方向，檢測到偏航角度大約0.5°.一段時間后，將磁場傳感器按正方向旋轉(zhuǎn)到20°，檢測到偏航角度為25°，誤差為5°.

2.4 高度測量模塊

BMP085氣壓傳感器能測量大氣壓，氣壓與海拔高度（h）存在如下函數(shù)關(guān)系，即

通過氣壓與高度的函數(shù)關(guān)系，間接計算海拔高度.BMP085只有一個控制寄存器，地址為0xF4，用來配置氣壓和溫度的測量模式.BMP085不需要上電初始化時進(jìn)行驅(qū)動，而是在每次測量前都需要向控制寄存器內(nèi)寫入指令.該傳感器通過自帶的補償算法進(jìn)行高度的計算，所以，在測量前，需要讀出傳感器內(nèi)部自帶EEPROM里的一些校準(zhǔn)數(shù)據(jù).

實測中氣壓傳感器數(shù)據(jù)波動較大，影響高度計算的準(zhǔn)確性.因此，采用滑動均值濾波的方法對氣壓數(shù)據(jù)進(jìn)行濾波.10個氣壓數(shù)據(jù)先后入隊列，進(jìn)行均值濾波，每次濾波后最先進(jìn)入隊里的數(shù)據(jù)被新的數(shù)據(jù)替換，取得了比較良好的效果，并且數(shù)據(jù)實時性沒有受到影響.利用樓層高度對氣壓傳感器進(jìn)行測試，經(jīng)濾波后的氣壓趨勢圖，如圖7（a）所示.

根據(jù)氣壓與高度的計算公式計算出海拔高度，圖7（a）所對應(yīng)的樓層海拔高度，如圖7（b）所示.由于海平面是處于不斷變化中的，氣壓隨氣候季節(jié)和溫度的變化也非常明顯.因此，絕對海拔高度的測量誤差較大.由圖7（b）可知：地平面海拔大約為－2m，與實驗當(dāng)?shù)氐暮０蜗嗖钶^大.相對高度的測量，樓層高度為3.84m，氣壓計測量到的最大值為3.97m，最小值為3.65m，誤差在20cm內(nèi).因此，在四旋翼飛行器系統(tǒng)初始化的時候，記錄此時的氣壓計數(shù)據(jù)，并將高度設(shè)置為零，通過測量氣壓變化差值.計算并反饋四旋翼飛行器的飛行高度，氣壓計的誤差在可接受范圍內(nèi)，可以有效地減少氣壓變化帶來的誤差.

圖7　氣壓趨勢測量圖Fig.7　Detection figure of pressure trend

3　結(jié)束語

基于STM32實現(xiàn)的多傳感器融合的姿態(tài)檢測方法，能夠彌補單個傳感器的缺陷.采用多種傳感器融合后的姿態(tài)檢測方法，抗干擾能力強，動態(tài)響應(yīng)快速，輸出俯仰角和橫滾角誤差不超過1°.偏航角誤差在5°左右.這是因為俯仰角和橫滾角的誤差會隨著傾斜補償公式累積到偏航角上.設(shè)計的姿態(tài)檢測

方案在四旋翼飛行器平臺上實驗效果良好.需要說明的是，設(shè)計的姿態(tài)測量硬件模塊同時也適用于其他一些姿態(tài)測量應(yīng)用，如人體姿態(tài)的檢測.

參考文獻(xiàn)：

［1］胡煉，趙祚喜，吳曉鵬.ADIS16300四自由度IMU在姿態(tài)測量中的應(yīng)用［M］.電子設(shè)計工程，2010，18（7）：103－106.［2］蔣少茵.機械手模型控制及軌跡定位精度測試［J］.華僑大學(xué)學(xué)報：自然科學(xué)版，1999，20（4）：396－399.

［3］陳勝奮，謝明紅.工業(yè)機器人運動碰撞的仿真實現(xiàn)［J］.華僑大學(xué)學(xué)報：自然科學(xué)版，2015，36（2）：137－141.

［4］文耀鋒.一種實時的跌倒姿態(tài)檢測和心率監(jiān)護(hù)系統(tǒng)的研究［D］.杭州：浙江大學(xué)，2008：2－3.

［5］秦勇，臧希喆，王曉宇，等.基于MEMS慣性傳感器的機器人姿態(tài)檢測系統(tǒng)的研究［J］.傳感技術(shù)學(xué)報2007，20（2）：298－301.

［6］何偉，陳偉.基于MEMS慣性傳感器的機器人水平姿態(tài)檢測系統(tǒng)設(shè)計［J］.機器人技術(shù)與應(yīng)用，2009，10（5）：51－55.

［7］MCGINNIS R S，PERKINS N C.A highly miniaturized，wireless inertial measurement unit for characterizing the dynamics of pitched baseballs and softballs［J］.Sensors，2012，12（9）：11933－11945.

［8］WENDEL J，MEISTER O，SCHLAILE C，et al.An integrated GPS/MEMS－IMU navigation system for an autonomous helicopter［J］.Aerospace Science and Technology，2006，10（6）：527－533.

［9］GRZONKA S，GRISETTI G，BURGARD W.A fully autonomous indoor quadrotor［J］.IEEE Press，2012，28（1）：90－100.

［10］BOUABDALLAH P，MURRIERI P，SIEGWAR R.Towards autonomous indoor micro VTOL［J］.Autonomous Robots，2005，18（1）：171－183.

［11］KIM S G，CRASSIDIS J L，CHENG Y，et al.Kalman filtering for relative spacecraft attitude and position estimation［J］.Journal of Guidance，Control，and Dynamics，2007，30（1）：133－143.

［12］REHBINDER H，HU X.Drift－free attitude estimation for accelerated rigid bodies［J］.Automatica，2004，40（4）：653－659.

［13］JURMAN D，JANKOVEC M，KAMNIK R，et al.Calibration and data fusion solution for the miniature attitude and heading reference system［J］.Sensors and Actuators A：Physical，2007，138（2）：411－420.

［14］邱文挺，杜勇前.可視化電氣火災(zāi)預(yù)警系統(tǒng)的設(shè)計與實現(xiàn)［J］.華僑大學(xué)學(xué)報：自然科學(xué)版，2013，34（6）：646－648.

［15］郭尚佳，唐艷芳，李鐘慎.一種高精度超聲波測距系統(tǒng)的設(shè)計［J］.華僑大學(xué)學(xué)報：自然科學(xué)版，34（5）：489－493.

［16］林生榮，張輝.三軸加速度傳感器校正方法研究［M］.傳感器與微系統(tǒng)，2011，30（11）：72－74.

［17］李文望.微機械振動陀螺隔離耦合的結(jié)構(gòu)設(shè)計［J］.華僑大學(xué)學(xué)報：自然科學(xué)版，2003，22（4）：380－384.

［18］林生榮，張輝.三軸加速度傳感器校正方法研究［J］.傳感器與微系統(tǒng)，2011，30（11）：72－74.

［19］YOO T S，HONG S K，YOON H M，et al.Gain－scheduled complementary filter design for a MEMS based attitude and heading reference system［J］.Sensors，2011，11（4）：3816－3830.

［20］COLORADO J，BARRIENTOS A，MARTINEZ A，et al.Mini－quadrotor attitude control based on hybrid backstepping and frenet－serret theory［C］∥IEEE International Conference on in Robotics and Automation（ICRA）.New York：IEEE Press，2010：1617－1622.

（責(zé)任編輯：陳志賢 英文審校：吳逢鐵）

Attitude Detection Based on STM32 Multi－Sensor Fusion

HUANG Zhi－wei，XU Su－nan，WEI Yi，TANG Ying
（College of Optical and Electronic Technology，China Jiliang University，Hangzhou 310018，China）

Abstract：Attitude detection was to measure the attitude information of the carrier in three dimension space and show it in the shape of euler angle visually.This article take STM32microcontroller as the main control chip，carry the MPU6050 sensor，HMC5883Lmagnetic sensor.We make some experiment about the attitude detection on the platform of quad－rotor helicopter and compare the experimental result between signal sensor and multi－sensor.The result shows that attitude detection based on multi－sensor can cover the weakness of signal sensor，enhance the accuracy of the attitude information.Keywords：attitude detection；STM32；MPU6050；HMC5883L；sensor

通信作者：唐瑩（1981－）女，副教授，博士，主要從事半導(dǎo)體器件及其嵌入式系統(tǒng)的研究.E－mail：tangy＠cjlu.edu.cn.

中圖分類號：TP 368.1

文獻(xiàn)標(biāo)志碼：A

文章編號：1000－5013（2015）04－0422－05

doi：10.11830/ISSN.1000－5013.2015.04.0422

收稿日期：2015－06－15基金項目：浙江省公益性技術(shù)應(yīng)用研究項目（2013C31068）