基于視覺和紅外引導(dǎo)的機(jī)器人自主充電算法

趙衛(wèi)東，唐 偉，唐顧杰

(安徽工業(yè)大學(xué)電氣與信息工程學(xué)院，安徽馬鞍山243032)

隨著人工智能在機(jī)器人領(lǐng)域的廣泛應(yīng)用，機(jī)器人的應(yīng)用場(chǎng)景、工作任務(wù)越來越復(fù)雜，其自主作業(yè)時(shí)間也逐漸相應(yīng)增加。但是機(jī)器人采用的機(jī)載蓄電池并不能滿足機(jī)器人的工作需求，增加蓄電池容量反而會(huì)給機(jī)器人帶來不必要的負(fù)擔(dān)[1]。故移動(dòng)機(jī)器人的自主充電對(duì)接技術(shù)成為其連續(xù)作業(yè)的重要環(huán)節(jié)。由于機(jī)器人導(dǎo)航精度的問題，機(jī)器人難以通過靜態(tài)地圖導(dǎo)航至充電樁所在位置進(jìn)行充電，目前多數(shù)自主充電技術(shù)是先讓機(jī)器人導(dǎo)航至充電區(qū)域，然后采用紅外信號(hào)、激光測(cè)距儀或視覺等方式引導(dǎo)機(jī)器人精確對(duì)接充電樁。Ye[2]把紅外對(duì)射技術(shù)應(yīng)用在室內(nèi)機(jī)器人的自主充電算法中，通過跟蹤紅外線完成機(jī)器人的自主充電任務(wù)，而單純的紅外信號(hào)傳輸距離較小并且傳播方向單一，導(dǎo)致單純的紅外引導(dǎo)對(duì)接充電效率低、范圍?。籗u等[3]利用SICK激光測(cè)距儀引導(dǎo)機(jī)器人自主充電對(duì)接，但激光測(cè)距儀的精度隨著量程的增加而減小，不能大范圍地引導(dǎo)機(jī)器人；曾世藩等[4]提出基于紅外和超聲波的自主充電對(duì)接方式，利用紅外回歸反射和超聲波檢測(cè)距離功能，設(shè)計(jì)自充充電系統(tǒng)，簡(jiǎn)化了設(shè)計(jì)方案，擴(kuò)大了引導(dǎo)范圍；王建元等[5]提出了一種基于圖像識(shí)別的自主充電方式，通過對(duì)獲取的圖像進(jìn)行紋理分析，并使用SIFT(scale-invariant feature transform)算法對(duì)興趣點(diǎn)進(jìn)行匹配，確定充電樁位置，完成對(duì)接任務(wù)。但使用SIFT算法進(jìn)行目標(biāo)匹配的方法太過復(fù)雜，獲取充電樁位置的實(shí)時(shí)性較差、效率低[6]。鄒建成[7]針對(duì)機(jī)器人自主充電技術(shù)的場(chǎng)地適應(yīng)性差、引導(dǎo)范圍小等問題，設(shè)計(jì)基于視覺標(biāo)靶的自主充電方式，有效地改善了場(chǎng)地部署方式，提高了引導(dǎo)范圍以及自主充電的成功率，但其在機(jī)器人運(yùn)動(dòng)速度過快時(shí)仍存在丟失標(biāo)靶的現(xiàn)象，導(dǎo)致充電效率低。

針對(duì)上述問題，文中結(jié)合視覺和紅外信號(hào)來引導(dǎo)機(jī)器人完成與充電樁的對(duì)接任務(wù)。通過安裝在機(jī)器人上的相機(jī)來識(shí)別標(biāo)記充電樁快速響應(yīng)(quick response，QR)碼，快速獲取機(jī)器人與QR碼的相對(duì)位置，通過該位置信息，機(jī)器人將直接向充電樁移動(dòng)。同時(shí)，為消除機(jī)器人在移動(dòng)過程中丟失QR碼以及距離QR碼較近時(shí)無法獲取完整圖形帶來的對(duì)定位信息的影響，利用紅外接收器確定無法使用QR碼定位時(shí)兩者之間的相對(duì)角度，完成機(jī)器人的自主充電對(duì)接任務(wù)。

