Files

T

robin 6350661c66 Add LIDAR/SLAM plan and improvements report docs

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>

2026-08-24 16:00:39 +09:00

17 KiB

Raw Blame History

07. Livox Mid-360S 기반 LiDAR SLAM 도입 계획 (Phase 6, 실행 전 검토용)

이 문서는 계획안이다. 코드는 아직 수정하지 않았다. 06-imu-integration-plan.md §Phase 6("장기, 선택")에서 예고된 자율주행 확장을 구체화한 문서로, 아래 내용을 검토하고 방향에 동의하면 그때 실제 작업(ROS2 설치, 패키지 빌드, URDF/ros2_control 작성, 코드 통합)을 시작한다.

7.1 현재 상태 (코드 근거)

로봇에는 Livox Mid-360S가 이미 장착되어 있고 xbox_motor_control.cpp가 Livox SDK2 (livox_lidar_api.h)로 직접 연동하고 있지만, 이건 반응형 장애물 회피 전용이지 SLAM이 아니다.

LidarObstacleDetector::handlePointCloud()(xbox_motor_control.cpp:532)는 패킷으로 들어오는 포인트를 하나씩 받아 전방/후방/좌/우 4개 구역의 최소 거리 스칼라값으로 즉시 축약하고, 원본 포인트는 저장하지 않은 채 버린다. 즉:

항목	현재	SLAM에 필요
포인트클라우드	수신 즉시 4개 거리값으로 축약 후 폐기	누적·저장 필요
자세 추정(6-DOF)	없음 (단순 거리 임계값 반응)	매 스캔마다 위치·자세 추정
지도	없음	포인트클라우드 지도 생성/관리
루프 클로저	해당 없음	누적 드리프트 보정 필요
로봇 모델(TF)	없음 — ROS 개념 자체가 없는 단일 C++ 프로그램이라 `base_link`↔바퀴/센서 간 좌표 관계가 코드 상수로만 흩어져 있음	URDF로 명시해 `robot_state_publisher`가 TF 트리 발행 — Nav2 costmap 풋프린트, RViz 정합, 센서 외부 파라미터(extrinsics)에 모두 필요
속도명령 실행 인터페이스	`setRPMs()` 직접 호출(자체 프로토콜)	Nav2가 내는 `/cmd_vel`을 표준화된 방식으로 받아 실제 바퀴 명령으로 변환하는 레이어 필요

7.2 Mid-360S는 SLAM에 유리한 센서

Mid-360S는 논-리피티티브(non-repetitive) 스캔 방식 솔리드스테이트 라이다로, Livox가 이 시리즈 (Mid-360/Avia 등) 전용으로 FAST-LIO2, Point-LIO 같은 LiDAR-Inertial Odometry(LIO) SLAM 패키지를 공식 지원·공개하고 있다. 지금 쓰는 SDK2도 이미 이 생태계의 표준이라 하드웨어/펌웨어 호환성 문제는 적을 것으로 예상된다.

주의: Mid-360S 내부에는 자체 IMU가 내장되어 있고 LIO 패키지들은 보통 이 내장 IMU(라이다와 시간 동기화됨)를 쓴다. 06번 문서에서 연동한 외부 WitMotion HWT905와는 별개의 센서이니 혼동하지 않는다.

7.3 권장 스택

