센서 융합 — 불완전한 측정으로 더 믿을 수 있는 상태 만들기

1. 센서 융합은 평균내기가 아니다

센서 융합은 여러 측정값을 무조건 평균내는 작업이 아닙니다. 서로 다른 센서가 무엇을 잘 보고 무엇을 놓치는지, 측정값이 어느 시각과 좌표계의 값인지, 오차가 얼마나 큰지를 모델로 표현하여 로봇의 숨은 상태를 추정하는 과정입니다.

Wheel Encoder는 짧은 시간의 이동량을 빠르고 매끄럽게 알려 주지만 미끄러짐을 모릅니다. IMU는 빠른 회전을 감지하지만 적분하면 표류합니다. LiDAR Scan Matching은 환경 기준의 이동을 관측하지만 유리·긴 복도·움직이는 사람에게 약합니다. GNSS는 전역 위치를 주지만 실내와 건물 사이에서 튈 수 있습니다.

좋은 융합은 센서가 서로 다른 방식으로 틀릴 때 가능합니다. 같은 드라이버가 계산한 같은 값을 두 Topic으로 복제해 넣는 것은 정보가 두 배가 된 것이 아닙니다. 상관된 측정을 독립이라고 가정하면 필터가 근거 없이 자신만만해집니다.

센서 측정 → 시간·좌표계 정렬 → 예측 → 관측과 비교 → 불확실성으로 가중 → 상태와 공분산 갱신 → 이상 여부 감시

핵심 융합 결과는 더 매끄러운 값이 아니라 값과 그 값이 틀릴 수 있는 정도를 함께 가진 추정치입니다.

2. 상태, 입력, 관측을 먼저 정의한다

필터를 고르기 전에 로봇이 알고 싶은 상태를 적어야 합니다. 평면 이동 로봇의 대표 상태는 다음과 같습니다.

x = [px, py, yaw, vx, vy, yaw_rate, gyro_bias]

• px, py: 기준 Frame에서의 위치
• yaw: 로봇이 바라보는 방향
• vx, vy: 로봇 좌표계 속도
• yaw_rate: 회전 속도
• gyro_bias: 자이로 영점 편향

상태를 너무 작게 잡으면 필요한 오차를 설명하지 못합니다. 반대로 관측할 수 없는 상태를 많이 넣으면 숫자는 나오지만 의미가 없습니다. 차동구동 로봇이 옆으로 미끄러지지 않는다고 가정한다면 vy = 0을 제약으로 사용할 수 있지만, 빙판이나 전방향 구동에서는 이 가정이 틀립니다.

센서 Message의 Field가 있다고 모두 융합하지 마십시오. IMU Driver가 Orientation을 제공하지 않는데 0으로 채웠다면 그것은 "0도" 관측이 아니라 없는 관측입니다.

3. 불확실성을 숫자로 표현하는 공분산

분산은 값이 평균 주변에 얼마나 퍼지는지 나타냅니다. 표준편차가 σ이면 분산은 σ²입니다. 공분산 행렬은 각 상태의 분산과 상태 사이 오차의 관계를 함께 기록합니다.

P = [ σx²      cov(x,y) ]
    [ cov(y,x) σy²      ]

공분산이 작으면 필터는 그 값을 강하게 믿습니다. 0을 "모름"이라는 뜻으로 넣으면 안 됩니다. 많은 알고리즘에서 0은 오차가 전혀 없는 완벽한 측정으로 해석됩니다. 모르는 값은 Message 규약에 맞게 제외하거나 큰 분산을 지정해야 합니다.

"""두 위치 센서의 분산에 따른 가중 평균을 계산한다."""
def fuse_scalar(a, var_a, b, var_b):
    weight_a = 1.0 / var_a
    weight_b = 1.0 / var_b
    estimate = (weight_a * a + weight_b * b) / (weight_a + weight_b)
    variance = 1.0 / (weight_a + weight_b)
    return estimate, variance

print(fuse_scalar(1.00, 0.01, 1.40, 0.25))
# (약 1.015, 약 0.0096)

