ROS2 × PX4로 시작하는 자율비행 드론 제어 (7) 쿼드로터 운동 모델 코드구현

항공학도 2025. 5. 20. 14:42

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

이번 글에서는 지난 글까지 이론으로 다졌던 드론의 ‘운동 모델’을 실제 코드로 옮겨보겠습니다. 앞에서 설명드린것 처럼 MPPI 컨트롤러는 "지금 입력을 주면 0.5초 뒤엔 어디 있을까?"를 수백 번 계산해 가장 좋은 입력을 고르는 방법입니다. 이러한 예측을 해주는 블랙박스가 바로 오늘 만들어 볼 운동모델입니다. 이를 통해 모터 PWM → 추력·토크 → 가속도·속도·위치가 전부 계산되고, MPPI·MPC 등 어떤 컨트롤러도 붙일 수 있습니다.

1. 파라미터 정리: 드론의 물리적 특성 한눈에 보기

변수	값 (예시)	의미
기체 관련
mass	2.0 kg	추력 → 가속도 $a=F/m$ 변환
Ixx, Iyy, Izz	0.022 · 0.022 · 0.040 kg·m²	롤·피치·요 축 관성
arm_length	0.24 m	모터축 ↔ 기체 중심 거리
모터 관련
motor_constant (k_f)	8.55×10−68.55×10^{-6} N/(rad/s)²	RPM² → 추력
moment_constant (k_m)	0.016 N·m/(rad/s)²	RPM² → 반작용 요 토크
max_rot_velocity	1000 rad/s	실제 모터의 회전 속도 한계 반영

질량과 관성

$mass$: 우리가 시뮬레이션에서 사용하는 드론은 2 kg짜리 X500 모델 입니다.
관성 는 드론의 프레임이 좌우 대칭이어서, 롤(좌우 기울기) · 피치(앞뒤 기울기) 움직임이 비슷하다는 뜻입니다.
$I_{zz}$ 가 더 큰 이유는 모터와 프로펠러가 수평으로 퍼져 있어서 회전 중심으로부터의 거리가 더 멀기 때문입니다.

팔 길이 (arm_length = 0.24 )

“모터축에서 기체 중심” 거리 입니다. 이 길이는 롤과 피치 토크를 힘으로 바꿀 때 지렛대 역할을 합니다.
24 cm 라면 500 급 프레임에 딱 맞는 크기 입니다. 보통 팔 길이 *2 해서 기체의 크기를 따집니다. (이경우에는 240*2: 480 -> 500 급)

중력 벡터 g

지구의 중력 가속도(-9.81 m/s²)를 z축에 적용합니다. ENU 좌표계(동쪽-북쪽-위)를 따라 z축의 위쪽이 (+)이고 아래쪽이 (-)로 설정됩니다. 이 값은 시뮬레이션마다 항상 더해져 추력이 없을 경우 드론이 땅으로 끌려 내려가게 만듭니다.

모터·프로펠러 상수

motor_constant ($k_f$)는 모터 RPM²을 추력 N(뉴턴)으로 바꾸는 비율 입니다
moment_constant ($k_m$) 은 RPM²을 토크 N·m로 바꾸는 비율 입니다. 이 값들은 실제 프로펠러 테스트나 System ID 로 실측·식별해야 합니다.
time_constant_up(0.0125초), time_constant_down(0.025초): 모터가 속도를 올릴 때(가속)와 내릴 때(감속)의 반응 속도 입니다. 가속 (전류를 공급받아 모터를 가속)이 감속 (프로펠러의 관성과 공기저항) 보다 빠른 게 일반적이죠. 이 값 덕에 모터가 현실처럼 부드럽게 반응합니다.
max_rot_velocity (1000 rad/s ≈ 9500 RPM): 모터가 터무니 없이 빠르게 돌지 않도록 제한하는 안전장치 입니다.
motor_directions [+1, +1, −1, −1]: 앞오른쪽과 뒤왼쪽 모터는 반시계 (CCW), 나머지는 시계 (CW) 방향으로 회전합니다. 요 토크를 계산할 때 각 모터의 회전 부호가 필요합니다.

