ใบรบรองวทยานพนธ บณฑตวทยาลย มหาวทยาลยเกษตรศาสตร

ปรญญา

สาขา ภาควชา

เรอง การออกแบบหนยนตสองลออจฉรยะ Design of an Intelligent Two-Wheeled Robot

นามผวจย นายบณฑต อนทรยมศกด

ไดพจารณาเหนชอบโดย

อาจารยทปรกษาวทยานพนธหลก

( ) อาจารยทปรกษาวทยานพนธรวม

( ) หวหนาภาควชา

( )

บณฑตวทยาลย มหาวทยาลยเกษตรศาสตรรบรองแลว

( ) คณบดบณฑตวทยาลย

วนท เดอน พ.ศ.

วศวกรรมศาสตรมหาบณฑต (วศวกรรมเครองกล)

วศวกรรมเครองกล วศวกรรมเครองกล

อาจารยวทต ฉตรรตนกลชย, Ph.D.

ผชวยศาสตราจารยวชย ศวะโกศษฐ, Ph.D.

รองศาสตราจารยกญจนา ธระกล, D.Agr.

รองศาสตราจารยชวลต กตตชยการ, Ph.D.

วทยานพนธ

เรอง

การออกแบบหนยนตสองลออจฉรยะ

Design of an Intelligent Two-Wheeled Robot

โดย

นายบณฑต อนทรยมศกด

เสนอ

บณฑตวทยาลย มหาวทยาลยเกษตรศาสตร เพอความสมบรณแหงปรญญาวศวกรรมศาสตรมหาบณฑต (วศวกรรมเครองกล)

พ.ศ. 2552

บณฑต อนทรยมศกด 2552: การออกแบบหนยนตสองลออจฉรยะ ปรญญาวศวกรรมศาสตร มหาบณฑต (วศวกรรมเครองกล) สาขาวศวกรรมเครองกล ภาควชาวศวกรรมเครองกล อาจารยทปรกษาวทยานพนธหลก: อาจารยวทต ฉตรรตนกลชย, Ph.D. 100 หนา

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

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

ในงานวจยนน าเสนอการออกแบบระบบควบคมของหนยนตสองลออจฉรยะ โดยอาศยตวคม

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

/ /

ลายมอชอนสต ลายมอชออาจารยทปรกษาวทยานพนธหลก

Bundit Inseemeesak 2009: Design of an Intelligent Two Wheeled Robot. Master of Engineering (Mechanical Engineering), Major Field: Mechanical Engineering, Department of Mechanical Engineering. Thesis Advisor: Mr. Withit Chatlatanagulchai, Ph.D. 100 pages. An intelligent two wheeled robot is independently driven by two concentric motors. Each motor has an encoder connected to its shaft to measure angle when it moves. The intelligent two wheeled robot is a marginally stable plant that avoids obstacles autonomously. Control of the robot for stabilizing and meeting some desired responses is complicated because the plant is nonlinear and non-minimum phase. Moreover, the number of inputs is lower than the number of outputs. In this research, we use the LQR control and LQR control with augmented integrator, including state observer in order to estimate states. Vision Builder software is used to capture image and send to Lab View to implement. Both simulation and experimental results of tilt angle estimation are accurate over a wide frequency range. Simulation and experimental results also demonstrate the reliability and effectiveness of the proposed control scheme even if oscillation occurs in experiment due to friction and oscillated electrical wire. Robot can still turn with high accuracy. Camera can capture high-accuracy image in the presence of obstacles enabling the robot to avoid the obstacles well.

/ /

Student’s signature Thesis Advisor’s signature

กตตกรรมประกาศ

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

ขอขอบคณมหาวทยาลยเกษตรศาสตร, สถาบนคนควาและพฒนาเทคโนโลยอตสาหกรรม(RDiPT), หองปฏบตการควบคมหนยนตและการสนสะเทอน (Control of Robot and Vibration Laboratory, CRV LAB) ทเออเฟอสถานทและอปกรณการท าวจย

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

บณฑต อนทรยมศกด

กนยายน 2552

1

สารบญ

หนา สารบญ (1) สารบญตาราง (2) สารบญภาพ (3) ค าอธบายสญลกษณ และค ายอ (6) ค าน า 1 วตถประสงค 2 การตรวจเอกสาร 3 อปกรณและวธการ 20 ผลและวจารณ 57 สรปและขอเสนอแนะ 70 สรป 70 ขอเสนอแนะ 70 เอกสารและสงอางอง 72 ภาคผนวก 76 ภาคผนวก ก การหาคาพารามเตอรดวยโปรแกรมเขยนแบบสามมต 77 ภาคผนวก ข การหาเอกลกษณของระบบ 80 ภาคผนวก ค การพสจนทางคณตศาสตร 87 ภาคผนวก ง โคดโปรแกรม MATLAB 93 ประวตการศกษา และการท างาน 100

(1)

2

สารบญตาราง

ตารางท หนา 1 คาพารามเตอรทวไปของหนยนต 29

(2)

3

สารบญภาพ

ภาพท หนา 1 (ซาย) Dean Kamen บน Segway (กลาง) Segway PT (ขวา) Winglet 4 2 การทดลองหนยนตสองลอของ Shiroma et al. 4 3 หนยนตสองลอ NXTway-GS 6 4 (ซาย) หนยนตสองลอ Yamabico Kuro (ขวา) หนยนตสองลอ YAIP 7 5 (ซาย) หนยนตสองลอของ Kim et al. (ขวา) หนยนตสองลอ T-WIP

mobile robot 7 6 หนยนตสองลอ JOE: a mobile, inverted pendulum 9 7 ฟงกชนของภาพ 12 8 กรอบพกดส าหรบระบบกลอง 14 9 ระบบเซอรโวภาพบนพนฐานต าแหนง 16

10 ระบบเซอรโวภาพบนพนฐาน 16 11 แสดงผลทมตอคณภาพของรป 18 12 อปกรณและการออกแบบการทดลอง 21 13 หนยนตสองลออจฉรยะในหองปฏบตการควบคมหนยนตและ

การสนสะเทอน 21 14 อปกรณเอนโคดเดอรเซนเซอร 22 15 อปกรณตวตรวจรวดความเอยง Inclinometer 22 16 อปกรณวงจรขบมอเตอรกระแสตรง 23 17 มอเตอรกระแสตรง 23 18 การดรบสงขอมล (Data Acquisition) NI PCI 6221 (CB68) 24 19 Power Supply GW INSTEK รน GPS-3030D 24 20 กลองรบภาพ Basler รน scA640-70fm 25 21 โปรแกรม Vision Builder 25 22 หนยนตสองลอทม 4 องศาอสระ 26 23 หนยนตสองลอบนแกนพกด , ,x y z 27 24 บลอกไดอะแกรมในการควบคมปอนกลบแบบรกษาเสถยรภาพ 42

(3)

4

สารบญภาพ (ตอ)

ภาพท หนา 25 บลอกไดอะแกรมในการควบคมปอนกลบแบบตามรอยสญญาณ 45 26 หนยนตรถสองลออจฉรยะ 47 27 บลอกไดอะแกรมในการควบคมปอนกลบแบบตามรอยสญญาณ 48 28 แสดงการใชฟงกชนของการตรวจจบขอบ และการตงคาตางๆ 49 29 แสดงการตงคาใหวดขอบเจอเพยงขอบเดยว 49 30 เพมฟงกชนการตรวจจบขอบใหครอบคลมพนท 50 31 การใสฟงกชนใหครบแนวลางขนบน 5 เสน 50 32 เพมเสนตรวจจบขอบในแนวซายไปขวา 5 เสน เพอใหครอบคลมพนท 51 33 เพมเสนตรวจจบในแนวขวาไปซายหาเสน เพอใหครอบคลมพนท 51 34 แสดงฟงกชนทใชในการค านวณเพอหาระยะขอบของวตถทวดได 53 35 แสดงการน าคาระยะทไดเปนพคเซล เพอเกบเปนคาตวเลข 53 36 การเพมฟงกชน Run Labview VI 54 37 แสดงการน าไฟล LabVIEW มาใชประยกตระหวางการตรวจจบ

ดวยภาพ และการควบคมมอเตอร 54 38 แผนภาพของไฟล LabVIEW ทจะรบคาตวแปรตางๆ ทงสบหาตว 55 39 แสดงหนาของไฟลควบคมการหมนของมอเตอร 56 40 ผลการจ าลองการรกษาเสถยรภาพของระบบควบคมอาศยตวคมคา

ก าลงสองนอยทสด 58 41 ผลการจ าลองการตดตามคาของระบบควบคมอาศยตวคมคาก าลงสอง

นอยทสด 59 42 ผลการจ าลองการเคลอนในสองมตของระบบควบคมอาศยตวคมคา

ก าลงสองนอยทสด 59 43 การทดลองการรกษาเสถยรภาพของหนยนตสองลอ 61 44 ผลการทดลองการตดตามคาของหนยนตสองลอ 62 45 ผลการทดลองการเคลอนในสองมตของหนยนตสองลอ 63 46 ผลการทดลองการตดตามคาของหนยนตสองลอทมวลเพม 64

(4)

5

สารบญภาพ (ตอ)

ภาพท หนา 47 ผลการทดลองการเคลอนในสองมตของหนยนตสองลอทมวลเพม 65 48 การวดระยะวตถดวยโปรแกรม Vision Builder 66 49 ตรรกะของรถสองลออจฉรยะ 67 50 สนามทดลองของรถสองลออจฉรยะ 67 51 ผลการทดลองจรงการแสดงมมลอของหนยนตสองลออจฉรยะ 68 52 ผลการทดลองจรงการแสดงแรงดนไฟฟาของหนยนตสองลออจฉรยะ 69 ภาพผนวกท ก1 รป 3 มตของหนยนตสองลอและแกนพกด (x0 ,y0 ,z0) 78 ข1 วงจรสมมลของมอเตอรกระแสตรง 81 ข2 การทดลองการพสจนเอกลกษณของมอเตอรกระแสตรง 82 ข3 แบบจ าลองของ Inclinometer อยางงาย 84 ข4 การทดลองการพสจนเอกลกษณของตวตรวจร Inclinometer 85

(5)

8

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

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

= มมเลยวของหนยนต ( )rad = มมหมนเฉลยของลอซายและขวา ( )rad

1 = มมเอยงจรง ( )rad

2 = มมเอยงของตวตรวจร Inclinometer ( )rad = มมยกของหนยนต ( )rad

g = อตราโนมเอยงจากตวตรวจร Rate Gyroscope 1.rad s = อนทกรลของเอาทพต = มมเอยงของหนยนต ( )rad c = สมประสทธความหนวง E = คาผดพลาดของสมการก าลงสองนอยทสด F = แรงภายนอกทกระท าในระบบพกด ( . )N m F = แรงภายนอกทกระท าในระบบพกด ( . )N m F = แรงภายนอกทกระท าในระบบพกด ( . )N m

lF = แรงภายนอกทกระท ากบลอซาย ( . )N m

rF = แรงภายนอกทกระท ากบลอขวา ( . )N m

mf = สมประสทธแรงเสยดทานระหวางตวถงและมอเตอร ( . )N m

wf = สมประสทธแรงเสยดทานระหวางลอและพน g = แรงโนมถวง 2( )ms

pG = ฟงกชนถายโอนของพลานต H = การควบคมโดยวเคราะหดวยนอรม H i = อนดบของระบบ J = คาดรรชนสมรรถนะ

byJ = โมเมนตความเฉอยตวถงรอบแกน 2

0 ( )y kgm

bzJ = โมเมนตความเฉอยตวถงรอบแกน 2

0 ( )z kgm

eJ = โมเมนตความเฉอยของเอนโคดเดอร 2( )kgm

(6)

9

ค าอธบายสญลกษณและอกษรยอ (ตอ) ค าอธบายสญลกษณ

gJ = โมเมนตความเฉอยของเกยรทดรอบมอเตอร 2( )kgm

mJ = โมเมนตความเฉอยอารเมเจอรรอบแกน 2

0 ( )y kgm

rJ = โมเมนตความเฉอยของพลเลยทดเอนโคดเดอร 2( )kgm

wyJ = โมเมนตความเฉอยของลอรอบแกน 2

0 ( )y kgm

wzJ = โมเมนตความเฉอยของลอรอบแกน 2

0 ( )z kgm

K = คาอตราขยายของตวควบคม, คาอตราขยายส าหรบ Dual System

1K = คาอตราขยายส าหรบเอาทพต, คาอตราขยายส าหรบอนทกรลของเอาทพต

2K = คาอตราขยายส าหรบคาสเตตของระบบ

bK = คาคงทแรงดนไฟฟายอนกลบมอเตอร 1( ) Vsrad

eK = คาอตราขยายของตวสงเกต

tK = คาคงทแรงบดมอเตอร 1( ) NmA

L = คาลากรานจ, ความสงจดศนยถวงของหนยนตจากแกนลอ ( ) m l = ความยาวแขนระหวางจดหมนกบโพโตไดโอด ( ) m m = มวลของโพโตไดโอด ( ) kg

wm = มวลลอ ( ) kg bm = มวลตวถง ( ) kg

n = อตราทดมอเตอร, เอาทพตของระบบ Dual System 2n = อตราทดเอนโคดเดอร

Q = เมตรกซถวงน าหนกของสเตต iQ = แรงในระบบพกดทสนใจ

iq = ระบบพกด R = รศมลอ ( ) m , เมตรกซถวงน าหนกของสญญาณควบคม

mR = คาความตานทานมอเตอร ( )

r = สญญาณอางอง T = พลงงานจลนรวมของระบบ 2 2( ) kgm s

(7)

10

ค าอธบายสญลกษณและอกษรยอ (ตอ)

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

1T = พลงงานจลนรวมจากการเคลอนทเชงเสน 2 2( ) kgm s 2T = พลงงานจลนรวมจากการเคลอนทแบบหมน 2 2( ) kgm s

u = สญญาณควบคม ( ) V V = พลงงานศกยรวมของระบบ 2 2( ) kgm s v = สญญาณควบคมของระบบ Dual System W = ระยะความกวางฐานลอ ( ) m x = คาสเตตของระบบ x = คาความเรงในแนวระดบ x = คาประมาณของสเตตของระบบ

ax = คาทวดไดจากเอาทพตส าหรบตวสงเกตลดอนดบ bx = คาประมาณส าหรบตวสงเกตลดอนดบ

y = เอาทพตของระบบ y = คาความเรงในแนวดง

ค าอธบายอกษรยอ IMU = Inertial Measurement Unit LQR = Linear Quadratic Regulator LTI = Linear Time Invariant MIMO = Multi-input Multi-output NMP = Non-minimum Phase WMR = Wheel Mobile Robot PID = Proportional Integral derivative Controller TWV = Two-wheeled vehicle

(8)

1

การออกแบบหนยนตสองลออจฉรยะ

Design of an Intelligent Two-Wheeled Robot

ค าน า งานดานวศวกรรมและวทยาศาสตรเทคโนโลยสมยใหม ไดน าหนยนตมาพฒนาขนหลายสบปแลว ซงมนถกใชเพอประโยชนในการท างานในดานตางๆ เชน การทดแทนแรงงานของมนษย การท างานดวยประสทธภาพสง การดความเสยงตอภยอนตรายขณะท างาน หรอเพออ านวยความสะดวกสบาย ในปร ะเทศไทยไดมการตนตวในการศกษาและพฒนาหนยนตในชวงเวลาหลายปทผานมาน งานวจยเกยวกบหนยนตสองลอนเปนงานวจยทประเทศตาง ๆ ไดพฒนามาในชวงสบปทผานมาและยงมการพฒนาตอเนอง โดยในประเทศไทยนบวามงานวจยดานนนอยอยมาก การควบคมการรกษาสมดลของหนยนตสองลอมลกษณะเดยวกบลกตมนาฬกากลบหว แตเคลอนทดวยลอทขนานกนสองลอทเปนอสระตอกน โดยในประเทศไทยนบวามงานวจยดานนอยนอยมาก และยงคงเปนปญหาเปดเนองจากความซบซอนจากระบบทไมมความเสถยรภาพ และระบบไมเปนเชงเสนอนเปนผลมาจากแรงโนมถวงของโลก แตมผวจยไดพฒนาตวตรวจรทมความละเอยดสงขน มความเรวมากขน และ มราคาต าลง จงสามารถประยกตระบบการวดความเอยงนน ส าหรบการผลตเชงพาณชยได เชน ยานพาหนะ Segway มหลกการคลายกบหนยนตสองลอ และงานวจยนไดน ากลองรบภาพและประมวลผลตดไวทตวหนยนตสองลอ และใช Machine Vision ซงเปนวธการทท าใหอปกรณประมวลผลตางๆ เชน คอมพวเตอรหรออปกรณประมวลผลสญญาณดจตอล (Digital Signal Processor, DSP) มความสามารถในการรบรภาพ ซงรวมทงการท าใหอปกรณประมวลผล สามารถตดสนใจและสงงานกลไกสวนตางๆ ได จากขอมลทไดจากภาพหรอกลมของภาพนนๆ จดมงหมายสงสดของ Machine Vision คอ ท าใหเครองจกรหรออปกรณประมวลผลตาง ๆมความสามารถใหไดเทยบเทากบระบบการมองเหนของมนษย ซงไดถกออกแบบรวมกบหนยนตสองลอเพอเปนหนยนตในการส ารวจทสามารถเคลอนทไดอยางคลองแคลว งานวจยนไดใชการออกแบบตวควบคมขนสงทมประสทธภาพในการทรงตวของหนยนตสองลอตวหนงมาใช คอ ตวคมคาก าลงสองนอยทสด (Linear Quadratic Regulator, LQR) ซงอยในหมวดการควบคมทเหมาะสมทสด (Optimal Control) เพอออกแบบระบบควบคมส าหรบหนยนตสองลอใหมสมรรถนะทเหมาะสมทสด

