1293.99 고속 센서 루프와 Tick 주기의 매칭

1. 센서 루프와 Tick 주기의 불일치 문제

로봇 시스템의 센서는 각각 고유한 갱신 빈도(update rate)로 동작한다. IMU는 100~1000 Hz, LiDAR는 10~40 Hz, 카메라는 15~60 Hz, 힘/토크 센서는 수백~수천 Hz로 데이터를 생성한다. 행동 트리의 Tick 주기가 이러한 센서 갱신 빈도와 적절히 매칭되지 않으면, 센서 데이터의 누락, 중복 처리, 또는 지연이 발생하여 의사 결정의 품질이 저하된다(Colledanchise & Ogren, 2018).

2. 매칭 문제의 유형

2.1 Tick이 센서보다 느린 경우

Tick 주기가 센서 갱신 주기보다 긴 경우, 연속된 Tick 사이에 다수의 센서 데이터가 갱신되지만 마지막 값만 사용된다. 중간 값에 포함된 일시적 이상(spike) 또는 빠른 변화가 감지되지 않을 수 있다.

센서 (100 Hz): s₁ s₂ s₃ s₄ s₅ s₆ s₇ s₈ s₉ s₁₀
Tick (10 Hz):  T₁                              T₂
               ↑ s₁ 사용                        ↑ s₁₀ 사용
               s₂~s₉ 무시됨 — 이상 감지 누락 가능

2.2 Tick이 센서보다 빠른 경우

Tick 주기가 센서 갱신 주기보다 짧은 경우, 동일한 센서 데이터를 여러 Tick에서 중복 처리한다. 이는 CPU 자원의 낭비이며, 조건 노드가 변하지 않은 데이터를 반복 평가하는 불필요한 연산이다.

센서 (10 Hz):  S₁                              S₂
Tick (100 Hz): T₁ T₂ T₃ T₄ T₅ T₆ T₇ T₈ T₉ T₁₀
               모두 S₁ 사용 — 9회의 중복 평가

3. 매칭 전략

3.1 나이퀴스트 기준 적용

센서 데이터의 변화를 정확히 포착하려면, Tick 주기는 센서 데이터 변화의 최고 주파수의 2배 이상이어야 한다. 이는 신호 처리의 나이퀴스트 정리(Nyquist theorem)에 기반한 원칙이다.

f_{tick} \geq 2 \cdot f_{change}

여기서 f_{change}는 센서 데이터의 유의미한 변화 주파수이다. 센서의 원시 갱신 빈도가 아닌, 의사 결정에 영향을 미치는 변화의 빈도를 기준으로 한다.

센서별 차등 처리

다양한 빈도의 센서를 단일 Tick 주기로 처리하는 대신, 센서 데이터의 중요도와 빈도에 따라 차등적으로 처리한다.

BT::NodeStatus tick() override {
    // IMU 데이터: 매 Tick마다 처리 (고빈도)
    processIMUData();
    
    // LiDAR 데이터: 새로운 스캔이 있을 때만 처리
    if (hasNewLidarScan()) {
        processLidarScan();
    }
    
    // GPS 데이터: 5 Tick마다 확인 (저빈도)
    if (tick_count_ % 5 == 0) {
        processGPSData();
    }
    
    tick_count_++;
    return evaluateConditions();
}

센서 데이터의 집계

고빈도 센서 데이터를 Tick 주기에 맞게 집계(aggregation)하여, 단일 Tick에서 요약된 정보를 사용한다. 이는 콜백 스레드에서 수행되며, Tick에서는 집계된 결과만 참조한다.

class SensorAggregator {
public:
    void onIMUCallback(const sensor_msgs::msg::Imu& msg) {
        std::lock_guard<std::mutex> lock(mutex_);
        sample_count_++;
        acc_sum_x_ += msg.linear_acceleration.x;
        acc_sum_y_ += msg.linear_acceleration.y;
        acc_sum_z_ += msg.linear_acceleration.z;
        
        // 최대 가속도 추적 (이상 감지용)
        double acc_mag = std::sqrt(
            msg.linear_acceleration.x * msg.linear_acceleration.x +
            msg.linear_acceleration.y * msg.linear_acceleration.y +
            msg.linear_acceleration.z * msg.linear_acceleration.z);
        max_acc_ = std::max(max_acc_, acc_mag);
    }

    struct Summary {
        double avg_acc_x, avg_acc_y, avg_acc_z;
        double max_acc;
        int sample_count;
    };

    Summary consumeSummary() {
        std::lock_guard<std::mutex> lock(mutex_);
        Summary s;
        s.sample_count = sample_count_;
        if (sample_count_ > 0) {
            s.avg_acc_x = acc_sum_x_ / sample_count_;
            s.avg_acc_y = acc_sum_y_ / sample_count_;
            s.avg_acc_z = acc_sum_z_ / sample_count_;
        }
        s.max_acc = max_acc_;
        
        // 초기화
        sample_count_ = 0;
        acc_sum_x_ = acc_sum_y_ = acc_sum_z_ = 0.0;
        max_acc_ = 0.0;
        
        return s;
    }

private:
    std::mutex mutex_;
    int sample_count_{0};
    double acc_sum_x_{0}, acc_sum_y_{0}, acc_sum_z_{0};
    double max_acc_{0.0};
};

이벤트 기반 트리거와 병행

고빈도 센서의 특이 이벤트(급격한 변화, 임계값 초과)를 콜백에서 감지하고, 이를 즉각 Tick의 트리거로 사용한다. 일반적인 상황에서는 주기적 Tick으로 처리하고, 긴급 상황에서만 즉각 Tick을 실행한다.

void onIMUCallback(const sensor_msgs::msg::Imu& msg) {
    aggregator_.addSample(msg);
    
    // 이상 가속도 감지 시 즉각 Tick 트리거
    double acc_mag = computeMagnitude(msg.linear_acceleration);
    if (acc_mag > collision_threshold_) {
        tree_blackboard_->set("collision_detected", true);
        tick_executor_->requestImmediateTick();
    }
}

다중 센서 융합과 Tick 동기화

다수의 센서 데이터를 융합하여 의사 결정에 사용하는 경우, 각 센서의 타임스탬프를 기준으로 시간적 정합(temporal alignment)을 수행해야 한다. Tick 시점에서 가장 최근의 동기화된 센서 세트를 사용한다.

struct SynchronizedData {
    sensor_msgs::msg::LaserScan scan;
    nav_msgs::msg::Odometry odom;
    rclcpp::Time sync_time;
    bool valid{false};
};

void synchronizeSensors() {
    auto scan_time = latest_scan_.header.stamp;
    auto odom_time = latest_odom_.header.stamp;
    
    // 두 센서의 시간 차이가 허용 범위 이내인지 확인
    double dt = std::abs((scan_time - odom_time).seconds());
    
    if (dt < max_sync_offset_sec_) {
        synced_data_.scan = latest_scan_;
        synced_data_.odom = latest_odom_;
        synced_data_.sync_time = scan_time;
        synced_data_.valid = true;
    }
}

설계 지침

센서 루프와 Tick 주기의 매칭은 고정된 규칙이 아닌, 로봇의 동작 환경, 센서 특성, 안전 요구, CPU 자원 등을 종합적으로 고려하여 결정해야 한다.

참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Faconti, D. (2022). BehaviorTree.CPP documentation and API reference. https://www.behaviortree.dev/