基于UKF的室內移動機器人定位技術研究

陳 朋，陳偉海，于守謙

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

0 引 言

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

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

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

1 移動機器人的定位系統

1.1 定位系統的構成

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

圖1 定位系統的結構圖

1.2 機器人的運動模型

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

圖2 輪式自主移動機器人

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

圖3 機器人簡化運動模型

1.3航姿推算法

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

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

圖4 航姿推算法示意圖

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

所以航姿推算法下機器人坐標的累加表達式為

1.4 機器人狀態方程

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

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

2 傳感器觀測模型的建立

2.1 陀螺儀

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

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

zero——零漂值；

n——5ms內采樣得到的data的總個數；

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

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

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

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

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

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

2.2 編碼器

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

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

編碼器的觀測是通過輸出的脈沖數獲得的，其數學模型為

式中：N——編碼器的線數；

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

m——計數器計得的編碼器輸出脈沖；

λ——編碼器測量誤差。

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

2.3 電子羅盤

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

電子羅盤的數學模型可以描述為

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

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

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

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

2.4 傳感器觀測方程

由各傳感器的觀測方程式（7），式（9）和式（11），可得整個系統連續的觀測方程為

對式（12）進行離散化處理，得到系統離散的觀測方程為

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

3 基于UKF的自定位算法

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

鑒于系統的狀態方程和觀測方程皆為非線性，選取UKF進行估計和傳感器數據的融合，具體算法如下：

（1）初始化計算

擴展后的初始狀態向量

協方差矩陣的初始值

（2）計算σ點及其權值

當k≥1時：

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

（3）系統狀態變量的估計，即時間的更新

系統的狀態變量可以通過系統模型進行估計，系統的先驗狀態為

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

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

根據系統的觀測模型，對測量變量進行估計，首先是系統觀測方程的更新

相應的協方差矩陣為

（5）位姿的更新

計算卡爾曼增益

由觀測變量更新估計

更新誤差協方差

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

4實 驗

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

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

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

（1）做勻速運動。

（2）在平面內做變加速度的變速運動，同時伴有旋轉運動。

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

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

表1 機器人勻速運動實驗結果

表2 機器人變速運動實驗結果

5 結束語

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

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

[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] 陳小寧，黃玉清，楊佳.多傳感器信息融合在移動機器人定位中的應用[J].傳感器與微系統，2008，27（6）：110-113.

[3] 黃玉清，張江梅，張華.模糊控制在移動機器人中的應用[J].西南科技大學學報，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] 蔡鶴皋，金明河，金峰.卡爾曼濾波與多傳感器數據融合技術[J].模式識別與人工智能，2000，13（2）：248-253.

[6] 宋驪平，姬紅兵.多被動傳感器UKF與EKF算法的應用與比較[J].系統工程與電子技術，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.