多傳感器信息融合在四足機(jī)器人避障中的應(yīng)用*

張 天，楊晨曦，朱 穎，宋明輝，張 地

(1.北京理工大學(xué) 光電學(xué)院，北京100081;2.北京理工大學(xué) 信息與電子學(xué)院，北京100081;3.北京理工大學(xué) 機(jī)械與車輛學(xué)院，北京100081;4 鄭州大學(xué) 國際學(xué)院，河南 鄭州450001)

0 引 言

多傳感器信息融合技術(shù)綜合了概率統(tǒng)計、信號處理、人工智能、控制理論等多個學(xué)科的最新科研成果，為機(jī)器人精確、全面、實時地感知各種復(fù)雜的、動態(tài)的、不確定的未知環(huán)境提供了一種先進(jìn)的技術(shù)手段。在研究基于多傳感器融合的移動機(jī)器人避障技術(shù)的相關(guān)文獻(xiàn)中，為獲取機(jī)器人本體與障礙物的距離信息，經(jīng)常使用的傳感器有超聲測距傳感器、紅外測距傳感器、里程計、GPS、激光傳感器等，文獻(xiàn)［1］中使用里程計和激光傳感器進(jìn)行信息融合;文獻(xiàn)［2］中使用激光雷達(dá)和超聲測距傳感器進(jìn)行信息融合;文獻(xiàn)［3］中使用超聲測距傳感器和紅外測距傳感器進(jìn)行信息融合，這些傳感器均為測距類傳感器，傳感器之間的冗余信息量大，互補信息量少［4］，在使用的過程中必須提供先驗知識，對于動態(tài)的、復(fù)雜的應(yīng)用場景，其效果并不理想。文獻(xiàn)［5，6］中對單目視覺傳感器和超聲測距傳感器進(jìn)行了信息融合，傳感器間的互補信息量變大，融合結(jié)果提高了系統(tǒng)的魯棒性，但單目視覺只有在特定的環(huán)境下才能得到距離信息，依然不能滿足動態(tài)的應(yīng)用場景。

雙目視覺傳感器對動態(tài)環(huán)境適應(yīng)性強(qiáng)，超聲測距傳感器的測量精度高，為滿足動態(tài)的應(yīng)用場景，本文將進(jìn)行這兩種傳感器的融合研究，需要指出，由于干擾信號的存在，在進(jìn)行融合之前，先使用卡爾曼濾波算法對兩種傳感器獲取的距離信息進(jìn)行濾波處理。

1 卡爾曼濾波算法與STF 融合算法

由于受雜波等干擾信號的影響，傳感器獲取的距離信息具有統(tǒng)計信號［7］的特征，為保證測量精度，需要進(jìn)行參數(shù)估計，根據(jù)具體的情況，解決參數(shù)估計問題的常用方法有卡爾曼濾波、α－β 濾波、α－β－γ 濾波等?？柭鼮V波算法主要有兩條主線，一條是基于自協(xié)方差矩陣的運算，另一條是基于濾波值和預(yù)測值的運算，兩者通過增益矩陣聯(lián)系起來。卡爾曼濾波算法的公式推導(dǎo)參見文獻(xiàn)［8］，這里不再贅述。

多傳感器信息融合方法大致可以分為三類，即，概率統(tǒng)計方法、邏輯推理方法和學(xué)習(xí)方法［9］。文獻(xiàn)［5，10，11］分別使用模糊推理、D－S 證據(jù)理論和產(chǎn)生式規(guī)則的方法進(jìn)行信息融合，這些方法都屬于邏輯推理的范疇;文獻(xiàn)［3，4，6］使用神經(jīng)網(wǎng)絡(luò)的方法進(jìn)行信息融合，該方法屬于學(xué)習(xí)方法的范疇，文獻(xiàn)中依據(jù)這些融合算法，均達(dá)到了預(yù)期效果。本文中使用的STF 融合算法則是概率統(tǒng)計方法的一種。

假設(shè)仿生四足機(jī)器人上的雙目視覺傳感器和超聲測距傳感器獲取的狀態(tài)向量的估計值分別為和，協(xié)方差矩陣分別為P1和P2，互協(xié)方差矩陣。當(dāng)P12=時，為了得到狀態(tài)向量和協(xié)方差矩陣的最佳估計值，可以使用STF 融合算法。

系統(tǒng)狀態(tài)向量和協(xié)方差矩陣的最佳估計分別為

2 勻速直線運動模型

勻速直線運動(constant velocity，CV)模型［12］。CV 直線運動模型的一般描述為:目標(biāo)做CV 直線運動，位移為x(t)，速度為˙x(t)，加速度a=¨x(t)=0。實際情況中，速度在有隨機(jī)擾動的情況下會發(fā)生輕微變化，假設(shè)這個隨機(jī)擾動是均值為零的高斯白噪聲。在此條件下，經(jīng)離散處理后，卡爾曼濾波的基本公式可表示如下

其中

