多目視覺定位協(xié)同引導(dǎo)自動(dòng)運(yùn)輸車研究

曾群生，蘇成悅，程天明，張洪釗，黃佩鐸

ZENG Qunsheng1,SU Chengyue2,CHENG Tianming2,ZHANG Hongzhao2,HUANG Peiduo2

1.廣東工業(yè)大學(xué) 計(jì)算機(jī)學(xué)院，廣州 510006

2.廣東工業(yè)大學(xué) 物理與光電工程學(xué)院，廣州 510006

1.School of Computer,Guangdong University of Technology,Guangzhou 510006,China

2.School of Physics&Optoelectronic Engineering,Guangdong University of Technology,Guangzhou 510006,China

1 引言

室內(nèi)定位技術(shù)在自動(dòng)化生產(chǎn)活動(dòng)中越來越重要[1]。目前自動(dòng)運(yùn)輸車AGV（Automated Guided Vehicle）的室內(nèi)定位導(dǎo)航方式有慣性定位導(dǎo)航、循線定位導(dǎo)航、衛(wèi)星定位導(dǎo)航、超聲波定位導(dǎo)航、紅外定位導(dǎo)航、激光測距、WiFi局域網(wǎng)、藍(lán)牙技術(shù)、RFID技術(shù)、ZigBee技術(shù)、UWB技術(shù)、特殊光定位技術(shù)以及視覺定位技術(shù)等[2-6]。但上述的定位技術(shù)存在不足，如靈活性差、適用性差、不穩(wěn)定等問題。

基于視覺定位技術(shù)具有靈活性高、精度高，無需場景輔助設(shè)施的優(yōu)勢。其中全局視覺定位技術(shù)具有柔性大、靈活性高，用戶可隨時(shí)用軟件定義自動(dòng)運(yùn)輸車的運(yùn)輸路徑[7-8]。文獻(xiàn)[9]提出一種基于全局視覺定位導(dǎo)引自動(dòng)運(yùn)輸車方案，采用半徑定比例的雙紅球作為自動(dòng)運(yùn)輸車的特定標(biāo)志物。上位機(jī)通過識(shí)別雙球位置計(jì)算出自動(dòng)運(yùn)輸車的物理空間位置以及姿態(tài)方向角。但是并未解決問題：（1）雙球標(biāo)志物被遮擋無法計(jì)算出自動(dòng)運(yùn)輸車的位置問題；（2）在大范圍的作業(yè)區(qū)域，需要多個(gè)攝像頭擴(kuò)展物理空間視場協(xié)同引導(dǎo)AGV問題。

針對(duì)上述存在的兩個(gè)問題，提出一種多目攝像頭全局視覺協(xié)同定位引導(dǎo)自動(dòng)運(yùn)輸進(jìn)行路徑規(guī)劃導(dǎo)航方案。采用紅色單球半徑15.0 cm作為車體標(biāo)志以及MPU6050陀螺儀實(shí)現(xiàn)慣性導(dǎo)航，多個(gè)攝像頭協(xié)同可以實(shí)現(xiàn)物理視場空間的擴(kuò)展，適用于工廠大范圍的定位。球體標(biāo)志物的好處是無論在視場中的任意角度看都是一個(gè)圓，具有不變性。使用紅色單球做為標(biāo)志物替代雙球具有快速識(shí)別、抗干擾性強(qiáng)以及實(shí)時(shí)性高的優(yōu)勢。PC機(jī)識(shí)別出紅色單球的位置并發(fā)送給下位機(jī)自動(dòng)運(yùn)輸車，AGV根據(jù)當(dāng)前的坐標(biāo)位置和運(yùn)輸終點(diǎn)位置運(yùn)輸。同時(shí)安裝在自動(dòng)運(yùn)輸車上的陀螺儀傳感器可以感知車的二維方向和加速度姿態(tài)，當(dāng)標(biāo)志物被遮擋的時(shí)候，自動(dòng)運(yùn)輸車?yán)猛勇輧x慣性導(dǎo)航，走出遮擋區(qū)域。方案具有適用性強(qiáng)、定位范圍大、靈活性高、穩(wěn)定性好的優(yōu)勢，切合實(shí)際工廠自動(dòng)運(yùn)輸車的定位作業(yè)需求。