두 번째 센서는 1.40 m라고 말하지만 분산이 크므로 결과는 신뢰도 높은 1.00 m에 가깝습니다. 이것이 불확실성 기반 융합의 가장 작은 형태입니다.

공분산은 사양서에서 복사하지 않는다

실제 로봇을 정지시키고 데이터를 기록해 평균과 분산을 계산하십시오. 직진·회전·거친 바닥·고속 주행처럼 운용 조건별 잔차도 측정해야 합니다. 정지 상태의 작은 분산 하나만 모든 상황에 쓰면 Wheel Slip과 진동을 과도하게 믿게 됩니다.

4. 상보 필터: 빠른 센서와 느린 기준을 결합한다

Gyro는 빠른 자세 변화를 잘 보지만 적분 Drift가 있습니다. Accelerometer의 중력 방향은 장기 기준이 되지만 진동과 선가속에 흔들립니다. 상보 필터는 두 센서의 주파수 특성을 나누어 결합합니다.

angle = α × (previous_angle + gyro_rate × dt)
      + (1 − α) × accelerometer_angle

α가 1에 가까우면 Gyro를 더 믿습니다. 이 계수는 감으로 정하기보다 시간 상수 τ와 실제 주기 dt로 α = τ / (τ + dt)처럼 계산할 수 있습니다.

import math

class ComplementaryFilter:
    def __init__(self, tau=0.5):
        self.tau = tau
        self.roll = 0.0

    def update(self, gyro_x, accel_y, accel_z, dt):
        accel_roll = math.atan2(accel_y, accel_z)
        alpha = self.tau / (self.tau + dt)
        predicted = self.roll + gyro_x * dt
        self.roll = alpha * predicted + (1.0 - alpha) * accel_roll
        return self.roll

상보 필터는 작고 빠르며 자세 안정화에 좋습니다. 하지만 여러 상태의 상관관계, 센서마다 달라지는 공분산, Bias 상태, 불규칙한 관측을 체계적으로 다루려면 Kalman 계열이 더 적합합니다.

5. 칼만 필터의 두 박자: 예측과 수정

Kalman Filter는 계속 두 단계를 반복합니다.

예측

이전 상태와 운동 모델로 현재 상태를 예상합니다. 동시에 과정 잡음 Q를 더해 불확실성을 키웁니다. 시간이 지날수록 확신이 줄어드는 것이 정상입니다.

수정

실제 관측과 예상 관측의 차이인 Innovation 또는 Residual을 계산합니다. 관측 잡음 R이 작으면 측정을 더 많이 반영하고, 예측 공분산 P가 작으면 모델을 더 믿습니다.

예측 상태·공분산 → 관측 도착 → Innovation 계산 → Kalman Gain 계산 → 상태 수정 → 공분산 축소

"""1차원 위치를 예측하고 GPS 측정으로 고치는 최소 Kalman 예제."""
x = 0.0       # 위치 추정
p = 1.0       # 추정 분산
q = 0.04      # 과정 잡음
r = 0.25      # GPS 측정 잡음

def update(position, variance, velocity, dt, measurement):
    predicted_x = position + velocity * dt
    predicted_p = variance + q
    innovation = measurement - predicted_x
    gain = predicted_p / (predicted_p + r)
    corrected_x = predicted_x + gain * innovation
    corrected_p = (1.0 - gain) * predicted_p
    return corrected_x, corrected_p, innovation, gain

for gps in [1.2, 2.1, 3.3, 4.0]:
    x, p, residual, k = update(x, p, 1.0, 1.0, gps)
    print(f"x={x:.3f}, P={p:.3f}, residual={residual:+.3f}, K={k:.3f}")

EKF와 UKF

일반 Kalman Filter는 선형 모델을 가정합니다. 로봇 회전과 좌표 변환은 비선형이므로 현재 상태 주변에서 모델을 선형화하는 EKF가 널리 쓰입니다. UKF는 여러 Sigma Point를 비선형 함수에 통과시켜 분포를 근사합니다. UKF가 언제나 더 정확한 것은 아니며 계산량, 모델 품질과 조정 상태를 함께 비교해야 합니다.

