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

ปรญญา

สาขา ภาควชา

เรอง การรกษาสมดลของหนยนตสองลอดวยตวคมคากาลงสองเชงเสน และตวควบคมโมเดลพรดกทฟ Self-Balancing of Two-Wheeled Mobile Robot with Linear Quadratic Regulator and Model Predictive Controller

นามผวจย นายพงศกร กวสนทรเสนาะ

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

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

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

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

( )

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

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

วนท เดอน พ.ศ.

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

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

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

ผชวยศาสตราจารยทวเดช ศรธนาพพฒน, Ph.D.

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

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

วทยานพนธ

เรอง

การรกษาสมดลของหนยนตสองลอดวยตวคมคาก าลงสองเชงเสน และตวควบคมโมเดลพรดกทฟ

Self-Balancing of Two-Wheeled Mobile Robot with Linear Quadratic Regulator and Model Predictive Controller

โดย

นายพงศกร กวสนทรเสนาะ

เสนอ

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

พ.ศ. 2557

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

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

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

Pongskorn Kaveesoontornsanoh 2014: Self-Balancing of Two-Wheeled Mobile Robot with Linear

Quadratic Regulator and Model Predictive Controller. Master of Engineering (Mechanical

Engineering), Major Field: Mechanical Engineering, Department of Mechanical Engineering.

Thesis Advisor: Associate Professor Withit Chatlatanagulchai, Ph.D. 80 pages.

Many researchers have paid attention on two-wheeled robot because it is a system that is difficult to

control due to its non-minimum phase property, its having multiple inputs and outputs, and the number of

inputs is fewer than the number of output. The two-wheeled robot is suitable for research that needs to prove the

efficiency of the applying control system. Besides, the two-wheeled robot is able to move swiftly similar to

human. There exist many research works showing the performance in robot movement. From this agility of the

robot movement, the two-wheeled robot can be applied to real work in place of humans.

This research applies the linear quadratic regulator and the model predictive control techniques to

controlling the robot. Each technique has different characteristics. The linear quadratic regulator depends on

accurate mathematical model to handle the system uncertainty and to balance the robot. Tilt angle measurement

of the robot is a major problem in balancing the two-wheeled robot. In this research, an accelerometer and a

gyroscope are used together.

Simulation results show the stability in balancing the two-wheeled robot of the linear quadratic

regulator and the model predictive controller. The two types of controller are different. The results show

specific characteristic of each controller. The model predictive controller incorporates constraints on inputs and

outputs. It can be applied to industrial work that consumes high energy. Experimental results show the

efficiency of the complementary filter that can handle noise and sensor drifts for both sensors.

___________________________ _________________________________ __ / __ / __ Student’s signature Thesis Advisor’s signature

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

ขาพเจาขอกราบขอบพระคณ รศ.ดร. วทต ฉตรรตนกลชย ประธานกรรมการทปรกษา

วทยานพนธ ทใหโอกาสขาพเจาเขามาสสาขาวชาการควบคมไดมอบความรพนฐานการควบคม

จนกระทงระดบสง ไดใหค าปรกษาทางดานทฤษฎและการปฏบตตลอดจนประสบการณตางๆ และ

ขอกราบขอบพระคณ ผศ.ดร. ทวเดช ศรธนาพพฒน กรรมการทปรกษาวชาเอก ทใหความรและ

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

ขอขอบพระคณมหาวทยาลยเกษตรศาสตร, หองปฏบตการควบคมหนยนตและการ

สนสะเทอน (Control of Robot and Vibration Laboratory, CRVLAB) ทเออเฟอสถานทและ

อปกรณการท าวจย อาจารยคณะวศวกรรมศาสตรทกทานทไดใหค าแนะน าและวชาความรตางๆ

ขอบพระคณ รศ.ดร. อนชา หรญวฒน ทใหค าแนะน าในการเรยนระดบปรญญาโท ขอขอบคณรนพ

และเพอนๆ ใน CRVLAB ทใหค าปรกษาและค าแนะน าเมอเกดปญหาในการท างานวจยน

นอกจากนขอขอบคณความรตางๆจากนกวจยทกทานทเกยวของทใหความรในงานประชมวชาการ,

บทความวชาการและวารสารวชาการ

สดทายน ขาพเจาขอกลาวค าขอบพระคณแดบดาและมารดาทเลยงดขาพเจา คอยอบรมสง

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

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

พงศกร กวสนทรเสนาะ

กรกฎาคม 2557

(1)

สารบญ

หนา

สารบญ (1) สารบญตาราง (2) สารบญภาพ (3) ค าอธบายสญลกษณและอกษรยอ (6) ค าน า 1 วตถประสงค 3 การตรวจเอกสาร 4 อปกรณและวธการ 13 อปกรณ 13 วธการ 19 ผลและวจารณ 46 สรปและขอเสนอแนะ 58 สรป 58 ขอเสนอแนะ 59 เอกสารและสงอางอง 60 ภาคผนวก 64

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

ภาคผนวก ค การจ าลองผลการทดลองดวยตวคมคาก าลงสองเชงเสน 71 ภาคผนวก ง การหาผลการจ าลองดวยตวควบคมโมเดลพรดกทฟ 73 ภาคผนวก จโคดโปรแกรม MATLAB ของผลการทดลอง 76

ประวตการศกษา และการท างาน 80

(2)

สารบญตาราง

ตารางท หนา

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

(3)

สารบญภาพ

ภาพท หนา

1 (ซาย) หนยนตส ารวจดาวองคาร (Curiosity) (กลาง) หนยนตเสรฟอาหาร (Hajime Robot) (ขวา) หนยนตเดนสองขาเสมอนมนษย (Humanoid robots) 4 2 (ซาย) หนยนตลอเดยว (Unicycle Robot) (กลาง) Lego NXTbike-GS (Self-Balanceing Two Wheeled Robot) 5 3 (ซาย) หนยนตสองลอทสามารถทรงตวดวยตวเอง (Quasimoro) (กลาง) หนยนต สองลอทสามารถเปลยนจดศนยกลางมวล (ขวา) หนยนตสองลอทมแขนสองขาง 7 4 การเคลอนทลงบนไดของหนยนตสองลอ 9 5 หลกการ Complementary filter 11 6 หนยนตสองลอและสวนประกอบตางๆ 13 7 อปกรณเอนโคดเดอรเซนเซอร 14 8 อปกรณเซนเซอรวดความเรง 14 9 อปกรณเซนเซอรวดความเรวเชงมม (Rate Gyroscope) 15 10 อปกรณวงจรขบมอเตอรกระแสตรง 15 11 มอเตอรไฟฟากระแสตรง (DC Motor) 16 12 อปกรณไมโครคอนโทรลเลอร (Arduino Mega 2560 R3) 16 13 อปกรณแบตเตอร 12 V 17 14 ตวอยางการใชงานโปรแกรม MATLAB 17 15 ตวอยางการใชการโปรแกรม Solidworks 18 16 โครงสรางของหนยนตสองลอจากโปรแกรม Solidworks 19 17 หนยนตสองลอบนแกนพกด x,y,z 20 18 การวดมมของเซนเซอรวดความเรง 29 19 ตวกรองสญญาณแบบดจตอล (Digital Filter) 30 20 บลอกไดอะแกรมการวดคาจากเซนเซอรทงสามชนด 31 21 บลอกไดอแกรมในการควบคมปอนกลบแบบรกษาเสถยรภาพ 32 22 พนฐานของตวควบคมโมเดลพรดกทฟ 36

23 เอาตพตของตวคมก าลงสองเชงเสน 46

(4)

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

ภาพท หนา

24 สญญาณควบคมของตวคมคาก าลงสองเชงเสน 47 25 ตวควบคมโมเดลพรดกทฟทไมมขอจ ากด (MPC without Constraints) 48 26 สญญาณควบคมของโมเดลพรดกทฟทไมมขอจ ากด (MPC without constraints) 48 27 ตวควบคมโมเดลพรดกทฟทมขอจ ากด (MPC with Constraints ( 0.01 Pitch)) 49 28 สญญาณควบคมของตวควบคมโมเดลพรดกทฟทมขอจ ากด (MPC with constraints

( 0.01 Pitch)) 49 29 ตวควบคมโมเดลพรดกทฟ (MPC ;

pH = 24) 50 30 สญญาณควบคมของตวควบคมโมเดลพรดกทฟ (MPC ;

pH = 24) 50 31 เปรยบเทยบผลการจ าลองระหวางตวคมคาก าลงสองเชงเสนและตวควบคมโมเดลพรดก ทฟทไมมขอจ ากด 51 32 เปรยบเทยบผลการจ าลองระหวางตวคมคาก าลงสองเชงเสนและตวควบคมโมเดลพรดก ทฟทไมมขอจ ากด ( 0.01 Pitch) 52 33 ภาพรวมของการวดมมเอยงดวยหลกการ Complementary Filter 53 34 เปรยบเทยบผลระหวางเซนเซอรวดความเรงและ Complementary Filter 53 35 เปรยบเทยบผลของมมจากเซนเซอรทงสองและ Complementary Filter ทความถสง 54 36 เปรยบเทยบผลระหวางเซนเซอรวดความเรวเชงมมและ Complementary Filter 54 37 คาเอาตพตของมม Pitch จากการรกษาสมดลของหนยนตสองลอ 56 38 สญญาณควบคม PWM จากการรกษาสมดลของหนยนตสองลอ 56

(5)

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

ภาพผนวกท หนา

ก1 การหาคาพารามเตอรดวยโปรแกรม Solidworks 66 ค1 การจ าลองผลการทดลองดวยตวคมคาก าลงสองเชงเสน 72 ง1 การหาผลการจ าลองดวยตวควบคมโมเดลพรดกทฟ 74 ง2 การก าหนดคาพารามเตอรของตวควบคมโมเดลพรดกทฟ 75 จ1 การรบคาจากเซนเซอรวดความเรงและเซนเซอรวดความเรวเชงมม 77 จ2 การรบคาจากเอนโคดเดอรเซนเซอรดวย S-function และอตราขยาย 77 จ3 การควบคมมอเตอรซายและมอเตอรขวาดวย PWM 78 จ4 การวดมมเอยงดวยหลกการ complementary filter (Subsystem) 78 จ5 การวดต าแหนงและมมเลยวของเอนโคดเดอรเซนเซอร

(Subsystem1 และ Subsystem2) 79 จ6 การใช S-function ในการวดคาจากเอนโคดเดอรเซนเซอร 79

(6)

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

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

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

l = มมของลอซาย ( rad )

r = มมของลอขวา ( rad ) = มมเอยงของหนยนตสองลอ ( rad ) = มมเลยวของหนยนตสองลอ ( rad ) = เวลาคงทของ Complementary Filter (time constant) = คาความผดพลาดของตวควบคมโมเดลพรดกทฟ = สมประสทธอตราขยายของโมเดลพรดกทฟ = สญญาณรบกวนสขาว a = คาสมประสทธของตวกรองความถ D = ระยะความลกของตวถง ( m ) F = สมประสทธอตราขยายของโมเดลพรดกทฟ F = แรงภายนอกทกระท าในระบบพกด ( .N m ) F = แรงภายนอกทกระท าในระบบพกด ( .N m ) F = แรงภายนอกทกระท าในระบบพกด ( .N m )

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

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

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

wf = คาสมประสทธแรงเสยดทานระหวางลอและพน g = แรงโนมถวงโลก ( 2/ secm ) H = ระยะความสงของตวถง ( m )

pH = ขอบเขตการท านายสเตทในอนาคต

cH = ขอบเขตการท านายอนพตในอนาคต i = อนดบของระบบ

,l ri = กระแสของมอเตอรซายหรอมอเตอรขวา J = ดรรชนสมรรถนะ (Cost function)

byJ = โมเมนตความเฉอยตวถงแกน y0 ( 2kgm )

(7)

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

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

bzJ = โมเมนตความเฉอยตวถงแกน z0 ( 2kgm )

mJ = โมเมนตความเฉอยของมอเตอร ( /Nm A )

wJ = โมเมนตความเฉอยของลอ ( 2kgm ) K = อตราขยายของตวควบคม

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

tK = คาคงทแรงบดมอเตอร k = เวลาปจจบน L = ระยะจากแกนลอถงศนยถวง ( m )

gL = คาลากรางจ

mL = คาของตวเหนยวน า M = มวลตวถง ( kg ) m = มวลลอ ( kg ) n = อดตราทดมอเตอร Q = เมตรกซถวงน าหนกของสเตท R = รศมลอ ( m ), เมตรกซถวงน าหนกของอนพต

mR = คาความตานทานของมอเตอร r = สญญาณอางองทตองการตามรอยสญญาณ s = จดทตองการตามรอยสญญาณ T = พลงงานจลนรวมของระบบ ( 2 2/kgm s )

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

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

refT = ความเรวในการตอบสนอง

sT = ระยะหางของชวงเวลา

iq = ระบบพกด U = เวกเตอรของผลตางอนพต

u = อนพต (V ) V = พลงงานศกยรวมของระบบ ( 2 2/kgm s )

(8)

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

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

refV = แรงดนทใชปฎบตการท างานของไมโครคอนโทรลเลอร

