張宇豪，龐辰驊，宋子韓，錢嘉毅，蔣書波

(南京工業(yè)大學(xué)電氣工程與控制科學(xué)學(xué)院，江蘇南京 211816)

0 前言

工業(yè)機(jī)器人是具有多關(guān)節(jié)或多自由度的機(jī)器裝置，代替人工完成一些繁重復(fù)雜甚至危險(xiǎn)環(huán)境下的工作，它具有一定的感知功能，能夠?qū)崿F(xiàn)對環(huán)境和工作對象的自主判斷與決策。工業(yè)機(jī)器人已經(jīng)廣泛應(yīng)用于生產(chǎn)和生活的各個(gè)方面，特別是在物流行業(yè)與分揀領(lǐng)域有著極大的研究與應(yīng)用價(jià)值，不僅大量減少了人為操作的時(shí)間和精力，還較大程度上提高了生產(chǎn)效率[1]。目前我國工業(yè)機(jī)器人的應(yīng)用主要集中在汽車制造產(chǎn)業(yè)、3C電子電氣產(chǎn)業(yè)、金屬加工產(chǎn)業(yè)、塑料和化學(xué)制品產(chǎn)業(yè)等多個(gè)領(lǐng)域[2]。在排爆、航空航天、放射物環(huán)境以及其他高危行業(yè)的工程應(yīng)用中，對移動機(jī)械臂視覺伺服系統(tǒng)進(jìn)行研究有著重要的工程應(yīng)用價(jià)值與廣闊的發(fā)展前景。

本文作者以此為背景，對機(jī)器視覺定位與機(jī)械臂的運(yùn)動控制進(jìn)行了探索，研究了基于視覺感知的移動機(jī)器人控制系統(tǒng)，引導(dǎo)機(jī)器人自主地從起始點(diǎn)向目標(biāo)點(diǎn)運(yùn)動，完成抓取操作。本文作者建立仿真模型，驗(yàn)證了運(yùn)動學(xué)求解方法；同時(shí)完成了硬件搭建以及軟件調(diào)試，對整套機(jī)械臂抓取控制系統(tǒng)的可行性進(jìn)行了驗(yàn)證。

1 系統(tǒng)總體方案設(shè)計(jì)

五自由度機(jī)械臂比市面上更多自由度的機(jī)械臂成本低，相比于四自由度的機(jī)械臂可以實(shí)現(xiàn)更加精確的操控。本文作者以五自由度機(jī)械臂為研究對象，通過視覺傳感系統(tǒng)來定位目標(biāo)物，通過對機(jī)械臂逆運(yùn)動學(xué)的求解及運(yùn)動軌跡的規(guī)劃控制機(jī)械臂。

此五自由度機(jī)械臂采用了5個(gè)數(shù)字金屬舵機(jī)相連接，其結(jié)構(gòu)如圖1所示。舵機(jī)的本質(zhì)是一套小型的閉環(huán)伺服系統(tǒng)，它接收目標(biāo)信號，通過驅(qū)動內(nèi)部的直流電機(jī)，并經(jīng)過減速齒輪組，從而調(diào)整輸出軸的角度。該輸出角度通過電位器采樣，再由反饋控制系統(tǒng)調(diào)整其輸出角度與目標(biāo)值匹配。整個(gè)過程構(gòu)成一個(gè)典型的閉環(huán)反饋控制系統(tǒng)。

圖1 五自由度機(jī)械臂結(jié)構(gòu)

機(jī)械臂包括舵機(jī)和機(jī)械手，舵機(jī)包括5個(gè)數(shù)字舵機(jī)。該數(shù)字舵機(jī)與傳統(tǒng)的模擬舵機(jī)相比，具有控制精度高、線性度好、響應(yīng)速度快的優(yōu)點(diǎn)。第一舵機(jī)固定設(shè)置在機(jī)械臂底座上，第一舵機(jī)與第二舵機(jī)通過第一支架相連接；第二舵機(jī)與第三舵機(jī)通過第二支架相連接，第三舵機(jī)與第四舵機(jī)通過第三支架相連接，第四舵機(jī)與第五舵機(jī)通過第四支架相連接，第五舵機(jī)連接機(jī)械手。第一舵機(jī)用于控制方向，第五舵機(jī)控制機(jī)械手的開合，第二、三、四舵機(jī)用于控制旋轉(zhuǎn)角度。

