基于雙目視覺的雙臂協(xié)作教學機器人研究與設(shè)計

溫秀蘭 胡仰 姚波 趙藝兵 宋愛國 崔偉祥

1 南京工程學院 自動化學院,南京,211167 2 南京工程學院 工業(yè)中心/創(chuàng)新創(chuàng)業(yè)學院,南京,211167 3 東南大學 儀器科學與工程學院,南京,211196

0 引言

隨著全球產(chǎn)業(yè)競爭格局的重大調(diào)整,重視制造業(yè)成為世界主要大國促進經(jīng)濟轉(zhuǎn)型發(fā)展的戰(zhàn)略選擇,歐美等發(fā)達國家紛紛以制造業(yè)作為振興實體經(jīng)濟的重要抓手．德國率先提出“工業(yè)4.0”概念,美國推行“先進制造伙伴關(guān)系”計劃,2015年我國提出了《中國制造2025》規(guī)劃[1],全球制造業(yè)的發(fā)展正在集中指向智能化制造、精密制造．為實現(xiàn)《中國制造2025》宏偉目標,教育部針對機器人及智能制造人才缺乏問題,分別于2015年和2017年批準成立“機器人工程”專業(yè)和“智能制造工程”專業(yè)．2015年東南大學獲批設(shè)置國內(nèi)首個“機器人工程”專業(yè),至2020年全國已有302所高校獲批“機器人工程”專業(yè),212所高效獲批“智能制造工程”專業(yè)．這兩個專業(yè)涉及新技術(shù)多,綜合性、實踐性強．為滿足新專業(yè)建設(shè)要求,提升學生實踐創(chuàng)新能力,亟需教學實驗平臺支撐[2-3]．

機器人作為高新技術(shù)的載體,集機、電、控、圖像處理于一體,涉及多門課程理論知識和實踐教學,且易于激發(fā)學生學習興趣，易于進行拓展實驗,可以提升學生創(chuàng)新能力,為此在實踐教學中越來越受到重視,成為近兩年研究熱點[4-10]．考慮到軌跡規(guī)劃及控制是機器人核心技術(shù),文獻[4]設(shè)計了一種平面二自由度的機器人實驗平臺,用于開展機器人的軌跡規(guī)劃和控制實驗．為模擬病人頸部和脊柱的運動和力學特性，文獻[5]開發(fā)設(shè)計了一種旋提操作教學機器人．針對工業(yè)機器人控制系統(tǒng)封閉和現(xiàn)有實驗教學設(shè)備功能有限的現(xiàn)狀,文獻[6]設(shè)計了一款機器人手臂開放式控制實驗平臺,基于該平臺能夠開展機器人關(guān)節(jié)運動控制、手臂協(xié)調(diào)控制實驗．為了解決實驗教學過程中機器人結(jié)構(gòu)復(fù)雜、成本高、編程效率低等問題,文獻[7]設(shè)計了一種基于上位機控制的拖動示教教學機器人系統(tǒng),該系統(tǒng)在設(shè)置好相關(guān)參數(shù)后只需根據(jù)上位機操作界面提示拖動機械臂運動就可以對機器人進行快速編程．文獻[8]將雙目視覺、搬運機器人和工業(yè)無線網(wǎng)等技術(shù)相結(jié)合,設(shè)計開發(fā)了一款智能搬運機器人實驗平臺;文獻[9]介紹了一種基于計算機視覺開源數(shù)據(jù)庫OpenCV,使用Python語言進行集成開發(fā)的管道巡檢機器人視覺系統(tǒng)實驗教學平臺設(shè)計;文獻[10]設(shè)計了一款俯視相機輔助的地面機器人導(dǎo)航系統(tǒng)綜合實驗平臺,旨在解決復(fù)雜地形造成的各種不確定性．目前有關(guān)機器人實驗教學平臺多是針對特定應(yīng)用對象或完成特定功能而研發(fā)的．“機器人工程”專業(yè)、“智

能制造工程”專業(yè)綜合性、實踐性強,為使學生能夠更好地掌握理論知識和實踐技能,提升實踐創(chuàng)新能力,本文研究設(shè)計了集機、電、控、圖像處理于一體,兼具拖動示教與開放性實驗特點的雙臂協(xié)作教學機器人．

1 機械結(jié)構(gòu)設(shè)計

