ระบบควบคุมและแบบจำลอง

ทำไมต้องรู้

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

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

ผังบล็อกของวงปิด

ระบบควบคุมวงปิด (closed-loop) วัดผลลัพธ์ $y$ แล้วเทียบกับค่าที่ต้องการ $r$ ได้ ค่าผิดพลาด $e=r-y$ ตัวควบคุมใช้ $e$ คำนวณคำสั่ง $u$ ส่งให้อากาศยาน ซึ่งถูก สิ่งรบกวน เช่น ลม กระทำอยู่ด้วย เซนเซอร์วัดผลแล้วป้อนกลับมาเทียบใหม่ เปรียบเหมือนคนขับรถที่มองถนนแล้วหมุนพวงมาลัยแก้ตลอดเวลา (Åström และ Murray, 2021)

แบบจำลองอันดับหนึ่งของมอเตอร์

เมื่อสั่งเร่งมอเตอร์ทันที ใบพัดไม่ได้หมุนเร็วขึ้นในพริบตา เพราะมีความเฉื่อย แรงยกจึงค่อย ๆ เพิ่มขึ้นแบบ อันดับหนึ่ง $y(t)=K(1-e^{-t/\tau })$ โดย $\tau$ คือ ค่าคงตัวเวลา ซึ่งเป็นเวลาที่ผลตอบสนองขึ้นถึง 63.2% ของค่าสุดท้าย ในรูป ฟังก์ชันถ่ายโอน (transfer function) เขียนเป็น $G(s)=K/(\tau s+1)$ ซึ่งเป็นวิธีเขียนความสัมพันธ์ของอินพุตกับเอาต์พุตในโดเมนลาปลาซ (Nise, 2019)

import math

DT = 0.01
thrust = [0.0, 1.09, 1.98, 2.71, 3.3, 3.79, 4.19, 4.52, 4.79, 5.01, 5.19, 5.34, 5.46, 5.55, 5.64,
          5.7, 5.76, 5.8, 5.84, 5.87, 5.89, 5.91, 5.93, 5.94, 5.95, 5.96, 5.97, 5.97, 5.98, 5.98, 5.99]
final = thrust[-1]
target = 0.632 * final
i = next(k for k, v in enumerate(thrust) if v >= target)
t63 = (i - 1) * DT + (target - thrust[i - 1]) / (thrust[i] - thrust[i - 1]) * DT
print(f"final thrust {final:.2f} N, 63.2% = {target:.2f} N, tau from the curve = {t63 * 1000:.1f} ms")

pts = [(k * DT, math.log(1 - v / final)) for k, v in enumerate(thrust[:15])]
slope = sum(t * y for t, y in pts) / sum(t * t for t, _ in pts)
print(f"tau from a log fit of the first 0.14 s = {-1 / slope * 1000:.1f} ms")

final thrust 5.99 N, 63.2% = 3.79 N, tau from the curve = 49.9 ms
tau from a log fit of the first 0.14 s = 49.6 ms

พลวัตแนวดิ่งและการทำให้เป็นเชิงเส้น

ในแนวดิ่ง โดรนมีแรงยกรวม $T$ สู้กับน้ำหนัก $mg$ ได้ $m\ddot{z}=T-mg$ แรงยกของใบพัดโดยประมาณแปรตามกำลังสองของคันเร่ง $T=k{u}^2$ ซึ่งไม่เป็นเชิงเส้น แต่เราควบคุมรอบ จุดลอยนิ่ง (hover) เป็นหลัก จึงประมาณด้วยเส้นตรงรอบจุดนั้นได้ $\Delta T\approx 2k{u}_h\Delta u$ ตำรา Beard และ McLain (2012) ใช้หลักเดียวกันกับอากาศยานไร้คนขับขนาดเล็ก

import math

M, G, N_MOTORS, K_MAX = 1.5, 9.80665, 4, 8.0      # kg, m/s², จำนวนมอเตอร์, N ที่คันเร่งเต็ม
t_hover = M * G / N_MOTORS
u_hover = math.sqrt(t_hover / K_MAX)
gain = N_MOTORS * 2 * K_MAX * u_hover                # N ต่อคันเร่ง 1.0 (ทุกมอเตอร์รวมกัน)
print(f"hover thrust per motor {t_hover:.2f} N, hover throttle {u_hover:.3f}")
print(f"local gain {gain:.1f} N per unit throttle -> +0.01 throttle gives {gain * 0.01 / M:.2f} m/s^2")
print(f"thrust-to-weight ratio {N_MOTORS * K_MAX / (M * G):.2f}")

hover thrust per motor 3.68 N, hover throttle 0.678
local gain 43.4 N per unit throttle -> +0.01 throttle gives 0.29 m/s^2
thrust-to-weight ratio 2.18

ฟังก์ชันถ่ายโอนอันดับสอง

หลายระบบประกอบด้วยส่วนอันดับหนึ่งสองส่วนหรือมีมวลกับสปริง ผลตอบสนองจึงเป็น อันดับสอง รูปมาตรฐานคือ $G(s)={\omega }_0^2/(s^2+2\zeta {\omega }_{0}s+{\omega }_0^2)$ โดย ${\omega }_0$ คือความถี่ธรรมชาติ และ $\zeta$ คือ อัตราส่วนความหน่วง ซึ่งกำหนดว่าผลตอบสนองจะแกว่งมากเพียงใด โมดูล 2 จะใช้รูปนี้ศึกษาผลตอบสนองและเสถียรภาพ

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

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

สรุป

• วงปิดวัดผลลัพธ์ เทียบกับค่าที่ต้องการ แล้วแก้คำสั่งต่อเนื่อง โดยมีสิ่งรบกวนกระทำอยู่ด้วย
• มอเตอร์ประมาณได้ด้วยแบบจำลองอันดับหนึ่ง $\tau$ คือเวลาถึง 63.2% ของค่าสุดท้าย
• พลวัตแนวดิ่ง $m\ddot{z}=T-mg$ และแรงยก $T=k{u}^2$ ทำให้เป็นเชิงเส้นรอบจุดลอยนิ่ง
• ระบบอันดับสองกำหนดด้วย ${\omega }_0$ และ $\zeta$

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

1. ระบบอันดับหนึ่งมี $\tau =0.1$ s ผลตอบสนองถึง 63.2% ของค่าสุดท้ายเมื่อใด
2. ที่เวลา $3\tau$ ผลตอบสนองถึงกี่เปอร์เซ็นต์ของค่าสุดท้าย
3. โดรน 2 kg สี่มอเตอร์ แรงยกลอยนิ่งต่อมอเตอร์เท่าใด
4. ถ้า $T=8u^2$ และแรงยกลอยนิ่งต่อมอเตอร์ 2 N คันเร่งลอยนิ่งเท่าใด
5. ทำไมต้องทำให้แบบจำลองเป็นเชิงเส้นรอบจุดลอยนิ่ง