此系統(tǒng)利用OpenMV攝像頭，完成對目標(biāo)球位置信息的采集，主控板采用STM32F103處理器，根據(jù)位置信息控制五自由度機(jī)械臂模塊，從而完成對目標(biāo)球的抓取。

圖2表示了一種基于位置反饋的視覺伺服控制系統(tǒng)框圖，首先從相機(jī)拍攝到的圖片獲取目標(biāo)物位姿，接著將其反饋到輸入端，與期望位姿比較得到偏差，并根據(jù)偏差值驅(qū)動關(guān)節(jié)控制器，最后驅(qū)動機(jī)械臂運(yùn)動完成工作任務(wù)。

圖2 基于位置的視覺伺服控制系統(tǒng)

1.1 系統(tǒng)硬件結(jié)構(gòu)設(shè)計(jì)

系統(tǒng)硬件結(jié)構(gòu)如圖3所示：OpenMV攝像頭采集圖像信息，采集到的圖像信息通過 A/D 轉(zhuǎn)換，生成數(shù)字圖像，采用圖像識別與處理算法對這些數(shù)字圖像進(jìn)行辨識[3]，并得到識別結(jié)果。如若符合乒乓球的特征即為識別成功，然后攝像頭通過串口通信發(fā)送指令到STM32F103主控板；主控板對機(jī)械臂的運(yùn)動姿態(tài)進(jìn)行控制，確定每個(gè)舵機(jī)轉(zhuǎn)動的角度，規(guī)劃出最優(yōu)運(yùn)動路徑，最終完成抓取球的動作。

圖3 系統(tǒng)硬件結(jié)構(gòu)

1.2 控制程序設(shè)計(jì)

圖4為系統(tǒng)控制主程序的流程，機(jī)器人各部分裝置協(xié)調(diào)有序運(yùn)行，互不干擾，保證程序運(yùn)行的安全。主要實(shí)施過程為尋物、移動和抓取，首先對目標(biāo)物進(jìn)行識別與定位，接著機(jī)器人根據(jù)目標(biāo)物的位置信息調(diào)整位姿，確保目標(biāo)物在機(jī)械臂手爪的工作范圍內(nèi)。

圖4 機(jī)械臂運(yùn)動控制主程序的流程

下一步要實(shí)現(xiàn)的是將視覺感知目標(biāo)物與機(jī)械臂操作相結(jié)合，圖5為機(jī)械臂的程序框圖。STM32F103主控板接收目標(biāo)物的位置信息，包括經(jīng)過數(shù)據(jù)處理后的角度信息，即視覺檢測裝置根據(jù)目標(biāo)物的位置采用幾何法求逆解得到的機(jī)械臂各個(gè)關(guān)節(jié)的轉(zhuǎn)動角度。由此，便得到機(jī)械臂起始與終止時(shí)刻的姿態(tài)，使用五次多項(xiàng)式插值法對機(jī)械臂的運(yùn)動軌跡進(jìn)行規(guī)劃，引導(dǎo)機(jī)器人從初始點(diǎn)向目標(biāo)點(diǎn)運(yùn)動。在完成抓取動作過程中，需要根據(jù)實(shí)際情況來確定各個(gè)關(guān)節(jié)舵機(jī)的速度快慢，或者插入中間運(yùn)動姿態(tài)來規(guī)避機(jī)械臂不能到達(dá)的姿態(tài)，使機(jī)械臂的運(yùn)動軌跡更加平滑，可靠性更高。

圖5 機(jī)械臂程序框圖

2 機(jī)械臂運(yùn)動學(xué)分析

2.1 運(yùn)動學(xué)模型建立

運(yùn)動學(xué)建模分析是實(shí)現(xiàn)機(jī)械臂運(yùn)動軌跡規(guī)劃等后續(xù)工作的基礎(chǔ)。機(jī)械臂最常用的建模方法是D-H模型法，其基本思想為：為機(jī)械臂的每一連桿建立一個(gè)坐標(biāo)系，用齊次變換矩陣來表示相鄰關(guān)節(jié)的位姿，然后獲得末端執(zhí)行器在基坐標(biāo)系下的位姿[4]。圖6所示為相鄰關(guān)節(jié)D-H模型示意。

圖6 D-H模型示意

