李昌明，梅 莉，秦東興

（1.電子科技大學(xué)，四川 成都 611731；2.克拉瑪依職業(yè)技術(shù)學(xué)院，新疆 獨(dú)山子 833600）

0 引 言

由于傳感器都存在著自身缺陷，獲取的信息量有限，單一傳感器信號(hào)難以保證輸入信息的準(zhǔn)確性、可靠性及充足性。針對(duì)這個(gè)問(wèn)題，該文采用擴(kuò)展卡爾曼濾波進(jìn)行多傳感信息融合，擴(kuò)展卡爾曼濾波器（EKF）具有收斂速度快、存儲(chǔ)量小的優(yōu)點(diǎn)[1]。其中里程計(jì)、陀螺儀、激光雷達(dá)為主要傳感器，將相對(duì)定位[2]與絕對(duì)定位[3]進(jìn)行組合，里程計(jì)與陀螺儀的融合濾波作為機(jī)器人運(yùn)動(dòng)學(xué)模型；通過(guò)激光雷達(dá)獲取環(huán)境特征，建立其模型作為機(jī)器位置觀測(cè)模型。運(yùn)動(dòng)模型與觀測(cè)模型進(jìn)行結(jié)合，利用擴(kuò)展卡爾曼濾波對(duì)環(huán)境特征進(jìn)行跟蹤，最終實(shí)現(xiàn)機(jī)器人的精確定位。

1 系統(tǒng)工作原理

圖1為設(shè)計(jì)的擴(kuò)展卡爾曼濾波機(jī)器人定位系統(tǒng)的原理圖。由圖可知，該系統(tǒng)的過(guò)程為一個(gè)遞推過(guò)程。第一步位置預(yù)測(cè)或動(dòng)作更新，把高斯誤差運(yùn)動(dòng)模型[4]直接應(yīng)用到機(jī)器人所測(cè)量的里程計(jì)與陀螺儀的行走，獲取里程計(jì)和陀螺儀的數(shù)據(jù)，通過(guò)里程計(jì)和陀螺儀的估計(jì)融合，產(chǎn)生機(jī)器人的預(yù)測(cè)位置。然后根據(jù)預(yù)測(cè)位置，在環(huán)境地圖數(shù)據(jù)庫(kù)中找到與之匹配的預(yù)測(cè)觀測(cè)值，即預(yù)測(cè)激光雷達(dá)將提取到的環(huán)境特征及特征的位置信息。在匹配過(guò)程中，機(jī)器人將預(yù)測(cè)的觀測(cè)值與激光雷達(dá)的實(shí)際觀測(cè)值進(jìn)行匹配，找出最佳匹配。最后應(yīng)用擴(kuò)展卡爾曼濾波將最佳匹配所提供的信息進(jìn)行數(shù)據(jù)融合，更新估計(jì)機(jī)器人的信任度狀態(tài)，得到機(jī)器人位置的最佳估計(jì)，最終實(shí)現(xiàn)機(jī)器人基于環(huán)境地圖定位的精確定位[5-6]。

圖1 系統(tǒng)工作原理圖

2 傳感器模型

2.1 里程計(jì)模型

建立機(jī)器人運(yùn)動(dòng)二維坐標(biāo)地圖，移動(dòng)機(jī)器人的姿態(tài)可以表示為

式中：x，y——機(jī)器人相對(duì)坐標(biāo)原點(diǎn)的X和Y方向運(yùn)動(dòng)距離；

θ——機(jī)器人自身姿態(tài)相對(duì)于X軸角度。

差動(dòng)驅(qū)動(dòng)機(jī)器人的運(yùn)動(dòng)模型[7]如圖2所示，在極短固定采樣時(shí)間Δt內(nèi)p到p′機(jī)器人位姿改變量（Δx，Δy，Δθ）為

式中：Δsr、Δsl——左右輪行走的距離；

b——兩差動(dòng)驅(qū)動(dòng)輪之間的間距。

圖2 差動(dòng)驅(qū)動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型

由此，更新過(guò)的位置p′為

式（6）為里程表位置更新的基本方程。在增量運(yùn)動(dòng)（Δsr，Δsl）中，存在傳感器不確定性積分誤差和近似運(yùn)動(dòng)模型帶來(lái)的誤差。由誤差傳播理論，里程表位置估計(jì)的協(xié)方差矩陣Σp′為

假定初始點(diǎn)協(xié)方差矩陣Σp已知，運(yùn)動(dòng)增量（Δsr，Δsl）協(xié)方差矩陣為

式中：kr、kl——誤差常數(shù)，代表驅(qū)動(dòng)電機(jī)、輪子和地面交互的非確定性參數(shù)，其具體值應(yīng)由實(shí)驗(yàn)確定。

利用式（6）可以計(jì)算2個(gè)雅可比矩陣Fp和FΔr1。

2.2 陀螺儀模型