2 多目視覺定位協(xié)同系統(tǒng)結(jié)構(gòu)

定位系統(tǒng)總設(shè)計(jì)結(jié)構(gòu)如圖1所示，在室內(nèi)安裝多個(gè)攝像頭，各路視頻流通過USB傳輸給上位機(jī)PC。上位機(jī)系統(tǒng)識(shí)別出AGV物理位置坐標(biāo)后，通過無線WiFi發(fā)送給下位機(jī)ESP8266串口WiFi模塊。自動(dòng)運(yùn)輸車通過自身當(dāng)前的坐標(biāo)以及要求運(yùn)輸終點(diǎn)目的位置信息計(jì)算出路徑。

圖1 定位系統(tǒng)總設(shè)計(jì)結(jié)構(gòu)

3 上位機(jī)系統(tǒng)設(shè)計(jì)

上位機(jī)系統(tǒng)由OpenCV和QT平臺(tái)進(jìn)行聯(lián)合開發(fā)設(shè)計(jì)。可以實(shí)時(shí)感知運(yùn)輸車的位置信息并發(fā)送運(yùn)輸車位置坐標(biāo)，支持多點(diǎn)路徑規(guī)劃導(dǎo)航且用戶可以靈活地更改導(dǎo)航路徑。系統(tǒng)由以下三部分組成：紅色球體標(biāo)志物識(shí)別、空間視場幾何投影映射關(guān)系的建模以及多攝像頭協(xié)同視場擴(kuò)展計(jì)算。

3.1 標(biāo)志物識(shí)別

在上位機(jī)的程序設(shè)計(jì)中，紅色球體標(biāo)志物的識(shí)別是整個(gè)過程最基本的一步[10]。識(shí)別算法設(shè)計(jì)如下：

步驟1標(biāo)定測得每個(gè)鏡頭的畸變參數(shù)，然后對(duì)輸入的圖像進(jìn)行仿射變換得到矯正圖像[11]。

步驟2將采集的RGB圖像轉(zhuǎn)換為HSV顏色空間，由于HSV顏色空間色度和亮度通道是單獨(dú)分開的，因而對(duì)光照的敏感度低于RGB圖像[12]。

步驟3分割經(jīng)矯正圖像的紅色區(qū)域。HSV紅色約束域?yàn)椋荷菻∈[0 ,10]?[1 56,180]，飽和度S∈[4 6,255]，亮度V∈[4 3,255]。

步驟4對(duì)分割后的圖像做數(shù)學(xué)形態(tài)學(xué)濾波。采用數(shù)學(xué)形態(tài)學(xué)的腐蝕和膨脹操作來濾波，以增加邊沿圓滑性，同時(shí)消除圖像上孤立點(diǎn)。

步驟5采用Canny算子提取出邊沿輪廓圖像，該方法在抗噪聲和精確邊沿提取上有出色的表現(xiàn)[13]。

步驟6濾波之后創(chuàng)建面積大小以及圓形度約束條件。通過球體大小，確定面積約束范圍S'∈[500,10 000]，圓形度C∈[0.8,1.0]，識(shí)別率最高。

上述公式（1），其中P表示物體的周長，S′表示面積。圓形度的值越接近1.0表示該圖形越接近圓，可用于消除非圓輪廓。

步驟7篩選出滿足條件的輪廓并輸出目標(biāo)小球的位置。

球體標(biāo)志物識(shí)別情況如圖2識(shí)別過程。

圖2 識(shí)別算法過程

3.2 空間視場建模

視場建模是建立起圖像平面和物理空間坐標(biāo)之間的映射關(guān)系[14]，根據(jù)小孔成像的原理建立幾何投影關(guān)系，數(shù)學(xué)模型如圖3所示。

如圖3所示區(qū)域ABCD為物理空間視場，A′B′C′D′為圖像區(qū)域，O′為攝像頭光心，O作為物理空間坐標(biāo)系原點(diǎn)，O′G為光軸。H和W分別為圖像的高和寬，G′為圖像坐標(biāo)系的原點(diǎn)，取P′(u,v)為圖像上坐標(biāo)，則映射到物理空間為P(x,y)坐標(biāo)。由映射模型可得關(guān)系式：

圖3 視場映射模型

