위치 추정 품질 확인 조건 노드 (Localization Quality Check Condition Node)

1. 개요

위치 추정 품질 확인 조건 노드는 로봇의 위치 추정(localization) 결과의 신뢰도를 평가하여, 위치 추정이 충분히 정확한 상태에서만 내비게이션을 수행하도록 하는 조건 노드이다. 위치 추정의 불확실성이 큰 상태에서의 자율 이동은 충돌이나 목표 이탈의 위험을 수반하므로, 위치 추정 품질을 지속적으로 감시하는 것은 안전한 자율 운용의 기본 요건이다.

2. 위치 추정 불확실성의 표현

2.1 공분산 행렬

ROS2에서 위치 추정 결과는 geometry_msgs::msg::PoseWithCovarianceStamped 메시지로 발행되며, 6 \times 6 공분산 행렬이 포함된다. 이 행렬의 대각 요소는 각 자유도의 분산(variance)을 나타낸다.

수평 위치 불확실성의 지표로 수평 위치 표준편차 \sigma_H를 사용한다.

\sigma_H = \sqrt{\sigma_x^2 + \sigma_y^2}

구현

공분산 기반 품질 확인

class IsLocalizationReliable
    : public BT::RosTopicSubNode<
          geometry_msgs::msg::PoseWithCovarianceStamped>
{
public:
    IsLocalizationReliable(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_xy_std", 0.5,
                "최대 허용 수평 위치 표준편차 (m)"),
            BT::InputPort<double>("max_yaw_std", 0.3,
                "최대 허용 요 표준편차 (rad)")
        });
    }

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

        double max_xy_std, max_yaw_std;
        getInput("max_xy_std", max_xy_std);
        getInput("max_yaw_std", max_yaw_std);

        // 수평 위치 표준편차
        double sigma_x = std::sqrt(msg->pose.covariance[0]);
        double sigma_y = std::sqrt(msg->pose.covariance[7]);
        double sigma_h = std::sqrt(
            sigma_x * sigma_x + sigma_y * sigma_y);

        // 요 표준편차
        double sigma_yaw = std::sqrt(msg->pose.covariance[35]);

        if (sigma_h <= max_xy_std && sigma_yaw <= max_yaw_std)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

공분산의 유효성 검증

AMCL 초기화 직후나 파티클 발산(divergence) 상태에서는 공분산 값이 비정상적으로 크거나 음수일 수 있다. 공분산의 대각 요소가 음수이면 물리적으로 의미가 없으므로, 이를 검증하는 로직을 포함한다.

// 분산이 유효한지 확인
if (msg->pose.covariance[0] < 0.0 ||
    msg->pose.covariance[7] < 0.0 ||
    msg->pose.covariance[35] < 0.0)
{
    return BT::NodeStatus::FAILURE;
}

XML 행동 트리에서의 활용

위치 추정 품질 전제 조건

<BehaviorTree ID="ReliableNavigation">
    <ReactiveSequence>
        <Condition ID="IsLocalizationReliable"
                   topic_name="/amcl_pose"
                   max_xy_std="0.3"
                   max_yaw_std="0.15"/>
        <Action ID="NavigateToGoal"/>
    </ReactiveSequence>
</BehaviorTree>

위치 추정 실패 시 재초기화

<BehaviorTree ID="LocalizationRecovery">
    <Fallback>
        <Sequence>
            <Condition ID="IsLocalizationReliable"
                       topic_name="/amcl_pose"
                       max_xy_std="1.0"/>
            <Action ID="ContinueNavigation"/>
        </Sequence>
        <Sequence>
            <Action ID="SpinForRelocalization"/>
            <Action ID="ReinitializeLocalization"/>
        </Sequence>
    </Fallback>
</BehaviorTree>

설계 시 고려 사항

AMCL 파티클 수와 공분산의 관계

AMCL의 파티클 수가 적으면 공분산이 과소 추정될 수 있으며, 파티클이 지역 최적해(local optimum)에 수렴한 경우에도 공분산이 작게 나타날 수 있다. 공분산만으로는 위치 추정의 정확도를 완전히 보장할 수 없으므로, 보완적인 품질 지표를 함께 활용하는 것이 바람직하다.

환경 특성에 따른 임계값 조정

복도와 같은 구조적으로 유사한 환경에서는 위치 추정의 불확실성이 특정 방향으로 크게 나타나는 비등방성(anisotropy)이 존재한다. 이 경우 \sigma_x와 \sigma_y를 분리하여 평가하는 것이 더 정밀한 품질 판정을 제공한다.

참고 문헌

• 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/