Go!!

17 กันยายน 2552

b5

5. Controller Design

ในการออกแบบระบบสำหรับควบคุมการทำงานของระบบ Rotary Invert Pendulum จะประกอบไปด้วย 2mode คือ

1.การออกแบบระบบควบคุมสำหรับการแกว่ง Link 2 ขึ้นมาตั้งในแนวดิ่ง (Swing-up controller design)

2.การออกแบบระบบควบคุมสำหรับการรักษาให้ Link 2 ยังคงตั้งในแนวดิ่งให้ได้นานที่สุด (Balancing controller design)

โดยการออกแบบแต่ละระบบจะใช้ระบบการควบคุมไม่เหมือนกันดังนี้

Swing-up controller design

การออกแบบระบบสำหรับการแกว่ง Link 2 ขึ้นมาตั้งในแนวดิ่ง จะใช้ระบบควบคุมแบบ Proportional–Integral–Derivative Controller หรือ (PID Controller) โดยจะใช้ Block Diagram ดังรูปข้างล่าง

รูปที่ 8 แสดง Block Diagram ของระบบควบคุมแบบ PID Controller

ซึ่งสามารถหาฟังก์ชั่นสำหรับการถ่ายโอน(Transfer Function)ของอินพุตและเอาท์พุตคือ

โดยที่

ซึ่งจะใช้การประมาณค่าแบบ Standard Second Order Transfer Function ที่มีอยู่ว่า

และ

เมื่อ Tpคือ Peak Times

ทำให้หาค่าอัตราขยายของระบบแบบ PID Controller ได้เป็น

Balancing controller design

การออกแบบระบบสำหรับการรักษาให้ Link 2 ยังคงตั้งในแนวดิ่งให้ได้นานที่สุด จะใช้ระบบควบคุมแบบLinear-Quadratic-Regulator Controller หรือ (LQR Controller) โดยเป็นระบบควบคุมสำหรับ State Space Systems ที่หามาได้ก่อนหน้านี้ซึ่งการหาค่าอัตราขยายของระบบ จะใช้โปรแกรม MATLAB ที่มี Function คือ [K,S,E] = LQR(A,B,Q,R,N) โดยให้

และ N=0 ทำให้ได้ค่าอัตราขยายดังรูปข้างล่าง

รูปที่ 4 แสดงการหาค่าอัตราขยายจากโปรแกรม MATHLAB ของระบบควบคุมแบบ LQR Controller

เมื่อนำระบบทั้งสองส่วนมารวมกันต้องมีการเลือกการทำงานระหว่างสองระบบ ซึ่งจะใช้การกำหนดค่าเริ่มต้นของการควบคุมใหม่เมื่อทำงานในอีกระบบหนึ่งดังแสดงในรูปด้านล่าง ที่เริ่มจากการกำหนดค่าเริ่มต้นของการ Input ข้อมูล หรือ data ในระบบ แล้วก็อ่านข้อมูลดังกล่าวเพื่อตรวจสอบตามเงื่อนไขในการเลือกการควบคุมว่าจะใช้ mode ไหนในการควบคุมระบบ เพื่อกำหนดการหมุนของมอเตอร์ต่อไป

รูปที่ 9 แสดงแผนภาพสำหรับการควบคุมทั้งสองระบบ

b4

4. Mathematics Model

รูปที่ 6 แสดงภาพรวมของระบบ

จากรูปที่แสดงภาพรวมระบบข้างต้นเป็นการสร้างอ้างอิงระบบ Rotary Invert Pendulum เพื่อมาสร้างแบบจำลองทางคณิตศาสตร์ โดยจะศึกษาจลน์ศาสตร์ ( Kinetic) ของระบบดังรูปดังกล่าวซึ่งจะประกอบ ด้วย Rotary arm หรือ Link1 และ Pendulum rod หรือ Link2 และ DC Motor การศึกษาการเคลื่อนที่ของระบบจะใช้ทฤษฎีของ Euler-Lagrange equation of motion ที่มีอยู่ว่า

