多傳感器信息融合

#式中X(k)二[x(k),y(k),6(k)]T；w(k)為模型誤差，是零均值的高斯白噪聲;0(k)為方差，T為采樣時(shí)間3)TOC\o"1-5"\h\z'x(k)+Trcos0(①)/2'3)lrf(X(k))=y(k)+Trsin0f(X(k))=lr、0(k)+Trcos0(①一①)/2丿rl式(3)就是移動(dòng)機(jī)器人的運(yùn)動(dòng)模型，也是系統(tǒng)的狀態(tài)方程。2傳感器觀測(cè)模型的建立2．1里程計(jì)里程計(jì)的工作原理是根據(jù)安裝在2個(gè)驅(qū)動(dòng)輪電機(jī)上的光電編碼器來檢測(cè)車輪在一定時(shí)間內(nèi)轉(zhuǎn)過的弧度，進(jìn)而推算機(jī)器人相對(duì)位姿的變化。設(shè)車輪半徑為r,光電碼盤為P線/轉(zhuǎn)，t時(shí)間內(nèi)光碼盤輸出的脈沖數(shù)為N,則該車輪移動(dòng)距離止為4)假設(shè)由光電碼盤檢測(cè)出機(jī)器人左右輪的移動(dòng)距離分別為As和As，且兩輪lr的間距為b，機(jī)器人從位姿X(k)=[x(k),y(k),0(k)]T運(yùn)動(dòng)到X(k+1)=[x(k+1),y(k+1),0(k+1)]t。則機(jī)器人移動(dòng)的距離As=(As+As)/2,機(jī)lr器人轉(zhuǎn)過的角度A0=(As-As)/b。lr因此，在已知初始位置的情況下，即可求出前輪轉(zhuǎn)過的距離，若采樣時(shí)間取得足夠短，通過計(jì)算出在時(shí)間內(nèi)機(jī)器人位置的橫、縱坐標(biāo)和方向的變化量，進(jìn)行累加，可推出機(jī)器人在全局坐標(biāo)中的位置坐標(biāo)和方向角，從而獲得自定位信息。2．2超聲波傳感器超聲波傳感器的基本原理是發(fā)送(超聲)壓力波包，一般為40-45kHz,當(dāng)波包遇到物體后，就會(huì)被反彈回，通過測(cè)量該波包反射和回到接收器所占用的時(shí)間，引起反射的物體距離d可以根據(jù)聲音傳播速度C和飛越時(shí)間t進(jìn)行計(jì)算d=ct(5)

其模型可簡(jiǎn)化為在一個(gè)固定的波帶開放角方位之內(nèi)，傳感器到莫一物體的最短距離。其讀數(shù)與機(jī)器人所在的環(huán)境和傳感器的安裝位置有關(guān)。設(shè)第i只超聲波傳感器在ORXRYR坐標(biāo)系中的坐標(biāo)為(x,y)，其方向(與RiRiX軸的夾角)為0。假定在地k個(gè)采樣時(shí)刻，機(jī)器人的位置為RRiX(k)二[x(k),y(k),0(k)]t，則經(jīng)過一個(gè)旋轉(zhuǎn)平移的坐標(biāo)變換，可將第i只超聲波傳感器在ORXrYr坐標(biāo)系中的坐標(biāo)為(x,y)轉(zhuǎn)換到OXY坐標(biāo)系中的坐標(biāo)RiRi(x(k),y(k))，寫成齊次坐標(biāo)的形式為ii'x(k)iy(k)i'x(k)iy(k)iI1'sin0(k)-cos0(k)<0cos0(k)sin0(k)0(x)Riy(k)yRi11丿6)同時(shí)，將超聲波傳感器的方向0轉(zhuǎn)換為與OXY坐標(biāo)系的X軸的夾角0(k)Rii0(k)=0(0(k)=0(k)+0iRi機(jī)器人移動(dòng)所在的環(huán)境中的反射墻面與障礙等可用OXY平面上的直線ax+by+c二0來表示(j=1,23……為環(huán)境中的墻與障礙物得個(gè)數(shù))。超聲波jjj的測(cè)量原理如圖2所示圖2超聲波傳感器的測(cè)量示意圖設(shè)?=arctan(a/b)，§為超聲波傳感器的波帶開放角。根據(jù)超聲波傳感器jjj

