基于慣性傳感器的室內(nèi)慣性導(dǎo)航與定位系統(tǒng)※＊

黃洪加

（廣東工業(yè)大學(xué) 自動化學(xué)院，廣州510006）

引 言

輪式遙控機器人已經(jīng)在地震、火災(zāi)等一些危險的室內(nèi)區(qū)域進行救援和探測，或執(zhí)行反恐任務(wù)。由于在這些特殊的環(huán)境下存在諸多未知因素，且室內(nèi)無GPS信號，人們不得不依靠先進的科學(xué)技術(shù)和儀器來獲取遙控機器人小車的導(dǎo)航信息。但是，目前輪式運動小車主要采用的導(dǎo)航傳感方式有視覺、光電、超聲、里程計等，比較容易被外界環(huán)境干擾，不能滿足廣大市場的需求。

但是慣性導(dǎo)航裝備安裝在運載體內(nèi)，工作時不依賴于外界信息，也不向外界輻射能量，不容易遭到干擾，是一種自立式導(dǎo)航系統(tǒng)，優(yōu)于常見的導(dǎo)航傳感方式。并且近年來MEMS（微機電系統(tǒng)）結(jié)構(gòu)的慣性傳感器隨半導(dǎo)體技術(shù)的進步得到了迅速發(fā)展，低成本而高精度的期望得到了實現(xiàn)。MEMS慣性傳感器組成的慣性導(dǎo)航裝置結(jié)合輪式小車的里程計，能夠產(chǎn)生導(dǎo)航和定位信息，減少對外部環(huán)境的依賴，實現(xiàn)在外部環(huán)境條件（例如光照、墻壁材質(zhì)）未知情況下的導(dǎo)航。

由于是在室內(nèi)區(qū)域進行勘測搜索，小車的運行特點與飛機、船、車不同，它的運動軌跡變化較快，且運動時存在一定的振動，因此常用的卡爾曼濾波算法需要改進才能應(yīng)用。慣性傳感器采集數(shù)據(jù)量大，且進行慣性導(dǎo)航時需要大量的浮點運算，因此本項目采用了具有強大數(shù)字信號處理功能的DSP芯片TMS320F28335和PC控制終端，實現(xiàn)慣性傳感器的數(shù)據(jù)采集、時序邏輯控制、與驅(qū)動系統(tǒng)通信和地圖顯示功能，具有體積小、成本低、功耗低等優(yōu)點。

1 系統(tǒng)體系架構(gòu)

本文的目標是研制一個輪式小車慣性導(dǎo)航系統(tǒng)，能夠通過WiFi實現(xiàn)PC終端或手持終端控制輪式小車行動以及小車所采集數(shù)據(jù)的傳輸。

搭建如圖1所示的系統(tǒng)，TI公司的浮點DSP芯片TMS320F28335采集各MEMS慣性傳感器的信號并處理，處理結(jié)果通過 WiFi輸送到PC終端；PC終端負責定位結(jié)果顯示和地圖顯示，并向小車驅(qū)動系統(tǒng)發(fā)送控制命令，同時接收驅(qū)動系統(tǒng)反饋的里程計信息。

圖1 系統(tǒng)總體架構(gòu)

2 硬件設(shè)計及詳細實現(xiàn)

硬件設(shè)計上，主要分為核心板和驅(qū)動板。核心板包括DSP最小系統(tǒng)、JTAG下載口設(shè)計、電源供給電路、MEMS傳感器和WiFi模塊等。而驅(qū)動板主要設(shè)計的內(nèi)容是直流大電機的驅(qū)動模塊。

2.1 核心板設(shè)計

TMS320F28335工作時需要的電壓不同：內(nèi)核電壓1.9 V，I/O供電電壓3.3 V，所以電源部分利用兩路輸出電源器件TPS767D318來實現(xiàn)，如圖2所示。同時根據(jù)仿真實驗和實際焊接電路的測試，電源模塊輸出端最好使用電容值不小于10μF的保護電容，且不能使用貼片電容，否則工作不穩(wěn)定。

圖2 DSP電源設(shè)計

在電源設(shè)計中，考慮到TPS767D318芯片可以產(chǎn)生復(fù)位信號，所以在核心板上并沒有另外為DSP設(shè)計復(fù)位電路。

