[브레드보드 IO 12편] 🤖 아두이노로 인버티드 펜듈럼(도립진자) [단계 1-B]: 상보 필터(Complementary Filte

[민한식]

 민선생의 전자공방 ・ 2026. 6. 29. 10:57

URL 복사  통계 

본문 기타 기능

브레드보드 활용 IO 인터페이스 12편

📐 인버티드 펜듈럼 — 단계 1-B
상보 필터 (Complementary Filter)
— 자이로와 가속도를 융합하여 부드럽고 정확한 각도를 —

상보필터 · dt 실측 · Angle Wrapping · 칼만필터 예고
아두이노 프로젝트 · 민선생의 전자공방

접기/펴기

동영상 재생은 민선생의 전자공방 https://blog.naver.com/hansikmin/224330366277

단계 1-B 상보필터 동작 영상 펜듈럼을 천천히·빠르게 흔들 때 Accel vs Filter 비교 / 리밋스위치 비상정지 테스트

■ 프롤로그

안녕하세요, 민선생입니다. 지난 11편에서 MPU-6050 가속도 센서만으로 각도를 출력하고 영점 캘리브레이션까지 완성했습니다. 하사점에서 정확히 0°가 출력되는 것을 확인했지요.

이번 12편에서는 한 단계 더 나아갑니다. 가속도 센서만으로는 빠른 움직임에서 노이즈가 심하고, 자이로만으로는 시간이 지나면 각도가 흘러갑니다. 이 두 가지 단점을 동시에 해결하는 상보 필터(Complementary Filter)를 적용합니다.

인버티드 펜듈럼 — 전체 구성 한눈에 보기

정면 모습 (카트가 중앙, 카본봉 펜듈럼이 하사점으로 늘어진 상태), 이 상태에서 좌우로 스윙(좌우로 운동하며 운동에너지로 직립까지 하도록)하며 직립하게 하는것이 목표입니다

📌 이전 편 참조

11편 단계 1-A — MPU-6050 캘리브레이션 & 가속도 각도 출력 (이번 코드의 베이스)

10편 하드웨어 구성 & 전체 로드맵

8편 MPU-6050 Roll·Pitch 자세 감지 → atan2 각도 계산 기초

9편 Non-blocking 타이머 구조 → 이번 코드의 loop() 설계 기반

🎯 이번 편에서 해 볼 것

① 가속도 센서 단독 사용의 한계 — 노이즈 문제

② 자이로 센서 단독 사용의 한계 — 드리프트 문제

③ 상보 필터의 원리와 수식 (α = 0.98)

④ dt를 micros()로 실측해야 하는 이유

⑤ Accel값과 Filter값의 차이 — 왜 차이가 나며 제어에 더 좋은 이유

⑥ 시리얼 플로터에서 Accel vs Filter 비교

⑦ 상보 필터의 한계 — 왜 직립 제어에서 칼만 필터가 필요한가

1. 회로 구성

회로 구성은 이전편과 동일합니다. 인버티드 펜듈럼 프로젝트 하드웨어는 아래의 회도와 같이 만들어 놓았습니다. 모든 과정에서는 회로도와 배선은 큰 오류가 없으면 변경 없이 유지하고 코드만 업그레이드됩니다.

Inverted Pendulum(역진자) 브레드보드 회로 배선도(배선도 툴 Wokwi)

실제 제어부 — Arduino R4 · DRV8825 드라이버 · 브레드보드 배선

단계 1-B LCD 출력 — Accel / Filter / Gyro 동시 표시

2. MPU6050 센서의 한계 — 왜 필터가 필요한가

2-1. 가속도 센서만 사용할 때의 문제

11편에서 가속도 센서로 각도를 계산할 때 atan2(AY, AZ) 수식을 사용했습니다. 정지 상태에서는 매우 정확합니다. 그런데 막대를 빠르게 흔들거나 카트가 가속하면 어떻게 될까요?

❌ 가속도 단독 사용의 문제

가속도 센서의 문제

가속도 센서는 중력(g = 9.8 m/s²)의 방향을 측정해 각도를 계산합니다.

그런데 카트가 빠르게 움직이면 관성력(ma)이 중력에 더해집니다.

센서가 느끼는 힘 = 중력(g) + 관성력(ma)

↑ 실제 기울기 ↑ 카트 가속도에 의한 가짜 신호

결과 : 카트가 가속하는 순간 각도가 실제와 다르게 튀어 보임

빠른 스윙업 중 각도 노이즈 심각 → PID에 입력되면 모터 급발진!

2-2. 자이로 센서만 사용할 때의 문제

❌ 자이로 단독 사용의 문제

자이로 드리프트 문제

각도 = 이전각도 + 각속도 × dt (적분)

문제 : 자이로는 정지 상태에서도 0이 아닌 미세한 노이즈를 출력

이 노이즈가 dt마다 누적 → 시간이 지날수록 각도 오차 증가

예시 : 오프셋 0.1°/s

10초 후 → 1° 오차 / 1분 후 → 6° 오차 / 5분 후 → 30° 오차

결론 : 자이로만으로는 장시간 정확한 각도 유지 불가능

2-3. 두 센서의 특성 비교

특성	가속도 센서	자이로 센서
반응 속도	즉각 반응 (atan2 직접 계산)	빠름 (각속도 측정)
장기 안정성	✅ 안정적 (중력 기준)	❌ 드리프트 발생
진동·충격	❌ 노이즈 심함	✅ 영향 없음
카트 가속 시	❌ 관성력으로 각도 오류	✅ 영향 없음
단독 사용	불가 (스윙업 중 노이즈)	불가 (드리프트)

💡 핵심 포인트

결론 : 두 센서를 융합하면 서로의 단점을 보완할 수 있습니다!

자이로 : 빠른 움직임 추적 (고주파 통과 필터 역할)

가속도 : 장기 드리프트 방지 (저주파 통과 필터 역할)

→ 이 두 가지를 α비율로 합치는 것이 상보 필터입니다.

3. 상보 필터 원리

3-1. 상보 필터 수식

상보(Complementary)는 '서로의 부족한 부분을 채워준다'는 뜻입니다. 자이로의 고주파 특성과 가속도의 저주파 특성을 α 계수로 합쳐 두 센서의 단점을 동시에 보완합니다.

📐 상보 필터 수식

상보 필터 수식

cfAngle = α × (cfAngle + gyroRate × dt)

+ (1-α) × accelAngle

첫 번째 항 α × (cfAngle + gyroRate × dt) → 자이로 담당

이전 필터 각도에 자이로 적분값을 더함

빠른 움직임을 즉각 반영 (고주파 통과)

α = 0.98 : 자이로를 98% 신뢰

두 번째 항 (1-α) × accelAngle → 가속도 담당

가속도 각도를 소량 반영

장기 드리프트를 천천히 보정 (저주파 통과)

(1-α) = 0.02 : 가속도를 2% 신뢰

3-2. α값의 의미와 선택

α 값	자이로 비중	가속도 비중	특성
0.90	90%	10%	가속도 영향 큼 · 노이즈 증가 가능
0.95	95%	5%	균형적
0.98	98%	2%	✅ 이 프로젝트 선택 · 부드럽고 드리프트 억제
0.99	99%	1%	매우 부드럽지만 드리프트 억제 약해짐

3-3. dt를 micros()로 실측해야 하는 이유

❌ 고정 dt 사용 시 문제

float dt = 0.05f; // 50ms 고정 가정

실제 loop() 실행 시간은 매번 미세하게 다름

예) 49.8ms, 50.2ms, 51.1ms ... → 자이로 적분 오차 누적

