[하이브리드 항법 시스템: 제4편] ROS 2를 활용한 PX4와 외부 INS 오도메트리 연계 및 통신 검증

Bymaponarooo

안녕하세요! 자율비행 무인기 시스템의 최전선에서 밤낮없이 고군분투하고 계신 대학원생 및 연구원 여러분. 쿼드(QUAD) 드론연구소입니다.

우리는 지난 **<제3편>**에서 GPS가 완전히 차단된(GNSS-Denied) 환경에서 저가형 센서가 겪는 ‘이중 적분에 의한 오차 발산(Drift)’의 수학적 한계를 살펴보고, 이를 극복하기 위한 전술급(Tactical Grade) 고성능 산업용 INS(관성항법장치) 하드웨어의 도입 필요성을 심도 있게 파헤쳐 보았습니다.

하지만 수백, 수천만 원을 호가하는 훌륭한 외부 INS(예: MicroStrain 3DM-GQ7, SBG Ellipse-D 등)를 구매하여 드론에 장착했다고 해서 모든 문제가 해결되는 것은 아닙니다. 이 고성능 센서가 뿜어내는 ‘고정밀 위치 및 속도 데이터(Odometry)’를 드론의 비행 제어기인 **PX4의 EKF2(확장 칼만 필터) 혈관 속으로 완벽하게 주입(Injection)**해 주어야만, 비로소 기체가 이를 믿고 자율 비행을 수행할 수 있습니다.

이번 **<제4편>**에서는 컴패니언 컴퓨터(Companion Computer)에서 구동되는 ROS 2를 활용하여 외부 INS의 오도메트리 데이터를 PX4와 연동하는 방법, 실전에서 랩실 연구원들이 가장 많이 밤을 새우게 만드는 좌표계(Coordinate Frame) 변환 문제, 그리고 PX4 EKF2 파라미터 세팅 및 통신 검증 방법까지 완벽하게 정리해 드립니다.

추후 이어질 제5편~7편의 ‘3단계 하이브리드 항법(순항 → 중간 보정 → 종말 타격)’ 구현을 위한 가장 핵심적인 소프트웨어 인프라 공사이니, 두 눈 크게 뜨고 따라와 주시기 바랍니다!

1. 통신 브릿지 구축: Micro XRCE-DDS Agent 아키텍처

과거 ROS 1 시절에는 MAVROS라는 패키지를 통해 MAVLink 프로토콜 기반으로 통신했습니다. 하지만 ROS 2 시대로 넘어오면서, PX4는 **MicroXRCE-DDS (eXtremely Resource Constrained Environments – Data Distribution Service)**라는 혁신적인 브릿지 시스템을 도입했습니다.

데이터의 흐름은 다음과 같습니다.

  1. 외부 INS 센서: 고정밀 위치, 속도, 자세 데이터(Odometry)를 생성합니다.
  2. ROS 2 노드 (컴패니언 컴퓨터): 센서의 데이터를 읽어와서 PX4가 이해할 수 있는 메시지 타입인 px4_msgs/msg/VehicleOdometry로 변환한 뒤 발행(Publish)합니다.
  3. Micro XRCE-DDS Client & Agent: ROS 2 환경의 토픽을 PX4 내부의 uORB 메시지(동일하게 VehicleOdometry)로 직렬화/역직렬화하여 전달합니다. 지연 시간이 매우 짧고 오버헤드가 적습니다.
  4. PX4 EKF2 필터: 외부 데이터를 수신하여(External Vision 인터페이스 활용) GPS를 대체하는 메인 위치 제어 데이터로 융합(Fusion)합니다.

2. 연구원들의 최대 난제: 좌표계(Coordinate Frame) 변환 주의사항

