Robot Hand

1

บทที่1 บทนํา การควบคุมแขนกลดวยการเคลื่อนไหวของแขนมนุษย ระยะที่2 ( MASTER-SLAVE ROBOT ARM PHASE 2 ) 1.1 หลักการและเหตุผล เนื่องจากในปจจุบันนี้ มีอุตสาหกรรมตางๆมากมายที่เปนอันตรายตอมนุษยซึ่งอันตรายเหลานั้นเกิด ขึ้นมาจากความรอน,สารพิษ เปนตน ซึ่งสิ่งเหลานี้เมื่อมนุษยไดสัมผัสแลวอาจทําใหเกิดอันตรายตอรางกายได ดั้งนั้นจึงไดมีการเสนอโครงการ MASTER-SLAVE ROBOT ARM ซึ่งแขนกลจะถูกควบคุมใหมีการเคลื่อนที่ ไปตามการเคลื่อนไหวของแขนมนุษยและยัง สามารถหยิบจับวัตถุได ซึ่งเราสามารถควบคุมแขนกลนี้ทํางานเปน อันตรายแทนมนุษยได

1.2 วัตถุประสงค 1. เพื่อศึกษาระบบการควบคุมและออกแบบ Master arm และ Slave arm 2. เพื่อปรับปรุงโครงการ MASTER SLAVE ROBOT ARM ระยะที่ 1 ในดานโครงสรางและการ ควบคุม 3. เพื่อพัฒนาแขนกล เพื่อใหสามารถทํางานแทนมนุษยในงานตางๆได

1.3 ขอบขายของงาน 1. ปรับปรุงเรื่องการตอบสนองของการควบคุม 2. ปรับปรุงเรื่องการตอบสนองของการควบคุม slave arm ซึ่งโครงการระยะที่ 1 ยังความผิดพลาด ของการเคลื่อนที่เกิดขึ้น 3. ปรับปรุงโครงสรางของของ slave arm ใหสามารถจับวัตถุไดโดยสามารถควบคุมมือของ slave arm ได

1.4 ผลที่คาดวาจะไดรับ 1. 2. 3. 4.

ไดความรูเกี่ยวกับ หลักการทํางานของ Master slave robot arm ไดความรูเกี่ยวกับการออกแบบและหลักการสรางแขนกล ไดความรูเกี่ยวกับการทํางานและชุดวงจรควบคุมมอเตอร ไดรับความรูเกี่ยวหลักการควบคุมแขนกลใหไดตามที่ตองการ

2

1.5 แผนการดําเนินงาน

กิจกรรม 1.คนควาขอมูลและหลักการทํางานของ Master slave robot arm 2. ศึกษาการทํางานของระบบควบคุมแบบ position control และ การควบคุมมอเตอร 3. ศึกษาระบบ mechanics เพื่อใชในการออกแบบ แขน กล 4. ศึกษาโครงการการควบคุมแขนกลดวยแขน มนุษย ในระยะที่ 1 5. ปรับปรุงโครงสรางของ master และ slave arm 6. ทําการทดสอบการปรับปรุงโครงสราง 7. ออกแบบ master และ slave arm และชุดการ ควบคุม แขนกล 8. จัดทําชุดวงจรควบคุมมอเตอรและทําชุดควบคุม แขน กล 9. ทําการทดสอบงานจริงของ master และ slave arm 10. สรุปผลการทดลอง

เดือน ป มิ.ย. ก.ค. ส.ค. ก.ย. ต.ค. พ.ย. ธ.ค. ม.ค.

3

บทที่ 2 ทฤษฎีที่เกี่ยวของ 2.1 หลักการทํางานของ Master Slave Robot Arm MASTER-SLAVE ROBOT ARM นั้นประกอบไปดวย สวนแรกคือ Master arm คือ แขนที่ใชควบคุมแขนกล ซึ่งที่ Master arm นี้จะติดอยูกับแขนผูควบคุม โดยแตละจุดหมุนจะมีการติดตั้ง sensor เพื่อตรวจวัดการเคลื่อนที่ของแขนผูควบคุมโดยในโครงการนี้ซึ่งหนาที่ ในการเปลี่ยนเแรงดันไฟฟาคือ potentiometer ซึ่งแรงดันไฟฟานี้เปน input สงไปควบคุม slave arm robot สวนที่สองคือ Slave Robot arm คือแขนกลที่ถูกควบคุมโดย master arm ซึ่ง slave robot armนี้จะ ใชมอเตอรกระแสตรงในการขับเคลื่อนแตละจุดหมุนโดยที่รับ input จาก master arm ซึ่ง slave robot arm จะ มีการตรวจสอบโดยการติดตั้ง output transducer หรือ potentiometer ทําหนาที่ในการวัดมุมของ slave robot arm วามีการเคลื่อนที่ตรงตาม master arm หรือไม ซึ่งในการควบคุมแขนกลนั้นจะใช การควบคุมแบบปอนกลับ ( feedback control )ระบบการควบคุม แบบการควบคุมแบบปอนกลับระบบควบคุมจะทําหนาที่เปนตัวควบคุมการทํางานของทั้งระบบซึ่งการทํางานจะ ทําการเปรียบเทียบสัญญาณที่วัดมาไดจากเซนเซอรหรือทรานดิวเซอร(control variable)แลวนํามาเปรียบเทียบ กับคาที่ตองการอางอิง( reference variable )แลวสรางสัญญาณที่เหมาะสมออกมาทางดานเอาทพุท(correcting variable )ซึ่งสามารถจะลดความผิดพลาดของสัญญาณใหนอยที่สุดได

รูปที่ 2.1 block diagram แสดงการควบคุมแบบปอนกลับ( feedback control)

4

2.2 การเลือกการควบคุมที่จะนํามาใชซึ่งมีแบบตางๆดังนี้ 2.2.1 ระบบควบคุมแบบสัดสวน (Proportional Control) ในระบบควบคุมแบบสัดสวน เราจะไดวาเอาทพุทของคอนโทรลจะเปนสัดสวนกับอินพุทของคอนโทรล และถาเรากําหนดสัญญาณอินพุทที่ใหกับคอนโทรลเปน คาความผิดพลาด( ) ซึ่งเปนฟงกชันของเวลาเราจะได output = Kfe เมื่อ Kf เปนคาคงที่เรียก proportional gain เราพบวาเอาทพุทที่ออกจากคอนโทรล Proportional control จะขึ้นกับขนาดของความผิดพลาดในขณะที่เรากําลังพิจารณาทําใหฟงกชันถายโอนของคอนโทรล G(S) จะมีคาเปน

G(S) = Kf

( 2.1 )

ดังนั้นการควบคุมดวยคอนโทรลแบบนี้ก็จะเปนเพียงการขยายสัญญาณความผิดพลาดเทานั้นการที่เรา ไดสัญญาณความผิดพลาดขนาดใหญที่เวลาหนึ่งทําใหเกิดเอาทพุทที่มีขนาดใหญจากคอนโทรลในเวลานั้อยางไร ก็ตามการที่เรากําหนดให gain คงที่นั้นในทางปฏิบัติเราอาจจะกําหนดไวในบางชวงของสัญญาณความผิดพลาด เทานั้น เราอาจกําหนดใหคอนโทรลของเรามีคาเอาทพุทไมนอยกวาคาคาหนึ่งและไมมากเกินกวาคาคาหนึ่งก็ได การกําหนดเอาทพุทแบบ proportional control ชวงที่มีการกําหนดสัดสวนนี้ เราจะเรียกวา proportional band.

รูปที่ 2.2 การกําหนดชวงจํากัดของเอาทพุท

