ชื่อโปรเจกต์: Ultrasound Sensor: 2D Tracking With Arduino

การวัดระยะด้วยคลื่นอัลตราซาวด์เป็นงานที่ซับซ้อน แต่ถูกทำให้ง่ายขึ้นด้วยโมดูลราคาถูกและหาซื้อได้ง่ายสำหรับ [Arduino](https://s.shopee.co.th/7fUgFAWSki) หลักการคือมันจะส่งสัญญาณไปยังเป้าหมาย แล้วเป้าหมายสะท้อนกลับมา Arduino จะวัดเวลาที่ใช้ในการเดินทางไปกลับทั้งหมด เมื่อรู้ความเร็วเสียงแล้ว ก็สามารถคำนวณระยะห่างระหว่างโมดูลกับเป้าหมายได้ง่ายๆ เลย

บทสอนนี้แบ่งเป็นสองส่วน ส่วนแรกจะอธิบายการปรับเทียบเซ็นเซอร์อัลตราซาวด์และการติดตามตำแหน่ง 2 มิติด้วยเซ็นเซอร์ 2 ตัว ส่วนที่สองจะพูดถึงการติดตามตำแหน่ง 2 มิติที่แม่นยำขึ้นด้วยเซ็นเซอร์หลายตัว

A. Ultrasound [sensor](https://s.shopee.co.th/7VBG2rX65j) calibration and 2d tracking

เซ็นเซอร์อัลตราซาวด์หรือโซนาร์ปกติใช้แค่วัดระยะห่างจากเป้าหมาย แต่ที่นี่เราจะพยายามหาตำแหน่ง/พิกัดที่แน่นอนของเป้าหมายให้ได้ เราใช้ HC-SR04 สำหรับงานนี้

ลองดูคลิปวิดีโอด้านล่างนี้เพื่อความเข้าใจที่ใกล้เคียงกัน

Explanation on 2d tracking with 2 Modules

Calibrating the Sensor:

เพื่อเช็คความแม่นยำของเซ็นเซอร์ เราเอาเป้าหมายวางไว้ข้างหน้า แล้ววัดระยะด้วยเซ็นเซอร์อัลตราซาวด์และด้วยไม้บรรทัด การวัดด้วยเซ็นเซอร์อัลตราซาวด์นั้นตรงไปตรงมา

จากกราฟด้านบนจะเห็นชัดเจนว่าผลลัพธ์จากเซ็นเซอร์เป็นเส้นตรงค่อนข้างดี ความชันก็เกือบเท่ากับหนึ่ง (ตามที่คาด) และค่าเบี่ยงเบนมาตรฐานของความคลาดเคลื่อนอยู่ที่ประมาณ 9mm ซึ่งส่วนใหญ่เป็นความคลาดเคลื่อนคงที่ เพราะเราวัดระยะจากขอบสุดของเซ็นเซอร์ ในขณะที่ Arduino วัดจากตัวส่งและตัวรับสัญญาณ

Measurement for 2D Coordinate: Concept

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

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

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

Measurement for 2D Coordinate: Calculation

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

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

รูปภาพที่แนบมาจะแสดงสูตรสำหรับคำนวณตำแหน่ง

Measurement for 2D Coordinate: Setup

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

ถ้าเป้าหมายอยู่ใกล้ เซ็นเซอร์ทั้งสองควรอยู่ใกล้กันเล็กน้อย เพื่อให้กรวยของเซ็นเซอร์ตัดกันที่ระยะใกล้

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

สามารถปรับมุมที่เซ็นเซอร์หันหน้าได้ตามต้องการ เพื่อให้กรวยของเซ็นเซอร์ตัดกัน

Woking of Code:

การทำงานของโค้ดจะเป็นไปตามขั้นตอนต่อไปนี้:

วัดระยะจากเซ็นเซอร์ 1,

วัดระยะจากเซ็นเซอร์ 2,

แก้สมการหาพิกัด

โค้ดมีให้ที่ลิงก์ด้านล่าง:

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

B. Ultrasound sensor calibration and 2d tracking

วิธีการข้างต้นยังถูกอธิบายในวิดีโอด้านล่างนี้ด้วย:

Working:

วิธีนี้ใช้เซ็นเซอร์หลายตัว ยิ่งมีเซ็นเซอร์มากเท่าไหร่ ก็ยิ่งมีประโยชน์สำหรับการวัดที่แม่นยำมากขึ้น ลองดูตัวอย่างการเซ็ตอัพด้วยเซ็นเซอร์ 4 ตัว (ตามที่ใช้ในบทสอนนี้) หลังจากวัดเสร็จ เราจะได้ระยะห่างจากเป้าหมายไปยังเซ็นเซอร์ทุกตัว

ในการคำนวณตำแหน่งของเป้าหมายในแนวราบ เราต้องการการวัดสองครั้งเพื่อสร้างสามเหลี่ยมและระบุตำแหน่งเป้าหมาย เนื่องจากเรามีเซ็นเซอร์ 4 ตัว เราจะมีคู่เซ็นเซอร์ที่เป็นไปได้ 6 ชุด (4C2) คู่เหล่านี้สามารถเห็นได้จากภาพด้านบน

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

วิธีนี้ยังไม่แม่นยำมากนัก เพราะเราไม่ได้วัดจากเป้าหมายที่เป็นจุดเดียว เนื่องจากเป้าหมายเป็นวัตถุแบบแผ่น เซ็นเซอร์จะวัดระยะจากจุดที่ใกล้ที่สุด

Preparing Setup:

การเซ็ตอัพนี้คล้ายกับบทสอนก่อนหน้าเกี่ยวกับการติดตาม 2 มิติด้วยเซ็นเซอร์สองตัว ขั้ว GND และ Vcc เชื่อมต่อกับไลน์ร่วมกัน และขา Tx และ Rx ทั้งหมดเชื่อมต่อกับขาใดขาหนึ่งของ Arduino สามารถเชื่อมต่อกับขา Arduino ใดก็ได้ที่กำหนดไว้ในโค้ด ดูภาพด้านบนสำหรับการเชื่อมต่อโดยรวม

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

Working of Code

โดยพื้นฐานแล้วมีฟังก์ชัน 4 ฟังก์ชันที่ทำงานเพื่อให้ได้ผลลัพธ์ที่แม่นยำและหลีกเลี่ยงการอ่านค่าที่ผิดพลาด

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


pulseIn(Rx[i], HIGH,5000);

ค่า 5000 ต้องเปลี่ยนตามตำแหน่งของวัตถุ เมื่อเรามีช่วงการทำงานโดยประมาณแล้ว เราสามารถคำนวณค่าเวลาที่ใช้ (ในหน่วยไมโครวินาที) โดยคลื่นเสียงในการเดินทางไปและกลับตามระยะทางนั้น และแทนที่ค่า 5000 ด้วยค่านั้น เมื่อฟังก์ชันทำงานเสร็จสิ้น เราจะได้อาร์เรย์ที่มีค่าที่วัดได้จากเซ็นเซอร์ทั้งหมด

2. Position_calc():
ฟังก์ชันนี้จะนำชุดค่าผสมต่างๆ (nC2 สำหรับเซ็นเซอร์จำนวน n ตัว) มาคำนวณตำแหน่งของเป้าหมายใน 2 มิติ

3. check();
ฟังก์ชันนี้จะตรวจสอบว่าทุกชุดค่าผสมที่นำมาโดยฟังก์ชัน position_calc() สามารถสร้างสามเหลี่ยมได้หรือไม่ ถ้าฟังก์ชันนี้สรุปว่าการทดสอบนี้เป็นลบ Position_calc() จะเพียงแค่บันทึกค่าเป็น 0 ฟังก์ชันนี้มีประโยชน์มากในการหลีกเลี่ยงการพิจารณาค่าที่ผิดพลาดจากเซ็นเซอร์ ฟังก์ชันนี้ใช้คุณสมบัติที่ว่าผลรวมของความยาวของด้านสองด้านใดๆ ของสามเหลี่ยมจะมากกว่าความยาวของด้านที่สามเสมอ

4. Fuse_data()
ฟังก์ชันด้านบนนี้จะหาค่าเฉลี่ยถ่วงน้ำหนักของการวัดทั้งหมด โดยให้น้ำหนักเป็นสัดส่วนกับระยะห่างระหว่างคู่เซ็นเซอร์

Setting Up the Code and Testing the Output:

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

เมื่อทุกอย่างเซ็ตอัพเสร็จแล้ว ก็สามารถวางเป้าหมายไว้หน้าซีเซ็นเซอร์และตรวจสอบผลลัพธ์จากเซ็นเซอร์ได้

EXPANDED TECHNICAL DETAILS

Acoustic Position Triangulation

โปรเจกต์นี้ไปไกลกว่าการตรวจจับระยะง่ายๆ ไปสู่การติดตามพิกัด (X, Y) ของวัตถุที่เคลื่อนที่ในพื้นที่ทำงาน 2 มิติ

Dual Sensor Array: ใช้ HC-SR04 ultrasonic sensors สองตัวติดตั้งห่างกันในระยะที่กำหนด โดยการวัด "Time of Flight" (TOF) จากเซ็นเซอร์ทั้งสองไปยังเป้าหมายเดียวกัน Arduino จะใช้ Law of Cosines ในการคำนวณหาตำแหน่งที่แน่นอนด้วยวิธีสามเหลี่ยม
Trigonometric Coordinate Conversion: เฟิร์มแวร์จะแปลงค่าระยะดิบสองค่า (dist1, dist2) เป็นพิกัดคาร์ทีเซียน $(X, Y)$ เทียบกับเส้นฐานของเซ็นเซอร์

Visualization

Live Plotting: ข้อมูลจะถูกส่งผ่าน Serial ไปยังสคริปต์ Processing ซึ่งจะวาดจุดเคลื่อนที่บนกริดดิจิทัล เลียนแบบการแสดงผลเรดาร์หรือโซนาร์ระดับมืออาชีพ