Invariant Stochastic Filtering on SE(3) for Inertial-Encoder State Estimation of Serial Rigid Manipulators

기술 요약: 직렬 강체 매니퓰레이터의 관성-엔코더 상태 추정을 위한 SE(3) 상의 불변 확률적 필터링

문제 정의
직렬 강체 매니퓰레이터의 정확한 상태 추정에는 기하학적 일관성과 확률적 엄밀함이 동시에 요구된다. 각 링크의 구성은 리 군(Lie group) $SE(3)$에 존재하지만, 고전적인 칼만 필터는 평평한 벡터 공간에서 작동하므로 큰 각도에서의 선형화 오차와 오일러 각 또는 쿼터니언 정식화에 내재된 짐벌 락(gimbal lock) 또는 정규화 제약 문제를 야기한다. 또한, 기존의 기하학적 필터들은 단일 물체 또는 전체 체인을 하나의 단일 상태로 취급하여 링크 수에 대해 세제곱의 계산 비용을 발생시킨다. 결정적인 공백은 IMU 노이즈를 모델링하는 데에도 존재한다. 표준적인 접근 방식은 자이로스코프와 가속도계 노이즈의 서로 다른 물리적 기원, 즉 가속도계 노이즈의 샘플 간격 스케일링과 자이로스코프 노이즈가 비선형 코리올리 역학을 통해 전파되는 상태 의존적 특성을 반영하지 못한다.

방법론
저자들은 임의의 수의 링크를 가진 매니퓰레이터를 위해 $SE(3)$ 리 군 내에서 완전히 정식화된 불변 확장 칼만 필터(IEKF)를 개발한다. 이 방법론은 네 가지 핵심 요소에 기반한다:

1. 기하학적 오차 역학: 필터는 추정치 $(\hat{g}_i, \hat{V}_i)$와 리 대수(Lie algebra) 오차 벡터 $\tilde{\xi}_i$ 사이의 엄격한 분리를 유지한다. 운동학 방정식의 군-아핀 성질(group-affine property) ($\dot{g}_i = g_i V_i$)을 활용함으로써, 선형화된 오차 역학이 자율적(autonomous)임을 입증한다. 이를 통해 리카티 방정식(Riccati equation)이 국소적 근사가 아닌 실제 오차 공분산을 지배하도록 보장한다.
2. 물리적으로 분리된 노이즈 모델: 본 논문은 자이로스코프와 가속도계 채널을 독립적으로 다루는 새로운 노이즈 모델을 도입한다.
   • 가속도계: 특정 힘(specific force)의 측정을 적분하여 병진 트위스트(translational twist)를 얻는다. 결과적인 측정 공분산은 프로세스 노이즈 이산화와 유사하게 샘플 간격 $\Delta t$에 따라 스케일링된다.
   • 자이로스코프 및 코리올리: 자이로스코프 노이즈는 회전 채널에 직접적으로 유입될 뿐만 아니라, 코리올리 항을 통한 비선형 뉴턴-오일러 역학을 통해 전파된다. 이는 정지 상태에서는 소멸하고 트위스트 크기에 따라 증가하는 상태 의존적 프로세스 노이즈 기여를 생성한다.
3. 모듈형 체인 합성: 필터는 링크당 IEKF의 체인으로 구조화된다. 링크 $i$의 예측 공분산은 오직 Adjoint 변환된 공분산을 통해서만 링크 $i-1$의 사후 확률에 의존한다. 이러한 전파 메커니즘은 총 계산 비용이 링크 수에 따라 선형적($O(n)$)으로 확장되도록 하여, 단일 상태 추정의 세제곱 계산 비용을 피한다.
4. 안정성 분석: 저자들은 평균 제곱 의미에서의 **지수적 궁극 유계성(Exponential Ultimate Boundedness, EUB)**을 확립한다. 리 대수 리아푸노프 함수를 사용하여 링크별 경계치를 도출하고, 이를 Adjoint 연산자 노름을 통해 매니퓰레이터 전체에 체인화한다. 결정적으로, 이 분석은 Adjoint 맵이 표준 유클리드 내적 하에서 등거리 사상(isometry)이 아니라는 점을 고려하여 단순한 안정성 논증을 교정하며, 즉 불확실성 증폭은 링크 기하 구조(길이)에 따라 달라짐을 명시한다.

주요 기여
본 논문은 기하학적 확률 추정에 대해 네 가지 구체적인 기여를 주장한다:

• (C1) 노이즈 모델링: 상태 의존적 코리올리 노이즈 항과 가속도계에 대한 샘플 간격 스케일링을 포함하는 물리적으로 분리된 IMU 노이즈 모델.
• (C2) 모듈형 아키텍처: Adjoint 공분산 전파를 통해 선형 계산 비용을 달성한 링크당 IEKF 체인.
• (C3) 완전한 유도: 혼합 포즈-트위스트 관측 자코비안을 포함한 IEKF의 전체 유도 과정.
• (C4) 안정성 인증: 리 대수 리아푸노프 함수로부터 도출된 체인된 지수적 궁극 유계성 인증으로, 임의의 체인 길이에 확장 가능한 모듈형 안정성 보장을 제공함.

결과
설계된 모델은 대진폭 3D 운동을 수행하는 3-DOF 2링크 강체 직렬 매니퓰레이터의 수치 시뮬레이션을 통해 검증되었다. 제안된 IEKF를 좌표 기반 EKF(CEKF) 및 원시 엔코더 베이스라인과 비교하였다.

• 정확도: IEKF는 모든 조인트에 대해 원시 엔코더 베이스라인 대비 33%의 RMSE 감소를, CEKF 대비 24%의 감소를 달 나타냈다.
• 강건성: CEKF의 오차는 좌표 선형화 오류로 인해 운동 진폭에 따라 증가한 반면, IEKF의 오차는 안정적으로 유지되어 $SE(3)$ 기하학적 오차 모델의 진폭 불변성을 입증했다.
• 일관성: 정규화된 추정 오차 제곱(NEES) 분석을 통해 필터의 통계적 일관성을 확인하였으며, 평균값이 95% 카이제곱 수용 범위 내에 떨어짐을 확인했다.
• 불확실성 전파: 결과는 Adjoint 맵을 통해 상류(upstream)의 추정 불확실성을 통합함에 따라 하류 링크(예: Link 2)의 공분산 밴드가 올바르게 넓어지는 것을 확인시켜 주었다.

의의
본 논문은 기하학적 직관과 확률적 엄밀함 사이의 간극을 메우는 엄격한 프레임워크로서 본 연구의 위치를 설정한다. $SE(3)$ 상에서 네이티브하게 작동하도록 보장함으로써, 평평한 공간 접근 방식의 특이점과 선형화 불일치를 피한다. 모듈형 구조는 과도한 계산 비용 없이 복잡하고 긴 체인의 매니퓰레이터로 확장할 수 있게 한다. 또한, 상태 의존적 노이즈를 명시적으로 처리하고 기하학적 증폭 인자를 고려한 안정성 인증을 도출함으로써, 기존 기하학적 필터들에 비해 유연하거나 강체인 매니퓰레이터 제어를 위한 더 물리적으로 정확하고 이론적으로 견고한 기초를 제공한다. 저자들은 이 프레임워크가 학습된 보정 하에서도 리 군 일관성을 유지하는 과제를 해결함으로써, 학습 기반 추정기를 위한 기하학적으로 일관된 사전 확률(prior) 역할을 할 수 있다고 제안한다.