多傳感器信息融合

1、多傳感器信息融合0前言移動(dòng)機(jī)器人的定位問(wèn)題是提高移動(dòng)機(jī)器人 自主能力的關(guān)鍵問(wèn)題之一。具體來(lái)說(shuō)， 定位是利用先驗(yàn)環(huán)境地圖信息、機(jī)器人位姿的當(dāng)前估計(jì)及傳感器的觀測(cè)值等輸入信息 ，經(jīng)過(guò)一定的處理和變換 ，產(chǎn)生更加準(zhǔn)確地對(duì)機(jī)器人當(dāng)前位姿的估計(jì)。機(jī)器人的定位方式有很多種， 如 ， 基于光電尋線的定位 、基于聲納的機(jī)器人自主定位 、基于全景視覺(jué)的定位及基于激光測(cè)距的定位等?？梢钥闯觯簷C(jī)器人的定位方式取決于所采用的傳感器。目前， 在移動(dòng)機(jī)器人上使用較多的傳感器有視覺(jué)傳感器、里程計(jì)和慣導(dǎo)系統(tǒng)、超聲傳感器、激光測(cè)距儀、gps定位系統(tǒng)等。其中，視覺(jué)傳感器具有信息量大、感應(yīng)時(shí)間短的優(yōu)點(diǎn)，但往往獲得的數(shù)據(jù)噪聲大、 信

2、息處理時(shí)間長(zhǎng)；激光傳感器在測(cè)距范圍和方向上具有較高的精度，但價(jià)格昂貴；超聲波傳感器雖然角度分辨力較低，但它處理信息簡(jiǎn)單、成本低 、速度快，因此，在自主移動(dòng)機(jī)器人上得到了廣泛的應(yīng)用；里程計(jì)是一種相對(duì)定位傳感器，它通過(guò)累計(jì)計(jì)算得到定位信息，缺點(diǎn)是存在累計(jì)誤差問(wèn)題，因此，可結(jié)合絕對(duì)定位傳感器，如超聲傳感器等，提供較準(zhǔn)確的定位。各傳感器都有它自己的局限性，因此，移動(dòng)機(jī)器人往往同時(shí)裝備多種傳感器，各自提供關(guān)于機(jī)器人定位的消息。目前的趨勢(shì)是：根據(jù)傳感器的可靠性。使用不同類(lèi)型的傳感器來(lái)測(cè)量相關(guān)數(shù)據(jù)。本文采用擴(kuò)展卡爾曼濾波( ekf) 技術(shù)，將里程計(jì)和超聲波傳感器所提供的數(shù)據(jù)進(jìn)行融合定位。 1 機(jī)器人運(yùn)動(dòng)模型

3、的建立 由于移動(dòng)機(jī)器人機(jī)構(gòu)復(fù)雜，為了便于構(gòu)造運(yùn)動(dòng)學(xué)模型與規(guī)劃控制機(jī)器人的位姿，本文選擇兩輪驅(qū)動(dòng)小車(chē)作為運(yùn)動(dòng)平臺(tái)。將整個(gè)機(jī)器人本體看作一個(gè)剛體， 車(chē)輪視為剛性輪，并在運(yùn)動(dòng)不是太快而轉(zhuǎn)彎半徑較大時(shí)， 不考慮車(chē)輪與地面?zhèn)认蚧瑒?dòng)的情況，其簡(jiǎn)化運(yùn)動(dòng)學(xué)模型如圖1 所示。 圖 1 兩輪驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型為了確定機(jī)器人在平面中的位置，建立平面全局參考坐標(biāo)系o x y和機(jī)器人局部參考坐標(biāo)系or xr yr， 把or xr yr坐標(biāo)系原點(diǎn)建立在 2個(gè)驅(qū)動(dòng)輪軸心連線的中點(diǎn)0r上，并將該點(diǎn)作為機(jī)器人的位置參考點(diǎn)。相交于點(diǎn)or的 2個(gè)軸分別定義為xr和yr軸。在 o x y坐標(biāo)系下， or的位置由坐標(biāo)x和y確定， 坐