การกําหนด proportional band นี้ จะชวยใหสัญญาณเอาทพุทมีคาจํากัดไมไปสูคาอนันต ทั้งทางดานบวกและ ทางดานลบ และเมื่อคอนโทรลมีเอาทพุทสูงที่สุดที่เปนไปไดคาหนึ่งแลว เราก็นิยมที่จะกําหนดเอาทพุทคาใด ๆ เปนรอยละของคาสูงสุดที่เปนไปได ดังนั้นการเปลี่ยนแปลงคาเอาทพุทของคอนโทรล100%ก็หมายถึงเอาทพุท จะเปลี่ยนจากคาต่ําสุดที่เปนไปได ไปเปนคาสูงสุดที่เปนไปได ซึ่งจะทําใหเราไดวา ( 2.2 )

5

เนื่องจากเอาทพุทของคอนโทรลจะเปนสัดสวนกับอินพุทดังนั้น ถาหากอินพุทมีลักษณะเปน stepเอาทพุทที่ไดก็ จะมีลักษณะเปนstep เชนกันโดยลักษณะของกราฟแสดงอินพุทและเอาทพุทจะมีสัดสวนที่แนนอนคาหนึ่ง ตาม รูปที่ 2.3 โดยรูปนี้แสดงถึงการตอบสนองของคอนโทรล เมื่ออินพุทอยูในชวง proportional band

รูปที่ 2.3 การตอบสนองของคอนโทรลแบบ proportional control ในทางปฏิบัติ proportional control นี้ จะมีลักษณะเหมือนกับเครื่องขยายสัญญาณรูปแบบหนึ่ง ซึ่งอาจจะเปนใน ลักษณะของอุปกรณไฟฟา หรืออาจจะเปนเครื่องขยายสัญญาณเชิงกล เชน คาน ก็ไดลักษณะของระบบที่ควบคุม แบบ proportional control จะมีลักษณะดังที่แสดงในรูปที่ 2.4 และจะทําใหไดฟงกชันถายโอนระบบเปดเปน ( 2.3 ) เมื่อ

เปนฟงกชันถายโอนของระบบ

รูปที่ 2.4 ระบบควบคุมแบบ proportional control ขอเสีย ระบบการควบคุมแบบสัดสวน( Proportional Control)ทําใหระบบการความคลาดเคลื่อนเสมอ ภายหลังการเปลี่ยนแปลงเปาหมายของการควบคุม

6

2.2.2 ระบบควบคุมแบบปริพันธ (Integral Control) ในระบบควบคุมแบบปริพันธเอาทพุทของคอนโทรลจะเปนสัดสวนกับปริพันธของสัญญาณผิดพลาเทียบ กับเวลา หรือ ( 2.4 ) เมื่อ Ki เปนคาคงที่เรียกวา integral gain ซึ่งจะมีหนวยเปน 1/sec รูปที่ 2.5 แสดงลักษณะการตอบสนองของ integral control เมื่อไดรับสัญญาณความผิดพลาดแบบ step คาปริพันธระหวางเวลา t และ 0 จะหมายถึงพื้นที่ ใตกราฟของสัญญาณความผิดพลาดจากเวลา 0 ถึง t ดังนั้นเนื่องจากเริ่มการมีสัญญาณความผิดพลาดแบบ step เอาทพุทที่ออกจากคอนโทรลจะมีคามากขึ้นเรื่อย ๆ ดวยอัตราที่คงที่ ทําใหเอาทพุทที่เวลาใดๆจะเปนสัดสวนกับ ความผิดพลาดที่เกิดขึ้น

รูปที่ 2.5 ลักษณะการตอบสนองของคอนโทรลแบบ integral control

เปลี่ยนรูปลาปลาสของสมการ 2.4 จะทําใหเราไดฟงกชันถายโอนของคอนโทรลเปน ( 2.5 )

ดังนั้นสําหรับระบบที่แสดงในรูปที่ 2.6 การควบคุมแบบ integral control จะให forward-path transfer function เปน

และทําใหมี ฟงกชันถายโอนระบบเปดเปน

7

รูปที่ 2.6 ระบบควบคุมแบบ integral control เราสามารถพิจารณาถึงขอไดเปรียบของการควบคุมแบบ integral controlไดจากสมการ 2.5 ซึ่งเราจะ เห็นวาระบบควบคุมแบบ integral control จะเพิ่มจํานวนโพลใหกับระบบควบคุมและเพิ่ม type ของระบบ จาก type 0 เปน type 1 ซึ่งทําใหระบบมีความผิดพลาดที่สภาวะคงตัวเปนศูนยเทียบตอstep inputอยางไรก็ตามการ เพิ่มโพลที่ S = 0 และไมมีการเพิ่มซีโรใหกับระบบควบคุม จะทําใหความแตกตางระหวางจํานวนโพล (n) และ จํานวนศูนย ( m ) เพิ่มขึ้นอีก 1 ซึ่งจะมีผลให asymptote angles ของทางเดินรากลดลง และจุดตัดจะเคลื่อนไป ทางครึ่งขวาของ s-plane มากขึ้น มีผลทําใหความเสถียรสัมพัทธของระบบลดลง 2.2.2 การควบคุมแบบอนุพันธ (Derivative Control) การควบคุมอีกแบบหนึ่งก็คือการควบคุมแบบอนุพันธ (Derivative Controller) การควบคุมแบบนี้เอาทพุท จะเปนสัดสวนกับอัตราการเปลี่ยนแปลงความผิดพลาดเทียบตอเวลา นั่นคือ ( 2.6) เมื่อ

คือ derivative gain และมีหนวยเปนวินาที

รูปที่ 2.7 การตอบสนองของ Derivative Control รูปที่ 2.7 แสดงสิ่งที่เกิดขึ้นเมื่อสัญญาณความผิดพลาดเปนสัญญาณแบบ rampเมื่อเริ่มไดรับสัญญาณ ความผิดพลาดและไมใชคาของความผิดพลาดซึ่งทําใหเราไดสัญญาณสงออกจากคอนโทรลมีคามากกอนที่เกิด ความผิดพลาดขึ้นมากจริงๆอยางไรก็ตามหากความผิดพลาดมีคาคงที่ก็จะไมมีการสะสมคาความผิดพลาดแมวา

8

คาความผิดพลาดจะมีมากก็ตาม ทําใหการควบคุมแบบอนุพันธนี้ไมออนไหวตอคาความผิดพลาดที่คงที่หรือ เปลี่ยนแปลงอยางชาๆซึ่งผลที่ตามมาการควบคุมแบบนี้จะไมใชเพียงตัวเดียว แตมักจะใชควบคุมรวมกับการ ควบคุมแบบอื่น เปลี่ยนรูปลาปลาสสมการ 2.6 เพื่อที่จะหาฟงกชันถายโอนของคอนโทรล ซึ่งจะเปน ดังนั้นสําหรับระบบควบคุม ดังที่แสดงในรูปที่ 2.8 การที่มีการควบคุมแบบอนุพันธจะทําใหเราได ฟงกชันถายโอนเปน ( 2.7 )

รูปที่ 2.8 แผนภาพบล็อกที่ประกอบดวย Derivative Control ถาหากวาระบบเปนแบบ type 1 หรือสูงกวา การควบคุมแบบอนุพันธจะลด Sในเทอมสวนลงและลด type ของ ระบบลง 1 อยางไรก็ตามเราไดกลาวกอนหนานี้แลววา การควบคุมแบบอนุพันธนี้มักจะไมใชเพียงลําพังแตเรา จะใชรวมกับการควบคุมแบบอื่น เพราะเมื่อเราใชการควบคุมแบบอนุพันธจะทําใหเราเพิ่มความเร็วตอบสนอง 2.2.4 การควบคุมแบบสัดสวนรวมกับปริพันธ (Proportional plus integral Control) การที่ระบบควบคุมมีความเสถียรสัมพัทธลดลง เมื่อเราใชการควบคุมแบบปริพันธสามารถที่จะแกไข ไดในระดับหนึ่งโดยการใชการควบคุมแบบสัดสวนรวมกับแบบปริพันธ ( Proportional plus Integral, PI ) ซึ่ง ลักษณะของระบบควบคุมจะเปนตามรูปที่ 2.9

