[하이브리드 항법 시스템: 제5편] [구현-1단계] 순항 단계: 고전적 추측 항법, 풍향 추정, 그리고 Gazebo 바람 시뮬레이션 구현

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

지난 **<제4편>**에서는 고성능 외부 INS 하드웨어의 데이터를 ROS 2와 Micro XRCE-DDS 브릿지를 통해 PX4 EKF2로 주입하는 인프라 구축 작업을 완료했습니다. 이제 우리의 자폭 드론은 GPS가 끊긴 상황에서도 외부 센서를 믿고 자신의 위치를 추정할 수 있는 ‘신경망’을 갖추게 되었습니다.

오늘부터는 드디어 본격적인 [구현] 단계로 돌입합니다. 이번 **<제5편>**의 주제는 **’순항 단계(Cruising Phase)’**입니다. 수백 킬로미터를 날아가는 자폭 드론에게 있어 가장 큰 적은 무엇일까요? 바로 **’바람(Wind)’**입니다.

GPS가 있을 때는 드론이 바람에 밀려도 실시간으로 절대 위치를 보정하며 경로를 유지하지만, GPS가 차단된(GNSS-Denied) 환경에서 추측 항법(Dead Reckoning)만으로 비행할 때 강력한 측풍(Crosswind)을 맞게 되면 드론은 수 킬로미터 밖으로 휩쓸려가 버립니다.

이번 포스팅에서는 제2차 세계대전 조종사들이 생존을 위해 사용했던 ‘크랩 각(Crab Angle)’ 계산법을 현대 로보틱스로 부활시키고, PX4 EKF2의 실시간 풍향 추정 원리, 그리고 ROS 2 Python을 이용한 보정 코드와 Gazebo 시뮬레이터의 바람 인젝션(Wind Injection) 테스트까지 친절하고 깊이 있게 다뤄보겠습니다.

1. 고전적 추측 항법과 ‘크랩 각(Crab Angle)’의 부활

제2차 세계대전 당시, 망망대해인 태평양 한가운데서 GPS 없이 목표물을 향해 날아가던 전투기 조종사들에게 ‘바람’은 죽음과 직결되는 요소였습니다. 목표물이 정북쪽(0도)에 있다고 해서 기수를 정북쪽으로만 향하고 날아가면, 서쪽에서 불어오는 바람(측풍)에 밀려 결국 동쪽으로 크게 벗어나게 됩니다.

이를 극복하기 위해 조종사들은 비행기의 기수를 바람이 불어오는 쪽으로 살짝 틀어서(게걸음 치듯) 비행했습니다. 이것을 크랩 각(Crab Angle) 또는 **바람 보정 각(Wind Correction Angle, WCA)**이라고 부릅니다.

수학적으로 바람 보정 각(WCA)은 다음 공식으로 계산됩니다. WCA=arcsin(Va​Vw​sin(θ)​)

• Vw​ (Wind Speed): 풍속
• θ (Wind Angle): 목표 경로(Desired Course)와 바람이 불어오는 방향 사이의 각도
• Va​ (True Airspeed): 기체의 진대기속도

이 단순한 벡터 합성 공식이 바로 수백 km를 GPS 없이 날아가는 추측 항법의 생명줄입니다. 그렇다면 우리 드론은 어떻게 풍속(Vw​)과 풍향(θ)을 실시간으로 알 수 있을까요?

2. PX4 EKF2의 실시간 풍향/풍속 추정 (Wind Estimation)

다행히도 똑똑한 PX4의 상태 추정기인 EKF2(Extended Kalman Filter) 내부에는 바람의 속도와 방향을 추정하는 기능이 내장되어 있습니다.

✈️ 고정익(Fixed-Wing)의 풍향 추정

고정익 드론은 기체 전면에 피토관(Airspeed Sensor)을 장착하여 대기속도(Va​)를 직접 측정합니다. EKF2는 GPS(또는 외부 INS)가 제공하는 지면 속도(Ground Speed)와 피토관이 제공하는 대기속도의 벡터 차이를 계산하여 바람을 추정합니다. 또한, GPS가 끊긴 상황에서도 측면 미끄러짐이 ‘0’이라는 가정을 활용하는 Synthetic Sideslip 기능(EKF2_FUSE_BETA = 1)을 통해 훌륭하게 바람을 추정해 냅니다.

🚁 멀티로터(Multicopter)의 풍향 추정: 항력 비사력(Drag Specific Forces)

피토관을 달기 힘든 멀티로터는 어떨까요? 멀티로터가 바람을 맞으며 호버링하거나 전진할 때, 바람의 저항(항력, Drag)을 이겨내기 위해 기체는 바람이 불어오는 방향으로 미세하게 기울어집니다(Pitch/Roll 발생). PX4 EKF2는 IMU의 가속도계가 측정하는 이 **비사력(Specific Force)**을 분석하여 역으로 현재 불어오는 바람의 북/동(North/East) 방향 속도를 추정해 냅니다.

