帶雙目視覺的SCARA機械臂運動學分析及軌跡規劃

李旭，裴旭明，劉迪，李浩

帶雙目視覺的SCARA機械臂運動學分析及軌跡規劃

李旭，裴旭明*，劉迪，李浩

（鄭州輕工業大學 河南省機械裝備智能制造重點實驗室，河南 鄭州 450002）

為了解決SCARA機械臂因目標位姿變化而引起抓取任務失敗問題，給出了一種基于機器視覺目標檢測與定位的機械臂控制方法。通過雙目相機成像模型實現坐標系之間的轉換，利用計算機視覺識別及三維匹配方法，達到對目標物體定位的目的，從而確定機械臂末端執行器的期望位置。結合使用-參數法建立機械臂的運動學模型，通過運動學方程的逆解，得到末端執行器到達期望位置時機械臂各驅動關節的變量參數，然后對其進行軌跡規劃仿真，得到了連續且平滑的各關節位移、速度、加速度變化規律曲線，同時使用SCARA機械臂進行裝配實驗。實驗結果表明該系統能夠準確、穩定地把物體放到目標位置，完成裝配任務，從而為開發機械臂運動控制系統提供重要的參考。

SCARA機械臂；機器視覺；-參數法；運動學；軌跡規劃

SCARA（Selective Compliance Assembly Robot Arm，選擇順應性裝配機器手臂）機械臂具有剛性高、高速度及安裝空間小等優勢，廣泛應用于抓取放置、裝配制造及貨物搬運等領域。這類機器人通過精確的示教作業，能完成復雜的點到點的動作，極大提高了生產加工效率。然而傳統的示教方法在生產環境和被控對象變化時，容易引起目標位置和姿態偏差，進而導致機器人任務執行失敗[1]，為了準確進行目標識別，使機械臂能自動調整位姿，使其在裝配過程中運動更平滑、平穩，國內外學者做了大量相應研究。基于雙目視覺技術的機械手臂目標定位系統，運用運動學分析和軌跡規劃，借助LabVIEW、Adams、MT-ARM等仿真軟件，依據雅可比函數、-（Denavit-Hantenberg）法等理論，對平行四連桿機構的碼垛機器人的運動控制與視覺抓取、進行運動學和視覺算法的研究，建立視覺自適應機械臂抓取系統，有效的解決了生產線上碼垛作業問題[2-4]；另外利用PIEPER準則推導逆運動學的封閉解，在笛卡爾空間中通過對空間插值點的優化減小軌跡偏差，實現機械臂執行機構的軌跡優化[5-6]。還有在避開機械臂奇異位姿情況下能夠實時計算出多關節中每個關節的速度并進行控制，有效解決因傳統插補方法使得機械臂運行緩慢的問題，實現對機械臂的正、逆運動學以及軌跡規劃仿真分析，大大的提高了機械臂運行效率[7]；針對多自由度桿件機構的機械臂在笛卡爾空間中對空間插值點，推導逆運動學的封閉解、解決了機械臂運動軌跡偏差較大的問題[8-9]；甚至采用改進的-法建立其運動學方程在Matlab Robotics Toolbox環境下，來驗證運動學方程的正確性[10-12]，這都為后者提供了一種更有效的工業機器人運動學的研究方法。

本文以SCARA機械臂為研究對象，將視覺算法應用到目標位置的識別和定位上，從而得出末端執行器的期望位置；進而基于運動學理論對機械臂逆運動學方程進行求解，計算出末端執行器達到預期位置時機械臂各個驅動關節的變量參數，對其進行軌跡規劃仿真，得到各個關節運動規律曲線圖，對仿真結果進行分析以及實驗驗證。

1 目標的識別與定位

1.1 視覺機械臂抓取系統

視覺裝配平臺如圖1所示，由雙目視覺系統、機械臂控制系統、機械臂本體及裝配平臺組成。工作原理如圖2所示，視覺系統由雙目相機負責采集圖像傳送到計算機里進行圖像處理，以完成目標的識別與定位，得到目標的位置信息。機械臂控制系統將這些信息計算出機械臂各自由度的變化量，驅動伺服電機轉動相應角度，使得末端執行器到達目標位置，實現對目標物體的準確裝配，同時在機械臂上安裝傳感器形成閉環控制，保證機械臂的控制精度。

圖1 雙目視覺機械臂裝配平臺組成

圖2 實驗系統工作原理圖

1.2 目標的識別與定位

