采用SLAM方法的電腦鼠

蘇 豪，黃心漢，王貞炎

(1. 華中科技大學(xué) 自動化學(xué)院，湖北 武漢 430074；2. 華中科技大學(xué) 電氣與電子工程學(xué)院，湖北 武漢 430074)

?

采用SLAM方法的電腦鼠

蘇 豪1，黃心漢1，王貞炎2

(1. 華中科技大學(xué) 自動化學(xué)院，湖北 武漢 430074；2. 華中科技大學(xué) 電氣與電子工程學(xué)院，湖北 武漢 430074)

與絕大部分采用紅外對管進行簡單測距的電腦鼠不同，在電腦鼠上使用線型激光器和攝像頭搭建了一個小型激光測距系統(tǒng)并進行SLAM。攝像頭以240 f/s的幀率輸出分辨率為188×120的圖像，使用Zynq-7010作為主控制器，在其可編程邏輯部分搭建了圖像采集系統(tǒng)，在其雙核處理器Cortex-A9部分搭建了用于運行圖像處理、傳感器數(shù)據(jù)融合和解迷宮算法的實時Linux操作系統(tǒng)。制作了一只重量為100.06 g、車體大小為7.45 cm(寬)×6.6 cm(長)的電腦鼠，能高速穩(wěn)定地在迷宮中運動。

電腦鼠；SLAM；激光測距；Zynq

0 引言

電腦鼠是一種典型的小型輪式移動機器人，國際IEEE電腦鼠走迷宮比賽規(guī)則要求其能在大小為180 mm×180 mm單元組成的迷宮中進行自主移動并求解迷宮。目前絕大部分的電腦鼠都是使用紅外對管在迷宮中進行測距定位[1-3]，然而使用紅外對管測距的方式需要針對不同的場地環(huán)境進行復(fù)雜的調(diào)整，導(dǎo)致使用這種方式的電腦鼠場地適應(yīng)性很差，限制了其使用范圍。

通過對外部環(huán)境進行識別和分析，創(chuàng)建地圖，同時利用地圖進行定位和導(dǎo)航，是移動機器人的同時定位與地圖創(chuàng)建(SLAM)問題[4]。由于電腦鼠體積較小，一般要求其體積小于90 mm×90 mm×3 mm，較大的體積不利于其在迷宮中高速運動，而較小的體積則不利于進行復(fù)雜的電路設(shè)計和機械構(gòu)造，因此在電腦鼠身上應(yīng)用SLAM具有很大難度。

隨著FPGA集成度和可編程邏輯資源數(shù)目的不斷提高，F(xiàn)PGA越來越廣泛地應(yīng)用于實時圖像處理領(lǐng)域[5]。本文使用高速成像芯片MT9V034和線型紅外激光器構(gòu)建了一個小型二維激光測距系統(tǒng)，使用Xilinx的Zynq-7010可擴展處理平臺對圖像數(shù)據(jù)進行實時處理，針對迷宮特點進行SLAM，應(yīng)用在電腦鼠上。

1 系統(tǒng)硬件結(jié)構(gòu)

1.1 整體系統(tǒng)結(jié)構(gòu)

系統(tǒng)由電源電路、傳感系統(tǒng)、主控制器部分和電機等機械結(jié)構(gòu)構(gòu)成。硬件系統(tǒng)結(jié)構(gòu)如圖1所示，主控制器部分包括Zynq-7010可擴展處理平臺、數(shù)據(jù)存儲單元DDR3和用于無線數(shù)據(jù)傳輸?shù)腟DIO WiFi模塊；傳感系統(tǒng)包括由高速成像芯片MT9V034和線型紅外激光器構(gòu)成的激光測距系統(tǒng)以及能測三軸角速度的慣性傳感器LSM330；機械結(jié)構(gòu)包括一對Faulhaber 1717-006SR空心杯直流電機、傳動件、輪轂、輪胎和支撐件等。

Zynq-7010可擴展處理平臺作為整個實時處理系統(tǒng)的核心，它包括Processing System(PS)和Programmable Logic(PL)兩大部分，其中PS部分包含了最高可運行在866 MHz的雙Cortex-A9核，PL部分包含了傳統(tǒng)意義的FPGA邏輯單元和DSP資源[6]。

