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

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

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

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

1 系統(tǒng)體系架構

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

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

圖1 總體架構

2. 硬件計劃及詳細實現(xiàn)

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

2.1 核心板設計

2.1.1 電源電路設計

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

圖2 DSP電源設計

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

2.1.2 JTAG下載口電路設計

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

圖3 JTAG電路設計

2.2 小車驅動板設計

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

圖4 驅動模塊電路設計

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

圖5 TLP521隔離電路

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

在軟件程序設計上，主要應用九自由度慣性導航傳感器(ITG3200+ADXL345+HMC5883L )，結合DSP和卡爾曼濾波算法，能實現(xiàn)較高精度的輪式小車導航和定位。

3.1傳感器器件程序設計

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

ITG3200是MEMS三軸陀螺儀，可以測量小車的旋轉角速度，同時也可以通過積分把角速度轉換為小車的傾角。程序中，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;[!--empirenews.page--]

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;

}

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

由于裝置是要在不同的室內(nèi)區(qū)域進行勘測搜索，再加上未知的環(huán)境，所以角速度信號可能存在一定的偏差，會導致積分后的角度出現(xiàn)大的誤差，無法得到實際的數(shù)值。為了消除這個由于偏差而產(chǎn)生的累積誤差，裝置上加上ADXL345三軸加速度傳感器對于獲得的角度信息進行校正。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;

}

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

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

HMC5883L三軸磁感應傳感器的作用相當于羅盤，在水平情況下，無需借助其他傳感器便可以計算出航向。其初始化如下：

unsigned char Init_HMC5883(void)

{

unsigned char Return1;

unsigned char Data;

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

Data = 0x00;

Return1 = IIC_WriteData(0x3C， 0x02， &Data， 1);

if(Return1)

return 1;

else

return 0;

}

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

3.2 卡爾曼濾波算法應用

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

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

//float incAngle：加速度器測得的角度值

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

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

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

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

float kalmanUpdate(const float gyro_m，const float incAngle

{

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

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

float Y0;

float Y1;

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

float Pdot[4];//過程協(xié)方差矩陣的微分矩陣

float angle_err;//角度偏量

float E;//計算的過程量

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

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

static float n[2][2] = {{ 1， 0 }， { 0， 1 }};//過程協(xié)方差矩陣

Rate = gyro_m - q_bias;

//計算過程協(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; //計算協(xié)方差矩陣

n[0][1] += Pdot[1] * dt;

n[1][0] += Pdot[2] * dt;

n[1][1] += Pdot[3] * dt;

angle_err = incAngle - angle; //計算角度偏差

E = R_angle + P[0][0];

K0 = n[0][0] / E; //計算卡爾曼增益

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)估計值

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

return angle;

｝

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

4 實驗測試與結果分析

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

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

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

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

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

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

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

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

5 結束語

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