識別定位目標物體在圖像中的坐標，是實現視覺對機械臂運動引導的關鍵。而在圖像檢測和識別的算法中，圖像的質量和一些干擾信息是影響算法處理精確的主要影響因素。因此，在對圖像進行目標檢測識別之前，首先對相機采集到的圖像進行灰度化處理，其次采用中值濾波及高斯濾波進行預處理，去除或者降低無關信息的干擾，然后通過三維匹配方法，結合相機內外參數和三角測距原理，最終計算得到目標的坐標[13-14]。

三維匹配是尋找同一空間點在不同相機采集到的圖像像素間的對應關系，其利用質心作為匹配點得到目標視差的估計值，再依據該值來設定搜索范圍，在一幅圖像中搜索與另一幅圖像目標質心相似度最高的匹配點從而得到視差。雙目相機采集的圖像之間灰度值的接近程度用r,y,p表示為：

搜索范圍可表示為：

式中：p為匹配時得到的視差值；u、d為對應視差值的上偏移量和下偏移量。

結合三角測距原理，以左相機焦點為坐標系原點，可計算得目標在空間中的三維坐標為：

式中：T為雙目視覺相機的基線長度；（x, y）和（c, c）分別為左相機圖像坐標系的形心坐標和校正后的坐標；為左目相機焦距；x為右目相機在圖像坐標系的橫坐標。

通過得到目標物體上某點在左右相機中拍攝到的圖像中的像素坐標，可以獲得該點在空間中的三維坐標，進而實現三維重建和識別。

2 機械臂運動學分析

2.1 機械臂D-H模型

為解決機械臂的幾何特性不易描述和計算問題，20世紀50年代，Denavit和Hantenberg研究出一種描述機械臂的-參數法，降低了運動學的計算量[15]。采用-參數法建立運動學坐標系模型，運用4×4的齊次變換矩陣代替復雜的運動學計算。-參數法以機械臂基座為初始坐標系，各關節旋轉軸軸線方向為坐標系的z向，軸z與z1的公垂線方向為坐標系的x向，y則可通過右手定則確定。依據該方法建立如圖3所示的SCARA機械臂坐標系模型，得到如表1所示的-參數。

圖3 SCARA機械臂運動學坐標系模型

表1 運動結構參數

注：1＝0.25 m；2＝0.15 m。

2.2 機械臂正運動學分析

為了獲得末端執行器的位姿，需要通過已知驅動關節參數對機械臂正運動學進行分析。根據機械臂連桿的坐標變換法則，相連的兩連桿間的關系可由齊次變換矩陣表示為：

式中：cθ＝cosθ；sθ＝sinθ；sα-1＝sinα-1；cα-1＝cosα-1。

將SCARA機械臂-參數代入式（4）可得到四個相鄰關節坐標系的齊次變換矩陣為：

由式（5）中將各個連桿變換矩陣依次相乘，即可得正運動學方程：

式中：[n o a]、[n o a]、[n o a]分別為末端執行器三個坐標軸的單位矢量相對于基座坐標系的軸上的投影分量；[p p p]T為末端執行器相對于基座的平移。

式（6）即為SCARA機械臂變換矩陣，指的是末端相對基的位姿，也是研究機械臂關節運動的重要方程。將式（5）各矩陣代入式（6），由兩矩陣對應元素相等即可得到機械臂末端執行器位置求解方程為：

2.3 機械臂逆運動學分析

機械臂逆運動學方程的解就是在給定目標期望位姿時，求得末端執行器達到預期位姿時機械臂各個驅動關節的變量參數。

（1）求關節變量1

令式（8）中組成方程：

解式（9）可得：

（2）求關節變量2

將1代入式（9）可得：

（3）求關節變量3

由式（8）可得：

（4）求關節變量4

由式（8）可得：

解式（13）可得：

3 SCARA機械臂軌跡規劃

使用合適的軌跡規劃方法，可以提高機械臂運行過程的穩定性，從而提高機械臂的工作效率以及使用壽命[16-17]。軌跡規劃實質上就是各驅動關節的位移、速度、加速度的變化量。

3.1 軌跡規劃算法

軌跡規劃分兩種情況，一種是關節空間規劃，另一種是笛卡爾空間規劃[18]。前者能夠避免求逆解，運算量小，容易避免機械臂的奇異位置；而后者運算量大，避免機械臂奇異位置比較困難。本文主要針對SCARA機械臂在關節空間中對點到點運動使用五次多項式插值算法，確保其平穩無震蕩運動。使用五次多項式插值算法時，機械臂初始點和終止點的關節參數分別為θ、θ、t和t，在初始位置和終止位置速度，加速度都為0的前提下，各關節變量與時間關系的五次插值函數()為：

對式（15）求導，可以得到變化的速度和加速度，分別為：

需要滿足的約束條件為：

