[하이브리드 항법 시스템: 제7편] [구현-3단계] 종말 단계: YOLO 기반 정밀 시각적 서보잉(IBVS)과 Offboard 제어 시뮬레이션

Bymaponarooo

안녕하세요! 자율비행 무인기 시스템의 최전선에서 연구에 매진하고 계신 대학원생 및 연구원 여러분. QUAD 드론연구소입니다.

지난 6편에서는 GPS가 철저히 차단된 환경에서 사전에 확보한 위성 지도와 실시간 카메라 영상을 대조하여 누적된 추측 항법 오차를 리셋하는 ‘중간 보정(AVL)’ 기술을 다루었습니다. 이제 우리의 자폭 드론(또는 정찰 무인기)은 적진 깊숙한 곳, 목표물 인근까지 무사히 접근했습니다.

이번 제7편에서는 대장정의 마지막 관문인 **’종말 단계(Terminal Phase)’**를 구현합니다. 목표물을 시각적으로 포착하고, 한 치의 오차도 없이 정밀하게 타격(또는 착륙)하기 위해 YOLO 기반의 객체 탐지와 이미지 기반 시각적 서보잉(Image-Based Visual Servoing, IBVS) 기술을 활용하는 방법, 그리고 이를 PX4의 Offboard 제어 모드와 연동하여 디지털 트윈 환경에서 시뮬레이션하는 전체 과정을 심도 있게 살펴보겠습니다.

1. 시각적 서보잉(Visual Servoing)의 이해: PBVS vs IBVS

시각적 서보잉(Visual Servoing)은 비전 센서(카메라)로부터 얻은 영상 피드백을 실시간으로 사용하여 로봇의 움직임을 제어하는 폐루프(Closed-loop) 제어 기술입니다. 목표물의 절대적인 3D 공간 좌표(GPS나 지도 상의 좌표)를 알 수 없는 상황에서도, 영상 정보만으로 로봇을 목표 위치로 유도할 수 있어 정밀 타격이나 도킹에 필수적입니다. 시각적 서보잉은 상태 벡터를 구성하는 기준에 따라 크게 두 가지로 나뉩니다.

  1. 자세 기반 시각적 서보잉 (PBVS, Pose-Based Visual Servoing): 카메라 영상에서 객체의 기하학적 형태를 분석하여 카메라 대비 대상 객체의 3D 상대 포즈(위치 및 자세)를 역산하여 추정합니다. 추정된 3D 공간상의 오차를 줄이도록 제어하지만, 3D 포즈를 추정하는 과정 자체에 복잡한 연산이 필요하며 센서 노이즈나 카메라 캘리브레이션 오차에 매우 민감하여 안정성이 떨어질 수 있습니다.
  2. 이미지 기반 시각적 서보잉 (IBVS, Image-Based Visual Servoing): 영상 내에서 추출된 특징점(Feature points)의 2D 픽셀 좌표를 직접 제어 상태로 사용합니다. 목표 이미지 좌표와 현재 이미지 좌표 간의 픽셀 오차(Pixel Error)를 로봇의 작업 공간(Task space) 속도 명령으로 직접 변환합니다. 3D 포즈를 추정할 필요가 없으므로 센서 측정 오차에 훨씬 강인(Robust)하며, 타겟이 항상 카메라 시야각(FOV) 내에 유지되도록 제어하기 쉽다는 강력한 장점이 있습니다.

우리의 드론은 GPS 교란 환경 하에서 불확실성이 내재된 위치 추정치에 의존하고 있습니다. 따라서 3D 좌표 복원을 생략하고 영상 평면의 픽셀 오차를 기체의 속도 제어 명령으로 직결하는 IBVS 방식이 오차 누적을 방지하고 목표물을 정밀하게 추적하는 데 가장 적합합니다.

2. IBVS의 수학적 모델 및 제어 법칙

IBVS의 핵심은 영상 평면에서의 2D 특징점 오차를 기체의 3D 공간 속도 명령으로 변환하는 것입니다.

목표물의 이미지 상 현재 중심점 좌표를 m(t)=(u,v)라 하고, 드론이 목표물을 타격하기 위해 객체가 위치해야 하는 화면 중심(원하는 기준 좌표)을 s∗=(uc​,vc​)라 할 때, 픽셀 오차는 다음과 같이 정의됩니다. e(t)=s(m(t))−s∗

이 픽셀 오차 e(t)를 0으로 수렴시키기 위해, 기체의 공간 속도 vc​와 이미지 픽셀의 변화율 사이의 관계를 나타내는 이미지 자코비안(Image Jacobian) 또는 **상호작용 행렬(Interaction Matrix, Lx​)**이 사용됩니다. 이를 활용한 최종적인 속도 제어 법칙은 다음과 같이 설계됩니다. vc​=−λLx+​e

