ตัวควบคุม PID

ทำไมต้องรู้

สูตร PID ในตำราสั้นมาก แต่ตัวควบคุมที่บินได้จริงต้องจัดการเรื่องที่สูตรไม่ได้บอก ได้แก่ คำสั่งที่เกินขีดจำกัดของมอเตอร์ การเปลี่ยนค่าตั้งกะทันหัน และสัญญาณรบกวนของเซนเซอร์ Åström และ Murray (2021) บทที่ 11 และ Åström กับ Hägglund (2006) อธิบายเรื่องเหล่านี้ไว้ละเอียด และ PX4 ใช้วิธีเดียวกันในลูปควบคุมของตัวเอง

สองรูปแบบของ PID

• แบบ parallel มีค่าขยายแยกกันสามค่า $K_p,K_i,K_d$
• แบบ standard (ideal) มีค่าขยายรวม $K$ คูณทั้งสามเทอม

คู่มือจูน PID ของ PX4 อธิบายว่าลูปอัตราหมุนเลือกได้ทั้งสองแบบ ตั้ง MC_ROLLRATE_K = 1 จะเป็นแบบ parallel ส่วนตั้ง P = 1 จะเป็นแบบ standard ซึ่งแยกการปรับค่าขยายรวมออกจาก I และ D ต้องรู้ว่าใช้แบบใดก่อนนำค่าจากที่อื่นมาใช้

Integral windup

มอเตอร์ให้แรงยกได้จำกัด เมื่อคำสั่งเกินขีดจำกัด สัญญาณจริงจะ อิ่มตัว (saturate) แต่เทอม I ยังสะสมค่าผิดพลาดต่อ เมื่อถึงเป้าแล้ว I ที่สะสมไว้มากจะดันเลยเป้าไปไกล เรียกว่า integral windup วิธีแก้ที่ใช้บ่อยมีสองแบบ

• Clamping หยุดสะสม I เมื่ออิ่มตัวและค่าผิดพลาดยังดันไปทางเดิม PX4 ใช้วิธีนี้ในลูปความเร็ว (Controller Diagrams)
• Back-calculation ป้อนผลต่างระหว่างคำสั่งกับค่าที่อิ่มตัวกลับไปลด I (Åström และ Murray ข้อ 11.4)

KP, KI, KD, AMAX, DT = 1.5, 0.4, 2.0, 3.0, 0.01

def climb(anti_windup):
    z = v = integ = 0.0
    zs = []
    for _ in range(int(40 / DT)):
        e = 10 - z
        a_cmd = KP * e + KI * integ - KD * v
        a = max(-AMAX, min(AMAX, a_cmd))
        pushing_further = (a_cmd > AMAX and e > 0) or (a_cmd < -AMAX and e < 0)
        if not (anti_windup and pushing_further):
            integ += e * DT
        v += a * DT
        z += v * DT
        zs.append(z)
    settle = next(i * DT for i in range(len(zs)) if all(abs(x - 10) <= 0.2 for x in zs[i:]))
    return max(zs) - 10, settle

for label, flag in (("without anti-windup", False), ("with clamping", True)):
    over, ts = climb(flag)
    print(f"{label:<20} overshoot {over:.2f} m, within 0.2 m after {ts:.1f} s")

without anti-windup  overshoot 4.14 m, within 0.2 m after 11.0 s
with clamping        overshoot 1.58 m, within 0.2 m after 9.8 s

อนุพันธ์จากค่าวัดและตัวกรอง

ถ้าคิดเทอม D จาก ค่าผิดพลาด การเปลี่ยนค่าตั้งแบบขั้นจะทำให้อนุพันธ์พุ่งสูงชั่วขณะ Åström และ Murray แนะนำให้ D (และอาจรวม P) ทำงานบน ค่าวัด แทน เรียกว่า setpoint weighting อีกเรื่องคืออนุพันธ์ขยายสัญญาณรบกวนความถี่สูง จึงต้องกรอง $K_{d}s/(1+s{T}_f)$ โดย $T_f=T_d/N$ และ $N$ อยู่ราว 5–20 PX4 ใช้ตัวกรองความถี่ต่ำกับเทอม D ของลูปอัตราหมุน

KP, KD, DT, N = 1.5, 2.0, 0.01, 10
TF = (KD / KP) / N                     # s ค่าคงตัวเวลาของตัวกรองอนุพันธ์

steps = [0.0, 10.0, 10.0, 10.0]        # ค่าตั้งที่แต่ละขั้นเวลา
meas = [0.0, 0.0, 0.02, 0.08]          # ค่าวัด (เพิ่งเริ่มขยับ)

def d_term(signal, sign, filtered):
    d, peak = 0.0, 0.0
    for k in range(1, len(signal)):
        raw = sign * KD * (signal[k] - signal[k - 1]) / DT
        d = d + DT / (TF + DT) * (raw - d) if filtered else raw
        peak = max(peak, abs(d))
    return peak

err = [r - y for r, y in zip(steps, meas)]
print(f"D on error, unfiltered:   peak {d_term(err, 1, False):7.1f}")
print(f"D on error, filtered N={N}: peak {d_term(err, 1, True):7.1f}")
print(f"D on measurement, filtered: peak {d_term(meas, -1, True):7.1f}")

D on error, unfiltered:   peak  2000.0
D on error, filtered N=10: peak   139.5
D on measurement, filtered: peak     1.1

ลำดับการปรับค่า

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

• P เพิ่มความเร็วตอบสนอง แต่มากเกินจะแกว่ง
• I แก้ค่าผิดพลาดคงที่ เช่น จุดศูนย์ถ่วงเยื้อง แต่มากเกินจะเกินเป้าช้า ๆ
• D หน่วงการแกว่ง แต่ขยายสัญญาณรบกวน จึงต้องกรองและดูอุณหภูมิมอเตอร์

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

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

สรุป

• PID มีแบบ parallel และแบบ standard ต้องรู้ว่าออโตไพลอตใช้แบบใด
• anti-windup แบบ clamping หรือ back-calculation ลด overshoot เมื่ออิ่มตัว
• ใช้อนุพันธ์จากค่าวัดและตัวกรอง $T_f=T_d/N$ ($N$ = 5–20)
• จูนลูปอัตราหมุนก่อน แล้วจึงลูปนอก

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

1. PID แบบ standard มี $K=2$, $T_i=4$ s ได้ $K_i$ ในแบบ parallel เท่าใด
2. integral windup เกิดเมื่อใด
3. $K_d=2$, $K_p=1.5$, $N=10$ ได้ $T_f$ เท่าใด
4. ทำไมจึงคิดเทอม D จากค่าวัดแทนค่าผิดพลาด
5. คู่มือ PX4 ให้ปรับลูปใดก่อน