基于二維碼掃描的IoT空間建模機(jī)器人

鄧開(kāi)連，虞嘉豪，劉肖燕，王文龍，燕 帥

（東華大學(xué) 信息科學(xué)與技術(shù)學(xué)院，上海 201620）

0 引 言

隨著物聯(lián)網(wǎng)、機(jī)器人技術(shù)的發(fā)展[1-2]，掃地機(jī)器人、服務(wù)員機(jī)器人等室內(nèi)工作機(jī)器人的建模避障與導(dǎo)航是實(shí)現(xiàn)機(jī)器人正常工作的關(guān)鍵[3]。例如，Gmapping算法通過(guò)采集激光傳感器、坐標(biāo)變換和里程計(jì)數(shù)據(jù)構(gòu)建柵格地圖[4]，通過(guò)360°可旋轉(zhuǎn)平臺(tái)上的3D結(jié)構(gòu)光掃描法得到3D云點(diǎn)陣[5]；E. Lachat等提出將TLS（Terrestrial Laser Scanner）與KINECT V2傳感器相結(jié)合以提高建模的細(xì)節(jié)水平[6]；張肇軒等提出的基于模板替換的方法優(yōu)化了單視角卷積神經(jīng)網(wǎng)絡(luò)室內(nèi)場(chǎng)景建模信息缺失的問(wèn)題[7]；許佩研究了一種非迭代的結(jié)構(gòu)光多視角自動(dòng)拼接算法，降低了多視角融合建模的計(jì)算復(fù)雜度[8]。為了降低空間建模的成本，提高空間建模的精度，簡(jiǎn)化空間建模的算法，設(shè)計(jì)了一種基于二維碼掃描進(jìn)行室內(nèi)建模的方法。

二維碼識(shí)別以其易識(shí)別、信息量大、門(mén)檻低的特性在各個(gè)領(lǐng)域有了廣泛的應(yīng)用[9-11]。傳統(tǒng)機(jī)器人導(dǎo)航算法中，在室內(nèi)運(yùn)行的機(jī)器人往往需要通過(guò)反復(fù)掃描、避障、計(jì)算等才可以得到室內(nèi)的運(yùn)行地圖[12]。在此方法中，在一個(gè)完整的智能家居體系中，智能家具本身附有二維碼，并且二維碼儲(chǔ)存了物體的大小以及占用空間信息，當(dāng)機(jī)器人掃描到物體時(shí)僅需將物體正確地放置到室內(nèi)地圖中所對(duì)應(yīng)的位置即可實(shí)現(xiàn)快速建模。相比較傳統(tǒng)建模方法，把對(duì)于物體的建模工作提前完成，機(jī)器人無(wú)需裝備各種掃描裝置來(lái)為物體建立模型，僅需確定物體的位置與朝向就可以把事先建好的模型安放于機(jī)器人的導(dǎo)航地圖內(nèi)，實(shí)現(xiàn)建模。系統(tǒng)不僅降低了對(duì)于機(jī)器人終端的硬件需求，也提高了建模精度和識(shí)別準(zhǔn)確度。

1 系統(tǒng)設(shè)計(jì)

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

基于二維碼掃描的空間建模機(jī)器人系統(tǒng)組成主要包括：電源（使用12 V的航模電池和DC降壓模塊）、STM32單片機(jī)（STM32F103ZET6、SK60長(zhǎng)距離激光測(cè)距模塊、一對(duì)短距ToF激光測(cè)距模塊ATK-VL53L0X、OpenMV-H7攝像頭模塊[13]）、舵機(jī)[14]（為OpenMV攝像頭模塊專(zhuān)用雙自由度舵機(jī)）、XD-37GB520電機(jī)（為12 V的直流電機(jī)，用于驅(qū)動(dòng)機(jī)器人前進(jìn)，以及NT35510LCD顯示屏模塊、L298N電機(jī)調(diào)速模塊[15-16]）。系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

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

1.2 二維碼設(shè)計(jì)