電腦鼠所設(shè)計的最高運動速度為4 m/s，為了在高速移動時仍能保持準(zhǔn)確定位，本系統(tǒng)所使用的高速成像芯片MT9V034能在188×120的分辨率下以240 fps的速率輸出圖像。即使電腦鼠以4 m/s的速度進行移動，在240 f/s的速率下，每一幀間隔內(nèi)的移動距離也僅為16.7 mm，在可接受范圍內(nèi)。

1.2 激光測距結(jié)構(gòu)

激光測距的結(jié)構(gòu)和原理如圖1和圖2所示。線型激光器向前方發(fā)射線型的激光束，照射在物體表面，經(jīng)過反射后傳入到攝像頭中，在攝像頭內(nèi)部的圖像傳感器形成光斑。物體與激光器的相對位置不同，激光在圖像傳感器中形成光斑的位置也不同，據(jù)此可以確定物體的位置。

圖1 激光測距結(jié)構(gòu)圖

圖2 激光測距原理圖

對于理想的針孔攝像頭模型，根據(jù)三角形相似：

由于u和l均為常數(shù)，因此可以得到障礙物距離d和障礙物在圖像中的成像位置s的關(guān)系。

s對d求偏導(dǎo)，得：

可見，障礙物距離原點越遠，障礙物距離的分辨率越小，由于圖像傳感器分辨率有限，在超出一定距離后，分辨率會小于一個像素，需要在圖像處理過程中進一步處理。

實際應(yīng)用時，需要準(zhǔn)確得到障礙物在圖像中的相應(yīng)位置，但是環(huán)境中還存在不相關(guān)的景物，因此需要盡可能過濾掉不相關(guān)景物。線型激光器發(fā)射出808 nm的紅外激光，在鏡頭前加裝了一個808 nm紅外窄帶濾波片，接收經(jīng)過障礙物反射回來的紅外激光，由可以接收紅外光線的MT9V034圖像傳感器采集生成包含環(huán)境障礙物信息的圖像。

2 系統(tǒng)軟件設(shè)計

2.1 整體系統(tǒng)軟件結(jié)構(gòu)

如圖3所示，系統(tǒng)軟件包括PL部分和PS部分。其中PL部分包括MT9V034的驅(qū)動與數(shù)據(jù)采集、編碼盤數(shù)據(jù)采集和電機驅(qū)動。PS部分包括對采集后的圖像數(shù)據(jù)進行處理，慣性傳感器LSM330數(shù)據(jù)的獲取與處理，融合圖像、慣性傳感器和編碼盤數(shù)據(jù)的姿態(tài)解算，運動路徑規(guī)劃，解迷宮算法等。

圖3 系統(tǒng)軟件結(jié)構(gòu)圖

可見PS需要處理很多任務(wù)，考慮到系統(tǒng)調(diào)試的便捷性、現(xiàn)有資源的可重復(fù)利用性、各個軟件模塊的協(xié)調(diào)運行，系統(tǒng)在PS部分搭建了嵌入式實時Linux操作系統(tǒng)，并接入一個SDIO WiFi作為運行時的調(diào)試接口。

2.2 圖像采集

MT9V034與Zynq-7010的接口位于PL側(cè)，考慮到與PS的數(shù)據(jù)通信問題，本文在PL部分設(shè)計了一個自定義IP核，實現(xiàn)了MT9V034與PS的數(shù)據(jù)交互。

如圖3系統(tǒng)軟件結(jié)構(gòu)圖內(nèi)的數(shù)據(jù)流所示，PS使用AXI_GP接口，通過AXI-Lite協(xié)議，使用Memory Map的方式讀寫自定義IP核的寄存器和AXI DMA，控制MT9V034和啟動DMA進行數(shù)據(jù)傳輸。

自定義IP核使用Verilog HDL語言實現(xiàn)，通過檢測MT9V034的幀同步、行同步、像素時鐘等信息，把MT9V034的圖像數(shù)據(jù)緩存于一個異步時鐘FIFO中，異步時鐘FIFO的寫時鐘由MT9V034的像素時鐘提供。AXI DMA提供異步時鐘FIFO的讀時鐘，請求一次讀操作時從異步時鐘FIFO中彈出一個數(shù)據(jù)寫入到AXI DMA中。

