//Logo Image
作者:涂超恒(2010-08-25);推薦:徐業良(2010-08-26)
附註:本文為九十八學年度元智大學機械工程研究所涂超恒碩士論文「智慧型機器人輪椅之史都華平台座椅機構設計研究背景與目的」第三章。

第三章 史都華平台之設計與製作

為建立史都華平台控制技術,本章實際建置一六軸史都華平台,以單晶片控制器達成其六軸同步控制,使用者可以在電腦端輸入欲達成之位置與角度,直接控制六軸史都華平台作動。

3.1 整體設計概念

3-1為本研究建置之史都華平台整體設計概要圖。「可動板」和「固定板」之間使用6支「線性致動器」連結,其接頭分別是球接頭以及萬向接頭,透過6支線性致動器的不同長度配合,可動板能有不同位置及角度的變換,來達到使用者的需求。使用者在「遠端電腦介面」上輸入欲達成之角度與位置,經由網際網路傳送給「單晶片控制器」,發送訊號給「馬達驅動模組」,控制6支線性致動器的動作,每一支線性致動器的「線性電阻尺」會回報訊號給單晶片控制器,檢查動作位置是否正確,完成史都華平台的模擬動作。

3-1. 史都華平台之概要圖

3.2 硬體元件

3-2為組裝完成之史都華平台,各硬體元件設計詳述如下。

3-2. 組裝完成之史都華平台

(1)   線性致動器與線性電阻尺

史都華平台上裝有6線性致動器,每一支線性致動器搭配一個線性電阻尺(如圖3-3),線性電阻尺會因線性致動器的作動而伸縮移動,長度改變會有不同的訊號值,回報訊號值給單晶片控制器,判斷線性致動器的作動位置是否正確。

C:\Documents and Settings\Hello\桌面\線性致動器.jpg

3-3. 線性致動器和線性電阻尺

本研究採用大銀公司的線性致動器,型號LAS-2-1-150-12G,工作電壓為12V,最大負荷電流為6A,最快速度為25mm/sec,標準行程為0~150mm。每組線性致動器並搭配長新科技公司的線性電阻尺使用,型號PA1-F-150-S01M,線性電阻尺會因為長度改變而有不同的電阻值,呈線性變化,電阻值介於0~4K歐姆,作為判斷線性電阻尺長度的訊號值。

(2)   單晶片控制器與馬達驅動器

本文使PIC18F452單晶片控制器,設定1個輸入腳位接收線性電阻尺訊號,搭配馬達驅動器TOSHIBA(TA7257P)馬達驅動IC來達到對電動缸的控制。3-4為馬達驅動器與PIC連接的電路簡圖,圖中的電容C建議為0.1mfR1R2分別為33歐姆以及20k歐姆。

TA7257

3-4. 馬達驅動器與PIC連接的電路簡圖

(3)   六軸史都華平台之可動板與固定板

3-5是史都華平台可動板與固定板之工程圖,可動板和固定板是利用6支線性致動器連結,連接點均為對稱分佈,線性致動器在可動板和固定板之間的接頭分別是球接頭以及萬向接頭,再藉由6支線性致動器不同長度的變化,可動板在三度空間中擁有六個自由度的運動,當6支線性致動器長度固定時,可動板便不再移動。由於線性致動器只受軸向力,且分布於每支線性致動器,平台擁有剛性高的特性。

3-5. 史都華平台可動板與固定板之工程圖

3.3 六軸同步控制程式

史都華平台程式分為二部分,包括單晶片控制器程式中的六軸同步控制,另一部分是電腦端程式,即是提供使用者操控之六軸史都華平台控制介面。

(1)   六軸同步控制策略

史都華平台是一並聯式的機器人,給定一欲達成之位置和方向,6支線性致動器各有不同目標長度,在做動的過程中,6支線性致動器容易發生互相牽引或拉扯的現象,因此在撰寫程式的控制策略上,六軸同步控制是很重要的。做動的過程中應使6線性致動器能同時伸長,伸長速度快慢不同(伸長量大者速度較快),使6線性致動器幾乎同時到達設定之目標長度,不致因為線性致動器的某些動作順序而產生干涉,或動作上的不平順

3-6是史都華平台的六軸同步控制程式中副函式actuator_control的流程圖,副函式actuator_control_2也是以相同邏輯下撰寫,執行步驟敘述如下:

(a)    首先將目標長度輸入至副函式actuator_control,藉由每組線性致動器上的線性電阻尺判斷出即時長度,計算出每組線性致動器目標長度與即時長度的差值,即是誤差值(error_),此時每支線性致動器的誤差值便是每支線性致動器的伸長量。

(b)   接下來判斷誤差值介於區間範圍error_table的哪個區間,透過速度選擇motor_speed_select選出適當速度,輸出給對應之線性致動器,誤差值大者速度比較快,反之誤差值小者則給予比較慢的速度,期望使6支線性致動器接近同時到達

(c)    在線性致動器尚未到達目標長度時,程式每秒會持續檢查是否已達目標長度,但是不再作任何輸出,等到距離目標長度的絕對值小於等3mm時,將停止輸出,此時速度為0,讓馬達自然煞車,如此達到緩衝的效果,提高位置精確度,並回歸到等待使用者輸入目標長度狀態。