,l rv = แรงดนของมอเตอรซายหรอมอเตอรขวา W = ความกวางของฐานลอ ( m ) w = สญญาณอนพตของสงรบกวนภายนอกระบบ x = คาสเตทของระบบ y = เอาตพตของระบบ

ค าอธบายอกษรยอ

LQR = Linear Quadratic Regulator MIMO = Multi-input Multi-output MPC = Model Predictive Control PWM = Pulse Width Modulation NMP = Non-minimum Phase

1

การรกษาสมดลของหนยนตสองลอดวยตวคมคาก าลงสองเชงเสน และตวควบคม

โมเดลพรดกทฟ

Self-Balancing of Two-Wheeled Mobile Robot with Linear Quadratic Regulator and Model Predictive Controller

ค าน า

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

จะพบงานวจยทเกยวของเปนจ านวนเพมมากขนในแตละป โดยเปนการควบคมการรกษาสมดลของ

หนยนตซงมความยากเนองจากเปนระบบทมความซบซอน เปนระบบทมหลายสญญาณปอนเขา

และหลายสญญาณปอนออก (Multi-input Multi output, MIMO) โดยมสญญาณปอนเขานอยกวา

สญญาณปอนออก มจ านวนแอคชเอเตอร (actuators) นอยกวาจ านวนองศาอสระ (degree of

freedom) จงท าใหเปนระบบอนเดอรแอคชเอด (under-actuated) อกทงยงเปนระบบไมใชมมเฟส

นอยสด (non-minimum phase system, NMP) และระบบไมเปนเชงเสน (nonlinear system)

จากความซบซอนในการควบคมขางตนจงมงานวจยมากมายทตองการน าเสนอเทคนคการ

ควบคมหลากหลายวธเขาไปจดการกบระบบดงกลาว ซงมสามตวแปรหลกทมกจะน ามา

เปรยบเทยบกน อยางแรกคอมมเอยง (tilt angle) ของหนยนตสองลอหรออาจเรยกไดวาการสนไหว

ของหนยนตสองลอ (oscillations) สองคอการควบคมความเรวของหนยนตสองลอ (velocity) สาม

คอต าแหนงทหนยนตสองลอเคลอนทไป (translational)

หนยนตสองลอเปนหนยนตทมความคลองแคลวเปนหนยนตทสามารถน าไปประยกตใช

ไดกบงานหลายรปแบบทเหนเดนชดคอ เสกเวย (Segway) ซงอาศยหลกการการเปลยนแปลงของ

จดศนยกลางมวล (center of mass) นนคอเมอผใชตองการไปขางหนาหรอขางหลงกจะท าการโนม

ตวไปในทศทางทตองการ ซงการโนมตวจะท าใหจดศนยกลางมวลเปลยนหนยนตเสกเวยกจะวงไป

ในทศทางทโนมตวเพอท าการรกษาสมดล

2

งานวจยนตองการจะรกษาสมดลของหนยนตสองลอซงการควบคมมลกษณะเดยวกบ

ลกตมนาฬกากลบหวโดยเคลอนทดวยลอทขนานกนสองลอทเปนอสระตอกน ใชเซนเซอรสาม

ประเภทคอ Accelerometer Gyroscope และ Encoder ตองการน าตวควบคมทเรยกวา ตวคมคาก าลง

สองนอยทสด (linear quadratic regulator, LQR) และการควบคมโมเดลพรดกทฟ (model

Predictive Control, MPC) ซงทงสองการควบคมมลกษณะคลายคลงกน

3

วตถประสงค

1. ออกแบบการวดมมเอยงดวยหลกการ Complementary filter โดยใชเซนเซอรประเภท

Acclerometer และ Gyroscope เพอใชเปนสญญาณปอนกลบส าหรบหนยนตสองลอ

2. ออกแบบระบบควบคมของหนยนตสองลอทสามารถรกษาสมดลและรกษาเสถยรภาพ

ของระบบได โดยใชการออกแบบดวยตวควบคมก าลงสองเชงเสนและตวควบคมโมเดลพรดกทฟ

ขอบเขตงานวจย

1. ศกษาการวดมมเอยงทรอบจดศนยของหนยนตสองลอและการจดการกบ noise และ drift

2. ปจจยหลกในการวจยคอ การตดตามสญญาณอางองทก าหนดดวยการใชตวคมคาก าลง

สองเชงเสน และตวควบคมโมเดลพรดกทฟ

4

การตรวจเอกสาร

1. หนยนตสองลอ

ในปจจบนนมจ านวนหนยนตเพมมากขนอยท วไปทกหนทกแหงและน าไปใชในงานท

หลากหลาย อาทเชน หนยนตส ารวจบนตางดาว, หนยนตใหความชวยเหลอ, หนยนตเสรฟอาหาร

ดงภาพท 1 ในขณะทหนยนตแบบใชขาเดน (Legged Robots) สามารถเคลอนทขามผานสงกดขวาง

ตางๆไดดกวาหนยนตทใชลอในการขบเคลอน (Wheeled Robots) โครงสรางทางกลของหนยนต

แบบใชขาเดนจะมความซบซอนในการออกแบบหนยนตและมจ านวนขององศาอสระ (degrees of

freedom) ทมากกวาหนยนตทใชลอในการขบเคลอน แตหนยนตทใชลอในการขบเคลอนจะม

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

ในการขบเคลอนนนควบคมไดงายกวาหนยนตแบบใชขาเดน

ภาพท 1 (ซาย) หนยนตส ารวจดาวองคาร (Curiosity) (กลาง) หนยนตเสรฟอาหาร (Hajime Robot)

(ขวา) หนยนตเดนสองขาเสมอนมนษย (Humanoid robots)

ทมา: วกพเดย. (2556)

ซงงานวจยนสนใจเกยวกบหนยนตทใชลอในการขบเคลอนจ านวนสองลอ ส าหรบหนยนต

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

(suspension system) พนททใหหนยนตเคลอนท ซงในหนยนตสองลอสามารถเคลอนทไดอยาง

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

5

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

สองลอยงมลกษณะการทรงตวทใกลเคยงมนษยมาก

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

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

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

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

แบงออกเปน 4 ประเภท โดย Siciliano and Khatib (2008) ตามจ านวนลอของหนยนต ประเภทแรก

หนยนตลอเดยวเปนหนยนตควบคมไดยากมความไมแนนอนสง (unstable) ยากตอการรกษาสมดล

ของหนยนต ซงตองท าการรกษาสมดลสองแกนคอแกนพท (pitch) และแกนโรล (roll) ในแกนพท

มการควบคมคลายคลงกบลกตมนาฬกากลบหวเหมอนกบหนยนตสองลอ สวนแกนโรล Lee et al.

(2013) ไดใชแผนจานกลม (Disk) มารกษาสมดล ดงภาพท 2 (ซาย) การรกษาสมดลทงสองแกนม

สวนท าใหหนยนตลอเดยวนนยงน าไปประยกตใชกบงานจรงไดยาก

ภาพท 2 (ซาย) หนยนตลอเดยว (Unicycle Robot) (กลาง) Lego NXTbike-GS (ขวา) NXTway-GS

(Self-Balanceing Two Wheeled Robot)

ทมา: Lee et al. (2013); Yomamoto et al. (2008)

ประเภททสองคอหนยนตสองลอโดยทวไปจะแบงออกเปนสองลกษณะ ลกษณะแรกจะม

ลกษณะเหมอนกบรถจกรยาน (bicycle-type robot) ดงภาพท 2 (กลาง) หนยนตสองลอลกษณะน

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

6

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

ลอลกษณะทสองซงเราสนใจในงานวจยนดงภาพท 2 (ขวา) จะสามารถรกษาสมดลของหนยนตท

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

หลายกรณ

ประเภททสามหนยนตสามลอเปนหนยนตทไมตองท าการรกษาสมดล มการออกแบบ

หนยนตประเภทนหลายลกษณะยกตวอยางเชน หนยนตสามลอทขบเคลอนสองลอและลอทสาม

เปนลกลอเลอน (caster wheel) หนยนตสามลอลกษณะนการเคลอนทคลายคลงกบหนยนตสองลอ

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

ลกษณะของหนยนตสามลอจะมขอดและขอเสยตามการออกแบบของหนยนตสามลอ ประเภททส

คอหนยนตสลอไดมการออกแบบหนยนตประเภทนหลายลกษณะเชนกนลกษณะทนยมกนคอ

โครงสรางเหมอนรถยนตซงมเสภยรภาพในขณะเคลอนทดวยความเรวสง แตการหมนของลอหนา

ทงสองไมสามารถหมนถง 90 องศาท าใหการเลยวหนยนตตองการพนทมาก

การออกแบบโครงสรางของหนยนตสองลอมความส าคญตอการรกษาสมดลซงม

ความสมพนธกบต าแหนงของจดศนยกลางของมวล (Center of mass) Ferdinando et al. (2011) ได

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

สมดลไดยาก ถาใหจดศนยกลางมวลอยต ากวาจดหมนกจะท าใหเปนหนยนตทรกษาสมดลไดดวย

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

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

ต ากวาจดหมน ซงการผลการทดลองทท าการควบคมดวย PID controller แสดงใหเหนวาการใส

โหลดน าหนกทต ากวาจดหมนชวยเพมประสทธภาพในการรกษาสมดล

Brkic and Kovacic (2009) ไดกลาวถงหลกการรกษาสมดลของหนยนตสองลอวาในหลก

เบองตนนนขนอยกบตวแหนงของจดศนยกลางของมวล (Center of mass) ซงการยายจดศนยกลาง

มวลใหอยสงกวาจดทหนยนตสองลอรกษาสมดลจะท าใหเกดปญหาในการรกษาสมดลของหนยนต

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

หนยนตสองลอทสามารถปรบต าแหนงของจดศนยกลางมวลดงภาพท 3 (กลาง) เพอท าใหงายตอ

7

การหาขอบเขตของการรกษาสมดลของหนยนตสองลอ ซงสามารถเคลอนยายจดศนยกลางมวลให

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

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

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

แบบสดสวนบวกอนทกรล (PI control) ในสวนของมมเอยงและความเรว และใชขนตอนการ

แสวงหาเสนทาง (pure pursuit algorithm) ส าหรบต าแหนงของหนยนตสองลอ

ภาพท 3 (ซาย) หนยนตสองลอทสามารถทรงตวดวยตวเอง (Quasimoro) (กลาง) หนยนตสองลอท

สามารถเปลยนจดศนยกลางมวล (ขวา) หนยนตสองลอทมแขนสองขาง

ทมา: Oryschuk et al. (2009); Brkic and Kovacic (2009); Feng et al. (2011)

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

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

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

ภายในพนทปฏบตงานนนได Deng et al. (2010) ไดอธบายเกยวกบการหลบสงกดขวางของ

หนยนตสองลอแบงออกเปนสองหวขอหลกคอ การวางแผนในการเคลอนท (motion planning) และ

ตวควบคมปอนกลบ (feedback control) ไดมนกวจยหลายทานท าการศกษาวจยเกยวกบการวางแผน

ในการเคลอนทโดย Ge and Cui (2000) ไดใชหลกในการก าหนดพนททหนยนตจะเคลอนทไปให

ถงเปาหมายโดยไมชนกบสงกดขวาง ขอดของหลกการนคอมสมการคณตศาตร (mathematical) ท

งายและไมซบซอน ในสวนของตวควบคมปอนกลบกมนกวจยหลายทานไดท าการศกษาและวจยไว

8

โดย Urakubo et al. (2004) อธบายถงหลกการวาเปนการสรางฟงชนกจ าลองขนมาแทนสงกดขวาง

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

กดขวาง Deng et al. (2010) ไดน าทงสองหวขอมาผสมผสานกนจงท าใหมประสทธภาพในหลบสง

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

ในกรณทหนยนตสองลอนนแบตเตอรก าลงจะหมดหรอมแรงกระท าภายนอกทรนแรงกจะ

ท าใหหนยนตสองลอไมสามารถรกษาสมดลและลมลง Feng et al. (2011) ไดออกแบบหนยนตสอง

ลอทมแขนสองขางดงภาพท 3 (ขวา) ซงแขนทงสองสามารถชวยในเรองการรกษาสมดลในกรณทม

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

ตงอยในงานแสดงหนยนต ไดใชหลกการ Kalman filter ในการวดมมเอยงของหนยนตซงมความ

แมนย าและสามารถจดการกบปญหา noise หรอ drift ของเซนเซอรวดความเรงและเซนเซอรวด

ความเรวเชงมมไดด และท าการควบคมต าแหนงและการรกษาสมดลดวยวธการวางต าแหนงโพล

(pole placement)

2. ตวคมคาก าลงสองเชงเสน

Oryschuk et al. (2009) ไดออกแบบหนยนตสองลอทสามารถรกษาสมดลของหนยนตได

ดวยตวหนยนตเอง (passive stabilization) ดงภาพท 3 (ซาย) โดยมจดศนยกลางมวลต ากวาจดหมน

ของหนยนตสองลอ ในการทดลองแสดงประสทธภาพในการรกษาเสถยรภาพของหนยนตในขณะ

ใสน าหนก (payload) ใหกบหนยนตสองลอตงแต 0 นวตนถง 60 นวตน ออกแบบตวควบคมโดยใช

