使機(jī)器人運(yùn)動(dòng)檢測(cè)更為精確的雙軸微機(jī)械陀螺儀傳感器

引言

在未知環(huán)境中，路況具有復(fù)雜性及未知性特點(diǎn)。移動(dòng)機(jī)器人準(zhǔn)確的路況探測(cè)及其自身的平穩(wěn)運(yùn)行，對(duì)機(jī)載設(shè)備與探測(cè)任務(wù)本身都非常重要。目前，對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)控制研究多集中在前向通道的算法研究，有關(guān)狀態(tài)信息檢測(cè)、傳輸反饋環(huán)節(jié)的研究相對(duì)較少。

雙軸微機(jī)械陀螺儀傳感器可以測(cè)量機(jī)器人的俯仰與翻轉(zhuǎn)，但微機(jī)械陀螺儀有隨機(jī)漂移性，無(wú)法直接應(yīng)用，需要對(duì)輸出值作算法處理。參考文獻(xiàn)中的微機(jī)械陀螺儀隨機(jī)漂移的算法，可以在一定程度上解決漂移方面的問(wèn)題，但仍有改進(jìn)的空間，濾波性能有待進(jìn)一步優(yōu)化提高。在現(xiàn)有自適應(yīng)UKF算法的基礎(chǔ)上，改變比例對(duì)稱采樣策略的相關(guān)參數(shù)可達(dá)到較好的濾波效果?，F(xiàn)有的以ATmegal6為微處理器的CAN總線程序還有一些不完備之處，本文采用PeliCAN模式加入了完備的錯(cuò)誤分析程序，并支持系統(tǒng)自身測(cè)試功能。

1 系統(tǒng)硬件設(shè)計(jì)

移動(dòng)機(jī)器人運(yùn)動(dòng)檢測(cè)系統(tǒng)原理如圖1所示。采用InvenSense公司生產(chǎn)的IDG300型雙軸微機(jī)械陀螺儀進(jìn)行移動(dòng)機(jī)器人運(yùn)動(dòng)過(guò)程的俯仰與翻轉(zhuǎn)角度檢測(cè)；A／D采樣電路的主芯片為8位A／D轉(zhuǎn)換器ADC0809。采集的數(shù)據(jù)經(jīng)改進(jìn)的自適應(yīng)UKF算法濾波后，經(jīng)CAN節(jié)點(diǎn)向運(yùn)動(dòng)控制平臺(tái)傳輸機(jī)器人的運(yùn)動(dòng)狀態(tài)數(shù)據(jù)。以ATmegal6為微處理器，SJAl000為CAN協(xié)議控制器，PCA82C250為CAN收發(fā)器的CAN節(jié)點(diǎn)原理如圖2所示。

2 系統(tǒng)軟件設(shè)計(jì)

2．1 改進(jìn)自適應(yīng)UKF濾波算法

改進(jìn)的自適應(yīng)UKF算法如圖3所示。

經(jīng)典UKF濾波算法采用某種策略得到狀態(tài)估計(jì)sigma點(diǎn)集后，大致分兩個(gè)步驟預(yù)測(cè)與更新，具體方程見(jiàn)參考文獻(xiàn)。不同的采樣策略會(huì)影響sigma點(diǎn)采樣及sigma點(diǎn)個(gè)數(shù)計(jì)算。比例對(duì)稱采樣方法中具體的sigma點(diǎn)采樣方法為：

式(1)～式(5)中需要確定3個(gè)參數(shù)α、β和κ。有一定的經(jīng)驗(yàn)值，但是微機(jī)械陀螺儀不同時(shí)間測(cè)量輸出差別較大，固定的值不能滿足角度測(cè)量的需要。在陀螺儀輸出值有明顯差別時(shí)，需要采用最小二乘支持向量回歸機(jī)(LSSVR)對(duì)α、β和κ值進(jìn)行回歸，支持向量機(jī)對(duì)小樣本的支持，因此可以做出下一步較準(zhǔn)確的預(yù)測(cè)。結(jié)合參考文獻(xiàn)中的算法原理，算法步驟如下：

①輸入狀態(tài)初始條件的值(；β=2，κ=O或3-n，n取1～3)。

②卡爾曼濾波估計(jì)的參數(shù)輸入RBF神經(jīng)網(wǎng)絡(luò)，利用神經(jīng)網(wǎng)絡(luò)的自適應(yīng)能力和函數(shù)逼近能力對(duì)這些特征量進(jìn)行訓(xùn)練調(diào)整。

③更新方程。若輸出角度輸出較準(zhǔn)確，則數(shù)據(jù)發(fā)往CAN總線，程序轉(zhuǎn)至第②步。