圖6中設(shè)連桿i的坐標(biāo)系為{i}，Zi軸與軸i同向，原點(diǎn)位于軸i、i+1的公垂線與軸i的交點(diǎn)處，Xi軸沿著公垂線由軸i指向軸i+1，Yi軸由右手定則確定。其中，4個(gè)參數(shù)αi-1、ai-1、θi、di為連桿i的D-H參數(shù)，描述了機(jī)械臂關(guān)節(jié)和連桿之間位置關(guān)系，其物理含義如表1所示。

表1 機(jī)械臂D-H參數(shù)

本文作者以五自由度機(jī)械臂為對象，根據(jù)D-H模型法在各個(gè)連桿建立合適的坐標(biāo)系，如圖7所示。其中，關(guān)節(jié)J1逆時(shí)針轉(zhuǎn)過的角度記為-θ1，關(guān)節(jié)J1與關(guān)節(jié)J2的連桿長度為L1，以此類推。

圖7 機(jī)械臂模型結(jié)構(gòu)

表2為機(jī)械臂的建模參數(shù)。

表2 建模參數(shù)

2.2 機(jī)械臂正運(yùn)動學(xué)分析

(1)

(2)

(3)

其中：

(4)

2.3 機(jī)械臂逆運(yùn)動學(xué)分析

逆運(yùn)動學(xué)分析是已知末端位姿反解出各關(guān)節(jié)角度，是機(jī)械臂控制中比較重要的分析方法[5]。逆運(yùn)動學(xué)的求解比正運(yùn)動學(xué)復(fù)雜得多，求解方法主要包括代數(shù)法、幾何法、迭代法等。本文作者使用代數(shù)法和幾何法來對逆運(yùn)動學(xué)求解進(jìn)行對比分析。

2.3.1 代數(shù)法求逆解

(5)

(6)

令等式(6)兩端元素(2，4)對應(yīng)相等，得：

qy·cθ1-qx·sθ1=-L1

(7)

求解得到θ1為

(8)

在已知θ1的情況下，可依次計(jì)算各關(guān)節(jié)角度值為

(9)

其中：

(10)

經(jīng)過分析可知，代數(shù)法計(jì)算的關(guān)節(jié)角度存在多解的情況。在實(shí)際情況中，由于關(guān)節(jié)結(jié)構(gòu)等限制，需要根據(jù)實(shí)際情況對旋轉(zhuǎn)角度的具體數(shù)值進(jìn)行取舍，最終得到一組最優(yōu)解。

2.3.2 幾何法求逆解

代數(shù)法在求解過程中代數(shù)較多，計(jì)算量較大，求逆也比較復(fù)雜，不利于機(jī)械臂的實(shí)時(shí)控制。本文作者使用一種基于機(jī)械臂結(jié)構(gòu)幾何特征提取推導(dǎo)求解的方法。

假設(shè)機(jī)械臂抓取時(shí)的姿態(tài)是唯一確定的，無論5個(gè)關(guān)節(jié)角度變量取何值，連桿L1、L2、L3、L4的運(yùn)動始終處于同一平面，平面的位置僅由θ1確定。選取該二維平面下的機(jī)械臂結(jié)構(gòu)進(jìn)行分析，如圖8所示。

圖8 二維平面下機(jī)械臂結(jié)構(gòu)

規(guī)定a為目標(biāo)物到機(jī)械臂底座的水平距離，b為J4距地面的高度，c為J1距地面的高度，L5為J5與目標(biāo)物中心的水平距離。根據(jù)圖8所示的機(jī)械臂結(jié)構(gòu)的幾何關(guān)系，可得關(guān)節(jié)轉(zhuǎn)動角度計(jì)算公式為

(11)

求解得到：

(12)

(13)

θ4=π/2-θ2-θ3

(14)

將目標(biāo)點(diǎn)在三維空間下的坐標(biāo)映射到基坐標(biāo)系中xOy平面得到點(diǎn)(x,y)，根據(jù)幾何關(guān)系可以直接求得：

θ1=arctan(y/x)

(15)

上述研究表明，利用幾何方法求5個(gè)自由度機(jī)器人逆解的速度快，效率高，推導(dǎo)過程簡單直觀。但是，由于幾何方法不具有通用性，且沒有一個(gè)系統(tǒng)的解法，因此，在多自由度機(jī)器人的運(yùn)動過程中，采用幾何法求解不僅計(jì)算困難，而且效率低下。

