陳 朋，陳偉海，于守謙

（北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京 100191）

0 引 言

移動機器人定位問題是移動機器人研究領(lǐng)域的熱點，是提高機器人自主能力的關(guān)鍵問題之一；同時，移動機器人的準(zhǔn)確定位還是機器人進(jìn)行導(dǎo)航、避障和追蹤的重要前提[1]。具體來說，機器人定位就是根據(jù)機器人當(dāng)前位置信息的估計、所處環(huán)境信息以及傳感器的觀測值等輸入信息，經(jīng)過一定的處理和變換，產(chǎn)生更加準(zhǔn)確的對機器人當(dāng)前位姿的估計。

針對移動機器人定位問題，國內(nèi)外學(xué)者在定位方法上作了大量的研究，如陳小寧等[2]利用里程計和超聲波進(jìn)行移動機器人的定位、黃玉清等[3]提出基于光電尋線的定位方法、Drocout等[4]提出基于全景視覺的定位等，可以看出機器人的定位方式主要取決于所采用的傳感器。目前，在移動機器人上使用較多的傳感器有視覺傳感器、慣導(dǎo)系統(tǒng)、超聲傳感器、激光測距儀、GPS定位系統(tǒng)等，但這些傳感器大多價格比較昂貴，且數(shù)據(jù)處理起來比較復(fù)雜。

該文討論了室內(nèi)輪式移動機器人的定位，開發(fā)了單一采用FPGA為主控芯片的信息采集和數(shù)據(jù)處理系統(tǒng)，構(gòu)建了機器人的狀態(tài)方程以及傳感器的觀測方程。機器人的定位坐標(biāo)采用航姿推算法（DR）推算得到，最后使用無味卡爾曼濾波（unscented Kalman filter，UKF）對陀螺儀、編碼器和電子羅盤的數(shù)據(jù)進(jìn)行融合，從而得到機器人更為精確的定位信息。

1 移動機器人的定位系統(tǒng)

1.1 定位系統(tǒng)的構(gòu)成

整個系統(tǒng)單一采用FPGA作為主控制器，傳感器數(shù)據(jù)的采集和處理都是集成在這一片F(xiàn)PGA中，從而降低了系統(tǒng)的冗余度，提高了系統(tǒng)的穩(wěn)定性。搭建了包括陀螺儀、編碼器和電子羅盤的傳感器數(shù)據(jù)采集硬件系統(tǒng)，F(xiàn)PGA負(fù)責(zé)并行采集各傳感器數(shù)據(jù)，同時進(jìn)行數(shù)據(jù)處理，得到機器人的行走距離和偏移角度。最后通過在FPGA上開發(fā)基于UKF的定位算法，得到更為精確的機器人位姿。系統(tǒng)結(jié)構(gòu)圖如圖1所示。

圖1 定位系統(tǒng)的結(jié)構(gòu)圖

1.2 機器人的運動模型

研究的是四輪自主移動機器人，它是四輪驅(qū)動的Pioneer3-AT智能小車，自身裝有電子羅盤傳感器，實物圖見圖2。為了獲得機器人的狀態(tài)方程，建立如圖3所示的簡化運動模型。

圖2 輪式自主移動機器人

首先建立平面全局坐標(biāo)系，坐標(biāo)原點O取在機器人后輪驅(qū)動軸中心位置，并且設(shè)該點始終為機器人的運動的初始位置。機器人開始運動時的正前方為Y軸正方向，順時針旋轉(zhuǎn)90°即為X軸正方向。假設(shè)機器人從O點運動到A點，相對初始位置的偏移角度為θ（以X正方向軸為起始位置并以逆時針方向為正），則機器人在平面中的位置可描述為（x，y，θ）T。

圖3 機器人簡化運動模型

1.3航姿推算法

移動機器人采用的定位算法為航姿推算法。航姿推算法是一種常用的自主式機器人定位技術(shù)，在該方法中姿態(tài)角和距離傳感器被用來測量偏移角度和行走距離，經(jīng)過不斷累積計算得到定位信息。此類方法成本較低，可靠性較高，對外部環(huán)境無特殊要求，可在一段時間內(nèi)獲得較高的定位精度。

