659.97 robot_state_publisher의 내부 동작 원리

1. 개요

robot_state_publisher는 URDF 기반 로봇 모델의 순운동학(forward kinematics)을 계산하여 TF2 변환을 발행하는 노드이다. 본 절에서는 이 노드의 내부 구현 메커니즘, KDL 운동학 엔진의 활용, 상태 갱신 전략, 그리고 성능 최적화 방법을 상세히 기술한다.

2. 내부 아키텍처

2.1 구성 요소

robot_state_publisher의 내부는 다음 구성 요소로 이루어져 있다.

2.2 초기화 과정

1. URDF 파싱: robot_description 매개변수에서 URDF 문자열을 읽고, urdf::Model로 파싱한다.
2. KDL 변환: kdl_parser를 사용하여 URDF 모델을 KDL::Tree로 변환한다. KDL 트리는 링크와 관절의 운동학적 관계를 효율적으로 표현한다.
3. 분절 맵 구성: KDL 트리의 각 분절(segment)을 순회하며, 관절 유형(고정/가동)에 따라 분리하여 관리한다.
4. 정적 변환 발행: 고정 관절에 해당하는 모든 변환을 /tf_static으로 일괄 발행한다.
5. 구독자/타이머 설정: /joint_states 구독자와 주기적 발행 타이머를 설정한다.

3. 순운동학 계산

3.1 KDL 기반 변환 계산

robot_state_publisher는 KDL의 Segment 구조체를 사용하여 각 가동 관절의 변환을 계산한다.

가동 관절 j의 변환 T_j(q_j)는 다음과 같이 계산된다.

T_j(q_j) = T_{\text{origin}} \cdot T_{\text{joint}}(q_j)

여기서:

• T_{\text{origin}}: URDF의 <origin> 요소에서 정의된 정적 오프셋
• T_{\text{joint}}(q_j): 관절 위치 q_j에 의한 운동학적 변환
• q_j: /joint_states에서 수신된 관절 위치 값

회전 관절(revolute, continuous)의 경우:

T_{\text{joint}}(q_j) = \text{Rotation}(\mathbf{a}, q_j)

직선 관절(prismatic)의 경우:

T_{\text{joint}}(q_j) = \text{Translation}(\mathbf{a} \cdot q_j)

여기서 \mathbf{a}는 URDF의 <axis> 요소에서 정의된 관절 축 벡터이다.

3.2 상태 캐싱

robot_state_publisher는 최근에 수신된 각 관절의 위치를 내부 맵에 캐시한다. /joint_states 메시지에 일부 관절의 데이터만 포함되어 있는 경우에도, 이전에 캐시된 값을 사용하여 전체 트리의 변환을 계산한다.

내부 맵: { "joint1": 0.5, "joint2": -0.3, "joint3": 1.2, ... }

새로운 /joint_states 메시지가 수신되면, 메시지에 포함된 관절만 갱신되고 나머지는 이전 값을 유지한다.

4. 갱신 전략

4.1 변경 감지 방식

robot_state_publisher는 관절 위치가 이전 값과 다른 경우에만 해당 관절의 변환을 재발행한다. 이는 불필요한 TF 발행을 줄여 네트워크 부하를 감소시킨다.

4.2 타이머 기반 발행

publish_frequency 매개변수에 의하여 설정된 주기로 타이머 콜백이 실행되며, 이 콜백에서 변경된 관절의 변환을 발행한다. 기본 주파수는 20 Hz이다.

4.3 이벤트 기반 발행

/joint_states 메시지가 수신되면 즉시 해당 관절의 변환이 갱신된다. 이 방식은 타이머 기반 발행과 독립적으로 동작하며, 저지연 갱신이 필요한 경우에 유용하다.

5. 동적 URDF 갱신

ROS2의 robot_state_publisher는 robot_description 매개변수를 동적으로 갱신하는 것을 지원한다. 매개변수가 변경되면 내부 운동학 트리를 재구성하고, 새로운 정적 변환을 발행한다. 이 기능은 런타임에 로봇 모델을 변경하여야 하는 경우(예: 도구 교환, 모듈 부착/탈착)에 활용된다.

6. 성능 고려 사항

6.1 발행 주파수 설정

6.2 다수의 가동 관절에서의 부하

가동 관절의 수가 많은 로봇(예: 휴머노이드)에서는 각 관절마다 변환을 발행하므로, /tf 토픽의 메시지 빈도가 증가한다. 이 경우 발행 주파수를 적절히 조절하여 네트워크 부하를 관리하여야 한다.

7. 요약

robot_state_publisher는 URDF를 KDL 운동학 트리로 변환하고, /joint_states 토픽에서 관절 상태를 수신하여 순운동학을 계산한 후, 결과를 TF2 변환으로 발행한다. 내부적으로 관절 상태 캐싱과 변경 감지 메커니즘을 통하여 효율적인 변환 발행을 수행하며, 고정 관절은 /tf_static, 가동 관절은 /tf로 분리하여 발행한다.

참고 문헌 및 출처

• robot_state_publisher 소스 코드, https://github.com/ros/robot_state_publisher (ROS2 Humble 브랜치)
• R. Smits, “KDL: Kinematics and Dynamics Library,” Orocos Project, https://www.orocos.org/kdl/
• kdl_parser 패키지, https://github.com/ros/kdl_parser (ROS2 Humble 브랜치)