解式（17），可得：

3.2 SCARA機械臂仿真分析

根據SCARA機械臂結構參數，使用MATLAB Robotics Toolbox中的Link函數創建SCARA機械臂模型[19]。Link函數為L=Link（[Alpha A Theta D Sigma], Convention），其中：Alpha、A、Theta、D分別對比桿件扭角、桿件長度、關節回旋角、關節平移量；Sigma為0代表旋轉關節、非0代表移動關節；convention取standard代表采用標準-參數，modified代表采用改進的-參數。

由表1可知，使用Link和SerialLink函數創建SCARA機械臂模型，并命名為“SCARA機械臂”，如下：

L1=Link([0 0.25 0 0 0], ’standard’);

L2=Link([0 0.15 0 0 0], ’standard’);

L3=Link([0 0 0 0 1], ’standard’);

L4=Link([0 0 0 0 0], ’standard’);

bot=SerialLink([L1 L2 L3 L4],

'name','SCARA機械臂');

通過plot函數將機械臂模型顯示出來，如圖4所示。

圖4 SCARA機械臂模型

使用MATLAB仿真軟件的jtraj五次多項式插值函數即可得到機械臂模型的規劃軌跡。其調用格式為[q,qd,qdd]=jtraj(T1,T2,t)，其中：q為從狀態T1=[0 0 0 0]到T2=[pi/2 0 pi/4 -pi/4]的關節空間規劃軌跡；qd和qdd為其速度和加速度；T為各關節角度；t為仿真時間長度。實驗設置機械臂從T1～T2的運行時間是2 s，得到機械臂末端執行器的運行軌跡如圖5及各關節角位移、速度、加速度的變化曲線如圖6所示。在0～0.4 s驅動關節1、3的角加速度平緩增加，在0.4～1 s角加速度平緩減少；在0～0.4 s驅動關節4角加速度反向平緩增加，在0.4～1 s角加速度反向平緩減少，關節變化規律曲線在1～2 s角加速度的變化與0～1 s大小相等方向相反。通過速度曲線可以看到在加速度平緩增加的時候，速度逐漸變大，到加速度變化為零時達到最大值，然后逐漸減少至零。通過位移曲線可以驗證速度加速度曲線的正確性。

圖5 機械臂末端執行器運動軌跡

綜合分析可知機械臂從初始狀態到末尾狀態運動過程中工作正常、運動平穩，速度和加速度曲線平滑無突變，表明其運行穩定，軌跡規劃合理，可以達到期望的效果。

4 機械臂物塊裝配試驗

為了驗證前述方法的有效性，在圖1所示的試驗平臺上進行視覺抓取裝配試驗。該試驗平臺視覺系統包括CCD相機、鏡頭及圖像采集卡。相機采用東芝TELI工業攝像頭，其CCD陣列含有像素個數為768×576，水平析像力為570行。鏡頭采用Tamron CCTV鏡頭，鏡頭焦距范圍為4～12 mm。圖像采集卡使用Matrox Morphis四通道圖像采集卡，其有支持PCI、PC/104-plus等多路總線。

將機械臂運動起點位置設置為仿真的T1點，目標物體放置在機械臂運動的終點位置上，其為仿真的T2點。通過控制系統控制機械臂進行裝配試驗，結果說明在給定路徑點的情況下能夠保證機械臂達到預期的位置，并且其在運動過程中平滑、穩定，和仿真結果表現的一致，驗證了軌跡規劃算法在機械臂實際運動中的有效性和可行性。機械臂裝配過程如圖9所示。

圖6 關節變化曲線

圖7 機械臂裝配過程

5 結論

針對SCARA機械臂在裝配過程中運動的穩定性，經視覺定位和軌跡規劃得到以下結論：

（1）由視覺系統預處理后的圖像結合三維匹配法及三角測距原理，可以準確得到目標物體的坐標；

（2）經逆運動學方程得出的解，可以獲得機械臂各個驅動關節的變量參數，并對其進行軌跡規劃仿真，結果表明機械臂位移、速度、加速度的變化曲線是平滑、無突變，證明其運動過程是穩定的。

（3）通過機械臂物體裝配實驗表明該系統能夠準確、穩定地把物體放到目標位置，完成裝配任務，為開發機械臂運動控制系統提供重要的參考。

[1]桑杲，俞經虎. 基于MATLAB的采摘手末端執行器的分析及優化[J]. 機械，2017，44（12）：36-40.

[2]Wang Y，Lv H Y，Wang N. A Machine Vision System for Measurement of Mechanical Parts Based on LabVIEW 2012[J]. Advanced Materials Research，2013（834-836）：1186-1189.