4、標(biāo)系 o x y和or xr yr之間的角度差(也是機(jī)器人線速度的方向)由給定?？梢詫C(jī)器人的姿態(tài)描述為具有這3個(gè)元素的向量：。圖1中，分別為左右兩驅(qū)動(dòng)輪的轉(zhuǎn)動(dòng)角速度，r為兩輪半徑，b為兩驅(qū)動(dòng)輪之間的距離，or點(diǎn)速度(也就是機(jī)器人的線速度) 為， 分別投影到 o x y坐標(biāo)系上得，。移動(dòng)機(jī)器人的角速度。于是，機(jī)器人的運(yùn)動(dòng)方程為（1）將方程(1)進(jìn)行離散化，并加上模型噪聲，可得機(jī)器人的離散隨機(jī)狀態(tài)空間表達(dá)式（2）式中；為模型誤差，是零均值的高斯白噪聲；為方差，t為采樣時(shí)間（3）式(3)就是移動(dòng)機(jī)器人的運(yùn)動(dòng)模型，也是系統(tǒng)的狀態(tài)方程。2 傳感器觀測(cè)模型的建立 21 里程計(jì) 里程計(jì)的工作原理是根據(jù)安裝

5、在2個(gè)驅(qū)動(dòng)輪電機(jī)上的光電編碼器來(lái)檢測(cè)車(chē)輪在一定時(shí)間內(nèi)轉(zhuǎn)過(guò)的弧度，進(jìn)而推算機(jī)器人相對(duì)位姿的變化。設(shè)車(chē)輪半徑為r，光電碼盤(pán)為p線轉(zhuǎn)，t時(shí)間內(nèi)光碼盤(pán)輸出的脈沖數(shù)為n，則該車(chē)輪移動(dòng)距離為（4）假設(shè)由光電碼盤(pán)檢測(cè)出機(jī)器人左右輪的移動(dòng)距離分別為和，且兩輪的間距為b，機(jī)器人從位姿運(yùn)動(dòng)到。則機(jī)器人移動(dòng)的距離， 機(jī)器人轉(zhuǎn)過(guò)的角度。 因此，在已知初始位置的情況下，即可求出前輪轉(zhuǎn)過(guò)的距離，若采樣時(shí)間取得足夠短，通過(guò)計(jì)算出在時(shí)間內(nèi)機(jī)器人位置的橫、縱坐標(biāo)和方向的變化量，進(jìn)行累加， 可推出機(jī)器人在全局坐標(biāo)中的位置坐標(biāo)和方向角，從而獲得自定位信息。 22 超聲波傳感器 超聲波傳感器的基本原理是發(fā)送(超聲)壓力波包，一般為4

6、0-45khz，當(dāng)波包遇到物體后，就會(huì)被反彈回，通過(guò)測(cè)量該波包反射和回到接收器所占用的時(shí)間，引起反射的物體距離 d可以根據(jù)聲音傳播速度 c和飛越時(shí)間 t 進(jìn)行計(jì)算 （5） 其模型可簡(jiǎn)化為在一個(gè)固定的波帶開(kāi)放角方位之內(nèi)，傳感器到莫一物體的最短距離。其讀數(shù)與機(jī)器人所在的環(huán)境和傳感器的安裝位置有關(guān)。設(shè)第i只超聲波傳感器在or xr yr坐標(biāo)系中的坐標(biāo)為，其方向（與軸的夾角）為。假定在地k個(gè)采樣時(shí)刻，機(jī)器人的位置為，則經(jīng)過(guò)一個(gè)旋轉(zhuǎn)平移的坐標(biāo)變換，可將第i只超聲波傳感器在or xr yr坐標(biāo)系中的坐標(biāo)為轉(zhuǎn)換到oxy坐標(biāo)系中的坐標(biāo)，寫(xiě)成齊次坐標(biāo)的形式為（6）同時(shí)，將超聲波傳感器的方向轉(zhuǎn)換為與oxy坐標(biāo)系的

7、x軸的夾角（7）機(jī)器人移動(dòng)所在的環(huán)境中的反射墻面與障礙等可用oxy平面上的直線來(lái)表示（j=1,2,3，為環(huán)境中的墻與障礙物得個(gè)數(shù)）。超聲波的測(cè)量原理如圖2所示圖 2 超 聲波傳感器的測(cè)量示意圖設(shè)，為超聲波傳感器的波帶開(kāi)放角。根據(jù)超聲波傳感器的測(cè)量原理， 當(dāng)?shù)趇只超聲波傳感器和第j個(gè)物體之間滿(mǎn)足時(shí)，超聲波傳感器i測(cè)得的到第j個(gè)物體的距離為；當(dāng)不滿(mǎn)足上述關(guān)系時(shí)，得到的數(shù)據(jù)要去除。如果超聲波傳感器測(cè)出的數(shù)據(jù)超出了其測(cè)量范圍，則超出范圍的數(shù)據(jù)也要去除。因此，在每個(gè)采樣時(shí)刻，不是所有的超聲波傳感器的讀數(shù)都被使用，而是結(jié)合環(huán)境地圖來(lái) 決定應(yīng)該使用哪些讀數(shù)。 可以得到該多傳感器系統(tǒng)的隨機(jī)觀測(cè)模型為（8）其中