ตวคมคาก าลงสองเชงเสน (LQR) ท าการจนเมตรกซถวงน าหนก (Q,R) ดวยวธลองผดลองถก (trial

and error) ท าใหหนยนตสองลอสามารถรกษาเสถยรภาพขณะขนเนนเปนมม 15 องศา และสามารถ

ควบคมความเรวของหนยนตสองลอ

สภาพแวดลอมรอบตวเราในปจจบนบนพนททเราอาศยหรอเคลอนทไปนนไมใชพนดนท

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

หนยนตสองลอไปใชจงตองมการปรบปรงในสวนนดวยเชนกน Yap and Hashimoto (2012) ไดท า

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

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

9

ใชตวควบคมดวยตวคมคาก าลงสองเชงเสน (LQR) ซงแบงออกเปนสสวนคอ กอนลอย ขณะลอย

ตกกระทบ และสมดลของหนยนต ไดท าการควบคมอนพต (input) ทกอใหเกดการตอบสนองของ

ทอรก (torque) ในระหวางทหนยนตสองลอก าลงลอยซงเปนการเตรยมพรอมกบการตกกระทบพน

ภาพท 4 การเคลอนทลงบนไดของหนยนตสองลอ

ทมา: Yap and Hashimoto (2012)

3. ตวควบคมโมเดลพรดกทฟ

Ekaputri et al. (2012) ไดกลาวถงขอดของตวควบคมโมเดลพรดกทฟคอสามารถท างานได

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

ซบซอนและเพมภาระในการค านวณทมากใหแกไมโครคอนโทรลเลอร ไดใชโปรแกรม MATLAB

ในการค านวณหาคาของ horizon ทเหมาะสมทสดไดเทากบ 9 horizons ซงจากผลการจ าลองแสดง

ใหเหนวาใชเวลาในการตอบสนองไดเรวทสด และไดใหความส าคญกบคาของ sampling time

เนองจากในการทดลองจรงนนจะมคาอตราหนวงทมาจากตวขบเคลอน (actuator),เซนเซอร หรอ

จากการแปลงแบบจ าลองทางคณตศาสตรแบบไมเชงเสน (nonlinear) ใหเปนแบบจ าลองทาง

คณตศาสตรแบบเชงเสน (linear) ดงนนการเลอกคาของ horizon ทถกตองจงมความส าคญตอการ

ออกแบบตวควบคมโมเดลพรดกทฟ

ในตวควบคมระดบสงเชน adaptive control หรอ on-line-based optimal control นนมกจะม

การค านวณสงและซบซอนท าใหไมโครคอนโทรลเลอรประมวลผลไดชาลง ซงปจจบน

ไมโครคอนโทรลเลอรไดมการพฒนาในเรองของความเรวในการประมวลผลอยางตอเนองเชนกน

อยางไรกตามการเลอกปรบคาพารามเตอรตางๆในตวควบคมโมเดลพรดกทฟนนจงมความส าคญ

ตอความเรวในการประมวลผลของไมโครคอนโทรลเลอร Lin et al. (2012) ไดแสดงถงการปรบ

10

คาพารามเตอรของตวควบคมโมดลพรดกทฟ โดยการปรบคาของ Prediction horizon (pH ) ให

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

Control horizon ( cH ) ใหสงขนกจะชวยเพมเสถยรภาพของเอาตพตเชนกนแตความเรวในชวง

transience จะชาลง ซงการปรบคาพารามเตอรทงสองตวนมผลโดยตรงตอความเรวในการ

ประมวลผลของไมโครคอนโทรลเลอร

งานวจยทเกยวกบความเรวในการตดตามสญญาณอางองนนมคอนขางจะนอย ซงในตว

ควบคมโมเดลพรดกทฟมความรวดเรวในการตดตามของเอาตพตในชวง transience คอนขางดอย

แลว Zeng et al. (2013) ไดน าทฤษฎของ Laguerre polynomials มาใชเพอจดการกบความเรวของ

การตดตามสญญาณอางอง และชวยลดภาระการค านวณใหแกไมโครคอนโทรลเลอร ไดก าหนด

ขอจ ากด (constraints) ใหแกความเรวและความเรงแกหนยนตเพอปองกนไมใหหนยนตลม ในผล

การทดลองจะแสดงใหเหนถงการใชคา N = 1 แทนใน cN (Control horizon) = 9 ซง

ประสทธภาพของเอาตพตทไดไมแตกตางกนมากนกแตในสวนของ N = 1 จะลดภาระการ

ค านวณไดถง 7.48 ms

ในงานอตสาหกรรมทวไปนนใชตวควบคมแบบสดสวนบวกอนทกรล (PI control) กบ

มอเตอรไฟฟากระแสสลบชนดสามเฟส (permanent magnet synchronous motor) ซงไมมการ

ควบคมในสวนขอจ ากด (constraints) ของกระแสทปอนใหกบมอเตอร Chai et al. (2013) ไดใชการ

ควบคมโมเดลพรดกทฟ (MPC) น ามาใชกบมอเตอรไฟฟากระแสสลบชนดสามเฟส สามารถ

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

ในโรงงานอตสาหกรรมตางๆได และสามารถลดสญญาณรบกวน (noise) ทเกดขนโดยการจนคา

gain matrix ของกระแสแลวความเรวของมอเตอร ซงงานวจยนไดใชการควบคมโมเดลพรดกทฟ

กบหนยนตสองลอในสวนของ simulation

11

4. การวดมมเอยงของหนยนต

ภาพท 5 หลกการ Complementary filter

ทมา: Wasif et al. (2013)

Cheng and Oelmann et al. (2010) อธบายถงหลกการเลอกใชเซนเซอร (sensor) สงทควร

ค านงถงคอ ความซบซอนในการใชงาน ความแมนย าของเซนเซอร ราคา ความนาเชอถอ และอาย

การใชงานของเซนเซอร ไดใช accelerometer และ gyroscope sensor ในการหามมทหมนไปของ

แขนกลของหนยนต โดยแบงออกเปน 4 วธ วธทหนงใช accelerometer จ านวน 2n-1 ตอจ านวน n-

joint-angle วธทสองและวธทสามใช accelerometer จ านวน n ตว และ gyroscope sensor จ านวน n

ตว ซงทงสองวธแตกตางกนทการรบคาจาก gyroscope sensor โดยวธทสองจะ integrate คาทวดจาก

เซนเซอร สวนวธทสามจะ differentiation คาทวดจากเซนเซอร วธทสใช accelerometers จ านวน 2n

ตว จากผลการทดลองแสดงใหเหนวาวธทสามและวธสมความแมนย าในการวดมมเอยงของแขนกล

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

ความแมนย าในการวดมมเอยงไดมากยงขน และในแตละวธมขอดขอเสยแตกตางกนไปตามการใช

งาน ซงในงานวจยนใชเซนเซอร accelerometer 1 ตว และ gyroscope sensor 1 ตว โดยตดตง

เซนเซอรใหอยใกลกบจดหมน (pivot point) ของหนยนตสองลอมากทสด

การวดมมเอยงของหนยนตสองลอถอเปนปจจยหลกทส าคญในการรกษาสมดลหนยนต

สองลอของงานวจยน Wasif et al. (2013) ใชเซนเซอรรวมกนระหวาง accelerometer และ

12

gyroscope sensor ในการวดมมเอยงและใชหลกการทเรยกวา complementary filter ภาพท 5 ซง

สามารถใชงานและเขาใจไดงายกวาหลกการของ Kalman filter ทมความซบซอนสง อกทงยงชวย

ลดภาระการค านวณอยางมากใหกบไมโครคอนโทรลเลอร เซนเซอรทงสองชนดสามารถวดมม

เอยงไดแตจะไมมความแมนย าเมอไมไดใชรวมกน ไดใหความส าคญของคา filter constant ในการ

ออกแบบดวยหลกการ complementary filter ซงไดท าการทดลองใหคาของ filter constant ท 0.75

ถง 0.90 จะเหนถงความแตกตางของความแมนย าและอตราหนวงในการวดมม ดงนนการเลอกคา

ของ filter constant ใหเหมาะสมกบคา sampling time ของไมโครคอนโทรลเลอรกจะชวยเพม

ประสทธภาพในการวดมมเอยงมากยงขน

13

อปกรณและวธการ

อปกรณ

1. หนยนตสองลอ เปนหนยนตขนาด 15.5 x 28 x 28.9 เซนตเมตร น าหนกรวม 5.08

กโลกรม วสดโครงท าจากอลมเนยม สวนประกอบทส าคญภายในหนยนตสองลอประกอบดวย

บอรดไมโครคอนโทรลเลอร (Arduino ATMega 2560) , วงจรขบมอเตอรกระแสตรง (Motor

Driver) , เซนเซอรสองชนด, มอเตอรกระแสตรง 2 ชด โดยตอตรงกบลอทงสองทท างานอยางอสระ

ตอกน และแบตเตอร 12V

ภาพท 6 หนยนตสองลอและสวนประกอบตางๆ

2. เซนเซอร ประกอบดวย

2.1 เอนโคดเดอรเซนเซอร (Encoder) อปกรณวดต าแหนงทท างานโดยอาศยการยงล าแสงผานแถบมด-สวางและสงออกเปนสญญาณดจตอล การอานคาอตราการเปลยนแปลงของระดบสญญาณถกน ามาค านวนเปนอตราเรวของการเปลยนแปลงต าแหนง งานวจยนใชเอนโคดเดอรประเภท Rotary Encoder HKT3006-301C-500B มความละเอยด 500 ครงตอรอบ (CPR) โดยม 2 ชด ตดอยกบมอเตอรและใชส าหรบการวดต าแหนงทหมนไปของลอทงสองขาง

14

ภาพท 7 อปกรณเอนโคดเดอรเซนเซอร

2.2 เซนเซอรวดความเรง (Accelerometer) คอเซนเซอรวดความเรงประเภท

MMA7341LC 3-Axis 3g (440 mV/g) ในงานวจยนใชส าหรบวดมมเอยงของหนยนตสองลอ

ในแกน Pitch ซงใชหลกแรงโนมถวงของโลก โดยท าการแปลงสญญาณ analog ทอานไดจาก

accelerometer ใหอยในรปของแรง “g” (หรอความเรง) น าหนก 0.7 กรม รายละเอยดเพมเตมท

www.pololu.com

ภาพท 8 อปกรณเซนเซอรวดความเรง

2.3 เซนเซอรวดความเรวเชงมม เรยกวา Rate Gyroscope เปนอปกรณวดความเรวเชงมม

ทมการตอบสนองทดในชวงความถสง โดยใชประเภท LPR550AL Dual-Axis 500 /s ในแกน

Pitch และ Roll หรอ XY น าหนก 0.7 กรม รายละเอยดเพมเตมท www.pololu.com

15

ภาพท 9 อปกรณเซนเซอรวดความเรวเชงมม Rate Gyroscope

3. วงจรขบมอเตอรกระแสตรง (Motor Drive Module) ท าหนาทขยายกระแสไฟฟา โดย

อปกรณนจะรบสญญาณปอนเขาคอกระแสไฟฟาทมปรมาณและแรงดนต า และจายกระแสไฟฟาท

มปรมาณและแรงดนสงกวา ซงงานวจยนใชอปกรณรน L298N Dual Full-bridge motor drive

module สามารถควบคมมอเตอรได 2 ตวแบบอสระ รายละเอยดเพมเตม www.micontechlab.com

ภาพท 10 อปกรณวงจรขบมอเตอรกระแสตรง

4.มอเตอรไฟฟากระแสตรง (DC Motor) เปนมอเตอรชนด ZYTD-38SRZ-R รบแรงดนไดระหวาง 12-24 V rpm : 250 เปน DC Motor ทใสเฟองทดรอบเขาไปเพอใหความเรวรอบลดลงตามการน าไปใชงาน

16

ภาพท 11 อปกรณมอเตอรไฟฟากระแสตรง

5.ไมโครคอนโทรลเลอรประเภทอาดยอโน (Arduino Mega 2560 R3) คอตวควบคมของระบบ รบสงขอมลทความเรว 16 MHZ ขนาดหนวยความจ า 256 KB เปนไมโครคอนโทรลเลอรทมราคาถกเนองจากระบบเปนแบบ Open Source ไมมลขสทธในการน าไปใชงานเชงพานชย และซอฟแวร Arduino IDE ทใชในการพฒนาสามารถดาวนโหลดไดฟร ซงงานวจยนใชไมโครคอนโทรลเลอรรวมกบโปรแกรม Matlab 2013a ผานทาง Arduino Support from Simulink รายละเอยดเพมเตม www.arduino.cc/

ภาพท 12 อปกรณไมโครคอนโทรลเลอร (Arduino Mega 2560 R3)

17

6. แบตเตอร 12V 3000 mAh ใชส าหรบจายไฟใหแก ไมโครคอนโทรลเลอร, มอเตอรกระแสตรงและวงจรขบมอเตอรกระแสตรง (Power VCC)

ภาพท 13 อปกรณแบตเตอร 12 V

7. เครองคอมพวเตอร ใชในการเขยนโคดซอฟตแวร Simulink ส าหรบ flash ลงไปยงบอรด ไมโครคอนโทรลเลอร และการใชโปรแกรม Solidworks

8.โปรแกรม MATLAB เปนโปรแกรมทสามารถค านวณสมการคณตศาสตรทซบซอนได

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

