多傳感器信息融合

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

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

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

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

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

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

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

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

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

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