fori_zltech_motor_test/doc/06-imu-integration-plan.md

06. IMU 도입을 통한 제어 고도화 — 조사 및 계획 (실행 전 검토용)

이 문서는 계획안이다. 코드는 아직 수정하지 않았다. 아래 내용을 검토하고 방향에 동의하면 그때 xbox_motor_control.cpp에 반영을 시작한다.

갱신 이력: 이 문서에 대한 1차 Q&A가 QnA/q1.md / QnA/a1.md에 정리되어 있다. 개루프/폐루프 계층 구조에 대한 실무 관행, 온라인 ICR 추정 가능성, 헤딩 홀드에 필요한 필터링 수준, IMU 하드웨어(HWT905) 확정 내용이 아래 §6.4, §6.7에 반영되어 있다.

출처: resource/husky.md(Clearpath Husky User Manual 링크), Clearpath robot_localization 연동 자료, Yi, J. et al., "Kinematic Modeling and Analysis of Skid-Steered Mobile Robots With Applications to Low-Cost Inertial-Measurement-Unit-Based Motion Estimation," IEEE Transactions on Robotics, 25(5), 2009 (이하 "Yi 2009"), Pentzer, J. et al., "Model-based Prediction of Skid-steer Robot Kinematics Using Online Estimation of Track Instantaneous Centers of Rotation," Journal of Field Robotics, 2014 (이하 "Pentzer 2014"), 그리고 본 저장소 xbox_motor_control.cpp 현재 구현.

6.1 왜 이 조사를 하는가

Husky(Clearpath Robotics)는 본 로봇과 동일한 스키드 스티어 4WD 실외 플랫폼이다. Husky의 표준 소프트웨어 스택은 휠 오도메트리만 쓰지 않고, robot_localization의 ekf_localization_node로 휠 오도메트리 + IMU를 융합해 /odometry/filtered를 만들고, 이를 헤딩 유지·자율주행의 기준으로 삼는다. 이 구조와 그 이론적 근거(Yi 2009)를 우리 시스템과 비교하면, 현재 우리 제어가 왜 "투박"한지, 그리고 IMU 추가가 실제로 어떤 문제를 해결하는지가 명확해진다.

6.2 현재 구현의 한계 (코드 근거)

#	한계	코드 근거
1	완전 개루프(open-loop) 텔레옵. 조이스틱 입력 → 킨매틱스 수식 → 저크제한 램프 → RPM 지령까지, 로봇이 실제로 그 지령대로 움직였는지 확인하는 피드백이 전무하다.	main() 루프 전체 — target_* 계산에 어떤 실측 모션 피드백도 관여하지 않음
2	헤딩/자세(x, y, θ) 추정 자체가 없다. 주행거리(trip meter)는 4륜 엔코더 델타의 중앙값이라는 스칼라 값 하나뿐이고, 회전 방향/누적 헤딩은 어디에도 저장되지 않는다.	dist_axle 계산부 (엔코더 tick → 거리만 산출, 각속도 적분 없음)
3	고정 트랙폭 공식이 ICR을 로봇 중심으로 가정. k_skid = (track_width + wheelbase²/track_width) / 2는 Yi 2009가 지적하는 "단순화된 근사"이며, 실제 좌/우/로봇 ICR이 중심에서 벗어나는 지면 마찰 효과를 반영하지 않는다.	k_skid, effective_w 계산부
4	매직넘버로 근사 오차를 손땜빵. spin_arc_bias=0.12(제자리 회전 시 스크럽 마찰 완화용 임의 전진 비율), spin_jerk_rate=220(제자리 회전만 저크를 더 낮춤) 등은 모두 "ICR이 실제로는 중심이 아니다"라는 동일 현상을 실측 없이 경험적으로 조정한 값들이다.	코드 상단 파라미터 선언부 주석
5	슬립 진단이 전류만으로 이루어지는 간접 추정. classifyStall/classifyAirPair는 지령 대비 실제 RPM과 전류만 보고 개별 바퀴 상태를 추정한다. 전류는 부하·경사·배터리 전압 새그에도 영향받으므로, "로봇 전체가 실제로 회전/가속하고 있는가"라는 직접적 진단은 불가능하다.	classifyStall(), classifyAirPair()
6	직진 시 편향(drift) 자동 보정 수단이 없다. 좌/우 지면 마찰이 다르면 동일 RPM을 줘도 로봇이 서서히 한쪽으로 틀어지는데, 이를 감지할 헤딩 기준 자체가 없어 순전히 운전자가 조이스틱으로 수동 보정해야 한다.	없음 (기능 자체 부재)

