基于多傳感器數(shù)據(jù)融合的機器人里程計設計與實現(xiàn)

余 翀，高 翔，邱其文

(1．復旦大學信息科學與工程學院，上海200433;2．南京郵電大學自動化學院，南京210046)

在智能空間［1－2］中，機器人依據(jù)指令到達指定地點完成任務，需要一個精確的定位系統(tǒng)作為保證。在機器人定位技術［3］中有兩類常用方法:航距推算法［4］和三角測量法［5］。其中航距推算法是一種直接進行移動機器人定位的方式，它不依賴外部參照，主要靠機器人自身攜帶的里程計完成到出發(fā)點的距離計量，故應用廣泛。常用的里程計有光電編碼器、加速度計等，但它們存在兩個主要問題。一是里程計測量距離的準確程度與車體自身運動情況有關，是一種相對測量結(jié)果;二是受傳感器自身精度、環(huán)境噪聲和人為干擾等因素的影響，單一的里程計結(jié)果具有不確定性。

本文結(jié)合應用場景及實際需求，設計實現(xiàn)了一種基于FPGA和多光電鼠標的高精度機器人里程計，該里程計可以很好的解決以上兩個缺陷。首先利用光電鼠標傳感器自身特性［6］，使得里程計工作不依賴于車體自身運動情況，得到絕對測量結(jié)果。其次在研究多傳感器數(shù)據(jù)融合算法［7］的基礎上，用FPGA硬件實現(xiàn)，將來自多個光電鼠標里程計的信息進行綜合處理［8］，得出比單一里程計更為準確可靠的結(jié)論。本文將詳細介紹該里程計設計方案及實現(xiàn)。

1 光電里程計

1．1 PS/2光電鼠標傳感器

PS/2協(xié)議與設備接口是由IBM公司開發(fā)、使用，并最早出現(xiàn)在IBM技術參考手冊里。物理上，PS/2端口連接器有兩種:一種是5腳的DIN，另一種是6腳的mini-DIN。本文設計實現(xiàn)的機器人里程計所選用光電鼠標PS/2接頭是6腳的mini-DIN規(guī)格，表1給出了六腳標準的引腳定義，表2給出了PS/2協(xié)議通訊數(shù)據(jù)幀格式，圖1為通訊時序圖。

表1 6-pin Mini-DIN(PS/2)連接器引腳定義

表2 PS/2設備到主機的通訊過程中數(shù)據(jù)幀中各個位含義

圖1 PS/2設備到主機的通訊時序

標準的PS/2鼠標支持如下的輸入信號:X(左右)方向的位移大小，Y(上下)方向的位移大小，左鍵狀態(tài)，中鍵狀態(tài)和右鍵狀態(tài)。鼠標以一個固定的頻率讀取這些輸入信號，并更新各種計數(shù)器和標志位，以反映出位移大小和按鍵所處狀態(tài)。表3給出了鼠標數(shù)據(jù)包格式。本文應用光電鼠標傳感器做機器人里程計，故只用到其數(shù)據(jù)包中的Byte 2、Byte 3及Byte 1中相應的狀態(tài)位。

表3 PS/2數(shù)據(jù)包格式

本論文所選用光電鼠標，其分辯率達800 DIP，數(shù)據(jù)刷新頻率為100 Hz。所設計的機器人里程計由傳感器支架和六個經(jīng)改造過的光電鼠標傳感器構(gòu)成，六個光電鼠標數(shù)據(jù)同時按PS/2協(xié)議與FPAG通訊，并在FPGA中進行6個光電鼠標的數(shù)據(jù)融合操作，得到融合出的光電鼠標里程計最終結(jié)果。

1．2 FPGA處理單元

在本論文設計的光電鼠標里程計中，F(xiàn)PGA上電初始化光電鼠標，使其進行到默認的Stream模式，而不對鼠標進行控制和操作，所以在里程計工作過程中，F(xiàn)PGA只需被動的讀取來自光電鼠標的數(shù)據(jù)包，并把數(shù)據(jù)包中的X、Y位移量提取出來便完成數(shù)據(jù)通訊，也就是說系統(tǒng)中只涉及PS/2設備到主機的通訊過程。

