โปรเจกต์ Kalman Filter บน BNO055 คืออะไร?

Iloke Alusala สร้างโปรเจกต์นี้เพื่อแสดงให้เห็นว่า เมื่อใช้ข้อมูล Gyroscope ชุดเดียวกัน แต่เลือกวิธีคำนวณมุมต่างกัน ผลลัพธ์จะเปลี่ยนไปอย่างไร

ฝั่งแรกนำอัตราการหมุนจาก Gyroscope มาสะสมเป็นมุมโดยตรง ส่วนอีกฝั่งใช้ Kalman Filter ผสานข้อมูลจาก Gyroscope และ Accelerometer เพื่อประมาณมุม Roll/Pitch พร้อมชดเชย Gyro Bias

ทั้งสองวิธีใช้ข้อมูลจากเซนเซอร์เดียวกัน แล้วส่งผลลัพธ์ไปแสดงบนหน้าเว็บที่มีบอร์ด 3D สองฝั่ง เพื่อให้สามารถเปรียบเทียบพฤติกรรมของอัลกอริทึมได้

Gyro Drift คืออะไร และ Kalman Filter ช่วยอย่างไร?

Gyroscope วัดความเร็วในการหมุน ไม่ได้วัดมุมโดยตรง

เมื่ออ่านอัตราการหมุนจาก Gyroscope เราสามารถสะสมค่านั้นตามช่วงเวลาเพื่อประมาณมุมที่เปลี่ยนไปได้ แต่ถ้าเซนเซอร์มี Error หรือ Bias แม้เพียงเล็กน้อย ความคลาดเคลื่อนก็อาจสะสมเพิ่มขึ้นเรื่อย ๆ

Accelerometer ช่วยอ้างอิงทิศทางแรงโน้มถ่วง

Accelerometer ให้ข้อมูลที่ช่วยประมาณ Roll และ Pitch ได้เมื่ออยู่ในสภาพแวดล้อมเหมาะสม แต่การสั่นและแรงเร่งจากการเคลื่อนไหวอาจรบกวนค่าที่อ่าน จึงไม่ควรใช้ Accelerometer เพียงตัวเดียวสำหรับทุกสถานการณ์

Kalman Filter ทำให้ข้อมูลสองแบบช่วยกัน

ในโปรเจกต์นี้ Kalman Filter มีสถานะสำคัญสองส่วนต่อแกน ได้แก่ Angle และ Gyro Bias โดยใช้ข้อมูล Gyroscope สำหรับ Predict และข้อมูล Accelerometer สำหรับ Update

Hardware และ Software ที่ใช้

จุดที่ต้องระวังคือ Shuttle Board 3.0 ไม่ได้มี Connector แบบเดียวกับ BNO055 Breakout Board ทั่วไป ผู้สร้างระบุว่าคอนเนกเตอร์ของบอร์ดมีระยะ 1.27 มม. และต้องเตรียมสายให้เข้ากัน

Step 1: ต่อ BNO055 Shuttle Board 3.0 เข้ากับ Raspberry Pi 5

ขั้นตอนเริ่มจากเตรียมสาย Connector ให้พอดีกับ Shuttle Board จากนั้นจึงเชื่อมต่อ VDD, VDDIO, GND และสัญญาณ I²C ไปยัง Raspberry Pi 5

การเตรียมสาย Connector

มีภาพเพิ่มเติมอีก 2 ภาพจากขั้นตอนเดียวกัน กด View more เพื่อดูภาพถ่ายจริง

ตาราง Wiring ที่ผู้สร้างใช้

Physical Wiring Diagram

ภาพการเชื่อมต่อที่ประกอบเสร็จ

ตรวจตำแหน่ง GPIO Pinout

ควรเปิด Bosch Shuttle Board 3.0 Flyer PDF ควบคู่กับ Diagram เพื่อตรวจการระบุขา P1/P2 ก่อนจ่ายไฟ

Step 2: ตั้งค่า I²C และ Python Environment

ผู้สร้างใช้งาน Raspberry Pi แบบ Headless ผ่าน SSH เริ่มจากเปิด I²C แล้วติดตั้งเครื่องมือสำหรับตรวจสอบการสื่อสารกับ BNO055

sudo raspi-config nonint do_i2c 0
sudo reboot
sudo apt install i2c-tools
i2cdetect -y 1

