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

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

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

　　綜上所述，本文將選用低成本的MEMS器件，結(jié)合DSP和卡爾曼濾波算法，能實(shí)現(xiàn)較高精度的輪式小車(chē)導(dǎo)航和定位。

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

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

　　搭建如下圖1所示的系統(tǒng)，TI公司的浮點(diǎn)DSP TMS320F28335芯片作為主數(shù)字信號(hào)處理器，采集各MEMS慣性傳感器的信號(hào)并處理，處理結(jié)果通過(guò)WIFI將數(shù)據(jù)輸送到PC終端；PC終端負(fù)責(zé)顯示定位結(jié)果和地圖顯示，并向小車(chē)驅(qū)動(dòng)系統(tǒng)發(fā)送控制命令，同時(shí)接收驅(qū)動(dòng)系統(tǒng)反饋的里程計(jì)信息。

　　圖1 總體架構(gòu)

　　2. 硬件計(jì)劃及詳細(xì)實(shí)現(xiàn)

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

　　2.1 核心板設(shè)計(jì)

　　2.1.1 電源電路設(shè)計(jì)

　　TMS320F28335 工作時(shí)需要的電壓不同：內(nèi)核電壓（1．9 V）與I／O供電電壓（3．3 V），對(duì)于電源比較敏感，所以電源部分利用兩路輸出電源器件TPS767D318來(lái)實(shí)現(xiàn)，如圖2所示。同時(shí)根據(jù)仿真實(shí)驗(yàn)和實(shí)際焊接電路的測(cè)試，電源模塊輸出端最好使用一些容值不小于10uf的保護(hù)電容，且不能使用貼片電容，否則工作不穩(wěn)定。

　　圖2 DSP電源設(shè)計(jì)

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

　　2.1.2 JTAG下載口電路設(shè)計(jì)

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

　　圖3 JTAG電路設(shè)計(jì)

　　2.2 小車(chē)驅(qū)動(dòng)板設(shè)計(jì)

　　在本裝置中，我們采用BTS7960作為直流電機(jī)驅(qū)動(dòng)芯片。BTS7960是集成的大電流半橋驅(qū)動(dòng)，其內(nèi)部包含了一片NMOS、一片PMOS和一片半橋門(mén)集驅(qū)動(dòng)，在IOUT = 9 A，VS= 13.5V，Tj = 25 °C 時(shí)，其內(nèi)阻抗為17mΩ。裝置采用了兩個(gè)直流大電機(jī)，如圖4所示為驅(qū)動(dòng)單個(gè)電機(jī)前后轉(zhuǎn)向的電路圖。

　　圖4 驅(qū)動(dòng)模塊電路設(shè)計(jì)

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

　　圖5 TLP521隔離電路

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

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

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

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

　　ITG3200是MEMS三軸陀螺儀，可以測(cè)量小車(chē)的旋轉(zhuǎn)角速度，同時(shí)也可以通過(guò)積分把角速度轉(zhuǎn)換為小車(chē)的傾角。程序中，ITG3200的初始化如下：

　　unsigned char Init_ITG3200（void）

　　{

　　unsigned char Return1，Return2，Return3，Return4;

　　unsigned char Data;

　　Data = 0x00;

　　Return1 = IIC_WriteData（0xD0， 0x3E， &Data， 1）;

　　Data = 0x07;

　　Return2 = IIC_WriteData（0xD0， 0x15， &Data， 1）;

　　Data = 0x1E;

　　Return3 = IIC_WriteData（0xD0， 0x16， &Data， 1）;

　　Data = 0x00;

　　Return4 = IIC_WriteData（0xD0， 0x17， &Data， 1）;

　　if（Return1 && Return2 && Return3 && Return4）

　　return 1;

　　else

　　return 0;

　　}

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

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

　　unsigned char Init_ADXL345（void）

　　{

　　unsigned char Return1，Return2，Return3，Return4;

　　unsigned char Data;

　　Data = 0x0b;

　　byReturn1 = IIC_WriteData（0xA6， 0x31， &Data， 1）;

　　Data = 0x08;

　　Return2 = IIC_WriteData（0xA6， 0x2c， &Data， 1）;

　　Data = 0x08;

　　Return3 = IIC_WriteData（0xA6， 0x2d， &Data， 1）;

　　Data = 0x80;

　　Return4 = IIC_WriteData（0xA6， 0x2e， &Data， 1）;

　　Data = 0x00;

　　Return4 = IIC_WriteData（0xA6， 0x1e， &Data， 1）;

　　Data = 0x00;

　　Return4 = IIC_WriteData（0xA6， 0x1f， &Data， 1）;

　　Data = 0x05;

　　Return4 = IIC_WriteData（0xA6， 0x20， &Data， 1）;

　　if（Return1&&Return2&&Return3&&Return4）

　　return 1;

　　else

　　return 0;

　　}

　　通過(guò)ADXL345所得到的角度，和陀螺儀積分后的角度進(jìn)行對(duì)比，然后使用它們的偏差改變陀螺儀的輸出，從而積分后的角度慢慢校正到實(shí)際的角度，如圖5所示。

　　圖5 通過(guò)加速度傳感器校正角度

　　HMC5883L三軸磁感應(yīng)傳感器的作用相當(dāng)于羅盤(pán)，在水平情況下，無(wú)需借助其他傳感器便可以計(jì)算出航向。其初始化如下：

　　unsigned char Init_HMC5883（void）

　　{

　　unsigned char Return1;

　　unsigned char Data;

　　// Bit4 Bit3 等于11時(shí)，選擇2000度/秒的量程

　　Data = 0x00;

　　Return1 = IIC_WriteData（0x3C， 0x02， &Data， 1）;

　　if（Return1）

　　return 1;

　　else

　　return 0;

　　}

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

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