在實際情況中，機器人不可能按照嚴(yán)格的直線行走，因此采用微元近似，將機器人在很短時間（軟件內(nèi)設(shè)置特定的中斷時間）內(nèi)所走的圓弧段近似為直線，如圖4所示。假定機器人在一定的中斷時間內(nèi)從點Ak（x，y，θ）沿著圓弧段走到點Ak+1（x+Δx，y+Δy，θ+Δθ），θ為點Ak相對機器人初始位置的偏移角度，R為圓弧段的半徑。

圖4 航姿推算法示意圖

設(shè)圓弧段AkAk+1的長度為Δl，兩點間的直線距離為Δs，當(dāng)Δl與Δs近似相等時，則可得

所以航姿推算法下機器人坐標(biāo)的累加表達(dá)式為

1.4 機器人狀態(tài)方程

設(shè)系統(tǒng)的控制變量為 U=（Δs，Δθ），將方程式（3）離散化之后，再加上模型噪聲，得到機器人的狀態(tài)方程為

式中：Wk——零均值的高斯白噪聲，設(shè)其協(xié)方差矩陣為Q。

2 傳感器觀測模型的建立

2.1 陀螺儀

系統(tǒng)選用的陀螺儀是美國BEI公司生產(chǎn)的單軸HZ1-90-100A型微機械陀螺儀，其測量精度高，性能穩(wěn)定，能夠滿足最終所需的機器人定位精度要求。它的輸出量為角速度的模擬電壓值，因此首先需要進(jìn)行模數(shù)轉(zhuǎn)換，得到角速度輸出的數(shù)字量。陀螺儀使用中的主要問題是零位電壓的穩(wěn)定性問題，即零位漂移。因此在計算角度之前，必須先求得陀螺儀的零漂值。該文采用的是均值濾波算法來求解零漂值，之后用每次的采樣值減去零漂值，然后設(shè)置定時中斷來對此角速率的差值進(jìn)行數(shù)值積分即可得到中斷時間內(nèi)變化的角度值，即Δθ。程序中采用5ms定時中斷來計算角度，如式（5）

式中：data——角速度的AD采樣值；

zero——零漂值；

n——5ms內(nèi)采樣得到的data的總個數(shù)；

20mv/（°/s）——陀螺儀的比例因子。

陀螺儀在使用過程中的主要測量誤差是由陀螺儀的零位漂移造成的，陀螺儀的數(shù)學(xué)模型為

式中：ωm——測量所得機器人的角速度；

ωr——機器人實際的角速度；

η——陀螺儀的漂移誤差，主要包括自身的偏置漂移和不相關(guān)的漂移2部分。

針對系統(tǒng)中所用的這一型號的陀螺儀，其偏置漂移在短時間之內(nèi)可以忽略不計，由此η可以例化為零均值的高斯白噪聲（0，σ2η），σ2η為誤差 η 的協(xié)方差，則陀螺儀的觀測方程為

2.2 編碼器

系統(tǒng)中選用的編碼器是歐姆龍公司生產(chǎn)的E 6B2-CWZ6C增量型編碼器，其分辨率為1000P/R。編碼器信息的采集就是對其輸出脈沖的計數(shù)，整個模塊包括鑒相、計數(shù)、鎖存和距離計算4部分。信息處理流程如圖5所示。

圖5 編碼器信息采集流程圖

編碼器的觀測是通過輸出的脈沖數(shù)獲得的，其數(shù)學(xué)模型為

式中：N——編碼器的線數(shù)；

D——機器人從動輪的周長；

m——計數(shù)器計得的編碼器輸出脈沖；

λ——編碼器測量誤差。

將編碼器測量誤差例化為零均值的高斯白噪聲（0，σ2λ），σ2λ為誤差 λ 的誤差協(xié)方差。所以編碼器的觀測方程為

2.3 電子羅盤

