โมดูล 4/5 · สัปดาห์ 10–12 · 27 ชม.

ปรับจูนจาก log

UAT 305 เทคโนโลยีออโตไพลอตและระบบควบคุม

เวลาเรียนประมาณ 90 นาทีร่าง รอตรวจปรับปรุงล่าสุด 28 กันยายน 2569

เมื่อเรียนจบโมดูลนี้ ผู้เรียนจะสามารถ

คำนวณการลดทอนและมุมเฟสของตัวกรอง notch ที่ความถี่ต่าง ๆ
ตั้งค่า harmonic notch ของ ArduPilot ให้ตามความถี่มอเตอร์
คำนวณมุมเฟสที่ตัวกรองผ่านต่ำเพิ่มให้ลูปควบคุม
ใช้เครื่องมือวิเคราะห์ log ตรวจผลก่อนและหลังปรับ

ความรู้พื้นฐานที่ควรมี: UAT 305 โมดูล 1–3 · UAT 303 โมดูล 3 (FFT ของการสั่น)

ทำไมต้องรู้

UAT 303 ใช้ FFT หาว่าการสั่นอยู่ที่ความถี่ใด โมดูลนี้ใช้ข้อมูลเดียวกันเพื่อ ตั้งตัวกรอง ให้ลูปควบคุมไม่ต้องตอบสนองต่อการสั่นของมอเตอร์ ถ้าไม่กรอง ค่า D ของลูปอัตราหมุนจะขยายสัญญาณรบกวนจนมอเตอร์ร้อน ถ้ากรองมากเกินไป ลูปจะมีเวลาหน่วงเพิ่มและแกว่ง (โมดูล 1) หน่วยความรู้เรื่องวิเคราะห์ filter จาก flight log ของคลังความรู้โดรนสอนอ่านสเปกตรัมและผลของตัวกรองแบบออฟไลน์ ส่วนหน่วยความรู้เรื่อง PX4 Flight Review และ PlotJuggler สาธิตเครื่องมือดู log

Notch และ harmonic notch

ตัวกรอง notch ลดสัญญาณเฉพาะแถบแคบรอบความถี่ $f_{0}$ แต่ปล่อยความถี่อื่นผ่าน สูตรต้นแบบตาม Audio EQ Cookbook ของ W3C คือ $H (s) = (s^{2} + ω_{0}^{2}) / (s^{2} + (ω_{0} / Q) s + ω_{0}^{2})$ แถบกว้าง $B W = f_{0} / Q$ ArduPilot ใช้ harmonic notch ที่วางตัวกรองที่ความถี่พื้นฐานและฮาร์มอนิก ตั้งด้วย INS_HNTCH_ENABLE, INS_HNTCH_FREQ (ค่าเริ่มต้น 80 Hz), INS_HNTCH_BW (40 Hz โดยทั่วไปตั้งครึ่งหนึ่งของความถี่พื้นฐาน), INS_HNTCH_ATT (ความลึก 40 dB) และ INS_HNTCH_MODE ซึ่งเลือกแหล่งความถี่ได้ เช่น 1 = ตามคันเร่ง 3 = ข้อมูล ESC telemetry (เอกสารระบุว่ามักดีที่สุด) และ 4 = FFT ระหว่างบิน ความถี่มอเตอร์เปลี่ยนตามคันเร่ง notch แบบคงที่จึงอาจพลาดเมื่อบินต่างจากตอนตั้งค่า

ตัวอย่างที่ 1 notch ที่ 80 Hz กรองได้เท่าใด

ตัวกรองต้นแบบ $f_{0}$ = 80 Hz $B W$ = 40 Hz (ลึกไม่จำกัด ของจริงถูกจำกัดที่ INS_HNTCH_ATT)

import cmath
import math

F0, BW = 80.0, 40.0
Q = F0 / BW

def notch(f):
    s = 1j * f / F0
    return (s * s + 1) / (s * s + s / Q + 1)

for f in (8, 60, 80, 83, 100, 160):
    h = notch(f)
    db = 20 * math.log10(max(abs(h), 1e-5))
    print(f"{f:>3} Hz: {db:7.1f} dB, phase {math.degrees(cmath.phase(h)):6.1f} deg")

  8 Hz:    -0.0 dB, phase   -2.9 deg
 60 Hz:    -2.4 dB, phase  -40.6 deg
 80 Hz:  -100.0 dB, phase    0.0 deg
 83 Hz:   -16.7 dB, phase   81.6 deg
100 Hz:    -3.5 dB, phase   48.0 deg
160 Hz:    -0.5 dB, phase   18.4 deg