실제 드론을 이용한 실험에서는 $k_f, k_m, I_{xx}...$ 등의 값은 모두 실측된 값으로 넣어주어야 합니다. 우리는 gazebo simulation에서 x500기체를 활용해 먼저 실험할 것이므로 시뮬레이션 상에 구현된 값을 넣어주겠습니다.

1. x500 기체 모델: [기체 무게, $I_{xx}, I_{yy}, I_{zz}$, arm 길이] 등이 정의 되어 있음

2. x500 모터 모델: [$k_f, k_m$, 모터 최대 회전속도] 등이 정의 되어 있음

그리고 이를 코드로 구현하면 다음과 같습니다.

def __init__(self):
        # Physical parameters
        self.mass = 2.0  # kg
        self.Ixx = 0.0216667  # kg⋅m²
        self.Iyy = 0.0216667  # kg⋅m²
        self.Izz = 0.04  # kg⋅m²
        self.arm_length = 0.24  # m
        self.max_thrust = 20.0  # N
        self.min_thrust = 0.0  # N
        self.g = np.array([0, 0, -9.81])  # m/s²
        
        # Motor parameters
        self.motor_constant = 8.54858e-06
        self.moment_constant = 0.016
        self.time_constant_up = 0.0125
        self.time_constant_down = 0.025
        self.max_rot_velocity = 1000.0
        self.motor_directions = np.array([1, 1, -1, -1])  # CCW/CW
        self.motor_speeds = np.zeros(4)

2. 모터모델: 제어 입력을 모터 속도로 바꾸기

드론은 4개의 모터로 움직입니다. 그런데 MPPI로부터 나오는 제어 입력은 다음과 같습니다. $u$: 총 추력 T, 롤 Mφ, 피치 Mθ, 요 Mψ. 결국 제어기는 드론에게 이렇게 말하는 것과도 같습니다.

"너가 앞으로 떠오르려면 총 몇 N의 힘이 필요하고, 몸을 얼마나 기울이고, 또 얼마나 회전해야 해"

하지만 드론은 이런 명령을 직접 이행하지 못합니다. 드론이 할 수 있는 것은 각 모터의 회전속도(RPM)를 바꾸는 것입니다. 따라서 우리는 제어 입력을 4개 모터의 속도로 바꾸는 함수를 만들어야 합니다. 이 과정을 6단계로 나누어 설명드리겠습니다.

총 추력 클리핑: 비현실적인 입력은 잘라내기
먼저, 입력으로 받은 총 추력 $u[0]$값이 너무 작거나 (음수) 크면 (드론의 실제 최대추력 이상) 미리 걸러냅니다. 드론은 공기 속에서 작동하는 실제 물체이기 때문에 역추력(음수)은 물리적으로 불가능하고, 너무 큰 추력도 모터의 한계를 초과할 수 있기 때문입니다.
- min_thrust = 0.0 → 역추력 방지
- max_thrust = 20.0 N → 모터한계 설정

thrust = np.clip(u[0], self.min_thrust, self.max_thrust)

모멘트를 힘으로 변환
입력으로 들어온 롤, 피치, 요의 회전 토크는 각 모터가 얼마만큼 더 돌거나 덜 돌아야 하는지로 바꿔야 합니다.
- 롤/피치 모멘트는 드론 중심에서의 팔 길이를 기준으로 환산합니다. 팔이 길수록 적은 힘으로도 큰 회전이 생깁니다.
- 요 모멘트는 모터 4개가 각각 회전 방향이 반대여서, 네 개의 회전 방향을 균등하게 고려해야 합니다.

roll_effect  = u[1] / (2 * self.arm_length)
pitch_effect = u[2] / (2 * self.arm_length)
yaw_effect   = u[3] / (4 * self.moment_constant)