ภาพท 14 ตวอยางการใชงานโปรแกรม MATLAB

18

9. โปรแกรม Solid works เปนโปรแกรมทออกแบบชนงานในระบบ 3 มต จะมเครองมอ

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

ภาพท 15 ตวอยางการใชงานโปรแกรม Solidworks

19

วธการ

การหาแบบจ าลองทางคณตศาสตร การจ าลองระบบดวยสมการคณตศาสตร (Mathematical Model) มความส าคญตอการออกแบบตวควบคม ตองมความแมนย าใกลเคยงกบระบบจรง ในบางระบบนนการหาสมการการเคลอนทเชงกลท าไดคอนขางยากอาจตองใชวธ system identification เพอใหไดแบบจ าลองทางคณตศาสตรมา ซงการหาแบบจ าลองทมประสทธภาพมหลายวธหาไดจากสมการการเคลอนทเชงกล ยกตวอยางเชน กฏขอทสองของนวตน (Grasser et al., 2002) ทฤษฎของเคน (Kim et al., 2005) และงานวจยนน าสมการลากรานจมาใชในการหาแบบจ าลองทางคณตศาสตรซงมความเหมาะสมกบส าหรบระบบทมความยงยากและซบซอน โดยจะค านวนจากสมการพลงงานศกยและพลงงานจลนในรปของตวแปรพกดหนยนตสองลออยก าจดใหอยสามระบบพกดคอ ( )

ภาพท 16 โครงสรางของหนยนตสองลอจากโปรแกรม Solidworks

20

ภาพท 17 หนยนตสองลอบนแกนพกด x,y,z ตารางท 1 คาพารามเตอรทวไปของหนยนต พารามเตอร ความหมาย ขนาด (หนวย)

g แรงโนมถวงโลก 9.81 ( 2/ secm )

m มวลลอ 0.990 ( kg )

R รศมลอ 0.10 ( m ) 2 / 2wJ mR โมเมนตความเฉอยของลอ 0.005 ( 2kgm )

M มวลตวถง 3.1 ( kg )

W ระยะความกวางฐานลอ 0.36 ( m )

D ระยะความลกของตวถง 0.155 ( m )

H ระยะความสงของตวถง 0.289 ( m )

L ระยะจากแกนลอถงศนยถวง 0.1359 ( m ) 2 / 3byJ ML โมเมนตความเฉอยตวถงแกน y0 0.001 ( 2kgm )

bzJ โมเมนตความเฉอยตวถงแกน z0 0.0039 ( 2kgm ) mJ โมเมนตความเฉอยของมอเตอร 1x10-5 ( /Nm A ) mR คาความตานทานของมอเตอร 6.69 ( )

21

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

bK คาคงทแรงดนไฟฟายอนกลบมอเตอร 0.468 ( sec/V rad ) tK คาคงทแรงบดมอเตอร 0.317 ( /Nm A )

n อดตราทดมอเตอร 1 mf คาสมประสทธแรงเสยดทานระหวางตวถง

และมอเตอร 0.0022

wf คาสมประสทธแรงเสยดทานระหวางลอและพน

0

คาพารามเตอรทหาไดยาก ( , , , , , ,m b t m m wR K K J n f f ) จงเลอกใชคาทใกลเคยงกบงานวจยอน (Yomamoto, 2008) ก าหนดคาไดดงตารางท 1 สมการลากรางจเกดจากผลตางระหวางพลงงานจนลและพลงงานศกยภายในระบบรปแบบดงน

gL T V และ g g

i

i i

L LdQ

dt q q

(1)

โดยท T = พลงงานจลนรวมของระบบ V = พลงงานศกยรวมของระบบ iq = ระบบพกด iQ = แรงในระบบพกดทก าลงสนใจ i = อนดบของระบบ ก าหนดให = มมเฉลยของลอซายและลอขวา = มมเอยงของหนยนตสองลอ = มมเลยวของหนยนตสองลอ

22

ทเวลาเรมตน t = 0 ก าหนดใหคาพารามเตอรของหนยนตสองลอใหอยบนระบบพกดดง แสดงในภาพท 17 ดงนนสามารถเขยนคาพารามเตอรไดดงน

1

, ( )2

l r r l

R

W

(2)

( , , ) ( 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)

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

และพลงงานศกย ( )V หาไดจากสมการ (7),(8) และ (9)

2 2 2 2 2 2 2 2 2

1

1 1 1( ) ( ) ( )

2 2 2w l l l w r r r b 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

1 1 1 1 1 1( ) ( )

2 2 2 2 2 2w l w r by bz m l m rT J J J J n J n J (8)

l r bV mgz mgz mgz (9) ดงนนคาลากรานจ ( )L คอ

1 2gL T T V (10) สมการลากรานจสามารถหาไดจากสมการ

23

g gL Ld

Fdt

(11)

g gL LdF

dt

(12)

g gL LdF

dt

(13)

จากสมการลากรานจ (11), (12) และ (13) แทนคาจากสมการท (10) จดรปใหมจะไดดงน

2 2 2

2 2 2

(2 ) 2 2 ( cos 2 )

sin (2 ) sin

w m mm M R J n J MLR n J

MLR m M R MLR F

(14)

2 2 2

2 2

( cos 2 ) ( 2 ) sin

( sin ) cos

byMLR n Jm ML J n Jm MgL

MLR ML F

(15)

2

2 2 2 2

2

2 2

1( ) (2 ) 2 sin

2 2

2 (2 ) sin cos ( sin cos )

bz w m

WmW J J n J m M R MLR

R

m M R ML MLR F

(16)

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

1( , , ) ( ), , ( )

2l r r l

RF F F F F F F F

W

(17)

( )l t l m l w lF nK i f f (18) ( )r t r m r w rF nK i f f (19)

( ) ( )t l t r m l m rF nK i nK i f f (20)

โดย ,l ri คอกระแสของมอเตอร

24

เนองจากไมสามารถใชคากระแสของมอเตอรไดโดยตรงจงตองก าหนดความสมพนธระหวาง ,l ri

และ ,l rv ดงน

, , , ,( )m l r l r b l r m l rL i v K R i (21)

สามารถประมาณคาของตวเหนยวน า ( ) 0mL ดงนนจะได

, ,

,

( )l r b l r

l r

m

v Ki

R

(22)

จากสมการท (22) สามารถเขยนอยในรปของแรงไดดงน

( ) ( )2

l r wF v v f

(23)

( ) 2 2l rF v v (24)

( )r l w

R WF v v f

W R

(25)

,t t bm

m m

nK nK Kf

R R (26)

ในสมการ (14), (15) และ (16) นนเปนระบบไมเชงเสน (nonlinear system) ตวควบคมแบบ

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

2 2 2(2 ) 2 2 ( 2 )w m mm M R J n J MLR n J F (27)

2 2 2( 2 ) ( 2 )m by mMLR n J ML J n J MgL F (28) 2

2 2

2

1( )

2 2bz w m

WmW J J n J F

R

(29)

25

จากสมการ (27), (28) และ (29) เขยนใหอยในรปอยางงายไดดงน

l

r

vE F G H

v

(30)

โดย 2 2 2

2 2 2

(2 ) 2 2 2

2 2

w m m

m by m

m M R J n J MLR n JE

MlR n J ML J n J

2 2

wfF

0 0

0G

MgL

2 2H

และ ( )r lK I J v v (31)

โดย w

WI f

R

RJ

W

22 2

2

1( )

2 2bz w m

WK mW J J n J

R

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

1 1

2 2

0 0

0 0

A Bx Ax Bu x u

A B

(32)

เมอ , , , , ,T

x และ ,T

l ru v v

26

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

1

1

1

1

1

(3,2) (1,2) / det( )

(4,2) (1,1) / det( )

(3,3) ( ) (2,2) 2 (1,2) / det( )

(4,3) ( ) (1,2) 2 (1,1) / det( )

(3,4) 2,2) 2 (1,2) / det( )

(4,4) (1,2) 2 (1,1) / det( )

w

w

A MgLE E

A MgLE E

A f E E E

A f E E E

A E E E

A E E E

1

1 1

1 1

0 0

0 0

(3) (3)

(4) (4)

BB B

B B

1

1

2

(3) (2,2) / 2 (1,2) / det( )

(4) (1,2) / 2 (1,1) / det( )

det( ) (1,1) (2,2) (1,2)

B E E E

B E E E

E E E E

0 1

20

A I

K

0 0

2B J J

K K

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

1 0 0 0 0 0

0 1 0 0 0 0

0 0 0 0 1 0

C

27

การออกแบบระบบควบคม

ระบบควบคมทใชในงานวจยนประกอบดวย 4 สวนคอ คณสมบตความควบคมไดและสงเกตได, การออกแบบการวดมมเอยงดวยหลกการ Complementary filter, การออกแบบตวคมคาก าลงสองเชงเสน, การออกแบบตวควบคมโมเดลพรดกทฟ (กตตพงศ , 2551; วโรดม, 2551; Colton, 2007; Ogata, 1997; Maciejowski, 2002; Wang, 2009.)

1. คณสมบตความควบคมไดและสงเกตได (Controllability and Observability)

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

1.1 คณสมบตความควบคมได (Controllability) นยาม ความควบคมไดของสเตต ระบบพลวต x Ax Bu จะควบคมสเตตได ถาหากเลอกสเตตเรมตน 0(0)x x

ใดๆและสเตตสดทาย 1 1( )x t x โดย 1 0t แลวสามารถหาอนพต ( )u t ทขบสเตตจาก 0x ไปหา

1x

(A,B) จะสามารถควบคมสเตตได ถาและเพยงแตถา เมทรกซความควบคมได

2 1... nC B AB A B A B (34)

มคาล าดบชน (rank) เทากบ n โดยท n คอจ านวนของสเตต ซงสามารถพจารณาไดดวยโปรแกรม MATLAB ดวยค าสง rank(ctrb(A,B))

1.2 คณสมบตความสงเกตได (Observability)

นยาม ความสงเกตไดของสเตต ระบบพลวต ,x Ax Bu y Cx Du จะสงเกตสเตตได ถาหาก 1 0t ใดๆ

สามารถรคาสเตตเรมตน 0(0)x x ไดจากคาของอนพต ( )u t และเอาตพต ( )y t ในชวงเวลา 10, t

28

(A,C) สามารถสงเกตสเตตไดถาและเพยงแตถา เมทรกซความสงเกตไดถา

2

1n

C

CA

O CA

CA

(35)

มคาล าดบชน (rank) เทากบ n โดยท n คอจ านวนของสเตต ซงสามารถพจารณาไดดวยโปรแกรม MATLAB ดวยค าสง rank(obsv(A,C))

2. ออกแบบการวดมมเอยงดวยหลกการ Complementary filter การออกแบบการวดมมเอยงโดยใชการท างานรวมกนระหวางเซนเซอรวดความเรง

(Accelerometer) และเซนเซอรวดความเรวเชงมม (Rate Gyroscope) โดยสวนใหญแลวคนทวไปมกจะคนเคยกบหลกการของ Kalman filter ซงมความซบซอนและเขาใจไดยากกวาหลกการ complementary filter

2.1 การอานหาจากเซนเซอร

X_acc = (ชนดของขอมล) * (คาทอานไดจากเซนเซอรวดความเรง - ระยะหาง) * อตราขยาย Gyro = (ชนดของขอมล) * (คาทอานไดจากเซนเซอรวดความเรวเชงมม - ระยะหาง) * อตราขยาย

-ชนดของขอมล (float) คอการแปลงคา ADC (analog to digital converter) ทอาน

จากบอรด Arduino ใหอยในหนวยโวลต (voltage)

* /1023refVolts ADC V

โดยท refV = 5V และ 10-bit ADC = 2^10-1 = 1023

-ระยะหาง (offset) ส าหรบเซนเซอรวดความเรงหาระยะหางไดโดยการวางเซนเซอรใหอยในแนวราบ (horizontal) แลวอานคาเฉลยทวดได หรอสามารถหาคาไดจาก

29

datasheet ของเซนเซอรวดความเรงโดยใชคา Zero g ในทนเทากบ 1.65 V ท าการแปลงเปนคา ADC ไดเทากบ 337.59 และส าหรบเซนเซอรวดความเรวเชงมมหาระยะหางไดโดยการวางเซนเซอรใหไมมการขยบเขยอนแลวจงอานคาเฉลยทวดได

-อตราขยาย (gain) หาไดจาก datasheet หรอจากการทดลอง ในทนคอ เซนเซอรวดความเรง = 440 /mV g และ เซนเซอรวดความเรวเชงมม = 2 / /mV s

สรปในงานวจยนอานคาจากเซนเซอรทงสองชนดดงน

_ (5 /1023)*( _ )*(1/ 0.44)

(5 /1023)*( )*(1/ 0.002)

X acc X accADC offset

Gyro GyroADC offset

2.2 การวดมมจากเซนเซอร -การวดมมของเซนเซอรวดความเรง สามารถหาไดจากแกน x เพยงแกนเดยว

เนองจากในแกน y มการเปลยนแปลงนอยมาก ซงมขอดคอชวยลดเวลาการประมวลผลและความไมเปนเชงเสนไดดกวาใชทงสองแกน ( , )x y

ภาพท 18 การวดมมของเซนเซอรวดความเรง

