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

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

?

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

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

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

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

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

1　硬件設(shè)計(jì)

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

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

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

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

圖2　四旋翼飛行器的實(shí)驗(yàn)平臺(tái)Fig.2　Test platform of quadrotor helicopter

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

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

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

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

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

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

2.2 姿態(tài)的測(cè)量

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

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

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

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

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

2.3 偏航角

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

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

式（1）中：Mx和My為磁場(chǎng)傳感器測(cè)量到的x軸和y軸磁場(chǎng)大小.當(dāng)磁場(chǎng)傳感器傾斜時(shí)，設(shè)磁場(chǎng)傳感器輸出為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)前載體偏航角.

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

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

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

2.4 高度測(cè)量模塊

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

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

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

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

圖7　氣壓趨勢(shì)測(cè)量圖Fig.7　Detection figure of pressure trend

3　結(jié)束語(yǔ)

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

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

參考文獻(xiàn)：

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

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

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

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

［6］何偉，陳偉.基于MEMS慣性傳感器的機(jī)器人水平姿態(tài)檢測(cè)系統(tǒng)設(shè)計(jì)［J］.機(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è)計(jì)與實(shí)現(xiàn)［J］.華僑大學(xué)學(xué)報(bào)：自然科學(xué)版，2013，34（6）：646－648.

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

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

［17］李文望.微機(jī)械振動(dòng)陀螺隔離耦合的結(jié)構(gòu)設(shè)計(jì)［J］.華僑大學(xué)學(xué)報(bào)：自然科學(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.

中圖分類(lèi)號(hào)：TP 368.1

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

文章編號(hào)：1000－5013（2015）04－0422－05

doi：10.11830/ISSN.1000－5013.2015.04.0422

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