Stretch-ICP: A Continuous-Trajectory Registration and Deskewing Algorithm in Scenarios of Aggressive Motions

기술 요약: 격렬한 운동 시나리오에서의 연속 궤적 등록 및 왜곡 제거 알고리즘 Stretch-ICP

문제 제기
복잡한 환경에서의 로봇 자율성은 "격렬한 운동"(급격한 변화, 충격, 예측 불가능성이 특징인 상태, 예를 들어 로봇이 언덕을 굴러떨어지는 경우) 으로 인해 자주 저해됩니다. 이러한 조건은 극심한 가속도와 각속도를 유발하여 센서 측정을 왜곡합니다. 구체적으로, 이는 동시 위치 추정 및 지도 작성 (SLAM) 시스템에서 두 가지 치명적인 실패를 초래합니다:

1. 자이로스코프 포화: 고속 회전 중 각속도는 종종 마이크로 전자 기계 시스템 (MEMS) 자이로스코프의 측정 범위를 초과합니다. 이로 인해 센서가 일정한 최대 값을 보고하는 포화 판독이 발생하여, 운동 추정에 표준 관성 적분이 무용지물이 됩니다.
2. 궤적 불연속성: Iterative Closest Point (ICP) 와 같은 고전적 등록 방법은 전체 스캔에 강체 변환을 적용하여 운동 왜곡을 보정합니다. 이는 전역 포즈 일관성을 향상시키지만, 스캔 경계에서 불연속성을 도입합니다. 이러한 점프는 추정된 선형 및 각속도에서 물리적으로 불가능한 급격한 변동을 초래하여 하위 제어 알고리즘 및 상태 추정의 성능을 저하시킵니다.

기존 데이터셋은 이러한 실패 모드를 테스트할 만큼 충분한 강도를 갖추지 못했으며, 현재 SLAM 알고리즘은 자이로스코프가 포화되거나 충돌 시 유효하지 않은 매끄러운 운동을 가정할 때 완전히 실패하는 IMU 사전 정보에 의존하는 경우가 많습니다.

방법론
저자들은 두 가지 새로운 데이터셋인 Tumbling-Induced Gyroscope Saturation (TIGS) 데이터셋과 High-Resolution Motion Capture (HRMC) 데이터셋의 도입을 지원하여 이러한 과제를 해결하기 위한 두 부분으로 구성된 솔루션을 제안합니다.

1. 포화 인식 각속도 추정 (SAAVE):
   자이로스코프가 포화되었을 때 각속도를 복구하기 위해, 저자들은 자유 낙하 가정을 하에서 가속도계 데이터를 활용합니다. 로봇의 질량 중심 (COM) 과 회전 축에 대한 IMU 의 위치를 모델링하여, 측정된 구심 가속도로부터 각속도를 유도합니다.

   • 주요 가정: IMU 는 회전 축에서 이격되어 있다; 로봇은 자유 낙하 상태이다 (COM 에서의 순 가속도는 0 이다); 회전 축은 측정 간 안정적으로 유지된다; 그리고 한 번에 하나의 자이로스코프 축만 포화된다.
   • 구현: 이 방법은 지렛대 길이의 크기와 구심 가속도 성분을 사용하여 각속도를 추정합니다. 노이즈와 자유 낙하 가정이 유효하지 않은 짧은 기간 (충돌) 을 처리하기 위해, 추정치는 백색 잡음 - 저크 운동 사전 정보를 가진 가우시안 프로세스 (GP) 를 사용하여 평활화됩니다.

2. Stretch-ICP:
   궤적 불연속성을 해결하기 위해, 저자들은 스캔 정렬을 강체 스캔 보정이 아닌 연속 시간 궤적 변형 문제로 취급하는 등록 및 왜곡 제거 알고리즘인 Stretch-ICP 를 도입합니다.

   • 메커니즘: 단일 강체 변환을 스캔에 적용하는 대신, Stretch-ICP 는 "스캔 내" 궤적을 "늘리기" 위해 팩터 그래프를 최적화합니다. 이 팩터 그래프는 세 가지 제약 조건을 포함합니다:
     • 궤적 시작 팩터: 현재 스캔 내 궤적의 시작이 이전 궤적의 끝과 일치하도록 강제하여 연속성을 보장합니다.
     • IMU 팩터: SAAVE 에서의 공분산 출력을 활용하는 IMU 프리적분을 사용하여 연속 상태를 제약합니다.
     • 신장 (Stretching) 팩터: 점 - 평면 매칭 오차에서 도출된 등록 보정을 전체 스캔 내 궤적에 분배하여 관성 제약과 일관성을 유지합니다.

