實際操作四軸機械手臂並且定位位置計算

在今日機械手臂廣泛運用於自動化，學習如何使用機械手臂並且進行定位非常重要。首先先進行四軸機械手臂的分析，然後再運用數學計算出定位機械手臂位置與角度關係，編寫程式使其自動化完成計算，最後再實際操作機械手臂使機器運作可以達到使用者之期望動作。

計畫參與人員：黃俊翰、彭柏祐、潘彥志

指導教授：范志海

前言

程式設計

摘要

感謝

本文所要探討的是機械手臂的運動原理以及路徑計算規劃，藉由計算才能確定位置分布以及方向性，再經由實際操作試驗是否可行。機械手臂在起點移動到目標位置時，通常是使用點到點運動的路徑規劃，也就是知道起點的各個關節角度、目標位置的各關節角度及運動時間，有了這三個條件就能寫出角位移方程式，可以再加入其他的條件，來寫出不同的角位移方程式。有了角位移方程式，機械手臂才能從起點移動到目標位置；或是在直角座標下，知道起點位置、目標位置及運動時間，也能寫出位移方程式。機械手臂之路徑規劃計算可以從兩個方面著手：一個是卡式座標分析法，另一個是節點座標分析法。由於使用卡式座標進行規劃就為容易，所以主要是以直線路徑來進行規劃，通常可由座標的齊次轉移矩陣（Homogeneous Transformation Matrix）直接完成。節點座標規劃法主要是利用多項式來規劃出各個關節點角度對時間的方程式，如常見的三次多項式（Cubic Spline Polynomial）或更高階的多項式；也有利用線性方程式來行此規劃，並同時混合拋物線方程式一起規劃以達到其連續性。完成數學計算之後，使用Microsoft Visual Studio 6.0編寫一可以自動計算其各關節角度對照XYZ平面之程式， 之後將程式編寫進入機械手臂試驗其動作

自動轉換後的結果

許哲誌，碩士論文，關節型機械臂系統整合晶片之研製，南台科技大學，民國九十四年。

羅致卿，碩士論文，機械手臂運動誤差之分析與路徑規劃，逢甲大學，民國九十二年。

羅致卿，碩士論文，以定位誤差為基準之機械手臂操作位置最佳化，逢甲大學，民國九十三年。

卓勝鵬編譯，機械人控制，全華科技圖書，2000

機械臂的運動是由已知的關節位置(馬達)向量經座標轉換後推導出其手部端點的位置向量。簡單的說就是空間中機械手臂能到的位置都是工作地點。使用正運動方程式可以從角度得知位置，所以反過來我們先把所要定位的位置以XYZ軸定位之後，就可以算出各關節角度。根據計算就可以得知程式如以下：假設我們所需要定位之座標為（X,Y,Z）

各軸長度為下：a(一號碼達到二號碼達距離)b(二號碼達到四號碼達距離)c(四號碼達到三號碼達距離)最後再設第一、二、三軸角度為e、f、g我們就可以計算出第一、二、三軸馬達角度

第一軸馬達角度(e)：e = ((Atn(Y/ X)) / 3.14) * 180

第三軸馬達角度(g)：假設數值 I、K1、K2k1 = X * Cos(((Atn(Y/ X)) * 3.14) / 180) + b *Sin(((Atn(Y/ X)) * 3.14) / 180)k2 = -Z + aI = (k1 * k1 + k2 * k2 - b * b - c * c) / (2 * b * c)g = (Atn(-I / Sqr(-I * I + 1)) + 2 * Atn(1)) * 180 /3.14

第二軸馬達角度(f)：假設數值JJ = 2 * (((-k1 * c * Sin(g)) + k2 * (b + c * Cos(g)))) /(k1 * (b + c * Cos(g)) + k2 * b * Sin(g))f = (Abs(Atn(J))) * 180 / 3.14

實際使用之四軸機械手臂完成品近照以及其基本概念圖

利用圖形推斷XYZ軸與各軸角度關係

電路說明93C66記憶體電路，儲存動作的資料：使用一個93C66 ，IC93C66為一串列通訊介面的記憶體，可規劃為512X8Bit或256X16Bit，同型IC還有93C46(128X8Bit OR 64X16Bit)、93C56(256X8Bit OR128X16Bit)、93C86(2048X8Bit OR 1024X16Bit)，若更換IC，則程式中的位址處理也要隨之修正，其餘的控制方法則都一樣。

控制步驟及指令：(1) CS=1，選擇晶片開始工作。(2) 由DI輸入串列位元，每輸入一個bit，必須配合從SK腳輸入一時脈信號，在正緣觸發時會將此位元送入EEPROM內。(3) 先由DI輸入開始位元(SB：Start Bit)=1和兩位的操作碼(OP：Operation Code)決定控制的動作，再輸入位址及資料。(4) 由ORG腳決定為word(ORG浮接)或byte(ORG接地)資料存取。(5) 致能寫入(EWEN)後，才可進行寫入(WRITE、WRAL)或清除(ERAL)動作。寫入完畢後，最好立即禁能抹除/寫入動作(EWDS)，以免受干擾。(6) 再寫入或清除動作時，若檢查DO=0表示忙碌中，若DO=1表示已經輸入完畢，可再輸入下一筆資料。(7) 如果是讀取動作時，在輸入位址後，從SK腳每輸入一個正緣觸發的時脈訊號，會由DO腳輸出1bit的串列訊號。(8) 最後令CS=0使晶片停止動作。

93C66的控制指令及步驟93C66的接腳定義

記憶體及介面電路

根據8051控制晶片對電腦連結所使用之四軸機械手臂控制程式與實際操作之輸入圖

8051基本電路

控制區晶片近照

控制區晶片近照

根據右圖計算程式寫出的角度與xyz軸相對關係自動轉換程式程式碼

未輸入數值之轉換程式

參考資料

感謝范志海教授教授程式寫作和機械手臂提供

以及感謝廠商益眾科技提供四軸機械手臂控制程式