접촉‑앵커드 고유감각 보행오도메트리: 사족보행 로봇을 위한 새로운 자세·속도 추정법

�에 유리하다. " categories: [“Research”]

📝 Abstract

Reliable odometry for legged robots without cameras or LiDAR remains challenging due to IMU drift and noisy joint velocity sensing. This paper presents a purely proprioceptive state estimator that uses only IMU and motor measurements to jointly estimate body pose and velocity, with a unified formulation applicable to biped, quadruped, and wheel-legged robots. The key idea is to treat each contacting leg as a kinematic anchor: joint-torque–based foot wrench estimation selects reliable contacts, and the corresponding footfall positions provide intermittent world-frame constraints that suppress long-term drift. To prevent elevation drift during extended traversal, we introduce a lightweight height clustering and time-decay correction that snaps newly recorded footfall heights to previously observed support planes. To improve foot velocity observations under encoder quantization, we apply an inverse-kinematics cubature Kalman filter that directly filters foot-end velocities from joint angles and velocities. The implementation further mitigates yaw drift through multi-contact geometric consistency and degrades gracefully to a kinematics-derived heading reference when IMU yaw constraints are unavailable or unreliable. We evaluate the method on four quadruped platforms (three Astrall robots and a Unitree Go2 EDU) using closed-loop trajectories. On Astrall point-foot robotA, a $\sim $200,m horizontal loop and a $\sim $15,m vertical loop return with 0.1638,m and 0.219,m error, respectively; on wheel-legged robotB, the corresponding errors are 0.2264,m and 0.199,m. On wheel-legged robot~C, a $\sim $700,m horizontal loop yields 7.68,m error and a $\sim $20,m vertical loop yields 0.540,m error. Unitree Go2 EDU closes a $\sim $120,m horizontal loop with 2.2138,m error and a $\sim $8,m vertical loop with less than 0.1,m vertical error. github.com/ShineMinxing/Ros2Go2Estimator.git

💡 Analysis

**

1. 연구 배경 및 동기

• 외부 센서 의존성 문제: 깊이 카메라·LiDAR는 조명, 입자, 식생 등 환경 변화에 취약하고, 무게·전력·비용 측면에서도 제한이 있다.
• 고유감각만으로의 추정 한계: IMU 단독 사용 시 편향·노이즈 누적으로 인한 급격한 위치 드리프트가 발생한다.
• 접지 제약 활용 필요성: 발이 지면에 고정된 순간을 이용하면 순간적인 속도·위치 제약을 얻을 수 있어 드리프트 억제에 유리하다.

2. 핵심 아이디어

3. 방법론 상세

1. 상태 모델

   • 몸체 위치 p_WB와 회전 R_WB를 포함한 6‑DOF 상태.
   • IMU 가속도·각속도는 연속적인 예측 단계에 사용.

2. 접지 선택

   • 관절 토크 τ와 Jacobian J를 이용해 발 힘 f = (Jᵀ)⁻¹ τ 추정.
   • 수직 성분 f_z < -f_th이면 접지로 판단.

3. 발 착지 기록

   • 접지 순간 c_Wi = p_WB + R_WB * r_Bi (발 위치) 저장.
   • 스탠스 동안 c_Wi를 고정 가정 → 몸체 위치를 역산.

4. 높이 보정

   • h_n(평면 높이), w_n(가중치), t_n(시간)으로 구성된 클러스터 집합 유지.
   • 새 착지 높이와 기존 평면 비교 후 가장 가까운 평면에 스냅, 가중치 업데이트(시간 감쇠).

5. IKVel‑CKF

   • 상태: 발끝 속도 v_foot.
   • 관측: 관절 각 q, 관절 속도 \dot q.
   • 비선형 관측 모델 v_foot = J(q) * \dot q.
   • CKF는 Jacobian 없이 샘플링 기반으로 평균·공분산 업데이트, 파라미터 튜닝 최소화.

6. Yaw 보정

   • 다중 접지 시 c_Wi 사이의 상대 위치·방향을 이용해 헤딩을 재계산.
   • IMU yaw가 불안정하거나 비활성화될 경우, 운동학 기반 헤딩을 그대로 사용.

4. 실험 및 결과

• 장점: 수백 미터 규모의 폐루프에서도 수십 센티미터 수준의 오차를 유지, 특히 수직 정확도가 매우 우수함.
• 한계: 매우 긴 거리(> 600 m)에서는 누적 오차가 증가(7.68 m)하여 추가 보정(예: 주기적 외부 리프팅) 필요.