當PS/2設備上傳數(shù)據(jù)給主機時，它們首先檢查時鐘信號，以確保時鐘信號是高電平狀態(tài)，否則表明主機抑制了數(shù)據(jù)通訊，那么設備須緩沖待上傳數(shù)據(jù)。PS/2鼠標是在檢測到位移量發(fā)生變化或發(fā)現(xiàn)有一個或多個鼠標鍵的狀態(tài)發(fā)生變化時觸發(fā)數(shù)據(jù)包傳送動作。

在光電鼠標里程計系統(tǒng)中FPGA-EP2C8Q208C 8N負責解析PS/2協(xié)議并讀取PS/2鼠標數(shù)據(jù)包，然后在芯片內(nèi)完成六個光電鼠標數(shù)據(jù)融合操作。系統(tǒng)結(jié)構(gòu)如圖2所示。

圖2 光電里程計系統(tǒng)結(jié)構(gòu)

2 多傳感器數(shù)據(jù)融合算法

2．1 數(shù)據(jù)融合背景

實際控制系統(tǒng)為了與周圍環(huán)境發(fā)生交互，必須有相應的系統(tǒng)去檢測周圍環(huán)境狀態(tài)［9］。在機器人的控制系統(tǒng)中，檢測系統(tǒng)還擔負著機器人在環(huán)境中定位的任務，所以檢測系統(tǒng)成為控制系統(tǒng)與周圍環(huán)境交互溝通的橋梁。為了使檢測到的數(shù)據(jù)更精確，更有參考價值，我們需要用多種傳感器來構(gòu)成檢測系統(tǒng)［10］。

在實際環(huán)境中，多傳感器檢測系統(tǒng)受到各種傳感器自身的精度、環(huán)境噪聲和人為干擾等因素的影響，使得系統(tǒng)測得的數(shù)據(jù)具有不確定性，相當于在真值上疊加了隨機噪聲。我們希望通過一定的算法和硬件電路的實現(xiàn)，將各傳感器實際測得的不確定數(shù)據(jù)綜合處理，完成多傳感器的數(shù)據(jù)融合，得出比單獨一個傳感器或是單獨一類傳感器更為準確、確定、可靠的信息和結(jié)論。

2．2 一致性數(shù)據(jù)融合算法

由n個傳感器組成的檢測系統(tǒng)，各傳感器從不同位置相互獨立地對某一目標參數(shù)μ進行測量，設第i個傳感器測得的數(shù)據(jù)為xi，它服從正態(tài)分布。其中是對該傳感器測量值偏離真值μ的一種度量，σi表示第i個傳感器的測量精度，則傳感器測量模型可用高斯概率密度函數(shù)表示為:

為了檢驗傳感器測量的一致性，定義置信距離測度 dij和 dji:

當i≠j，且第i個傳感器與第j個傳感器的測量精度不同時，置信距離一般不等于，而通常置信距離的定義要求滿足對稱性，即，這是經(jīng)典的Luo一致性數(shù)據(jù)融合方法［11］一個不足之處。為了克服因為各個傳感器測量精度不同而導致的置信距離的不一致，現(xiàn)對置信距離做如下定義:

根據(jù)物理實際，測量精度低的傳感器所測得的數(shù)據(jù)，其方差σ2較大，而測量精度高的傳感器所測得的數(shù)據(jù)，其方差σ2較小。由式(4)可見，測量精度低的傳感器所測得的數(shù)據(jù)被賦予了較小的權重，而測量精度高的傳感器所測得的數(shù)據(jù)被賦予了較大的權重。因此，通過權重大小的不同來反映和補償因各傳感器測量精度的差異而對置信距離產(chǎn)生的影響，其物理意義明確。

當n個傳感器同時測量某一物體特征時，各傳感器之間的置信距離測度構(gòu)成一個矩陣Dn，稱為置信距離矩陣。

置信距離矩陣描述了各傳感器間的一致性支持程度，根據(jù)該矩陣可以確定一個傳感器對另一個傳感器的支持關系。

