李世云， 關(guān) 樂， 褚金奎

(大連理工大學 精密特種加工教育部重點實驗室，遼寧 大連 116023)

基于光流傳感器的移動機器人定位方法*

李世云， 關(guān) 樂， 褚金奎

(大連理工大學精密特種加工教育部重點實驗室，遼寧大連116023)

詳細介紹了光流傳感器移動機器人定位系統(tǒng)的運動學原理，給出了理論模型推導(dǎo)；基于理論模型，搭建了基于光流傳感器實驗裝置，將3只傳感器置于移動機器人的固定位置，通過運動學幾何關(guān)系，解算機器人的位姿；在室內(nèi)進行了移動機器人的導(dǎo)航實驗，推算了機器人的預(yù)測軌跡。實驗證明了理論的可行性，可以作為短距離定位的一種導(dǎo)航方案。

光流傳感器； 移動機器人； 航跡推算

0 引 言

隨著智能化時代的到來，移動機器人在工業(yè)、農(nóng)業(yè)、醫(yī)療、服務(wù)等領(lǐng)域的應(yīng)用越來越廣泛，在其應(yīng)用過程中最重要的就是要讓機器人隨時明確位置和姿態(tài)，否則無法正確完成其他工作，因此，移動機器人的定位研究成為長期以來的研究熱點[1,2]。

定位是確定移動機器人在二維環(huán)境中相對于世界坐標系的位置及其本身的姿態(tài)，是移動機器人導(dǎo)航的最基本的環(huán)節(jié)[3]。其中，航跡推算是一種廣泛應(yīng)用的直接進行移動機器人定位的方式，優(yōu)點在于不用依靠外部參照物，通常使用的航跡推算方法是使用安裝在驅(qū)動輪上的旋轉(zhuǎn)光電編碼器來測量旋轉(zhuǎn)角度，再利用機器人的機械特性計算出機器人距離出發(fā)點的距離和方位，從而得到機器人的位置信息。由于存在主動輪打滑、轉(zhuǎn)向、反沖等因素的影響，導(dǎo)致此方法在定位中存在較大的誤差。

光流傳感器作為一種新型CMOS圖像傳感器，將圖像采集模塊采集的連續(xù)圖像信息轉(zhuǎn)化為傳感器自身的二維速度信息，目前主要應(yīng)用于機器人的避障、懸停、定位等方面。

本文提出了一種基于光流傳感器的新型移動機器人的定位的方法，具有定位精度高、可以有效避免主動打滑、轉(zhuǎn)向、反沖等因素導(dǎo)致的誤差。

1 光流傳感器原理

光流是指時變圖像中模式運動速度。生物學家研究發(fā)現(xiàn)蜜蜂等許多昆蟲可以利用光流信息用來導(dǎo)航?；诜律鷮W原理，國內(nèi)外許多學者研制了光流傳感器并應(yīng)用于移動導(dǎo)航[4～7]。如圖1所示，采用PX4FLOW光流傳感器[8]，傳感器主要由752480MT9V034CMOS圖像傳感器、16mm M12鏡頭、L3GD20三軸陀螺、Maxbotix超聲波傳感器和STM32F405ARM Cortex—M4處理器5個部分組成。

圖1 光流傳感器

光流傳感器采用最小絕對誤差和(SAD)塊匹配算法，計算步驟如下：

1)圖像傳感器采集連續(xù)2幀圖像，采用SAD算法進行最佳匹配

(1)

式中x,y分別為后一幀圖像相對于參考幀圖像匹配塊的水平、垂直兩個方向的偏移量；i,j分別為當前塊內(nèi)某像素點的位置；S為當前塊某像素的灰度值；R為匹配幀對應(yīng)像素的灰度值；d為最大的搜索距離。選擇搜索區(qū)域內(nèi)的SAD最小值作為最佳匹配。

2)雙線性插值亞像素化：設(shè)非整像素點(x，y)在鄰域的4個像素點(i，j),(i+1，j),(i，j+1),(i+1，j+1),亞像素點的插值為

