humanoid_Tonkla_รามคำแหง

ท่าเดินเก่าเทียบกับท่าเดินใหม่

เก่า

ใหม่

ทดสอบกับบอลสีส้ม ล่าสุดทำได้แค่นี้ รอ PC104 อยู่

ท่าเดินเก่าดูดีกว่าของใหม่แฮะ :~~

 Klaman Filter (ใส่ลงบนตัว Kookai จบละ)

พร้อมด้วยใส่สมการควบคุมแบบ PID ลงบนตัวหุ่นยนต์ โดยใช้ในการควบคุมมอเตอร์ 4 ตัว ซึ่งควมคุมการเคลื่อนที่ของหุ่นยนต์ในระนาบหน้า-หลัง  คือ มอเตอร์ที่สะโพก 2 ตัว ซ้าย 1 ตัว และขวา 1 ตัว ส่วนมอเตอร์ที่ข้อเท้า 2 ตัวก็เช่นเดียวกัน  ซ้าย 1 ตัวและขวา 1 ตัว ทั้งหมดนี้ คือ ระบบใหม่ที่จะนำมาทดลองใช้ในตัวหุ่นยนต์

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

การทดลองที่2  ระบบใหม่ คือ ใช้ค่ามุมจาก

 ทดลองเดินบนผิวไม่เรียบบ้าง เป็นยังไง

 ทดลองเดินเร็วสลับกับเดินช้าเพื่อนำผลที่ได้มาทดลอง Plot graph 

 
กราฟแสดงค่ามุมที่ไจโรอ่านได้เส้นสีแดง ค่ามุมที่
Accel. อ่านได้เส้นสีน้ำเงิน และมุมที่ปรับแก้ค่าแล้วผ่าน Kalman Filter เส้นสีเขียว
 

ทดลองเดินขึ้นลงพื้นเอียง 5 องศา โดยใช้ระบบใหม่   ค่า gain ของ Klaman Filter ดังนี้  Q11=Q22 = 0.001 , R = 1 และค่า gain ของ PID ดังนี้ Kp = 1 , Kd=2 , Ki=0.01  เซนเตอร์เป็น -5 ปรับให้ค่าเริ่มต้นก้มตัวมาข้างหน้า

ทดลองเดินขึ้นลงพื้นเอียง 10 องศา โดยใช้ระบบใหม่   ค่า gain ของ Klaman Filter ดังนี้  Q11=Q22 = 0.001 , R = 1 และค่า gain ของ PID ดังนี้ Kp = 1 , Kd=2 , Ki=0.01  เซนเตอร์เป็น -5 ปรับให้ค่าเริ่มต้นก้มตัวมาข้างหน้า

Kalman Filter (Microcontroller)
จากที่นั่งงงทดลองเขียนโปรแกรมใน Microcontroller แล้วนำค่ามุมที่ได้ไป plot graph เปรียบเทียบกับมุมที่คำนวณจากโปรแกรม Matlab ไม่ยอมตรงกันสักที  เป็นเพราะความเข้าใจผิดในเรื่องตัวแปรนั่นเอง แต่ตอนนี้แก้ไขแล้วผลที่ได้ออกมาจึงตรงกันเหมือนรูปที่ 1 ด้านล่าง เส้นสีแดงเป็นค่ามุมที่ได้จากการคำนวณโดยใช้โปรแกรม Matlab ส่วนเส้นสีน้ำเงินเป็นมุมที่ได้จากการเขียนโปรแกรมคำนวณใน Microcontroller  

รูปที่ 1

 จากนั้นทดลองนำค่ามุมที่ได้รับการปรับปรุงโดย Kalman Filter แล้วมาทดลองใช้ในการควบคุมการทรงตัวของรถ 2 ล้อ ตอนนี้ผลที่ได้ยังไม่ค่อยพอใจ แต่ก็สามารถจะรักษาสมดุลได้บ้างแล้วตาม Clip VDO จ้า