q(t)คือ เวกเตอร์แสดงตำแหน่งของมุมในการเคลื่อนที่ของระบบ

q(t)คือ เวกเตอร์แสดงความเร็วในการเคลื่อนที่ของระบบ

Qiคือ เวกเตอร์แสดงแรงภายนอกที่กระทำกับระบบ

L คือ Lagrangian

W คือ พลังงานนที่สูญเสียในระบบ

ซึ่งตัวLagrangianที่หาได้จาก

เมื่อ

Ttotalคือ พลังงานจลน์รวมของระบบ

Vtotal คือ พลังงานศักย์รวมของระบบ

พลังงานจลน์รวมของระบบ = พลังงานจลน์รวมของ Link1 + พลังงานจลน์รวมของ Link2 เขียนได้เป็นว่า

ซึ่ง พลังงานจลน์รวมของ Link1 คือ

ขณะที่จุดหมุนของระบบอยู่ที่กึ่งกลางของ Link1 ดังนั้น L1=0 ทำให้

ส่วน พลังงานจลน์รวมของ Link2 หาได้จาก

และนอกจากนี้จำเป็นต้องหาพลังงานศักย์ของระบบด้วยโดยที่พลังงานศักย์ ของ Link1 เป็นศูนย์เพราะจุดอ้างอิงของระบบอยู่ที่ Link1 ทำให้ได้พลังงานศักย์รวมของระบบ คือ

ทำให้ค่าของ Lagrangian คือ

และแทนค่าพลังงานต่างๆ จากที่หามาทำให้ได้ว่า

ส่วนพลังงานที่สูญเสียของระบบหาได้จาก

จากกฎของ Euler-Lagrange equation of motion ทำให้สามารถเขียนสมการของระบบใหม่ได้เป็น

เมื่อแทนค่าต่างๆลงไปทำให้ได้ระบบสมการเป็น

หาค่าของTorqeของมอเตอร์ จาก

และนำ Parameter จากโมเดลของระบบมาแทนค่าใหม่จะได้ตารางคือ

ตารางที่ 1 Defintion of parameter in the model

เพื่อทำให้ระบบสมการดูง่ายขึ้นจึงแทนค่า Parameter ใหม่ทำให้ได้เป็น

แต่ระบบสมการยังไม่เป็นแบบเชิงเส้นจึงต้องมีการทำ Linearizationเพื่อประมาณค่าของระบบใหม่โดยที่ให้

ทำให้ได้ระบบที่เป็นเชิงเส้นคือ

นำระบบสมการมาเขียนเป็น สมการ ODE โดยใช้ Solving System of ODE Elimination ดังตาราง

ตารางที่ 2 Defintion of parameter in H^-1 matrix

ทำให้ได้สมการ ODE เพื่อบอกว่าระบบตอบสนองเป็นยังไงดังนี้

เมื่อนำค่าคงที่ต่างๆ ในระบบจริงดังแสดงในตารางข้างล่าง

ตารางที่ 3 ตารางค่าคงที่ในระบบ

แทนลงไปในสมการจะทำให้ได้ State Space

หลังจากได้ State Space ของระบบแล้วก็นำสมการไปพล็อตกราฟโดยใช้โปรแกรม MATHLAB ที่ป้อนอินพุตให้กับระบบแบบ Step Input ทำให้ได้กราฟดังแสดงข้างล่าง

รูปที่ 7 แสดงผลการตอบสนองของระบบเมื่อป้อน Step Input

b3

3. System Block Diagram

Rev.1

รูปที่ 2 System Block Diagram ระบบที่ 1

จากบล็อกไดอะแกรมข้างต้น เมื่อเริ่มการทำงาน Arduinoจะทำการอ่านข้อมูลดิจิตอล 4 บิต จาก Magnetic rotary encoder เพื่อประมวลผล แล้วสั่งงานมอเตอร์ผ่านทาง Motor driver โดยที่สั่งงานแบบ PWM (Pluse Width Modulation) เพื่อปรับความเร็วในการหมุนให้สัมพันธ์กับข้อมูลที่ได้รับจาก Magnetic rotary encoder ตามสมการที่ได้ตั้งไว้ในตอนแรก โดยในระบบแรกนี้ไม่มีการป้อนกลับของตำแหน่งของมอเตอร์ อีกทั้งมอเตอร์เป็นชนิดที่มีการทดรอบทำให้การตอบสนองไม่ดีเท่าที่ควร จึงทำการปรับปรุงเป็นระบบที่สอง

