โปรแกรมเชอมตอกบผใชทางกราฟกในการหาสมการการเคลอนทของหนยนตดวยโปรแกรม MATLAB

ปรญญานพนธ ของ

วรช สรทรพยไพบลย เสนอตอบณฑตวทยาลย มหาวทยาลยศรนครนทรวโรฒ เพอเปนสวนหนงของการศกษา

ตามหลกสตรปรญญาวศวกรรมศาสตรมหาบณฑต สาขาวชาวศวกรรมเครองกล เมษายน 2551

โปรแกรมเชอมตอกบผใชทางกราฟกในการหาสมการการเคลอนทของหนยนตดวยโปรแกรม MATLAB

ปรญญานพนธ ของ

วรช สรทรพยไพบลย เสนอตอบณฑตวทยาลย มหาวทยาลยศรนครนทรวโรฒ เพอเปนสวนหนงของการศกษา

ตามหลกสตรปรญญาวศวกรรมศาสตรมหาบณฑต สาขาวชาวศวกรรมเครองกล เมษายน 2551

ลขสทธเปนของมหาวทยาลยศรนครนทรวโรฒ

โปรแกรมเชอมตอกบผใชทางกราฟกในการหาสมการการเคลอนทของหนยนตดวยโปรแกรม MATLAB

บทคดยอ ของ

วรช สรทรพยไพบลย เสนอตอบณฑตวทยาลย มหาวทยาลยศรนครนทรวโรฒ เพอเปนสวนหนงของการศกษา

ตามหลกสตรปรญญาวศวกรรมศาสตรมหาบณฑต สาขาวชาวศวกรรมเครองกล เมษายน 2551

วรช สรทรพยไพบลย (2551). โปรแกรมเชอมตอกบผใชทางกราฟกในการหาสมการการเคลอนทของหนยนตดวยโปรแกรม MATLAB. ปรญญานพนธ วศ.ม. (เครองกล). กรงเทพฯ: บณฑตวทยาลย มหาวทยาลยศรนครนทรวโรฒ. คณะกรรมการควบคม : พนโท ผชวยศาสตราจารย ดร.ทววชร วระแกลว, พนโท ผชวยศาสตราจารย ดร.อโณทย สขแสงพนมรง.

ในงานวจยนมวตถประสงคเพอออกแบบและสรางโปรแกรม Graphical User Interface

สาหรบรบคาอนพตคาตวแปรตางๆ ทใชในการคานวณเพอหาสมการการเคลอนทของหนยนต รวมถงการแสดงผลเอาทพตหลงการคานวณในรปแบบทเหมาะสมดวยโปรแกรม MATLAB และสามารถหาคา Moment of Inertia ของแขนกลรปทรงเรขาคณตอยางงายเพอนามาชวยใหหาสมการการเคลอนทไดถกตองยงขน

การดาเนนการวจยนไดออกแบบและสรางสมการการเคลอนทของหนยนตดวยทฤษฎของ ออยเลอร-ลากรานซ และวเคราะหปญหาของหนยนตแบบ Forward Kinematics ผใชตองมขอมลทเปนตาราง D-H (Denavit-Hartenberg) อนพตทโปรแกรมโดยทผใชสามารถอนพตคาทเปนเชงตวเลขหรอคาทเปนตวแปรกได ซงโปรแกรมจะนาคาทไดไปคานวณตอโดยใชฟงกชน Symbolic ถาคาทอนพตเปนตวแปร คาผลลพธคาตอบทไดจะตดเปนตวแปรเชนกน ในสวนการอนพตคาโมเมนตความเฉอยทเปนรปทรงเรขาคณตอยางงาย ผใชสามารถเลอกไดงายโดยมรปภาพมาประกอบใหเลอกและใสคาตวแปรตางๆ ดงรป การแสดงผลของผลลพธคาตอบสามารถแสดงออกมาไดหลายโปรแกรม แลวแตผใชจะเปนผเลอก เพอทสามารถนาไปประยกตกบโปรแกรมอนเพอหาคาตอบทเปนเชงตวเลขไดอกครง

จากการทดสอบโปรแกรมพบวาวธการหาสมการการเคลอนทของหนยนตดวยโปรแกรมนนมความถกตอง เมอแสดงใน โปรแกรม แตเมอผลทไดออกมาใหรปแบบของ Microsoft Office Word หรอโปรแกรมอน จะไมสามารถทาใหอยในรปของสมการทเขาใจไดงาย ดงนนจงตองเขาใจในตวแปรของสมการดงกลาวดวย

THE GRAPHIC USER INTERFACE FOR THE EQUATIONS OF MOTION ROBOT USING MATLAB PROGRAM

AN ABSTRACT

BY WIRAT SIRISAPPAIBUN

Presented in Partial Fulfillment of the Requirements for the Master of Engineering Degree in Mechanical Engineering

at Srinakharinwirot University April 2008

Wirat Sirisappaibun. (2008). The Graphic User Interface for The Equations of Motion Robot Using MATLAB Program. Master thesis, M.Eng. (Mechnical Engineering). Bangkok: Graduate School, Srinakharinwirot University. Advisor Committee : Lt.Col. Asst. Prof. Dr. Tawiwat Veeraklaew , Lt.Col. Asst. Prof. Dr. Anotai Suksangpanomrung.

In this thesis have the objective for designs and construction Graphical User Interface for input the variable at use in the computation for find dynamic equation of a robot include outcome calculation show in the format that is appropriate with a program MATLAB and find inertia momentum of arm simply geometric for bring to help give can seek dynamic equation is correct increasingly. Operation researched this design and construction equation of robot motion describing by Euler-Lagrange equations and analytical robotic manipulators in term of Forward Kinematics, D-H (DENAVIT-HARTENBERG) can be incorporated to the program including the digital and variable inputs. MATLAB program determines the data by symbolic function, if input is the variable then the outcome is also the variable, in case of input the inertia momentum is simply geometry the user simply chose by have the picture assemble and input variable follow picture the expression of result can be illustrated in many programs base on user preference which aims to apply with the other program that generates the result as digital once again. From program tested discover that the method find dynamic equation of a robot with program has the justice, when show in program but when at get come out give the format of MS. Word or a program is other will can not display in format of the equation that simply understand thus then must understand in the variable of the equation with.

ปรญญานพนธ เรอง

โปรแกรมเชอมตอกบผใชทางกราฟกในการหาสมการการเคลอนทของหนยนตดวยโปรแกรม MATLAB

ของ วรช สรทรพยไพบลย

ไดรบอนมตจากบณฑตวทยาลยใหนบเปนสวนหนงของการศกษาตามหลกสตร

ปรญญาวศวกรรมศาสตรมหาบณฑต สาขาวชาวศวกรรมเครองกล ของมหาวทยาลยศรนครนทรวโรฒ

........................................................................ คณบดบณฑตวทยาลย

(ผชวยศาสตราจารย ดร.เพญสร จระเดชากล) วนท….. เดอน เมษายน พ.ศ. 2551

คณะกรรมการควบคมปรญญานพนธ คณะกรรมการสอบปากเปลา ........................................ ประธาน ........................................ ประธาน (พ.ท.ผศ.ดร.ทววชร วระแกลว) (รศ.ดร.เศรษฐพงค มะลสวรรณ) …………………………… กรรมการ …………………………… กรรมการ (พ.ท.ผศ.ดร.อโณทย สขแสงพนมรง) (พ.ท.ผศ.ดร.ทววชร วระแกลว) .….…............................... กรรมการ (พ.ท.ผศ.ดร.อโณทย สขแสงพนมรง) ........................................ กรรมการ (ผศ.วชต บวแกว)

ประกาศคณปการ ปรญญานพนธฉบบน สาเรจไดดวยความกรณาของ พนโท ผชวยศาสตราจารย ดร. ทววชร วระแกลว ประธานกรรมการควบคมการทาปรญญานพนธ และ พนโท ผชวยศาสตราจารย ดร. อโณทย สขแสงพนมรง กรรมการควบคมการทาปรญญานพนธ ทกรณาใหคาปรกษา แนะนาและใหความชวยเหลอแกไขความบกพรองและปญหาตางๆ อกทงใหกาลงใจขณะดาเนนการทาปรญญานพนธ ผวจยขอกราบขอบพระคณเปนอยางสงตลอดไป ขอขอบพระคณ ผชวยศาสตราจารย วชต บวแกว ทชวยตรวจแกปรญญานพนธ และใหขอเสนอแนะทเปนประโยชนตองานวจย ขอขอบคณบดา มารดา ตลอดจนเพอนรวมงานทกทาน ในความสนบสนน ชวยเหลอและใหกาลงใจ เปนอยางดตลอดเวลา ขอขอบคณเพอนรวมชนรนท 3 และรนพรนนองในโครงการความรวมมอหลกสตรปรญญาโท มศว. และ รร.จปร. สาขาวชาวศวกรรมเครองกล ทกทานทใหความชวยเหลอและเปนกาลงใจตลอดมา

วรช สรทรพยไพบลย

สารบญ

บทท หนา 1 บทนา................................................................................................................ 1 ความเปนมาและความสาคญของปญหา...................................................... 1 วตถประสงคของการวจย............................................................................ 1 ขอบเขตของการวจย................................................................................... 2 ประโยชนทคาดวาจะไดรบ.......................................................................... 2 2 งานวจยและทฤษฎทเกยวของ........................................................................... 3 ทบทวนงานวจยทเกยวของ.......................................................................... 3 ทฤษฎทเกยวของ........................................................................................ 4 3 วธดาเนนการวจย.............................................................................................. 24 การกาหนดปญหาหนยนต……………………………................................... 24 การแกปญหาเชงสญลกษณ........................................................................ 28 ลาดบขนตอนการทางานของโปรแกรม…………………................................ 30 4 ผลการทดสอบและวเคราะหผลการทดสอบ..................................................... 31 ผลการทดสอบ........................................................................................... 31 วเคราะหผลการทดสอบ............................................................................... 39 5 สรปและวจารณ................................................................................................. 40 สรปและวจารณผลการทดสอบ.................................................................... 40 ขอเสนอแนะ............................................................................................... 41 บรรณานกรม................................................................................................................. 42

สารบญ(ตอ)

บทท หนา ภาคผนวก…...……………………………………………….…………………………… 44 อภธานศพท………………………………………………….…………………………… 70 คาอธบายสญลกษณ……………………………………..…….………………………… 72 ประวตยอผวจย……................................................................................................. 73

บญชตาราง ตาราง หนา

1 คาตวแปร D-H ของหนยนตแบบแขนกลทมลกษณะเปนขอตอแบบงอ ……….....……. 25 2 คาตวแปร D-H ของหนยนตสเตมฟอดร ………………………………..………………. 27 3 คาตวแปร D-H ของหนยนตสกาลาร ……………………………………...……………. 28

บญชภาพประกอบ ภาพประกอบ หนา

1 แสดงตาแหนงจดศนยกลางมวล ……….…………………………... …………………. 5 2 แทงกลมขนาดเลก สวนทแรเงาคอมวล dm เลก ๆ ทแบงในชวง dx …………………. 6 3 แสดงภาพประกอบทฤษฎแกนขนาน ……………………………….…………………. 8 4 แสดงแกนทตดกบเฟรมของแขนกลรปทรงคลายขอศอก ………………………………. 10