本研究機器人組成如圖1所示,由機器人本體、控制裝置、視覺裝置組成．其中機器人本體包括左機械臂、右機械臂、末端機械手、總線舵機、舵機連接件,控制裝置包括樹莓派4B、舵機控制板、PC機及串口通信電路,視覺裝置由雙目攝像頭、光源等組成．

圖1 機器人系統(tǒng)組成Fig.1 Robot system composition

為了提高機械臂穩(wěn)定性、降低設(shè)計成本,機械臂底座設(shè)計成大面積,雙臂呈對稱結(jié)構(gòu)[11],在不增加機械制造難度的前提下達到示教的任務(wù)要求．左、右機械臂分別由6個舵機、舵機控制板、舵機連接件以及末端執(zhí)行機構(gòu)組成,機械結(jié)構(gòu)如圖2所示．該機器人左、右臂共12個舵機均由上位機控制,可以在上位機里通過拖動各舵機對應(yīng)的滑動按鈕改變各舵機的角度值,或者是編輯一系列動作構(gòu)成動作組,燒錄上傳至舵機控制板．為了使機器人末端執(zhí)行機構(gòu)更具適應(yīng)性,選取了可拆卸的機械手掌,可以針對不同工作場合變換末端執(zhí)行機構(gòu),以節(jié)省成本、提高工作效率．

圖2 雙臂機器人機械結(jié)構(gòu)Fig.2 Mechanical structure diagram of the dual-arm robot

2 軟硬件設(shè)計

2.1 硬件設(shè)計

機器人硬件由主控制器、舵機控制板、總線舵機、雙目攝像頭、末端機械手等組成．主控制器采用樹莓派4B,搭載1.5 GHz的64位四核處理器,支持藍牙5.0,為滿足高速圖像處理的要求,選用500 MHz的 GPU，也支持Wi-Fi與有線網(wǎng)絡(luò),并自帶2個USB 3.0與2個USB 2.0,適用于不同場景下的數(shù)據(jù)傳輸需求,有雙目攝像頭的預(yù)留接口．該機器人的舵機控制板使用STM32F1,和8/16位設(shè)備相比,所采用的ARM Cortex-M3 32位RISC處理器提供了更高的浮點計算能力,最高工作頻率72 MHz,其拓展I/O口可以滿足高達32路舵機控制．總線舵機采用LDX-227和LDX-180型號,轉(zhuǎn)動角度范圍分別為0～270°與0～180°．雙目攝像頭采用小覓攝像頭標準版,型號為S1030-IR-120/Mono,深度分辨率為60 fps．

2.2 軟件設(shè)計

機器人軟件系統(tǒng)由上位機和下位機兩部分組成．上位機主要實現(xiàn)機器人的拖動示教、視覺引導(dǎo)、運動分析、在線編程、協(xié)作抓取5大實驗功能,由QT語言編寫,主要由操作界面程序、串口程序、舵機程序以及接口程序構(gòu)成．在拖動示教實驗?zāi)K中,操作人員可以在操作界面上通過拖動各個舵機參數(shù)的滑動按鈕發(fā)送相應(yīng)的舵機參數(shù)值到下位機,控制其實現(xiàn)不同的功能以及修改必要的運行參數(shù)值．在示教結(jié)束后,在按下“復(fù)位”按鈕后,機械臂會恢復(fù)初始舵機角度值,實現(xiàn)機器人整體復(fù)位．在運動學分析實驗?zāi)K中,操作人員能實現(xiàn)左、右機械臂的正逆運動學解算．在視覺引導(dǎo)實驗?zāi)K中,操作人員通過選擇標定方式、圖像處理算法以及雙目視覺算法,實現(xiàn)目標工件準確定位．為了提升學生創(chuàng)新能力,在線編程實驗?zāi)K和協(xié)調(diào)抓取實驗?zāi)K設(shè)計成開放性實驗,由操作人員設(shè)計編寫機械臂協(xié)作抓取工程項目文件,通過編譯上傳,來進行機械臂的協(xié)作抓取,并能在協(xié)作抓取界面中,清晰地觀察到協(xié)作抓取的成功率．

下位機軟件由C語言編寫,主要分為示教程序和還原程序,上位機負責切換這兩種模式．在上位機開始拖動示教時,該舵機控制板負責接收上位機軟件傳遞的舵機參數(shù)及動作組文件,來控制舵機以一定速度旋轉(zhuǎn)相應(yīng)角度.如果接收到復(fù)位指令,舵機控制板負責將預(yù)先存儲好的初始舵機參數(shù)調(diào)出,控制舵機復(fù)位至初始位置．

3 算法設(shè)計