6.3 Yi 2009 논문이 제시하는 제어/추정 방식 (요약)

1. 3-ICR 운동학 모델: 좌측 휠(트랙), 우측 휠(트랙), 로봇 몸체 각각에 별도의 순간회전중심 (ICR_l, ICR_r, ICR_b) 좌표를 부여한다. 기존 "트랙폭 하나로 결정되는" 모델을 일반화한 것으로, (v_l, v_r) → (v_x, ω) 관계가 이 3개 ICR 좌표를 파라미터로 하는 선형식이 된다.
2. 오프라인/온라인 캘리브레이션: ICR 좌표는 고정 상수로 가정하지 않고, 실측 모션(외부 기준 또는 IMU)에 대해 최소자승으로 피팅하거나 EKF 상태의 일부로 추정한다. Pentzer 2014는 이를 한 발 더 발전시켜 속도·지형에 따라 ICR이 실시간으로 변한다고 보고 온라인으로 계속 추정하는 방식을 제안한다.
3. IMU-EKF 융합: 자이로 요레이트는 슬립과 무관하게 로봇의 실제 회전각속도를 직접 측정한다. 이를 ICR 모델의 휠-기반 예측치와 EKF로 결합하면, 저가 스트랩다운 IMU + 휠 인코더만으로도 위치/헤딩 추정 정확도가 크게 향상됨을 실험으로 보인다.
4. 부가 효과 — 잔차 기반 슬립 검출: 휠 기반 ω 예측치와 자이로 실측치의 차이(잔차) 자체가 정량적 슬립 지표가 된다. 이는 §6.2-5에서 지적한 "전류만으로 하는 간접 추정"보다 훨씬 직접적인 신호다.

6.4 단계별 개선 계획 (Phase 1 → 5, 각 단계는 독립적으로 가치가 있고 이전 단계에 얹는 구조)

설계 원칙: 각 Phase는 먼저 관측/로깅으로 검증한 뒤에만 제어 루프에 개입하도록 순서를 잡았다. 검증 없이 곧바로 폐루프 보정을 넣으면, IMU 노이즈나 마운트 문제를 슬립으로 오판해 오히려 제어가 나빠질 위험이 있기 때문이다(§6.6 리스크 참고).

Phase 0 — IMU(HWT905) 정식 매뉴얼 확보 및 레지스터 맵 문서화 (선행 작업)

• IMU 하드웨어는 WitMotion HWT905-RS485로 확정 (근거: QnA/a1.md §Q4 — 9축 AHRS 융합 출력, RS485/Modbus 프로토콜 지원, IP67, 최대 200Hz).
• RS485+Modbus라는 점에서 xbox_motor_control.cpp의 SerialPort(Modbus RTU, CRC16, readReg/writeReg/readRegs)를 상당 부분 재사용 가능할 것으로 예상되나, 정확한 레지스터 주소(각속도/각도/가속도/지자기, 출력레이트, 슬레이브 주소 설정)는 아직 확정하지 못했다 — 확보한 정식 PDF가 이미지 스캔본이라 텍스트 추출이 안 됐기 때문.
• 실행 순서: (1) 텍스트 검색 가능한 정식 매뉴얼을 resource/에 추가 → (2) ZLAC8015D 문서와 동일한 정밀도로 doc/07-imu-hwt905-spec.md(가칭) 작성 → (3) 그 다음에야 Phase 1 코드 작업 착수.
• 버스 분리 여부 결정 필요: ZLAC8015D 드라이버 2대가 쓰는 RS485 버스에 IMU를 슬레이브로 얹을지, 별도 USB-RS485 포트로 분리할지는 실측(3개 슬레이브를 한 버스에 얹었을 때 100Hz 루프의 comm_ms 여유)으로 결정한다(§6.7 참고).