2.3 圖像處理

要實現(xiàn)一個激光測距系統(tǒng)，需要對物體反射的激光圖像進行處理。盡管在硬件中通過加裝紅外濾光片能過濾掉絕大部分無用的環(huán)境光，但環(huán)境中仍會存在干擾源，需要在圖像處理中把干擾源去掉，獲取真實的激光圖像。

圖4為圖像傳感器采集得到的原始的圖像，由于線型激光器發(fā)射的激光平面與x軸平行，圖像中每一列數(shù)據(jù)最多有一個真實的激光光斑，對每一列圖像數(shù)據(jù)進行局部峰值檢測，結(jié)果如圖5所示。由于圖像分辨率的限制，為了提高測距精度，光斑的峰值位置采用加權(quán)的方法得到其亞像素位置。

圖4 原始圖像

圖5 峰值檢測結(jié)果

圖像中每一個光斑位置與載體坐標(biāo)系中的一個位置一一對應(yīng)，需要通過數(shù)據(jù)標(biāo)定來獲取該對應(yīng)關(guān)系。

通過標(biāo)定的數(shù)據(jù)對應(yīng)關(guān)系得到了點云數(shù)據(jù)后，對點云數(shù)據(jù)進行鄰近點聚類、線段分割，刪除不合理或距離原點太遠的數(shù)據(jù)，識別出墻壁位置。

2.4 定位算法

得到墻壁位置信息后，需要利用墻壁位置信息估計電腦鼠的實際位置。國際IEEE電腦鼠走迷宮比賽規(guī)則中的場地為由180 mm×180 mm的單元組成的迷宮，設(shè)迷宮坐標(biāo)原點與迷宮的左下角重合，則迷宮具有以下特點：

(1)墻壁端點坐標(biāo)為180 mm的整數(shù)倍；

(2)墻壁與坐標(biāo)軸(x軸或y軸)平行；

(3)墻壁與間隔為180 mm的正方形網(wǎng)格重合。

根據(jù)上述特點，可以使用最小二乘法估計電腦鼠在迷宮中的姿態(tài)。

2.4.1 估計電腦鼠的偏航角

估計電腦鼠的偏航角可以看作一個最優(yōu)化問題：給定電腦鼠上一時刻偏航角θN，計算一個盡可能接近于0的θ，使得通過圖像處理后得到的線段特征在繞著原點旋轉(zhuǎn)(θN+θ)后與坐標(biāo)軸(x軸或y軸)平行，則當(dāng)前時刻的電腦鼠偏航角為(θN+θ)。

首先考慮如下表示單一線段的點云數(shù)據(jù)的角度匹配問題，已知點云數(shù)據(jù)pi=(xi，yi)，i∈{0, 1, … ,Np-1}表示該點云數(shù)據(jù)的線段已經(jīng)繞原點旋轉(zhuǎn)θN，因此表示該點云數(shù)據(jù)的線段已經(jīng)與x軸接近平行，求角度θ，使得點云數(shù)據(jù)繼續(xù)繞原點旋轉(zhuǎn)θ后，點云數(shù)據(jù)與x軸盡可能平行，即目標(biāo)函數(shù)：

取最小值，其中：

令J對θ的偏導(dǎo)為0，得：

(1)

對公式(1)左右同時除以cos2θ得：

Aw2-Bw-A=0

解此一元二次方程可得w，通過θ=arctan(w)即可求得θ。然而，此方程會有兩個根，分別為w1和w2，它們所表示的角度相差90°，由于θ=arctan(w)為單調(diào)函數(shù)，且θ≈0，取w1和w2絕對值的較小者求θ。

同理，對于點云數(shù)據(jù)所表示的線段與y軸接近平行的角度匹配，亦可求得類似的形式。推廣到表示若干線段的點云數(shù)據(jù)，需要判斷其最近鄰匹配角度(匹配到x軸還是y軸)，解方程，即得到最小二乘形式的θ。

2.4.2 估計電腦鼠的坐標(biāo)

