[PX4] 멀티콥터 자세제어 알고리즘 - 3

PX4의 위치 제어기에서 추력 벡터를 계산한 후, 별도로 주어지는 방위각(azimuth angle) 명령 \(\psi_{cmd}\) 와 합쳐서 멀티콥터가 취해야 할 자세(attitude)를 결정한다.

 

 

이 자세는 쿼터니언 \(\mathbf{q}_{cmd}\) 로 파라미터화 되는데 이를 좌표계로 표시하면 다음 그림과 같다.

 

 

여기서 \(\vec{F}_{cmd}\) 는 위치 제어기에서 계산한 추력 벡터 명령, \(\{i\}\) 는 관성 좌표계, \(\{b\}\) 는 동체 좌표계, \(\{d\}\) 는 쿼터니언 \(\mathbf{q}_{cmd}\) 로 파라미터화된 좌표계이다.

\(\mathbf{q}_{cmd}\) 는 관성 좌표계에 대한 좌표계 \(\{d\}\) 의 자세를 의미하므로 \(\mathbf{q}_{cmd}=\mathbf{q}_d^i\) 로 쓸 수 있다. 쿼터니언 오차 \(\mathbf{q}_e\) 를 \(\mathbf{q}_{cmd}=\mathbf{q}_b^i \otimes \mathbf{q}_e\) 로 정의했으므로 쿼터니언 오차는 \(\mathbf{q}_e=\mathbf{q}_d^b\) 가 된다.

멀티콥터의 추력 방향은 동체 좌표계의 \(-\hat{b}_3\) 의 방향과 일치해야 하므로, 자세 제어기의 목적은 좌표계 \(\{b\}\) 가 좌표계 \(\{d\}\) 와 일치하도록 멀티콥터의 자세를 바꾸는 것이다.

 

 

별도로 주어지는 방위각 명령 \(\psi_{cmd}\) 를 일단 무시하고, 동체 좌표계의 \(\hat{b}_3\) 축을 \(\hat{d}_3\) 축과 일치시키기 위한 좌표 변환만 고려하면 다음과 같이 회전축 \(\hat{k}\) 를 중심으로 \(\alpha\) 만큼 좌표계 \(\{b\}\) 를 회전시키면 된다.

 

 

이렇게 해서 나온 좌표계를 \(\{p\}\) 라고 하면 좌표계 \(\{b\}\) 에서 좌표계 \(\{p\}\) 로의 쿼터니언 \(\mathbf{q}_p^b\) 는 다음과 같다.

 

\[ \mathbf{q}_p^b= \begin{bmatrix} \cos \left( \frac{\alpha}{2} \right) \\ (\hat{k} \cdot \hat{b}_1 ) \sin \left( \frac{\alpha}{2} \right) \\ ( \hat{k} \cdot \hat{b}_2 ) \sin \left( \frac{\alpha}{2} \right) \\ ( \hat{k} \cdot \hat{b}_3 ) \sin \left( \frac{\alpha}{2} \right) \end{bmatrix} \tag{1} \]

 

여기서

 

\[ \hat{k}= \frac{ \hat{b}_3 \times \hat{d}_3}{ \lVert \hat{b}_3 \times \hat{d}_3 \rVert_2}, \ \ \ \ \ \alpha = \cos^{-1} (\hat{b}_3 \cdot \hat{d}_3 ) \tag{2} \]

 

이다. 식 (2)에 의하면 \(\hat{k}\) 와 \(\hat{b}_3\) 는 항상 직각이므로 식 (1)에서 \(\hat{k} \cdot \hat{b}_3=0\) 이 된다. 따라서 식 (1)을 다시 쓰면 다음과 같다.

 

\[ \mathbf{q}_p^b= \begin{bmatrix} \cos \left( \frac{\alpha}{2} \right) \\ (\hat{k} \cdot \hat{b}_1 ) \sin \left( \frac{\alpha}{2} \right) \\ ( \hat{k} \cdot \hat{b}_2 ) \sin \left( \frac{\alpha}{2} \right) \\ 0 \end{bmatrix} \tag{3} \]

 

관성 좌표계 \(\{i\}\) 에서 좌표계 \(\{p\}\) 까지의 쿼터니언 \(\mathbf{q}_p^i\) 는 다음과 같다.

 

\[ \mathbf{q}_{cmd,red}=\mathbf{q}_p^i=\mathbf{q}_b^i \otimes \mathbf{q}_p^b \tag{4} \]

 

쿼터니언 \(\mathbf{q}_{cmd,red}\) 를 축소된 목표 자세(reduced desired attitude) 또는 축소된 자세 명령이라고 하고 \(\mathbf{q}_{e,red}=\mathbf{q}_p^b\) 를 축소된 쿼터니언 오차라고 한다.

 

 

원래 각속도 제어인 식 (5)에서

 

\[ \omega_{ib}^b= k_{cmd} \ sgn(q_{e0} ) \mathbf{q}_{e1:3} \tag{5} \]

 