Phase 1 — IMU 연동 + 순수 관측 (제어 루프 미개입)

• Phase 0에서 확정한 레지스터로 HWT905에서 요레이트(Z축 자이로) 최소, 가능하면 roll/pitch도 함께 읽는다. 장착 위치는 차체 무게중심에 근접하고 진동이 적은 곳.
• 별도 스레드에서 100Hz 이상으로 폴링해 값을 전역/원자적으로 공유(기존 LidarObstacleDetector가 이미 별도 스레드+뮤텍스 패턴을 쓰고 있으므로 동일 패턴 재사용 가능).
• 제어 로직은 건드리지 않는다. 요레이트 값을 HUD 한 줄에 추가 표시하고, --log CSV에 imu_gyro_z 컬럼만 추가.
• 목적: 실제 하드웨어 노이즈 특성, 진동 영향, 드리프트/바이어스 크기를 먼저 눈으로 확인. HWT905는 온보드 AHRS 필터를 내장하므로(QnA/a1.md §Q3), 애플리케이션 레벨에서 별도 저역통과/상보필터를 얼마나 더 추가해야 하는지는 이 단계의 실측으로 판단한다.

Phase 2 — 헤딩(θ) 적분 + x,y 오도메트리 노출

• 자이로 요레이트를 단순 적분해 헤딩 θ를 추정하고, 기존 dist_axle(휠 기반 선속도 크기)과 결합해 x, y 위치도 함께 적분.
• 휠 기반 ω(현재 k_skid 공식으로부터 나오는 값)와 IMU 기반 ω를 나란히 HUD/CSV에 출력해 두 추정치가 실제로 얼마나 차이 나는지 정량적으로 확인. 이 차이가 바로 §6.3-4의 슬립 잔차다.
• 아직 제어에는 반영하지 않음 — Phase 1과 마찬가지로 "계측 단계".

Phase 3 — 직진 헤딩 홀드 (첫 폐루프 개입)

• 조향 입력(lx, rx)이 0이고 전진/후진 중일 때, θ를 "락(lock)"하고 IMU 실측 요레이트가 0에서 벗어나면 작은 PI 트림을 좌/우 cmd_*에 더해 편향을 보정.
• 개입 폭에 상한을 두어(예: 최대 트림 RPM 제한) 사용자의 의도적 조향을 절대 압도하지 않게 함.
• 이 단계부터 "운전자가 매번 손으로 직진 편향을 보정하는" 현재의 투박함이 실질적으로 개선됨.

Phase 4 — IMU 기반 전신(whole-body) 슬립 감지 (기존 전류 진단 보완)

• 지령 ω 대비 IMU 실측 ω의 오차가 임계값을 넘으면 "전체 슬립"(예: 젖은 잔디, 빙판, 4륜 동시 헛돎)으로 판정 — 기존 classifyStall/classifyAirPair(개별 휠·전류 기반)와는 독립적인 신호이므로 서로 교차검증하는 2중 진단 체계가 됨.
• 전체 슬립 판정 시 속도를 일시적으로 낮추는 등, 기존 라이다 회피의 speed_scale 메커니즘과 유사한 패턴으로 개입 가능.

Phase 5 — ICR 실측 캘리브레이션으로 매직넘버 제거 (정적 → 온라인 확장)

