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

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

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

0 前言

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

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

1 系統總體方案設計

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

此五自由度機械臂采用了5個數字金屬舵機相連接，其結構如圖1所示。舵機的本質是一套小型的閉環伺服系統，它接收目標信號，通過驅動內部的直流電機，并經過減速齒輪組，從而調整輸出軸的角度。該輸出角度通過電位器采樣，再由反饋控制系統調整其輸出角度與目標值匹配。整個過程構成一個典型的閉環反饋控制系統。

圖1 五自由度機械臂結構

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

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

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

圖2 基于位置的視覺伺服控制系統

1.1 系統硬件結構設計

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

圖3 系統硬件結構

1.2 控制程序設計

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

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

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

圖5 機械臂程序框圖

2 機械臂運動學分析

2.1 運動學模型建立

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

圖6 D-H模型示意

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

表1 機械臂D-H參數

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

圖7 機械臂模型結構

表2為機械臂的建模參數。

表2 建模參數

2.2 機械臂正運動學分析

(1)

(2)

(3)

其中：

(4)

2.3 機械臂逆運動學分析

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

2.3.1 代數法求逆解

(5)

(6)

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

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

(7)

求解得到θ1為

(8)

在已知θ1的情況下，可依次計算各關節角度值為

(9)

其中：

(10)

經過分析可知，代數法計算的關節角度存在多解的情況。在實際情況中，由于關節結構等限制，需要根據實際情況對旋轉角度的具體數值進行取舍，最終得到一組最優解。

2.3.2 幾何法求逆解

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

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

圖8 二維平面下機械臂結構

規定a為目標物到機械臂底座的水平距離，b為J4距地面的高度，c為J1距地面的高度，L5為J5與目標物中心的水平距離。根據圖8所示的機械臂結構的幾何關系，可得關節轉動角度計算公式為

(11)

求解得到：

(12)

(13)

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

(14)

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

θ1=arctan(y/x)

(15)

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

2.4 軌跡規劃

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

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

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

(16)

關節角度、角速度以及角加速度的邊界條件為

(17)

求解得到各項系數為

(18)

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

3 圖像識別與處理

3.1 圖像特征選取

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

(19)

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

3.2 圖像處理

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

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

(20)

(21)

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

圖9 圖像處理效果

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

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

(22)

式中：S表示腐蝕后的二值圖像；B表示用來進行腐蝕操作的結構元素；X表示原圖經過二值化后的像素集合。腐蝕的數學表達式為

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

(23)

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

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

3.3 OpenMV程序設計

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

圖10 OpenMV程序流程

4 實驗及結果

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

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

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

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

表4 測試結果

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

5 結語

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