實(shí)驗(yàn)中所用物體的長(zhǎng)寬以及特征信息儲(chǔ)存在二維碼中，二維碼中含有8位十進(jìn)制數(shù)。目前，針對(duì)立方體的物體進(jìn)行了實(shí)驗(yàn)，第1～2位表示物體的特征碼，每一個(gè)物體都有一個(gè)單獨(dú)的特征碼，已經(jīng)被加入到模型中的物體的特征碼會(huì)被記錄，以防反復(fù)計(jì)算；第3～5位表示物體的長(zhǎng)度，即與二維碼平面垂直方向的物體長(zhǎng)度，單位為cm；第6～9位表示物體的寬度，即與二維碼平面平行的物體長(zhǎng)度；二維碼的上方有一個(gè)紅色矩形指示條，用于指示OpenMV攝像頭找到二維碼的位置。

1.3 場(chǎng)地設(shè)計(jì)

實(shí)驗(yàn)測(cè)試場(chǎng)地為一塊寬度2.4 m，長(zhǎng)度4 m的空間，周?chē)眉埌鍢?shù)立起圍墻，機(jī)器人的行進(jìn)路線為一條直線，如圖2中線段所示。物體被擺放在道路兩側(cè)，在與機(jī)器人攝像頭等高位置處貼上二維碼貼紙。場(chǎng)地設(shè)計(jì)區(qū)域俯視圖如圖2所示。圖中1，2，3分別表示不同的物體，提前測(cè)量了長(zhǎng)與寬，并且數(shù)據(jù)已經(jīng)錄到二維碼中。

圖2 場(chǎng)地設(shè)計(jì)區(qū)域俯視圖

1.4 機(jī)械結(jié)構(gòu)

系統(tǒng)整機(jī)結(jié)構(gòu)實(shí)物圖如圖3所示。機(jī)器人用XD-37GB520電機(jī)牽引一對(duì)履帶前進(jìn)，以防打滑導(dǎo)致機(jī)器人偏離原先軌道。激光測(cè)距模塊指向機(jī)器人前進(jìn)的方向，OpenMV攝像頭和一對(duì)平行的紅外激光測(cè)距模塊固定在舵機(jī)上方，舵機(jī)可以水平360°旋轉(zhuǎn)。

圖3 系統(tǒng)整機(jī)結(jié)構(gòu)實(shí)物

2 系統(tǒng)建模

2.1 系統(tǒng)控制

STM32F103單片機(jī)含有2個(gè)I2C接口[17]用于連接激光傳感器ATK-VL53L0X，PB6連接SCL，PB7連接SDA，它們連接攝像頭左側(cè)的激光傳感器。PB10連接SCL，PB11連接SDA，它們連接攝像頭右側(cè)的激光傳感器。UART1連接OpenMV的串口，UART2連接SK60激光傳感器，均采用異步全雙工通信。PB5和PB4分別連接定時(shí)器3的通道2和1，輸出PWM波[18]控制L298N對(duì)電機(jī)進(jìn)行調(diào)速，以平衡電機(jī)的轉(zhuǎn)速差異，讓機(jī)器人能實(shí)現(xiàn)直線行駛。OpenMV攝像頭模塊輸出PWM波控制舵機(jī)。

2.2 建模原理

首先，機(jī)器人沿著直線緩慢前進(jìn)，OpenMV攝像頭模塊控制舵機(jī)左右旋轉(zhuǎn)掃描紅色矩形標(biāo)識(shí)，當(dāng)紅色標(biāo)識(shí)的面積大于一定值，即攝像頭與二維碼足夠近時(shí)，控制舵機(jī)使紅色標(biāo)識(shí)位于畫(huà)面中間，開(kāi)始識(shí)別二維碼。識(shí)別后，OpenMV攝像頭模塊將二維碼數(shù)據(jù)和此時(shí)的舵機(jī)角度β傳送給STM32主控芯片處理。角度測(cè)量原理示意圖如圖4所示，此時(shí)STM32主控芯片控制平行于相機(jī)的一對(duì)紅外測(cè)距模塊，測(cè)量二維碼和相機(jī)之間的距離得到兩者之間的距離X，由于2個(gè)激光測(cè)距模塊的距離A可以直接提前測(cè)量，則物體二維碼所在平面與攝像頭平面的夾角α為：

圖4 角度測(cè)量原理示意圖

然后，由舵機(jī)的角度β和機(jī)器人的位置綜合計(jì)算出物體的位置，并將相應(yīng)位置的顯示屏顯示呈現(xiàn)為紅色。之后繼續(xù)前進(jìn)，重復(fù)上述過(guò)程直到機(jī)器人到達(dá)終點(diǎn)。最終可以從LCD顯示屏中直觀地看到掃描結(jié)果，如有后續(xù)開(kāi)發(fā)需要，單片機(jī)也可以讀取對(duì)應(yīng)LCD RAM的RGB值，得到相應(yīng)的數(shù)據(jù)。

