MPC 란? (Model Predictive Control) 7. Multi-rotor system Model

참고: Mina Kamel Paper

MPC는 Optimal Control의 한 방법인데 로보틱스의 planning및 제어에 많이 활용되고 있다. MPC를 사용하면 로봇의 속도 및 가속력과 같은 dynamics와 주변 환경 조건을 cost function으로 넣어 상황에 맞는 최적화된 제어 명령을 생성할 수 있고 이를 통해 안정적으로 로봇의 자율항법이 가능하다. 아주 오래전부터 해보고 싶었던 분야 인데, 이 글을 작성하면서 공부하고 실제 드론에 탑재해서 실험까지 수행해보고자 한다.

이전글까지 MPC의 기초에 대해 알아 보았다. 이번글 부터는 MPC를 쿼드로터에 활용하기 위한 본격적인 작업에 들어가도록 한다. 제일 첫번째로 Quadrotor System model에 대해 알아보고 이를 Linear MPC에 적용해보도록 한다.

1. Frame Definition

쿼드로터 시스템의 6자유도 운동(3 translation + 3 rotation)을 정의하기 위해서는 먼저 기준점이 되는 좌표계를 설정해야 한다. 일반적으로 Inertial axis(관성 좌표계)를 fixed frame으로 해서 '$w$'라고 표시하고 이러한 Inertial axis위에서 비행하는 쿼드로터의 좌표계를 body frame이라고 해서 '$b$' 로 표기한다. 이룰 그림으로 나타내면 다음과 같다.

Body fixed frame $\textbf{B}$와 inertial frame $\textbf{W}$

frame $\textbf{W}$안에 있는 frame $\textbf{B}$의 원점의 위치를 $\textbf{p}$로 표기하고, 마찬가지로 frame $\textbf{W}$안에 있는 frame $\textbf{B}$의 회전 정보를(rotation matrix) $\textbf{R}$이라고 표기한다.

2. Cascade Control structure

여기에서는 MPC를 구성할 때, 원하는 각도를 올바르게 추종할 수 있는 low-level 자세제어 controller가 inner-loop로 존재한다고  가정한다. 즉 Pixhawk와 같은 자세제어기가 존재하고, 그위에 High-level mission computer에서 MPC를 활용한 model-based trajectory tracking controller가 outer-loop로 동작하는 계층적 제어구조 (cascade control structure)를 갖는다.

이러한 접근법은 대부분의 비행제어 시스템이 navigation시스템과 별도로 구성되어 navigation 시스템 (High-level computer)가 어떠한 이유에서 동작불능이 되어도 critical한 부분인 비행제어기는 정상 동작하게 한 것에 착안하여 도입되었다.

3. Non-linear model of quadrotor system

$\dot{\textbf{p}}(t) = \textbf{v}(t)$

$\dot{\textbf{v}}(t) = \textbf{R}(\psi, \theta, \phi)\begin{pmatrix}0\\0\\T\end{pmatrix} + \begin{pmatrix}0\\0\\-g\end{pmatrix} - \begin{pmatrix}A_x & 0 & 0\\0 & A_y & 0\\0 & 0 & A_z\end{pmatrix}\textbf{v}(t) + \textbf{d}(t)$

$\dot{\phi}(t) = \frac{1}{\tau_\phi}(K_\phi\phi_d(t)-\phi(t))$

$\dot{\theta}(t) = \frac{1}{\tau_\theta}(K_\theta\theta_d(t)-\theta(t))$

여기서 $\textbf{v}(t)$는 기체속도를 나타내고 inertial frame에서 표현된다. $\dot{\textbf{v}}$는 가속도를 나타내고 $a = \frac{F}{m}$이므로 기체질량에 대해 normalize해주어야 한다. 따라서 $T$는 기체질량에 대해 normalize된 기체의 thrust vector인데 이 값은 body frame에서 표현되므로 roatation matrix $\textbf{R}_\textbf{B}^\textbf{W}$ (body to world rotation)을 통해 inertial frame으로 변환된다. 더불어 $A_x, A_y, A_z$도 기체 질량에 대해 normalize된 drag coefficient이며 $\textbf{d}$는 외란을 의미한다.

마지막으로 $\tau_\phi, K_\phi, \tau_\theta, K_\theta$는 attitude loop parameter로써 inner-loop 자세제어기의 time constant와 gain이다. 이 값들은 system identification을 통해 식별할 수 있는데 자세한 방법은 추후 작성할 system identification에 관한글에서 다루기로 한다.