• 5a. 정적 캘리브레이션 (먼저 검증): 다양한 속도·회전반경 조합으로 주행하며 지령 대비 IMU 실측 ω를 기록, 최소자승으로 유효 트랙폭/ICR 오프셋을 피팅. k_skid, effective_w, 그리고 경험적으로 넣은 spin_arc_bias=0.12 같은 값을 이 실측 피팅값으로 교체하거나 근거를 마련.
• 5b. 온라인 ICR 추정 (선택적 확장, QnA/a1.md §Q2): ICR은 실제로는 경사·무게중심·지형 마찰·트레드 마모에 따라 계속 변하는 값이므로(Pentzer 2014), 5a의 정적 상수 대신 effective_ICR_gain = 실측_요레이트(IMU) / 지령_차동속도를 매 틱 갱신하는 방식으로 확장할 수 있다. 구현/검증 부담이 커지므로 5a가 실외에서 충분히 검증된 뒤에만 착수.
• 커밋 이력(19fb70d Fix curve-steering wheel-reversal at partial throttle, ae3f92f Improve outdoor traction control)에서 반복적으로 나타난 "저속/부분 스로틀에서 커브가 이상하게 도는" 류의 버그들은 상당수가 이 근사식의 부정확성에서 기인했을 가능성이 높다 — 근본 원인 제거 시도.

Phase 6 (장기, 선택) — 자율주행 확장 가능성

• Phase 2에서 만든 x,y,θ 추정을 Livox Mid-360S(이미 장착, 현재는 반응형 회피에만 사용)와 결합하면 SLAM/웨이포인트 주행으로 자연스럽게 확장 가능. 이번 계획의 범위 밖이지만, IMU 도입이 갖는 장기적 가치로 기록해 둔다.

6.5 Husky 실제 소프트웨어 스택과의 대응 관계

Husky (robot_localization 기반)	본 계획의 대응 Phase
휠 인코더 오도메트리 (/husky_velocity_controller/odom)	이미 존재 (readFeedback() 기반 l_fb/r_fb, l_tick/r_tick)
IMU (/imu/data)	Phase 1
ekf_localization_node (휠+IMU 융합 → /odometry/filtered)	Phase 2, 장기적으로 정식 EKF로 고도화 가능
헤딩 기반 주행 안정성	Phase 3
Nav Stack 기반 자율주행	Phase 6 (장기)

6.6 리스크 / 트레이드오프

• 진동 노이즈: 실외 오프로드 특성상 차체 진동이 크다. 저역통과/상보필터 없이 자이로 값을 바로 쓰면 오히려 노이즈를 슬립으로 오판할 수 있다 — Phase 1의 "순수 관측" 단계를 반드시 거쳐 실제 노이즈 특성을 먼저 파악해야 한다.
• 마운트 위치: 무게중심에서 멀거나 진동이 심한 곳에 달면 가속도계 값이 왜곡된다. 요레이트는 위치에 덜 민감하지만 완전히 자유롭지는 않다.
• 추가 통신/스레드 부하: 100Hz 제어 루프에 영향을 주지 않도록 IMU 폴링은 별도 스레드로 분리(라이다와 동일 패턴)하고, 메인 루프는 원자적 읽기만 수행해야 한다.
• 개발/검증 비용: Phase 3 이후의 폐루프 개입은 실외 실주행 테스트로만 튜닝 가능하다 — 단계별로 나눠 각 단계를 충분히 검증한 뒤 다음 단계로 넘어가는 접근이 필요하다.

6.7 결정이 필요한 사항

#	항목	상태
1	목표 범위: Phase 3(헤딩 홀드)까지만 목표로 할지, Phase 6(자율주행 확장)까지 염두에 두고 설계할지 — Phase 2 오도메트리 구조(단순 적분 vs 정식 EKF)에 영향	미결정
2	IMU 하드웨어	결정됨 — WitMotion HWT905-RS485 (QnA/a1.md §Q4)
3	RS485 버스 분리 여부: IMU를 ZLAC8015D와 같은 버스에 얹을지, 별도 포트로 분리할지	미결정 — Phase 1 실측 후 결정
4	시작 지점: Phase 0(매뉴얼 확보/문서화)부터 순차 진행할지, 특정 Phase만 우선 원하는지	미결정

이 문서에 대한 피드백을 반영해 계획을 확정한 뒤 코드 작업을 시작한다.