ROS2 + livox_ros_driver2(Livox 공식 드라이버, Mid-360S 정식 지원)
FAST-LIO2 또는 Point-LIO (Point-LIO가 상대적으로 최신/고속)
URDF/xacro — 휠베이스·트랙폭·휠 반경 등 차체 치수와 Mid-360S/WitMotion IMU 마운트 위치를 정의해 robot_state_publisher로 TF 트리를 발행. Nav2 costmap 풋프린트, RViz 시각화, 센서 외부 파라미터(extrinsics) 계산 등 이후 모든 단계의 기반이 된다.
ros2_control — Nav2가 내는 /cmd_vel을 이 로봇의 실제 4바퀴 RPM으로 바꾸는 실행 레이어를 ROS2 표준 프레임워크 안에서 구현(자세한 내용은 §7.4.4). 스캔매칭·루프클로저·지도관리를 직접 C++로 새로 구현하는 것과 달리, 이 실행 레이어는 이 로봇 고유의 하드웨어/킨매틱스 지식이 필요해 검증된 오픈소스로 대체할 수 없는 부분이다 — xbox_motor_control.cpp의 기존 로직을 ros2_control 플러그인 형태로 옮기는 작업이 된다.
스캔매칭·루프클로저·지도관리를 직접 C++로 새로 구현하는 것은 권장하지 않는다 — 검증된 오픈소스 대비 얻는 이득이 적고, 06번 문서 Phase 1~5a와는 작업 성격이 다른(이미 성숙한 알고리즘을 재발명하는) 리스크가 크다.

7.4 통합 아키텍처 — 결정이 필요한 지점

xbox_motor_control.cpp는 ROS 없이 라이다 SDK를 직접 붙잡는 단일 C++ 프로그램인데, SLAM 스택은 보통 ROS2 기반이라 아래를 먼저 결정해야 한다.

7.4.1 라이다 동시 접근 가능 여부

Livox SDK2는 라이다가 UDP로 데이터를 뿌리는 방식이라, SDK 레벨에서 멀티 클라이언트가 되는지 먼저 확인이 필요하다. 안 되면 SLAM 프로그램만 라이다를 잡고, xbox_motor_control.cpp의 반응형 회피는 SLAM 노드가 만드는 로컬 코스트맵/장애물 토픽을 구독하는 구조로 바꿔야 한다.

7.4.2 통합 방식 두 가지

방식	설명	트레이드오프
(A) Nav2로 완전 이관	ROS2 Navigation Stack이 경로계획+속도명령(`/cmd_vel`)을 전담. 06번 문서 §6.5의 Husky 대응표가 가리키는 정석 구조.	아래 §7.4.3 참고 — 전부 버려지는 게 아니라 레이어가 나뉜다. `/cmd_vel`→바퀴 RPM 실행 레이어는 §7.4.4의 URDF+ros2_control로 표준화해서 구현
(B) 얇은 브릿지	SLAM/Nav2는 지도 생성 + 웨이포인트 좌표만 내주고, `xbox_motor_control.cpp`는 지금 구조(RC + 자체 제어루프) 그대로 두고 웨이포인트를 받아 목표 방향으로 이동하는 자율주행 모드를 추가.	웨이포인트↔RC급 명령 변환 브릿지 코드를 직접 작성해야 함. ROS2 표준 실행 레이어(ros2_control)를 쓰지 않으므로 URDF도 TF/RViz 정합 용도 외에는 필수가 아님

7.4.3 (A)를 선택해도 지금까지의 제어 로직이 사라지지 않는 이유

Nav2가 대체하는 건 "지도 위에서 어디로 가야 하는지 결정하는" 상위 계획 레이어(경로 계획, 웨이포인트 추종, 전역 장애물 회피 → /cmd_vel 생성)뿐이다. /cmd_vel(목표 v_x, omega)을 이 로봇의 특정 하드웨어에 맞는 실제 4바퀴 RPM으로 바꾸는 실행 레이어는 Nav2가 전혀 모르는 영역이라 (A)를 선택해도 계속 필요하다:

Phase 5a(k_skid/effective_w 캘리브레이션) — Nav2는 "이만큼 회전해라"만 던질 뿐, 이 로봇이 스킷스티어라 회전 시 차동을 얼마나 더 세게 걸어야 하는지는 모른다. 이 변환은 계속 우리 코드의 몫이다.
저크 제한 감속 / 급가속 시 반전 방지 — 백EMF 보호, 급정지 시 바퀴 튀는 걸 막는 액추에이터 레벨 안전은 Nav2 담당이 아니다. 계속 필요.
Phase 4(전신 슬립 감지), 걸림/들뜸 진단 — 전류·엔코더·IMU로 보는 바퀴 트랙션 이상은 라이다 기반 회피(Nav2 담당)와 완전히 다른 종류의 안전장치라 계속 필요.
Phase 2(휠+IMU 융합 오도메트리) — 오히려 Nav2/robot_localization이 정확히 이런 형태의 오도메트리 토픽을 입력으로 기대하므로, 없어지는 게 아니라 더 핵심적인 역할을 하게 된다.

