고도 범위 확인 조건 노드 (Altitude Range Check Condition Node)

1. 개요

고도 범위 확인 조건 노드는 드론의 현재 비행 고도가 지정된 최소 및 최대 고도 범위 내에 있는지를 판정하는 조건 노드이다. 고도 제한은 항공 규제 준수, 장애물 회피, 비행 안전 보장, 센서 유효 범위 유지 등 다양한 목적으로 설정되며, 고도 이탈은 지상 충돌이나 항공법 위반으로 이어질 수 있으므로 실시간 감시가 필수적이다.

2. 고도 측정의 유형

2.1 고도 기준면의 분류

2.2 ROS2 고도 관련 메시지

3. 상대 고도 기반 구현

3.1 PoseStamped 기반 고도 확인

class IsAltitudeInRange
    : public BT::RosTopicSubNode<geometry_msgs::msg::PoseStamped>
{
public:
    IsAltitudeInRange(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>("min_altitude", 1.0,
                "최소 허용 고도 (m)"),
            BT::InputPort<double>("max_altitude", 120.0,
                "최대 허용 고도 (m)")
        });
    }

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

        double min_alt, max_alt;
        getInput("min_altitude", min_alt);
        getInput("max_altitude", max_alt);

        double altitude = msg->pose.position.z;

        if (altitude >= min_alt && altitude <= max_alt)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

pose.position.z는 로컬 좌표계(일반적으로 NED 또는 ENU)에서의 고도를 나타낸다. ENU 좌표계에서 z 양의 방향은 상방이며, NED 좌표계에서 z 양의 방향은 하방이다. 사용하는 좌표 규약에 따라 부호를 올바르게 해석하여야 한다.

3.2 히스테리시스를 적용한 고도 범위 확인

class IsAltitudeSafe
    : public BT::RosTopicSubNode<geometry_msgs::msg::PoseStamped>
{
public:
    IsAltitudeSafe(const std::string& name,
                   const BT::NodeConfiguration& config,
                   const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params),
          is_safe_(true)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("danger_min", 0.5,
                "위험 최소 고도 (m)"),
            BT::InputPort<double>("safe_min", 1.5,
                "안전 최소 고도 (m)"),
            BT::InputPort<double>("safe_max", 115.0,
                "안전 최대 고도 (m)"),
            BT::InputPort<double>("danger_max", 120.0,
                "위험 최대 고도 (m)")
        });
    }

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

        double danger_min, safe_min, safe_max, danger_max;
        getInput("danger_min", danger_min);
        getInput("safe_min", safe_min);
        getInput("safe_max", safe_max);
        getInput("danger_max", danger_max);

        double alt = msg->pose.position.z;

        if (alt < danger_min || alt > danger_max)
        {
            is_safe_ = false;
        }
        else if (alt >= safe_min && alt <= safe_max)
        {
            is_safe_ = true;
        }

        return is_safe_ ? BT::NodeStatus::SUCCESS
                        : BT::NodeStatus::FAILURE;
    }

private:
    bool is_safe_;
};

이중 히스테리시스 구조를 적용하여, 하한과 상한 각각에 위험 임계값과 안전 임계값을 설정한다. 고도가 위험 구간에 진입하면 FAILURE로 전환되고, 안전 구간으로 복귀하여야만 SUCCESS로 전환된다.

4. 지상 고도(AGL) 기반 구현

거리 센서(하방 라이다, 초음파)를 활용하여 지표면으로부터의 실제 고도를 측정한다.

class IsAglAltitudeInRange
    : public BT::RosTopicSubNode<sensor_msgs::msg::Range>
{
public:
    IsAglAltitudeInRange(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>("min_agl", 1.0,
                "최소 지상 고도 (m)"),
            BT::InputPort<double>("max_agl", 30.0,
                "최대 지상 고도 (m)")
        });
    }

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

        if (!std::isfinite(msg->range) ||
            msg->range < msg->min_range ||
            msg->range > msg->max_range)
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_agl, max_agl;
        getInput("min_agl", min_agl);
        getInput("max_agl", max_agl);

        if (msg->range >= min_agl && msg->range <= max_agl)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

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

5.1 고도 안전 감시 비행

<BehaviorTree ID="AltitudeSafeFlight">
    <ReactiveSequence>
        <Condition ID="IsAltitudeInRange"
                   topic_name="/mavros/local_position/pose"
                   min_altitude="2.0"
                   max_altitude="100.0"/>
        <Action ID="ExecuteWaypointMission"/>
    </ReactiveSequence>
</BehaviorTree>

5.2 지형 추종 비행

<BehaviorTree ID="TerrainFollowing">
    <ReactiveSequence>
        <Condition ID="IsAglAltitudeInRange"
                   topic_name="/rangefinder/range"
                   min_agl="5.0"
                   max_agl="15.0"/>
        <Action ID="AdjustAltitudeToTerrain"/>
    </ReactiveSequence>
</BehaviorTree>

5.3 고도 이탈 시 귀환

<BehaviorTree ID="AltitudeRecovery">
    <Fallback>
        <Sequence>
            <Condition ID="IsAltitudeSafe"
                       topic_name="/mavros/local_position/pose"
                       danger_min="0.5"
                       safe_min="2.0"
                       safe_max="110.0"
                       danger_max="120.0"/>
            <Action ID="ContinueMission"/>
        </Sequence>
        <Action ID="ReturnToLaunch"/>
    </Fallback>
</BehaviorTree>

6. 설계 시 고려 사항

6.1 고도 측정의 정확도

기압계 기반 고도 측정은 기압 변화에 의해 수 미터의 오차가 발생할 수 있다. GPS 고도는 수평 위치보다 정밀도가 낮아 수십 미터의 오차를 가질 수 있다. 라이다나 레이더 고도계는 지상 고도를 직접 측정하므로 정밀도가 높으나, 측정 범위가 제한된다. 임무의 정밀도 요구에 맞는 고도 소스를 선택하여야 한다.

6.2 좌표계 규약

PX4는 NED(North-East-Down) 좌표계를 사용하며, ROS2는 ENU(East-North-Up) 좌표계를 사용한다. MAVROS는 이 변환을 자동으로 수행하나, 좌표 변환의 부호를 올바르게 이해하여야 한다. ENU에서 z 양의 값은 상방 고도이고, NED에서 z 양의 값은 하방(지하)이다.

6.3 항공 규제 고도 한계

대부분의 국가에서 소형 드론의 최대 비행 고도는 지상 120m(400ft) 이하로 제한된다. 이 규제 한계를 조건 노드의 max_altitude로 설정하여 규제 준수를 자동으로 보장할 수 있다. 다만, 이륙 지점의 해발 고도에 따라 상대 고도와 해발 고도가 상이할 수 있으므로, 규제가 어느 기준면을 사용하는지를 확인하여야 한다.

7. 참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Meier, L., et al. (2015). “PX4: A Node-Based Multithreaded Open Source Robotics Framework for Deeply Embedded Platforms.” ICRA 2015.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
• PX4 공식 문서. https://docs.px4.io/