系統(tǒng)中使用的電子羅盤是移動機器人自帶的傳感器，它的測量精度高，最大量程可達(dá)到300°/s。其上預(yù)留有與外設(shè)進(jìn)行通信的接口，因此在機器人運動過程中可以實時地直接讀取電子羅盤的輸出數(shù)據(jù)。

電子羅盤的數(shù)學(xué)模型可以描述為

式中：θm——測量所得的機器人偏移角；

θr——機器人實際的偏移角；

μ——電子羅盤的測量誤差。

可將電子羅盤的測量誤差例化為零均值的高斯白噪聲（0，σ2μ），σ2μ為誤差 μ 的誤差協(xié)方差。由此可得電子羅盤的觀測方程為

2.4 傳感器觀測方程

由各傳感器的觀測方程式（7），式（9）和式（11），可得整個系統(tǒng)連續(xù)的觀測方程為

對式（12）進(jìn)行離散化處理，得到系統(tǒng)離散的觀測方程為

式中：Vk——零均值的高斯白噪聲，設(shè)其協(xié)方差矩陣為R。

3 基于UKF的自定位算法

當(dāng)針對一個非線性系統(tǒng)進(jìn)行參數(shù)估計和數(shù)據(jù)融合時，擴展卡爾曼濾波（EKF）是常用的算法。它利用泰勒級數(shù)將非線性方程進(jìn)行線性化，然后再遵循經(jīng)典的卡爾曼濾波算法對系統(tǒng)狀態(tài)進(jìn)行估計。但此算法有2個明顯的缺陷：（1）它必須滿足這樣的假設(shè)，即假設(shè)非線性方程的理論解與實際解之間的誤差為小量，也就是說EKF只滿足于弱非線性系統(tǒng)，而對于強非線性系統(tǒng)則很難穩(wěn)定，甚至發(fā)散；（2）EKF必須計算雅可比矩陣及其冪，而這在許多實際系統(tǒng)中是難以計算得到的[5-6]。UKF摒棄了線性化的這一做法，它基于無跡變換，通過在原狀態(tài)中按某一規(guī)則選取一些點，由此來估計獲得隨機變量實際的均值和協(xié)方差；同時，UKF不必計算矩陣的雅可比矩陣，在一定程度上降低了算法的計算量，而且其精度至少可以達(dá)到二階。因此現(xiàn)在UKF越來越多地被用在非線性系統(tǒng)的估計之中[7-9]。

鑒于系統(tǒng)的狀態(tài)方程和觀測方程皆為非線性，選取UKF進(jìn)行估計和傳感器數(shù)據(jù)的融合，具體算法如下：

（1）初始化計算

擴展后的初始狀態(tài)向量

協(xié)方差矩陣的初始值

（2）計算σ點及其權(quán)值

當(dāng)k≥1時：

計算σ的權(quán)值式中0表示初始時刻，k表示當(dāng)前時刻，w表示權(quán)重，其上標(biāo)m和c分別表示均值與方差；χ表示一實數(shù)值且可描述為 χ=α（2n+δ）-n，其中 δ表示次要的尺度參數(shù)，其值通常取作零；式中的α決定σ點的分散程度，通常取一個較小的正值（例如0.1或0.01等），而β用來描述采樣得到的σ點的分布信息（正態(tài)分布情況之下的最優(yōu)解通常為2）。而對于（求解，由于不同的平方根方法不會影響通過σ點采樣得到的均值和方差，因此可以采用魯棒性強的cholesky分解法進(jìn)行求解。根據(jù)協(xié)方差矩陣P不同i的的分解形式取相應(yīng)分解矩陣的第i行或第i列的平方根值。

（3）系統(tǒng)狀態(tài)變量的估計，即時間的更新

系統(tǒng)的狀態(tài)變量可以通過系統(tǒng)模型進(jìn)行估計，系統(tǒng)的先驗狀態(tài)為

計算先驗的協(xié)方差矩陣，即向前推算誤差協(xié)方差

（4）觀測變量的估計，即測量更新

根據(jù)系統(tǒng)的觀測模型，對測量變量進(jìn)行估計，首先是系統(tǒng)觀測方程的更新

相應(yīng)的協(xié)方差矩陣為

（5）位姿的更新