รูปที่ 2.9 แผนภาพบล็อกที่ประกอบดวย Proportional plus Integral

9

สําหรับระบบดังกลาวจะมีเอาทพุทของคอนโทรลเปน

( 2.8 )

รูปที่ 2.10 การตอบสนองของ PI controller รูปที่ 2.10 แสดงเอาทพุทของคอนโทรลที่ไดรับเมื่อมีอินพุทเปนสัญญาณความผิดพลาดแบบ stepถาเราเปลี่ยน รูปลาปลาสของสมการ เราจะไดฟงกชันถายโอนของคอนโทรลแบบ PI เปน

( 2.9 ) เราให integral time constant เปน

( 2.10 ) ดังนั้นเราจะได

( 2.11 )

10

และจะทําใหเราไดฟงกชันถายโอนระบบเปดเปน

( 2.12 )

และโพลที่ S=0 เพิ่มใหกับฟงกชันถายโอนของระบบเมื่อเราใชการควบคุม เราจะเห็นวาเรามีศูนยที่ แบบ PI การที่เราเพิ่มตัวประกอบ S เขากับเทอมสวนของฟงกชันถายโอนก็เสมือนกับเราเพิ่มแบบของระบบขึ้น ไป 1 จึงทําใหระบบนี้จะไมมีความผิดพลาดที่สภาพคงตัวสําหรับอินพุทแบบขั้นบันได นอกจากนั้นการที่เราเพิ่ม ศูนยใหกับระบบไปพรอมๆ กัน ก็จะทําใหความแตกตางระหวางจํานวนโพล n และจํานวนศูนย m มีคาคงที่ดัง นั้นมุมของ asymptote สําหรับทางเดินของรากมีคาคงเดิม อยางไรก็ตามจุดตัดของเสน asymptotes บนแกนจริง จะเคลื่อนที่เขาหาจุดกําเนิดมากขึ้น ยังผลใหความเสถียร ของระบบลดลงบาง

การเพิ่มโพลที่ S=0 และศูนย ที่ จะทําใหจุดตองเปลี่ยนไปเทากับ ซึ่งจะทําใหมีคาเปน บวกมากขึ้นและจุดตัดจะเคลื่อนที่มาทางขวามือเขาใกลจุดกําเนิดมากขึ้นอยางไรก็ตามการลดลงของความเสถียร สัมพัทธนี้จะนอยกวาการที่เราใชการควบคุมแบบปริพันธเพียงอยางเดียว คาของ

และ

จะเปนคาที่ใชกําหนดตําแหนงของศูนยและโพลของระบบ โดยตําแหนงของศูนยจะกําหนด

ดวยคา

ในขณะที่

จะเปนคาที่ใชกําหนดโพลระบบปด

2.2.3 การควบคุมแบบสัดสวนรวมกับอนุพันธ (Proportional plus Derivative Control) ถาการควบคุมแบบอนุพันธใชรวมกับการควบคุมแบบสัดสวน (PD) ดังที่แสดงในรูปที่ 2.11 เราจะไดฟงกชัน ถายโอนระบบเปดเปน

( 2.13 )

11

เมื่อ คือ derivative time constant ซึ่งในการควบคุมแบบนี้ จะมีศูนยเพิ่มขึ้นที่ เห็นวาไมมีการเปลี่ยนแปลงของระบบ ทําใหไมมีการเปลี่ยนแปลงคาความผิดพลาดที่สภาวะคงตัว

และจะ

รูปที่ 2.11 แผนภาพบล็อกที่ประกอบดวย PD Control 2.2.5 การควบคุมแบบ PID CONTROL

รูปที่ 2.12 Block diagram ที่ประกอบดวย PID control การควบคุมโดยใชแบบสัดสวนรวมกับแบบปริพันธและรวมกับแบบอนุพันธ (PID control) หรือ ที่เรียก การ ควบคุมแบบ 3 เทอม (tree-term control) ระบบจะมีลักษณะตามรูปที่ 2.12 จะทําใหเอาทพุทของคอนโทรล เมื่อรับอินพุทเปนความผิดพลาด ดังนี้

( 2.14 ) ฟงกชันถายโอนของคอนโทรล จะเปน

( 2.15 )

12

ซึ่งเราสามารถจัดรูปไดเปน

( 2.16 ) ฟงกชันถายโอนระบบเปดของระบบที่แสดงในรูปที่ 2.12 จะเปน

( 2.17 ) ดังนั้นการควบคุมแบบ PID controller จะเพิ่มจํานวนศูนยใหกับระบบเทากับ 2 และเพิ่มจํานวนโพล 1 โพล และทําใหชนิด type ระบบเพิ่ม

2.3 มอเตอรกระแสตรง DC MOTOR 2.3.1 หลักการทํางานของมอเตอรกระแสตรง เมื่อมีการผานกระแสไฟฟาเขาไปยังขดลวดในสนามแมเหล็กจะทําใหเกิดแรงแมเหล็กซึ่งมีสัดสวนของ แรงขึ้นกับกระแสแรงของสนามแมเหล็ก โดยแรงจะเกิดขึ้นเปนมุมฉากกับกระแสและสนามแมเหล็ก ขณะที่ ทิศทางของแรงกลับตรงกันขามกัน ถาหากกระแสของสนามแมเหล็กไหลยอนกลับจะทําใหเกิดการเปลี่ยนแปลง ของกระแส และ สนามแม เ หล็ ก เป น ผลทํ า ให ทิ ศ ทางของแรงเปลี่ ย นไป ด ว ยคุ ณ สมบั ติ นี้ ทํ า ให ม อเตอร กระแสตรงกลับทิศทางการหมุนได สนามแมเ หล็กของมอเตอร สว นหนึ่ง เกิด ขึ้นจากแม เ หล็ กถาวรซึ่ ง จะถูกยึ ด ติด กับแผน เหล็ก หรื อ เหล็กกลา โดยปกติสวนนี้จะเปนสวนที่ยึดอยูกับที่ และ ขดลวดเหนี่ยวนําจะพันอยูกับสวนที่เปนแกนหมุนของ มอเตอร

รูปที่ 2.13 แสดงโครงสรางทั่วไปของมอเตอรกระแสตรง

13

2.3.2 คุณสมบัติของมอเตอรกระแสตรง ในการอธิบายคุณสมบัติของมอเตอรกระแสตรงใหละเอียดนั้นตองพิจารณาแรงดันที่ปอนและความ ตานทานของโรเตอรดวย วงจรภายในของมอเตอรเขียนไดดังรูปที่ 3

รูปที่ 2.14 แสดงวงจรภายในของมอเตอรกระแสตรง โดยสมมติใ หทุน โรเตอรไมมีความตานทานอยูเ ลย อนุกรมกับความตานทานซึ่ง ในที่นี้ก็คือ ความ ตานทานของขดลวดนั่นเอง แรงดันที่ขั้วตอสายของมอเตอรก็คือผลบวกระหวางแรงดันที่ทุนโรเตอร (VA) และ แรงดันตกครอมความตานทานขดลวด (VR) แรงดัน VA ถูกเรียกวา แรงเคลื่อนเหนี่ยวนําปอนกลับ (BACK EMF) ซึ่งเกิดขึ้นในโรเตอรขณะที่หมุน แรงดันที่เกิดขึ้นนี้เปนไปตากกฎของการเหนี่ยวนําแมเหล็กไฟฟาจากการเคลื่อนที่ของตัวนําในสนามแมเหล็ก สัมพันธกับแรงเคลื่อนเหนี่ยวนําแมเหล็ก และ ความเร็วในการเคลื่อนที่ของตัวนํา แรงดันที่เกิดขึ้นจะมีขั้ว ตรงกันขามกับแรงดันที่ปอนใหกับมอเตอร และ แปรผันตรงกับความเร็วในการหมุน ผลบวกของแรงดันที่ทุนโร เตอร (VA) และแรงดันตกครอมขดลวด (VR) ตองเทากับแรงดันที่ปอนใหกับมอเตอร (V)ดังแสดงในสมการ 2. 18 V = VA + V R