6. 시간 동기화가 틀리면 공간이 틀린다

로봇이 1 m/s로 움직일 때 Timestamp가 100 ms 어긋나면 위치 대응이 10 cm 틀립니다. 90 deg/s로 회전할 때 50 ms 차이는 4.5도입니다. 필터 설정을 아무리 조절해도 시각이 다른 데이터를 같은 순간이라고 합치면 해결되지 않습니다.

확인할 시각은 세 가지입니다.

1. 물리 현상을 Sensor가 측정한 시각
2. Driver가 Message를 만든 시각
3. Subscriber가 Message를 받은 시각

header.stamp에는 가능한 한 실제 측정 시각을 넣어야 합니다. Callback 도착 시각을 쓰면 Network·Executor·Queue 지연이 측정값에 섞입니다.

ros2 topic hz /imu/data
ros2 topic delay /imu/data
ros2 topic hz /wheel/odometry
ros2 bag record /imu/data /wheel/odometry /tf /tf_static

Camera와 LiDAR처럼 한 Frame 또는 Scan을 얻는 데 시간이 걸리는 센서는 노출 중간 시각, Scan 시작·끝 시각과 Point별 시각을 구분해야 합니다. 빠르게 움직이는 로봇에서는 Deskewing이 먼저이고 Fusion은 그다음입니다.

7. 좌표계와 단위를 하나로 맞춘다

IMU가 imu_link, Odometry가 odom, 속도가 base_link 기준이라면 그대로 숫자만 더할 수 없습니다. 측정값을 필터가 요구하는 Frame으로 변환하고, 변환이 측정 시각에 존재하는지 확인해야 합니다.

map → odom → base_link → imu_link
                     ├→ lidar_link
                     └→ camera_link

• ROS는 오른손 좌표계와 SI 단위를 사용합니다.
• 각속도는 rad/s, 가속도는 m/s²입니다.
• IMU 장착 방향은 Static TF와 실제 축 방향이 일치해야 합니다.
• odom → base_link를 여러 Node가 동시에 Publish하면 안 됩니다.
• GNSS의 NED와 ROS ENU를 섞지 마십시오.

ros2 run tf2_ros tf2_echo base_link imu_link
ros2 run tf2_tools view_frames
ros2 topic echo /imu/data --once
ros2 topic echo /wheel/odometry --once

진단 순서 Filter Parameter를 바꾸기 전에 stamp → frame_id → 단위 → 부호 → covariance 순서로 원시 데이터를 확인하십시오.

8. 이동 로봇의 대표 융합 구조

로컬 제어에는 연속적이지만 Drift할 수 있는 odom이 필요합니다. 전역 Navigation에는 장기적으로 맞지만 보정 때 이동할 수 있는 map이 필요합니다.

Wheel Encoder ─┐
               ├─ Local EKF ── odom → base_link
IMU ───────────┘

LiDAR SLAM 또는 GNSS ── map → odom

Local EKF가 Wheel Odometry와 IMU를 결합해 부드러운 odom → base_link를 만듭니다. SLAM·Localization 또는 Global Filter는 전역 관측으로 map → odom을 보정합니다. 이 역할을 섞어 odom 자체가 갑자기 튀게 만들면 Controller가 위험해집니다.

어떤 변수를 넣을까

같은 Wheel Tick에서 계산한 Pose와 Velocity를 둘 다 독립 관측처럼 넣으면 정보가 중복됩니다. 한 센서에서 파생된 값의 상관관계를 모델링하지 않는다면 더 직접적이고 신뢰할 수 있는 변수만 선택하는 편이 안전합니다.

9. robot_localization으로 EKF 구성하기