실제로 역할이 애매해지는 건 Phase 3(헤딩홀드) 정도다 — RC 수동조종 중 직진 편향 보정용인데, 자율주행 중에는 Nav2의 로컬 플래너가 그 역할을 대신한다. 다만 RC 수동조종 모드를 비상/테스트용 폴백으로 남겨둔다면(대부분의 실제 로봇이 그렇게 한다) 그 모드에서는 계속 쓰인다.

정리하면 (A)를 선택해도 RC 수동조종 관련 부분(CRSF 파싱, 헤딩홀드의 RC 트리거 조건)만 자율주행 중엔 쓰이지 않는 정도지, 킨매틱스/안전진단/오도메트리는 전부 계속 쓰인다.

7.4.4 URDF + ros2_control로 실행 레이어 구현

§7.4.3에서 정리한 "Nav2가 몰라도 계속 필요한" 로직들(Phase 5a 킨매틱스 캘리브레이션, 저크 제한 S-curve, 전신 슬립 감지, 온도 모니터링)은 없어지지 않되, xbox_motor_control.cpp라는 독립 C++ 프로그램에서 ROS2 표준 실행 레이어인 ros2_control로 옮겨 담는 재구성이 필요하다.

URDF: 휠베이스·트랙폭·휠 반경 등 실측 치수와 Mid-360S/WitMotion IMU 마운트 위치를 담아 robot_state_publisher가 TF 트리(base_link → 4개 바퀴 → 라이다/IMU)를 발행하게 한다. 이 URDF 안에 <ros2_control> 태그로 하드웨어 인터페이스를 선언하는 것이 표준 패턴이므로, URDF 작성과 ros2_control 플러그인 작성은 사실상 한 세트로 진행한다.
ros2_control 하드웨어 인터페이스: hardware_interface::SystemInterface를 구현하는 플러그인을 새로 작성해, 기존 xbox_motor_control.cpp가 갖고 있던 다음 로직을 그대로 이식한다 — ZLAC8015D Modbus 통신, 06번 문서 Phase 5a의 k_skid/ effective_w 실측 캘리브레이션, 저크 제한 감속, 전신 슬립 감지, 모터/드라이버 온도 모니터링. 즉 하드웨어와 직접 통신하는 코드는 거의 그대로 재사용하고, 그걸 감싸는 인터페이스만 ros2_control 표준(read()/write() 사이클)에 맞춘다.
컨트롤러 선택: 이 로봇은 4륜 스킷스티어라 ROS2 표준 컨트롤러 중 정확히 맞는 것이 없다. 좌우 바퀴 쌍을 각각 하나의 가상 바퀴로 묶어 diff_drive_controller를 쓰는 방법과, 스킷스티어 전용 커스텀 컨트롤러를 작성하는 방법 중 하나를 골라야 한다 — diff_drive_controller 재사용 쪽이 구현 비용은 적지만, k_skid/effective_w 보정이 컨트롤러 레벨(바퀴 속도 변환)과 하드웨어 인터페이스 레벨(Phase 5a 캘리브레이션 자체) 중 어디서 적용돼야 하는지 먼저 정리해야 이중 보정 같은 실수를 피할 수 있다.
RC 수동조종과의 공존: CRSF 파싱과 헤딩홀드는 §7.4.3에서 확인했듯 계속 필요하므로, 이 부분은 ros2_control 밖(별도 ROS2 노드 또는 하드웨어 인터페이스 내 폴백 모드)에 남기고, RC 브레이크(CH5)가 눌리면 ros2_control 경유 명령보다 우선하도록 안전 우선순위를 명확히 설계한다.
이 작업은 Phase 4(§7.5)에서 (A)를 선택했을 때 실제로 손대는 코드이며, 아직 SLAM/Nav2 경로 판단이 검증되지 않은 Phase 1~3 단계에서는 시작하지 않는다 — §7.5의 "먼저 관측으로 검증 후 개입" 원칙을 여기에도 그대로 적용한다.