ต่อเนื่องจากครั้งที่แล้ว  ทำให้เราได้ค่าจาก gyro และ accel. ที่น่าจะถูกต้องแล้วขั้นตอนต่อไป คือ ทดลองเขียนสมการ Kalman Filter  เป้าหมายคือต้องการเขียนสมการลงบนไมโครคอนโทรลเลอร์

ปัญหา 

1. สมการต่างๆล้วนเป็นเมตริกซ์ ซึ่งถ้าทำการคำนวณในไมโครคอนโทรลเลอร์ค่อนข้างจะยุ่งยากสักหน่อย (แต่ก็ต้องทำนะ T-T)

2. ไม่สามารถทำการ plot กราฟออกมาดูผลได้เลย  ต้องนำผลที่ได้มา plot กราฟใน excel

3. ค่าที่ได้จะถูกต้องหรือไม่ต้องหาข้อมูลจากแหล่งอื่นมาเปรียบเทียบดู

ดังนั้นจึงแก้ไขโดย

ทดลองเขียนสมการ Klaman Filter บนโปรแกรม MATLAB ก่อน ซึ่งจะสามารถแก้ปัญหาเบื้องต้นของการเขียนโปรแกรมบนไมโครคอนโทรลเลอร์ได้ทั้ง 3 ข้อเมื่อทำการทดลองบน MATLAB เรียบร้อยแล้ว ทำการหาค่า gain ที่เหมาะสมกับระบบ จากนั้นจึงเขียนสมการ Kalman ลงบนไมโครคอนโทรลเลอร์ แล้วนำผลที่ได้มาเปรียบเทียบกัน

 เมื่อรู้ว่าจะต้องทำอะไรบ้างแล้วก็เริ่มลงมือทำกันเลย

1.   เขียนสมการ Kalman Filter บน MATLABเรื่องนี้ก็ไม่ค่อยมีความรู้ ดังนั้นจึงต้องขอความช่วยเหลือไปยัง อ.แพรว และ น้องแซม จาก FIBO จึงได้โปรแกรมที่น้องเค้าเขียนเสร็จแล้วนำมาทดลองกับข้อมูลของเรา เนื่องจากกรรมวิธีนำข้อมูลของน้องแซมกับเราต่างกันนิดหน่อยจึงต้องทำการแก้ไข code โปรแกรมกันเล็กน้อย 

 

 

1 คือ sampling time เราใช้อยู่ที่  10 มิลลิวินาที
2     คือ  คำสั่งดึงเอาข้อมูลมาจากไฟล์ excel ชื่อไฟล์ว่า data_gyro_accel10_n04 มาเก็บในตัวแปรชื่อ Axis โดยตัวแปรที่จะนำมาใช้ได้ต้องจัดรูปให้เป็นแบบนี้ก่อนนะ

 

 

หลัก A คือค่าดิบ Gyro หลัก B คือ ค่ามุมสะสมของ Accel. หน่วยเป็นองศาแล้ว และหลัก C คือ ค่าที่ได้จากคำนวณสมการ Kalman ในไมโครคอนโทรลเลอร์ 

3    คือ  ค่ากลางของ Gyro ซึ่งเอามาจากค่าดิบที่อ่านได้จาก ADC ของ ARM7
4    ค่าคงที่สำหรับใช้ในการแปลงค่าดิบที่อ่านได้จาก ADC ของ ARM7 มาเป็นมุมที่มีหน่วยเป็นองศา
5 เป็นสมการแปลงค่าดิบจาก ADC ของ ARM7 เป็นความเร็วเชิงมุมมีหน่วยเป็น deg/s โดยการนำค่าที่อ่านได้ลบด้วยค่ากลางคูณด้วยค่าคงที่ ปล. Axis(i,1) คือ การเรียกตัวแปร Axis แถวที่ i หลักที่ 1
6     เป็นการดึงเอาค่ามุมสะสมของ Accel. ในตัวแปร Axis แถวที่ i หลักที่ 2  มาเก็บไว้ในตัวแปร Z_k

 เริ่มต้นทำการทดลองกันเลย