θ′、h、?h、y1、θ 分別表示為水平視場角的半角、攝像頭距離地面高度、目標(biāo)物中心距離地面高度、光軸與地面交點(diǎn)G到O點(diǎn)的距離、垂直視場角的半角，這些參數(shù)是可測量的。

3.3 多目攝像頭協(xié)同導(dǎo)引AGV

3.3.1 兩目視場擴(kuò)展結(jié)構(gòu)及步驟

圖4是以兩個(gè)攝像頭物理視場擴(kuò)建為例的模型，規(guī)劃運(yùn)輸路徑如圖中1-2-3-4-5-6。在室內(nèi)安裝兩個(gè)攝像頭，并讓它們之間存在一定的視場交叉域。通過3步實(shí)現(xiàn)視場擴(kuò)建：

步驟1計(jì)算出2號(hào)攝像頭的坐標(biāo)系和1號(hào)基準(zhǔn)攝像頭坐標(biāo)系的夾角θ；

步驟22號(hào)攝像頭坐標(biāo)系通過θ校正為跟基準(zhǔn)坐標(biāo)的方向一致的坐標(biāo)系，再計(jì)算出2號(hào)攝像頭相對(duì)基準(zhǔn)攝像頭的位置偏移量(△x,△y)；

步驟3后面2號(hào)攝像頭的坐標(biāo)位置同樣經(jīng)過θ校正再加上偏移量就是擴(kuò)展的坐標(biāo)位置。

圖4 兩目視場擴(kuò)建模型

3.3.2 兩目視場擴(kuò)展原理

設(shè)1號(hào)基準(zhǔn)攝像頭坐標(biāo)原點(diǎn)為O1，2號(hào)攝像頭坐標(biāo)原點(diǎn)為O2，在視場交叉域中任取兩個(gè)點(diǎn)A和B，視場1攝像頭計(jì)算出的物理空間坐標(biāo)為A(x1,y1),B(x2,y2)，視場2攝像頭計(jì)算出的物理空間坐標(biāo)為A(u1,v1),B(u2,v2)。其中α、β分別為AB線與1號(hào)和2號(hào)攝像頭坐標(biāo)軸的正切角，有θ=β-α，幾何關(guān)系如圖5所示。

圖5 視場擴(kuò)展數(shù)學(xué)建模

通過圖5的幾何關(guān)系，計(jì)算得到兩坐標(biāo)軸夾角θ，以及2號(hào)攝像頭偏移量(△x,△y)。

取2號(hào)攝像頭中任一點(diǎn)坐標(biāo)為C(u,v)，故其擴(kuò)展后坐標(biāo)為 C′(u′,v′)，其中有：

3.3.3 多目視場擴(kuò)展

多目視場擴(kuò)建是在兩目視場擴(kuò)展理論基礎(chǔ)上，進(jìn)行兩兩視場擴(kuò)展進(jìn)而實(shí)現(xiàn)多目視場引導(dǎo)自動(dòng)運(yùn)輸車。設(shè)1號(hào)攝像頭為基準(zhǔn)攝像頭，2號(hào)攝像頭相對(duì)1號(hào)測得擴(kuò)展參數(shù)，用向量 P1(θ1,Δx1,Δy1)表示；K∈1,2,…,Z ，第K號(hào)攝像頭相對(duì)K-1號(hào)攝像頭的擴(kuò)展參數(shù)向量為PK-1(θK-1,ΔxK-1,ΔyK-1)，則第 i號(hào)攝像頭相對(duì)1號(hào)基準(zhǔn)攝像頭測得的擴(kuò)展參數(shù)為向量 Qi(θi,Δxi,Δyi)，i∈1,2,…,K ，其中

故第i號(hào)攝像頭中任取一點(diǎn)Ji(x,y)經(jīng)參數(shù)向量Qi(θi,Δxi,Δyi)計(jì)算得擴(kuò)展坐標(biāo) J′i(x′,y′)。

4 下位機(jī)AGV導(dǎo)航控制

下位機(jī)采用一套分時(shí)雙閉環(huán)反饋機(jī)制對(duì)運(yùn)輸車進(jìn)行控制。設(shè)運(yùn)輸車當(dāng)前位置坐標(biāo)為E(x,y)，到達(dá)點(diǎn)位置坐標(biāo)F(u,v)，控制停止誤差為△d。