✅ micros() 실측 dt 사용

float dt = (float)(micros() - lastMicros) / 1000000.0f;

→ 실제 경과 시간을 마이크로초 단위로 정확히 측정

→ 자이로 적분 오차 최소화

4. Accel값과 Filter값의 차이 — 왜 차이가 나며, 제어에 유리한 점은 무엇인가?

이 장은 처음 상보필터를 접하는 분들이 가장 혼란스러워하는 부분입니다. LCD에서 Accel과 Filter 값이 다르게 표시되는 것을 보고 '코드가 잘못된 거 아닌가?' 하고 의아해하실 수 있습니다. 전혀 그렇지 않습니다!

4-1. 정지 상태에서의 동작

✅ 정지 상태 — 두 값 거의 동일 (정상!)

정지 상태 (펜듈럼이 가만히 있을 때)

Accel ≒ Filter ≒ 0°

두 값이 거의 동일하게 표시됩니다.

→ 이것이 정상 동작입니다. ✅

이유 :

gyroRate ≒ 0 → 자이로 적분 기여 거의 없음

cfAngle = 0.98 × cfAngle + 0.02 × accelAngle

→ 가속도가 천천히 cfAngle을 끌어당겨 거의 일치

4-2. 빠른 움직임에서의 동작

⚡ 빠른 움직임 — Filter가 더 정확

빠르게 흔들 때 (예: 막대를 빠르게 -90°로 기울이면)

Accel : 즉각 -90° 반영 (들쭉날쭉·노이즈 포함)

Filter : 자이로 주도로 부드럽게 -90°를 향해 수렴

→ 이때 두 값의 차이가 발생합니다.

→ 이것도 정상 동작이며 오히려 Filter가 더 정확합니다! ✅

왜 Filter가 더 정확한가?

Accel은 카트 가속도(관성력)가 섞여 실제 각도와 다를 수 있음

Filter는 자이로(98%) 기반이라 관성력 영향을 거의 받지 않음

4-3. 왜 제어에는 Filter가 더 좋은가

🎯 Filter가 제어에 더 좋은 이유

비유 : 두 가지 운전 스타일

Accel(가속도 단독)

→ 신경질적인 운전자

→ 차선 조금만 벗어나도 핸들 급조작

→ 차가 좌우로 흔들리며 불안정

Filter(상보필터)

→ 노련한 운전자

→ 부드럽게 핸들 조작

→ 안정적으로 차선 유지

PID 제어기는 각도 입력값의 노이즈에 매우 민감합니다.

Accel의 순간적인 튐이 PID에 입력되면 → 모터 급발진 → 탈조!

Filter의 부드러운 값이 PID에 입력되면 → 안정적인 제어

4-4. LCD에서 차이가 나 보이는 또 다른 이유

📖 수렴 현상 이해

수렴 현상 — 전원 ON 직후 발생

전원 ON 직후

cfAngle = 현재 가속도 각도로 초기화 (코드에서 처리)

이후 매 50ms마다 가속도를 2%씩만 반영

큰 각도 변화가 있을 때

Accel : 즉각 변화 반영

Filter : 자이로 적분으로 따라가므로 약간 느림

→ 수렴 시간 : 시정수 τ = dt/(1-α) = 0.05/0.02 = 2.5초

→ 약 5~8초 후 안정화

★ 전원 ON 후 10초 기다린 뒤 사용하세요!

★ 이후에는 Accel과 Filter가 거의 일치하며 Filter가 더 부드러움

4-5. 정리 — 세 가지 출력값의 의미

출력값	단위	의미	특성
accelAngle	°	가속도 센서만으로 계산한 각도 atan2(AY,AZ) - angleOffset	즉각 반응 노이즈·관성력 포함
cfAngle (Filter)	°	상보필터 융합 각도 자이로 98% + 가속도 2%	부드럽고 안정적 실제 제어에 사용
gyroRate	°/s	자이로 각속도 (드리프트 보정 후)	막대 쓰러지는 속도 각도가 아닌 속도값!

