采用SLAM方法的電腦鼠

蘇 豪，黃心漢，王貞炎

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

?

采用SLAM方法的電腦鼠

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

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

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

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

0 引言

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

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

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

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

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

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

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

電腦鼠所設(shè)計(jì)的最高運(yùn)動速度為4 m/s，為了在高速移動時(shí)仍能保持準(zhǔn)確定位，本系統(tǒng)所使用的高速成像芯片MT9V034能在188×120的分辨率下以240 fps的速率輸出圖像。即使電腦鼠以4 m/s的速度進(jìn)行移動，在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)，得：

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

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

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

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

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

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

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

2.2 圖像采集

MT9V034與Zynq-7010的接口位于PL側(cè)，考慮到與PS的數(shù)據(jù)通信問題，本文在PL部分設(shè)計(jì)了一個自定義IP核，實(shí)現(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進(jìn)行數(shù)據(jù)傳輸。

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

2.3 圖像處理

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

圖4為圖像傳感器采集得到的原始的圖像，由于線型激光器發(fā)射的激光平面與x軸平行，圖像中每一列數(shù)據(jù)最多有一個真實(shí)的激光光斑，對每一列圖像數(shù)據(jù)進(jìn)行局部峰值檢測，結(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)系得到了點(diǎn)云數(shù)據(jù)后，對點(diǎn)云數(shù)據(jù)進(jìn)行鄰近點(diǎn)聚類、線段分割，刪除不合理或距離原點(diǎn)太遠(yuǎn)的數(shù)據(jù)，識別出墻壁位置。

2.4 定位算法

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

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

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

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

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

2.4.1 估計(jì)電腦鼠的偏航角

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

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

取最小值，其中：

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

(1)

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

Aw2-Bw-A=0

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

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

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

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

2.4.3 SLAM結(jié)果

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

圖6 SLAM結(jié)果

3 結(jié)論

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

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

[2] 金余義,劉克申,任陽暉. 基于測距紅外傳感器的輪式迷宮機(jī)器人設(shè)計(jì)[J]. 單片機(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ì)[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].微型機(jī)與應(yīng)用，2017,36(12)：54-56，60.

2016-12-22)

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

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

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