在經(jīng)典的Luo一致性數(shù)據(jù)融合方法中，第i個傳感器對第j個傳感器支持程度指標rij一般是由帶有經(jīng)驗性的置信距離閾值εij決定的，具有很大的主觀性，閾值選取不當，就可能會對結(jié)果產(chǎn)生很大的影響。并且rij要么取0，要么取1，當測量數(shù)據(jù)發(fā)生擾動時，可能會加大數(shù)據(jù)融合結(jié)果的誤差［12］。作為改進，在本算法中rij應結(jié)合各個傳感器的相互支持程度情況，取值是模糊而非絕對的。從實際物理意義上來說，置信距離dij越是接近于經(jīng)驗閾值εij，則此距離所涉及的第i個傳感器對第j個傳感器支持與否結(jié)論越為模糊;而只有置信距離dij越遠離經(jīng)驗閾值εij，才能越清晰的說明第i個傳感器對第j個傳感器的支持程度。同時，置信距離dij在閥值εij附近存在模糊，但是隨著置信距離dij增大，這種模糊性將很快消除，所以用二次曲線可以很好地表示這種模糊性。利用橢圓曲線將傳感器測量數(shù)據(jù)之間的相互支持程度大小rij定義為:

在置信距離閾值εij的作用下，由置信距離矩陣Dn，可以得到傳感器之間的關系矩陣(又稱為支持矩陣)Rn

關系矩陣(支持矩陣)Rn可以量化出各個傳感器的測量數(shù)據(jù)被其他傳感器測量數(shù)據(jù)支持的程度。

2．3 多傳感器數(shù)據(jù)融合

在支持矩陣Rn中，rij僅表示2個傳感器的測量值xi和xj之間的相互支持程度，并不能反映第i個傳感器測量值xi，被系統(tǒng)中所有傳感器測量值的綜合支持程度［13］。

設βi表示第i個傳感器測量值xi被所有傳感器測量值的綜合支持程度，βi數(shù)值越大，表明xi被其他測量值支持的程度越高，即在數(shù)據(jù)融合中的重要程度越高。根據(jù)信息分享原理，最優(yōu)融合估計的信息量之和可等效分解成若干信息量之和，或者說一個信息可被若干個子系統(tǒng)所分享，且各信息所具有的權系數(shù)應滿足

根據(jù)概率源合并理論，存在一組非負數(shù)p1，p2，…，pn，使得

將式(9)寫成矩陣形式，則有

式中:β=(β1，β2，…，βn)T，P=(p1，p2，…，pn)T。

支持矩陣Rn是一個非負對稱矩陣，由Perron-Frobenius定理可知，Rn存在最大模特征值λ(λ＞0)，使得該特征值λ對應的特征向量

為正，并滿足下式:

那么λY可以作為測量數(shù)據(jù)的綜合支持程度的度量，則第k個傳感器的綜合支持程度可以通過矩陣展開得到:

比較式(9)和式(12)，可知P即為Y歸一化后的結(jié)果，故令:

則αk為第k個傳感器的綜合支持程度，利用α1，α2，…，αn，可以給出多傳感器的最終數(shù)據(jù)融合值為:

3 實驗研究

由于實際應用場景中6個光電鼠標傳感器兩兩夾角為60°，直接測量所得數(shù)據(jù)沒有相互可比性，所以首先在直角坐標系中對每個光電鼠標測得的位移數(shù)據(jù)做預處理。取10組處理后的典型數(shù)據(jù)(其中有1個光電鼠標傳感器測量值有較大誤差)來測試傳感器數(shù)據(jù)融合算法的效果，實驗數(shù)據(jù)如表4所示。

表4 6個光電鼠標傳感器預處理后的測量值和數(shù)據(jù)融合結(jié)果 單位:mm

為了便于比較，采用簡單平均法對6個光電鼠標傳感器測得數(shù)據(jù)進行處理，將10組目標真值、算術平均值和多傳感器數(shù)據(jù)融合結(jié)果用圖3表示。

圖3 目標真值、算術平均值與多傳感器數(shù)據(jù)融合結(jié)果比較

由于光電鼠標傳感器精度高，所以當各光電鼠標傳感器測量準確時，采用本文的多傳感器數(shù)據(jù)融合算法得到的數(shù)據(jù)融合結(jié)果和簡單算術平均數(shù)，都很接近于目標真值。而當某些個光電鼠標傳感器測量含有較大誤差或測量錯誤時，通過分析上圖中三條折線可知:采用本文的多傳感器數(shù)據(jù)融合算法得到的數(shù)據(jù)融合結(jié)果，比簡單算術平均數(shù)更加接近于目標真值，可以大大降低這些異常數(shù)據(jù)的干擾。