式中 T 為采樣周期，σw為過程噪聲的標(biāo)準(zhǔn)差，σv為量測噪聲的標(biāo)準(zhǔn)差。

3 參數(shù)確定

基于雙目視覺傳感器和超聲測距傳感器，在CV 模型下應(yīng)用卡爾曼濾波算法，可以得到兩組狀態(tài)向量的估計值和，以及相應(yīng)的協(xié)方差矩陣P1和P2，由于以上兩組數(shù)據(jù)來自兩個不同的傳感器系統(tǒng)，故滿足這一條件，可以使用STF 融合算法得到整個系統(tǒng)的狀態(tài)向量和協(xié)方差矩陣的最佳估計值^X 和P。為此，需要確定以下參數(shù)，系統(tǒng)的過程噪聲標(biāo)準(zhǔn)差σw，雙目視覺傳感器的量測噪聲標(biāo)準(zhǔn)差σv1，超聲測距傳感器的量測噪聲標(biāo)準(zhǔn)差σv2和卡爾曼濾波算法的初始值。下面結(jié)合仿生四足機(jī)器人的實際情況，確定以上參數(shù)。

3.1 確定系統(tǒng)的過程噪聲標(biāo)準(zhǔn)差

由于在機(jī)械結(jié)構(gòu)設(shè)計和裝配過程中產(chǎn)生的誤差，使得仿生四足機(jī)器人在Walk 步態(tài)下行走時，并不是理論上以0.4 m/s的速度做勻速直線運動，而是在做變速直線運動，系統(tǒng)的過程噪聲標(biāo)準(zhǔn)差是機(jī)器人在Walk 步態(tài)下行走時的加速度值。下面介紹獲取該加速度值的方法。

在Adams 仿真軟件中，建立仿生四足機(jī)器人的運動學(xué)模型，如圖1 所示。在仿生四足機(jī)器人機(jī)體的質(zhì)心處建立一個前進(jìn)方向的加速度測量，運行仿真，打開Adams 仿真軟件的后處理器，對獲得的加速度曲線進(jìn)行巴特沃斯濾波，然后計算加速度的平均值，將其作為該次仿真的加速度值。重復(fù)進(jìn)行50 次，得到50 個加速度值，求出標(biāo)準(zhǔn)差，即為系統(tǒng)的過程噪聲標(biāo)準(zhǔn)差。圖2 為某次仿真中的加速度曲線。最終求出系統(tǒng)的過程噪聲標(biāo)準(zhǔn)差為0.012 m/s2，即σw=0.012 m/s2。

圖1 Adams 中機(jī)器人的運動學(xué)模型Fig 1 Kinetic model of robot in Adams

圖2 加速度曲線Fig 2 Curve of acceleration

3.2 確定傳感器的量測噪聲標(biāo)準(zhǔn)差

對于傳感器的量測噪聲標(biāo)準(zhǔn)差，在仿真情況下，可以由其測量誤差來反映。

在實際應(yīng)用中，利用兩個CCD 攝像機(jī)獲取視差信息，再根據(jù)三角測量原理恢復(fù)出場景的深度信息，如此即可測量出障礙物與機(jī)器人之間的距離信息，然而，由于CCD 攝像機(jī)所拍攝的圖像是以像元大小為單位的一組離散的數(shù)據(jù)，故在用雙目視覺進(jìn)行測量時存在最小分辨率誤差，仿生四足機(jī)器人上搭載的雙目視覺傳感器的測量誤差約為6.8 cm，即σv1=0.068 m。

超聲測距傳感器的發(fā)射頭發(fā)出超聲波信號，此信號被障礙物反射后，由接收頭接收，根據(jù)發(fā)射和接收到信號的時間差和聲速，即可得到障礙物的距離信息。當(dāng)探測范圍內(nèi)有目標(biāo)物體之外的物體存在時，會產(chǎn)生測量誤差。仿生四足機(jī)器人上搭載的超聲測距傳感器的測量誤差為1 cm，即σv2=0.01 m。

3.3 確定卡爾曼濾波算法的初始值

卡爾曼濾波算法作為一個迭代過程，需要賦予其初值，初值的選擇至關(guān)重要，如果初值選擇不合適，就不能滿足收斂性的要求。在CV 模型中，P(0|0)的確定方法已經(jīng)由模型給出，這里只需給出X(0|0)的取值，本文中取X(0|0)=［10，－0.4］＇。

4 仿真實驗與結(jié)果分析

在完成上述準(zhǔn)備工作后，筆者在Matlab 軟件中進(jìn)行仿真實驗，仿真實驗流程如圖3 所示。

圖3 仿真實驗流程圖Fig 3 Flow chart of simulation experiment

根據(jù)仿真實驗流程圖，在Matlab 中先對模擬出的目標(biāo)位置信息進(jìn)行卡爾曼濾波處理，如圖4 和圖5 所示，這里的目標(biāo)指的是所測障礙物。首先，從圖4 和圖5 可以看出:經(jīng)卡爾曼濾波處理后的目標(biāo)位置的估計值在前2 s 偏離真實值較遠(yuǎn)，從第4 s 以后，無論觀測值如何波動，估計值曲線均能很好地跟蹤真實值曲線，說明卡爾曼濾波算法起到了良好的濾波效果。

