王丹丹，譚開(kāi)拓，趙建周

（安陽(yáng)工學(xué)院 電子信息與電氣工程學(xué)院，河南安陽(yáng)454000）

0 引言

進(jìn)入20世紀(jì)以來(lái)，科學(xué)技術(shù)發(fā)展迅速，人們的生活進(jìn)入了微型化時(shí)代[1]。微機(jī)電系統(tǒng)（Micro-Elector-Mechanical System，MEMS）[2]受到了越來(lái)越多的關(guān)注。加速度計(jì)是一種用來(lái)感測(cè)外部力量的一種器件[3]。當(dāng)外力作用于傳感器上時(shí)，傳感器內(nèi)部的耐震物質(zhì)會(huì)發(fā)生一定的位移從而壓電元件會(huì)產(chǎn)生感應(yīng)，通過(guò)測(cè)量傳感器的電荷數(shù)，可以得到載體的加速度。其內(nèi)部結(jié)構(gòu)如圖1所示。

圖1 加速度計(jì)結(jié)構(gòu)圖

陀螺儀又稱為角速度傳感器，是一種用來(lái)測(cè)量載體的轉(zhuǎn)動(dòng)角速度和轉(zhuǎn)動(dòng)角度的測(cè)量工具[4]，利用自身的慣性特性去測(cè)量和感知載體的運(yùn)動(dòng)狀態(tài)[5]。它不受外界的干擾，比如磁場(chǎng)或者信號(hào)等外界因素，所以得到了廣泛的應(yīng)用。利用測(cè)量載體旋轉(zhuǎn)量的陀螺儀和測(cè)量載體線性運(yùn)動(dòng)的加速度計(jì)，組合成一個(gè)測(cè)量載體慣性的單元IMU[6]，可以獲取到載體空間中姿態(tài)的各種信息，從而在生活中得到更加廣范的應(yīng)用。比如一些微型的飛機(jī)、計(jì)步器等。本文設(shè)計(jì)一個(gè)慣性導(dǎo)航小系統(tǒng)，先由陀螺儀測(cè)量載體的角速度得到獲得載體的運(yùn)動(dòng)姿態(tài)[7]，之后結(jié)合上述測(cè)量值并利用加速度計(jì)獲得的測(cè)量值，準(zhǔn)確地獲得載體的運(yùn)動(dòng)姿態(tài)的信息。修正由重力所帶來(lái)的誤差，通過(guò)一個(gè)二次的積分得到載體的位置。

1 慣性傳感器裝置設(shè)計(jì)方案的選擇

記k-1時(shí)刻的載體傾角為φk-1，角速度為?k-1，則k時(shí)刻的估計(jì)值為

式中，?為角速度的偏差，用來(lái)彌補(bǔ)輸出的角速度(wk-?k-1)；Δt為輸出時(shí)間差值。

k時(shí)刻下，系統(tǒng)的狀態(tài)模型表示為：

式中，A是狀態(tài)轉(zhuǎn)移矩陣，B為輸入模型。uk表示k時(shí)刻系統(tǒng)輸出量。wk為k時(shí)刻的噪聲。假定在k時(shí)刻載體輸出值為zk，測(cè)量噪聲vk。

卡爾曼濾波器中的輸入向量表示

式中wk為陀螺儀輸出的角速度，θk為加速度計(jì)計(jì)算得到的角度。

卡爾曼濾波器的工作過(guò)程主要分為預(yù)測(cè)過(guò)程和更新過(guò)程。此外我們還需要輸入一個(gè)初始化的值，在初始化之后k時(shí)刻系統(tǒng)的狀態(tài)的協(xié)方差矩陣為xk|k-1，得到這個(gè)矩陣用來(lái)評(píng)估與理想值之間的差距，pk|k-1值越大代表其誤差越大。

Qk是噪聲wk在k時(shí)刻的協(xié)方差矩陣，即輸出值和陀螺儀狀態(tài)估計(jì)值的協(xié)方差矩陣，因兩者無(wú)關(guān)聯(lián)，兩者的協(xié)方差等于兩者值的方差，因Qk不受時(shí)間影響，該矩陣可定義為：

卡爾曼參數(shù)Kk用于測(cè)量估計(jì)值的權(quán)值，在每個(gè)時(shí)刻根據(jù)誤差的協(xié)方差進(jìn)行更新。HPk|k-1HT+R是為了預(yù)測(cè)先驗(yàn)誤差的協(xié)方差矩陣Pk|k-1和測(cè)量值的協(xié)方差R，測(cè)量噪聲越大，該值就越大。

接著計(jì)算：xk|k的協(xié)方差，然后計(jì)算誤差協(xié)方差矩陣后，重新循環(huán)。

主要步驟為：

1）設(shè)置x和p初始值為0。

2）將陀螺儀輸出的角速度w及公式（4）帶入（5）得

4）將式 (6)代入 (8)可得

6）進(jìn)一步

2 基于卡爾曼濾波的傾角估計(jì)

對(duì)傾角去噪處理并仿真，基于卡爾曼濾波的傾角估計(jì)如圖2所示。

圖2 基于卡爾曼濾波的傾角仿真

圖2中可以看出去噪前的原始數(shù)據(jù)曲線比較平滑，對(duì)角度變化的動(dòng)態(tài)響應(yīng)有延時(shí)，敏感性低，且角度的偏差較大；經(jīng)卡爾曼濾波器后的傾角估計(jì)有了明顯的改善，且響應(yīng)速度加快。

3 裝置硬件部分的設(shè)計(jì)

該模塊內(nèi)置有電源管理寄存器，該地址為0X6B，具體描述見(jiàn)表1。

表1 電源管理寄存器描述

表中DEVICE-RESET是復(fù)位地址，當(dāng)設(shè)為1時(shí)為復(fù)位，復(fù)位結(jié)束后MPU數(shù)據(jù)清零該位，具體各位解釋見(jiàn)表2。

MPU6050模塊的電氣原理圖如圖3所示。

4 裝置上位機(jī)演示

打開(kāi)上位機(jī)軟件，選用Mini Mu軟件。將MPU6050模塊連接到TTL串口，鏈接到電腦，點(diǎn)擊串口選擇COM3，選取9600B波特率或者115200B波特率，將模塊由水平狀態(tài)，然后慢慢傾斜可觀察到屏幕上加速度圖像，角速度值和角度值，記錄不同的模塊加速度如圖4所示。

表2 CLKSEL選擇列表

圖3 主模塊電氣圖

圖4 模塊加速度

對(duì)加速度，角速度，角度及溫度進(jìn)行Matlab仿真，如圖5所示。

圖5 MATLAB仿真圖

通過(guò)TTL串口加入一個(gè)USB與手機(jī)接口的轉(zhuǎn)換器，把MPU6050模塊與手機(jī)相連接，可將模塊的輸出信息顯示出來(lái)，本實(shí)驗(yàn)證明基于智能小車的陀螺儀及加速度計(jì)的設(shè)計(jì)，達(dá)到了自主導(dǎo)航的高性能要求，且響應(yīng)迅速。