陀螺儀是一種慣性器件，用于測(cè)量其載體的轉(zhuǎn)動(dòng)角速度和轉(zhuǎn)動(dòng)角度，其輸出經(jīng)積分得到機(jī)器人的位姿角度變化量。因此，使用陀螺儀時(shí)首先給定一個(gè)基準(zhǔn)方向，機(jī)器人運(yùn)動(dòng)中的姿態(tài)將由自身角度變化量的不斷累加得到。一定時(shí)間Ts內(nèi)更新過(guò)的姿態(tài)角 θ′為

式中：θ——未更新時(shí)的姿態(tài)角；

ωi——陀螺儀的輸出角速度。

該文假定陀螺儀的測(cè)量值服從高斯白噪聲分布，對(duì)零輸入時(shí)建立擬合誤差模型，確定模型參數(shù)，最后求出方差Q（K）。

2.3 激光雷達(dá)模型

如圖3所示，在機(jī)器人傳感器所處的極坐標(biāo)中，產(chǎn)生n個(gè)測(cè)量點(diǎn)xRi=（ρRi，θRi），假定距離信息ρR和掃描角度信息θR的測(cè)量誤差受高斯概率密度曲線的約束，方差分別為常量（σρ，σθ），且彼此獨(dú)立。給定測(cè)量點(diǎn)（ρR，θR）計(jì)算相應(yīng)的歐氏坐標(biāo)為

xR=ρRcosθR， yR=ρRsinθR

給定一條直線方程為

特定點(diǎn)xRi=（ρRi，θRi）到直線之間的正交距離dRi為

圖3 最小平方意義的估計(jì)直線及環(huán)境坐標(biāo)W到機(jī)器人坐標(biāo)框架R位置表示

將屬于同一條直線的數(shù)據(jù)點(diǎn)歸并成類(lèi)，并采用最小二乘法進(jìn)行直線擬合，最后得出擬合直線參數(shù)αR和 γR。

激光雷達(dá)傳感器測(cè)量的不確定性將影響所提取直線的不確定性。用A和E分別表示隨機(jī)輸出變量αR和γR，系統(tǒng)輸出協(xié)方差矩陣為

式中：CX——給定2n×2n輸入?yún)f(xié)方差矩陣，即

Fρθ——雅可比。

因此，在考慮激光雷達(dá)傳感器數(shù)據(jù)誤差的基礎(chǔ)上，根據(jù)提取的直線環(huán)境特征建立了傳感器模型。從傳感器數(shù)據(jù)提取的每一條直線，都有一對(duì)參數(shù)（αW，rW）和其對(duì)應(yīng)的誤差協(xié)方差CAE與其對(duì)應(yīng)，該協(xié)方差就是觀測(cè)方程中的觀測(cè)誤差方差Σ

R，i。

3 環(huán)境特征的預(yù)測(cè)

多傳感融合中最重要的就是測(cè)量預(yù)測(cè)環(huán)境特征與觀測(cè)環(huán)境特征之間的匹配。預(yù)測(cè)的機(jī)器人位置p?（k|k）將產(chǎn)生期望特征 zt，i。在環(huán)境地圖中，存儲(chǔ)的特征是直線特征，以環(huán)境坐標(biāo)系參數(shù)給出。而傳感器所提取的直線特征是以機(jī)器人自身局部坐標(biāo)給出，因此需要將環(huán)境坐標(biāo)系W中的測(cè)量預(yù)測(cè)特征變換到機(jī)器人框架R中，變換由式（14）給出

它的雅可比▽hi為

由機(jī)器人預(yù)測(cè)的位置，機(jī)器人框架下可能提取的環(huán)境特征被獲取。根據(jù)擴(kuò)展卡爾曼濾波可以得出觀測(cè)方程為

式中：wi（k）——傳感器的觀測(cè)誤差。

4 擴(kuò)展卡爾曼濾波器原理

卡爾曼濾波器是一種基于概率模型進(jìn)行狀態(tài)和參數(shù)估計(jì)的有效方法，由一系列遞歸數(shù)學(xué)公式描述。它們提供了一種高效可計(jì)算的方法來(lái)估計(jì)過(guò)程的狀態(tài)，并使估計(jì)均方誤差最小。卡爾曼濾波器應(yīng)用廣泛且功能強(qiáng)大，它可以估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)，即使并不知道模型的確切性質(zhì)。擴(kuò)展卡爾曼濾波器（EKF）用于處理觀測(cè)變量與過(guò)程的關(guān)系是非線性的問(wèn)題，首先對(duì)其進(jìn)行線性近似，再利用線性系統(tǒng)卡爾曼濾波器的基本方程實(shí)現(xiàn)狀態(tài)估計(jì)。在移動(dòng)機(jī)器人位姿估計(jì)中，在系統(tǒng)噪聲和測(cè)量服從高斯白噪聲分布情況下，卡爾曼濾波用帶有噪聲的狀態(tài)議程和觀測(cè)議程遞推決定統(tǒng)計(jì)意義上的最優(yōu)位姿估計(jì)。