다음은 평면 차동구동 로봇의 출발점 예시입니다. 실제 Topic, Frame, 공분산과 센서 특성에 맞게 조정해야 합니다.

ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.2
    two_d_mode: true
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /wheel/odometry
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  false, false,
                   false, false, true,
                   false, false, false]
    odom0_queue_size: 10

    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, true,
                  false, false, false]
    imu0_remove_gravitational_acceleration: true
    imu0_queue_size: 20

15개 Boolean은 순서가 중요합니다.

x, y, z,
roll, pitch, yaw,
vx, vy, vz,
vroll, vpitch, vyaw,
ax, ay, az

sudo apt install ros-${ROS_DISTRO}-robot-localization
ros2 run robot_localization ekf_node --ros-args --params-file config/ekf.yaml
ros2 topic echo /odometry/filtered --once
ros2 topic hz /odometry/filtered
ros2 run tf2_ros tf2_echo odom base_link
ros2 param dump /ekf_filter_node

Launch 파일

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=['config/ekf.yaml'],
            remappings=[('odometry/filtered', '/localization/odometry')],
        )
    ])

10. Innovation으로 이상치를 거부한다

Innovation은 측정 − 예상 측정입니다. 정상이라면 0 주변에 머물고 그 분포는 예측 불확실성과 측정 불확실성으로 설명되어야 합니다. 갑자기 큰 Innovation이 생기면 다음 중 하나입니다.

• Sensor Outlier 또는 Multipath
• Wheel Slip·충돌 같은 실제 급변
• Timestamp 지연
• TF·축 부호·단위 오류
• 지나치게 작은 Q 또는 R

Innovation을 공분산으로 정규화한 Mahalanobis Distance를 이용하면 단위가 다른 여러 축의 이상치를 일관되게 판정할 수 있습니다. 그러나 임계값을 너무 작게 하면 실제 급회전도 버리고, 너무 크게 하면 GNSS Jump를 받아들입니다.

이상치를 버릴 때는 조용히 버리지 말고 Sensor 이름, Timestamp, Residual, 임계값과 누적 횟수를 Log와 진단 Topic으로 남기십시오.

11. Q와 R을 어떻게 조정할까

• Q 과정 잡음: 운동 모델이 얼마나 틀릴 수 있는가
• R 측정 잡음: 센서 관측이 얼마나 틀릴 수 있는가
• P 상태 공분산: 현재 추정이 얼마나 불확실한가

숫자를 무작정 바꾸지 마십시오. Bag을 고정하고 한 변수만 바꾸며 Residual RMS, 최종 위치 오차, 지연과 공분산 일관성을 비교해야 재현 가능한 조정이 됩니다.

12. 센서 고장과 성능 저하를 설계한다

현장에서는 센서가 완전히 꺼지는 경우보다 그럴듯하게 틀리는 경우가 더 위험합니다. GNSS가 20 m 튀고도 Fix를 유지하거나, IMU가 진동으로 포화되거나, Encoder 한쪽이 같은 값을 반복할 수 있습니다.

각 입력에 다음 상태를 두는 것이 좋습니다.

HEALTHY → DEGRADED → REJECTED → RECOVERING → HEALTHY

• Timestamp가 너무 오래되면 사용하지 않습니다.
• 물리 범위와 변화율을 벗어난 값은 거부합니다.
• 반복 Outlier면 해당 센서의 가중치를 낮추거나 분리합니다.
• Sensor가 줄어 관측 불가능해진 상태를 표시합니다.
• Localization 품질이 안전 기준 아래면 감속·정지합니다.
• 복구 직후 한 Sample만 보고 다시 믿지 말고 연속 정상 구간을 요구합니다.

센서 융합은 안전 기능을 대신하지 않습니다. 사람 보호가 필요한 로봇은 인증된 Safety Sensor와 독립된 정지 회로를 사용해야 합니다.

13. 실전 검증 절차