1 QR碼引導(dǎo)算法

二維碼技術(shù)是一種利用某種特定幾何圖形按相應(yīng)規(guī)律排列的數(shù)據(jù)編碼存儲(chǔ)手段。當(dāng)下，市場(chǎng)上充斥著多種二維碼碼制，應(yīng)用最廣的有QR碼、Data Matrix碼等[8]。QR碼具識(shí)別、結(jié)構(gòu)特別、糾錯(cuò)能力強(qiáng)等優(yōu)點(diǎn)，故文中采用QR碼完成對(duì)充電樁的定位。

1.1 QR碼的獲取

機(jī)器人到達(dá)充電區(qū)域后，需在采集到的圖像中確定QR碼的位置，才能獲取QR碼的邊緣信息以及其中包含的編碼信息。圖像中QR碼的查找算法的流程如圖1，主要分為兩步。

圖1 QR碼的查找算法流程圖Fig.1 Flowchart of search algorithmforQR code

1) 圖像的預(yù)處理 為能夠獲取清晰且不受外界干擾的圖像，需對(duì)相機(jī)采集到的圖形進(jìn)行預(yù)處理。通過自適應(yīng)閾值二值化方式以及中值濾波對(duì)圖像進(jìn)行處理，解決圖像受外界環(huán)境影響產(chǎn)生的噪聲如光照不均勻、高斯噪聲等，提高QR碼的辨識(shí)度[9]；

2) QR 碼的區(qū)域獲取 如圖2。根據(jù)QR 碼的3 個(gè)特殊結(jié)構(gòu)位置探測(cè)圖形，準(zhǔn)確并快速確定圖像中QR 碼的區(qū)域[10]；利用輪廓信息檢測(cè)算法的抗噪聲能力較強(qiáng)的特點(diǎn)檢測(cè)圖像中的位置探測(cè)圖形，確定QR碼的位置[11]。

圖2 QR碼的區(qū)域獲取Fig.2 Area acquisition of QR code

考慮到相機(jī)沒有標(biāo)定或相機(jī)與QR碼平面不嚴(yán)格垂直導(dǎo)致獲取的QR發(fā)生形變，對(duì)位置探測(cè)圖形的結(jié)構(gòu)比率按1∶1∶3∶1∶1進(jìn)行區(qū)間放大處理，提高查詢位置探測(cè)圖形的準(zhǔn)確率。在成功獲取QR碼中的3個(gè)位置探測(cè)圖形后，根據(jù)位置探測(cè)圖形的中心點(diǎn)坐標(biāo)確定像平面下QR碼的位置、幾何中心坐標(biāo)以及面積信息等。

1.2 機(jī)器人相對(duì)位置的確定

移動(dòng)機(jī)器人通過相機(jī)精確定位到QR 碼后，需估計(jì)QR 碼與機(jī)器人的相對(duì)位置。根據(jù)針孔成像模型原理[12]，利用已經(jīng)標(biāo)定好的相機(jī)內(nèi)參建立相機(jī)投影模型。假設(shè)QR 碼中一點(diǎn)在世界坐標(biāo)系下為Pw(Xw,Yw,Zw)，對(duì)應(yīng)在像平面下的坐標(biāo)點(diǎn)pc(u,v)，則有：

式中：K 為相機(jī)的內(nèi)參矩陣；R 為3×3的旋轉(zhuǎn)矩陣；t 為3×1的平移列向量；αx，αy為相機(jī)內(nèi)參；T 為外參矩陣。

在檢測(cè)目標(biāo)QR碼所在平面垂直于相機(jī)的光軸中心線時(shí)，旋轉(zhuǎn)矩陣R=I。設(shè)Pw到相機(jī)的真實(shí)距離為d，平移列向量t=[0 ,0,d]T，則有：

式(2)可簡(jiǎn)化為：

則有

式中：S為QR碼的實(shí)際測(cè)量面積；SI為圖像平面中QR碼的面積；αx與αy均為相機(jī)內(nèi)參；d 為目標(biāo)與相機(jī)光心的實(shí)際距離。

確定QR碼與相機(jī)的距離d 后，可根據(jù)式(2)相機(jī)的投影模型確定兩者的相對(duì)位姿，該式可引入李代數(shù)ζ表示相機(jī)的位姿矩陣R 和t，則