1.ทำการทดลองเหมือนกับใน clip vdo อะนะ สั่งให้มอเตอร์หมุนไปมาที่มุม 0 กับ 90 องศาหมุนกลับไปมา 5 รอบ พร้อมกับเก็บข้อมูลของ Gyro กับ Accel.จากนั้นนำผลที่ได้มาจัดรูปเก็บในไฟล์excel ชื่อ data_gyro_accel4 โดยใช้ค่า   Q = [0.00000005 0; 0 0.0000005];  R = 5; เหมือนของแซมก่อน ผลที่ได้เป็นแบบนี้นะ

 

 

 รูปที่ 1

กราฟแสดงผลเปรียบเทียบระหว่างค่ามุมสะสมของ Gyro เส้นสีน้ำเงินกับค่ามุมสะสมของ Accel. เส้นสีแดง ซึ่งทั้งสองเป็นค่าที่ยังไม่ผ่านการแก้ไขโดยใช้สมการ Kalman Filter จากกราฟจะเห็นว่าค่าของ Accel. ค่อนข้างจะเที่ยงตรงกว่า gyro เพราะค่าของ gyro จะยิ่งเลื่อนขึ้นเรื่อยๆยิ่งทำซ้ำมากขึ้นก็ยิ่งเลื่อนขึ้นเรื่อยๆ แต่นี่เฉพาะกรณีที่ไม่มีการเคลื่อนนะจ๊ะ หมุนอยู่กับที่

รูปที่ 2
กราฟแสดงผลเปรียบเทียบระหว่างค่ามุมสะสมของ Gyro เส้นสีน้ำเงินกับค่ามุมสะสมของ Accel. เส้นสีแดง ซึ่งทั้งสองเป็นค่าที่ผ่านการแก้ไขโดยใช้สมการ Kalman Filter แล้ว จะเห็นว่าค่ามุมสะสมของ Gyro ไม่เลื่อนขึ้นแล้วถูกดึงกลับมีค่าใกล้เคียงกับ Accel.

 

รูปที่ 3
กราฟแสดผลการเปรียบเทียบระหว่างกราฟรูปที่ 1 กับรูปที่ 2

 

 

2.  อืม กราฟที่ได้น่าหน้าดีแฮะ แต่ก็ยังไม่เข้าใจว่าค่า Q กับ R มีผลอย่างไรกับระบบ เพราะค่าที่ใช้นี้ก็ทดลองตามที่แซมใช้ เลยต้องทำการทดลองเพิ่มเพื่อความเข้าใจเกี่ยวกับตัวแปร Q กับ R จากการทดลองเปลี่ยนค่า Q และ R ไปเรื่อยๆแล้วได้ข้อสรุปมา 2 ข้อ โดยกำหนดให้ Q11 = Q21 ใช้ข้อมูลเดิม คือ ไฟล์ data_gyro_accel4

     2.1   ถ้าอัตราส่วนระหว่างค่า Q/R มีค่ามากกราฟที่ได้จะออกจะมีแนวโน้มไปทาง Accel.

            ถ้าอัตราส่วนระหว่างค่า Q/R มีค่าน้อยกราฟที่ได้จะออกจะมีแนวโน้มไปทาง Gyro 
คำว่ามากหรือน้อยเนี่ยจะไม่รู้จะอ้างอิงยังไงดี เอาเป็นว่าดูจากผลการทดลองเอาก่อนละกันแล้วค่อยสรุปอีกที กราฟที่นำมาแสดงจะมี 2 กราฟเปรียบเทียบกันโดย

กราฟด้านบน คือ ข้อมูลที่ยังไม่ผ่านสมการ

Kalman จะเป็นกราฟที่มี 2 เส้นสีแดงคือค่ามุมสะสมของ Accel. หน่วยเป็นองศา เส้นสีน้ำเงินเป็นค่ามุมสะสมของ Gyro หน่วยเป็นองศาเช่นเดียวกัน
กราฟด้านล่าง คือ ข้อมูลผ่านสมการ Kalman แล้วเป็นกราฟค่ามุมสะสมของ Gyro หน่วยเป็น องศา   มึนเหมือนกันครับ 