연구원 튜닝 팁: 멀티로터에서 이 기능을 활성화하려면 QGroundControl에서 다음 파라미터를 세팅해야 합니다.

• EKF2_DRAG_CTRL: 항력 융합 활성화 (Enable)
• EKF2_BCOEF_X, EKF2_BCOEF_Y: X, Y축 방향의 탄도 계수(Ballistic coefficient). 기체의 형태와 무게에 따라 튜닝해야 합니다.
• 바람 없는 날 Position 모드에서 기체를 전후좌우로 급가속/급정거시키며 비행한 후, .ulg 로그를 다운받아 PX4에서 제공하는 mc_wind_estimator_tuning.py 스크립트를 돌리면 최적의 파라미터 값을 자동으로 얻을 수 있습니다.

3. [실습] ROS 2 Python을 이용한 크랩 각(Crab Angle) 보정 알고리즘 구현

이제 우리의 컴패니언 컴퓨터(ROS 2)에서 작동할 제어 노드를 작성해 보겠습니다. 이 코드는 PX4가 추정한 실시간 바람 데이터(WindEstimate 메시지)를 구독(Subscribe)하고, 크랩 각(WCA)을 계산한 뒤, 드론이 실제로 바라보아야 할 요(Yaw) 각도를 수정하여 TrajectorySetpoint로 명령을 내리는 구조입니다.

import rclpy
from rclpy.node import Node
import math

# PX4 uORB 메시지 임포트
from px4_msgs.msg import WindEstimate, TrajectorySetpoint, VehicleOdometry

class DeadReckoningWindCorrectionNode(Node):
    def __init__(self):
        super().__init__('dr_wind_correction_node')

        # 바람 추정치 구독 (EKF2가 계산한 실시간 풍향/풍속)
        self.wind_sub = self.create_subscription(
            WindEstimate,
            '/fmu/out/wind_estimate',
            self.wind_callback,
            10
        )
        
        # 현재 속도 구독 (대기속도 추정을 위해)
        self.odom_sub = self.create_subscription(
            VehicleOdometry,
            '/fmu/out/vehicle_odometry',
            self.odom_callback,
            10
        )

        # Offboard 제어를 위한 Setpoint 퍼블리셔
        self.setpoint_pub = self.create_publisher(
            TrajectorySetpoint,
            '/fmu/in/trajectory_setpoint',
            10
        )

        # 제어 루프 타이머 (10Hz)
        self.timer = self.create_timer(0.1, self.control_loop)

        # 상태 변수 초기화
        self.wind_north = 0.0
        self.wind_east = 0.0
        self.current_vel_x = 0.0
        self.current_vel_y = 0.0

        # 목표 경로 설정 (예: 정북쪽 0도 방향으로 10m/s 이동)
        self.desired_course_deg = 0.0  
        self.target_speed = 10.0 # m/s

    def wind_callback(self, msg):
        # EKF2로부터 북쪽(North) 및 동쪽(East) 풍속 획득
        self.wind_north = msg.windspeed_north
        self.wind_east = msg.windspeed_east

    def odom_callback(self, msg):
        self.current_vel_x = msg.velocity
        self.current_vel_y = msg.velocity[7]

    def control_loop(self):
        # 1. 풍속(Wind Speed) 및 풍향(Wind Direction) 계산
        wind_speed = math.sqrt(self.wind_north**2 + self.wind_east**2)
        wind_dir_rad = math.atan2(self.wind_east, self.wind_north)
        
        # 2. 목표 경로(Desired Course) 라디안 변환
        desired_course_rad = math.radians(self.desired_course_deg)
        
        # 3. Wind Angle (목표 경로와 바람 방향의 차이)
        wind_angle_rad = wind_dir_rad - desired_course_rad
        
        # 4. 크랩 각(Wind Correction Angle, WCA) 계산
        # WCA = arcsin( (WindSpeed * sin(WindAngle)) / TrueAirspeed )
        # 드론의 대기속도를 목표 속도로 근사하여 사용
        airspeed = max(self.target_speed, 1.0) # 0으로 나누기 방지
        crosswind_component = wind_speed * math.sin(wind_angle_rad)
        
        # 물리적 한계 방지 (풍속이 대기속도보다 빠르면 정상 비행 불가)
        if abs(crosswind_component) > airspeed:
            crosswind_component = math.copysign(airspeed * 0.99, crosswind_component)
            self.get_logger().warn("경고: 측풍이 기체 속도보다 강력합니다!")

        wca_rad = math.asin(crosswind_component / airspeed)
        
        # 5. 최종 보정된 Yaw 각도 (Required Heading)
        required_heading_rad = desired_course_rad - wca_rad

        # 6. PX4 Offboard Setpoint 퍼블리시
        sp_msg = TrajectorySetpoint()
        sp_msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        
        # 글로벌 좌표계(NED) 기준으로 북쪽으로 이동명령 시,
        # 속도 벡터는 목표 경로 유지, 기체의 기수(Yaw)는 바람을 향해 틂
        sp_msg.velocity = [
            self.target_speed * math.cos(desired_course_rad),
            self.target_speed * math.sin(desired_course_rad),
            0.0 # 고도 유지
        ]
        sp_msg.yaw = required_heading_rad

        self.setpoint_pub.publish(sp_msg)

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