X axis :read (1 ) sin( )g ( _ *arcsin )*180 /Angle X acc pi (36)

-การวดมมของเซนเซอรวดความเรวเชงมมดวย Integration

Angle *gyro dt (37)

30

ภาพท 19 ตวกรองสญญาณแบบดจตอล (Digital Filter)

2.3 ตวกรองสญญาณแบบดจตอล (Digital Filter) -ตวกรองความถต ากวาผานได (Low-Pass Filter) ส าหรบเซนเซอรมความแมนย าใน

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

-ตวกรองความถสงกวาผานได (High-Pass Filter) มจดเดนคอสามารถตดปญหาการ drift ไดด ใชส าหรบเซนเซอรวดความเรวเชงมมและมความแมนย าในการวดมมเอยงชวงความถสงน าตวกรองความถทงสองชนดมาใชรวมกนเขยนเปนสมการไดดงน

*( * ) (1 )*( _ )angle a angle gyro dt a x acc (38)

โดยท

adt

ในกรณทคาของเซนเซอรวดความเรวเชงมมมคา drift ประมาณ 2 องศาตอวนาท

(กรณท drift คอนขางมาก) ควรลด คา (time constant) ใหนอยกวา 1 เพอใหแนใจวามมทวดไดจะไม drift เกน 2 องศา แตการลดคา กจะท าใหสญญาณรบกวน (noise) จากเซนเซอรวดความเรงผานเขามา ดงนนการเลอกคา จงมความส าคญตอตวกรองความถมาก

31

ภาพท 20 บลอกไดอะแกรมการวดคาจากเซนเซอรทงสามชนด

3. ตวคมคาก าลงสองเชงเสน (Linear Quadratic Regulator, LQR)

ในวธการวางต าแหนงโพลนนมความเหมาะสมส าหรบระบบทมอนดบไมสงมาก หรอระบบทมลกษณะเดนเปนพลวตอนดบสองและขอจ ากดเรอง ระดบความเปนอสระ (degree of freedom) ตวคมคาก าลงสองเชงเสนคอการเลอกอตราขยายของตวปอนกลบสเตต ( )K ทเหมาะสมทสด โดยการออกแบบสญญาณควบคมซงท าใหคา (Cost function, J) มคานอยทสด (minimization) ก าหนดให (Cost function, J) คอ

0

( )T TJ x Qx u Ru dt

(39)

โดยท Q เปนเมตรกซสมมาตรทเปนบวกแนนอน (positive-definite) และเปนเมตรกซถวงน าหนก

ใหความส าคญในการควบคมของสเตตแตละตว R เปนเมตรกซสมมาตรทเปนกงบวกแนนอน (semi-definite) และเปนเมตรกซถวงน าหนกใหแกสญญาณควบคม

32

ภาพท 21 บลอกไดอะแกรมในการควบคมปอนกลบแบบรกษาเสถยรภาพ ก าหนดสมการปรภมสถานะเชงเสนคอ x Ax Bu (40) y Cx ( ) ( )u t Kx t (41) เมอ , l rx u v v

11 12 13 14 15 16

21 22 23 24 25 26

k k k k k kK

k k k k k k

แทนคา u (41) ลงในสมการ (40) จะได

( )x Ax BKx A BK x (42)

ก าหนดใหเมตรกซ A BK มเสถยรภาพหรอมคาเจาะจงเปนคาจรงเครองหมายลบทงหมด เมอแทนคา ( )u t ในสมการ Cost function (39) จะได

33

0

( )T T TJ x Qx x K RKx dt

(43)

0

( )T Tx Q K RK dt

ก าหนดให

( ) ( )T T Tdx Q K RK x x Px

dt

โดยท P เปนเมตรกซสมมาตรทเปนบวกแนนอน (positive-definite) เราจะได

( ) ( ) ( )T T 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 (44)

สมการ (44) แสดงใหเหนวา A BK เปนเมตรกซทมเสถยรภาพ ถาระบบมเสถยรภาพแลว

จะมคา P ทเปนเมตรกซสมมาตรทเปนบวกแนนอนไดเพยงคาเดยว ดงนนคา P อนๆทไดจากการแกสมการ (44) จะไมเปนเมตรกซสมมาตรทเปนบวกแนนอน

จากสมการ Cost function จะได

0

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

(45)

คา eigenvalues ของ A BK ถกก าหนดใหเปนลบทงหมด หรอ ( ) 0x ดงนนเราจะได (0) (0)TJ x Px ดงนน Cost function สามารถหาไดจากเทอมของ (0)x และเมตรกซ P เพอหาค าตอบ ก าหนดให TR T T

34

เมอ R เปนเมตรกซสมมาตรทเปนบวกแนนอน และ T ไมเปนเมตรกซหนง จากสมการ (44) สามารถเขยนไดดงน

( ) ( ) 0T T T T TA K B P P A BK Q K T TK

1 1 1( ) ( ) 0T

T T T T T TA P PA TK T B P TK T B P PBR B P Q (46)

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

1 1( ) ( )T

T T T T Tx TK T B P TK T B P x เนองจากเปนพจนทมคาเปนกงบวกแนนอนดงนนคาต าสดเปนศนย และจะเปนเชนนนเมอ

1( )T TTK T B P ดงนน 1 1 1( )T T TK T T B P R B P (47)

สมการ (47) เปนสมการทใหคา K ทเหมาะสมทสดส าหรบระบบดงกลาว ดงนนเราสามารถค านวณหาสญญาณควบคมทเหมาะสมทสดคอ

1( ) ( ) ( )Tu t Kx t R B Px t (48)

ซงเมตรกซ P ในสมการ (48) ตองสอดคลองกบสมการ (44) หรอทลดรปลงเปน Reduced-matrix Riccati Equation เขยนเปนสมการดงน

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

เมอสามารถหาคาเมตรกซ P ซงเปนค าตอบของสมการ Reduced-matrix Riccati Equation และ P เปนเมตรกซสมมาตรทเปนบวกแนนอน กจะแสดงไดวาระบบนนมเสถยรภาพ จากนนจงน าคา P แทนคาในสมการ (47) กจะไดคา K ทเหมาะสมทสด ซงในทางปฎบตการค านวณหาคา

35

K เราสามารถหาไดใชโปรแกรม Matlab โดยการปอนค าสงคอ ( , , , )K lqr A B Q R โดยท ,A B คอพลานตจากสมการ (32) และคา Q และ R เปนเมตรกซถวงน าหนกของ Cost function, J ซงหาไดโดยการลองผดลองถกจากการทดลอง ในสวนของผลการจ าลองในงานวจยนจะก าหนดใหตดตามสญญาณดงนคอ

x Ax Bu

y Cx

(50)

u Ke (51)

เมอ ,T T

l rx u v v

0 0 0T

ref ref refr

e r y

11 12 13 14 15 16

21 22 23 24 25 26

k k k k k kK

k k k k k k

การออกแบบตวขยาย K ใหสมการ Cost function ทมลกษณะคลายกบสมการ (43) ใหม

คาต าสด โดยสมการ Cost function ส าหรบระบบนคอ

0

T T TJ x Qx e K RKe dt

(52)

4. การออกแบบตวควบคมโมเดลพรดกทฟ (Model predictive control)

ตวควบคมโมเดลพรดกทฟไดรบความสนใจจากวงการอตสาหกรรมและนกวชาการตางๆซงจดเดนทส าคญคอการก าหนดขอจ ากด (constraint) ใหกบ อนพต (input), เอาตพด (output) และการเปลยนแปลงของอนพต นอกจากนยงสามารถจดการกบสงรบกวนภายนอก (disturbance) และความไมแนนอน (uncertainty) ซงมความเหมาะสมทจะน ามาใชกบหนยนตสองลอทมสงรบกวนภายนอกและความไมแนนอนสง

36

4.1 หลกการพนฐานของการออกแบบตวควบคมโมเดลพรดกทฟอธบายดงตวอยางน ทมงานวศวกรกลมหนงตองการออกแบบและควบคมหนยนตสองลอโดยใชตวควบคมโมเดลพรดกทฟ ซงมกฎในการด าเนนงานคอจะตองวางแผนงานทจะท าลวงหนาไป 8 ชม. และจะใชแผนงานทวางไวเฉพาะชม.แรกเทานน จะใชกฎนจนท าการควบคมหนยนตสองลอส าเรจ ทมงานวศกรไดเรมท างานเวลา 9 โมงเชาไดเรมวางแผนลวงหนาไป 8 ชม. อาทเชนพจารณาเกยวกบอปกรณตางๆทใชสรางหนยนตสองลอ การหาแบบจ าลองทางคณตศาสตร การออกแบบการวดมมเอยงของหนยนต ตวควบคมทจะน ามาใชงาน หลกการการรกษาสมดลของหนยนตสองลอ ซงเรมด าเนนงานตางๆนใหส าเรจทนเวลา 5 โมงเยน จากนนทมงานวศวกรไดเรมด าเนนงานตามแผนทวางไวเฉพาะชวงเวลา 9 โมง ถง 10 โมงเชา เมอถงเวลา 10 โมงเชาทมงานไดตรวจเชคสงทท าไปในชม.แรกวาท างานสวนใดเรยบรอยและสวนใดทยงท าไมส าเรจ แลวจงน าขอมลทอฟเดทจากชวโมงแรกมาวางแผนใหมโดยวางแผนลวงหนาไป 8 ชม. (ระยะเวลาทวางแผนลวงหนาเทาเดมคอ 8 ชม.) โดยแผนทวางไววางานจะส าเรจกอน 6 โมงเยน จะใชเฉพาะชวงเวลา 10 โมง ถง 11 โมง ซงจะใชกฎนไปจนกระทงท าการควบคมหนยนตสองลอส าเรจ

ภาพท 22 พนฐานของตวควบคมโมเดลพรดกทฟ ทมา : Maciejowski, (2002)

37

( )s t จดทตองการตามรอยสญญาณ (set point trajectory) ( / )r t k สญญาณอางองทตองการตามรอยสญญาณ (reference trajectory) เรมตนจาก

เอาตพตปจจบน ( )y k และสมมตใหเขาสจดทตองการตามรอยสญญาณแบบ exponentially

( )y t เอาตพตกอนหนา ˆ( / )y t k เอาตพตในอนาคตทท านาย ณ เวลาปจจบน k เวลาปจจบนดงตวอยางนเรมตนทเวลา 9 โมง ดงนน 9k Hp Prediction horizon ดงตวอยางน 8Hp Hc Control horizon คาความผดพลาดทเวลาปจจบนคอ

( ) ( ) ( )k s k y k (53) เมอสญญาณอางองทตองการตามรอยสญญาณเลอกคาความผดพลาดท i สเตปกอนหนาจะได

/( ) ( )refiTs Tk i e k

(54)

( ) ( )ik i k (55) โดยท

refT ความเรวในการตอบสนอง

sT sampling interval

และ /s refT Te

สญญาณอางองทตองการตามรอยสญญาณก าหนดเปน

( / ) ( ) ( )r k i k s k i k i (56) /

( / ) ( ) ( )s refiT Tr k i k s k i e k

(57)

38

4.2 ตวควบคมโมเดลพรดกทฟส าหรบระบบหนงอนพตและหนงเอาตพต (SISO)

ก าหนดสมการปรภมสถานะเชงเสนคอ

( 1) ( ) ( )m m m mx k A x k B u k (58) ( ) ( ) ( )m m my k C x k D u k (59)

โดยท mx คอเวกเตอรของแตละสเตทมขนาด 1n

u คออนพตทใหแกระบบ

y คอเอาตพตของระบบ

เนองจากสมมตใหอนพตไมมผลกระทบตอเอาตพตจงก าหนดให 0mD

หาผลตางเวลาของสมการท (58) เราจะได

( 1) ( ) ( ( ) ( 1)) ( ( ) ( 1))m m m m m mx k x k A x k x k B u k u k

( 1) ( 1) ( )m m mx k x k x k ( ) ( ) ( 1)m m mx k x k x k

และผลตางเวลาของอนพตคอ ( ) ( ) ( 1)u k u k u k ดงนนเขยนผลตางของปรภมสเตทไดดงน

( 1) ( ) ( )m m m mx k A x k B u k (60)

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

( ) ( ) ( )T

T

mx k x k y k

39

ผลตางเวลาของเอาตพตคอ

( 1) ( ) ( ( 1) ( ))

( 1) ( ) ( 1)

( 1) ( ) ( ) ( )

m m m

m m

m m m m m

y k y k C x k x k

y k y k C x k

y k y k C A x k C B u k

(61)

จากสมการท (60) และ (61) เขยนปรภมสเตทใหมคอ

( 1) ( )

( 1) ( )( )

( 1) ( )1

( )( ) 1

( )

x k x kA B

Tmm mm

m mm m

C

m

m

Bx k x kAm ou k

C By k y kC A

x ky k o

y k

(62)

โดยท 1

0 0 ...

n

mo o และ ( , , )A B C คอสวนขยายของโมเดล (augmented model) ซงจะน าไปใชตอในสวนถดไปของการออกแบบตวควบคมโมเดลพรดกทฟ

4.3 การท านายลวงหนาของสเตทและเอาตพต สมมตใหเวลาปจจบน , 0i ik k และขอมลของสเตททเวลาปจจบนสามารถวดได