2

วตถประสงค 1. สรางรถสองลออจฉรยะ 2. ศกษาโปรแกรม Machine Vision และ LabVIEW เพอน าไปใชกบรถสองลออจฉรยะ 3. ศกษาและออกแบบระบบควบคมอตโนมต Visual Servo ใหกบรถอจฉรยะ

3

การตรวจเอกสาร เนองจากการพฒนาโปรแกรมเพอควบคมอตโนมตของหนยนตสองลอ ยงคงเปนปญหาทมความซบซอนมากในปจจบน เนองจากมเพยงสองลอเทานนทก าหนดทงการเคลอนทในสองมตรวมถงการทรงตวดวย แตทวาการเคลอนทดวยลอเพยงสองลอนนกลบท าใหหนยนตสามารถเคลอนทไดคลองแคลว จงตองท าการศกษาทางดานโปรแกรม และพลศาสตรของตวหนยนต ดงนนการหาระบบควบคมและระบบสมการ ทใชในการควบคมหนยนต ทมอยทวไป โดยสวนใหญจะยงไมสมบรณแบบ เ นองมาจากขอมลตางๆ ยงขาดหายไป ดงนนจงตองมการคนหาขอมลตางๆ เพมขนเพอท าการอธบายระบบของหนยนต พ.ศ. 2548 สองเมอง นนทขวาง ไดออกแบบตวควบคมอยางงายในการเหวยงขนและรกษาเสถยรภาพของลกตมนาฬกากลบหวโดยอาศยตวควบคมสองสวน สวนแรกเปนตวควบคมการเหวยง ขนของลกตมโดยอาศยตวควบคมพด (PD controller) ถกออกแบบดวยเทคนคเสนทางราก (root locus) ตวควบคมทสองควบคมลกตมใหมเสถยรภาพ จดสมดลบนโดยใชตวควบคมเซอรโวยอนกลบสถานะ (servo state feedback) ซงออกแบบดวยวธการวางโพล (pole-placement method) ซงสามารถควบคมระบบใหมเสถยรภาพไดภายในระยะเวลา 5 วนาท ค.ศ. 2001 Dean Kamen ภาพท 1 (ซาย) ผคดคน Segway พาหนะสองลอสวนบคคลทเปดตวอยางเปนทางการในป และมตวแทนจ าหนายในประเทศไทยดวย (www.segway.co.th) แตวายงมราคาสงอยมาก และมปญหาเรองความปลอดภย พาหนะสองลอสวนบคคลวตกรรมล าสมย Segway PT ถกน ามาใชอยางแพรหลาย ไมวาจะเปนยานพาหนะส าหรบ การเดนทางในตวเมอง หรอ ในการแขงขนกฬาโอลมปก 2008 ทปกกงนน เจาหนาทรกษาความปลอดภยเตรยมความพรอมรบมอเหตการณฉกเฉนทอาจจะเกดขนดวย Segway PT นบวาเปนการเลอกใชอปกรณทล าสมยและเขากบสถานการณทโลกรณรงคหนมาใชพลงงานสะอาดเปนมตรกบสงแวดลอม และในชวงเดอนสงหาคม พ.ศ. 2551 บรษท Toyota ประเทศญปน (www.toyota.co.jp) เปดตวหนยนตหคทชอวาทมขนาดลกษณะทเลกกวา Segway แตยงเคลอนทโดยมความเรวสงสดเพยง 6 กโลเมตรตอชวโมงเทานนเมอเทยบกบ Segway PT มความเรวสงสด 20 กโลเมตรตอชวโมง

4

ภาพท 1 (ซาย) Dean Kamen (กลาง) Segway PT (ขวา) Winglet ในป ค .ศ. 1996 Shiroma et al. ไดศกษาเกยวกบพฤตกรรมของหนยนตสองลอทเคลอนทรวมกนตงแตสองตว เปนการตอยอดมาจากงานวจยเดมส าหรบพฤตกรรมของหนยนตทเคลอนทรวมกนของ (Hashimoto and Oba, 1993) ทมลกษณะเปนรถสลอท าใหแรงภายนอกไมสงผลตอเสถยรภาพ แตงานวจยเขานนพจารณาความสมดลของหนยนตสองลอทเปนระบบทไมมเสถยรภาพจากแรงภายนอกส าหรบระบบเคลอนทรวมกน ใชระบบควบคมจากการประมาณแรงภายนอกดวยตวสงเกตในขณะทรงตวเพอชดเชยแรง จากการทดลองการประมาณแรงภา ยนอกสามารถสามารถท าใหระบบทรงตวและชดเชยแรงได และสามารถท างานรวมกนไดระหวางมนษยและหนยนต

ภาพท 2 การทดลองหนยนตสองลอของ Shiroma et al.

5

ในป ค .ศ. 1986 ไดแสดงการขบเคลอนโดยอตโนมตของรถยนตทอาศยการประมวลผลภาพแบบ real-time ทความเรว 96 km/h บนทางดวนในประเทศเยอรมน Dickmans โดยใช Kalman Filter ในการประมวลผลภาพแบบทเปน Sequence (image sequence analysis) ในป 2003 Salerno and Angeles ไดท าหนยนต Wheel Mobile Robot (WMRs) โดยใช การควบคมความเปลยนแปลงของระบบไมเชงเสน ดวยวธ Quasiholonomic ซงไดท าการวเคราะห การกระตนภายใตการควบคมทอาจจะเปลยนแปลงได และจ าลองการเคลอนทของหนยนต ดวยวธ linearized รอบจดสมดล เขาท าการวเคราะหการเคลอนทในสวนลกของระบบหนยนต ดวยระบบลกตมนาฬกากลบหว (inverted pendulum system) เปนการวเคราะหทถกตอง และเขากเชอวาจะใชแคลอกบมอเตอรกสามารถทรงตวได Alessio and Jorge กลาววามระบบทวเคราะหลกตมนาฬกากลบหวคลายกนกบของเขา คอJOE แตจะแตกตางกนท 1. ตวตรวจร ใชวดความเอยงของหนยนต Alessio and Jorge ใช inclinometer แต JOE ใช rate gyro 2. ต าแหนงการวางมวล Alessio and Jorge วางจดศนยมวลอยต ากวาเสนผานศนยกลางลอ แต JOE วางจดศนยมวลอยเหนอเสนผานศนยกลางลอ 3. ปญหาทเกดขน JOE ใชการ integration ขอมลทไดจาก rate gyro แต rate gyro จะมความไวตอการรบสญญาณ จงท าใหเกดการแกวงของสญญาณ จะสงผลใหหนยนตเคลอนทชาลงในการเดนหนาหรอถอยหลง แต Alessio and Jorge ใช inclinometer ท าใหปญหาทกลาวมานนลดลง 4. การใชตวควบคม JOE ใช linear เปนตวควบคม และเขามองขามแรงทไมเปนเชงเสน (nonlinear) ซงเกยวของกบการเคลอนไหวของหนยนต แต Alessio and Jorge ใชวธ Quasiholonomic เขาเอาชนะปญหาตางๆ โดยการใชการควบคมระบบยอนกลบไมเชงเสน (nonlinear feedback control)

6

การควบคมระบบหนยนตสองลอเปนตวอยางทนาสนใจและเหมาะส าหรบการเรยนร มหนยนตประเภทนส าหรบการศกษาในทองตลาด โดยเฉพาะจากคาย LEGO ดงภาพท 4 โดยในหนยนตราคาต าสามารถใชตวตรวจรความเอยงทใชหลกการวดระยะทางดวยแสง เพอวดระยะทางระหวางหนยนตกบพน แตผลลพธทไดจะไมแมนย าและใชไดเฉพาะกบพนทมลกษณะเรยบ ส าหรบหนยนตแบบทดกวานยมใชตวตรวจร Rate Gyroscope รวมกบ Accelerometer ซงสามารถใชงานไดดและมราคาต า ซงเรยกวา Inertial Measurement Unit (IMU) ตวอยางเชน งานวจยการออกแบบระบบควบคมของ NXTway-GS โดย (Yomamoto, 2008)

ภาพท 3 หนยนตสองลอ NXTway-GS การออกแบบระบบควบคมในกบงานวจยน คอ การควบคมทเหมาะสมทสดโดยใชตวคมคาก าลงสองถอวาเปนการออกแบบตวควบคมทมประสทธภาพอยางหนง โดย (Ha and Yuta, 1996) น าหนยนต Yamabico Kuro มาออกแบบระบบควบคมแบบทเหมาะสมทสดโดยใชตวคมคาก าลงสองทมตวอนทกรล โดยสรางสมการพลศาสตรมาจากสมการลากรานจส าหรบควบคมความเรวของ การเคลอนทและการทรงตว แตใชตวควบคม PID แบบสมคาอตราขยายส าหรบการควบคมมมเลยว และในป ค.ศ. 2008 Imamura et al. น าหนยนตดงกลาวมาศกษาถงประสทธภาพการควบคมส าหรบใชเปนหนยนตส ารวจ นอกนน (Akesson et al., 2006) ไดท างานวจยหนยนตสองลอ YAIP โดยใชการประมาณคาความเรวจากอะนาลอกเอนโคดเดอร และใชระบบควบคมแบบตวคมคาก าลงสองรวมกบฟงกชนชดเชยแรงเสยดทาน

7

ภาพท 4 (ซาย) หนยนตสองลอ Yamabico Kuro (ขวา) หนยนตสองลอ YAIP โดยการหาแบบจ าลองทางคณตศาสตรของระบบนนแบงออกเปน 3 วธ คอ วธแรกวเคราะหมาจากกฎขอทสองของนวตน ซงคอการน าเอาแรงในแตละแนวใหมาสมดลกน ถงไดวาเปนวธพนฐานมากทสดเหมาะส าหรบระบบทไมซบซอนมากนก แตยงระบบซบซอนมาขนยงท าไดยากขนมาก วธทสองเปนวธทนยมวธหนงทถกพฒนามาภายหลง โดยการพจา รณาความสมดลระหวางแรงทปอนเขาในระบบพกดกบพลงงานศกยและพลงงานจลนของระบบ ท าใหเหมาะสมส าหรบระบบทมความซบซอนมากขน วธทสาม คอ วธของเคน ไดพฒนาตอมาอกในภายหลง โดยการพจารณาในแตละระนาบ ท าใหสามารถลดรปสมการใหมความซบซอนนอยลงได ตวอ ยางเชน งานวจยของ (Kim et al., 2005) ออกแบบระบบควบคมแบบตวคมคาก าลงสองนอยสดโดยจ าลองระบบมาจากสมการของเคน เชนเกยวกบหนยนตสองลอ T-WIP mobile robot ของ (Nawawi et al., 2007)

ภาพท 5 (ซาย) หนยนตสองลอของ Kim et al. (ขวา) หนยนตสองลอ T-WIP mobile robot

8

หนยนตสองลอถกขบเคลอนดวยมอเตอรกระแสตรงทเปนอสระตอกนแตมแกนหมนรวมกน หนยนตสองลอถกแบงยอยตามลกษณะการทรงตวเปน 3 ประเภท ประเภทแรก คอ หนยนตสองลอทมเสถยรภาพในตวเอง เนองจากหนยนตนนมจดศนยถวงอยต ากวาแกนลอ หรอมลกษณะเหมอนลกตมนาฬกา (pendulum) จงไมตองการระบบควบคมส าหรบการทรงตว (Anonymous, 1935) อยางไรกตามหนยนตประเภทแรกนไมไดเปรยบส าหรบการเปลยนแปลงต าแหนงจดศนยถวง ส าหรบการขนลงทางลาดชดรวมทงการน าเอาไปใชประโยชนกท าไดยาก หนยนตสองลอประเภททสองและสาม มลกษณะทางพลศาสตรประเภทลกตมนาฬกากลบหว (inverted pendulum) เปนยานพาหนะทมเสถยรภาพอยางมขอบเขต (marginally stable) เมอไมมแรงภายนอกกระท า ระบบนนมจดสมดล (equilibrium point) อยทต าแหนงมมเอยงศนยองศาเทานน แตแรงโนมถวงของโลกจะท าใหหนยนตลมลง หนยนตนนจงตองมตวตรวจรเพอบอกถงสถานะความเอยงของหนยนต เพอรกษาสมดลหรอต าแหนงจดศนยถวง การควบคมห นยนตนนตองอาศยทงระบบควบคมทแมนย าและมความไวสง เนองจากตองเผชญกบความโนมถวงของโลกทมลกษณะไมเชงเสนทจะท าใหระบบไมมเสถยรภาพ ซงสวนใหญระบบทมความซบซอนเชนนมกจะใชส าหรบการทดสอบประสทธภาพของตวควบคม โดยหนยนตสองลอประเภททสองนนไดรบการควบคมจากแรงภายนอก ตวอยางเชน พาหนะสองลอสวนบคคล Segway ทเปนยานพาหนะทรจกกนด โดยหนยนตนมระบบการควบคมอตโนมตซงท างานดวยการเคลอนทยานพาหนะตามการเคลอนไหวของบคคลทอยบนพาหนะ เชน เมอ ผขบขถายน าหนกไปดานหนา หนยนตจะทรงตวไมใหลมโดยขบเคลอนลอใหเคลอนทไปดานหนา ในการถอยหลงจะมลกษณะเดยวกน หรอเมอผขบขเอยงคนโยกไปดานใดดานหนงหนยนตจะเลยวซายหรอเลยวขวาจากการหมนลอทงสองดวยความเรวทแตกตางกน ซงงานวจยหลายๆงาน (Sasaki et al., 2005; Lin et al., 2007; Morrell and Field, 2007) ไดชแจงเกยวกบการออกแบบหนยนตสองลอประเภททสองน ในงานวจยนเปนการออกแบบหนยนตสองลอประเภททสามมลกษณะคลายคลงกบประเภท ทสอง แตไมมคนควบคมบนพาหนะ แตอาศยการควบคมอตโนมตตามค าสงสญญาณอางอง ดงนนการเคลอนทของหนยนตเปนระบบไมใชมมเฟสนอยทสด นอกจากนนยงเปนระบบหลายอนพตหลายเอาทพต (multi-input multi-output, MIMO) ทมสญญาณปอนเขานอยกวาสญญาณปอนออก ซงการออกแบบหนยนตประเภทนจะแพรหลายในชวงหลงป ค.ศ. 1995

9

ในป ค .ศ. 2002 Grasser และคณะ ไดออกแบบหนยนตสองลอดงภาพท 6 ขนาดความสง 65 เซนตเมตร น าหนกประมาณ 12 กโลกรม สามารถวงไดดวยความเรวสงสด 1.5 เมตรตอวนาท สามารถรกษาสมดล, ทนทานตอแรงรบกวนภายนอกและตอบสนองตอสญญาณอางองได และไดกลาวถงปญหาของ Planetary Gearbox ของมอเตอรเมอโมเมนตความเฉอยมอเตอรต ากวาลอมากๆ สญญาณรบกวนอาจจะท าใหเกดการเสยหายกบมอเตอรได โดยแกไขไดดวยการใชตวกรองความถสงท 10 เฮรต ในการออกแบบนนเรมจากการสรางสมการคณตศาสตรจ าลองพลศาสตรของระบบดวยกฎขอทสองของนวตนและประมาณระบบเปนระบบเชงเสนตรง จะไดสมการพลศาสตรเปนรปแบบปรภมสเตตสองสวน และออกแบบตวควบคมดวยวธการวางโพลไวหลายรปแบบ ในแตละรปแบบจะไดผลตอบสนองดในชวงการท างานแตกตางกน เพอเพมประสทธภาพ ผวจยดงกลาวจงไดเสนอใหออกแบบตวควบคมดวยเทคนค Adaptive Control เชน อาศย Fuzzy Logic เปนตน

ภาพท 6 หนยนตสองลอ JOE: a mobile, inverted pendulum การออกแบบตวควบคมหนยนตสองลอดวยเทคนค H ซงกลาวไดวาเปนการออกแบบตวควบคมขนสงชนดหนงถกน ามาเปรยบเทยบกบการออกแบบตวควบคมดวยวธการวางโพล (Yano et al., 1998) โดยออกแบบใหตวควบคมทนทานตอแรงรบกวนสองขอ ไดแก แรงบดอนเกดจากแรงโนมถวงของโลกและความผดพลาดของมมลกตมนาฬกากลบหว และคาผดพลาดอตราขยายของตวควบคมและแรงเสยดทานในแกนหมน พบวา การออกแบบวธการวางโพลนน ท าไดล าบากในการหาโพลทเหมาะสมแตกตางกบเทคนค H ทสามารถจดรปสมการฟงกชนถายโอนทมประสทธภาพในการเพมความสมดล (robust stability) และลดผลของแรงรบกวน ท าใหลดการสนของลกตมนาฬกากลบหวในสภาวะคงตว

10