if __name__ == '__main__':
    main()

import rclpy
from rclpy.node import Node
import math

# PX4 uORB 메시지 임포트
from px4_msgs.msg import WindEstimate, TrajectorySetpoint, VehicleOdometry

class DeadReckoningWindCorrectionNode(Node):
    def __init__(self):
        super().__init__('dr_wind_correction_node')

        # 바람 추정치 구독 (EKF2가 계산한 실시간 풍향/풍속)
        self.wind_sub = self.create_subscription(
            WindEstimate,
            '/fmu/out/wind_estimate',
            self.wind_callback,
            10
        )
        
        # 현재 속도 구독 (대기속도 추정을 위해)
        self.odom_sub = self.create_subscription(
            VehicleOdometry,
            '/fmu/out/vehicle_odometry',
            self.odom_callback,
            10
        )

        # Offboard 제어를 위한 Setpoint 퍼블리셔
        self.setpoint_pub = self.create_publisher(
            TrajectorySetpoint,
            '/fmu/in/trajectory_setpoint',
            10
        )

        # 제어 루프 타이머 (10Hz)
        self.timer = self.create_timer(0.1, self.control_loop)

        # 상태 변수 초기화
        self.wind_north = 0.0
        self.wind_east = 0.0
        self.current_vel_x = 0.0
        self.current_vel_y = 0.0

        # 목표 경로 설정 (예: 정북쪽 0도 방향으로 10m/s 이동)
        self.desired_course_deg = 0.0  
        self.target_speed = 10.0 # m/s

    def wind_callback(self, msg):
        # EKF2로부터 북쪽(North) 및 동쪽(East) 풍속 획득
        self.wind_north = msg.windspeed_north
        self.wind_east = msg.windspeed_east

    def odom_callback(self, msg):
        self.current_vel_x = msg.velocity
        self.current_vel_y = msg.velocity[7]

    def control_loop(self):
        # 1. 풍속(Wind Speed) 및 풍향(Wind Direction) 계산
        wind_speed = math.sqrt(self.wind_north**2 + self.wind_east**2)
        wind_dir_rad = math.atan2(self.wind_east, self.wind_north)
        
        # 2. 목표 경로(Desired Course) 라디안 변환
        desired_course_rad = math.radians(self.desired_course_deg)
        
        # 3. Wind Angle (목표 경로와 바람 방향의 차이)
        wind_angle_rad = wind_dir_rad - desired_course_rad
        
        # 4. 크랩 각(Wind Correction Angle, WCA) 계산
        # WCA = arcsin( (WindSpeed * sin(WindAngle)) / TrueAirspeed )
        # 드론의 대기속도를 목표 속도로 근사하여 사용
        airspeed = max(self.target_speed, 1.0) # 0으로 나누기 방지
        crosswind_component = wind_speed * math.sin(wind_angle_rad)
        
        # 물리적 한계 방지 (풍속이 대기속도보다 빠르면 정상 비행 불가)
        if abs(crosswind_component) > airspeed:
            crosswind_component = math.copysign(airspeed * 0.99, crosswind_component)
            self.get_logger().warn("경고: 측풍이 기체 속도보다 강력합니다!")

        wca_rad = math.asin(crosswind_component / airspeed)
        
        # 5. 최종 보정된 Yaw 각도 (Required Heading)
        required_heading_rad = desired_course_rad - wca_rad

        # 6. PX4 Offboard Setpoint 퍼블리시
        sp_msg = TrajectorySetpoint()
        sp_msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        
        # 글로벌 좌표계(NED) 기준으로 북쪽으로 이동명령 시,
        # 속도 벡터는 목표 경로 유지, 기체의 기수(Yaw)는 바람을 향해 틂
        sp_msg.velocity = [
            self.target_speed * math.cos(desired_course_rad),
            self.target_speed * math.sin(desired_course_rad),
            0.0 # 고도 유지
        ]
        sp_msg.yaw = required_heading_rad

        self.setpoint_pub.publish(sp_msg)

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

if __name__ == '__main__':
    main()