（1）當(dāng)球體標(biāo)志物沒被遮擋時(shí)，通過比較車當(dāng)前位置和目標(biāo)點(diǎn)位置的X方向和Y的反饋差量是否同時(shí)在設(shè)定的△d范圍內(nèi)，如果是則表示到達(dá)目的地，判斷條件如下：

（2）當(dāng)標(biāo)志物被遮擋時(shí)，運(yùn)輸車將失去上位機(jī)實(shí)時(shí)的位置坐標(biāo)，此時(shí)利用MPU6050的加速度傳感器計(jì)算自身當(dāng)前位置，從而實(shí)現(xiàn)慣性導(dǎo)航[15]。設(shè)遮擋t=0時(shí)刻，到解除遮擋t=T整個(gè)過程X方向和Y方向瞬時(shí)加速度分別為ax(t)和ay(t)，速度為vx(t)和vy(t)，位移為sx(t)和 sy(t)。

X方向和Y方向瞬時(shí)速度為：

X方向和Y方向位移為：

故判斷到達(dá)目標(biāo)點(diǎn)條件如下：

根據(jù)差量利用PID技術(shù)控制小車的運(yùn)動(dòng)狀態(tài)[16]，算法控制如圖6導(dǎo)航控制流程圖。

圖6 下位機(jī)導(dǎo)航控制

5 引導(dǎo)實(shí)驗(yàn)

本次實(shí)驗(yàn)環(huán)境在范圍比較大的工作室環(huán)境，單目定位視場范圍大致為5.0 m×5.0 m。實(shí)驗(yàn)環(huán)境如圖7所示。

實(shí)驗(yàn)前參數(shù)：

（1）攝像頭垂直視角 2 θ=70°，水平視角 2 θ′=70°；

（2）攝像頭分辨率為1 280×720，采集速度30 f/s；

（3）攝像頭高度h=285.5 cm，標(biāo)志物質(zhì)心距離地面的高度?h=51.5 cm，攝像頭光軸與地面交點(diǎn)G到O點(diǎn)的距離y1=134.6 cm。

圖7 實(shí)驗(yàn)環(huán)境

5.1 單目路徑規(guī)劃導(dǎo)航實(shí)驗(yàn)

自動(dòng)運(yùn)輸車單個(gè)攝像頭規(guī)劃路徑導(dǎo)航場景，如圖8所示。為了模擬標(biāo)志物被遮擋的情況，在場地中人為地加入了一塊比較高的擋板，當(dāng)AGV從2點(diǎn)運(yùn)輸?shù)?點(diǎn)的時(shí)候，過程中車的紅色球標(biāo)志物會(huì)被擋住，失去實(shí)時(shí)坐標(biāo)靠MPU6050陀螺儀慣性導(dǎo)航行走。并記錄運(yùn)輸車到達(dá)每一個(gè)目標(biāo)點(diǎn)的坐標(biāo)情況制表1。

圖8 單目路徑規(guī)劃導(dǎo)航

表1 單目路徑規(guī)劃導(dǎo)航 cm

表1表明AGV實(shí)際到達(dá)坐標(biāo)跟設(shè)定目標(biāo)點(diǎn)存在有±2.2 cm左右誤差，這是因?yàn)锳GV自身程序設(shè)定允許±5.0 cm左右誤差，因此該誤差屬于合理值，也是滿足自動(dòng)運(yùn)輸車定位作業(yè)的精度要求。設(shè)定±5.0 cm左右誤差是為了解決實(shí)際運(yùn)輸中超過了目標(biāo)坐標(biāo)需要運(yùn)輸車不斷調(diào)整問題。實(shí)驗(yàn)表明設(shè)定合理的誤差閾值，可以使定位系統(tǒng)更加高效、快速。

5.2 雙目協(xié)同擴(kuò)展空間誤差分析