估計電腦鼠的坐標(biāo)可以看作一個最優(yōu)化問題：給定電腦鼠上一時刻在迷宮坐標(biāo)系的坐標(biāo)值(x，y)，計算一個盡可能接近于0的Δx和Δy，使得通過圖像處理后得到的線段特征在平移(x+Δx，y+Δy)后與間隔為180 mm的網(wǎng)格重合。則當(dāng)前時刻的電腦鼠坐標(biāo)為(x+Δx，y+Δy)。

2.4.3 SLAM結(jié)果

圖6所示為電腦鼠經(jīng)過SLAM后的結(jié)果。其中黑色線條表示識別出該位置具有墻壁，虛線表示該位置尚未探索，梯形框表示電腦鼠的有效視野，有一根指向梯形框的黑色線條表示電腦鼠的位置與朝向，黑點則表示當(dāng)前時刻的點云數(shù)據(jù)。

圖6 SLAM結(jié)果

3 結(jié)論

成品電腦鼠總重量為100.06 g，車體大小為7.45 cm(寬)×6.6 cm(長)。與使用紅外對管作為位置傳感器的傳統(tǒng)電腦鼠相比，本文的電腦鼠可以充分利用迷宮信息進行路徑規(guī)劃，以機械結(jié)構(gòu)允許的最大速度運動且不與墻壁發(fā)生碰撞。其最高運動速度為3.5 m/s，最大加速度為15 m/s2，可以完成迷宮探索及快速沖刺功能。

[1] 張嘉夫,李迅波,余岷,等. 基于ARM的走迷宮電腦鼠的設(shè)計與實現(xiàn)[J]. 工業(yè)控制計算機,2013,26(11):136-138.

[2] 金余義,劉克申,任陽暉. 基于測距紅外傳感器的輪式迷宮機器人設(shè)計[J]. 單片機與嵌入式系統(tǒng)應(yīng)用,2013(9):74-77.

[3] Zhang Haoming, LIAN S P, Wang Yinghai. Calibrate the turning of micromouse[J]. Advanced Materials Research, 2014, 971:1088-1091.

[4] THRUN S, LEONARO J J. Simultaneous localization and mapping[A]. Springer Handbook of Robotics[M]. 2007: 871-889.

[5] 吳長江,趙不賄,鄭博,等. 基于FPGA的動態(tài)目標(biāo)跟蹤系統(tǒng)設(shè)計[J]. 電子技術(shù)應(yīng)用,2010,36(3):45-50.

[6] Xilinx Inc. Zynq-7000 all programmable SoC technical reference manual (v1.6.1) [Z].2013.

Micromouse using SLAM

Su Hao1, Huang Xinhan1, Wang Zhenyan2

(1. School of Automation, Huazhong University of Science and Technology, Wuhan 430074, China;2. School of Electrical and Electronic Engineering, Huazhong University of Science and Technology, Wuhan 430074, China)

Most micromouses use infrared tubes as simpler position sensors, but this paper built a small laser ranging system with linear laser and camera on a micromouse and run simultaneous localization and mapping (SLAM) on it. The camera output images with resolution of 188×120 at speed of 240 f/s. The system used Zynq-7010 as main controller and built an image acquisition system on its programmable logic part. Also we built a real-time Linux system to realize the image processing, sensor data fusion and maze solver algorithm on its processing system part. We made a micromouse with total weight of 100.06 g and body size of 7.45 cm (width) × 6.6 cm (length), which can move fast and stably in the maze.

micromouse; SLAM; laser ranging; Zynq

TP242

A

10.19358/j.issn.1674- 7720.2017.12.016

蘇豪，黃心漢，王貞炎.采用SLAM方法的電腦鼠[J].微型機與應(yīng)用，2017,36(12)：54-56，60.

2016-12-22)

蘇豪(1992-)，男，碩士研究生，主要研究方向：圖像處理與計算機視覺，嵌入式系統(tǒng)。

黃心漢(1946-)，男，博士，教授，主要研究方向：智能控制、智能機器人、圖像處理與模式識別、信息融合。

王貞炎(1984-)，男，碩士，工程師，主要研究方向：模擬信號調(diào)理、數(shù)字信號處理和FPGA應(yīng)用技術(shù)。