IMU 자세 이상 감지 조건 노드 (IMU Attitude Anomaly Detection Condition Node)

1. 개요

IMU 자세 이상 감지 조건 노드는 관성 측정 장치(Inertial Measurement Unit, IMU)로부터 수신된 자세(attitude) 데이터를 분석하여, 로봇의 자세가 허용 범위 내에 있는지를 판정하는 조건 노드이다. 롤(roll), 피치(pitch) 각도가 설정된 한계를 초과하는 경우, 로봇의 전도(toppling) 위험이나 비정상 상태를 나타내므로, 이를 조기에 감지하여 안전 행동을 유발하는 것이 이 조건 노드의 목적이다.

2. ROS2 IMU 메시지

2.1 sensor_msgs::msg::Imu

orientation 필드는 쿼터니언 (x, y, z, w)으로 표현되며, 이를 오일러 각도(Euler angles)로 변환하여 롤, 피치, 요(yaw)를 추출한다.

3. 쿼터니언에서 오일러 각도로의 변환

쿼터니언 \mathbf{q} = (x, y, z, w)에서 롤(\phi), 피치(\theta), 요(\psi) 각도를 추출하는 공식은 다음과 같다.

\phi = \text{atan2}(2(wx + yz), 1 - 2(x^2 + y^2))

\theta = \text{asin}(2(wy - zx))

\psi = \text{atan2}(2(wz + xy), 1 - 2(y^2 + z^2))

롤/피치 범위 확인 조건 노드

class IsAttitudeNormal
    : public BT::RosTopicSubNode<sensor_msgs::msg::Imu>
{
public:
    IsAttitudeNormal(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_roll", 0.5236,
                "최대 허용 롤 각도 (rad, 기본 30°)"),
            BT::InputPort<double>("max_pitch", 0.5236,
                "최대 허용 피치 각도 (rad, 기본 30°)")
        });
    }

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

        // 자세 공분산이 -1이면 orientation이 유효하지 않음
        if (msg->orientation_covariance[0] < 0.0)
        {
            return BT::NodeStatus::FAILURE;
        }

        double max_roll, max_pitch;
        getInput("max_roll", max_roll);
        getInput("max_pitch", max_pitch);

        // 쿼터니언에서 롤/피치 추출
        double x = msg->orientation.x;
        double y = msg->orientation.y;
        double z = msg->orientation.z;
        double w = msg->orientation.w;

        double roll = std::atan2(
            2.0 * (w * x + y * z),
            1.0 - 2.0 * (x * x + y * y));

        double sinp = 2.0 * (w * y - z * x);
        double pitch;
        if (std::abs(sinp) >= 1.0)
        {
            pitch = std::copysign(M_PI / 2.0, sinp);
        }
        else
        {
            pitch = std::asin(sinp);
        }

        if (std::abs(roll) <= max_roll &&
            std::abs(pitch) <= max_pitch)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

orientation_covariance[0]이 -1인 경우, IMU 드라이버가 자세 데이터를 제공하지 않음을 의미하는 ROS2 규약이다. 이 경우 FAILURE를 반환한다.

각속도 이상 감지 조건 노드

각속도의 크기가 허용 한계를 초과하는지를 판정한다.

class IsAngularVelocityNormal
    : public BT::RosTopicSubNode<sensor_msgs::msg::Imu>
{
public:
    IsAngularVelocityNormal(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_angular_vel", 3.0,
                "최대 허용 각속도 (rad/s)")
        });
    }

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

        double max_vel;
        getInput("max_angular_vel", max_vel);

        double magnitude = std::sqrt(
            msg->angular_velocity.x * msg->angular_velocity.x +
            msg->angular_velocity.y * msg->angular_velocity.y +
            msg->angular_velocity.z * msg->angular_velocity.z);

        if (magnitude <= max_vel)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

각속도의 크기 \lVert \boldsymbol{\omega} \rVert = \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2}가 허용 한계를 초과하면 로봇이 비정상적으로 회전하고 있음을 의미한다.

선형 가속도 이상 감지

자유 낙하, 충격, 비정상 진동 등을 감지하기 위해 선형 가속도의 크기를 평가한다.

class IsAccelerationNormal
    : public BT::RosTopicSubNode<sensor_msgs::msg::Imu>
{
public:
    IsAccelerationNormal(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_accel", 7.0,
                "최소 가속도 크기 (m/s², 자유낙하 감지)"),
            BT::InputPort<double>("max_accel", 15.0,
                "최대 가속도 크기 (m/s², 충격 감지)")
        });
    }

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

        double min_accel, max_accel;
        getInput("min_accel", min_accel);
        getInput("max_accel", max_accel);

        double magnitude = std::sqrt(
            msg->linear_acceleration.x * msg->linear_acceleration.x +
            msg->linear_acceleration.y * msg->linear_acceleration.y +
            msg->linear_acceleration.z * msg->linear_acceleration.z);

        if (magnitude >= min_accel && magnitude <= max_accel)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

정상 상태의 로봇은 중력 가속도 g \approx 9.81 \text{ m/s}^2에 해당하는 선형 가속도를 보인다. 이 값이 현저히 낮으면 자유 낙하 상태이고, 현저히 높으면 충격이나 외력이 작용한 것이다.

XML 행동 트리에서의 활용

<BehaviorTree ID="AttitudeSafety">
    <ReactiveSequence>
        <Condition ID="IsAttitudeNormal"
                   topic_name="/imu/data"
                   max_roll="0.5236"
                   max_pitch="0.5236"/>
        <Condition ID="IsAngularVelocityNormal"
                   topic_name="/imu/data"
                   max_angular_vel="3.0"/>
        <Action ID="ContinueOperation"/>
    </ReactiveSequence>
</BehaviorTree>

설계 시 고려 사항

IMU 캘리브레이션

IMU의 자이로스코프와 가속도계는 바이어스(bias)와 스케일 오차를 가지며, 온도에 따라 바이어스가 변동한다. 캘리브레이션이 수행되지 않은 IMU 데이터를 기반으로 자세를 판정하면 오차가 축적될 수 있다.

짐벌 잠금 문제

피치 각도가 \pm 90°에 근접하면 오일러 각도 표현의 짐벌 잠금(gimbal lock) 문제가 발생한다. 이 경우 std::asin() 함수의 입력값이 \pm 1을 초과할 수 있으므로, std::copysign()을 사용한 클램핑 처리가 필요하다.

동적 환경에서의 자세 변동

지형이 고르지 않은 환경에서 이동하는 로봇은 일시적으로 큰 롤/피치 각도를 보일 수 있다. 이러한 일시적 변동을 이상으로 판정하지 않기 위해, 이동 평균 필터를 적용하거나 일정 시간 동안 지속적으로 한계를 초과하는 경우에만 이상으로 판정하는 시간 기반 필터링을 적용할 수 있다.

참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Titterton, D., & Weston, J. (2004). Strapdown Inertial Navigation Technology. IET.
• ROS2 공식 문서. https://docs.ros.org/en/humble/
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/