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

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

 

 

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

 

 

여기서 ${\overrightarrow{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$ 는 다음과 같다.

 

$$\begin{array}{}\text{(1)}& {\mathbf{q}}_p^b=\left[\begin{array}{c}\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{array}\right]\end{array}$$

 

여기서

 

$$\begin{array}{}\text{(2)}& \hat{k}=\frac{{\hat{b}}_3\times {\hat{d}}_3}{\Vert {\hat{b}}_3\times {\hat{d}}_3{\Vert }_2},\alpha ={\cos }^{-1}({\hat{b}}_3\cdot {\hat{d}}_3)\end{array}$$

 

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

 

$$\begin{array}{}\text{(3)}& {\mathbf{q}}_p^b=\left[\begin{array}{c}\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{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(4)}& {\mathbf{q}}_{cmd,red}={\mathbf{q}}_p^i={\mathbf{q}}_b^i\otimes {\mathbf{q}}_p^b\end{array}$$

 

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

 

 

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

 

$$\begin{array}{}\text{(5)}& {\omega }_{ib}^b=k_{cmd}sgn(q_{e0}){\mathbf{q}}_{e1:3}\end{array}$$

 

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

 

$$\begin{array}{}\text{(6)}& {\left({\omega }_{ib}^b\right)}_{red}=k_{cmd}sgn(q_{e,red0}){\mathbf{q}}_{e,red1:3}\end{array}$$

 

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

 

$$\begin{array}{}\text{(7)}& {\left({\omega }_{ib}^b\right)}_{red}=\left[\begin{array}{c}{\omega }_{red,1}\\ {\omega }_{red,2}\\ 0\end{array}\right]\end{array}$$

 

이는 곧 ${\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\}$ 의 관계식은 다음과 같다.

 

$$\begin{array}{}\text{(8)}& {\mathbf{q}}_d^i={\mathbf{q}}_p^i\otimes {\mathbf{q}}_d^p\end{array}$$

 

또는

 

$$\begin{array}{}\text{(9)}& {\mathbf{q}}_{cmd}={\mathbf{q}}_{cmd,red}\otimes {\mathbf{q}}_{mix}\end{array}$$

 

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

 

$$\begin{array}{}\text{(10)}& {\mathbf{q}}_{mix}=\left[\begin{array}{c}\cos \left(\frac{{\alpha }_{mix}}{2}\right)\\ 0\\ 0\\ \sin \left(\frac{{\alpha }_{mix}}{2}\right)\end{array}\right]\end{array}$$

 

여기서

 

$${\alpha }_{mix}={\cos }^{-1}({\hat{p}}_3\cdot {\hat{d}}_3)$$

 

이다.

 

 

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

 

$$\begin{array}{}\text{(11)}& {\mathbf{q}}_{cmd}={\mathbf{q}}_{cmd,red}\otimes \left[\begin{array}{c}\cos \left(\frac{w_{yaw}{\alpha }_{mix}}{2}\right)\\ 0\\ 0\\ \sin \left(\frac{w_{yaw}{\alpha }_{mix}}{2}\right)\end{array}\right],w_{yaw}\in [0,1]\end{array}$$

 

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