　　于是裝置在室內(nèi)區(qū)域進(jìn)行勘測(cè)搜索，小車(chē)的運(yùn)行特點(diǎn)與一般的飛機(jī)、船、車(chē)不同，它的運(yùn)動(dòng)變化快，軌跡不定，而且要適用于不同的環(huán)境下工作，因此常用的卡爾曼濾波算法需要進(jìn)一步改進(jìn)才能應(yīng)用?？柭^(guò)濾是用前一個(gè)估計(jì)值和最近一個(gè)觀(guān)察數(shù)據(jù)，來(lái)估計(jì)信號(hào)的當(dāng)前值，它是用狀態(tài)方程和遞推的方法進(jìn)行估計(jì)的，它的解是以估計(jì)值形式給出的。其運(yùn)用在加速度器和陀螺儀上的卡爾曼濾波程序如下：

　　// float gyro_m：陀螺儀測(cè)得的量（角速度）

　　//float incAngle：加速度器測(cè)得的角度值

　　#define dt 0.0015//卡爾曼濾波采樣頻率

　　#define R_angle 0.71 //測(cè)量噪聲的協(xié)方差（即是測(cè)量偏差）

　　#define Q_angle 0.0001//過(guò)程噪聲的協(xié)方差

　　#define Q_gyro 0.0003 //過(guò)程噪聲的協(xié)方差 過(guò)程噪聲協(xié)方差為一個(gè)一行兩列矩陣

　　float kalmanUpdate（const float gyro_m，const float incAngle

　　{

　　float K0;//含有卡爾曼增益的另外一個(gè)函數(shù)，用于計(jì)算最優(yōu)估計(jì)值

　　float K1;//含有卡爾曼增益的函數(shù)，用于計(jì)算最優(yōu)估計(jì)值的偏差

　　float Y0;

　　float Y1;

　　float Rate;//去除偏差后的角速度

　　float Pdot［4］;//過(guò)程協(xié)方差矩陣的微分矩陣

　　float angle_err;//角度偏量

　　float E;//計(jì)算的過(guò)程量

　　static float angle = 0; //下時(shí)刻最優(yōu)估計(jì)值角度

　　static float q_bias = 0; //陀螺儀的偏差

　　static float n［2］［2］ = {{ 1， 0 }， { 0， 1 }};//過(guò)程協(xié)方差矩陣

　　Rate = gyro_m - q_bias;

　　//計(jì)算過(guò)程協(xié)方差矩陣的微分矩陣

　　Pdot［0］ = Q_angle - P［0］［1］ - P［1］［0］;

　　Pdot［1］ = - n［1］［1］;

　　Pdot［2］ = - n［1］［1］;

　　Pdot［3］ = Q_gyro;

　　angle += Rate * dt; //角速度積分得出角度

　　n［0］［0］ += Pdot［0］ * dt; //計(jì)算協(xié)方差矩陣

　　n［0］［1］ += Pdot［1］ * dt;

　　n［1］［0］ += Pdot［2］ * dt;

　　n［1］［1］ += Pdot［3］ * dt;

　　angle_err = incAngle - angle; //計(jì)算角度偏差

　　E = R_angle + P［0］［0］;

　　K0 = n［0］［0］ / E; //計(jì)算卡爾曼增益

　　K1 = n［1］［0］ / E;

　　Y0 = n［0］［0］;

　　Y1 = n［0］［1］;

　　n［0］［0］ -= K0 * Y0; //跟新協(xié)方差矩陣

　　n［0］［1］ -= K0 * Y1;

　　n［1］［0］ -= K1 * Y0;

　　n［1］［1］ -= K1 * Y1;

　　angle += K0 * angle_err; //給出最優(yōu)估計(jì)值

　　q_bias += K1 * angle_err;//跟新最優(yōu)估計(jì)值偏差

　　return angle;

　?。?/p>

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

　　4 實(shí)驗(yàn)測(cè)試與結(jié)果分析

　　在裝置硬件和軟件搭建完成后，通過(guò)原先設(shè)計(jì)好的PC端軟件和平板安卓軟件，搜索到Wi-Fi，自動(dòng)連入局域網(wǎng)。在實(shí)驗(yàn)室測(cè)試環(huán)境下，小車(chē)可以自由行駛并反饋回實(shí)時(shí)的數(shù)據(jù)并進(jìn)行測(cè)試比較。

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

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

　　圖6 航行角數(shù)據(jù)采集和對(duì)比圖

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

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

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

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

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

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

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

　　本文所設(shè)計(jì)的輪式小車(chē)室內(nèi)慣性導(dǎo)航裝置，分析了該軟件設(shè)計(jì)的各個(gè)模塊的具體實(shí)現(xiàn)方法。經(jīng)實(shí)驗(yàn)結(jié)果表明，該設(shè)計(jì)能夠?qū)崟r(shí)監(jiān)測(cè)到移動(dòng)機(jī)器人的位置信息，并對(duì)其能實(shí)現(xiàn)有效控制。同時(shí)其低成本、高精度、易操作的特點(diǎn)將進(jìn)一步應(yīng)用于例如巡邏機(jī)器人、救援機(jī)器人等專(zhuān)業(yè)領(lǐng)域，必將吸引國(guó)內(nèi)外眾多的投資商對(duì)其投資并進(jìn)行更進(jìn)一步的研發(fā)與應(yīng)用，使其有著十分廣大的創(chuàng)新創(chuàng)業(yè)前景、應(yīng)用前景和市場(chǎng)前景。