5 แสดงการตงเฟรมตามวธเดนาวต-ฮารเทนเบอรก ……………...………………………. 12 6 แขนกลทมลกษณะเปนขอตอแบบงอ 2 ขอตอ ………………..………………..……… 24

7 กาหนดพกด DH เฟรมสาหรบหนยนตสเตมฟอดร ………………..……………………. 26 8 กาหนดพกด DH เฟรมสาหรบหนยนตสกาลาร …..………………………….…………. 27 9 แสดงลาดบขนตอนการทางานของโปรแกรมแบบยอ …….………………….…………. 30 10 แสดงตวแปรทมในแขนกลทมลกษณะเปนขอตอแบบงอ 2 ขอตอ ………….…………. 31 11 แสดงตวแปรทมในหนยนตสเตมฟอดร …....………….…………………..….…………. 36 12 แสดงตวแปรทมในหนยนตสกาลาร .………………………………………...…………. 38

บทท 1 บทนา

1.1 ความเปนมาและความสาคญของปญหา ในยคสมยทเทคโนโลยมการพฒนาเจรญกาวหนานน งานภาคอตสาหกรรมกลบมความตองการจางแรงงานจานวนมาก ทาใหตนทนการผลตของแตละโรงงานเพมสงขน ประกอบกบงานอตสาหกรรมบางอยางอนตรายและมความเสยงสง หรอเปนงานทตองการความรวดเรวและแมนยาในการผลต ทาใหหนยนตกลายมาเปนทางออกของงานภาคอตสาหกรรม หนยนตสามารถทางานไดแบบอตโนมต โดยมอปกรณควบคมใหหนยนตทางานตามเปาหมายทกาหนด ในกระบวนการทางานทดาเนนไปอยางชา ๆ นน จะสามารถควบคมตวแปรดวยมอหรอเครองมอพนฐานได แตสาหรบกระบวนการทตองการความรวดเรวและถกตองสง เราไมอาจสามารถควบคมดวยมอได เพอหลกเหลยงความลาชาและไมสมาเสมอของมนษย จงมผนาเอาคอมพวเตอรมาประยกตเขากบหนยนต ทาใหเกดผลสาเรจของการควบคมอยางรวดเรว เทยงตรง และมเสถยรภาพ

ในการควบคมการเคลอนทของหนยนตนน มตวแปรหลายอยางทเขามาเกยวของ และมการคานวณทซบซอนหลายขนตอน อาจทาใหเกดความผดพลาดไดงาย การนาโปรแกรมคอมพวเตอรมาใชจงเปนอกวธหนงในการแกไขปญหา และสงผลใหการควบคมการเคลอนทถกตอง แมนยา และรวดเรวมากยงขน

1.2 วตถประสงคของการวจย

1.2.1 เพอออกแบบและสรางโปรแกรมเชอมตอกบผใชทางกราฟกสาหรบรบคาอนพตคาตวแปรตางๆ ทใชในการคานวณเพอหาสมการการเคลอนทของหนยนต รวมถงการแสดงผลเอาทพตหลงการคานวณในรปแบบทเหมาะสมดวยโปรแกรม MATLAB

1.2.2 เพอออกแบบและสรางโปรแกรมเชอมตอกบผใชทางกราฟกจาก MATLAB เพอหาคาโมเมนตความเฉอยรปทรงเรขาคณตอยางงาย

2 1.3 ขอบเขตของการวจย 1.3.1 โปรแกรมเชอมตอกบผใชทางกราฟกสามารถรองรบจานวนแขนตอ (Link) ทมใน

ระบบไดไมจากดจานวน แตงในงานวจยนจะทดสอบเพยง 6 แขนตอ 1.3.2 สรางและแสดงผลสมการการเคลอนทใหออกมาในรปแบบของ MATLAB และ

Microsoft Office Word 1.3.3 วเคราะหปญหาของหนยนตแบบหาจลศาสตรไปขางหนา (Forward Kinematics) 1.3.4 ใสรปแบบของโมเมนตความเฉอยของแขนตอทมรปทรงเรขาคณตอยางงายได เชน

ทรงวงกลม ทรงวงรทรงกระบอก ทรงพระมด ทรงพาราโบลอย เปนตน 1.3.5 สามารถหาโมเมนตความเฉอยของแขนตอทแกนหมน ณ จดใด ๆ 1.4 ประโยชนทคาดวาจะไดรบ

1.4.1 ชวยในดานการเรยนการสอน โดยผทกาลงศกษาหรอสนใจเรองหนยนตสามารถนาสมการการเคลอนท ทไดจากการคานวณดวยมอมาตรวจสอบกบโปรแกรม

1.4.2 ผลของการวจยจะทาใหโปรแกรมคอมพวเตอรสามารถหาสมการการเคลอนทไดอยางรวดเรว แมนยาและถกตอง

1.4.3 ผลของการวจยจะทาใหโปรแกรมคอมพวเตอรทสามารถหาคาโมเมนตความเฉอยรปทรงตางๆ ทงทมรปทรงเรขาคณตอยางงาย

1.4.4 เปนการสรางองคความรพนฐาน เพอทจะพฒนาไปสความรทางวชาการและวทยาการทสงขน

1.4.5 เปนแนวทางในการสรางและพฒนาโปรแกรมทเกยวกบหนยนตในอนาคต 1.4.6 สามารถนาเอาขอมลทฤษฎทไดจากการศกษางานวจยน นาไปวเคราะหรวมกบ

งานวจยอนทมลกษณะคลายคลงกนได

บทท 2 งานวจยและทฤษฎทเกยวของ

ในการวจยครงน ไดศกษาเอกสารและงานวจยทเกยวของ และนาเสนอตามหวขอตอไปน 1. ทบทวนงานวจยทเกยวของ 2. ทฤษฎทเกยวของ 2.1 ทบทวนงานวจยทเกยวของ ในการออกแบบหนยนตจะตองมการคานวณหาสวนตางๆมากมาย หนงในนนมการหาสมการการเคลอนทของหนยนต ซงถอวาสาคญอยางมาก โดยสมการการเคลอนทจะเปนตวกาหนดเสนทางการเคลอนทของหนยนตใหเปนไปตามทตองการ จงไดมการคดคนพฒนาโปรแกรมเพอชวยในการคานวณหา อาทเชน เครองมอทใชเกยวกบหนยนตในโปรแกรม MATLAB (Robotics Toolbox for MATLAB) , 2000 มฟงกชนใหเลอกใชมากมายและเปนประโยชนในเรองหนยนต เชน จลนศาสตร (Kinematics) การเคลอนทและการสรางเสนทางการเคลอนท เครองมอนยงมประโยชนสาหรบการจาลองผลการวเคราะหจากการทดลองดวยหนยนตจรง โดยหลกๆใชวธของจลนศาสตรและการเคลอนทของการผสมผสานจดตอหลายรปราง ซงแบบหนยนตทใชกรจกกนดเชน พมา560 (Puma 560) และ สแตมฟอรดอาม (Stanford Arm) ฟงกชนของเครองมอทใชเชน เวกเตอร การเปลยนรปของวตถทประกอบเขากนและการแบงภาคระนาบทงสซงจาเปนกบการแทนคาของจดสามมตและการพลกหกเห จะสงผลใหการแสดงภาพกราฟกทาทางของหนยนตไดดอกดวย เพยงใหคา DH (Denavit and Hartenberg Parameters) หนยนตกจะสามารถเคลอนเปนลาดบจากจดขอตอไปหาจดอางองผลทไดรบจากโปรแกรมคอ จดตาแหนงทแสดงในกราฟฟกซงสามารถนาไปเปรยบเทยบกบวธการอนทหาไดและไดภาษาโปรแกรมหลก (Source Code) ซงนนจะเปนประโยชนสาหรบการทาความเขาใจและการเรยนการสอน Web Enabled Robot Design and Dynamic Control Simulation Software Solutions From Task Points Description โดย ทาเลค โซบ สาโรดร พาเทลและไบล หวง (Tarek Sobh, Sarosh Patel and Bei Wang) แหงมหาวทยาลยบรจพอรต, 2005 ไดรวมกนสรางเวบไซดสาหรบการออกแบบหนยนต และการจาลองการเคลอนท โดยไดจากรายละเอยดของจดงาน การรวมโปรแกรมตางๆ และการคานวณอยางมประสทธภาพดวยการใชหลกคณตศาสตรและความสามารถของ MATLAB การกาหนดตาแหนงและความเรวของแตละจด โปรแกรมจะออกแบบหนยนต 3 ระดบขนความเสรให

4

เรยบรอย โดยใชหลกการคานวณของตวแปรทางการเคลอนททเหมาะสมอาทเชน ความยาวของขอตอ มวลและโมเมนตความเฉอย สรปจากการศกษางานวจยทเกยวของทาใหทราบวามการพยายามสรางโปรแกรมชวยในการออกแบบหนยนตใหมความถกตองและงายตอการศกษาเรยนร ซงสวนใหญจะสรางจากการอางองรปแบบหนยนตทเปนมาตรฐาน จากดจานวนขอตอ (Link) ของแขนหนยนตและออกแบบเปนแบบสาเรจรป โดยไมสามารถจะบงบอกหรอแสดงสมการการเคลอนททคานวณไดดวยโปรแกรม ซงจะนามาศกษาเปรยบเทยบความถกตองหลงคานวณไดดวยมอจากทศกษามา จงทาใหผวจยมความสนใจทจะศกษาและสรางโปรแกรมทสามารถคานวณหาสมการการเคลอนท ใหแสดงอยในรปแบบตาง ๆ เพอสะดวกตอการนาไปใชงาน โดยโปรแกรมสามารถหาจานวนขอตอไดไมจากดจานวน อกทงสามารถกาหนดรปทรงของขอตอ ในรปทรงเรขาคณตอยางงายในรปทรงตาง ๆ ไดดวย ซงจะทาใหสมการการเคลอนทมความถกตองมากยงขน 2.2 ทฤษฎทเกยวของ 2.2.1 โมเมนตความเฉอยของวตถ (Moment of Inertia of the Mass) 2.2.1.1 จดศนยกลางมวล (Center of Mass; CM) จดศนยกลางมวลถอวาเปนจดเสมอนมวลสาหรบวตถขนาดใหญ หรอระบบอนภาครวมอยทจดนน โดยปกตจดศนยกลางมวลและจดศนยกลางความถวง จะเปนจดเดยวกนและอาจอยภายในหรอภายนอกวตถกได ถาวตถมขนาดใหญมาก ในทานองเดยวกนกบจดศนยกลางความถวงกจะหาจดศนยกลางมวล

..mcx = ∑∑

mmx ii (2.1)

..mcy = ∑∑

mmy ii (2.2)

..mcz = ∑∑

mmz ii (2.3)

ให ..mcx , ..mcy , ..mcz คอ จดพกดของจดศนยกลางมวล ถาให rv เปนเวกเตอร ตาแหนงของจดศนย

กลางมวล จะได ..mcrv =