ก าหนดอนพตในอนาคตคอ

( ), ( 1),..., ( 1)i i i cu k u k u k H และก าหนดสเตทในอนาคต

( 1/ ), ( 2 / ),...., ( / ),...., ( / )i i i i i i i p ix k k x k k x k m k x k H k โดยท ( / )i ix k m k คอสเตททท านายทเวลา ik m โดยใชขอมลจากเวลาปจจบน

จากสมการท (62) เมตรกซ ( , , )A B C สามารถเขยนใหอยในรปของการท านายสเตทดงน

40

2

( 1/ ) ( ) ( )

( 2 / ) ( 1/ ) ( 1)

( 3 / ) ( ) ( ) ( 1)

i i i i

i i i i i

i i i i i

x k k Ax k B u k

x k k Ax k k B u k

x k k A x k AB u k B u k

1 2

( / ) ( ) ( ) ( 1)p p pH H H

i p i i i ix k H k A x k A B u k A B u k

(63) ... ( 1)p cH H

i cA B u k H

สามารถเขยนสมการการท านายของเอาตพตคอ

( 1/ ) ( ) ( )i i i iy k k CAx k CB u k 2

3 2

( 2 / ) ( ) ( ) ( 1)

( 3 / ) ( ) ( ) ( 1)

i i i i i

i i i i i

y k k CA x k CAB u k CB u k

y k k CA x k CA B u k CAB u k

( 2)iCB u k

1 2

( / ) ( ) ( ) ( 1)p p pH H H

i p i i i iy k H k CA x k CA B u k CA B u k

(64) ... ( 1)p cH H

i cCA B u k H

สเตททกตวทท านายสามารถเขยนอยในรปของสเตทปจจบน ( )ix k และอนพตในอนาคต ( )iu k j ซง 0,1,.... 1cj H

