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

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

0 引 言

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

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

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

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

多傳感器信息融合方法大致可以分為三類，即，概率統(tǒng)計(jì)方法、邏輯推理方法和學(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)計(jì)方法的一種。

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

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

2 勻速直線運(yùn)動(dòng)模型

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

其中

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

3 參數(shù)確定

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

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

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

在Adams 仿真軟件中，建立仿生四足機(jī)器人的運(yùn)動(dòng)學(xué)模型，如圖1 所示。在仿生四足機(jī)器人機(jī)體的質(zhì)心處建立一個(gè)前進(jìn)方向的加速度測(cè)量，運(yùn)行仿真，打開Adams 仿真軟件的后處理器，對(duì)獲得的加速度曲線進(jìn)行巴特沃斯濾波，然后計(jì)算加速度的平均值，將其作為該次仿真的加速度值。重復(fù)進(jìn)行50 次，得到50 個(gè)加速度值，求出標(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ī)器人的運(yùn)動(dòng)學(xué)模型Fig 1 Kinetic model of robot in Adams

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

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

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

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

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

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

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

4 仿真實(shí)驗(yàn)與結(jié)果分析

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

圖3 仿真實(shí)驗(yàn)流程圖Fig 3 Flow chart of simulation experiment

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

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

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

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

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

5 結(jié) 論

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

圖6 融合前后目標(biāo)位置估計(jì)值曲線和方差曲線的對(duì)比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］ 周 霞，何建忠，王寶磊.多傳感器信息融合伺服的移動(dòng)機(jī)器人快速跟隨［J］.計(jì)算機(jī)工程與設(shè)計(jì)，2013，34(6):2247－2251.

［3］ 姜志兵.模糊神經(jīng)網(wǎng)絡(luò)信息融合在移動(dòng)機(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ò)在移動(dòng)機(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ù)理論在移動(dòng)機(jī)器人信息融合中的應(yīng)用［J］.計(jì)算機(jī)與數(shù)字工程，2009，37(11):125－129.

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

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