Rev.2

รูปที่ 3 System Block Diagram ระบบที่ 2

จากบล็อกไดอะแกรมข้างต้น เมื่อเริ่มทำงาน Arduinoจะทำการอ่านข้อมูลดิจิตอล 4 บิต จาก Magnetic rotary encoder เพื่อประมวลผลเช่นเดียวกับระบบแรก แต่ในระบบที่สองนี้ยังมีการอ่านข้อมูลดิจิตอล 4 บิต จาก Encoder ของมอเตอร์ด้วย แล้วสั่งงานมอเตอร์ผ่านทาง Motor driver โดยที่สั่งงานแบบ PWM เพื่อปรับความเร็วในการหมุนให้สัมพันธ์กับข้อมูลที่ได้รับจาก Magnetic rotary encoder และ Encoder ตามสมการที่ได้ตั้งไว้ ซึ่งในระบบที่สองนี้สามารถควบคุมตำแหน่งของมอเตอร์ให้สัมพันธ์กับการเปลี่ยนแปลงของมุม Pendulum rod และสามารถอ่านค่าตำแหน่งของมอเตอร์กลับมาเพื่อแก้ใขความผิดพลาดในรอบต่อไปได้ด้วย แต่เนื่องด้วย Encoder ไม่ละเอียดพอและมอเตอร์ยังคงตอบสนองได้ไม่ดีเท่าที่ควร จึงทำการปรับปรุงเป็นระบบที่สาม

Rev.3

ATMEGA 8

Variable

Resistor

Host PC

Serial

Module

DC Motor

Encoder

รูปที่ 4 System Block Diagram ระบบที่ 3

จากบล็อกไดอะแกรมข้างต้น เมื่อเริ่มทำงาน Arduinoจะทำการอ่านข้อมูลอนาล็อกจาก Variable resistorแปลงเป็นค่าดิจิตอลแล้วส่งให้กับคอมพิวเตอร์โดยส่งข้อมูลผ่านทาง Serialport หลังจากนั้นคอมพิวเตอร์จะนำข้อมูลที่ได้มาคำนวนตามสมการที่ตั้งไว้ งจากนั้นตคอมพิวเตอร์์ผ่านทาง อมพิวเตอร์ผ่านทาง XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXแล้วสั่งงานมอเตอร์ผ่านทาง Serial portโดยที่สั่งงานแบบ PWM เพื่อปรับความเร็วในการหมุนให้สัมพันธ์กับข้อมูลที่ได้รับจากArduinoและ Encoder ที่ป้อนค่าตำแหน่งกลับมา ซึ่งในระบบนี้สามารถควบคุมตำแหน่งของมอเตอร์ให้สัมพันธ์กับการเปลี่ยนแปลงของมุม Pendulum rod และสามารถอ่านค่าตำแหน่งของมอเตอร์กลับมาเพื่อแก้ใขความผิดพลาดในรอบต่อไปได้ แต่เนื่องด้วยค่าที่ได้จาก Variable resistor ไม่ละเอียดพอทำให้การควบคุมตำแหน่งของมอเตอร์ที่ควรจะเป็นผิดพลาดไปด้วย

Interface

A/B Quarture

PWM

Serial

รูปที่ 5 Interface ของระบบ