( 2.18 )

เมื่อพิจารณาตั้งแตมอเตอรหยุดนิ่ง ความเร็วมีคาเปนศูนย ดังนั้น VA = 0, VR = V กระแสที่ไหลใน มอเตอรหาไดจาก I=

VR R

A ( 2.19 )

เมื่อมอเตอรเริ่มหมุนจะมีความเร็ว และ VA เพิ่มขึ้นเปนเสนตรงตามความเร็ว VR ซึ่งมีคาเทากับความ แตกตางระหวาง VA และ V จะเริ่มลดลง กระแส I ก็จะเริ่มลดลงเชนกันขณะที่มอเตอรยังมีความเรงอยู ความเร็วจะเพิ่มขึ้น แรงบิดจะลดลงจนกวาจะถึงจุดซึ่งแรงบิดของมอเตอรรับภาระโหลดไดสมดุลพอดี ขณะที่ มอเตอรไมมีโหลดและหมุนอยางอิสระจะมีเพียงคาความฝดของแบริ่งและแรงตานอากาศทําให VA เกือบเทากับ คา V

14

2.4 วงจรขับมอเตอร 2.4.1 การใชทรานซิสเตอรควบคุมการทํางานของมอเตอร ในโครงการนี้เราจะใชวงจร H-bridge ซึ่งเปนวงจรที่สามารถขับมอเตอรและยังสามารถควบคุมทิศ ทางการหมุนของมอเตอรไดอีกดวย ซึ่งวงจร H-bridge นี้จะอาศัยการ switching จาก Transistor โดย Transistor เปนสารกึ่งตัวนํา (Semiconductor device) ที่เราสามรถนําคุณสมบัติของการ Cutoff และการ Saturation มาประยุกตใชงานเปนสวิตซได และที่สําคัญ Transistor เปนสวิตซอิเล็กทรอนิกสที่เราสามารถ ควบคุมการเปด-ปดไดดังรูปที่ 2.15

รูปที่ 2.15 แสดงการควบคุมการปด-เปดของ Transistor จากรูปที่2.15 เปนวงจรอยางงาย โดยการนําทรานซิสเตอรมาควบคุมการทํางานของมอเตอร เมื่อเรา ปอนกระแส Ib ดวยปริมาณที่มากพอที่จะทําใหทรานซิสเตอรอยูในสภาวะอิ่มตัว (Saturation mode) ซึ่งใน สภาวะอิ่มตัวนี้ทรานซิสเตอรจะทํางานเหมือนกับสวิตซปดวงจร คาความตานทานระหวางขา C และขา E จะมีคา เขาใกลศูนย ทําใหมีกระแส Ic ไหลผานมอเตอร ทําใหมอเตอรสามารถทํางานได และในทางตรงกันขามถาเรา หยุดจายกระแส Ib ทางดานขา B ทรานซิสเตอรจะอยูในสภาวะคัตออฟ (Cutoff mode) ซึ่งในสภาวะคัตออฟนี้ ทรานซิสเตอรจะทํางานเหมือนกับสวิตซเปดวงจร คาความตานทานระหวางขา C และ E จะมีคาเปนอนันต ทํา ใหไมมีกระแสไหลผานมอเตอร 2.4.2 การควบคุมทิศทางการหมุนของมอเตอร วงจร H-bridge จะมีทรานซิสเตอร 4 ตัว เพื่อใชในการควบคุมทิศทางการหมุนของมอเตอร โดย ควบคุมใหหมุนในทิศทาง Forward เมื่อ Q1 และ Q3 ทํางาน (On) และหมุนในทิศทาง Backward เมื่อ Q2 และ Q4 ทํางาน (On) ซึ่งแสดงวงจร H-bridge ไวดังรูปที่2.16

15

รูปที่ 2.16 แสดงวงจร H-bridge

2.5 ความรูพื้นฐานเกี่ยวกับ Potentiometer Potentiometer หรือตัวตานทานปรับคาไดจะนํามาใชเปนตัวเซนเซอรในการควบคุมแขนกล

รูปที่ 2.17 ลักษณะของตัวตานทานแบบปรับคาได ซึ่งในโครงการนี้จะนําเอา potentiometer มาติดตั้งในSlave robot arm เพื่อนําคาความตางศักยระหวาง A-B และ C-B มาเปรียบเทียบกับคาความตางศักยที่ master slave robot arm ที่ไดมาpotentiometer เชนกันซึ่ง คาที่ไดจากการเปรียบเทียบนี่จะเปนตัวกําหนดทิศทางของ DC MOTOR

16

2.6 การควบคุมแบบดิจิตอล (digital control ) ในการควบคุมระบบหรือ Plant ที่ศึกษานั้นสวนใหญเปนระบบที่มีความตอเนื่องของสัญญาณ หรือ continuous signal โดยที่ตัวควบคุมหรือ controllerที่ใชควบคุมPlantเปนระบบที่มีความตอเนื่องของสัญญาณ เชนเดียวกับ Plant ซึ่งการควบคุมระบบโดย controller แบบนี้เปนวิธีการแบบ classical control โดย block diagram สวนใหญจะมีลักษณะดังนี้

รูปที่ 2.18 แสดง block diagram แบบ continuous control โดยที่ D(s) คือ continuous controller ที่ถูกสรางขึ้นจาก analog electronic จาก block diagram เรา สามารถเปลี่ยนตัวcontrollerที่อยูในรูปของ analog เปนตัวควบคุมแบบ digital หรือdigital controller ซึ่งหนาที่ ของ digital controller จะมีลักษณะคลายกับ analog controller ที่จุดประสงคในการปรับปรุงระบบที่มีอยูใหดี ขึ้น แตพื้นฐานการทํางานของ digital controller จะทํางานกับสัญญาณที่ไมตอเนื่องทางเวลาหรือ discrete time signal โดยเมื่อใสตัวควบคุมแบบ digital จะทําให block diagram มีลักษณะดังนี้

รูปที่ 2.19 แสดง block diagram แบบ digital control

17

โดยที่สัญญาณที่สังเกตไดในในแตละจุดมีลักษณะดังนี้

รูปที่ 2.20 แสดงสัญญาณที่สังเกตได จาก block diagram แบบ digital control จาก Block diagram ของ digital controller จะเห็นไดวา ในระบบที่ใช digital controlจะมีทั้งสัญญาณ ที่เปน continuous time และ discrete timeรวมกันอยูจึงจําเปนตองมีมีตัวแปลงสัญญาณระหวางสัญญาณทั้งสอง แบบ โดยตัวแปลงสัญญาณที่แปลงสัญญาณ continuous time เปน discrete time สามารถใช A2D conversion เปนตน และสัญญาณ discrete time แปลงเปน continuous timeสามารถใช Zero order hold หรือ PWMเปนตัว แปลงสัญญาณ ผลตอบสนองทรานเชียนต และ เสถียรภาพของระบบDigital control (Transient response and stability) ใน ระบบที่ตอ เนื่องทางเวลา(Continuous time system) เราจะสามารถวิเคราะหวาระบบที่ศึกษาอยูจะ เสถียรถาโพลของระบบอยูทางฝงซายมือของS-plane เราจะสามารถบอกไดวาระบบนั้นเสถียร ( stable ) แตถา ระบบที่ตอเนื่องทางเวลา ( Continuous time system ) ถูกเปลี่ยนไปเปนระบบที่ไมตอเนื่องทางเวลา ( discrete time system)เราจะทําการวิเคราะหโพลใน z-plane โดยเราจะสามารถบอกไดวา ระบบนี้ ถามีโพลอยูในภายใน unit cycle ระบบนั้นจะเสถียร โดยโพลของระบบที่ตอเนื่องทางเวลา ( Continuous time system )จะถูกเปลี่ยน จากรูปใน S เปน Z อาจใชวิธี matched pole-zero mapping method โดยที่