3.1 示教動作還原算法

本設(shè)計由上位機為舵機控制板提供復(fù)位請求,由舵機控制板程序的中斷服務(wù)函數(shù)負責執(zhí)行復(fù)位操作,控制左、右機械臂各舵機恢復(fù)初始角度值．該舵機控制板是以STM32F1為核心,有6個普通定時器和2個高級定時器,可設(shè)置復(fù)位請求為最高中斷優(yōu)先級,置于中斷向量表里的第1位．在還原示教動作前,舵機控制板首先需要將12個舵機的初始角度值加載到內(nèi)存,初始化每個舵機的數(shù)據(jù)寄存器、GPIO引腳,然后,舵機控制板會進入還原示教動作的執(zhí)行程序中,將12個舵機的初始角度值轉(zhuǎn)化為PWM參數(shù)值,通過PWM參數(shù)值控制舵機轉(zhuǎn)動至初始角度．當“復(fù)位”按鈕按下時,舵機控制板會不斷比較當前舵機角度值與初始角度值,直到兩者相等,完成示教動作.還原程序流程如圖3所示．

圖3 動作還原程序流程Fig.3 Flow chart of the action restoring program

3.2 重投影誤差最小化算法

在該基于雙目視覺的雙臂協(xié)調(diào)機器人系統(tǒng)中,目標工件的識別與定位是必不可少的一步．傳統(tǒng)的目標識別常常使用迭代最近點的點云配準方,這種方法一般需要提供一個較好的初值,也就是需要粗配準,但是由于算法本身缺陷,最終迭代結(jié)果可能會陷入局部最優(yōu),導(dǎo)致配準失敗,往往達不到想要的效果[12]．

本設(shè)計采用重投影誤差最小化[13]的方法,能夠在一定程度上抑制重投影誤差對目標工件定位過程帶來的影響,可以得到更為準確的工件中心點坐標值．用于優(yōu)化的目標函數(shù)[14]為

(1)

Step1:將雙目攝像頭獲取的n組觀測數(shù)據(jù)作為輸入數(shù)據(jù);

4 機器人原型系統(tǒng)及軟件平臺

為了驗證設(shè)計方案的可行性,根據(jù)上述原理開發(fā)的機器人原型系統(tǒng)如圖4所示．

圖4 機器人原型系統(tǒng)Fig.4 Robot prototype system

基于軟件系統(tǒng)的多任務(wù)要求,軟件平臺在Windows操作系統(tǒng)下通過QT開發(fā)工具對系統(tǒng)功能進行設(shè)計編程．上位機界面如圖5所示,由拖動示教、視覺引導(dǎo)、運動分析、在線編程、協(xié)作抓取5大實驗?zāi)K組成．下位機借助Keil5平臺對舵機控制板進行開發(fā)調(diào)試．

圖5 上位機界面Fig.5 Upper computer interface

5 實驗功能測試

5.1 拖動示教實驗

拖動示教實驗通過調(diào)節(jié)滑條上滑塊的位置來改變各舵機角度值,接著點擊 “開始示教”按鈕把機器人各舵機參數(shù)值直接上傳到程序編輯窗口．本系統(tǒng)的示教程序格式統(tǒng)一為“舵機ID:參數(shù)”格式,每條程序以分號結(jié)尾．當“停止示教”按鈕按下時,舵機參數(shù)值不變,機器人位姿也不會發(fā)生變化．示教過程結(jié)束后,可以通過點擊“復(fù)位”按鈕讓機器人執(zhí)行還原動作程序,恢復(fù)初始舵機角度值．該實驗?zāi)K能同時實現(xiàn)動作組控制示教功能,在點擊“添加動作”按鈕后,會彈出動作組添加窗口,通過按序編輯多組“舵機ID:參數(shù)”的方法,程序會自動整合動作組文件．在點擊“上傳”按鈕后,程序?qū)⒖刂苿幼鹘M文件傳輸至下位機程序,由舵機控制板負責控制動作組文件的執(zhí)行時間與循環(huán)次數(shù)．

5.2 視覺引導(dǎo)實驗

視覺引導(dǎo)實驗?zāi)K如圖6所示,學生可以通過連接雙目攝像頭抓拍圖像,來進行標定與圖像處理．還可選擇SAD、SSD以及SGBM等算法來識別目標工件的中心點坐標,然后反饋給舵機控制板,來引導(dǎo)機器人的一個機械臂準確、快速地抓取到目標工件．