7.5 단계적 진행 순서 (Phase 별)

06번 문서와 같은 원칙을 따른다 — 먼저 순수 관측으로 검증하고, 주행 제어에는 그 검증이 끝난 뒤에만 개입한다. 각 Phase는 이전 Phase의 검증 게이트를 통과해야 다음으로 넘어간다.

Phase 1 — 인프라 확인 (관측 없음, 설치/연결 확인만)

Livox SDK2가 멀티 클라이언트를 지원하는지 확인(§7.4.1) — 지원 안 되면 xbox_motor_control.cpp의 기존 반응형 회피(LidarObstacleDetector)와 라이다를 동시에 잡을 수 없다는 뜻이므로, 이후 아키텍처(§7.4.2의 A/B 선택, 회피 로직 재구성 여부)에 직접 영향을 준다. 가장 먼저 확인해야 나머지 계획이 헛수고가 되지 않는다.
ROS2 설치 + livox_ros_driver2 빌드, Mid-360S 데이터가 ROS2 토픽(/livox/lidar, /livox/imu)으로 정상 발행되는지 확인. 아직 SLAM 패키지는 안 돌린다 — 순수 드라이버 연결 확인 단계.
URDF(xacro) 작성: 휠베이스·트랙폭·휠 반경 등 실측 치수와 Mid-360S/WitMotion IMU 마운트 위치를 반영해 robot_state_publisher가 TF 트리(base_link → 바퀴/센서)를 발행하도록 한다. 아직 ros2_control 태그는 비워두거나 mock 하드웨어로 둔다 — 이 단계에서는 모터 제어와 전혀 연결하지 않는다. §7.4.4 참고.
게이트: ros2 topic hz/rviz2로 포인트클라우드가 끊김 없이 정상 주파수로 들어오고, 내장 IMU 토픽도 같이 나오는 것을 확인해야 다음 단계로 넘어간다. RViz에서 라이다 포인트가 URDF 로봇 모델과 정렬되어 보이는지(TF 트리 정합성)도 함께 확인한다.

Phase 2 — 실내/정적 SLAM 검증 (주행 제어 완전히 미개입)

FAST-LIO2 또는 Point-LIO를 빌드해, 로봇을 손으로 들거나 실내에서 천천히 밀며 지도가 쌓이는지 확인한다. 아직 로봇의 모터 제어와는 전혀 연결하지 않는다 — 라이다+SLAM 노드만 독립적으로 돌리는 단계.
확인 항목: 지도 품질(벽/기둥 등 기준점이 흐트러지지 않는지), 제자리로 돌아왔을 때 루프 클로저가 잘 닫히는지, 실시간 처리 속도가 로봇 탑재 컴퓨팅 자원으로 감당되는지.
게이트: 통제된 실내 환경에서 지도/루프클로저가 안정적으로 나와야 다음 단계로 넘어간다. 여기서 불안정하면 그 상태로 야외에 나가봐야 결과가 더 나빠질 뿐이다.

Phase 3 — 로봇 탑재 야외 검증 (여전히 주행 제어 미개입, RC 수동조종과 병행 관측)

실제 로봇에 SLAM 노드를 올리고, RC로 수동 조종하며 지금까지 테스트해온 실야외 노면(파인 보도블럭/아스팔트/잔디)에서 지도/자세추정이 안정적인지 확인한다. 실내 벤치마크보다 훨씬 가혹한 환경(직사광선 반사, 개활지라 루프클로저용 기준점이 적음, 진동)이라 별도 검증이 필요하다.
06번 문서 Phase 2에서 만든 휠+IMU 융합 오도메트리(x, y, fused_yaw)를 대조군으로 병행 로깅해 SLAM 결과와 비교한다 — 이미 두 소스(엔코더, WitMotion IMU)로 한 번 검증해본 경험이 있으니, 세 번째 소스(라이다 SLAM)까지 서로 얼마나 일치하는지 교차검증하면 어느 쪽이 이 환경에서 더 신뢰할 만한지 판단할 근거가 생긴다.
게이트: 야외 실주행 중 SLAM 드리프트가 왕복 경로 기준으로 실용적인 수준(예: 수십 미터 주행에 수십 cm 이내)인지 확인해야 다음 단계로 넘어간다.