g(x,y)=A00+A10α+A01β+A11αβ

(2)

式中A00=g(i，j)；A10=g(i+1，j)-g(i，j)；A01=g(i，j+1)-g(i，j)；A11=g(i+1，j+1)+g(i，j)-g(i，j+1)-g(i+1)；α，β分別為點(x,y)到點(i，j)距離的水平垂直分量。計算出最佳匹配塊的亞像素移動量。

3)旋轉(zhuǎn)補償：為了消除由于CMOS鏡頭自身的旋轉(zhuǎn)產(chǎn)生的光流，通過自帶陀螺儀所測得的旋轉(zhuǎn)角速度ω和透鏡的焦距f代入公式(3)、式(4)中，對光流值速度補償，分別得到圖像平面的光流分量vx，vy如下

(3)

(4)

式中T=[Tx,Ty,Tz]為平移變換矩陣；Z為高度；x，y分別為x，y方向的像素偏移量。

4)坐標系變換：將超聲波傳感器提供的高度信息Z和光流信息vx,vy代入式(5)，根據(jù)投影關(guān)系計算出傳感器相對于地面的水平增量Vx,Vy如下

(5)

2 多光流傳感器定位運動學分析

如圖2所示，設(shè)世界坐標系、機器人坐標系和傳感器坐標系分別為OA-XAYA,OR-XRYR,OS-XSYS，其中機器人坐標系的的原點為機器人兩個驅(qū)動輪的幾何中心，機器人坐標系與世界坐標系夾角為θ，傳感器的夾角與機器人坐標系的夾角為Φ。傳感器i到機器人幾何中心距離為Ri。

圖2 機器人與傳感器的坐標關(guān)系

傳感器和機器人的相對位置在世界坐標系下滿足

(6)

對式(6)微分整理得

(7)

(8)

整理式(8)，得到

Au=b

(9)

其中

b=R(θ+φ)ΔS

由式(9)可以得知，方程的個數(shù)少于未知數(shù)的個數(shù)，方程不可解，因此，必須有2只及以上的傳感器方程才可解。如果有兩個傳感器，其中,矩陣A和b為

(10)

則機器人在世界坐標系中的增量

u=A-1b

(11)

設(shè)某時刻移動機器的當前坐標為[Xt,Yt,θt]，則下一時刻的位置坐標為

(12)

3 系統(tǒng)方案與實驗

3.1 硬件系統(tǒng)

為了驗證上述方案的可行性，采用自主搭建的移動機器人[9]作為載體，搭建了實驗平臺。機器人平臺采用了四輪并且對稱的驅(qū)動底盤，左右輪為驅(qū)動輪，前后輪為萬向輪，將3個光流傳感器安裝于機器人平臺，推算得到機器人的運動軌跡。光流傳感器的數(shù)據(jù)采集模塊如圖3所示，微控制器(CY7C68013A—56P)通過輸入/輸出(input/output,I/O)口讀取傳感器的數(shù)據(jù)。為了保證傳感器數(shù)據(jù)的同步，3只傳感器共用一根數(shù)據(jù)線，微處理器通過USB口將數(shù)據(jù)塊傳輸至電腦。

圖3 數(shù)據(jù)采集模塊

數(shù)據(jù)采集的微處理器采用美國賽普拉斯公司(Cypress)的EZ—USB FX2LP高速USB 2.0接口芯片CY7C68013A—56P，具有和8051兼容的CPU和指令系統(tǒng)，同時包含USB接口和完整的USB 2.0協(xié)議引擎，并提供了完整的固件及主機程序開發(fā)包。該微處理器采用增強型8051內(nèi)核，硬件資源更豐富，功能更強大。

3.2 實驗數(shù)據(jù)

為了驗證理論模型的可行性，增強系統(tǒng)的魯棒性，搭建了基于三傳感器的實驗裝置，并完成了室內(nèi)的軌跡推算實驗。將3只傳感器放置在距離機器人幾何中心Ri=350 mm，ψi=60°，120°，270°處，并且傳感器的鏡頭面向地面。按照表1的路徑坐標進行室內(nèi)機器人的路徑實驗，并由傳感器推算出機器人的航跡。

