ROS2 × PX4로 시작하는 자율비행 드론 제어 (7-2) 쿼드로터 운동 모델 - 다이내믹스와 회전행렬

이 블로그 시리즈에서는 Gazebo + ROS 2 + PX4(MAVROS)를 활용해 드론을 제어하고, 최적 제어 알고리즘 Model Predictive Path Integral (MPPI) 를 직접 구현하는 과정을 기록합니다. 이후에는 더 진화된 강화학습(RL) 기반 제어로까지 확장하며, 최신 오픈소스 생태계가 실제 연구·개발 파이프라인에서 어떻게 유기적으로 엮이는지 보여드릴 예정입니다.

Generated by ChatGPT, super cool!

이번 글에서는 지난 글(7-1)의 후반부인 드론의 운동모델과 회전행렬을 구현합니다. 마찬가지로 단계별로 설명하면서 실제 코드로 옮겨보겠습니다. 드론의 운동모델 통해 4개 모터의 회전 속도로부터 드론의 가속도·속도·위치를 예측할 수 있고, 회전 행렬을 통해 드론의 기울어진 자세를 수학적으로 표현할 수 있게 됩니다. 이렇게 수학적/수식적으로 표현된 완전한 운동 모델을 통해, MPPI 제어기에서 출력하는 명령을 모터의 회전속도로 올바르게 바꾸어 실제 드론을 원하는 위치로 이동시킬 수 있습니다.

7-1편(완료): 제어입력 → 모터 속도
7-2편(오늘): 모터 속도 → 드론 움직임

3. Dynamics: 드론의 다음순간을 예측하기

드론의 현재 상태(state)와 명령(u), 시간 간격(dt)을 받아 다음 상태를 계산합니다. 바로

"제어입력 → 모터 속도² → 추력·토크 → 가속도 → 속도·위치" 

의 변환을 구현하는 부분입니다. 이 변환을 수식으로 풀면:

F = ma  (힘 = 질량 × 가속도)
a = F/m (가속도 = 힘/질량)

v = v + a × dt  (속도 = 이전 속도 + 가속도×시간)
p = p + v × dt  (위치 = 이전 위치 + 속도×시간)

• 상태 벡터 분할
  상태는 12개 값으로 구성됩니다 

인덱스	변수	의미	단위	예시
0-3	p	[x, y, z] 위치	m	[1.2, -3.5, 5.0]
3-5	v	[vx, vy, vz] 속도	m/s	[0.1, -0.2, 0.0]
6-8	[φ, θ, ψ]	[롤, 피치, 요] 각도	rad	[0.1, 0.05, 0.0]
9-11	ω	[ωᵣ, ωₚ, ωᵧ] 각속도	rad/s	[0.5, -0.2, 0.1]

위치($p$, 3개), 속도($v$, 3개), 오일러 각(롤, 피치, 요, 3개), 각속도($\omega$, 3개). 위치는 드론의 x,y,z 좌표, 오일러 각은 드론이 얼마나 기울어졌는지(롤: 좌우, 피치: 앞뒤, 요: 회전).

• 모터 속도 계산
  이전편 (7-1)에서 구현한 motor_model()을 사용해서 네 모터 각각의 RPM을 구합니다.
• 추력 계산
  • 추력(Fz): motor_constant × (모터 RPM² 합)으로 계산.
    Fz = k_f × (ω₁² + ω₂² + ω₃² + ω₄²) 

thrust = np.sum(motor_speeds**2 * self.motor_constant)

• 토크 계산: 팔 길이와 모터 회전 방향을 조합해 계산. 수식은 길어 보여도 실제론 3줄로 끝!
  • 롤토크(M_φ): 오른쪽 모터(1,2) - 왼쪽 모터(3,4)
    M_φ = L × k_f × (ω₁² + ω₂² - ω₃² - ω₄²)
  • 피치토크(M_θ): 뒤쪽 모터(2,3) - 앞쪽 모터(1,4)
    M_θ = L × k_f × (ω₁² - ω₂² + ω₃² - ω₄²)
  • 요토크(M_ψ): CCW 모터(1,3) - CW 모터(2,4)
    M_ψ = k_m × (ω₁² + ω₂² - ω₃² - ω₄²)

