산업용 자율주행 핵심 이론을 하나로 잇는 LiDAR-IMU 상태추정 파이프라인

9월 28일에 산업용 장비의 자율주행 기술 동향과 개발 현황을 다룬 강의를 들었어요. 강의는 자율주행 이론을 일곱 묶음으로 나누고, 자율주행 개발 개념을 이해하기 어려운 쪽과 쉬운 쪽으로 갈라 보여 줬어요. 어려운 쪽 이름들은 따로 떨어진 과목처럼 보이지만, 라이다 스캔 한 장을 로봇의 위치로 바꾸는 LiDAR-IMU 상태추정 한 줄기 안에서 각자 자리를 가져요.

강의 슬라이드의 개념 목록을 처리 순서에 맞춰 새로 구성한 흐름도예요. 그림 위에서 확대·이동할 수 있고, 노드를 누르면 연결된 경로만 남아요. 새 탭에서 크게 보기

강의가 나눈 일곱 묶음과 두 목록

첫 슬라이드는 이론을 아래 일곱 묶음으로 정리했어요. 세부 항목은 슬라이드 표기를 그대로 옮겼어요.

묶음	세부 항목
SLAM 기본 이론	Global SLAM, Pose Graph, Submap, Loop Closure
State Estimation	SLAM 알고리즘 안의 Jacobian
LiDAR-IMU Odometry 센서 융합	KD-tree, Voxelization, Point-to-Plane
Kinematics	역기구학
그래프 최적화	Factor Graph
EKF와 Kalman Filter	Prediction, Jacobian, Residual, Update
IMU-Wheel Encoder 센서 융합	공분산

다음 슬라이드는 이해하기 어려운 부분으로 C++, Kalman Filter, Kinematics, Graph 모델, State Estimation, Jacobian, Optimization, Covariance, Residual, KD-tree, LIO, Point-to-Plane, Voxelization을 꼽았어요. 쉬운 부분으로는 ROS 토픽 송수신, TF tree, lifecycle management, BT Navigator, ROS 토픽에서 bridge를 거쳐 FMS로 가는 송수신 프로토콜을 꼽았어요.

두 목록은 맡은 일이 달라요. 쉬운 쪽은 데이터가 어느 노드에서 어느 노드로, 어떤 좌표계로 흐르는지를 정하는 연결 계층이라 문서와 예제를 따라가면 동작을 눈으로 확인할 수 있어요. 어려운 쪽은 그 데이터로 로봇이 지금 어디에 있고 그 추정을 얼마나 믿을 수 있는지를 계산하는 수치 계층이라, 식 하나를 모르면 파라미터를 바꿨을 때 결과가 왜 달라지는지 설명할 수 없어요. 언어인 C++을 뺀 나머지 열두 개는 위 흐름도의 단계 안에서 자리를 찾아요. 흐름도에 노드로 그린 것은 복셀화·KD-tree·점-평면 잔차·칼만 갱신·포즈 그래프이고, 자코비안·공분산·최적화·기구학은 그 단계들 안에서 쓰이는 계산이에요.

스캔을 줄이고 대응점을 찾는 전처리

3D 라이다 스캔은 한 바퀴에 점이 수만 개이고, 가까운 벽에는 빽빽하게 먼 곳에는 성기게 찍혀요. 그대로 정합에 넣으면 가까운 벽 하나가 결과를 지배하고 계산도 느려요. 복셀화는 공간을 정해진 크기의 정육면체 격자로 나누고 한 칸에 점 하나(보통 칸 안 점들의 평균)만 남겨, 밀도를 고르게 하면서 점 수를 줄여요. 격자 크기는 지도 해상도와 처리 속도를 맞바꾸는 손잡이예요.

줄인 점마다 지도에서 가장 가까운 점 몇 개를 찾아야 하는데, 지도 점 전체와 하나씩 비교하면 점 수에 비례해 느려져요. KD-tree는 점들을 축마다 번갈아 반으로 나눠 저장해서 평균적으로 로그 시간에 이웃을 찾게 해요. FAST-LIO2는 지도를 새 점이 들어올 때마다 부분만 고치는 ikd-Tree로 관리하고, 저장소 설명에 100Hz가 넘는 라이다 속도를 처리한다고 적어요.

점-평면 잔차와 자코비안

스캔 점 $p$ 를 현재 추정한 회전 $R$ 과 이동 $t$ 로 지도 좌표에 옮긴 뒤, KD-tree로 찾은 이웃 점들에 평면을 맞춰요. 그 평면의 단위 법선을 $n$ , 평면 위 한 점을 $q$ 라 하면 점-평면 잔차는 옮긴 점이 평면에서 수직으로 떨어진 거리예요.

r = n^{⊤} (R p + t - q)

자코비안은 이 잔차가 포즈 여섯 성분(이동 셋, 회전 셋)의 작은 변화에 얼마나 민감한지를 담은 한 줄짜리 행이에요. 회전을 오른쪽에서 작게 흔드는 표기로 쓰면 이동 쪽은 $n^{⊤}$ , 회전 쪽은 $- n^{⊤} R [p]_{\times}$ 가 되고, 점 수만큼 이 행을 쌓은 행렬 $H$ 로 가우스-뉴턴 한 스텝 $H^{⊤} H δ = - H^{⊤} r$ 을 풀어 포즈를 조금씩 고쳐요.