3 軟件算法

3.1 程序設(shè)計(jì)

系統(tǒng)程序主要分為STM32主控芯片部分和OpenMV攝像頭部分，其程序流程圖如圖5所示。主控部分采用C語(yǔ)言環(huán)境編寫(xiě)，代碼在Keil 5環(huán)境下編譯，并使用ST-LINK調(diào)試下載，主要負(fù)責(zé)輸出PWM波、收發(fā)I2C數(shù)據(jù)、收發(fā)串口數(shù)據(jù)、算法處理和LCD顯示。攝像頭部分使用OpenMV IDE編程負(fù)責(zé)物體及二維碼識(shí)別、舵機(jī)控制、串口數(shù)據(jù)收發(fā)。

圖5 程序流程

OpenMV攝像頭模塊尋找紅色矩形的方法為：在攝像頭的畫(huà)面中建立直角坐標(biāo)系，原點(diǎn)位于畫(huà)面左下角的像素點(diǎn)，X軸水平方向向右，Y軸向上，每個(gè)像素點(diǎn)對(duì)應(yīng)一個(gè)直角坐標(biāo)系中的整數(shù)點(diǎn)。提取攝像頭中的每個(gè)像素點(diǎn)的RGB值，根據(jù)RGB值判定像素點(diǎn)為是否為紅色，通過(guò)紅色像素點(diǎn)橫坐標(biāo)平均值得出紅色矩形的中心橫坐標(biāo)如下：

式中：Xi為第i個(gè)紅色像素點(diǎn)的橫坐標(biāo)；k為紅色像素點(diǎn)的總個(gè)數(shù)。

該方法可以降低光線環(huán)境產(chǎn)生的噪點(diǎn)對(duì)判斷方位的影響。通過(guò)將Xc與屏幕的中心點(diǎn)相比較得到紅色矩形的偏移量，并結(jié)合PID算法調(diào)整舵機(jī)的方向，使得二維碼位于屏幕中央。

3.2 建模算法

以機(jī)器人前進(jìn)的路線為Y軸正方向，機(jī)器人的起點(diǎn)為原點(diǎn)，機(jī)器人起點(diǎn)的右側(cè)為X軸正方向建立直角坐標(biāo)系，坐標(biāo)系以cm為單位，則機(jī)器人的起始位置為（120，0）。由激光測(cè)距模塊得到和最前方墻壁的距離為d，機(jī)器人經(jīng)過(guò)校準(zhǔn)以達(dá)到直線行駛，則機(jī)器人的實(shí)時(shí)位置可以表示為（120，240-d）。當(dāng)OpenMV掃描到二維碼時(shí)，會(huì)返回舵機(jī)的偏移角度β、物體的長(zhǎng)度a、寬度b。此時(shí)機(jī)器人通過(guò)雙激光測(cè)距模塊測(cè)得距離再通過(guò)公式（1）得到角度α。β和α均為矢量。對(duì)于β，以機(jī)器人前進(jìn)方向?yàn)?°，俯視圖上順時(shí)針?lè)较驗(yàn)檎?；?duì)于α，以2個(gè)激光測(cè)距模塊之間的連線為0°，俯視圖上以順時(shí)針?lè)较驗(yàn)檎?。由?個(gè)激光測(cè)距模塊的連線非常接近舵機(jī)的轉(zhuǎn)軸，激光測(cè)距模塊的凸出部分可忽略，因此得出實(shí)際物體相對(duì)Y軸正方向的偏移角度γ為：

由短距激光測(cè)距模塊得到的2個(gè)數(shù)據(jù)為X1，X2，那么物體到機(jī)器人的距離為：

則物體二維碼面的中點(diǎn)位置的坐標(biāo)可以表示為：

因此，物體上被掃描到二維碼的一面所對(duì)應(yīng)坐標(biāo)系中的直線為：

則和二維碼平行的一面所對(duì)應(yīng)的直線為：

因此，與這2條邊垂直的邊可以表示為：

L1，L2，L3，L4共同構(gòu)成了矩形，最終得到矩形4個(gè)點(diǎn)的坐標(biāo)表達(dá)式如下：

3.3 算法分析