為獲取最優(yōu)的相機(jī)位姿，使用光束平差法構(gòu)建重投影誤差，并通過最小二乘法使得誤差最小

通過式(7)可獲取機(jī)器人與QR碼的相對(duì)位置。機(jī)器人在QR碼的引導(dǎo)下不斷調(diào)整自身位姿，逐漸向充電樁移動(dòng)。

2 紅外引導(dǎo)算法

在純視覺的定位算法中，機(jī)器人在移動(dòng)過程中會(huì)出現(xiàn)丟失QR碼的現(xiàn)象。同時(shí)，在機(jī)器人距離QR碼太近的情況下，相機(jī)不能獲取完成QR碼信息。鑒于此，設(shè)計(jì)1個(gè)紅外接收器接收裝載在充電樁上紅外發(fā)射管發(fā)出的紅外信號(hào)，進(jìn)行角度補(bǔ)償。圖3 為紅外信號(hào)引導(dǎo)機(jī)器人進(jìn)行對(duì)接充電樁示意圖，其中充電樁上紅外發(fā)射管的紅外信號(hào)覆蓋范圍是1個(gè)半徑為2 m、角度為20°的扇形區(qū)域。文中設(shè)計(jì)的接收器由5 個(gè)紅外接收管組成(以機(jī)器人坐標(biāo)系y軸正向?yàn)闇?zhǔn)，右側(cè)第一個(gè)為紅外接收管A，以此類推)，每?jī)蓚€(gè)紅外接收管間隙為5 cm。各接收管均能接收35°范圍內(nèi)的紅外信號(hào)。紅外接收管屬于光敏二極管，一般只對(duì)紅外線起作用，靈敏度高，并且輸出數(shù)字信號(hào)，數(shù)據(jù)處理簡(jiǎn)單。

圖3 紅外信號(hào)引導(dǎo)示意圖Fig.3 Infrared signal guide diagram

為方便計(jì)算，紅外信號(hào)與QR 碼的采樣時(shí)間間隔均設(shè)為Δt，機(jī)器人在t 時(shí)刻的后退速度為v，旋轉(zhuǎn)速度為r，由于Δt 較小，機(jī)器人的移動(dòng)距離可近似為直線。故設(shè)ti時(shí)刻機(jī)器人相對(duì)于充電樁的距離為d，CD間的間距c=0.05 m，根據(jù)三角形的正弦定理有

c=0.05 m，故ΔCPD 三角形可近似為等腰三角形，則d1=d2=di，根據(jù)三角形的余弦定理獲取θi

所以，在紅外對(duì)接過程中，根據(jù)前兩個(gè)時(shí)刻的QR 碼確定相對(duì)位姿以及機(jī)器人速度反饋，通過式(9)對(duì)機(jī)器人和充電樁的相對(duì)位置進(jìn)行估計(jì)。在PID(proportion integral differention)算法下對(duì)機(jī)器人的位姿進(jìn)行調(diào)整，調(diào)整方式主要為直行、左轉(zhuǎn)和右轉(zhuǎn)：

若-0.5°≤θi≤0.5°，則機(jī)器人沒有偏離充電樁，向后直線行駛；

若θi＜-0.5°，則機(jī)器人在充電樁的中心偏左位置，機(jī)器人順時(shí)針旋轉(zhuǎn)；

若θi＞0.5°，則機(jī)器人在充電樁的中心偏右位置，機(jī)器人逆時(shí)針旋轉(zhuǎn)。

3 自主充電對(duì)接算法

在機(jī)器人的自主充電對(duì)接系統(tǒng)中，機(jī)器人調(diào)整到指定姿態(tài)后，認(rèn)為機(jī)器人已與充電樁對(duì)準(zhǔn)，機(jī)器人保持直行。但機(jī)器人在實(shí)際運(yùn)行過程中，出現(xiàn)QR碼丟失或近距離對(duì)接充電樁時(shí)，QR碼部分信息在相機(jī)視野中逐漸消失，無法獲取充電樁與機(jī)器人之間的距離與角度。并且機(jī)器人在直行時(shí)，不同電機(jī)性能的微小差異及外界環(huán)境的影響都會(huì)導(dǎo)致機(jī)器人偏離既定航線。故使用紅外信號(hào)引導(dǎo)機(jī)器人進(jìn)行精確對(duì)接，需通過QR碼獲取充電樁相對(duì)于自身的位姿，才能通過紅外信號(hào)對(duì)丟失QR 碼時(shí)進(jìn)行角度的估計(jì)。文中設(shè)計(jì)的自主充電對(duì)接系統(tǒng)工作流程如圖4。