점과 점을 짝짓는 방식은 스캔 점과 지도 점이 같은 지점이라고 가정하지만, 라이다는 매번 조금씩 다른 자리를 찍어요. 점-평면은 평면을 따라 미끄러지는 방향의 어긋남에는 벌점을 주지 않아서, 벽과 바닥이 많은 실내·공장에서 더 적은 반복으로 수렴해요.

자코비안을 보면 정합이 약한 방향도 읽혀요. 긴 복도에서는 벽 법선이 모두 복도 축에 수직이라 모든 행의 축 방향 이동 성분이 0에 가깝고, $H^{⊤} H$ 가 그 방향의 고윳값을 거의 잃어요. 잔차를 줄여 줄 방향 정보가 없으니 축 방향 위치는 관성 센서 적분에만 기대게 돼요. 50m 복도 매핑에서 지도가 부채꼴로 벌어진 원인과 왕복 주행 교정에서 편도 주행 지도가 벌어진 것도 같은 구조의 문제로 읽을 수 있어요.

칼만 필터의 네 단계와 공분산

슬라이드가 EKF 항목에 적은 Prediction, Jacobian, Residual, Update는 필터 한 주기의 순서 그대로예요. 상태 $x$ , 공분산 $P$ , 모델 잡음 $Q$ , 측정 잡음 $R$ 로 쓰면 이렇게 돌아요.

예측 : x^{-} = f (x, u), P^{-} = F P F^{⊤} + Q 잔차 : y = z - h (x^{-}) 갱신 : K = P^{-} H^{⊤} (H P^{-} H^{⊤} + R)^{- 1}, x = x^{-} + K y, P = (I - K H) P^{-}

예측은 IMU의 각속도와 가속도로 상태를 앞으로 굴리고 불확실성을 $Q$ 만큼 키우는 단계예요. 자코비안 $F$ 와 $H$ 는 곡선인 운동 모델 $f$ 와 측정 모델 $h$ 를 현재 추정점 근처에서 직선으로 편 것이고, 잔차는 실제 측정과 예측한 측정의 차이, 갱신은 그 차이를 이득 $K$ 만큼 반영하는 단계예요. LIO에서는 점이 평면 위에 있어야 한다는 뜻으로 측정값을 $z = 0$ 으로 두므로 $y = - r$ 이 되고, 앞 절의 자코비안이 그대로 $H$ 가 돼요. FAST-LIO2는 이 갱신을 한 번에 끝내지 않고 선형화 지점을 옮겨 가며 반복하는 반복 확장 칼만 필터로 라이다와 IMU를 한 필터 안에서 함께 풀어요.

1차원 이득의 움직임은 센서 융합 — 결함이 정확히 반대인 두 센서에서 표로 따라갔고, 여러 차원에서는 행렬 $P$ 의 대각선 바깥 값이 성분 사이의 상관을 담아요.

IMU와 휠 엔코더를 공분산으로 섞는 숫자 예제

휠 엔코더 값은 기구학을 거쳐야 속도가 돼요. 좌우 바퀴 각속도 $ω_{R}, ω_{L}$ , 바퀴 반지름 $r$ , 바퀴 간격 $b$ 인 차동 구동이라면 순기구학은 $v = r (ω_{R} + ω_{L}) /2$ , $ω = r (ω_{R} - ω_{L}) / b$ 이고, 역기구학은 주행 제어기가 낸 $(v, ω)$ 명령을 이 식을 거꾸로 풀어 바퀴 속도로 바꿔요. 바퀴 반지름이 틀리거나 바퀴가 미끄러지면 이 식이 내놓는 속도가 곧 틀린 입력이 돼요.

회전 속도 하나만 두고 숫자로 섞으면 공분산의 역할이 드러나요. 자이로가 0.42 rad/s(분산 0.0025, 표준편차 0.05)를, 휠 엔코더가 0.50 rad/s(분산 0.01, 표준편차 0.1)를 냈다고 가정해요. 자이로 값을 예측으로, 휠 값을 측정으로 두면 이렇게 계산돼요.

단계	식	값
잔차	$y = 0.50 - 0.42$	0.08
이득	$K = 0.0025/ (0.0025 + 0.01)$	0.2
추정	$x = 0.42 + 0.2 \times 0.08$	0.436 rad/s
공분산	$P = (1 - 0.2) \times 0.0025$	0.002

분산이 작은 자이로 쪽으로 80% 기울었고, 합친 분산 0.002는 두 센서 어느 쪽보다도 작아요.