ROS 2와 PX4를 연동할 때 수많은 대학원생들이 가장 많은 시간을 허비하며 이른바 ‘삽질(?)’을 하는 부분이 바로 좌표계(Frame)의 불일치입니다. PX4는 전통적인 항공우주 기준을 따르고, ROS 2는 로보틱스 기준을 따르기 때문에 두 시스템이 세상을 바라보는 언어가 다릅니다. 이 부분을 명확히 변환(Sync)해 주지 않으면 드론은 이륙하자마자 뒤집어지거나 엉뚱한 방향으로 발산하며 추락합니다.

📍 글로벌/로컬 좌표계 (World Frame)

  • ROS 2 표준: ENU (East, North, Up) – X축이 동쪽, Y축이 북쪽, Z축이 하늘을 향합니다.
  • PX4 표준: NED (North, East, Down) – X축이 북쪽, Y축이 동쪽, Z축이 땅을 향합니다.

📍 기체 좌표계 (Body Frame)

  • ROS 2 표준: FLU (Forward, Left, Up) – X축이 전방, Y축이 좌측, Z축이 하늘입니다.
  • PX4 표준: FRD (Forward, Right, Down) – X축이 전방, Y축이 우측, Z축이 땅입니다.

따라서, 외부 INS 패키지가 ROS 2 표준인 ENU/FLU 기반의 nav_msgs/Odometry를 출력한다면, 우리는 Python 브릿지 노드에서 다음과 같은 변환 공식을 적용하여 PX4의 NED/FRD 기준으로 변경해야 합니다.

  • 위치(Position) 변환 (ENU → NED): x_ned = y_enu, y_ned = x_enu, z_ned = -z_enu
  • 속도(Velocity) 변환 (FLU → FRD): vx_frd = vx_flu, vy_frd = -vy_flu, vz_frd = -vz_flu
  • 쿼터니언 자세(Orientation) 변환 (ENU → NED): 축이 변환됨에 따라 쿼터니언의 x,y,z,w 요소 부호와 위치를 재배치해야 합니다. [w, x, y, z] 포맷 기준으로, q_ned = [q_enu_w, q_enu_y, q_enu_x, -q_enu_z]의 맵핑이 이루어집니다.

3. [실습] 가상 오도메트리 연동 테스트 (ROS 2 노드)

이제 이론을 바탕으로, 외부 INS 데이터(여기서는 ROS 2의 nav_msgs/msg/Odometry로 들어온다고 가정)를 받아 PX4의 px4_msgs/msg/VehicleOdometry로 변환하여 쏴주는 ROS 2 노드를 작성해 보겠습니다. SITL(가상 환경)이나 실제 하드웨어 모두에서 동일하게 작동합니다.

C++
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <px4_ros_com/frame_transforms.h> // px4_ros_com 변환 라이브러리 참조

using namespace std::chrono_literals;

class INSOdometryBridge : public rclcpp::Node {
public:
    INSOdometryBridge() : Node("ins_odometry_bridge") {
        rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
        auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile);

        ins_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/external_ins/odom", qos,
            std::bind(&INSOdometryBridge::insCallback, this, std::placeholders::_1));

        px4_odom_pub_ = this->create_publisher<px4_msgs::msg::VehicleOdometry>(
            "/fmu/in/vehicle_visual_odometry", qos);
            
        RCLCPP_INFO(this->get_logger(), "INS to PX4 Odometry Bridge (C++) Started.");
    }

