李少雄，張黎明，曹筱歐，郝爽，徐志宏

(1.國(guó)網(wǎng)天津市電力公司 濱海供電分公司，天津 300450；2.國(guó)網(wǎng)天津市電子公司 城東供電分公司，天津 300010)

0 引言

充電是電動(dòng)公交車運(yùn)營(yíng)過(guò)程中的重要輔助工作，人工操作勞動(dòng)強(qiáng)度大、成本高，這促使充電作業(yè)向機(jī)械化和自動(dòng)化發(fā)展。近年來(lái)，機(jī)器人已廣泛應(yīng)用于汽車行業(yè)，充電機(jī)器人研究成為國(guó)內(nèi)外機(jī)器人領(lǐng)域的熱點(diǎn)之一。采用機(jī)器人代替人工自動(dòng)插接充電槍是電動(dòng)公交車運(yùn)營(yíng)的未來(lái)趨勢(shì)[1]。機(jī)器人在電動(dòng)公交車停車位置不可能重復(fù)的情況下代替人工操作，必須能自主感知充電孔的位置，同時(shí)具有靈活、可靠的控制系統(tǒng)。

目前，已有學(xué)者將智能制造領(lǐng)域的技術(shù)應(yīng)用于充電機(jī)器人感知與控制。姚安慶采用雙目視覺(jué)感知定位充電孔，實(shí)驗(yàn)研究表明只在特定場(chǎng)景下具有可行性[2]。段崢祺研究了一種基于特征點(diǎn)匹配的充電接口感知定位算法，即通過(guò)控制三維運(yùn)動(dòng)裝置使得采集圖像與標(biāo)準(zhǔn)圖像完全匹配，但實(shí)際充電槍與充電孔插接時(shí)，無(wú)法采集充電孔圖像，該算法的可行性還需驗(yàn)證[3]。紀(jì)柱只研究了電動(dòng)汽車充電口識(shí)別算法，沒(méi)有根據(jù)充電機(jī)器人研究標(biāo)定方法[4]。馬立焱、王曉安采用超聲波感知定位充電孔，需要改造充電孔并設(shè)置超聲波接收探頭，實(shí)用性不足[5-6]。孫成研究的充電孔感知方法需要添加外部特征，再應(yīng)用單目視覺(jué)基于圓特征的位姿測(cè)量算法[7]。張輝等研究了基于曲率濾波和反向P-M擴(kuò)散的充電孔視覺(jué)感知方法，但沒(méi)有給出手眼標(biāo)定方法，所得結(jié)果不能用于機(jī)器人控制[8]。與充電孔感知方法研究相比，充電機(jī)器人控制系統(tǒng)研究不足，僅施瑩在研究雙目視覺(jué)傳感器感知充電孔的位置時(shí)，設(shè)計(jì)用于機(jī)器人自動(dòng)插接軌跡規(guī)劃實(shí)驗(yàn)研究的控制電路，尚不具有工程應(yīng)用價(jià)值。

本文針對(duì)面向電動(dòng)公交車的充電機(jī)器人，根據(jù)其工作過(guò)程研究感知與控制系統(tǒng)。

1 方案設(shè)計(jì)

面向電動(dòng)公交車的充電機(jī)器人如圖1所示。當(dāng)接收到充電指令后，機(jī)器人從充電樁抓取充電槍，運(yùn)動(dòng)到電動(dòng)公交車充電孔正前方，感知充電孔位置后將充電槍插入充電孔，充電樁啟動(dòng)充電。充電完成后，機(jī)器人拔出充電槍并送回充電樁。機(jī)器人需要配置合適的感知與控制系統(tǒng)才能滿足應(yīng)用要求。在感知方面，雙目視覺(jué)如果不能同時(shí)拍攝到充電孔，則無(wú)法確定空間位置，而線激光掃描方法和3D視覺(jué)系統(tǒng)成本較高，綜合考慮可靠性和經(jīng)濟(jì)性因素，機(jī)器人采用激光測(cè)距儀和單目視覺(jué)感知充電孔的位置。手動(dòng)示教表明，對(duì)于同型號(hào)電動(dòng)公交車和充電槍，只需感知充電孔在機(jī)器人坐標(biāo)系中x、y和z軸的坐標(biāo)，定位誤差在0.5 mm內(nèi)都能夠成功插接充電槍；在控制方面，示教確定充電槍姿態(tài)后，機(jī)器人根據(jù)感知結(jié)果通過(guò)平移直線運(yùn)動(dòng)就可以將充電槍與充電孔插接。此外，在整個(gè)動(dòng)作過(guò)程中，機(jī)器人還需與其末端的機(jī)械手、故障處理等協(xié)調(diào)控制才具有工程應(yīng)用價(jià)值。

