การควบคุมการหยิบจับสิ่งของ ของแขนกลด้วย PID

Introduction

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

หลักการ

การหยิบจับของแขนกลต่อไปนี้จะพิจารณาเฉพาะการหยิบจับ(คีบ)สิ่งของขึ้นในแนวดิ่งเท่านั้น การหยิบจับของแขนกลมีลักษณะดังรูป



ที่หน้าสัมผัสของแขนกลจะมี sensor จับการลื่นไถลของวัตถุทำให้สามารถรู้ถึงทิศทางการลื่นไถลและความเร็วในการลื่นไถล ถ้าวัตถุยังมีการลื่นไถลอยู่แสดงว่า ยังจับวัตถุไม่อยู่จึงต้องเพิ่มแรงบีบ (N)ขึ้นอีก ดังนั้นถ้าวัตถุมีการลื่นไถลเร็วก็ต้องเพิ่มแรงบีบให้เร็วขึ้นด้วย (เช่นในกรณีแขนกลยกสิ่งของขึ้นด้วยความเร็วไม่เท่ากัน) หลักการอยู่ที่ว่าถ้าวัตถุยิ่งไถลเร็วเท่าไหร่ยิ่งเพิ่มแรงมากขึ้นเร็วตามไปด้วยจนกว่า วัตถุจะหยุดไถล(หยิบได้แล้ว) จึงหยุดแรงให้คงที่ไว้เท่านั้น ก็จะสามารถหยิบ สิ่งของได้โดยใช้แรงน้อยที่สุดกับสิ่งของนั้น

Transfer Function

จากรูปข้างบนสามารถสร้างเป็นสมการได้ดังนี้
2uN = m(g+a)
2uN = mdv/dt -----[1]

เปลี่ยนให้อยู่ใน S โดเมน
2uN(s) = mSV(s) -----[2]

ดังนั้นจะได้ transfer function เป็น
V(s) / N(s) = 2u / mS -----[3]

ให้ u = 1; m = 2 จะได้transfer function ใน open-loop ของระบบเป็น
V(s) / N(s) = 1 / S -----[4]

Open-loop step response

เมื่อได้สมการของระบบมาแล้วก็นำมาทดลองหา Step response จาก matlab โดยใส่ค่าต่อไป

num = 1;
den = [1 0];
t = 0:0.01:2;
step(num, den, t);

จะได้การฟดังรูป
จะเห็นว่า raise time ของระบบนั้นใช้เวลาประมาณ 1 วินาทีและระบบก็ไม่เข้าสู่ stable ด้วยดังนั้นยังยอมรับไม่ได้ จึงต้องเพิ่มการป้อนกลับที่เหมาะสมให้แก่ระบบ ระบบจะกลายเป็นระบบที่มีการป้อนกลับหรือที่เรียกว่า Closed-Loop system

Closed-loop with Proportional control (P)

การเพิ่มค่าคงที่ Kp เข้าไประบบ จะเป็นการขยายสัญญาณ Error ที่ย้อนกลับมา ดังนั้น rise time ของระบบจะลดลง แต่ overshoot จะเพิ่มขึ้น transfer function ของระบบใหม่เป็น
Kp / (S+Kp)
ถ้าให้ Kp = 5 และใส่คำสั่งต่อไปนี้ลงใน matlab

Kp = 5;
num = Kp;
den = [1 Kp];
t = 0:0.01:2;
step(num, den, t);

เมื่อสั่ง run จะได้กราฟดังรูป
จะเห็นว่ารูปร่างของกราฟใกล้เคียงกับกราฟ unit-step มากขึ้น แต่ raise time ของระบบยังไม่เป็นที่น่าพอใจดังนั้นอาจทำให้ raise time ของระบบสั้นลงด้วยการเติมส่วนของ Integral เข้าไป

Closed-loop with Proportional-Integral control (PI)

คราวนี้เราจะทำการเพิ่ม Integral controller ให้กับระบบบ้าง การเพิ่ม Integral นั้นจะช่วยลด raise time ของระบบแต่ Overshoot จะสูงขึ้น
Closed-loop transfer function ของระบบจะเป็น
V(s) / N(s) = S² / (S² + SKp + Ki)
ให้ Kp = 5, Ki = 25 แทนค่าลงใน matlab ดังนี้

Kp = 5;
Ki = 25;
num = [Kp Ki];
den = [1 Kp Ki];
t= 0:0.01:4;
step(num, den, t);

จะได้กราฟดังรูป
เมื่อใส่ส่วนที่เป็น Integral เข้าไป raise time ของระบบลดลงเหลือประมาณ 0.2 วินาทีเท่านั้นซึ่งถือว่าใช้ได้ แต่ Overshoot ของระบบสูงขึ้นมาถึง 1.3 และมีการแกว่งของกราฟอย่างเห็นได้ชัด ถ้าเป็นในระบบแขนกลที่ใช้งานจริงสิ่งของที่แขนกลจับอยู่อาจจะหล่นมาก็ได้ หรือถ้าเป็นไข่ไก่ก็อาจโดนบีบจนแตกก็ได้ Overshoot ที่สูงขึ้นมาถึง 30% นั้นยอมรับไม่ได้จึงต้องมการปรับปรุงระบบโดยใช้ส่วนของการ Derivative เข้ามาช่วย อีกปัญหาของระบบที่เห็นได้ชัดคือ setting time ใช้เวลานานมาก (ประมาณ 2.7 วินาที)

Closed-loop with Proportional-Integral control (PI)

คราวนี้เราจะทำการเพิ่ม Derivative controller ให้กับระบบบ้าง การเพิ่ม Derivative นั้นจะช่วยลด overshoot ของระบบแต่ raise time จะมากขึ้น
Closed-loop transfer function ของระบบจะเป็น
V(s) / N(s) = (S²Kd + SKp + Ki) / ((1+Kd)S² + SKp + Ki)
ทดลองให้ Kp = 5, Ki = 25, Kd = 2 แล้วแทนค่าลงใน matlab ดังนี้

Kp = 5;
Ki = 25;
Kd = 2;
num = [Kd Kp Ki];
den = [1+Kd Kp Ki];
t= 0:0.01:10;
step(num, den, t);

จะได้กราฟดังรูป
เมื่อเพิ่มส่วนที่เป็นDerivative เข้าไป raise time ของระบบเพิ่มขึ้นจากเดิมไม่มากและ Overshoot ก็ลดลงมาไม่มากเช่นกัน ที่สำคัญคือ setting time ของระบบใช้เวลานานมากคือประมาณ 8.5 วินาที จากระบบนี้ไม่ว่าจะเปลี่ยนแปลงค่า Kd ไปเท่าไรก็ไม่ค่อยช่วยให้กราฟของระบบใกล้เคียง unit-step เลย ในทางกลับกันเมื่อย้อนกลับไปดูกราฟของ Proportional control จะเห็นว่าเป็นกราฟที่มีรูปร่างใกล้เคียงที่สุด ดังนั้นเราจะกลับไปทดลองปรับค่า Kp ของระบบ ตอนที่ระบบมี Proportional control อย่างเดียวอีกที โดยลองปรับให้ค่า Kp เป็น 40 จะได้กราฟดังรูป