江豪杰,周 鋒,陸欣云,王培楠,朱 寧,姜翟躍
(南京工程學(xué)院 信息與通信工程學(xué)院,江蘇 南京 211167)
機(jī)械臂是現(xiàn)代工業(yè)革命變革、現(xiàn)代工業(yè)水平高度提高催生的一種新技術(shù)產(chǎn)品,從較高層次來(lái)說(shuō),機(jī)械臂是集機(jī)械設(shè)計(jì)、計(jì)算機(jī)程序控制等多領(lǐng)域知識(shí)和多種設(shè)計(jì)方法于一身的一種新型自動(dòng)化裝備,特別是近年來(lái)互聯(lián)網(wǎng)、大數(shù)據(jù)的發(fā)展和應(yīng)用機(jī)械臂已開(kāi)始從自動(dòng)化向智能化領(lǐng)域邁進(jìn)。FPGA技術(shù)應(yīng)用于步進(jìn)電機(jī)控制領(lǐng)域的技術(shù)優(yōu)勢(shì)在于FPGA控制實(shí)時(shí)性高,單片機(jī)實(shí)時(shí)控制能力弱。并且FPGA內(nèi)部有集成鎖相環(huán),可實(shí)現(xiàn)外部時(shí)鐘的倍頻,適合高速場(chǎng)合,單片機(jī)的運(yùn)行速度是比不上FPGA的。
圖1主控芯片采用Alter公司的EP4CE10F17C8芯片和勘智公司的K210芯片。實(shí)現(xiàn)根據(jù)由K210芯片驅(qū)動(dòng)攝像頭識(shí)別指定顏色物體后將其坐標(biāo)通過(guò)串口發(fā)送給FPGA,使其產(chǎn)生相應(yīng)的PWM波驅(qū)動(dòng)電機(jī)以使機(jī)械臂抓取相應(yīng)顏色的物體。
圖1 系統(tǒng)框圖
通過(guò)UG軟件設(shè)計(jì)整個(gè)機(jī)械臂的結(jié)構(gòu),通過(guò)激光切割機(jī)加工制作。機(jī)械臂為三自由度關(guān)節(jié)構(gòu)成。
建模分析:
圖2便是在CAD作的三維建模前的準(zhǔn)備。
圖2 三維建模前的準(zhǔn)備
首先,選擇三自由度機(jī)械臂的側(cè)視圖為分析面。分別以軸承底座,關(guān)節(jié)1,關(guān)節(jié)2,末端執(zhí)行機(jī)構(gòu)建立坐標(biāo)系,量出各個(gè)需要的角度以及長(zhǎng)度,為在solidworks中三維建模以及后續(xù)進(jìn)行機(jī)械臂的運(yùn)動(dòng)學(xué)分析做好準(zhǔn)備。
其次,在solidworks根據(jù)CAD作的平面圖完整詳細(xì)地建模出這個(gè)機(jī)械臂的整體結(jié)構(gòu),以便于后續(xù)分析。雖然在solidworks中建模能夠很好地表示出整個(gè)三自由度機(jī)械臂的外形結(jié)構(gòu),但是仍然無(wú)法觀察到機(jī)械臂的內(nèi)部結(jié)構(gòu),于是便只能選擇作機(jī)械臂的爆炸圖。當(dāng)我們作完機(jī)械臂的爆炸圖之后便可以清楚地觀察到整個(gè)三自由度機(jī)械臂的具體組成:軸承底座,支撐關(guān)節(jié)1的外殼,支撐關(guān)節(jié)2的外殼,末端執(zhí)行機(jī)構(gòu),以及最核心的凸輪四桿機(jī)構(gòu)。
此時(shí),便可以發(fā)現(xiàn)軸承底座的作用是對(duì)整個(gè)機(jī)械臂進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng),但最核心的仍然是這個(gè)凸輪四桿機(jī)構(gòu),我們需要對(duì)其再次分析,重新建模才能更清楚地了解機(jī)械臂的運(yùn)動(dòng)原理。由于關(guān)節(jié)2的外殼僅僅是與三角件連接機(jī)構(gòu)鉸接并無(wú)其他作用,于是便可以把它簡(jiǎn)化成一個(gè)長(zhǎng)度相同、質(zhì)量相同的桿。
硬件電路主要由電機(jī)驅(qū)動(dòng)模塊以及K210板卡模塊組成,在電機(jī)驅(qū)動(dòng)模塊的設(shè)計(jì)中加入了光電感應(yīng)開(kāi)關(guān)以及限位開(kāi)關(guān)。電機(jī)驅(qū)動(dòng)模塊通過(guò)輸入端的坐標(biāo)從而控制FPGA產(chǎn)生相應(yīng)的PWM波來(lái)控制電機(jī)的輸出狀態(tài)。K210板卡模塊則是負(fù)責(zé)顏色識(shí)別與所要識(shí)別目標(biāo)的坐標(biāo)的發(fā)送功能。傳感系統(tǒng)由光電感應(yīng)開(kāi)關(guān)和限位開(kāi)關(guān)構(gòu)成,輔助機(jī)械臂可以更加穩(wěn)定地抓取目標(biāo)物體。
機(jī)械臂的動(dòng)作采用步進(jìn)電機(jī)驅(qū)動(dòng),步進(jìn)電機(jī)采用PL42B010B的型號(hào),驅(qū)動(dòng)電流為0.7A,步進(jìn)電機(jī)驅(qū)動(dòng)芯片采用Allegros公司出品的A4988雙極型微步電動(dòng)機(jī)驅(qū)動(dòng)器。該驅(qū)動(dòng)器具有可調(diào)限流、過(guò)流、過(guò)熱保護(hù),以及五種不同的細(xì)分(最低為1/16步)。它的工作電壓為8-35V,在沒(méi)有散熱片或有空氣流通的時(shí)候,每相最高可提供約為1A的電流(在有足夠冷卻的情況下,每相最高可提供約為2A的電流)。根據(jù)FPGA產(chǎn)生的不同的PWM波驅(qū)動(dòng)電機(jī)以不同方式的轉(zhuǎn)動(dòng),來(lái)達(dá)到抓取不同位置的物體。
顏色識(shí)別部分采用勘智公司的以K210為主控芯片的Maix py bit產(chǎn)品來(lái)驅(qū)動(dòng)OV7725來(lái)進(jìn)行物體顏色的識(shí)別及物體坐標(biāo)的發(fā)送。Maix py bit是基于microPython的嵌入式機(jī)器視覺(jué)模塊,目標(biāo)是成為機(jī)器視覺(jué)界的“Arduino”。它成本低易拓展,開(kāi)發(fā)環(huán)境友好,除了用于圖像處理外,還可以用microPython調(diào)用其硬件資源,進(jìn)行I/O控制,與現(xiàn)實(shí)世界進(jìn)行交互。實(shí)現(xiàn)顏色識(shí)別的過(guò)程中,首先在軟件中的機(jī)器視覺(jué)欄中根據(jù)當(dāng)前環(huán)境的亮度進(jìn)行閾值編輯,從而實(shí)現(xiàn)對(duì)指定顏色的識(shí)別與追蹤。將Maix py bit上的15腳作為串口數(shù)據(jù)發(fā)送端口,接到FPGA上的串口數(shù)據(jù)接收端口,同理,將Maix py bit上的17腳作為接收端口并與FPGA上的數(shù)據(jù)發(fā)送端口相連。同時(shí)將兩塊芯片進(jìn)行共地以確保數(shù)據(jù)準(zhǔn)確實(shí)時(shí)的傳輸。
串口發(fā)送坐標(biāo)中在有效數(shù)據(jù)之前加上B3、B4作為起始標(biāo)志符,有效數(shù)據(jù)之后加上B5、B6作為末尾標(biāo)志符,以方便通過(guò)串口將數(shù)據(jù)傳輸至FPGA進(jìn)行處理的時(shí)候,防止意外狀況從而處理錯(cuò)誤的坐標(biāo)。
3.3.1 限位開(kāi)關(guān)
在機(jī)械臂三個(gè)軸的起始位置裝配了機(jī)械限位開(kāi)關(guān),它一方面對(duì)每一個(gè)軸的運(yùn)動(dòng)極限進(jìn)行保護(hù),另一方面將所有軸回歸到限位開(kāi)關(guān)的位置對(duì)應(yīng)的機(jī)械臂末端執(zhí)行機(jī)構(gòu)的位置定義為原點(diǎn),從而可以對(duì)后續(xù)機(jī)械臂的位置控制建立參考原點(diǎn)。
3.3.2 光電感應(yīng)開(kāi)關(guān)
傳感器采用光電感應(yīng)開(kāi)光來(lái)檢測(cè)物體是否到來(lái),檢測(cè)到物體后發(fā)送信號(hào)到芯片從而給電機(jī)驅(qū)動(dòng)模塊。
圖3為電機(jī)驅(qū)動(dòng)原理圖。
圖3 電機(jī)驅(qū)動(dòng)原理圖
4.1.1 顏色識(shí)別
使用find_blobs函數(shù)來(lái)查找顏色圖像中的顏色色塊。為了使色彩追蹤效果好應(yīng)當(dāng)在非常受控制的照明環(huán)境中進(jìn)行實(shí)驗(yàn)。圖像選擇色彩圖RGB565的格式,選用QVGA的速度來(lái)進(jìn)行識(shí)別。實(shí)驗(yàn)中關(guān)閉白平衡,白平衡是默認(rèn)開(kāi)啟的,在顏色識(shí)別實(shí)驗(yàn)中需要將其關(guān)閉。
設(shè)置顏色閾值:設(shè)置在當(dāng)前穩(wěn)定光照環(huán)境下所要追蹤的顏色的閾值,其語(yǔ)法句式如下Colour_threshold=(minL,maxL,minA,maxA,minB,maxB),括號(hào)中的數(shù)值分別為L(zhǎng)、A、B的最大值和最小值,L、A、B的值可以在軟件maix IDE圖像左側(cè)三個(gè)坐標(biāo)圖中獲得。設(shè)置blobs作為判定變量,當(dāng)找到顏色時(shí)即對(duì)其使能,否則不使能,語(yǔ)法句式如下blobs=img.find_blobs([colour_threshold])。
4.1.2 坐標(biāo)定位
通過(guò)迭代找到目標(biāo)顏色區(qū)域,并將具體數(shù)據(jù)賦給變量b,相應(yīng)語(yǔ)法句式見(jiàn)下:for b in blobs:此句的作用就是從blobs進(jìn)行迭代取值,每次把值賦給b,而這里的blobs即為顏色識(shí)別部分的blobs,通過(guò)顏色識(shí)別中的blobs=img.find_blobs([colour_threshold]);即可將所要查找色塊對(duì)象的列表信息全部賦給blobs,此處數(shù)據(jù)很多但是通過(guò)對(duì)庫(kù)函數(shù)imag.find_blobs的學(xué)習(xí)可以知道b[5],b[6]分別對(duì)應(yīng)著所要識(shí)別色塊中心坐標(biāo)的x和y坐標(biāo)。從而進(jìn)行賦值x=b[5],y=b[6]即可實(shí)現(xiàn)對(duì)識(shí)別顏色對(duì)象的中心x和y坐標(biāo)了。
4.1.3 坐標(biāo)發(fā)送
這里我們使用串口通信的方式將所要識(shí)別顏色對(duì)象的中心x和y坐標(biāo)發(fā)送給FPGA。這里我們將K210上的15腳作為數(shù)據(jù)發(fā)送腳,17腳作為數(shù)據(jù)接收腳,其對(duì)應(yīng)的例化程序如下:
fm.register(board_info.PIN15,fm.fpioa.UART1_TX)
fm.register(board_info.PIN17,fm.fpioa.UART1_RX)
這里串口發(fā)送用庫(kù)函數(shù)uart_A.write()來(lái)實(shí)現(xiàn),其中括號(hào)里面就是要發(fā)送的數(shù)據(jù),這里我們將FH賦值為0作為前校驗(yàn)位,以防實(shí)際應(yīng)用時(shí)出現(xiàn)干擾。如果直接將采集到的坐標(biāo)數(shù)據(jù)作為有效數(shù)據(jù)發(fā)送出去就會(huì)產(chǎn)生錯(cuò)誤。x與y就是上面通過(guò)x=b[5],y=b[6](所賦值得到的識(shí)別顏色對(duì)象的中心x和y坐標(biāo))。
此次電機(jī)驅(qū)動(dòng)部分采用DDA插補(bǔ)控制實(shí)現(xiàn)方案,本文在設(shè)計(jì)積分插補(bǔ)器模塊中采用了有限狀態(tài)機(jī)的方法。積分累加的過(guò)程分為三個(gè)狀態(tài):空閑狀態(tài)(Idle)、累加狀態(tài)(ADD)和溢出狀態(tài)(Full)。在空閑狀態(tài)Idle,如果還沒(méi)有啟動(dòng)信號(hào)start,則保持處于空閑等待狀態(tài),直到啟動(dòng)信號(hào)start為1有效時(shí),則從空閑狀態(tài)讀入被積函數(shù)寄存器和累加器的初始值,轉(zhuǎn)入累加狀態(tài)(ADD)。在累加狀態(tài)(ADD),置工作忙信號(hào)Busy為1,將被積函數(shù)寄存器的值和累加器的值進(jìn)行累加,累加的結(jié)果存在累加器中,如果累加有溢出,則轉(zhuǎn)入溢出狀態(tài)(Full|)。在溢出狀態(tài)(Full|),輸出插補(bǔ)脈沖,計(jì)數(shù)器的值(為被積函數(shù)寄存器的終點(diǎn)坐標(biāo)值)減1,然后繼續(xù)累加,直至計(jì)數(shù)器的值減為0,返回到空閑狀態(tài)。
以下為狀態(tài)轉(zhuǎn)移代碼部分:
if(x[16]==1'b1)//x[16]作為x軸進(jìn)位標(biāo)志,如果為1,進(jìn)入狀態(tài)2,
S=2;
if(y[16]==1'b1)//y[16]作為y軸進(jìn)位標(biāo)志,如果為1,進(jìn)入狀態(tài)3,
S=3;
if(z[16]==1'b1)//z[16]作為z軸進(jìn)位標(biāo)志,如果為1,進(jìn)入狀態(tài)4,
S=4;
if((counter_A==16'b0)&(counter_B==16'b0))//如果計(jì)數(shù)器為0就回到空閑狀態(tài),
begin Busy﹤=1'b0;S=0;end
主程序流程圖見(jiàn)圖4。
圖4 主程序流程圖
通過(guò)對(duì)顏色識(shí)別、物體抓取進(jìn)行測(cè)試,可以看到機(jī)械臂可以實(shí)現(xiàn)對(duì)不同顏色物體的準(zhǔn)確抓取。其具體實(shí)物圖如圖5。
圖5 機(jī)械臂實(shí)物圖