1. 각 Sensor를 단독으로 정지·직진·회전 시험합니다.
2. Topic의 Timestamp, 주기, Frame, 단위, 부호를 검증합니다.
3. 정지 데이터로 Bias와 Noise를 추정합니다.
4. Ground Truth가 있는 짧은 경로를 기록합니다.
5. Bag을 반복 재생해 동일한 결과가 나오는지 확인합니다.
6. 센서를 하나씩 추가하고 성능 차이를 측정합니다.
7. Delay·Drop·Outlier·Freeze를 주입합니다.
8. 최종 오차뿐 아니라 지연, Residual, 공분산과 복구 시간을 확인합니다.

ros2 bag record \
  /wheel/odometry /imu/data /scan \
  /odometry/filtered /tf /tf_static /diagnostics

ros2 bag info fusion_test
ros2 bag play fusion_test --clock

14. 자주 하는 실패

모든 필드를 true로 켠다

관측하지 않는 값, 중복 파생값과 잘못된 Orientation까지 들어가 필터가 모순된 정보를 받습니다.

공분산을 모두 0으로 둔다

센서를 완벽하다고 선언하는 것과 같아 다른 센서가 수정할 수 없게 됩니다.

출력 Odometry를 다시 입력한다

같은 정보를 되먹임해 양의 Feedback과 과신을 만듭니다. 데이터 출처를 Graph로 추적하십시오.

TF 발행자가 둘이다

odom → base_link가 번갈아 바뀌며 로봇이 떨립니다. 한 변환에는 한 권위자만 둡니다.

매끄러우면 정확하다고 생각한다

강한 Low-pass Filter는 보기에는 좋지만 장애물을 늦게 보고 Pose가 실제보다 지연될 수 있습니다. Ground Truth와 시간축으로 검증하십시오.

15. 실제 로봇 적용 시나리오

실내 배송 로봇

Wheel+IMU Local EKF로 Controller가 쓸 연속 Odometry를 만들고 LiDAR Localization이 map → odom을 보정합니다. 엘리베이터 문턱에서 Slip이 커지면 Wheel 공분산을 늘리고 Localization 품질이 낮으면 감속합니다.

야외 자율 주행 로봇

Wheel, IMU, GNSS, LiDAR Odometry를 결합합니다. 건물 사이 GNSS Multipath를 Innovation으로 거부하고, GNSS가 끊긴 동안 IMU+Wheel+LiDAR로 이어 가며 공분산을 정직하게 키웁니다.

로봇 팔

Joint Encoder는 관절 위치를, Motor Current는 접촉 가능성을, Force/Torque Sensor는 실제 외력을 관측합니다. 서로 다른 Sampling Rate와 Tool Frame 변환을 맞춰 충돌 감지와 힘 제어에 사용합니다.

연습 문제

1. IMU와 Wheel Odometry의 주기·지연·Timestamp 차이를 측정하세요.
2. 정지 Bag에서 Gyro Bias와 각 축 분산을 계산하세요.
3. 상보 필터의 τ를 세 값으로 바꾸고 응답 속도와 Noise를 비교하세요.
4. 1차원 Kalman 예제에서 Q와 R을 각각 10배 바꾸어 결과를 설명하세요.
5. base_link → imu_link 축을 RViz와 tf2_echo로 검증하세요.
6. robot_localization에서 Wheel vx와 IMU vyaw만 융합해 보세요.
7. /odometry/filtered의 Pose·Twist Frame과 공분산을 검사하세요.
8. 같은 원천에서 나온 Pose와 Twist 중복 융합의 영향을 비교하세요.
9. Bag에 20 m 위치 Jump를 주입하고 Outlier 처리 결과를 기록하세요.
10. IMU Topic을 2초 중단했을 때 상태와 공분산 변화를 관찰하세요.
11. 10 m 직진·제자리 회전·사각형 경로의 Ground Truth 오차를 표로 만드세요.
12. 센서별 HEALTHY·DEGRADED·REJECTED 전이 조건을 설계하세요.

참고 자료

• robot_localization 공식 문서
• sensor_msgs/Imu 메시지
• nav_msgs/Odometry 메시지
• REP-103 표준 단위와 좌표계
• REP-105 이동 로봇 좌표 Frame
• message_filters 공식 문서

16. 확인 퀴즈 10문항