센서 데이터 기반 조건 노드 설계 (Sensor Data-Based Condition Node Design)

1. 개요

센서 데이터 기반 조건 노드는 로봇에 장착된 다양한 센서로부터 수집된 데이터를 평가하여 행동 트리의 의사 결정을 수행하는 조건 노드이다. 로봇 시스템에서 센서 데이터는 환경 인식, 안전 판단, 상태 모니터링의 기초를 형성하며, 이를 행동 트리의 조건 평가에 체계적으로 통합하는 것은 자율 행동 제어의 핵심 과제이다. 본 절에서는 센서 데이터 기반 조건 노드의 설계 원칙, 공통 아키텍처, 주요 고려 사항을 다룬다.

2. 센서 데이터의 특성과 조건 평가

2.1 ROS2 센서 메시지 체계

로봇 시스템에서 사용되는 주요 센서 데이터는 ROS2 표준 메시지 타입으로 발행된다. 다음 표는 대표적인 센서 메시지 타입과 그 특성을 정리한 것이다.

2.2 센서 데이터의 불확실성

센서 데이터에는 본질적으로 잡음(noise)과 불확실성(uncertainty)이 존재한다. 조건 노드 설계 시 이를 고려하지 않으면, 잡음에 의한 순간적 값 변동으로 인해 조건 평가 결과가 불안정하게 진동(oscillation)하는 현상이 발생한다.

센서 측정값 z는 일반적으로 다음과 같이 모델링된다.

z = h(x) + \nu

여기서 h(x)는 실제 상태 x에 대한 관측 함수이고, \nu는 잡음 항으로 통상적으로 정규분포 \nu \sim \mathcal{N}(0, \sigma^2)를 따른다. 조건 노드에서 단일 측정값을 임계값과 직접 비교하면 잡음의 영향을 받으므로, 적절한 필터링이나 히스테리시스 적용이 필요하다.

공통 설계 패턴

기본 센서 조건 노드 아키텍처

센서 데이터 기반 조건 노드는 ROS2 토픽 기반 조건 노드의 특수한 형태로, 다음 공통 구조를 따른다.

template <typename SensorMsgT>
class SensorConditionBase
    : public BT::RosTopicSubNode<SensorMsgT>
{
public:
    SensorConditionBase(const std::string& name,
                        const BT::NodeConfiguration& config,
                        const BT::RosNodeParams& params)
        : BT::RosTopicSubNode<SensorMsgT>(name, config, params)
    {}

    BT::NodeStatus onTick(
        const typename SensorMsgT::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        if (!validateMessage(msg))
        {
            return BT::NodeStatus::FAILURE;
        }

        return evaluateCondition(msg);
    }

protected:
    virtual bool validateMessage(
        const typename SensorMsgT::SharedPtr& msg) = 0;

    virtual BT::NodeStatus evaluateCondition(
        const typename SensorMsgT::SharedPtr& msg) = 0;
};

이 기반 클래스는 세 단계의 처리 흐름을 정의한다.

1. 널 검사: 메시지가 수신되지 않은 경우 즉시 FAILURE를 반환한다.
2. 유효성 검증: validateMessage() 메서드를 통해 메시지의 형식적 유효성을 확인한다.
3. 조건 평가: evaluateCondition() 메서드에서 실제 조건을 평가한다.

히스테리시스 적용 패턴

센서 잡음에 의한 조건 평가 진동을 방지하기 위해 히스테리시스(hysteresis)를 적용한다. 히스테리시스는 상승 임계값(upper threshold)과 하강 임계값(lower threshold)을 분리하여, 조건 전환에 필요한 값의 변화량을 증가시킨다.

\text{result}(t) = \begin{cases} \text{SUCCESS} & \text{if } v(t) > c_{\text{upper}} \\ \text{FAILURE} & \text{if } v(t) < c_{\text{lower}} \\ \text{result}(t-1) & \text{if } c_{\text{lower}} \leq v(t) \leq c_{\text{upper}} \end{cases}

여기서 c_{\text{upper}}는 상승 임계값, c_{\text{lower}}는 하강 임계값이며, 히스테리시스 폭은 \Delta c = c_{\text{upper}} - c_{\text{lower}}이다.

template <typename SensorMsgT>
class HysteresisCondition
    : public BT::RosTopicSubNode<SensorMsgT>
{
public:
    HysteresisCondition(const std::string& name,
                        const BT::NodeConfiguration& config,
                        const BT::RosNodeParams& params)
        : BT::RosTopicSubNode<SensorMsgT>(name, config, params),
          previous_result_(BT::NodeStatus::FAILURE)
    {}

    static BT::PortsList providedPorts()
    {
        return BT::RosTopicSubNode<SensorMsgT>::providedBasicPorts({
            BT::InputPort<double>("upper_threshold",
                                  "상승 임계값"),
            BT::InputPort<double>("lower_threshold",
                                  "하강 임계값")
        });
    }

    BT::NodeStatus onTick(
        const typename SensorMsgT::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double upper, lower;
        this->getInput("upper_threshold", upper);
        this->getInput("lower_threshold", lower);

        double value = extractValue(msg);

        if (value > upper)
        {
            previous_result_ = BT::NodeStatus::SUCCESS;
        }
        else if (value < lower)
        {
            previous_result_ = BT::NodeStatus::FAILURE;
        }
        // else: 히스테리시스 대역 내이면 이전 결과를 유지

        return previous_result_;
    }

protected:
    virtual double extractValue(
        const typename SensorMsgT::SharedPtr& msg) = 0;

private:
    BT::NodeStatus previous_result_;
};

