基于OpenMV的五自由度機械臂控制方法研究

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

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

0 前言

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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 控制程序設(shè)計

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

圖4 機械臂運動控制主程序的流程

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

圖5 機械臂程序框圖

2 機械臂運動學(xué)分析

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

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

圖6 D-H模型示意

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

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

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

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

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

表2 建模參數(shù)

2.2 機械臂正運動學(xué)分析

(1)

(2)

(3)

其中：

(4)

2.3 機械臂逆運動學(xué)分析

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

2.3.1 代數(shù)法求逆解

(5)

(6)

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

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

(7)

求解得到θ1為

(8)

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

(9)

其中：

(10)

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

2.3.2 幾何法求逆解

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

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

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

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

(11)

求解得到：

(12)

(13)

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

(14)

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

θ1=arctan(y/x)

(15)

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

2.4 軌跡規(guī)劃

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

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

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

(16)

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

(17)

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

(18)

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

3 圖像識別與處理

3.1 圖像特征選取

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

(19)

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

3.2 圖像處理

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

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

(20)

(21)

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

圖9 圖像處理效果

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

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

(22)

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

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

(23)

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

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

3.3 OpenMV程序設(shè)計

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

圖10 OpenMV程序流程

4 實驗及結(jié)果

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

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

表3 視覺部分理論值與實際值

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

表4 測試結(jié)果

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

5 結(jié)語

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