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

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

 

 

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

 

$$\begin{array}{r}\text{(1)}& \mathbf{q}& =\left[\begin{array}{c}\cos \left(\frac{\beta }{2}\right)\\ \mathbf{p}\sin \left(\frac{\beta }{2}\right)\end{array}\right]=\left[\begin{array}{c}\cos \left(\frac{2\pi -\beta }{2}\right)\\ -\mathbf{p}\sin \left(\frac{2\pi -\beta }{2}\right)\end{array}\right]\\ & =\left[\begin{array}{c}-\cos \left(\frac{\beta }{2}\right)\\ -\mathbf{p}\sin \left(\frac{\beta }{2}\right)\end{array}\right]\\ \\ & =-\mathbf{q}\end{array}$$

 

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

 

 

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

 

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

 

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

 

$$\begin{array}{}\text{(3)}& {\mathbf{q}}_e=\left[\begin{array}{c}{q}_{e0}\\ q_{e1}\\ q_{e2}\\ q_{e3}\end{array}\right]=\left[\begin{array}{c}{q}_{e0}\\ {\mathbf{q}}_{e1:3}\end{array}\right]\end{array}$$

 

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

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

 

$$\begin{array}{r}\text{(4)}& V& =({\mathbf{q}}_e-{\tilde{\mathbf{q}}}_I{)}^T({\mathbf{q}}_e-{\tilde{\mathbf{q}}}_I)\\ & ={\mathbf{q}}_{e1:3}^T{\mathbf{q}}_{e1:3}+(q_{e0}+1{)}^2\end{array}$$

 

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

 

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

 

식 (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)를 하나의 수식으로 묶는 것을 제안하고 있다.

 

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

 

여기서

 

$$sgn(q_{e0})=\{\begin{array}{ll}1,& q_{e0}\ge 0\\ -1,& q_{e0}<0\end{array}$$

 

이다.

아래 그림은 식 (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)에 의하면 각속도 명령의 크기는 다음과 같이 계산할 수 있다.

 

$$\begin{array}{}\text{(7)}& \Vert {\omega }_{ib}^b{\Vert }_2\le k_{cmd}\Vert {\mathbf{q}}_{e1:3}{\Vert }_2\le k_{cmd}\end{array}$$

 

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