5實(shí)驗(yàn)

該實(shí)驗(yàn)平臺(tái)為ARM8962 32位ARM Corte-M3主控芯片，協(xié)處理器DSP作為數(shù)據(jù)處理單元進(jìn)行控制的輪式自主機(jī)器人。根據(jù)實(shí)驗(yàn)?zāi)Ｐ?，機(jī)器人上同時(shí)安裝了內(nèi)部傳感器光電編碼器、陀螺儀和外部環(huán)境傳感器激光雷達(dá)。激光雷達(dá)采用SICK LMS200，該傳感器能對(duì)0～180°范圍進(jìn)行掃描，返回環(huán)境測(cè)量點(diǎn)距離信息和角度信息。

如圖4所示，實(shí)驗(yàn)在走廊通道中進(jìn)行，建立機(jī)器人運(yùn)動(dòng)環(huán)境地圖。在O點(diǎn)上建立環(huán)境坐標(biāo)系，路徑規(guī)劃?rùn)C(jī)器人由起始位置A移動(dòng)到終點(diǎn)C，行走距離20 m。機(jī)器人移動(dòng)速度為1m/s，采樣時(shí)間控制為0.5 s，估計(jì)初始置 A（x，y，θ）為（0，0，0）。為驗(yàn)正算法收斂性，實(shí)際A一定程度偏離原點(diǎn)。

機(jī)器人移動(dòng)過(guò)程中采用上述擴(kuò)展卡爾曼濾波多傳感融合算法進(jìn)行定位。在B點(diǎn)給出機(jī)器人位置預(yù)測(cè)（細(xì)線）與測(cè)量（粗線），以及修正融合后得到的機(jī)器人位置更新估計(jì)（粗虛線）。機(jī)器的位置預(yù)測(cè)、測(cè)量及最新估計(jì)都存在不確定性，因此機(jī)器人的位置以橢圓表示。機(jī)器人預(yù)測(cè)與觀測(cè)的特征匹配中采用Mahalanobis距離[8]進(jìn)行判斷，找出相應(yīng)匹配。機(jī)器人采用線性控制算法進(jìn)行路徑跟蹤。

圖4 機(jī)器人行走路徑

圖5 實(shí)驗(yàn)中機(jī)器人行走路線圖

圖5給出實(shí)驗(yàn)中機(jī)器人完整行走路線圖。機(jī)器人按要求達(dá)到目的地，每個(gè)位置的定位均方差在2 cm左右，航向角小于0.5rad。實(shí)驗(yàn)結(jié)果表明，上述多傳感融合系統(tǒng)的對(duì)機(jī)器人的導(dǎo)航起到改進(jìn)作用。

6 結(jié)束語(yǔ)

采用擴(kuò)展卡爾曼濾波算法作為機(jī)器人移動(dòng)中的定位方法，克服了以往單一傳感器或多傳感系統(tǒng)系統(tǒng)誤差表達(dá)方式的缺點(diǎn)。將該定位方法應(yīng)用到實(shí)際機(jī)器人導(dǎo)航定位當(dāng)中，能有效提高機(jī)器人定位精度，提高機(jī)器人定位的可靠性，具有一定的實(shí)際應(yīng)用。

[1] 何子述，夏威.現(xiàn)代數(shù)字信號(hào)處理及其應(yīng)用[M].北京：清華大學(xué)出版社，2009：250-350.

[2]吳偉，劉興剛，王忠實(shí).多傳感器融合實(shí)現(xiàn)機(jī)器人精確定位[J].東北大學(xué)學(xué)報(bào)，2007，28（2）：161-164.

[3]楊柏勝，姬紅兵.基于卡爾曼濾波的被動(dòng)多傳器融合跟蹤[J].控制與決策，2008，23（4）：460-463.

[4] 楊國(guó)勝.基于多傳器融合的移動(dòng)機(jī)器人定位與導(dǎo)航研究[D].北京：中國(guó)科學(xué)院自動(dòng)化研究所，2004.

[5] 徐立.基于信息融合的機(jī)器人定位及地圖創(chuàng)建研究[D].綿陽(yáng)：西南科技大學(xué)，2009.

[6] 孫鐵波，劉奎武，李宏.基于模糊控制的機(jī)器人尋線控制系統(tǒng)改進(jìn)設(shè)計(jì)[J].電子設(shè)計(jì)工程，2009，17（11）：32-33.

[7] Siegwart R，Nourbakhsh I R.自主移動(dòng)機(jī)器人導(dǎo)論[M].西安：西安交通大學(xué)出版社，2006：225-300.

[8] Jetto L.Development and experimental validation of an adaptive extended kalman filter for the localization of mobile robots[J].IEEE Trans.Robotics and Automation，1999，15（2）：219-229.