5. 강점

• 센서 최소화: IMU·모터만으로 충분히 동작, 경량·저비용 로봇에 적합.
• 형태 불변성: 동일 프레임워크가 바이펀드·쿼드펀드·휠‑레그 모두에 적용 가능.
• 수치 안정성: CKF는 Jacobian 계산 오류를 회피하고, 파라미터 튜닝 부담이 적음.
• 오픈소스: 구현·데이터 공개로 재현성 확보.

6. 약점 및 개선점

7. 향후 연구 방향

1. 학습 기반 접지 검출 – 딥러닝으로 토크·가속도·전류 패턴을 동시에 활용해 슬립·충격을 더 정밀히 구분.
2. 다중 센서 융합 – 초음파·레이다 등 저비용 거리 센서를 선택적으로 추가해 고도 클러스터링을 보강.
3. 동적 지형 적응 – 계단·불규칙 지형에서 실시간 평면 클러스터 업데이트와 비선형 보정 알고리즘 개발.
4. 전역 루프 클로징 – 제한된 외부 비전(예: 저해상도 라이트 카메라)과 결합해 장거리 누적 오차를 주기적으로 리셋.
5. 실시간 구현 최적화 – GPU/FPGA 기반 CKF 가속화 및 ROS2 실시간 스케줄링 검증.

**

📄 Content

사족보행 로봇, 고유감각(프로프리오셉티브) 오도메트리, 센서 융합, 구면칼만 추정기

다리형 로봇의 자세와 속도를 정확히 추정하는 것은 움직임 계획 및 안정적인 보행 제어의 전제 조건이다[1],[2]. 실제 적용되는 많은 시스템에서는 깊이 카메라와 LiDAR와 같은 외부감각 센서를 SLAM과 결합하여 전역 상태 피드백을 제공한다[3]‑[5]. 그러나 이러한 센서는 조명 조건이 열악하거나, 공기 중 입자·식생이 많을 때 신뢰성이 떨어질 수 있으며, 경량·저비용 플랫폼에 부적합한 계산량·설치 요구사항을 갖는다. 따라서 비전이나 LiDAR 피드백이 전혀 없을 때도 유효한 고유감각 기반 상태 추정 방법에 대한 필요성이 대두된다.

대부분의 다리형 로봇은 관성측정장치(IMU)와 모터 인코더 데이터를 포함한 풍부한 고유감각 측정을 제공한다; 일부 플랫폼은 추정된 관절 토크나 접촉 관련 신호까지 제공한다. 초기 접근 방식은 IMU 가속도 적분(프레임 변환 및 추가 동적 정보 포함)에 크게 의존하였다[6]‑[8]. 실제로 이중 적분은 바이어스, 타이밍 불일치, 이산 샘플링 노이즈에 매우 민감해 빠르게 드리프트한다. 이를 완화하기 위해 현대적인 추정기들은 스탠스(지면에 고정된) 발에서 유도되는 속도 제약을 도입한다: 발이 지면에 고정돼 있을 때, 몸체 속도는 발‑몸체 상대 운동으로부터 추정될 수 있다. 핵심 난관은 불완전한 접촉 상황에서도 어느 다리가 유효한 제약을 제공하는지 판단하는 것이다. 확률적 접촉 추정 및 충격 감지를 이용해 이러한 측정을 게이트하고 미끄러짐을 감지한다[9]. 접촉 인식된 운동학 제약을 기반으로 불변 EKF(Invariant EKF) 형태가 다리형 상태 추정에 탐색되었으며[10], 학습 기반 측정 모델이 어려운 지형에서 접촉 분류를 더욱 향상시켰다[11]. 카메라를 결합한 하이브리드 시스템은 관측 가능성을 크게 높이지만, 처음에 고유감각 오도메트리를 도입한 이유인 외부감각의 취약성을 다시 불러온다. 또한 속도 추정이 개선되더라도 위치 드리프트는 시간이 지남에 따라 누적될 수 있으며, 노이즈가 많은 관절 속도로부터 직접 계산된 발 속도는 인코더 양자화와 수치 미분에 의해 발생하는 급격한 스파이크를 보인다. 본 연구는 IMU와 모터 측정치만을 이용해 이족보행, 사족보행, 휠‑레그 로봇에 대한 순수 고유감각 오도메트리를 견고하게 구현하는 것을 목표로 한다. 핵심 아이디어는 몸체 상태를 이산적인 발 착지 이벤트에 고정시키는 것이다: 착지 시 세계 좌표계에서의 발 위치를 기록하고, 스탠스 단계 동안 이를 간헐적 제약으로 활용한다. 구현에서는 관절 토크 기반 발 렌치 추정을 통해 스탠스를 선택하고, 휠‑레그 보행은 지면 접촉 시 유효 휠 회전을 보정함으로써 처리한다. 장기적인 고도 드리프트를 방지하기 위해 가벼운 고도 클러스터링 및 시간 감쇠 보정을 도입해 새로 기록된 발 착지 고도를 기존에 관측된 지지 평면에 일정 해상도로 스냅한다. 인코더에서 발생하는 스파이키한 속도 관측을 완화하기 위해, 역기구학 관측 모델 하에 구면칼만 필터(CKF)를 적용해 관절 각도와 각속도로부터 발‑끝 속도를 직접 추정·평활한다.

