徐萬芝 高國偉 潘宏生 楊寰 毛瑞燕

1.北京信息科技大學(xué)傳感器重點(diǎn)實(shí)驗(yàn)室，北京100192；

2.北京國科艦航傳感技術(shù)有限公司，北京 100101

3.北京機(jī)械設(shè)備研究所物資供應(yīng)室，北京 100039

一、引言

隨著我國工業(yè)技術(shù)的不斷發(fā)展，姿態(tài)測量廣泛應(yīng)用于各個領(lǐng)域中，無論是定位、定向，還是導(dǎo)航都離不開姿態(tài)測量。運(yùn)動體姿態(tài)測量系統(tǒng)是提供運(yùn)動體三軸的角速率、俯仰角和橫滾角信息的裝置，是運(yùn)動體實(shí)現(xiàn)姿態(tài)控制的前提，它不僅可以為運(yùn)動體姿態(tài)控制系統(tǒng)提供三維姿態(tài)信息，也可以為數(shù)據(jù)記錄器提供運(yùn)動體的三維姿態(tài)信息以備事后分析。

隨著微電子技術(shù)的發(fā)展而出現(xiàn)的微電子機(jī)械系統(tǒng)（MEMS）具有尺寸小、重量輕、成本低、功耗小和動態(tài)范圍寬等優(yōu)點(diǎn)，被廣泛應(yīng)用于測量領(lǐng)域[1,2]。本文應(yīng)用MEMS技術(shù)，設(shè)計出一種成本低、性能好、功耗低的運(yùn)動體姿態(tài)測量系統(tǒng)，能較好的實(shí)現(xiàn)運(yùn)動體姿態(tài)角度的測量。

二、系統(tǒng)總體設(shè)計

系統(tǒng)基本的結(jié)構(gòu)框圖如圖1所示。 本文選用六軸運(yùn)動處理組件，包含了三個單軸的加速度計和三個單軸的陀螺儀，加速度計測量運(yùn)動體在坐標(biāo)系統(tǒng)中獨(dú)立三軸的加速度信號，而陀螺儀檢測運(yùn)動體相對于導(dǎo)航坐標(biāo)系的角速度信號，從而測量運(yùn)動體在三維空間中的角速度和加速度。本文的設(shè)計思路是通過慣性測量模塊測量運(yùn)動體的加速度和角速度信號，再經(jīng)微處理器模塊進(jìn)行姿態(tài)解算，進(jìn)而得出運(yùn)動體的姿態(tài)信息，最后通過串口通信模塊將數(shù)據(jù)傳輸?shù)缴衔粰C(jī)，得到實(shí)時且可視化的姿態(tài)信息。

三、系統(tǒng)硬件設(shè)計

1、微處理器模塊

本系統(tǒng)微處理器模塊選用STM32F103C8T6，這是意法半導(dǎo)體公司 生 產(chǎn) 的ARM 32位 的Cortex?-M3 CPU，工作頻率最高可達(dá)72MHz，內(nèi)置高速存儲器，擁有64K字節(jié)的閃存和20K字節(jié)的SRAM，豐富的增強(qiáng)I/O端口和聯(lián)接到兩條APB總線的外設(shè)，工作電壓范圍在2.0～3.6V，具有高速性、低成本、高性價比等優(yōu)點(diǎn)。STM32F103C8T6的最小系統(tǒng)如圖2所示。

2、慣性測量模塊

慣性測量模塊是姿態(tài)測量系統(tǒng)的重要組成部分，本系統(tǒng)所選用的是硬件為MPU-6050模塊。MPU-6050是一個可直接使用的模塊，整合了MPU-6050芯片、供電電路及其端口電路，測量精準(zhǔn)且性價比較高。MPU-6050芯片設(shè)有3個16位模擬-數(shù)字轉(zhuǎn)換器，用于數(shù)字化陀螺儀和加速度計輸出。用戶可對陀螺儀和加速度計進(jìn)行編程控制，陀螺儀滿量程測量范圍為 ±250、±500、±1000與 ±2000°/s，加速度計滿量程測量范圍為±2g、±4g、±8g與±16g，對于快速和慢速運(yùn)動可進(jìn)行精確跟蹤。模塊實(shí)物及端口電路如圖3所示。