5. 상보 필터의 한계 — 왜 직립 제어에서 칼만 필터가 필요한가

5-1. 상보 필터가 잘 동작하는 구간

스윙업 구간 (0° ~ ±170°) → 상보필터로 충분

막대가 빠르게 흔들리는 구간

→ 자이로 비중 98%로 빠른 움직임 잘 추적

→ 연산 부하 적음 → Non-blocking 구조에 유리

→ 14편(단계 3-A)~15편(단계 3-B) 내내 상보필터 사용

5-2. 상보 필터의 근본적 한계

❌ 상보 필터의 한계

직립 제어 구간 (±170° ~ ±180°)에서의 문제

① α를 고정값(0.98)으로 사용

→ 모든 상황에서 자이로 98%, 가속도 2% 고정

→ 최적 비율이 상황에 따라 달라지는데 대응 불가

② 자이로 바이어스를 실시간 추정 불가

→ 온도 변화, 시간에 따라 바이어스가 조금씩 변함

→ 상보필터는 setup()에서 한 번 측정한 오프셋만 사용

→ 장시간 동작 시 미세한 드리프트 누적 가능

③ 직립 제어에서의 치명적 문제

±180° 근처에서 0.1°~0.5° 오차도 PID를 불안정하게 만듦

→ 상보필터의 미세한 드리프트가 직립 유지를 방해

5-3. 칼만 필터 — 직립 제어의 해답

칼만 필터(Kalman Filter)는 1960년 루돌프 칼만(Rudolf Kalman)이 개발한 최적 상태 추정 알고리즘입니다. 노이즈가 있는 측정값에서 실제 상태를 수학적으로 가장 정확하게 추정합니다.

✅ 칼만 필터의 장점 (16편에서 구현)

칼만 필터가 상보 필터보다 우수한 점

① 자이로 바이어스를 상태변수로 함께 실시간 추정

→ 온도나 시간에 따른 드리프트 변화도 자동 보정

→ 장시간 동작해도 각도 오차 누적 없음

② 측정 노이즈 Q, R을 통계적으로 최적화

→ 상황에 따라 자이로와 가속도의 신뢰도를 자동 조정

→ 고정 α값보다 정확한 융합

③ 칼만 게인(K)이 매 주기 최적값으로 자동 계산

→ 수동 튜닝 최소화

5-4. 필터 구간별 적용 전략

구간	적용 필터	이유	해당 편
초기화·캘리브	상보필터	연산 부하 최소	11~12편
스윙업 0°~±170°	상보필터	빠른 반응 충분	14~15편
전환 ±170°~±180°	상보→칼만 전환	모드 전환과 동시 필터 전환	15편
직립 PID 제어	칼만 필터	드리프트 없는 정밀 각도	16~17편

📐 칼만 필터 수식 미리보기

칼만 필터 수식 미리보기 (16편에서 구현 예정)

상태변수 : x = [각도, 자이로바이어스]

예측 단계

x⁻ = A·x + B·u (자이로로 각도 예측)

P⁻ = A·P·Aᵀ + Q (오차 공분산 예측)

갱신 단계

K = P⁻·Hᵀ / (H·P⁻·Hᵀ + R) (칼만 게인 계산)

x = x⁻ + K·(z - H·x⁻) (측정값으로 보정)

P = (I - K·H)·P⁻

Q : 자이로 노이즈 분산 / R : 가속도 노이즈 분산

6. 단계 1-B 코드 상세 설명

11편(단계 1-A) 코드에서 추가·변경된 부분만 집중적으로 설명합니다. 나머지는 11편과 완전히 동일합니다.

6-1. 1-A 대비 추가·변경 사항

항목	단계 1-A	단계 1-B (변경)
상보필터	없음	complementaryFilter() 함수 추가
dt 계산	없음	micros() 실측 (lastMicros 변수 추가)
신규 전역변수	—	cfAngle, lastMicros, cfInitialized
시리얼 출력	accelAngle, gyroRate (2개)	accelAngle, cfAngle, gyroRate (3개)
LCD 1행	Stage 1-A Monitor	Stage 1-B Monitor
LCD 2행	Accel 각도	Accel 각도 (유지)
LCD 3행	Gyro 각속도	Filter 각도 (신규)
LCD 4행	AY/AZ 원시값	Gyro 각속도
cfAngle 초기화	없음	캘리브 완료 직후 accelAngle로 초기화

6-2. 핵심 신규 함수 — complementaryFilter()

코드 ① — 상보필터 핵심 함수

// ★ 상보필터 — 1-B 핵심 신규 함수

float complementaryFilter(float accelAngle, float gyroRate) {

// ① dt 실측 (마이크로초 → 초 변환)

unsigned long nowUs = micros();

float dt = (float)(nowUs - lastMicros) / 1000000.0f;

lastMicros = nowUs;

// ② dt 이상값 방지

// 처음 실행이거나 루프가 너무 오래 걸린 경우 기본값 사용

if (dt <= 0.0f || dt > 0.5f) dt = 0.05f;

// ③ 상보필터 수식

// 자이로 98% : 빠른 반응 + 노이즈 억제

// 가속도 2% : 장기 드리프트 방지

cfAngle = CF_ALPHA * (cfAngle + gyroRate * dt)

+ (1.0f - CF_ALPHA) * accelAngle;

// ④ Angle Wrapping : -180° ~ +180° 유지

while (cfAngle > 180.0f) cfAngle -= 360.0f;

while (cfAngle < -180.0f) cfAngle += 360.0f;

return cfAngle;

}

6-3. cfAngle 초기화 — 수렴 시간 단축

코드 ② — cfAngle 초기화

// calibrateAngleOffset() 함수 마지막에 추가