計算卡爾曼增益

由觀測變量更新估計

更新誤差協(xié)方差

按此步驟逐步更新計算，則可得到較為精確的機器人狀態(tài)變量，即平面坐標(biāo)值。

4實 驗

將選用的傳感器正確安裝在移動機器人上，并將所有的軟件算法固化到作為主控芯片的FPGA中，進(jìn)而通過實驗驗證算法的正確性以及整個系統(tǒng)的定位精度。

機器人在一個較為平坦的平面上運動，將其運動的起點作為平面坐標(biāo)系的坐標(biāo)原點，即設(shè)機器人的初始位姿為（0 mm，0 mm，0 rad），而機器人在運動過程中的坐標(biāo)可以通過控制板上的數(shù)碼管顯示出來。

機器人采用以下2種方式運動：

（1）做勻速運動。

（2）在平面內(nèi)做變加速度的變速運動，同時伴有旋轉(zhuǎn)運動。

采用多次試驗的方法，求取誤差的最大值以及平均值。為了比較UKF使用前后的誤差大小，分別給出了UKF使用前后的實驗結(jié)果，數(shù)據(jù)見表1和表2。

從實驗結(jié)果可以看出，使用UKF算法之后，定位精度得到了明顯提高。而且，機器人勻速運動時的誤差明顯小于變速運動，主要原因在于機器人做變速運動時傳感器自身的測量誤差增大，尤其是編碼器的滑動誤差變大。

表1 機器人勻速運動實驗結(jié)果

表2 機器人變速運動實驗結(jié)果

5 結(jié)束語

該文采用無味卡爾曼濾波算法融合了陀螺儀、編碼器和電子羅盤的信息，實現(xiàn)了移動機器人的自主定位。當(dāng)然，從實驗所得的誤差數(shù)據(jù)可以看出，相對于機器人機體自身的空間大小，誤差是在可接受的范圍之內(nèi)。但由于將系統(tǒng)的誤差都例化為高斯白噪聲，這也在一定程度上影響了UKF的性能。

單一的這種定位方式，誤差會隨著時間逐漸累加，甚至于失去了定位作用。因此考慮后期在機器人上安裝GPS模塊，采用絕對定位與相對定位相結(jié)合的方式，以期進(jìn)一步提高系統(tǒng)的定位精度。

[1] Woo J，Kim Y J，Lee J，et al.Localization of mobile robot using particle filter[C]∥SICE-ICASE International Joint Conference.Busan，2006：3031-3034.

[2] 陳小寧，黃玉清，楊佳.多傳感器信息融合在移動機器人定位中的應(yīng)用[J].傳感器與微系統(tǒng)，2008，27（6）：110-113.

[3] 黃玉清，張江梅，張華.模糊控制在移動機器人中的應(yīng)用[J].西南科技大學(xué)學(xué)報，2004，19（1）：22-25.

[4] Drocout C，Delahoche L，Pegard C.Mobile robot localization based on an omnidirectionalstereoscopic vision perception system[J].Robotics and Automation，1999（2）：1322-1328.

[5] 蔡鶴皋，金明河，金峰.卡爾曼濾波與多傳感器數(shù)據(jù)融合技術(shù)[J].模式識別與人工智能，2000，13（2）：248-253.

[6] 宋驪平，姬紅兵.多被動傳感器UKF與EKF算法的應(yīng)用與比較[J].系統(tǒng)工程與電子技術(shù)，2009，31（5）：1083-1086.

[7] Jetto L，Longhi S，Venturini G.Developmentand experimental validation of an adaptive extended kalman filter for the localization of mobile robots[J].IEEE Transactions on Robotics and Automation，2006，15（2）：219-229.

[8] Bao W，Zhang C W，Xiao B X，et al.Self-location of mobile robot based on binocular camera and unscented kalman filter[C]∥International Conference on Automation and Logistics：Jinan，2007：277-281.

[9]Kwok N M，Liu D K，Dissanayake G.Evolutionary computing based mobile robot localization [J].Engineering Applications of Arti.cial Intelligence，2006，19（8）：857-868.