ออโตไพลอตและโหมดการบิน

ทำไมต้องรู้

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

โครงควบคุมแบบซ้อน

PX4 อธิบายไว้ในหน้า Controller Diagrams ว่าใช้ การควบคุมแบบซ้อน (cascaded control) มาตรฐาน

1. ตำแหน่ง ตัวควบคุม P แปลงค่าผิดพลาดตำแหน่งเป็นความเร็วที่ต้องการ (MPC_XY_P, MPC_Z_P)
2. ความเร็ว ตัวควบคุม PID แปลงเป็นความเร่งหรือแรงขับที่ต้องการ (MPC_XY_VEL_P_ACC ฯลฯ) มี anti-windup แบบ clamping
3. ท่าทาง ตัวควบคุม P แบบ quaternion แปลงเป็นอัตราหมุนที่ต้องการ (MC_ROLL_P ฯลฯ)
4. อัตราหมุน ตัวควบคุม PID พร้อมตัวกรองอนุพันธ์ แปลงเป็นคำสั่งมอเตอร์ (MC_ROLLRATE_P/I/D/K)

ArduPilot ใช้หลักเดียวกัน ตัวควบคุม P ของมุม (ใช้ square-root controller) ส่งต่อให้ PID ของอัตราหมุน พารามิเตอร์คือ ATC_ANG_RLL_P และ ATC_RAT_RLL_P/I/D พร้อมตัวกรอง ATC_RAT_RLL_FLTT, FLTE, FLTD และทำงานที่ 400 Hz บน Pixhawk

Åström และ Murray (2021) ข้อ 15.4 อธิบายว่าการควบคุมแบบซ้อนออกแบบทีละลูป เริ่มจากลูปใน ลูปในที่ตอบสนองแน่นช่วยลดผลของสิ่งรบกวนและทำให้ออกแบบลูปนอกง่ายขึ้น และเมื่อลูปในอิ่มตัว ต้องบอกให้ anti-windup ของลูปนอกรู้ด้วย

โหมดการบินกับลูปที่ทำงาน

โหมดการบินกำหนดว่านักบินหรือระบบอัตโนมัติสั่งที่ลูปใด ยิ่งอัตโนมัติมาก ลูปนอกก็ทำงานมากขึ้น

• Stabilized (PX4) / Stabilize (ArduPilot) นักบินสั่งมุมเอียงและคันเร่งโดยตรง ใช้เฉพาะลูปท่าทางและอัตราหมุน
• Altitude / Alt Hold เพิ่มลูปความสูง คันเร่งกลางคือคงความสูง
• Position / Loiter เพิ่มลูปตำแหน่งแนวราบ ปล่อยคันบังคับแล้วโดรนอยู่กับที่
• Mission, Return / Auto, RTL ระบบสร้างเส้นทางเองและใช้ทุกลูป

คู่มือ ArduPilot แนะนำให้ผู้เริ่มฝึกไล่จาก Stabilize ไป Alt Hold, Loiter, RTL และ Auto ส่วน PX4 แบ่งโหมดเป็นกลุ่มโหมดบังคับด้วยมือ เช่น Position, Altitude, Stabilized, Acro และกลุ่มอัตโนมัติ เช่น Hold, Return, Mission, Takeoff, Land, Offboard

LOOPS = ["rate", "attitude", "altitude", "horizontal position", "path"]
MODES = {
    "Stabilized / Stabilize": 2,
    "Altitude / Alt Hold": 3,
    "Position / Loiter": 4,
    "Mission / Auto": 5,
}
for mode, n in MODES.items():
    print(f"{mode:<24} active loops: {', '.join(LOOPS[:n])}")

symptom_modes = ["Position / Loiter", "Mission / Auto"]      # โหมดที่โดรนส่ายไปมา
common = set(LOOPS)
for m in symptom_modes:
    common &= set(LOOPS[:MODES[m]])
fine = set(LOOPS[:MODES["Altitude / Alt Hold"]])              # โหมดที่บินปกติ
print("suspect loops:", sorted(common - fine))

Stabilized / Stabilize   active loops: rate, attitude
Altitude / Alt Hold      active loops: rate, attitude, altitude
Position / Loiter        active loops: rate, attitude, altitude, horizontal position
Mission / Auto           active loops: rate, attitude, altitude, horizontal position, path
suspect loops: ['horizontal position']

จัดการไฟล์พารามิเตอร์

ก่อนเปลี่ยนค่า ให้บันทึกไฟล์พารามิเตอร์เดิมเสมอ Mission Planner มีปุ่ม Save to file, Load from File และ Compare Params ที่เทียบค่าบนเครื่องกับไฟล์ แล้วเลือกได้ว่าจะเปลี่ยนค่าใด ซึ่งยังต้องกด Write Params จึงจะเขียนลงเครื่องจริง QGroundControl ก็บันทึกและโหลดไฟล์พารามิเตอร์ได้เช่นกัน

before = """ATC_RAT_RLL_P,0.135
ATC_RAT_RLL_I,0.135
ATC_RAT_RLL_D,0.0036
ATC_RAT_RLL_FLTD,20
ATC_ANG_RLL_P,4.5
INS_GYRO_FILTER,20"""
after = """ATC_RAT_RLL_P,0.19
ATC_RAT_RLL_I,0.19
ATC_RAT_RLL_D,0.004
ATC_RAT_RLL_FLTD,20
ATC_ANG_RLL_P,6.0
INS_GYRO_FILTER,40"""
LIMIT = 30.0                                  # % เปลี่ยนเกินนี้ให้ทบทวนก่อนบิน

def parse(text):
    return {name: float(value) for name, value in (line.split(",") for line in text.splitlines())}

old, new = parse(before), parse(after)
for name in sorted(old.keys() | new.keys()):
    a, b = old.get(name), new.get(name)
    if a == b:
        continue
    pct = (b - a) / a * 100
    flag = "REVIEW" if abs(pct) > LIMIT else ""
    print(f"{name:<18} {a:>8g} -> {b:<8g} {pct:+6.1f}% {flag}")

ATC_ANG_RLL_P           4.5 -> 6         +33.3% REVIEW
ATC_RAT_RLL_D        0.0036 -> 0.004     +11.1%
ATC_RAT_RLL_I         0.135 -> 0.19      +40.7% REVIEW
ATC_RAT_RLL_P         0.135 -> 0.19      +40.7% REVIEW
INS_GYRO_FILTER          20 -> 40       +100.0% REVIEW

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

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

สรุป

• PX4 ใช้ cascade ตำแหน่ง P → ความเร็ว PID → ท่าทาง P → อัตราหมุน PID ส่วน ArduPilot ใช้มุม P → อัตราหมุน PID
• ออกแบบและจูนจากลูปในออกไปลูปนอก
• โหมดการบินต่างกันที่ลูปที่ทำงาน ใช้แยกหาสาเหตุของอาการได้
• บันทึก เปรียบเทียบ และเปลี่ยนพารามิเตอร์ทีละน้อย

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

1. ในโครงของ PX4 ตัวควบคุมความเร็วส่งอะไรให้ลูปถัดไป
2. โหมด Alt Hold ของ ArduPilot เพิ่มลูปใดจาก Stabilize
3. โดรนสั่นในทุกโหมด ควรสงสัยลูปใดก่อน
4. พารามิเตอร์เปลี่ยนจาก 0.135 เป็น 0.19 คิดเป็นกี่เปอร์เซ็นต์
5. Compare Params ใน Mission Planner เขียนค่าลงเครื่องทันทีหรือไม่