[發明專利]基于雙目視覺的雙臂協作機器人系統和控制方法

有效

• 申請號: 202110284409.0
• 申請日: 2021-03-16
• 公開（公告）號: CN113146620B
• 公開（公告）日: 2022-06-10
• 發明（設計）人: 溫秀蘭;胡仰;賀順;周逸一;喬貴方;趙藝兵;李子康
• 申請（專利權）人: 南京工程學院
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J18/00;B25J19/04
• 代理公司: 南京鐘山專利代理有限公司 32252
• 代理人: 陳月菊

1.一種基于雙目視覺的雙臂協作機器人系統的控制方法，其特征在于，實現基于雙目視覺的雙臂協作機器人系統的控制方法的雙臂協作機器人系統包括第一機械臂裝置(1)、第二機械臂裝置(3)、雙目檢測裝置(2)、工作區域裝置(4)、底盤裝置(7)和控制裝置；

所述底盤裝置(7)呈扁平長方體狀，沿長邊的延伸方向設置有第一端部和第二端部；所述工作區域裝置(4)水平安裝在底盤裝置(7)上表面的中心區域；結構相同的第一機械臂裝置(1)和第二機械臂裝置(3)鏡像安裝在底盤裝置(7)的第一端部和第二端部，用于對放置在工作區域裝置(4)上方的工件I(402)和工件II(403)進行裝配作業；所述雙目檢測裝置(2)包括L型連接桿(201)與雙目攝像頭(202)，雙目攝像頭(202)通過固定在底盤裝置(7)一側的L型連接桿(201)安裝在工作區域裝置(4)的正上方，用于識別工件I(402)、工件II(403)、第一機械臂裝置(1)末端和第二機械臂裝置(3)末端的點云數據；

所述控制裝置分別以工作區域裝置(4)、第一機械臂裝置(1)、第二機械臂裝置(3)和雙目檢測裝置(2)的中心點為原點構建基坐標系OXYZ、第一機械臂工作坐標系O₁X₁Y₁Z₁、第二機械臂工作坐標系O₂X₂Y₂Z₂和測量坐標系O₃X₃Y₃Z₃；

所述控制裝置實時跟蹤第一機械臂裝置(1)末端和第二機械臂裝置(3)末端的空間位姿，計算得到測量坐標系與基坐標系、第一機械臂工作坐標系和第二機械臂工作坐標系的轉換關系，建立基于雙目檢測裝置(2)的手眼關系模型，通過將測量坐標系下任意位置處坐標的重投影誤差最小化，得到手眼關系模型的最優模型參數，使第一機械臂裝置(1)、第二機械臂裝置(3)協作抓取過程中兩個機械臂末端中心點與待抓取工件中心點之間的誤差以及協作組合過程中工件I(402)和工件II(403)中心點之間的誤差最??；再在測量坐標系下控制第一機械臂裝置(1)和第二機械臂裝置(3)對工件I(402)和工件II(403)進行雙臂協作抓取與組合，完成對工件I(402)和工件II(403)的裝配任務；

所述控制裝置根據機械臂DH模型幾何參數值及其末端的期望位姿，求解逆運動學方程獲得末端到達期待位姿時各個關節變量值，根據第一機械臂裝置運動范圍最小以及行程最短到達的關節角，確定一組第一機械臂裝置最優解關節角；并且根據第一機械臂裝置最優解關節角，控制第一機械臂裝置(1)先行運動至工件I(402)所在區域，完成抓取任務，再根據第二機械臂裝置運動范圍最小以及行程最短到達的關節角，確定一組第二機械臂裝置最優解關節角，并根據第二機械臂裝置最優解關節角控制第二機械臂裝置(3)運動軌跡至第二工件所在區域，最后，由第一機械臂裝置(1)和第二機械臂裝置(3)協作完成裝配任務；