ที่ความถี่ 80 Hz พอดี ตัวกรองลดสัญญาณลงมาก แต่ถ้ามอเตอร์หมุนที่ 83 Hz ความลึกเหลือไม่ถึง 20 dB นี่คือเหตุผลที่ควรให้ notch ตามความถี่มอเตอร์จริง ที่ 8 Hz ซึ่งใกล้ความถี่ครอสโอเวอร์ของลูป notch เพิ่มเฟสล้าหลังเพียงไม่กี่องศา จึงแทบไม่กระทบเสถียรภาพ ต่างจากการลดความถี่ตัดของตัวกรองผ่านต่ำ

กราฟขนาดของตัวกรอง notch เป็นเดซิเบลตามความถี่ 0 ถึง 200 เฮิรตซ์ เส้นสีฟ้าใกล้ศูนย์ส่วนใหญ่ ดิ่งลงลึกที่ 80 เฮิรตซ์ จุดสีชมพูที่ 83 เฮิรตซ์อยู่ราวลบ 17 เดซิเบล — ภาพที่ 1 การลดทอนของตัวกรอง notch รอบ 80 Hz

ตัวกรองผ่านต่ำกับมุมเฟส

INS_GYRO_FILTER ของ Copter มีค่าเริ่มต้น 20 Hz และเป็นตัวกรองผ่านต่ำอันดับสองตามซอร์สโค้ด เอกสารของ Texas Instruments ระบุว่าตัวกรองผ่านต่ำอันดับสองมีเฟสล้าหลัง 90° ที่ความถี่ตัด และเข้าใกล้ 180° เมื่อความถี่สูงกว่ามาก แม้ที่ความถี่ต่ำกว่าความถี่ตัด ตัวกรองก็เพิ่มเฟสล้าหลังให้ลูปแล้ว

ตัวอย่างที่ 2 ความถี่ตัดกับเฟสที่ความถี่ครอสโอเวอร์ 8 Hz

คำนวณด้วยตัวกรองผ่านต่ำอันดับสองแบบ Butterworth ( $Q$ = 0.707) เพื่อเปรียบเทียบ

import math

F_CROSS = 8.0
for fc in (40, 20, 10):
    r = F_CROSS / fc
    phase = -math.degrees(math.atan2(math.sqrt(2) * r, 1 - r * r))
    print(f"gyro filter {fc:>2} Hz: phase at {F_CROSS:.0f} Hz = {phase:6.1f} deg")

gyro filter 40 Hz: phase at 8 Hz =  -16.4 deg
gyro filter 20 Hz: phase at 8 Hz =  -34.0 deg
gyro filter 10 Hz: phase at 8 Hz =  -72.3 deg

การลดความถี่ตัดจาก 20 Hz เหลือ 10 Hz เพื่อลดสัญญาณรบกวน ทำให้เฟสล้าหลังที่ความถี่ครอสโอเวอร์เพิ่มจากราว 34° เป็น 72° ซึ่งมากกว่าส่วนเผื่อเฟส 60° ของลูปตัวอย่างในโมดูล 1 ลูปจะไม่เสถียร วิธีที่ดีกว่าคือใช้ notch ตัดเฉพาะความถี่มอเตอร์ แล้วคงความถี่ตัดของตัวกรองผ่านต่ำไว้สูงพอ

แผนภูมิแท่งเฟสล้าหลังที่ 8 เฮิรตซ์ของตัวกรองผ่านต่ำความถี่ตัด 40 20 และ 10 เฮิรตซ์ ค่าราว 16 34 และ 72 องศา แท่ง 10 เฮิรตซ์เป็นสีชมพู — ภาพที่ 2 เฟสล้าหลังที่ความถี่ครอสโอเวอร์ตามความถี่ตัดของตัวกรอง

ขั้นตอนจูนจาก log

เอกสารขั้นตอนการจูนของ ArduPilot ให้ทำตามลำดับ: แก้การสั่นทางกลก่อน ตั้งตัวกรองจากสเปกตรัม แล้วจึงจูนลูปอัตราหมุนและลูปมุม ทุกครั้งที่เปลี่ยนค่า ให้บินเก็บ log ใหม่แล้วเทียบกับครั้งก่อน เครื่องมืออย่าง ArduPilot WebTools (FilterReview) PX4 Flight Review และ PlotJuggler ช่วยดูสเปกตรัมก่อนและหลังกรอง และเทียบค่าที่ต้องการกับค่าจริง

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

ปฏิบัติการ: ตั้ง harmonic notch จาก log