ก าหนดเวกเตอร ( 1/ ) ( 2 / ) ( 3 / ) .... ( /

T

i i i i i i i p iY y k k y k k y k k y k H k

( ) ( 1) ( 2) .... ( 1)T

i i i i cU u k u k u k u k H โดยท Y มขนาด

pH และ U มขนาด cH จดรปสมการท (63) และ (64) ไดดงน ( )iY Fx k U (65)

41

ซงก าหนดให

2

3 2

1 2 3

0 0 ... 0

0 ... 0

... 0;

...p p p p p cH H H H H H

CA CB

CA CAB CB

CA CA B CAB CBF

CA CA B CA B CA B CA B

4.4 ขอจ ากด (Constraints) ของอนพตและเอาตพต ในสวนนจะก าหนดขอจ ากดใหแกการเปลยนแปลงอนพตเรมทเวลาปจจบน ik

min max( )iu u k u

ขอจ ากดของการเปลยนแปลงอนพตลวงหนา 3 sample time เขยนไดดงน

min max

min max

min max

( )

( 1)

( 2)

i

i

i

u u k u

u u k u

u u k u

คาของ sample time ทจะท านายลวงหนามความส าคญเพราะมความสมพนธเกยวกบภาระการค านวณของไมโครคอนโทรลเลอร ดงนนไมควรเลอกคาของ cH ทสงมาก ซงเราสามารถเขยนอยในรปอยางงายดงน

min maxU U U เขยนไดสองอสมการดงน

minU U (66) maxU U (67)

เขยนในรปเมตรกซ

42

min

max

I UU

I U

(68)

เมอเขยนอยในรปของการท านายลวงหนาในทก sample time และทกขอจ ากดขางในเทอมของ

U ไดดงน

( ) ( )0 0 0

( 1) ( 1)0 0

( 2) ( 1) ( 2)0

( 1) ( 1)

i i

i i

i i i

i c i c

u k u kI I

u k u kI I I

u k u k u kI I I I

u k N u k NI I I I I

(69)

และเขยนใหสนลงในรปของเมตรกซดวยการแทนคา 1 2,C C ไดดงน

min

1 2( ( 1) )iC u k C U U (70) max

1 2( ( 1) )iC u k C U U (71) โดยท minU และ maxU มขนาดเวกเตอร cH ของ minu และ maxu ตามล าดบ ดงนนสามารถเขยนขอจ ากดของเอาตพตในเทอมของ U ไดดงน

min max( )iY Fx k U Y (72)

4.5 ตวควบคมโมเดลพรดกทฟส าหรบระบบหลายอนพตและหลายเอาตพต (MIMO) ส าหรบระบบ MIMO สงทเพมเตมเขามาคอสญญาณอนพตของสงรบกวนภายนอก

ระบบ ( ( ))w k และ สมมตใหอนพตจ านวน m และ เอาตพตจ านวน q

ก าหนดสมการปรภมสถานะเชงเสนส าหรบระบบ MIMO

( 1) ( ) ( ) ( )

( ) ( )

m m m m d

m m

x k A x k B u k B w k

y k C x k

(73)

43

โดยท mx คอสเตตเวกเตอร, y คอเอาตพต (output), u คอสญญาณอนพต (input)

( )w k คอสญญาณอนพตของสงรบกวนภายนอกระบบ (input disturbance)

ล าดบของสญญาณสงรบกวนภายนอกระบบแสดงไดดงสมการ

( ) ( 1) ( )w k w k k (74)

จากสมการท (73) เขยนในรปผลตางสมการคอ

( ) ( 1) ( 1) ( 1)m m m m dx k A x k B u k B w k (75)

ก าหนดให ( ) ( ) ( 1)m m mx k x k x k และ ( ) ( ) ( 1)u k u k u k จากสมการ (74) และ

(75) จะไดดงน

( 1) ( ) ( ) ( )m m m m dx k A x k B u k B k (76)

ดงนนความสมพนธระหวางเอาตพตกบสเตทคอ

( 1) ( 1) ( ) ( ) ( )m m m m m m m m dy k C x k C A x k C B u k C B k

โดยท ( 1) ( 1) ( )y k y k y k

เราสามารถเขยนสมการปรภมสถานะเชงเสนส าหรบระบบ MIMO ไดดงน

( 1) ( )

( 1) ( )( ) ( )

( 1) ( )

( )( )

( )

BAx k x k B

T

m dmm m

m m m dm m qxq

C

m

m qxq

B BAm ox k x ku k k

C B C BC A Iy k y k

x ky k o I

y k

โดยท qxqI คอเมตรกซเอกลกษณทมมต q q เมอ q คอจ านวนของเอาตพต, m คอจ านวนของ

อนพต และ mo คอเมตรกซศนยทมมต 1q n ในขณะทเมตรกซ ,m mA B และ mC มมตเทากบ

1 1 1,n n n m และ 1q n

44

4.6 การหาสญญาณควบคมทเหมาะสมทสด (Optimization) การท าใหเอาตพตทเกดจากการท านายเขาใกลสญญาณอางอง (Set-point) จง

ตองการหาคา u ทเหมาะสมและท าให J (Costfunction) มคานอยทสด ก าหนดเวกเตอรสญญาณ

อางอง (Set-potion) ดงนคอ

1 1 1 ( ),

pN

T

s iR r k

ก าหนด Cost Function J คอ

( ) ( ) ,T T

s sJ R Y R Y U R U (77)

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

(predictied output) และสญญาณอางอง (Set-point) R คอเมตรกซทะแยงมม (diagonal matric)

c cw N NR r I (78)

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

ศนย เพอหาสญญาณควบคมทเหมาะสมโดยใชสมการท (65) จะได

( ( )) ( ( )) 2 ( ( )) ( ) .T T T T T

s i s i s iJ R Fx k R Fx k U R Fx k U R U (79)

จากอนพนธอนดบหนงสมการ (79) จะไดสมการ

0,J

U

เราจะได U ทเหมาะสมดงน

1( ) ( ( )),T T

s iU R R Fx k (80)

เมตรกซ 1( )T R คอ Hessian matrix และ sR คอ

1 1 1 1 ( ) ( ),

pN

s i s iR r k R r k

45

ซง 1 1 1 1

pN

T

sR

ดงนนการหาสญญาณควบคมทเหมาะสมทสดเชอมโยงถงสญญาณอางอง (Set-point) ( )ir k และ

ตวแปรสเตท ( )ix k เขยนสมการไดดงน

1( ) ( ( ) ( )).T T

s i iU R R r k Fx k (81)

ในการค านวณเชงตวเลขงานวจยนใช Model Predictive Control Toolbox ของโปรแกรม

MATLAB ในการค านวณ ค าสงโดยการออกแบบดไดในภาคผนวก ง. ซง Costfunction คอ

1

0

( 1/ ) ( 1) ( 1/ ) ( 1)

( / ) ( / ) ( / ) ( ) ( / ) ( )

Tp

i

TT

u u

J y k i k r k i Q y k i k r k i

u k i k R u k i k u k i k u k i R u k i k u k i

โดยท , uQ R และ uR เปนเมทรกซถวงน าหนกของเอาตพท,การเปลยนแปลงของอนพตและ

อนพตตามล าดบ

46

ผลและวจารณ

ผลการจ าลองดวยโปรแกรม MATLAB ในสวนของผลและวจารณน มงเนนเกยวกบประสทธภาพในการรกษาเสถยรภาพของตวคมคา ก าลงสองเชงเสนและการออกแบบตวควบคมโมเดลพรดกทฟ ซงจะน าตวควบคมทงสองมาเปรยบเทยบกน แสดงถงคณสมบตเฉพาะของตวควบคมโมเดลพรดกทฟในเรองของขอจ ากด (constraints) โดยจะใชโปรแกรม MATLAB ในการจ าลองผล

1. ตวคมคาก าลงสองเชงเสน (Linear Quadratic Regulator, LQR) ก าหนดให เมทรกซถวงน าหนก ( , )Q R และอตราขยาย ( )K คอ

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

Q

, 1 0

0 1R

,

0.7071 72.3187 7.8973 1.1923 0.7071 1.6314

0.7071 72.3187 7.8973 1.1923 0.7071 1.6314K

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

positio

n (

rad)

time

0 5 10 15 20 25 30-0.01

-0.005

0

0.005

0.01

time

pitch (

rad)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

yaw

(ra

d)

LQR

REF

47

ภาพท 23 เอาตพตของตวคมคาก าลงสองเชงเสน

ภาพท 24 สญญาณควบคมของตวคมคาก าลงสองเชงเสน

ก าหนดใหต าแหนงเรมตนทต าแหนง position ( ) = 0.5 rad และต าแหนงมมเลยว yaw (

) = 0.5 rad ซงตวคมคาก าลงสองเชงเสนสามารถตดตามต าแหนงอางอง (Reference) ทเปนสญญาณ

แบบ Pulse Generator (เสนสเขยว) ในสวนของต าแหนง (position) นนตดตามไดดกวาในสวนของ

มมเลยว (yaw) ในการเลอกคาของเมตรกซถวงน าหนก ( , )Q R จะก าหนดใหเปนเมตรกซทะแยง

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

ควบคมโมเดลพรดกทฟทก าหนดเมตรกซถวงน าหนกแบบเดยวกน และจากภาพท 24 สญญาณ

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

(actuator) ไมสามารถสงการท างานไดทน ซงในสวนของตวควบคมโมเดลพรดกทฟ จะมความ

ราบเรยบมากกวา

2. ตวควบคมโมเดลพรดฟทฟ

ก าหนดคาพารามเตอรในการออกแบบตวควบคมดงนคอ Control Interval Time = 0.1,

Prediction Horizon ( pH ) =10, Control Horizon ( cH ) = 2, Input Weight (R) = 0.1, Output

Weight (Q) = 1 และก าหนดใหตดตามสญญาณแบบ Pulse Generator เหมอนกบตวคมคาก าลงสอง

0 5 10 15 20 25 30-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

time

InputL

(ra

d)

0 5 10 15 20 25 30-5

-4

-3

-2

-1

0

1

2

3

4

5

time

InputR

(ra

d)

48

เชงเสนทต าแหนง position ( ) = 0.5 rad และต าแหนงมมเลยว yaw ( ) = 0.5 rad ซงจะน ามา

เปรยบเทยบกนภายหลงโดยแบงการออกแบบเปน 3 กรณดงน

2.1 ตวควบคมโมเดลพรดกทฟทไมมขอจ ากด (MPC without Constraints)

ภาพท 25 ตวควบคมโมเดลพรดกทฟทไมมขอจ ากด (MPC without Constraints)

ภาพท 26 สญญาณควบคมของโมเดลพรดกทฟทไมมขอจ ากด (MPC without constraints)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

positio

n (

rad)

0 5 10 15 20 25 30-0.05

0

0.05

time

pitch (

rad)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

yaw

(ra

d)

MPC

REF

0 5 10 15 20 25 30-4

-3

-2

-1

0

1

2

3

4

time

inputL

(volt)

0 5 10 15 20 25 30-6

-4

-2

0

2

4

6

inputR

(volt)

time

49

2.2 กรณก าหนดขอจ ากดใหแกมมพตชท 0.01 rad

ภาพท 27 ตวควบคมโมเดลพรดกทฟทมขอจ ากด (MPC with Constraints ( 0.01 Pitch))

ภาพท 28 สญญาณควบคมของตวควบคมโมเดลพรดกทฟทมขอจ ากด (MPC with constraints

( 0.01 Pitch))

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

positio

n (

rad)

time

0 5 10 15 20 25 30-0.02

-0.01

0

0.01

0.02

time

pitch (

rad)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

yaw

(ra

d)

MPC

REF

0 5 10 15 20 25 30-5

-4

-3

-2

-1

0

1

2

3

4

5

inputL

(volt)

time

0 5 10 15 20 25 30-5

-4

-3

-2

-1

0

1

2

3

4

5

inputR

(volt)

time

50

2.3 กรณก าหนดให (pH = 24)

ภาพท 29 ตวควบคมโมเดลพรดกทฟ (MPC ;pH = 24)

ภาพท 30 สญญาณควบคมของตวควบคมโมเดลพรดกทฟ (MPC ; pH = 24)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

positio

n (

rad)

time

0 5 10 15 20 25 30-0.02

-0.01

0

0.01

0.02

time

pitch (

rad)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

yaw

(ra

d)

MPC

REF

0 5 10 15 20 25 30-4

-3

-2

-1

0

1

2

3

4

5

inputL

(volt)

time

0 5 10 15 20 25 30-5

-4

-3

-2

-1

0

1

2

3

4

5

inputR

(volt)

time

51

ภาพท 25 ในกรณของตวควบคมโมเดลพรดกทฟทไมมขอจ ากดคาของสญญาณ

ควบคมมความราบเรยบมากกวาในสวนของตวคมคาก าลงสองเชงเสนและสามารถรกษาเสถยรภาพ

ในการตดตามของเอาตพตทงสามไดด ภาพท 27 แสดงถงจดเดนของตวควบคมโมเดลพรดกทฟน

คอการก าหนดขอจ ากดใหเอาตพต ซงในงานวจยก าหนดขอจ ากดใหแกมมพตชท 0.01 และใน

สวนของสญญาณควบคมกมความราบเรยบเชนเดยวกบกรณไมมขอจ ากด ภาพท 29 การก าหนดให

pH = 24 ใหมคาสง ท าใหเสถยรภาพของเอาตพตทงสามนนดขน แตจะท าใหชวง Transient ชาลง

ซงท าใหการตดตามต าแหนง (position) เขาสจดอางองไดชากวากรณก าหนดให pH = 10 ดงนน

การเลอกคาของ Prediction horizon (pH ) ใหตรงกบความตองการจงมความส าคญตอการออกแบบ

ระบบควบคม

3. เปรยบเทยบผลการจ าลองระหวางตวคมคาก าลงสองเชงเสนและ ตวควบคมโมเดล

พรดกทฟ

ภาพท 31 เปรยบเทยบผลการจ าลองระหวางตวคมคาก าลงสองเชงเสนและตวควบคมโมเดลพรดก

ทฟทไมมขอจ ากด

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

positio

n (

rad)

0 5 10 15 20 25 30-0.05

0

0.05

time

pitch (

rad)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

yaw

(ra

d)

LQR

MPC

REF

52

ภาพท 32 เปรยบเทยบผลการจ าลองระหวางตวคมคาก าลงสองเชงเสนและตวควบคมโมเดลพรดก

ทฟทมขอจ ากด ( 0.01 Pitch)

จากผลการจ าลองภาพ 31 การตดตามต าแหนงของต าแหนง (position) และการตดตามมม

การเลยว (yaw) ตวควบคมโมเดลพรดกทฟตดตามไดรวดเรวกวาตวคมคาก าลงสองเชงเสน ซงเกด

คาโอเวอรชตเพยงเลกนอย แตในสวนของมมพตช (pitch) นนตวคมคาก าลงสองเชงเสนม

ประสทธภาพทดกวา การก าหนดขอจ ากด (Constraints) ใหกบตวควบคมโมเดลพรดกทฟ ในสวน

ของมมพตช (pitch) ท 0.01 องศา ดงภาพท 32 ซงเปนการลดคาความเคลอนของมมพตชใหม

ความใกลเคยงกบตวคมคาก าลงสองเชงเสน ซงในสวนของการตดตามต าแหนงของต าแหนง

(position) และการตดตามมมการเลยว (yaw) ตวควบคมโมเดลพรดกทฟกยงสามารถตดตามได

รวดเรวกวาตวคมคาก าลงสองเชงเสน

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

positio

n (

rad)

0 5 10 15 20 25 30-0.02

-0.01

0

0.01

0.02

time

pitch (

rad)

0 5 10 15 20 25 30-0.2

0

0.2

0.4

0.6

time

yaw

(ra

d)

LQR

MPC

REF

53

ผลการทดลอง

1. การวดมมเอยง (pitch) ของหนยนตสองลอดวยหลกการ Complementary Filter

ภาพท 33 ภาพรวมของการวดมมเอยงดวยหลกการ Complementary Filter

ภาพท 34 เปรยบเทยบผลระหวางเซนเซอรวดความเรงและ Complementary Filter

0 20 40 60 80 100 120-80

-60

-40

-20

0

20

40

60

80

time

Pitch (deg)

Acc (deg)

Gyro (deg)

Gyro (deg/s)

10 12 14 16 18 20 22

-8

-6

-4

-2

0

2

4

6

8

10

time

Pitch (deg)

Acc (deg)

Gyro (deg)

Gyro (deg/s)

54

ภาพท 35 เปรยบเทยบผลของมมจากเซนเซอรทงสองและ Complementary Filter ทความถสง

ภาพท 36 เปรยบเทยบผลระหวางเซนเซอรวดความเรวเชงมมและ Complementary Filter

ก าหนดคาของ Sampling time 0.05 และ a = 0.96 การก าหนดคา a ไดอธบายแลวในสวน

ของการออกแบบการควบคม ในภาพท 34 ไดขยบหนยนตไปทมม 20 ทเวลา 5 วนาทถง 35

วนาท ท าการเปรยบเทยบคาของมมทวดไดจากเซนเซอรวดความเรงกบคามมทผานตวกรองความถ

Complementary จะเหนไดวาเสนสน าเงนจะมการสนทมากกวาเสนสแดง ซงในชวงแรกคามมทวด

35 40 45 50 55 60

-15

-10

-5

0

5

10

15

20

time

Pitch (deg)

Acc (deg)

Gyro (deg)

Gyro (deg/s)

70 75 80 85 90 95-15

-10

-5

0

5

10

15

20

time

Pitch (deg)

Acc (deg)

Gyro (deg)

Gyro (deg/s)

55

จากเซนเซอรวดความเรวเชงมมในเสนสเขยว ไดผลใกลเคยงกบคามมทผานตวกรองความถ

Complementary ในภาพท 35 ไดท าการขยบหนยนตสองลอใหสนดวยความถสงทเวลา 40 วนาทถง

60 วนาท จะเหนวาคาความเรวเชงมมเสนสเหลองและคาของมมทวดไดจากเซนเซอรวดความเรง

เสนสน าเงนมการสนทสงมาก แตในสวนของคามมทผานตวกรองความถ Complementary เสนส

แดง มการสนทนอยกวามาก ในภาพท 36 คาของมมทวดจากเซนเซอรวดความเรวเชงมมในเสนส

เขยวทเวลา 70 วนาท นน Drift ออกถง 10 องศา ซงในสวนของเสนสน าเงนและสแดงยงคงอยท 0

องศา

2. การรกษาสมดลของหนยนตสองลอ

ในสวนของผลการทดลองนไดใช support packages arduino mega 2560 ผานทาง

ซอฟแวร Simulink ของโปรแกรม MATLAB ซงไมโครคอนโทรลเลอรสามารถวดคาต าแหนงจาก

เอนโคดเดอรเซนเซอรไดโดยผาน S-Functions (ภาคผนวกท จ6) แตเมอวดต าแหนงดวยความเรวท

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

ของเอนโคดเซนเซอรทมากหรออาจเกดจากการใช support packages ของโปรแกรม ซงการใช

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

ดงนนงานวจยในอนาคตจะแกปญหาในสวนนตอไป

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

สามารถรบคาจากเซนเซอรเอนโคดเดอรไดทนจงก าหนดใหอตราขยายของมมเฉลยของลอซายและ

ลอขวา = 0 และอตรายาขยายของมมเลยว = 0 และมผลกระทบท าใหอตราขยายของมมพทลดลงไป

มากเขยนไดดงนคอ

0 35.0 0 0.5 0 0

0 35.0 0 0.5 0 0K

56

ภาพท 37 คาเอาตพตของมม Pitch จากการรกษาสมดลของหนยนตสองลอ

ภาพท 38 สญญาณควบคม PWM จากการรกษาสมดลของหนยนตสองลอ

จากผลการทดลองในภาพท 37 จะเหนไดวาคาเอาตพตของมม pitch นนรกษาเสถยรภาพท

มม 0.05 (rad) อยางไรกตามคามมของมม pitch นนคอนขางจะสนและมการ drift ออกจากจด

ศนย เนองจากการวดมมเอยงทไมแมนย า และการวดมมเอยงทไมแมนย านนมผลมาจากคาของ dt

(sample time) ท 0.05 ท าใหตองลดคาสมประสทธ a นอยกวา 0.98 มาก การก าหนดใหนอยกวา

0 2 4 6 8 10 12-0.06

-0.04

-0.02

0

0.02

0.04

0.06

degre

e (

rad)

time

0 2 4 6 8 10 12-150

-100

-50

0

50

100

150

PW

M

time

57

0.98 กจะท าใหคา noise จากเซนเซอรวดความเรงเขามาไดมาก และถาท าการก าหนดคาสมประสทธ

a ใหเทากบ 0.98 กจะท าใหคามมทวดได drift ออกจากจดศนย ดงนนคา sample time ควรนอยกวา

กวาหรอเทากบ 0.01 กจะท าใหลดคา noise และคา drift ลงไดมาก ซงโดยหลกการของ

complementary filter นนการก าหนดคาของสมประสทธ a และคาของ (time constant) นนมผล

โดยตรงตอการวดมมเอยง

58

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

สรป

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

ควบคมโมเดลพรดกทฟ ซงไดแสดงถงเสถยรภาพของตวควบคมทงสองดงผลการจ าลอง การใชตว

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

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

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

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

รดกทฟสามารถจดการกบปญหาขางตนไดดเมอเปรยบเทยบกบตวคมคาก าลงสองเชงเสนอกทงยงม

ความเรวในการตดตามสญญาณไดดกวาในชวงของ Transient และไดแสดงถงคณสมบตพเศษคอ

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

ตองการลดการใชพลงงานได

สรปการวดคามมจากเซนเซอรวดความเรงเพยงอยางเดยวนนจะพบปญหาในเรองของการ

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

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

หยดนงได แตจะวดคามมไดดในชวงความถต าหรอความเรวนอยๆ สวนการวดมมจากเซนเซอรวด

ความเรวเชงมมเพยงอยางเดยวกจะพบปญหาในเรองการ Drift ออกจากคามมทถกตอง กลาวคอเมอ

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

แมนย าเมอความถสงหรอความเรวมาก ดงนนดวยหลกการ Complementary Filter เปนการน าขอด

ของเซนเซอรทงสองชนดมาใชงานรวมกน และจดการขอเสยของเซนเซอรทงสองดวยตวกรอง

ความถสงและตวกรองความถต า

59

ขอเสนอแนะ

ตวคมคาก าลงสองเชงเสนสามารถหาอตราขยายไดงายแตการเลอกเมตรกซถวงน าหนก

( ,Q R ) ยงตองใชวธการลองผดลองถกซงในสวนนอาจท าใหเวลาคอนขางมาก ซงในงานวจยทวไป

นนจะใหเมตรกซถวงน าหนกแกเอาตพตมากกวาและส าหรบหนยนตสองลอมกจะใหน าหนกแกคา

มม Pitch เพอรกษาเสถยรภาพของระบบ นอกจากนความแมนย าของตวคมคาก าลงสองเชงเสนน

ตองอาศยความแมนย าของแบบจ าลองทางคณตศาสตรของหนยนตสองลอ ซงมความสมพนธกบ

การหาคาพารามเตอรของตวหนยนตทแมนย าเชนกน ดงนนคาพารามเตอรบางตวทหาไดคอนขาง

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

ในการก าหนดขอจ ากดใหแกตวควบคมโมเดลพรดกทฟเมอก าหนดใหขอจ ากดมขนาดท

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

ท าการทดลองในการเลอกขอจ ากดของคาอนพตและเอาตพตของระบบ การเลอกคาพารามเตอรท

ส าคญของตวควบคมโมเดลพรดกทฟมผลกระทบตอเสถยรภาพของระบบ ดงนนจ าเปนตองมความ

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

ส าหรบการน าไปใชงานจรงการเลอกคาของ cH นนสงผลกระทบตอการประมวลผลของ

ไมโครคอนโทรลเลอรวามความเรวเพยงพอหรอไม

การวดมมเอยงของหนยนตสองลอดวยการเขยนโคดโปรแกรมลงในซอฟแวร Simulink

ของโปรแกรม MATLAB นนมความเหมาะสมส าหรบการศกษาและท าความเขาใจในตวควบคมทง

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

ประสงค ดงนนหากผวจยมความรความเขาใจทางดานภาษา C++ ในการเขยนโคดส าหรบ Arduino

โดยตรงกจะท าใหลดปญหาของการประมวลผลและ Bug ได การใชโปรแกรม LabVIEW และใช

Hardware ของ NI กอาจเปนอกทางเลอกนงทดแตตองแลกกบราคาทคอนขางแพงกวา

ไมโครคอนโทรเลอร (Arduino) หลายเทาตว

60

เอกสารและสงอางอง

กตตพงศ เยาวาจา. 2551. การควบคมทเหมาะสมทสดของหนยนตสองลอดวยการความเอยง

แบนดวทธสง.มหาวทยาลยเกษตรศาสตร, กรงเทพฯ.

วโรดม ตจนดา. 2550. การวเคราะหและออกแบบระบบควบคมทางวศวกรรม.

มหาวทยาลยเกษตรศาสตร, กรงเทพมหานคร

วกพเดย สารานกรมเสร. 2556. หมวดหมเทคโนโลยและหนยนต. แหลงทมา:

th.wikipedia.org/wiki/หนยนต, 2 กนยายน 2557.

Brkic, K. and Z. Kovacic. 2009. Decoupled Control and Path Tracking of a Two-wheeled

Self-balancing Mobile Robot. IEEE International Symposium on Industrial

Electronics (ISlE 2009), 5-8 July 2009, Seoul Olympic Parktel, Seoul, Korea.

Chai, S., L.Wang and E. Rogers. 2013. Model predictive control of a permanent magnet

synchronous motor with experimental validation. Control Engineering Practice21

(2013) pp. 1584–1593

Cheng, P. and B. Oelmann. 2010. Joint-Angle Measurement Using Accelerometers and

Gyroscopes—A Survey. IEEE Transactions on Instrumentation and Measurement,

VOL. 59, NO. 2, FEBRUARY 2010.

Colton, S. 2007. The Balance Filter. Submitted as a Chief Delphi white paper. 25 June 2007.

Deng, M., A. Inoue., K. Sekiguchi and L.Jiang. 2010. Two-wheeled mobile robot motion control

in dynamic environments. Robotics and Computer-Integrated Manufacturing. No. 26

(2010) pp. 268–272

61

Ekaputri, C. and A. Rohman. 2012. Implementation Model Predictive Control (MPC)

Algorithm-3 for Inverted Pendulum. IEEE Control and System Graduate Research

Colloquium (ICSGRC 2012).

Ferdinando H., H. Khoswanto and S. Tjokro. 2011. Design and Evaluation of Two-wheeled

Balancing Robot Chassis. International Symposium on Innovations in Intelligent

Systems and Applications (INISTA). 15-18 June 2011.

Feng, T., T. Liu and X. Wang. 2011. Modeling and Implementation of Two-wheel Self-

balancing Robot Equipped With Supporting Arms. IEEE Conference on Industrial

Electronics and Applications (ICIEA). No. 6, 2011.

Ge, S. S. and Y. J. Cui. 2000. New Potential Functions for Mobile Robot Path Planning. IEEE

Transactions on Robotics and Automation, VOL. 16, No. 5, OCTOBER 2000.

Grasser, F., A. D. Arrigo, S. Colombi and A. Ruffer. 2002. JOE: A Mobile, Inverted Pendulum.

IEEE Transactions on Industrial Electronics, 49(1).

Kim, Y. H., S. H. Kim and Y. K. Kwak. 2005. Dynamic Analysis of a Nonholonomic

Twowheeled Inverted Pendulum Robot, pp. 505-513. Proceedings of the 8th on

Artificial Life and Robotics, 21(3).

Lee, J., S. Han and J. Lee. 2013. Decoupled Dynamic Control for Pitch and Roll Axes of the

Unicycle Robot. IEEE Transactions on Industrial Electronics, VOL. 60, NO.9,

September 2013.

Lin, C. and Y. Liu. 2012. Precision Tracking Control and Constraint Handling of Mechatronic

Servo Systems Using Model Predictive Control. IEEE/ASME Transactions on

Mechatronics, VOL 17, NO.4, AUGUST 2012.

62

Maciejowski, J.M. 2002. Predictive Control with Constraints. Published by Pearson Education

under The Prentice Hall imprint. June 2001.

Ogata, K. 1997. Modern Control Engineering. 3rd Edition. Prentice-Hall. New York.

Oryschuk, P., A. Salerno., A. M. Al-Husseini and J. Angeles. 2009. Experimental Validation of

an Underactuated Two-Wheeled Mobile Robot. IEEE/ASME TRANSACTIONS ON

MECHATRONICS, VOL. 14, NO. 2, APRIL 2009.

Siciliano, B. and O. Khatib. 2008. Springer Handbook of Robotics. 2008 edition. (pp.391-409)

Publisher Springer.

Urakubo, T., K. Okuma and Y. Tada. 2004. Feedback Control of a Two Wheeled Mobile Robot

with Obstacle Avoidance Using Potential Functions. IEEE/RSJ International

Conference on Intelligent Robots and Systems 28 September - 2 October 2004,

Sendai, Japan.

Wang, L. 2009. Model Predictive Control System Design and Implementation Using

MATLAB. Advances in Industrial Control ISSN 1430-9491. Springer-Verlag London

Limited, 2009.

Wasif, A. and D. Raza., et al. 2013. Design and Implementation of a Two wheel Self

Balancing Robot with a Two Level Adaptive Control. IEEE International Conference

on Control and Automation (ICCA), 2013.

Yap, H. and S. Hashimoto. 2012. Development of a stair traversing two wheeled robot.

IEEE/RSJ International Conference on Intelligent Robots and Systems, 7-12

October, 2012. Vilamoura, Algarve, Portugal.

Yomamoto, Y. 2008. NXTway-GS (Self-Balancing Two-Wheeled Robot) Controller Design.

http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=19147

63

Zeng, Z. and H. Lu., and Z. Zhiqiang. 2013. High-speed Trajectory Tracking Based on Model

Predictive Control for omni-directional Mobile Robots. Control and Decision

Conference (CCDC), No. 25, 2013. Chinese.

64

ภาคผนวก

65

ภาคผนวก ก การหาคาพารามเตอรดวยโปรแกรม Solidworks

66

ภาพผนวกท ก1 การหาคาพารามเตอรดวยโปรแกรม Solidworks

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

L ระยะจากแกนลอถงศนยถวง 0.1359 ( m )

67

ภาคผนวก ข การหาแบบจ าลองทางคณตศาสตรของหนยนตสองลอและอตราขยายของตวคมคาก าลงสองเชง

เสนดวยโคดโปรแกรม MATLAB

68

คาพารามเตอรของหนยนตสองลอ %parameters of two wheel robot g = 9.81; %Gravity acceleration (m/sec^2) m = 0.990; %Wheel weight (kg) R = 0.1; %Wheel radius (m) Jw = (m*R^2)/2; %Wheel inertia moment (kgm^2) M = 3.1; %Body weight (kg) W = 0.36; %Body width (m) D = 0.155; %Body depth (m) H = 0.339 ; %Body height (m) L = 0.1359 ; %Distance of the center of mass from the wheel axle (m) Jp = (M*L^2)/3; %Body pitch inertia moment (kgm^2) Jy = M*(W^2+D^2)/12; %Body yaw inertia moment (kgm^2) Jm = 1*10^-5; %DC motor inertia moment Rm = 3.4; %DC motor resistance Kb = 0.468; %DC motor back EMF constant Kt = 0.317 ; %DC motor torque constant n = 1 ; %Gear ratio fm = 0.0022; %Friction coefficient between body and DC motor fw = 0 ; %Friction coefficient between wheel and floor %We use the values described in reference [2] for Rm,Kb,Kt %We use the values that seems to be appropriate for Jm,n,fm,fw, because it is difficult to measure การหาแบบจ าลองทางคณตศาสตร Beta = ((n*Kt*Kb)/Rm) + fm; Lfa = (n*Kt)/Rm;

69

E = [((2*m+M)*R^2+(2*Jw)+(2*n^2*Jm)) (M*L*R-2*n^2*Jm); (M*L*R-(2*n^2*Jm)) (M*L^2+Jp+(2*n^2*Jm))]; E11 = ((2*m+M)*R^2+(2*Jw)+(2*n^2*Jm)); E12 = (M*L*R-2*n^2*Jm); E21 = (M*L*R-(2*n^2*Jm)); E22 = (M*L^2+Jp+(2*n^2*Jm)); F = [(Beta+fw) -Beta; -2*Beta 2*Beta]; G = [0 0; 0 -M*g*L]; H = [Lfa/2 Lfa/2; -Lfa -Lfa]; I = [Beta + (W/R)*fw]; J = [(R/W)*Lfa]; K = [1/2*m*W^2 + Jp + (W^2/(2*R^2))*(Jw + n^2*Jm)]; A32 = [(-M*g*L*E12)/det(E)]; A42 = [(M*g*L*E11)/det(E)]; A33 = [-((Beta+fw)*E22 + 2*Beta*E12)/det(E)]; A43 = [((Beta+fw)*E12 + 2*Beta*E11)/det(E)]; A34 = [Beta*(E22 + 2*E12)/det(E)]; A44 = [-Beta*(E12 + 2*E11)/det(E)]; B3 = [Lfa*((E22/2) + E12)/det(E)]; B4 = [-Lfa*((E12/2) + E11)/det(E)]; A1 = [0 0 1 0; 0 0 0 1; 0 A32 A33 A34; 0 A42 A43 A44];

70

B1 = [0 0; 0 0; B3 B3; B4 B4]; A2 = [0 1; 0 -I/K]; B2 = [0 0; -J/K J/K]; A_bar = [A1, zeros(4, 2); zeros(2,4), A2] B_bar = [B1;B2] C_bar = [ 1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 0 1 0] D_bar = [zeros(3,2)] Q_bar = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1] R_bar = [1 0; 0 1] twowheel = ss(A_bar,B_bar,C_bar,D_bar); อตราขยายของตวคมคาก าลงสองเชงเสน K = lqr(A_bar,B_bar,Q_bar,R_bar)