เริ่มจาก   Q/R   =   1    ค่านี้ถือว่ามากแล้ว…  ใช้ค่า   Q  = [1 0; 0 1];  R = 1;   ซึ่ง Q กับ R อาจเป็นเท่าไหร่ก็ได้ขอให้จับหารกันแล้วได้  1  กราฟที่ได้เป็นแบบนี้จ้า

รูปที่ 4  

 สังเกตุดูจะพบว่ากราฟด้านล่างมีหน้าตาละไม้คล้ายกับกราฟด้านบนเส้นสีแดงมากมายซึ่งเป็นกราฟของ Accel. นั่นเอง

Q/R   =   0.01    
ใช้ค่า Q  = [1 0; 0 1];  R = 100;  กราฟที่ได้เป็นแบบนี้จ้า 

 
 

รูปที่ 5

สังเกตุดูจะพบว่ากราฟด้านล่างยังมีหน้าตาละไม้คล้ายกับกราฟด้านบนเส้นสีแดงมากมายซึ่งเป็นกราฟของ Accel. อ๊ะ อ๊ะ แต่สังเกตุดูดีๆ เมื่อเทียบดูระหว่างกราฟด้นล่างเส้นสีน้ำเงินของกราฟรูปที่ 4 กับรูปที่ 5 พบว่า ที่ยอดของกราฟที่ 5 ดูน้อยลงกว่ารูปที่ 4 นะว่าไหม

 
Q/R   =   0.0001    
ใช้ค่า Q  = [0.001 0; 0 0.001];  R = 10;   กราฟที่ได้เป็นแบบนี้จ้า  

 

รูปที่ 6
กราฟรูปนี้มีค่าติดลบด้วยล่ะ แต่ก็เริ่มแตกต่าง

จากกราฟของ Accel. มากขึ้นแล้วนะ 

 

Q/R   =   0.000001    
ใช้ค่า Q  = [0.00001 0; 0 0.00001];  R = 10;   กราฟที่ได้เป็นแบบนี้จ้า

รูปที่ 7
กราฟด้านล่างเริ่มจะมีเค้าโครงเหมือนกราฟด้านบนสีน้ำเงินมั่งแล้วสินะ

Q/R   =   0.00000001    
ใช้ค่า  Q  = [0.00001 0; 0 0.00001];  R = 1000;  
กราฟที่ได้เป็นแบบนี้จ้า

รูปที่ 8

กราฟล่างนี้เห็นชัดเลยคร๊าบบ… เริ่มต้นจะเลื่อนขึ้นเหมือนกราฟด้านบนสีน้ำเงินเห็นไหมละ เป็นเหมือนที่บอกใช่มะQ/R   =   0.0000000001    
ใช้ค่า      Q  = [0.0000001 0; 0 0.0000001];  R = 1000;  กราฟที่ได้เป็นแบบนี้จ้า

 

รูปที่ 9กราฟนี้เริ่มจะเลื่อนขึ้นจนฉุดจะไม่อยู่แล้วจร้า……

 

Q/R   =   0.000000000001    
ใช้ค่า  Q  = [0.000000001 0; 0 0.000000001];  R = 1000;   กราฟที่ได้เป็นแบบนี้จ้า

 

 

 

 

รูปที่ 10
กราฟที่ได้จากสมการ Kalman ด้านล่างเหมือนกราฟสีน้ำเงินด้านบนมากมายเลย

สรุปผลที่ได้ก็อย่างที่บอกด้านบนถ้าค่า Q/R มากแสดงว่าเชื่อผลของ Accel. มาก แต่ถ้าค่า Q/R น้อย แสดงว่าเชื่อ Gyro มาก ส่วนค่ามากแค่ไหนน้อยแค่ไหนดูเอาจากการทดลองนะ ไม่รู้จะบอกยังไงดี … 

 

 

 

     2.2  ไม่ว่าค่า Q กับ R จะมีค่าเป็นเท่าไหร่ถ้า Q/R แล้วเท่ากัน กราฟที่ได้จะออกมาเหมือนกันเลยคร๊าบบ งงไหม ไปดูผลจากการทดลองกันเผื่อจะเข้าใจ 

เริ่มต้นที่  Q/R = 1
ใช้ค่า Q = [100 0; 0 100];   R = 100; 

