基于Camshift視覺跟蹤和D-H建模算法的視覺機械臂控制裝置及方法
【技術領域】
[0001] 本發明屬于機械臂領域，設及一種視覺機械臂裝置，特別設及一種基于Camshift 視覺跟蹤和D-H建模算法的視覺機械臂控制裝置及方法。
【背景技術】
[0002] 近些年來，隨著科技的快速發展和工業生產規模的日益擴大，機械臂憑借其生產 速度快、穩定性強、加工精度高、生產成本低等優勢，已逐步替代各種人工操作，廣泛地應用 到工業各個生產流程中。與此同時，隨著機械臂的廣泛普及，越來越多的生產流程需要機械 臂具有較強的環境適應能力和人工智能，使其可W獨立、高效地完成各種生產流程，滿足生 產過程中的各種要求。而視覺機械臂可W滿足運樣的需求，因此視覺機械臂成為了當今機 器人領域的研究熱點之一。
[0003] 視覺機械臂是一種具有計算機視覺、可獨立完成各種動作的機械臂。視覺機械臂 通過對相機采集到的圖像序列進行分析和處理，針對不同工作環境和加工流程進行判斷和 決策，采用人工智能完成相應的生產動作，從而替代人類完成零件抓取、加工和組裝等各種 過程。但是，設計一種簡易、高效、準確的視覺機械臂裝置W及對視覺機械臂精確控制的研 究，一直是亟待解決和研究的困難和問題。
【發明內容】
[0004] 為了克服上述現有技術的缺點，本發明的目的在于提供一種基于Camshift視覺 跟蹤和D-H建模算法的視覺機械臂控制裝置及方法，解決了背景中無法對視覺機械臂進行 準確控制的難題，具備靈活性強、定位準確、定位目標不易丟失、控制精度高、節能等多個優 點。
[0005] 為了實現上述目的，本發明采用的技術方案是：
[0006] -種基于Camshift視覺跟蹤和D-H建模算法的視覺機械臂控制方法，機械臂為六 自由度機械臂，包括基部艙機4、肩部艙機5、肘部艙機6、腕部艙機7、握手艙機8和腕部轉 動艙機9 ;
[0007] 采用Camshift視覺跟蹤方法精確定位目標工件的位置坐標信息，具體步驟如下：
[0008]步驟1. 1)，根據待加工工件的特征信息，創建表示各種工件的Camshifit直方圖， 用于跟蹤和匹配；
[0009]步驟1. 2)，利用相機掃描各工件，在相機攝取的第一帖圖像中，W所述各種工件的 Camshift直方圖為特征，對圖像中的所有像素進行匹配，捜索到某一工件的像素區域，得到 該工件中屯、相對相機坐標中屯、的位置信息，即X和Y坐標；
[0010]步驟1. 3)，根據工件所占像素點多少、相機焦距W及工件真實大小，利用小孔成像 原理，計算工件在圖像中的深度，得到工件距離相機的高度位置信息，即Z坐標；其中X、Y、 Z坐標均是在相機坐標系忙}中的值；
[0011] 步驟1.4)，w第一帖圖像中工件的中屯、為起始點，對下一帖圖像中的工件進行捜 索和定位，不斷重復步驟2)和步驟3)，從而實現在連續的圖像中對工件精確位置信息的準 確獲取；
[0012] 采用D-H建模算法實現對視覺機械臂、目標工件化及工作臺等多目標的準確定位 具體步驟如下：
[0013] 步驟2. 1)，利用基坐標系巧}、工作臺坐標系怯}、腕部坐標系{W}、工具坐標系m 和目標坐標系{G}實現對視覺機械臂、目標工件和工作臺在同一坐標系下的準確定位；巧曰 圖3)
[0014]步驟2. 2)，對視覺機械臂建立W各艙機關節為坐標系原點的多坐標系模型巧曰圖 4)，根據步驟2. 1得到的機械臂的初始位置信息和期望目標位置信息，采用逆運動學對機 械臂的各個關節進行解禪運算，從而得到機械臂各個運動關節的控制參數，實現對機械臂 的移動控制。
[0015] 所述步驟1. 1)中，待加工工件的特征信息包括其形狀、尺寸和顏色信息。
[001引所述步驟2.1)中：
[0017] 基坐標系{B}位于操作臂的基座上，X軸與操作臂轉軸重合（方向任意），Z軸垂 直于水平面向上，y軸通過右手定則確定；
[0018] 工作臺坐標系怯}位于工作臺的一個角上，是一個通用的坐標系，Z軸垂直于水平 面向上，xy軸根據實際工作環境標定（需符合右手定則），機械臂的所有運動均相對于該坐 標系執行，工作臺坐標系怯}根據基坐標系{B}確定，即|7 ,夢^表示從工作臺坐標系怯} 到基坐標系巧}的變換矩陣；
[0019] 腕部坐標系{W}位于機械臂的末端連桿，X軸與腕部旋轉軸重合（方向任意），y 軸與腕部連桿重合（方向任意），Z軸通過右手定則確定；
[0020] 工具坐標系{T}附于機械臂所夾持工具的末端，X軸與腕部連桿重合（方向任 意），y軸垂直于X軸（方向任意，）Z軸通過右手定則確定，當手部沒有夾持工具時，工具坐 標系{T}的原點位于機械臂的指端之間，工具坐標系{T}根據腕部坐標系{W}確定，即
[0021] 目標坐標系{G}是機械臂移動工具時對工具位置的描述，在機械臂運動結束時， 工具坐標系{T}與目標坐標系{G}重合，目標坐標系{G}根據工作臺坐標系怯}由人為指 定或根據實際情況初始化進行確定；
[002引同時，確定相機坐標系似在腕部坐標系{W}中的定義奸，奸=襪抒。
[0023] 本發明通過如下方式對視覺機械臂建立W各艙機關節為坐標系原點的多坐標系 模型：巧日圖4所示）
[0024] 步驟1)，找出各關節軸，并標出運些軸線的延長線，在下面的步驟2)至步驟5)中， 僅考慮兩個相鄰的軸線，即關節軸i和i+1 ;其中，i表示連桿和關節軸的編號，連桿i和連 桿i+1之間通過關節i+1相連
[00巧]步驟2)，找出關節軸i和i+1之間的公垂線或交點，W該公垂線或交點與關節軸i的交點作為連桿坐標系{i}的原點；
[0026] 步驟3)，規定連桿坐標系U}中Z軸為沿關節軸i的指向；
[0027] 步驟4)，規定連桿坐標系{i}中V軸沿公垂線的指向，如果關節軸i和i+1相交， 則規定A>,;軸垂直于關節軸i和i+1所在的平面；
[002引步驟5)，按右手定則確定連桿坐標系W的；f;軸；
[0029] 將關節i對應坐標系U-1}，用4個連桿參數來描述，具體如下：
[0030] 1)曰1表示第i個連桿的長度；
[003。 。a康示第i關節軸與第i+1關節軸線的夾角；
[003引扣0康示第i個連桿和第i+1個連桿的夾角；
[0033] 4)di表示在第i個關節軸線上相鄰兩連桿的距離；
[0034] 由坐標系a-u至挫標系w的變換順序為：
[0035] 1)繞衣1軸旋轉a1 1角度；
[003引。沿和，洋由平移曰1 1距離；
[0037]扣繞為軸旋轉0 1角度；
[003引 4)沿之.軸平移di距離。
[0039] 所述步驟2. 2)中，先進行坐標變換求出相對于基坐標系{B}的腕部坐標系{W}的 變換矩降
然后應用逆運動學求關節角。
[0040] 所述腕部坐標系{W}相對于基坐標系腳的位置
表示腕部坐標系{W}的坐標原點P在基坐標系巧}的坐標位置），第1個連桿和第2個 連桿的夾角
第2個連桿和第3個連桿的夾角02=-(P+ilO(其中，
，第3個連桿和第4個連桿的夾角
第4個連桿和第5個連桿的夾角0 4= - ( 0 3+e2)，其 中曰1表示第i個連桿的長度。
[0041] 綜上所述，通過幾何解法計算得到機械臂各關節艙機角的準確值，總結如下：
[0042]
[004引其中，0康示基部艙機4的轉動角度，0康示肩部艙機5的轉動角度，0 3表示 肘部艙機6的轉動角度，0 4表示腕部艙機7的轉動角度，0 5表示腕部轉動艙機9的轉動 角度，握手艙機8的轉動角度根據零件大小通過初始化設定來確定。
[0044] 本發明利用關節空間規劃法規劃機械臂運動，設其軌跡為一條確定的光滑運動曲 線0 (t) =au+ait+a2t2+a3t3。考慮在一定時間內將工具從初始位置移動到目標位置的問題。 應用逆運動學可W計算出對應于目標位姿的各個關節角。記其在t。時刻的值為初始位置， 在tf時刻為目標位置。
[0045] 根據約束條件為：
[0046]
[0047] 最終得到方程中未知量曰1為：
[0052] 本發明還提供了一種基于Camshift視覺跟蹤和D-H建模算法的視覺機械臂控制 裝置，其中，機械臂為六自由度機械臂，包括基部艙機4、肩部艙機5、肘部艙機6、腕部艙機 7、握手艙機8和腕部轉動艙機9,控制裝置包括：
[0053]USB相機1，位于機械臂腕部，掃描工作臺上的各種零件，獲取各個零件的定位信 息；
[0054] 卡片電腦2,接收USB相機1發送的定位信息，根據機械臂待完成動作指令，獲取機 械臂各艙機的控制參數；
[0055] 機械臂艙機控制器3,接收卡片電腦2發送的各艙機的控制參數，分別傳輸至各艙 機執行相應的動作。
[0056]與現有技術相比，本發明可實現視覺機械臂的精確控制。
【附圖說明】
[0057] 圖1是視覺機械臂系統裝置示意圖。
[0058] 圖2是視覺機械臂控制流程示意圖。
[0059] 圖3是視覺機械臂坐標系描述示意圖。
[0060] 圖4是機械臂多機構坐標系建模示意圖。
[0061] 圖5是腕部坐標系相對于基坐標系的位置。
[0062] 圖6是視覺機械臂在多坐標系描述中的各連桿的平面幾何關系。
【具體實施方式】
[0063] 下面結合附圖和實施例詳細說明本發明的實施方式。
[0064] 如圖1所示，本發明基于Camshift視覺跟蹤和D-H建模