將自動(dòng)運(yùn)輸車以不定距無規(guī)律地?cái)[放在兩個(gè)攝像頭交叉視場域中6個(gè)不同的位置，兩個(gè)攝像頭分別計(jì)算出小車的實(shí)際物理空間坐標(biāo)，制得表2視場擴(kuò)展誤差分析。

表2兩目誤差分析結(jié)果為兩個(gè)攝像頭計(jì)算物理空間坐標(biāo)誤差是±3.0 cm，理論上AGV擺放一個(gè)位置兩個(gè)攝像頭計(jì)算出來的物理空間坐標(biāo)應(yīng)該是一致的，但是由于設(shè)備的誤差、實(shí)驗(yàn)參數(shù)測量的誤差導(dǎo)致視場擴(kuò)展也存在一定的誤差，因而兩個(gè)視場計(jì)算出的坐標(biāo)就存在一定的誤差值。在定位視場寬高5.0 m×5.0 m視場范圍內(nèi)，±3.0 cm的擴(kuò)展誤差是可接受的，可實(shí)現(xiàn)正常的規(guī)劃導(dǎo)航。

表2 雙目視場擴(kuò)展誤差分析 cm

5.3 多目協(xié)同路徑規(guī)劃導(dǎo)航實(shí)驗(yàn)

以兩目擴(kuò)展導(dǎo)航為實(shí)驗(yàn)，測試場景如圖9所示。擺放好兩個(gè)攝像頭位置，讓兩個(gè)視場有一定的交叉域，開啟上位機(jī)的視場校正模式。將AGV車擺放在交叉域中兩個(gè)不同位置，軟件自動(dòng)計(jì)算出擴(kuò)展參數(shù)完成兩個(gè)視場的物理空間擴(kuò)展。記錄運(yùn)輸車到達(dá)每一個(gè)目標(biāo)點(diǎn)的坐標(biāo)情況制表3。

圖9 多目路徑規(guī)劃導(dǎo)航場景

表3 多目路徑規(guī)劃導(dǎo)航cm

AGV成功地依次經(jīng)過6個(gè)目標(biāo)導(dǎo)航點(diǎn)，最終抵達(dá)終點(diǎn)。表3顯示導(dǎo)航誤差為±5.0 cm，屬于設(shè)定的誤差范圍內(nèi)。兩目協(xié)同導(dǎo)航的成功驗(yàn)證了視場擴(kuò)展算法的正確性。如果擴(kuò)展算法錯(cuò)誤，則當(dāng)運(yùn)輸車從一個(gè)視場到達(dá)另外一個(gè)視場的時(shí)候坐標(biāo)會(huì)不一致，這個(gè)時(shí)候自動(dòng)車就會(huì)重新計(jì)算出一條新路徑，而這條路徑是不可預(yù)知的，將會(huì)帶來一些不可預(yù)料問題最終無法實(shí)現(xiàn)多目視覺多點(diǎn)規(guī)劃導(dǎo)航。同時(shí)兩目視場物理空間的擴(kuò)建推導(dǎo)理論是可以應(yīng)用于多目攝像頭視場的擴(kuò)建，且擴(kuò)展靈活性高，只需要讓攝像頭之間有一定交叉域就可以實(shí)現(xiàn)多目協(xié)同導(dǎo)引AGV運(yùn)輸作業(yè)，從而實(shí)現(xiàn)大范圍導(dǎo)航。

6 結(jié)束語

針對(duì)目前視覺定位存在的未解決問題，文中提出一種以紅色單球?yàn)檐圀w標(biāo)志物的多目視覺協(xié)同導(dǎo)引AGV運(yùn)輸車的方案并實(shí)驗(yàn)驗(yàn)證。具有以下三方面的特點(diǎn)：（1）采用單球代替雙球標(biāo)志物，具有識(shí)別速度快、穩(wěn)定性高的優(yōu)勢。選擇球體標(biāo)志物的好處是任意視角下看到的都是圓，具有不變性。（2）自動(dòng)運(yùn)輸車主控導(dǎo)航控制算法采用分時(shí)雙閉環(huán)反饋設(shè)計(jì)，通過安裝MPU6050利用慣性導(dǎo)航解決球體被遮擋情況。（3）多目攝像頭視場協(xié)同引導(dǎo)自動(dòng)運(yùn)輸車，攝像頭之間通過視場交叉域建立起統(tǒng)一的坐標(biāo)關(guān)系，解決了在寬廣的室內(nèi)環(huán)境中需要大范圍的定位導(dǎo)航作業(yè)。