圖1 充電機(jī)器人

充電機(jī)器人感知與控制系統(tǒng)結(jié)構(gòu)如圖2所示。系統(tǒng)各部分通過(guò)路由器構(gòu)成控制局域網(wǎng)，觸摸屏(HMI)用于供用戶操作和查看工作狀態(tài)，相機(jī)和工業(yè)控制計(jì)算機(jī)完成圖像采集與處理，可編程控制器讀取激光測(cè)距儀測(cè)量值、控制充電樁和機(jī)械手，同時(shí)與工業(yè)控制計(jì)算機(jī)協(xié)同控制機(jī)器人。

圖2 充電機(jī)器人感知與控制系統(tǒng)結(jié)構(gòu)

2 充電機(jī)器人感知

充電機(jī)器人感知即識(shí)別充電孔，再通過(guò)手眼標(biāo)定與激光測(cè)距儀、機(jī)器人配合確定充電孔在機(jī)器人坐標(biāo)系中的位置。

2.1 充電孔識(shí)別

機(jī)器人感知充電孔在Oyz平面坐標(biāo)的前提是識(shí)別和定位充電孔在采集圖像中的位置，本文采用模板匹配實(shí)現(xiàn)。充電孔的采集圖像如圖3所示。從圖中可以看出充電孔的內(nèi)部結(jié)構(gòu)復(fù)雜，灰度特征不明顯，特征顯著的是充電孔直流正負(fù)極的邊緣，采用基于邊緣特征的形狀匹配算法在可行性、抗噪性和定位精度方面優(yōu)于灰度匹配。應(yīng)用形狀匹配首先需要?jiǎng)?chuàng)建模板，創(chuàng)建時(shí)首先在采集圖像中應(yīng)用交互式感興趣區(qū)域(ROI)定義功能，創(chuàng)建包含直流正負(fù)極的矩形ROI，通過(guò)對(duì)角坐標(biāo)確定ROI區(qū)域的大小，并從采集圖像中截取ROI圖像，再?gòu)慕厝D像中提取直流正負(fù)極的邊緣。邊緣檢測(cè)的效果決定了充電孔識(shí)別的準(zhǔn)確性。已有研究表明，Canny算子在信噪比、檢測(cè)精度和抗噪聲性能方面優(yōu)于Roberts、Sobel、Prewitt等其他邊緣檢測(cè)算子，能夠檢測(cè)出較多、較細(xì)的邊緣點(diǎn)信息[9]。本文應(yīng)用Canny算子檢測(cè)直流正負(fù)極的邊緣，根據(jù)檢測(cè)的邊緣創(chuàng)建用于匹配的模板。創(chuàng)建模板時(shí)，圖像金字塔級(jí)數(shù)與匹配速度、匹配成功率相關(guān)，金字塔級(jí)數(shù)值越大匹配速度越快，但匹配成功率較低?？蓹?quán)衡準(zhǔn)確性和搜索速度的選擇，使匹配算法在兩方面均能被充電孔識(shí)別應(yīng)用接受。由于實(shí)際電動(dòng)公交車的充電孔不可能存在明顯的旋轉(zhuǎn)角度，模板可能發(fā)生旋轉(zhuǎn)的范圍就可以設(shè)置一個(gè)很小的范圍，從而提高匹配速度。

感知過(guò)程中，從采集圖像中通過(guò)形狀匹配確定充電孔中心的像素坐標(biāo)，需要選擇合適的算法參數(shù)才能滿足準(zhǔn)確性和匹配速度的要求。需要確定的參數(shù)有最小匹配分值、金字塔級(jí)數(shù)、搜索精度控制，其中最小匹配分值越小，匹配時(shí)越容易識(shí)別，但為了防止充電孔誤定位導(dǎo)致充電機(jī)器人或充電孔損壞，在保證匹配成功的情況下，應(yīng)盡量增加最小匹配分值；搜索時(shí)金字塔級(jí)數(shù)一般使用創(chuàng)建模板時(shí)的級(jí)數(shù)，同時(shí)使用基于最小二乘法的亞像素精度匹配保證充電孔定位精度；由于實(shí)際充電孔旋轉(zhuǎn)角度非常小，可以限定搜索角度范圍加快搜索速度。在采集圖像中，通過(guò)形狀匹配定位充電孔中心的實(shí)例如圖3所示。