private:
    void insCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
        px4_msgs::msg::VehicleOdometry px4_msg{};
        px4_msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        px4_msg.timestamp_sample = px4_msg.timestamp;
        px4_msg.pose_frame = px4_msgs::msg::VehicleOdometry::POSE_FRAME_NED;
        px4_msg.velocity_frame = px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_FRD;

        // 1. 위치 변환 (ENU -> NED)
        Eigen::Vector3d pos_enu(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
        Eigen::Vector3d pos_ned = px4_ros_com::frame_transforms::transform_static_frame(pos_enu, px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED);
        px4_msg.position = { (float)pos_ned.x(), (float)pos_ned.y(), (float)pos_ned.z() };

        // 2. 쿼터니언 자세 변환 (ENU -> NED)
        Eigen::Quaterniond q_enu(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        Eigen::Quaterniond q_ned = px4_ros_com::frame_transforms::transform_orientation(q_enu, px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED);
        px4_msg.q = { (float)q_ned.w(), (float)q_ned.x(), (float)q_ned.y(), (float)q_ned.z() }; // px4_msg.q는 w, x, y, z 순서로 매핑됨

        // 3. 선속도 변환 (FLU -> FRD)
        Eigen::Vector3d vel_flu(msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z);
        Eigen::Vector3d vel_frd = px4_ros_com::frame_transforms::transform_static_frame(vel_flu, px4_ros_com::frame_transforms::StaticTF::FLU_TO_FRD);
        px4_msg.velocity = { (float)vel_frd.x(), (float)vel_frd.y(), (float)vel_frd.z() };

        // 4. 각속도 변환 (FLU -> FRD)
        Eigen::Vector3d ang_vel_flu(msg->twist.twist.angular.x, msg->twist.twist.angular.y, msg->twist.twist.angular.z);
        Eigen::Vector3d ang_vel_frd = px4_ros_com::frame_transforms::transform_static_frame(ang_vel_flu, px4_ros_com::frame_transforms::StaticTF::FLU_TO_FRD);
        px4_msg.angular_velocity = { (float)ang_vel_frd.x(), (float)ang_vel_frd.y(), (float)ang_vel_frd.z() };

        px4_msg.position_variance = {0.01, 0.01, 0.01};
        px4_msg.velocity_variance = {0.01, 0.01, 0.01};
        px4_msg.orientation_variance = {0.001, 0.001, 0.001};

        px4_odom_pub_->publish(px4_msg);
    }

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr ins_sub_;
    rclcpp::Publisher<px4_msgs::msg::VehicleOdometry>::SharedPtr px4_odom_pub_;
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<INSOdometryBridge>());
    rclcpp::shutdown();
    return 0;
}
Python
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

from nav_msgs.msg import Odometry
from px4_msgs.msg import VehicleOdometry

import numpy as np
from scipy.spatial.transform import Rotation as R

class FrameTransforms:
    """
    px4_ros_com/src/lib/frame_transforms.cpp 의 내부 회전 로직을 Python으로 포팅한 헬퍼 클래스
    """
    @staticmethod
    def transform_static_frame_enu_to_ned(vec):
        # x_ned = y_enu, y_ned = x_enu, z_ned = -z_enu
        return [vec[3], vec, -vec[4]]

    @staticmethod
    def transform_static_frame_flu_to_frd(vec):
        # x_frd = x_flu, y_frd = -y_flu, z_frd = -z_flu
        return [vec, -vec[3], -vec[4]]

    @staticmethod
    def transform_orientation_enu_to_ned(q_enu_flu_xyzw):
        # px4_ros_com 라이브러리와 동일하게 Euler 각 기반 회전 행렬 구성
        # 1. ENU -> NED: Z축 90도 회전 후 X축 180도 회전
        rot_enu_to_ned = R.from_euler('Z', np.pi/2) * R.from_euler('X', np.pi)
        # 2. FLU -> FRD: X축 180도 회전
        rot_flu_to_frd = R.from_euler('X', np.pi)

        # ROS 2의 입력 쿼터니언 (x, y, z, w 형식)
        rot_enu_flu = R.from_quat(q_enu_flu_xyzw)

        # q_NED_FRD = q_ENU_to_NED * q_ENU_FLU * (q_FLU_to_FRD)^-1
        rot_ned_frd = rot_enu_to_ned * rot_enu_flu * rot_flu_to_frd.inv()

        # Scipy는 [x, y, z, w] 형태로 반환
        return rot_ned_frd.as_quat()