// ★ 상보필터 초기값 설정

// 캘리브 완료 직후 현재 가속도 각도로 cfAngle 초기화

// → 초기화 안 하면 0°에서 실제 각도까지 5~10초 수렴 필요

// → 초기화 하면 처음부터 accelAngle과 거의 일치

int16_t ay0 = 0, az0 = 0;

if (readAccelRaw(ay0, az0)) {

cfAngle = calcAccelAngle(ay0, az0);

}

lastMicros = micros();

cfInitialized = true;

Serial.print("CF initial angle: ");

Serial.println(cfAngle, 2);

접기/펴기

단계 1-B 상보필터 동작 영상 펜듈럼을 천천히·빠르게 흔들 때 Accel vs Filter 비교 / 리밋스위치 비상정지 테스트

7. 완성 코드 전체 ( 코딩 작업에 AI의 도움을 받았습니다.)

실제 하드웨어에서 동작 확인을 마친 단계 1-B 전체 코드입니다. 11편(1-A) 코드에 상보필터를 추가한 형태입니다.

/*

* ================================================================

* Inverted Pendulum — 단계 1-B

* 상보 필터 (Complementary Filter) + Angle Wrapping

*

* 1-A에서 추가된 것

* ① 상보필터 : cfAngle = α×(cfAngle + gyro×dt) + (1-α)×accelAngle

* ② dt = micros() 실측 (고정 dt 사용 금지)

* ③ 시리얼 플로터 : accelAngle vs cfAngle 동시 출력 비교

*

* 1-A에서 유지되는 것

* ① 자이로 드리프트 오프셋 캘리브레이션 (500회 평균)

* ② 가속도 각도 영점 캘리브레이션 (200회 평균)

* ③ Non-blocking loop() 구조

* ④ 리밋스위치 비상정지 + 디바운싱

*

* LCD 표시

* Accel : 가속도 단독 각도 (직관적)

* Filter : 상보필터 각도 (부드러움)

* Gyro : 각속도

*

* 시리얼 플로터 출력 (115200bps)

* accelAngle:xx.xx cfAngle:xx.xx gyroRate:xx.xx

*

* 검증 기준

* ✔ 빠른 회전에서 Filter가 Accel보다 부드러움

* ✔ ±180° 경계에서 Filter 튐(점프) 없음

* ✔ 정지 1분 → Filter 드리프트 ±1° 이내

*

* 작성 : 민선생의 전자공방 2026

* ================================================================

*/

#include <Wire.h>

#include <LiquidCrystal_I2C.h>

// ── 핀 정의 ─────────────────────────────────────

#define MPU_ADDR 0x68

#define PIN_LIMIT 3

#define PIN_EN 6

#define PIN_STEP 5

#define PIN_DIR 4

// ── MPU6050 레지스터 ────────────────────────────

#define REG_PWR_MGT 0x6B

#define REG_SMPLRT 0x19

#define REG_CONFIG 0x1A

#define REG_GYRO_CFG 0x1B

#define REG_ACCEL_CFG 0x1C

#define REG_ACCEL_OUT 0x3B

// ── 캘리브레이션 설정 ───────────────────────────

#define CALIB_SAMPLES 500

#define CALIB_DELAY_MS 2000

// ── 상보필터 설정 (1-B 신규) ────────────────────

#define CF_ALPHA 0.98f // 자이로 98%, 가속도 2%

// ── LCD ─────────────────────────────────────────

LiquidCrystal_I2C lcd(0x27, 20, 4);

// ── 전역 변수 (1-A 유지) ────────────────────────

float gyroOffsetY = 0.0f;

float angleOffset = 0.0f;

bool emergencyStop = false;

unsigned long lastPrintMs = 0;

const unsigned long PRINT_INTERVAL = 50; // 50ms

// ── 전역 변수 (1-B 신규) ────────────────────────

float cfAngle = 0.0f;

unsigned long lastMicros = 0;

bool cfInitialized = false;

// ── 함수 선언 ───────────────────────────────────

bool initMPU6050();

void calibrateGyro();

void calibrateAngleOffset();

bool readAccelRaw(int16_t &ay, int16_t &az);

bool readGyroRaw(int16_t &gy);

float calcAccelAngle(int16_t ay, int16_t az);

float calcGyroRate(int16_t gy);

float complementaryFilter(float accelAngle, float gyroRate);

void checkEmergency();

void lcdPrint2(const char* l1, const char* l2);

void lcdPrint4(const char* l1, const char* l2,

const char* l3, const char* l4);

// ════════════════════════════════════════════════

// MPU6050 초기화 (1-A 동일)

// ════════════════════════════════════════════════

bool initMPU6050() {

Wire.beginTransmission(MPU_ADDR);

if (Wire.endTransmission() != 0) {

Serial.println("ERROR: MPU6050 not found");

return false;

}

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_PWR_MGT); Wire.write(0x00);

if (Wire.endTransmission() != 0) return false;

delay(100);

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_SMPLRT); Wire.write(0x07); // 125Hz

Wire.endTransmission();

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_CONFIG); Wire.write(0x05); // DLPF 10Hz

Wire.endTransmission();

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_GYRO_CFG); Wire.write(0x00); // ±250°/s

Wire.endTransmission();

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_ACCEL_CFG); Wire.write(0x00); // ±2g

Wire.endTransmission();

delay(50);

Serial.println("MPU6050 init OK");

return true;

}

// ════════════════════════════════════════════════

// 자이로 캘리브레이션 (1-A 동일)

// ════════════════════════════════════════════════