หนยนตสองลอ Two-wheeled vehicle (TWV) โดย (Ren et al., 2008) ใชตวควบคมทมลกษณะของ Neural Network ท าการปรบแกคาอตราขยายแบบออนไลนเพอแกปญหาตอสงรบกวนภายนอกและความไมแนนอนของแบบจ าลอง ซงแบบจ าลองนนไดมาจากการแกสมการนวตน และการใชตวควบคมนนชวยปรบเปลยนคาอตราขยายของสญญาณควบคม การทดลองแสดงใหเหนถงผลดของระบบควบคมดงกลาวเปรยบเทยบกวาการใชคาอตราขยายคงทของ PID แบบทวไปในดานสมรรถนะและความทนทานโดยเฉพาะเมอเปลยนแปลงภาระการท างาน ในระบบทสวนหนงสวนใดของระบบตดตงถาวรอยกบพน การตรวจวดการเคลอนทของพลศาสตรไมใชเรองยากโดยการใชเอนโคดเดอรเซนเซอร แตในทางตรงกนขามระบบยานพาหนะสองลอนนเปนระบบทเคลอนทอยางอสระ ดงนน จงไมสามารถใชเอนโคดเดอรเซนเซอรส าหรบการวดความเอยงได โดยหนยนตนนตองการตวตรวจรทแมนย าและตอบสนองตอแบนดวทธสง (high bandwidth) การใชตวตรวจรประเภทตววดความเรงสามารถตอบสนองตอแบนดวทธสงไดด แตพบวามปญหาตอการตอบสนองตอการสนสะเทอนทสงเกนไป (Vaganay et al., 1993) ในป ค .ศ. 1996 Baerveldt และ Klang ไดออกแบบตวตรวจรส าหรบวดความเอยงโดยใชหลกการของ Complementary Filter ระหวางตวตรวจรสองตวคอ Pendulum Inclinometer และ Rate Gyroscope โดยสามารถชดเชยการตอบสนองทางพลศาสตรทชาของ Inclinometer และการเบยงเบนของสญญาณของ Rate Gyroscope ได แตพบวาจากการประมาณคาพารามเตอรของ Inclinometer ดวย First-Order Low-Pass filter ดวยเวลาคงท ตวตรวจรนจงตอบสนองตอความถไดดเฉพาะชวง 0.15 ถง 1.5 เฮรต และในป ค.ศ. 2004 Rehbinder และ Hu พฒนาการวดความเอยงจาก Rate Gyroscope และ Accelerometer โดยอาศย Switching State Esimator กสามารถแกปญหาการ Drift ของสญญาณ Rate Gyroscope ได ในป ค .ศ. 2004 Wang, Teoh and Shen ไดน าเสนอวธการทเรยกวา B-snake ซงใชความสมพนธของเสนขนานใน 3D เพอทจะหารปรางของแบบจ าลองใน 2D จากการค านวณจด vanishing ขอมลทไดจากภาพจะถกน ามาปรบคาพารามเตอรรปแบบการจ าลองของถนน ซงเปนฟงกชน B-spline โดยใช Minimum Mean square Error

11

ในป ค .ศ. 2006 Leavitt และคณะ ไดพฒนาตวตรวจรจาก Pendulum Inclinometer และ Rate Gyroscope โดยเพมตวตรวจรวดความเรง อาศยความรของ State Observation จ าลองโมเดลตวตรวจรทกชนดแบบไมแปรผนตามเวลา (linear time invariant, LTI) จ าลองโมเดล Rate Gyroscope เปนคาการประมาณความเรวเชงมมทรวมกบคาไบอสและคาสเกลทไมทราบคา และจ าลองสมการคณตศาสตรของ Inclinometer เปน Second-Order Transfer Function โดยคาพารามเตอรตางๆ นนไดมาจากการทดลองการจ าลองการตอบสนองเชงความถ วธการออกแบบตวตรวจรนนคลายเดมคอ การจ ากด Gyro Bias ออก ตวตรวจรทถกพฒนาขนนสามารถตอบสนองตอแบนดวทธสงไดมากกวา 4 เฮรต ทงน ขนกบความแมนย าของแบบจ าลอง Inclinometer และสามารถท างานไดเมอระบบมความเรงในแนวแกนตางๆ Gans and Hutchinson (2006) ไดท าอปกรณทใชสญญาณรบภาพเขามาควบคมความเรว และการทรงตวของ Wheeled Inverted Pendulum (WIP) ดวยระบบสญญาณปอนกลบเชงเสน (Partial-Feedback Linearization) ซงมลกษณะเหมอนยานพาหนะทใชในปจจบน เรยกวา Segway แตมขนาดเลกกวา และมกลองถายภาพอยบนตวหนยนต โดยควบคมการทรงตวดวยตวตรวจร rate gyro ซงท าหนาทตรวจวดความเอยงของตวหนยนตในขณะเคลอนท ใชการเคลอนทโดยอตโนมต และหาตว Controller ดวยวธ Partial-Feedback Linearization ส าหรบควบคมหนยนต นยามของรปภาพดจทล รปภาพเปนคาแถวล าดบสองมต (2D array) ซงตวแทนของความเขมแสง ส าหรบจดประสงคของการประมวลผลภาพ ในพจนของรปภาพอางองถงรปภาพดจทล รปภาพมฟงกชนของความเขมแสงดงน

( , )f x y

เมอ f เปนจดความสวางของแสง (x, y) และ x และ y แทนพกดเชงพนทของสมาชกรปภาพหรอพกเซล (pixel) ดงภาพท 7

12

ภาพท 7 ฟงกชนของภาพ ในการประมวลผลภาพดจทล ตวรบรภาพจะแปลงรปภาพเปนตวเลขวยต (discrete number) ของพกเซลตวรบรจะก าหนดแตละพกเซลในต าแหนงเชงตวเลข และคาของระดบสเทาหรอสโดยระบความสวางและสของแตละพกเซล การออกแบบระบบการมองเหน ระบบการใชกลองทใชเทคนคของ Machine Vision ในการประมวลผลแบบภาพอตโนมตนน มกมความยดหยนการใชงานสง สามารถใชกบการตรวจจบถนน เสนแบงทาง สง กดขวางบนถนน ยานพาหนะอนๆ หรอลกษณะเดนทก าหนดจดหมายปลายทางของการเคลอนท ภาพทไดอาจจะเปนภาพสหรอขาวด ากไดขนอยกบการประมวลผลขอมล การใชกลองเดยว งานวจยในชวงเรมตนมกใชกลองเดยว หรอทเรยกวา Monocular ซงมขอด คอ ในการค านวณผลนน คอนขางเรวและคาใชจายต าแต Monocular นน ไมสามารถทจะดงขอมลทส าคญของภาพทไดออกมาคอระยะหางหรอความลกของวตถกบกลองออกมากได Dickmans (1986) ทไดแสดงการขบเคลอนโดยอตโนมตของรถยนตทอาศยประมวลผลภาพแบบ real-time ทความเรว 96 km/h บนทางดวนในประเทศเยอรมน Dickmans โดยใช Kalman Filter ในการประมวลผลภาพทเปน Sequence (image sequence analysis) เพอควบคมการเคลอนทในเวลาจรง

13

งานวจยของมหาวทยาลยคารเนกเมลลอน ซงเปนผบกเบกการใชระบบเครอขายประสาทเทยมในการตรวจจบถนนเรยกกวาระบบ ALVINN งานวจยสวนใหญเกยวกบการตรวจจบถนน นนคอ วธการหาขอบ (edge detection) ในการตดสนใจและแยกแยะบรเวณถนนออกจากสงแวดลอมอนๆและยงใชวธการแปลงภาพเพอหาเสนตรงในภาพ เชน การใช Hough transform ขอบทแสดงเสนซาย-ขวาของขอบถนนมกมสทโดดเดนอยางชดเจน และสามารถลากกนไปตดทจด vanishing ท horizon row ในภาพได เชน ระบบ LANELOCK วธการทไดพฒนาขนมาในชวงแรกนมก ไมทนทานตอการตรวจจบทางโคงและขอบถนนซงไมมเสนบอกทางทชดเจน การฉายของกลอง (Camera Projection Models) เพอทจะควบคมหนยนตโดยใชขอมลจากการมองเหน เราจงตองศกษาการะบวนการการเกดภาพทจ าเปน กลองแตละตวจะประกอบดวยเลนส (lens) ทท าใหเกดการฉาย 2 มต (2D projection) ลงบนระนาบทมตวรบร (sensor) อย การฉายนท าใหขอมลความลกหายไป ดงนน แตละจดบนระนาบภาพจะเปนรงส (ray) ในปรภม 3 มต (3D space) เพราะฉะนนจงตองการขอมลเพมเตมเพอทจะระบพกด 3 มต จากจดบนระนาบภาพขอมลนอาจมาจากกลองหลายๆตว, หลายมมมองจากกลองตวเดยว หรอความรเกยวกบความสมพนธทางเรขาคณตระหวางจดหลายๆ จดบนเปาหมาย ในสวนนจะอธบายตวแบบการฉาย 3 แบบทใชกนอยางกวางขวางในการจ าลองกระบวนการสรางภาพ ไดแก การฉายเชงทศนมต (perspective projection) การฉายเชงเสนตงฉากตามมาตรา (scaled orthographic projection) และการฉายเชงสมพรรค (affine projection) ส าหรบตวแบบการฉายแตละแบบเราก าหนดระบบพกดกลองดวยแกน X และ Y เพอเปนพนฐานส าหรบระนาบภาพ โดยใหแกน Z ตงฉากกบระนาบภาพ (แนวเดยวกบแกนเชงแสง-optical axis) และมจดก าเนด (origin) อยทระยะ หลงระนาบภาพ โดยทเปนความยาวโ ฟกส (focal length) ของเลนสของกลองดงรป

14

ภาพท 8 กรอบพกดส าหรบระบบกลอง 1. การฉายเชงทศนมต (perspective projection) ถากลองถกจ าลองโดยใชการฉายภาพเชง ทศนมต จด [ , , ]c TP x y z ซงพกดอยในกรอบพกด (coordinate frame) ของกลอง C จะฉายลงบนระนาบภาพทมพกด [ , ]Tp u v ไดเปน

( , , )

u xx y z

v y

ถาพกดของ P อยในกรอบพกด x เราจะตองท าการแปลงพกด ( )c c x

xP x P กอน 2. การฉายเชงเสนตงฉากตามมาตรา (scaled orthographic) การฉายเชงทศนมตเปน การสงแบบไมเชงเสน (nonlinear) จากคารทเซยนไปยงพกดภาพ ในหลายกรณ เปนไปไดทจะประมาณการสงนโดยการฉายเชงเสนตงฉากตามมาตรา ซงเปนการสงเชงเสนภายใตตวแบบน พกดภาพส าหรบจด cP จะเปน

u x

v y

เมอ s คอ ตวประกอบมาตราคงท (fixed scale factor)

15

ตวแบบการฉายเชงเสนตงฉากตามมาตราเหมาะส าหรบกรณทความลกสมพนธของจดคอนขางนอย เมอเทยบกบระยะทางจากกลองไปยงฉาก เชน เครองบนอยบนฟาหรอกลองทมความยาวโฟกสคอนขางยาววางอยหลายเมตรจากปรภมงาน (workspace) 3. การฉายเชงสมพรรค (affine projection) เปนการประมาณการฉายเชงทศนคตแบบหนง ทเปนการประมาณเชงเสน ในกรณพกดภาพส าหรบการฉายของจด cP จะเปน

cu

A P Cv

เมอ A คอ เมตรกขนาด 2 3 ใดๆ และ C คอ เวกเตอรขนาด 2 ใดๆ จะเหนไดวา การฉายเชงเสนตงฉากตามมาตราเปนกรณพเศษของการฉายเชงสมพรรค การฉายเชงสมพรรคอาจจะไมตรงกบสถานการณเกดภาพใดๆโดยเฉพาะ แตเปนการประมาณการฉายเชงทศนมตทดในบรเวณ เนองจากตวแบบเปนแบบเชงเสน A และ C จงสามารถค านวณไดงายโดยใชเทคนคการถดถอยเชงเสน ท าใหลดความซบซอนของปญหาไปไดมาก สถาปตยกรรมการเซอรโว (Servoing Architectures) ระบบเซอรโวภาพสามารถแยกตามลกษณะอนพตของสวนควบคมได 2 ประเภทไดแก ระบบบนพนฐานต าแหนง (position-based system) และระบบบนพนฐาน (image-based system) ในระบบบนพนฐานต าแหนง ลกษณะเฉพาะเชงภาพ (image feature) เชน สญลกษณทท าไวทจดปลายของหนยนต จะถกดงออกมาจากภาพ 2 มต แลวใชขอมลทางเรขาคณตกบตวแบบกลอง เพอทจะประมาณต าแหนงในโลกจรง

16

สวนควบคม

สวนประมาณต าแหนงสวนประมวลผล

ลกษณะเฉพาะเชงภาพ

หนยนตรถสองลออจฉรยะ

+-

ต าแหนงของหนยนต

กลองรบภาพ

ต าแหนงของเปาหมาย

ภาพ 2 มต

ภาพท 9 ระบบเซอรโวภาพบนพนฐานต าแหนง สวนระบบพนฐานภาพ คาทปอนกลบไปจะอยในรปแบบคาทไดจากลกษณะเฉพาะเชงภาพ ทเรยกวาพารามเตอรลกษณะเฉพาะเชงภาพ (image feature parameter) โดยตรง เชน คาพกดระนาบภาพ (image plane coordinates) ของลกษณะเฉพาะเชงภาพ จะเหนวา ระบบบนพนฐานของภาพ จะไมแปลความหมายของภาพเปนต าแหนงในโลกจรง ท าใหลดปญหาความผดพลาด เนองจากการสรางแบบจ าลองของกลอง โครงสรางของระบบบนพนฐานภาพ ดงภาพท 10

สวนควบคม

สวนประมวลผลลกษณะเฉพาะเชงภาพ

หนยนตรถสองลออจฉรยะ

+-

พารามเตอรลกษณะเชงภาพของหนยนต

กลองรบภาพ

พารามเตอรลกษณะเชงภาพของเปาหมาย

ภาพ 2 มต

ภาพท 10 ระบบเซอรโวภาพบนพนฐาน

เครองรบภาพ เครองรบภาพ (Machine Vision) คอ วธการทท าใหอปกรณประมวลผลตางๆ เชน คอมพวเตอรหรออปกรณประมวลผลสญญาณดจตอล (Digital Signal Processor, DSP) มความสามารถในการรบรภาพ ซงรวมทงการท าใหอปกรณประมวลผลนนๆ สามารถตดสนใจและสงงานกลไกสวนตาง ๆ ได จากขอมลทไดจากภาพหรอกลมของภาพนนๆ จดมงหมายสงสดของ เครองรบภาพ คอ เครองจกร

17

หรออปกรณในการประมวลผลตาง ๆ มความสามารถใหไดเทยบเทากบระบบการมองเหนของมนษย ทมววฒนาการตอเนองกนมาหลายสบลานป อยางไรกตามเทคโนโลยในปจจบนยงคงไมสามารถท าใหเครองจกรสามารถมองเหนและรบร ไดเทยบเทากบความสามารถของมนษย ยกตวอยางเชน ผอานทเปนมนษยทมสภาพรางกายปกต จะสามารถแยกแยะสงของทตองการจากกองสงของหลาย ๆอยางได หรอสามารถแยกแยะหนาคนทคนเคยได จากกลมคนทอยภายใตสภาพแวดลอมทมฝนตก หรอหมอกลงจด การท ากจกรรมดงกลาวนน ผอานจะสามารถท าไดโดยแทบจะไมตองใชความพยายามมากเทาไรนก ซงหาก ตองการใชเครองจกรหรออปกรณประมวลผลมความสามารถทจะท ากจกรรมดงกลาวไดนน นอกจากจะตองใชความพยายามเปนอยางมากแลว ยงตองใหกระบวนการทางคณตศาสตรทซบซอนอกดวย ทงน เนองจากความแตกตางกนระหวางการท างานของอปกรณประมวลผลและสมองของมนษย ทแมอปกรณประมวลผลจะมความเรวในการประมวลผลทางคณตศาสตรสงกวาสมองของมนษยมาก ดงจะเหนไดงายๆ จากการบวกเลขยสบหลกเขาดวยกน จะพบวาคอมพวเตอรทมอยในปจจบน หรอแมกระทงเครองคดเลขธรรมดาทวไป จะสามารถท างานดงกลาวไดโดยใชเวลาเพยงเศษเสยววนาทเทานน ซงตางกบสมองของมนษยทเปนหนวยประมวลผลอยางงายๆ อยางไรกตามเนองจากหนวยยอยๆ ของสมองมนษยเหลาน มจ านวนมากมายมหาศาล และท างานไปพรอมๆกน (Parallel Processing) แทนทจะท างานทละขนตอน (Serial Processing) ซงเปนวธการท างานของอปกรณประมวลผลทมอยในปจจบน จงท าใหประสทธภาพการท างานของสมองมนษยสงกวาอปกรณประมวลผลทมอยในปจจบนเปนอยางมาก แมวาดวยเทคโนโลยทมอยในปจจบนจะไมสามารถพฒนาความสามารถการมองเหน และรบรของเครองจกรใหไดเทยบเทากบของสมองมนษย แตผลทไดจากการพฒนาความรเรอง Machine Vision กสามารถน าไปใชกบระบบรกษาความปลอดภย หรอแมกระทงการประยกตใชในเชงอตสาหกรรม เปนตน ระบบการรบภาพ (Vision System) หรอบางคนอาจเรยกวา แมชชนวชน (Machine Vision) จงเขามามบทบาทในการตรวจเชคคณภาพ เนองจากระบบนสามารถตรวจเชคได หนงรอยเปอรเซนของผลตภณฑ และมความละเอยดแมนย ากวาสายตามนษย สามารถควบคมคณภาพไดสม าเสมอตางจากสายตามนษยทอาจมความเหนอยลา เปนสาเหตท าใหเกดความผดเพยนได นอกจากนระบบนยงสามารถตรวจเชคคณภาพชนงานไดตงแตชวงแรกๆ ของกระบวนการผลต ซงบางกระบวนการคนไมสามารถเขาไปในพนทนนๆ ได ท าใหสามารถคดแยกชนงานทเสยออกแตเนน ๆ ไมตองปลอยใหเสรจสนกระบวนการถงจะมการตรวจเชค เพราะถงเวลานนชนงานมมลคาทสง