Z = est t = Sampling time (sec/sample) s = Location in the s-plane z = Location in the z-plane

( 2.20 )

18

โดยรูปขางลางจะแสดงใหเห็นการ Mapping จาก s-plane เปน z-plane ซึ่ง คาคงที่ของ damping ratio และ natural frequency ก็จะ map เปนดังรูป

รูปที่ 2.21 แสดง Natural frequency and damping loci in z-plane สําหรับการวิเคราะหทรานเชียนของโพลที่อยูใน z-plane นั้นยังคงสามารถใชสมการของ s-plane วิเคราะหไดอยู ดังนั้นสมการที่ใชในการวิเคราะหทรานเชียนคือ

โดยที่

zeta = Damping ratio Wn = Natural frequency (rad/sec) ; Natural freq. ของ z-plane มีหนวยเปน rad/sample หากจะใชสมการเดียวกันกับ s-plane ตองเปลี่ยนใหเปน rad/sec Ts = Settling time, Tr = Rise time, Mp = Maximum overshoot

19

บทที่ 3 การดําเนินงาน จากการศึกษาโครงการการควบคุมแขนกลดวยแขนมนุษยระยะที่ 1 สามารถควบคุมแขนกลไดโดยการ ใชสันญาณ Analog ในการควบคุมแขนกลสามารถหยิบจับวัตถุขนาดเล็กและมีน้ําหนักเบาได ซึ่งในโครงการนี้จะ ทําการออกแบบแขนกลใหสามารถที่จะวัตถุใหไดมีน้ําหนักและขนาดมากขึ้น

3.1 การออกแบบ และสวนประกอบของ MASTER ARM และ SLAVE ARM โดยในการออกแบบสวนของ Master Arm และ Slave Arm ไดทําการออกแบบ Master Arm ซึ่งนํา แบบการเคลื่อนไหวของมนุษยมาทําการออกแบบโดยที่จะแบงเปนสวนตางๆดังนี้ 3.1.1 หัวไหลที่ 1 การเคลื่อนที่ของแขนมนุษยบริเวณหัวไหลโดยแบงการเคลื่อนไหวเปนหัวไหลที่ 1 ซึ่งสามารถเคลื่อนที่ ไดประมาณ1800 และการออกแบบไดอาศัยระยะจากหัวไหลของมนุษยเปนแบบอางอิงในการออกแบบหัวไหลที่ 1

a)

b) รูปที่ 3.1 แบบของ Master Arm บริเวณหัวไหลที่ 1, a) ภาพแขนมนุษยในแนวหัวไหลที่ 1 , b) ภาพ 3มิติของ หัวไหลที่ 1

20

3.1.2 หัวไหลที่ 2 ในการออกแบบหัวไหลที่ 2 นั้นทําการออกแบบใหสามารถเคลื่อนที่ไดประมาณ 1800 เชนกันแต ทิศทางในการเคลื่อนที่จะอยูในทิศทางดังที่แสดงในรูปที่ 3.2 a)

a)

b)

c) รูปที่ 3.2 Master Arm บริเวณหัวไหลที่ 2 , a) การเคลื่อนที่แขนมนุษยในแนวหัวไหลที่ 2 , b) ภาพ 3 มิติ ของ Master arm , c) แสดงขนาดของหัวไหลที่ 2

21

3.1.3 ขอศอก บริเวณขอศอกนั้นไดออกแบบใหสามารถเคลื่อนที่ได 1800 ตามการเคลื่อนไหวของขอศอกมนุษย

a)

b)

รูปที่ 3.3 แสดง Master Arm บริเวณขอศอก a) การเคลื่อนที่แขนมนุษยในแนวของขอศอก b) แสดงภาพ 3 มิติและขนาดของขอศอก 3.1.4

ขอมือ บริเวณทอนแขนออกแบบใหสามารถหมุนได 900 ตามการเคลื่อนไหวของทอนแขนมนุษย

a)

b)

รูปที่ 3.4 แสดงแบบMaster Arm บริเวณขอมือ , a) การเคลื่อนที่แขนมนุษยในแนวของขอมือ , b) แสดง ภาพ 3 มิติและขนาดของขอมือ

22

3.1.5 บริเวณมือจับ การออกแบบบริเวณที่เปนมือ (gripper) ใหสามารถหมุนไดประมาณ 900 โดยที่การออกแบบไดอาศัย แบบมาจากมือของมนุษยดังรูป 3.5 a)

a)

b)

รูปที่ 3.5 แบบของMaster Arm บริเวณมือ , a) การเคลื่อนที่ของมือมนุษย , b)ภาพ 3 มิติ ของมือ จากแบบที่ไดออกแบบไวในแตละสวนนั้นไดจัดทําMaster Armโดยวัสดุที่ใชเปนอลูมิเนียมหนาและแต ละสวนจะมีตัวตานทานปรับคา (Potentiometer ) ติดไวเปนตัวเซ็นเซอรในทุกจุดหมุน ดังรูป

a)

b) รูปที่ 3.6 Master Armที่จัดทําขึ้น

23

3.2 SLAVE ARM ในสวนของแขนกล ( Slave Arm ) ไดทําการแบงเปนจุดหมุนดังนี้ซึ่งจะมีจุดหมุนเชนเดียวกันกับของ Master Arm ซึ่งจะออกแบบใหไดการเคลื่อนที่ของจุดหมุนตางๆดังตาราง จุดหมุน หัวไหลที่ 1 หัวไหลที่ 2 ขอศอก ทอนแขน มือ

ความสามารถในการเคลื่อนที่( องศา) 1800 1800 900 900 900

ตารางที่ 3.1 การออกแบบความสามารถในการหมุนของ Master arm 3.2.1 หัวไหลที่ 1 ในสวนของหัวไหลที่ 1นั้นออกแบบใหแกนมอเตอรยึดติดอยูกับแกนเพลาโดยยึดอยูในกลองเหล็กดัง รูปที่ 7

รูปที่ 3.7 แบบของ Slave Armบริเวณหัวไหลที่ 1 3.2.2 หัวไหลที่ 1 และขอศอก ออกแบบใหโครงสรางของหัวไหลที่ 2และขอศอกใชโครงสรางลักษณะเดียวกันโดยที่หัวไหลที่ 2 สามารถหมุนได1800และขอศอกสามารถหมุนได1800 ไดเชนกันดังรูป

รูปที่ 3.8 แบบของ Slave Armบริเวณหัวไหลที่ 2และขอศอก

24

3.2.3 ขอมือ ออกแบบใหโครงสรางบริเวณทอนแขนใหสามารถหมุนได 900

รูปที่ 3.9 แบบของSlave Armบริเวณทอนแขน 3.2.4 บริเวณมือจับ ออกแบบบริเวณมือจับใหสามารถหยิบจับวัตถุไดโดยที่ขนาดวัตถุเปนประมาณกระปองกาแฟซึ่งแบบ ของมือจับเปนดังรูป

รูปที่ 3.10 แบบของSlave Armบริเวณมือจับ แบบของ Slave Arm เมื่อทําการประกอบรวมแลว

รูปที่ 3.11 Master arm ที่ออกแบบ

25

โครงสรางของ Slave Arm ที่ไดจัดทําขึ้น วัสดุที่ใชเปนอลูมิเนียมบางและใชแกนเพลาขนาด 6 มม. เปนแกนเชื่อมตอระหวางจุดหมุนตางๆโดย ที่จะมีตัวตานทานปรับคาได( Potentiometer )ขนาด 10 k เปนตัวเซ็นเซอร

a)

c)

b)

d)

รูปที่ 3.12 โครงสรางของแขนกล (Slave Arm ) , a) หัวไหลที่ 1 , b) หัวไหลที่ 2 , c) ขอมือ , d) มือ

26

ตาราง Denavit-Hatenberg เพื่อแสดงการหมุนของจุดหมุนตางๆในสวนของแขนกล( Slave Arm )

L1 L2

L3

END OF EFECTOR