表1 機器人預(yù)設(shè)坐標

將電腦接收的傳感器數(shù)據(jù)，通過數(shù)據(jù)處理得到移動機器人的推算軌跡如圖4所示。

圖4 機器人的預(yù)設(shè)軌跡和推算軌跡

可知，短距離內(nèi)，本文提出的方法可以得到具有較高精度的推算軌跡，可以滿足移動機器人定位導(dǎo)航要求。

4 結(jié) 論

本文提出了一種利用光流傳感器實現(xiàn)移動機器人室內(nèi)外導(dǎo)航定位方法，并且搭建了硬件平臺和數(shù)據(jù)采集模塊，通過室內(nèi)實驗驗證了該定位系統(tǒng)的可行性，可以作為一種短距離定位的導(dǎo)航方案。下一步的研究需要對定位系統(tǒng)進行標定，誤差分析，提高裝置的導(dǎo)航精度。

[1] Kim S,Jeong I,Lee S.Mobile robot velocity estimation using a regular polygonal array of optical flow sensors[M].Berlin Heidelberg:Springer,2007:641-649.

[2] 唐 琎,白 濤,蔡自興.基于光電鼠標的移動機器人室內(nèi)定位方法[J].微計算機信息,2005,21(5):20-21.

[3] 于金霞.未知環(huán)境中移動機器人自定位技術(shù)[M].北京：電子工業(yè)出版社，2001.

[4] Dahmen H,Mallot H A.Odometry for ground moving agents by optic flow recorded with optical mouse chips[J].Sensors,2014,14(11):21045-21064.

[5] 呂 強,倪佩佩,王國勝,等.基于光流傳感器的四旋翼飛行器懸停校正[J].裝甲兵工程學院學報,2014(3):68-72.

[6] 宋 宇,翁新武,郭昕剛.基于光流和慣性導(dǎo)航的小型無人機定位方法[J].傳感器與微系統(tǒng),2015,34(1):13-16.

[7] Unhelkar V V,Perez J,Boerkoel J C,et al.Towards control and sensing for an autonomous mobile robotic assistant navigating assembly lines[C]∥2014 IEEE International Conference on Robo-tics and Automation(ICRA),IEEE,2014:4161-4167.

[8] Honegger D,Meier L,Tanskanen P,et al.An open source and open hardware embedded metric optical flow CMOS camera for indoor and outdoor applications[C]∥2013 IEEE International Conference on Robotics and Automation(ICRA),IEEE,2013:1736-1741.

[9] 戎成功.基于ARM和DSP的移動機器人控制系統(tǒng)的搭建[D].大連：大連理工大學,2012:23-24.

Dead-reckoningformobilerobotbasedonmultipleopticalflowsensors*

LI Shi-yun, GUAN Le, CHU Jin-kui

(KeyLaboratoryofTechnologyPrecision&Non-traditionalMachiningofMinistryofEducation,DalianUniversityofTechnology,Dalian116023,China)

Kinematics principle of flow sensor positioning system is introduced in detail; theoretical model derivation is given; based on theoretical model, experimental device based on optical flow sensor is set up. Three sensors are set in fixed position of mobile robot. Through Kinematics geometry relation, pose of robot is resolved. Navigation experiment of mobile robot is carried out in indoor, predicted trajectory of robot is reckoned. Feasibility of theory is proved by experiment, and it can be a navigation scheme for short-distance positioning.

optical flow sensors; mobile robot; dead-reckoning

10.13873/J.1000—9787(2017)10—0061—03

2016—03—22

國家自然科學基金資助項目(51305057)； 國家重點基礎(chǔ)研究發(fā)展計劃資助項目(2011CB302105)

TP 212

A

1000—9787(2017)10—0061—03

李世云(1986- )，男，碩士研究生，研究方向為機器人導(dǎo)航定位、嵌入式開發(fā)。