void calibrateGyro() {

lcdPrint4("=== CALIBRATING ===",

"MPU6050 Gyro Offset",

"DO NOT MOVE !!!",

"Please wait...");

Serial.println("=== Gyro Calibration Start ===");

Serial.println("DO NOT MOVE the pendulum!");

delay(CALIB_DELAY_MS);

long sumGy = 0;

int16_t gy = 0;

int fail = 0;

for (int i = 0; i < CALIB_SAMPLES; i++) {

if (readGyroRaw(gy)) sumGy += gy;

else fail++;

delay(2);

if ((i + 1) % 50 == 0) {

Serial.print("Calibrating... ");

Serial.print(i + 1); Serial.print("/");

Serial.println(CALIB_SAMPLES);

}

}

gyroOffsetY = (float)sumGy / (float)(CALIB_SAMPLES - fail);

Serial.print("Gyro Offset (raw LSB): "); Serial.println(gyroOffsetY, 4);

Serial.print("Gyro Offset (deg/s) : "); Serial.println(gyroOffsetY / 131.0f, 4);

Serial.println("=== Gyro Calib Done ===");

char buf[21];

snprintf(buf, sizeof(buf), "Offset:%.4f d/s", gyroOffsetY / 131.0f);

lcdPrint4("=== CALIB DONE ===", buf, "Starting...", "");

delay(1500);

}

// ════════════════════════════════════════════════

// 각도 영점 캘리브레이션 (1-A 동일)

// ★ 캘리브 완료 후 cfAngle 초기값 설정

// ════════════════════════════════════════════════

void calibrateAngleOffset() {

lcdPrint4("=== ANGLE ZERO ====",

"Hang pendulum DOWN",

"DO NOT MOVE !!!",

"Please wait...");

Serial.println("=== Angle Offset Calibration ===");

Serial.println("Pendulum must hang freely at BOTTOM.");

delay(CALIB_DELAY_MS);

const int ANG_SAMPLES = 200;

double sumAngle = 0.0;

int fail = 0;

for (int i = 0; i < ANG_SAMPLES; i++) {

int16_t ay = 0, az = 0;

if (readAccelRaw(ay, az))

sumAngle += atan2f((float)ay, (float)az) * (180.0f / PI);

else

fail++;

delay(5);

}

angleOffset = (float)(sumAngle / (double)(ANG_SAMPLES - fail));

Serial.print("Angle Offset: "); Serial.print(angleOffset, 4);

Serial.println(" deg");

Serial.println("=== Angle Offset Done ===");

char b1[21];

snprintf(b1, sizeof(b1), "Offset:%+.2f deg", angleOffset);

lcdPrint4("=== ZERO DONE ====", b1, "Down=0 Up=+-180", "Starting...");

delay(1500);

// ★ 상보필터 초기값 설정

// 캘리브 완료 직후 가속도 각도로 cfAngle 초기화

// → 첫 루프에서 튐 방지

int16_t ay0 = 0, az0 = 0;

if (readAccelRaw(ay0, az0)) {

cfAngle = calcAccelAngle(ay0, az0);

}

lastMicros = micros();

cfInitialized = true;

Serial.print("CF initial angle: "); Serial.println(cfAngle, 2);

}

// ════════════════════════════════════════════════

// 가속도 읽기 (1-A 동일)

// ════════════════════════════════════════════════

bool readAccelRaw(int16_t &ay, int16_t &az) {

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_ACCEL_OUT);

if (Wire.endTransmission(false) != 0) return false;

if (Wire.requestFrom(MPU_ADDR, 6, true) != 6) return false;

int16_t ax = (Wire.read() << 8) | Wire.read(); (void)ax;

ay = (Wire.read() << 8) | Wire.read();

az = (Wire.read() << 8) | Wire.read();

return true;

}

// ════════════════════════════════════════════════

// 자이로 읽기 (1-A 동일)

// ════════════════════════════════════════════════

bool readGyroRaw(int16_t &gy) {

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x43);

if (Wire.endTransmission(false) != 0) return false;

if (Wire.requestFrom(MPU_ADDR, 4, true) != 4) return false;

int16_t gx = (Wire.read() << 8) | Wire.read(); (void)gx;

gy = (Wire.read() << 8) | Wire.read();

return true;

}

// ════════════════════════════════════════════════

// 가속도 각도 계산 (1-A 동일)

// 영점 보정 + Angle Wrapping

// ════════════════════════════════════════════════

float calcAccelAngle(int16_t ay, int16_t az) {

float raw = atan2f((float)ay, (float)az) * (180.0f / PI);

float angle = raw - angleOffset;

while (angle > 180.0f) angle -= 360.0f;

while (angle < -180.0f) angle += 360.0f;

return angle;

}

// ════════════════════════════════════════════════

// 각속도 계산 (1-A 동일)

// ════════════════════════════════════════════════

float calcGyroRate(int16_t gy) {

return ((float)gy - gyroOffsetY) / 131.0f;

}

// ════════════════════════════════════════════════

// ★ 상보필터 (1-B 핵심 신규)

//

// 수식

// cfAngle = α × (cfAngle + gyroRate × dt)

// + (1-α) × accelAngle

//

// α = 0.98 : 자이로 98% + 가속도 2% 융합

//

// dt = micros() 실측

// ★ 고정 dt 절대 사용 금지

// ★ 실제 경과 시간으로 계산해야 정확

// ════════════════════════════════════════════════

float complementaryFilter(float accelAngle, float gyroRate) {

unsigned long nowUs = micros();

float dt = (float)(nowUs - lastMicros) / 1000000.0f;

lastMicros = nowUs;

// dt 이상값 방지 (너무 크거나 작으면 기본값 사용)

if (dt <= 0.0f || dt > 0.5f) dt = 0.05f;

// 상보필터 수식

cfAngle = CF_ALPHA * (cfAngle + gyroRate * dt)

+ (1.0f - CF_ALPHA) * accelAngle;

// Angle Wrapping : -180° ~ +180° 유지

while (cfAngle > 180.0f) cfAngle -= 360.0f;

while (cfAngle < -180.0f) cfAngle += 360.0f;

return cfAngle;

}