2.3 이동 평균 필터링 패턴

단일 측정값 대신 최근 N개 측정값의 이동 평균(moving average)을 기준으로 조건을 평가하여 잡음의 영향을 감소시킨다.

\bar{v}(t) = \frac{1}{N} \sum_{i=0}^{N-1} v(t-i)

class MovingAverageFilter
{
public:
    explicit MovingAverageFilter(size_t window_size)
        : window_size_(window_size), sum_(0.0)
    {}

    double update(double value)
    {
        buffer_.push_back(value);
        sum_ += value;

        if (buffer_.size() > window_size_)
        {
            sum_ -= buffer_.front();
            buffer_.pop_front();
        }

        return sum_ / static_cast<double>(buffer_.size());
    }

    bool isFull() const
    {
        return buffer_.size() >= window_size_;
    }

private:
    size_t window_size_;
    std::deque<double> buffer_;
    double sum_;
};

이 필터를 센서 조건 노드에 내장하여, 필터링된 값을 기반으로 조건을 평가한다.

센서 유형별 조건 노드 분류

로봇 시스템에서 사용되는 센서 데이터 기반 조건 노드는 다음과 같이 분류된다.

근접 센서 관련 조건

관성/자세 센서 관련 조건

위치/항법 센서 관련 조건

전력/환경 센서 관련 조건

센서 데이터 유효성 검증

무효 측정값 처리

센서 메시지에는 무효한 측정값이 포함될 수 있다. 대표적인 무효 값의 유형은 다음과 같다.

bool isValidRange(float range, float range_min, float range_max)
{
    return std::isfinite(range) &&
           range >= range_min &&
           range <= range_max;
}

타임스탬프 기반 신선도 검증

센서 데이터의 타임스탬프를 확인하여, 오래된 데이터를 기반으로 조건을 평가하는 것을 방지한다. 센서 유형에 따른 권장 최대 경과 시간은 다음과 같다.

XML 행동 트리에서의 센서 조건 활용 패턴

안전 조건 지속 감시

ReactiveSequence를 활용하여 센서 기반 안전 조건을 지속적으로 감시한다.

<BehaviorTree ID="SafeAutonomy">
    <ReactiveSequence>
        <!-- 안전 조건 -->
        <Condition ID="IsBatteryAbove"
                   topic_name="/battery_state"
                   min_percentage="0.15"/>
        <Condition ID="IsIMUStable"
                   topic_name="/imu/data"
                   max_roll="0.5"
                   max_pitch="0.5"/>
        <Condition ID="IsNoObstacleNear"
                   topic_name="/scan"
                   min_distance="0.3"/>
        <!-- 임무 수행 -->
        <Action ID="ExecuteMission"/>
    </ReactiveSequence>
</BehaviorTree>

이 구조에서 세 개의 센서 조건 중 하나라도 FAILURE를 반환하면 ExecuteMission 액션이 즉시 중단된다.

센서 기반 행동 선택

Fallback을 활용하여 센서 상태에 따라 적절한 행동을 선택한다.

<BehaviorTree ID="AdaptiveBehavior">
    <Fallback>
        <Sequence>
            <Condition ID="IsObstacleDetected"
                       topic_name="/scan"
                       detection_range="1.0"/>
            <Action ID="AvoidObstacle"/>
        </Sequence>
        <Sequence>
            <Condition ID="IsBatteryLow"
                       topic_name="/battery_state"
                       threshold="0.20"/>
            <Action ID="ReturnToChargingStation"/>
        </Sequence>
        <Action ID="ContinueNavigation"/>
    </Fallback>
</BehaviorTree>

설계 시 고려 사항

QoS 프로파일 선택

센서 데이터 토픽은 일반적으로 BEST_EFFORT 신뢰성과 VOLATILE 내구성을 가진 SensorDataQoS 프로파일을 사용한다. 조건 노드의 구독자도 이에 맞는 QoS를 설정하여야 한다.

계산 복잡도

센서 조건 노드의 tick() 메서드는 행동 트리의 tick 주기 내에 완료되어야 한다. PointCloud2와 같이 대용량 데이터를 포함하는 메시지에 대해 복잡한 분석(예: 점군 클러스터링)을 수행하면 tick 지연이 발생할 수 있다. 이 경우 별도의 처리 노드에서 분석을 수행하고 결과를 토픽이나 블랙보드를 통해 전달하는 구조가 바람직하다.

센서 융합과 조건 노드

복수 센서의 데이터를 융합하여 조건을 평가하여야 하는 경우, 개별 센서별 조건 노드를 Sequence로 결합하는 방법과 단일 조건 노드 내에서 복수 센서를 참조하는 방법이 있다. 전자는 모듈성이 높으나 센서 간 시간 동기화를 보장하기 어렵고, 후자는 동기화를 직접 제어할 수 있으나 복잡도가 증가한다. 일반적으로 개별 센서별 조건 노드를 분리하는 접근을 권장하며, 센서 융합이 필요한 경우에는 별도의 융합 노드에서 처리한 결과를 조건 노드가 참조하도록 설계한다.

참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
• Macenski, S., et al. (2020). “The Marathon 2: A Navigation System.” arXiv preprint arXiv:2003.00368.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
• ROS2 공식 문서. https://docs.ros.org/en/humble/