圖3為JTAG下載電路，按照仿真器的通信引腳選擇14腳的仿真接口，同時要注意EMU0和EMUl信號必須通過上拉電阻連接至電源，其中上拉電阻為10 kΩ。

2.2 小車驅(qū)動板設(shè)計

在本裝置中，采用BTS7960作為直流電機驅(qū)動芯片。BTS7960是集成的大電流半橋驅(qū)動，其內(nèi)部包含了一片NMOS、一片 PMOS和一片半橋門集驅(qū)動，在IOUT＝9 A，VS＝13.5 V，Tj＝25℃時，其內(nèi)阻抗為17 mΩ。裝置采用兩個直流大電機，圖4為驅(qū)動單個電機前后轉(zhuǎn)向的電路圖。

圖3 JTAG下載電路

圖4 驅(qū)動模塊電路設(shè)計

電機驅(qū)動中，需要注意一個細節(jié)，就是電機在轉(zhuǎn)動過程中有可能會產(chǎn)生反向的電動勢，使電流一瞬間過大，導(dǎo)致單片機復(fù)位甚至有可能燒壞芯片。因此在設(shè)計過程中，可以考慮在單片機PWM輸入到電機驅(qū)動接口的地方添加光耦器件或者二極管隔離。如圖5所示，驅(qū)動板選擇了TLP521-4光耦器件設(shè)計隔離電路，減小了電壓的干擾，簡化電路的設(shè)計，同時也把4路PWM的I/O電平從3.3 V上拉到5 V。

3 軟件設(shè)計及具體實現(xiàn)

在軟件程序設(shè)計上，主要應(yīng)用9自由度慣性導(dǎo)航傳感器（ITG3200＋ADXL345＋HMC5883L），結(jié)合DSP和卡爾曼濾波算法，實現(xiàn)較高精度的輪式小車導(dǎo)航和定位。

3.1 傳感器程序設(shè)計

圖5 TLP521-4隔離電路

9自由度慣性導(dǎo)航傳感器在許多領(lǐng)域都得到了實際應(yīng)用，如無人機、救災(zāi)機器人等。它包括ITG3200三軸陀螺儀、ADXL345三軸加速度傳感器和HMC5883L三軸磁感應(yīng)傳感器，可以得到角速度、加速度以及角度的實時數(shù)值。

ITG3200是MEMS三軸陀螺儀，可以測量小車的旋轉(zhuǎn)角速度，同時也可以通過積分把角速度轉(zhuǎn)換為小車的傾角。ITG3200的初始化程序略——編者注。

其具體功能實現(xiàn)可以在主程序中通過SCI讀取其值。所讀取的值為角速度，不會受到小車運動的影響，因此信號噪聲很小，同時可以由它積分得到小車傾斜角度，從而平滑信號使其更加穩(wěn)定。

由于裝置在不同的室內(nèi)區(qū)域進行勘測搜索，再加上未知的環(huán)境，所以角速度信號可能存在一定的偏差，導(dǎo)致積分后的角度出現(xiàn)大的誤差，無法得到實際的數(shù)值。為了消除由于偏差而產(chǎn)生的累積誤差，裝置上加上ADXL345三軸加速度傳感器對于獲得的角度信息進行校正。ADXL345初始化程序略——編者注。

通過ADXL345所得到的角度，與陀螺儀積分后的角度進行對比，然后使用它們的偏差改變陀螺儀的輸出，從而使積分后的角度慢慢校正到實際值，如圖6所示。

HMC5883L三軸磁感應(yīng)傳感器的作用相當于羅盤，在水平情況下，無需借助其他傳感器便可以計算出航向。其初始化程序略——編者注。

由于裝置是要在不同環(huán)境下進行工作的，所以其并不能保持時刻是水平的，就需要加速度傳感器來糾正由于傾斜引起的誤差。

圖6 通過加速度傳感器校正角度

3.2 卡爾曼濾波算法應(yīng)用