선형 칼만 필터와 EKF‑형 근사[13]‑[18] 및 불변 EKF·UKF[10],[11],[19]‑[22]와 비교했을 때, CKF는 수치적으로 안정적인 야코비안‑프리 비선형 필터링 규칙을 제공하고 UKF에서 요구되는 스케일링 파라미터를 회피한다. 이는 역기구학 측정[23]‑[25]에 특히 편리하다. 마지막으로, 다중 접촉 기하학적 일관성을 이용해 몸체 프레임 운동학과 세계 프레임 접촉 기록 사이의 일관성을 강제함으로써 장기적인 요(Yaw) 드리프트를 보정한다. 이는 IMU 요 제약이 약화되거나 비활성화될 때도 보조 요 기준을 제공한다.

주요 기여

1. 형태에 구애받지 않는 통합 운동학 기반 고유감각 오도메트리

   • IMU와 모터 측정(관절 각도·속도·추정 토크)만을 이용하는 접촉‑고정 오도메트리 프레임워크를 제안한다.
   • 프레임워크는 형태에 독립적이며, 매 시간 단계마다 현재 접촉 중인 엔드‑이펙터(발 또는 휠)만을 이용해 제약을 만든다. 따라서 동일한 추정기가 이족보행, 사족보행, 휠‑레그 로봇에 적용 가능하다.
   • 구현에서는 관절 토크 기반 발 렌치 추정으로 스탠스를 선택하고, 휠‑레그 플랫폼은 접촉 시 유효 휠 회전 보정을 통해 지원한다.

2. 높이 보정을 포함한 발 착지 기록

   • 착지 시 세계 좌표계에서의 접촉(발착지) 위치를 기록하고, 스탠스 단계 동안 간헐적 제약으로 활용해 장기적인 드리프트를 억제한다.
   • 연속적인 이동 중 고도 드리프트를 방지하기 위해 가벼운 지지‑평면 고도 클러스터링과 시간‑감쇠 신뢰도를 도입한다. 새로 기록된 고도는 설정 가능한 해상도로 기존 평면에 스냅된다.

3. 역기구학 기반 비선형 필터링으로 인코더 유발 속도 스파이크 억제

   • 인코더 양자화와 이산 미분으로 발생하는 급격한 발 속도 스파이크를 완화하기 위해, 역기구학 CKF(IKVel‑CKF)를 도입한다.
   • 관절 각도·각속도로부터 발‑끝 속도를 직접 추정하고, 이를 필터링한 뒤 세계 좌표계로 변환해 융합 추정기에 전달한다.

4. 운동학 전용 요(Yaw) 드리프트 억제

   • 다중 접촉 기하학적 일관성을 이용해 몸체 프레임 운동학과 세계 프레임 접촉 기록 사이의 일관성을 강제함으로써 요 드리프트를 억제한다.
   • 장시간 서 있을 때 안정적인 다중 접촉이 유지되면, 이 제약은 IMU 요 드리프트를 지속적으로 재고정한다. IMU 요 제약이 비활성화된 경우에도, 추정기는 운동학 기반 요 기준을 제공한다. 실험 결과, 짧은 보행에서는 한 바퀴 회전당 약 10°, 긴 보행에서는 약 30° 정도의 누적 오차가 관측되었으며, 이는 모델링되지 않은 자세 결합 및 접촉 탄성에 기인한다.