2.4 軌跡規(guī)劃

經(jīng)過逆運(yùn)動學(xué)求解得到機(jī)械臂各關(guān)節(jié)角度后，可以確定終止時(shí)刻機(jī)械臂的姿態(tài)。軌跡規(guī)劃目的是使機(jī)械臂從初始時(shí)刻至終止時(shí)刻的運(yùn)動軌跡平滑，各關(guān)節(jié)不發(fā)生急劇的速度變化，不產(chǎn)生強(qiáng)烈的沖擊。在機(jī)械臂關(guān)節(jié)空間中以關(guān)節(jié)角函數(shù)的形式對運(yùn)動軌跡進(jìn)行描述，其中多項(xiàng)式插值法是一種經(jīng)典的機(jī)械臂軌跡的插值計(jì)算方法[6]。

如果已知機(jī)械臂初始時(shí)刻t=0和終止時(shí)刻t=tf的關(guān)節(jié)位置、角速度和角加速度，可以采用五次多項(xiàng)式對關(guān)節(jié)軌跡進(jìn)行插值：

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(16)

關(guān)節(jié)角度、角速度以及角加速度的邊界條件為

(17)

求解得到各項(xiàng)系數(shù)為

(18)

根據(jù)邊界條件可以唯一確定五次多項(xiàng)式插值軌跡，保證機(jī)械臂關(guān)節(jié)角度、角速度、角加速度曲線平滑連續(xù)。使用五次多項(xiàng)式插值法進(jìn)行軌跡規(guī)劃。

3 圖像識別與處理

3.1 圖像特征選取

選取合適的圖像特征，可以減少視覺伺服控制算法的復(fù)雜性，以獲得更好的控制效果。閾值分割法是圖像分割的一種經(jīng)典方法[7]，它利用要提取的目標(biāo)與背景在灰度上的差異，通過設(shè)定閾值對圖像進(jìn)行分組聚類，將圖像分成若干個(gè)特定的有意義的區(qū)域，目的是把目標(biāo)從背景中分離出來，便于圖像分析[8]。用數(shù)學(xué)表達(dá)式來表示，設(shè)原始圖像為f(x,y)，T為設(shè)定的閾值，則經(jīng)閾值分割后的圖像g(x,y)定義[9]為

(19)

式(19)中：標(biāo)記為1的像素對應(yīng)于目標(biāo)，標(biāo)記為0的像素對應(yīng)于背景。攝像頭采集的原圖像和原圖二值化圖像的背景干擾噪聲較大，分割效果不理想，直接識別目標(biāo)球非常困難。

3.2 圖像處理

為獲得高質(zhì)量圖像，需要對圖像做進(jìn)一步處理：

(1)雙邊濾波。雙邊濾波是一種非線性的圖像濾波方法，它是一種結(jié)合圖像的空間鄰近度和像素值相似度的折中處理，同時(shí)考慮空間信息和灰度相似性，可以很好地保存邊緣，達(dá)到去噪的目的。公式(20)給出了雙邊濾波的操作[10-11]：

(20)

(21)

圖9(a)(b)分別展示了雙邊濾波后的彩圖與二值化圖像，由于保存了過多的高頻信息，雙邊濾波器不能夠干凈地濾除彩色圖像里的高頻噪聲，只能夠較好地過濾低頻信息。

圖9 圖像處理效果

(2)腐蝕與膨脹。腐蝕與膨脹是最基本的形態(tài)學(xué)操作[12-13]，主要利用圖像中干擾像素與目標(biāo)物所占面積的巨大差異，通過腐蝕操作消除物體的邊界點(diǎn)，使邊界向內(nèi)部收縮，再經(jīng)膨脹操作后還原對象的基本特征，使邊界向外擴(kuò)張。腐蝕的數(shù)學(xué)表達(dá)式為

S=X?B={x,y|Bxy?X}

(22)

式中：S表示腐蝕后的二值圖像；B表示用來進(jìn)行腐蝕操作的結(jié)構(gòu)元素；X表示原圖經(jīng)過二值化后的像素集合。腐蝕的數(shù)學(xué)表達(dá)式為

S=X⊕B={x,y|Bxy∩X≠φ}

(23)

式中：S表示膨脹后的二值圖像；B表示用來進(jìn)行膨脹操作的結(jié)構(gòu)元素；X表示原圖經(jīng)過二值化后的像素集合。圖9(c)將雙邊濾波后的二值化圖像進(jìn)行腐蝕與膨脹操作來使圖像保持邊緣，降噪平滑。

