บูรณาการระบบอัตโนมัติ

ทำไมต้องรู้

ตัวควบคุมการบินเก่งเรื่องรักษาท่าทางและตำแหน่งหลายร้อยครั้งต่อวินาที แต่ไม่มีกำลังพอจะรันโมเดล AI หรือสร้างแผนที่ งานเหล่านั้นจึงย้ายไปอยู่บน คอมพิวเตอร์บนลำ (companion computer) เช่น บอร์ด Jetson หรือ Raspberry Pi ที่คุยกับตัวควบคุมการบินตลอดเวลา เหมือนนักบินผู้ช่วยที่บอกทิศทางให้นักบินหลักซึ่งยังถือคันบังคับอยู่ ถ้าสองฝั่งเข้าใจทิศทางไม่ตรงกัน หรือผู้ช่วยเงียบไปกลางอากาศ ผลอาจร้ายแรง โมดูลนี้ฝึกเชื่อมทั้งสองฝั่งให้ถูกและปลอดภัย

ทั้งวิชาใช้กรณีสมมติเดียว: โดรนสำรวจอัตโนมัติที่มีคอมพิวเตอร์บนลำ ตรวจจับสิ่งกีดขวางด้วยกล้องและตัววัดระยะ และทดสอบจาก SITL ไปจนถึงภาคสนาม โค้ด Python ของทุกโมดูลดาวน์โหลดได้ที่ /downloads/uat-322/

แบ่งหน้าที่ให้ชัด

• ตัวควบคุมการบิน ประมาณสถานะด้วย EKF ควบคุมตำแหน่งและท่าทาง และ ถือสิทธิ์ failsafe ไว้เสมอ ไม่ว่าคอมพิวเตอร์บนลำจะสั่งอะไร
• คอมพิวเตอร์บนลำ รับรู้สิ่งแวดล้อม วางแผนเส้นทาง และส่ง setpoint (ตำแหน่ง ความเร็ว หรือมุมหันที่ต้องการ) ให้ตัวควบคุมการบิน
• สถานีภาคพื้น ใช้ตั้งค่า เฝ้าดู และสั่งยกเลิกภารกิจ

ช่องทางเชื่อมต่อมีสองแบบหลัก

ArduPilot ก็มีอินเทอร์เฟซ ROS 2 ในตัว (AP_DDS) เช่นกัน เอกสาร PX4 แนะนำ ROS 2 Jazzy บน Ubuntu 24.04 ส่วนไลบรารี MAVSDK สำหรับ Python ต้องระวังว่าแพ็กเกจ mavsdk รุ่น 4 เป็น API ใหม่ที่ต่างจากตัวอย่างรุ่นเก่า ตัวอย่างเดิมที่ใช้ mavsdk_server ย้ายไปเป็นแพ็กเกจ mavsdk-grpc จึงต้องตรึงรุ่นให้ตรงกับเอกสารที่อ่าน

ทิศทางต้องตรงกัน

ROS ใช้ ENU (x ไปตะวันออก y ไปเหนือ z ขึ้น) ตามมาตรฐาน REP 103 แต่ PX4 ใช้ NED (x ไปเหนือ y ไปตะวันออก z ลง) ถ้าส่งตำแหน่งจาก ROS ไป PX4 โดยไม่แปลง คำสั่ง “ขึ้นไป 3 เมตร” จะกลายเป็น “ลงไป 3 เมตร”

import math


def enu_to_ned(east, north, up):
    return north, east, -up


def yaw_enu_to_ned(yaw_enu):
    yaw = math.pi / 2 - yaw_enu
    return math.atan2(math.sin(yaw), math.cos(yaw))   # จัดให้อยู่ในช่วง −π ถึง π


print("target NED:", enu_to_ned(10.0, 5.0, 3.0))
for deg in (0, 90, 180):
    print(f"yaw ENU {deg:>3}° -> NED {math.degrees(yaw_enu_to_ned(math.radians(deg))):6.1f}°")

target NED: (5.0, 10.0, -3.0)
yaw ENU   0° -> NED   90.0°
yaw ENU  90° -> NED    0.0°
yaw ENU 180° -> NED  -90.0°

