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

> **이 문서는 계획안이다. 코드는 아직 수정하지 않았다.** 아래 내용을 검토하고 방향에 동의하면
> 그때 `xbox_motor_control.cpp`에 반영을 시작한다.
>
> **갱신 이력**: 이 문서에 대한 1차 Q&A가 [`QnA/q1.md`](../QnA/q1.md) / [`QnA/a1.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만 우선 원하는지 | 미결정 |

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