รูปที่ 11

Q/R = 1
ใช้ค่า Q = [1000 0; 0 1000];   R = 1000;

    

 รูปที่ 12
เหมือนกับกราฟรูปที่ 11 เลยเน้อ

Q/R = 0.001
ใช้ค่า Q = [1 0; 0 1];   R = 1000;

 รูปที่ 13

 Q/R = 0.001
ใช้ค่า Q = [0.001 0; 0 0.001];   R = 1;

 รูปที่ 14
เปรียบเทียบกับกราฟรูปที่ 13 แล้วเหมือนกันเลยเน้อ

สรุปว่าไม่ว่าค่า R หรือ Q จะเป็นเท่าไหร่ถ้า Q/R แล้วมีค่าเท่ากันกราฟที่ได้จะหน้าตาเหมือนกันคร๊าบ

ตอนนี้เราน่าจะมีข้อมูลพอที่จะไปทดลองเขียนโปรแกรมลงไมโครคอนโทรลเลอร์แล้วละมั้ง

เริ่มต้นเขียนโปรแกรมสมการ Kalman ลงบนไมโครคอนโทรลเลอร์ โอ้ย โอ้ย งงจังเลย

 

 

 

 

Kalman Filter

วัตถุประสงค์ในการใช้ Kalman Filter
-    เพื่อเพิ่มความถูกต้องในการควบคุมระบบที่มีเซนเซอร์มากกว่า 1 ตัว โดยการเลือกเชื่อค่าที่วัดได้จากเซนเซอร์ตัวไหน ณ เวลาใด ซึ่งสมการ Kalman Filter สามารถบอกได้

ขั้นตอนในการใช้สมการ
1.   ทำความเข้าใจกับระบบที่ต้องการควบคุมก่อนว่าเราต้องการปรับปรุงค่าจากเซนเซอร์ตัวไหน ซึ่งกรณีนี้ เซนเซอร์ที่นำมากับการปรับปรุงค่ามี 2 ชนิด คือ gyro sensor และ accelerometer โดยหลักๆต้องการปรับปรุงค่าที่ได้จาก Gyro
2.   ทำการ celibate เซนเซอร์ทั้งสองเพื่อให้ได้ค่าที่ถูกต้อง ก่อนนำมาใช้ในสมการ Kalman
3.   นำค่าที่ได้มาคำนวณตามสมการทั้ง 5 จะสามารถปรับปรุงค่าที่ได้จากเซนเซอร์ทั้งสองซึ่งมีความถูกต้องมากขึ้น โดยสมการทั้ง 5 มีดังนี้

ขออธิบายด้วยการยกกรณีศึกษาของตัวเองบนหุ่นยนต์ 2 ขา จากที่ได้ไปปรึกษากับ อ.แพรวมาได้ความดังนี้

สมการที่ 1  เป็นการหาค่าที่ได้จาก Gyro sensor
สมการที่ 2  ชาวบ้านเค้าเรียก “State prediction covariance” แปลตรงตัวก็เป็นสมการในการทำนายค่า Covariance ของระบบซึ่งในสมการจะเป็นตัว Q_k
สมการที่ 3 ชาวบ้านเค้าเรียก “Measurement prediction covariance” แปลตรงตัวก็เป็นสมการในการทำนายค่า Covariance ของการวัดซึ่งในสมการจะเป็นตัว  R_k
สมการที่ 4  “Filter gain”   น่าจะเป็นสมการในการเลือกว่าจะเชื่อค่าที่ได้จากเซนเซอร์ตัวไหน (ไม่แน่ใจเหมือนกัน 555)
สมการที่ 5 “Update state covariance”   ใช้เพื่ออัฟเดท ค่าในการสมการที่ 2

จะเห็นได้ว่าแต่ละสมการมีความสัมพันธ์กันดังนี้

ภาพนี้ขอยืมมาจาก