추력과 모멘트 조합
각 모터에 “총 추력의 ¼”에 모멘트 효과를 더하거나 빼서 힘을 분배해요.
회전 방향 적용
CCW 모터는 +1, CW 모터는 -1로 부호를 곱해요. 이렇게 해야 나중에 RPM²로 계산할 때 요 토크가 제대로 상쇄됩니다.
RPM 상한 클리핑
모터가 물리적으로 불가능한 속도로 돌지 않도록 최대 회전 속도(1000 rad/s)로 제한해요. 이걸 안 하면 시뮬레이터가 터무니없는 명령을 시도할 수 있어요.
결과 반환
네 모터의 RPM을 배열로 반환해요. 이 값은 다음 0.05초 동안 “이 속도로 돌았다”고 가정합니다.

그리고 이를 코드로 구현하면 다음과 같습니다.

  def motor_model(self, u):
        """Convert control inputs to motor speeds."""
        # u[0] is thrust, u[1:4] are roll, pitch, yaw moments
        thrust = np.clip(u[0], self.min_thrust, self.max_thrust)
        
        # Convert moments to motor speeds
        roll_effect = u[1] / (2 * self.arm_length)
        pitch_effect = u[2] / (2 * self.arm_length)
        yaw_effect = u[3] / (4 * self.moment_constant)
        
        # Calculate individual motor speeds
        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
        
        # Apply motor direction and limits
        self.motor_speeds *= self.motor_directions
        self.motor_speeds = np.clip(self.motor_speeds, 0, self.max_rot_velocity)
        
        return self.motor_speeds

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

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

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

의 변환을 구현하는 부분입니다.

상태 벡터 분할
상태는 12개 값으로 구성됩니다: 위치($p$, 3개), 속도($v$, 3개), 오일러 각(롤, 피치, 요, 3개), 각속도($\omega$, 3개).
모터 속도 계산
위에서 구현한 motor_model()을 사용해서 네 모터의 RPM을 구합니다.
추력과 토크 계산
- 추력(Fz): motor_constant × (모터 RPM² 합)으로 계산.
- 롤, 피치, 요 토크: 팔 길이와 모터 회전 방향을 조합해 계산. 수식은 길어 보여도 실제론 3줄로 끝!
회전행렬 (R)
오일러 각을 3×3 회전행렬로 변환합니다. 이 행렬은 드론의 기울어진 z축을 세계 좌표계로 맞춰줍니다.
선가속도 (a)
드론의 z축 추력을 회전행렬로 세계 좌표계로 변환한 뒤 중력(g)을 더합니다: R @ [0, 0, Fz/m] + g.
각가속도 (ω̇)
간단히 말해, [Mφ/Ixx, Mθ/Iyy, Mψ/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])

4. euler_to_rotmat(): 오일러 각도를 회전행렬로 변환

오일러 각(요→피치→롤 순)을 3×3 회전행렬로 바꾸는 함수예요. 드론이 ±85° 이상 기울어지면 짐벌락 문제가 생길 수 있지만, MPPI처럼 짧은 시간 단위로 재계산하는 구조에선 큰 문제가 안 됩니다. 기울임이 큰 기동을 하게되면 쿼터니언 버전을 쓰는 것도 방법이에요 (후에 다루도록 하겠습니다).

  def euler_to_rotmat(self, euler):
        """Convert euler angles to rotation matrix."""
        roll, pitch, yaw = euler
        cr, cp, cy = np.cos(roll), np.cos(pitch), np.cos(yaw)
        sr, sp, sy = np.sin(roll), np.sin(pitch), np.sin(yaw)
        
        return np.array([
            [cp*cy, sr*sp*cy - cr*sy, cr*sp*cy + sr*sy],
            [cp*sy, sr*sp*sy + cr*cy, cr*sp*sy - sr*cy],
            [-sp, sr*cp, cr*cp]
        ])

여기까지 하셨으면 드론의 운동모델 구현은 마무리가 된 것입니다. 코드로 구현한 운동모델은 위에 구현한것 과 같이 간단하게 완성이 되죠. 다음 편에선 MPPI제어기를 이제 본격적으로 구현해보도록 하겠습니다.

저작자표시 비영리 변경금지 (새창열림)