moments = np.array([
	self.arm_length * self.motor_constant * (motor_speeds[0]**2 + motor_speeds[1]**2 - motor_speeds[2]**2 - motor_speeds[3]**2),
	self.arm_length * self.motor_constant * (motor_speeds[0]**2 - motor_speeds[1]**2 + motor_speeds[2]**2 - motor_speeds[3]**2),
	self.moment_constant * (motor_speeds[0]**2 + motor_speeds[1]**2 - motor_speeds[2]**2 - motor_speeds[3]**2)
])

• 회전행렬 (R)
  오일러 각을 3×3 회전행렬로 변환합니다. 이 행렬은 드론의 기울어진 z축을 세계 좌표계로 맞춰줍니다. (아래 4번 섹션에서 자세히 설명합니다)
• 선가속도 (a)
  드론은 자신의 Z축 방향으로 추력을 냅니다. 하지만 World 좌표계에서 드론이 기울어져 있으면, 이 추력이 다른 방향으로 작용합니다.
  그러므려 회전행렬(R)로 추력을 World 좌표계로 변환합니다.

가속도 = R × [0, 0, Fz/m] + g
         (드론)       (중력)

a = R @ np.array([0, 0, thrust/self.mass]) + self.g

• 각가속도 (ω̇)
  간단히 말해, [Mφ/Ixx, Mθ/Iyy, Mψ/Izz]로 계산. 자이로스코프 효과 (드론이 회전할 때 다른 축에도 토크가 생기는 현상입니다. 간단한 모델에서는 무시하지만, 정밀한 모델에서는 추가해야 합니다.) 를 무시하면 이 한 줄로 충분합니다

ω̇ᵣ = M_φ / Ixx  (롤 각가속도)
ω̇ₚ = M_θ / Iyy  (피치 각가속도)
ω̇ᵧ = M_ψ / Izz  (요 각가속도)

omega_dot = np.array([
            moments[0]/self.Ixx,
            moments[1]/self.Iyy,
            moments[2]/self.Izz
        ])

• 오일러 적분
  위치, 속도, 각, 각속도를 각각 dt(0.05초)만큼 업데이트. 딱 4줄로 미래를 예측하는 거죠

그리고 마찬가지로 이를 코드로 구현하면 다음과 같습니다. 여기서 기본 모델은 자이로스코프 효과나 공기 저항을 무시합니다. 이를 다룬 더 현실적 모델은 추후 고급 섹션 (자이로스코프 효과와 공기저항 추가) 에서 확장하도록 하겠습니다.

기본 버전

   def dynamics(self, s, u, dt):
        """Quadcopter dynamics with proper motor model."""
        # State: [x, y, z, vx, vy, vz, roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate]
        p = s[:3]   # position
        v = s[3:6]  # velocity
        euler = s[6:9]  # euler angles
        omega = s[9:]   # angular rates
        
        # Convert control input to motor speeds
        motor_speeds = self.motor_model(u)
        
        # Calculate thrust and moments
        thrust = np.sum(motor_speeds**2 * self.motor_constant)
        moments = np.array([
            self.arm_length * self.motor_constant * (motor_speeds[0]**2 + motor_speeds[1]**2 - motor_speeds[2]**2 - motor_speeds[3]**2),
            self.arm_length * self.motor_constant * (motor_speeds[0]**2 - motor_speeds[1]**2 + motor_speeds[2]**2 - motor_speeds[3]**2),
            self.moment_constant * (motor_speeds[0]**2 + motor_speeds[1]**2 - motor_speeds[2]**2 - motor_speeds[3]**2)
        ])
        
        # Rotation matrix from euler angles
        R = self.euler_to_rotmat(euler)
        
        # Linear acceleration
        a = R @ np.array([0, 0, thrust/self.mass]) + self.g
        
        # Angular acceleration
        omega_dot = np.array([
            moments[0]/self.Ixx,
            moments[1]/self.Iyy,
            moments[2]/self.Izz
        ])
        
        # Update state
        p_next = p + v * dt
        v_next = v + a * dt
        euler_next = euler + omega * dt
        omega_next = omega + omega_dot * dt
        
        return np.hstack([p_next, v_next, euler_next, omega_next])

