陳 朋，陳偉海，于守謙

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

0 引 言

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

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

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

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

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

整個(gè)系統(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ù)，同時(shí)進(jìn)行數(shù)據(jù)處理，得到機(jī)器人的行走距離和偏移角度。最后通過在FPGA上開發(fā)基于UKF的定位算法，得到更為精確的機(jī)器人位姿。系統(tǒng)結(jié)構(gòu)圖如圖1所示。

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

1.2 機(jī)器人的運(yùn)動(dòng)模型

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

圖2 輪式自主移動(dòng)機(jī)器人

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

圖3 機(jī)器人簡(jiǎn)化運(yùn)動(dòng)模型

1.3航姿推算法

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

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

圖4 航姿推算法示意圖

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

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

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

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

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

2 傳感器觀測(cè)模型的建立

2.1 陀螺儀

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

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

zero——零漂值；

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

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

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

式中：ωm——測(cè)量所得機(jī)器人的角速度；

ωr——機(jī)器人實(shí)際的角速度；

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

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

2.2 編碼器

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

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

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

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

D——機(jī)器人從動(dòng)輪的周長(zhǎng)；

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

λ——編碼器測(cè)量誤差。

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

2.3 電子羅盤

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

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

式中：θm——測(cè)量所得的機(jī)器人偏移角；

θr——機(jī)器人實(shí)際的偏移角；

μ——電子羅盤的測(cè)量誤差。

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

2.4 傳感器觀測(cè)方程

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

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

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

3 基于UKF的自定位算法

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

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

（1）初始化計(jì)算

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

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

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

當(dāng)k≥1時(shí)：

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

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

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

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

（4）觀測(cè)變量的估計(jì)，即測(cè)量更新

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

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

（5）位姿的更新

計(jì)算卡爾曼增益

由觀測(cè)變量更新估計(jì)

更新誤差協(xié)方差

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

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

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

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

機(jī)器人采用以下2種方式運(yùn)動(dòng)：

（1）做勻速運(yùn)動(dòng)。

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

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

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

表1 機(jī)器人勻速運(yùn)動(dòng)實(shí)驗(yàn)結(jié)果

表2 機(jī)器人變速運(yùn)動(dòng)實(shí)驗(yàn)結(jié)果

5 結(jié)束語

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

單一的這種定位方式，誤差會(huì)隨著時(shí)間逐漸累加，甚至于失去了定位作用。因此考慮后期在機(jī)器人上安裝GPS模塊，采用絕對(duì)定位與相對(duì)定位相結(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] 陳小寧，黃玉清，楊佳.多傳感器信息融合在移動(dòng)機(jī)器人定位中的應(yīng)用[J].傳感器與微系統(tǒng)，2008，27（6）：110-113.

[3] 黃玉清，張江梅，張華.模糊控制在移動(dòng)機(jī)器人中的應(yīng)用[J].西南科技大學(xué)學(xué)報(bào)，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].模式識(shí)別與人工智能，2000，13（2）：248-253.

[6] 宋驪平，姬紅兵.多被動(dòng)傳感器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.