// ════════════════════════════════════════════════

// 비상정지 (1-A 동일)

// ════════════════════════════════════════════════

void checkEmergency() {

static bool lastState = LOW;

static unsigned long lastMs = 0;

bool cur = digitalRead(PIN_LIMIT);

if (cur == HIGH && lastState == LOW) {

if (millis() - lastMs > 20) {

lastMs = millis();

lastState = HIGH;

emergencyStop = true;

digitalWrite(PIN_EN, HIGH);

Serial.println("!!! EMERGENCY: Limit switch triggered !!!");

lcdPrint2("!!! EMERGENCY !!!", "Limit SW triggered");

}

}

if (cur == LOW) lastState = LOW;

}

// ════════════════════════════════════════════════

// LCD 헬퍼 (1-A 동일)

// ════════════════════════════════════════════════

void lcdPrint2(const char* l1, const char* l2) {

lcd.clear();

lcd.setCursor(0, 0); lcd.print(l1);

lcd.setCursor(0, 1); lcd.print(l2);

}

void lcdPrint4(const char* l1, const char* l2,

const char* l3, const char* l4) {

lcd.clear();

lcd.setCursor(0, 0); lcd.print(l1);

lcd.setCursor(0, 1); lcd.print(l2);

lcd.setCursor(0, 2); lcd.print(l3);

lcd.setCursor(0, 3); lcd.print(l4);

}

// ════════════════════════════════════════════════

// setup()

// ════════════════════════════════════════════════

void setup() {

Serial.begin(115200);

Wire.begin();

Wire.setClock(400000);

lcd.init();

lcd.backlight();

lcdPrint2("IP Stage 1-B", "Initializing...");

pinMode(PIN_LIMIT, INPUT_PULLUP);

pinMode(PIN_EN, OUTPUT);

pinMode(PIN_STEP, OUTPUT);

pinMode(PIN_DIR, OUTPUT);

digitalWrite(PIN_EN, HIGH);

digitalWrite(PIN_STEP, LOW);

Serial.println("================================");

Serial.println(" Inverted Pendulum Stage 1-B");

Serial.println(" Complementary Filter");

Serial.println("================================");

if (!initMPU6050()) {

lcdPrint2("MPU6050 ERROR!", "Check wiring A4/A5");

Serial.println("HALT: MPU6050 init failed.");

while (true) delay(1000);

}

calibrateGyro();

calibrateAngleOffset(); // ★ 내부에서 cfAngle 초기값 설정

lcdPrint4(" Stage 1-B Filter",

"Rotate pendulum!",

"Check SerialPlotter",

"115200bps");

Serial.println("=== Monitoring Start ===");

Serial.println("Format: accelAngle:xx.xx cfAngle:xx.xx gyroRate:xx.xx");

Serial.println("");

}

// ════════════════════════════════════════════════

// loop() — Non-blocking 구조

// [0순위] 비상정지

// [1순위] 50ms — MPU 읽기 + CF 계산 + 시리얼 출력

// [2순위] 200ms — LCD 갱신

// ★ delay() 없음 ★ lcd.clear() 없음

// ════════════════════════════════════════════════

void loop() {

checkEmergency();

if (emergencyStop) return;

unsigned long nowMs = millis();

if (nowMs - lastPrintMs >= PRINT_INTERVAL) { // 50ms

lastPrintMs = nowMs;

int16_t ay = 0, az = 0, gy = 0;

if (!readAccelRaw(ay, az) || !readGyroRaw(gy)) {

Serial.println("WARNING: MPU6050 read failed");

return;

}

float accelAngle = calcAccelAngle(ay, az);

float gyroRate = calcGyroRate(gy);

float cf = complementaryFilter(accelAngle, gyroRate);

// 시리얼 플로터 출력 (세 선 동시 비교)

Serial.print("accelAngle:"); Serial.print(accelAngle, 2);

Serial.print(" cfAngle:"); Serial.print(cf, 2);

Serial.print(" gyroRate:"); Serial.println(gyroRate, 2);

// LCD 갱신 200ms마다

static unsigned long lastLcdMs = 0;

if (nowMs - lastLcdMs >= 200) {

lastLcdMs = nowMs;

char b1[21], b2[21], b3[21], b4[21];

snprintf(b1, sizeof(b1), "Stage 1-B Monitor ");

snprintf(b2, sizeof(b2), "Accel:%7.2f deg ", accelAngle);

snprintf(b3, sizeof(b3), "Filter:%6.2f deg ", cf);

snprintf(b4, sizeof(b4), "Gyro :%7.2f d/s ", gyroRate);

lcd.setCursor(0, 0); lcd.print(b1);

lcd.setCursor(0, 1); lcd.print(b2);

lcd.setCursor(0, 2); lcd.print(b3);

lcd.setCursor(0, 3); lcd.print(b4);

}

}

}

/*

* ================================================================

* 검증 체크리스트

*

* □ 시리얼 모니터 115200bps → "MPU6050 init OK" 확인

* □ 캘리브 완료 → "CF initial angle" 출력 확인

*

* 시리얼 플로터에서 확인

* □ 정지 → accelAngle ≒ cfAngle ≒ 0.00°

* □ 천천히 회전 → accelAngle과 cfAngle 거의 동일

* □ 빠르게 흔들기 → Filter가 Accel보다 훨씬 부드러움 ← 핵심!

* □ ±180° 경계 통과 → cfAngle 튐(점프) 없음

* □ 정지 1분 → cfAngle 드리프트 ±1° 이내

*

* LCD에서 확인

* □ Accel과 Filter 값이 동시 표시

* □ 빠른 흔들림에서 Filter가 더 안정적

*

* ★ 모두 통과 → 13편 단계 2 오토 호밍 진행!

* ================================================================

*/