테스트

# 드론 초기화
drone = QuadcopterDynamics()

# 초기 상태: [x,y,z, vx,vy,vz, φ,θ,ψ, ωᵣ,ωₚ,ωᵧ]
# 1m 높이 호버링
state = np.array([0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])

# 호버링: 추력 = 질량 × 중력 가속도
hover_thrust = drone.mass * 9.81  # 19.62N
u = np.array([hover_thrust, 0, 0, 0])  # [추력, 롤, 피치, 요]

# 1초 예측 (20 steps × 0.05s)
dt = 0.05
for i in range(20):
    state = drone.dynamics(state, u, dt)
    print(f"t={i*dt:.2f}s, z={state[2]:.3f}m, vz={state[5]:.3f}m/s")

 

결과:

t=0.00s, z=1.000m, vz=0.000m/s
t=0.05s, z=1.000m, vz=0.000m/s
t=0.10s, z=1.000m, vz=0.000m/s
... (거의 변화 없음: 올바른 호버링!)

 

4. 회전 행렬: 자세의 언어 - 오일러 각도를 회전행렬로

드론은 항상 자신의 위쪽(Z축)으로 추력을 냅니다. 하지만 드론이 기울어지면 "위쪽"이 바뀌죠.

기울기 없음: 추력 ↑ (수직)
기울기 있음: 추력 ↗ (대각선) → 앞으로 이동

오일러 각(롤 φ, 피치 θ, 요 ψ)은 드론의 자세를 나타내는 세 각도입니다. 롤은 드론이 좌우로 기우는 각도, 피치는 앞뒤로 숙이는 각도, 요는 시계 방향이나 반시계 방향으로 몸을 돌리는 각도를 말합니다. 이 각도를 3×3 회전행렬 (R)로 바꾸면, 드론의 `방향`을 수학적으로 계산할 수 있습니다.