由于裝置在室內(nèi)區(qū)域進行勘測搜索，它的運動變化快，軌跡不定，而且要適用于不同的環(huán)境，因此常用的卡爾曼濾波算法需要進一步改進才能應(yīng)用。卡爾曼過濾算法是用前一個估計值和最近一個觀察數(shù)據(jù)，來估計信號的當前值，它是用狀態(tài)方程和遞推的方法進行估計，解也是以估計值形式給出的。其運用在加速度器和陀螺儀上的卡爾曼濾波程序略——編者注。

通過濾波時數(shù)據(jù)平滑將加速度輸出電壓附近產(chǎn)生的波動噪聲濾掉。

4 實驗測試與結(jié)果分析

在裝置硬件和軟件搭建完成后，通過設(shè)計好的PC端軟件和平板電腦安卓軟件，搜索到 WiFi，自動連入局域網(wǎng)。在實驗室測試環(huán)境下，小車可以自由行駛并反饋實時的數(shù)據(jù)進行測試比較。

4.1 航向角數(shù)據(jù)測試

HMC5883L采集到的航向角存在隨機波動性，因此可以對其進行多次采集平均值濾波，以提高系統(tǒng)輸出的數(shù)據(jù)穩(wěn)定性。圖7為一定時間內(nèi)濾波前和濾波后航向角的數(shù)據(jù)。

圖7 航行角數(shù)據(jù)濾波前和濾波后對比圖

由圖7的數(shù)據(jù)對比可以得知，航向所采集到的數(shù)據(jù)進行平均值采樣濾波優(yōu)化后誤差大大減小，比原有數(shù)據(jù)穩(wěn)定許多，說明濾波效果明顯。

4.2 角度數(shù)據(jù)測試

可以使用ADXL345和ITG3200所得的數(shù)據(jù)，轉(zhuǎn)換為橫滾角和俯仰角，再結(jié)合卡爾曼算法對數(shù)據(jù)進行濾波以達到更準確的實驗結(jié)果。表1為濾波前角度的數(shù)據(jù)及誤差，表2為濾波后角度的數(shù)據(jù)及誤差。

表1 濾波前角度的數(shù)據(jù)及誤差

表2 濾波后角度的數(shù)據(jù)及誤差

由表1和表2的測試數(shù)據(jù)可知，經(jīng)過濾波后，最大的誤差從3.80°降到-0.40°，而且大體上都在1°內(nèi)，大大提高了角度的輸出精度。

結(jié) 語

本文所設(shè)計的輪式小車室內(nèi)慣性導(dǎo)航裝置實驗結(jié)果表明，能夠?qū)崟r監(jiān)測到移動機器人的位置信息，并能對其實現(xiàn)有效控制。同時因其低成本、高精度、易操作的特點將進一步應(yīng)用于例如巡邏機器人、救援機器人等專業(yè)領(lǐng)域，有著良好的創(chuàng)業(yè)前景、應(yīng)用前景和市場前景。

編者注：本文為期刊縮略版，全文見本刊網(wǎng)站www.mesnet.com.cn。

[1]陳志崇.機器人慣性定位信息顯示系統(tǒng)設(shè)計[D].廣州：廣東工業(yè)大學(xué)，2013.

[2]劉文國.四輪全向驅(qū)動吸塵機器人的避障實現(xiàn)[J].經(jīng)濟技術(shù)協(xié)息，2009（33）：113.

[3]曾慶華，張為華.IMEMS速率陀螺芯片在MAV飛行控制系統(tǒng)中應(yīng)用研究[J].測控技術(shù)，2004（2）.

[4]周雪峰，江勵，朱海飛.一個模塊化機器人平臺的設(shè)計[J].華南理工大學(xué)學(xué)報：自然科學(xué)版，2011（4）：50-55.

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

[6]劉晶紅，朱志強，沈宏海.加速度在控制系統(tǒng)擾動補償中的應(yīng)用[J].光學(xué)精密工程，2009（9）：2191-2197.

[7]Pasolini F.MEMS加速計、陀螺儀和地磁感應(yīng)計推動創(chuàng)新型消費電子應(yīng)用發(fā)展[J].中國集成電路（CIC），2010（12）：72-75.

[8]劉陵順，高艷麗，張樹團.TMS320F28335 DSP原理及開發(fā)編程[M].北京：北京航空航天大學(xué)出版社，2011：324-349.