8. 동작 확인 & 검증

8-1. 전원 ON 후 순서

순서	LCD 표시	시리얼 출력	소요시간
①	IP Stage 1-B Initializing...	MPU6050 init OK	약 0.2초
②	=== CALIBRATING === DO NOT MOVE!!!	=== Gyro Calibration Start === Calib 50/500 ... 500/500	약 3~4초
③	=== CALIB DONE === Offset: x.xx	Gyro Offset LSB: x.xxxx	약 1.5초
④	=== ANGLE ZERO ==== Hang pendulum DOWN	=== Angle Offset Calib ===	약 3초
⑤	=== ZERO DONE ==== Offset: +x.xx deg	CF initial angle: x.xx	약 1.5초
⑥	Stage 1-B Monitor Accel/Filter/Gyro 표시	accelAngle:x cfAngle:x gyroRate:x	이후 계속

8-2. LCD 출력 확인

단계 1-B LCD 실제 출력 — Accel 0.03° / Filter -0.46° / Gyro -0.12 d/s

LCD 표시 설명

Stage 1-B Monitor ← 1행 : 단계 표시

Accel: 0.03 deg ← 2행 : 가속도 단독 각도 (직관적·즉각 반응)

Filter: -0.46 deg ← 3행 : 상보필터 각도 (부드럽고 안정적)

Gyro : -0.12 d/s ← 4행 : 각속도 (막대가 얼마나 빨리 기우는가)

Accel과 Filter 차이가 0.49° → 정지 상태에서 거의 일치 ✅

gyroRate ≒ 0 → 거의 정지 상태 ✅

시리얼 플로터 화면 accelAngle / cfAngle / gyroRate 세 선 동시 표시

8-3. 검증 체크리스트

✅ 동작 확인 체크리스트

□ 시리얼 모니터 → 'MPU6050 init OK' 확인

□ 'CF initial angle' 출력 확인 (캘리브 후 초기각도)

□ 전원 ON 후 10초 대기 → cfAngle 수렴 확인

□ 정지 상태 → Accel ≒ Filter ≒ 0.00° (차이 < 1°)

□ 천천히 회전 → Accel과 Filter 거의 동일하게 추종

□ 빠르게 흔들기 → Filter가 Accel보다 훨씬 부드러움 ← 핵심!

□ ±180° 경계 통과 → Filter 튐(점프) 없음

□ 정지 1분 → cfAngle 드리프트 ±1° 이내

□ 리밋스위치 누르기 → EMERGENCY 출력 확인

★ 모두 통과 → 13편 단계 2 오토 호밍 진행!

9. 마무리

이번 12편에서는 11편의 가속도 단독 각도에 상보 필터를 더해 훨씬 부드럽고 안정적인 각도를 얻었습니다. 자이로의 빠른 반응성과 가속도의 장기 안정성을 α = 0.98 비율로 융합한 결과입니다.

Accel값과 Filter값이 차이 나는 것을 보고 혼란스러우셨던 분들도 이제는 이해하셨을 겁니다. 그 차이가 오히려 상보필터가 제대로 동작하고 있다는 증거이며, PID 제어에는 Filter값이 훨씬 더 적합합니다.

이번 편에서 완성한 것

✅ 상보 필터 원리 이해 (α = 0.98, 자이로 98% + 가속도 2%)

✅ dt = micros() 실측 적용

✅ complementaryFilter() 함수 구현

✅ cfAngle 초기화 (수렴 시간 단축)

✅ Angle Wrapping 필터 결과에도 적용

✅ Accel vs Filter 차이 이유 및 제어 적합성 이해

✅ 상보 필터 한계 & 칼만 필터 필요성 이해

✅ 실제 하드웨어 동작 확인 완료!

📖 보충 설명: 상보필터의 '수렴 현상'과 시스템의 안정화

Accel vs Filter — 상보필터가 실제 각도에 수렴하는 과정

영상을 보시면 전원을 켠 직후 Filter 값이 Accel 값을 즉시 따라가지 않고 천천히 쫓아가는 것을 볼 수 있습니다. 처음 접하는 분들은 이를 보고 "필터가 느린 것 아닌가?"라고 생각할 수 있습니다. 하지만 이는 상보필터가 정상적으로 동작하고 있다는 가장 확실한 증거입니다.

① 왜 처음에 천천히 수렴하는가? (초기 수렴 현상)

상보필터의 수식 cfAngle = 0.98 * (cfAngle + gyroRate * dt) + 0.02 * accelAngle을 보면, 가속도 값을 단 2%만 반영합니다. 전원 ON 직후에는 cfAngle이 아직 실제 각도를 모르는 상태이므로, 가속도 데이터를 조금씩 흡수하며 실제 각도에 도달하기까지 시간이 필요합니다.

• 이론적 수렴 시간: 시정수 τ = dt / (1 - α) = 0.05 / (1 - 0.98) = 0.05 / 0.02 = 2.5초

• 완전 안정화: 완전 안정화 시간 T = τ × (3 ~ 4배) ≒ 7 ~ 10초

시정수의 약 3~4배인 약 7~10초가 지나야 Filter 값이 실제 각도와 일치하게 됩니다.

💡 데이터로 확인하는 수렴 과정 (코드 기반 계산값)

아래 표는 CF_ALPHA = 0.98, dt = 50ms 코드 설정을 기반으로 계산한 이론적 수렴값입니다.

Accel은 0초에 즉시 목표 각도(90°)에 도달하지만, Filter는 자이로 98%를 신뢰하는 특성상 지수함수 곡선을 그리며 천천히 수렴합니다.