รูปที่ 3.12 จุดหมุนตางๆบนแขนกล

link c d e

variable θ1 α2 θ2

θ θ1 θ3

d L1 L3

a L2 -

ตารางที่ 3.2 ตาราง Denavit-Hatenberg

L1 L2 L3

ขนาดของความยาวของ link(cm) Slave arm 30 16.9 25

ตารางที่ 3.3 แสดงความยาวของแขนกล( Slave Arm )

α α2 (+)90º

27

3.3 การออกแบบระบบควบคุม 3.3.1การออกแบบวงจร H-bridge

รูปที่ 3.12 shacematic วงจร H-bridge ใชการswitch transistor เพื่อใหกระแสไหลผานตัวมอเตอรโดยใชสัญญาณจาก Microcontroller เปน ตัวควบคุมการ on-off switchเพื่อใชควบคุมทิศทางการหมุนของมอเตอร และใชสัญญาณ PWM ที่สรางจาก Microcontroller ตอเขากับ PORT Enable ของวงจรเพื่อใหสัญญาณ PWM ทําการควบคุมความเร็วของ มอเตอร ซึ่งสัญญาณที่ตอเขาวงจรH-bridge จะถูกแยกโดยใช opto coupler แยกสัญญาณจาก 5 volt ออกจาก 12 volt และสัญญาณที่ใชควบคุมจะมีลักษณะการทํางานตาม truth table ดังนี้ Enable 0 0 0 0 1

FW 0 1 0 1 x

REV 0 0 1 1 x

condition Stop Forward Reverse Break Stop

ตารางที่ 3.4 แสดงการทํางานของสัญญาณควบคุมของวงจร

28

3.3.2 การออกแบบวงจรควบคุมแบบดิจิตอล

รูปที่ 3.13 Pin out ATMEGA16 (ซาย), ATMEGA8 (ขวา) 3

4

5

6

10

Vcc C14

1 2 3

FWD2 REV2 EN2

MOTOR2 JP3 1 2 3

FWD4 FWD2 REV2 EN2 EN3 REV4 EN4

FWD3 REV3 EN3

MOTOR3 C3

JP4 1 2 3

FWD4 REV4 EN4

22pF

MOTOR4 C

C4

JP5 1 2 3

FWD5 REV5 EN5

RESET1

14 15 16 17 18 19 20 21 9 13

Y1 CRYSTAL 12

PD0 (RXD) PD1 (TXD) PD2 (INT0) PD3 (INT1) PD4 (OC1B) PD5 (OC1A) PD6 (ICP) PD7 (TOSC2)

(ADC0) PA0 (ADC1) PA1 (ADC2) PA2 (ADC3) PA3 (ADC4) PA4 (ADC5) PA5 (ADC6) PA6 (ADC7) PA7

RESET

X2

10 9 8 7 6 5 4 3 2 1

C8 1uF

PC

7 EN5 REV5 FWD5 SCK1 XTAL1 XTAL2

C10 1uF JP9

B

1 2 3

JP14

XTAL1

PD0 (RXD) PD1 (TXD) PD2 (INT0) PD3 (INT1) PD4 (XCK/T0) PD5 (T1) PD6 (AIN0) PD7 (AIN1)

2 1

+ C2 1uF

D

2 4 6 8 10

1 3 5 7 9

RESET1 SCK MISO REV3

C

Vcc AVCC AREF GND

(ADC0) PC0 (ADC1) PC1 (ADC2) PC2 (ADC3) PC3 (ADC4/SDA) PC4 (ADC5/SCL) PC5 (RESET) PC6

20 21 22

23 24 25 26 27 28 1

+

Vcc

C9 1uF

JP16 RESET2 SCK1 FWD5 REV5

2 4 6 8 10

1 3 5 7 9

RESET2 SCK1 FWD5 REV5

PROGRAMER

HEADER 15X2

JP13

B

Vcc

1 2 3 4 5 RESET2

2 4 6 8 10 12 14 16 18 20 22 24 26 28 30

R19 1k

A/D RESET2 +

Vcc

C11 1uF

S3 RESET

8

C5

PB0 (ICP) PB1 (OC1A) PB2 (SS/OC1B) PB3 (MOSI/OC2) PB4 (MISO) PB5 (SCK) PB6 (XTAL1/TOSC1) PB7 (XTAL2/TOSC2)

ATMEGA8_DIP28

GND

PD

PORTD

22pF

2 3 4 5 6 11 12 13

1 2 3 4 5

JP15

VCC

U3

RESET1 +

Vcc 3

JP11 1 3 5 7 9 11 13 15 17 19 21 23 25 27 29

Vcc

14 15 16 17 18 19 9 10

Vout

PROGRAMER Vcc

R18 1k

Vin

+ C1 CAP POL

JP8 RESET1 SCK MISO REV3

Vcc

MOTOR5 Vcc

U1 VOLTREG

POWER SUPPLY 5 VOLT CIRCUIT

JP12 POWER

ATMEGA16_DIP40

22pF

S2 RESET

Rq 4.7k

D1 ON

JP10

Vcc

Vcc

1 SW

POWER

A/D

32 31 30

AREF AGND AVCC

X1

1 2 3 4 5 6 7 8

22 23 24 25 26 27 28 29

PC0 PC1 PC2 PC3 PC4 PC5 (TOSC1) PC6 (TOSC2) PC7

S1 1 2

JP7

40 39 38 37 36 35 34 33

1 2

MOTOR1 JP2

JP6

CAP U2

PB0 (T0) PB1 (T1) PB2 (AIN0) PB3 (AIN1) PB4 (SS) PB5 (MOSI) PB6 (MISO) PB7 (SCK)

VCC

FWD1 REV1 EN1 FWD3 REV3 MISO SCK

1 2 3 4 5 6 7 8

GND

FWD1 REV1 EN1

11

1 2 3

+

JP1 D

GND

2

2

1

POWER

Y2 CRYSTAL

C6 XTAL2 22pF A

A Title DIGITAL CONTRLLOLER Size

Number

Revision

B Date: File: 1

2

3

4

29-Jan-2007 Sheet of By: C:\Documents and Settings\Administrator\MyDrawn Documents\Robot\shacematic project ee 2006

5

6

รูปที่ 3.14 shacematic diagram digital control จาก Pin out ของ ATMEGA16 ATMEGA 8 Microcontroller ทั้งสองตัวมี A2D และมี TIMER ที่สามารถสราง PWM โดย ATMEGA16 มี A2D จํานวน 8 ports มีชองกําเนิดสัญญาณPWM จํานวน 4 ports ทําการควบคุมมอเตอร4ตัว และ ATMEGA 8 มี A2D จํานวน 6 ports มีชองกําเนิดสัญญาณPWM จํานวน 3 portsใชทําการควบคุมมอเตอร 1ตัว

29

โดยมีตารางการใช ports ควบคุมมอเตอรดังนี้

มอเตอร 1 2 2 4

FWD PB.1 PD.2 PB.4 PD.1

REV PB.2 PD.3 PB.5 PD.6

EN

A2D

PB.3 PD.4 PD.5 PD.7

master PA.0 PA.2 PA.4 PA.6

salve PA.1 PA.3 PA.5 PA.7

ตารางที่ 3.5 แสดง ports ที่ใชควบคุมมอเตอร ที่ใช ATMEGA 16 เปนตัวควบคุม มอเตอร 5

FWD PB.4

REV PB.3

EN

A2D

PB.2

master salve PA.0 PA.1