④若輸出角度有偏差，則LSSVR算法調(diào)整參數(shù)α、β、κ的值，程序轉(zhuǎn)至第②步。

2．2 CAN節(jié)點(diǎn)程序設(shè)計(jì)

以ATmegal6作CAN節(jié)點(diǎn)的微處理器需要解決端口(ALE，RD／E，／WR)的模擬問(wèn)題。CAN節(jié)點(diǎn)部分主從機(jī)，節(jié)點(diǎn)ID地址決定是否接收任務(wù)以及發(fā)送的順序；幀格式采用數(shù)據(jù)幀；幀類型采用擴(kuò)展幀；PB0～PB7作地址數(shù)據(jù)總線；PC3輸出片選信號(hào)；PC4控制SJAl000的復(fù)位引腳；INT0(PD2)控制終端信號(hào)。錯(cuò)誤檢測(cè)分析程序包括：仲裁丟失捕捉(ALE)、錯(cuò)誤代碼捕捉(ECC)、錯(cuò)誤警告限制(EWLR)、RX錯(cuò)誤計(jì)數(shù)(RXERR)、TX錯(cuò)誤計(jì)數(shù)(TXERR)寄存器配置。

程序流程如下：

①SJAl000寄存器地址配置；

②CAN初始化、進(jìn)入復(fù)位狀態(tài)配置時(shí)鐘分頻、代碼驗(yàn)收、驗(yàn)收屏蔽寄存器。

③等待總線數(shù)據(jù)，判別報(bào)文是否有效：若有效，進(jìn)入收發(fā)函數(shù)，發(fā)送完畢轉(zhuǎn)至第③步；若報(bào)文無(wú)效，則進(jìn)行錯(cuò)誤處理，程序轉(zhuǎn)至下一步；

④錯(cuò)誤分析，丟掉錯(cuò)誤報(bào)文。程序轉(zhuǎn)至第③步。

實(shí)驗(yàn)條件：陀螺儀測(cè)試系統(tǒng)一套(包括360個(gè)脈沖／圈的編碼器、IDG-300軟硬件系統(tǒng))，使用Matlab仿真平臺(tái)。測(cè)試系統(tǒng)可以在±50°擺動(dòng)運(yùn)動(dòng)，陀螺儀的偏轉(zhuǎn)角度通過(guò)編碼器輸出，精度為1°。

2．3 陀螺儀數(shù)據(jù)處理

所有數(shù)據(jù)通過(guò)測(cè)試系統(tǒng)實(shí)驗(yàn)獲得。篇幅有限，下面給出X軸輸出數(shù)據(jù)采用改進(jìn)自適應(yīng)UKF算法后的濾波效果，如圖4～圖6所示。從圖中可以看出，該方法明顯縮成了數(shù)據(jù)波動(dòng)范圍。

2．4CAN總線數(shù)據(jù)傳輸

對(duì)CAN總線數(shù)據(jù)傳輸穩(wěn)定性做了相應(yīng)實(shí)驗(yàn)。取數(shù)量相同的數(shù)據(jù)，分別通過(guò)總線和串口進(jìn)行傳輸，信號(hào)線長(zhǎng)度都是2 m，分別進(jìn)行相同數(shù)據(jù)量的1 h和24 h數(shù)據(jù)傳輸實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果如表1所列。

CAN總線通信方式是指在兩個(gè)節(jié)點(diǎn)間通信；串口通信是指使用ATmegal6芯片的RXD、TXD引腳功能進(jìn)行數(shù)據(jù)傳輸。正確率的計(jì)算方法為：通信正確率=正確接收數(shù)量個(gè)數(shù)／發(fā)送數(shù)據(jù)總個(gè)數(shù)。

結(jié)語(yǔ)

通過(guò)改進(jìn)自適應(yīng)UKF濾波算法處理新型雙軸微機(jī)械陀螺儀數(shù)據(jù)，使微機(jī)械陀螺儀輸出數(shù)據(jù)相對(duì)穩(wěn)定，一定程度上降低了微機(jī)械陀螺儀隨機(jī)漂移。數(shù)據(jù)傳輸采用CAN總線通信的方式，提高了數(shù)據(jù)傳輸?shù)恼_率與穩(wěn)定性。

但該系統(tǒng)尚不完善，特別是處理微陀螺儀隨機(jī)漂移問(wèn)題的方法有待進(jìn)一步改進(jìn)，以提高微機(jī)械陀螺儀的精度，使機(jī)器人運(yùn)動(dòng)檢測(cè)更精確。