71

ภาคผนวก ค การจ าลองผลการทดลองดวยตวคมคาก าลงสองเชงเสน

72

ภาพผนวกท ค1 การจ าลองผลการทดลองดวยตวคมคาก าลงสองเชงเสน ก าหนดคาอตราขยายและ plant ดงน

0.7071 72.3187 7.8973 1.1923 0.7071 1.6314

0.7071 72.3187 7.8973 1.1923 0.7071 1.6314K

0 0 1 0 0 0

0 0 0 1 0 0

0 60.7802 1.3670 1.3670 0 0

0 87.6433 1.3923 1.3923 0 0

0 0 0 0 0 1

0 0 0 0 0 0.2113

A

0 0

0 0

1.3287 1.3287

1.3533 1.3533

0 0

0.1141 0.1141

B

1 0 0 0 0 0

0 1 0 0 0 0

0 0 0 0 1 0

C

0 0

0 0

0 0

D

73

ภาคผนวก ง การหาผลการจ าลองดวยตวควบคมโมเดลพรดกทฟ

74

ภาพผนวกท ง1 การหาผลการจ าลองดวยตวควบคมโมเดลพรดกทฟ ก าหนดคาพารามเตอร และ plant ดงน Control Interval Time = 0.1, Prediction Horizon (

pH ) = 10, Control Horizon ( cH ) = 2

0 0 1 0 0 0

0 0 0 1 0 0

0 60.7802 1.3670 1.3670 0 0

0 87.6433 1.3923 1.3923 0 0

0 0 0 0 0 1

0 0 0 0 0 0.2113

A

0 0

0 0

1.3287 1.3287

1.3533 1.3533

0 0

0.1141 0.1141

B

1 0 0 0 0 0

0 1 0 0 0 0

0 0 0 0 1 0

C

0 0

0 0

0 0

D

75

ภาพผนวกท ง2 การก าหนดคาพารามเตอรของตวควบคมโมเดลพรดกทฟ

76

ภาคผนวก จ โคดโปรแกรม MATLAB ของผลการทดลอง

77

ภาพผนวกท จ1 การรบคาจากเซนเซอรวดความเรงและเซนเซอรวดความเรวเชงมม

ภาพผนวกท จ2 การรบคาจากเอนโคดเดอรเซนเซอรดวย S-function และอตราขยาย

78

ภาพผนวกท จ3 การควบคมมอเตอรซายและมอเตอรขวาดวย PWM

ภาพผนวกท จ4 การวดมมเอยงดวยหลกการ complementary filter (Subsystem)

79

ภาพผนวกท จ5 การวดต าแหนงและมมเลยวของเอนโคดเดอรเซนเซอร (Subsystem1 และ Subsystem2)

ภาพผนวกท จ6 การใช S-function ในการวดคาจากเอนโคดเดอรเซนเซอร

80

ประวตการศกษาและการท างาน

ชอ – นามสกล นายพงศกร กวสนทรเสนาะ วน เดอน ป ทเกด วนท 10 สงหาคม 2532 สถานทเกด จงหวดกรงเทพมหานคร ประวตการศกษา ปรญญาวศวกรรมศาสตรบณฑต (วศวกรรมขนถาย

วสด) มหาวทยาลยเทคโนโลยพระจอมเกลาพระนครเหนอ พ.ศ. 2553

ต าแหนงหนาทการงานปจจบน - สถานทท างานปจจบน - ผลงานดเดนและรางวลทางวชาการ - ทนการศกษาทไดรบ -