แต่ตอนนี้เพิ่งเริ่มทำการทดลองยังอยู่ที่ขั้นตอน Celibate เซนเซอร์อยู่เลยคร๊าบเลยจะนำวิธีการมาเขียนเอาไว้ให้ตัวเองดูกันลืม ของแบ่งเป็น 2 ส่วน
ส่วนที่ 1  Gyro sensor  รุ่นที่ใช้คือ ADXRS150 ข้อมูลใน data sheet ที่ต้องใช้มีดังนี้

 เอาไปใช้ยังไงละเนี่ย?  เริ่มจากเราอ่านสัญญาณจากเซนเซอร์ผ่านไมโครคอนโทรลเลอร์ตระกูล ARM7 ซึ่ง ADC ของ ARM มีความละเอียด 10 บิท เท่ากับ 1024 เป้าหมายคือเราต้องการค่าของ Gyro sensor ที่อ่านออกมาได้เป็นมุมมีหน่วยเป็น องศา ดังนั้นที่ที่อ่านได้จาก ADC จึงต้องทำการแปลงหน่วยดังนี้

1.   แปลงค่าดิบจาก ADC เป็น mv ก่อน  ตรงนี้ต้องทำการทดลองโดยอ่านค่าจาก Potentiometer ที่ค่าแรงดันไฟต่างๆ ซึ่งวัดโดยการใช้ meter พร้อมทั้งจากค่าผ่าน ADC ของไมโครคอนโทรลเลอร์เพื่อจะหาค่าที่ใช้ในการแปลงหน่วยของค่าที่ได้จาก ADC เป็น mv โดยมีค่าที่ได้จากการทดลองดังนี้

จากนั้นนำค่าที่ได้มาเปรียบเทียบดังนี้
ถ้าค่า ADC อ่านได้ 910 = อ่านแรงดันไฟได้ 5 V
ถ้าค่า ADC อ่านได้ 1 = อ่านแรงดันไฟได้ 5/910 = 5.495 mV
ได้แล้วค่าที่ต้องการแล้ว   5.495  mV 

2.   แปลงค่าจาก mV ที่ได้ไปเป็นหน่วย deg/s โดยดูค่าจาก data sheetด้านบน ทำให้รู้ว่า แรงดันไฟ 12.5 mV  เท่ากับ  1  deg/s นั่นเอง
3.   แปลงค่าจากหน่วย deg/s เป็น deg ทำได้โดย คูณค่า sampling time เข้าไป ซึ่งตอนนี้เท่ากับ  10 มิลลิวินาที
4.   พร้อมจะไปเข้าสมการที่ 1 แล้ว แต่ว่าค่าที่ได้จากการแปลงหน่วยทั้งหมดนี้จะถูกต้องไหมล่ะ ต้องนำผลที่ได้มาทดลองโดยแปะเซนเซอร์ติดไว้กับก้านแข็งหนึ่งอันซึ่งต่อกับ RC servo motor ที่สามารถควบคุมตำแหน่งการหมุนได้ จากนั้นทำการทดลอง 2 การทดลอง คือ
4.1 ทดลองหาค่า 0 ของ gyro โดยการอ่านค่าจาก gyro ขณะอยู่นิ่ง 1000 ค่า จากนั้นนำมาหาค่าเฉลี่ยเพื่อให้ได้ค่า 0 ของ gyro                                  
4.2 ทดลองทำการหมุนที่ตำแหน่ง 0-90 องศา หมุนกลับไปมาทดลองซ้ำๆ 20 รอบ ดังวีดีโอ จากนั้นมา plot graph ดูกันได้ผลดังนี้ 

จากนั้นมา plot graph โดยค่า gyro ที่ได้มาจากผลรวมของมุมที่สะสมมาเรื่อยๆกับมุมปัจจุบันตามสมการที่ 1 ของ Kalman Filterได้ผลดังนี้

 ส่วนที่ 2  Accelerometer   รุ่นที่ใช้คือ Memsic 2125  ส่วนที่ต้องใช้ใน data sheet คือ

 พอได้กราฟจากเซนเซอร์ทั้ง 2 แล้วนำมา plot เปรียบเทียบกันดูเพราะตอนทำการทดลองนั้นติดเซนเซอร์ทั้งสองไว้บนก้านที่ต่อกับมอเตอร์อันเดียวกันหมุนไปพร้อมๆกัน เพียงแต่ด้านบนเป็นการแยกกันคำนวณ แยกกัน plot กราฟเท่านั้น เมื่อนำมา plot เปรียบเทียบกันพบว่า เส้นกราฟของ gyro จะมีการเลื่อนขึ้นไปเรื่อยๆ ยิ่งทำการทดลองมากก็ยิ่งจะเลื่อนไปมากขึ้น ขณะที่เส้นกราฟของ accel. ค่อนข้างจะสม่ำเสมอ  ดังรูปกราฟนี้