5. 다중 플랫폼 실험 및 공개 자료

   • 세 대의 Astrall 로봇과 Unitree Go2 EDU 총 4대 플랫폼에서 장거리 폐루프 궤적(수평·수직 움직임 모두) 실험을 수행하였다.
   • 구현 코드와 대표적인 ROS bag(동기화된 비디오 포함)을 공개하여 재현 가능한 평가를 가능하게 한다.

논문의 구성

• Section II : 접촉‑고정 고유감각 오도메트리 수식 전개(스탠스 선택, 발착지 기록, 고도 클러스터링 전략).
• Section III : 엔드‑이펙터 반경 모델링 및 휠 접촉 보정, 원형 포인트‑풋을 효과적인 다리 연장선으로 근사했을 때 발생하는 근사 오차 분석.
• Section IV : 역기구학 CKF(IKVel‑CKF) 소개 및 인코더 유발 속도 스파이크 억제 방법.
• Section V : 다중 접촉 기하학적 일관성을 이용한 요(Yaw) 추정 방법 및 IMU 요 제약이 불안정할 때의 graceful degradation.
• Section VI : 물리 시뮬레이션 및 실제 로봇 실험 결과(평지·계단·장거리 폐루프 궤적) 보고.
• Section VII : 결론 및 향후 연구 방향.

II. 접촉‑고정 고유감각 오도메트리 구현

1) 좌표계 정의

• 세계 좌표계 : (F_W)
• 몸체 좌표계(트렁크) : (F_B)
• 시각 (t_k) 에서 트렁크 자세는 위치 (\mathbf{p}{W B,k} \in \mathbb{R}^3) 와 회전 (\mathbf{R}{W B,k} \in SO(3)) 로 표현한다.
• 각 다리 (i) 에 대해 모터는 관절 각도 (\mathbf{q}{i,k}), 관절 각속도 (\dot{\mathbf{q}}{i,k}), 추정 토크 (\boldsymbol{\tau}_{i,k}) 를 제공한다.

2) 접촉 엔드‑이펙터 집합

시간 (t_k) 에 접촉 중인 엔드‑이펙터(발·휠) 집합을 (\mathcal{C}k) 라고 한다. 각 다리마다 전방 운동학을 이용해 힙‑엔드‑이펙터 벡터 (\mathbf{r}^B{i,k}) 와 속도 (\dot{\mathbf{r}}^B_{i,k}) 를 구하고, 관절 토크와 기하학적 야코비안을 이용해 접촉‑법선 힘 (\mathbf{f}^B_{i,k}) 를 추정한다(자세는 부록 A).

3) 스탠스(접촉) 선택

수직 성분이 사전 정의된 임계값 (\mathrm{f}_{\text{th}}) 보다 작을 때(음수 방향) 해당 다리를 “접촉 중”으로 선언한다:

[ \text{Contact}(i,k) \iff f^B_{i,k,z} < \mathrm{f}_{\text{th}} ]

4) 발착지 기록 및 고도 보정

착지 이벤트는 다음 조건이 만족될 때 감지한다:

[ \text{Touchdown}(i,k) \iff \text{Contact}(i,k-1)=\text{false} \land \text{Contact}(i,k)=\text{true} ]

착지 시 세계 좌표계에서의 접촉점 (\mathbf{c}^W_i) 를 기록하고, 스탠스 단계 동안 이 점을 고정된 제약으로 사용한다(그림 1 참고).

고도 클러스터링

• 기록된 고도 ({h_n, w_n, t_n}) (높이, 신뢰도, 마지막 업데이트 시각) 를 유지한다.
• 현재 착지 고도 (z = c^W_{i,z}) 와 기존 클러스터를 비교해 거리 (|z - h_n| < \delta_h) (설정 가능한 해상도) 인 경우 해당 클러스터의 신뢰도를 시간 감쇠 (\exp(-\lambda (t_k - t_n))) 로 업데이트하고, 고도를 평균화한다.
• 매치가 없으면 새로운 클러스터를 생성한다.
• 보정된 고도는 착지 기록에 다시 쓰여 장기적인 고도 드리프트를 억제한다(그림 3).

5) 접촉‑고정 위치·속도 관측

스탠스 중( (i \in \mathcal{C}_k) ) 발착지점이 세계 좌표계에서 정지한다고 가정하면, 트렁크 위치는

[ \mathbf{p}{W B,k}^{(i)} = \mathbf{c}^W_i - \mathbf{R}{W

이 글은 AI