자동 플러그인 충전기 (APC)
템플릿 매칭과 분산 기반 접촉 감지로 95% 이상 성공률을 달성한 자동 플러그 삽입 시스템
firmware

배경
- 목표: 로봇 충전을 위한 자동 플러그 삽입 시스템
- 풀스택 담당: 기구, 전장, 소프트웨어 전체 설계 및 제작
- CAD: 커스텀 부품(base_housing, camera_bracket, gear_carrier, ring_gear) 포함 SOLIDWORKS 어셈블리
- 코드: Python ~1,822줄 (비전/FSM) + C++ ~257줄 (모터 제어)
핵심 문제
- 플러그 삽입에 서브밀리미터 정렬 정밀도가 필요합니다
- Linux에서는 실시간 모터 제어가 불안정합니다 (비결정적)
- 접촉 감지용 힘 센서가 없습니다
- 라즈베리파이에서는 신경망 추론용 GPU를 사용할 수 없습니다
어려운 점: 서브밀리미터 정밀도를 위해서는 Linux가 보장하지 못하는 실시간성이 필요합니다. 접촉 감지는 보통 힘 센서가 필요한데 이 설계에는 없습니다.
핵심 아이디어
힘 센서 없이도 위치 분산을 모니터링해서 접촉을 감지할 수 있습니다. 액추에이터가 고정물을 누르면 모터 진동이 증가해서 위치가 불안정해집니다.
구현:
- RPi4: 비전 처리 + 상위 상태 머신 (Python)
- Arduino Mega 2560: 실시간 모터 제어 (C++)
- 분산 임계값: 1.5 (정상 상태 분산 <0.5의 3배)
접근 방식
1단계: 하드웨어 설계
4축 모션 시스템:
- X, Z축: 8mm 피치 리드 스크류 선형 스테퍼 모터 (약 5마이크로미터/스텝)
- Y축: 2mm 피치 리드 스크류 선형 스테퍼 모터 (약 1.25마이크로미터/스텝)
- Pi축: 각도 정렬용 회전 서보 (STS-3215)
설계 결정: Y축은 속도를 희생하고 정밀도를 위해 더 미세한 피치(8mm 대신 2mm)를 사용했습니다.
2단계: 비전 파이프라인
템플릿 매칭 (OpenCV cv2.TM_CCOEFF_NORMED):
- 포트용 템플릿 9개, 충전기용 템플릿 3개
- 임계값: 0.70~0.80
- 감지 시간: 프레임당 <50ms
- 거리 계산:
1.36 * (156 - port_width)픽셀→mm 변환
설계 결정: YOLOv8 대신 템플릿 매칭을 선택했습니다. YOLOv8 모델은 백업용(ONNX 12MB, TFLite 3.2MB)으로 준비했지만, GPU 없는 RPi에서는 템플릿 매칭이 더 빠릅니다.
3단계: 제어 시스템
충전 시퀀스용 10-상태 FSM: WAIT → XY_PREPARE → Z_APPROX → MOVE_TO_CONTACT → XY_PRECISE → INSERT → CHARGE → CHARGING → DISCHARGE → DISCHARGE_MOVE_BACK
실패 복구: 접촉 감지 타임아웃이 발생하면 후퇴하고 정렬을 재시도합니다. 최대 3회 재시도 후 중단합니다.
바이너리 시리얼 프로토콜:
- 56바이트 데이터 패킷 (Arduino → RPi)
- 6바이트 명령 패킷 (RPi → Arduino)
- 115200 baud, 10 Hz 업데이트 (100ms)
분산 분석으로 접촉 감지:
- 100-샘플 위치 버퍼 (10 Hz에서 10초)
- 정상 상태 분산: <0.5
- 분산 임계값: 1.5 (정상의 3배, 오탐 방지)
- Z축 분산이 임계값을 초과하면 장애물에 부딪힌 것으로 판단
트레이드오프
| 결정 | 이유 | 대가 |
|---|---|---|
| RPi + Arduino 분리 | 실시간 모터 제어와 비전 처리 분리 | 시리얼 통신 복잡도 |
| 템플릿 매칭 우선 | GPU 없이 RPi에서 빠름 | 수동 템플릿 제작 필요 |
| Y축 2mm 피치 | 수직 정밀도 향상 | Y축 이동이 느려짐 |
| 분산 기반 접촉 감지 | 힘 센서 불필요 | 임계값 튜닝 필요 |
| 바이너리 시리얼 프로토콜 | 파싱 효율, 크기 고정 | 텍스트보다 유연성 낮음 |
결과
검증 (100회 이상 연속 테스트):
- 성공률: >95% (실패해도 재시도로 복구)
- 평균 도킹 시간: 15초 (수동 삽입 ~30초 대비)
- 위치 정확도: ±0.5mm (포트 공차 ±2mm의 4배 마진)
테스트 인프라:
- 단위 테스트: test_agent.py, test_detector.py (FSM, 비전 검증)
- 회귀 로그: success.log, fail.log로 실패 패턴 분석
핵심 교훈
- 컨트롤러 분리로 유연한 비전 처리와 안정적인 실시간 모터 제어를 양립
- 분산 기반 접촉 감지로 힘 센서 없이 통계 분석만으로 접촉 판단
- 템플릿 매칭이 GPU 없이도 정밀 정렬에 충분한 정확도 제공
- 기구 + 전장 + 소프트웨어 전체 통합으로 엔드투엔드 시스템 구현