ตารางที่ 3.6 แสดง ports ที่ใชควบคุมมอเตอร ที่ใช ATMEGA 8 เปนตัวควบคุม ลักษณะการทํางานของโปรแกรม ใชการ Interrupt ของ Timer 0 overflow เปนตัวกําหนดการ sampling time ของสัญญาณที่เขาตัว Microcontroller และเลือกใช A2Dขนาด 10bit เพื่อใหสัญญาณที่เขา Microcontroller มีความแมนยํามากที่สุด เมื่อแรงดันที่ออกมาจาก potentiometer ที่ติดอยูที่ตัวmaster จะมาเขาที่PORTAของ MEGA16 เพื่อแปลง แรงดันใหเปนสัญญาณดิจิตอลแลวนําสัญญาณที่ไดมาทําการเปรียบเทียบกับ สัญญาณที่มาจาก potentiometer ที่ติดที่ตัวslave แลวนําคาที่แตกตางกันมาทําการคํานวณโดยใช PI controller เปนตัวคํานวณคา duty cycle แลว นําคาที่ไดไปสราง PWM โดยมี สมการการคํานวณคา duty cycleดังนี้ U [k ] = K p E[k ] +

Ki {E[k ] + E[k − 1]} + U [k − 1] 2 fs

3.1

โดยที่ U[k] = คาduty cycle E[K]=คาerror (คาจากmaster – คาจากslave) fs=sampling frequency (ใชการinterrupt overflow ของ Timer 0เปนตัวกําหนดsampling frequency โดยใช T0 โหมด Phase correct PWM และเลือกใช Prescaler frequency โดยและความถี่ fs = 125 kHz ) การทํางานของโปรแกรมจะทํางานทุกๆ ความถี่ fs

30

Flowchart การทํางานของโปรแกรม Interrupt subroutine

Yes

MAIN PROGRAM

No No

Yes

รูปที่ 3.15 flowchart การทํางานของโปรแกรม

31

3.3.3 การออกแบบ Digital PI controller

E(s)

U(s)

Gc(s)

รูปที่ 3.16 block diagram continous time control พิจารณา ไดวา Gc( s ) =

U (s) E ( s)

Ki s 2 ( z - 1) หา Discrete time equivalent โดยใช Tustin Formula ให s = T z + 1 Ki × T × ( z + 1) ทําให Gc( z ) = Kp + เมื่อพิจารณาใน z domain ไดวา 2( z - 1)

เมือให

Gc(s ) เปน PI controller ไดวา

E(z)

Gc( s ) = Kp +

U(s)

Gc(z) รูปที่ 3.17 block diagram discrete time control

จาก Block diagram

U(z) = Gc(z)E(z)

U ( z ) = [ Kp +

( z - 1)U ( z ) = KpE ( z ) +

Ki × T × ( z + 1) E (z) 2

zU ( z ) = KpE ( z ) +

u[k + 1] = Kpe[k ] +

Ki × T × ( z + 1) ]E ( s ) 2( z - 1)

Ki × T × ( z + 1) E( z) + U ( z) 2

Ki × T (e[k + 1] + e[k ]) + u[k ] 2

32

โดย KpและKi เปนคาที่ไดจากการปรับแตง Gc(s) ใน continuous time system นั้นคือหากสมการที่ โปรแกรมใหกับ Microcontroller ผลที่ไดก็คือ Microcontroller จะทํางานเลียนแบบ Gc(s) ผลที่ไดคือ Digital controller แตการออกแบบ Digital controller ดวยวิธีนี้ ผลลัพธที่ไดดีที่สุดคือ Gc(z)ในz domain ทํางานได เหมือนกับ Gc(s) ใน s domain

การหาแบบจําลองของระบบ (Modeling system)

Gp(s)

U(s)

Y(s)

รูปที่ 3.18 block diagram discrete ของ plant ที่ศึกษา โดยที่ U(s) คือ input plant Y(s) คือ output plant Gp(s)คือ plant

จาก Block diagram ไดวา

Gp =

Y (s) U (s)

การหาแบบจําลองของมอเตอรและแขน slave (Motor and slave arm modeling) เปนการหา Transfer function ของระบบ โดยที่ระบบที่จะทําการวิเคราะหเพื่อสรางตัวcontrollerที่จะ นํามาใชควบคุม คือแขน slaveโดยที่ตําแหนงของแขนนั้นขึ้นอยูกับการทํางานของ DC motorโดย model ของ DC motor อาจจะ

รูปที่ 3.19 Equivalent circuit motor

33

- พิจารณา Motor model แบบ first-order systems ไดวา Ea(s)

Gp(s)

Wn(s)

รูปที่ 3.20 block diagram ของมอเตอรแบบ First-order system โดยที่ Ea(s) =DC Voltage Wn(s)=angular velocity Gp(s) =plant ทําใหไดวาGp(s)หาไดจาก G p ( s) =

ωn ( s ) E a ( s)

เมือ พิจารณากราฟ First-order system step response

รูปที่ 3.21 แสดงกราฟ First-order system step response

34

รูปที่ 3.22 แสดงการทดลองหากราฟ First-order system step response เปนกราฟที่หาไดจาการทดสอบโดยการปอน step input ใหกับ plant แลวทําการหาคา time constant และระบบนี้เปนระบบปด เนื่องจาก คา output ของ plant จะมีคาคงที่คาหนึ่งเมื่อเวลาผานไป ซึ้ง DC motor ก็ เปนระบบเชนนี้ดวย ดังนั้นการหา transfer function ของมอเตอร ดวยวิธีนี้ ไดวา G p ( s) =

K s+a

หากพิจารณาระบบเปน DC motor ไดวา K = อัตราสวนความเร็วของmotorที่ steady stateกับ step input DC voltage a =

1 โดยที่ τ = time constant τ

หากพิจารณา outputของ motor เปน Θ(s) Ea(s)

Gp(s)

รูปที่ 3.23 block diagram ของมอเตอร เนื่องจาก

ω=

dθ dt

ไดวา Gp =

ωn( s) 1 θ ( s) = Ea( s) s Ea( s )

ดังนั้น G p ( s) =

K s( s + a)

Θ(s)

35

แตการ model motor ดวยวิธีนี้จะทําให plantที่ไดจากการหาtransfer function จาก model นี้ ไมใกลเคียงกับ แขน slave ซึ่งเปนระบบจริงเนื่องจาก ไมไดคิด load torque ที่ทํากับ มอเตอร

- พิจารณา model motor แบบ Electromechanical system

รูปที่ 3.24 model motor แบบ Electromechanical system เมื่อพิจารณา จากรูปที่ มีขอมูลดังนี้ J moment of inertia of the rotor ( kg.m^2/s^2) B damping ratio of the mechanical system (Nms) K=Ke=Kt electromotive force constant (Nm/Amp) R electric resistance (ohm) L electric inductance (H) V input (Voltage) θ output (position of shaft) The rotor and shaft are assumed to be rigid โดยที่ความสัมพันธของทอรกกับกระแส และ แรงดันกับความเร็วเชิงมุมของมอเตอร ไดวา Τ = K t ia โดยที่ Kt คือ torque constant

และ E a = K a ωn

โดยที่ Ka คือ back emf constant

36

จาก Equivalent circuit เมือพิจารณาจาก Newton's law และ Kirchhoff's law ไดสมการคือ d 2θ dθ J 2 +B = Ki dt dt L

จากสมการที่

di + Ri = V − Kθ dt

ใช Laplace Transforms s ( Js + B )θ ( s ) = KI ( s ) ( Ls + R) I ( s ) = V − Ksθ ( s )

จากสมการที่ ทําการหาtransfer function ไดดังนี้ ωn( s ) K = V (s) ( Js + B)( Ls + R) + K 2

θ (s) K = V ( s ) s (( Js + B)( Ls + R) + K 2 )

การ modeling แขน slave โดยใชวิธีนี้ทําให สามารถทราบ plant ของระบบ ใกลเคียงกับของจริงมาขึ้น เนื่องจากมีการนําเอาคาที่มีผลตอระบบมาคํานวณดวย ทําใหสามรถออกแบบตัว controller ไดดีขึ้น แต วิธีการ modeling ดวยวิธีนี้จําเปนตองมีเครื่องวัดความเร็วรอบมอเตอร เครื่องวัดทอรกของมอเตอรหรือตอง ทําการทดลองเพื่อใหทราบคาตามที่ไดmodelไว เพราะฉะนั้นขั้นตอนแรกที่จะทําการออกแบบระบบควบคุมคือตองทําการศึกษาระบบและสราง แบบจําลองขึ้นเพื่อนํามาคํานวณและออกแบบตัวควบคุมใหเหมาะสมกับระบบ