(3)設(shè)定面積閾值。圖9(d)設(shè)定一定大小的面積閾值來過濾掉較大的色塊，即背景干擾的部分，通過輪廓檢測識別出距離最近的乒乓球，并用矩形框出。

3.3 OpenMV程序設(shè)計(jì)

OpenMV主控為STM32H7，主頻為 480 MHz，RAM 為1 MB，使用靈活便捷的 Python語言，在其專有的 OpenMV IDE 開發(fā)環(huán)境下調(diào)用基于視覺的圖像識別與處理算法[14]。圖10為OpenMV程序流程，設(shè)計(jì)涉及的顏色識別是計(jì)算機(jī)端通過顏色閾值編輯器選取目標(biāo)物的最佳閾值，然后按照目標(biāo)物的顏色進(jìn)行閾值分割，得到二值化圖像。圖像處理算法主要基于二值化圖像進(jìn)行濾波處理、形態(tài)學(xué)處理以及輪廓檢測得到目標(biāo)色塊，從而提高識別目標(biāo)物的準(zhǔn)確度。

圖10 OpenMV程序流程

4 實(shí)驗(yàn)及結(jié)果

搭建實(shí)驗(yàn)硬件時(shí)，攝像頭固定在機(jī)器人后方，安裝在距地面45 cm高度處，并與豎直碳素桿的角度為30°；機(jī)械臂安裝在機(jī)器人中間靠前部的位置，能使機(jī)械臂的運(yùn)動不與機(jī)器人碰撞。各功能模塊裝置在合適位置，保證系統(tǒng)模塊互不干擾、協(xié)調(diào)工作。經(jīng)過測試，智能抓取機(jī)器人能夠準(zhǔn)確識別到目標(biāo)球并抓取回收。

從表3可以看出：二維圖像坐標(biāo)(x，y)理論值與實(shí)際值相差很小，平均定位偏差為1.29 mm，具有較高定位精度；目標(biāo)物與機(jī)器人之間的距離理論值與實(shí)際值存在偏差，最大相對誤差為4.87%，最小相對誤差為1.46%，平均相對誤差為2.87%，滿足機(jī)器人的抓取要求。經(jīng)分析，定位偏差與距離偏差來源于相機(jī)實(shí)際參數(shù)的影響，在坐標(biāo)變換時(shí)產(chǎn)生誤差。

表3 視覺部分理論值與實(shí)際值

對拾取球的成功率進(jìn)行測試，測試結(jié)果如表4所示：能夠拾取大多數(shù)的球，成功率較高，但還是會受到光照強(qiáng)度、球密度、球位置等環(huán)境因素的影響。

表4 測試結(jié)果

除上述環(huán)境影響因素外，PWM占空比的精度也會使實(shí)驗(yàn)結(jié)果產(chǎn)生誤差。舵機(jī)從0°到270°不同角度的控制是通過改變占空比實(shí)現(xiàn)的，占空比精度越高，控制舵機(jī)角度更精準(zhǔn)[15]。在一個(gè)脈沖周期內(nèi)，若占空比的精度只有1%，它只能產(chǎn)生 30%、56%這種整數(shù)的占空比，不能產(chǎn)生26.5%這種占空比。

5 結(jié)語

在機(jī)械臂運(yùn)動學(xué)分析與軌跡規(guī)劃的研究中，作者使用代數(shù)法與幾何法對機(jī)械臂的逆運(yùn)動學(xué)求解進(jìn)行對比分析。同時(shí)，在系統(tǒng)設(shè)計(jì)方案方面，給出了各個(gè)模塊的硬件選型與功能介紹。為了精確定位目標(biāo)物，分析了圖像識別與處理方法。在系統(tǒng)控制程序設(shè)計(jì)中，選取幾何法求機(jī)械臂運(yùn)動學(xué)逆解。結(jié)合OpenMV的圖像識別與處理算法、智能小車的路徑規(guī)劃算法，主控板通過PWM信號控制舵機(jī)的機(jī)械臂從而控制系統(tǒng)精準(zhǔn)地定位并抓取目標(biāo)物，對整套機(jī)械臂抓取控制系統(tǒng)的可行性進(jìn)行了驗(yàn)證。