的測(cè)量原理，當(dāng)?shù)趇只超聲波傳感器和第j個(gè)物體之間滿足兀/2-ag[0(k)-a/2,0(k)+a/2]時(shí)，超聲波傳感器i測(cè)得的到第j個(gè)物體的距jii離為dj=j*空*°;當(dāng)不滿足上述關(guān)系時(shí)，得到的數(shù)據(jù)要去除。如果超聲波1Ja2+b2傳感器測(cè)出的數(shù)據(jù)超出了其測(cè)量范圍，則超出范圍的數(shù)據(jù)也要去除。因此，在每個(gè)采樣時(shí)刻，不是所有的超聲波傳感器的讀數(shù)都被使用，而是結(jié)合環(huán)境地圖來決定應(yīng)該使用哪些讀數(shù)。可以得到該多傳感器系統(tǒng)的隨機(jī)觀測(cè)模型為(8)Z(k)=h(X(k))+v(k)其中，v(k)為觀測(cè)誤差，是零均值的高斯白噪聲，方差為R(k)。Z(k)包括里程計(jì)的輸出與超聲波傳感器的讀數(shù)，所以，Z(k)的維數(shù)為里程計(jì)輸出向量個(gè)數(shù)加上超聲傳感器的只數(shù)。里程計(jì)的輸出為9)10)”z(k)=x(k)+v(k)9)10)11<z(k)=y(k)+v(k)22z(k)=0(k)+v(k)33超聲波傳感器的輸出為z(k)=dj*v(k)3*ii3*i定位算法的結(jié)構(gòu)如圖3所示。圖3定位算法框圖3EKF算法EKF假設(shè)系統(tǒng)狀態(tài)的概率分布是高斯分布，噪聲是相互獨(dú)立的。式(2)與式(8)分別為機(jī)器人的運(yùn)動(dòng)模型和傳感器模型。初始化已凍結(jié)i氣人的狀態(tài)向量X和誤差協(xié)方差矩陣p。0011)12)預(yù)測(cè)機(jī)器人位置：根據(jù)機(jī)器人在k時(shí)刻的位置預(yù)測(cè)時(shí)間11)12)X(k+1/k)=f(X(f/k))P(k+1/k)二F(k)P(1/k)F(k)t+Q(k)其中，P為X的誤差協(xié)方差矩陣，F(xiàn)為f的Jacobian矩陣，可得

觀測(cè)傳感器的預(yù)測(cè)值為施+1；=hNIL5:.:.，用來對(duì)預(yù)測(cè)狀態(tài)進(jìn)行校正的量為實(shí)際的測(cè)量值與預(yù)測(cè)值之差，即信息為丁*」-必飛7丈11：,信息的協(xié)方差矩陣為X-〃*；，h為測(cè)量方程中h(X(k))的Jacobian矩陣H(k)=VH(k)=Vh(k)=xsin?(k)+a)+yrijricos?(k)+a)j丿14)式中x，y為第i只超聲波傳感器的參數(shù)；a，b為外部環(huán)境中第j個(gè)物ririjj體的參數(shù)；H(k)的行數(shù)是不固定的，與各個(gè)采樣時(shí)間實(shí)際所使用的超聲波傳感器的只數(shù)有關(guān)。對(duì)于每一次測(cè)量，由于測(cè)量的不確定性，需要對(duì)每次測(cè)量進(jìn)行校驗(yàn)來決定是否匹配。若測(cè)量結(jié)果滿足下式，則測(cè)量值匹配。否則，不匹配并舍棄Y(k+1)S(k+1)-1Y(k+1)t<G2(15)式中G2為正確性檢驗(yàn)門。糾正16)17)EKF的增益矩陣16)17)W(k+1)二P(k+1/k)HtS-1(k+1)機(jī)器人的狀態(tài)糾正為X(k+1/k+1)=X(k+1/k)+w(k+1)Y(k+1)協(xié)方差矩陣更新為P(k+1/k+1)=(I-W(k+1)H(k))P(k+1/k)返回步驟(2)遞歸執(zhí)行上述步驟(2—5)。

4實(shí)驗(yàn)實(shí)驗(yàn)環(huán)境設(shè)為四周是垂直的墻壁，移動(dòng)機(jī)器人逆時(shí)針沿著墻面做矩形軌線運(yùn)動(dòng)，用里程計(jì)和超聲波傳感器進(jìn)行自定位。圖4曲線分別表示系統(tǒng)的觀測(cè)真值Z(k)，觀測(cè)估計(jì)Z(k)，狀態(tài)真值X(k)，狀態(tài)先驗(yàn)估計(jì)X(k+1/k)，狀態(tài)后驗(yàn)估計(jì)X(k+1/k+1)。煽測(cè)農(nóng)實(shí)(?估計(jì)⑷狀態(tài)貞實(shí)(--)先驗(yàn)估計(jì)⑴肩姻折to圖4系統(tǒng)觀測(cè)向量與狀態(tài)向量曲線圖5為僅使用里程計(jì)的位置估計(jì)，圖6為融合里程計(jì)和超聲波傳感器數(shù)據(jù)信息后的位置估計(jì)。機(jī)器人的移動(dòng)速度為0.25m/s,采樣時(shí)間T=lS,估計(jì)的初始位置為(0,0)，實(shí)際的初始位置為(0.15,0)，P0=105I，模型噪聲的方差取為0.1，測(cè)量噪聲方差取為0.8。0200400600Jr/Cm從實(shí)驗(yàn)結(jié)果中可以看出：由于里程計(jì)的測(cè)量噪聲不斷地被累加，其估計(jì)過程中估量值與實(shí)際量之間在轉(zhuǎn)角處偏差大，在運(yùn)動(dòng)不到2圈時(shí)，位姿估計(jì)信息就已經(jīng)完全丟失，估計(jì)效果較差。采用里程計(jì)和超聲波傳感器融合后的定位算法，基本消除了里程計(jì)的累計(jì)誤差，提高了定位的精度。5結(jié)論采用多傳感器信息融合算法將里程計(jì)和超聲波傳感器采集的信息融合后進(jìn)行機(jī)器人的自定位，該方法基于EKF算法，通過對(duì)機(jī)器人和傳感器的建模，在路徑規(guī)劃相對(duì)簡(jiǎn)單、軌跡相對(duì)較短的情況下，可以取