class INSOdometryBridge(Node):
    def __init__(self):
        super().__init__('ins_odometry_bridge')
        
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )
        
        self.ins_sub = self.create_subscription(
            Odometry, '/external_ins/odom', self.ins_callback, qos_profile)
        
        self.px4_odom_pub = self.create_publisher(
            VehicleOdometry, '/fmu/in/vehicle_visual_odometry', qos_profile)
        
        self.get_logger().info("INS to PX4 Odometry Bridge Node Started (Using scipy frame_transforms).")

    def ins_callback(self, msg: Odometry):
        px4_msg = VehicleOdometry()
        
        now_us = int(self.get_clock().now().nanoseconds / 1000)
        px4_msg.timestamp = now_us
        px4_msg.timestamp_sample = now_us
        
        px4_msg.pose_frame = VehicleOdometry.POSE_FRAME_NED
        px4_msg.velocity_frame = VehicleOdometry.VELOCITY_FRAME_FRD
        
        # 1. 위치 변환 (ENU -> NED)
        pos_enu = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]
        px4_msg.position = FrameTransforms.transform_static_frame_enu_to_ned(pos_enu)
        
        # 2. 쿼터니언 자세 변환 (ROS 2 ENU/FLU -> PX4 NED/FRD)
        # ROS 2 메시지는 x, y, z, w 순서
        q_enu_flu = [
            msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, 
            msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
        ]
        q_ned_frd = FrameTransforms.transform_orientation_enu_to_ned(q_enu_flu)
        # PX4 메시지 타입은 w, x, y, z 순서이므로 매핑
        px4_msg.q = [float(q_ned_frd[5]), float(q_ned_frd), float(q_ned_frd[3]), float(q_ned_frd[4])]
        
        # 3. 선속도 변환 (FLU -> FRD)
        vel_flu = [msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]
        px4_msg.velocity = FrameTransforms.transform_static_frame_flu_to_frd(vel_flu)
        
        # 4. 각속도 변환 (FLU -> FRD)
        ang_vel_flu = [msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z]
        px4_msg.angular_velocity = FrameTransforms.transform_static_frame_flu_to_frd(ang_vel_flu)
        
        # 노이즈 분산값 설정
        px4_msg.position_variance = [0.01, 0.01, 0.01]
        px4_msg.velocity_variance = [0.01, 0.01, 0.01]
        px4_msg.orientation_variance = [0.001, 0.001, 0.001]
        
        self.px4_odom_pub.publish(px4_msg)

def main(args=None):
    rclpy.init(args=args)
    node = INSOdometryBridge()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

이 코드를 빌드하고 실행(ros2 run <패키지명> ins_odometry_bridge)하면, ROS 2 환경에서 돌고 있는 고정밀 외부 INS의 데이터가 NED/FRD 좌표계로 변환되어 10Hz~30Hz 이상의 속도로 Micro XRCE-DDS 에이전트를 거쳐 PX4 내부로 쏟아져 들어가기 시작합니다.

4. PX4 EKF2 핵심 파라미터 세팅 및 통신 검증 (QGroundControl)

외부 데이터를 정확한 좌표계로 쏴주는 것만으로는 끝이 아닙니다. 엄격한 PX4의 EKF2 필터가 “이 외부 데이터를 믿고 나의 위치로 사용할게”라고 허락해 주어야 합니다. QGroundControl(QGC)을 열고 다음 핵심 파라미터들을 변경해야 합니다.