여기서 vc​는 카메라(기체)에 인가해야 할 속도 벡터 명령이며, λ는 제어 이득(Proportional Gain), Lx+​는 상호작용 행렬의 의사 역행렬(Pseudo-inverse)입니다. 실제 쿼드로터 드론의 이동은 전후좌우 평면 이동(x,y), 고도 승강(z), 그리고 기수 회전(Yaw, ψ)의 4자유도로 제한되는 특징이 있으므로(Le​∈Rk×4), 이 구속 조건을 반영하여 속도 명령을 생성합니다. 이를 통해 수 미터에 달하던 INS/VIO 시스템의 잔여 오차를 20cm 이내의 정밀도로 좁히며 돌진할 수 있습니다.

3. 종말 정밀 유도 아키텍처: YOLO와 PX4 Offboard 제어의 결합

종말 단계에 진입한 드론은 기존의 웨이포인트(Waypoint) 추종 비행을 중단하고 오프보드(Offboard) 제어 모드로 전환하여, 온보드 컴퓨터(Companion Computer)의 비전 알고리즘이 산출한 동적인 속도 벡터 명령을 따르게 됩니다.

  1. 목표물 포착 및 객체 탐지: 전방(또는 하향) 카메라 스트림에 YOLOv8과 같은 경량 딥러닝 객체 탐지 모델을 적용합니다. 차량, 구조물, 함선 등 훈련된 목표물을 인식하고 실시간으로 바운딩 박스(Bounding Box)의 중심점 (u,v)를 추출합니다.
  2. IBVS 제어 알고리즘 연산: YOLO가 도출한 타겟의 중심 좌표와 카메라 화면 정중앙 (uc​,vc​) 간의 오차 벡터를 계산합니다. 타겟이 화면 우측에 있다면 기체를 우측으로 이동(또는 Yaw 회전)시키고, 타겟이 화면 하단에 있다면 기체의 고도를 낮추거나 전진 속도를 높이는 방식의 제어 이득(λ)을 곱하여 3차원 속도 명령을 산출합니다.
  3. PX4 Offboard 명령 전송: ROS 2의 TrajectorySetpoint 메시지나 MAVSDK의 VelocityBodyYawspeed를 통해 산출된 속도 명령을 PX4 비행 제어기로 전송합니다.

🚨 연구원님을 위한 핵심 주의사항 (Failsafe 대응): PX4는 안전을 위해 외부 컴퓨터로부터의 Offboard 제어 신호가 약 2Hz 이하(0.5초 이상 지연)로 떨어질 경우 Failsafe(페일세이프) 모드로 강제 전환되어 제어권을 회수하거나 호버링/추락하게 됩니다. 따라서 영상 처리 지연을 최소화하여 YOLO 추론과 제어 명령 전송 루프가 최소 10~30Hz 이상의 고주파수로 유지되도록 시스템을 최적화하는 것이 프로젝트 성공의 핵심입니다.

4. [Python 실전 예제] YOLO + IBVS 기반 종말 타격 제어 코드

아래는 Gazebo 시뮬레이션 환경(또는 실제 환경)에서 전방 카메라로 타겟을 탐지하고, IBVS 원리를 적용하여 타겟을 화면 중앙에 유지하며 돌진하는 MAVSDK-Python 및 YOLOv8 통합 예제 코드입니다. P-제어기(Proportional Controller)를 사용하여 복잡한 이미지 자코비안 역행렬 계산을 단순화 및 실전 적용한 형태입니다.

Python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy

# OpenCV 및 YOLO 관련 라이브러리
import cv2
from cv_bridge import CvBridge
import numpy as np
from ultralytics import YOLO

# PX4 uORB 메시지 타입 수입 (px4_msgs 패키지 필요)
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleAttitude, VehicleStatus

