Cascade control

ตัวอย่างที่ 1 เพิ่มอัตราขยายลูปนอกเกินไปเกิดอะไร

แบบจำลองอย่างง่ายของการหมุนแกน roll: ลูปอัตราหมุนแบบ PI มอเตอร์มีค่าคงตัวเวลา 30 ms และเวลาหน่วง 10 ms สั่งเอียง 20° แล้วเปลี่ยนเฉพาะอัตราขยายมุม (ค่าในแบบจำลองไม่ใช่หน่วยของ ArduPilot ยกเว้นอัตราขยายมุมที่ใช้ช่วงเดียวกัน)

import math
import numpy as np

def step(k_angle, k_rp=0.15, k_ri=0.1, T=2.0, dt=0.0025, tau=0.03, J=0.02, delay=0.01):
    n = int(T / dt)
    ang = rate = u_act = integ = 0.0
    buf = [0.0] * int(delay / dt)
    out = []
    for _ in range(n):
        rate_cmd = k_angle * (math.radians(20) - ang)      # ลูปนอก: มุม -> อัตราหมุน
        e = rate_cmd - rate
        integ += e * dt
        buf.append(k_rp * e + k_ri * integ)                # ลูปใน: PI ของอัตราหมุน
        u_act += (buf.pop(0) - u_act) * dt / tau           # มอเตอร์ตอบสนองช้า
        rate += u_act / J * dt
        ang += rate * dt
        out.append(math.degrees(ang))
    out = np.array(out)
    overshoot = (out.max() - 20) / 20
    outside = np.where(abs(out - 20) > 0.4)[0]
    settle = (outside[-1] + 1) * dt
    rise = np.argmax(out >= 18) * dt
    return overshoot, settle, rise

for k in (3, 4.5, 9, 12):
    os_, ts, tr = step(k)
    print(f"angle P {k:>4}: rise {tr:.2f} s, overshoot {os_:5.1%}, settle (2%) {ts:.2f} s")

angle P    3: rise 0.53 s, overshoot  1.8%, settle (2%) 0.64 s
angle P  4.5: rise 0.37 s, overshoot 12.3%, settle (2%) 1.25 s
angle P    9: rise 0.23 s, overshoot 40.0%, settle (2%) 1.55 s
angle P   12: rise 0.20 s, overshoot 54.4%, settle (2%) 1.96 s

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

ตัวอย่างที่ 2 เอียง 20° ด้วยความเร่งต่างกัน

import math

presets = {"default 110000": 110000, "Medium 108000": 108000, "Slow 72000": 72000, "VerySlow 30000": 30000}   # cdeg/s²
ANGLE = 20                                      # องศา
for name, cdeg in presets.items():
    a = cdeg / 100                              # deg/s²
    t = 2 * math.sqrt(ANGLE / a)
    peak = math.sqrt(ANGLE * a)
    print(f"{name:<15} {a:6.0f} deg/s2: {t:.2f} s to {ANGLE} deg, peak rate {peak:5.1f} deg/s")

default 110000    1100 deg/s2: 0.27 s to 20 deg, peak rate 148.3 deg/s
Medium 108000     1080 deg/s2: 0.27 s to 20 deg, peak rate 147.0 deg/s
Slow 72000         720 deg/s2: 0.33 s to 20 deg, peak rate 120.0 deg/s
VerySlow 30000     300 deg/s2: 0.52 s to 20 deg, peak rate  77.5 deg/s

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