根據(jù)常用的數(shù)據(jù)處理方法，在對測量結(jié)果取算術平均值時，應當先剔除有較大誤差的數(shù)據(jù)結(jié)果，即通過設定一個閾值，將和其他數(shù)據(jù)傳感器測量結(jié)果相差較大的結(jié)果去掉。但是作為應用在機器人定位系統(tǒng)上的里程計，其測量結(jié)果依賴于機器人的運動狀態(tài)。因此在機器人運動狀態(tài)快速變化的情況下，無法通過設定閾值的方式來剔除異常數(shù)據(jù)的干擾。此時采用本文的多傳感器數(shù)據(jù)融合算法，在不剔除這些異常數(shù)據(jù)時，數(shù)據(jù)融合結(jié)果仍然能夠達到較高的測量精度要求。

4 結(jié)論

機器人的精確定位是為實現(xiàn)智能空間中機器人提供相應服務的前提，而機器人里程計是實現(xiàn)機器人定位技術的核心。本文在前人研究基礎上，改進了經(jīng)典的Luo一致性數(shù)據(jù)融合算法，設計實現(xiàn)了多傳感器數(shù)據(jù)融合算法。該算法計算步驟固定，方便在FPGA上實現(xiàn)，在此基礎上，利用PS/2光電鼠標傳感器，設計實現(xiàn)了一種高精度機器人里程計，可以應用于各種機器人系統(tǒng)中。

［1］ Reetz Eike S，Tonjes Ralf，Baker Nigel．Towards Global Smart Spaces:Merge Wireless Sensor Networks into Context-Aware Systems［C］//5th IEEE International Symposium on Wireless Pervasive Computing(ISWPC)，Modena，Italy:IEEE，2010:337－342．

［2］ Blum N，Yamada J，F(xiàn)ukayama A，et al．A Smart Information Sharing Architecture in a Multi-Access Network，Multi-Service Environment［C］//International Conference on Communications(ICC)，Kyoto，Japan:IEEE，2011:1－6．

［3］ Ruiz D，Urena J，Garcia J C，et al．Sim-Ultaneous Mobile Robot Positioning and LPS Self-Calibration in a Smart Space［C］//International Symposium on Industrial Electronics(ISIE)，Henares，Spain:IEEE，2010:2865－2870．

［4］ 白亮，秦永元，成研．基于序貫處理的航位推算組合導航算法研究［J］．傳感技術學報，2010，23(5):687－690．

［5］ 王忠巍，曹其新，欒楠，等．基于多傳感器數(shù)據(jù)融合的管道機器人精確定位技術［J］．機器人，2008，30(3):238－241．

［6］ Kim S，Lee S．Roubust Mobile Robot Velocity Estimation Using Redundant Number of Optical Mice［C］//Information and Automation，2008．ICIA 2008．International Conference on，June 2008，107－112．

［7］ Xu Shu-sheng，Lin Xiao-gong．Asynchronous Multi-Sensor Hierarchical Adaptive Data Fusion Algorithm［C］//International Conference on Complex Medical Engineering(CME)，Harbin，China:IEEE/ICME，2011:285－288．

［8］ 萬樹平．運用距離測度進行多傳感器目標級信息融合［J］．計算機工程與與應用，2010，46(18):128－129．

［9］ 薛晗，李迅，馬宏緒．基于無線傳感器網(wǎng)絡的移動機器人智能導航算法［J］．傳感技術學報，2008，21(5):834－840．

［10］ Kermorgant Olivier，Chaumette Francois．Multi-Sensor Data Fusion in Sensor-Based Control:Application to Multi-Camera Visual Servoing［C］//International Conference on Robotics and Automation(ICRA)，Beaulieu，F(xiàn)rance:IEEE，2011:4518－4523．

［11］ Lou R C，Lin M，Scherp P S．Dynamic Multi-Sensor Data Fusion Systemfor Intelligent Robots［J］．IEEE Journal of Robotics and Automation，1988，4(4):386－396．

［12］ Jin Tae-Seok，Lee Jang Myung，Tso S K．A New Approach Using Sensor Data Fusion for Mobile Robot Navigation［J］．Robotica，2004，22:52－59．

［13］ George，Jemin，Kaplan，et al．Multi-Sensor Data Fusion:An Unscented Least Squares Approach［C］//Proceedings of the 14th International Conference on Information Fusion(FUSION)，Chicago，IL，USA:IEEE，2011:1－8．