design and development of an exoskeleton robot for...
TRANSCRIPT
รายงานการวจย เรอง
การออกแบบและพฒนาชดโครงหนยนตชวยเดนส าหรบ
ผทพพลภาพครงทอนลาง
DESIGN AND DEVELOPMENT OF AN EXOSKELETON ROBOT FOR LOWER LIMB ASSISTANCE
วนายทธ แสนเงน
งานวจยน ไดรบทนอดหนนการวจยจากมหาวทยาลยศรปทม
ปการศกษา 2557
กตตกรรมประกาศ
ขอขอบพระคณ รองศาสตราจารย ดร. มนกจ พานชกล ทปรกษางานวจย ทใหค าแนะน าและดแลเอาใจใสตลอดระยะเวลาการด าเนนงาน ตลอดจนใหความชวยเหลอในการแกไขปญหาตางๆทเกดขน ท าใหงานวจยสงประดษฐส าเรจตามวตถประสงค
ขอบคณคณาจารย คณะวศวกรรมศาสตร ทคอยใหขอเสนอแนะและค าแนะน า ในการสรางชนงานและการประกอบชนงาน ใหสามารถท างานไดอยางสมบรณ และนกศกษาวศวกรรมเมคคาทรอนกส ทชวยในการทดสอบการท างานตลอดระยะเวลาการด าเนนงานวจย
ขอขอบคณ ศนยสงเสรมและพฒนางานวจย มหาวทยาลยศรปทม ทคอยใหค าปรกษา ค าแนะน า ตลอดจนสนบสนนในการด าเนนงานตางๆ ตลอดระยะเวลาด าเนนการ
งานวจยนประสบความส าเรจลลวงได ดวยการสนบสนนจากมหาวทยาลยศรปทม ตลอดระยะเวลาด าเนนงาน ทงสถานท เครองมออปกรณ และการสนบสนนงบประมาณงานวจยแบบเตมจ านวน
วนายทธ แสนเงน ผวจย
มนาคม 2560
หวขอวจย : การออกแบบและพฒนาชดโครงหนยนตชวยเดนส าหรบผทพพลภาพครงทอนลาง ผวจย : นายวนายทธ แสนเงน หนวยงาน : คณะวศวกรรมศาสตร มหาวทยาลยศรปทม ปทพมพ : พ.ศ. 2560
บทคดยอ
ชดตนแบบโครงหนยนตชวยเดนส าหรบผทพพลภาพครงทอนลาง วตถประสงคหลกไดออกแบบส าหรบผทไรความรสกครงทอนลาง ใหสามารถเคลอนทไดตามรปแบบการเคลอนไหวทไดบนทกไวจากคนปกต ประกอบไปดวยระบบควบคมสามฟงกชนการท างานคอ การลกนงในลกษณะทาทางนงบนเกาอ การกาวเดนในลกษณะทาทางการยน และการเดนในลกษณะการเดนตอเนอง สามารถสงการดวยสวทชจากไมเทาค ายน โดยชดหนยนตมน าหนกประมาณ 25 กโลกรม ปฏบตงานไดไมต ากวา 180 นาท สามารถรองรบผสวมใสน าหนกไมเกน 90 กโลกรม สวนสง 150-180 เซนตเมตร ความเรวสงสดเฉลยไมเกน 0.25 เมตรตอวนาท ภายใตระบบควบคมสมองกลฝงตวแบบแยกสวน แบงเปนตวประมวลผลหลก ส าหรบประมวลผลรปแบบการเดน (Gait pattern) และตวประมวลผลรองจ านวน 4 ชด ส าหรบควบคมสวนสะโพกและขอเขาทงสองขาง โดยระบบควบคมต าแหนงองศาของการเคลอนท ดวยตวควบคมพไอด โดยระบบสอสารขอมลระหวางตวประมวลทงหมดผานการสอสารแบบ RS485 ในการทดลองท าการอานขอมลต าแหนงการเคลอนไหวจากคนปกต (เพศชาย สง 173 เซนตเมตร น าหนก 73 กโลกรม) มระยะการกาวเดนเฉลย 25 เซนตเมตรตอการกาวเดน ความเรวเฉลย 0.15 เมตรตอวนาท โดยระบบควบคมหลกประมวลผลต าแหนงการเคลอนไหวและสงขอมลต าแหนงไปยงตวควบคมยอย ใหควบคมต าแหนงองศาการเคลอนทของสวนสะโพกและขอเขา โดยสามารถควบคมการต าแหนงไดตรงตามตองการ ซงมคาองศาผดพลาดของต าแหนงสวนของสะโพกเฉลยเทากบ 8 องศา และคาองศาผดพลาดของต าแหนงสวนของสะโพกเฉลยเทากบ 12 องศา
ค ำส ำคญ : ชดโครงหนยนตแบบยดตด รปแบบกำรเคลอนไหวทอนลำง หนยนตชวยเดน ระบบชวยผทพพลภำพครงทอนลำง ระบบควบคมสมองกลฝงตวแบบแยกสวน
Research Title : Design and Development of An Exoskeleton Robot for
Lower Limb Assistance
Name of Researcher : Mr.Wanayuth Sanngoen
Name of Institution : School of Engineering, Sripatum University
Year of Publication : B. E. 2560
ABSTRACT
The prototype of an exoskeleton robot for lower limb assistance is developed
for serving who is paralysis lower limb or muscle weak that assist to step walk and sit
up/down. The author is called WAR (Walking Assist Robot). In this research, the gait
pattern data is archived from the gait normal by human. Three function of step walk are
proposed in this research. First, sit up/down function with sit on chair, second, step
walk with single phase walking, third, walk pattern with continue walking that function
can operate to control by the switch button on the cane. The WAR specification is
weight about 25 kg. and the life time of robot operation at least 180 min. It can support
the user which is maximum weight 90 kg. and height in range of 150 cm. to 180 cm.
The maximum velocity is 0.25 m/sec. The embedded system is applied in order to
control with a master controller and four slave controllers, the angle position of gait
pattern is computed by a master controller and the slave controller is computed to
control the position of hip joint and knee joint of both legs. The data communication
between them is performed by using RS485 serial communication. In the experiment,
the motion recording from a gait normal person (Male, height 173 cm. weight 73 kg.)
is obtained the average distance of step walking is 0.25 meter per a single phase the
average velocity is 0.15 m/sec. The distributed control, the PID control is performed to
control the position of angle for each joint. As the results, it can achieved to control the
position of angle for hip joint and knee joint. The average of position error is 8 degree
at hip joint and 12 degree at knee joint.
Keywords: Exoskeleton robot, Gait pattern, Walking assist robot, Lower limb
assistance, Robotics, Embedded system.
สารบญ
บทท หนา 1 บทน า............................................................................................................................ 1 ความเปนมาและความส าคญของปญหา ........................................................................ 1 วตถประสงคของการวจย ...................................................................... ......................... 3 ขอบเขตการวจย ................................................................................. ........................... 3 นยามศพท .......................................................................... ........................................... 3 ประโยชนและผลทคาดวาจะไดรบ..................................................................................
4
2 ทฤษฏทเกยวของ ......................................................................................................... 5 สาเหตของการเกดอมพาตและอมพาตครงทอนลาง........................................................ 5 ทฤษฏเกยวกบการเดน .................................................................................................. 7 ชดหนยนตชวยเดน………................................................................................ ................ 13 จลนศาสตรหนยนต ...................................................................................... .................
ทฤษฎการควบคมมอเตอรดวย พไอดคอนโทรล…………………………………….……………...
15 23
3 การออกแบบหนยนตชวยเดนส าหรบผปวยอมพาตครงลาง.......................................... 25 การออกแบบระบบ ............................................................... ....................................... 25 รปแบบจลศาสตรของขาหนยนต ............................................................................... 27 การออกแบบโครงสราง .......................................................................... ...................... 29 การออกแบบระบบควบคม ................................................................... .......................
อปกรณขบเคลอนตนก าลง ................................................................... ........................ วงจรไมโครคอนโทรลเลอร ................................................................... ........................ วงจรขบเคลอนมอเตอร .................................................................. .............................. การออกแบบผงการท างานของโปรแกรม......................................................................
31 33 34 36 37
4 ผลการวเคราะหขอมล ................................................................................................. การตดตงและการใชงาน.................................................................. ............................. ผลการทดสอบ………………………………………………………………………………………...………. ระบบควบคมพไอด……………………………………………………………………………..….………… การเกบขอมลส าหรบการลกและนง…………………………………………………………...……….
40 40 42 42 46
ระบบควบคมการลกและนง………………………………………………………………….….….……. การเกบขอมลการกาวเดนครงละกาว………………………………………………….…..……….… ระบบควบคมการกาวเดนครงละกาว…………………………………………….…………….……… การเกบขอมลรปแบบการเดน…………………………………………………….……………….…….. ระบบควบคมรปแบบการกาวเดน……………………………………………….…………………..…. สรปผลการทดลอง…………………………………………………………….………………………….….
47 51 47 56 57 61
5 สรป อภปรายผล และขอเสนอแนะ ............................................................................. 62 สรปผลการวจย ............................................................................... ............................. 62 ปญหาและอปสรรค....................................................................................................... 62 ขอเสนอแนะ ..................................................................................................... ...........
แนวทางการพฒนา……………………………………………………………………………………………. ผลงานการวจย………………………………………………………………………………………………….
63 63 65
บรรณานกรม ............................................................................................................... 66 ภาคผนวก .................................................................................................................... 67 ภาคผนวก ก โปรแกรมคอมพวเตอร.................................................................... ........... 67 ภาคผนวก ข ใบประกาศนยบตร รางวลผลงานการวจย ................................................ 79 ประวตยอผวจย ........................................................................................................... 84
สารบญ(ตอ) บทท หนา
สารบญตาราง ตารางท หนา 2.1 การท างานของขอตางๆและกลามเนอในขณะเดน 10 2.2 การก าหนดพารามเตอรของ Denavit-Hartenberg 20 4.1 คาความผดพลาดสงสดของต าแหนงมมองศา 61
สารบญภาพประกอบ ภาพประกอบ หนา
2.1 สวนขอตอกระดกสนหลง 7 2.2 ขนตอนการเดนปกตของมนษย (Gait Pattern) 7 2.3 การวดความยาวของขา 12 2.4 ขอมลการเปรยบเทยบนาหนกขา 13 2.5 ชดตนกาลงชวยทนแรง BLEEX (a), และ HAL-5 (b) 14 2.6 องคประกอบการเคลอนทของจลนศาสตรหนยนต 15 2.7 จลนศาตรหนยนตสองแกน 16 2.8 มมภายในจลนศาสตรหนยนตตามของกฏโคไซน 17 2.9 การกาหนดพารามเตอรของจดหมนและกานโยง 19 2.10 จลนศาสตรทางกลของหนยนตแบบสองแกน 20 2.11 บลอกไดอะแกรมของการควบคมพไอด 23 3.1 การทางานของระบบหนยนตชวยเดนครงทอนลาง 25 3.2 การทางานของระบบควบคมสมองกลแบบแยกสวน 26 3.3 จดหมนสาหรบครงทอนลาง สะโพก, ขอเขาและขอเทา (ก) จลศาสตรทางกล (ข) 27 3.4 การออกแบบชดโครงหนยนตชวยเดนครงทอนลาง 30 3.5 โฟรซารทการทางานของระบบควบคมรปแบบการกาวเดน 31 3.6 หลกการทางานของหนวยประมวลผลยอย 32 3.7 โฟรซารทการทางานของหนวยประมวลผลยอย 33 3.8 ชดสงกาลงมอเตอรกระแสตรง 34 3.9 3.10 3.11 3.12 4.1 4.2 4.3 4.4 4.5 4.6
วงจรไมโครคอนโทรลเลอรระบบควบคมสมองกล
วงจรขบเคลอนมอเตอรกระแสตรง
แผนผงการทางานของโปรแกรมหลก
แผนผงการทางานของโปรแกรมยอยสถานะหนยนต ชดตนแบบโครงหนยนตชวยเดนฯ พรอมอปกรณไมเทาคายน
วธการใชงานชดโครงหนยนตชวยเดนฯ สาหรบสถานะการนง การยนและการเดน
ระบบควบคมแบบพไอด แสดงผลลพธการควบคมตาแหนงดวยระบบควบคมพไอด Kp = 1, Ki = 1, Kd = 1
แสดงคาตาแหนงของความผดพลาดระบบควบคมพไอด Kp = 1, Ki = 1, Kd = 1
แสดงผลลพธการควบคมตาแหนงดวยระบบควบคมพไอด Kp = 30, Ki = 10, Kd = 2
35 36 38 39 40 41 42 43 43 44
สารบญภาพประกอบ ภาพประกอบ หนา
4.7 4.8 4.9 4.10 4.11 4.12 4.13 4.14 4.15 4.16 4.17 4.18 4.19 4.20 4.21 4.22 4.23 4.24 4.25 4.26 4.27 4.28 4.29 4.30 4.31 4.32 4.33 4.34 4.35
แสดงคาตาแหนงของความผดพลาดระบบควบคมพไอด Kp = 30, Ki = 10, Kd = 2
แสดงผลลพธการควบคมตาแหนงดวยระบบควบคมพไอด Kp = 50, Ki = 30, Kd = 1
แสดงคาตาแหนงของความผดพลาดระบบควบคมพไอด Kp = 50, Ki = 30, Kd = 1
ขอมลการเคลอนไหวจากสถานะนงถงสถานะยนและสถานะยนถงสถานะนง
ขอมลตาแหนงมมองศาของการเคลอนไหวสถานะการลกนงรวม 5 รอบ การควบคมตาแหนงการลงนงของสะโพกดานซาย คาความผดพลาดของระบบควบคมสาหรบมมองศาของการลงนง สะโพกดานซาย การควบคมตาแหนงการลงนงของขอเขาดานซาย
คาความผดพลาดของระบบควบคมสาหรบมมองศาของการลงนง ขอเขาดานซาย
การควบคมตาแหนงการลงนงของสะโพกดานขวา
คาความผดพลาดของระบบควบคมสาหรบมมองศาของการลงนง สะโพกดานขวา
การควบคมตาแหนงการลงนงของขอเขาดานขวา
คาความผดพลาดของระบบควบคมสาหรบมมองศาของการลงนง ขอเขาดานขวา
การบนทกขอมลการเคลอนไหวสาหรบการกาวเดนครงละหนงกาว ขอมลตาแหนงมมองศาของการกาวเดนครงละหนงกาวตดตอกนรวม 5 รอบ
การควบคมตาแหนงการกาวเดนของสะโพกดานซาย
คาความผดพลาดของระบบควบคมสาหรบมมองศาของการกาวเดน สะโพกดานซาย
การควบคมตาแหนงการกาวเดนของขอเขาดานซาย
คาความผดพลาดของระบบควบคมสาหรบมมองศาของการกาวเดน ขอเขาดานซาย
การควบคมตาแหนงการกาวเดนของสะโพกดานขวา
คาความผดพลาดของระบบควบคมสาหรบมมองศาของการกาวเดน สะโพกดานซาย
การควบคมตาแหนงการกาวเดนของสะโพกดานขวา
คาความผดพลาดของระบบควบคมสาหรบมมองศาของการกาวเดน สะโพกดานขวา
การบนทกขอมลการเดนของผสวมใส ขอมลตาแหนงมมองศาการเดนของทอนลางจานวน 5 รอบ
การควบคมตาแหนงตามรปแบบการเดนของสะโพกดานซาย
คาความผดพลาดสาหรบมมองศาการเดนสวนสะโพกดานซาย
การควบคมตาแหนงตามรปแบบการเดนของขอเขาดานซาย
คาความผดพลาดของมมองศาการเดนสวนขอเขาดานซาย
44 45 45 46 46 47 47 48 48 49 49 50 50 51 51 52 52 53 53 54 54 55 55 56 56 57 57 58 58
(ตอ)
สารบญภาพประกอบ ภาพประกอบ หนา
4.36 4.37 4.38 4.39
การควบคมตาแหนงตามรปแบบการเดนของสะโพกดานขวา
คาความผดพลาดของมมองศาการเดนสวนสะโพกดานขวา
การควบคมตาแหนงตามรปแบบการเดนของขอเขาดานขวา
คาความผดพลาดของมมองศาการเดนสวนขอเขาดานขวา
59 59 60 60
(ตอ)
1
บทท 1 บทน ำ
1.1 ควำมเปนมำและควำมส ำคญของปญหำ
ขอมลการส ารวจประชากรคนพการในประเทศไทย จากสถตขอมลคนพการทมบตรประจ าตว
คนพการจ าแนก เปนระดบภมภาค จ านวน 1,650,379 คน กรงเทพมหานคร จ านวน 68,976 คน
รวมทงสน 1,719,355 คน โดยจ าแนกเปนหลายประเภทแบงเปน ทางการเคลอนไหวหรอทางรางกาย
จ านวน 825,799 คน ทางการมองเหน จ านวน 186,389 คน ทางการไดยน จ านวน 287,211 คน
ทางจตใจหรอพฤตกรรม จ านวน 116,006 คน ทางสตปญญา จ านวน 124,322 คน ทางการเรยนร
จ านวน 6,296 คน ทางออทสตก จ านวน 7,285 คน พการมากกวา 1 ประเภท จ านวน 122,465 คน
และไมระบ จ านวน 43,582 คน ซงผพการทางการเคลอนไหวหรอรางกาย ถอวาเปนจ านวนผพการท
มจ านวนสงสดของประเทศ (ทมา : ขอมลประมวลผลจากฐานขอมลทะเบยนกลางคนพการ กรม
สงเสรมและพฒนาคณภาพชวตคนพการ ขอมล ณ. วนท 3 เดอนสงหาคม พ.ศ. 2558) ทงนผวจยม
ความตระหนกในผพการทกๆดาน โดยงานวจยน มงเนนผพการทางการเคลอนไหวหรอทางรางกาย
เปนหลก โดยหากสามารถท าใหผพการทางการเคลอนไหวสามารถกลบมาด ารงคชวตขนพนฐานได ก
จะเปนคณประโยชนและมคณคายง โดยการน าเทคโนโลยหนยนตผนวกกบการแพทย เพอเปนการ
สรางและพฒนาอปกรณทางการแพทย ใหมขดความสามารถทนเทยมกบนานาประเทศ และสามารถ
เขามาชวยในการสงเสรมและพฒนาคณภาพชวตของผพการไมมากกนอย
เทคโนโลยหนยนต มการพฒนาอยางรวดเรว และมแนวโนวในการพฒนาใหมประสทธภาพ
สง เพมความแมนย า ถกตองและความสามารถในการท างานรวดเรวมากยงขน ในปจจบนเทคโนโลย
หนยนตทางการแพทย ไปประยกตใชงานในดานทางการแพทยอยางแพรหลาย เชน หนยนตชวย
ผาตด หนยนตดแลผปวย หนยนตชวยกายภาพบ าบด หนยนตขนยายขยะสารพษ เปนตน ซงการ
ท างานของหนยนตจะมความแมนย าสง ทงยงสามารถท างานไดอยางตอเนองและประสทธภาพ
มากกวามนษย ซงวตถประสงคหลกคอการชวยเหลอและท างานทดแทนมนษยในงานท เสยงอนตราย
ตอชวต
2
เทคโนโลยหนยนตทางการแพทยนน เปนการน าองคความรทางดานวศวกรรมศาสตร มา
ผนวกกบความรทางการแพทยศาสตรรวมเขาดวยกน ท าใหเกดความรวทยาการหนยนตทาง
การแพทย ทงยงสงประดษฐส าหรบเครองมอและอปกรณทางการแพทยอกดวย การพฒนาเทคโนโลย
หนยนตทางการแพทย ส าหรบทางดานกายภาพบ าบด ทางดานฟนฟทางกายภาพและจตใจ ทางดาน
ผทพพลภาพรางกาย มการน าเทคโนโลยหนยนตไปประยกตใชงานอยางมาก เพอใหกลมคนเหลานน
มขดความสามารถในการท างานรวมกบมนษยอกครง ทงยงสามารถชวยเหลอตนเองในการด ารงคชวต
ของสภาวะโลกทเปลยนแปลง
ในปจจบนเทคโนโลยหนยนตประสานกบความรทางการแพทย กอใหเกดการพฒนาสงประดษฐทางการแพทย อปกรณชวยเดนทง แบบบงคบมอ แบบกงอตโนมต และแบบอตโนมต ซงสามารถเปลยนแปลงตอกลมคนทมปญหาบกพรองทางรางกาย ใหมการพฒนาคณภาพชวตทดขนและสามารถด ารงคชวตดวยตนเองไดตอไปผปวยทไมสามารถควบคมการเคลอนไหวของอวยวะครงลางได ทางการแพทยเรยกอาการนวา “อมพาตครงลาง” มจ านวนเพมขนมากขนทกวน ไมวาสาเหตจะเกดจากความผดปกตทางรางกายกอใหเกดโรคตางๆ เชน เสนเลอดในสมองตบหรอแตก (Stroke) หรอจะเปนจากการประสบอบต เหต ทสงผลกระทบตอระบบประสาทควบคมการเคลอนไหว (Motor Neuron) การทผปวยไมสามารถเคลอนไหวรางกายครงลางไดเองนน สงผลใหไมสามารถชวยเหลอตนเอง ในการใชชวตประจ าวนได ซงในปจจบนกมสงประดษฐทชวยเหลอใหผปวยอมพาตครงลางใหเคลอนทได คอ รถเขน (Wheelchair) แตการเคลอนทดวยรถเขนกยงมขอจ ากดทไมสามารถเคลอนทผานไปได เชน พนตางระดบ , บนได และพนทขรขระ เปนตน ผวจยจงเกดความคดวา ถาหากมเทคโนโลยใหมๆ ทท าใหผปวยอมพาตครงลางสามารถกลบเคลอนไหวได เชน ยนและกาวเดน นอกจากจะท าใหผปวยสามารถเคลอนไหวผานอปสรรคตางๆ ได และยงสงผลตอสภาวะจตใจของผปวย ใหมสขภาพจตทดยงขน สงผลใหมก าลงใจในการด ารงคชวตตอไป โดยผวจยมแนวคดทจะน าเอาความรทางวศวกรรมเมคคาทรอนกส (Mechatronics), กายวภาคศาสตร (Anatomy) และรปแบบการเดน (Gait Pattern) น าองคความรมาบรณาการ เพอออกแบบชดหนยนตชวยเดนส าหรบผปวยอมพาตครงลางทสามารถท างานดวยทาทางการเดนทถกตองตามหลกการของรปแบบการเดน (Gait Pattern) ท าใหผปวยอมพาตครงลางกลบมาด ารงคชวตประจ าวนได
3
1.2 วตถประสงคของกำรวจย
1. เพอศกษาทาทางการกาวเดน การเปลยนสถานะนงเปนสถานะยน และจากสถานะยนเปนสถานะนง (บนเกาอขนาดมาตรฐาน) ส าหรบกายภาพคนปกต
2. เพอออกแบบระบบกลไกทสามารถชวยเหลอใหผทมปญหาการเคลอนไหวทไรความรสก เชน โรคอมพาตครงทอนลาง กลามเนอทอนลางออนแรง เปนตน ใหสามารถเคลอนไหวและกาวเดนได เทยบเคยงกบคนปกต
3. เพอตรวจสอบการเคลอนไหว ของขอตอ สะโพก หวเขา ขอเทา ส าหรบการวดมมองศา ส าหรบการเคลอนไหวในขอตางๆ
4. เพอสรางกลไกของชดโครงหนยนตแบบยดตดขาดานนอก ใหสามารถปรบระดบชวงทอนลาง ไดอยาง อสระ ใหเหมาะสมกบกายภาพของผสวมใส
1.3 ขอบเขตกำรวจย
สรางหนยนตตนแบบสามารถรบน าหนกของผใชงานไดไมเกน 80 กโลกรม สามารถรองรบสวนสงของผใชงาน 150–180 เซนตเมตร โดยมฟงกชนการเดน การลกนง ดวยความเรวไมเกน 0.5 เมตรตอวนาท 1.4 นยำมศพท Exoskeleton Robot คอ ชดโครงสรางภายนอกเสมอนเปลอกแบบแขงของหอยทาก ทสามารถยดตดและถอดออก โดยสามารถยดตดกบอวยวะของรางกาย ท าหนาทชวยปกปอง หรอเสรมสรางก าลง หรอชวยทนแรง ใหกบผสวมใส โดยอาศยชดตนก าลงในการเคลอนไหว Gait Pattern คอ รปแบบการเดนของขาทงสองขางใหเปนรปแบบการกาวเดนเดยวกน การกาวเดนในระยะทางเฉลยทเทากนและตอเนอง โดยใชสวนสะโพก ขอเขา และขอเทา ใหเคลอนไหว จงท าใหมมองศาของสวนสะโพก ขอเขา และขอเทา เกดการเปลยนแปลงและต าแหนงองศาเฉลยใกลเคยงกน ระหวางขาทงสองขาง Embedded System คอ ระบบสมองกลฝงตว ใชในการควบคมอปกรณตางๆ สามารถในไปใชในการควบคมแบบเฉพาะทาง การควบคมแบบแยกสวน โดยอาศยตวประมวลผล ทท าหนาทวเคราะหและประมวลผลขอมลตามโปรแกรมทไดออกแบบไว ซงน าไปใชในระบบควบคมฝงตวแบบแยกสวน โดยแยกตวประมวลเปนสวนๆ ใชในระบบควบคมทตองการท างานเฉพาะดานทแตกตางกน โดยอาศยการสอสารขอมลแบบเครอขาย
4
Lower Limb Assistant Robot คอ หนยนตชวยเหลอครงทอนลาง ใหสามารถเคลอนไหวได โดยอาศยอปกรณและเครองมอกลไกตางๆ ชวยในการเคลอนไหวครงทอนลาง โดยอาศยตนก าลงจากอปกรณขบเคลอน เพอใหทอนลาง สวนสะโพก ขอเขา ขอเทา ใหสามารถเคลอนไหวได โดยมระบบควบคมแบบปด เพอควบคมต าแหนงการเคลอนท โดยอาศยอปกรณเซนเซอรเปนตวตรวจสอบการท างาน 1.5 ประโยชนและผลทคำดวำจะไดรบ
1. สามารถเปนอปกรณทางเลอกทางการแพทย ส าหรบผทพพลภาพครงทอนลาง 2. เปนอปกรณทชวยในการเคลอนไหวครงทอนลาง เพอใหผสวมใสสามารถกาวเดนไปขางหนา
และลกนงดวยตนเอง 3. เพอเปนการพฒนาอปกรณทางการแพทยในประเทศไทย ประเภทการเคลอนไหว และ
สามารถผลตในประเทศ สามารถลดการน าเขาอปกรณจากตางประเทศ 4. เขารวมการประกวดนวตกรรมและสงประดษฐทางการแพทย ทงภายในประเทศและ
ตางประเทศ 5. ผประกอบการหรอผสนใจ น าไปพฒนาตอยอดในเชงพาณชย
5
บทท 2 ทฤษฏทเกยวของ
2.1 สาเหตของการเกดอมพาตและอมพาตครงทอน
2.1.1 อมพาต อมพาต คอ ภาวะทเกดจากการทเลอดไมสามารถไปเลยงสมอง ท าใหการท างานของสมอง
หยดชะงก ถาสมองจะไดรบออกซเจนและอาหาร ทมอยในเลอดมาบ ารงเลยง การท างานของรางกายกจะเปนปกต หากมเสนเลอดบางสวนทไปเลยงสมองเกดตบลง อดตน เปนโรคหรอบาดเจบ ผลทตามมากคออาการอมพาต แสดงอาการออนแรงซกใดซกหนงหรอทงสองซกของรางกาย อาจมอาการรวมกบตามองไมเหน หไมไดยนหรออาจจะพดไมได บอยครงทการหมนเวยนของเลอดชะงกไปเปนเวลาสนๆ ฉะนนภายในเวลาไมกชวโมงหรอไมกสปดาห อาการกจะบรรเทาลง ซงเรยกวา อมพฤกษ แตบางครงความเสยหายรายแรงกวานนและผปวยตองไดรบการรกษาบ าบดอยางด จงจะกล บชวยเหลอตนเองไดใกลเคยงเดม สาเหตส าคญทกอใหเกดโรคอมพาต คอ
1. เกดภาวะหลอดเลอดสมองตบ มกเกดขนเนองจากหลอดเลอดมการแขงตว จงมผลใหเลอดไปยงสมองไดนอยและมการตนในหลอดเลอดนนเอง
2. เกดการอดตนในหลอดเลอด สาเหตเพราะมกอนเลอดหลดมาจากทตาง ๆ ของรางกาย เชน จากหวใจแลวมาอดหลอดเลอดในสมอง จงท าใหสมองขาดเลอด
3. เกดหลอดเลอดในสมองบางเสนแตก เกดจากการทหลอดเลอดในสมองแขง แลวมการแตกของหลอดเลอด
4. การอกเสบของหลอดเลอด เกดจากภาวะทมการอกเสบในสมอง เชน เยอหมสมองอกเสบ วณโรคของสมองและมผลท าใหหลอดเลอดของสมองอกเสบตามมา ท าใหเลอดไปเลยงสมองไมพอ
5. อบตเหตของสมอง ภาวะทมอบตเหตตอสมองหรอภยนตรายตอสมองจะท าใหมการฉกขาดของหลอดเลอดและมเลอดออกในสมองอาจจะกดหรอเบยด ท าใหเกดอมพาตขนไดเชนกน
นอกจากสาเหตดงกลาวขางตนยงมปจจยเสยงตาง ๆ ทส าคญทท าใหผปวยเปนอมพาต ไดแก โรคความดนโลหตสง ภาวะนเปนสาเหตใหญทท าใหหลอดเลอดแขงตวและท าใหเกดโรค
อมพาตไดมากทงชนดหลอดเลอดแตกและหลอดเลอดตบ ภาวะโรคความดนโลหตสงนจะท าใหผปวยมโอกาสเปนอมพาตมากกวาคนปกตสงถง 3 – 17 เทา แลวแตอายความรนแรงของความดนโลหตสง
โรคเบาหวาน ผปวยทเปนโรคเบาหวานเปนเวลานาน ๆ โดยมไดนบการรกษาหรอควบคมระดบน าตาลในเลอดใหอยในเกณฑปกต จะมอตราเสยงในการเกดอมพาตชนดหลอดเลอดตบไดสง เพราะโรคเบาหวานท าใหเกดภาวะหลอดเลอดแขงไดงาย โดยจะมหลอดเลอดแขงทวรางกายและถา
6
เปนทหลอดเลอดของสมองจะเกดอมพาตขน อตราเสยงของผปวยทเปนโรคเบาหวานท าใหเกดอมพาตไดสงกวาผปวยปกตถง 2 – 4 เทา
ภาวะทมไขมนสงในหลอดเลอด ทงชนด cholesterol , triglyceride ซงเปนไขมนทไปเกาะผนงหลอดเลอดและจะท าใหผนงหลอดเลอดแขง อนจะมผลตามมาท าใหเกดอมพาตไดงาย
การสบบหร การสบบหรนนจะเปนปจจยเสรมท าใหผปวยเกดอมพาตไดงาย โดยผทสบบหรจดจะมโอกาสเปนอมพาตไดมากกวาผทไมสบบหรถง 3 เทา
2.1.2 อมพาตครงทอน รจกกนดในภาษาไทยวาอมพาตครงทอน เรยกแบบภาษาองกฤษวา spinal cord injury
หรอการบาดเจบของไขสนหลง ซงจะแตกตางจากอมพาตครงซก ตรงตนก าเนดการเกดโรคเนองจากอมพาตครงซกมกเกดทสมอง สวนอมพาตครงทอนมสาเหตไดหลากหลายสาเหต แตสวนมากอมพาตครงทอนตนเหตมกเกดทไขสนหลง เกดการบาดเจบ จากอบตเหตบนทองถนน อบตเหตการกฬา มการกดทบของไขสนหลงจากเนองอก ถกยง ฟนแทงดวยอาวธ ตกจากทสง หกลม และหลงกระแทกพน การบาดเจบทไขสนหลงท าใหขาดการตดตอระหวางสมองกบประสาทสวนปลายทอยต ากวาระดบทไดบาดเจบ ผปวยอมพาตครงทอน จะสญเสยการควบคม การเคลอนไหว การรบความรสก การควบคมการขบถาย และมการเปลยนแปลงการท างานของระบบตางๆในรางกาย เชน ระบบหวใจและหลอดเลอด เปนตน
การแบงชนดของอมพาตครงทอน ตามการบาดเจบของไขสนหลงระดบการเคลอนไหว จะเรยก ระดบสดทายของไขสนหลงทท าหนาทควบคมก าลงกลามเนอใหมก าลงตงแต ระดบ 3 แตถาหากพยาธสภาพอมพาตครงทอน เกดขนทระดบ อก มการสญเสยการรบความรสกตงแตชวงล าตวลงไป ใชวธการแบงตามระดบทของอมพาตครงทอน มการสญเสยความรสก คอจะเรยกตามระดบแรกทมผปวยอมพาตครงทอน มการเสยการรบความรสกหรอเรมรบความรสกลดลง
การแบงพยาธสภาพของอมพาตครงทอนตามความรนแรงของการสญเสยการท างานของระบบประสาท โดยแยกวาอมพาตครงทอนเปนการบาดเจบของไขสนหลงแบบสมบรณหรอการบาดเจบของไขสนหลงแบบไมสมบรณ โดยใชวธทเรยกวา sacral sparing definition คอการกลบมาของไขสนหลงระดบกนกบ บางสวนของอมพาตครงทอนหลงจากไขสนหลงเกดการบาดเจบ โดยจะมการตรวจอยอยางนอย 1 ใน 3 ถอเปนอมพาตครงทอนของการบาดเจบของไขสนหลงแบบสมบรณ 1) ความสามารถในการงอนวหวแมเทา 2) การรบความรสกของผวหนงรอบๆทวาร 3) ตรวจโดยการกระตกสาบปสสาวะทวางไว
7
ภาพประกอบ 2.1 สวนขอตอกระดกสนหลง
[ทมา: http://www.thailovehealth.com/disease/health-1485.html]
2.2 ทฤษฏเกยวกบการเดน การเดนคอ การเคลอนไหวของขา 2 ขางสลบกน (Alternating) อยางเปนจงหวะ
(Rhythmic) และมการเคลอนไหวของล าตวเพอใหจดศนยถวง (Center of gravity) ของรางกายเคลอนไปขางหนา การเดนจะตองเปนวงจร (Cycle ) หรอเรยกวา “Gait cycle” ซงครอบคลมระยะตางๆคอ
1) Stance phase คอระยะทขาขางนนยงคงวางอยบนพน 2) Swing phase คอ ระยะทขานนยกลอยขนจากพน
ภาพประกอบ 2.2 ขนตอนการเดนปกตของมนษย (Gait Pattern)
[ทมา: https://s3.amazonaws.com]
8
จากภาพประกอบ 2.2 เมอกาวออกเดนดวยเทาขวา ในชวงแรก เทาขวาจะยนพนรบน าหนกตว (Stance phase) ล าตวจะเคลอนไปขางหนา โดยเทาซายจะดนตวไป เมอล าตวเคลอนไปขางหนา เทาซายซงลอยกจะมาแตะพนเพอจะรบน าหนก เทาขวากจะกลายเปนตวดนล าตวใหเคลอนทไปขางหนา และตวเทาเองกจะลอยจากพน (Swing phase) เพอจะไปคอยรบน าหนกตวตอไป ชวงการเดนทกลาวถงน คอการศกษาขาขางใดขางหนง ซงจะเหนวาขาขวาจะม Stance phase และ Swing phase ตามกนไป เมอขาขวาเปน Swing phase ขาซายตองเปน Stance phase สลบกนไป จงท าใหการเดนเปนไปตามธรรมชาต การท างานอยางละเอยดของแตละชวงยงแบงออกไดเปนขนตอนดงตอไปน
2.1.1 Stance phase ประกอบไปดวยขนตอนตางๆ 5 ขนตอนดงน 1) Heel strike คอขนตอนทสนเทากระทบพน กลามเนอทท างานระยะน คอ - กลามเนอ Quadriceps ท าหนาทใหเขาเหยยด - กลามเนอ Gluteu maximus ท าหนาทเหยยดสะโพกใหสนเทากดกบพน - กลามเนอ Hamstrings ท าหนาทชวยเหยยดสะโพกเพอใหสนเทากดกบพน - กลามเนอกลมกระดกขอเทาขน ท าใหเทากระดกพนพน
2) Foot flat เปนระยะของการเดนทเทาแตะพน ใชกลามเนอเหมอนกบระยะ Heel strike เวนแตขอเทาอยในทากระดกลง 15 องศา
3) Mid-stance เปนระยะของการเดนทมการเคลอนไหวของล าตวมาดานหนามากขนน าหนกตวตกลงในแนวศนยกลางมวลของรางกาย ผานหลงขอสะโพกขอเขาและขอเทา กลามเนอทท างานระยะนคอ - กลามเนอ Iliopsoas ท าหนาทใหขอสะโพกงอ กนไมให Hyperestension - กลามเนอ Gluteus medius , Gluteus minimus และ Tensor fascia lata
ท าหนาทใหกระดกเชงกรานอยในระดบ ไมใหเอยงไปดานใดดานหนง และ ไมใหเสยหลกงาย
- กลามเนอ Quadriceps ท าหนาทไมใหเขางอพบลงมา - กลามเนอกระดกเทา ท าหนาทใหผาเทาแนบกบพน
4) Heel off เปนระยะทสนเทายกสงจากพน ล าตวจะเอนไปขางหนามากขนกลามเนอทท างานในระยะน คอ - กลามเนอ Iliopsoas ท าหนาทปองกนไมใหขอสะโพก Hyperestension - กลามเนอ Hamstring ท าหนาทงอเขา - กลามเนอกระดกเทาลง ท าหนาทกระดกขอเทาลง
9
5) Toe Off เปนระยะทนวเทาดนใหขาไปขางหนา เพอเขาสระยะ Swing phase กลามเนอทท างานในระยะน คอ - กลามเนอ Iliopsoas ท าหนาทงอสะโพกเพอเหวยงขาไปขางหนา - กลามเนอ Quadriceps ท าหนาทปองกนการงอเขาเรวเกนไป - กลามเนอกระดกเทาลงท าหนาทดนใหขาเหวยงไปขางหนา
2.2.1 Swing phase กนเวลาประมาณ 1/3 ของวงจรการเดน แบงเปน 3 ระยะดงน
1) Acceleration คอระยะทเขาเหวยงไปขางหนาดวยความเรวจากแรงเฉอยมาเรงกลามเนอทท างานระยะนคอ
- กลามเนอ Iliopsoas หนาทเหวยงและงอขอสะโพกไปขางหนา - กลามเนอ Quadriceps ท าหนาทปองกนการงอเขามากเกนไป - กลามเนอกระดกเทาขนท าใหขอเทากระดกขนเพอไมใหปลายเทาแตะ
พนกนลม 2) Mid swing เปนระยะทขาขางนนถกดกใหลอยมาอยใตล าตว กลามเนอทใช
เหมอนระยะ acceleration 3) Deceleartion คอ ระยะการเดนทขาทถกเหวยงไปขางหนาจากแรง
กลามเนอทกลาวมา ถกชะลอใหชาลงเพอเขาส Stance phase อกครงเปนวงจรการเดนใหมตอไป
10
ตารางท 2.1 การท างานของขอตางๆและกลามเนอในขณะเดน
Phases HIP(degree) Knee(degree) Ankle(degree) Muscles
Heel strike Flex 25 Fully extend Neutral Gluteus maximus, hamstrings, foot and toe dorsiflexors
Foot flat Flex 23 Flex 20 Plantarflex 15 Same Heel strike Mid stance Flex 10 Flex 10 Dorsiflex 3 Iliopsoas, gluteus
minimus, tensor fascialata, quadriceps, plantar flexors
Heel off Hyperextend 10
Flex 2 Dorsiflex 15 Iliopsoas, hamstrings, plantar flexors
Toe off Flex 10 Flex 40 Plantarflex 20 Iliopsoas, quadriceps, plantar flexors
Acceleration Flex 5 Flex 65 Neuaral Iliopsoas, quadriceps, foot and toe dorsiflexors
Mid swing Flex 25 Flex 65 Neuaral Iliopsoas Deceleration Flex 25 Fully extend Neuaral Gluteus maximus,
hamstrings, foot and toe dorsiflexors
ขนตอนตางๆ ของการเดนสามารถสรปการท างานของขอตางๆและกลามเนอไดดงน จาก
ตารางท 2.1 แสดงใหคามมของสะโพก (Hip) ขอเขา (Knee) และขอเทา (Ankle) ซงในแตละขนตอนจะมมมของขอตอตางๆ จากขอมลมมทขอสะโพกทแสดงในตารางเปนคามมเทยบกบแนวแกนตง สวนขอเขา แสดงมมระหวางขาทอนบนและทอนลาง และขอเทาแสดงมมทเปลยนแปลงไปจากมมปกต
11
(มมปกต (Neutral) คอ มมตงฉากระหวางขาทอนลางและผาเทา) คามมขอสะโพก (Hip angle) มมมงอ (Flex) ตงแต 25 องศา ถง -25 องศา เทยบกบแนวตง โดยแบงเปน 2 ชวง คอ ชวงทขาทอนบนอยหนาล าตว (ขนตอน Heel Strike, Foot flat, Mid stance และ Mid swing, Deceleration) ชวงนคามมจะเปนบวก และชวงทขาทอนบนอยขางหลงล าตว (ขนตอน Heel off งอไปขางหลงเกน (Hyperextend) 10 องศา, Toe off และ Acceleration) คามมทขอเขา (Knee angle) มมมงอตงแต 0 องศา (Fully extend) จนถง 65 องศา และคามมทขอเทา (Ankle angle) มคามมปกต กางออก (Plantar flex) และงอเขา (Dorsiflex)
2.2.2 การพจารณาวงจรการเดน Stride width หรอความกวางของฐานการเดน (Width of walk bases หรอ Base of gait)
คอระยะทางระหวางจดกงกลางของสนเทาทงสองขางขณะ Heel strike คาปกตประมาณ 2 – 4 นว หรอ 5 – 10 ซม. การพจารณาควรดวา ฐาน (Base) กวางหรอแคบ
Stride length คอ ระยะทางระหวาง Heel strike ของเทาขางหนงถง Heel strike ของเทาขางเดยวกน ในวงจรการเดนถดไป การพจารณาควรดระยะทางของการเดน 1 รอบในเทาแตละขางวาเทากนหรอไม (Symmetrical หรอ Asymmetrical)
Step length คอระยะทางระหวาง Heel strike ของเทาขางหนงถง Heel strike ของเทาอกขางหนง ซงจะเทากบครงหนงของ Step length
Cadence คอ จ านวนกาวตอ 1 นาท คาปกตประมาณ 70-130 กาว/นาท การพจารณาควรด ความถ (Rate) ความเทากน (Symmetry) ความคลองตว (Flexibility) และความหนกแนนของการลงฝเทา (พจารณาขณะทเราเดนจดศนยถวง(C.G.)) จะเคลอนไหวขนและลง จดศนยถวงจะเคลอนไหวถงจดสงสดในชวง Mid stance และต าสดชวง Double support ซงในผใหญและเพศชาย จะมระยะในแนวดง(Vertical displacement) ประมาณ 2 นว และระยะดานขาง (Lateral displacement) 2 นว สวนกระดกเชงกราน (pelvis) จะลดระดบลงตามขาขางทลงน าหนกและสลบกบอกขางหนง โดยการเปลยนแปลงนเรยกวา horizontal dip of pelvis ประมาณ 5 องศา การเคลอนทของ center of gravity จะนอยและนมนวลได ตองอาศยขอมลตางๆ ดงตอไปน
Pelvic rotation โดยจะมการหมนขางละ 4 องศา รวมเปน 8 องศาโดยจะมากทสดเมอม double support การมการหมนชวยลงระดบแอมพลจด (amplitude) ของจดศนยถวงประมาณ 3/8
Pelvic tilt จะท าให pelvis ต าลง 5 องศา ชวยลด จดศนยถวงลงมา 3/16 Knee flexion ในระหวาง mid stance 15 องศา ลด จดศนยถวงประมาณ 7/16 Knee motion ชวยใหการขยบระดบของ จดศนยถวงนมนวล Ankle motion ชวยใหการขยบระดบของ จดศนยถวงนมนวล
12
Motion of the center of gravity in the horizontal plane โดยจะมการขยบทางซาย และขวาขณะกาว โดยมความแตกตางประมาณ 6 นว
2.2.3 การวดความยาวของขา ความยาวของขาทงสองขางหากแตกตางกนเกนกวาประมาณ 1 นวขนไปจะท าใหทายนและ
เดนผดปกตเราอาจวดความยาวของขาทงสองขางเปรยบเทยบกนไดโดย การวดความยาวของขาแบบ True length วดจาก anterior superior iliac spine (ASIS) (กระดดปกสะโพกดานหนา) ไปถงปลายของตาตมใน หรอ plantar surface (ผวฝาเทา) ของสนเทา โดยจดใหขาทงสองขางอยในทาเหมอนๆกน หากตองการวดเพยงความยาวของสวนตนขา ใชวดจากกระดกปกสะโพกดานหนา (ASIS) ถง medial joint-line (แนวกลางขอหมน) ของขอเขา
การวดความยาวของขาแบบ Apparent length วดจากสะดอ (Umbilicus) ไปยงปลายของตาตมใน ความยาวนไมใชความยาวทแทจรงของขา แตเปนความยาวเทาทปรากฏใหเหน ซงอาจแตกตางจากความยาวทแทจรงได
ภาพประกอบ 2.3 การวดความยาวของขา [แหลงทมา http://firstphysioclinic.com/ ]
13
2.2.4 น าหนกของขา น าหนกของขาจะคดจากน าหนกตวทงหมดคดเปนเปอรเซนตคอ น าหนกของตนขา
จะเทากบ 9.6% ของน าหนกตว, น าหนกของหนาแขงจะเทากบ 4.5% ของน าหนกตว, น าหนกของเทาจะเทากบ 1.4% ของน าหนกตว
ภาพประกอบ 2.4 ขอมลการเปรยบเทยบน าหนกขา [ทมา: http://firstphysioclinic.com/]
2.3 ชดหนยนตชวยเดน
ในปจจบนการพฒนางานวจยทางดานอปกรณทางการแพทยอยางแพรหลาย และมวตถประสงคทจะชวยเหลอผทมปญหาทางการเดน ใหสามารถเคลอนไหว และเสรมสรางกลามเนอทอนลาง โดยการเคลอนไหวใหเทยบเคยงกบคนปกต โดยไดอธบายพฤตกรรมรปแบบของการเดนของคนปกต (82 kg, 0.99 m leg-length, 28 year-old male, walking at 1.27 m/s (Aaron M, et al.)) ในสวนของครงลางตงแต สะโพก (Hip), หวเขา (Knee), ขอเทา (Ankle) โดยการสรางโครงสรางตนแบบเพอเกบขอมลทไดจากการเดนของคนปกต ซงท าใหเหนถงวฎจกรของการเดนในสวนของครงลาง โดยผลลพธแสดงใหเหนถงมมของการเคลอนทของหวเขา ขอเทา และสะโพก รวมถงก าลงทใชในการเคลอนไหวในแตละสวน
ระบบตนก าลงในการเคลอนไหว การเดนพนผวแนวระนาบ (Viet Anh Dung Cai, et al.) โดยมการออกแบบอปกรณเฉพาะในสวนของขอหวเขา แบบหกแกนอสระในสวนของหวเขา (6DOFs Knee Exoskeleton) ทสามารถปรบเปลยนแรงดวยตวเองจากการเคลอนไหวหวเขา เพอใหเกดความยดหยนและปองกนการเคลอนไหวทเกนขอบเขต โดยอาศยก าลงขบเคลอนมอเตอรไฟฟากระแสตรง (DC Motor) ท าการควบคมแรงบด (Torque) ของการเดนในสวนหวเขา เพอตานทานแรงบดทเกดจากการเดน ทงนยงมอปกรณชวยในการเคลอนไหวเรยกวา Roboknee (J.E. Pratt, et al.) อปกรณขบเคลอนแนวระนาบเรยกวา linear series elastic actuator (SEA) เพอเพมก าลงในสวนของขอเขา ในการขนลงบนได และชวยรองรบน าหนกของชดในการท างานโดยควบคมแรงบดขอเขา ซงมเซนเซอรวดแรงกดจากพนราบ (ในแนวตง) จ านวน 2 ชด ดานซายและขวา เพอควบคมแรงบดในสวนของขอเขา เปนอปกรณตนก าลงชวยในการเคลอนไหวทงนยงมการพฒนาชดโครงสรางหนยนต
14
ตนแบบในการชวยเดน (Andrew Chu, et al.) โดยเนนก าลงทสวนขา และทบรรจของส าหรบแบบภาระของหนก โดยใชระบบ Linear Hydraulic Actuator เปนตวสงก าลงหลกของระบบ สามารถรบภาระการแบกสงของไดมากไมต ากวา 100 กโลกรม เพอชวยเหลอในการแบกสมภาระตางๆ แตขอจ ากดของน าหนกชดรวม 75 kg จงเปนอปสรรคอกประการหนง
โดยการออกแบบและพฒนาชดโครงหนยนตเสรมก าลงแบบเตมตว เรยกวา Hal (Hybrid Assistive Leg) จากมหาวทยาลยสทคบะ ประเทศญปน (H. Kawamoto, et al.) ออกแบบส าหรบเพมประสทธภาพการท างานของผสวมใส มมอเตอรขบเคลอนพรอมระบบทดก าลงแบบฮารโมนก ในสวนของสะโพก และขอเขา โดยน าไปใชชวยเหลอในการเคลอนยายผปวยในสถานพยาบาล และชวยเหลอส าหรบผทมปญหาทางการเดน โดยอาศยการวดสญญาณกลามเนอ เพอแปลงเปนสญญาณทางไฟฟาอเลกทรอนกส (EMG: Electromyography) และสงสญญาณไปยงสมองกลหนยนตเพอประมวลผล ใหสามารถท างานไดอยางถกตองตามรปแบบของการเคลอนไหวทางกายภาพ ทงยงออกแบบพนรองเทาใหมระบบตรวจจบแรงกด ทเกดขนในขณะปฏบตงาน เพอใหระบบสมองกลควบคมการท างานของมอเตอร ใหสงก าลงตอบสนองการท างานไดอยางมประสทธภาพ โดยระบบของ HAL-5 ตดตงเซนเซอรตรวจวดกลามเนอระหวางสะโพกและขอเขา โดยตดตงทงสองฝงของขา เซนเซอรแบบ Potentiometers ส าหรบการวดต าแหนงมมองศา เซนเซอรวดแรงกด เซนเซอรวดระดบความสมดลยและเซนเซอรวดความเรง โดยอปกรณดงกลาวถกน ามาใชใน ระบบควบคมรปแบบการเดนและระบบ EMG โดย HAL-5 มน าหนก 21 กโลกรม พลงงานหลกจากแบตเตอรสามารถรองรบการท างานไดอยางตอเนองประมาณ 160 นาท โดยการทดลองสามารถรองรบน าหนกมากถง 40 กโลกรม ทงน HAL-5 ไดพฒนาใหเขาสเชงพาณชย
(a) (b)
ภาพประกอบ 2.5 ชดตนก าลงชวยทนแรง BLEEX (a), และ HAL-5 (b)
15
โดยงานวจยดงกลาว ไดตดตงอปกรณตรวจวดคลนไฟฟากลามเนออเอมจ (EMG: Electromyography) สญญาณจากกลามเนอเพออานสถานะการเคลอนไหว ใหอปกรณสามารถขบเคลอนตามรปแบบการเคลอนท เสมอนชดตนก าลงชวยทนแรง ซงหากผทสญเสยความรสกทอนลาง หรอเปนอมพาต กจะยากทจะตรวจจบสญญาณไฟฟากลามเนอทอนลาง จงท าใหมขอจ ากด ตอการน าไปใชงานในผทพพลภาพครงทอนลาง 2.4 จลนศาสตรหนยนต
การเคลอนทของหนยนตหรอจลนศาสตรของหนยนต ประกอบดวยจดหมน (Joint) และกานโยง (Link) โดยใหการเคลอนทของจดปลาย (End-effector) เปลยนแปลง ทงนสามารถท างานในพนท ทก าหนดไวตามขอจ ากดของจดหมนและกานโยง ดงภาพประกอบ 2.6 ส าหรบการวเคราะหจลนศาสตรของหนยนตนนสามารถค านวณ ต าแหนง ความเรว และความเรงของทกกานโยง โดยไมพจารณาแรงทเกดจาการเคลอนท ซงจะวเคราะหความสมพนธระหวางการเคลอนท (Motion) แรง (Force) และแรงบด (Torque) จะเปนการวเคราะหในสวนของการศกษาพลศาสตร (Dynamics) ทงนจลนศาสตรหนยนต เปนคณตศาสตรทใชแสดงความสมพนธระหวางต าแหนงและการหมนของแตละกานโยงของหนยนต
ภาพประกอบ 2.6 องคประกอบการเคลอนทของจลนศาสตรหนยนต
จลนศาสตรหนยนต (Robot Kinematics) แบงเปน 2 ประเภทคอ
- Forward Kinematics หรอ Direct Kinematics คอ เปนการก าหนดความยาวของกานโยง และมมแตละจดหมน โดยสามารถค านวณหาต าแหนงของจดปลาย
- Invert Kinematics คอ เปนการก าหนดความยาวของกานโยง และต าแหนงของเปาหมายหรอจดปลาย ภายในพนทการท างานของหนยนต โดยสามารถค านวณหามมของจดหมน
Joint
Link
Link Link
Joint Joint
End-Effector
Base
16
ในการวเคราะหจลนศาสตรหนยนต ลกษณะทางกายภาพของหนยนต สามารถอธบายในรปแบบของKinematics diagram เพอใหสามารถเขาใจไดงาย สวนวธการค านวณ มหลายวธการค านวณ เชน Denavit-Hartenberg, การแปลงเลขาคณต และวธการพชคณต เปนตน
2.4.1 จลนศาสตรหนยนตดวยวธการพชคณต จลนศาสตรหนยนตดวยวธการพชคณต เปนการค านวณสมการทางคณตศาสตร ทประกอบดวย
วธการ เชน กฏของโคไซน ฟทากอรส เปนตน โดยอาศยรปทรงเลขคณตพนฐาน เขามาชวยในการค านวณ โดยการค านวณไมไดเกยวของกบ แรง พลงงานจลน พลศาสตร เนองจากเปนการหาสมการของจลนศาสตรหนยนต โดยท าการค านวณจากฐานลาง(Base) จนถงจดปลาย (End-effector) เพอหาต าแหนงปลายทางของระบบ โดยก าหนดคาพารามเตอรทเกยวของในการค านวณคอ ความยาวของกานโยง และมมของการหมน ดงแสดงใหเหนในภาพประกอบ 2.7 จลนศาสตรหนยนตสองแกน 2-DOF (DOF: degree of freedom)
ภาพประกอบ 2.7 จลนศาตรหนยนตสองแกน
เมอ l คอความยาวของกานโยง, θ คอมมของจดหมน ดงนน ค านวณหาต าแหนงของจด P(x,y) ดวยวธ Forward kinematics
𝑥 = 𝑙1𝑐𝑜𝑠𝜃1 + 𝑙2𝑐𝑜𝑠(𝜃1 + 𝜃2) 2.1 𝑦 = 𝑙1𝑠𝑖𝑛𝜃1 + 𝑙2𝑠𝑖𝑛(𝜃1 + 𝜃2) 2.2
𝑙1
𝑃(𝑥, 𝑦)
𝑙2
𝜃2
𝜃1 𝑥
𝑦
𝑃(𝑥, 𝑦)
𝑦
𝑥
𝑙1
𝑙2
𝜃1
𝜃2
17
การหามมของการหมน โดยก าหนดระยะของกานโยง l และจดปลาย P(x,y) สามารถค านวณไดจากวธการ Invert Kinematics จากรปขางตน
ภาพประกอบ 2.8 มมภายในจลนศาสตรหนยนตตามของกฏโคไซน จากกฏของโคไซน
𝑐2 = 𝑎2+𝑏2 − 2𝑎𝑏𝑐𝑜𝑠𝛼 2.3 ดงนน
𝑟2 = 𝑙12+ 𝑙2
2 + 2𝑙1𝑙2𝑐𝑜𝑠𝛼 2.4 เมอ 𝑟2 = 𝑥2 + 𝑦2
𝑐𝑜𝑠𝛼 =𝑥2+𝑦2−𝑙1
2−𝑙22
2𝑙1𝑙2 2.5
จาก 𝑠𝑖𝑛2𝜃 + 𝑐𝑜𝑠2𝜃 = 1 ดงนน
𝑠𝑖𝑛𝛼 = √1 − 𝑐𝑜𝑠2𝛼 2.6 จากสมการ (2.5) และ (2.6) จะได
𝑡𝑎𝑛𝛼 = 𝑠𝑖𝑛𝛼
𝑐𝑜𝑠𝛼 2.7
𝛼 = 𝑎𝑡𝑎𝑛2(𝑠𝑖𝑛𝛼, 𝑐𝑜𝑠𝛼) 2.8
ก าหนดให 𝜃2 = 𝛼 − 180 ดงนน
𝜃2 = 𝑎𝑡𝑎𝑛2(√1 − 𝑐𝑜𝑠2𝛼, 𝑐𝑜𝑠𝛼) − 180 2.9
โดยหามม 𝜃1 โดย 𝜃1 = 𝛽 + 𝛾 2.10
หามม 𝛾 โดยใชกฏของสามเหลยมมมฉาก 𝑡𝑎𝑛 𝛾 =
𝑦
𝑥 2.11
𝑦
𝑥
𝑙1
𝑙2
𝜃1
𝜃2
𝛼
𝛽
𝛾
𝑃(𝑥, 𝑦)
18
ดงนนเพอใหสามารถหามม 𝛾 ออกมาไดทงทางบวกและลบ ไดดงน
𝛾 = 𝑎𝑡𝑎𝑛2(𝑦, 𝑥) 2.12 หามม 𝛽 โดยใชกฏของโคไซน
𝑙22 = 𝑙1
2 + 𝑟2 − 2𝑙1𝑟𝑐𝑜𝑠𝛽 2.13
𝑙22 = 𝑙1
2 + 𝑥2 + 𝑦2 − 2𝑙1√𝑥2 + 𝑦2𝑐𝑜𝑠𝛽 2.14
ดงนน
𝑐𝑜𝑠𝛽 = 𝑙12+𝑥2+𝑦2−𝑙2
2
2𝑙1√𝑥2+𝑦2
2.15
จาก 𝑠𝑖𝑛2𝜃 + 𝑐𝑜𝑠2𝜃 = 1 ดงนน
𝑠𝑖𝑛𝛽 = √1 − 𝑐𝑜𝑠2𝛽 2.16 จากสมการ (2.15) และ (2.16) จะได
𝑡𝑎𝑛𝛽 = 𝑠𝑖𝑛𝛽
𝑐𝑜𝑠𝛽 2.17
ดงนน 𝛽 = 𝑎𝑡𝑎𝑛2(𝑠𝑖𝑛𝛽, 𝑐𝑜𝑠𝛽) 2.18
𝛽 = 𝑎𝑡𝑎𝑛2(√1 − 𝑐𝑜𝑠2𝛽, 𝑐𝑜𝑠𝛽) 2.19 ดงนนสามารถค านวณหาคา 𝜃1 = 𝛽 + 𝛾 และ 𝜃2 = 𝛼 − 180 โดยสมการทใชในการค านวณหาคามมของจดหมนของกานโยงหนยนตทงสองคอ
θ1 = atan2(√1 − cos2β, cosβ) − atan2(y, x) 2.20
θ2 = atan2(√1 − cos
2α, cosα) − 180 2.21
19
2.4.2 จลนศาสตรหนยนตดวย Homogenous Transformation การค านวณจลนศาสตรหนยนตประกอบดวย กานโยงเชอมตอกนแบบอนกรม และมม
ของจดหมนในแตละกานโยง เพอค านวณหาต าแหนงและมมหมนของจดปลายทาง โดยวธนเรยกวา Forward Kinematic สามารถค านวณไดจากวธ Denavit-Hartenberg โดยอาศยพารามเตอรในการค านวณ ai−1 คอความยาวของกานโยง αi−1คอมมเยองของกานโยงทเกดจากการบดแนวแกนใหม diคอระยะเหลอมของกานโยงทเปลยนไปของแนวแกน และθ𝑖 คอมมทมการเปลยนแปลงของจดหมน โดยคาพารามเตอรตางๆ
ภาพประกอบ 2.9 การก าหนดพารามเตอรของจดหมนและกานโยง
จากภาพประกอบ 2.9 การก าหนดระยะ ai−1 โดยวดระยะหางจากแกน Zi−1 ถง Zi
ตามแนวแกน Xi−1, การก าหนดมม αi−1โดยมมทเปลยนแปลงระหวางแนวแกน Zi−1 และ Zi ตามแนวแกน Xi , การก าหนดระยะ di โดยวดระยะหางจากแนวแกน Xi−1 ถง Xi ตามแนวแกน Zi , การก าหนดมม θ𝑖 โดยมมทเปลยนแปลงระหวางแนวแกน Xi−1 ถง Xi ตามแนวแกน Zi
เมตรกซตามรปแบบ Denavit-Hartenberg จากเฟรม {i} อางองจากเฟรม {i-1} ท าโดยเรมจากการค านวณการหมน (Rotation) และการเลอน (Translation) โดยการคณดานขวา (Post multiplication) ดงน
𝑇 = 𝑅𝑥𝑖
𝑖−1 (𝛼𝑖−1)𝐷𝑥(𝑎𝑖−1)𝑅𝑧(𝜃𝑖)𝐷𝑧(𝑑𝑖) 2.22
= [
1 0 0 00 𝑐𝑜𝑠𝛼𝑖−1 −𝑠𝑖𝑛𝛼𝑖−1 00 𝑠𝑖𝑛𝛼𝑖−1 𝑐𝑜𝑠𝛼𝑖−1 00 0 0 1
] [
1 0 0 𝑎𝑖−10 1 0 00 0 1 00 0 0 1
] [
𝑐𝑜𝑠𝜃𝑖 −𝑠𝑖𝑛𝜃𝑖 0 0𝑠𝑖𝑛𝜃𝑖 𝑐𝑜𝑠𝜃𝑖 0 00 0 1 00 0 0 1
] [
1 0 0 00 1 0 00 0 1 𝑑𝑖0 0 0 1
]
20
= [
𝑐𝑜𝑠𝜃𝑖 −𝑠𝑖𝑛𝜃𝑖 0 𝑎𝑖−1𝑠𝑖𝑛𝜃𝑖𝑐𝑜𝑠𝛼𝑖−1 𝑐𝑜𝑠𝜃𝑖𝑐𝑜𝑠𝛼𝑖−1 −𝑠𝑖𝑛𝛼𝑖−1 −𝑠𝑖𝑛𝛼𝑖−1𝑑𝑖𝑠𝑖𝑛𝜃𝑖𝑠𝑖𝑛𝛼𝑖−1 𝑐𝑜𝑠𝜃𝑖𝑠𝑖𝑛𝛼𝑖−1 𝑐𝑜𝑠𝛼𝑖−1 𝑐𝑜𝑠𝛼𝑖−1𝑑𝑖
0 0 0 1
] 2.23
เมอ Rx และ Rz คอ Rotation matrix, Dx และ Dz คอ Translation matrix โดยการ
ค านวณหา Forward kinematics จะค านวณจากฐานลางจนถงจดปลาย สามารถค านวณดวยการคณของเมตรกซจากฐานตามล าดบดงน
T = T10 end_effector
base T21 … 𝑇𝑛
𝑛−1 2.24 ดงนนสมการจะได
T = [
r11 r12 r13 pxr21 r22 r23 pyr31 r32 r33 pz0 0 0 1
] end_effectorbase 2.25
ตวอยางการค านวณดวยวธการ Denavit-Hartenberg
ภาพประกอบ 2.10 จลนศาสตรทางกลของหนยนตแบบสองแกน
ตารางท 2.2 การก าหนดพารามเตอรของ Denavit-Hartenberg i θi αi−1 ai−1 di 1 θ1 0 0 0 2 θ1 0 l1 0 3 0 0 l2 0
𝑙1
𝑙2
𝜃1
𝜃2 𝑦0
𝑥0
𝑦1
𝑥1
𝑦3
𝑥3
21
T = [
𝑐𝑜𝑠𝜃1 −𝑠𝑖𝑛𝜃1 0 0𝑠𝑖𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 00 0 1 00 0 0 1
] 10 T = [
𝑐𝑜𝑠𝜃2 −𝑠𝑖𝑛𝜃2 0 𝑙1𝑠𝑖𝑛𝜃2 𝑐𝑜𝑠𝜃2 0 00 0 1 00 0 0 1
] 21
T = [
1 0 0 𝑙20 1 0 00 0 1 00 0 0 1
] 32 2.26
ดงนนสมการจะได T = T1
0 T 21 T3
2 30
𝑇30 = [
𝑐𝑜𝑠(𝜃1 + 𝜃2) −𝑠𝑖𝑛(𝜃1 + 𝜃2) 0 𝑙1𝑐𝑜𝑠𝜃1 + 𝑙2𝑐𝑜𝑠(𝜃1 + 𝜃2)𝑠𝑖𝑛(𝜃1 + 𝜃2) 𝑐𝑜𝑠(𝜃1 + 𝜃2) 0 𝑙1𝑠𝑖𝑛𝜃1 + 𝑙2𝑠𝑖𝑛(𝜃1 + 𝜃2)
0 0 1 00 0 0 1
] 2.27
จากวธการค านวณพกดปลายทางโดยการก าหนดมมและระยะกานโยง จะสามารถทราบถงต าแหนงจดปลายจาก Translation Matrix ของ Px, Py, Pz และ Rotation Matrix, Rij (i=j=1,2,3) ทงนยงสามารถก าหนดพกดปลายทางและระยะกานโยง เพอค านวณหามมของแตละจดหมน สามารถค านวณในรปแบบของ Invert Kinematics โดยก าหนดให
T = [
r11 r12 r13 pxr21 r22 r23 pyr31 r32 r33 pz0 0 0 1
] = 𝑇10 𝑇2
1 𝑇32
30 2.28
น า 𝑇−11
0 คณทงสองขางของสมการท (2.28)
𝑇−110 𝑇3
0 = 𝑇−110 𝑇1
0 𝑇21 𝑇3
2 2.29 จาก 𝑇−1 𝑇10 = I1
0 𝑇−110 𝑇3
0 = 𝑇21 𝑇3
2 2.30 ดงนน
22
[
𝑐𝑜𝑠𝜃1 𝑠𝑖𝑛𝜃1 0 0−𝑠𝑖𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 00 0 1 00 0 0 1
] [
r11 r12 r13 pxr21 r22 r23 pyr31 r32 r33 pz0 0 0 1
]
= [
𝑐𝑜𝑠𝜃2 −𝑠𝑖𝑛𝜃2 0 𝑙1𝑠𝑖𝑛𝜃2 𝑐𝑜𝑠𝜃2 0 00 0 1 00 0 0 1
] [
1 0 0 𝑙20 1 0 00 0 1 00 0 0 1
]
2.31 น าต าแหนง (1,4) และ (2,4) ของเมตรกซ จากสมการท (2.31) 𝑐𝑜𝑠2𝜃1𝑃𝑥
2 + 𝑠𝑖𝑛2𝜃1𝑃𝑦2 + 2𝑃𝑥𝑃𝑦𝑐𝑜𝑠𝜃1𝑠𝑖𝑛𝜃1 = 𝑙2
2𝑐𝑜𝑠2𝜃2 + 2𝑙1𝑙2𝑐𝑜𝑠𝜃2 + 𝑙12
2.32
𝑠𝑖𝑛2𝜃1𝑃𝑥2 + 𝑐𝑜𝑠2𝜃1𝑃𝑦
2 − 2𝑃𝑥𝑃𝑦𝑐𝑜𝑠𝜃1𝑠𝑖𝑛𝜃1 = 𝑙22𝑠𝑖𝑛2𝜃2 2.33
น าสมการ (2.32) บวกสมการ (2.33)
𝑃𝑥2(𝑐𝑜𝑠2𝜃1 + 𝑠𝑖𝑛
2𝜃1) + 𝑃𝑦2(𝑠𝑖𝑛2𝜃1 + 𝑐𝑜𝑠
2𝜃1)
= 𝑙22(𝑐𝑜𝑠2𝜃2 + 𝑠𝑖𝑛
2𝜃2) + 2𝑙1𝑙2𝑐𝑜𝑠𝜃2 + 𝑙12 2.34
𝑃𝑥2 + 𝑃𝑦
2 = 𝑙22 + 2𝑙1𝑙2𝑐𝑜𝑠𝜃2 + 𝑙1
2 2.35
𝑐𝑜𝑠𝜃2 = 𝑃𝑥2+𝑃𝑦
2−𝑙12−𝑙2
2
2𝑙1𝑙2 2.36
เมอ cosθ = a จะมคาเทากบ θ = Atan2(±√1 − a2, a) ดงนน
𝜃2 = 𝐴𝑡𝑎𝑛2(±√1 − [𝑃𝑥2+𝑃𝑦
2−𝑙12−𝑙2
2
2𝑙1𝑙2]2
,𝑃𝑥2+𝑃𝑦
2−𝑙12−𝑙2
2
2𝑙1𝑙2) 2.37
หามม θ1 โดยน าต าแหนง (1,4) ของเมตรกซ จากสมการท (2.31)
𝑐𝑜𝑠𝜃1𝑃𝑥 + 𝑠𝑖𝑛𝜃1𝑃𝑦 = 𝑙2𝑐𝑜𝑠𝜃2 + 𝑙1 2.38
23
เมอ a sinθ + b cosθ = c จะมคาเทากบ θ = Atan2(a, b) ± Atan2(√a2 + b2 − c2, c) ดงนน
𝜃1 = 𝐴𝑡𝑎𝑛2(𝑃𝑥 , 𝑃𝑦) ± 𝐴𝑡𝑎𝑛2(√𝑃𝑦2 + 𝑃𝑥
2 − (𝑙2𝑐𝑜𝑠𝜃2 + 𝑙1)2, 𝑙2𝑐𝑜𝑠𝜃2 + 𝑙1)
2.39 ทงนการค านวณหา Forward kinematics โดยมขอก าหนดให ก าหนดคามมของการหมนและก าหนดระยะของกานโยง จะสามารถหาพกดจดปลายได และในทางกลบกน เมอก าหนดพกดจดปลายและระยะของกานโยง จะสามารถค านวณหามมของแตละแกนหมนได ซงเรยกวา Invert kinematics
2.5 ทฤษฎการควบคมมอเตอรดวย พไอดคอนโทรล (PID controller)
พไอดคอนโทรล หรอ Proportional Integral Derivative controller (PID) เปนระบบควบคมทใชในภาคอตสาหกรรมอยางแพรหลาย เพอทจะพยายามลดขอผดพลาดในขณะระบบท างาน เชน ระบบควบคมต าแหนง ระบบควบคมความเรว ระบบควบคมแรงบด เปนตน ในการควบคมตองอาศยคาอางองในการเรมตนการท างาน หรอการควบคมโดยก าหนดตวแปรตามทตองการ (set point) โดยการค านวณตวแปร set point จะน ามาประมวลผลและสงคาผลลพธจากการค านวณไปด าเนนการแกไขตามกระบวนการไดอยางทนท เพอใหระบบสามารถตอบสนองไดอยางรวดเรว และลดขอผดพลาดใหนอยทสด โดยการค านวณ PID ประกอบดวย 3 สวนส าคญ คอ Proportional, Integral และ Derivative สามารถแยกเปนสามพารามเตอร โดยสามารถปรบเปลยนคาคงทของแตละสวนไดอยางเหมาะสมกบระบบนนๆ ดวยวธควบคมพไอด สามารถด าเนนการควบคมเพอความตองการเฉพาะกระบวนการ โดยการตอบสนองของการควบคมจะสามารถอธบายในแงของการตอบสนองของการควบคมขอผดพลาด โดยสามารถแยกออกไดเปน 3 สวน ดงน
ภาพประกอบ 2.11 บลอกไดอะแกรมของการควบคมพไอด
𝐾𝑝 𝑒(𝑡)
𝐾𝑖 𝑒(𝑡)𝑑𝑡𝑡
0
𝐾𝑑 𝑑𝑒(𝑡)
𝑑𝑡
Process Σ Σ Set point +
-
Error Output
24
𝑢(𝑡) = 𝐾𝑝𝑒(𝑡) + 𝐾𝑖 ∫ 𝑒(𝑡)𝑑𝑡 + 𝐾𝑑𝑑
𝑑𝑡𝑒(𝑡)
𝑡
0 2.40
เทอม Proportional เปลยนแปลงตามอตราสวนของคา error ปจจบน ซงสามารถเขยนได
ตามสมการท 2.41 ซงคา Pout สามารถค านวณไดโดยการน าคา error มาคณกบคาคงท Kp เมอผลลพธมคาสงมากๆมกจะหมายถงการตอบสนองไดเรวขน แตเนองจากคาทสงมาก อาจท าใหเกดขอผดพลาดไดมากเหมอนกน สงมากเกนไปจะน าไปสการไมมเสถยร ซงท าใหเกดการสน (Oscillate) ก าหนดให Pout คอคาเอาทพทของระบบทสงออกมา, Kp คอคาอนพทพารามเตอรทก าหนดเขาไป, e คอคาความผดพลาด (คาตงตน – คาทวดได), t คอคาบเวลาใดๆ
𝑃𝑜𝑢𝑡 = 𝐾𝑝𝑒(𝑡) 2.41 สมการ Integral สามารถเขยนไดตามสมการท 2.42 โดย Iout คอคาทสงออกมา, Ki คอ
คาพารามเตอรทเราใสเขาไป, e คอคาความผดพลาด (คาตงตน – คาทวดได), t คอคาบเวลา ณ เวลา
ใดๆ, τ คอตวแปรผนของคาบเวลา
𝐼𝑜𝑢𝑡 = 𝐾𝑖 ∫ 𝑒𝑡
0(𝑡)𝑑𝑡 2.42
สมการ Derivative สามารถเขยนไดตามสมการท 2.43 โดย Dout คอคาทสงออกมา, Kd คอ
คาพารามเตอรทใสเขาไป, e คอคาความผดพลาด (คาตงตน – คาทวดได), t คอคาบเวลา ณ เวลาใด ๆ
𝐷𝑜𝑢𝑡 = 𝐾𝑑𝑑
𝑑𝑡𝑒(𝑡) 2.43
โดยสรปตวควบคมแบบพไอด จะมพารามเตอรทตองท าการปรบอยสามตว คอเกณฑการ
ขยาย, การตงคาเวลา และอตราเวลา ซงการควบคมแบบพไอดเหมาะทจะน ามาใชกบการควบคมกระบวนการ หรอลปการควบคมทมคาคงตวเวลายาวนานและปราศจากสญญาณรบกวนใดๆ โดยขอดของการควบคมแบบพไอด คอใหความสามารถในการดแลขณะทกระบวนการก าลงด าเนน หรอเปลยนแปลงอย ก าจดออฟเซตทเกดจากการควบคมแบบสดสวน
25
บทท 3 การออกแบบหนยนตชวยเดนส าหรบผปวยอมพาตครงลาง
3.1 การออกแบบระบบ (System Overview) ชดโครงหนยนตชวยเดนไดออกแบบส ำหรบผทพพลภำพครงทอนลำง โดยผใชงำนตองสวมใส
แบบยดตดดำนขำงของสวนขำทงสองขำง โดยสำมำรถสวมใสและตดตงดวยตนเอง มจดยดตดตงในสวนของ เอว ตนขำ ขอเขำ ขอเทำ รวม 10 จด เพอใหกระชบทอนลำงและควำมปลอดภยในกำรใชงำน โดยผสวมใสจะตองสวมใสชดโครงหนยนตชวยเดนและสำมำรถสงกำรใหเครองท ำงำนไดจำกไมเทำชวยพยง โดยสำมำรถกดปมเลอกฟงกชนกำรใชงำนคอ กำรยน กำรนง กำรกำวเดนและปมขอควำมชวยเหลอ โดยระบบจะรบค ำสงจำกปมกดตลอดเวลำหำกมกำรปลอยปมกด ระบบจะสงกำรใหหยดท ำงำน เมอผสวมใสเลอกปมสงกำรท ำงำนส ำหรบเคลอนไหวทอนลำง ระบบสมองกลของเครองจะรบสญญำณทสงมำและประมวลผลรปแบบกำรเดนทไดเกบขอมลกำรเดนของคนปกต และสงกำรไปยงอปกรณขบเคลอนตำมจดหมนตำงๆ ของทอนลำง ในสวนส ำคญ เชน สะโพก ขอเขำ เปนตน โดยสมองกลจะท ำกำรควบคมกำรท ำงำนใหตรงตำมรปแบบกำรกำวเดนใหสมบรณทสด ทงนชดหนยนตชวยเดนฯจะท ำกำรเชอมตอกบไมเทำค ำยน เพอควบคมกำรท ำงำนของชดหนยนตชวยเดนฯ ทงยงชวยประคองขณะกำวเดนของผสวมใสอกดวย โดยมปมกดส ำหรบกำรท ำงำน และปมกดส ำหรบขอควำมชวยเหลอ ถำผสวมใสเกดเหตขดของอยำงใดอยำงหนง ซงสำมำรถกดปมในกรณขอควำมชวยเหลอหรอเหตฉกเฉน ระบบจะสงสญญำณไปใหผดแลทนท ผำนเครอขำยโทรศพทสญญำณไรสำย
ภำพประกอบ 3.1 กำรท ำงำนของระบบหนยนตชวยเดนครงทอนลำง
cane
Master
Controller
Accelerometer
Slave
Controller
Slave
Controller
ผานเครอขายโทรศพทไรสาย
Smart Phone
Communication
Module
Gyroscope
Slave
Controller
Slave
Controller
Left Leg Right Leg Cane
Walking Assist Robot
26
หลกกำรท ำงำนของระบบหนยนตชวยเดนส ำหรบผปวยอมพำตครงลำง ดวยระบบสมองกลฝงตว (Embedded System) แบงเปน ตวประมวลผลหลก (Master Controller) จ ำนวน 1 ตว และตวประมวลผลยอย (Slave Controller) จ ำนวน 4 ตว โดยตวประมวลผลยอยถกออกแบบใหท ำงำนเฉพำะสวน ส ำหรบกำรควบคมมมองศำกำรหมนของสะโพก และขอเขำทงสองขำง ตวประมวณผลยอยจะท ำหนำควบคมกำรเคลอนทของจดหมนของทอนขำทงสองขำ โดยควบคมกำรท ำงำนของมอเตอรใหหมนไปยงองศำทก ำหนดแตละตว โดยระบบประมวลยอยจะรบสญญำณจำกตวประมวลผลหลกในกำรควบคมกำรขบเคลอนมอเตอรกระแสตรง (DC Motor) โดยมเซนเซอรแบบแอฟโซลทเอนโคตเดอร (Absolute Encoder) เปนตวตรวจวดคำมมองศำของจดหมนทมกำรเปลยนแปลง เพอท ำกำรตรวจสอบควำมถกตองของกำรหมนของมมองศำ ซงตวประมวลผลหลกจะท ำหนำทประมวลผลรปแบบกำรเดน และสงขอมลไปยงตวประมวลผลยอย ผำนระบบเครอขำยสอสำรแบบไรสำย (Wireless Communication) โดยอำศยสญญำณจำก Communication module มำตรฐำน IEEE 802.11n ควำมถ 2.4 GHz เปนระบบสอสำรภำยในระบบสมองกลฝงตวแบบแยกสวน และยงเชอมตออปกรณเซนเซอร Gyroscope และ Accelerometer เพอตรวจสอบสถำนะ กำรทรงตว และควำมเรว ในขณะทมกำรเคลอนไหว และรบค ำสงกำรควบคมกำรท ำงำนจำกผสวมใส ผำนปมกดทถกตดตงทมอจบของไมเทำค ำยน
ภำพประกอบ 3.2 กำรท ำงำนของระบบควบคมสมองกลแบบแยกสวน
Master Controller
27
3.2 รปแบบจลศาสตรของขาหนยนต รปแบบกำรกำวเดนตองอำศยกำรเคลอนทของสวนตำงๆ ของทอนลำงโดยม 3 จดหมน คอจดหมนในสวนสะโพก (Hip) ขอเขำ (Knee) และขอเทำ (Ankle) แสดงดงภำพประกอบ 3.3 จงท ำใหรำงกำยสวนทอนลำงสำมำรถเคลอนไหวได ทงน ผทพพลภำพทำงกำรเคลอนไหวทไรควำมรสกทอนลำง จงตองอำศยกำรเคลอนทของสวนทส ำคญทง 3 สวนคอ สะโพก ขอเขำ และขอเทำ จงออกแบบจลศำสำตรของขำหนยนต ใหสำมำรถเคลอนทของมมสะโพกขอเขำและขอเทำ เพอใหรปแบบกำรเคลอนไหวของขำในแตละขำงเทยบเคยงรปแบบของกำรเดน
(ก) (ข)
ภำพประกอบ 3.3 จดหมนส ำหรบครงทอนลำง สะโพก, ขอเขำและขอเทำ (ก) จลศำสตรทำงกล (ข)
โดยก ำหนดใหมมทเกดขน จดหมนสวนสะโพก (θhip) จดหมนสวนขอเขำ(θknee) และจดหมนสวนขอเทำ(θankle) เพอหำสมกำร กำรเคลอนทของกำรเดนของแตละขำง ซงในกำรหำจนศำสตรทำงกลดวย invert kinematics จะแสดงสมกำรของจดหมนดงตอไปน
T = Rot(θhip)Trans(L1)Rot(θknee)Trans(L2) Rot(θankle) (เมอ θhip= θ1 , θknee= θ2 , และ θankle= θ3)
T = [
𝑐𝑜𝑠𝜃1 −𝑠𝑖𝑛𝜃1 0 0𝑠𝑖𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 0
0 0 1 00 0 0 1
] 10 , T = [
𝑐𝑜𝑠𝜃2 −𝑠𝑖𝑛𝜃2 0 𝑙1
𝑠𝑖𝑛𝜃2 𝑐𝑜𝑠𝜃2 0 00 0 1 00 0 0 1
] 21 ,
T = [
𝑐𝑜𝑠𝜃3 −𝑠𝑖𝑛𝜃3 0 𝑙2
𝑠𝑖𝑛𝜃3 𝑐𝑜𝑠𝜃3 0 00 0 1 00 0 0 1
] 32 3.1
+θhip
-θknee
-θankle
Joint
Revolution
θhip= θ1
θknee = θ2
θankle = θ3
L2
L1
x1
y1
x2
x3
y0
x0
28
𝑇30 = [
𝑐𝑜𝑠𝜃123 −𝑠𝑖𝑛𝜃123 0 𝑙1𝑐𝑜𝑠𝜃1 + 𝑙2𝑐𝑜𝑠𝜃12
𝑠𝑖𝑛𝜃123 𝑐𝑜𝑠𝜃123 0 𝑙1𝑠𝑖𝑛𝜃1 + 𝑙2𝑠𝑖𝑛𝜃12
0 0 1 00 0 0 1
] 3.2
เมอ 𝜃12 = 𝜃1 + 𝜃2, 𝜃123 = 𝜃1 + 𝜃2 + 𝜃3 ก ำหนดให
𝑇30 = [
𝑐𝑜𝑠∅ −𝑠𝑖𝑛∅ 0 𝑥𝑠𝑖𝑛∅ 𝑐𝑜𝑠∅ 0 𝑦
0 0 1 00 0 0 1
] 3.3
เมอ 𝑐𝑜𝑠∅ = 𝑐𝑜𝑠𝜃123 , 𝑠𝑖𝑛∅ = 𝑠𝑖𝑛𝜃123
จำกสมกำร 3.3 ก ำหนดให 𝑥 = 𝑙1𝑐𝑜𝑠𝜃1 + 𝑙2𝑐𝑜𝑠𝜃12 3.4 𝑦 = 𝑙1𝑠𝑖𝑛𝜃1 + 𝑙2𝑠𝑖𝑛𝜃12 3.5
แกสมกำร 3.4 และสมกำร 3.5 โดยยกก ำลงสองทงสองขำงตลอดสมกำร
𝑥2 + 𝑦2 = 𝑙12 + 𝑙2
2 + 2𝑙1𝑙2𝑐𝑜𝑠𝜃2 3.6
จำกกฏของโคโซค 𝑐𝑜𝑠𝜃12 = 𝑐𝑜𝑠𝜃1𝑐𝑜𝑠𝜃2 − 𝑠𝑖𝑛𝜃1𝑠𝑖𝑛𝜃2 และ
𝑠𝑖𝑛𝜃12 = 𝑐𝑜𝑠𝜃1𝑠𝑖𝑛𝜃2 + 𝑠𝑖𝑛𝜃1𝑐𝑜𝑠𝜃2 ดงนนสำมำรถแกสมกำรท 3.6
𝑐𝑜𝑠𝜃2 =𝑥2+𝑦2−𝑙1
2−𝑙2
2
2𝑙1𝑙2 3.7
จำกกฏของไซค √𝑐𝑜𝑠𝜃2 + 𝑠𝑖𝑛𝜃2 = 1 ดงนน แกสมกำรท 3.7 จะได
𝑠𝑖𝑛𝜃2 = ±√1 − 𝑐𝑜𝑠𝜃22 3.8
จำกสมกำร 3.7 และสมกำร 3.8 ดงนน 𝜃2 = 𝐴𝑡𝑎𝑛2(𝑠𝑖𝑛𝜃2, 𝑐𝑜𝑠𝜃2) 3.9
สำมำรถค ำนวณหำมม 𝜃1 จำกสมกำร 3.4 และสมกำร 3.5 ดงนน
𝑥 = 𝑘1𝑐𝑜𝑠𝜃1 − 𝑘2𝑠𝑖𝑛𝜃1 3.10
𝑦 = 𝑘1𝑠𝑖𝑛𝜃1 + 𝑘2𝑐𝑜𝑠𝜃1 3.11
เมอ 𝑘1 = 𝑙1 + 𝑙2𝑐𝑜𝑠𝜃2 , 𝑘2 = 𝑙2𝑠𝑖𝑛𝜃2
จำกกฏของพทำโกรส ก ำหนดให
29
𝑟 = √𝑘12 + 𝑘2
2 3.12
และ
𝛾 = 𝐴𝑡𝑎𝑛2(𝑘2, 𝑘1) 3.13
เมอ 𝑘1 = 𝑟 𝑐𝑜𝑠 𝛾 , 𝑘2 = 𝑟 𝑠𝑖𝑛 𝛾 3.14
แทนคำในสมกำร 3.10 และสมกำร 3.11
𝑥
𝑟= 𝑐𝑜𝑠 𝛾 𝑐𝑜𝑠𝜃1 − 𝑠𝑖𝑛 𝛾 𝑠𝑖𝑛𝜃1 = 𝑐𝑜𝑠 (𝛾 + 𝜃1) 3.15
𝑦
𝑟= 𝑐𝑜𝑠 𝛾 𝑠𝑖𝑛𝜃1 + 𝑠𝑖𝑛 𝛾 𝑐𝑜𝑠𝜃1 = 𝑠𝑖𝑛 (𝛾 + 𝜃1) 3.16
น ำสมกำรท 3.15 และ 3.16 หำคำมม
𝛾 + 𝜃1 = 𝐴𝑡𝑎𝑛2 (𝑦
𝑟,
𝑥
𝑟) = 𝐴𝑡𝑎𝑛2(𝑦, 𝑥) 3.17
จะได 𝜃1 = 𝐴𝑡𝑎𝑛2(𝑦, 𝑥) − 𝐴𝑡𝑎𝑛2(𝑘2, 𝑘1) 3.18
ดงนน 𝜃1 + 𝜃2 + 𝜃3 = 𝐴𝑡𝑎𝑛2(𝑠𝑖𝑛∅, 𝑐𝑜𝑠∅) = ∅ 3.19
3.3 การออกแบบโครงสราง
โครงสรำงหนยนตส ำหรบผสวมใสถกออกแบบส ำหรบยดตดดำนขำงของสวนขำทงสองขำง เพอใหงำยตอกำรสวมใสและกระชบยดตดกบขำ โดยออกแบบใหสำมำรถปรบระดบควำมยำวของทอนขำไดทงสองสวนคอ ตนขำระหวำงสะโพกถงขอเขำ และขำสวนลำง ระหวำงขอเขำถงขอเทำ โดยสำมำรถปรบระยะไดเพอรองรบควำมสงของผสวมใส ทมสวนสงระหวำง 160 ถง 190 เซนตเมตร เพอใหเหมำะสมกบควำมสงของผสวมใส ซงผสวมใสตองมน ำหนกไมเกน 90 กโลกรม ถำปรบเปลยนระบบตนก ำลงใหมขนำดใหญกจะสำมำรถใหรองรบน ำหนกใหมำกขนได ซงท ำใหน ำหนกของชดจะมน ำหนกเพมขนเชนกน
กำรออกแบบโครงสรำง จะแบงออกเปน 2 สวนคอ สวนของโครงเครองและสวนเคลอนไหว โดยโครงเครองจะถกออกแบบเพอตดตงระบบสมองกลและแบตเตอร และในสวนทเคลอนไหวจะยดตดอยกบจดหมน ทงหมด 4 จด โดยมตวสงก ำลงเปนมอเตอรกระแสตรงและมเซนเซอรเอนโคดเดอร (Absolute Encoder) เปนตวอำนองศำกำรหมนของจดหมน โดยวสดทน ำมำใชงำนท ำมำจำกอลมเนยมอลลอยซงมคณสมบตแขงแรงและน ำหนกเบำ โดยมน ำหนกรวมประมำณ 25 กโลกรม
30
เนองจำกไดออกแบบใหมแผนรองเทำ เพอรองรบน ำหนกของผสวมใสและเพอควำมปลอดภยของขอเทำ ดงนนจดหมนขอเทำจงออกแบบใหยดตดไมมกำรหมน โดยรปแบบโครงสรำงชดหนยนตแสดงดงภำพประกอบ 3.4
ภำพประกอบ 3.4 กำรออกแบบชดโครงหนยนตชวยเดนครงทอนลำง
โดยสวนประกอบของชดโครงสรำงหนยนต สำมำรถอธบำยรำยละเอยดไดดงน ตำมหมำยเลขทก ำหนดดงรปขำงตน
1. สวนซบพอรทแผนหลง ส ำหรบรองรบแผนหลงในกำรสวนใส และรองรบกำรตดตงชดอปกรณควบคมสมองกล
2. สวนซบพอรทสะโพก ส ำหรบรองรบจดหมนของสะโพกและกำรปรบระยะของควำมยำวทอนลำง
3. สวนซพพอรทตนขำ ส ำหรบรองรบจดหมนของขอเขำและกำรปรบระยะของควำมยำวของขำสวนลำง
4. สวนซบพอรทขำสวนลำงและเทำ ส ำหรบกำรรองรบฝำเทำของผสวมใส 5. ชดตนก ำลงขบเคลอน ส ำหรบตนก ำลงของจดหมนใหสำมำรถเคลอนไหว 6. ชดทดก ำลง ส ำหรบอตรำทดก ำลงในกำรขบเคลอนเพอใหแรงบดทสงขน 7. อปกรณตรวจจบส ำหรบตรวจจบกำรเปลยนแปลงของกำรหมนเพอใหทรำบถงมมองศำทม
กำรเปลยนแปลง
1
2
3
4
5
6
7
31
ชดโครงหนยนตชวยเดนออกแบบใหผทพพลภำพครงทอนลำงสำมำรถสวมใสดวยตนเองและชวยในกำรกำวเดน หำกผทสญเสยอวยวะสวนใดสวนหนงของทอนลำงสำมำรถน ำไปใชงำนไดเชนกน โดยตองอำศยอปกรณเสรม เชน ขำเทยม เปนตน เพอใหชดโครงหนยนตชวยเดนสำมำรถยดตดได 3.4 การออกแบบระบบควบคม
ระบบควบคมส ำหรบชดโครงหนยนตชวยเดน สำมำรถแยกหนวยประมวลผลเปน 2 สวน ดงนคอ หนวยประมวลหลก (Master Controller) และหนวยประมวลผลยอย (Slave Controller) ซงสำมำรถแบงออกเปนหนวยประมวลผลหลกจ ำนวน 1 หนวย ท ำหนำทประมวลผลมมองศำของรปแบบกำรเดนและกำรสอสำรภำยนอก และหนวยประมำณผลยอยจ ำนวน 4 หนวย ท ำหนำทควบคมมมองศำในแตละสวน คอ สะโพก 2 หนวย ขอเขำ 2 หนวย โดยตรวจวดกำรเปลยนแปลงมมองศำทเกดขน
หลกกำรควบคมของชดหนยนตชวยเดนจะรบค ำสงจำกผใชงำน ผำนปมกดของไมเทำค ำยน โดยจะรบค ำสงโดยตรงจำกผใชงำนในสถำนะทท ำงำนและจะหยดท ำงำนเมอปลอยปมกด โดยหนวยประมวลผลหลก จะตรวจสอบสถำนะของปมกด เมอปมกดเรมกำรท ำงำน ระบบประมวลผลหลกจะท ำหนำประมวลผลรปแบบกำรเดน และประมวลผลมมองศำของกำรกำวเดนในแตละสวนของ สะโพก ขอเขำและขอเทำ ทงสองขำ โดยจะสงขอมลมมองศำตำงๆไปยงหนวยประมวลผลยอยตอไป โดยตวประมวลผลยอยแตละสวนจะรบขอมลของมมองศำเพอไปควบคมมมองศำของแตละสวน จงท ำใหอปกรณสงก ำลงท ำงำน ท ำใหชดโครงหนยนตสำมำรถเคลอนไหวไดตำมรปแบบกำรเดนทก ำหนดไว
ภำพประกอบ 3.5 โฟรซำรทกำรท ำงำนของระบบควบคมรปแบบกำรกำวเดน
รอรบค าสงจากป มกด
ประมวลผลรปแบบการเดน
ประมวลผลมมองศา
ควบคมมมองศารปแบบการเดน
32
สวนของหนวยประมวลผลหลก จะท ำหนำทประมวลผลกำรท ำงำนของรปแบบกำรเดน ตำมขอมลทไดบนทก โดยรปแบบกำรเดนจะถกเกบขอมล โดยน ำชดโครงหนยนตตดตงกบผใชงำนทรำงกำยปกตทวไป เพออำนขอมลกำรเดนในรปแบบกำรกำวเดนปกต ซงระบบสมองกลหนยนตจะอำนขอมลมมองศำตำงๆ ของทอนลำงและสำมำรถบนทกขอมลได เชน รปแบบกำรกำวเดน กำรลกนง กำรยน กำรขนลงบนได และอนๆ โดยขอมลทไดจะน ำไปประมวลผลในรปแบบฟงกชน ทงนหนวยประมวลผลหลกยงไดออกแบบส ำหรบฟงกชนขอควำมชวยเหลอ ใหผสวมใสสำมำรถรองขอควำมชวยเหลอได โดยกำรกดปมทไมเทำค ำยน ตวประมวลผลหลกจะสงขอมลไปยงโมดลสอสำร โดยใชสญญำณสอสำรเครอขำยโทรศพทภำยในประเทศ สงสญญำณไปยงเลขหมำยปลำยทำงทก ำหนด เพอใหผดแลหรอคนใกลชดไดท ำกำรตดตอและชวยเหลอในเบองตน โดยระบบนไดค ำนงถงควำมปลอดภยและหวงใยผสวมใส ซงอำจจะเกดอบตเหตในชวงเวลำทสวมใสชดโครงหนยนตชวยเดน
สวนของหนวยประมวลผลยอยจะท ำหนำทควบคมกำรมมองศำทมกำรเปลยนแปลง เพอใหเปนไปตำมรปแบบกำรเดน โดยตวประมวลผลยอยแตละสวนจะประมวลผลกำรท ำงำนและควบคมมมองศำทไดรบจำกตวประมวลผลหลก ซงจะควบคมกำรท ำงำนของอปกรณสงก ำลงใหท ำงำน พรอมกบตรวจสอบกำรหมนหรอมมองศำ ตำมทก ำหนด โดยหนวยประมวลผลยอย ถกออกแบบเปนระบบควบคมแบบปดท ำหนำทควบคมมมองศำ โดยคำมมองศำอำงองจะน ำมำเปรยบเทยบกบมมองศำทวดได น ำมำลบกนเพอหำคำผดพลำด(error) หำกคำทผดพลำดมำกกวำศนย ระบบจะท ำกำรประมวลผลตอไปเพอใหคำผดพลำดเขำใกลศนย โดยจะใหเกดควำมผดพลำดนอยทสด ก ำหนดให คำผดพลำด = คำเรมตน – คำทวดได
ภำพประกอบ 3.6 หลกกำรท ำงำนของหนวยประมวลผลยอย
Amplify Microcontroller
Sensor
Actuator θ
Slave Controller
-
+
Robot Platform
33
ภำพประกอบ 3.7 โฟรซำรทกำรท ำงำนของหนวยประมวลผลยอย
3.4.1 อปกรณขบเคลอนตนก าลง ชดอปกรณตนก ำลงถอวำเปนสวนส ำคญในกำรขบเคลอนกลไกตำงๆ ใหสำมำรถท ำงำนไดอยำงมประสทธภำพและใชงำนใหเหมำะสมกบรปแบบกำรท ำงำนทำงกลทใชงำน โดยในงำนวจยนไดออกแบบใหสำมำรถขบเคลอนดวยพลงงำนไฟฟำกระแสตรง ดงนนจงน ำระบบสงก ำลงชนดมอเตอรกระแสตรง (DC motor) มำใชงำน แตเนองจำกมหลำกหลำยรปแบบกำรใชงำน ซงค ำนงถงน ำหนกของผสวมใสเพอใหชดตนก ำลงสำมำรถสงก ำลงไดเหมำะสมกบน ำหนกของผใชงำน ซงกำรค ำนวณหำน ำหนกของขำมนษย โดยทวไปแลวน ำหนกของสวนตำงๆของรำงกำยจะคดเปนเปอรเซนตของน ำหนกโดยรวมของรำงกำย (ในบทท 2 แสดงในภำพประกอบ 2.4) โดยตวอยำงกำรค ำนวณน ำหนกขำของมนษย ทหนก 75 กโลกรม
ค ำนวณหำน ำหนกของขำทงหมด = 75 kg×9.6
100 = 7.2 กโลกรม
ค ำนวณหำน ำหนกของหนำแขง = 75 kg×4.5
100 = 3.3 กโลกรม
ค ำนวณหำน ำหนกของเทำ = 75 kg×1.4
100 = 1.05 กโลกรม
ค ำนวณหำน ำหนกของตนขำ = 7.2 – 3.3 – 1.05 = 2.85 กโลกรม
ดงนนเมอทรำบถงน ำหนกของทอนลำงในแตละสวนกสำมำรถออกแบบระบบสงก ำลงใหมแรงบด (Torque) สำมำรถขบ เคลอนกลไกในแตละสวนเพอยกน ำหนกของขำในแตละสวนได
โดยชดอปกรณควบคมกำรเคลอนทชนดสงก ำลงชนดมอเตอรกระแสตรง เปนอปกรณหลกในกำรขบเคลอนและสงก ำลง ซงเปลยนพลงงำนไฟฟำกระแสตรงใหเปนพลงงำนกลแบบหมนรอบแกน โดยชดสงก ำลงประกอบดวยตวมอเตอรและชดเกยรทดสงก ำลง น ำไปตดตงในระบบสงก ำลงของชดโครงหนยนตชวยเดนฯ แสดงดงภำพประกอบ 3.8
ประมวลผลมมองศา
ประมวลผลระบบสงก าลง
ประมวลผลมมองศาเปลยนแปลง
34
ภำพประกอบ 3.8 ชดสงก ำลงมอเตอรกระแสตรง
กำรค ำนวณหำประสทธภำพของมอเตอรเกยร ควำมเรว 50 รอบ/นำท แรงดน 24 VDC ควำมเรวรอบ 50 รอบ/นำท หรอเทำกบ 0.833 รอบ/วนำท จำก สมกำรแรงบด = ก ำลง / ควำมเรวเชงมม
ควำมเรวเชงมม, ω = 2𝜋 × ควำมเรวรอบตอนำท = 5𝜋
3 rad/sec
แรงบด = ก ำลง / ควำมเรวเชงมม = 4805𝜋
3
=91.67 𝑁. 𝑚 หรอ 9.167 𝑘𝑔. 𝑚
ดงนน กำรค ำนวณแรงบดของมอเตอรกระแสตรง สำมำรถน ำไปใชในระบบขบเคลอนกลไกได เพอเปนชดตนก ำลงในกำรเคลอนไหวของทอนลำง โดยไดถกออกแบบใหตดตงในสวนของสะโพกและขอเขำ
3.4.2 วงจรไมโครคอนโทรลเลอร กำรออกแบบวงจรของระบบควบคมดวยไมโครคอนโทรลเลอร ส ำหรบกำรควบคมกำรท ำงำนของระบบ โดยน ำไปใชในสวนของหนวยประมวลผลหลก และหนวยประมวลผลยอย ใหสำมำรถประมวลผล กำรท ำงำนภำยใตโปรแกรมคอมพวเตอรใหท ำงำนเฉพำะทำง ซงสำมำรถเชอมตอกบอปกรณภำยนอกในระดบสญญำณดจตอลและสญญำณอนำลอค โดยไมโครคอนโทรลเลอรมชองทำงกำรสอสำรขอมลอนกรม เพอเชอมตอกบอปกรณภำยนอกแบบเครอขำย รวมถงกำรสอสำรขอมลระหวำงไมโครคอนโทรลเลอรดวยกน
35
ภำพประกอบ 3.9 วงจรไมโครคอนโทรลเลอรระบบควบคมสมองกล
ภำพประกอบ 3.9 เปนวงจรของสวนตวประมวลผลดวยไมโครคอนโทรลเลอร ATMEGA2560-16AU แบบ 8-bit Microcontroller มหนวยควำมจ ำ 256 กโลไบค (Kbyte) มหนวยควำมจ ำแบบ EEPROM จ ำนวน 4 Kbyte และหนวยควำมจ ำภำยในแบบ SRAM จ ำนวน 8 Kbyte โดยรองรบอนพทและเอำทพท 86 บต, สญญำณควำมถ (PWM) รองรบ 12 บต ทงยงกำรสอสำรแบบอนกรม (Serial Communication) จ ำนวน 4 ชด และชองรบสญญำณอนำลอก (ADC) จ ำนวน 16 ชองสญญำณ โดยแรงดนไฟฟำทใชงำน 5 VDC โดยไมโครคอนโทรลเลอรจะมกำรสงสญญำณตำงๆ ดงน รบอนพทจำกปมไมเทำค ำยน จ ำนวน 2 บต คอ PD7, PG2 ส ำหรบเปดปดกำรท ำงำนของระบบ อนพทจำกอปกรณเซนเซอรตรวจวดมม จ ำนวน 2 บต คอ PA0, PA1 ส ำหรบกำรอำนขอมลควำมโนมเอยงและควำมเรง และสงสญญำณเอำทพทใหกบโมดลของระบบสอสำรเครอขำย GSM จ ำนวน 2 บต คอ PB4-PB5 ส ำหรบกำรสอสำรขอมลผำนเครอขำยโทรศพท และสงสญญำณอนพทและเอำทพท ใหกบโมดลสอสำร RS485 จ ำนวน 2 บต คอ TXD0, RXD1 ส ำหรบกำรสอสำรขอมล
36
3.4.3 วงจรขบเคลอนมอเตอร กำรออกแบบวงจรระบบขบเคลอนมอเตอรกระแสตรง ไดออกแบบใหสำมำรถควบคมทศทำงในกำรหมนของมอเตอร แบบกำรหมนตำมเขมนำฬกำ (Counter Wise: CW) กำรหมนทวนเขมนำฬกำ (Counter Clock Wise: CCW) และควำมเรวของมอเตอร ส ำหรบขบเคลอนมอเตอรกระแสตรง โดยออกแบบใหรองรบกำรท ำงำนของมอเตอรเฉพำะตว เพอใหระบบขบเคลอนท ำงำนไดอยำงมประสทธภำพและสำมำรถควบคมทศทำงของมอเตอรดวย ระบบไมโครคอนโทรลเลอร
ภำพประกอบ 3.10 วงจรขบเคลอนมอเตอรกระแสตรง
จำกภำพประกอบ 3.10 วงจรขบเคลอนมอเตอรกระแสตรง สำมำรถรองรบแรงดนอนพท 12 VDC ถง 24 VDC เชอมตอแหลงจำยไฟจำกแบตเตอร เขำมำทขำ +VM เขำทขำ EXT GND เพอน ำไปใชงำนในสวนของมอเตอรกระแสตรง โดยเชอมตอกบวงจรในทขำ M+ และ M- เปนจดเชอมตอเขำกบมอเตอร โดยกำรท ำงำนของมอเตอร จะรบสญญำณจำกไมโครคอนโทรลเลอร จ ำนวน 3 ชองสญญำณ ดงตอไปน - ชองสญญำณท 1 ขำสญญำณ ENA เปนขำสญญำณทใชควบคมกำรเปดปดกำรท ำงำนของมอเตอร ดวยสญญำณระดบโลจก ‘0’ คอหยดกำรท ำงำน และ ‘1’ คอเปดกำรท ำงำน และสำมำรถควบคมควำมเรวของมอเตอรไดโดยใชสญญำณ PWM จำกไมโครคอนโทรลเลอร - ชองสญญำณท 2 ขำสญญำณ DIR_1 เปนขำสญญำณทใหควบคมทศทำงกำรหมนของมอเตอร ใหมทศทำงกำรหมนไปตำมเขมนำฬกำ ดวยสญญำณระดบโลจก ‘0’ คอหยดกำรท ำงำน และ ‘1’ คอเปดกำรท ำงำน
37
- ชองสญญำณท 3 ขำสญญำณ DIR_2 เปนขำสญญำณทใหควบคมทศทำงกำรหมนของมอเตอร ใหมทศทำงกำรหมนไปทวนเขมนำฬกำ ดวยสญญำณระดบโลจก ‘0’ คอหยดกำรท ำงำน และ ‘1’ คอเปดกำรท ำงำน
โดยทง 3 สญญำณนจะสงสญญำณผำน IC PC817 (OPTO ISOLATOR) เปนสวตชอเลกทรอนกสเปดปดกำรท ำงำนดวยแสง เพอใชในกำรท ำงำนของระดบแรงดนทแตกตำงกน ระหวำงระดบแรงดนต ำกบแรงดนสง และปองกนกำรไหลยอนกลบของกระแส เขำสขำสญญำณของไมโครคอนโทรลเลอร โดย PC817 จะเปนตวสงสญญำณใหกบ Power MOFET IRF9540 และ IRF9540N ซงออกแบบเปนวงจร H-BRIDGE แบบ P&N channel ท ำหนำทขบเคลอนมอเตอรใหท ำงำนภำยใตกำรควบคมดวยไมโครคอนโทรลเลอร
3.4.4 การออกแบบผงการท างานของโปรแกรม กำรออกแบบผงกำรท ำงำนของชดโครงหนยนตชวยเดนฯ มควำมส ำคญในกำรท ำงำนของระบบสมองกล เพอระบบสมองกลสำมำรถท ำงำนไดอยำงมประสทธ มแบบแผนกำรท ำงำนตำมผงกำรท ำงำน ใหสอคคลองกบอปกรณตำงๆและกลไกกำรท ำงำนของโครงสรำงทไดออกแบบไว
หลกกำรท ำงำนของผงกำรท ำงำน โดยเรมตนกำรท ำงำนจะอำนคำสถำนะของชดโครงหนยนตชวยเดนฯ เพอตรวจสอบจดหมนของสวนสะโพกและขอเขำทงสองขำง ใหทรำบถงกำรเรมตนกำรใชงำนของผสวมใสวำอยในสถำนะใด สำมำรถสวมใสไดทงแบบนงและแบบยน โดยท ำกำรตรวจสอบจำกอปกรณเซนเซอรวดมมทตดตงในจดหมนของทอนลำง เมอทรำบถงสถำนะของชดโครงสรำงหนยนตชวยเดนฯ จะท ำกำรตรวจเชคสถำนะของสวทชแบบปมกด ทถกตดตงในสวนของไมเทำค ำยน ใหผสวมใสไดสงกำรใหท ำงำนเพอเลอกฟงกชนกำรท ำงำน โดยจะท ำกำรตรวจสอบกำรสงกำรอยในเงอนไขใดเชน กำรยน กำรเดน กำรนง หำกไมมกำรกดปม โปรแกรมจะสงกำรใหหยดกำรสงกำรเสมอนหยดท ำงำน และโปรแกรมจะสงงำนใหกลบมำท ำงำนอกครงเมอมกำรกดปม หลงจำกทไดเลอกฟงกชนกำรท ำงำนแลวโปรแกรมจะสงใหระบบควบคมสงงำนมอเตอรในแตละสวน ท ำกำรหมนไปตำมรปแบบทไดโปรแกรมไว ดงแผนผงกำรท ำงำนของโปรแกรม ดงภำพประกอบ 3.11 และภำพประกอบ 3.12
38
ภำพประกอบ3.11 แผนผงกำรท ำงำนของโปรแกรมหลก
False
ท าการนง
เรมตนการท างาน
ค าสงยน ท าการลกยน
ค าสงนง
True
หยดการท างาน
ค าสงเดน
ค าสงหยด
ท าการเดน
จบการท างาน
สถานะหนยนต
โหมดการท างาน
True
True
True
False
False
False
ป มกด
สถานะป มกด
True
39
ภำพประกอบ3.12 แผนผงกำรท ำงำนของโปรแกรมยอยสถำนะหนยนต
อานคาจากเซนเซอร (สะโพก ขอเขา)
สถานะยน
เรมตน
ค านวณมมองศา (สะโพก ขอเขา)
มมองศา < 100
สถานะนง
ก าหนดสถานะชดเรมตน
สนสด
True
False
40
บทท 4 ผลการทดสอบการท างาน
4.1 การตดตงและการใชงาน 4.1.1 การตดตงชดโครงหนยนตชวยเดนส าหรบผทพพลภาพครงลาง
ผสวมใสจะตองอยในลกษณะทานงเพอสามารถยดตดดวยตนเอง โดยวธการตดตงคอชดโครงหนยนตชวยเดนฯ จะตองจดวางใหอยในลกษณะทาทางการนง จากนนใหผสวมใสเคลอนยายตวเองขนมานงบรเวณชองวางของเครองเพอสวมใส จากนนท าการรดสายรดตางๆ โดยเรมจากรดสายรดทขอเทาทงสองขาง จากนนรดสายรดหนาแขง รดสายรดขอเขา รดสายรดตนขา รดสายรดหนาทอง และรดสายรดหนาอก ใหแนนและกระชบตามล าดบ เพอใหรางกายสวนทอนลางเคลอนไหวพรอมกบชดโครงหนยนตชวยเดนฯในขณะท างาน จากนนกดปมจากไมเทาค ายนเครองจะท าการยกรางกายใหยนขน และใชไมเทาค ายนในการประคองตว เพอไมใหเกดการลมหรอเอยงเอนไปในทางใดทางหนง แสดงดงภาพประกอบ 4.1
ภาพประกอบ 4.1 ชดตนแบบโครงหนยนตชวยเดนฯ พรอมอปกรณไมเทาค ายน
โดยผสวมใสควรสวมกางเกงใหมดชด เนองจากผวรางกายจะสมผสกบชนสวนของชดโครงหนยนตโดยตรง เพอลดการเสยดสทอาจจะท าใหบาดเจบไดและตองสวมใสกางเกงใหกระชบและรดกม โดยควรทจะใชไมเทาค ายนเนองจากผปวยไมเคยสมผสการทรงตวเปนระยะเวลานาน จงตองเรมตนการใชไมเทาค ายนชวยในการเคลอนไหว ทงนการสวมใสดวยตนเองจะเปนการสรางความคนเคย ในการใช
สายรดหนาอก
สายรดหนาทอง
สายรดตนขา
สายรดหนาแขง
สายรดขอเทา
สายรดขอเขา ไมเทาค ายน
41
งานและสามารถชวยเหลอตนเองได ทงนในสวนของจดหมนขอเทาไมไดน ามาใชงานในระบบ โดยยดตดไมมการเคลอนไหวในสวนขอเทา
4.1.2 ขนตอนการใชงานชดโครงหนยนตชวยเดนส าหรบผปวยอมพาตครงลาง
การใชงานชดโครงหนยนตชวยเดนฯ ขนตอนการเปลยนสถานะจากการนงใหยน และสถานะยนใหกาวเดน เมอตองการถอดชดโครงหนยนตชวยเดนฯ ขนตอนการเปลยนสถานะจากการยนใหนง เพอท าการถอดชดโครงหนยนตชวยเดนฯ ทงนการเปลยนสถานะตางๆ ผสวมใสตองท าการกดปมทไมเทาค ายน โดยม 2 ปมกด คอสเขยวและสแดง สถานะการท างานคอกดปมคางไว หากมการปลอยปมกดระบบสมองกลจะตดการท างานของระบบขบเคลอนทนท โดยมฟงกชนการใชงานดงน
1) กดปมสเขยว จะท าหนาทเรมการท างานของฟงกชนการกาวเดน 2) กดปมสแดง จะท าหนาทเรมการท างานของฟงกชนการลกและการนง
3) กดปมสแดงและกดปมสเขยวพรอมกน จะท าหนาทแจงสถานะฉกเฉนเพอขอความชวยเหลอ
ระบบสมองกลจะอานสถานะของชดโครงหนยนตชวยเดนฯ เพอปองกนการใชงานทผดพลาด เชน หากมการกดปมสเขยวในขณะสถานะการนง ระบบสมองกลจะปฏเศษการท างาน เนองจากอยในสถานะนงไมพรอมทจะกาวเดน ดงนนจงตองกดปมสแดงเพอเปลยนสถานะการนงใหเปนสถานะการยนใหเสรจสน จงสามารถทจะกาวเดนไดโดยกดปมสเขยว ในขณะทระบบท างานหากมการปลอยปมกด ระบบสมองกลจะหยดการท างานของระบบขบเคลอนเชนกน
ภาพประกอบ 4.2 วธการใชงานชดโครงหนยนตชวยเดนฯ ส าหรบสถานะการนง การยนและการเดน
42
4.2 ผลการทดสอบ 4.2.1 ระบบควบคมพไอด (PID Control) ระบบควบคมพไอดส าหรบการควบคมต าแหนงส าหรบมมองศาของสะโพกและขอเขา ของ
ชดโครงหนยนตชวยเดนฯ โดยอนพทคอมมองศาการเคลอนไหวส าหรบมมองศาของสวนสะโพกและสวนขอเขา สวนเอาทพทคอผลต าแหนงมมองศาทผานกระบวนการประมวลผลดวยระบบควบคมแบบพไอด และสมการจนศาสตรทางกล (Kinematics Model) โดยระบบควบคมพไอด ท าการปรบแตงคา Kp, Ki และ Kd เพอใหการควบคมมเสถยรภาพ และตอบสนองใหรวดเรว ทงนระบบสามารถท างานไดอยางมประสทธภาพ ขนอยกบการปรบแตงคา Kp, Ki และ Kd (หนวยคาคงท:unit)
ภาพประกอบ 4.3 ระบบควบคมแบบพไอด
จากภาพประกอบ 4.3 ก าหนดให θref. คออนพทของต าแหนงมมองศาของทอนลาง ซงอนพทในสวนของสะโพก θhip_pattern และขอเขา θknee_pattern ส าหรบรปแบบทาทางการเคลอนไหวของการลกนง และการเดน โดยน าผลลพธจากการค านวณคาผดพลาดระหวางอนพทและเอาทพทมาประมวลผลน าไปประมวลผลดวยตวควบคมพไอด เพอน ามาค านวณในสมการของ Kp, Ki และ Kd และน าผลลพธทได น าไปใชค านวณในสมการ kinematics model สมการท 4.1 และสมการท 4.2 โดยผลลพธน าไปค านวณอกครง จนกวาผลลพธของคาผดพลาดระหวางอนพทและเอาทพท จะเขาใกลศนยหรอคาต าสด
222
)21
(,
2
222
)21
(12tan
yx
kneeSyl
kneeCllx
yx
kneeSyl
kneeCllx
Ahip
4.1
212
22
21
22
,
212
22
21
22
12tanll
llyx
ll
llyxA
knee 4.2
Kinematics Model Σ Σ + θoutput
-
θref.
𝐾𝑝 𝑒(𝑡)
𝐾𝑖 𝑒(𝑡)𝑑𝑡𝑡
0
𝐾𝑑 𝑑𝑒(𝑡)
𝑑𝑡
43
ผลการทดสอบการปรบคา Kp, Ki และ Kd โดยก าหนดคาอนพทต าแหนงอางอง 92 องศา ก าหนดคาควบคมพไอด ให Kp = 1, Ki = 1, Kd = 1
ภาพประกอบ 4.4 แสดงผลลพธการควบคมต าแหนงดวยระบบควบคมพไอด Kp = 1, Ki = 1, Kd = 1
ภาพประกอบ 4.5 แสดงคาต าแหนงของความผดพลาดระบบควบคมพไอด Kp = 1, Ki = 1, Kd = 1
44
ผลการทดสอบการปรบคา Kp, Ki และ Kd โดยก าหนดคาอนพทต าแหนงอางอง 92 องศา ก าหนดคาควบคมพไอด ก าหนดให Kp = 30, Ki = 10, Kd = 2
ภาพประกอบ 4.6 แสดงผลลพธการควบคมต าแหนงดวยระบบควบคมพไอด Kp =30, Ki =10, Kd =2
ภาพประกอบ 4.7 แสดงคาต าแหนงของความผดพลาดระบบควบคมพไอด Kp =30, Ki =10, Kd =2
45
ผลการทดสอบการปรบคา Kp, Ki และ Kd โดยก าหนดคาอนพทต าแหนงอางอง 92 องศา ก าหนดคาควบคมพไอด ก าหนดให Kp = 50, Ki = 30, Kd = 1
ภาพประกอบ 4.8 แสดงผลลพธการควบคมต าแหนงดวยระบบควบคมพไอด Kp =50, Ki =30, Kd =1
ภาพประกอบ 4.9 แสดงคาต าแหนงของความผดพลาดระบบควบคมพไอด Kp =50, Ki =30, Kd =1
46
จากผลลพธการทดสอบปรบคา Kp, Ki และ Kd จะสงเกตไดถงคาความผดพลาดของการควบคมต าแหนงมมองศา โดยระบบควบคมจะท าการประมวลผลตามคาของ Kp, Ki และ Kd หากมคาเขาใกลศนย จะท าใหระบบตอบสนองชาและอตราคาความผดพลาดสง หากมการปรบคาของ Kp, Ki และ Kd เพมขนจะท าใหระบบตอบสนองไดรวดเรว อยางมประสทธภาพหากมการปรบคาเพมขน ระบบจะขาดเสถยรภาพและไมตอบสนองในชวเวลาขณะหนง ดงนนผลลพธจากการทดสอบท าใหทราบถงการปรบคาของระบบควบคมพไอด โดย Kp = 30, Ki = 10, Kd = 2 น าไปใชในระบบควบคมต าแหนง
4.2.2 การเกบขอมลส าหรบการลกและนง การเกบขอมลในลกษนะทาทางการลกและการนงส าหรบคนปกต โดยสวมใสชดเกบขอมล
และท าการลกและนง จ านวน 5 ครง โดยการเกบขอมลจากการอานเซนเซอรเอนโคดเดอร ทตดตงอยทสะโพกและหวเขาทงสองขาง และท าการอานขอมลและบนทกขอมลทกๆ 100 msec โดยการทดสอบเปลยนสถานะจากสถานะทานงเปนทาทางการยน (1)-(4) และสถานะทาทางยนเปนทาทางการนง (4)-(6) รวมเปน 1 ไซเคล หรอ 1 รอบ ดงทแสดงภาพประกอบ 4.10 และภาพประกอบ 4.11 การเกบขอมลสถานะการนงไปสถานะยนและสถานะยนไปสถานะนง ตดตอกนจ านวน 5 ครง
(1) (2) (3) (4) (5) (6)
ภาพประกอบ 4.10 ขอมลการเคลอนไหวจากสถานะนงถงสถานะยนและสถานะยนถงสถานะนง
ภาพประกอบ 4.11 ขอมลต าแหนงมมองศาของการเคลอนไหวสถานะการลกนงรวม 5 รอบ
ระยะ (1)-(4)
ระยะ (4)-(6)
1 Cycle
47
4.2.3 ระบบควบคมการลกและนง ส าหรบการควบคมสถานะการนงและการยน โดยน าขอมลรปแบบการนงตามรปแบบไซเคล (1) ถง (4) และการยนตามรปแบบไซเคล (4) ถง (6) เพอใหชดโครงหนยนตชวยเดนฯ สามารถเคลอนไหวตามรปแบบทไดก าหนดไว โดยระบบควบคมจะอานขอมลจากไฟลดวยโปรแกรม และประมวลผลใหเคลอนทตามรปแบบมมองศาทก าหนดไว ในแตละสวนของจดหมน โดยผลการทดลองระบบควบคมต าแหนงองศา แสดงดงภาพประกอบ 4.12 ถงภาพประกอบ 4.19 ส าหรบการควบคมต าแหนงองศาการเคลอนทตามรปแบบและคาผดพลาดของระบบควบคมต าแหนง
ภาพประกอบ 4.12 การควบคมต าแหนงการลงนงของสะโพกดานซาย
ภาพประกอบ 4.13 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการลงนง สะโพกดานซาย
48
ภาพประกอบ 4.14 การควบคมต าแหนงการลงนงของขอเขาดานซาย
ภาพประกอบ 4.15 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการลงนง ขอเขาดานซาย
49
ภาพประกอบ 4.16 การควบคมต าแหนงการลงนงของสะโพกดานขวา
ภาพประกอบ 4.17 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการลงนง สะโพกดานขวา
50
ภาพประกอบ 4.18 การควบคมต าแหนงการลงนงของขอเขาดานขวา
ภาพประกอบ 4.19 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการลงนง ขอเขาดานขวา
51
4.2.4 การเกบขอมลการกาวเดนครงละกาว จากการทดลองใหคนปกตใสชดเกบขอมลและท าการเดนไปขางหนาทละกาว โดยเรมการ
เกบขอมลส าหรบรปแบบการกาวเดนขางขวา และตามดวยซายและหยดยนทาตรง โดยการเกบขอมลจะเกบจากการอานจากอปกรณเซนเซอรเอนโคดเดอร ท าการอานขอมลทกๆ 100 msec โดยเรมจาก (1) การยนตรง (2) กาวเทาขวา (3) กาวเทาซาย (4) หยดทายนตรง เปนรปแบบการกาวเดนรวม 1 ไซเคล ดงทแสดงภาพประกอบ 4.20 และภาพประกอบ 4.21 การเกบขอมลสถานะการกาวเดนหนงกาว ตดตอกนจ านวน 5 กาว
(1) (2) (3) (4)
ภาพประกอบ 4.20 การบนทกขอมลการเคลอนไหวส าหรบการกาวเดนครงละหนงกาว
ภาพประกอบ 4.21 ขอมลต าแหนงมมองศาของการกาวเดนครงละหนงกาวตดตอกนรวม 5 รอบ
1 Cycle
52
4.2.5 ระบบควบคมการกาวเดนครงละกาว (Single phase) ส าหรบการควบคมสถานะการกาวเดนครงละกาว โดยน าขอมลรปแบบการนงตามรปแบบไซเคล (1) ถง (4) เพอใหชดโครงหนยนตชวยเดนฯ สามารถเคลอนไหวตามรปแบบทไดก าหนดไว โดยระบบควบคมจะอานขอมลจากไฟลดวยโปรแกรม และประมวลผลใหเคลอนทตามมมองศาในแตละสวนของจดหมนทก าหนดไว แสดงดงภาพประกอบ 4.22 ถงภาพประกอบ 4.29 ส าหรบการควบคมต าแหนงองศาการเคลอนทตามรปแบบและคาผดพลาดของระบบควบคมต าแหนง
ภาพประกอบ 4.22 การควบคมต าแหนงการกาวเดนของสะโพกดานซาย
ภาพประกอบ 4.23 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการกาวเดน สะโพกดานซาย
53
ภาพประกอบ 4.24 การควบคมต าแหนงการกาวเดนของขอเขาดานซาย
ภาพประกอบ 4.25 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการกาวเดน ขอเขาดานซาย
54
ภาพประกอบ 4.26 การควบคมต าแหนงการกาวเดนของสะโพกดานขวา
ภาพประกอบ 4.27 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการกาวเดน สะโพกดานซาย
55
ภาพประกอบ 4.28 การควบคมต าแหนงการกาวเดนของสะโพกดานขวา
ภาพประกอบ 4.29 คาความผดพลาดของระบบควบคมส าหรบมมองศาของการกาวเดน สะโพกดานขวา
56
4.2.6 การเกบขอมลรปแบบการเดน จากการทดลองใหคนปกตสวมใส ท าการเกบขอมลการเดนโดยการกาวไปดานหนา เพออาน
ต าแหนงมมองศาของการเดนของคนปกต โดยการเรมกาวเดนจากทายน เรมกาวเทาขวา กาวเทาซายและหยดทายน โดยการเกบขอมลจะเกบจากการอานอปกรณเซนเซอรเอนโคดเดอร ท าการอานขอมลทกๆ 100 msec โดยเรมจาก (1) การยน (2) กาวเทาขวาไปขางหนา (3) วางเทาขวาเตรยมยกเทาซาย (4) กาวเทาซายไปขางหนา (5) วางเทาซาย (6) กาวเทาขวา (7) หยดทายน แสดงดงภาพประกอบ 4.30 และภาพประกอบ 4.31 ขอมลต าแหนงองศาของรปแบบการเดน
(1) (2) (3) (4) (5) (6) (7)
ภาพประกอบ 4.30 การบนทกขอมลการเดนของผสวมใส
ภาพประกอบ 4.31 ขอมลต าแหนงมมองศาการเดนของทอนลางจ านวน 5 รอบ
1 Cycle
57
4.2.7 ระบบควบคมรปแบบการกาวเดน ส าหรบการควบคมสถานะการกาวเดนครงละกาว โดยน าขอมลรปแบบการนงตามรปแบบ
ไซเคล (1) ถง (4) เพอใหชดโครงหนยนตชวยเดนฯ สามารถเคลอนไหวตามรปแบบทไดก าหนดไว โดยระบบควบคมจะอานขอมลจากไฟลดวยโปรแกรม และประมวลผลใหเคลอนทตามมมองศาในแตละสวนของจดหมนทก าหนดไว แสดงดงภาพประกอบ 4.32 ถงภาพประกอบ 4.39 ส าหรบการควบคมต าแหนงองศาการเคลอนทตามรปแบบและคาผดพลาดของระบบควบคมต าแหนง
ภาพประกอบ 4.32 การควบคมต าแหนงตามรปแบบการเดนของสะโพกดานซาย
ภาพประกอบ 4.33 คาความผดพลาดส าหรบมมองศาการเดนสวนสะโพกดานซาย
58
ภาพประกอบ 4.34 การควบคมต าแหนงตามรปแบบการเดนของขอเขาดานซาย
ภาพประกอบ 4.35 คาความผดพลาดของมมองศาการเดนสวนขอเขาดานซาย
59
ภาพประกอบ 4.36 การควบคมต าแหนงตามรปแบบการเดนของสะโพกดานขวา
ภาพประกอบ 4.37 คาความผดพลาดของมมองศาการเดนสวนสะโพกดานขวา
60
ภาพประกอบ 4.38 การควบคมต าแหนงตามรปแบบการเดนของขอเขาดานขวา
ภาพประกอบ 4.39 คาความผดพลาดของมมองศาการเดนสวนขอเขาดานขวา
61
4.3 สรปผลการทดลอง ผลการทดลองการเกบขอมล จากผสวมใส สง 173 เซนตเมตร น าหนก 70 เซนตเมตร
ส าหรบการเกบขอมลรปแบบการลกนง การกาวเดน และการเดน โดยเกบขอมลต าแหนงองศาการเคลอนไหวครงทอนลาง โดยการตรวจวดต าแหนงมมองศาสามารถตอบสนองตอการเคลอนไหวไดอยางมประสทธภาพ และสามารถเขาใจรปแบบการเคลอนไหวของผสวมใสไดอยางชดเจน ระยะในการกาวเดนเฉลย 0.25 เมตรตอกาว ความเรวเฉลย 0.15 เมตรตอวนาท
ผลการทดลองระบบควบคมดวยพไอด สามารถควบคมต าแหนงของการเคลอนไหวของสวนสะโพก และขอเขา โดยน าคาต าแหนงองศาจากการเกบขอมลเปนอนพทของระบบควบคม โดยผลการทดสอบการควบคมต าแหนงองศา ตามรปแบบการลกนง การกาวเดน การเดน มคาองศาผดพลาดของต าแหนงสวนของสะโพกเฉลยเทากบ 8 องศา และคาองศาผดพลาดของต าแหนงสวนของสะโพกเฉลยเทากบ 12 องศา โดยคาความผดพลาดสงสดของต าแหนงมมองศา ในสวนของสะโพกและขอเขา แสดงดงตารางท 4.1
ตารางท 4.1 คาความผดพลาดสงสดของต าแหนงมมองศา องศาความผดพลาด (Degree)
Maximum point of position error การลกนง การกาวเดน การเดน
ขาดานซาย ขาดานขวา ขาดานซาย ขาดานขวา ขาดานซาย ขาดานขวา สะโพก (Hip) 5 7 8 9 9 10 ขอเขา (Knee) 9 6 12 14 15 17
ความผดพลาดทเกดขน อาจเกดจากระบบทางกล เชนระบบเฟองทดก าลง กลไกการท างาน การยดประกอบชนงาน ซงท าใหการอานขอมลต าแหนงองศาของสวนตางๆ เกดความคลาดเคลอนไดในขณะปฏบตงาน
62
บทท 5 สรป อภปรายผล และขอเสนอแนะ
5.1 สรปผลการวจย
โครงสรางหนยนตตนแบบสามารถปรบระยะความสงของผสวมใส ในชวง 150 - 180 เซนตเมตร รองรบน าหนกผสวมใสไมเกน 90 กโลกรม สามารถปรบความเรวสงสดในการเดนได 0.5 m/sec. และรองรบการทางานไดไมเกน 3 ชวโมง โดยออกแบบใหมจดยดสวนขารวม 10 จด ทาใหกระชบ และสามารถสวมใสดวยตนเองได ในสถานะทาทางการนง โดยมฟงกชนในการลกนง การกาวเดน การเดน และมฟงกชนฉกเฉนสาหรบการรองขอความชวยเหลอ ผานระบบเครอขายสญญาณโทรศพทภายในประเทศ
ผลการทดลองการเกบขอมลตาแหนงองศาจากชดโครงสรางหนยนตชวยเดนฯ ในบทท 4 ดงกราฟแสดงตาแหนงองศาการเคลอนไหวของครงทอนลาง จะเหนวามมองศาในการลกนงและการเดน มรปแบบใกลเคยงกน แตกตางชวงมมองศาการกาวเดน โดยขอมลตาแหนงองศาการเคลอนไหวของคนปกต ไดนามาเปนขอมลอนพทสาหรบตนแบบการเคลอนทครงทอนลาง โดยทาการควบคมตาแหนงองศาใหเคลอนไหวตามตนแบบ ไดประยกตระบบควบคมแบบพไอด มาใชควบคมตาแหนงองศาการเคลอนไหวของครงทอนลาง โดยทาการควบคมสวนของสะโพกและขอเข า โดยสามารถเคลอนไหวตาแหนงองศาไดตามขอมลตนแบบดงผลการทดสอบการทางาน โดยมคาตาแหนงองศาผดพลาดสงสด สวนขอเขา 17 องศา และสวนสะโพก 15 องศา คาตาแหนงองศาผดพลาดตาสด สวนขอเขา 0.05 องศา และสวนสะโพก 0.02 องศา
งานวจยสงประดษฐน มวตถประสงคเพอออกแบบและสรางตนแบบ เครองชวยเดนสาหรบผปวยทพพลภาพครงทอนลาง โดยมงเนนผทมความบกพรองทางรางกายครงทอนลาง ไรความรสก และไมสามารถเคลอนไหวไดดวยตนเอง ท งยงเปนอปกรณทางการแพทยอกทางเลอกหนง ใหผสวมใสไดเคลอนไหวรางกายทอนลางและการเปนแผลกดทบและชวยกระตนกลามเน อทอนลาง ยงเปนการชวยสงเสรมคณภาพชวตใหดข น ใหสามารถดารงคชวตประจาวนข นพ นฐานไดตอไป
5.2 ปญหาและอปสรรค
- โครงสรางมน าหนก 25 กโลกรม ทาใหเปนอปสรรคในการเคลอนยาย - ขนาดของแบตเตอรทใชงานมขนาดใหญ และน าหนกมาก - อปกรณมอเตอรขบเคลอนมจานวนรอบการทางานไมเทากน ความเรวรอบตางกน - อปกรณสายรดมความบกพรองรดไมกระชบจงทาใหเวลาทดสอบมการหลวมในบรเวณทรด
เกดข น
63
- อปกรณและช นสวนของกลองชดควบคม มน าหนกใหญตองใชพ นทในการตดต ง จงทาใหชดโครงหนยนตมน าหนกเกนความตองการ
- การชารตแบตเตอร ใชเวลานาน ไมตากวา 3 ชวโมงตอคร ง - เสยงดง เกดจากระบบเกยรทดกาลงของมอเตอร - รปลกษณภายนอกไมนาใชงาน
5.3 ขอเสนอแนะ - ควรมความเขาใจในศาสตรของวสดวศวกรรมเพอเลอกใชวสดและอปกรณทมความเหมาะสม
น าหนกเบาและตองมความทดทานสง - พ นฐานความเขาใจในเรองกายภาพของมนษย โดยขอคาแนะนาและปรกษา หรอทาความ
รวมมอกบผเชยวชาญทางการแพทย เกยวกบกายภาพบาบด หรอสถานประกอบการเกยวกบสขภาพและกายภาพบาบด
- ระบบวเคราะหและประมวลผลควรมประสทธภาพสง ความเรวในการประมวลผล และทนทานตอทกสภาพอากาศ
- มาตรฐานเครองมอทางการแพทย เพอผลตช นงานในเชงพาณชย - มาตรฐานอปกรณกนน า สาหรบมาตรฐานการปองกนระบบไฟฟาอเลกทรอนกส เพอความ
ปลอดภยในการใชงาน
5.4 แนวทางการพฒนา
การพฒนาระบบทางกลและอปกรณ - เปลยนวสดทใชทาโครงสรางใหแขงแรงและน าหนกเบา เชน Carbon fiber, Polymer, PE
เปนตน - ระบบกลไกปองกนการหมนเกนตาแหนงของจดหมนในสวนสะโพก ขอเขาและขอเทา - ปรบโครงสรางใหรองรบชดกลไกระบบปองกนการลม - ปรบเปลยนมอเตอรขบเคลอนแบบ Blushless motor - ปรบเปลยนระบบทดกาลง Gear Box - เปลยนแหลงจายพลงงานใหมขนาดเลก เบา และกาลงไฟฟาสง เชน lithium-polymer
เปนตน
64
การพฒนาซอฟแวร - พฒนาโปรแกรมสาหรบการลกนง ยน การเดน และรปแบบกาวข นบนไดหรอทางตางระดบ - พฒนาโปรแกรมสาหรบระบบปองกนการลม - พฒนาโปรแกรมสาหรบระบบแจงเตอนการใชพลงงานตา - พฒนาโปรแกรมสาหรบระบบระบพกด GPS เพอตรวจสอบตาแหนงและตดตามผสวมใส - พฒนาโปรแกรมสาหรบระบบ การทากายภาพบาบดสวนขาแบบ manual, semi-
autonomous, Fully-autonomous
65
5.4 ผลงานการวจย
รางวลระดบนานาชาต 2 รางวล
“Leading Innovation Award” by International Intellectual Property Network Forum (IIPNF) at Taipei International Invention Show & Technomart” (INST 2015) ณ กรงไทเป ประเทศไตหวน “Honorable Mention” at Taipei International Invention Show & Technomart” (INST 2015) ณ กรงไทเป ประเทศไตหวน
รางวลระดบชาต 2 รางวล
รางวลนกคดสงประดษฐรนใหม ประจาป 2558 โดยสานกงานคณะกรรมการวจยแหงชาต (วช.) ผลงาน: รางวลชนะเลศ, กลมสงประดษฐเพอประโยชนทางการแพทยและสาธารณสข ประเภทอาชวศกษาและอดมศกษา หวขอ “หนยนตชวยเดนสาหรบผทพพลภาพครงทอนลาง”
การประกวดผลงานดานหนยนตเพอกระบวนการผลต (RACMP2014) ประเภทเพอสงคม ผลงาน: รางวลชนะเลศ, หวขอ “หนยนตชวยเดนสาหรบผทพพลภาพครงทอนลาง”
65
บรรณานกรม
66
บรรณานกรม
กรมควบคมโรค กระทรวงสาธารณสข. 2550. คมอความรเรองอมพาตส าหรประชาชน. กรงเทพมหานคร: ส านกงานหลกประกนสขภาพแหงชาต.
กลมงานการพยาบาล สถาบนประสาทวทยา กรมการแพทย กระทรวงสาธารณสข. 2543. คมอการพยาบาล ผปวยโรคหลอดเลอดสมอง. 2543:10-25.
เฉลมผล คงเขยว. 2553. “ระบบภาพเคลอนไหวสาหรบการวเคราะหการเดนของมนษย.” วทยานพนธปรญญามหาบณฑต สาขาวชาวศวกรรมไฟฟา บณฑตวทยาลยมหาวทยาลยสงขลานครนทร.
นพนธ พวงวรนทร. 2544. ระบาดวทยาโรคหลอดเลอดสมอง. พมพครงท 2. กรงเทพมหานคร : โรงพมพเรอนแกวการพมพ.
สถาบนประสาทวทยา กรมการแพทย กระทรวงสาธารณสข. 2550. คมอโรคหลอดเลอดสมอง ส าหรบประชาชน. พมพครงท 2. กรงเทพหานคร: ส านกงานกจการโรงพมพองคการสงเคราะหทหารผานศก
สธ สทศน ณ อยธยา. 2533. “กายวภาคศาสตรระบบการเคลอนไหว (Anatomy of the Locomotion System).” พมพครงท 2 : เมดคล มเดย.
Arron M. Dollar and Hugh Herr. 2007. “Active Orthoses for Lower-Limbs: Challenges and State of the Art.” IEEE 10th International Conference on Rehabilitation Robotics, July 13-15, 2007, Nertherland, 968-977.
Craig, J. J., 1989. “Introduction to Robotics Mechanics and Control.” Addision Wesley USA: Addision-Wesley Publishing Company.
Kawamoto, H., Lee, S., Kanbe, S., and Sankai, Y. 2003. “Power assist method for HAL-3 using EMG-based feedback controller.” IEEE International Conference on Systems, Man and Cybernetics, Oct. 5-8, 2003, Washington D.C., USA, 4: 1648–1653.
Kawamoto, H., and Sankai, Y., 2002. “Power assist system hal-3 for gait disorder person.” International Conference on Computers for Handicapped Persons, 9: July 7–9, 2002, Paris, France, 2398: 196–203.
Kazerooni, H. and Steger, R., 2006. “The Berkeley Lower Extremity Exoskeleton.” Transection ASME Journal of Dynamic System, Measurement and Control 128: 14–25.
67
Pratt, J. E., Krupp, B. T., Morse, C. J., and Collins, S. H., 2004. “The RoboKnee:An exoskeleton for enhancing strength and endurance during walking.” IEEE International Conference on Robotics and Automation, April 26 - May 1, 2004, New Orleans, LA, USA, 5: 2430–2435.
Rose, J. and Gamble, J. G., 1994. “HumanWalking.” 2nd ed. Baltimore: Williams & Wilkins.
Saeed, B., Niku, 2001. “Introduction to Robotics Analysis, Systems, Applications.” Prentice Hall. ISBN 0-13-061309-6 : 29-94.
Viet Anh Dung Cai, Philippe Bidaud, and Vincent Hayward. 2011. “Self-adjusting, Isostatic Exoskeleton for the Human Knee Joint.” International Conference of the IEEE Engineering in Medicine and Biology Society 33: Aug. 30 – Sep. 03, 2011, Boston, USA, 30: 612-618.
66
ภาคผนวก
67
ภาคผนวก ก
โปรแกรมคอมพวเตอร
68
#include <arduino.h> #include "SIM900.h" #include <SoftwareSerial.h> #include "sms.h" SMSGSM sms; #define SELECT_PIN 30 #define CLOCK_PIN 31 // Hip Left #define HL_DATA_PIN 32 // Hip Right #define HR_DATA_PIN 33 // Knee Left #define KL_DATA_PIN 34 // Knee Right #define KR_DATA_PIN 35 const byte SW1 = 36; const byte SW2 = 37; //Define Variables Motor // Hip Left const byte HLD1 = 22; const byte HLD2 = 23; const byte HLEN1 = 2; //Define Variables Motor // Hip Right const byte HRD1 = 24; const byte HRD2 = 25; const byte HREN1 = 3; //Define Variables Motor // Knee Left const byte KLD1 = 26; const byte KLD2 = 27; const byte KLEN1 = 4; //Define Variables Motor // Knee Right const byte KRD1 = 28; const byte KRD2 = 29; const byte KREN1 = 5;
//variables to keep track of position volatile unsigned int Data[2][2][2][2][2][2][2][2][2][2]; volatile int Angle[1024]; volatile int HL_Degree[360]; volatile int HR_Degree[360]; volatile int KL_Degree[360]; volatile int KR_Degree[360]; volatile int HL_Reading = 0; volatile int HR_Reading = 0; volatile int KL_Reading = 0; volatile int KR_Reading = 0; volatile unsigned char HL_PWM = 255; volatile unsigned char HR_PWM = 255; volatile unsigned char KL_PWM = 255; volatile unsigned char KR_PWM = 255; volatile int HL_WalkL[10] = {20,20,0}; volatile int HL_WalkR[10] = {-20,-20,0}; volatile int HL_Stand = 0; volatile int HL_Sit = 90; volatile int HR_WalkL[10] = {-20,-20,0}; volatile int HR_WalkR[10] = {20,20,0}; volatile int HR_Stand = 0; volatile int HR_Sit = 90; volatile int KL_WalkL[10] = {-30,0,0}; volatile int KL_WalkR[10] = {0,0,0}; volatile int KL_Stand = 0; volatile int KL_Sit = -90; volatile int KR_WalkL[10] = {0,0,0}; volatile int KR_WalkR[10] = {-30,0,0}; volatile int KR_Stand = 0; volatile int KR_Sit = -90; volatile char Start = 0;
69
volatile byte Pace = 0; volatile byte go = 0; volatile int speed1 = 0; volatile int speed2 = 0; volatile byte st = 1; volatile byte hl = 0; volatile byte hr = 0; volatile byte kl = 0; volatile byte kr = 0; void setup() { Serial.begin(9600); pinMode(SW1, INPUT); pinMode(SW2, INPUT); pinMode(HLD1, OUTPUT); pinMode(HLD2, OUTPUT); pinMode(HLEN1, OUTPUT); pinMode(HRD1, OUTPUT); pinMode(HRD2, OUTPUT); pinMode(HREN1, OUTPUT); pinMode(KLD1, OUTPUT); pinMode(KLD2, OUTPUT); pinMode(KLEN1, OUTPUT); pinMode(KRD1, OUTPUT); pinMode(KRD2, OUTPUT); pinMode(KREN1, OUTPUT); //setup our pins pinMode(CLOCK_PIN, OUTPUT); pinMode(SELECT_PIN, OUTPUT); pinMode(HL_DATA_PIN, INPUT); pinMode(HR_DATA_PIN, INPUT); pinMode(KL_DATA_PIN, INPUT); pinMode(KR_DATA_PIN, INPUT);
cli(); // stop interrupts TCCR1A = 0; // set entire TCCR1A register to 0 TCCR1B = 0; // same for TCCR1B TCNT1 = 0; //initialize counter value to 0 // set compare match register for 1hz increments OCR1A = 1562; // = ((16*10^6)*(100*10^-3)/1024)-1 (must be <65536) // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS12 and CS10 bits for 1024 prescaler TCCR1B |= (1 << CS12) | (1 << CS10); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei(); //give some default values digitalWrite(CLOCK_PIN, HIGH); digitalWrite(SELECT_PIN, HIGH); digitalWrite(SW1, HIGH); digitalWrite(SW2, HIGH); int k = 0; for(int a = 0; a < 2; a++) for(int b = 0; b < 2; b++) for(int c = 0; c < 2; c++) for(int d = 0; d < 2; d++) for(int e = 0; e < 2; e++) for(int f = 0; f < 2; f++) for(int g = 0; g < 2; g++) for(int h = 0; h < 2; h++) for(int i = 0; i < 2; i++) for(int j = 0; j < 2; j++)
70
{ Data[a][b][c][d][e][f][g][h][i][j] = k; k++; } for(int i = 0; i < 1024; i++) Angle[i] = (i / 1024.0) * 360.0; for(int i = 0; i < 360; i++) { if((i <= 86)&&(i >= 0)) HL_Degree[i] = i - 86; else if((i <= 266)&&(i >= 87)) HL_Degree[i] = i - 86; else if((i <= 359)&&(i >= 267)) HL_Degree[i] = i - 446; } for(int i = 0; i < 360; i++) { if((i <= 121)&&(i >= 0)) HR_Degree[i] = 121 - i; else if((i <= 300)&&(i >= 122)) HR_Degree[i] = 121 - i; else if((i <= 359)&&(i >= 301)) HR_Degree[i] = 481 - i; } for(int i = 0; i < 360; i++) { if((i <= 79)&&(i >= 0)) KL_Degree[i] = i - 79; else if((i <= 259)&&(i >= 80)) KL_Degree[i] = i - 79; else if((i <= 359)&&(i >= 260)) KL_Degree[i] = i - 439; }
for(int i = 0; i < 360; i++) { if((i <= 79)&&(i >= 0)) KR_Degree[i] = 79 - i; else if((i <= 258)&&(i >= 80)) KR_Degree[i] = 79 - i; else if((i <= 359)&&(i >= 259)) KR_Degree[i] = 439 - i; } Serial.println("CLEARDATA"); Serial.println("LABEL,Time,Hip Left,Knee Left,Hip Right,Knee Right"); delay(2000); } ISR(TIMER1_COMPA_vect)//timer1 interrupt 1Hz toggles pin 13 (LED) { //generates pulse wave of frequency 1Hz/2 = 0.5kHz (takes two cycles for full wave- toggle high then toggle low) readPosition(); PrintDegree(); } void loop() { if((digitalRead(SW1) == LOW)&&(digitalRead(SW2) == LOW)) { sms.SendSMS("0859232650", "Emergency"); while((digitalRead(SW1) == LOW)||(digitalRead(SW2) == LOW)); }
71
else if((digitalRead(SW1) == LOW)&&(digitalRead(SW2) == HIGH)) { if(st == 1) { if(Pace == 0) { for(int i = 0; i < 3; i++) { if((HL_Degree[Angle[HL_Reading]] < HL_WalkL[i])&& (HL_Degree[Angle[HL_Reading]] > -20)) { HL_CW(HL_PWM); } else if((HL_Degree[Angle[HL_Reading]] < 120)&& (HL_Degree[Angle[HL_Reading]] > HL_WalkL[i])) { HL_CCW(HL_PWM); } else { HL_STOP(); hl = 1; } if((HR_Degree[Angle[HR_Reading]] < HR_WalkL[i])&& (HR_Degree[Angle[HR_Reading]] > -20)) { HR_CCW(HR_PWM); }
else if((HR_Degree[Angle[HR_Reading]] < 120)&& (HR_Degree[Angle[HR_Reading]] > HR_WalkL[i])) { HR_CW(HR_PWM); } else { HR_STOP(); hr = 1; } if((KL_Degree[Angle[KL_Reading]] < KL_WalkL[i])&& (KL_Degree[Angle[KL_Reading]] > -90)) { KL_CW(KL_PWM); } else if((KL_Degree[Angle[KL_Reading]] < 0)&& (KL_Degree[Angle[KL_Reading]] > KL_WalkL[i])) { KL_CCW(KL_PWM); } else { KL_STOP(); kl = 1; } if((KR_Degree[Angle[KR_Reading]] < KR_WalkL[i])&& (KR_Degree[Angle[KR_Reading]] > -90))
72
{ KR_CCW(KR_PWM); } else if((KR_Degree[Angle[KR_Reading]] < 0)&& (KR_Degree[Angle[KR_Reading]] > KR_WalkL[i])) { KR_CW(KR_PWM); } else { KR_STOP(); kr = 1; } while(1) { if(HL_WalkL[i] == HL_Degree[Angle[HL_Reading]]) { HL_STOP(); hl = 1; } if(HR_WalkL[i] == HR_Degree[Angle[HR_Reading]]) { HR_STOP(); hr = 1; } if(KL_WalkL[i] == KL_Degree[Angle[KL_Reading]]) { KL_STOP();
kl = 1; } if(KR_WalkL[i] == KR_Degree[Angle[KR_Reading]]) { KR_STOP(); kr = 1; } if((hl == 1)&&(hr == 1)&&(kl == 1)&&(kr == 1)) { hl = 0; hr = 0; kl = 0; kr = 0; break; } } } Pace = 1; } else { for(int i = 0; i < 3; i++) { if((HL_Degree[Angle[HL_Reading]] < HL_WalkL[i])&& (HL_Degree[Angle[HL_Reading]] > -20)) { HL_CW(HL_PWM); } else if((HL_Degree[Angle[HL_Reading]] < 120)&&
73
(HL_Degree[Angle[HL_Reading]] > HL_WalkL[i])) { HL_CCW(HL_PWM); } else { HL_STOP(); hl = 1; } if((HR_Degree[Angle[HR_Reading]] < HR_WalkL[i])&& (HR_Degree[Angle[HR_Reading]] > -20)) { HR_CCW(HR_PWM); } else if((HR_Degree[Angle[HR_Reading]] < 120)&& (HR_Degree[Angle[HR_Reading]] > HR_WalkL[i])) { HR_CW(HR_PWM); } else { HR_STOP(); hr = 1; } if((KL_Degree[Angle[KL_Reading]] < KL_WalkL[i])&& (KL_Degree[Angle[KL_Reading]] > -90)) { KL_CW(KL_PWM);
} else if((KL_Degree[Angle[KL_Reading]] < 0)&& (KL_Degree[Angle[KL_Reading]] > KL_WalkL[i])) { KL_CCW(KL_PWM); } else { KL_STOP(); kl = 1; } if((KR_Degree[Angle[KR_Reading]] < KR_WalkL[i])&& (KR_Degree[Angle[KR_Reading]] > -90)) { KR_CCW(KR_PWM); } else if((KR_Degree[Angle[KR_Reading]] < 0)&& (KR_Degree[Angle[KR_Reading]] > KR_WalkL[i])) { KR_CW(KR_PWM); } else { KR_STOP(); kr = 1; } while(1) {
74
if(HL_WalkR[i] == HL_Degree[Angle[HL_Reading]]) { HL_STOP(); hl = 1; } if(HR_WalkR[i] == HR_Degree[Angle[HR_Reading]]) { HR_STOP(); hr = 1; } if(KL_WalkR[i] == KL_Degree[Angle[KL_Reading]]) { KL_STOP(); kl = 1; } if(KR_WalkR[i] == KR_Degree[Angle[KR_Reading]]) { KR_STOP(); kr = 1; } if((hl == 1)&&(hr == 1)&&(kl == 1)&&(kr == 1)) { hl = 0; hr = 0; kl = 0; kr = 0; break; }
} } Pace = 0; } } } else if((digitalRead(SW1) == HIGH)&&(digitalRead(SW2) == LOW)) { if(st == 2) { if((HL_Degree[Angle[HL_Reading]] < HL_Sit)&& (HL_Degree[Angle[HL_Reading]] > -20)) { HL_CW(HL_PWM); } else if((HL_Degree[Angle[HL_Reading]] < 120)&& (HL_Degree[Angle[HL_Reading]] > HL_Sit)) { HL_CCW(HL_PWM); } else { HL_STOP(); hl = 1; } if((HR_Degree[Angle[HR_Reading]] < HR_Sit)&& (HR_Degree[Angle[HR_Reading]] > -20)) { HR_CCW(HR_PWM);
75
} else if((HR_Degree[Angle[HR_Reading]] < 120)&& (HR_Degree[Angle[HR_Reading]] > HR_Sit)) { HR_CW(HR_PWM); } else { HR_STOP(); hr = 1; } if((KL_Degree[Angle[KL_Reading]] < KL_Sit)&& (KL_Degree[Angle[KL_Reading]] > -90)) { KL_CW(KL_PWM); } else if((KL_Degree[Angle[KL_Reading]] < 0)&& (KL_Degree[Angle[KL_Reading]] > KL_Sit)) { KL_CCW(KL_PWM); } else { KL_STOP(); kl = 1; } if((KR_Degree[Angle[KR_Reading]] < KR_Sit)&& (KR_Degree[Angle[KR_Reading]] > -90)) {
KR_CCW(KR_PWM); } else if((KR_Degree[Angle[KR_Reading]] < 0)&& (KR_Degree[Angle[KR_Reading]] > KR_Sit)) { KR_CW(KR_PWM); } else { KR_STOP(); kr = 1; } st = 1; } else { if((HL_Degree[Angle[HL_Reading]] < HL_Stand)&& (HL_Degree[Angle[HL_Reading]] > -20)) { HL_CW(HL_PWM); } else if((HL_Degree[Angle[HL_Reading]] < 120)&& (HL_Degree[Angle[HL_Reading]] > HL_Stand)) { HL_CCW(HL_PWM); } else { HL_STOP();
76
hl = 1; } if((HR_Degree[Angle[HR_Reading]] < HR_Stand)&& (HR_Degree[Angle[HR_Reading]] > -20)) { HR_CCW(HR_PWM); } else if((HR_Degree[Angle[HR_Reading]] < 120)&& (HR_Degree[Angle[HR_Reading]] > HR_Stand)) { HR_CW(HR_PWM); } else { HR_STOP(); hr = 1; } if((KL_Degree[Angle[KL_Reading]] < KL_Stand)&& (KL_Degree[Angle[KL_Reading]] > -90)) { KL_CW(KL_PWM); } else if((KL_Degree[Angle[KL_Reading]] < 0)&& (KL_Degree[Angle[KL_Reading]] > KL_Stand)) { KL_CCW(KL_PWM); }
else { KL_STOP(); kl = 1; } if((KR_Degree[Angle[KR_Reading]] < KR_Stand)&& (KR_Degree[Angle[KR_Reading]] > -90)) { KR_CCW(KR_PWM); } else if((KR_Degree[Angle[KR_Reading]] < 0)&& (KR_Degree[Angle[KR_Reading]] > KR_Stand)) { KR_CW(KR_PWM); } else { KR_STOP(); kr = 1; } st = 2; } } HL_STOP(); HR_STOP(); KL_STOP(); KR_STOP(); } void HL_CW(unsigned char PWM) {
77
digitalWrite(HLD1, HIGH); digitalWrite(HLD2, LOW); analogWrite(HLEN1, PWM); } void HL_CCW(unsigned char PWM) { digitalWrite(HLD1, LOW); digitalWrite(HLD2, HIGH); analogWrite(HLEN1, PWM); } void HR_CW(unsigned char PWM) { digitalWrite(HRD1, HIGH); digitalWrite(HRD2, LOW); analogWrite(HREN1, PWM); } void HR_CCW(unsigned char PWM) { digitalWrite(HRD1, LOW); digitalWrite(HRD2, HIGH); analogWrite(HREN1, PWM); } void KL_CW(unsigned char PWM) { digitalWrite(KLD1, HIGH); digitalWrite(KLD2, LOW); analogWrite(KLEN1, PWM); } void KL_CCW(unsigned char PWM) { digitalWrite(KLD1, LOW); digitalWrite(KLD2, HIGH); analogWrite(KLEN1, PWM);
} void KR_CW(unsigned char PWM) { digitalWrite(KRD1, HIGH); digitalWrite(KRD2, LOW); analogWrite(KREN1, PWM); } void KR_CCW(unsigned char PWM) { digitalWrite(KRD1, LOW); digitalWrite(KRD2, HIGH); analogWrite(KREN1, PWM); } void HL_STOP() { digitalWrite(HLD1, HIGH); digitalWrite(HLD2, HIGH); digitalWrite(HLEN1, HIGH); } void HR_STOP() { digitalWrite(HRD1, HIGH); digitalWrite(HRD2, HIGH); digitalWrite(HREN1, HIGH); } void KL_STOP() { digitalWrite(KLD1, HIGH); digitalWrite(KLD2, HIGH); digitalWrite(KLEN1, HIGH); } void KR_STOP() {
78
digitalWrite(KRD1, HIGH); digitalWrite(KRD2, HIGH); digitalWrite(KREN1, HIGH); } int readPosition() { byte HL[10]; byte HR[10]; byte KL[10]; byte KR[10]; //shift in our data for(int j = 0; j < 4; j++) { digitalWrite(SELECT_PIN, LOW); delayMicroseconds(1); for (int i=9; i>=0; i--) { digitalWrite(CLOCK_PIN, LOW); delayMicroseconds(1); digitalWrite(CLOCK_PIN, HIGH); delayMicroseconds(1); switch(j) { case 0 : HL[i] = digitalRead(HL_DATA_PIN); break; case 1 : HR[i] = digitalRead(HR_DATA_PIN); break; case 2 : KL[i] = digitalRead(KL_DATA_PIN); break; case 3 : KR[i] = digitalRead(KR_DATA_PIN); break; } }
digitalWrite(SELECT_PIN, HIGH); } HL_Reading = Data[HL[9]][HL[8]][HL[7]][HL[6]] [HL[5]][HL[4]][HL[3]][HL[2]][HL[1]][HL[0]]; HR_Reading = Data[HR[9]][HR[8]][HR[7]][HR[6]] [HR[5]][HR[4]][HR[3]][HR[2]][HR[1]][HR[0]]; KL_Reading = Data[KL[9]][KL[8]][KL[7]][KL[6]] [KL[5]][KL[4]][KL[3]][KL[2]][KL[1]][KL[0]]; KR_Reading = Data[KR[9]][KR[8]][KR[7]][KR[6]] [KR[5]][KR[4]][KR[3]][KR[2]][KR[1]][KR[0]]; } void PrintDegree() { Serial.print("DATA,TIME,"); Serial.print(HL_Degree[Angle[HL_Reading]], DEC); Serial.print(","); Serial.print(KL_Degree[Angle[KL_Reading]], DEC); Serial.print(","); Serial.print(HR_Degree[Angle[HR_Reading]], DEC); Serial.print(","); Serial.println(KR_Degree[Angle[KR_Reading]], DEC); }
ภาคผนวก ข
ใบประกาศนยบตร รางวลผลงานการวจย
84
ประวตยอผวจย
ชอ นายวนายทธ แสนเงน
วน เดอน ปเกด วนท 24 กนยายน 2522
สถานทเกด อ.เมอง จ.ล าปาง
สถานทอยปจจบน 127/21 แขวงตลาดบางเขน เขตหลกส กรงเทพมหานคร 10210
ต าแหนงหนาทการงานปจจบน หวหนาหลกสตรวศกรรมเมคคาทรอนกส
สถานทท างานปจจบน คณะวศวกรรมศาสตร มหาวทยาลยศรปทม
ประวตการศกษา
พ.ศ. 2543 วศ.บ. (วศวกรรมไฟฟา) จาก มหาวทยาลยศรปทม
พ.ศ. 2549 M.Eng (Mechatronics) จาก Asian Institute of Technology
พ.ศ. 2557 Ph.D. in Engineering (Intelligent Interaction
Technologies) จาก University of Tsukuba, Japan