圖6 視覺引導(dǎo)實驗Fig.6 Visual guidance experiment

5.3 運動學分析實驗

該實驗?zāi)K能夠完成單臂運動學分析實驗．該機器人左右臂結(jié)構(gòu)相同,均由6個關(guān)節(jié)串聯(lián)連接,根據(jù)Denavit-Hartenberg (DH)模型,可以得到機器人連桿相鄰坐標系之間的變換關(guān)系[15],即

Ai=R(Zi,θi)T(Zi,di)T(Xi,ai)R(Xi,αi),

(2)

其中:ai,di,αi,θi分別表示機器人第i個關(guān)節(jié)的連桿長度、連桿偏距、連桿扭角及關(guān)節(jié)角的名義值;R(Zi,θi)表示繞Zi軸轉(zhuǎn)動關(guān)節(jié)角θi的旋轉(zhuǎn)矩陣,T(Zi,di)表示沿Zi軸移動距離di的平移矩陣．機器人正運動學分析,即由已知的6個關(guān)節(jié)轉(zhuǎn)角θ1,θ2,θ3,θ4,θ5,θ6求末端的位姿矩陣Tn過程．

(3)

根據(jù)機械臂DH模型幾何參數(shù)值及其末端的期望位姿,求解逆運動學方程可以得到6個關(guān)節(jié)角:

(4)

其中,IKP()表示工業(yè)機器人逆運動學求解函數(shù)．式(4)可以得到8組解．在實際運動過程中,根據(jù)機器人運動范圍及以最小的能量消耗或者最短的行程到達的關(guān)節(jié)角,確定一組最優(yōu)解關(guān)節(jié)角．該實驗?zāi)K如圖7所示．

圖7 左機械臂逆運動學分析實驗Fig.7 Inverse kinematics analysis experiment of left manipulator

5.4 在線編程實驗

在線編程實驗?zāi)K設(shè)計成開放式結(jié)構(gòu)，如圖8所示,學生可以在該實驗中學習如何編寫舵機控制板下位機程序,熟悉工程文件的創(chuàng)建規(guī)則、GPIO口的使用、庫函數(shù)的調(diào)用、STM32的系統(tǒng)文件、位操作、時鐘電路與PWM驅(qū)動舵機等有關(guān)代碼及編程邏輯．

圖8 在線編程實驗Fig.8 Online programming experiment

5.5 協(xié)作抓取實驗

協(xié)作抓取開放實驗需要根據(jù)在線編程的實驗結(jié)果進一步完成,如圖9所示.學生可以通過標定結(jié)果示意圖觀察到被框出的工件輪廓以及中心點坐標,在點擊“讀取標定結(jié)果”按鈕后,將所得的工件中心點坐標反饋給舵機控制板,由在線編程實驗中設(shè)計編寫的協(xié)作抓取工程文件,控制一只機械臂對目標工件的抓取;當按下“開始協(xié)作抓取”按鈕后,控制另一機械臂仿照前一機械臂的運動軌跡運動至相同位置執(zhí)行交換、組合與放置任務(wù),來達到協(xié)作抓取的目的.該界面會顯示自行設(shè)定好的協(xié)作抓取次數(shù),并對抓取成功率做出顯示,綜合評估學生的實踐設(shè)計能力．

圖9 協(xié)作抓取實驗Fig.9 Collaborative grasping experiment

6 結(jié)論

為解決智能制造、機器人工程等新專業(yè)實驗裝置短缺、學生創(chuàng)新實踐能力亟待提升等問題,本文完成了基于雙目視覺的雙臂協(xié)作教學機器人機械結(jié)構(gòu)設(shè)計、硬件設(shè)計、上位機軟件和下位機控制軟件研發(fā)．為提高機器人末端位姿數(shù)據(jù)采集精度,提出了重投影誤差最小化算法用于該機器人使雙臂協(xié)作抓取更準確穩(wěn)定．經(jīng)機器人原型系統(tǒng)設(shè)計制造及實驗研究,驗證了該系統(tǒng)不僅能夠通過上位機拖動控制雙臂運動完成示教編程及動作組實驗,而且還能夠進行基于視覺引導(dǎo)的圖像處理及單臂抓取實驗、運動分析實驗、在線編程實驗及協(xié)調(diào)抓取開放實驗．該系統(tǒng)不僅兼具拖動示教編程效率高與開放靈活提升創(chuàng)新能力等特點,而且成本低,非常適于在新專業(yè)自主創(chuàng)新實踐教學中推廣應(yīng)用.