위 코드가 실행되면, 측풍이 불 때 기체가 옆으로 흘러가지 않도록 스스로 바람을 향해 고개를 비스듬히 틀고(Crab) 전진하는 모습을 볼 수 있습니다.

저희 QUAD 드론 연구소 📺YouTube 채널 멤버쉽 [💎다이아몬드 회원]으로 가입하시면 풀 테스트를 위한 모든 교재와 소스를 받아 보실 수 있습니다.

4. 내장된 windy.sdf World 파일을 활용한 측풍(Crosswind) 시뮬레이션

우리가 작성한 알고리즘이 잘 작동하는지 확인하려면 시뮬레이터 세상에 ‘강한 바람’을 불어넣어야 합니다.

해당 파일은 PX4-gazebo-models 리포지토리의 worlds/windy.sdf 경로에 위치하고 있으며, 내부적으로 gz-sim-wind-effects-system 플러그인을 사용하여 강력한 측풍을 모사하도록 구현되어 있습니다.

💨 바람 환경 실행 및 테스트 방법

터미널을 열고 환경 변수 PX4_GZ_WORLD에 windy를 지정하여 SITL을 실행하기만 하면 됩니다.

# PX4 펌웨어 디렉토리로 이동
cd /home/user/PX4-Autopilot

# windy 월드를 지정하여 Gazebo 시뮬레이터 실행
make px4_sitl gz_x500_windy #PX4_GZ_WORLD=windy

# PX4 펌웨어 디렉토리로 이동
cd /home/user/PX4-Autopilot

# windy 월드를 지정하여 Gazebo 시뮬레이터 실행
make px4_sitl gz_x500_windy #PX4_GZ_WORLD=windy

이 월드 파일 내부 설정(windy.sdf)을 열어보면 다음과 같은 바람 플러그인 속성을 확인할 수 있습니다.

    <wind>
      <linear_velocity>0 0 0</linear_velocity>
    </wind>

    <wind>
      <linear_velocity>0 0 0</linear_velocity>
    </wind>

연구원 팁: gz-sim-wind-effects-system 플러그인을 이용하면 보다 세밀한 바람 세팅을 할 수 있습니다. YouTube 멤버쉽 강좌 영상에 자세히 설명되어 있으니 꼭 채널 멤버쉽 가입 하셔서 확인해 주세요!

실험 결과 해석:

1. 보정 적용 전: 10m/s로 정북쪽으로 10분간 비행하도록 명령하면, 측면에서 부는 10m/s의 측풍을 맞아 10분 뒤 드론은 목표 지점에서 크게 벗어나 엄청난 위치 오차를 기록하게 됩니다. 완전한 실패입니다.
2. 크랩 각 보정 적용 후 (위 Python 코드 실행): 드론이 EKF2 풍향 추정치를 기반으로 즉각적으로 기수를 바람이 부는 쪽으로 틀어(Crab) 측풍을 상쇄합니다. 10분 뒤 목표 지점 주변 수십 미터 반경 이내에 도달하는 놀라운 결과를 보여줍니다.

마무리하며 및 다음 편 예고

이번 **<제5편>**에서는 제2차 세계대전의 산물인 **’크랩 각(Crab Angle)’**의 수학적 원리를 적용하여, GPS가 끊긴 상황에서도 바람에 휩쓸리지 않고 목표를 향해 순항(Cruising)하는 로직을 Python 코드와 PX4의 windy.sdf 환경을 통해 구현해 보았습니다.

우리의 자폭 드론은 이제 거센 바람을 뚫고 묵묵히 전진할 수 있습니다. 하지만 여전히 문제가 남아있습니다. 아무리 전술급 INS를 사용하고 바람 보정 각을 정밀하게 계산한다고 한들, 100km, 200km를 날아가다 보면 센서 노이즈 이중 적분과 미세한 풍향 추정 오차로 인해 결국 목표 지점으로부터 수십~수백 미터의 오차가 쌓이게 됩니다. (이는 3편에서 시뮬레이션으로 확인했었죠!)

이 누적 오차를 비행 중간중간 ‘0’으로 완벽히 초기화(Reset)해 주어야만 합니다. 과거 미 해군 조종사들이 섬 모양을 눈으로 보고 지도를 대조하여 위치를 수정했던 것처럼 말입니다.

그래서 다음 **<제6편: [구현-2단계] 중간 보정 단계: OpenCV/YOLO 지형지물 대조 및 Digital Twin 테스트>**에서는 무인기 항법의 꽃, 하향 카메라를 이용해 랜드마크(건물, 섬 등)를 시각적으로 인식하고 누적된 오차를 깎아내는 ‘지형 참조 항법(TRN)과 템플릿 매칭’ 기법을 깊이 있게 다뤄보겠습니다. 기대해 주세요!

YouTube Tutorial

Author: maponarooo, CEO of QUAD Drone Lab

Date: May 11, 2026