18

คณภาพของกลองรบภาพ คณภาพของกลองรบภาพทดตองมการเกบขอมลของภาพตนแบบ และสงขอมล ภาพเขาจอ คอมพวเตอรโดยมความผดพลาดของภาพนอยทสดจากภาพตนแบบ จงขนอยกบองคประกอบตางๆ ดงภาพท 11

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

สตรทใช: % Contrast = minmax

minmax

II

II

Depth of Field เปนความสามารถในการรกษาคณภาพของรปใหชดเจน เมอวตถมการขยบออกจากจดโฟกสทดทสด 2. การรกษาคณภาพของส คอ ความสามารถในการรกษาคณภาพของภาพใหชดเจนเหมอนตนแบบ เมอวตถมการขยบออกจากจดโฟกส

19

3. การเกบขอมล คอ เมอแสงตกกระทบตวตรวจร จะท าการเกบขอมลของพกเซล ณ จดตาง ๆของภาพ ซงโดยทวไปแลว รปหนงรปจะประกอบไปดวย พกเซล ขนาดเลก ๆตางๆ รวมตวกนขนมา ขอมลเหลานจะถกรวบรวมและจดรปกอนทจะสงไปแสดงผล ยง Sensor ใหญ FOV กจะยงมาก โดยทวไป ตวตรวจรจะมขนาด สตอสาม (แนวยาว ตอ แนวตง) ซงถาหากตวตรวจรมขนาดใหญมากเมอเทยบกบเลนส ภาพทออกมาจะเลอนราง หรอจาง โดยเฉพาะอยางยงในบรเวณขอบหรอมมของรป (Tunnel Effect) 4. ความผดปกตของภาพ เกดจากความผดปกตทางเรขาคณตของสวนทจบภาพเขามาของเลนส ซงเลนสโดยทวไปจะเกดการผดรปแบบโคง (Radial Distortion) ซงท าใหภาพทกลองรบภายได อยไกลกวาความเปนจรง เทยบจากจดศนยกลางของมตทมองเหน แตขอมลไมไดหายไป วธการแกไข คอ ใชเทคนคทเรยกวา การวดระยะหาง (Spatial Calibration) ในโปรแกรมหรอใชการปรบทตวเลนสของกลองรบภาพทตดอยกบรถสองลอ 5. ขนาดของภาพ คอ การ เกดจากแนวแกนของกลองไมไดตงฉากกบวตถทตองการตรวจสอบ ซงจะสงผลท าใหการเปลยนแปลงก าลงขยายของวตถ จะขนอยกบระยะหางของเลนสกบวตถ ซงวธการแกไขนนจะตองพยายามปรบตวกลองใหตงฉากกบวตถ หรอใ ชวธการวดขนาด ในโปรแกรม หรอใชการหมนเลนสทตวกลองเพอปรบระยะหางของวตถ

20

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

อปกรณการทดลอง การออกแบบและจ าลองระบบควบคม ประเดนส าคญในงานวจยน คอ การออกแบบระบบควบคมและการจ าลองเพอวเคราะหผล โปรแกรมทน ามาใช คอ โปรแกรม MATLAB ซงถกใชอยางแพรหลายในงานดานวศวกรรมศาสตร โปรแกรมดงกลาว เปนภาษาขนสงทชวยลดระยะเวลาในการเขยนโปรแกรมดวยฟงกชนตางๆ เชน กระบวนการทางเมตรกซ การออกแบบตวควบคม เปนตน โดยคาพารามเตอรของระบบส าหรบ การจ าลองไดมาอปกรณทดลองจรงในหวขอถดไป โดยดไดจากภาคผนวก ก การทดลองระบบควบคมของหนยนตสองลอ อปกรณการทดลองประกอบดวย หนยนตสองลอ ระบบปอนเขาสญญาณ ซงประกอบดวยระบบควบคมความแรงดนไฟฟามอเตอรและแหลงจายพลงงาน ระบบปอนกลบสญญาณ คอ ระบบรบคาสญญาณจากตวตรวจร ตวควบคมประกอบดวยการดรบสงสญญาณตดตงภายในคอมพวเตอรตงโตะทท าหนาทเปน Target Computer และคอมพวเตอรโนตบคท าหนาทเปน Host Computer ดงแสดงในภาพท 12

21

1

3

2

4 5

ภาพท 12 อปกรณและการออกแบบการทดลอง 1. หนยนตสองลอ เปนหนยนตขนาด 14.7×36.6×52 เซนตเมตร น าหนกรวม 3.179 กโลกรม วสดโครงท ามาจากอลมเนยม ภายในหนยนตจะมมอเตอรกระแสตรง 2 ชด โดยตอตรงกบลอทงสอง ทท างานอยางอสระตอกน เพอควบคมการทรงตวของหนยนตสองลอในหองปฏบตการควบคมหนยนตและการสนสะเทอน ดงภาพท 13

ภาพท 13 หนยนตสองลออจฉรยะในหองปฏบตการควบคมหนยนตและการสนสะเทอน 2. ตวตรวจร คอ อปกรณทเปลยนพลงงานกลเปนพลงงานไฟฟาเพอบอกถงสถานะของหนยนต ตวตรวจรส าหรบหนยนตสองลอ ประกอบดวย 2.1 เอนโคดเดอรเซนเซอร (Encoder) คอ อปกรณ วดต าแหนงทท างานโดยอาศยการยงล าแสงผานแถบมด-สวางและสงออกเปนสญญาณดจตอล การอานคาอตราการเปลยนแปลงของระดบสญญาณถกน ามาค านวณเปนอตราเรวของการเปลยนแปลงต าแหนง งานวจ ยนใชตวตรวจร Omron

22

รน E6B2 CWZ6BC มความละเอยดเทากบ 360 ครงตอรอบ (CPR) โดยม 2 ชด ส าหรบการวดอตราเรวของลอซายและลอขวา เอนโคดเดอรตออยกบมอเตอรผานพลเลยและสายพาน

ภาพท 14 อปกรณเอนโคดเดอรเซนเซอร 2.2 ตวตรวจรวดความเอยง (Inclinometer) คอ อปกรณวดมมเอยงของของโครงหนยนต ลกษณะการท างานเหมอนเอนโคดเดอรเซนเซอร แตอาศยการแกวงของลกตมทอยภายใน งานวจยนใชตวตรวจร USDigital รน EM1/HEDS T6 มความละเอยดเทากบ 2500 ครงตอรอบ (CPR)

ภาพท 15 อปกรณตวตรวจรวดความเอยง Inclinometer 3. วงจรขบมอเตอรกระแสตรง (Power Amplifier) ท าหนาทขยายกระแสไฟฟา โดยอปกรณนจะรบสญญาณปอนเขาคอกระแสไฟฟาทมขนาดและปรมาณต า และจายกระแสไฟฟาทมขนาดและแรงดนสงกวา โดยใชพลงงานจากแหลงก าเนดไฟฟาไปยงมอเตอรกระแสตรงทตดตงอยบนหนยนต ซง ในงานวจยนใชอปกรณรน Sabertooth2×10 ท าหนาทเปนวงจรขบทรบและจายไฟฟาโดยแปรผนตามแรงดนไฟฟา

23

ภาพท 16 อปกรณวงจรขบมอเตอรกระแสตรง 4. มอเตอรกระแสตรง (DC Motor) ใชแรงดนไฟฟา 12 โวลต กระแสไฟฟา 0.075 แอมแปร ความเรวสงสด 8100 รอบตอนาท และมอตราทดเกยร 68: 1 รอบ

ภาพท 17 มอเตอรกระแสตรง 5. Host Computer คอ คอมพวเตอรส าหรบพฒนาแอปพลเคชน และในคอมพวเตอรตวนตดตงโปรแกรม LabVIEW 8.2.07. โปรแกรม Vision Builder โปรแกรมทใชจดการกบภาพอกโปรแกรมหนง เหมาะกบงานทไมมรายละเอยดมากนก มความสามารถหลายอยาง 6. Target Computer คอ คอมพวเตอรทใชเปนตวควบคมของระบบ ในคอมพวเตอรนถกตดตงการดรบสงขอมล (Data Acquisition) NI PCI 6221 ท าหนาทรบสงสญญาณ มความเรว 250kSตอวนาทและความละเอยด 16 บตส าหรบอนาลอก ความละเอยด 8 บตส าหรบดจตอล และความละเอยด 32 บต 80 เมกกะเฮรต ส าหรบชองตอเคาทเตอร แตอยกบการดเชอมตอ (CB68) และในคอมพวเตอรตวนตดตงโปรแกรม LabVIEW Real-Time 8.2.0

24

ภาพท 18 การดรบสงขอมล (Data Acquisition) NI PCI 6221 (CB68) 7. Power Supply วงจรขบมอเตอรกระแสตรง ท าหนาท เพมกระแสไฟฟา สามารถจายกระแสไฟฟาสงสดได 3แอมแปร และแรงดนไฟฟาสงสด 30 โวลต

ภาพท 19 Power Supply GW INSTEK รน GPS-3030D 8. กลองวดโอของ Basler รน scA640-70fm ใชไฟฟากระแสตรง 12 VDC กลองมขนาด กวาง 44 มลลเมตร ยาว 73.7 มลลเมตร สง 29 มลลเมตร น าหนก 110 กรมขนาดภาพ 659×490 พกเซล ความละเอยดภาพ 7.4×7.4 ไมโครเมตร

25

ภาพท 20 กลองรบภาพ Basler รน scA640-70fm 9. โปรแกรม Vision Builder เปนโปรแกรมทใชส าหรบรบภาพจากกลองวดโอ และสามารถค านวณระยะหางระหวางตวรถกบวตถได และน าภาพทรบไดมาแสดงผลบนหนาจอคอมพวเตอร

ภาพท 21 โปรแกรม Vision Builder

26

การหาแบบจ าลองทางคณตศาสตร สวนทส าคญในการออกแบบระบบควบคมทอาศยแบบจ าลอง (model-based control) คอ การสรางแบบจ าลองทางคณตศาสตรทแมนย า เนองจากขนตอนการตดสนใจในขบวนการออกแบบตวควบคมจะท าไดดเมอเขาใจถงพฤตกรรมของระบบทจะท าการควบคม และบางครงพบวาแบบจ าลองทางคณตศาสตรจากวธกลองด า (Black Box) ทมอนดบตางๆ ไมสามารถบอกถงพลศาสตรของระบบไดดนก หนยนตสองลอนนสามารถหาแบบจ าลองทมประสทธภาพดไดจากสมการการเคลอนทเชงกล ไดแก กฎขอทสองของนวตน (Grasser et al., 2002) ทฤษฎของเคน (Kim et al., 2005) และสมการลากรานจทงานวจยนน ามาใช ภาพท 22 แสดงถงองศาอสระทงหมดของหนยนตเทากบ 4 องศาอสระ ประกอบดวย คาเฉลยมมหมนของลอทงสอง ( ) / 2l r , มมเอยงของหนยนต ( ) , มมเลยวของหนยนต ( ) และมมยกของหนยนต ( ) แตในงานวจยนจะพจารณาเฉพาะกรณทหนยนตวงบนพนทไมมความลาดชนหรอคามมยกของหนยนตมคาเทากบศนยเสมอ สงผลใหระบบหนยนตสองลอในงานวจยนมองศาอสระเทากบ 3

ภาพท 22 หนยนตสองลอทม 4 องศาอสระ

27

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

W

my

ly

ry

lx rxmx

by

bx

y

x

0x

0y

z

,l r

R

L

bz

mz

0z

ภาพท 23 หนยนตสองลอบนแกนพกด , ,x y z

= 1( )

2l r = มมหมนเฉลยของลอซายและลอขวา

= ( )r l

R

W = มมหมนของหนยนตสองลอ (Body yaw angle)

= มมเอยงของหนยนตสองลอ (Body pitch angle) โดย ,l r = มมของลอซายและลอขวา

และความสมพนธของ และ จาก 1 ( )2

l r และ ( )r l

R

W จะไดวา

,

2 2r l

W W

R R (1)

28

สมการลากรานจส าหรบหาแบบจ าลองทางพลศาสตรสามารถเขยนไดดงน

i

i i

d L LQ

dt q q

(2)

โดย L = คาลากรานจ (Lagrangian) = T V T = พลงงานจลนรวมของระบบ V = พลงงานศกยรวมของระบบ iq = ระบบพกด iQ = แรงในระบบพกดทก าลงสนใจ i = อนดบของระบบ เมอขณะเวลาเรมตน ก าหนดใหคาพารามเตอรของหนยนตสองลอใหอยบนระบบพกด ดงแสดงในภาพท ดงนน สามารถเขยนคาพารามเตอรไดดงน ( , , ) ( cos , sin , )m m mx y z R R R (3)

( , , ) ( sin , cos , )2 2

l l l m m m

W Wx y z x y z (4)

( , , ) ( sin , cos , )2 2

r r r m m m

W Wx y z x y z (5)

( , , ) ( sin cos , sin sin , cos )b b b m m mx y z x L y L z L (6)

29

ตารางท 1 คาพารามเตอรทวไปของหนยนต พารามเตอร ความหมาย ขนาด (หนวย)

g แรงโนมถวงโลก 9.81 2/m s

wm มวลลอ 0.5675 kg

bm มวลตวถง 1.94033 kg

R รศมลอ 0.146 m

W ระยะความกวางฐานลอ 0.3103 m

n อตราทดมอเตอร 64

2n อตราทดเอนโคดเดอร 0.25

eJ โมเมนตความเฉอยของเอนโคดเดอร 61 10 2kgm

( eJ ขอมลจากโรงงานผลต Omron)

จากภาคผนวก ก จะไดคาพารามเตอรดงน L = ระยะหางของจดศนยถวงในแนวแกน 0z = 0.04895 m

byJ = โมเมนตความเฉอยตวถงรอบแกน 0y = 0.0173198 2kgm

wyJ = โมเมนตความเฉอยของลอรอบแกน 0y = 0.0013824 2kgm

bzJ = โมเมนตความเฉอยตวถงรอบแกน 0z = 0.0117333 2kgm

wzJ = โมเมนตความเฉอยของลอรอบแกน 0z = 0.0144355 2kgm

rJ = โมเมนตความเฉอยของเกยรทดเอนโคดเดอร = 0.0000578 2kgm

จากภาคผนวก ก จะไดคาพารามเตอรดงน

mR = คาความตานทานมอเตอร

bK = คาคงทแรงดนไฟฟายอนกลบมอเตอร

tK = คาคงทแรงบดมอเตอร

30

_b lK = 0.004129229, _b rK = 0.004420931

t

m l

K

R

= 0.003718956,

t

m r

K

R

= 0.0038156

พารามเตอรอนๆ ทหาไดยากแตมคาใกลเคยงกบระบบอน จงก าหนดคาไดดงน

mJ = โมเมนตความเฉอยอารเมเจอรรอบแกน 0y = 51 102kgm

gJ = โมเมนตความเฉอยของเกยรทดรอบมอเตอร = 51 102kgm

mf = สมประสทธแรงเสยดทานระหวางตวถงและมอเตอร = 0.0022

wf = สมประสทธแรงเสยดทานระหวางลอและพน = 0 พลงงานจลนจากการเคลอนทเชงเสน 1T , พลงงานจลนจากการเคลอนทแบบหมน 2T และพลงงานศกย U หาไดจากสมการ (7), (8) และ (9)

2 2 2 2 2 2 2 2 2

1

1 1 1( ) ( ) ( )

2 2 2l r bw l l w r r b b bT m x y z m x y z m x y z (7)

2 2 2 2 2

2

2 2 2 2 2

2

1 1 1 1

2 2 2 2

1 1( ) ( )

2 2

by bz wz wy l wy r

l r g m e r

T J J J J J

J J n J J n n

(8)

l r bU mgz mgz mgz (9) จากสมการพลงงานจลนจากการเคลอนทเชงเสน (7) แทนคาจากสมการ (3) , (4) , (5) , (6) เปลยนรปใหอยในระบบพกดไดดงน

2 2 2

1

2 2 2

2

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

2 2 2

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

2 2 2

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

2

d W d W dT m R R R

dt dt dt

d W d W dm R R R

dt dt dt

d dm R L R L

dt dt

2 2

( cos ) )d

R Ldt

31

2 2

1 2 2

sin cos cos cos sin sin2 21