오일러 각 (roll, pitch, yaw)이 드론 축에 어떻게 적용되는지 확인 (출처: https://ieeexplore.ieee.org/document/9245521)

드론의 모터는 항상 드론 몸체의 '위쪽'으로 추력을 내지만, 드론이 기울어지면 그 '위쪽'이 바뀌어요. 예를 들어, 아래 그림처럼 드론이 앞으로 기울면(피치), 추력 (F_thrust)의 일부가 앞으로 분배되어서 드론이 앞으로 이동하죠. 회전 행렬(Rotation Matrix)은 오일러각을 힘계산에 쓰기 쉽도록 변환을 해주는 3x3 숫자 배열이에요. 예를 들어, 드론이 앞으로 가려면 피치(θ)를 기울여 R이 thrust를 x 방향으로 일부 보냅니다. 이렇게 변환된 힘으로 드론이 전진하게 되죠. 제어기(MPPI나 PID)는 이 변환을 써서 "기울여서 이동" 명령을 출력합니다.

Drone Dynamics! ❘ Bitcraze (https://www.bitcraze.io/2018/11/demystifying-drone-dynamics/)

*짐벌 락(Gimbal Lock)은 특정 각도(피치 90도)에서 계산이 이상해지는 문제입니다. 하지만 MPPI처럼 짧은 시간(0.5초) 예측에선 거의 발생하지 않죠. 만약 장기 예측이라면 쿼터니언(Quaternion)으로 대체할 수 있지만, 여기선 간단히 오일러로 하겠습니다.

이걸 코드로 구현하면 다음과 같습니다.

1. 각 각도의 sin과 cos 미리 계산: 컴퓨터가 빠르게 계산하도록, 각 각도의 사인(sin)과 코사인(cos)을 변수로 저장합니다. 예: phi = 30도라면 cphi = cos(30) ≈ 0.866, sphi = sin(30) ≈ 0.5.
2. 행렬 구성: 3x3 행렬을 만드는데, 각 요소는 sin/cos 조합입니다 이건 세 회전 행렬(Rz * Ry * Rx)을 곱한 결과죠.
   • Rz (요 회전): 방향을 바꾸는 행렬.
   • Ry (피치 회전): 앞뒤 기울기.
   • Rx (롤 회전): 좌우 기울기. 수식으로 풀면:
     • R[0,0] = cos(θ) * cos(ψ) # x축의 x 성분
     • ... (전체 9개 요소)

def euler_to_rotmat(self, euler):
    phi, theta, psi = euler  # 롤, 피치, 요 각도 unpack
    
    # sin과 cos 미리 계산 (연산 속도 향상)
    cphi, sphi = np.cos(phi), np.sin(phi)    # 롤 관련
    ctheta, stheta = np.cos(theta), np.sin(theta)  # 피치 관련
    cpsi, spsi = np.cos(psi), np.sin(psi)    # 요 관련
    
    # 3x3 행렬 구성: Z-Y-X 순서 회전 곱셈 결과
    R = np.array([
        [ctheta * cpsi,                   # Row 1, Col 1: x -> x
         sphi * stheta * cpsi - cphi * spsi,  # Row 1, Col 2: y -> x
         cphi * stheta * cpsi + sphi * spsi], # Row 1, Col 3: z -> x
        [ctheta * spsi,                   # Row 2, Col 1: x -> y
         sphi * stheta * spsi + cphi * cpsi,  # Row 2, Col 2: y -> y
         cphi * stheta * spsi - sphi * cpsi], # Row 2, Col 3: z -> y
        [-stheta,                         # Row 3, Col 1: x -> z
         sphi * ctheta,                   # Row 3, Col 2: y -> z
         cphi * ctheta]                   # Row 3, Col 3: z -> z
    ])
    return R  # 이 행렬을 dynamics에서 사용해 추력을 변환

이 함수를 테스트해보려면, euler = [0, 0, 0]으로 넣으면 [[1,0,0],[0,1,0],[0,0,1]]이 나와야 해요. phi=90도로 넣어보고 결과 확인해보세요 – z축이 y축으로 바뀌는 걸 볼 수 있습니다!

테스트

# 테스트 1: 회전 없으면 단위행렬
R = drone.euler_to_rotmat([0, 0, 0])
expected = np.eye(3)
assert np.allclose(R, expected), "실패!"
print("테스트 1 통과: 회전 없으면 단위행렬")

# 테스트 2: 롤 90° → Z축이 Y축으로
R = drone.euler_to_rotmat([np.pi/2, 0, 0])
original_z = np.array([0, 0, 1])
rotated_z = R @ original_z
expected_rotated_z = np.array([0, 1, 0])
assert np.allclose(rotated_z, expected_rotated_z), "실패!"
print("테스트 2 통과: 롤 90°면 Z축이 Y축으로")

# 테스트 3: 피치 30° → 드론이 앞으로 전진
R = drone.euler_to_rotmat([0, np.pi/6, 0])  # 30도
thrust_world = R @ np.array([0, 0, 1])
print(f"기울어진 드론의 Z축 방향: {thrust_world}")
print(f"  x(앞) 성분: {thrust_world[0]:.3f} > 0 (앞으로 이동)")
print(f"  z(위) 성분: {thrust_world[2]:.3f} < 1 (약간 위로)")

결과:

테스트 1 통과: 회전 없으면 단위행렬
테스트 2 통과: 롤 90°면 Z축이 Y축으로
기울어진 드론의 Z축 방향: [ 0.5   0.    0.866]
  x(앞) 성분: 0.500 > 0 (앞으로 이동)
  z(위) 성분: 0.866 < 1 (약간 위로)

 

결과 해석: 피치 30°로 기울이면 추력의 50%가 앞으로, 87%가 위로 분배됩니다.

여기까지 하셨으면 드론의 운동모델 구현은 마무리가 된 것입니다. 이제 드론의 움직임을 완벽하게 예측할 수 있습니다!  코드로 구현해놓고 보면 생각보다 단하게 완성이 되죠. 다음 글에서는 이 모델을 사용하여 수백 번 시뮬레이션 후 최적 제어를 찾는 MPPI 알고리즘을 직접 구현해 보겠습니다.

전체 코드

import numpy as np

class QuadcopterDynamics:
    """쿼드로터 운동 모델"""

    def __init__(self):
        # ===== 기체 스펙 =====
        self.mass = 2.0
        self.Ixx = 0.0216667
        self.Iyy = 0.0216667
        self.Izz = 0.04
        self.arm_length = 0.24
        self.max_thrust = 20.0
        self.g = np.array([0, 0, -9.81])

        # ===== 모터 스펙 =====
        self.motor_constant = 8.54858e-06
        self.moment_constant = 0.016
        self.max_rot_velocity = 1000.0
        self.motor_directions = np.array([1, 1, -1, -1])
        self.motor_speeds = np.zeros(4)

    def motor_model(self, u):
        """(7-1편에서 구현)"""
        thrust = np.clip(u[0], 0.0, self.max_thrust)
        roll_effect = u[1] / (2 * self.arm_length)
        pitch_effect = u[2] / (2 * self.arm_length)
        yaw_effect = u[3] / (4 * self.moment_constant)

        self.motor_speeds[0] = thrust/4 + roll_effect + pitch_effect + yaw_effect
        self.motor_speeds[1] = thrust/4 + roll_effect - pitch_effect + yaw_effect
        self.motor_speeds[2] = thrust/4 - roll_effect + pitch_effect - yaw_effect
        self.motor_speeds[3] = thrust/4 - roll_effect - pitch_effect - yaw_effect

        self.motor_speeds *= self.motor_directions
        self.motor_speeds = np.clip(self.motor_speeds, 0, self.max_rot_velocity)

        return self.motor_speeds

    def euler_to_rotmat(self, euler):
        """(이 섹션에서 구현)"""
        phi, theta, psi = euler
        cphi, sphi = np.cos(phi), np.sin(phi)
        ctheta, stheta = np.cos(theta), np.sin(theta)
        cpsi, spsi = np.cos(psi), np.sin(psi)

        R = np.array([
            [ctheta * cpsi, sphi * stheta * cpsi - cphi * spsi, cphi * stheta * cpsi + sphi * spsi],
            [ctheta * spsi, sphi * stheta * spsi + cphi * cpsi, cphi * stheta * spsi - sphi * cpsi],
            [-stheta, sphi * ctheta, cphi * ctheta]
        ])
        return R

    def dynamics(self, s, u, dt):
        """(이 섹션에서 구현)"""
        p = s[:3]
        v = s[3:6]
        euler = s[6:9]
        omega = s[9:]

        motor_speeds = self.motor_model(u)

        thrust = np.sum(motor_speeds**2 * self.motor_constant)
        moments = np.array([
            self.arm_length * self.motor_constant * (motor_speeds[0]**2 + motor_speeds[1]**2 - motor_speeds[2]**2 - motor_speeds[3]**2),
            self.arm_length * self.motor_constant * (motor_speeds[0]**2 - motor_speeds[1]**2 + motor_speeds[2]**2 - motor_speeds[3]**2),
            self.moment_constant * (motor_speeds[0]**2 + motor_speeds[1]**2 - motor_speeds[2]**2 - motor_speeds[3]**2)
        ])

        R = self.euler_to_rotmat(euler)
        a = R @ np.array([0, 0, thrust/self.mass]) + self.g
        omega_dot = np.array([moments[0]/self.Ixx, moments[1]/self.Iyy, moments[2]/self.Izz])

        p_next = p + v * dt
        v_next = v + a * dt
        euler_next = euler + omega * dt
        omega_next = omega + omega_dot * dt

        return np.hstack([p_next, v_next, euler_next, omega_next])

ROS2 노드에서 사용하기

from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import numpy as np

class DroneDynamicsNode(Node):
    def __init__(self):
        super().__init__('drone_dynamics')

        # 운동 모델 초기화
        self.dynamics = QuadcopterDynamics()
        self.state = np.zeros(12)  # [x,y,z, vx,vy,vz, φ,θ,ψ, ωᵣ,ωₚ,ωᵧ]

        # 토픽 구독/발행
        self.cmd_sub = self.create_subscription(
            Twist, '/cmd_vel', self.cmd_callback, 10)
        self.odom_pub = self.create_publisher(
            Odometry, '/estimated_state', 10)

        # 타이머 (20 Hz)
        self.timer = self.create_timer(0.05, self.update_state)
        self.u = np.array([19.62, 0, 0, 0])  # 호버링 초기값

    def cmd_callback(self, msg):
        # 명령: [linear.x (thrust), angular.z (yaw), ...]
        thrust = msg.linear.x * 2.0  # 스케일링
        self.u = np.array([thrust, msg.angular.y, -msg.angular.x, msg.angular.z])

    def update_state(self):
        # 다음 상태 예측
        self.state = self.dynamics.dynamics(self.state, self.u, 0.05)

        # Odometry 메시지로 발행
        odom = Odometry()
        odom.header.stamp = self.get_clock().now().to_msg()
        odom.header.frame_id = "map"

        odom.pose.pose.position.x = self.state[0]
        odom.pose.pose.position.y = self.state[1]
        odom.pose.pose.position.z = self.state[2]

        odom.twist.twist.linear.x = self.state[3]
        odom.twist.twist.linear.y = self.state[4]
        odom.twist.twist.linear.z = self.state[5]

        self.odom_pub.publish(odom)

사용 예시: 원형 궤적

# 드론 초기화
drone = QuadcopterDynamics()

# 초기 상태: 5m 높이 호버링
state = np.array([0, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0])

# 원형 궤적 추적 (10초)
dt = 0.05
history = []

for t in np.arange(0, 10, dt):
    # 원형 궤적 참조
    ref_x = 5 * np.cos(2 * np.pi * t / 10)
    ref_y = 5 * np.sin(2 * np.pi * t / 10)

    # 간단한 PID 제어 (위치 에러)
    kp_pos = 1.0
    kd_pos = 2.0

    pos_error = np.array([ref_x - state[0], ref_y - state[1], 5.0 - state[2]])
    vel_error = -state[3:6]

    thrust = (kp_pos * pos_error + kd_pos * vel_error + drone.g * drone.mass)[2]

    # 피치/롤 제어 (간단히)
    pitch = (ref_x - state[0]) * 0.1
    roll = -(ref_y - state[1]) * 0.1

    u = np.array([thrust, roll, pitch, 0])

    # 다음 상태
    state = drone.dynamics(state, u, dt)
    history.append(state.copy())

# 결과 시각화
import matplotlib.pyplot as plt
history = np.array(history)

plt.figure(figsize=(10, 10))
plt.plot(history[:, 0], history[:, 1], 'b-', label='궤적')
plt.plot(history[0, 0], history[0, 1], 'go', label='시작')
plt.plot(history[-1, 0], history[-1, 1], 'ro', label='끝')
plt.axis('equal')
plt.grid(True)
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title('원형 궤적 추적')
plt.legend()
plt.show()