建模算法主要包括被掃描物體的定位，以及在建模坐標(biāo)系中還原被掃描物體的兩個(gè)過(guò)程。物體的定位算法是涉及三角函數(shù)的簡(jiǎn)單解析幾何運(yùn)算，調(diào)用C語(yǔ)言中的math庫(kù)即可。由于物體的數(shù)據(jù)均被提前測(cè)量并錄入二維碼，因此建模的精度可以很高，并且在掃描并建立模型時(shí)，可以避免模型不完整的情況。但在實(shí)際應(yīng)用中，由于室內(nèi)的場(chǎng)景元素的復(fù)雜性和多樣性，使得提前測(cè)量的數(shù)據(jù)導(dǎo)入二維碼中的工作較為困難，而且若物體的形狀或大小可變則會(huì)使建模結(jié)果不可靠。

在掃描并建模一個(gè)物體的過(guò)程中，算法僅需執(zhí)行一次，無(wú)需迭代循環(huán)。建模方法定性對(duì)比見(jiàn)表1所列，由表1可知，與單視角卷積神經(jīng)網(wǎng)絡(luò)和Gmapping算法相比，二維碼掃描方法具有時(shí)間復(fù)雜度較低、對(duì)于單個(gè)物體建模精度高、傳感器簡(jiǎn)單等優(yōu)點(diǎn)，但機(jī)器人本身的定位依賴(lài)于矩形的規(guī)則場(chǎng)地和激光傳感器精度，在復(fù)雜場(chǎng)景下定位性能較差，若是能夠與里程計(jì)等傳感器相結(jié)合可以提高定位精度。

表1 建模方法定性對(duì)比

4 實(shí)驗(yàn)結(jié)果與分析

在多次實(shí)驗(yàn)中，將3個(gè)長(zhǎng)寬不同的矩形物體隨機(jī)擺放在行駛路線兩側(cè)，在機(jī)器人前進(jìn)掃描后，都可以較好地還原實(shí)際的物體擺放圖，并將其顯示在LCD屏中，顯示結(jié)果如圖6所示。3個(gè)方塊即表示3個(gè)被掃描到的物體。掃描顯示結(jié)果與實(shí)際的誤差主要來(lái)源于：機(jī)器人簡(jiǎn)化為質(zhì)點(diǎn)處理；云臺(tái)旋臂的空間被壓縮。

圖6 基于空間模型的掃描顯示圖

多次實(shí)驗(yàn)結(jié)果統(tǒng)計(jì)顯示，掃描物體的成功率約為93%，但若兩個(gè)物體擺放過(guò)近時(shí)，成功率可能會(huì)降低，這是由于機(jī)器人誤以為相鄰的物體是同一個(gè)物體。

5 結(jié) 語(yǔ)

本文所提系統(tǒng)使用了二維碼識(shí)別的方法，并結(jié)合了簡(jiǎn)單的解析幾何算法，解決了相對(duì)復(fù)雜的空間建模與掃描問(wèn)題。在提前給物體建模并制成二維碼后，機(jī)器人僅需對(duì)二維碼進(jìn)行識(shí)別，無(wú)需重新掃描構(gòu)建物體的模型。因此，系統(tǒng)具有建模速度快、運(yùn)算要求低和識(shí)別成功率高等優(yōu)點(diǎn)。提前測(cè)量并錄入物體數(shù)據(jù)到二維碼中的方法提高了建模精度，但是同時(shí)也降低了系統(tǒng)的通用性。在實(shí)驗(yàn)中，機(jī)器人很好地建立了空間俯視圖模型，但在復(fù)雜場(chǎng)景下的不規(guī)則物體識(shí)別與建模方面，系統(tǒng)效果還有待提高。系統(tǒng)給室內(nèi)機(jī)器人等物聯(lián)網(wǎng)設(shè)備的室內(nèi)導(dǎo)航與識(shí)別提供了一種新的思路，在低功耗、低成本室內(nèi)運(yùn)行機(jī)器人領(lǐng)域和智能家居生態(tài)IoT體系內(nèi)有很好的應(yīng)用前景。物體的避障、抓取實(shí)驗(yàn)未來(lái)可以繼續(xù)實(shí)施。此外，物體的二維碼是否可以更換為RFID標(biāo)簽來(lái)提高機(jī)器人的定位精度、建模，并與手臂的動(dòng)力學(xué)模型相結(jié)合應(yīng)用到物體的抓取與移動(dòng)中等都是未來(lái)研究的方向。