3-6. 六軸同步控制程式中副函式actuator_control的流程圖

(2)   六軸同步控制程式

同步控制之程式可分為主程式與副函式,其程式內容的變數與撰寫定義如下:

(a)    定義變數

Ÿ             error_control_length減去a_length_now所得的誤差值,即是線性致動器必須做動的伸長量。

Ÿ             error_abs_tmperror_絕對值的值。

Ÿ             error_table[10]為類模糊控制中,程式判斷線性致動器error_abs_tmp值的區間範圍。

Ÿ             motor_speed_select[8]為類模糊控制中,程式經由判斷後可以選擇之速度。

(b)   副函式

Ÿ             “actuator_control  (int8 ch , int16 control_length)”為控制線性致動器速度之程式,其中ch表示輸出第幾組PWM訊號,ch的值為123control_length表示被控制的長度,主要功能為計算出線性致動器之模擬速度。

Ÿ             “actuator_control_2 (int8 ch , int16 control_length)”actuator_control的意義相同,其ch的值為567

(c)    主程式

等待使用者輸入指令,接收後即便傳送目標長度至主程式的副函式actuator_controlactuator_control_2,判斷每一支線性致動器當時的誤差值,並且計算出每一支線性致動器應該有的模擬速度,誤差值大者速度較快。計算出6支線性致動器速度後同時發送給每支線性致動器,同時開始做動,因為伸長速度快慢不同,使6線性致動器幾乎同時到達設定之伸長量,達到同步的效果

(3)   PWM訊號程式

本文使用PWM方式控制線性致動器,希望以一個PIC18F452單晶片控制器輸出6PWM訊號,控制6支線性致動器,達到對史都華平台的六軸同步控制。PIC18F452單晶片控制器的Library初始設定只有3PWM訊號輸出,必須另外撰寫新的3PWM訊號,完成新的Library

PWM訊號

直流馬達控制通常採取脈波寬度調變(Pulse Width Modulation)的方式,簡稱為PWMPWM訊號其原理如圖3-7所示,圖中高電位的部分是馬達動作時間,低電位的部分是馬達停止時間,兩者時間和即為一周期。而平均電流正比於圖3-7中的脈波的面積和,因此當使用者想降低馬達轉速時,只要減少動作時間,增加停止時間,並且保持周期不變,即可降低平均電流。由於改變轉速是利用改變動作的時間比例,此種控制方式被稱做為脈波寬度調變。

3-7. PWM訊號原理

定義變數

3-86PWM訊號的程式,以C語言撰寫,圖3-8(b)5759行定義了6PWM訊號的變數,以其中一組訊號舉例說明,M1AM1B表示01的數位訊號,0代表沒有輸出,1代表有輸出,圖3-8(b)77行和第78行的程式邏輯是將挑選圖3-8(a)所撰寫好的給電訊號,從中選出一行當作輸出訊號,其中M1這組PWM訊號所輸出的腳位即是D1D0,而其他組PWM訊號也是以相同邏輯下所撰寫。

3-8(a). 6PWM訊號的程式(1)

3-8(b). 6PWM訊號的程式(2)

副函式

3-8(c)中的motor(ch, speed)motor_2(ch, speed)表示控制馬達PWM輸出的程式寫法,其中ch表示每一組PWM訊號的的代號,在副函式motor( )其值為123而在副函式motor_2( )其值為567speed表示模擬的速度,其值在-15~15間,正、負值分別表示馬達正轉和反轉。“motor_off( )” “motor_off_2( )”其功能分別是將motor( )motor_2( )的速度預設為0,藉此停止線性致動器的動作。

3-8(c). 6PWM訊號的程式(3)

撰寫了新的PWM訊號程式後,在引用現有的PWM訊號程式的Library會造成錯誤產生,而在撰寫主要程式的時候,必須引用"pic_PROTO_lib.h",裡面包含了現有的PWM訊號的Library。為避免發錯誤,需做以下修改:搜尋YZ_PIC裡檔名為pic_PROTO_lib的檔案,其路徑為C:\YZ_PIC\Samples\Library_5.05,打開後在第140行的#include "pic_PROTO_motors.c"前面加上兩個斜線"//",將此行當成註解,不開起此行程式功能,如圖3-9所示。在撰寫程式時,必須加入新的LibraryPWM訊號程式。

程式3.jpg

3-9. pic_PROTO_lib程式

3.4史都華平台電腦端控制介面之設計

開發之電腦端六軸史都華平台控制介面以VB2005撰寫,使用者在控制介面上輸入史都華平台可動板中心點預計達成的三維座標及方向,藉由逆向運動學程式計算出史都華平台每支線性致動器的長度,並且在介面上顯示出史都華平台模擬姿態,如所輸入之三維座標及方向超出史都華平台之工作空間,也會顯示警示訊息告知使用者待使用者確認後,即操控六軸史都華平台每支線性致動器的長度至欲達成之目標長度,完成其作動。