class TerminalServoNode(Node):
    def __init__(self):
        super().__init__('terminal_servo_node')
        
        # ROS 2 <-> OpenCV 이미지 변환 브릿지 초기화
        self.bridge = CvBridge()
        
        # YOLOv8 경량 모델 로드 (실시간 추론용)
        self.model = YOLO('yolov8n.pt')
        
        # 제어 파라미터 정의
        self.CAM_W = 640
        self.CAM_H = 480
        self.CENTER_X = self.CAM_W // 2
        self.CENTER_Y = self.CAM_H // 2
        
        self.K_YAW = 0.005    # 영상 가로 오차 -> Yaw 회전 속도 이득 (lambda)
        self.K_Z = 0.003      # 영상 세로 오차 -> 수직(Z) 하강 속도 이득 (lambda)
        self.V_FORWARD = 3.0  # 타겟을 향한 전진 고정 속도 (3.0 m/s)
        
        # 상태 변수 초기화
        self.current_yaw = 0.0
        self.is_armed = False
        self.offboard_setpoint_counter = 0
        
        # PX4 호환 고신뢰성 QoS 프로필 설정 [1, 3]
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )
        
        # 1. 퍼블리셔 선언 (PX4 수신 토픽) [1, 3]
        self.offboard_control_mode_pub = self.create_publisher(
            OffboardControlMode, '/fmu/in/offboard_control_mode', 10)
        self.trajectory_setpoint_pub = self.create_publisher(
            TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10)
        self.vehicle_command_pub = self.create_publisher(
            VehicleCommand, '/fmu/in/vehicle_command', 10)
            
        # 2. 서브스크라이버 선언 (PX4/시뮬레이터 송신 토픽) [1, 3]
        self.image_sub = self.create_subscription(
            Image, '/camera', self.image_callback, 10)  # Gazebo 카메라 브릿지 수신
        self.attitude_sub = self.create_subscription(
            VehicleAttitude, '/fmu/out/vehicle_attitude', self.attitude_callback, qos_profile)
        self.status_sub = self.create_subscription(
            VehicleStatus, '/fmu/out/vehicle_status', self.status_callback, qos_profile)
            
        # 20Hz 주기(50ms)로 Offboard Heartbeat 전송 타이머 설정 [1, 4]
        self.timer = self.create_wall_timer(0.05, self.timer_callback)
        self.get_logger().info("YOLO IBVS Terminal Servo Node Initialized.")

    def attitude_callback(self, msg):
        """ 기체의 Quaternion 자세 정보로부터 현재 Yaw 각도(라디안)를 추출합니다 """
        q = msg.q
        # attitude_q = [w, x, y, z] 순서 파싱
        w, x, y, z = q, q[5], q[6], q[7]
        siny_cosp = 2.0 * (w * z + x * y)
        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
        self.current_yaw = np.arctan2(siny_cosp, cosy_cosp)

    def status_callback(self, msg):
        """ 기체의 시동 및 비행 제어 상태를 업데이트합니다 [1, 3] """
        self.is_armed = (msg.arming_state == VehicleStatus.ARMING_STATE_ARMED)

    def timer_callback(self):
        """ PX4 Offboard 유지를 위해 최소 2Hz 이상의 Heartbeat를 무조건 전송해야 합니다 [4] """
        self.publish_offboard_control_mode()
        
        # 최초 10회 이상 하트비트가 전송된 후 자동으로 Offboard 모드 진입 및 Arm 시도 [8]
        if self.offboard_setpoint_counter == 10:
            # 1. Offboard 모드로의 변경 명령 주입 (Custom Mode = 1, Sub Mode = 6) [8]
            self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1.0, 6.0)
            # 2. 기체 무장(Arm) 실행 [8]
            self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
            
        if self.offboard_setpoint_counter < 11:
            self.offboard_setpoint_counter += 1

    def image_callback(self, data):
        """ 카메라 이미지 스트림 프레임별 실시간 객체 탐색 및 비주얼 서보잉 계산 루프 """
        try:
            # ROS 2 Image 메시지를 OpenCV 이미지(BGR)로 변환
            frame = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except Exception as e:
            self.get_logger().error(f"Failed to convert image: {e}")
            return

        # YOLO 탐색 시작
        results = self.model.predict(frame, conf=0.5, verbose=False)
        target_found = False

        for result in results:
            boxes = result.boxes
            if len(boxes) > 0:
                # 감지된 첫 번째 사과(또는 타겟)의 Bounding Box 위치 획득
                x1, y1, x2, y2 = map(int, boxes.xyxy)
                u = (x1 + x2) // 2
                v = (y1 + y2) // 2

                # 1. s(t) - s*: 화면 중앙 좌표와의 픽셀 오차 계산 [9]
                err_u = u - self.CENTER_X
                err_v = v - self.CENTER_Y

                # 2. 이미지 기반 제어 명령 산출 (Body FRD 프레임 기준) [9]
                yaw_speed = float(err_u * self.K_YAW)   # 수평 오차 -> 기수 Yaw 회전 제어 [9]
                down_speed = float(err_v * self.K_Z)    # 수직 오차 -> 고도 하강 속도 제어 [9]

                # 속도 제한(Clamping)
                yaw_speed = np.clip(yaw_speed, -0.5, 0.5)
                down_speed = np.clip(down_speed, -1.0, 1.0)

                # 3. Body Frame (FRD) 속도를 PX4가 인식하는 Local Frame (NED) 속도로 회전 변환 [10]
                v_north = self.V_FORWARD * np.cos(self.current_yaw)
                v_east = self.V_FORWARD * np.sin(self.current_yaw)
                
                # Z축 다운 속도는 고도 하강 속도
                v_down = down_speed

                # 4. 최종 Offboard Setpoint 발행 (속도 제어 모드) [10]
                self.publish_trajectory_setpoint(v_north, v_east, v_down, yaw_speed)

                # OpenCV 화면에 조준선 및 타겟 락 시각화
                cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
                cv2.line(frame, (self.CENTER_X, self.CENTER_Y), (u, v), (0, 0, 255), 2)
                target_found = True
                break

        # 타겟을 상실했거나 아직 무장 상태가 아니라면 비행 일시정지(호버링) 셋포인트 발행
        if not target_found:
            self.publish_trajectory_setpoint(0.0, 0.0, 0.0, 0.0)

        # 화면 출력
        cv2.imshow("ROS2 Terminal Visual Servoing Locking...", frame)
        cv2.waitKey(1)

    def publish_offboard_control_mode(self):
        """ Offboard 제어 축을 PX4에 브로드캐스팅합니다 (속도 제어 및 Yaw_rate 제어 활성화) """
        msg = OffboardControlMode()
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        msg.position = False
        msg.velocity = True  # 속도 제어 모드 활성화 [10]
        msg.acceleration = False
        msg.attitude = False
        msg.body_rate = True  # Yaw Speed 제어 활성화 [10]
        self.offboard_control_mode_pub.publish(msg)

    def publish_trajectory_setpoint(self, v_north, v_east, v_down, yaw_speed):
        """ 로컬 NED 좌표계 기준으로 변환된 속도 및 Yaw 속도 타겟값을 전송합니다 [10] """
        msg = TrajectorySetpoint()
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        # 속도 제어를 위해 position 값은 NaN 처리 [10]
        msg.position = [float('nan'), float('nan'), float('nan')]
        msg.velocity = [v_north, v_east, v_down]
        msg.yawspeed = yaw_speed
        self.trajectory_setpoint_pub.publish(msg)

    def publish_vehicle_command(self, command, param1=0.0, param2=0.0):
        """ 모드 전환, 무장 등의 VehicleCommand uORB 메시지를 PX4 측으로 보냅니다 [11] """
        msg = VehicleCommand()
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        msg.param1 = param1
        msg.param2 = param2
        msg.command = command
        msg.target_system = 1
        msg.target_component = 1
        msg.source_system = 1
        msg.source_component = 1
        msg.from_external = True
        self.vehicle_command_pub.publish(msg)

