안전 울타리 침입 확인 조건 노드 (Safety Fence Intrusion Check Condition Node)

1. 개요

안전 울타리 침입 확인 조건 노드는 로봇의 작업 영역을 둘러싼 안전 울타리(safety fence) 내부에 허가되지 않은 사람이나 물체가 침입하였는지를 판정하는 조건 노드이다. 산업용 로봇 시스템에서 안전 울타리는 작업자를 로봇과의 접촉으로부터 보호하는 물리적 경계이며, 침입이 감지되면 로봇의 속도를 제한하거나 정지시켜야 한다. 이 조건 노드는 안전 레이저 스캐너, 라이트 커튼(light curtain), 압력 매트 등의 안전 센서 데이터를 기반으로 침입 여부를 판정한다.

2. 안전 구역의 정의

2.1 ISO 13855에 따른 안전 거리

안전 구역은 로봇으로부터의 거리에 따라 다음과 같이 계층적으로 정의된다.

2.2 안전 레이저 스캐너 기반 구역 감시

안전 레이저 스캐너(safety laser scanner)는 보호 영역(protective field)과 경고 영역(warning field)을 정의하여, 각 영역의 침입을 독립적으로 감지한다.

3. 구현

3.1 안전 센서 토픽 기반 침입 확인

안전 레이저 스캐너의 상태를 std_msgs::msg::Bool 또는 사용자 정의 메시지로 수신하여 침입을 확인한다.

class IsNoSafetyZoneViolation
    : public BT::RosTopicSubNode<std_msgs::msg::Bool>
{
public:
    IsNoSafetyZoneViolation(const std::string& name,
                            const BT::NodeConfiguration& config,
                            const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params),
          node_(params.nh),
          ever_received_(false)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("timeout_sec", 0.5,
                "안전 센서 토픽 타임아웃 (초)")
        });
    }

    BT::NodeStatus onTick(
        const std_msgs::msg::Bool::SharedPtr& msg) override
    {
        double timeout;
        getInput("timeout_sec", timeout);

        if (msg)
        {
            last_received_ = node_->get_clock()->now();
            ever_received_ = true;

            // data == false: 침입 없음 (안전)
            // data == true: 침입 감지 (위험)
            if (!msg->data)
            {
                return BT::NodeStatus::SUCCESS;
            }
            return BT::NodeStatus::FAILURE;
        }

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

        auto elapsed =
            (node_->get_clock()->now() - last_received_).seconds();
        if (elapsed > timeout)
        {
            return BT::NodeStatus::FAILURE;
        }

        return BT::NodeStatus::SUCCESS;
    }

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp::Time last_received_;
    bool ever_received_;
};

3.2 라이다 기반 구역 침입 감지

안전 전용 센서가 없는 경우, 일반 라이다 데이터를 활용하여 안전 구역 침입을 감지할 수 있다.

class IsZoneClearFromLidar
    : public BT::RosTopicSubNode<sensor_msgs::msg::LaserScan>
{
public:
    IsZoneClearFromLidar(const std::string& name,
                         const BT::NodeConfiguration& config,
                         const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("zone_radius", 1.5,
                "안전 구역 반경 (m)"),
            BT::InputPort<int>("min_intrusion_points", 5,
                "침입 판정 최소 포인트 수")
        });
    }

    BT::NodeStatus onTick(
        const sensor_msgs::msg::LaserScan::SharedPtr& msg) override
    {
        if (!msg || msg->ranges.empty())
        {
            return BT::NodeStatus::FAILURE;
        }

        double zone_radius;
        int min_points;
        getInput("zone_radius", zone_radius);
        getInput("min_intrusion_points", min_points);

        int intrusion_count = 0;
        for (const auto& range : msg->ranges)
        {
            if (std::isfinite(range) &&
                range >= msg->range_min &&
                range <= msg->range_max &&
                range < zone_radius)
            {
                ++intrusion_count;
            }
        }

        if (intrusion_count < min_points)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

min_intrusion_points 매개변수를 사용하여 잡음에 의한 단발성 포인트를 무시하고, 일정 수 이상의 포인트가 안전 구역 내에서 감지된 경우에만 침입으로 판정한다.

4. XML 행동 트리에서의 활용

4.1 계층적 안전 구역 대응

<BehaviorTree ID="ZonedSafetyResponse">
    <Fallback>
        <Sequence>
            <Condition ID="IsNoSafetyZoneViolation"
                       topic_name="/safety_scanner/protective_field"/>
            <Condition ID="IsNoSafetyZoneViolation"
                       topic_name="/safety_scanner/warning_field"/>
            <Action ID="NormalOperation"/>
        </Sequence>
        <Sequence>
            <Condition ID="IsNoSafetyZoneViolation"
                       topic_name="/safety_scanner/protective_field"/>
            <Action ID="ReducedSpeedOperation"/>
        </Sequence>
        <Action ID="ImmediateStop"/>
    </Fallback>
</BehaviorTree>

경고 영역에 침입이 감지되면 속도를 제한하고, 보호 영역에 침입이 감지되면 즉시 정지한다.

5. 설계 시 고려 사항

5.1 안전 등급 센서 사용

산업 환경에서는 안전 인증(예: SIL 2, Performance Level d)을 받은 안전 센서를 사용하여야 한다. 일반 라이다를 안전 목적으로 사용하는 것은 인증되지 않은 안전 기능을 제공하는 것이며, 규제 준수 측면에서 부적절하다.

5.2 사각 지대(Blind spot)

센서의 시야각, 장착 위치, 장애물에 의한 차폐 등으로 인해 안전 구역 내에 사각 지대가 존재할 수 있다. 복수의 센서를 배치하여 사각 지대를 최소화하여야 한다.

5.3 동적 안전 구역

협동 로봇(collaborative robot) 환경에서는 로봇의 속도와 자세에 따라 안전 구역의 크기가 동적으로 변화한다. 속도가 빠를수록 안전 거리가 커지며, 이를 반영하여 안전 구역 매개변수를 동적으로 조정하여야 한다.

6. 참고 문헌

• ISO 13855:2010. Safety of machinery — Positioning of safeguards with respect to the approach speeds of parts of the human body.
• ISO 10218-2:2011. Robots and robotic devices — Safety requirements for industrial robots — Part 2: Robot systems and integration.
• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/