[3]Corke P. I. A robotics toolbox for MATLAB[J]. IEEE Robotics Automation Magazine，1996，3（1）：24-32.

[4]張燕超，崔吉. 全方位移動機器人動力學研究[J]. 機械， 2019，46（3）：24-27，69.

[5]曹成濤，許倫輝，趙雪，孫永嘉. 四軸工業機器人運動控制與視覺碼垛[J]. 機械設計與制造，2016（11）：158-161.

[6]熊艷梅，楊延棟. 碼垛機器人運動學分析與仿真[J]. 機械，2015（1）：62-66.

[7]云洋，宋華，徐炳吉. 六自由度工業機械臂運動學仿真研究[J]. 機械科學與技術，2018（8）：1167-1176.

[8]Jing H，Wang X L，Liu D S，et al. A New Method for Solving Inverse Kinematics of an Industrial Robot[C]. International Conference on Computer Science Electronics Engineering，10.1109/ICCSEE.2012.8.

[9]李宏強，底劍豪，等. 平面3R操作臂動力學分析[J]. 機械，2011，38（12）：47-50.

[10]孫亮，馬江，阮曉鋼. 六自由度機械臂軌跡規劃與仿真研究[J].控制工程，2010，17（03）：388-392.

[11]Malekabadi A J，Khojastehpour M，Emadi B，et al.Development of a machine vision system for determination of mechanical properties of onions[J]. Computers Electronics in Agriculture，2017（141）：131-139.

[12]王鵬程，沈惠平，孟慶梅，等. 一種冗余自由度機械臂逆運動學解析算法[J]. 機械科學與技術，2016，35（5）：706-710.

[13]Viana I，Bugarin F，Cornille N，et al. CAD-guided inspection of aeronautical mechanical parts using monocular vision[J]. Proceedings of SPIE - The International Society for Optical Engineering，2015：9534.

[14]Lian H，Xiwen L，Shan Z，et al. Plant recognition and localization for intra-row mechanical weeding device based on machine vision[J]. Transactions of the Chinese Society of Agricultural Engineering，2013，29（10）：12-18.

[15]Wen S H，Ma Z Y，Zhao Y S，et al. The study of NAO robot arm based on direct kinematics by using D-H method[C]. Ukacc International Conference on Control，2014.

[16]Huang G S，Tung C K，Lin H C，et al. Inverse kinematics analysis trajectory planning for a robot arm[C]. ASCC 2011 - 8th Asian Control Conference - Final Program and Proceedings，2011.

[17]Potts A S，Da Cruz J J. Kinematics Analysis of a Quadruped Robot[J]. IFAC Proceedings Volumes，2010，43（18）：261-266.

[18]蔣偉，袁亮. 串聯抓取機械臂運動軌跡規劃算法研究[J]. 機械設計與制造，2019（7）：106-110.

[19]曾劍，林義忠，廖小平，等. 6R型噴涂機器人運動學分析及仿真[J]. 機械設計與制造，2010（6）：145-14.

Kinematics Analysis and Trajectory Planning of SCARA Manipulator with Binocular Vision

LI Xu，PEI Xuming，LIU Di，LI Hao

( Henan Key Laboratory of Intelligent Manufacturing of Mechanical Equipment, Zhengzhou University of Light Industry, Zhengzhou450002, China)

A control method for SCARA manipulator is presented on the basis of the detection and location of machine vision to solve the robotic gripping failure caused by the change of target pose. The coordinate system is transformed by employing binocular camera imaging model, and the location of target object is realized through computer vision recognition and three-dimensional matching method to determine the expected position for the end-effector of the manipulator. The variable parameters of each driving joint while the end-effector reaching the expected position is obtained by establishing kinematics model though-parametric method and solving the inverse kinematics equation. Then, a set of continuous and smooth curves of joint displacement, velocity and acceleration are obtained by simulation. Meanwhile, the assembly test on the SCARA manipulator is carried out. The results indicate that the system is able to move the object to the target position and complete the assembly mission accurately and stably, which provides important reference for the development of the motion control system for manipulators.

SCARA manipulator；machine vision；-parameter method；kinematics；trajectory planning

O311.2

A

10.3969/j.issn.1006-0316.2020.02.002

1006－0316 (2020) 02－0007－08

2019－10－08

國家自然科學基金資助項目（51775517）；河南省科技攻關項目（102102210134）

李旭（1994－），男，河南駐馬店人，碩士研究生，主要研究方向為機器人控制技術。

裴旭明（1962－），女，河南南陽人，碩士，教授、碩士生導師，主要研究方向為機械動力學與控制。