시간 (s)	Accel값 (°)	Filter값 (°)	상태 설명
0.0	90.0	1.8	펜듈럼 90° 급가속 시작
1.0	90.0	31.1	초기 빠른 수렴 구간
2.5	90.0	56.9	1차 시정수(τ) 도달 — 63% 수렴
5.0	90.0	78.3	2차 시정수 — 87% 수렴
7.5	90.0	85.7	3차 시정수 — 95% 수렴
10.0	90.0	88.4	실용적 안정화 완료 (≒98%)

데이터로 확인하는 수렴 과정 (코드 기반 계산값)

수렴 시간 계산 시정수 τ = dt / (1 - α) = 0.05 / 0.02 = 2.5초

완전 안정화 시간 T = τ × (3~4배) ≒ 7~10초

★ 10초 후에도 Filter값은 88.4°로 목표값 90°에 완전히 도달하지 않습니다.

이것은 α = 0.98의 수학적 특성으로, 이론적으로 완전한 100% 수렴은 무한대의 시간이 필요합니다.

하지만 실제 제어에서는 88° 이상이면 충분히 정확한 값으로 사용 가능합니다.

② 실제 제어에서도 느린가? (움직임 속의 즉각성)

영상 속 모습은 정지 상태에서 각도를 급격히 바꿀 때의 '초기 수렴'일 뿐입니다. 실제 제어 상황은 다릅니다.

• 정지 상태: 자이로의 각속도 데이터(gyroRate)가 0이므로 필터는 매우 차분하게 움직입니다.

• 스윙업 및 PID 제어: 펜듈럼이 움직이기 시작하면 자이로 데이터가 실시간으로 입력됩니다. 상보필터는 자이로 비중이 98%이므로, 막대의 움직임을 즉각적으로 적분하여 Accel보다 훨씬 빠르고 부드럽게 각도를 추종합니다.

• 결론: '초기 수렴'만 느릴 뿐, 실제 주행 중에는 가장 빠르고 정확한 값을 제공합니다. 이것이 우리가 상보필터를 사용하는 이유입니다.

③ 시스템 엔지니어링 관점: '상태 머신(State Machine)'의 필요성

이러한 물리적 특성 때문에 우리는 제어 시스템 설계 시 상태 머신(State Machine) 기법을 도입해야 합니다. 시스템이 즉시 제어를 시작하는 것이 아니라, 센서 데이터가 수학적으로 신뢰할 수 있는 상태에 도달할 때까지 단계를 나누어 관리하는 것입니다.

• STATE_WARMING_UP (안정화 대기 단계): 전원 인가 후 약 10초간 각도 데이터를 수렴시키는 단계입니다. 이 동안에는 호밍이나 PID 제어를 하지 않고 대기합니다.

• STATE_HOMING (기구적 중심 확립): 필터 수렴이 완료된 후, 기구적 중심을 잡는 오토 호밍을 시작합니다.

• STATE_READY (제어 준비 완료): 모든 좌표계가 물리적으로 일치된 상태에서 PID 제어를 진입합니다.

다음 13편에서는 이 로직을 코드에 적용하여, 펜듈럼 스스로 10초간 '웜업(Warming-up)'을 하고 정확한 원점을 찾아가는 모습을 구현해 보려 합니다.

민선생의 Tip: >

다음편의 오토 호밍은 **'기구적 중심'**을 잡는 것이고, 본 편의 상보 필터는 **'좌표계의 중심'**을 잡는 것입니다. ^^

▶ 다음 편 예고

▶ 다음 편 예고 — 13편 : 단계 2 오토 호밍 & 중앙 정렬

드디어 스텝모터를 움직입니다!

LM 가이드 양끝을 스스로 터치하고 정중앙으로 이동하는

CNC·3D 프린터와 동일한 오토 호밍 알고리즘을 구현합니다.

• CW 이동 → 리밋 터치 → Back-off → leftPos = 0 확정

• CCW 이동 → 반대쪽 리밋 터치 → maxPos 확정

• 중앙 = maxPos / 2 → 이동 후 정지

• Non-blocking 스텝 펄스 생성 첫 적용

민선생의 전자공방 · 브레드보드 활용 IO 인터페이스 12편 · 2026

[함께 읽으면 좋은 브레드보드 IO 시리즈]

• 10편: [인버티드 펜듈럼 로드맵 & 하드웨어 구성] 바로가기

[브레드보드 IO 10편] 🤖 아두이노 R4로 세우는 인버티드 펜듈럼(역진자)— 하드웨어 구성 & 프로젝트 로드맵 —

■ 프롤로그 안녕하세요, 민선생입니다. 지금까지 브레드보드 활용 IO 인터페이스 시리즈를 통해 I2C ...

blog.naver.com

• 11편: [MPU-6050 캘리브레이션 & 정확한 각도 출력] 바로가기

[브레드보드 IO 11편]📐 인버티드 펜듈럼 (단계 1-A): MPU-6050 캘리브레이션과 정확한 각도 출력

■ 프롤로그 안녕하세요, 민선생입니다. 지난 10편에서 인버티드 펜듈럼 프로젝트의 전체 하드웨어 구성과 ...

blog.naver.com

• 다음 편(13편):

[브레드보드 IO 13편], 🏁 인버티드 펜듈럼 [단계 2-A] 스텝모터 한계 실측 170mm/s! — 오토 호밍 · 7단계 속도 테스트 · 관성탈조 예방 —

■ 프롤로그 안녕하세요, 민선생입니다. 지난 12편에서 상보필터로 부드럽고 정확한 각도를 완성했습니다. ...

blog.naver.com

태그#아두이노#아두이노R4#인버티드펜듈럼#역진자#MPU6050#상보필터#ComplementaryFilter#자이로센서#가속도센서#센서융합#데이터드리프트#마이크로초#제어공학#로봇제어#브레드보드IO#include#define 태그수정