MPU-6050芯片是全球首例整合性六軸運(yùn)動處理傳感器，它消除了組合陀螺儀與加速度計時存在軸間差的問題，使封裝空間得到了大量減少，消除了加速度計與陀螺儀的軸間敏感度，降低了人為設(shè)定產(chǎn)生的影響與傳感器的漂移。內(nèi)含數(shù)字運(yùn)動處理(DMP: Digital Motion Processing)引擎，可減少復(fù)雜的融合演算數(shù)據(jù)、傳感器同步化、姿勢感應(yīng)等的負(fù)荷。MPU-6050芯片連接電路如圖4所示。

MPU-6050的供電電壓為3.3V，通過一個低壓差線性穩(wěn)壓器供電，電路圖如圖5所示。

3、串口通信模塊

為了將計算得到的運(yùn)動體姿態(tài)信息顯示出來，我們采用RS232 接口與PC機(jī)進(jìn)行交互通信。一方面，PC機(jī)實(shí)時接收運(yùn)動體的加速度和角速度等參數(shù)；另一方面，可通過PC機(jī)對測量系統(tǒng)的工作參數(shù)進(jìn)行補(bǔ)償設(shè)置。STM32103C8T6的最高工作電壓3.6V，但與RS232的電壓標(biāo)準(zhǔn)本不匹配，故而本系統(tǒng)采用MAX3232 芯片來實(shí)現(xiàn)電平轉(zhuǎn)換。串行通信接口電路如圖6所示。

四、系統(tǒng)軟件設(shè)計

1、姿態(tài)解算原理

運(yùn)動體的姿態(tài)解算算法是實(shí)現(xiàn)運(yùn)動體姿態(tài)精確測量的核心技術(shù)之一。目前描述動態(tài)坐標(biāo)系方位關(guān)系的方法有多種，主要分為三個類別：歐拉角法、方向余弦法、四元數(shù)法。其中，歐拉角法存在“奇點(diǎn)”現(xiàn)象，不能用于全姿態(tài)運(yùn)動載體上，且實(shí)時計算較為困難。方向余弦法雖然避免了歐拉角法的“奇點(diǎn)”現(xiàn)象，但方程的計算量較大，使得工作效率降低[3]。相比于前兩類方法，四元數(shù)法能更加合理有效地描述載體的剛體空間運(yùn)動，因此本系統(tǒng)選用四元數(shù)法進(jìn)行姿態(tài)解算。

四元數(shù)微分方程的表達(dá)式為：

其中，Ωb—運(yùn)動體坐標(biāo)系相對地理坐標(biāo)系的旋轉(zhuǎn)角速度的斜對稱矩陣。

式（1）可擴(kuò)展為：

其中，λ、P1、P2、P3—四元數(shù)的四個元；

ω—運(yùn)動載體坐標(biāo)系相對參考坐標(biāo)系的旋轉(zhuǎn)角速度，且有：

采用和矩陣微分方程式求解相似的方法，求得：

其中，I—單位四元數(shù)。

（3）式為四元數(shù)微分方程式的解， 且有：

通過矩陣實(shí)時計算，由四元數(shù)可確定出運(yùn)動體機(jī)體軸確定的坐標(biāo)系M到導(dǎo)航坐標(biāo)系E的坐標(biāo)變換矩陣：

設(shè)運(yùn)動體的航向角為Ψ，俯仰角為θ，橫滾角為γ，選取導(dǎo)航坐標(biāo)系為地理坐標(biāo)系，地理坐標(biāo)系可由三次基本旋轉(zhuǎn)得到運(yùn)動體的機(jī)體坐標(biāo)系，其坐標(biāo)變換矩陣為：

通過式（7）和式（8）可得：

通過上述分析可知：若表征坐標(biāo)系變換的旋轉(zhuǎn)四元數(shù)以確定，則按式（6）可計算出姿態(tài)矩陣，再按式（9）即可確定出運(yùn)動體的航向角、俯仰角和橫滾角[4～7]。

