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