เมื่อคอมพิวเตอร์บนลำเงียบ

ในโหมด Offboard ของ PX4 คอมพิวเตอร์บนลำต้องส่งสัญญาณยืนยันว่ายังทำงานอยู่ต่อเนื่องอย่างน้อย 2 Hz และต้องเริ่มส่ง setpoint ก่อน สลับเข้าโหมด ถ้าสัญญาณขาดนานกว่าพารามิเตอร์ COM_OF_LOSS_T (ค่าเริ่มต้น 1.0 วินาที) PX4 จะออกจาก Offboard ไปทำตามที่ตั้งใน COM_OBL_RC_ACT (ค่าเริ่มต้นคือโหมด Position) ส่วน ArduPilot ในโหมด Guided แนะนำให้ส่งคำสั่งความเร็วซ้ำทุกวินาที และโดรนจะหยุดถ้าไม่ได้รับคำสั่งนาน 3 วินาที

stamps = [round(0.1 * k, 1) for k in range(31)] + [round(4.5 + 0.1 * k, 1) for k in range(11)]
LOSS_T = 1.0            # s เท่ากับค่าเริ่มต้นของ COM_OF_LOSS_T
print("setpoints in the first second:", sum(s < 1.0 for s in stamps))

last, failsafe, i, t = None, None, 0, 0.0
while t <= 5.5:
    while i < len(stamps) and stamps[i] <= t + 1e-9:
        last = stamps[i]
        i += 1
    if last is not None and failsafe is None and t - last > LOSS_T:
        failsafe = t
    t = round(t + 0.05, 2)
print(f"failsafe at {failsafe} s (stream stopped after 3.0 s)")

setpoints in the first second: 10
failsafe at 4.05 s (stream stopped after 3.0 s)

งบเวลาหน่วง

ทุกขั้นในเส้นทางตั้งแต่กล้องจนถึงตัวควบคุมการบินใช้เวลา ผลรวมคือ เวลาหน่วง (latency) ระหว่างนั้นโดรนยังเคลื่อนที่ต่อ

stages = {"camera frame": 33, "detector": 45, "planner": 12, "DDS link": 4, "FC control": 3}   # ms
total = sum(stages.values())
speed = 5.0   # m/s
print(f"total latency {total} ms; at {speed} m/s the drone moves {speed * total / 1000:.2f} m before reacting")

total latency 97 ms; at 5.0 m/s the drone moves 0.48 m before reacting

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

ปฏิบัติการประจำโมดูล

ข้อผิดพลาดที่พบบ่อย

สรุป

• ตัวควบคุมการบินรักษาเสถียรภาพและถือสิทธิ์ failsafe ส่วนคอมพิวเตอร์บนลำรับรู้ วางแผน และส่ง setpoint
• PX4 เชื่อม ROS 2 ผ่าน uXRCE-DDS และทั้ง PX4 กับ ArduPilot รองรับ MAVLink
• ROS ใช้ ENU แต่ PX4 ใช้ NED ต้องแปลงทั้งตำแหน่งและมุมหัน
• Offboard ต้องได้สัญญาณอย่างน้อย 2 Hz และจะออกจากโหมดเมื่อขาดนานกว่า COM_OF_LOSS_T
• งบเวลาหน่วงบอกระยะที่โดรนเคลื่อนไปก่อนตอบสนอง

แบบฝึกตรวจความเข้าใจ

1. จุด ENU (east 4, north −2, up 6) เป็นพิกัด NED อะไร
2. มุมหัน ENU 45° เป็นมุมหัน NED เท่าใด
3. โหมด Offboard ของ PX4 ต้องการ setpoint อย่างน้อยกี่ Hz
4. เวลาหน่วงรวม 120 ms ที่ความเร็ว 6 m/s โดรนเคลื่อนไปกี่เมตรก่อนตอบสนอง
5. ทำไม failsafe จึงควรอยู่ที่ตัวควบคุมการบิน ไม่ใช่คอมพิวเตอร์บนลำ