37

การเลือกใช POTENTIOMETER RESISTANCE vs Degree 10 9 8 7

R vs Degree

R(k ohm)

6 5 4 3 2 1 0

0

40 50

100

150 Degree

200

250 260

300

รูปที่ 3.25 แสดงกราฟการวัดคาความตานทานของ potentiometer เลือกใช POTENTIOMETER ขนาด 10k ohm โดยจากกราฟที่ ทําใหทราบวาPOTENTIOMETER มีชวงที่ linear อยูระหวาง 40-260 องศา

38

บทที่ 4 การทดลอง 4.1 การเปรียบเทียบโครงสรางของMaster Arm กับ Slave Arm ตารางขางลางเปนการเปรียบเทียบโครงสรางของMaster Arm กับ Slave Armที่ไดออกแบบ เปรียบเทียบกับโครงสรางของ Master Arm กับ Slave Arm ที่ไดจัดทําขึ้น

หัวไหลที่ 1 หัวไหลที่ 2 ขอศอก ทอนแขน มือ

ชวงมุมโครงสรางที่สามารถหมุนได ออกแบบ(องศา) Master Arm Slave Arm 0 180 900 1800 1800 1800 1800 900 900 900 900

ชวงมุมของโครงสรางที่ใชงานจริง(องศา) Master Arm 1800 1500 1800 900 900

Slave Arm 900 1500 1200 900 900

ตารางที่ 4.1 การเปรียบเทียบกับโครงสรางของ Master Arm กับ Slave Arm 4.2 การเปรียบเทียบการเคลื่อนที่ของ Master Arm และ Slave Arm ในการเปรียบเทียบจะทําการเปรียบเทียบแตละจุดหมุน ( Joint )โดยการตอการควบคุมเขาไปควบคุมและทํา การเปรียบเทียบมุมการเคลื่อนที่ของ Master Arm และ Slave Arm

รูปที่ 4.1 แสดงการติดตั้ง Master Arm และ Slave Arm

39

4.2.1 หัวไหลที่ 1 ในการทดลองนั้นใชแผนมุมวัดองศาติดไวกับ master arm และ slave armบริเวณหัวไหลที่ 1 โดยทําการวัดคาครั้งละ 30 องศา มุมการเคลื่อนที่( องศา ) Master arm 0 30 60 90

Error( %) Slave arm 0 29 61 81

0 3.33 3.33 10

ตารางที่ 4.2 การเปรียบเทียบการเคลื่อนที่ของ Master Arm กับ Slave Armบริเวณหัวไหลที่ 1

4.2.2 หัวไหลที่ 2 ในการทดลองนั้นใชแผนมุมวัดองศาติดไวกับ master arm และ slave armบริเวณหัวไหลที่ 2 โดยทําการวัดคาครั้งละ 30 องศา มุมการเคลื่อนที่( องศา ) Master arm 0 30 60 90 120 150 180

Error( %) Slave arm 0 20 35 80 115 145 175

0 33.33 41.66 11.11 4.16 3.33 2.77

ตารางที่ 4.3 การเปรียบเทียบการเคลื่อนที่ของ Master Arm กับ Slave Armบริเวณหัวไหลที่ 2

40

4.2.3 ขอศอก ในการทดลองนั้นใชแผนมุมวัดองศาติดไวกับ master arm และ slave armบริเวณขอศอก โดยทําการวัดคาครั้งละ 30 องศา มุมการเคลื่อนที่( องศา ) Master arm Slave arm 0 0 30 29 60 55 90 87 120 116

Error( %) 0 3.33 8.33 3.33 3.33

ตารางที่ 4.4 การเปรียบเทียบการเคลื่อนที่ของ Master Arm กับ Slave Armบริเวณขอศอก

41

บทที่ 5 สรุปและขอเสนอแนะ 5.1 สรุปการดําเนินงาน จากการดําเนินการการควบคุมแขนกลดวยแขนมนุษยระยะที่2โดยใชการควบคุมแบบดิจิตอลคอนโทล ไดขอสรุปดังนี้ ขอดี 1. ไดจัดทําโครงสรางของ master arm และ slave arm ขึ้นมาใหม โดยที่มีความคงทนและแข็งแรง 2. การควบคุมแขนกลแบบดิจิตอลคอนโทลโดยที่มีไมโครคอนโทลเลอรเปนตัวควบคุมและใชวงจร h-bridge ในการขับมอเตอรซึ่งในโครงการนี้สามารถที่จะขับได 3 จุดหมุนคือ หัวไหลที่ 1 , หัวไหลที่ 2 และ ขอศอก 3. การศึกษาและประยุกตใชแขนกลในงานดานตางๆ ขอเสีย 1. ปญหาเกี่ยวกับโครงสรางของ slave arm ซึ่งเกิดปญหาจากน้ําหนักของ slave arm มีน้ําหนักมาก เกินไป ทําใหการควบคุมทําไดยาก 2. การควบคุมยังไมสามารถทําไดอยางแมนยําเนื่องจากการที่ตองหาคาของ ki และ kp โดยการปรับคา ไปเรื่อยๆทําใหตองเสียเวลามาก 3. การควบคุมมอเตอรบริเวณขอมมือและมือ( gripper ) ยังไมสามารถที่จะทําการควบคุมได 4. แหลงจายไฟที่ใชไมสามารถจายไฟไดเทาที่ตองการเมื่อมีการทดลองที่ใชระยะเวลานานๆ

5.2 สิ่งที่ไดรับจากโครงการนี้ 1. ทําใหมีความรูความเขาใจเกี่ยวกับการทําหุนยนตมากขึ้น 2. ความรูเกี่ยวกับการควบคุมแขนกลแบบดิจิตอล คอนโทลเพื่อใชควบคุมมอเตอร 3. ความรูเกี่ยวกับการทําชุดขับมอเตอรแบบ H-bridge

42

5.3 ขอเสนอแนะ 1. 2. 3.

ในการเลือกใชวัสดุที่จะจัดทําแขนควรมีน้ําหนักเบาซึ่งจะสามารถทําใหควบคุมไดงาย ควรแยกแหลงจายไฟใหกับมอเตอรแตละตัวซึ่งจะสามารถทําใหการควบคุมมอเตอรจะไมเกิด ปญหาเนื่องจากแหลงจายไฟ มอเตอรที่ใชไมควรใชความเร็วรอบสูงเกินไปเพราะจะเกิดปญหาในการควบคุม

43

เอกสารอางอิง 1. Norman S.Nise, “ Control System Engineering ” ,John Wiley And Sons , California 2. มงคล ทองสงคราม, “Direct current machines” ,กทม ,รามาการพิมพ 3.สุวลัย กลั่นความดี, “ระบบควบคุม” ,กทม ,จุฬาลงกรณการพิมพ

44

ภาคผนวก

45

อุปกรณทีใชในโครงการ มอเตอรกระแสตรง ( DC MOTOR ) มอเตอรที่ใชในการทําโครงการนี้คือ มอเตอรกระแสตรง( dc motor ) ซึ่งมีขนาดตางๆดังตอไปนี้

มอเตอรกระแสตรง 12 V 150 rpm แรงบิดที่ 30 kg-cm ใชขับหัวไหลที่ 1

มอเตอรกระแสตรง 12 V 30 rpm แรงบิดที่ 20 kg-cm ใชขับหัวไหลที่2,ขอศอก

มอเตอรกระแสตรง 12 V 30 rpm แรงบิดที่ 1 kg-cm ใชขับที่ทอนแขนและนิ้ว

46

- ตัวตานทานปรับคาได ( Potentiometer ) เปนตัวตานทานขนาด 10k โอหม

-

แหลงจายไฟขนาด 12 โวลต