[發明專利]一種基于雙目立體視覺的SCARA機器人軌跡規劃方法

在審

• 申請號: 201711086587.2
• 申請日: 2017-11-07
• 公開（公告）號: CN107932502A
• 公開（公告）日: 2018-04-20
• 發明（設計）人: 李頎;強華
• 申請（專利權）人: 陜西科技大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 西安智大知識產權代理事務所61215
• 代理人: 王晶

1.一種基于雙目立體視覺的SCARA機器人軌跡規劃方法，其特征在于，包括以下步驟；

Step1、安裝工業相機，獲取工作區域圖片

將兩個相機安裝在機器人工作區域的正上方，調整相機的焦距使整個工作區域處在兩個相機的視野范圍內；

Step 2、進行雙目立體相機的標定

通過兩個相機對不同方位的標定板進行同時拍照，并將每一次的標定板的位置與標定板的狀態都記錄下來，然后尋找一個公共的平面，使得兩個相機的光軸平行,確定了此平面后，每次采集到的左右相機的圖像都要先變換到此平面，然后利用三角定位法根據相同物體在左右相機中的視差來計算物體的高度H₁和障礙物的高度H₂；

Step 3、障礙物和工件的位置的確定

第一步，對工作區域進行拍照，利用背景相減法，用左相機的圖像與背景圖像相減得到只含有工件和障礙物的圖像；

第二步，利用模板匹配的方法確定工件的位置，在背景相減后的圖像中搜索模板圖像，即將制作好的模板在當前圖像上進行滑動和轉動，用相似性度量來算出每塊圖像與模板圖像的相似度，最終當相似值大于0.75時，認為匹配到模板，同時記錄下當前坐標為匹配到工件的坐標，圖片中去掉工件的圖像，就可以確定障礙物的位置；

Step 4、確定軌跡插值點，軌跡的起點為工件坐標位置(X₀，Y₀)，終點為裝配位置(X_f，Y_f)，為了繞開障礙物增加插值點(X_c，Y_c)；

Step 5、對SCARA機器人進行建模

計算機器人末端執行器與各個關節角度的轉換關系，利用DH方法對機器人進行建模，機器人末端執行器的坐標為(Px,Py,Pz)，機器人第一和第二關節轉過的角度分別為θ₁與θ₂，第一關節與第二關節的長度分別為L₁和L₂，第三個關節上下移動的距離為d₃；⁰T₄表示O₃坐標系相對于O₀坐標系的轉換關系可由坐標系的轉換公式得到公式1，最后一列與機器人坐標系的對應關系如公式2所示，即已知一點的機器人坐標(Px,Py,Pz)就可得到對應各個關節旋轉的角度；

T04=T01T12T23T34=C12-4S12-40L2C12+L1C1S12-4-C12-40L2S12+L1S100-1-d30001---(1)]]>

Px=L2Cos(θ1+θ2)+L1Cosθ1Py=L2Sin(θ1+θ2)+L1Sinθ1Pz=-d3---(2)]]>

Step 6、對機器人進行關節空間的軌跡規劃

采用五次多項式對軌跡進行擬合，其表達式如公式3所示，如公式4所示，計算起點的角度、速度和加速度分別為θ₀、和計算終點的角度、速度和加速度分別為θ_f、和軌跡規劃要保證軌跡的初始速度和加速度都為零，終點的速度與加速度也為零，即起點的角度θ₀和終點的角度θ_f都已知，即可解方程得到關節的運動曲線方程如公式5所示，以此類推，其他關節運動的軌跡也可得到，這樣就完成了機器人的軌跡規劃，保證了機器人各個關節運動的速度和加速度都是連續可導的；

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5θ·(t)=a1+2a2t+3a3t2+4a4t3+5a5t4θ··(t)=2a2+6a3t+12a4t2+20a5t3---(3)]]>

θ0=a0θf=a0+a1tf+a2tf2+a3tf3+a4tf4+a5tf5θ·0=a1θ·f=a1+2a2tf+3a3tf2+4a4tf3+5a5tf4θ··0=2a2θ··f=2a2+6a3tf+12a4tf2+20a5tf3---(4)]]>

θ(t)=θ0+10(θf-θ0)tf3t3+15(θ0-θf)tf4t4+6(θf-θ0)tf5t5θ·(t)=30(θf-θ0)tf3t2+60(θ0-θf)tf4t3+30(θf-θ0)tf5t4θ··(t)=60(θf-θ0)tf3t+180(θ0-θf)tf4t2+120(θf-θ0)tf5t3---(5).]]>