curl -LsSf https://astral.sh/uv/install.sh | sh
source $HOME/.local/bin/env
mkdir -p ~/imu-cube && cd ~/imu-cube
uv venv
source .venv/bin/activate
uv pip install smbus2 aiohttp

เมื่อใช้ i2cdetect -y 1 หากเห็น Address 28 ในตาราง แสดงว่ามีอุปกรณ์ตอบสนองที่ตำแหน่งนี้ แต่ยังต้องตรวจ CHIP_ID อีกครั้งเพื่อยืนยันว่าเป็น BNO055

การติดตั้ง Python ใช้เครื่องมือ uv ตามขั้นตอนของผู้สร้าง โดยควรตรวจคำสั่งดาวน์โหลดสคริปต์จากแหล่งภายนอกก่อนรันบนเครื่องทุกครั้ง

Step 3: ทดสอบ BNO055 ด้วย NDOF Mode

ก่อนเริ่ม Kalman Filter ผู้สร้างใช้ไฟล์ bno055_bringup.py ตรวจการสื่อสาร โดยอ่าน CHIP_ID ที่ Register 0x00 และคาดหวังค่า 0xA0

จากนั้นสคริปต์ใช้ NDOF Mode ซึ่ง BNO055 ทำ Sensor Fusion ภายในชิปอยู่แล้ว เพื่อดู Euler Angle และ Calibration Status

ADDR = 0x28
REG_CHIP_ID = 0x00

with SMBus(1) as bus:
    chip_id = bus.read_byte_data(ADDR, REG_CHIP_ID)
    print(f"CHIP_ID: 0x{chip_id:02X} (want 0xA0)")
    if chip_id != 0xA0:
        raise SystemExit(
            "Not a BNO055, check PS0/PS1 grounding and address."
        )

ดาวน์โหลดไฟล์เต็มได้จาก bno055_bringup.py ที่ Hackster

Step 4: อ่านข้อมูลดิบด้วย AMG Mode และสร้าง Kalman Filter

ไฟล์ imu_fusion_server.py เปลี่ยน BNO055 เป็น AMG Mode โดยใช้ค่า 0x07 เพื่ออ่านข้อมูล Accelerometer, Magnetometer และ Gyroscope แบบดิบ

โค้ดอ่านข้อมูลเป็นบล็อก 18 ไบต์จาก I²C แล้วแยกค่า Accelerometer และ Gyroscope มาคำนวณ Roll/Pitch ด้วย Kalman Filter แบบ 2-State

Kalman1D คำนวณอย่างไร?

Predict ใช้ข้อมูลอัตราการหมุนหลังหัก Bias ส่วน Update ใช้มุมจาก Accelerometer เพื่อช่วยปรับค่าประมาณและเรียนรู้ Bias ของ Gyroscope

def predict(self, rate, dt):
    self.angle += dt * (rate - self.bias)
    self.p00 += dt * (
        dt * self.p11 - self.p01 - self.p10 + self.q_angle
    )
    self.p01 -= dt * self.p11
    self.p10 -= dt * self.p11
    self.p11 += self.q_bias * dt

def update(self, measurement, r_scale=1.0):
    s = self.p00 + self.r_measure * r_scale
    k0 = self.p00 / s
    k1 = self.p10 / s
    y = measurement - self.angle
    self.angle += k0 * y
    self.bias += k1 * y

ดูโค้ดทั้งหมดได้จาก imu_fusion_server.py บน GitHub ของผู้สร้าง

Step 5: แสดงผลผ่าน Web Dashboard แบบ 3D

Raspberry Pi ทำหน้าที่เป็น Web Server และส่งข้อมูลผ่าน WebSocket ให้หน้าเว็บที่ใช้ Three.js แสดงบอร์ด 3D สองฝั่งโดยอาศัยข้อมูลจากเซนเซอร์ตัวเดียวกัน

ปุ่ม Align raw ใช้จัดแนวของบอร์ด Raw ให้เริ่มต้นตรงกับฝั่ง Kalman อีกครั้ง ส่วน Zero yaw ใช้รีเซ็ต Heading ของทั้งสองฝั่ง

หน้า Dashboard เปิดศึกษาซอร์สได้ที่ index.html ของโปรเจกต์บน GitHub

Step 6: รันระบบและทดลองสังเกต Gyro Drift