圖4 自主充電對(duì)接算法工作流程圖Fig.4 Workflow of autonomous charging docking algorithm

當(dāng)機(jī)器人檢測(cè)到自身電量低于設(shè)定值，開啟自主充電服務(wù)。機(jī)器人根據(jù)靜態(tài)地圖導(dǎo)航至充電區(qū)域，通過自轉(zhuǎn)來查找QR碼。若其并非處在引導(dǎo)的有效范圍內(nèi)，則通過超時(shí)判斷，終止當(dāng)前服務(wù)；若其查詢到QR碼，則按照?qǐng)D4的算法流程進(jìn)行：

1)查找到QR碼后，計(jì)算QR碼與機(jī)器人的相對(duì)距離與角度，控制機(jī)器人向充電樁運(yùn)動(dòng)；

2)丟失QR碼后，通過紅外引導(dǎo)算法進(jìn)行對(duì)接，連續(xù)獲取五幀圖像仍無法獲取QR碼，開始計(jì)時(shí)，并通過機(jī)器人自轉(zhuǎn)繼續(xù)查找QR碼，計(jì)時(shí)超過2 min則返回充電失敗標(biāo)志，結(jié)束當(dāng)前自主充電服務(wù)；

3)當(dāng)機(jī)器人距離充電樁0.5 m且紅外接收器獲得信號(hào)的個(gè)數(shù)C不為0時(shí)，終止QR碼查找服務(wù)并根據(jù)紅外引導(dǎo)算法進(jìn)行充電樁對(duì)接。

單純使用紅外或激光器的對(duì)接算法多是通過一定的角度左右移動(dòng)以“之”字形路徑接受傳感器信號(hào)從而完成對(duì)接任務(wù)。這種情況下，機(jī)器人對(duì)接路徑復(fù)雜，對(duì)接效率低。圖5(a)為采用單一傳感器進(jìn)行對(duì)接的路徑。使用文中設(shè)計(jì)的結(jié)合視覺和紅外算法，機(jī)器人通過相機(jī)快速對(duì)QR碼定位，直接向充電樁移動(dòng)，大大節(jié)約了時(shí)間。對(duì)接過程中出現(xiàn)的定位信息通過紅外信號(hào)進(jìn)行彌補(bǔ)，估計(jì)充電樁位置，保證對(duì)接的準(zhǔn)確性。采用視覺和紅外引導(dǎo)算法引導(dǎo)的對(duì)接路徑如圖5(b)。

圖5 自主充電算法對(duì)接路徑Fig.5 Docking paths of autonomous charging algorithm

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

為驗(yàn)證本文提出算法的有效性，在室內(nèi)搭建一個(gè)實(shí)驗(yàn)平臺(tái)。圖6(a)為移動(dòng)機(jī)器人運(yùn)動(dòng)底座，采用由3個(gè)麥克納姆輪構(gòu)成的360°全方位移動(dòng)平臺(tái)。在移動(dòng)平臺(tái)后方安裝充電銅條頭，并安裝1個(gè)USB攝像頭定位QR碼和紅外接收器。充電樁上，除基座、充電銅條等結(jié)構(gòu)外，還配備紅外發(fā)射管以及尺寸為10 cm×10 cm 大小的QR 碼計(jì)算機(jī)器人底座與充電樁之間的距離。另外，在自主充電算法的實(shí)驗(yàn)平臺(tái)中，充電銅條、QR碼以及紅外發(fā)射管均安裝在充電樁的水平中軸線上，紅外發(fā)射器與紅外接收器處于同一水平面，避免產(chǎn)生固定的偏差距離。設(shè)計(jì)充電樁時(shí)，考慮到相機(jī)在0.3 m處查找QR碼時(shí)丟失輪廓，在銅條上方設(shè)計(jì)0.07 m的凹平面來安裝QR碼，以提高定位精度。為提高充電角度容忍度并減小充電誤差，適當(dāng)調(diào)整充電銅條的弧度和長(zhǎng)度。圖6(b)為充電樁的實(shí)物圖。