2、系統(tǒng)主程序設(shè)計

系統(tǒng)軟件設(shè)計的主程序流程圖如圖7所示。首先進(jìn)行的是系統(tǒng)初始化，主要任務(wù)有確定運(yùn)動體的初始信息，如初始位置、初始速度等；數(shù)學(xué)平臺的初始對準(zhǔn)，以確定姿態(tài)矩陣的初始值；慣性器件的初始校準(zhǔn)，對MPU-6050的各標(biāo)度因數(shù)進(jìn)行標(biāo)定。然后進(jìn)行姿態(tài)矩陣的更新計算，以提供運(yùn)動體姿態(tài)以及計算姿態(tài)參數(shù)時的必要數(shù)據(jù)，這是算法中最重要的一部分。最后進(jìn)行姿態(tài)計算，以獲取個姿態(tài)參數(shù)，并對獲得的參數(shù)進(jìn)行提取驗(yàn)證。

五、測試結(jié)果及分析

本系統(tǒng)的測試過程是通過SGT320E型三軸轉(zhuǎn)臺來完成的，如圖8所示，它是U—O—O結(jié)構(gòu)，即臺體由U形外框、O形中框以及O形內(nèi)框組成，三軸均可連續(xù)無限旋轉(zhuǎn)，既可單軸旋轉(zhuǎn)也可多軸旋轉(zhuǎn)，它可以精確地提供系統(tǒng)在工作期間可能經(jīng)受的各種形式的角運(yùn)動模式。當(dāng)轉(zhuǎn)臺采用本機(jī)控制方式時，三軸均具有速率模式、位置模式和正弦擺動模式，通過串口仿真控制轉(zhuǎn)臺以設(shè)定的方式轉(zhuǎn)動，可為待測系統(tǒng)提供更為復(fù)雜的仿真測試環(huán)境。

本系統(tǒng)的測試方法為設(shè)定三軸轉(zhuǎn)臺的旋轉(zhuǎn)速率為10°/s和20°/s 對系統(tǒng)分別進(jìn)行兩次測量，得到兩組測量數(shù)據(jù)，測量過程中各方向角單獨(dú)測量，其他方向角置零位。俯仰角和橫滾角的測量范圍都是-90°～+90°，每10°一測，航向角的測量范圍是0°～360°，每20°一測。經(jīng)過測量得到俯仰角，橫滾角，航向角的測量數(shù)據(jù)，各角度誤差如圖9、圖10、圖11所示，其中測量數(shù)據(jù)中第一組轉(zhuǎn)臺旋轉(zhuǎn)速率為10°/s，第二組轉(zhuǎn)臺旋轉(zhuǎn)速率為 20°/s 。

通過測量數(shù)據(jù)可知，系統(tǒng)俯仰角測量的角度誤差基本保持在±2°以內(nèi)，橫滾角測量的角度誤差除個別點(diǎn)以外基本保持在±1.5°以內(nèi)，航向角測量的角度誤差則較好地保持在±1.5°以內(nèi)，且在轉(zhuǎn)臺不同的轉(zhuǎn)動速率下，轉(zhuǎn)動速率對系統(tǒng)的測量有一定影響，但測量誤差基本都保持在一定的誤差范圍內(nèi)?？梢?，系統(tǒng)對運(yùn)動體姿態(tài)角度的檢測具有較好的測量效果，系統(tǒng)設(shè)計可行。

六、結(jié)束語

本文應(yīng)用慣性傳感器模塊MPU-6050，設(shè)計了一款高性價比、高穩(wěn)定性、低功耗的運(yùn)動體姿態(tài)角度測量系統(tǒng)，可實(shí)時測量顯示運(yùn)動體各姿態(tài)角的角度。實(shí)驗(yàn)結(jié)果顯示，該系統(tǒng)測量角度較為準(zhǔn)確，在各姿態(tài)角的測量過程中，最大測量誤差不超過2°，且系統(tǒng)測量穩(wěn)定性較高，達(dá)到了系統(tǒng)設(shè)計的測量目的。