Phase 4 — 아키텍처 결정 + 저위험 프로토타입 (모터 제어에 처음으로 연결, 자동 개입은 없음)

§7.4.2의 (A) Nav2 완전 이관 / (B) 얇은 브릿지 중 방향을 확정한다.
(A)라면: Nav2 스택(costmap, 플래너)을 세팅하고, §7.4.4에서 설명한 ros2_control 하드웨어 인터페이스(hardware_interface::SystemInterface)를 작성해 xbox_motor_control.cpp의 ZLAC8015D 통신/킨매틱스/안전로직을 이식하고 URDF의 <ros2_control> 태그로 선언한다. 컨트롤러 (diff_drive_controller 재사용 또는 커스텀 스킷스티어 컨트롤러)까지 붙이되, 경로/속도명령이 실제로 잘 나오는지만 확인 — 아직 ros2_control 노드가 실제 모터로 명령을 내보내지 않도록 하드웨어 인터페이스 안에서 출력을 로그로만 찍고, RViz 등에서 시각적으로만 검증.
(B)라면: 웨이포인트를 받아 방향/거리를 계산하는 브릿지 코드를 작성하되, 처음엔 콘솔에 "이 방향으로 가라"만 출력하고 실제 setRPMs() 호출은 아직 연결하지 않는다. 이 경로는 ros2_control 없이도 진행 가능하다(§7.4.2 참고).
게이트: 두 경우 모두 "이 로봇이 지금 어디로 가야 하는지"에 대한 판단이 사람이 보기에 타당한지부터 확인한 뒤에야 실제 구동을 연결한다 — 잘못된 목표를 실제 바퀴에 바로 연결하면 위험하다.

Phase 5 — 통제된 구역 폐루프 자율주행 첫 테스트

울타리 등으로 막힌 좁은 구역, 저속, 사람이 항시 대기하며 즉시 개입(CH5 브레이크 또는 프로그램 종료)할 수 있는 상태에서 첫 자율주행 테스트를 진행한다.
짧은 웨이포인트(수 미터) 단위로 끊어서 검증 — 한 번에 긴 경로를 맡기지 않는다.
06번 문서 Phase 4(전신 슬립 감지)가 자율주행 중에도 그대로 살아있는지, 즉 SLAM/Nav2 경로를 따라가는 도중에도 슬립 감지 시 속도를 낮추는 안전장치가 정상 작동하는지 반드시 같이 확인한다.
게이트: 짧은 구간에서 반복 검증이 안정적이어야 범위를 넓힌다.

Phase 6 — 범위 확장

더 길고 개방된 구역, 동적 장애물(사람/다른 물체 이동) 대응, Nav2 로컬 코스트맵과 기존 전신 슬립 감지·바퀴 이상 진단을 함께 쓰는 이중 안전 체계로 확장한다.

7.6 규모감

06번 문서의 Phase 1~5a는 전부 xbox_motor_control.cpp 한 파일 안에서의 점진적 추가였다. 이번 작업은 완전히 다른 소프트웨어 스택(ROS2)을 새로 설치·구성하고 별도 패키지들을 빌드해야 하는, 성격이 다르고 규모가 훨씬 큰 작업이다. (A) 경로를 선택하면 SLAM 패키지 도입뿐 아니라 URDF 작성 + xbox_motor_control.cpp의 하드웨어/킨매틱스 로직을 ros2_control 하드웨어 인터페이스로 이식하는 작업까지 포함되므로, 사실상 이 프로그램의 실행 레이어 전체를 ROS2 표준 구조로 재작성하는 셈이다 — §7.4.4 참고.

17 KiB Raw Blame History