이번에는 진흙에서 바퀴가 헛돌아 휠 값이 틀렸고 참값이 0.42 rad/s라고 가정해요. 추정은 0.016 rad/s만큼 치우치고, 10초면 0.16 rad(약 9.2도)의 방향 오차가 쌓여요. 잔차를 혁신 분산 $S = 0.0125$ 로 나눈 마할라노비스 거리 제곱은 $0.0 8^{2} /0.0125 = 0.512$ 로, 자유도 1의 95% 기각선 3.84보다 한참 작아요. 이상치 게이트로는 걸리지 않는 크기라, 작은 미끄러짐은 상황에 맞게 $R$ 을 키워야 막혀요. 바퀴 넷을 좌우로만 따로 돌리는 스키드 스티어 차체는 회전 자체가 바퀴 미끄러짐으로 이뤄지므로, 휠로 계산한 회전 속도를 구조적으로 덜 믿어야 해요.

서브맵과 포즈 그래프, 루프 클로저

필터는 과거를 공분산 안에 접어 넣고 현재 상태만 고쳐 나가서, 오래 달리면 작은 오차가 쌓인 드리프트를 되돌릴 수단이 없어요. Global SLAM은 이 누적을 궤적 전체 차원에서 다시 맞추는 뒷단이에요. 서브맵은 짧은 구간의 스캔으로 만든 지역 지도로, 그 안에서는 드리프트가 지도 해상도보다 작다고 봐요. 포즈 그래프는 로봇 위치(또는 서브맵 원점)를 노드로, 두 위치 사이의 상대 이동 측정과 그 공분산을 엣지로 둬요.

루프 클로저는 예전에 지나간 장소를 다시 알아보고 멀리 떨어진 두 노드 사이에 새 엣지를 더하는 일이에요. 최적화는 모든 엣지 잔차를 공분산으로 나눈 제곱합을 최소로 만드는 포즈 배치를 찾고, 여기서도 잔차·자코비안·공분산이 필터에서와 같은 자리를 맡아요. 필터와 달리 과거 위치가 변수로 남아 있어서, 루프 클로저 한 번으로 과거 궤적 전체가 다시 펴져요. Cartographer가 앞단에서 서브맵을 쌓고 뒷단에서 루프 클로저 제약을 찾아 포즈 그래프를 다시 푸는 구조가 슬라이드의 네 항목과 그대로 겹쳐요.

팩터 그래프는 이 그래프를 일반화해서 추정할 값(변수 노드)과 측정 제약(팩터 노드)을 나눠 그려요. 상대 이동 말고도 IMU 적분 구간, GPS 절대 위치, 루프 클로저를 모두 같은 형태의 팩터로 넣을 수 있어요. LIO-SAM은 라이다 오도메트리, GPS, IMU 사전적분, 루프 클로저 네 종류 팩터를 GTSAM으로 최적화해요.

노지 추종 운반 로봇에서 흔들리는 가정

강의에서는 사람을 따라가는 농업용 추종 운반 로봇 사례도 다뤘어요. 슬라이드에는 “실내·데모 환경에서는 보이지 않던 문제가 실제 농장에 나가면서 나타납니다”, “알고리즘보다 환경 변화가 더 큰 문제가 되는 순간이 많았습니다”라는 두 문장과 함께 노지 변수 열두 개가 적혀 있었어요. 각 변수를 앞에서 따라온 계산의 가정에 대어 보면 이렇게 갈려요.

현장 변수	흔들리는 가정	걸리는 자리
흙길·진흙, 미끄러짐	바퀴 회전량이 곧 이동량	휠 순기구학, 측정 잡음 $R$
경사·요철	차체가 평면 위를 달림	평면 운동 모델, 중력 보정
나무와 풀	스캔 점이 단단한 면에 맺힘	점-평면 대응, 복셀화
GPS 오차	절대 위치 측정의 잡음이 일정함	측정 갱신, GPS 팩터
센서 노이즈	잡음 크기를 미리 앎	공분산 $Q$ · $R$
햇빛·그림자	영상 밝기가 일정함	카메라 기반 인식
다른 사람	따라갈 대상이 하나	추종 대상 식별
장애물, 갑작스런 정지	경로가 비어 있음	행동 트리 복구
안전 정지	멈춘 뒤 상태가 이어짐	lifecycle 상태 전이
통신 단절	명령과 상태가 끊기지 않음	bridge와 FMS 연결

표의 위쪽 다섯 줄에 든 여섯 변수가 수치 계층의 입력 가정을 흔들어요. 열두 개 가운데 절반이 앞에서 따라온 수치 계층의 가정에 걸린다는 점이 첫 슬라이드와 마지막 사례를 잇는 지점으로 보여요. 나머지 가운데 장애물·갑작스런 정지·안전 정지·통신 단절 네 개는 강의가 쉽다고 꼽은 행동 트리·lifecycle·bridge 쪽에 걸리고, 햇빛·그림자와 다른 사람 두 개는 두 목록 어디에도 없는 인식 계층의 몫이에요.

개념을 공부할 때 식과 함께 그 식이 기대는 가정을 한 줄씩 적어 두면, 현장에서 무엇이 깨졌는지를 식의 자리로 되짚을 수 있어요. 점-평면 잔차는 단단한 면을, 휠 순기구학은 미끄러지지 않는 바퀴를, 측정 잡음 $R$ 은 미리 아는 잡음 크기를 가정해요.

hello, robot