บินลอยตัวเก็บ log โดยเปิดการบันทึกข้อมูล IMU แบบละเอียด ดูสเปกตรัมใน FilterReview
หาความถี่พื้นฐานของมอเตอร์ขณะลอยตัว คำนวณการลดทอนด้วยตัวอย่างที่ 1
ตั้ง INS_HNTCH_* ตามโหมดที่อุปกรณ์รองรับ บินซ้ำ เทียบสเปกตรัมหลังกรอง
ทดลองความถี่ตัดของตัวกรองผ่านต่ำใน SITL คำนวณเฟสด้วยตัวอย่างที่ 2 และดูผลต่อลูป
บันทึกค่าที่เปลี่ยนทุกครั้งในบัญชี configuration

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

ระวัง

ลดความถี่ตัดของตัวกรองผ่านต่ำ แทนการใช้ notch
ตั้ง notch คงที่ ทั้งที่ความถี่มอเตอร์เปลี่ยนตามคันเร่ง
จูน PID ก่อนแก้การสั่น
เปลี่ยนหลายค่าพร้อมกัน แล้วบอกไม่ได้ว่าค่าใดได้ผล
ไม่เก็บ log ก่อนและหลัง จึงเทียบผลไม่ได้

สรุป

notch ลดสัญญาณเฉพาะแถบแคบ ถ้าความถี่มอเตอร์เลื่อนไปจากศูนย์กลาง ความลึกลดลงเร็ว
harmonic notch ของ ArduPilot ตามความถี่มอเตอร์ได้จากคันเร่ง ESC telemetry หรือ FFT
ตัวกรองผ่านต่ำเพิ่มเฟสล้าหลังแม้ที่ความถี่ต่ำกว่าความถี่ตัด การลดความถี่ตัดจึงเสี่ยงต่อเสถียรภาพ
จูนตามลำดับ แก้การสั่น ตั้งตัวกรอง แล้วจูนลูป และเทียบ log ทุกครั้ง

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

notch $f_{0}$ 100 Hz แถบกว้าง 50 Hz มีค่า $Q$ เท่าใด
ถ้ามอเตอร์หมุน 5,400 rpm ความถี่พื้นฐานเท่าใด
ตัวกรองผ่านต่ำอันดับสองมีเฟสล้าหลังเท่าใดที่ความถี่ตัด
ทำไม notch จึงกระทบเสถียรภาพน้อยกว่าการลดความถี่ตัดของตัวกรองผ่านต่ำ
โหมดใดของ INS_HNTCH_MODE ที่เอกสารระบุว่ามักดีที่สุด

เฉลย

$100/50 = 2$
$5400/60 = 90$ Hz
90°
notch มีผลเฉพาะแถบแคบรอบความถี่มอเตอร์ ซึ่งห่างจากความถี่ครอสโอเวอร์ จึงเพิ่มเฟสล้าหลังที่ความถี่ครอสโอเวอร์น้อย
ESC telemetry (ค่า 3)

สรุปสูตรสำคัญ

ตัวกรอง notch	$H (s) = \frac{s ^{2} + ω _{0}^{2}}{s ^{2} + \frac{ω _{0}}{Q} s + ω _{0}^{2}}, Q = \frac{f _{0}}{B W}$
เฟสของตัวกรองผ่านต่ำอันดับสอง (Butterworth)	$ϕ = - atan2 (2 r, 1 - r^{2}), r = \frac{f}{f _{c}}$

แหล่งอ้างอิงหลัก

ArduPilot Dev Team. Managing gyro noise with the dynamic harmonic notch filters. ArduPilot Copter documentation. link
Toy, R. (Ed.). (2021). Audio EQ cookbook (W3C Working Group Note). link
Karki, J. (2023). Active low-pass filter design (SLOA049D). Texas Instruments. link
ArduPilot Dev Team. ArduPilot source code, tag Copter-4.6.3 [Computer software]. GitHub. link
PX4 Autopilot. Log analysis using Flight Review. PX4 guide (main). link
ArduPilot Dev Team. Tuning process instructions. ArduPilot Copter documentation. link

อ่านเพิ่มเติม

ศึกษาหน่วยความรู้ที่กำหนดล่วงหน้า ดูสื่อประกอบ และทำ quiz ประจำโมดูล

พร้อมใช้D06D10

ในชั้นเรียน / ภาคสนาม

ปฏิบัติการในห้องแล็บหรือภาคสนามตามใบงาน พร้อม checklist ความปลอดภัย

หลักฐานการเรียนรู้: ใบงานที่ผ่านการตรวจและผล quiz

แบบทดสอบประจำโมดูล

แบบทดสอบนี้ใช้ตรวจความเข้าใจ (formative) ไม่ใช่การสอบเก็บคะแนน

โดเมนความรู้: การควบคุม ออโตไพลอต และการนำทาง · การติดตั้ง บำรุงรักษา และการทดสอบ