基于DSP的室內(nèi)慣性導(dǎo)航系統(tǒng)設(shè)計
輪式遙控機器人已經(jīng)應(yīng)用在地震、火災(zāi)等一些危險的室內(nèi)區(qū)域進(jìn)行救援和探測,或執(zhí)行反恐任務(wù)。由于在這些特殊的環(huán)境下存在諸多的未知因素,且室內(nèi)無GPS信號,人們不得不依靠先進(jìn)的科學(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ù)的進(jìn)步得到了迅速發(fā)展,使其低成本而高精度的期望得到了實現(xiàn)。MEMS慣性傳感器組成的慣性導(dǎo)航裝置結(jié)合輪式小車的里程計,能夠產(chǎn)生導(dǎo)航和定位信息,減少對外部環(huán)境的倚賴,實現(xiàn)在外部環(huán)境條件(例如光照、墻壁材質(zhì))未知情況下的導(dǎo)航。
由于是在室內(nèi)區(qū)域進(jìn)行勘測搜索,小車的運行特點與一般的飛機、船、車不同,它的運動軌跡變化較快,且在運動時存在一定的振動,因此常用的卡爾曼濾波算法需要進(jìn)一步改進(jìn)才能應(yīng)用。慣性傳感器采集數(shù)據(jù)量大,且進(jìn)行慣性導(dǎo)航時需要大量的浮點運算,因此本項目采用了具有強大數(shù)字信號處理功能的DSP 28335芯片和PC控制終端,實現(xiàn)慣性傳感器的數(shù)據(jù)采集、時序邏輯控制、與驅(qū)動系統(tǒng)通信和地圖顯示功能, 具有體積小、成本低、功耗低等優(yōu)點。
綜上所述,本文將選用低成本的MEMS器件,結(jié)合DSP和卡爾曼濾波算法,能實現(xiàn)較高精度的輪式小車導(dǎo)航和定位。
1 系統(tǒng)體系架構(gòu)
本文的目標(biāo)是研制一個輪式小車慣性導(dǎo)航系統(tǒng),能夠通過wifi實現(xiàn)PC終端和手持終端控制輪式小車行動以及小車所采集數(shù)據(jù)的傳輸。
TI公司的浮點DSP TMS320F28335芯片作為主數(shù)字信號處理器,采集各MEMS慣性傳感器的信號并處理,處理結(jié)果通過WIFI將數(shù)據(jù)輸送到PC終端;PC終端負(fù)責(zé)顯示定位結(jié)果和地圖顯示,并向小車驅(qū)動系統(tǒng)發(fā)送控制命令,同時接收驅(qū)動系統(tǒng)反饋的里程計信息。
2. 硬件計劃及詳細(xì)實現(xiàn)
硬件設(shè)計上,主要分為核心板和驅(qū)動板。核心板包括DSP最小系統(tǒng),JTAG下載口設(shè)計,系統(tǒng)電源供給電路和MEMS傳感器,WIFI模塊等。而驅(qū)動板主要設(shè)計的內(nèi)容是直流大電機的驅(qū)動模塊。
2.1 核心板設(shè)計
2.1.1 電源電路設(shè)計
TMS320F28335工作時需要的電壓不同:內(nèi)核電壓(1.9 V)與I/O供電電壓(3.3 V),對于電源比較敏感,所以電源部分利用兩路輸出電源器件TPS767D318來實現(xiàn)。同時根據(jù)仿真實驗和實際焊接電路的測試,電源模塊輸出端最好使用一些容值不小于10uf的保護(hù)電容,且不能使用貼片電容,否則工作不穩(wěn)定。
在電源設(shè)計中,考慮到TPS767D318芯片可以產(chǎn)生復(fù)位信號,所以在核心板上并沒有再另外為DSP設(shè)計復(fù)位電路。
2.1.2 JTAG下載口電路設(shè)計
按照仿真器的通信引腳選擇14腳的仿真接口,同時要注意EMU0和EMUl信號必須通過上拉電阻連接至電源,其中上拉電阻為10kΩ。
2.2 小車驅(qū)動板設(shè)計
在本裝置中,我們采用BTS7960作為直流電機驅(qū)動芯片。BTS7960是集成的大電流半橋驅(qū)動,其內(nèi)部包含了一片NMOS、一片PMOS和一片半橋門集驅(qū)動,在IOUT = 9 A,VS= 13.5V,Tj = 25 °C 時,其內(nèi)阻抗為17mΩ。裝置采用了兩個直流大電機。
在電機驅(qū)動這里,需要注意一個細(xì)節(jié),就是電機在轉(zhuǎn)動過程中有可能會產(chǎn)生反向的電動勢,使電流一瞬間過大,導(dǎo)致單片機復(fù)位甚至有可能燒壞芯片。因此在設(shè)計過程中,可以考慮在單片機PWM輸入到電機驅(qū)動接口的地方添加光耦器件隔離或者二極管。驅(qū)動板選擇了tlp521-4這種光耦器件設(shè)計隔離電路,減小電壓的干擾,減化電路的設(shè)計,同時也把四路PWM的I/O電平從3.3上拉到5V。
3.軟件設(shè)計及具體實現(xiàn)
在軟件程序設(shè)計上,主要應(yīng)用九自由度慣性導(dǎo)航傳感器(ITG3200+ADXL345+HMC5883L ),結(jié)合DSP和卡爾曼濾波算法,能實現(xiàn)較高精度的輪式小車導(dǎo)航和定位。
3.1傳感器器件程序設(shè)計
九自由度慣性導(dǎo)航傳感器在許多領(lǐng)域都得到了實際的應(yīng)用,如無人機,救災(zāi)機器人等。它包括ITG3200三軸陀螺儀,HMC5883L三軸磁感應(yīng)傳感器和ADXL345三軸加速度傳感器,所以可以得到加速度,角速度以及角度實時的數(shù)值。
ITG3200是MEMS三軸陀螺儀,可以測量小車的旋轉(zhuǎn)角速度,同時也可以通過積分把角速度轉(zhuǎn)換為小車的傾角。程序中,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;
}
其具體功能實現(xiàn)可以在主程序中通過SCI讀取其值。所讀取的值為角速度,不會受到小車運動的影響,因此該信號噪聲很小,同時可以由它積分得到小車傾斜角度,可以平滑信號使其更加穩(wěn)定。
由于裝置是要在不同的室內(nèi)區(qū)域進(jìn)行勘測搜索,再加上未知的環(huán)境,所以角速度信號可能存在一定的偏差,會導(dǎo)致積分后的角度出現(xiàn)大的誤差,無法得到實際的數(shù)值。為了消除這個由于偏差而產(chǎn)生的累積誤差,裝置上加上ADXL345三軸加速度傳感器對于獲得的角度信息進(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;
}
通過ADXL345所得到的角度,和陀螺儀積分后的角度進(jìn)行對比,然后使用它們的偏差改變陀螺儀的輸出,從而積分后的角度慢慢校正到實際的角度,HMC5883L三軸磁感應(yīng)傳感器的作用相當(dāng)于羅盤,在水平情況下,無需借助其他傳感器便可以計算出航向。其初始化如下:
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)境下進(jìn)行工作的,所以其并不能保持時刻水平,就需要加速度傳感器來糾正由于傾斜引起的誤差。
3.2 卡爾曼濾波算法應(yīng)用
于是裝置在室內(nèi)區(qū)域進(jìn)行勘測搜索,小車的運行特點與一般的飛機、船、車不同,它的運動變化快,軌跡不定,而且要適用于不同的環(huán)境下工作,因此常用的卡爾曼濾波算法需要進(jìn)一步改進(jìn)才能應(yīng)用??柭^濾是用前一個估計值和最近一個觀察數(shù)據(jù),來估計信號的當(dāng)前值,它是用狀態(tài)方程和遞推的方法進(jìn)行估計的,它的解是以估計值形式給出的。其運用在加速度器和陀螺儀上的卡爾曼濾波程序如下:
// 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 實驗測試與結(jié)果分析
在裝置硬件和軟件搭建完成后,通過原先設(shè)計好的PC端軟件和平板安卓軟件,搜索到Wi-Fi,自動連入局域網(wǎng)。在實驗室測試環(huán)境下,小車可以自由行駛并反饋回實時的數(shù)據(jù)并進(jìn)行測試比較。
4.1 航向角數(shù)據(jù)測試
HMC5883L采集到的航向角存在隨機波動性,因此可以對于進(jìn)行多次采集平均值濾波,以提高系統(tǒng)輸出的數(shù)據(jù)穩(wěn)定性。如圖6為一定時間內(nèi)濾波前和濾波后航向角的數(shù)據(jù)。
航向所采集到的數(shù)據(jù)進(jìn)行平均值采樣濾波優(yōu)化后誤差大大減小,比原有數(shù)據(jù)穩(wěn)定許多,說明濾波效果明顯。
4.2 角度數(shù)據(jù)測試
在角度獲得的數(shù)據(jù),可以使用ADXL345和ITG3200所得的數(shù)據(jù),轉(zhuǎn)換為橫滾角和俯仰角。再結(jié)合卡爾曼濾波對數(shù)據(jù)進(jìn)行濾波以達(dá)到更準(zhǔn)確的實驗結(jié)果。
5 結(jié)束語
本文所設(shè)計的輪式小車室內(nèi)慣性導(dǎo)航裝置,分析了該軟件設(shè)計的各個模塊的具體實現(xiàn)方法。經(jīng)實驗結(jié)果表明,該設(shè)計能夠?qū)崟r監(jiān)測到移動機器人的位置信息,并對其能實現(xiàn)有效控制。同時其低成本、高精度、易操作的特點將進(jìn)一步應(yīng)用于例如巡邏機器人、救援機器人等專業(yè)領(lǐng)域,必將吸引國內(nèi)外眾多的投資商對其投資并進(jìn)行更進(jìn)一步的研發(fā)與應(yīng)用,使其有著十分廣大的創(chuàng)新創(chuàng)業(yè)前景、應(yīng)用前景和市場前景。