以下及分別就史都華平台電腦端控制介面程式中「逆向運動學程式」和「使用者介面程式」作一說明。

(1)   逆向運動學程式

本研究開發之史都華平台可動板以及固定板的半徑、球接頭與水平方向的夾角等尺寸如圖3-10所示,線性致動器尚未伸長之原始長度為340mm,行程為150mm

3-10. 固定板與可動板示意圖

逆向運動學程式主要功能,是由使用者輸入之史都華平台可動板中心點預計達成的三維座標及方向,計算出史都華平台每支線性致動器的長度,如所輸入之三維座標及方向超出史都華平台之工作空間則顯示警示訊息告知使用者。

3-11為逆向運動學程式,程式內容先行定義參數,接著分成可動板程式、固定板程式、旋轉矩陣程式和主程式等三個部分。

(1)   定義參數(圖3-11(a)):

Ÿ    Lmin(0, 5)Lmax(0, 5)分別是線性致動器未伸長之原始長度以及行程。

Ÿ    x, y, z, yaw, pitch, roll為使用者輸入的可動板座標及角度,對固定板座標系統之X軸旋轉yaw角度,再對固定板之Y軸旋轉pitch角度,再對固定板之Z軸旋轉roll角度。

Ÿ    Rp, pcp(0, 2), Rb, bcp(0, 2)分別是可動板及固定板的半徑和座標。

Ÿ    Pjx(0, 6), Pjy(0, 6), Pjz(0, 6), Bjx(0, 5), Bjy(0, 5), Bjz(0, 5)為可動板和固定板尚未作動的接點座標。

(2)   可動板程式(圖3-11(a)):

Ÿ    可動板程式計算出初始位置時可動板上6個球接頭座標。

(3)   固定板程式(圖3-11(b)):

Ÿ    固定板程式計算出固定板上6個球接頭座標。

(4)   旋轉矩陣程式(圖3-11(b)):

Ÿ    旋轉矩陣程式依據使用者輸入之可動板中心點預計達成的方向,計算其旋轉矩陣,程式中yaw, pitch, roll分別為可動板對x軸、y軸以及z軸的夾角。

3-11(a). 史都華平台逆向運動學程式

3-11(b). 史都華平台逆向運動學程式

(5)   主程式:

3-12為史都華平台逆向運動學程式主程式,主要功能是要計算出6支線性致動器的伸長量。主程式流程說明如下:

Ÿ    給定史都華平台可動板中心點預計達成的三維座標及方向(工作位置),先推算出可動板位於工作位置上的6個球接頭座標,對應固定板上6個球接頭座標(由固定板程式計算),便可計算出每一支線性致動器的總長度及伸長量(圖3-12(b))。

Ÿ    可動板位於工作位置上的6個球接頭座標是由可動板程式計算初始位置的6個球接頭座標作平移和旋轉後的計算結果,將位於工作位置之可動板的中心點和初始位置之可動板的中心點相減,初始位置的6個球接頭座標加上相減的結果,作平移計算,再乘上旋轉矩陣(旋轉矩陣由旋轉矩陣程式計算),做旋轉計算,所得結果即是位於工作位置上的6個球接頭座標。

Ÿ    最後判斷若有任一支致動器的長度在最大伸長長度範圍之外,則顯示「超出範圍」。

3-12(a). 史都華平台逆向運動學程式主程式

3-12(b). 史都華平台逆向運動學程式主程式

(2)   使用者介面設計

敘述網際網路遠端控制六軸史都華平台之使用者介面設計及網際網路傳送程式。

(a)    使用者介面設計

史都華平台遠端控制使用者介面如圖3-13所示,大致分成左右兩邊,右半邊上方是遠端史都華平台的單晶片控制器的IP位置(IP Address),下方則是提供使用者鍵入預計可動板中心點可達成的三維座標(Coordinate)及方向(Orientation)

Ÿ    Measure」按鍵的功能是利用逆向運動學程式計算出每支線性致動器的長度,並且在介面上顯示長度,並且執行繪圖程式,在左半邊顯示出史都華平台模擬姿態。如超出工作空間,則顯示「超出範圍」以警示使用者,

Ÿ    Submit」按鍵的功能則是透過網際網路傳送網路指令到遠端史都華平台的單晶片控制器。

Ÿ    Reset」按鍵的功能是傳送一每支線性致動器都未伸長的長度之網路指令。

Ÿ    Test」按鍵的功能是每隔5秒發送一組已計算過之線性致動器長度的網路指令,總共5組。

Ÿ    Clear」按鍵的功用是清除左半邊已繪製好之圖形。

Ÿ    使用者介面左半邊所繪製之平台模擬圖形上為俯視圖,而下為前視圖,提供使用者作參考。另外VB2005在繪製圖形時,其座標軸原點位於左上角,非左下角,因此前視圖必須作一座標轉換,將其座標原點轉換至左下角,座標轉換程式如圖3-14所示。

3-13. 使用者介面

3-14. 前視圖座標轉換程式

參考資料

[1]       大銀微系統股份有限公司http://www.karma.com.tw/index.asp

[2]       長新科技公司http://www.arico.com.tw/tw/company.php