GPS 신호 품질 확인 조건 노드 (GPS Signal Quality Check Condition Node)

1. 개요

GPS 신호 품질 확인 조건 노드는 위성 항법 시스템(GNSS)의 신호 품질을 평가하여 위치 추정의 신뢰성을 판정하는 조건 노드이다. GPS 신호 품질은 위성 수, 측위 상태(fix status), 수평/수직 정밀도 저하 계수(DOP) 등에 의해 결정되며, 신호 품질이 낮은 상태에서의 GPS 기반 내비게이션은 위치 오차가 증가하여 안전한 운용을 보장하지 못한다. 본 조건 노드는 GPS 기반 임무 수행의 전제 조건으로 활용된다.

2. ROS2 GPS 메시지 타입

2.1 sensor_msgs::msg::NavSatFix

GPS 위치 정보의 표준 메시지 타입이다.

측위 상태 상수는 다음과 같다.

3. 측위 상태 기반 조건 노드

3.1 기본 Fix 상태 확인

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

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<int>("min_status", 0,
                "최소 요구 측위 상태 (-1:NoFix, 0:Fix, 1:SBAS, 2:GBAS)")
        });
    }

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

        int min_status;
        getInput("min_status", min_status);

        if (msg->status.status >= min_status)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

3.2 위치 공분산 기반 정밀도 확인

위치 공분산 행렬의 대각 요소를 통해 위치 추정의 불확실성을 평가한다.

class IsGpsPrecisionAcceptable
    : public BT::RosTopicSubNode<sensor_msgs::msg::NavSatFix>
{
public:
    IsGpsPrecisionAcceptable(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>("max_horizontal_std", 5.0,
                "최대 허용 수평 위치 표준편차 (m)")
        });
    }

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

        if (msg->status.status <
            sensor_msgs::msg::NavSatStatus::STATUS_FIX)
        {
            return BT::NodeStatus::FAILURE;
        }

        if (msg->position_covariance_type ==
            sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN)
        {
            return BT::NodeStatus::FAILURE;
        }

        double max_horizontal_std;
        getInput("max_horizontal_std", max_horizontal_std);

        // position_covariance: [0]=East-East, [4]=North-North, [8]=Up-Up
        double sigma_east = std::sqrt(msg->position_covariance[0]);
        double sigma_north = std::sqrt(msg->position_covariance[4]);
        double horizontal_std = std::sqrt(
            sigma_east * sigma_east + sigma_north * sigma_north);

        if (horizontal_std <= max_horizontal_std)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

수평 위치 표준편차 \sigma_H는 다음과 같이 계산된다.

\sigma_H = \sqrt{\sigma_E^2 + \sigma_N^2}

여기서 \sigma_E는 동향(East) 방향 표준편차, \sigma_N은 북향(North) 방향 표준편차이다. position_covariance 배열은 ENU(East-North-Up) 좌표계의 3 \times 3 공분산 행렬을 행 우선으로 저장한다.

XML 행동 트리에서의 활용

GPS 의존 내비게이션의 전제 조건

<BehaviorTree ID="GpsNavigation">
    <ReactiveSequence>
        <Condition ID="IsGpsFixAvailable"
                   topic_name="/gps/fix"
                   min_status="0"/>
        <Condition ID="IsGpsPrecisionAcceptable"
                   topic_name="/gps/fix"
                   max_horizontal_std="3.0"/>
        <Action ID="NavigateToGpsWaypoint"/>
    </ReactiveSequence>
</BehaviorTree>

GPS 품질 저하 시 대체 전략

<BehaviorTree ID="AdaptiveNavigation">
    <Fallback>
        <Sequence>
            <Condition ID="IsGpsPrecisionAcceptable"
                       topic_name="/gps/fix"
                       max_horizontal_std="2.0"/>
            <Action ID="GpsWaypointNavigation"/>
        </Sequence>
        <Sequence>
            <Condition ID="IsGpsFixAvailable"
                       topic_name="/gps/fix"
                       min_status="0"/>
            <Action ID="LidarBasedNavigation"/>
        </Sequence>
        <Action ID="StopAndWaitForGps"/>
    </Fallback>
</BehaviorTree>

설계 시 고려 사항

GPS 신호 차단 환경

실내, 터널, 밀집한 도심 지역(도시 협곡, urban canyon) 등에서는 GPS 신호가 차단되거나 다중 경로(multipath) 반사에 의해 정밀도가 저하된다. 조건 노드에서 GPS 품질이 지속적으로 FAILURE를 반환하는 경우, 대체 항법 전략(라이다 기반 위치 추정, 시각적 오도메트리 등)으로 전환하는 행동 트리 구조를 설계하여야 한다.

HDOP와 위성 수

일부 GPS 수신기는 NavSatFix 메시지 외에 별도의 토픽으로 HDOP(Horizontal Dilution of Precision)와 가시 위성 수를 발행한다. HDOP 값이 낮을수록(일반적으로 < 2.0) 위치 정밀도가 높으며, 가시 위성 수가 많을수록(일반적으로 \geq 6개) 측위 신뢰도가 높다.

RTK 보정 상태

RTK(Real-Time Kinematic) GPS를 사용하는 경우, RTK 보정 상태(float 또는 fixed)에 따라 센티미터급 정밀도의 달성 여부가 결정된다. RTK fixed 상태에서만 고정밀 임무를 수행하도록 조건을 설정하는 것이 적절하다.

참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Kaplan, E. D., & Hegarty, C. J. (2017). Understanding GPS/GNSS: Principles and Applications. Artech House.
• ROS2 공식 문서. https://docs.ros.org/en/humble/
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/