定位系統(tǒng)支持單點(diǎn)和多點(diǎn)路徑規(guī)劃導(dǎo)航，可靈活更改自動(dòng)運(yùn)輸車的規(guī)劃路徑，同時(shí)通過軟件擴(kuò)展算法可靈活地實(shí)現(xiàn)多目視覺定位協(xié)同導(dǎo)引自動(dòng)運(yùn)輸車。最終實(shí)驗(yàn)結(jié)果表明：單目定位視場范圍在5.0 m×5.0 m大小內(nèi)，計(jì)算出的雙目物理視場擴(kuò)展誤差為±3.0 cm。無論是單目還是多目視覺的多點(diǎn)路徑導(dǎo)航誤差都達(dá)到±5.0 cm預(yù)期內(nèi)，具有可擴(kuò)展性好、定位范圍大、靈活性高的優(yōu)勢，可應(yīng)用于工業(yè)上復(fù)雜的室內(nèi)環(huán)境以及運(yùn)輸作業(yè)范圍要求大的物理空間場合。接下來繼續(xù)研究：（1）多車在多目視場中的調(diào)度運(yùn)輸導(dǎo)航；（2）自動(dòng)運(yùn)輸車避障功能。

參考文獻(xiàn)：

[1]趙銳，鐘榜，朱祖禮，等.室內(nèi)定位技術(shù)及應(yīng)用綜述[J].電子科技，2014（3）：154-157.

[2]Ye X，Wang Y，Hu W，et al.WarpMap：Accurate and efficient indoor location by dynamic warping in sequencetype radio-map[C]//2016 13th Annual IEEE Int Conf on Sensing，Communication，and Networking，2016：1-9.

[3]Xu D，Han L，Tan M，et al.Ceiling-based visual positioning for an indoor mobile robot with monocular vision[J].IEEE Transactions on Industrial Electronics，2009，56（5）：1617-1628.

[4]Takahashi Y.Mobile robot self localization based on multiantenna-RFID reader and IC tag textile[C]//IEEE Workshop on Advanced Robotics and Its Social Impacts，Tokyo，2013：106-112.

[5]Huang H，Yang A，F(xiàn)eng L，et al.Indoor positioning method based on metameric white light sources and subpixels on a color image sensor[J].IEEE Photonics Journal，2016，8（6）：1-10.

[6]李金超.基于視覺的自動(dòng)尋跡AGV控制系統(tǒng)研究[D].河北秦皇島：燕山大學(xué)，2014.

[7]李君.全局視覺導(dǎo)航AGV控制原理與技術(shù)的研究[D].武漢：武漢理工大學(xué)，2007.

[8]易弘.AGV視覺導(dǎo)航研究[J].國外電子測量技術(shù)，2010（2）：44-46.

[9]林劍冰，蘇成悅，鄭俊波，等.機(jī)器視覺導(dǎo)引的室內(nèi)自動(dòng)運(yùn)輸車定位系統(tǒng)[J].機(jī)械科學(xué)與技術(shù)，2015（11）：1675-1681.

[10]Vaishnavi D，Subashini T S.Recognizing image splicing forgeries using histogram features[C]//Mec Int Conf on Big Data&Smart City，2016：1-4.

[11]涂波，劉璐，劉一會(huì)，等.一種擴(kuò)展小孔成像模型的魚眼相機(jī)矯正與標(biāo)定方法[J].自動(dòng)化學(xué)報(bào)，2014（4）：653-659.

[12]趙紅雨，吳樂華，史燕軍，等.基于HSV顏色空間的運(yùn)動(dòng)目標(biāo)檢測方法[J].現(xiàn)代電子技術(shù)，2013（12）：45-48.

[13]趙同剛，陳迅.基于監(jiān)督學(xué)習(xí)的Canny圖像邊緣檢測改進(jìn)算法研究[J].半導(dǎo)體光電，2016（5）：731-734.

[14]王文波.基于單目視覺的實(shí)時(shí)測距方法研究[D].遼寧大連：大連理工大學(xué)，2014.

[15]王旭鳳.基于慣性導(dǎo)航的室內(nèi)定位系統(tǒng)的研究[D].長春：吉林大學(xué)，2015.

[16]張奎，雷勇.無刷直流電機(jī)智能PID控制系統(tǒng)建模與仿真[J].測控技術(shù)，2015（4）：81-84.