티스토리 뷰
R2LIVE - 센서 퓨전을 통한 mapping 정밀도 향상
사용 센서: 1) LiDAR 2) IMU 3) Camera
- 시스템 구조
- 고속의 필터 기반 오도메트리와 저속의 factor graph 최적화로 구성
- LiDAR, IMU, 카메라 센서를 긴밀하게(tightly-coupled) 융합
- 실시간 상태 추정과 매핑 수행
- 필터 기반 오도메트리 (Filter-based Odometry)
- Error-state Iterated Kalman Filter를 사용
- LiDAR 측정치:
- 평면 특징점 추출
- point-to-plane residual 계산
- 모션 왜곡 보정(de-skewing)
- 비주얼 측정치:
- FAST 코너 검출
- KLT optical flow로 특징점 추적
- reprojection error 계산
- IMU 측정치로 상태 전파(propagation)
- Factor Graph 최적화
- 슬라이딩 윈도우 방식의 시각적 번들 조정(visual bundle adjustment)
- LiDAR 포즈 제약조건 포함
- IMU pre-integration으로 LiDAR와 카메라 factor 연결
- 시각적 랜드마크와 키프레임 포즈 최적화
- 카메라-LiDAR/IMU 간 시간 오프셋 보정
시간 0ms: LiDAR 데이터 수신
- LiDAR point cloud로 위치 업데이트
- 이 위치가 기준점이 됨
시간 5ms: IMU 데이터 수신
- 이전 위치(0ms)에서 IMU로 새 위치 예측
- 아직 다음 카메라/LiDAR 없어서 예측값만 사용
시간 10ms: IMU 데이터 수신
- 이전 예측(5ms)에서 다시 예측
- 계속 누적되는 중
시간 33ms: 카메라 데이터 수신
- 지금까지 IMU로 예측한 위치를 카메라로 보정
- 새로운 기준점이 됨
시간 35ms: IMU 데이터 수신
- 보정된 위치(33ms)에서 다시 예측 시작
...
- 센서 별 데이터 처리
1) LiDAR
- (k+1)번째 프레임이 LiDAR 데이터일 경우:
* 원본 3D 포인트에서 평면 특징점 추출
* 프레임 내 모션 보정 수행
* 결과를 Lk+1로 저장
- LiDAR 좌표계의 점 Lpj를 글로벌 좌표계로 변환
- 변환식: Gpj = GRIk+1(IRLLpj + IpL) + GpIk+1
* GRIk+1: 글로벌-IMU 회전
* IRL: IMU-LiDAR 회전
* IpL: IMU-LiDAR 위치
- 맵에서 가장 가까운 평면 특징점 검색
- 평면 피팅 수행하여 얻음:
* uj: 평면의 법선 벡터
* qj: 평면 위의 한 점
- 잔차 계산: rl(x̌k+1, Lpj) = ujT(Gpj - qj)
- 측정 노이즈 고려: Lpj = Lpjgt + nj
- 노이즈는 정규분포 가정: nj ~ N(0, Σnj)
- 참값에서의 잔차는 0이 되어야 함
- 선형화: 0 = rl(xk+1, Lpjgt) = rl(x̌k+1, Lpj) + HljδX̌k+1 + αj
- Hlj: 자코비안 행렬
- Σαj: 노이즈의 공분산
2) camera
- (k+1)번째 프레임이 카메라 이미지일 때:
* FAST 코너 검출로 특징점(Ck+1) 추출
* KLT optical flow로 이전 프레임들과 특징점 매칭
* 새로운 특징점은 3D 공간에서 삼각측량(triangulation)
- 2D 특징점: Cps = [us vs]T
- 3D 랜드마크: GPs
- 상태 추정 업데이트에 재투영 오차 사용
- 3D->2D 변환:
* 월드 좌표계 -> 카메라 좌표계 변환
* pin-hole 카메라 모델로 투영
- 잔차: rc = Cps - π(CPs)
(실제 관측된 2D 점과 3D 점을 투영한 위치의 차이)
- 3D 랜드마크의 노이즈: GPs = GPsgt + nPs
- 2D 관측의 노이즈: Cps = Cpsgt + nps
- 둘 다 가우시안 분포 가정
- Taylor 전개로 선형화
- 자코비안 행렬 Hcs 계산
- 노이즈 공분산 Σβs 계산
- Error-state Iterated Kalman Filter
세 가지 항의 합을 최소화:
1) 사전 분포 항: ||x̌k+1 ⊟ x̂k+1 + Hδx̌k+1||²
2) LiDAR 측정 잔차 항: Σ||rl(x̌k+1, Lpj) + Hljδx̌k+1||²
3) 비주얼 측정 잔차 항: Σ||rc(x̌k+1, Cps, GPs) + Hcsδx̌k+1||²
- 통합된 측정 행렬: HT = [Hl1,...,Hlml,HcT1,...,Hcmc]T
- 측정 노이즈 행렬: R = diag(Σα1,...,Σαml,Σβ1,...,Σβmc)
- Kalman 게인: K = (HTR-1H + P-1)-1HTR-1
- 상태 업데이트: x̌k+1 = x̌k+1 ⊞ (...)
수렴 후의 상태 추정값을 사용하여:
1) LiDAR 포인트를 월드 좌표계로 변환 및 맵 업데이트
2) 새로운 비주얼 랜드마크 삼각측량
3) 다음 단계의 시작점으로 활용
- Factor Graph Optimization
- 새로운 키프레임의 비주얼 랜드마크는 초기에 정밀도가 낮음
- 이유: 키프레임 포즈 추정 오차가 있기 때문
- 해결책: Factor Graph 최적화를 통해 정밀도 향상
- 카메라 포즈
- 비주얼 랜드마크 위치
- 카메라와 LiDAR-IMU 서브시스템 간의 시간 오프셋
- 노드:
* 카메라 포즈 (빨간 삼각형)
* LiDAR 포즈 (녹색 사각형)
* 비주얼 랜드마크 (주황색 박스)
- 제약 조건:
* IMU preintegration (파란 원)
* Keypoints constrain (주황색 선)
- VINS-Mono 기반이지만 LiDAR 측정값도 활용
- Sliding window 방식으로 계산 부하 관리
- LiDAR 포즈는 고정하여 최적화 과정을 가볍게 유지
- Error-state Iterated Kalman Filter와 Factor Graph Optimization 차이
Error-state Iterated Kalman Filter (ESKF):
- 실시간 순차적 처리 (Sequential Processing)
- 새로운 측정이 들어올 때마다 즉시 처리
- 이전 추정치를 기반으로 현재 상태 추정
- IMU는 200Hz, 카메라 20Hz, LiDAR 10Hz로 들어오는 데이터를 순차적으로 처리
Factor Graph Optimization:
- 일정 구간의 데이터를 모아서 한꺼번에 처리 (Batch Processing)
- Sliding Window 내의 모든 데이터를 동시에 고려
- 과거의 측정값들도 재활용하여 전체적으로 최적화
- 실시간으로 현재 위치 파악 (ESKF)
- 주기적으로 더 정확한 지도와 경로 생성 (Factor Graph)
- 두 과정이 계속 상호작용하며 정확도 향상
- 전체 과정
1. 센서 데이터 수집
- LiDAR (10~50Hz):
"앞쪽 벽까지 5m, 복도 폭 3m..." 데이터 수집
→ 평면 특징점 추출 (벽, 바닥, 천장 등)
→ 로봇의 움직임 보정
- IMU (높은 주파수):
"x축으로 0.5m/s 이동 중, y축으로 회전 중..."
→ 지속적인 로봇의 움직임 추정
- 카메라 (10~30Hz):
주변 환경 이미지 캡처
→ FAST 코너 검출
→ KLT로 특징점 추적
2. 필터 기반 상태 추정(실시간 처리)
세 가지 처리가 병렬로 진행:
1) LiDAR 처리:
- 평면-평면 매칭으로 위치 추정
- "이 벽은 이전에 본 벽과 같다"
2) IMU 처리:
- 연속적인 상태 예측
- "현재 속도와 방향으로는 여기일 것"
3) 비주얼 처리:
- 특징점 재투영 오차 계산
- "이 모서리가 카메라에서 이렇게 보여야 함"
→ Error-state Kalman Filter로 통합
3. 지도 생성 및 최적화
위치 추정 결과를 바탕으로:
- LiDAR features maps:
* 발견된 평면들을 전체 지도에 추가
* "이 벽의 실제 위치는 여기"
- Visual landmarks:
* 시각적 특징점들의 3D 위치 계산
* "이 모서리점은 공간상에서 이 위치"
- Factor graph optimization:
* 최근 경로와 랜드마크 재조정
* "이전 관측과 현재 관측을 종합해서 보정"
4. 최종 출력
- LiDAR rate odometry: 정밀한 위치 추정
- IMU rate odometry: 빠른 움직임 추정
- Camera rate odometry: 시각적 위치 추정
→ 세 정보를 결합한 신뢰성 높은 로봇 위치 추정
- RADAR data 사용
1)
- IMU (200Hz): 고주파 상태 예측
- RADAR (30Hz): 거리/속도 업데이트
- LiDAR (10Hz): 정밀 기하학적 구속
- Camera (30Hz): 시각적 특징점 추적
시간순 예시:
t=0ms: IMU 데이터
t=5ms: RADAR 데이터 → 속도 업데이트
t=10ms: IMU 데이터
t=20ms: 카메라 데이터 → 시각적 업데이트
t=25ms: RADAR 데이터 → 속도 업데이트
t=30ms: LiDAR 데이터 → 위치 업데이트
- 장점
- 고속 이동 시 속도 추정 향상
- 악천후 상황에서 강인성 확보
- LiDAR/카메라 실패 시 백업 센서로 활용
- 주의사항
- RADAR 데이터의 노이즈 특성 모델링
- 다중 경로 반사 처리
- 센서 간 시간 지연 보상
2)
RADAR의 장점:
- 날씨/조명에 강인
- 직접적인 속도 측정 가능 (도플러 효과)
- 긴 측정 거리
주요 특징점:
- 반사가 강한 물체 (금속 구조물, 기둥 등)
- 속도 정보를 가진 moving target
새로 추가할 부분:
1) RADAR Input (10~20Hz)
- 입력: 반사 강도, 도플러 속도
- 특징점 추출 모듈 필요
2) RADAR Feature Extraction
- CFAR 기반 특징점 검출
- 클러스터링으로 의미있는 타겟 그룹화
- 도플러 정보 처리
3) RADAR Measurement Model
- range-azimuth-doppler 잔차 계산
- 측정 불확실성 모델링
새로운 Factor 추가:
- RADAR Range Factor
- RADAR Doppler Factor
- RADAR-LiDAR Constraint Factor
최적화 수정:
- RADAR 측정 불확실성 가중치 조정
- 도플러 정보를 속도 추정에 활용
1) 단계별 구현:
- RADAR 특징점 추출 구현/테스트
- 측정 모델 구현/테스트
- Kalman Filter 통합
- Factor Graph 통합
2) 센서 캘리브레이션:
- RADAR-IMU 외부 파라미터
- RADAR-LiDAR 시간 동기화
- RADAR 측정 바이어스 추정
3) 성능 최적화:
- RADAR 특징점 매칭 고속화
- 불필요한 특징점 필터링
주의사항
- RADAR 데이터의 노이즈 처리
- 다중 경로 반사 문제 해결
- 이동 물체 처리 전략
- 계산 부하 관리
3)
1. RADAR 데이터 처리
Input: 포인트 단위 데이터 (x,y,z, range, velocity, intensity 등)
↓
A. 정적/동적 포인트 분류
- velocity 값으로 간단히 분류
- doppler_bin 정보로 신뢰도 확인
↓
B. 정적 랜드마크 등록
- intensity가 높은 포인트 위주로 선택
- bearing, range 정보로 불확실성 모델링
2. Error-state Iterated Kalman Filter
상태 벡터 [기존 R2LIVE 상태 + RADAR 외부 파라미터]
↓
A. 측정 모델 추가
- range residual: measured_range와 예측 위치 간 차이
- velocity residual: measured_velocity와 상태 추정 속도 간 차이
↓
B. 불확실성 모델링
- intensity에 따른 측정 신뢰도 조정
- bearing 각도에 따른 불확실성 반영
3. Factor Graph
기존 R2LIVE factor들
↓
추가할 RADAR factor들:
A. RangePointFactor
- 정적 랜드마크까지의 거리 제약
- intensity 기반 가중치 설정
B. VelocityFactor
- 속도 측정치 제약
- doppler_bin 신뢰도 반영
↓
슬라이딩 윈도우 최적화
4. 캘리브레이션
A. 초기 캘리브레이션
- radar frame_id ("ti_mmwave")와 다른 센서 간 변환 행렬 추정
- 간단한 시간 동기화 (topic timestamp 활용)
↓
B. 온라인 미세 조정
- factor graph에서 외부 파라미터 최적화
- 필요한 경우 시간 오프셋 보정
장점:
- 이미 처리된 포인트 데이터를 사용하므로 구현이 단순화됨
- intensity, bearing 정보를 활용해 측정 신뢰도를 더 정확히 모델링 가능
- topic timestamp를 활용해 시간 동기화가 비교적 간단함
주의사항:
- velocity=0이어도 실제로는 움직이는 물체일 수 있으므로, 연속된 프레임에서의 추적이 필요할 수 있음
- bearing 각도에 따라 측정 불확실성이 달라지므로 이를 반영해야 함
- range 값과 x,y,z 좌표 간의 정합성 확인 필요