Interface ที่ใช้ติดต่อระหว่าง Arduino กับคอมพิวเตอร์ ใช้การเชื่อมต่อแบบอนุกรมผ่านทาง Serial port ซึ่งเป็นการส่งค่าที่อ่านได้จาก Magnetic rotary encoder และ Motor Encoder โดยแสดงผลทาง SerialMonitor เพื่อทำการวิเคราะห์ต่อไปส่วนการติดต่อระหว่าง Magnetic rotary encoder และ Motor Encoder กับ Arduino นั้นเป็นแบบ A/B Quarture mode ซึ่งสามารถแสดงองศาการหมุนและทิศทางการหมุนได้ ว่าเป็นแบบทวนเข็มนาฬิกาหรือตามเข็มนาฬิกา ส่วนการติดต่อระหว่าง Arduino กับ Motor driver นั้นใช้ PWM ซึ่งสามารถควบคุมความเร็วของมอเตอร์ได้อ่านข้อมูล ดิจิตอลจาก์ชนิดXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

b2

2. Hardware component

Sensor

AS5040 10 bit 360° programmable magnetic rotary encoder

Variable resistor 50k ohms

Motor Driver

ET-OPTO DC Motor 6-24 V/10A Motor driver

Controller

ATMEGA 8 Microcontroller

Motor

Zheng DC Motor 12V 400 RPM

Cool muscle Stepping Motor 24V 3000 RPM

Minertia DC Motor 12V 2000 RPM

b2

2. Hardware component

Sensor

AS5040 10 bit 360° programmable magnetic rotary encoder

Variable resistor 50k ohms

Motor Driver

ET-OPTO DC Motor 6-24 V/10A Motor driver

Controller

ATMEGA 8 Microcontroller

Motor

Zheng DC Motor 12V 400 RPM

Cool muscle Stepping Motor 24V 3000 RPM

Minertia DC Motor 12V 2000 RPM

b1

1.ภาพรวมของระบบ (System Overview)

รูปที่ 1 แสดงภาพรวมของ Rotary Inverted Pendulum

ภาพรวมของ Rotary Inverted Pendulum จะเป็นดังภาพที 1 ซึ่งจะประกอบไปด้วยโครงสร้างที่มีฐานที่กว้างยาวมากกว่าแผ่นด้านบนพร้อมกับมีขารองรับอีก 4 จุดเพื่อกระจายการรับแรงไปที่มุมเพิ่มความมั่นคงคณะเกิดการหมุนที่รุนแรง ส่วนต่อมาก็จะเป็นในส่วนของเสาเพื่อประคองแผ่นบน ที่มีหน้าที่ยึดมอเตอร์ ส่วนต่อมาของแขนหมุน (Rotary Arm) ถูกออกแบบให้ส่วนของด้านท้ายมีความยามกว่าด้านที่ยึดกับ เสาเพนดูลัม (Pendulum Rod) ก็เพื่อช่วยถ่วงน้ำหนักของ เสาเพนดูลัม (Pendulum Rod) เพราะหากไม่มีตรงจุดนี้ก็จะเกิดโมเมนดัดที่ปลายของแกนมอเตอร์ ซึ่งอาจทำให้มอเตอร์อายุการใช้งานสั้นลง ส่วนต่อมาก็จะเป็นในส่วนของ เสาเพนดูลัม (Pendulum Rod) ก็จะมีจุดสำคัญอยู่สองจุดคือตัว เสาเพนดูลัม (Pendulum Rod) เอง และจุดยึด ในส่วนของจุดยึดเองเราได้ออกแบบโดยฝังลูกปืน(Bearing)เข้าไปก่อนเพื่อต้องการค่าความเสียดทาน (Friction) ที่เกิดขึ้นน้อยที่สุด เพื่อง่ายต่อการหมุนขึ้นของ เสาเพนดูลัม (Pendulum Rod) ส่วนสุดท้ายก็คือส่วน ของ เสาเพนดูลัม (Pendulum Rod) การออกแบบต้องคำนึงถึงคือน้ำหนักของตัว เสาเพนดูลัม (Pendulum Rod) และความยาว ซึ่งหากน้ำหนักและความยาวมากเกินไปก็จะส่งผลกับขนาดของมอเตอร์ที่ต้องการตอบสนองที่รวดเร็ว เพื่อให้สามารถเลี้ยง เสาเพนดูลัม (Pendulum Rod) ให้อยู่ในแนวตั้งได้