圖3 形狀匹配結(jié)果

2.2 手眼標(biāo)定

識(shí)別和定位獲得的是充電孔在采集圖像中的像素坐標(biāo)，還必須通過(guò)手眼標(biāo)定建立像素坐標(biāo)系與機(jī)器人坐標(biāo)系的對(duì)應(yīng)關(guān)系。確定充電孔的位置坐標(biāo)才能用于機(jī)器人控制，手眼標(biāo)定是充電機(jī)器人感知的關(guān)鍵[10]。

手眼標(biāo)定模型如圖4所示，OR-xRyRzR為機(jī)器人坐標(biāo)系，OF-UV為像素坐標(biāo)系，P(y,z)是機(jī)器人坐標(biāo)系中的點(diǎn)，Q(u,v)是P在相機(jī)成像平面的投影點(diǎn)，P(y,z)和Q(u,v)可以通過(guò)旋轉(zhuǎn)和平移來(lái)轉(zhuǎn)換，表示為

圖4 手眼標(biāo)定模型

(1)

通過(guò)3組不在一條直線的P、Q坐標(biāo)，即可求解出旋轉(zhuǎn)矩陣R和平移矩陣T。為了便于用矩陣運(yùn)算來(lái)表示仿射變換，式(1)可寫為

(2)

其中A為仿射矩陣。

標(biāo)定過(guò)程中，通常用具有固定參考點(diǎn)的標(biāo)定板和標(biāo)定尖端，分別確定中心點(diǎn)的像素坐標(biāo)和機(jī)器人坐標(biāo)。但充電孔的位置內(nèi)嵌于車身裝飾蓋板上，傳統(tǒng)標(biāo)定方法不具備可操作性，而充電機(jī)器人采用手部相機(jī)，實(shí)際標(biāo)定時(shí)可以固定充電孔，保持相機(jī)拍攝姿態(tài)和距離不變，通過(guò)移動(dòng)機(jī)器人和相機(jī)采用模板匹配獲得多組不同的P(y,z)和Q(u,v)。為減小手眼標(biāo)定誤差，實(shí)際標(biāo)定一般采用9組點(diǎn)[11-12]?？刂茩C(jī)器人在Oyz平面內(nèi)按“田”字型運(yùn)動(dòng)，移動(dòng)到“田”字的9個(gè)交叉點(diǎn)，記錄9個(gè)位置點(diǎn)的機(jī)器人坐標(biāo)和匹配得到的模板中心像素坐標(biāo)，手眼標(biāo)定流程如圖5所示。

圖5 手眼標(biāo)定流程

2.3 感知步驟

充電機(jī)器人將充電槍與充電孔插接前，感知充電孔位置的步驟為：

1)機(jī)器人移動(dòng)到標(biāo)準(zhǔn)拍攝位置P0(x0,y0,z0)；

2)讀取激光測(cè)距儀的檢測(cè)值l，并判斷檢測(cè)值是否在量程范圍內(nèi)，如果超出量程，跳轉(zhuǎn)到8)輸出位置感知錯(cuò)誤；

3)計(jì)算充電孔在機(jī)器人坐標(biāo)系x軸相對(duì)示教時(shí)變化量Δx=l-l0；

5)相機(jī)采集圖像并進(jìn)行形狀匹配，匹配成功，則確定充電孔模板中心的像素坐標(biāo)(u,v)，匹配失敗，跳轉(zhuǎn)到8)輸出位置感知錯(cuò)誤；

6)根據(jù)仿射變換計(jì)算充電孔模板中心在機(jī)器人坐標(biāo)系中的坐標(biāo)(y,z)；

8)輸出位置感知錯(cuò)誤。

3 充電機(jī)器人控制