วันนี้เพิ่งทดลองไปได้แค่นี้แล้วจะมาเขียนต่อละกันคร๊าบ

                       
เนื่องจากเพิ่งเริ่มต้นทำจึงยังไม่ค่อยเข้าใจอย่างลึกซึ้งอาจมีบางอย่างผิดพลาดบ้าง ถ้ามีผู้ใดพบเห็นข้อผิดพลาดช่วยบอกกันด้วยคร๊าบบ

เทคนิคการสร้างท่าเดินคร๊าบบบ…

ในการจัดท่าทางการเดินของหุ่นยนต์ต้องทำการกำหนดท่าย่อยของท่าหลักแต่ละท่าขึ้นมาก่อน เช่น ต้องการสร้างท่าก้าวขาขวา ต้องกำหนดท่าย่อยดังนี้  1. เอียงตัวไปทางซ้าย  2. ยกขาขวา  3. ก้าวขาขวา  4. วางขาขวา  เราสามารถหาตำแหน่งของมอเตอร์เพื่อให้สร้างท่าทางเหล่านี้ได้   โดยใช้การกำหนดตำแหน่งที่ปลายเท้าทั้ง 2 ข้างของหุ่นยนต์ จากนั้นนำไปคำนวณโดยใช้สมการ Inverse Kinematic เราก็จะสามารถรู้ถึงตำแหน่งในการหมุนของมอเตอร์แต่ละตัวเพื่อให้เกิดเป็นท่าทางตามที่กำหนดไว้ได้    ในหุ่นยนต์ประเภททที่มีมอเตอร์หลายตัวต้องทำงานสัมพันธ์กันเพื่อให้เกิดท่าทางนั้น การสั่งงานมอเตอร์ให้เคลื่อนที่จากตำแหน่งหนึ่งไปยังอีกตำแหน่งหนึ่งนั้นไม่ควรสั่งเพียงครั้งเดียว เช่น จากรูปที่ 1  ต้องการสั่งให้มอเตอร์เคลื่อนที่จากตำแหน่ง x ไปยังตำแหน่ง y ไม่ควรสั่งให้มอเตอร์หมุนครั้งเดียวแล้วเคลื่อนจาก x ไป y เลยดังรูป A   ควรมีการสร้าง trajectory ขึ้นระหว่างทางเพื่อให้เราสามารถกำหนดเวลาในการเคลื่อนที่ของมอเตอร์ทุกตัวให้สิ้นสุดพร้อมกันได้ ดังรูป B   สามารยกตัวอย่างให้เห็นภาพมากขึ้นได้ เช่น  ถ้าหุ่นย่อตัวอยู่ สั่งให้ยืดตัวขึ้นไปตรงๆ   กรณีที่สั่งงานมอเตอร์แบบ A จะทำให้หุ่นยนต์พุ่งล้มไปข้างหน้า  เนื่องจากในการจะเปลี่ยนท่าของหุ่นยนยต์จากท่าย่อเป็นยืดตัวนั้น ต้องมีมอเตอร์ที่ทำงานพร้อมกันอยู่ 6 ตัว คือมอเตอร์ที่สะโพก 2 ตัว หัวเข่า 2 ตัว และ ข้อเท้า 2 ตัว โดยตำแหน่งในการหมุนของมอเตอร์ที่สะโพกและข้อเท้าจะหมุนไปเป็นมุมที่เท่ากัน แต่มอเตอร์ที่หัวเข่าจะต้องหุมนไปเป็น 2 เท่าของมุมที่สะโพก ทำให้เวลาที่ใช้ในการหมุนของมอเตอร์ไม่เท่ากัน มีผลทำให้หุ่นยนต์ล้มไปด้านหน้า  แต่ถ้าเปลี่ยนไปสั่งงานแบบ B จะทำให้มอเตอร์ทุทตัวทำงานเสร็จพร้อมกัน ผลคือหุ่นยนต์จะค่อยๆ ยืดตัวขึ้นทีละน้อยจนถึงตำแหน่งที่กำหนดไว้นั่นเอง