在第一機械臂裝置(1)和第二機械臂裝置(3)的移動和裝配過程中，雙目檢測裝置(2)不斷對雙臂末端位姿進行采集，在重投影誤差最小化的前提下，對其機械臂末端中心點坐標與待抓取工件I(402)、工件II(403)進行接近反饋；

所述控制裝置構建基坐標系OXYZ、第一機械臂工作坐標系O₁X₁Y₁Z₁、第二機械臂工作坐標系O₂X₂Y₂Z₂和測量坐標系O₃X₃Y₃Z₃的過程包括以下步驟：

以工作區域中心點O為坐標原點，以第一機械臂裝置(1)和第二機械臂裝置(3)的底盤中心點O₁、O₂連線為X軸，垂直于連線的方向為Y軸，根據右手法則確立Z軸，建立基坐標系OXYZ；

以第一機械臂裝置(1)底盤中心點O₁為坐標原點，以方向為X₁軸，與Y軸平行方向為Y₁軸，與Z軸平行方向為Z₁軸建立第一機械臂工具坐標系O₁X₁Y₁Z₁；

以第二機械臂裝置底盤中心點O₂為坐標原點，以方向為X₂軸，與Y軸平行方向為Y₂軸，與Z軸平行方向為Z₂軸建立第二機械臂工具坐標系O₂X₂Y₂Z₂；

以雙目檢測裝置的雙目攝像頭視角中心O₃為坐標原點，與X軸平行方向為X₃軸，與Y軸平行同向的方向為Y₃軸，以軸方向且與Z重合為Z₃軸建立測量坐標系O₃X₃Y₃Z₃；

所述第一機械臂裝置(1)包括第一機械臂底盤(111)、第一固定銅柱(110)、第一多孔圓盤(109)、第一肩關節總線舵機(108)、第一工字型連接桿(107)、第一機械臂末端(106)、第一掌關節總線舵機(105)、第一腕關節總線舵機(104)、第一肘關節總線舵機(103)、第一底部連接機構(102)，所述第一機械臂底盤(111)固定于底盤裝置(7)上；

所述第二機械臂裝置(3)包括第二機械臂底盤(311)、第二固定銅柱(310)、第二多孔圓盤(309)、第二肩關節總線舵機(308)、第二工字型連接桿(307)、第二機械臂末端(306)、第二掌關節總線舵機(305)、第二腕關節總線舵機(304)、第二肘關節總線舵機(303)、第二底部連接機構(302)，所述第二機械臂底盤(311)固定于底盤裝置(7)上；

所述控制裝置包括用于控制第一機械臂裝置(1)的第一機械臂舵控板(101)和用于控制第二機械臂裝置(3)的第二機械臂舵控板(301)，分別通過信號線一(5)和信號線二(6)與雙目檢測裝置(2)連接；

所述基于雙目視覺的雙臂協作機器人系統的控制方法，包括以下步驟：

S1，建立基坐標系OXYZ，第一、第二機械臂工具坐標系O₁X₁Y₁Z₁、O₂X₂Y₂Z₂及測量坐標系O₃X₃Y₃Z₃；

S2，對工件I(402)、工件II(403)的點云數據進行采集，經雙目攝像頭(202)取重心點作為中心點的方法，輸出工件I(402)、工件II(403)與兩個機械臂末端中心點在測量坐標系下的坐標；

S3，雙目檢測裝置(2)實時跟蹤第一、第二機械臂末端的空間位姿，建立基于雙目攝像頭的手眼關系模型，通過將測量坐標系下任意位置處坐標的重投影誤差最小化的方法，減小第一、第二機械臂協作抓取過程中機械臂末端中心點與待抓取工件中心點之間的誤差以及協作組合過程中工件I(402)與工件II(403)中心點之間的誤差；步驟S3中，所述雙目檢測裝置(2)實時跟蹤第一、第二機械臂末端的空間位姿，建立基于雙目攝像頭的手眼關系模型的過程包括以下步驟：