充電機(jī)器人運(yùn)行過(guò)程中需要根據(jù)感知系統(tǒng)的輸出，在線修改插槍動(dòng)作的目標(biāo)位置坐標(biāo)才能將充電槍插入充電孔，和取槍充電、充完收槍機(jī)器人動(dòng)作軌跡不同，在機(jī)器人控制器上實(shí)現(xiàn)過(guò)程控制程序靈活性不足[13]。PLC和IPC協(xié)同控制機(jī)器人的方案如圖6所示，PLC程序決策動(dòng)作執(zhí)行、動(dòng)作順序，IPC根據(jù)相應(yīng)動(dòng)作向機(jī)器人控制器發(fā)送目標(biāo)位置坐標(biāo)，機(jī)器人控制器僅執(zhí)行具體動(dòng)作。

圖6 機(jī)器人協(xié)同控制原理框圖

3.1 機(jī)器人控制器程序

充電機(jī)器人取槍充電、充完收槍動(dòng)作過(guò)程的運(yùn)動(dòng)軌跡可分解為一個(gè)或多個(gè)轉(zhuǎn)出、轉(zhuǎn)入、直線動(dòng)作，機(jī)器人控制器程序只需根據(jù)PLC輸出的執(zhí)行條件執(zhí)行動(dòng)作指令，流程圖如圖7所示。輪詢動(dòng)作執(zhí)行條件，條件使能時(shí)機(jī)器人執(zhí)行相應(yīng)的動(dòng)作指令，動(dòng)作完成后，機(jī)器人控制器輸出完成信號(hào)反饋到PLC，PLC復(fù)位動(dòng)作執(zhí)行條件。

圖7 機(jī)器人控制器程序流程圖

3.2 PLC控制程序

根據(jù)實(shí)際充電機(jī)器人充電作業(yè)過(guò)程，PLC控制程序分為取槍充電、充完收槍兩個(gè)部分。PLC程序采用狀態(tài)轉(zhuǎn)移法設(shè)計(jì)，根據(jù)運(yùn)動(dòng)軌跡將動(dòng)作過(guò)程劃分為若干狀態(tài)進(jìn)行順序控制，每一狀態(tài)對(duì)應(yīng)一個(gè)動(dòng)作，同時(shí)PLC置位標(biāo)志位，通知IPC向機(jī)器人控制器發(fā)送當(dāng)前狀態(tài)的目標(biāo)位置坐標(biāo)，IPC發(fā)送完成后復(fù)位標(biāo)志位。充電作業(yè)過(guò)程涉及的功能組件都向PLC返回執(zhí)行結(jié)果，當(dāng)PLC檢測(cè)到當(dāng)前狀態(tài)的動(dòng)作完成后，轉(zhuǎn)移到下一個(gè)狀態(tài)，直至過(guò)程結(jié)束。取槍充電過(guò)程如圖8所示。機(jī)器人從充電樁抓取充電槍插入電動(dòng)公交車的充電孔，包含20個(gè)狀態(tài)，圖中橢圓矩形表示機(jī)器人動(dòng)作，除轉(zhuǎn)入、轉(zhuǎn)出動(dòng)作外，其他動(dòng)作都是直線運(yùn)動(dòng)。PLC程序互鎖輸出執(zhí)行條件保證機(jī)器人動(dòng)作的唯一性；機(jī)器人故障、動(dòng)作軌跡規(guī)劃失敗或急停時(shí)，轉(zhuǎn)移到機(jī)器人故障狀態(tài)，發(fā)出報(bào)警，由人工介入處理；距離檢測(cè)異?；蛞曈X(jué)系統(tǒng)異常、充電樁故障時(shí)，程序輸出故障提示和報(bào)警。

圖8 取槍充電過(guò)程狀態(tài)轉(zhuǎn)移圖

充完收槍過(guò)程如圖9所示?？刂茩C(jī)器人從電動(dòng)公交車充電孔拔出充電槍送回充電樁，充完收槍過(guò)程中的狀態(tài)名稱定義和取槍充電一致的原因是機(jī)器人位置坐標(biāo)相同，而且收槍時(shí)抓取充電槍的位置坐標(biāo)是在取槍充電過(guò)程中確定的，區(qū)別僅在于具有相反的機(jī)械手?jǐn)y槍、無(wú)槍狀態(tài)，這樣可以簡(jiǎn)化IPC與機(jī)器人控制器的通信程序。此外，在取槍充電過(guò)程中發(fā)生距離檢測(cè)異?；蛞曈X(jué)系統(tǒng)異常時(shí)，自動(dòng)跳轉(zhuǎn)到充完收槍轉(zhuǎn)入狀態(tài)，將充電槍送回充電樁。