자세 명령 \(\mathbf{q}_{cmd}\) 대신에 \(\mathbf{q}_{cmd,red}\) 로, 쿼터니언 오차를 \(\mathbf{q}_e\) 대신에 \(\mathbf{q}_{e,red}\) 로 하면,

 

\[ \left( \omega_{ib}^b \right)_{red}= k_{cmd} \ sgn(q_{e,red0} ) \mathbf{q}_{e,red1:3} \tag{6} \]

 

각속도 제어 (6)은 멀티콥터의 자세 \(\mathbf{q}_b^i\) 를 \(\mathbf{q}_p^i\) 로 변환시킬 것이다. 식 (3)과 (6)에 의하면 \( \left( \omega_{ib}^b \right)_{red}\) 의 세번째 성분은 \(0\) 이 된다.

 

\[ \left( \omega_{ib}^b \right)_{red}= \begin{bmatrix} \omega_{red,1} \\ \omega_{red,2} \\ 0 \end{bmatrix} \tag{7} \]

 

이는 곧 \(\hat{b}_3\) 축 또는 멀티콥터의 추력 방향을 중심으로 하는 회전 각속도 명령이 \(0\) 이 된다는 의미이다.

멀티콥터에서는 \(\hat{b}_3\) 축을 중심으로 하는 회전 운동은 \(\hat{b}_1\) 축과 \(\hat{b}_2\) 축을 중심으로 하는 회전 운동 보다 훨씬 느리다. 따라서 본래 각속도 제어 식 (5)에서 동일한 게인으로 제어할 경우 \(\hat{b}_1\) 축과 \(\hat{b}_2\) 축을 중심으로 하는 회전 운동이 매우 느리거나, 혹은 \(\hat{b}_3\) 축 회전 운동에 오버슈트가 발생할 수 있어서 좋은 결과를 기대할 수 없다. 이런 점에서 식 (6)으로 주어지는 축소된 각속도 명령은 매력적이다.

사실 원하는 궤적(desired trajectory)을 따르기 위해서는 방위각을 제어할 필요가 없고 추력 명령만 따르면 되기 때문에 식 (6)의 각속도 제어만으로도 원하는 궤적을 추종할 수 있다. 하지만 방위각 명령 \(\psi_{cmd}\) 를 멀티콥터에 요구할 때는 그 이유가 있을 것이므로 이를 추종하는 것이 바람직하긴 하다. 이를 감안하여 기술보고서 'Nonlinear Quadcopter Attitude Control Technical Report, 2013' 에서는 식 (5)와 (6)을 혼합한 각속도 명령을 제안했다.

본래 원하는 자세인 쿼터니언 \(\mathbf{q}_{cmd}=\mathbf{q}_d^i\) 로 파라미터화된 좌표계 \(\{d\}\) 와 축소된 자세인 \(\mathbf{q}_{cmd,red}=\mathbf{q}_p^i\) 로 파라미터화된 좌표계 \(\{p\}\) 의 관계식은 다음과 같다.

 

\[ \mathbf{q}_d^i=\mathbf{q}_p^i \otimes \mathbf{q}_d^p \tag{8} \]

 

또는

 

\[ \mathbf{q}_{cmd}=\mathbf{q}_{cmd,red} \otimes \mathbf{q}_{mix} \tag{9} \]

 

기술보고서에서는 \(\mathbf{q}_{mix}=\mathbf{q}_d^p\) 로 표현했다. \(\hat{p}_3=\hat{d}_3\) 이기 때문에 \(\mathbf{q}_{mix}\) 는 다음과 같이 표현할 수 있다.

 

\[ \mathbf{q}_{mix} = \begin{bmatrix} \cos \left( \frac{\alpha_{mix}}{2} \right) \\ 0 \\ 0 \\ \sin \left( \frac{\alpha_{mix}}{2} \right) \end{bmatrix} \tag{10} \]

 

여기서

 

\[ \alpha_{mix}= \cos^{-1} (\hat{p}_3 \cdot \hat{d}_3 ) \]

 

이다.

 

 

기술보고서에서는 식 (9)와 (10)을 이용하여 다음과 같은 혼합형 자세 명령을 제안했다.

 

\[ \mathbf{q}_{cmd}= \mathbf{q}_{cmd,red} \otimes \begin{bmatrix} \cos \left( \frac{w_{yaw} \alpha_{mix}}{2} \right) \\ 0 \\ 0 \\ \sin \left( \frac{w_{yaw} \alpha_{mix}}{2} \right) \end{bmatrix}, \ \ \ w_{yaw} \in [0, 1] \tag{11} \]

 

\(w_{yaw}=0\) 이면 자세 명령은 축소 자세 명령, \(w_{yaw}=1\) 이면 본래 자세 명령에 해당하므로 혼합형 자세 명령은 원래 자세 명령과 축소 자세 명령 사이에 있는 어떤 값이다. 식 (11)의 자세 명령은 멀티콥터 운동에 크게 중요하지 않는 방위각을 제어하는데 소요되는 제어력 낭비를 줄일 수 있다.