基于多傳感器數據融合的機器人里程計設計與實現

余 翀，高 翔，邱其文

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

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

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

1 光電里程計

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

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

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

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

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

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

表3 PS/2數據包格式

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

1．2 FPGA處理單元

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

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

在光電鼠標里程計系統中FPGA-EP2C8Q208C 8N負責解析PS/2協議并讀取PS/2鼠標數據包，然后在芯片內完成六個光電鼠標數據融合操作。系統結構如圖2所示。

圖2 光電里程計系統結構

2 多傳感器數據融合算法

2．1 數據融合背景

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

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

2．2 一致性數據融合算法

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

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

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

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

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

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

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

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

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

2．3 多傳感器數據融合

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

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

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

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

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

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

為正，并滿足下式:

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

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

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

3 實驗研究

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

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

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

圖3 目標真值、算術平均值與多傳感器數據融合結果比較

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

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

4 結論

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

［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，Fukayama 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］ 王忠巍，曹其新，欒楠，等．基于多傳感器數據融合的管道機器人精確定位技術［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］ 薛晗，李迅，馬宏緒．基于無線傳感器網絡的移動機器人智能導航算法［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，France: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．