徐萬(wàn)芝 高國(guó)偉 潘宏生 楊寰 毛瑞燕

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

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

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

一、引言

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

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

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

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

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

1、微處理器模塊

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

2、慣性測(cè)量模塊

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

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

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

3、串口通信模塊

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

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

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

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

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

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

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

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

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

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

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

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

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

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

通過(guò)式（7）和式（8）可得：

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

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

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

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

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

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

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

六、結(jié)束語(yǔ)

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