基于光流傳感器的移動(dòng)機(jī)器人定位方法*

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

(大連理工大學(xué) 精密特種加工教育部重點(diǎn)實(shí)驗(yàn)室，遼寧 大連 116023)

基于光流傳感器的移動(dòng)機(jī)器人定位方法*

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

(大連理工大學(xué)精密特種加工教育部重點(diǎn)實(shí)驗(yàn)室，遼寧大連116023)

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

光流傳感器； 移動(dòng)機(jī)器人； 航跡推算

0 引 言

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

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

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

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

1 光流傳感器原理

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

圖1 光流傳感器

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

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

(1)

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

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

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)；α，β分別為點(diǎn)(x,y)到點(diǎn)(i，j)距離的水平垂直分量。計(jì)算出最佳匹配塊的亞像素移動(dòng)量。

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

(3)

(4)

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

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

(5)

2 多光流傳感器定位運(yùn)動(dòng)學(xué)分析

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

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

傳感器和機(jī)器人的相對(duì)位置在世界坐標(biāo)系下滿足

(6)

對(duì)式(6)微分整理得

(7)

(8)

整理式(8)，得到

Au=b

(9)

其中

b=R(θ+φ)ΔS

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

(10)

則機(jī)器人在世界坐標(biāo)系中的增量

u=A-1b

(11)

設(shè)某時(shí)刻移動(dòng)機(jī)器的當(dāng)前坐標(biāo)為[Xt,Yt,θt]，則下一時(shí)刻的位置坐標(biāo)為

(12)

3 系統(tǒng)方案與實(shí)驗(yàn)

3.1 硬件系統(tǒng)

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

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

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

3.2 實(shí)驗(yàn)數(shù)據(jù)

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

表1 機(jī)器人預(yù)設(shè)坐標(biāo)

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

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

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

4 結(jié) 論

本文提出了一種利用光流傳感器實(shí)現(xiàn)移動(dòng)機(jī)器人室內(nèi)外導(dǎo)航定位方法，并且搭建了硬件平臺(tái)和數(shù)據(jù)采集模塊，通過室內(nèi)實(shí)驗(yàn)驗(yàn)證了該定位系統(tǒng)的可行性，可以作為一種短距離定位的導(dǎo)航方案。下一步的研究需要對(duì)定位系統(tǒng)進(jìn)行標(biāo)定，誤差分析，提高裝置的導(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] 唐 琎,白 濤,蔡自興.基于光電鼠標(biāo)的移動(dòng)機(jī)器人室內(nèi)定位方法[J].微計(jì)算機(jī)信息,2005,21(5):20-21.

[3] 于金霞.未知環(huán)境中移動(dòng)機(jī)器人自定位技術(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] 呂 強(qiáng),倪佩佩,王國(guó)勝,等.基于光流傳感器的四旋翼飛行器懸停校正[J].裝甲兵工程學(xué)院學(xué)報(bào),2014(3):68-72.

[6] 宋 宇,翁新武,郭昕剛.基于光流和慣性導(dǎo)航的小型無人機(jī)定位方法[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的移動(dòng)機(jī)器人控制系統(tǒng)的搭建[D].大連：大連理工大學(xué),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

國(guó)家自然科學(xué)基金資助項(xiàng)目(51305057)； 國(guó)家重點(diǎn)基礎(chǔ)研究發(fā)展計(jì)劃資助項(xiàng)目(2011CB302105)

TP 212

A

1000—9787(2017)10—0061—03

李世云(1986- )，男，碩士研究生，研究方向?yàn)闄C(jī)器人導(dǎo)航定位、嵌入式開發(fā)。