圖6 自主充電算法實(shí)驗(yàn)平臺(tái)Fig.6 Experimental platform of autonomous charging algorithm

為能夠直觀地分析機(jī)器人在結(jié)合視覺和紅外引導(dǎo)算法的對(duì)接流程，對(duì)機(jī)器人相對(duì)于充電樁的縱向距離和橫向距離進(jìn)行實(shí)驗(yàn)。在充電樁前方2 m的范圍內(nèi)隨機(jī)進(jìn)行50次實(shí)驗(yàn)，列出其中3次自主充電對(duì)接過程中兩者的相對(duì)位置。圖7為機(jī)器人底座在充電樁右側(cè)、正前方和左側(cè)位置時(shí)在本文算法引導(dǎo)過程中相對(duì)于充電樁的位置信息。曲線ydis表示機(jī)器人距離充電樁的縱向距離；曲線xdis表示機(jī)器人距離充電樁的橫向距離。

從圖7(a)可看出：機(jī)器人底座在充電樁前1.58 m 處執(zhí)行對(duì)接任務(wù)過程中，能夠快速地由充電樁右側(cè)0.4 m處調(diào)整至0.01 m；機(jī)器人在充電樁前0.6 m處經(jīng)過外界環(huán)境的擾動(dòng)后仍能迅速恢復(fù)穩(wěn)態(tài)。根據(jù)機(jī)器人相機(jī)與QR碼的安裝位置相距0.2 m判斷，機(jī)器人能夠穩(wěn)定對(duì)接充電樁。同理，圖7(b)～(c)亦表明該算法的穩(wěn)定性強(qiáng)，具有良好的魯棒性。

圖7 機(jī)器人與充電樁的相對(duì)位置Fig.7 Relative position of robot and charging pile

在50次實(shí)驗(yàn)中，設(shè)置對(duì)接算法超時(shí)為2 min，機(jī)器人自主充電對(duì)接實(shí)驗(yàn)結(jié)果如表1。從表1可看出：50次實(shí)驗(yàn)中，成功49次，對(duì)接成功率達(dá)98%，平均對(duì)接時(shí)間38 s，有效減少了自主充電對(duì)接時(shí)間，提高了對(duì)接效率；對(duì)接實(shí)驗(yàn)過程中，1次超時(shí)失敗。在設(shè)計(jì)充電樁時(shí)為盡量增加QR 碼與相機(jī)的距離，使其能夠在一定距離情況下仍能獲取QR碼的信息，在充電樁上設(shè)計(jì)0.07 m的凹平面放置QR碼，這使得機(jī)器人在初始位置無法獲取QR碼，最終導(dǎo)致對(duì)接超時(shí)，充電對(duì)接失敗。

表1 自主充電實(shí)驗(yàn)結(jié)果Tab.1 Experimental results of autonomous charging

5 結(jié) 論

1)結(jié)合視覺和紅外信號(hào)改進(jìn)移動(dòng)機(jī)器人自主充電對(duì)接算法，采用改進(jìn)的算法既能夠防止單一視覺情況下機(jī)器人丟失充電樁位置，又能夠解決單一紅外信號(hào)耗時(shí)查找充電樁。

2)對(duì)接實(shí)驗(yàn)表明：本文算法成功實(shí)現(xiàn)了移動(dòng)機(jī)器人在室內(nèi)環(huán)境條件下自主充電對(duì)接，提高了機(jī)器人自主充電對(duì)接效率，增加其穩(wěn)定性。但該算法僅適用于充電區(qū)域無障礙物環(huán)境，且充電樁的設(shè)計(jì)限制了自主充電對(duì)接范圍，后續(xù)需進(jìn)一步優(yōu)化移動(dòng)機(jī)器人自主充電對(duì)接算法，以適應(yīng)于更復(fù)雜的環(huán)境。