8、，為觀測(cè)誤差，是零均值的高斯白噪聲，方差為r(k)。包括里程計(jì)的輸出與超聲波傳感器的讀數(shù)，所以，的維數(shù)為里程計(jì)輸 出向量個(gè)數(shù)加上超聲傳感器的只數(shù)。里程計(jì)的輸出為（9）超聲波傳感器的輸出為（10）定位算法的結(jié)構(gòu)如圖3所示。圖3定位算法框圖3 ekf算法 ekf假設(shè)系統(tǒng)狀態(tài)的概率分布是高斯分布，噪聲是相互獨(dú)立的。式(2)與式(8)分別為機(jī)器人的運(yùn)動(dòng)模型和傳感器模型。 1）初始化已凍結(jié)i氣人的狀態(tài)向量和誤差協(xié)方差矩陣。2）預(yù)測(cè)機(jī)器人位置：根據(jù)機(jī)器人在k時(shí)刻的位置預(yù)測(cè)時(shí)間k+1時(shí)刻的位置（11）（12）其中，p為x的誤差協(xié)方差矩陣，f為f的jacobian矩陣，可得3）觀測(cè)傳感器的預(yù)測(cè)值為，用來(lái)對(duì)預(yù)測(cè)

9、狀態(tài)進(jìn)行校正的量為實(shí)際的測(cè)量值與預(yù)測(cè)值之差，即信息為，信息的協(xié)方差矩陣為，h為測(cè)量方程中的jacobian矩陣（14）式中，為第i只超聲波傳感器的參數(shù)；，為外部環(huán)境中第j個(gè)物體的參數(shù)；h（k）的行數(shù)是不固定的，與各個(gè)采樣時(shí)間實(shí)際所使用的超聲波傳感器的只數(shù)有關(guān)。4）對(duì)于每一次測(cè)量，由于測(cè)量的不確定性，需要對(duì)每次測(cè)量進(jìn)行校驗(yàn)來(lái)決定是否匹配。若測(cè)量結(jié)果滿(mǎn)足下式，則測(cè)量值匹配。否則，不匹配并舍棄(15)式中為正確性檢驗(yàn)門(mén)。5）糾正ekf的增益矩陣w更新為（16）機(jī)器人的狀態(tài)糾正為（17）協(xié)方差矩陣更新為（18）6）返回步驟（2）遞歸執(zhí)行上述步驟（25）。4 實(shí)驗(yàn) 實(shí)驗(yàn)環(huán)境設(shè)為四周是垂直的墻壁，移動(dòng)機(jī)器

10、人逆時(shí)針沿著墻面做矩形軌線運(yùn)動(dòng)，用里程計(jì)和超聲波傳感器進(jìn)行自定位。圖4曲線分別表示系統(tǒng)的觀測(cè)真值 z( k )，觀測(cè)估計(jì)，狀態(tài)真值x( k )，狀態(tài)先驗(yàn)估計(jì)，狀態(tài)后驗(yàn)估計(jì)。 圖4系統(tǒng)觀測(cè)向量與狀態(tài)向量曲線圖5為僅使用里程計(jì)的位置估計(jì)，圖6為融合里程計(jì)和超聲波傳感器數(shù)據(jù)信息后的位置估計(jì)。機(jī)器人的移動(dòng)速度為s， 采樣時(shí)間t=ls，估計(jì)的初始位置為（0,0），實(shí)際的初始位置為（,0），p 0 =105i， 模型噪聲的方差取為，測(cè)量噪聲方差取為。 圖 5 實(shí)際位 置與里程計(jì)定位信息圖 6 實(shí)際位置與傳感器融合定位信息從實(shí)驗(yàn)結(jié)果中可以看出：由于里程計(jì)的測(cè)量噪聲不斷地被累加，其估計(jì)過(guò)程中估量值與實(shí)際量之間在轉(zhuǎn)角處偏差大，在運(yùn)動(dòng)不到2圈時(shí)，位姿估計(jì)信息就已經(jīng)完全丟失，估計(jì)效果較差。采用里程計(jì)和超聲波傳感器融合后的定位算法，基本消除了里程計(jì)的累計(jì)誤差，提高了定位的精度。 5 結(jié)論 采用多傳感器信息融合算法將里程計(jì)和超聲波傳感器采集的信息融合后進(jìn)行機(jī)器人的自定位，該方法基于ekf算法，通過(guò)對(duì)機(jī)器人和傳感器的建模 ，在路徑規(guī)劃相對(duì)簡(jiǎn)單、軌