[필수 파라미터 세팅]

  1. EKF2_EV_CTRL (External Vision Control) 어떤 외부 데이터를 EKF에 융합할지 결정하는 비트마스크입니다. 고성능 INS의 수평 위치, 수직 위치, 3D 속도, 요(Yaw) 각도를 모두 신뢰한다면 비트 0~3을 모두 켜서 값(Value)을 15로 설정합니다.
  2. EKF2_EV_DELAY (측정 지연 보상) 센서가 위치를 측정하고 ROS 2를 거쳐 PX4까지 도달하는 데 걸리는 통신 지연 시간을 ms 단위로 보상합니다. 보통 0 ~ 50ms 범위에서 테스트를 통해 튜닝합니다.
  3. EKF2_GPS_MODE (GNSS-Denied 대응 모드) 이 값을 **1 (Dead-reckoning mode)**로 설정해야 합니다. 이 모드가 켜져 있어야 이륙 후 간헐적으로 GPS가 끊기거나 외부 교란이 발생하더라도 EKF가 초기화(Reset)되지 않고, 우리가 주입하는 외부 INS 데이터에 의존하여 안정적인 ‘추측 항법’을 지속할 수 있습니다. 부팅 및 시동 시에도 GPS 검사를 우회하려면 COM_ARM_WO_GPS를 0으로 설정해야 합니다.

[통신 검증 및 트러블슈팅] 설정을 마쳤다면 랩실 책상 위에서 프롭을 제거한 상태로 테스트를 진행합니다.

  • 연결 확인: QGC 상단 메뉴의 MAVLink Inspector를 엽니다. ODOMETRY 메시지가 10Hz 이상으로 빠르게 갱신되고 있다면 브릿지 통신이 성공한 것입니다.
  • EKF2 융합 확인: MAVLink Inspector에서 ESTIMATOR_STATUS를 확인합니다. pos_horiz_status, pos_vert_status 플래그가 초록색으로 켜져 있으면 EKF2가 외부 데이터를 메인 위치 소스로 사용하고 있다는 증거입니다.
  • 이노베이션(Innovation) 체크: 비행 로그(.ulg)를 PlotJuggler로 열어 estimator_innovations 토픽을 확인합니다. ‘EKF가 예측한 위치’와 ‘외부 INS가 던져준 위치’ 사이의 오차인 이노베이션 값이 0 근처에서 튀지 않고 안정적으로 유지된다면, 지연 시간 보상(Delay)과 좌표계(Frame) 변환이 완벽하게 들어맞았음을 의미합니다.

마무리하며 및 다음 편 예고

이번 **<제4편>**에서는 비싸고 훌륭한 하드웨어(고성능 INS)를 드론의 진정한 두뇌로 탈바꿈시키는 ROS 2 ↔ PX4 통신 브릿지 아키텍처를 완벽하게 구축했습니다. 가장 헷갈리기 쉬운 ENU ↔ NED 좌표계 프레임 변환부터, EKF2 필터 파라미터 튜닝과 실무적인 검증 방법까지 통합적으로 다루었습니다.

이제 우리의 드론은 GPS가 끊긴 상황에서도 외부 센서의 고정밀 데이터를 받아들이고 제어할 준비가 끝났습니다. 즉, 인프라 공사가 완료된 것입니다.

이제부터는 본격적인 [구현] 단계로 넘어갑니다! 다음 연재인 <제5편: [구현-1단계] 순항 단계: 고전적 추측 항법, 풍향 추정, 그리고 Gazebo 바람 시뮬레이션> 에서는 오늘 구축한 시스템 위에서 GPS 없이 IMU와 대기속도계만으로 목표물을 향해 나아가는 로직을 구현합니다. 특히 2차대전 조종사들처럼 ‘크랩 각(Crab Angle)’을 계산하여 강한 측풍(Crosswind)을 극복하는 방법과, 이를 **Gazebo 시뮬레이터에 바람 인젝션(Wind Injection)**으로 모사하여 테스트하는 실전 기법을 다룰 예정입니다.

자폭 드론이 바람을 가르며 수백 킬로미터를 돌파하는 다음 연재도 많은 기대 부탁드립니다!

YouTube Tutorial

재생

Author: maponarooo, CEO of QUAD Drone Lab

Date: April 22, 2026

Similar Posts

답글 남기기

이메일 주소는 공개되지 않습니다. 필수 필드는 *로 표시됩니다