รูปที่ 1 แสดงตำแหน่งในการเคลื่อนที่ของหุ่นยนต์

 ในการสั่งงานของหุ่นยนต์กุ๊กไก่เวลาที่ใช้ในการส่งคำสั่งไปยังมอเตอร์ คือทุกๆ 10 ms คือ 100 ครั้งต่อนาที เนื่องจากการส่งคำสั่งแต่ละคำสั่งต่อมอเตอร์ 1 ตัว ใช้ข้อมูลประมาณ 11 byte มอเตอร์ 20 ตัวก็ 220 byte คือประมาณ 2200 bit/10 ms ดังนั้นคำนวน Baud rate ต่ำสุดที่จะเลือกใช้  คือ

แต่ในที่นี้เลือกบอดเรตที่ 1 M เพราะต้องเพื่อช่วงการคำนวน  Inverse Kinematic ด้วย

 2200 x 100 = 220,000   bit/s

ท่าเดินสามารถแบ่งได้เป็น 2 แบบ คือ

1.ท่าเดินแบบ  Static  (เดินโดย จุดรวมมวลหรือ CM ไม่หลุดจากฝ่าเท้า)

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

1. เอียงตัวถ่ายน้ำหนักไปไว้ที่ขาข้างหนึ่ง

2. ยกขาด้านที่ไม่มีน้ำหนัก

3. ก้าวขาข้างที่ยกไปในระยะที่ต้องการ

4. วางขาข้างที่ก้าวลงกับพื้น

5. โยกถ่ายน้ำหนักจากขาข้างหนึ่งไปยังอีกข้างหนึ่ง ถ้าใช้การจัดท่าจะทำยาก ถ้าใช้การ

    คำนวณด้วย Inverse Kinematic จะทำได้ง่ายมาก  

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

2.การเดินแบบ Dynamic

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

1. ยกขาขึ้น

2. วางขาพร้อมก้าวลงกับพื้น

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

แผนภาพแสดงประวัติความไปมาก่อนจะมาเป็น กุ๊กไก่ กะ กระต๊าก จ้า

รูปแสดงตำแหน่งอ้างอิงในการเคลื่อนที่ของหุ่นยนต์ โดย
แกน x คือ แกนที่หุ่นยนต์เคลื่อนที่แบบย่อ-ยืดตัว
แกน y คือ แกนที่หุ่นยนต์เคลื่อนที่แบบเอียงตัวซ้าย-ขวา
แกน z คือ แกนที่หุ่นยนต์เคลื่อนที่แบบก้าวขาไปด้านหน้า-หลัง

หน่วยประมวลผลที่ใช้ในระบบมี 2 ตัว ซึ่งไมโครคอนโทรลเลอร์ที่ใช้เป็นตระกูล Phillips รุ่น ARM7 [60MHz]
หน่วยประมวลผล 1  ทำหน้าที่ควบคุมการทำงานของมอเตอร์ทั้งหมด
หน่วยประมวลผล 2  ทำหน้าที่ประมวลผลภาพและทำการตัดสินใจ

ข้อมูลพื้นฐานของ กุ๊กไก่ และ กระต๊าก

SERVO ที่ใช้ Robotis DX-117

ครั้งแรกกับการแข่งขันหุ่นยนต์สองขาในประเทศไทย หรือ Thailand Humanoid Robot Soccer Championship 2009

หุ่นที่ใช้ในการแข่งขันครั้งนี้มีสองตัว ตัวแรกชื่อกุ๊กไก่01 ตัวที่สอง ชื่อ กระต๊าก