A Benchmark Comparison of Learned Control Policies for Agile Quadrotor Flight (ICRA 2022) 논문 참고

• Drone Control에서 사용될 수 있는 Quadrotor Dynamics Model
• 충분히 simple하게 잘 simulation할 수 있을 정도 수준

Notation

1. Frame 정의

• World frame $W$ : $\{x_W,y_W,z_W \}$
• body frame $B$ : $\{ x_B, y_B,z_B \}$ → 드론의 무게중심에 위치
• 드론의 state는 world 좌표계를 기준으로 쓰는게 일반적
  • 위치 : $p_{WB}$
  • 자세 : $q_{WB}$ → 쿼터니언으로 표기. 여기서 쿼터니언은 단일 쿼터니언 사용.

2. 벡터 표시

• 벡터 $v$ : world frame에서 $p_1$에서 $p_2$로 향하는 벡터 = $_Wv_{12}$
• 행렬 $M$

3. 좌표 변환

• SE3 transformation : 한 좌표계에서 표현된 강체의 pose를 다른 좌표계에서 표현하는 강체 변환
  → 회전 + 평행이동 동시에 표현
  → 여기서는 reference frame을 body에서 world로 바꾸는 방법으로 봐도 무방

• Body frame의 어떤 점 $p_{B1}$에 대해

• 여기서 $\odot$은 quaternion-vector product → quaternion을 이용해 vector 회전시키는 연산

Quadrotor dynamics

• 쿼드로터를 6-자유도를 가진 rigid body로 정의

  • 3개의 선형 DOF : x,y,z translation
  • 3개의 회전 DOF : roll, pitch, yaw
  • mass = $m$
    • 선가속도 $a=\frac{F}{m}$
    • 중력 $mg=weight$
  • diagonal moment of inertia matrix $J=dia(J_x,J_y,J_z)$
    $J=\begin{bmatrix} J_x \ 0 \ 0 \\ 0 \ J_y \ 0 \\ 0 \ 0 \ J_z \end{bmatrix}$
    • 드론은 대칭 구조이기 때문에 중심을 기준으로 축들 사이에 coupling inertia 없다고 가정.

• 각 프로펠러 $i$의 실제 회전 속도 $\Omega_i$는 first-order system으로 모델링

  • 실제 모터는 전기 신호를 주면 즉시 원하는 속도로 변하지 않는다.

    ⇒ 1차 지연 시스템으로 모델링

    $\dot{\Omega}_i=\frac{1}{k_{mot}}(\Omega_{i,cmd}-\Omega_i)$

  • 여기서 time constant $k_{mot}$이 크면 느리게 반응, 작으면 빠르게 반응

  • 실제 제어 입력은 thrust, torque가 아니고 motor speed command $u=\Omega_{cmd}$
    → 그리고 실제 dynamcis에 영향을 주는 것은 실제 회전 속도 $\Omega_i$

    ✅ 즉, 드론 제어는 실제로 "입력 → 모터 RPM → thrust/torque → translation/rotation"

Dynamics 분석

• 그러므로 state space는 17-차원 (3+4+3+3+4)이며, dynamics는 아래와 같이 기술할 수 있음.

1. 위치 미분
2. 자세 미분
3. 선속도 dynamics

• force = thrust + drag
  → 둘 다 body frame에서 계산 → world frame으로 회전
  → 중력 더해줌
• world frame에서 newton law 적용
• 총 thrust는 각 propellor force의 합산
  $f_{prop}=\sum_if_i$
• drag force는 속도의 반대 방향으로 생성 (linear drag model)
  • 여기서 $k$는 linear drag coefficients
    $f_{drag}= - \begin{bmatrix} k_{vx}v_{B,x} \\ k_{vy}v_{B,y} \\ k_{vz}v_{B,z} \end{bmatrix}$

4. 회전 dynamics

• rigid-body rotational dynamics (euler equation)
• $J^{-1}\tau_{prop}$ : 모터에서 발생하는 torque가 회전 가속도를 만듬
  • 총 torque는 drag torque와 moment arm torque 합
    • $r_{P,i}$는 propeller의 body frame에서의 위치
      $\tau_{prop}=\sum_i(\tau_i+r_{P,i} \times f_i)$
• $-w \times Jw$ : coriolis/gyroscopic effect
  • 회전하는 좌표계 (body frame)에서 각운동량을 미분할 때 나타나는 항
  • 각운동량 $L$은 inertial frame에서 정의되는데, 이를 body frame에서 표현할 경우 시간 미분 시, 회전 좌표계의 미분 규칙에 의해 $w \times (Jw)$항이 추가로 나타남.
    $\tau = \frac{dL}{dt} |_{inertial}= \frac{d}{dt}(Jw) |_{body} +w \times (Jw) =J\dot{w}+w \times (Jw)$

5. motor dynamics
- 실제 모터 속도는 입력 모터 속도의 1차 지연 시스템으로 반응

Propeller thrust and torque

• 프로펠러 thrust와 drag는 아래와 같이 모델링한다.
  → 즉, 프로펠러가 공기를 밀어낼 때, 발생하는 모멘텀이 프로펠러 속도의 제곱에 비례한다~
  

• 여기서 $c_l$은 thrust coefficient, $c_d$는 drag coefficient → 모두 실험으로 결정.

• $k_{mot}$ 값도 실험을 통해 얻어낼 수 있다.

• dynamics 계산은 1ms time step으로 구현된다.

Dynamics 활용 예시

드론 시뮬레이션 설계 예시

인터페이스 → FC → Dynamics → Simulator → …

• 시뮬레이션 상에서 사용자가 드론을 비행하고 싶다고 해보자.
  → 시뮬레이터 인터페이스를 통해 velocity command, position command 등을 준다

• 이 명령은 FC로 들어감

  → FC에서는 보통 state feedback을 이용한 cascade pid 구조 제어기를 통해 motor speed command 계산
  

• Motor speed command는 dynamics model의 입력으로 사용됨
  → first-order motor dynamics를 통해 실제 모터 속도를 계산
  → 프로펠러 force / torque 모델을 통해 thrust 및 torque를 계산
  → 해당 힘과 토크를 이용하여 Rigid-body dynamics 정의
  → 수치 적분 수행하여 다음 시점의 state를 계산
  

• 다음 state가 계산되면 이 state를 기반으로 시뮬레이터는 드론의 위치나 자세, 속도 등을 업데이트 해준다.
• 새로운 pose를 기반으로 시뮬레이션 CG로 이미지를 rendering할 수도 있고, sensor를 simulation할 수도 있다.
• 센서 시뮬레이션 결과는 상태 추정 모듈을 통해 다시 FC로 전달되며, FC는 이를 제어 입력 생성에 활용
  → 반복