(2

sin cos cos cos sin sin2 2

sin cos sin sin c1

2

w

b

W WR R R R

T mW W

R R R R

R R L Lm

2

2 2

os cos

cos sin sin cos cos sin sinR R L L L

จดรปใหมจะได

22 2 2 2 2 2 2 2 2 2

2 2 2 2 2 2 2 2 2

22 2 2 2 2

1 22 2 2 2 2 2 2 2

sin cos cos 2 sin cos4

sin cos cos cos sin

sin 2 sin cos sin cos sin1 4

2sin cos cos

4

w

WR R R

RW RW R R

WR RW RW

T mW

R R

2 2

2 2 2 2 2 2 2 2 2

22 2 2 2 2

2 sin cos

sin cos cos cos sin

sin 2 sin cos sin cos sin4

R

RW RW R R

WR RW RW

2 2 2 2 2 2 2 2 2 2 2 2 2 2 2

2 2 2

2

2 2 2 2 2 2 2

sin cos sin sin cos cos

2 sin cos 2 sin sin 2 sin cos cos

2 sin cos sin 2 cos cos

2 sin cos sin cos cos si1

2b

R R L L

R LR LR

LR LR

L R Rm

2

2 2 2 2 2 2 2 2 2

2 2

2

2 2 2 2

n

cos sin sin cos 2 sin cos

2 cos sin 2 sin cos cos

2 sin cos sin 2 sin cos

2 sin cos sin cos sin

L L R

LR LR

LR LR

L L

32

222 2 2 2 2 2

12 2

2 2 2 2 2 2

2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2

2 2 2 2 2 2 2 2 2 2 2

2 2 sin21

(2

2 2 cos2

sin cos sin sin

cos sin sin cos cos co1

2

w

b

WR R

T mW

R R

R R R R L

L L Lm

2

2 2 2 2 2

2 2 2 2

s

2 sin sin 2 cos sin 2 sin cos

2 cos cos sin

LR LR LR

LR L

222 2 2 2 2

1

2 2 2 2 2 2 2 2 2 2 2

12 2

2 2

1 sin 2 sin 2 cos

2

w

b

WT m R R

m R R L L LR LR

(10)

แทนคาสมการ (1) ในสมการพลงงานจลนจากการเคลอนทแบบหมน (8) จะเปลยนรปใหอยในระบบพกดไดดงน

2 2

2 2

2

2 2 2 2

1 1 1 1

2 2 2 2 2 2

1 1( ) ( )

2 2 2 2

w w

m m

d W d WT J J J J

dt R dt R

W Wn J n J

R R

22 2 2 2

2 2

22 2 2 2

22

22 2

2

1 1 12

2 2 2 2

21 4

22

4

w

m

WT J J J

R

W W W

R R Rn J

W W W

R R R

22 2 2 2

2 2

22 2 2 2 2 2

2 2

1 1 12 2

2 2 2 2

1(2 2 4 )

2 2

by bz wz wy

g m e r

WT J J J J

R

WJ J n J J n n

R

(11)

33

จากสมการพลงงานศกย (9) เปลยนรปใหอยในระบบพกดไดดงน ( cos ) 2 cosw w b w b bU m gR m gR m g R L m gR m gR m gL จะไดคาลากรานจ L คอ 1 2L T T U (12)

222 2 2 2 2

2 2 2 2 2 2 2 2 2 2 2

22 2 2 2

2

2

12 2 4

2 2

sin 2 sin1

2 2 cos 2 2 cos

1 1 1 2 2

2 2 2 2

1

2

w

b

wy by bz wz

g m e

WL m R R gR

R R L L LRm

LR gR gL

WJ J J J

R

J J n J

2

2 2 2 2 2

2 22 2 4

2r

WJ n n

R

(13)

ก าหนดให 2 2 2

2o g m e rJ J J n J J n n (14) สมการลากรานจสามารถหาไดจากสมการ

d L LF

dt

(15)

d L LF

dt

(16)

d L LF

dt

(17)

จากสมการลากรานจทง 3 พกดจะไดอยในรป 3 สมการ คอ สมการ (18), (19), (20)

2 2 2 2 22 sinw b b

Lm R m R m LR

34

2 22 cos 2 2 2w b b wy o o

Lm R m R m LR J J J

2 2 22 cos sin 2 2 2w b b b wy o o

Lm R m R m LR m LR J J J

t

2 2

2 2 2 2

2 2 2 cos 2

sin 2 sin

w b wy o b o

b w b

L Lm R m R J J m LR J

t

m LR m R m R mLR F

(18)

2 2 2sin cos cos sin sinb b b b

Lm L m LR m LR m gL

2

0 0cos 2 2b b by

Lm L m LR J J J

2

0 0cos sin 2 2b b b by

Lm L m LR m LR J J J

t

2

0 0

2 2

2 cos 2 sin

sin cos

b b b

b b

L Lm L J J m LR J m gL

t

m L m LR F

(19)

0L

2 2 2 2 2 2 2

2

0 2

12 sin

2

12 sin 2 2

2 2

w w b b

b wy bz wz

Lm R m W m R m L

Wm LR J J J J

R

35

2 2 2 2 2 2 2

2

0 2

2 2 2

12 sin

2

12 sin 2 2

2 2

2(2 sin cos cos sin )

w w b b

b wy bz wz

w b b b b

Lm R m W m R m L

t

Wm LR J J J J

R

m R m R m L m LR m LR

22

02

2 2 2 2

2 2

1 12 ( )

2 2 2

(2 ) 2 sin sin

2 2 sin cos sin cos

w bz wz wy

w b b b

w b b b

Wm W J J J JL L

Rt

m m R m LR m L

m m R m L m LR F

(20)

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

1

2l rF F F (21)

r l

RF F F

W (22)

แบบจ าลองหนยนตสองลอแบบพารามเตอรมอเตอรซายขวาเทากน โดยพจารณาใหแรงทกระท ากบลอและแรงทท าใหเกดการโนมเอยงคอ แรงบดของมอเตอรกระแสตรง แรงเสยดทานระหวางมอเตอรกบลอ และแรงเสยดทานระหวางลอกบพน ซงสามารถเขยนไดดงสมการ _ ( )l t l l m l w lF nK i f f (23) _ ( )r t r r m r w rF nK i f f (24) _ _ ( ) ( )t l l t r r m l m rF nK i nK i f f (25)

โดย ,l b brl l r r

m m m m

V K KVi i

R R R R (26)

36

แทนคาสมการ (26) ใน (23), (24) ,(25) จะไดสามสมการของแรงในสามระบบพกดในเทอมของสญญาณปอนเขา ,l rV V

1

2

t b bl r t m w t

m m m

nK K KF V V nK f f nK

R R R

(27)

_ _

_ _ _

2 2b l b l

l r t m t m

m l m l m l

K KnKF V V nK f nK f

R R R

(28)

t t br l w

m m

K K KRF n V V n f

W R R

(29)

แทนคา (27), (28), (29) ในสมการ (18), (19), (20) และแทนสมการ (1) เพอถกจดรปใหอยในระบบพกด , , จะไดสมการพลศาสตรสามสมการ

2

0 0

2 2 2 2

2 2 2 cos 2

sin 2 sin

1

2

w b wy b

b w b

t b bl r t m w t

m m m

m m R J J m LR J

m LR m R m R mLR

nK K KV V nK f f nK

R R R

(30)

2

0 0

2 2

_ _

_ _ _

2 cos 2 sin

sin cos

2 2

b b b

b b

b l b l

l r t m t m

m l m l m l

m L J J m LR J m gL

m L m LR

K KnKV V nK f nK f

R R R

(31)

22

02

2 2 2 2

2 2

1 12 ( )

2 2 2

(2 ) 2 sin sin

2 2 sin cos sin cos

w bz wz wy

w b b b

w b b b

t t br l w

m m

Wm W J J J J

R

m m R m LR m L

m m R m L m LR

K K KRn V V n f

W R R

(32)

37

การประมาณระบบเปนเชงเสนในรปปรภมสเตต สมการ (30), (31), (32) นน เปนระบบไมเชงเสนทมความแมนย าแตมความซบซอน การออกแบบระบบควบคมในงานวจยนยงคงอยภายใตทฤษฎเชงเสน ดงนนจงประมาณระบบดงกลาวเปนเชงเสน คอ ก าหนดใหคา มคานอยๆ จงสามารถประมาณคาเทอมทไมเปนเชงเสน เชน sin , cos 1 และ 2 0 จะไดสามสมการพลศาสตรเชงเสนส าหรบการออกแบบระบบควบคมเชงเสน คอ สมการ

2

0 02 2 2 2

1

2

w b wy b

b b tt m w t l r

m m m

m m R J J m LR J

K K nKnK f f nK V V

R R R

(33)

2

0 02 cos 2 sin

2 2

b b b

b bt m t m l r

m m m

m L J J m LR J m gL

K K nKnK f nK f V V

R R R

(34)

22

02

1 12 ( )

2 2 2w bz wz wy

t b tw r l

m m

Wm W J J J J

R

K K KRn f n V V

R W R

(35)

สมการ(33), (34) และ (35) จดรปไดคอ

l

r

VE F G H

V

(36)

โดย 2

0 0

2

0 0

2 2 2 2

2 2

w b wy b

b b by

m m R J J m LR JE

m LR J m L J J

2 2

b bt m w t

m m

b bt m t m

m m

K KnK f f nK

R RF

K KnK f nK f

R R

38

0 0

0 b

Gm gL

1 1

2 2

t t

m m

m m

nK nK

R RH

nK nK

R R

และ r lK I J V V

โดย 2

2

02

1 12 ( )

2 2 2w bz wz wy

WK m W J J J J

R

t bw

m

K KI n f

R

t

m

KRJ n

W R

จากสมการ (36) จะไดสมการ 1,1 1,2 1,1 1,2 1,1 1,2l rE E F F H V H V (37)

และ

2,1 2,2 2,1 2,2 2,2

2,1 2,2l r

E E F F G

H V H V

(38)

สามารถแกสามารถหาคา ไดจาก (37) 2,2E - (38) 1,2E สามารถแกสมการหา ไดจาก (38) 1,1E - (37) 2,1E

ก าหนดให t

m

nK

R , t b

m

m

nK Kf

R

39

สามารถเขยนใหอยในรปปรภมสเตตไดคอ

1 1

2 2

0 0

0 0

A Bx Ax Bu x u

A B

(39)

เมอ , , , , ,T

x และ ,T

l ru V V

1

1 1 1

1 1 1

0 0 1 0

0 0 0 1

0 3,2 3,3 3,4

0 4,2 4,3 4,4

AA A A

A A A

1 3,2 1,2 detbA m gLE E

1 4,2 1,1 detbA m gLE E

1 3,3 2,2 2 1,2 detwA f E E E

1 3,4 2,2 2 1,2 detA E E E

1 4,3 1,2 2 1,1 detwA f E E E

1 4,4 1,2 2 1,1 detA E E E

1

1 1

1 1

0 0

0 0

3 3

4 4

BB B

B B

1 3 2,2 2 1,2 detB E E E

1 4 1,2 2 1,1 detB E E E

2det 1,1 2,2 1,2E E E E

2

0 1

0A I

K

40

2

0 0

B J J

K K

โดยเอาทพตระบบ คอ y Cx (40)

1 0 0 0 0 0

0 1 0 0 0 0

0 0 0 0 1 0

C

การออกแบบระบบควบคม บทนน าเสนอการออกแบบตวควบคมซงเปนหวใจของงานวจยน ระบบการควบคม ทใชคอการออกแบบระบบควบคมทเหมาะสมทสด (optimal control) (วบลย, 2548; วโรดม, 2551; Lewis and Syrmos, 1995; Ogata, 1997) และในบทนประกอบดวย 3 สวน คอ คณสมบตความควบคมไดและสงเกตได การออกแบบตวควบคมดวยตวคมคาก าลงสองนอยทสด ส าหรบการออกแบบระบบควบคมในงานวจยนจะสอดคลองและเปนไปตามทฤษฎเมอระบบทตองการควบคมนนมคณสมบตความสามารถควบคมไดและสงเกตได

41

1. คณสมบตความควบคมได (Controllability) ถาระบบมคณสมบตความควบคมได (controllable) หมายความวา จะมสญญาณควบคมแบบตอเนองทสามารถควบคมระบบจากต าแหนงเรมตนไปยงต าแหนงใด ๆ ทตองการไดในชวงเวลาหนง และสามารถหาตวควบคมทเปนหนงเดยวตามสมการกฎการควบคมทท าใหสมการลกษณะ เฉพาะแบบพหนาม ( )c s ก าลง n ใดๆ ( ) detc s sI A BK เมอ 1nrank B AB A B n (41)

การพสจนคณสมบตความควบคมไดแสดงอยในภาคผนวก ค

2 3 4 5 6rank B AB A B A B A B A B

2 3 4 5ˆ ˆ ˆ ˆ ˆˆ ˆ ˆ ˆ ˆ ˆ 9rank B AB A B A B A B A B

ซงสามารถพจารณาไดดวยโปรแกรม MATLAB ดวยค าสง rank (ctrb (A, B)) 2. คณสมบตความสงเกตได (Observability) ถาระบบ ,A B มคณสมบตความควบคมได (Observability) หมายความวา ทเวลาใดๆ และระบบอยทต าแหนงใดๆ สามารถทบอกคาสเตตของระบบจากการพจารณาสญญาณปอนออกของระบบไดในชวงเวลาหนง เมอ 1nT T T T Trank C A C A C n

(42)

การพสจนคณสมบตความสงเกตไดแสดงอยในภาคผนวก ค 2 3 4 5 6T T T T T T T T T T Trank C A C A C A C A C A C ซงสามารถพจารณาไดดวยโปรแกรม MATLAB ดวยค าสง rank (obsv (A, C))

42

ตวคมคาก าลงสองนอยทสด (Linear Quadratic Regulator, LQR) จดเดนของการออกแบบดวยตวคมคาก าลงสองนอยทสด (linear quadratic regulator) ทเหนอกวาวธการวางโพลกคอ มแบบแผนทเปนระบบมากกวาในการค านวณหาคาอตราขยาย โด ยการออกแบบสญญาณควบคมซงท าใหคาดรรชนสมรรถนะ (performance index, J ) มคานอยทสด (minimization) ก าหนดใหดรรชนสมรรถนะ (performance index) คอ

0

T TJ x Qx u Ru dt

(43)

Q เปนเมตรกซสมมาตรทเปนบวกแนนอนซงเปนเมตรกซถวงน าหนกใหความส าคญในการควบคมของสเตตแตละตว และ R เปนเมตรกซสมมาตรทเปนกงบวกแนนอนท าหนาทถวงน าหนกส าหรบสญญาณควบคม สญญาณควบคมนถกก าหนดใหไมมขอจ ากด (unconstraint) บลอก ไดอะแกรมแสดงอยในภาพท 24

+ + cx

B

A

y

K

u

ภาพท 24 บลอกไดอะแกรมในการควบคมปอนกลบแบบรกษาเสถยรภาพ จากภาพท 24 จะได x Ax Bu (44)

y Cx ( ) ( )u t Kx t (45)

เมอ T

x , T

l ru V V

43

11 12 13 14 15 16

21 22 23 24 25 26

k k k k k kK

k k k k k k

เมตรกซ ,A B คอ พลานตจากสมการ (39) แทนคา u (45) ลงในสมการ (44) จะได ( )x Ax BKx A BK x (46) ก าหนดใหเมตรกซ A BK มเสถยรภาพหรอมคาเจาะจงเปนคาจรงเครองหมายลบ ทงหมด เมอแทนคา ( )u t ในสมการดรรชนสมรรถนะ (43) จะได

0

T T TJ x Qx x K RKx dt

(47)

0

T Tx Q K RK xdt

ก าหนดให

T T Tdx Q K RK x x Px

dt

TT T T T Tx Q K RK x x Px x Px x A BK P P A BK x

T TA BK P P A BK Q K RK (48)

ในภาคผนวก ค แสดงใหเหนวาเมอ A BK เปนเมตรกซทมเสถยรภาพ จะม P เปนเมตรกซสมมาตรทเปนบวกแนนอนสอดคลองกบสมการ (48) และในกรณทระบบมเสถยรภาพแลวสามารถหาคา P ทเปนเมตรกซสมมาตรทเปนบวกแนนอน ไดเพยงคาเดยว ดงนนคา P อนๆ ทไดไดจากการแกสมการ (48) จะไมเปนเมตรกซสมมาตรทเปนบวกแนนอน จากดรรชนสมรรถนะจะได

0

0 0T T T T TJ x Q K RK xdt x Px x Px x Px

(49)

และเนองจากคาเจาะจงของ A BK ถกก าหนดใหเปนลบทงหมด หรอ 0x ดงนน จะได 0 0TJ x Px

44

แสดงวาดรรชนสมรรถนะเปนฟงกชนของคาเรมตน 0x และเมตรกซ P เพอหาค าตอบ ก าหนดให TR T T เมอ R เปนเมตรกซสมมาตรทเปนบวกแนนอน และ T ไมเปนเมตรกซหนงหนวย จากสมการ (48) สามารถเขยนไดเปน 0T T T T TA K B P P A BK Q K T TK (50)

1 1

1 0T

T T T T T TA P PA TK T B P TK T B P PBR B P Q

(51)

โดยพยายามท าใหคา J ต าสดโดยการปรบคา K หรอตองการคาต าสดของ

1 1

TT T T T Tx TK T B P TK T B P x

ซงเปนพจนทมคาเปนกงบวกแนนอนจงมคาต าสดคอศนย ซงจะเปนเชนนนเมอ

1T TTK T B P

ดงนน

11 1T T TK T T B P R B P

(52)

สมการ (52) เปนสมการทใหคา K ทเหมาะสมทสดส าหรบระบบดงกลาว ดงนนเราสามารถค านวณหาสญญาณควบคมทเหมาะสมทสด คอ 1( ) ( ) Tu t Kx t R B Px t (53) ซงเมตรกซ P ในสมการ (53) ตองสอดคลองดบสมการ (48) หรอทลดรปลงดงสมการ (54) เรยกวา Reduced-matrix Riccati Equation คอ

1 0T TA P PA PBR B P Q (54)

45

สรปการออกแบบตวควบคมดวยเทคนคดงกลาว คอ 1. แกค าตอบของสมการ Reduced-matrix Riccati Equation (54) เพอหาคาเมตรกซ P เมอปรากฏคา P ทเปนเมตรกซสมมาตรทเปนบวกแนนอนแสดงไดวา ระบบมเสถยรภาพหรอเมตรกซ A BK มเสถยรภาพ 2. แทนคาเมตรกซ P ในสมการ (52) เพอค านวณหาคา K ทเหมาะสมทสด สวนถดไปในงานวจยนเปนปญหาการตามรอยสญญาณ (tracking) ในบางครงถกเรยกวา Servo Systems ซงเปนการขยายขอบเขตของปญหาการรกษาเสถยรภาพออกไปเลกนอย รปแบบการควบคมแสดงดงภาพท 25

+ + cx

B

A

y

2K

-1

r u+1K

ภาพท 25 บลอกไดอะแกรมในการควบคมปอนกลบแบบตามรอยสญญาณ จากภาพท 25 x Ax Bu (55) y Cx 2 1u K x K r y (56)

เมอ T

x , T

l ru V V

T

ref ref refr

11 12 15

1

21 22 25

k k kK

k k k

, 13 14 16

2

23 24 26

0 0 0

0 0 0

k k kK

k k k

46

11 12 13 14 15 16

21 22 23 24 25 26

k k k k k kK

k k k k k k

จากสมการ (55) , (56) 1x Ax Bu A BK x Bk r (57) ระบบมเสถยรภาพเมอโพลระบบปดอยในบนฝงซายของระนาบเชงซอน ดงนนคา y จะมคาลเขาสคาคงท r และ u มคาลเขาสคาศนย เมอ r เปนฟงกชนขนบนได พบวา 1x A BK x Bk r (58) ท r เปนฟงกชนขนบนได r r t r มคาคงทเมอ 0t และพจารณาสมการ (57) ลบดวยสมการ (58) จะได x t x A BK x t x ก าหนดให ex t x t x

( )e ex t A BK x t หรอ ( )e ex t Ax t Bu (59) และ eu Kx (60) ผลทได คอ การออกแบบระบบ (59) และ (60) ใหมเสถยรภาพหรอมคาลเขา โดยการออกแบบอตราขยาย K ใหสมการดรรชนสมรรถนะทมลกษณะคลายกบสมการ (43) ใหมคาต าสด โดยสมการดรรชนสมรรถนะส าหรบระบบนคอ

0

T T T

e e e eJ x Qx x K RKx dt

(61)

ท าการออกแบบโดย 0.1,1, 0.1,1, 0.1, 0.1Q diag

1,1R diag

47

จะไดคาอตราขยาย คอ

0.2236 6.0222 0.6541 1.4885 0.2236 0.5249

0.2236 6.0222 0.6541 1.4885 0.2236 0.5249K

วธการท า Vision Builder 1. ศกษาการใชโปรแกรม LabVIEW และ Vision Builder AI 2. สรางรถตนแบบจากอปกรณทม ไดแก เอนโคดเดอรสองตว มอเตอรสองตว สายพานสองเสน ลอรถสองขางและแผนอลมเนยม ดงแสดงในภาพท 26 และท าการตอสายเชอมตอขอมลกบคอมพวเตอร และสายไฟตางๆ

ภาพท 26 หนยนตรถสองลออจฉรยะ 3. สรางสนามทดลองหนยนตรถสองลออจฉรยะ 4. จ าลองการตรวจจบวตถโดยจ าลองวตถในกรณท วตถเปนชนเดยว ซงรถตองหลบหลก ดงภาพท 27

48

5. ท าการออกแบบการตรวจจบวตถในโปรแกรม Vision Builder AI โดยมรายละเอยดดงน 5.1 รบภาพทไดจากกลอง เขามายงโปรแกรมดงภาพท 27 โดยใชฟงกชน Acquire Image

ภาพท 27 รบภาพจากกลองเขามายงโปรแกรมโดยใชฟงกชน Acquire Image 5.2 ใชฟงกชน Find Edge เพอหาขอบของวตถทตรวจจบโดยแบงเปนในแนวซายไปขวา ขวาไปซาย และ ลางขนบน โดยสวนละหาเสน เพอวดเปนหาชวง ปรบคาของทกเสนดงน ฟงกชน Edge Polarity ตงคาเปน Bright to Dark ฟงกชน Edge Strength ตงคาเปน 15-25 ฟงกชน Smoothing ตงคาเปนยสบหาถงสามสบหา Steepness ตงคาเปนสบหาถงยสบหา คาทไดเปนคาทไดจากการประมาณและการทดลอง ดงแสดงในภาพท 29-35

49

ภาพท 28 แสดงการใชฟงกชนของการตรวจจบขอบ และการตงคาตางๆ

ภาพท 29 แสดงการตงคาใหวดขอบเจอเพยงขอบเดยว

50

ภาพท 30 เพมฟงกชนการตรวจจบขอบใหครอบคลมพนท

ภาพท 31 การใสฟงกชนใหครบแนวลางขนบน 5 เสน

51

ภาพท 32 เพมเสนตรวจจบขอบในแนวซายไปขวา 5 เสน เพอใหครอบคลมพนท

ภาพท 33 เพมเสนตรวจจบในแนวขวาไปซาย 5 เสน เพอใหครอบคลมพนท

52

เมอท าการเขยนฟงกชนในการตรวจวดขอบแลว จะตองท าการเกบคาของระยะตางๆ ทท าการตรวจวดได โดยระยะตางๆ นนประกอบดวย เสนขอบทวดจากลางขนบน (เพอวดระยะจากพนถงวตถเพอกะระยะทตองการหยดตวรถ) เสนทหนง ก าหนดใหชอ Floor 1 เสนทสอง ก าหนดใหชอ Floor 2 เสนทสาม ก าหนดใหชอ Floor 3 เสนทส ก าหนดใหชอ Floor 4 เสนทหา ก าหนดใหชอ Floor 5 เสนขอบทวดจากซายไปขวา (เพอวดระยะจากขอบของภาพจากกลองไปยงวตถ เพอตดสนใจในการเลยวรถ) เสนทหนง ก าหนดใหชอ Right 1 เสนทสอง ก าหนดใหชอ Right 2 เสนทสามก าหนดใหชอ Right 3 เสนทส ก าหนดใหชอ Right 4 เสนทหา ก าหนดใหชอ Right 5 เสนขอบทวดจากขวาไปซาย (เพอวดระยะจากขอบของภาพจากกลองไปยงวตถ เพอตดสนใจในการเลยวรถ) เสนทหนง ก าหนดใหชอ Left 1 เสนทสอง ก าหนดใหชอ Left 2 เสนทสาม ก าหนดใหชอ Left 3 เสนทส ก าหนดใหชอ Left 4 เสนทหา ก าหนดใหชอ Left 5 ใชฟงกชน Calculator ในการวดระยะวตถเปนหนวย พกเซล เนองจากไมตองปรบคา สามารถใชไดทนท โดยระยะ ในแนวแกน x จะเรมจากซายสดไปยงขวาสดของกรอบจะมคาเทากบ หกรอยหกสบพกเซล และจากบนสดไปยงลางสดมคาเทากบสรอยสพกเซล ซงเปนไปตามคณสมบตของกลอง คอ มขนาดหกรอยหกสบคณสรอยเกาสบสพกเซล โดยก าหนดใหวดคาของทกเสน โดยก าหนดใหเปนชอ xxx distance เชน Floor distance 1, Left distance 2 เปนตน และสามารถแสดงไดดงภาพท 34

53

ภาพท 34 แสดงฟงกชนทใชในการค านวณเพอหาระยะขอบของวตถทวดได

ภาพท 35 แสดงการน าคาระยะทไดเปน พกเซล เพอเกบเปนคาตวเลข

54

ใชฟงกชน Run Labview VI เพอน าโปรแกรมทเขยนจากโปรแกรม LabVIEW เพอใชงานรวมกบคาตวแปรทวดไดจากวตถเพอน าไปควบคมมอเตอรโดยจะกลาวถงตอไป และแสดงไดดงภาพท 36

ภาพท 36 การเพมฟงกชน Run Labview VI

ภาพท 37 แสดงการน าไฟล LabVIEW มาใชประยกตระหวางการตรวจจบดวยภาพ และ การควบคมมอเตอร

55

เขยนฟงกชนควบคมมอเตอรในโปรแกรม LabVIEW โดยการรบคามาจากโปรแกรม Vision Builder AI โดยจะแบงเปนสองไฟล คอ ไฟลทใชในการรบคาตวแปรตางๆ จาก Vision Builder ทงสบหาตวแปร โดยรบคามาแลวก าหนดใหเปนตวแปร ส าหรบไฟลนจะเปนไฟลทเรยกโดย Vision Builder เพอเกบคามาและสงใหเปนตวแปรทจะใชงานยงโปรแกรมอนตอไป และไฟลทใชในการประมวลผล หรอการตดสนใจในการเคลอนทหรอการหมนของมอเตอร ดงแสดงในภาพท 38

ภาพท 38 แผนภาพของไฟล LabVIEW ทจะรบคาตวแปรตางๆ ทงสบหาตว

56

ภาพท 39 แสดงหนาของไฟลควบคมการหมนของมอเตอร ส าหรบฟงกชนการควบคมมอเตอรนน จะใชหลกการเลยวซายและขวาเมอ มการตรวจวดระยะทพนพบ และเมอพบแลวจะตรวจวดระยะ ดานซาย และดานขวา ใดๆ หากสวนใดมคาม ากกวาใหท าการหมนมอเตอรไปดานนน โดยการเปดโปรแกรมนนจะตองเรมจากโปรแกรมควบคมมอเตอรเปนล าดบแรก ตอดวย Vision Builder สวนโปรแกรมทจะใชในการสงคาตวแปรตางๆ จะขนมาอยางอตโนมต ส าหรบการเคลอนทหากไมสามารถตรวจจบได จะสงใหวงไป ดานหนาตรงๆ และเลยวซาย หรอเลยวขวากขนอยกบจะพบวตถดานใด

57

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

ผลการจ าลองดวยโปรแกรม MATLAB การจ าลองดวยโปรแกรม MATLAB แบงออกเปน 5 สวน สวนท 1 ถง 4 เปนการออกแบบตวควบคมประเภทตางๆ สวนท 5 เปนการออกแบบการวดมมเอยงดวยตวสงเกตสเตต ผลการออกแบบตวควบคมอาศยตวคมคาก าลงสองนอยทสด (LQR) จากการออกแบบระบบควบคมแบบอาศยตวคมคาก าลงสองนอยทสด ออกแบบสมการสมรรถนะโดยก าหนด 0.1,1, 0.1,1, 0.1, 0.1Q diag และ 1,1R diag จะไดคาอตราขยาย คอ

0.2236 6.0222 0.6541 1.4885 0.2236 0.5249

0.2236 6.0222 0.6541 1.4885 0.2236 0.5249K

คาอตราขยายดงกลาวน าไปใชกบระบบควบคมแบบอาศยตวคมคาก าลงสองนอยทสด จะได ผลดงน 1. ระบบรกษาเสถยรภาพ การรกษาเสถยรภาพ คอ เมอเวลาผานไปสเตตระบบจะลเขาหาคาทแนนอน ก าหนดใหต าแหนงเรมตนของสเตตของระบบไมอยทศนยแตอยท 1 เรเดยน และ 0.2 เรเดยน จากผลการจ าลองระบบดงภาพท 40 แสดงใหเหนวาระบบสามารถรกษาเสถยรภาพไดโดยต าแหนงมม จะเคลอนทเขาสต าแหนงสมดลไดดวยเวลา 3 วนาท แตต าแหนงมม มลกษณะ

58

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

0 1 2 3 4 5 6 7-2

0

2

0 1 2 3 4 5 6 7-0.5

0

0.5

0 1 2 3 4 5 6 7 8

-0.5

0

0.5

0 1 2 3 4 5 6 7

-2

0

2

วนาท

วนาท

วนาท

ต าแหนงอางอง ต าแหนงจรง

ต าแหนงมม (rad)

ต าแหนงมม (rad)

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

แรงดนไฟฟา (volt)

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

มอเตอรขวา มอเตอรซาย

0 1 2 3 4 5 6 7-2

0

2

0 1 2 3 4 5 6 7-0.5

0

0.5

0 1 2 3 4 5 6 7 8

-0.5

0

0.5

0 1 2 3 4 5 6 7

-2

0

2

ภาพท 40 ผลการจ าลองการรกษาเสถยรภาพของระบบควบคมอาศยตวคมคาก าลงสองนอยทสด

59

0 10 20 30 40 50 60 70 80 90 100-5

0

5

0 10 20 30 40 50 60 70 80 90 100-0.2

0

0.2

0 10 20 30 40 50 60 70 80 90 100

-0.5

0

0.5

0 10 20 30 40 50 60 70 80 90 100

-2

0

2

0 10 20 30 40 50 60 70 80 90 100-5

0

5

0 10 20 30 40 50 60 70 80 90 100-0.2

0

0.2

0 10 20 30 40 50 60 70 80 90 100

-0.5

0

0.5

0 10 20 30 40 50 60 70 80 90 100

-2

0

2

วนาท

ต าแหนงมม (rad)

ต าแหนงมม (rad)

ต าแหนงอางอง ต าแหนงจรง

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5 ต าแหนงมม (rad)

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

rV lV

วนาท

วนาท

วนาท

แรงดนไฟฟา (volt)

ภาพท 41 ผลการจ าลองการตดตามคาของระบบควบคมอาศยตวคมคาก าลงสองนอยทสด

-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3-0.1

-0.05

0

0.05

0.1

0.15

0.2

ต าแหนง x (m)

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

ต าแหนงอางอง ต าแหนงจรง

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

ต าแหนง y (m)

ภาพท 42 ผลการจ าลองการเคลอนในสองมตของระบบควบคมอาศยตวคมคาก าลงสองนอยทสด

60

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

31 5s สญญาณอางองส าหรบมม คอ

ศนย สญญาณอางองส าหรบมม คอสญญาณรปสเหลยมขนาดหนงหนวยทผานตวกรองความถต า

31 5s เชนเดยวกน

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

61

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

0.2236 6.0222 0.6541 1.4885 0.2236 0.5249

0.2236 6.0222 0.6541 1.4885 0.2236 0.5249K

โดยแบงการทดลองเปน 3 แบบ คอ การรกษาเสถยรภาพ การตาม รอยสญญาณอางอง และการตามรอยสญญาณอางองโดยเพมมวลใหระบบ ในการทดลองท 1 เปนการทดลองการรกษาเสถยรภาพของระบบ โดยคาเรมตนของทกสเตตเปนศนย และพลกหนยนตใหลมลงทเวลาประมาณ 2 ถง 2.5 วนาท ผลทไดแสดงดวยภาพท 3

2 3 4 5 6 7-5

0

5

2 3 4 5 6 7-0.5

0

0.5

0 1 2 3 4 5 6 7 8 9-0.05

0

0.05

2 3 4 5 6 7

-2

0

2

2 3 4 5 6 7-5

0

5

2 3 4 5 6 7-0.5

0

0.5

0 1 2 3 4 5 6 7 8 9-0.05

0

0.05

2 3 4 5 6 7

-2

0

2

วนาท

วนาท

วนาท

ต าแหนงอางอง ต าแหนงจรง

ต าแหนงมม (rad)

ต าแหนงมม (rad)

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

แรงดนไฟฟา (volt)

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

มอเตอรขวา มอเตอรซาย

2 3 4 5 6 7-5

0

5

2 3 4 5 6 7-0.5

0

0.5

0 1 2 3 4 5 6 7 8 9-0.05

0

0.05

2 3 4 5 6 7

-2

0

2

2 3 4 5 6 7-5

0

5

2 3 4 5 6 7-0.5

0

0.5

0 1 2 3 4 5 6 7 8 9-0.05

0

0.05

2 3 4 5 6 7

-2

0

2

ภาพท 43 การทดลองการรกษาเสถยรภาพของหนยนตสองลอ

62

จากภาพท 43 รปบนแสดงการเคลอนทของหนยนต ต าแหนงมม แสดงผลลกษณะเดยวกบการจ าลองคอจะเปนระบบไมใชมมเฟสนอยทสด หนยนตเคลอนทไปในทศทางตรงกนขามกอนทจะเขาสจดสมดล รปกลางแสดงต าแหนงมม คอ การเอยงของหนยนต โดยหนยนตสามารถรกษาเสถยรภาพไดโดยหลงจากทเวลา 2.5 วนาท มม 0.2 เรเดยน ระบบพยายามหนวงมมเอยงท าใหโอเวอรชตท 0.1 เรเดยนและกลบเขาสจดสมดลในระยะเวลา 3 วนาท รปลางแสดงแรงดนไฟฟาของมอเตอร ซงมอเตอรทงสองเคลอนทดวยแรงดนไฟฟาเทากนทเวลา 2.5 วนาท จะมแรงดนไฟฟาสงสดแตไมเกนกวา 2.5 โวลล การทดลองท 2 เปนการทดลองการตดตามคาเชนเดยวกบในการออกแบบดวยการจ าลอง ระบบสญญาณทตองการตดตามหรอสญญาณอางองมลกษณะเดยวกน ผลทไดแสดงดงภาพท 44

90 100 110 120 130 140 150 160 170 180-5

0

5

90 100 110 120 130 140 150 160 170 180-0.5

0

0.5

90 100 110 120 130 140 150 160 170 180-1

0

1

90 100 110 120 130 140 150 160 170 180

-2

0

2

90 100 110 120 130 140 150 160 170 180-5

0

5

90 100 110 120 130 140 150 160 170 180-0.5

0

0.5

90 100 110 120 130 140 150 160 170 180-1

0

1

90 100 110 120 130 140 150 160 170 180

-2

0

2

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4 แรงดนไฟฟา (volt)

วนาท

มอเตอรขวา มอเตอรซาย

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

ต าแหนงมม (rad)

ต าแหนงอางอง

ต าแหนงมม (rad)

วนาท

วนาท

วนาทต าแหนงจรง

ต าแหนงมม (rad)

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

ภาพท 44 ผลการทดลองการตดตามคาของหนยนตสองลอ

63

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

-0.4 -0.2 0 0.2 0.4-0.05

0

0.05

0.1

0.15

0.2

0.25

ต าแหนง x (m)

ต าแหนง y (m)

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

ต าแหนงอางอง ต าแหนงจรง

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

ก

ข

ค

ภาพท 45 ผลการทดลองการเคลอนในสองมตของหนยนตสองลอ

64

จากภาพท 45 แสดงการเคลอนทในสองมต การเคลอนเรมจากต าแหนง ก . ไปยง ข . และ ค. และ ข . ตามล าดบ โดยทต าแหนงสมดล หรอจด ก . และ ค . มการแกวงของระบบใหเหน และ การเคลอนทของหนยนตตามเสนทางการเคลอนทอางองมความผดพลาด เนองจากแรงเสยดทานและน าหนกสายไฟเปนหลก การทดลองอนดบสดทาย คอ การตดตามคาโดยเพมมวลใหกบระบบ การเพมมวล 680 กรม ท าใหมวลตวถงเพมและต าแหนงจดศนยถวงเปลยนไป ในการทดลองนเปนการทดสอบความทนทานของระบบควบคมเมอแบบจ าลองนนเปลยนไป แสดงในภาพท 46

90 100 110 120 130 140 150 160 170 180-5

0

5

90 100 110 120 130 140 150 160 170 180-0.2

0

0.2

90 100 110 120 130 140 150 160 170 180-1

0

1

90 100 110 120 130 140 150 160 170 180

-2

0

2

90 100 110 120 130 140 150 160 170 180-5

0

5

90 100 110 120 130 140 150 160 170 180-0.2

0

0.2

90 100 110 120 130 140 150 160 170 180-1

0

1

90 100 110 120 130 140 150 160 170 180

-2

0

2

วนาท

ต าแหนงมม (rad)

ต าแหนงมม (rad)

ต าแหนงอางอง ต าแหนงจรง

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5ต าแหนงมม (rad)

วนาท

วนาท

วนาท

แรงดนไฟฟา (volt)

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

มอเตอรขวา มอเตอรซาย

ภาพท 46 ผลการทดลองการตดตามคาของหนยนตสองลอทมวลเพม

65

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

-0.4 -0.2 0 0.2 0.4

-0.05

0

0.05

0.1

0.15

0.2

0.25

ต าแหนง x (m)

ต าแหนง y (m)

8.3 8.35 8.4 8.45 8.5 8.55

-1

0

1

2

3

4

ต าแหนงอางอง ต าแหนงจรง

7 7.5 8 8.5 9 9.5 10 10.5 11 11.5 12

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

ก

ข

ค

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

66

ผลการรบภาพจากกลองวดโอ เมอกลองวดโอสามารถจบภาพของพนถนน และมองเหนสงกดขวางได กลองวดโอจะสงภาพผานสายเคเบลเขามายงโปรแกรม Vision Builder และแสดงผลบนหนาจอคอมพวเตอร เปนภาพเดยวกนกบกลองวดโอมองเหน ดงรปท 48 โดยใชฟงกชน Acquire Image

ภาพท 48 การวดระยะวตถดวยโปรแกรม Vision Builder

ตรรกะของรถสองลออจฉรยะ เมอปลอยใหรถสองลอวง รถวงไปพบวตถทางดานขวา ถาวตถนนขวางรถสองลอไมสามารถเดนหนาตอไปไดกจะเลยวซาย แตถาไมขวางก จะวงไปตอ แตถาพบ วตถทางดานขวาอกรถกจะท าตามค าสงดานบน แตถาพบวตถทางดานซายขวางอยไมสามารถเดนหนาตอไปได รถกจะเลยวขวาและวงไปตอ แตถาไมขวาง รถสองลอกจะวงไปตอ แตถาพบ วตถทางดานซายหรอดานขวาขวางอก รถกจะท าตามค าสงดานบนอกครง และถาเจอวตถทงสองดานรถกจะหยดและถอย หลงเรมการการประมวลผลอกครงตามตรรกะทก าหนดไว ดงภาพท 49

67

START

ภาพท 49 ตรรกะของรถสองลออจฉรยะ

สนามในการทดสองรถสองลออจฉรยะ

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

ภาพท 50 สนามทดลองของรถสองลออจฉรยะ

68

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

69

ภาพท 52 ผลการทดลองจรงการแสดงแรงดนไฟฟาของหนยนตสองลออจฉรยะ

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

70

สรปและขอเสนอแนะ

สรป การควบคมหนยนตสองลออจฉรยะเปนปญหาทระบบม 3 เอาทพต และม 2 อนพต และการควบคมเอาทพตสองตวทมความเกยวโยงกนมาก คอ การควบคมการเคลอนทไปดานหนาและหลงหรอมม กบความเอยงหรอมม ซงหนยนตจะสามารถเคลอนทไปดานหนาไดตอเมอหนยนตเอยงไปดานหนา ในขณะทการควบคมมมเลยวสามารถควบคมไดอยางมประสทธภาพ ซงการออกแบบระบบควบคมนนตองมความสมพนธกบกลองรบภาพดวย ซงท าหนาทรบภาพสงกดขวางบนถนน และน าไปประมวลผลใหรถสองลออจฉรยะหลบสงกดขวางได การออกแบบระบบควบคมของหนยนตสองลออจฉรยะ โดยอาศยตวคมคาก าลงสองเชงเสน และใชโปรแกรม Vision bulder ในการควบคมกลองรบภาพ และสงคาตางๆ ผานโปรแกรม Lab View เพอการประมวลผล และผลทไดผลจากการจ าลองและการทดลองตววดความเอยงมประสทธภาพทดในยานความถสง ในชวงของความถทก าหนด ผลการจ าลองและทดลองการควบคมระบบดงกลาวแสดงผลเปนทนายอมรบ และมประสทธภาพของตวควบคมทไดออกแบบไว ถงแมวาการทดลองระบบจะเกดการแกวง เนองมาจากแรงเสยดทานของมอเตอร ระยะหางของเฟองมอเตอร และน าหนกของสายไฟทแกวงไปมา แตกสามารถทรงตวไดดในขณะเคลอนท และกลองรบภาพสามารถรบภาพไดอยางแมนย า ขณะพบสงกดขวาง ท าใหหนยนตสองลออจฉรยะหลบสงกดขวางไดด

ขอเสนอแนะ การออกแบบระบบควบคมนอยพนฐานของการควบคมเชงเสน การหาเอกลกษณของมอเตอรและตวตรวจร Inclinometer ดวยก าลงเชงเสนนอยทสดใหผลคลาดเคลอนบางสวน การหาแบบ จ าลองของระบบดวยสมการลากรานจอาจละเลยพลงงานในบางสวนไป และการประมาณระบบดงกลาวเปนเชง ท าใหผลการควบคมทหางจากมมศนยองศามากขนยงมความคลาดเคลอนมากขน การปรบเปลยนใหเปนระบบไมเชงเสนนนนาจะท าใหสามารถควบคมหนยนตไดดขน รวมถงการใชแบบแปรผนคาได (Adaptive Control) เชน Neural Network หรอ Fuzzy Logic กนาจะเปนอกทางทเหมาะสม

71

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

72

เอกสารและสงอางอง วโรดม ตจนดา. 2550. การวเคราะหและออกแบบระบบควบคมทางวศวกรรม. มหาวทยาลยเกษตรศาสตร, กรงเทพฯ. วบลย แสงวระพนธศร. 2548. การควบคมระบบพลศาสตร. พมพครงท 2. จฬาลงกรณมหาวทยาลย, กรงเทพฯ. กตตพงศ เยาวาจา. 2551. การควบคมทเหมาะสมทสดของหนยนตสองลอดวยการความเอยง แบนดวทธสง. มหาวทยาลยเกษตรศาสตร, กรงเทพฯ. Akesson, J., A. Blomdell and R. Brarun. 2006. Design and Control of YAIP-an Inverted Pendulum on Two Wheels Robot. Proceedings of the 2008 IEEE International Conference on Control Applications, October 4-6, 2006, Munich, Germany. Anonymous. 1935. Gyro-wheel car zooms along on giant tires at 116 mph. Modern Mech. Inventions, 14. Baerveldt, A. J. and R. Klang. 1996. A low-cost and low-weight attitude estimation system for an autonomous helicopter, pp 391-395. Proceedings of IEEE Conf. Intelligent Engineering System. Grasser, F., A. D. Arrigo, S. Colombi and A. Ruffer. 2002. JOE: A Mobile, Inverted Pendulum. IEEE Transactions on Industrial Electronics, 49 (1). Ha, Y. and S. Yuta. 1996. Trajectory Tracking Control for Navigation of The Inverse Pendulum Type Self-contained Mobile Robot. Robotic and Autonomous System, 17(1-2): 65-80.

73

Hashimoto, M. and F. Oba. 1993. Dynamic control approach for motion coordination of multiple wheeled mobile robots transporting a single object, Proceedings of 1993 IEEE/RSJ International Conference on Intelligent Robots and Systems: 1944-1951. Imamura, R., T. Takei and S. Yuta. 2008. Sensor Drift Compensation and Control of a Wheeled Inverted Pendulum Mobile Robot, pp 137-142. Proceedings of Advanced Motion Control, 10th IEEE International Workshop. Jeong, S. and T. Takahashi. 2007. Wheeled Inverted Pendulum Type Assistant Robot: Inverted Mobile, Standing, and Sitting Motions. Proceedings of the International Conference on Intelligent Robots and Systems, Oct 29- Nov 2, 2007, San Diego, CA, USA. Kaiko, M., T. Miyashita, N. Mitsunaga, H. Ishiguro and N. Hagita. 2006. Natural Reflexive Behavior for Wheeled Inverted Pendulum Type Humanoid Robots. The 15th IEEE International Symposium on Robot and Human Interactive Communication (RO-MAN06), September 6-8, 2006, Hatfield, UK. Kim, Y. H., S. H. Kim and Y. K. Kwak. 2005. Dynamic Analysis of a Nonholonomic Two-wheeled Inverted Pendulum Robot, pp. 505-513. Proceedings of the 8th on Artificial Life and Robotics 21 (3). Ko, A., H. Y. K. Lau and T. L. 2005. Lau SOHO security with mini self-balancing robots. Industrial Robot: An International Journal 32(6) (2005): 492-498. Leavitt, J., A. Sideris and J. E. Bobrow. 2006. High Bandwidth Tilt Measurement Using Low-Cost Sensors. IEEE/ASME Transactions on Mechatronics, 11 (3). Lewis, F. L. And V. L. Syrmos. 1995. Optimal Control. 2nd ed. John Wiley & Sons, Inc. New York.

74

Lin, S. C., C. C. Tsai and W. L. Luo. 2007. Adaptive Neural Network Control of a Self-balancing Two-wheeled Scooter, pp. 869-873. The 33rd Annual Conference of the IEEE Industrial Electronics Society (IECON), Nov. 5-8, 2007, Taipei, Taiwan. Morrell, B. J. and D. Field. 2007. Design of a closed loop controller for a two wheeled balancing transporter, pp 4059-4064. Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Oct 29-Nov 2, 2007 San Diego, CA, USA. Nawawi, S. W., M. N. Ahmad and J. H. S. Osman. 2007. Development of a Two-Wheeled Inverted Pendulum Mobile Robot. Proceedings of the 5th Student Conference on Research and Development-SCOReD 2007, 11-12 December, Malaysia. Ogata, K. 1997. Modern Control Engineering. 3rd ed. Prentice-Hall. New York. Rehbinder, H. and X. Hu. 2004. Drift-free attitude estimation for accelerated rigid bodies. Automatica 40: 653-659. Ren, T. J., T. C. Chen and C. T. Chen. 2008. Motion control for a two-wheeled vehicle using a self-tuning PID controller. Control Engineering Practice 16: 365-375. Shiroma, N., O. Matsumoto, S. Kajita and K. Tani. 1996. Cooperative Behavior of a Wheeled Inverted Pendulum for Object Transportation. Proceeding of IROS 96. Sasaki, M., N. Yanagihara, O. Matsumoto and K. Komoriya. 2005. Steering control of the personal riding-type wheeled mobile platform (PMP), pp. 1697-1702. Conference on Intelligent Robots and Systems, 2005 IEEE/RSJ International. (IROS 2005), 2-6 Aug. 2005.

75

Vaganay, J., M. J. Aldon and A. Fournier. 1993. Mobile robot attitude estimation by fusion of inertial data, pp. 277-282. Proceedings of IEEE. Internation Conference of Robot and Automation. 1. 2-6 May 1993, Atlanta, GA, USA. Yomamoto, Y. 2008. NXTway-GS (Self-Balancing Two-Wheeled Robot) Controller Design. http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=19147 Yano, M., S. Kuramochi and H. Ikeda. 1998. Evaluation of the Robustness of H Control Design versus Pole Allocation Control Design Using of a Wheeled Inverted Pendulum. AMC’ 98.

76

ภาคผนวก

77

ภาคผนวก ก

การหาคาพารามเตอรดวยโปรแกรมเขยนแบบสามมต

78

การหาคาพารามเตอรดวยโปรแกรมเขยนแบบสามมต โมเมนตความเฉอยเปนตวแปรทส าคญส าหรบการวเคราะหพลศาสตรทางกลของหนยนตสองลอ การหาโมเมนตความเฉอยส าหรบวตถทรงเรขาคณตสามารถหาไดจากสตรทางคณตศาสตรซงมอยในหนงสอกลศาสตรวศวกรรมทกเลม แต ส าหรบรปรางทซบซอนมากขนการค านวณดวยมออาจยงยาก โปรแกรมเขยนแบบสามมตเปนอกทางเลอกส าหรบการแกปญหาดงกลาว ส าหรบงานวจยนจงอาศยโปรแกรมเขยนแบบสามมต Solid Works รวมถงการสามารถหาจดศนยถวงของหนยนตสองลอดวย ในแตละชนสวนของหนยนตอยวดระยะและชงน าหนกอยางละเอยด เพอน ามาเขยนแบบสามมตในแตละชนสวน โดยในโปรแกรมดงกลาว สวนของชนงานเรยกวา Part ซงคาน าหนกและขนาดถกน ามาค านวณความหนาแนนโดยเพอระบภายในโปรแกรม การเขยนแบบของสวนประกอบเรยกวา Assembly ซงประกอบมาจากชนงานตางๆ ทเราเขยนในขนตอนกอนหนาน จากนนสรางแกนพกด , ,x y z ดงแสดงในภาพผนวกท ก1 จงสามารถค านวณคาโมเมนตความเฉอยของหนยนตสองลอตอแกนพกด และสามารถหาคาจดศนยถวงของตวโครงหนยนตจากคาเฉลยแบบถวงน าหนกของแตละชนงาน

xz

y

ภาพผนวกท ก1 รป 3 มตของหนยนตสองลอและแกนพกด 0 0 0( , , )x y z ทมา: กตตพงศ (2551)

79

คาพารามเตอรทค านวณได

L = ระยะหางของจดศนยถวงในแนวแกน 0z = 0.04895 m

byJ = โมเมนตความเฉอยตวถงรอบแกน 0y = 0.0173198 2kgm

wyJ = โมเมนตความเฉอยของลอรอบแกน 0y = 0.0013824 2kgm

bzJ = โมเมนตความเฉอยตวถงรอบแกน 0z = 0.0117333 2kgm

wzJ = โมเมนตความเฉอยของลอรอบแกน 0z = 0.0144355 2kgm

rJ = โมเมนตความเฉอยของสวนหมนทดรอบ 2n = 0.0000578 2kgm

80

ภาคผนวก ข

การหาเอกลกษณของระบบ

81

การหาเอกลกษณของระบบ 1. การหาเอกลกษณของมอเตอรกระแสตรง จากวงจรสมมลของมอเตอรกระแสตรง ดงภาพผนวกท ข1

Armature

mRmL

i

beV

J

m

ภาพผนวกท ข1 วงจรสมมลของมอเตอรกระแสตรง ทมา: กตตพงศ (2551) โดยแรงดนปอนเขาเทากบ V กระแสไฟฟาปอนเขา i คาความตานทานของมอเตอร mR คาความเหนยวน าของมอเตอร mL คาคงทแรงดนไฟฟายอนกลบมอเตอร bk และความสมพนธของแรงดนไฟฟายอนกลบ b be K สามารถเขยนความสมพนธของวงจรดงกลาวไดคอ m m b mV iL R i K (62) เนองจากคาความเหนยวน ามอเตอร mL มคาต ามากเมอเทยบกบพจนอนในสมการจงประมาณเทากบศนยได tk คอ คาคงทแรงบดมอเตอร และจากความสมพนธของแรงบดมอเตอร mT กบกระแสไฟฟาปอนเขา m tT nK i และสมการท (62) จะได

t t b m

m t

m m

nK V nK KT nK i

R R

(63)

82

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

ภาพผนวกท ข2 การทดลองการพสจนเอกลกษณของมอเตอรกระแสตรง ทมา: กตตพงศ (2551) อปกรณถกตดตงกบระบบปอนหนงสญญาณผานเขามอเตอรกระแสตรงและหนงสญญาณปอนกลบผานเอนโคดเดอรเซนเซอร และสมการการเคลอนทระบบนสามารถเขยนไดอยในรป 2m w m g e r wT J n J J n n J J (64)

แทนคาแรงบดมอเตอร (63) ในสมการท (64) จะได

2t t b m

m w m g e r w

m m

nK V nK KT J n J J nn J J

R R

2m

w m g e r w b m

t

RV J n J J nn J J K

nK

,T

w mV A B

83

สมการดงกลาวอยรปส าหรบใหวธการของก าลงสองนอยทสด คอ

Y

โดย 2 3 1 1, ,n nw mA V

YB

เมอ n คอ จ านวนของขอมลทไดจากการทดลองการพสจนเอกลกษณระบบ สมการดงกลาวก าหนดมาเพอท าใหคาความผดพลาด E มคานอยทสดเมอ E สอดคลองกบสมการ

2

2

YE

จะไดสมการ

1ˆ T TY

(65) ส าหรบการทดลองนจะสรางสญญาณปอนเขามอเตอรกระแสตรงเปนสญญาณไซนทแปรผนความถสญญาณจาก 0.1 เฮรต ถง 1 เฮรตภายในระยะเวลา 60 วนาท เพอใหคาทไดเปนคาทครอบคลมชวงความถท างานของระบบ สญญาณปอนกลบจากเอนโคดเดอรเซนเซอร คอ คา สามารถค านวณ หาคาสมประสทธ ไดจากสมการท (65) และจากการทดลอง 35 ครง พบวา คาความเบยง

มาตรฐานของขอมลต ากวา 58.3 10 และคาเฉลยของขอมลดงกลาว คอ 0.013794265 ส าหรบคาสมประสทธ A ลอซาย 0.004129229 ส าหรบคาสมประสทธ B ลอซาย 0.013444875 ส าหรบคาสมประสทธ A ลอขวา 0.004420931 ส าหรบคาสมประสทธ B ลอขวา คาพารามเตอรของมอเตอรทได คอ

_b lK = 0.004129229, _b rK = 0.004420931

84

t

m l

K

R

= 0.003718956, t

m r

K

R

= 0.003815600

2. การหาเอกลกษณของตวตรวจรวดความเอยง Inclinometer จากสมการพลศาสตรของตวตรวจร Inclinometer ดงแสดงในภาพผนวกท ข4 คอ

1 2 2 1 2 1 2 1 2sin sin cosJ c mgl ml y x

ภาพผนวกท ข3 แบบจ าลองของ Inclinometer อยางงาย c คอ สมประสทธความหนวง, l คอ ความยาวแขนระหวางจดหมนกบโพโตไดโอด, m คอ มวลของโพโตไดโอด, J คอ คาโมเมนตความเฉอย ประมาณใหระบบเปนเชงเสนได โดย 1 2 1 2sin , 1 2cos 1 และไมคดคาของ 1 2y

1 2 1 2 2

/, sin

/

J cg

ml c

หรอ 2 1 2 1 2, sin / /

Tg J c ml c

สมการดงกลาวอยรปส าหรบใหวธการของก าลงสองนอยทสด คอ

Y

(66)

85

โดย

2 3 1 1, ,n nw mA V

YB

เมอ n คอ จ านวนของขอมลทไดจากการทดลองการพสจนเอกลกษณระบบ สมการดงกลาวก าหนดมาเพอท าใหคาความผดพลาด E มคานอยทสดเมอ E สอดคลองกบสมการ

2

2

YE

จะไดสมการ

1ˆ T TY

(66) ภาพผนวกท ข4 แสดงอปกรณการทดลองส าหรบการพสจนเอกลกษณของตวตรวจร Inclinometer ประกอบดวยอปกรณสามชนดตอรวมแกนหมนกนอย โดยใหจดศนยกลางของ inclinometer ตอรวมแกนหมนดวย

ภาพผนวกท ข4 การทดลองการพสจนเอกลกษณของตวตรวจร Inclinometer ทมา: กตตพงศ (2551)

86

ส าหรบการทดลองนจะสรางสญญาณปอนเขามอเตอรกระแสตรงเปนสญญาณไซนทแปรผนความถสญญาณจาก 0.1 เฮรต ถง 1 เฮรต ภายในระยะเวลา 60 วนาท เพอใหคาทไดเปนคาทครอบคลมชวงความถท างานของระบบ สญญาณปอนกลบจากเอนโคดเดอรเซนเซอร คอ คา สามารถค านวณหาคาสมประสทธได จากสมการท (66) และจากการทดลอง 35 ครง และ

คาเฉลยของขอมลดงกลาวคอ จากคาสมประสทธ A คอ / 0.0022J c จากคาสมประสทธ B คอ / 1.0314ml c

87

ภาคผนวก ค การพสจนทางคณตศาสตร

88

การพสจนทางคณตศาสตร 1. คณสมบตความควบคมได (Controllability) จากระบบควบคม x Ax Bu y Cx Du สามารถหาค าตอบสมการไดจาก

( )

0

( ) (0) ( )

t

At A tx t e x e Bu d (67)

ก าหนดใหเวลาเรมตน 0 0t สเตตเรมตน 0x จากนยามของคณสมบตการวบคมได คอ สเตตสดทายจะอยทจดก าเนด

1

1 1( )

0

( ) 0 (0) ( )

t

At A t

fx t e x e Bu d

(68)

หรอ 1

0

(0) ( )

t

Ax e Bu d (69)

จาก Cayley-Hamilton Theorem1

0

( )n

At k

k

k

e A

(70)

แทนคา (70) ใน (69) 11

0 0

(0) ( ) ( )

tnk

k

k

x A B u d

(71)

ก าหนดให 1

0

( ) ( )

t

k ku d (72)

แทนคา (72) ใน (71) 1

0

(0)n

k

k

k

x A B

0

1 1

1

(0) n

n

x B AB A B

(73)

89

ระบบจะมคณสมบตความควบคมไดเมอคาเรมตนใดๆ (0)x สามารถแกสามการ (73) เพอหาค าตอบมราเปนเอกภาพอยางนอยหนงค าตอบไดเมอ 1nrank B AB A B n

2. การพสจนเมตรกซ P ทสอดคลองกบสมการ Riccati Equation จากระบบควบคม x Ax

A เปนเมตรกซของคาคงทขนาด n n และก าหนดใหเปนเมตรกซทไมเทากบหนง (non-singular matrix) จากเมตรกซของสมการอนพนธ

TX A X XA มผลเฉลย คอ

TA t AtX e Qe เมอ Q เปนเมตรกซทเปนบวกแนนอน, (0)X Q เมออนทเกรตเมตรกซของสมการอนพนธจาก 0X ถง X จะได

0 0

0 TX X A Xdt Xdt A

พจารณาให A เปนเมตรกซสมดล ดงนน 0X

0 0

0 TX A Xdt Xdt A

ก าหนดให 0 0

TA t AtP Xdt e Qe dt

90

เมอ Ate สามารถหาคาไดจาก 1, ,...,i i i it t m t

e te t e เมอ i เปนคาเจาะจง (eigenvalues)

ของ A และ im คอจ านวนผลคณของ i เพราะ i เปนคาจรงจ านวนลบ ดงนน 0

TA t Ate Qe dt

สามารถหาคาได และก าหนดให

0

TT A t AtP e Qe dt P

ดงนน P เปนเมตรกซ Hermitian หรอเมตรกซสมมาตรทเปนจ านวนจรง สามารถแสดงไดวาเมตรกซสมดล A และเมตรกซ Q ทมคาเปนบวกแนนอน จะไดคา เมตรกซ P ทสอดคลองกบสมการ (74) เมอพจารณาจาก

0

TT T A t Atx Px x e Qe dtx

0

0T

T At Atx Px e x Q e x dt

ส าหรบ 0x และมคาเทากบ 0 เมอ 0x ดงนน ทสภาวะสมดล 0x ส าหรบระบบสมดลแบบมขอบเขต (asymptotically stable) หรอ A เปนเมตรกซทสมดล จะม P ทเปนเมตรกซเปนบวกแนนอนทสอดคลองกบสมการท (74)

TA P PA Q (74) 3. การพสจนการหาคา P และ K ของระบบตวคมคาก าลงสองนอยทสด จากระบบควบคม x ax bu

เมอ 0x สมรรถนะ คอ 2 2

0

J qx ru dt

91

โดยคา 0q และ 0r สญญาณควบคมทเหมาะสมไดจากสมการสมรรถนะทมคาต าสด คอ

u Kx

ดงนน จะไดสมรรถนะ 2 2

0

J q rK x dt

ก าหนดให 2 2 2dq rK x px

dt

หรอ 2 2 22 2q rK x pxx p a bK x จดรปได คอ 2 22 0q rK p a bK x จะเปนจรงทก x t 2 2 0q rK p a bK เมอ (75)

ส าหรบ a bk มเสถยรภาพจะเปนคาลบ ดงนนจะมคา p ทเปนบวกสอดคลองกบสมการ (75) หรอ 2a bK p p a bK q rK

หรอ

2

1

q rKp

a bK

(76)

โดยก าหนดให 2 2 2 2

0 0

0d

J q rK x dt px dt pxdt

ท าใหสมรรถนะต าสดไดเมอ 0p

K

จากสมการ (76)

2

2

20

2

rK a bk b q rKp

K a bK

92

จะได pbK

r (77)

และแทนคาสมการ (77) ในสมการ (75) จะไดสมการส าหรบหาคา p ทเปนคาบวก

2 2

2 0p b

q par

ทมา: กตตพงศ (2551)

93

ภาคผนวก ง โคดโปรแกรม MATLAB

94

โคดโปรแกรม MATLAB

1. การหาเอกลกษณของมอเตอรกระแสตรง

%load raw data input = load('File Location\ File Name.txt'); theta_encoder = load('File Location\ File Name.txt'); theta_encoderd=diff(theta_encoder)/ts; theta_encoderdd=diff(theta_encoderd)/ts; theta_motor=theta_encoder/n_2; theta_motord=diff(theta_motor)/ts; theta_motordd=diff(theta_motord)/ts; theta_wheel=theta_motor/n; theta_wheeld=diff(theta_wheel)/ts; theta_wheeldd=diff(theta_wheeld)/ts; J_wy=0.001382417; J_r=0.0000578; J_m=1*10^(-5); J_g=1*10^(-5); J_e=1*10^(-6); n=64; n_2=0.25; % 55mm and 13mm J_id=J_wy+(J_g+J_m)*n+n_2*n*(J_e+J_r); ts = 0.1; %sampling period %perform linear least-square [x,resnorm]=lsqlin([theta_wheeldd,theta_motord(1:length(theta_wheeldd))],… input(1:length(theta_wheeldd); Kb=x(2) Kt_by_R_m=1/(x(1)*n/J_id)

ทมา: กตตพงศ (2551)

95

2. การหาเอกลกษณของตวตรวจรวดความเอยง Inclinometer

%load raw data theta_gyr = load(''File Location\ File Name.txt'); theta_til = load(''File Location\ File Name.txt'); theta_enc = load(''File Location\ File Name.txt'); thetad_gyr = load(''File Location\ File Name.txt'); thetad_enc = load(''File Location\ File Name.txt'); theta2d_enc = load(''File Location\ File Name.txt'); ts = 5000/1000000; %in seconds t = 0:ts:(length(theta_gyr)-1)*ts; for i=1:length(t)-1 thetad_til(i) = (theta_til(i+1)-theta_til(i))/ts; end for i=1:length(t)-2 theta2d_til(i) = (thetad_til(i+1)-thetad_til(i))/ts; end %select ranges of data t1 = ts; t2 = 60; theta_gyr = theta_gyr(t1/ts:t2/ts); theta_til = theta_til(t1/ts:t2/ts); theta_enc = theta_enc(t1/ts:t2/ts); thetad_gyr = thetad_gyr(t1/ts:t2/ts); thetad_til = thetad_til(t1/ts:t2/ts)'; thetad_enc = thetad_enc(t1/ts:t2/ts); theta2d_enc = theta2d_enc(t1/ts:t2/ts); theta2d_til = theta2d_til(t1/ts:t2/ts)'; t = t(t1/ts:t2/ts); %linear least square g = 9.81;

96

[x,resnorm]=lsqlin([theta2d_enc-theta2d_til g.*sin(theta_enc-theta_til)]... ,thetad_til,[],[],[],[],[0 0],[+inf +inf]); ทมา: กตตพงศ (2551) 3. แบบจ าลองทางคณตศาสตรของหนยนตสองลอ % plant model.............................................................. g=9.81; m_w=0.5675; m_b=1.94033; R=0.147; W=0.3103; n=64; n_2=0.25; %from specification J_m=1*10^(-6); J_g=1*10^(-6); J_e=1*10^(-6); f_m=0.0022; f_w=0; %from SolidWorks L=0.04895; J_by=0.0173198; J_wy=0.0013824; J_bz=0.0117333; J_wz=0.0144355; J_r=0.0000578; %form system ID k_b=0.004275;

97

kt_by_Rm=0.0037673; alpha=n*kt_by_Rm; beta=alpha*k_b+f_m; E=[(2*m_w+m_b)*R^2+2*J_wy+2*J_o,m_b*L*R-2*J_o;... m_b*L*R-2*J_o,m_b*L^2+J_by+2*J_o]; F=[beta+f_w, -beta; -2*beta, 2*beta]; G=[0,0;0,-m_b*g*L]; H=[alpha/2, alpha/2; -alpha, -alpha]; I=beta+W/R*f_w; J=R/W*alpha; K=1/2*m_w*W^2+J_bz+2*J_wz+W^2/2/R^2*(J_wy+J_o); A1=[0,0,1,0;0,0,0,1;0,-m_b*g*L*E(1,2)/det(E),((beta+f_w)*E(2,2)+2*beta*E(1,2))/det(E),… beta*(E(2,2)+2*E(1,2))/det(E);0,m_b*g*L*E(1,1)/det(E),((beta+f_w)*E(1,2)+... 2*beta*E(1,1))/det(E),-beta*(E(1,2)+2*E(1,1))/det(E)]; B1=[0,0;0,0;alpha*(E(2,2)/2+E(1,2))/det(E),alpha*(E(2,2)/2+E(1,2))/det(E);... -alpha*(E(1,2)/2+E(1,1))/det(E),-alpha*(E(1,2)/2+E(1,1))/det(E)]; C1=[1,0,0,0;0,1,0,0]; D1=[0,0;0,0]; A2=[0,1;0,-I/K]; B2=[0,0;-J/K,J/K]; C2=[1,0]; D2=[0,0]; sys1 = ss(A1,B1,C1,D1,'statename',{'theta' 'psi' 'thetadot' 'psidot'},... 'inputname',{'vl' 'vr'},'outputname',{'theta' 'psi'}); sys2 = ss(A2,B2,C2,D2,'statename',{'phi' 'phidot'},'inputname',{'vl' 'vr'},'outputname',{'phi'});

98

sysss = [sys1; sys2] %state-space model [A,B,C,D] = ssdata(sysss); ทมา: กตตพงศ (2551) 4. สญญาณอางองส าหรบการออกแบบระบบควบคม %desired trajectories..................................................... ts = 0.005; %sampling period tend = 20; t = 0:ts:tend; zeta = 1; omegan = 5; filt = tf(omegan^2,[1 2*zeta*omegan omegan^2]); filtin = 1*square(0.5*pi*0.1*t); filtout = lsim(filt,filtin,t); ref_theta = 3*filtout; % Tracking case %ref_theta = zeros(size(filtout)); % Stabilization case ref_psi = zeros(size(filtout)); % All case ref_phi = 0.5*filtout; % Tracking case %ref_phi = zeros(size(filtout)); % Stabilization case ทมา: กตตพงศ (2551)

99

5. ระบบควบคมอาศยตวคมคาก าลงสองนอยทสด (LQR) ตอจาก แบบจ าลองทางคณตศาสตรของหนยนตสองลอ สญญาณอางองส าหรบการออกแบบระบบควบคม %control design........................................................... [K,S,e]=lqr(A,B,diag([0.1 10 0.1 10 0.1 0.1]),1*eye(2)); %gain K design x1(1)=0; x2(1)=0; x3(1)=0; x4(1)=0; x5(1)=0; x6(1)=0; %initial states for i=1:length(t) u(:,i) = -K*[x1(i);x2(i);x3(i);x4(i);x5(i);x6(i)]... %LQR controller + K(:,1)*ref_theta(i)... + K(:,2)*ref_psi(i)... + K(:,5)*ref_phi(i); xdot = A*[x1(i);x2(i);x3(i);x4(i);x5(i);x6(i)]+B*u(:,i); %CL system x1(i+1) = x1(i) + xdot(1)*ts; x2(i+1) = x2(i) + xdot(2)*ts; x3(i+1) = x3(i) + xdot(3)*ts; x4(i+1) = x4(i) + xdot(4)*ts; x5(i+1) = x5(i) + xdot(5)*ts; x6(i+1) = x6(i) + xdot(6)*ts; end ทมา: กตตพงศ (2551)

100

ประวตการศกษา และการท างาน ชอ –นามสกล นายบณฑต อนทรยมศกด วน เดอน ป ทเกด 16 พฤศจกายน พ.ศ. 2527 สถานทเกด สมทรสาคร ประวตการศกษา ปรญญาตร วศวกรรมศาสตรบณฑต

มหาวยาลยเอเชยอาคเนย พ.ศ. 2550 ต าแหนงหนาทการงานปจจบน - สถานทท างานปจจบน - ทนการศกษาทไดรบ ทนอดหนนและสงเสรมวทยานพนธระดบปรญญาโท

บณฑตวทยาลย มหาวทยาลยเกษตรศาสตร ประจ าปงบประมาณ 2551