圖9 充完收槍過(guò)程狀態(tài)轉(zhuǎn)移圖

PLC程序還包括機(jī)械手邏輯控制、激光測(cè)距儀模擬量采集及距離計(jì)算、通信看門狗等功能，應(yīng)用位邏輯、比較指令和定時(shí)器等，采用梯形圖可以很容易實(shí)現(xiàn)。

3.3 IPC控制程序

IPC控制程序的功能是根據(jù)PLC工作過(guò)程的動(dòng)作標(biāo)志位與機(jī)器人控制器通信，發(fā)送動(dòng)作軌跡坐標(biāo)、充電孔位置坐標(biāo)和查詢機(jī)器人工作狀態(tài)。IPC需要同時(shí)和PLC、機(jī)器人控制器通信，采用具有網(wǎng)絡(luò)開(kāi)發(fā)工具、支持多種底層網(wǎng)絡(luò)協(xié)議的高級(jí)語(yǔ)言設(shè)計(jì)。

PLC支持OPC、TCP、MODBUS-TCP等多種協(xié)議，通過(guò)以太網(wǎng)通信，其中OPC通信無(wú)需設(shè)計(jì)底層通信程序，具有配置簡(jiǎn)單、使用方便的優(yōu)點(diǎn)[14]。OPC服務(wù)器軟件包含PLC驅(qū)動(dòng)，通過(guò)添加變量和設(shè)置PLC元件地址就能實(shí)現(xiàn)讀寫PLC數(shù)據(jù)。在IPC控制軟件中添加OPC客戶端、創(chuàng)建綁定變量就可以與PLC中的數(shù)據(jù)建立對(duì)應(yīng)關(guān)系。IPC程序中只需建立與PLC程序中動(dòng)作狀態(tài)標(biāo)志位對(duì)應(yīng)的共享變量，通過(guò)OPC服務(wù)器讀寫并同步更新。

機(jī)器人控制器提供基于TCP以客戶端/服務(wù)器模式的通信控制方式，IPC需要向機(jī)器人控制器發(fā)送狀態(tài)查詢幀和控制命令幀，查詢數(shù)據(jù)包括機(jī)器人當(dāng)前坐標(biāo)和工作狀態(tài)，控制命令幀為修改目標(biāo)位置坐標(biāo)。

4 實(shí)驗(yàn)驗(yàn)證與應(yīng)用測(cè)試

為驗(yàn)證充電機(jī)器人感知與控制系統(tǒng)的可行性和有效性，設(shè)計(jì)了如圖10所示的實(shí)驗(yàn)平臺(tái)。充電孔安裝支架可以在底板上的槽中整體移動(dòng)，槽的方向與機(jī)器人坐標(biāo)軸方向一致，可以模擬不同的停車位置。

圖10 實(shí)驗(yàn)平臺(tái)

手眼標(biāo)定時(shí)，先通過(guò)示教機(jī)器人得到充電孔的標(biāo)準(zhǔn)位置坐標(biāo)為PI(338,1 227,791)mm，選擇拍攝距離為400 mm，標(biāo)準(zhǔn)拍攝位置坐標(biāo)為P0(182,1 240,650)mm，標(biāo)準(zhǔn)拍攝位置與充電孔位置的偏移量Δy、Δz分別為-13 mm、141 mm。設(shè)置“田”字交叉點(diǎn)間距為30 mm，手眼標(biāo)定數(shù)據(jù)如表1所示，計(jì)算仿射變換矩陣A為

表1 拍攝位置手眼標(biāo)定數(shù)據(jù)

序號(hào)機(jī)器人坐標(biāo)(y, z)/mm像素坐標(biāo)(u, v)/像素1(1 240.00, 650.00)(686.27, 474.95)2(1 270.00, 650.00)(516.56, 477.52)3(1 270.00, 680.00)(522.26, 301.78)4(1 240.00, 680.00)(680.33, 306.92)5(1 210.00, 680.00)(861.20, 308.04)6(1 210.00, 650.00)(855.46, 472.74)7(1 210.00, 620.00)(850.85, 647.18)8(1 240.00, 620.00)(691.69, 645.21)9(1 270.00, 620.00)(511.51, 641.93)