將融合處理前后，目標(biāo)位置的估計值曲線和目標(biāo)位置估計值的方差曲線分別置于同一幅圖中，如圖6 所示，通過對比反映STF 融合算法的優(yōu)點。從圖6(b)中可以看出:融合處理后，目標(biāo)位置估計值的方差變小，說明融合處理后對目標(biāo)位置的估計更加準(zhǔn)確。從圖6 中可以發(fā)現(xiàn)，融合曲線介于雙目視覺傳感器的估計值曲線和超聲測距傳感器的估計值曲線之間，且更加靠近準(zhǔn)確度高的超聲測距傳感器的估計值曲線。

在本文所引文獻(xiàn)中，實驗驗證環(huán)節(jié)均在具體的應(yīng)用場景下進(jìn)行，實驗結(jié)果是移動機(jī)器人能夠進(jìn)行無礙行走，文中均未給出具體的測量精度。本文仿真實驗的結(jié)果表明:融合處理后，測量精度可達(dá)4.6 cm，滿足了仿生四足機(jī)器人對測距的精度要求。

圖4 雙目視覺傳感器系統(tǒng)的卡爾曼濾波Fig 4 Kalman filtering of binocular sensor

圖5 超聲測距傳感器系統(tǒng)的卡爾曼濾波Fig 5 Kalman filtering of ultrasonic ranging sensor system

5 結(jié) 論

為提高仿生四足機(jī)器人在復(fù)雜、動態(tài)環(huán)境下對障礙物位置信息的感知能力，本文針對仿生四足機(jī)器人在結(jié)構(gòu)化路面上以Walk 步態(tài)行走的情況，將雙目視覺傳感器和超聲測距傳感器獲取的障礙物距離信息進(jìn)行融合研究。仿真實驗結(jié)果表明:濾波后的距離信息的估計值曲線很好地跟蹤了真實值曲線，說明卡爾曼濾波算法發(fā)揮了出色的濾波作用;與融合前相比，融合處理后的距離信息的估計值的方差明顯減小，說明融合處理后獲得的障礙物的位置信息更加準(zhǔn)確，且測量精度為4.6 cm，滿足了機(jī)器人的應(yīng)用要求。

圖6 融合前后目標(biāo)位置估計值曲線和方差曲線的對比Fig 6 Comparison of curve of estimation value of target position before and after fusion and covariance curve

［1］ Bao Bizhen，Yang Ping，Cao Huwen.A method for modeling and simulation of differential drive mobile robot localization based on EKF multisensor fusion［J］.Advanced Materials Research，2011，383－390:2339－2345.

［2］ 周 霞，何建忠，王寶磊.多傳感器信息融合伺服的移動機(jī)器人快速跟隨［J］.計算機(jī)工程與設(shè)計，2013，34(6):2247－2251.

［3］ 姜志兵.模糊神經(jīng)網(wǎng)絡(luò)信息融合在移動機(jī)器人導(dǎo)航中的應(yīng)用［J］.機(jī)床與液壓，2009，37(7):154－156.

［4］ 王耀南，李樹濤.多傳感器信息融合及其應(yīng)用綜述［J］.控制與決策，2001，16(5):518－522.

［5］ Nouara Dechemi.Multisensor data fusion based on fuzzy logic［C］∥International Conference on Automation，Robotics and Control System，Orlando，F(xiàn)L，USA，2010:121－127.

［6］ 丁承君，張明路，段 萍.模糊神經(jīng)網(wǎng)絡(luò)在移動機(jī)器人信息融合中的應(yīng)用［J］.控制理論與應(yīng)用，2004，21(1):59－62.

［7］ Steven M.Kay fundamentals of statistical signal processing［M］.Beijing:Publishing House of Electronics Industry，2003:14－20.

［8］ 何 友，王國宏，陸大金，等.多傳感器信息融合及應(yīng)用［M］.北京:電子工業(yè)出版社，2007:13－15.

［9］ 王 軍，蘇劍波，席裕庚.多傳感器融合綜述［J］.數(shù)據(jù)采集與處理，2004，19(1):72－77.

［10］余 華，吳文全，劉 忠.D－S 證據(jù)理論在移動機(jī)器人信息融合中的應(yīng)用［J］.計算機(jī)與數(shù)字工程，2009，37(11):125－129.

［11］許 芬，咸寶金，李正熙.基于產(chǎn)生式規(guī)則多傳感器數(shù)據(jù)融合方法的移動機(jī)器人避障［J］.電子測量與儀器學(xué)報，2009，23(10):73－79.

［12］姚惠元.TWS 雷達(dá)目標(biāo)航跡跟蹤及預(yù)測算法研究［D］.哈爾濱:哈爾濱工業(yè)大學(xué)，2011:6－8.