mrm ii∑ เทยบกบแกนพกด xyz กบแกนพกด ..mcx , ..mcy , ..mcz จะมจด

กาเนดทจดศนยกลางมวล

5

ภาพประกอบ 1 แสดงตาแหนงจดศนยกลางมวล (CM)

2.2.1.2 วธคานวณหาโมเมนตความเฉอย (Moment of Inertia) 2

ii rmI ∑= เปนสมการทหาโมเมนตความเฉอยในกรณทมวลเปนจด แตถามวลมรปทรงขนาดใหญและเนอวตถกระจายอยางสมาเสมอจะตองใชวธการอนทกรลแทนโดยแบงมวลของวตถออกเปนชนเลก ๆ มคา dm อยหางจากแกนหมนเปนระยะ r โมเมนตความเฉอยของอนภาคเลกๆ นจะเปน

dI = dmr 2 (2.4) โมเมนตความเฉอยของวตถทงกอน

I = ∫ dmr 2 (2.5) ถา ρ เปนความหนาแนนของวตถ และ dV เปนปรมาตรเลก

dm = ρdV (2.6) แทนในสมการ(2.5) จะได

6

I = ∫ 2r ρdV (2.7)

การกระจายของมวลเปนเนอเดยวสมาเสมอ ความหนาแนนจะคงท สามารถนาออกนอกเครองหมาย อนทกรลได

I = ρ∫ 2r dV (2.8) dV คอปรมาตรเลก ๆ ของมวล dm จงมลกษณะเหมอนจด ดงเชน วธคานวณหาโมเมนตความเฉอยของแทงกลมขนาดเลก มวล m ยาว l เนอวตถกระจายสมาเสมอ แกนหมนตงฉากกบจด O หางจากปลายแทงขางหนงเปนระยะ h แบงแทงกลมเปนชวงเลก ๆ dx หางจากจด O เปนระยะ x อตราสวนของมวลกอนเลก ๆ dm ตอมวลทงกอน m จะเทากบ อตราสวนของ dx กบความยาวทงหมด l ดงน

ภาพประกอบ 2 แทงกลมขนาดเลก สวนทแรเงาคอมวล dm เลก ๆ ทแบงในชวง dx

ดงนนจะได

ldx

mdm

= (2.9)

lmdxdm = (2.10)

แทนคา dm ลงในสมการ (2.5) และอนทเกรตบนแกน x จาก h− ถง hl − ดงน

h hL −

7

0I = ∫ dmx 2 = dxx

lm hl

h∫−

−

2

= hlh

xlm −

−3

3