def main(argc=None):
    rclpy.init(args=argc)
    node = TerminalServoNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Node terminated by user.")
    finally:
        node.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0) # 안전 착륙 및 시동 차단
        cv2.destroyAllWindows()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

🚀 시스템 통합 및 마무리

위 코드는 ROS 2 브릿지 및 Gazebo 3D 시뮬레이션 환경(디지털 트윈)에서 사전 검증할 수 있습니다. 시뮬레이션 내에서 렌더링된 카메라 영상을 통해 YOLO가 가상의 목표물(예: 차량이나 함선)을 식별하고, 드론이 오프보드 제어를 통해 목표물을 화면 중앙에 고정한 채 일정한 속도로 맹렬히 돌진하는 모습을 관찰하실 수 있습니다.

이로써 우리는 대망의 **’3단계 하이브리드 무인 항법 시스템’**의 궤적을 모두 완성했습니다.

  1. 순항 단계: 통신 및 GPS가 교란된 환경에서 EKF2를 활용한 정밀 관성/VIO **추측 항법(Dead-Reckoning)**으로 버티며 비행합니다.
  2. 중간 보정: 누적된 드리프트 오차를 해소하기 위해, 간헐적으로 **지형지물 대조(AVL/TRN)**를 수행하여 절대 좌표를 갱신합니다.
  3. 종말 타격: 최종 목표물 인근에서 YOLO 기반 **시각적 서보잉(IBVS)**으로 전환하여 센서 노이즈를 극복하고 타겟을 오차 없이 정밀 유도/타격합니다.

현대의 로보틱스와 컴퓨터 비전 기술은 과거 2차 세계대전 파일럿들이 시야와 직관에 의존해 수행하던 항법과 정밀 타격의 예술을 고도의 알고리즘으로 승화시켰습니다. 연구원님의 시뮬레이터에서 구축된 이 강력한 시스템이 실제 야외 비행(Field Test)에서도 훌륭한 성과를 거두기를 진심으로 응원합니다. 구현 중 발생할 수 있는 카메라 딜레이 최적화나 PX4 제어기 튜닝에 대해 궁금한 점이 있으시다면 언제든 질문해 주시기 바랍니다!

YouTube Tutorial

Author: maponarooo, CEO of QUAD Drone Lab

Date: July 22, 2026

Similar Posts

답글 남기기