針對(duì)充電孔固定位置，機(jī)器人在標(biāo)準(zhǔn)拍攝位置10次感知的結(jié)果如表2所示。y、z軸坐標(biāo)相對(duì)實(shí)際位置最大誤差不超過(guò)0.5 mm，重復(fù)性能夠滿足要求。

表2 感知重復(fù)性測(cè)試數(shù)據(jù)

進(jìn)一步，通過(guò)定量改變充電孔的位置驗(yàn)證感知準(zhǔn)確性和控制可靠性，部分實(shí)驗(yàn)結(jié)果如表3所示。x軸的實(shí)際坐標(biāo)誤差較大的原因是充電孔端面與機(jī)器人Oyz平面不平行，人工無(wú)法準(zhǔn)確測(cè)量實(shí)際坐標(biāo)，但在標(biāo)準(zhǔn)拍攝位置，機(jī)器人通過(guò)激光位移傳感器能夠感知拍攝距離的變化量，并計(jì)算出目標(biāo)坐標(biāo)。結(jié)果表明，機(jī)器人能適應(yīng)充電孔端面在三軸方向存在的微小偏轉(zhuǎn)角。

表3 感知與控制測(cè)試結(jié)果

實(shí)驗(yàn)驗(yàn)證感知與控制方法后，充電機(jī)器人在某公交充電站現(xiàn)場(chǎng)試運(yùn)行，如圖11所示。統(tǒng)計(jì)表明，在充電孔感知成功時(shí)，PLC、IPC與機(jī)器人控制器都能協(xié)調(diào)自動(dòng)完成充電動(dòng)作過(guò)程；當(dāng)停車距離超過(guò)激光測(cè)距儀的量程時(shí)，以及電動(dòng)公交車停車位置超過(guò)規(guī)定范圍、充電孔圖像采集不完整時(shí)，機(jī)器人都能將充電槍送回充電樁并報(bào)警。其他失敗的原因是停車時(shí)車身與停車線的夾角超過(guò)設(shè)計(jì)范圍，與示教時(shí)偏差較大，相機(jī)光軸與充電孔端面法向之間的夾角過(guò)大，而視覺(jué)系統(tǒng)的檢測(cè)閾值設(shè)置偏低，充電孔感知坐標(biāo)誤差超過(guò)允許范圍，插槍卡阻導(dǎo)致機(jī)器人過(guò)載并保護(hù)性停機(jī)。

圖11 充電機(jī)器人樣機(jī)

5 結(jié)語(yǔ)

本文根據(jù)充電機(jī)器人的工作過(guò)程和控制要求研究了感知與控制系統(tǒng)。感知系統(tǒng)通過(guò)激光測(cè)距儀和眼在手上的單目視覺(jué)，與機(jī)器人配合感知充電孔在機(jī)器人坐標(biāo)系中的位置?；谑窘太@得的標(biāo)準(zhǔn)拍攝位置、拍攝距離和充電孔標(biāo)準(zhǔn)位置，機(jī)器人通過(guò)激光測(cè)距儀感知充電孔x軸位置變化量，并調(diào)整拍攝位置，在采集圖像中通過(guò)形狀匹配、結(jié)合手眼標(biāo)定感知y軸和z坐標(biāo)?；赑LC和IPC的協(xié)同控制系統(tǒng)實(shí)現(xiàn)充電過(guò)程自動(dòng)控制，PLC與機(jī)器人控制器采用IO輸出動(dòng)作條件、查詢動(dòng)作結(jié)果，IPC通過(guò)OPC讀取動(dòng)作標(biāo)志位，并采用TCP向機(jī)器人發(fā)送動(dòng)作目標(biāo)位置坐標(biāo)。實(shí)驗(yàn)和應(yīng)用結(jié)果表明，充電孔感知誤差不超過(guò)0.5 mm，控制系統(tǒng)功能完善、可靠性好，在電動(dòng)公交車運(yùn)營(yíng)過(guò)程中具有較強(qiáng)的實(shí)用性。