= )3ln3(31 22 hlm +− (2.11)

สมการ (2.11) ทได สามารถหาโมเมนตความเฉอยรอบแกนหมนไดทก ๆ แกน ตวอยางเชน ถาแกนหมนอยทปลายแทงดานซาย 0=h จะได

2

31mlI = (2.12)

แกนหมนอยทปลายแทงดานขวา lh = จะได

2

31mlI =

เทากนเพราะมวลมลกษณะสมมาตร ทงซายขวา ถาแกนหมนผานจดศนยกลางมวล h =

2l

2

121 mlI = (2.13)

2.2.1.3 ทฤษฎแกนขนาน (Parallel Axis Theorem) รอบแกนทผานจดศนยกลางมวล แทนดวย ..mcI แตถาเราตองการหาโมเมนตความเฉอยรอบแกนหมนใหมทขนานกบแกนหมนเดมเปนระยะ d กสามารถคานวณหาได โดยใชทฤษฎของสไตเนอร (Steiner's Theorem) หรอทฤษฎแกนขนาน (Parallel Axis Theorem) ซงเขยนเปนรปของสมการไดวา

I = ..mcI + 2md (2.14)

8

..mcI คอโมเมนตความเฉอยของวตถมวล m รอบแกนหมนทผานจดศนยกลางมวล I คอโมเมนตความเฉอยของวตถรอบแกนซงขนานกบแกนทผานจดศนยกลางมวลและหางเปนระยะ d

ภาพประกอบ 3 แสดงภาพประกอบทฤษฎแกนขนาน การพสจนหาความสมพนธของสมการ (2.14) เราจะกาหนดใหจด C เปนจดศนยกลางมวลของวตถรปใด ๆ มพนทภาคตดขวางดงภาพประกอบ 3 มพกดท ( ..mcx , ..mcy , ..mcz = 0) ใหแกนหมนทผานจด C มทศตงฉากกบระนาบของกระดาษและแกนหมนทจด P ซงมแกนขนานกบแกนทผานจดศนยกลางมวล P จะมจดพกดอยท ( ax mc +.. , by mc +.. , ..mcz = 0) และระยะระหวางแกนทงสองมคา d ซงเทากบ 22 ba + จะทาใหอนภาคมวล im อยหางจากจด 22

ii yxc += และจด 22 )()( byaxP ii −+−= คาโมเมนตของความเฉอยรอบแกนทผานจด P มคาเทากน

∑ −+−= })(){( 22 byaxmI iii

∑ ∑ ∑ ∑++−−+= iiiiiiii mbaymbxmayxm )(22)( 2222 เนองจากคา ..mcx และ ..mcy เปน 0 เพราะเปนจดศนยกลางมวล ดงนน ∑ ∑ == 0icmii mxxm ∑ ∑ == 0icmii myym

..mcY

..mcX O

Y

X

ix

iy

ab

d C ),( .... mcmc yx

),( .... byaxP mcmc ++ im

9

I = ∑ ∑+++ iiii mbayxm )()( 2222 I = 2MdIcm + (2.15)

2.2.2 สมการการเคลอนทของหนยนต (Dynamic Equations for Robot)

2.2.2.1 ทฤษฎจลนศาสตร (Kinematics Theory) จดมงหมายของการวเคราะหจลนศาสตร เหมอนกบหนยนตสามารถคดได ตามทกาหนดขอตอเชอมตอดวยกนทขอตอตางชนดกน ถาขอตอสามารถเหมอนกนไดยงทาใหงายยงขน เชน จดตอหมน (Revolute Joint) หรอ จดตอเลอน (Prismatic Joint) หรอนอกจากนนเปนรปทรงซบซอนกวาได เชน ลกบอลและจดตอทเปนเบา (จากทกลาวมา จดตอหมน เปนเหมอนจดแกวงและหมนรอบแกนเดยว จดตอเลอน เคลอนทเปนเสนตรงตามแกนๆ เดยว กลาวคอ ยดหรอหดเขา) ความแตกตางระหวาง 2 อยางนนคอ ประการแรก ขอตอมเพยง หนงระดบขนความเสรของการเคลอนท มมของการหมนในกรณของจดตอหมนและถาเปนการเคลอนทแบบเสนตรงในกรณของจดตอเลอน ในทางตรงกนขาม ลกบอลและขอตอแบบเบามสองระดบขนความเสร สมมตใหหนยนต มขอตอจานวน 1+n ขอตอ จาก 0 ถง n เรมตนจากฐานของหนยนต เปนขอตอท 0 จานวนจดตอเรมจาก 1 ถง n และลาดบจดตอท i เปนจดในชองวางทขอตอ

1−i และ i ทตอกน จดตอท i เปลยนมาใชแทนเปน iq ในกรณของจดตอหมน iq เปนมมของการหมนและในกรณของจดตอเลอน iq เปนระยะจดตอเคลอนท สมมตให iA เปนโฮโมจเนยเมตรกนนเปลยนรปตาแหนงของจดจากเฟรม i ถงเฟรม 1−i เมตรก iA ไมคงทแตจะแตกตางตามรปรางของหนยนตทเปลยนไป อยางไรกตามสมมตใหทกจดตอทงทหมนหรอเลอนเปน iA คอทม iq เปนฟงกชนของจดตอเดยวทเปลยนแปลงเทานน จะได

)( iii qAA = (2.16)

ขณะทโฮโมจเนยเมตรกนนเปลยนรปตาแหนงของจดจากเฟรม j ถงเฟรม i เรยกไดวาเปน ทรานฟอรมเมชนเมตรก และใชแทนเปน j

iT จะไดวา

jjiiji AAAAT 121 ... −++= if ji < (2.17)

IT ji = if ji = (2.18)

1)( −= ij

ji TT if ij > (2.19)

10

ภาพประกอบ 4 แสดงแกนทตดกบเฟรมของแขนกลรปทรงคลายขอศอก

ตาแหนงและการหกเหของจดรบแรงสดทายดวยสวนของจดตงตนหรอฐานเฟรมทสามเวกเตอร nd0

และเปน 3x3 โรเตชนเมตรก nR0 โดยลาดบและกาหนดเปนโฮโมจเนยสเมตรก

=

1000nn dR

H (2.20)

ดงนนตาแหนงและการหกเหของจดรบแรงสดทายในสวนของเฟรมตงตนจะแสดงไดวา

)()...( 110 nnn qAqATH == (2.21)

แตละโฮโมจเนยสทรานฟอรมเมชน iA เปนแบบน

= −−

1011ii

ii

idR

A (2.22)

== + 10

...1

ji

ji

jiji

dRAAT (2.23)

เมตรก j

iR แสดงการหกเหของเฟรม j สมพนธกบโครง i และใหสวนทหมนของเมตรก Aตามท

11

jj

ii

ji RRR 1

1... −+= (2.24)

เวกเตอร j

id จะยอมใหตดกนไปตามหลก ทงหมดนจะถกใชในเรองของเมตรกจาโคเบยน

jj

ji

ji

ji dRdd 1

11−

−− += (2.25) 2.2.2.2 เดนาวต-ฮารเทนเบอรก (Denavit-Hartenberg) Denavit-Hartenberg หรอ แบบแผน D-H ในความเปนแบบแผนแตละโฮโมจเนยส ทรานฟอรมเมชน iA เปนการนาเสนอตามทผลคณของสลกษณะพนฐานในการเปลยนรป

iiiiiiii xaxdzzi RotTransTransRotA αθ= (2.26)

=

−

−

100000000001

100001000010

001

1000100

00100001

110011000000

ii

iiii

ii

cssc

a

dcssc i

i αα

ααθθ

θθ

=

−

−

10000 i

i

i

dcssascccscasscsc

ii

iiiiii

iiiiii

αα

θαθαθθ

θαθαθθ

(2.27)

ทสขนาด iiii da αθ ,,, เปนตวแปรของขอตอ i และจดตอ i เปนตวแปรตางๆใน (2.26) โดยปกตจะแทนความหมายตามน ia ใชเรยกแทนความยาว iα ใชเรยกแทนการบด id ใชเรยกแทนระยะทเยอนออกมาและ iθ ใชเรยกแทนมม เมอเมตรก iA เปนฟงกชนของตวแปรเดยวซงเกดขนมาจากสามสงทนอกเหนอไปกวาสขนาดของคาคงททไดมาจากขอตอ ในขณะทสตวแปรจะม iθ ของจดตอหมนและ id ของจดตอเลอนเปนคาไมคงทของจดตอนนๆ

12

ภาพประกอบ 5 แสดงการตงเฟรมตามวธเดนาวต-ฮารเทนเบอรก (DENAVIT-HARTENBERG)

เปนการตงเฟรมบนขอตอตางๆ โดยใชวธของเดนาวต-ฮารเทนเบอรก (DENAVIT-HARTENBERG) จดกาเนดของเฟรมท )0( ii จะอยบนจดตดระหวางจดตอ 1+i กบเสนตงฉากระหวางจดตอ i กบ 1+i แกน ix จะอยบนจดตอ 1+i แกน iy จะอยบนเสนตงฉากระหวางจดตอ i กบ จดตอ 1+i แกน iz จะเปนไปตามกฎมอขวา ia คอ ความยาวของเสนตงฉากระหวาง 1−iz กบ iz id คอ ระยะระหวาง 1−ix กบ ix ตามแนวแกน 1−iz iα คอ มมระหวาง 1−iz กบ iz วดรอบแกน ix ตามกฎมอขวา iθ คอ มมระหวาง 1−iz กบ iz วดรอบแกน 1−iz ตามกฎมอขวา 2.2.2.3 จาโคเบยน (Jacobian) การพจารณาหลกจลศาสตรของแขนกล จะนาไปสความเรวของหนยนตซงมความสมพนธของความเรวจะหาไดโดยการใชจาโคเบยน ซงจะแปลงคาระหวางอนพตทจดเรมตนกบเอาทพตทปลายแขนกล จาโคเบยน มความสาคญมากในการวเคราะหและการควบคมการเคลอนไหวของหนยนตเพราะจะทาใหไดมาซงสมการการเคลอนทของหนยนตไดในทสด สาหรบหนยนตทมขอตอจานวน n ขอตอ จะสามารถหาจาโคเบยน ไดโดยการแปลงคาระหวางเวกเตอรความเรวของจดตอจานวน n เวกเตอร กบ 6 เวกตอรความเรวของจดปลายแขนกล ซงจะมทงความเรวเชงเสนและความเรวเชงมม เมอพจารณาหนยนตทมขอตอจานวน n ขอตอ จะทาใหมการเปลยนของจดตอจานวน nqq ,...,1 จะไดวา

13

=

10)()(

)(00

0 qPqRqT nn

n (2.28)

เมอ T

nqqq ),...,( 1= ซงเปนเวกเตอรของตวแปรจดตอ เมอหนยนตมการเคลอนท )(tqi จะทาใหมการเปลยนแปลงสองลกษณะคอ การเลอนตาแหนงและการหมน ดงนนจงสามารถหา

ความเรวเชงเสนและความเรวเชงมมดวยการคดเทยบกบเวลาทเปลยนแปลงไปคอ

qJV

ii

i &=

ω

(2.29)

จะไดวา

=

i

vi

i

J

JJ

ω

L

โดยท เมตรก iJ ถกเรยกวาจาโคเบยนซงจะมขนาดเทากบ n×6 เมอ n คอจานวนของขอตอ 2.2.2.4 สมการออยเลอร-ลากรานซ (Euler-Lagrange Equations) ในหวขอนไดอธบายการหาสมการในหวขอเรองโฮโลโมนกคอนสเทรน (Holomonic Constraints) เมอแรงบงคบมคาสอดคลองกบงานทเกดขน ซงเปนการเคลอนทแบบออยเลอร – ลา กรานซ (Euler-Lagrange Equations) ม 2 วธเปนอยางนอยทจะหาสมการน วธทนาเสนอ ณ ทนอาศยการแทนท แตอยางไรกตามสามารถหาสมการนไดดวยหลกการของฮามนตน เมอระบบประกอบดวย k อนภาค ซงสมพนธกบ krr ,...,1 ถาอนภาคเหลานเคลอนทอสระโดยไมมขอจากดแลว กเปนการงายทจะอธบายการเคลอนทของอนภาคนนๆโดยการเปลยนแปลงโมเมนตมของวตถมคาเทากบแรงภายนอกทมตอสงนน อยางไรกดถาการเคลอนทของอนภาคถกบงคบ แรงทนามาคดจะประกอบดวยแรงภายนอกและแรงบบบงคบ ซงนนกคอยงตองการแรงบงคบเพมอกหนงแรง อธบายงายๆกคอ สมมตวาระบบประกอบดวย 2 อนภาค ซงมาเชอมตอกนดวยเชอกทไรนาหนก ความยาวเทากบ l ดงนน 1r และ 2r เทากบ

121 lrr =− or ( ) ( ) 22121 lrrrr T =−− (2.30)

ถามแรงจากภายนอกมากระทาตอแตละอนภาคแลว อนภาคเหลานจะถกกระทาไมแตเฉพาะจากแรงภายนอกนเทานน ยงรวมไปถงแรงทมาจากเชอกดวยทศทาง 12 rr − ประกอบกบขนาดทเหมาะสม ดงนนเพอทจะหาการเคลอนทของอนภาคทงสองน สามารถทาไดโดย วธแรก

14

คานวณจากแรงภายนอกแตละแรงทมากระทาโดยแรงบงคบทเกดขนอยในลาดบของสมการ วธทสองคานวณจากวธวเคราะหซงไมอาศยแรงบงคบ (ซงไมจาเปนตองรแรงบงคบ) ซงวธทสองนเปนทนยมมากวาวธแรกเพราะวาโดยทวไปแลวตองคานวณหาแรงบงคบทเกดขน จดประสงคของหวขอนคอศกษาวธทสอง เรมตนจากการอธบายคา “Holonomic” คอ แรงบงคบทเกดบนของคาพกด k เปน

krr ,...,1 เมอแรงนเทากนจะเกดสมการ

lirrg k ,...,1,0),...,( 11 == (2.31) ถาสงทเกดขนมลกษณะตรงกนขามกบสงทกลาวมา จะเรยกวา “Nonholonomic” แรงบงคบดงกลาว (2.30) กาหนดโดยการเชอมตอกนของ 2 อนภาค ดวยเชอกทไรนาหนก ซงนนกคอโฮโลโมนกคอนสเทรน สวนตวอยางของนอนโฮโลโมนกคอนสเทรนพจารณาจากการเคลอนทของอนภาคหนงๆ ภายใน หนงความยาวรศมของทรงกลม ρ ทจดศนยกลาง บรเวณจดเรมตนของระบบคาพกดในกรณนคาพกดเวคเตอร r ของอนภาค สมพนธกบแรงบงคบ

ρ≤r (2.32) การเคลอนทของอนภาค จะมแรงบงคบ ดงนนอนภาคยงคงรกษาระยะหางจากพนผววตถทรงกลม แตเมอใดกตามทอนภาคเขาใกลจนสมผสกบพนผววตถทรงกลม มนกเทากบแรงบงคบนนเอง ถาระบบขนอยกบโฮโลโมนกคอนสเทรน l เมอนนใหคานงถงนยามของระบบคอนสเทรนทม l ระดบขนความเสรมากกวาระบบอนคอนสเทรนเลกนอย ในกรณน กอาจเปนไปไดทจะแสดงคา คาพกดของ k อนภาคในเทอม ของ n คาพกดทวไป nqq ,...,1 สวนในความหมายอน เราคาดวาคาพกดของอนภาคทแตกตางกน ขนอยกบชดของแรงบงคบ ซงสามารถแสดงดงลกษณะตอไปน

kiqqrr nii ,...,1),,...,( 1 == (2.33) โดยท nqq ,...,1 เปนอสระจากกน จรงๆแลวแนวคดของคาพกดทวไปถกนามาใชเมออนภาคมปรมาณมากมายไมจากดจานวน ตวอยางเชน วตถคงตวรปแบบแทนทมอนภาคอยมากมาย แตเนองจาก

15

ระยะหางระหวาง 2 อนภาคถกจากดตลอดการเคลอนทของแทนวตถดงกลาว ดงนนเพยงแคคาพกด 6 จดกเพยงพอทเราจะระบคาพกดของอนภาคใดๆ ในแทนวตถขางตนไดอยางสมบรณแบบ โดยเฉพาะอยางยงสามารถใช คาพกด 3 จด เพอบงชตาแหนงจดศนยกลางมวลของแทนวตถ และใชมมออยเลอร 3 มมเพอกาหนดทศทางของแทนวตถ และเพอใหงายตอการอธบายเราจะสมมตใหจานวนของอนภาคถกจากด โดยทวไปคาพกดทวไป กคอตาแหนงทตง หรอมม เปนตน เราสามารถกาหนดความหมายของตวแปรของจดตอสญลกษณ nqq ,...,1 ไวอยางชดเจน เพราะตวแปรของจดตอเหลานจะแสดงสญลกษณเปนชดของคาพกดทวไป สาหรบหนยนตทมขอตอจานวน n ขอ เมอเรากลาวถงเวอชวลดสพเลซเมนต (Virtual Displacements) (การแทนททเกดขนจรง) มนคอชดของ krr δδ ,...,1 ของการแทนททไมสามารถนบได โดยสอดคลองกนกบแรงบงคบ ตวอยางเชน เมอพจารณาถงแรงบงคบอยางเชนในสมการ (2.30) และสมมตให 21,rr ถกกระจายเปน

,, 2211 rrrr δδ ++ ตามลาดบ ดงนนเพอทจะใหสมพนธกบแรงบงคบ เราตองทาใหอยในสมการดงน

222112211 )()( lrrrrrrrr T =−−+−−+ δδδδ (2.34)

จากนนเมอเรากระจายสมการดงกลาวขางตน และถอเอาความจรงทวาคาพกดเดม 21,rr สมพนธกบแรงบงคบ ทาใหเราตดพจนกาลงสองของ 21 , rr δδ จะไดวา

0)()( 2121 =−− rrrr T δδ (2.35) ดงนนตาแหนงทยงยากของสองอนภาคตองสอดคลองกบสมการขางตน เพอวาตาแหนงทยงยากจะไดสมพนธกบแรงบงคบ (2.30) คของเวกเตอร ทไมสามารถวดคาได 21 , rr δδ ซงสมพนธกบ (2.35) นนประกอบดวย ชดของเวอชวลดสพเลซเมนต เหตผลของการใชคาพกดทวไปกเพอทจะหลกเลยงการจดการความสมพนธทซบซอน ดงเชนในสมการ (2.35) และถายงมสมการ (2.33) อยจะพบวาชดของเวอชวลดสพเลซเมนต คอนขางกระชบดงเชน

∑=

=∂∂

=n

jj

j

i kiqqr

r1

1 ,...,1,δδ (2.36)

โดยทเวอชวลดสพเลซเมนต nqq δδ ,...,1 ของคาพกดทวไป คอการไมมแรงบงคบ (ซงนนกทาใหกลายเปน คาพกดทวไป)

16

ตอนไปจะอธบายระบบแรงบงคบในสภาวะสมดล โดยกาหนดใหแตละอนภาคอยในภาวะสมดล จากนนใหแรงสทธของแตละอนภาค เปนศนย ซงกเทากบวา งานททาในแตละชดของ เวอรชอลดสพเลซเมนตเปนศนย ดวยดงนน

∑=

=k

ii

Ti rF

10δ (2.37)

โดยท iF คอ แรงทงหมดตออนภาค i ตามทไดอางไวขางตน แรง iF จงเปนคารวมของสองแรง หนงคอ แรงทกระทาตออนภาคจากภายนอก if และสองคอแรงบงคบ )(a

if โดยสมมตใหงานทงหมดกระทาดวยแรงบงคบซงสมพนธหรอเกยวของกบชดของเวอชวลดสพเลซเมนตใดๆทเปนศนย ดงสมการ

∑=

=k

ii

Tai rf

1

)( 0)( δ (2.38)

สงทกลาวมาขางตนจะถกตองกตอเมอ แรงบงคบระหวางคของอนภาค มงตรงไปตามเสนรศมซงเชอมตอกนระหวางสองอนภาคนนๆ โดยการแทนทสมการ (2.38) เขาไปใน (2.37) จะไดผลลพธดงในสมการ

∑=

=k

ii

Ti rf

10δ (2.39)

ความสมบรณแบบของสมการดงกลาวคอ จะไมมแรงบงคบทไมรคามาเกยวของกบในสมการนเลย แตจะใชเฉพาะแรงภายนอกทรคา สมการนแสดงหลกการของงานทเกดขนจรง ซงกลาวไววา งานททาโดยแรงภายนอกสมพนธกบชดของเวอชวลดสพเลซเมนตทเปนศนย จาไววา หลกการนใชไมไดกบทกงาน แตสมการ (2.38) ตองยงคงอย นนคอแรงบงคบทไมทางาน ฉะนนถากลาวถงหลกการของงานทเกดขนจรงกสามารถวเคราะหกลศาสตรของระบบโดยปราศจากการหาคาแรงบงคบ มนงายทจะพสจนหลกการของงานทเกดขนจรง เมอไหรกตามแรงบงคบระหวางคของอนภาค มงตรงไปตามแรงทเชอมกนระหวางตาแหนงทคาพกดของอนภาคทงสอง โดยเฉพาะอยางยงเมอถกบงคบในรปแบบสมการ (2.30) ซงเปนหลกการของการประยกตใช เมอพจารณาถง แรงบงคบแรงเดยวจากสมการ (2.30) จะตองออกแรงโดยเชอกทไรนาหนก ดงนนแรงตองมงตรงไปตามเสนรศมระหวางอนภาคทงสอง หรออาจกลาววา แรงทกระทาตออนภาคทหนงดวยเชอก จะตองอยในฟอรมของ

17

)( 21

)(1 rrcf a −= (2.40)

สาหรบคาคงตว c (ซงจะเปลยนแปลงเมอ อนภาคมการเคลอนท) ตามกฎของแรงกระทาและแรงปฏกรยา, แรงทกระทาตออนภาคทสองดวยเชอกตองมคาเปนลบ หรอตรงกนขามกบสมการขางตน นนคอ

)( 21)(

2 rrcf a −−= (2.41) ตอนนทราบแลววา งานทกระทาโดยแรงบงคบซงสมพนธกบชดของเวอชวลดสพเลซเมนต คอ

)()()()( 21212)(

21)(

1 rrrrcrfrf TTaTa δδδδ −−=+ (2.42)

แตในสมการ (2.35) แสดงใหเหนชดของเวอชวลดสพเลซเมนต สวนสมการขางบนผลลพธตองเปนศนย ดงนนหลกการของงานทเกดขนจรงซงนาไปใชในระบบใดๆ จะถกบงคบโดยสมการ (2.30) เหตผลเดยวกนสามารถนาไปใชกบระบบทประกอบดวยหลายอนภาค ซงแตละคของอนภาคเชอมกนดวยเชอกทไรนาหนกซงจากดความยาวเชนในกรณของระบบทถกกาหนดไวสาหรบหลายแรงบงคบเหมอนในสมการ (2.30) สวนขอกาหนดทวา การเคลอนทของวตถทมความคงตวสามารถแสดงไดเทากบขอบงคบทวาระยะทางระหวางคของจดใดๆตอวตถยงมคาคงตว แมวาวตถจะมการเคลอนท เหมอนกบการบงคบทไมจากดในแบบสมการ (2.30) ดงนนหลกการของงานทเกดขนจรงถกนาไปใชเมอ ความแขงคงตวเปนเพยงแรงบงคบตอการเคลอนไหว มบางสถานการณทหลกการนไมไดถกนาไปใชเชน เมออยในสนามแมเหลก อยางไรกตาม ในทกเหตการณทกลาวขนน เราสมมตใหหลกการของงานทเกดขนนนใชไดจรง ในสมการท (2.39) เวอชวลดสพเลซเมนต irδ ไมเปนอสระตอกน ดงนนเราไมสามารถสรปไดจากสมการน ซงแตละสมประสทธ if เทากบศนย โดยไมขนตรงตอกน และเพอทจะประยกตเหตผลดงกลาว ตองเปลยนรปเปนคาพกดทวไป และกอนทจะเปลยนรป เราถอวาระบบนนไมจาเปนตองอยในสมดล ซงเรยกระบบดงกลาววา “หลกการของ D’Alembert” ไดกลาวไววา เมอมการใหแรงซงสมมตขนเปนคา ip กระทาตออนภาค i โดยแตละ i ท ip คอ แรงเคลอนทของอนภาค i เมอแตละอนภาคอยในสมดล ดงนนถามการดดแปลงสมการ (2.37) ดวยการแทนท iF โดย ii pF &−

18

เมอนน ผลลพธของสมการจะใชไดจรงสาหรบระบบทเราเลอก และทาใหสามารถตดแรงบงคบกอนทจะใชหลกการของงานทเกดขนจรง ดงแสดงใน

∑ ∑= =

=−k

i

k

ii

Tii

Ti rprf

1 10δδ & (2.43)

สมการขางตนไมไดหมายความถงวา แตละสมประสทธของ irδ เทากบศนย แตมวตประสงคแสดงแตละ irδ ในเทอมทเกยวของกบเวอชวลดสพเลซเมนตของคาพกดทวไป อยางเชนททาในสมการ (2.36) ดงนนงานทเกดขนจรงกระทาโดยแรง if จะได

∑=

=k

ii

Ti rf

1

δ ∑∑∑== =

=∂∂ n

jjjj

k

i

n

j j

iTi qqqr

f11 1

δψδ (2.44)

โดยท

∑= ∂

∂=

k

i j

iTij q

rf

1

ψ (2.45)

ซงถกเรยกวา แรงทวไป ลาดบท j จาไววา jψ ไมจาเปนตองมมตของแรง ขณะเดยวกบท jq กไมตองมมตของความยาว อยางไรกตาม jj qδψ ตองมมตของงานเสมอ จากนนทาใหเราศกษาถงผลรวมลาดบท 2 ของสมการ (2.43) เนองจาก iii rmp &=

∑∑∑ ∑= == = ∂

∂==

k

i

n

jj

j

iTii

k

i

k

ii

Tiii

Ti q

qrrmrrmrp

1 11 1

δδδ &&&&&

(2.46) จากนนเมอนาผลลพธของการดฟเฟอเรนเชยล (Differentiate) มาใช จะเหนไดวา

∂∂

−

∂∂

=∂∂ ∑∑

== j

iTii

k

i j

iTii

k

i j

iTji q

rdtdrm

qr

rmdtd

qr

rm &&&&11

(2.47)

เมอดฟเฟอเรนเชยล สมการ (2.33) ดวยการใชกฎลกโซ (Chain Rule) ทาใหได

19

j

n

j j

iij q

qr

rv && ∑= ∂∂

==1

(2.48)

สงเกตจากสมการขางบนซง

j

i

j

i

qr

qv

&& ∂∂

=∂∂ (2.49)

และ

∑= ∂

∂=

∂∂∂

=

∂∂ n

i j

il

lj

i

j

i

qv

qqqr

qr

dtd

1

2

& (2.50)

โดย สมการ (2.40) เลยนแบบสมการ (2.48) การแทนทของสมการ (2.49) และ (2.50) เขาไปในสมการ (2.47) และให ii vr =& จะได

∂∂

−

∂∂

=∂∂ ∑∑

== j

iTii

k

i j

iTji

k

i j

iTii q

vvm

qv

vmdtd

qr

rm11 &

&& (2.51)

เมอเรากาหนดให พลงงานจลน (พลงงานของการเคลอนท) เปนปรมาณทนบได

iTii

k

ivvmK ∑

=

=1 2

1 (2.52)

ดงนนผลรวมขางตนสามารถแสดงใหกระชบขนดวย

jjj

iTi

k

ii q

KqK

dtd

qr

rm∂∂

−∂∂

=∂∂∑

= &&&

1

(2.53)

การแทนทของ สมการ (2.53) เขาไปในสมการ (2.46) พบวา ผลรวมลาดบท 2 ใน สมการ (2.43) คอ

20

∑ ∑= =

∂∂

−∂∂

=k

i

n

jj

jji

Ti q

qK

qK

dtdrp

1 1δδ

&& (2.54)

และสดทาย เมอรวมสมการ (2.54) และ สมการ (2.34) จะให

∑=

=

−∂∂

−∂∂n

jjj

jj

qqK

qK

dtd

10δψ

& (2.55)

เนองจากเวอชวลดสพเลซเมนต jqδ เปนคาอสระ ดงนนเราสามารถสรปไดวา แตละสมประสทธในสมการ (2.45) เทากบศนย นนกคอ

njqK

qK

dtd

jjj

,...,1, ==∂∂

−∂∂ ψ&

(2.56)

ถาแรงทวไป jψ คอผลรวมของแรงทวไป ทถกกระทาจากภายนอกและแฝงอย ถาหากวา มฟกชน jτ และ )(qV ดงนน

jj

j qV τψ +

∂∂

−= (2.57)

ทาใหสมการ (2.56) เขยนอยในรปของ

jjj qL

qL

dtd τ=

∂∂

−∂∂&

(2.58)

โดยท VKL −= คอ ลากรานซเจยน (Lagrangian) ของระบบฟงกชน V คอ พลงงานศกยของระบบ และฟงกชน K คอพลงงานจลนของระบบ จาไววา สมการ (2.56) และ (2.58) คอ สมการลากรานซเจยน หรอ สมการออยเลอร-ลากรานซของการเคลอนท 2.2.2.5 สมการของการเคลอนท (Equation of Motion) ในทนเราใชสมการออยเลอร-ลากรานซ ทหาไดในหวขอผานมา เพอนามาเปนกรณศกษา เมอกาหนดใหม 2 เงอนไขคอ อนดบแรกพลงงานจลนคอฟงกชนยกกาลงสองของเวคเตอร q& ซงอยในรปแบบของ

21

qqDqqqqdK Tj

n

jiiij &&&& )(

21:)(

21

,== ∑ (2.59)

โดยท nn× “เมตรกความเฉอย” )(qD คอ คาสมมาตรและเปนบวก สาหรบแตละ

nIRq∈ อนดบสองพลงงานศกย )(qVV = ไมขนอยกบคา q& ไดกลาวถงการควบคมหนยนตทสอดคลองกบเงอนไขดงกลาวนไปเรยบรอยแลว สมการออยเลอร-ลากรานซ สาหรบระบบขางตนสามารถหาไดดงน เนองจาก

∑ −=−=ji

jiij qVqqqdVKL,

)()(21

&& (2.60)

จงได

∑=∂∂

ijkj

k

qqdqL

&&

)( (2.61)

และ

∑ ∑ ∂

∂+=

∂∂

i jiji

i

kjjkj

k

qqqd

qqdqL

dtd

,)( &&&&

& (2.62)

∑ ∂∂

−∂

∂=

∂∂

ji kji

k

ij

k qVqq

qd

qL

,21

&& (2.63)

ดงนนสมการออยเลอร-ลากรานซ สามารถเขยนไดเปน

kk

jij ji k

ij

i

kjjkj q

Vqqqd

qd

qqd τ=∂∂

−

∂

∂−

∂

∂+∑ ∑ &&&&

, 21)(

nk ,...,1= (2.64) อาศยการสลบ ลาดบของผลรวมและสมบตการสมมาตรทาใหเราได

jiji ji j

ki

i

kjji

i

kj qqqd

qd

qqqd

&&&&∑ ∑

∂∂

+∂

∂=

∂

∂

, ,21 (2.65)

22

เนองจาก

∑∑

∂

∂−

∂∂

+∂

∂=

∂

∂−

∂

∂

jiji

k

ij

j

ki

i

kj

jiji

k

ij

i

kj qqqd

qd

qd

qqqd

qd

,, 21

21

&&&& (2.66)

ดงนนจะได

∂

∂−

∂∂

+∂

∂=

k

ij

j

ki

i

kjijk q

dqd

qd

c21 (2.67)

หรอทเรยกกนวา “สญลกษณครซโทฟ” สาหรบคาคงตว k เราให jikijk cc = ซงจะชวยลดการคานวณสญลกษณเหลาน สดทายถาให

k

k qV

∂∂

=φ (2.68)

ดงนนเราสามารถเขยนสมการออยเลอร-ลากรานซ ไดเปน

∑ ∑ =++j ji

kkjiijkjkj qqqqcqqd,

,)()()( τφ&&&& nk ,...,1= (2.69)

จากสมการขางตน สามารถแบงไดออกเปน 3 เทอม เทอมแรก เกยวของกบ อนพนธอนดบสองของคาพกดทวไป เทอมทสอง คอ สมการกาลงสองในเทอมของอนพนธอนดบหนงของ q โดยทคาสมประสทธขนอยกบ q จากนนทาใหสามารถจดแบงออกเปน 2 กลมยอย คอเทอมทเกยวของกบผลคณของ 2

iq เรยกวา “เหวยงออกจากศนยกลาง” ในขณะทอกเทอมหนงเกยวของกบผลคณของ ji qq เมอ ji ≠ หรอเรยกวา “Coriolis” สวนเทอมทสามเกยวของกบเฉพาะคา q แตไมรวมอนพนธของ q ซงจะเขาใจงายขนเมอทาการดฟเฟอเรนเชยลพลงงานจลน และทาใหสามารถเขยน (2.69) ใหอยในเมตรกรปแบบดงน

τ=++ )(),()( qgqqqCqqD &&&& (2.70)

23

โดยท thjk, เปนสวนของเมตรก C ),( qq & ถกนยามใหเปน

∑=

=n

iiijkkj qqcc

1

)( &

∑=

∂

∂−

∂∂

+∂

∂=

n

ii

k

ij

j

ki

i

kj qqd

qd

qd

1 21

& (2.71)

จากนนเราหาความสมพนธระหวาง เมตรกความเฉอย )(qD และเมตรก C ),( qq & ทปรากฏอยใน (2.70) ซงเปนความสมพนธขนพนฐานของปญหาทเกดขนกบการผสมการควบคม

บทท 3 วธดาเนนการวจย

ในการวจยครงน ผวจยไดดาเนนการตามขนตอนดงน 1. การกาหนดปญหาหนยนต (State of The Problem Robot) 2. การแกปญหาเชงสญลกษณ (Symbolic Methods) 3. ลาดบขนตอนการทางานของโปรแกรม (Workflow Program) 3.1 การกาหนดปญหาหนยนต (State of the Problem Robot) กาหนดปญหาของหนยนตทมลกษณะและรปแบบทแตกตางกน โดยนามาแกปญหาในรปของโปรแกรมเพอใชเปนตวอยางตงตนและทดสอบโปรแกรม กรณท 1 เปนหนยนตแบบแขนกลทมลกษณะแบนและมขอตอแบบงอ โดยมลกษณะเปนขอตอ 2 ขอตอ โดยใหเฟรมหลกคอ 0000 zyxo เปนจดกาเนด ดงภาพประกอบ 6

ภาพประกอบ 6 แขนกลทมลกษณะเปนขอตอแบบงอ 2 ขอตอ

จากตาราง 1 คา D-H จะมตวแปรเพยงตวเดยวเปนตวแปรของมมคอ θ ดงนนเพอใหงายในการเขยนจงเปน ic ใชสาหรบ iθcos ถาเปน 21 θθ + กใหเปน 12θ และ )cos( 21 θθ + เปน 12c โดยกาหนดให

0000 zyxo

25

−

=

10000100

00

1111

1111

1

sacscasc

A (3.1)

−

=

10000100

00

2222

2222

2

sacscasc

A (3.2)

จากคาทไดจากเมตรกทงสอง จะทาใหสามารถหาตาราง D-H ของหนยนต จากท ia คอ ความยาวของเสนตงฉากระหวาง 1−iz กบ iz เรยกวา Link Length ในกรณนแขนกลท 1 มคาคงทเปน

1l สวนแขนกลท 2 มคาคงทเปน 2l ตามรป id คอ ระยะระหวาง 1−ix กบ ix ตามแนวแกน 1−iz เรยกวา Link Offset ในกรณนมคาเปน 0 ทงสองแขน iα คอ มมระหวาง 1−iz กบ iz วดรอบแกน ix ตามกฎมอขวา เรยกวา Link Twist ในกรณนมคาเปน 0 ทงสองแขน iθ คอ มมระหวาง 1−iz กบ iz วดรอบแกน 1−iz ตามกฎมอขวา เรยกวา Joint Angle ในกรณนมแขนกลท 1 มคาเปลยนแปลงตามเวลาเปน )(1 tq สวนแขนกลท 2 มคาเปลยนแปลงตามเวลาเปน )(2 tq นาไปใสในตารางจะได

ตาราง 1 คาตวแปร D-H ของหนยนตแบบแขนกลทมลกษณะเปนขอตอแบบงอ

Link link offset ( id ) link length ( ia ) link twist ( iα ) joint angle ( iθ ) 1 2

0 0

1l 2l

0 0

)(1 tq )(2 tq

กรณท 2 เปนหนยนตสเตมฟอดร โดยมลกษณะเปนรปทรงกลมทมรปแบบของขอตอแรกเปนแบบหมน ขอตอทสองเปนแบบหมน ขอตอทสามเปนแบบเลอน (Revolute Revolute Prismatic) ดวยการทแตละขอตอเปนทรงกลม หนยนตมระยะตงฉากของขอตอตาแหนงทเปนไหลนนมความซบซอนเลกนอยซงจะคลายกบการปญหาทางจลนศาสตรแบบไปขางหนาและแบบยอนกลบ ดงภาพประกอบ 7

26

ภาพประกอบ 7 กาหนดพกด DH เฟรมสาหรบหนยนตสเตมฟอดร

โดยกาหนดให

−

−

=

100000100000

11

11

1

cssc

A (3.3)

−

=

1000010

0000

2

22

22

2 dcssc

A (3.4)

=

1000100

00100001

33 dA (3.5)

−

−

=

100000100000

44

44

4

cssc

A (3.6)

−

=

100000100000

55

55

5

cssc

A (3.7)

27

−

=

1000100

0000

6

66

66

6 dcssc

A (3.8)

ตาราง 2 คาตวแปร D-H ของหนยนตสเตมฟอดร

Link link offset ( id ) link length ( ia ) link twist ( iα ) joint angle ( iθ ) 1 2 3 4 5 6

0

2d )(td

0 0 0

0 0 0 0 0 0

-90 +90

0 -90 +90

0

)(tθ )(tθ

0 )(tθ )(tθ )(tθ

กรณท 3 เปนหนยนตสกาลาร โดยมลกษณะรปแบบของแขนขอตอแรกเปนแบบหมน ขอตอทสองเปนแบบหมน ขอตอทสามเปนแบบเลอน (RRP) และขอตอมหนงระดบขนความเสร ซงเคลอนทเปนวงรอบแกนแนวตง ขนแรกเปนทตงและตดกบแกนของจดตอ เมอทกแกนของจดตอขนานกนจะทาใหมความอสระบางในตาแหนงเรมตน ดงภาพประกอบ 8

ภาพประกอบ 8 กาหนดพกด DH เฟรมสาหรบหนยนตสกาลาร

โดยกาหนดให

28

−

=

10000100

00

1111

1111

1

sacscasc

A (3.8)

−

=

10000100

00

2222

2222

2

sacscasc

A (3.9)

−−

=

1000110

00100001

33 dA (3.10)

−

=

1000100

0000

4

44

44

4 dcssc

A (3.11)

ตาราง 3 คาตวแปร D-H ของหนยนตสกาลาร

Link link offset ( id ) link length ( ia ) link twist ( iα ) joint angle( iθ ) 1 2 3 4

0 0

)(td 4d

1a 2a

0 0

0 0

180 0

)(tθ )(tθ

0 )(tθ

3.2 การแกปญหาเชงสญลกษณ (Symbolic Methods) ในการแกปญหาทางคณตศาสตรดวยโปรแกรมมวธการมากมายหลายแบบ ซงในแตละแบบจะใชวธการทแตกตางกนซงผลลพธทไดกแตกตางกนตามวธการนน ๆ สวนใหญจะใชการกระทาทเปนตวเลขเพอหาคาตอบของผลลพธทตองการทาใหยงยากและเสยเวลา ในงานวจยนไดเลอกใชวธการแบบเชงสญลกษณ (Symbolic Methods) ซงสามารถหาคาตางๆ เหลานโดยไมตองกาหนดคาทเปนตวเลขหรอจานวนใหเปนคาเชงตวเลขแตจะกาหนดคาตวแปรหรอจานวนใหเปนสญลกษณทเรยกวา Symbolic กอนแลวจงใชฟงกชนหาคาผลลพธทตองการ จะทาใหไดผลลพธทเปนคาตอบทเปนจรงไดสมการหรอคาทยงตดเปนตวแปรทเปนเชงสญลษณและผลลพธทเปนจานวนจรงทงนกเพอความ

29

สะดวกรวดเรวในการหาคาผลลพธทตองการ โดยสามารถนาไปประยกตใชกบโปรแกรมอนๆ หรอนามาแทนคาเพอหาคาตอบทเปนจานวนจรงได อกทงจะทาใหการสรางโปรแกรมและตรวจสอบความถกตองของโปรแกรมทาไดงายมาก ในงานวจยนไดใชวธการแบบเชงสญลกษณ ในรปทเปนแบบเมตรก สมการ ฟงกชน ดงน

จากลกษณะของตารางคา D-H parameter จะเปนเมตรกขนาด 4 คณจานวนของแขนกล ( n ) เปนเมตรกทรบคาจากโจทย โดยสามารถใสคาทเปนทงเชงตวเลขและเชงตวแปรได จะทาใหเปนในรปแบบของเชงสญลกษณไดโดยการสรางเมตรกเตรยมไวทเปนเชงสญลกษณ ทมขนาด 4 x n และมคาเรมตนทศนย จากนนจงทาการรบคาเขามาใสในเมตรกอกครง

])0([):1,4:1( symndh =

คาของมวล กเชนกนสามารถใสคาไดทงเชงตวเลขและเชงตวแปร เพอใหงายขนจงสรางเปนเมตรกไวรบคาและนาไปคานวณตอ

])0([),1( symnm =

หรอเมตรกทไดจากการคานวณในโปรแกรมกอน จงตองไดทาการเตรยมเมตรกทเปนเชงสญลกษณ กอนแลวจงรบคาทไดจากการคานวณมาใสอกครงเชน ในเมตรกจาโคเบยน ทมขนาด 6 x n

])0([),6( symnJ =

ในงานวจยนสวนใหญไดนาเอาฟงกชน Symbolic มาใชรวมกบฟงกชนใน Evalution ซงจะทาใหตวอกษรสามารถทาใหเปนสมการได และนาเอาตวแปรทเปนเชงสญลกษณ มาคานวณตอในสมการนนอยางเชน ในการคานวณหาคาพลงงานศกยของแตละแขนกล

]))''('''0)(''([' nmndGtransposegnVeval ×××−=

โดยม V 0)( dGtransposeg ××−= (m× และ ) ทเปนตวอกษรเพอสรางสมการ ซงในคาตวอกษรนนมคาทเปนเชงสญลกษณ อยเมอนาไปคานวณตอกจะไดคาผลลพธไดเลย

30

3.3 ลาดบขนตอนการทางานของโปรแกรม (Workflow Program)

Input D-H parameter

Input Mass and Moment of Inertia

Input Gravity

Homogeneous Transformations

Homogeneous Transformations

iiiiiiii xaxdzzi RotTransTransRotA αθ=

Jacobian

Moment of Inertia of Mass

Christoffel symbols

kk q

V∂∂

=φ

Euler-Lagrange equation

∑ ∑ =++j ji

kkjiijkjkj qqqqcqqd,

)()()( τφ&&&&

Display Output And Send to File

ภาพประกอบ 9 แสดงลาดบขนตอนการทางานของโปรแกรมแบบยอ

บทท 4 ผลการทดสอบและวเคราะหผลการทดสอบ

ในการวจยศกษาครงน ผวจยนาเสนอผลงาน ดงน 1. ผลการทดสอบ 2. วเคราะหผลการทดสอบ 4.1 ผลการทดสอบ ผลการทดสอบในงานวจยนคอความถกตองของโปรแกรม การทจะหาความถกตองของโปรแกรม กอนอนเราตองทราบถงผลลพธสดทายหรอคาตอบทไดจากกระบวนการคานวณกอน หลงจากนนแลวจงนาเอามาเปรยบเทยบกบคาตอบทไดจากโปรแกรม และการทดสอบผลคาตอบระหวางทโปรแกรมกาลงดาเนนการหรอตรวจคาตอบของสมการบางชวงในโปรแกรมเพอเปนการทดสอบความถกตองทงหมดของโปรแกรม ดงนนจงแบงปญหาเปน 3 กรณใหผลของปญหาทดสอบไมใหตรงจดกนโดยขอแรกหาผลลพธสดทายทเปนสมการการเคลอนท กรณทสองหาเวกเตอร iz ทจะเตรยมไปหาเมตรกจาโคเบยน กรณทสามหาเมตรกจาโคเบยน จากกรณท 1 เปนหนยนตแบบแขนกลทมลกษณะแบนและมขอตอแบบงอ โดยมลกษณะเปนขอตอ 2 ขอตอ โดยใหเฟรมหลกคอ 0000 zyxo เปนจดกาเนด ดงภาพประกอบ 10

ภาพประกอบ 10 แสดงตวแปรทมในแขนกลทมลกษณะเปนขอตอแบบงอ 2 ขอตอ

0000 zyxo

32

จะไดเมตรก T ดงน 1

10 AT =

++−

==

10000100

00

122111212

122111212

2120

slslcsclclsc

AAT

ทสาคญสองคาทใสเขาในทายหลกของ 2T คอคา x และ y สวนประกอบของจด 2o ในเฟรมหลก จะเปน

12211 clclx += 12211 slsly +=

คาทไดคอคาพกดของจดปลายในเฟรมหลก การหมนของสวนทสองของ 2T จะใหสวนกลบของเฟรม 2222 zyxo สมพนธมายงเฟรมหลก เมอขอตอทงสองเหมอนกนคอแบบหมน เมตรกจาโคเบยนในกรณนจะเปน 6x2 ในรปแบบ

−×−×=

10

121020 )()()(

zzoozooz

qJ

แสดงการคานวณได

=000

0o

=

011

11

1 slcl

o

++

=0

12211

12211

2 slslclcl

o

==100

10 zz

33

แสดงการคานวณจากความตองการจะได

+

−−−

=

1100000012212211

12212211

clclclslslsl

J

พจารณาจากภาพประกอบ 10 ให iq เปนมมของขอตอ , im เปนมวลของขอตอ i , il เปนความยาวของขอตอ i , cil เปนระยะจากขอตอกอนหนาถงจดกงกลางมวลของขอตอ i และ iI เปนโมเมนตความเฉอยของขอตอ i จากเงอนไขของจาโคเบยนในการคานวณพลงงานจลน จะได

qJvcvc &11 =

−=

000cos0sin

11

11

1qlql

J c

c

vc

ในทานองเดยวกน

qJvcvc &22 =

++++−+−−

=00

)cos()cos(cos)sin()sin(sin

21221211

21221211

2qqlqqlqlqqlqqlql

J cc

cc

vc

ดวยเหตนสวนการเคลอนยายของพลงงานจลนเปน

{ }qJJmJJmqvvmvvm

cccc vTvv

Tvc

Tcc

Tc &&

2211 21222111 21

21

21

+=+ การหมนของพลงงานจลนทงหมดของระบบเปน

34

qIIqT &&

+

1111

0001

21

21

ขณะนเรมเขาสรปแบบของเมตรกความเฉอย )(qD ตามทมงหมายโดยการแทนคา 2 เมตรกใน (3.18) และ (3.19) ตามลาดบ ดงนน

+++=

22

22121 2211

)(IIIII

JJmJJmqDcccc v

Tvv

Tv

ทาใหไดผลลพธจากการคณและใชกฎของเมตรกเอกลกษณ ของสมการ 1sincos 22 =+ θθ ,

)cos(sinsincoscos βαβαβα −=+ จะได

2122122

212

21111 )cos2( IIqllllmlmd ccc +++++=

22212222112 )cos( Iqlllmdd cc ++==

222222 Ilmd c +=

สามารถคานวณหาสญลกษณครซโทฟ โดยกาหนดใหเปน

021

1

11111 =

∂

∂=

qd

c

hqllmqd

cc c =−=∂∂

== 22122

11211121 sin

21

hqd

qdc =

∂∂

−∂∂

=1

22

2

12221 2

1

hqd

qdc −=

∂∂

−∂∂

=2

11

1

21112 2

1

021

1

22212122 =

∂∂

==qdcc

021

2

22222 =

∂∂

=qdc

ตอมาเปนพลงงานศกยใชผลรวมของทงสองขอตอ สาหรบพลงงานศกยตอละขอตอใชมวลคณความเรงของแรงโนมถวงและความสงของจดกงกลางมวล ดงนน

35

1111 sin qglmV c=

))sin(sin( 2121122 qqlqlgmV c ++= )sin(sin)( 21221121121 qqglmqglmlmVVV cc +++=+=

จากนไปฟงกชน kφ แทนคา

kk q

V∂∂

=φ

จะได

)cos(cos)( 2122112111

1 qqglmqglmlmqV

cc +++=∂∂

=φ

)cos( 21222

2 qqglmqV

c +=∂∂

=φ

สดทายสามารถเขยนสมการการเคลอนทของระบบในรปของสมการ ออยเลอร-ลากรานซ

kk

jij ji k

ij

i

kjjkj q

Vqqqd

qd

qqd τ=∂∂

−

∂

∂−

∂

∂+∑ ∑ &&&&

, 21)(

nk ,...,1= แทนคาตวแปรในสมการและตดเทอมสมการทเปนศนย จะได 11

222212121121121212111 τφ =+++++ qcqqcqqcqdqd &&&&&&&&&

2221112222121 τφ =+++ qcqdqd &&&&&

ถาในกรณทเปนเมตรก ),( qqC & จะไดเปน

−

+=

01

122

qhqhqhqh

C&

&&&

36

กรณท 2 เปนหนยนตสเตมฟอดร โดยมลกษณะเปนรปทรงกลมทมรปแบบของขอตอเปนแบบหมน หมน เลอน (RRP) ดวยการทแตละขอตอเปนทรงกลม หนยนตมระยะตงฉากของขอตอตาแหนงทเปนไหลนนมความซบซอนเลกนอยซงจะคลายกบการปญหาทางจลนศาสตรแบบไปขางหนาและแบบยอนกลบ ดงรป

ภาพประกอบ 11 แสดงตวแปรทมในหนยนตสเตมฟอดร

60T จะได

6160 ...AAT =

=

1000333231

232221

131211

z

y

x

drrrdrrrdrrr

ท

)(])([ 646541652646542111 ccccsscsssscccccr +−−−= )(])([ 646541652646542121 ccccsccssssccccsr ++−−= 65264654231 )( cscsscccsr −−−= )(])([ 646541652646542112 ccscssssscssccccr +−−++−= )(])([ 646541652646542122 ccscscssscsscccsr +−+++−= 65264654232 )( ssccssccsr ++= 54152542113 )( ssscsscccr −+= 54152542123 )( ssccssccsr ++= 5254233 ccscsr +−=

37

)( 5412515421621321 ssssccscccddsdscd x −++−= )( 2155142541621321 sscssccsscddcdssd y ++++=

)( 52452632 sscccddcdz −+=

ม 3 ขอตอทเปนแบบเลอน (Prismatic) และท 543 ooo == ซงเปนผลมาจากขอตอทเปนรปทรงกลมและการกาหนดเฟรม โดยมจดรวมกาเนดท o จะไดรปแบบของจาโคเบยนเปน

−−−−−−−−−×=

54310

6564632161060

0)()()()()(

zzzzzoozoozoozzoozooz

J

ท j

oR เปนสวนทหมนของ joT ดวยเหตนจงเปนสงเดยวทจาเปนดวยการคานวณเมตรก j

oT ดวยการคานวณจาโคเบยน นาออกมาคานวณตามเงอนไขสาหรบหนยนตสเตมฟอดร

−++++−−++−

==)(

)()(

),,(

52452632

2155142541621321

5412515421621321

6

sscccddcsscssccsscddcdssssssccscccddsdsc

dddo Tzyx

+−

===

32

21321

21321

543

dcdcdssdsdsc

ooo

จะไดเวกเตอร iz เปน

=100

0z

−=

01

1

1 cs

z

=

2

21

21

2

csssc

z

38

=

2

21

21

3

csssc

z

+−−−

=

42

41421

41421

4

ssccscscsscc

z

+−+++−

=

52542

5215415421

5215415421

5

ccscscsssscsccscscssssccc

z

กรณท 3 เปนหนยนตสกาลาร โดยมลกษณะรปแบบของแขนขอตอเปนแบบหมน หมน เลอน (RRP) และขอตอมหนงระดบขนความเสร ซงเคลอนทเปนวงรอบแกนแนวตง ขนแรกเปนทตงและตดกบแกนของจดตอ เมอทกแกนของจดตอขนานกนจะทาใหมความอสระบางในตาแหนงเรมตนดงภาพ

ภาพประกอบ 13 แสดงตวแปรทมในหนยนตสกาลาร จากสมการ Forward Kinematic ดงนนจะได

−−+−−−++−+

==

100010000

...43

12211412412412412

12211412412412412

4140 dd

sasaccsssccscacacsscsscc

AAT

39

เมอขอตอ 1,2 และ 4 เปนแบบหมนและขอตอ 3 เปนแบบเลอน และตงแต 34 oo − ขนานตาม 3z รปแบบของจาโคเบยนจะได

−×−×=

312

2141040

00)()(zzz

zoozoozJ

แสดงการคานวณได

=

011

11

1 saca

o

++

=0

12211

12211

2 sasacaca

o

−++

=

43

12211

12211

4

ddsasacaca

o

ในทานองเดยวกน kzz == 10 และ kzz −== 32 ดงนนจาโคเบยนของหนยนตสกาลารคอ

−

−−

−−−

=

10110000000001000000

12212211

12212211

cacacacasasa

J

4.2 วเคราะหผลการทดสอบ จากคาตอบของปญหาทง 3 กรณทได เมอนาไปเปรยบเทยบกบคาตอบจากโปรแกรมแลวตรงกน จงวเคราะหไดวาโปรแกรมถกตอง สวนเรองของความยากงายในการใชงานโปรแกรม ขนอยกบผใชมประสบการณการใช MATLAB มากนอยและมความรเรองตวแปรสมการการเคลอนหนยนตเพยงใด ซงในโปรแกรมไดมเมนสาหรบชวยเหลอในสวนของตวแปรของสมการ

บทท 5 สรปและวจารณ

5.1 สรปและวจารณผลการทดสอบ จากปญหาเปน 3 กรณใหผลของปญหาทดสอบไมใหตรงจดกนโดยกรณแรกเปนเปนหนยนตแบบแขนกลทมลกษณะแบนและมขอตอแบบงอ โดยมลกษณะเปนขอตอ 2 ขอตอ จะทาการหาผลลพธสดทายทเปนสมการการเคลอนท จากผลการทดสอบปรากฏวาไดสมการทคานวณคอ

11222212121121121212111 τφ =+++++ qcqqcqqcqdqd &&&&&&&&&

ซงตรงกบทไดจากโปรแกรมจากนนทาการทดสอบกรณท 2 เปนหนยนตสเตมฟอดร โดยมลกษณะเปนรปทรงกลมทมรปแบบของขอตอเปนแบบหมน หมน เลอน (RRP) ดวยการทแตละขอตอเปนทรงกลม หนยนตมระยะตงฉากของขอตอตาแหนงทเปนไหลนนมความซบซอนเลกนอย จะทาการหาเวกเตอร iz ทจะเตรยมไปหาเมตรกจาโคเบยน จากผลการทดสอบปรากฏวาไดสมการทคานวณคา iz ตรงกบ

ทไดจากโปรแกรมจากนนทาการทดสอบกรณท 3 เปนหนยนตสกาลาร โดยมลกษณะรปแบบของแขนขอตอเปนแบบหมน หมน เลอน (RRP) และขอตอมหนงระดบขนความเสร ซงเคลอนทเปนวงรอบแกนแนวตง ดวยการหาเมตรกจาโคเบยน จากผลการทดสอบปรากฏวาไดสมการทคานวณคอ

−

−−

−−−

=

10110000000001000000

12212211

12212211

cacacacasasa

J

ซงตรงกบทไดจากโปรแกรม จากการทดสอบทง 3 กรณจงสรปไดวาโปรแกรมสามารถใหผลลพธคาตอบของสมการการเคลอนของหนยนตไดอยางถกตอง โดยทวไปการสรางโปรแกรมจะมหลกในการทดสอบหลายรปแบบขนอยกบลกษณะของการใชงานในโปรแกรมนนๆ ในงานวจยนจะเลอกหลกในการทดสอบโปรแกรม 2 หลกคอ คานวณถกตองและใชงานงาย สะดวก จากผลการทดสอบทาใหทราบไดวาวธการหาสมการการเคลอนทของหนยนตดวยโปรแกรมนนมความถกตอง เมอแสดงใน โปรแกรม แตเมอผลทไดออกมาใหรปแบบของ

41

Microsoft Office Word หรอโปรแกรมอน จะไมสามารถทาใหอยในรปของสมการทเขาใจไดงาย ดงนนจงตองมคมอการใชงานโปรแกรมและตวชวยเหลอสาหรบอธบายคาความหมายของตวแปรตางๆ ในสมการและอธบายทมาการหาคาตวแปรนนๆ เพอไมใหผใชใสคาผดพลาดและไดผลคาตอบทไมถกตอง ในสวนการใชงานจากการออกแบบหนาจอททาใหผใชโปรแกรมสะดวกและงาย โดยการออกแบบใหแสดงผลคาตอบทนททหนาจอนนเลยเพอตรวจทานคาตอบและงายตอการปรบเปลยนคาทปอนใหในตอนแรก อกสวนของการออกแบบโปรแกรมใหสะดวก โดยการนารปภาพมาประกอบใหผใชเปนผเลอกเงอนไขแลวโปรแกรมจะแสดงรปภาพตามเงอนไขทเลอกและใสคาตวแปรตามรปภาพประกอบ 5.2 ขอเสนอแนะ 5.2.1 ขอเสนอแนะทวไป การทางานวจยครงนไดดาเนนการหาโมเมนตความเฉอยของแขนกลทเปนรปทรงเรขาคณตอยางงาย ซงจะถกจากดใหสามารถเลอกใชไดแตเฉพาะในโปรแกรมทกาหนดให ไมสามารถเพมเตม หรอแกไขไดจงทาใหบางปญหาไมสามารถตอบโจทยได 5.2.2 ขอเสนอแนะงานวจยครงตอไป ในการพฒนาครงตอไปอาจทาใหสามารถหาโมเมนตความเฉอยของแขนกลทเปนรปทรงใดๆ กไดขนอยกบผใชกาหนด ซงอาจสามารถรบคาขอมลทเปนภาพจากแบบหรอภาพ 3 มตจากโปรแกรมทใชสาหรบออกแบบ หรอเพมเตมโปรแกรมในเรองของการควบคมแรงของหนยนตในการเคลอนท จะทาใหหนยนตเคลอนทไดอยางมประสทธภาพมากขนจากเดมอก

บรรณานกรม

บรรณานกรม มนส สงวรศลป; และ วรรตน ภทรอมรกล. (2543). คมอ MATLAB ฉบบสมบรณ. กรงเทพฯ:

สานกพมพอนโฟเพรส. Spong, MW.; & Vidyasagar, M. (1986). Robot Dynamics and Control, John Wiley and Sons. Tarek Sobh, Sarosh Patel and Bei Wang (2005) .Web Enabled Robot Design and Dynamic

Control Simulation Software Solutions From Task Points Description. School of Engineering University of Bridgeport.

P.I. Corke. (1996). A Robotics Toolbox for MATLAB. IEEE Robotics and Automation Magazine. Volume 3(1), March 1996 P. 24-32

Mark, WS. (1989). Robot Dynamics and Control. University of Illinois at Urbana-Champaign.

ภาคผนวก

อภธานศพท

71

อภธานศพท

Holomonic Constraints แรงบงคบทเกดขน Homogeneous Matrix โฮโมจเนยสเมตรก Jacobian จาโคเบยน Jacobian Matrix จาโคเบยนเมตรก Lagrange Method วธของลากรางจ Lagrange Multipliers ตวคณลากรางจ Nonholonomic Constraints แรงทเกดขนตรงขามกบแรงบงคบ Numerical Method วธเชงตวเลข Optimum คาทเหมาะสมทสด Prismatic Joint จดตอแบบเลอน Revolute Joint จดตอแบบหมน Rotation Matrix เมตรกทไดจากการทวตถหมนรอบแกนใด ๆ Symbolic Method วธเชงสญลกษณ Transformation Matrix เมตรกทไดจากการเปลยนรปของวตถ Virtual Displacements การเคลอนททเกดขนจรง

72

คาอธบายสญลกษณ

iA คอโฮโมจเนยเมตรก (Homogeneous Matrix) D คอเมตรกความเฉอย if คอแรงทกระทาตออนภาคจากภายนอก )(a

if คอแรงบงคบ iF คอแรงทงหมดตออนภาค i iI คอโมเมนตความเฉอยของขอตอ i iJ คอจาโคเบยนเมตรก (Jacobian Matrix) K คอพลงงานจลน L คอลากรานซเจยน (Lagrangian) im คอมวลของขอตอ i ip คอแรงเคลอนทของอนภาค i nR0 คอเมตรกทไดจากการทวตถหมนรอบแกนใด ๆ (Rotation Matrix) j

iT คอทรานฟอรมเมชนเมตรก (Transformation Matrix) V คอพลงงานศกย irδ คอการเคลอนททเกดขนจรง (Virtual Displacements) jψ คอผลรวมของแรงทวไปทถกกระทาจากภายนอกและแรงทแฝงอยภายใน kφ คอสญลกษณครซโทฟ (Christoffel Symbols)

ประวตยอผวจย

74

ประวตยอผวจย

ชอ ชอสกล นายวรช สรทรพยไพบลย วนเดอนปเกด 10 สงหาคม 2522 สถานทเกด กรงเทพมหานคร สถานทอยปจจบน 56/113 ถ.พหลโยธน สายไหม กทม. 10220 ประวตการศกษา

พ.ศ. 2537 มธยมศกษาตอนตน จากโรงเรยนฤทธยะวรรณาลย กรงเทพมหานคร

พ.ศ. 2538 มธยมศกษาตอนปลาย จากโรงเรยนดอนเมองทหารอากาศบารง กรงเทพมหานคร

พ.ศ. 2542 อดมศกษาประกาศณยบตรวชาชพชนสง จากสถาบนเทคโนโลยราชมงคล วทยาเขตนนทบร จ.นนทบร

พ.ศ. 2546 อดมศกษาอตสาหกรรมศาสตรบณฑต (อส.บ. อตสาหการ) จากสถาบนเทคโนโลยราชมงคล วทยาเขตนนทบร จ.นนทบร

พ.ศ. 2551 บณทตศกษาวศวกรรมศาสตรมหาบณฑต (วศ.ม. เครองกล) จากมหาวทยาลยศรนครนทรวโรฒ