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

회전축 \(\hat{p}\) 를 중심으로 회전각 \(\beta\) 만큼 회전하거나, 회전축 \(-\hat{p}\) 를 중심으로 회전각 \(2\pi-\beta\) 만큼 회전하거나 물리적으로 같은 회전이다.

 

 

따라서 쿼터니언(quaternion)의 정의에 의하면 다음식이 성립한다.

 

\[ \begin{align} \mathbf{q} &= \begin{bmatrix} \cos⁡ \left( \frac{\beta}{2} \right) \\ \mathbf{p} \sin \left( \frac{\beta}{2} \right) \end{bmatrix} = \begin{bmatrix} \cos⁡ \left( \frac{2\pi-\beta}{2} \right) \\ -\mathbf{p} \sin \left( \frac{2\pi-\beta}{2} \right) \end{bmatrix} \tag{1} \\ \\ & = \begin{bmatrix} -\cos⁡ \left( \frac{\beta}{2} \right) \\ -\mathbf{p} \sin \left( \frac{\beta}{2} \right) \end{bmatrix} \\ \\ &= -\mathbf{q} \end{align} \]

 

이와 같이 동일한 물리적인 자세에 대해서 두 가지의 쿼터니언 표현식이 존재하는 문제가 있는데, 이것을 대척점 모호성(antipodal ambiguity)이라고 한다.

 

 

앞서 유도한(https://pasus.tistory.com/245) 멀티콥터 각속도 명령

 

\[ \omega_{ib}^b=k_{cmd} \mathbf{q}_{e1:3} \tag{2} \]

 

은 멀티콥터의 자세 \(\mathbf{q}_b^i\) 를 명령값 \(\mathbf{q}_{cmd}\) 로 수렴시킨다. 여기서 \(\mathbf{q}_e\) 는 쿼터니언 오차로서

 

\[ \mathbf{q}_e= \begin{bmatrix} q_{e0} \\ q_{e1} \\ q_{e2} \\ q_{e3} \end{bmatrix} = \begin{bmatrix} q_{e0} \\ \mathbf{q}_{e1:3} \end{bmatrix} \tag{3} \]

 

이다. 이 때 쿼터니언 오차는 \(\mathbf{q}_I= \begin{bmatrix} 1 & 0 & 0 & 0 \end{bmatrix}^T\) 로 수렴한다. 쿼터니언의 대척점 모호성에 의하면 멀티콥터의 자세 \(-\mathbf{q}_b^i\) 를 \(\mathbf{q}_{cmd}\) 으로 수렴시켜도 물리적으로는 동일한 자세로부터 출발하는 것이므로 알고리즘에서는 이를 반영해야 한다. 이 때는 쿼터니언 오차가 \(-\mathbf{q}_I= \tilde{\mathbf{q}}_I= \begin{bmatrix} -1 & 0 & 0 & 0 \end{bmatrix}^T\) 로 수렴하는 것과 동일한 경우다.

이를 반영하여 앞서 설명한 멀터콥터 각속도 명령 유도식에서 사용한 리야프노프 함수 후보(Lyapunov function candidate)대신에 다음과 같은 함수 \(V\) 를 정의하고,

 

\[ \begin{align} V &= ( \mathbf{q}_e-\tilde{\mathbf{q}}_I )^T (\mathbf{q}_e- \tilde{\mathbf{q}}_I ) \tag{4} \\ \\ &= \mathbf{q}_{e1:3}^T \mathbf{q}_{e1:3}+ (q_{e0}+1)^2 \end{align} \]

 

동일한 방법으로 수식을 전개하면 각속도 명령은 다음과 같이 된다.

 

\[ \omega_{ib}^b= - k_{cmd} \mathbf{q}_{e1:3} \tag{5} \]

 

식 (5)는 쿼터니언 오차를 \(-\mathbf{q}_I\) 로 수렴시킨다.

결국 \(\omega_{ib}^b=k_{cmd} \mathbf{q}_{e1:3}\) 와 \(\omega_{ib}^b=-k_{cmd} \mathbf{q}_{e1:3}\) 은 멀티콥터의 자세를 모두 물리적으로 동일한 자세인 \(\mathbf{q}_{cmd}\) 로 수렴시킴을 알 수 있다. 둘 사이의 차이는 전자는 쿼터니언 오차가 \(\mathbf{q}_I\) 로, 후자는 \(-\mathbf{q}_I\) 로 수렴한다는 것 밖에 없다.

기술보고서 'Nonlinear Quadcopter Attitude Control Technical Report, 2013' 에서는 이 두 개의 각속도 명령 (2)와 (5)를 하나의 수식으로 묶는 것을 제안하고 있다.

 

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

 

여기서

 

\[ sgn(q_{e0} )= \begin{cases} 1, & q_{e0} \ge 0 \\ -1, & q_{e0} \lt 0 \end{cases} \]

 

이다.

아래 그림은 식 (2), (5), (6)의 차이점을 보여준다. 식 (2)를 사용할 경우에는 쿼터니언 오차 \(\mathbf{q}_e\) 가 \(q_{e0}=1\) 로 수렴하고, 식 (5)를 사용할 경우는 \(q_{e0}=-1 \) 로 수렴하며, 식 (6)을 사용할 경우는 \(q_{e0}=0\) 을 기준으로 하여 각각 가까운 \(q_{e0}=1\) 또는 \(q_{e0}=-1\) 로 수렴한다.

 

 

다음 그림은 자세제어 알고리즘을 구조를 보여준다.

 

 

식 (2), (5), (6)에 의하면 각속도 명령의 크기는 다음과 같이 계산할 수 있다.

 

\[ \lVert \omega_{ib}^b \rVert_2 \le k_{cmd} \lVert \mathbf{q}_{e1:3} \rVert_2 \le k_{cmd} \tag{7} \]

 

각속도 명령의 크기의 상한선을 미리 알 수 있다는 것은 제어 신호의 포화(saturation)를 고려할 때 큰 장점이라고 볼 수 있다. 이제 멀티콥터의 요(yaw)운동이 피치(pitch)와 롤(roll) 운동보다 훨씬 느리다는 점을 자세제어 알고리즘에 고려하는 일만 남았다.