S31，將雙目攝像頭采集的圖像以M×N的矩陣形式存儲，其矩陣中元素的坐標為像素坐標(m,n)，其m軸、n軸互相垂直且分別與基坐標系的X軸、Y軸平行，原點O在m-n像素坐標系中的坐標為(m₀,n₀)，每一個像素在X軸、Y軸方向上的物理尺寸分別為d_x、d_y，則基坐標系與像素坐標系之間的關系表示為：

用齊次坐標系表示為：

S32，根據雙目攝像頭的透視投影模型，得到如下關系：

其中雙目攝像頭視角中心到基坐標原點的長為有效焦距f，(x₃,y₃,z₃)為測量坐標系上的點，則測量坐標系與基坐標系的轉換關系用齊次坐標表示為：

S33，通過機械臂末端和標定板的關系推導出標定板相對于雙目攝像頭的位姿即雙目攝像頭的外參數，設O₃、O、O₁、O₂、O₁′和O₂′分別為雙目攝像頭、工作區域、第一機械臂末端、第二機械臂末端、第一機械臂底盤和第二機械臂底盤中心點，得到以下等式：

其中，式(5.1)對應第一機械臂裝置，式(5.2)對應第二機械臂裝置，T表示旋轉平移矩陣，包括旋轉和平移兩部分，旋轉部分包含3個未知數，即分別繞X、Y、Z軸的三個轉角(θ_x,θ_y,θ_z)，以歐拉角表示的旋轉矩陣為：

平移部分包含3個未知數(t_x,t_y,t_z)；

S34，建立基于雙目攝像頭觀測模型的手眼關系模型：

其中，式(7.1)是第一機械臂裝置對應的手眼關系模型，式(7.2)是第二機械臂裝置對應的手眼關系模型，G是雙目攝像頭的內參數，[m n]^T、[x₃ y₃ z₃]^T是觀測數據，[m n]^T通過雙目攝像頭得到，由第一機械臂末端和第二機械臂末端執行返回，和是待求解的模型參數；

S35，將n組觀測數據作為輸入數據，將第一機械臂裝置第k次機械臂末端與雙目攝像頭之間的模型參數與第二機械臂裝置第k次機械臂末端與雙目攝像頭之間的模型參數作為已知參數代入待求解的目標方程，將第一機械臂裝置第k-1次機械臂底盤與雙目攝像頭之間模型參數與第二機械臂裝置第k-1次機械臂底盤與雙目攝像頭之間模型參數作為優化的初始值，采用非線性最小二乘求解與優化的目標函數為：

其中，分別代表像素坐標系上的點q以及測量坐標系下的點P；當該目標函數最小時，得到最終優化結果否則置k＝k+1，在n組數據中篩選出最精確的觀測數據作為最好的雙目攝像頭定位結果即測量坐標系下的坐標值；采用Levenberg-Marquardt算法求解每一步中的非線性優化問題；

S4，控制第一機械臂裝置(1)和第二機械臂裝置(3)對工件I(402)、工件II(403)進行雙臂協作抓取與組合，完成對工件I(402)、工件II(403)的裝配任務。

2.根據權利要求1所述的基于雙目視覺的雙臂協作機器人系統的控制方法，其特征在于，步驟S2中，對工件I(402)、工件II(403)的點云數據進行采集，經雙目攝像頭(202)取重心點作為中心點的方法，輸出工件I(402)、工件II(403)與兩個機械臂末端中心點在測量坐標系下的坐標的過程包括以下步驟：

在工作區域處放置標定板，在保證工作區域與標定板的大小與安裝位置一致的前提下，采用張正友標定法標定雙目相機的內參數，確定工作區域上的點在基坐標系與測量坐標系之間的一一對應關系；

獲取處于工作區域的工件I(402)和工件II(403)的點云數據，由雙目攝像頭(202)輸出測量坐標系下工件I(402)、工件II(403)中心點與機械臂末端中心點坐標。