주요 기여

• TIGS 데이터셋: 가파른 언덕을 굴러떨어지는 지각 장비를 32 회 실행하여 구성한 새로운 데이터셋으로, 최대 18.6 rad/s (유사 데이터셋보다 4 배 높음) 의 각속도와 최대 157.8 m/s²의 선형 가속도를 특징으로 합니다. 여기에는 고범위 자이로스코프에서 측정한 실제 각속도 측정이 포함됩니다.
• HRMC 데이터셋: Vicon 모션 캡처 시스템으로 실내에 기록된 데이터셋으로, 궤적 연속성과 속도 추정 오차를 평가하기 위한 고주파수 (200 Hz) 6-DOF 실제 값을 제공합니다.
• SAAVE: 자이로스코프 포화 기간 동안 각속도를 추정하는 방법으로, 포화된 센서 판독치에 비해 추정 오차를 83.4% 감소시킵니다.
• Stretch-ICP: 스캔 경계 전반에 걸쳐 궤적 연속성을 강제하는 새로운 등록 알고리즘으로, 스캔 경계에서의 선형 및 각속도 오차를 각각 95.2% 와 94.8% 감소시킵니다.

결과
저자들은 표준 ICP 기반 SLAM 과 포화에 강건한 최첨단 방법인 Point-LIO 와 비교하여 TIGS 및 HRMC 데이터셋에서 자신의 방법을 평가했습니다.

• 각속도 복구: SAAVE 는 포화 기간 동안 중앙값 각속도 오차를 83.4% 크게 감소시켰습니다.
• SLAM 강건성: SAAVE 를 SLAM 프레임워크 (SAAVE-ICP-SLAM) 에 통합한 것은 원시 포화 IMU 데이터를 사용하는 것에 비해 중앙값 변위 위치 추정 오차를 71.5%, 회전 오차를 65.5% 개선했습니다. 또한 매핑 실패 (15% 이상의 이상치로 정의됨) 를 37.5%(32 회 중 12 회) 에서 0% 로 감소시켰습니다.
• 궤적 연속성: HRMC 데이터셋에서 SAAVE-ICP-SLAM 과 SAAVE-Stretch-SLAM 을 비교했을 때, Stretch-ICP 는 스캔 경계에서의 중앙값 선형 속도 추정 오차를 4.51 m/s 에서 0.24 m/s 로 (95.2% 개선), 각속도 오차를 1.80 rad/s 에서 0.10 rad/s 로 (94.8% 개선) 감소시켰습니다.
• 트레이드오프: Stretch-ICP 의 사용은 표준 ICP 에 비해 최종 포즈 위치 추정 오차에서 modest 한 증가를 초래했지만, 훨씬 더 매끄럽고 물리적으로 일관된 궤적 및 속도 추정을 제공했습니다.

의의 및 주장
이 논문은 견고한 로봇 자율성을 위해서는 센서 포화와 재구성된 궤적의 물리적 일관성 모두를 해결해야 한다고 주장합니다. 저자들은 굴러떨어지는 것이 극단적인 스트레스 테스트이지만, 제안된 방법들이 충격, 미끄러짐, 단단한 착륙, 그리고 불규칙한 지형의 격렬한 통과와 관련된 실제 시나리오의 강건성을 향상시킨다고 주장합니다.

주요 의의는 SAAVE 와 Stretch-ICP 의 결합에 있습니다:

1. SAAVE: 표준 자이로스코프가 실패할 때 운동 사전 정보를 복구하여 전체 SLAM 발산을 방지합니다.
2. Stretch-ICP: 결과 궤적이 연속적이고 물리적으로 타당하도록 보장하여, 고전적 강체 등록에서 일반적으로 발생하는 속도 급변을 방지합니다.

저자들은 현재 구현이 오프라인 연구 프로토타입이라고 지적합니다. SAAVE 는 가우시안 프로세스 평활화에 의해 계산적으로 지배되며, Stretch-ICP 는 등록 반복마다 최적화 오버헤드를 추가합니다. 따라서 이 작업은 격렬한 역학 하에서 궤적 연속성과 운동 추정 신뢰성의 상당한 향상을 달성하기 위해 최종 포즈 오차의 modest 한 증가를 수용하는 트레이드오프를 강조합니다. 향후 작업은 이러한 방법을 실시간 성능에 최적화하고 조정을 위한 민감도 분석을 수행하는 것으로 식별되었습니다.