หลังเตรียมโฟลเดอร์ ~/imu-cube และคัดลอกไฟล์ imu_fusion_server.py กับ index.html ลงในโฟลเดอร์เดียวกัน ให้เปิด Python Environment และรัน Server

cd ~/imu-cube
source .venv/bin/activate
python imu_fusion_server.py

ตรวจ IP ของ Raspberry Pi ด้วย hostname -I จากนั้นเปิดหน้าเว็บในรูปแบบ http://<pi-ip>:8080 จากคอมพิวเตอร์ที่อยู่เครือข่ายเดียวกัน

ทดลองเอียงเซนเซอร์เพื่อดูการหมุนของทั้งสองฝั่ง แล้วกด Align raw และวางเซนเซอร์นิ่งเพื่อสังเกตว่าค่า Raw Gyro กับ Kalman Filter เปลี่ยนแปลงต่างกันอย่างไร

วิดีโอเสริม: การ Calibration BNO055 จาก Bosch Sensortec

ต้นฉบับของ Iloke ไม่ได้ฝังวิดีโอสาธิต Kalman Filter ไว้ แต่มีวิดีโออย่างเป็นทางการจาก Bosch Sensortec ที่อธิบายการ Calibration ของ BNO055 ซึ่งเหมาะสำหรับศึกษา Sensor Fusion ภายในชิปเพิ่มเติม

Video Source: Bosch Sensortec: How to Calibrate the Absolute Orientation Sensor BNO055

วิดีโอนี้เป็นคู่มือจากผู้ผลิต ไม่ใช่วิดีโอทดสอบของ Iloke และไม่ได้ใช้แทนผลลัพธ์ Kalman Filter ในโปรเจกต์นี้

Troubleshooting: ปัญหาที่ผู้สร้างพบ

อ่านรายละเอียดปัญหาอื่น ๆ ได้จาก Step 8: Troubleshooting ของบทความต้นฉบับ

Source Code และไฟล์ดาวน์โหลดที่เกี่ยวข้อง

โปรเจกต์นี้มี Source Code หลักบน GitHub และมี Bring-up Script ที่ผู้สร้างแนบไว้ใน Hackster แยกจาก Repository ส่วนเอกสาร Bosch ช่วยตรวจรายละเอียด Hardware, Pinout และ Operating Mode

โปรเจกต์นี้ไม่มีไฟล์ STL หรือ Gerber ที่ยืนยันว่าเป็นส่วนหนึ่งของการสร้างระบบ Kalman Filter จึงไม่เพิ่มลิงก์ดาวน์โหลดที่ไม่เกี่ยวข้อง

FAQ: คำถามที่พบบ่อยเกี่ยวกับ Kalman Filter และ BNO055

สรุป: Kalman Filter ทำให้เข้าใจ Sensor Fusion ผ่านการทดลองจริง

จุดเด่นของโปรเจกต์นี้คือการนำ BNO055 กับ Raspberry Pi 5 มาทดลองเปรียบเทียบ Raw Gyro และ Kalman Filter โดยใช้ข้อมูลเดียวกัน เพื่อให้เห็นว่าการชดเชย Bias และการรวมข้อมูลจาก Accelerometer ช่วยเปลี่ยนพฤติกรรมของค่ามุม Roll/Pitch อย่างไร

สิ่งที่ได้เรียนรู้ครอบคลุมการเชื่อมต่อ I²C, การตั้งค่า Sensor Mode, การทำ Kalman Filter ด้วย Python และการแสดงผลผ่าน Web Dashboard ซึ่งเป็นพื้นฐานที่นำไปศึกษาต่อยอดด้าน Robotics และ Embedded Systems ได้

โปรเจกต์มีรายละเอียด Wiring และ Software หลายส่วน แนะนำให้อ่าน Tutorial ฉบับเต็มจาก Hackster พร้อมกับ Source Code บน GitHub ของผู้สร้าง ก่อนลงมือประกอบจริง

References / แหล่งข้อมูลต้นฉบับ

1. Hackster: Set Up a Kalman Filter on a BNO055 With a Raspberry Pi 5
2. GitHub: BNO055-Kalman_Filter โดย Iloke Alusala
3. Bosch Sensortec: BNO055 Datasheet
4. Bosch Sensortec: BNO055 Shuttle Board Flyer
5. Astral: uv Documentation
6. Embedder: เครื่องมือที่ผู้สร้างระบุว่าใช้
7. Bosch Sensortec: BNO055 Calibration Tutorial