쿼터니언 기반 자세제어

질량중심을 기준으로 강체의 회전 운동방정식은 다음과 같다 (https://pasus.tistory.com/191).

 

\[ \vec{M}_G= \bar{I}_G \cdot \frac{ ^b d ^i \vec{\omega}^b}{dt} + {^i \vec{\omega}^b} \times (\bar{I}_G \cdot { ^i \vec{\omega}^b }) \tag{1} \]

 

여기서 \(\{i\}\) 는 관성좌표계, \(\{b\}\) 는 강체좌표계, \(^i \vec{\omega}^b\) 는 강체좌표계의 각속도 벡터, \(G\) 는 강체의 질량중심, \(\bar{I}_G\) 는 질량중심점에 대한 관성 다이아딕, \(\vec{M}_G\) 는 강체에 작용하는 질량 중심점에 대한 모멘트이다.

 

 

식 (1)을 강체좌표계 \(\{b\}\) 로 표현하면 다음과 같다.

 

\[ [I_G ] \dot{\omega}_{ib}^b=-[\omega_{ib}^b \times ][I_G ] \mathbf{\omega}_{ib}^b+\mathbf{M}_G^b \tag{2} \]

 

여기서 \([I_G ]\) 는 관성행렬이고, \(\omega_{ib}^b= [\omega_1 \ \omega_2 \ \omega_3 ]^T\) 는 각속도 벡터 \(^i \vec{\omega}^b\) 를 좌표계 \(\{b\}\) 로 표현한 것으로서 그 빗대칭(skew-symmetric) 행렬은 다음과 같다.

 

\[ [\omega_{ib}^b \times]= \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \tag{3} \]

 

쿼터니언(quaternion) 기반 자세제어(attitude control)의 목적은 쿼터니언으로 표시된 강체의 현재 자세 \(\mathbf{q}_b^i\) 를 원하는 자세 \(\mathbf{q}_{cmd}\) 가 되도록 모멘트 \(\mathbf{M}_G^b\) 를 제어하는 것이다.

 

 

원하는 자세와 현재 자세의 차이, 즉 자세 오차를 다음과 같이 곱셈형 쿼터니언 오차 \(\mathbf{q}_e\) 로 정의한다.

 

\[ \mathbf{q}_{cmd}= \mathbf{q}_b^i \otimes \mathbf{q}_e \tag{4} \]

 

그러면 자세 오차가 \(0\) 일 때 쿼터니언 오차 \(\mathbf{q}_e\) 는 \(\mathbf{q}_I\) 가 된다. 여기서 \(\mathbf{q}_I\) 는 회전각이 \(0\) 일 때의 쿼터니언으로서 다음과 같다.

 

\[ \mathbf{q}_I= \begin{bmatrix} 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \tag{5} \]

 

따라서 제어 목적은 \(\mathbf{q}_e\) 를 \(\mathbf{q}_I\) 가 되게 하는 것이다. 이제 제어 목적을 바탕으로 다음과 같이 쿼터니언 기반 자세제어 법칙을 정의한다.

 

\[ \mathbf{M}_G^b=K \mathbf{q}_{e1:3}-D \omega_{ib}^b \tag{6} \]

 

여기서 \(K \gt 0\), \(D \gt 0\) 는 게인 행렬이고, \(\mathbf{q}_{e1:3}\) 는 쿼터니언 오차 \(\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{7} \]

 

쿼터니언 오차 \(\mathbf{q}_e\) 의 미분은 식 (4)에서 \(\mathbf{q}_{cmd}\) 를 상수(constant)라고 가정하고 구한다.

 

\[ 0= \dot{\mathbf{q}}_b^i \otimes \mathbf{q}_e+ \mathbf{q}_b^i \otimes \dot{\mathbf{q}}_e \tag{8} \]

 

위 식에 의하면 \(\dot{\mathbf{q}}_e\) 는 다음과 같이 계산된다.

 

\[ \begin{align} \dot{\mathbf{q}}_e & = -\left( \mathbf{q}_b^i \right)^{-1} \otimes \dot{\mathbf{q}}_b^i \otimes \mathbf{q}_e \tag{9} \\ \\ &= -\frac{1}{2} \left( \mathbf{q}_b^i \right)^{-1} \otimes \mathbf{q}_b^i \otimes \bar{\omega}_{ib}^b \otimes \mathbf{q}_e \\ \\ &=- \frac{1}{2} \bar{\omega}_{ib}^b \otimes \mathbf{q}_e \end{align} \]

 

여기서

 

\[ \bar{\omega}_{ib}^b= \begin{bmatrix} 0 \\ \omega_{ib}^b \end{bmatrix} \tag{10} \]

 

이며 식 (9)를 계산할 때 다음과 같이 쿼터니언의 미분을 이용하였다.

 

\[ \dot{\mathbf{q}}_b^i= \frac{1}{2} \mathbf{q}_b^i \otimes \bar{\omega}_{ib}^b \tag{11} \]

 

쿼터니언 곱 관계식을 이용하면 식 (9)는 다음과 같이 쓸 수 있다.

 

\[ \begin{align} \dot{\mathbf{q}}_e &= -\frac{1}{2} [\bar{\omega}_{ib}^b ] \mathbf{q}_e \tag{12} \\ \\ &= - \frac{1}{2} \begin{bmatrix} 0 & -(\omega_{ib}^b )^T \\ \omega_{ib}^b & [\omega_{ib}^b \times] \end{bmatrix} \begin{bmatrix} q_{e0} \\ \mathbf{q}_{e1:3} \end{bmatrix} \\ \\ &= -\frac{1}{2} \begin{bmatrix} -\mathbf{q}_{e1:3}^T \omega_{ib}^b \\ q_{e0} \omega_{ib}^b+[\omega_{ib}^b \times] \mathbf{q}_{e1:3} \end{bmatrix} \end{align} \]

 

또는

 

\[ \begin{align} & \dot{q}_{e0}= \frac{1}{2} (\omega_{ib}^b )^T \mathbf{q}_{e1:3} \tag{13} \\ \\ & \dot{\mathbf{q}}_{e1:3}= -\frac{1}{2} [\omega_{ib}^b \times ] \mathbf{q}_{e1:3}- \frac{1}{2} q_{e0} \omega_{ib}^b \end{align} \]

 

이다.

 

 

이제, 식 (6)을 (2)에 대입하면 다음과 같다.

 

\[ [I_G ] \dot{\omega}_{ib}^b= -[ \omega_{ib}^b \times ][I_G ] \omega_{ib}^b+K \mathbf{q}_{e1:3}-D \omega_{ib}^b \tag{14} \]

 

식 (14)로 주어지는 피드백 시스템의 안정성을 해석하기 위해서 다음과 같이 리야프노프 함수 후보(Lyapunov function candidate) \(V\) 를 정의한다.

 

\[ \begin{align} V &= \frac{1}{2} (\omega_{ib}^b )^T K^{-1} [I_G ] \omega_{ib}^b+(\mathbf{q}_e-\mathbf{q}_I )^T (\mathbf{q}_e-\mathbf{q}_I ) \tag{15} \\ \\ &= \frac{1}{2} (\omega_{ib}^b )^T K^{-1} [I_G ] \omega_{ib}^b+ \mathbf{q}_{e1:3}^T \mathbf{q}_{e1:3}+(q_{e0}-1)^2 \end{align} \]

 

위 식을 시간 미분하면 다음과 같다.

 

\[ \dot{V}= (\omega_{ib}^b )^T K^{-1} [I_G ] \dot{\omega}_{ib}^b+2(\mathbf{q}_e-\mathbf{q}_I )^T \dot{\mathbf{q}}_e \tag{16} \]

 

식 (12)와 (14)를 식 (16)에 대입하면 \(\dot{V}\) 은 다음과 같이 된다.

 

\[ \begin{align} \dot{V} &= (\omega_{ib}^b )^T K^{-1} (-[\omega_{ib}^b \times ][I_G ] \omega_{ib}^b+K \mathbf{q}_{e1:3}-D \omega_{ib}^b ) \tag{17} \\ \\ & \ \ \ \ \ -[(q_{e0}-1) \ \ \mathbf{q}_{e1:3}^T ] \begin{bmatrix} -\mathbf{q}_{e1:3}^T \omega_{ib}^b \\ q_{e0} \omega_{ib}^b+[\omega_{ib}^b \times ] \mathbf{q}_{e1:3} \end{bmatrix} \\ \\ &= - (\omega_{ib}^b )^T K^{-1} [\omega_{ib}^b \times ][I_G ] \omega_{ib}^b+( \omega_{ib}^b )^T \mathbf{q}_{e1:3} \\ \\ & \ \ \ \ \ -(\omega_{ib}^b )^T K^{-1} D \omega_{ib}^b - \mathbf{q}_{e1:3}^T \omega_{ib}^b \\ \\ &= -(\omega_{ib}^b )^T K^{-1} [\omega_{ib}^b \times ][I_G ] \omega_{ib}^b-(\omega_{ib}^b )^T K^{-1} D \omega_{ib}^b \end{align} \]

 

여기서 \(K\) 를 다음과 같은 값을 갖도록 선정한다면

 

\[ K^{-1}=c_1 [I_G ]+c_2 I, \ \ \ c_1 \gt 0, \ c_2 \gt 0 \tag{18} \]

 

식 (17)은 다음과 같이 된다.

 

\[ \begin{align} \dot{V} &= -(\omega_{ib}^b )^T (c_1 [I_G ]+c_2 I) [\omega_{ib}^b \times ][I_G ] \omega_{ib}^b \tag{19} \\ \\ & \ \ \ \ \ -(\omega_{ib}^b )^T K^{-1} D \omega_{ib}^b \\ \\ &= -c_1 ([I_G] \omega_{ib}^b )^T [\omega_{ib}^b \times ][I_G ] \omega_{ib}^b -c_2 (\omega_{ib}^b)^T [\omega_{ib}^b \times ][I_G ] \omega_{ib}^b \\ \\ & \ \ \ \ \ -(\omega_{ib}^b )^T K^{-1} D \omega_{ib}^b \\ \\ &= -(\omega_{ib}^b )^T K^{-1} D \omega_{ib}^b \end{align} \]

 

위 식에서 \([\omega_{ib}^b \times]\) 가 빗대칭 행렬이라는 점과 \((\omega_{ib}^b )^T [\omega_{ib}^b \times]=0\) 임을 이용하였다. 식 (19)에 의하면 \(K^{-1} D \gt 0\) 이므로 \(\dot{V} \lt 0\) 이 되어서 시스템 (14)는 전역에서 점근적으로 안정(globally asymptotically stable)하다.

또한 식 (19)에 의하면 \(c_1=0\) 인 경우에도, 또는 \(c_2=0\) 인 경우에도 \(\dot{V} \lt 0\) 이므로 시스템 (14)는 전역에서 점근적으로 안정하다.

\(c_1=0\) 인 경우는 \(K^{-1}=c_2 I\) 또는 \(K=\frac{1}{c_2} I\) 이므로 자세제어 법칙 (6)은 시스템의 관성 행렬 \([I_G ]\) 의 정보를 필요로 하지 않는다.

멀티콥터 자세제어 편(https://pasus.tistory.com/246)에서 언급했듯이 최단 각경로(angular path)를 보장하기 위하여 쿼터니언의 대척점 모호성(antipodal ambiguity)을 반영하면 식 (6)의 자세제어 법칙을 다음과 같이 수정할 수 있다.

 

\[ \mathbf{M}_G^b=sgn(q_{e0} ) K \mathbf{q}_{e1:3}-D \omega_{ib}^b \tag{20} \]

 

여기서

 

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

 

이다. 한편 논문 "Quaternion Feedback Regulator for Spacecraft Eigenaxis Rotations by B. Wie, H. Weiss and A. Arapostathis" 에 의하면 자세제어 법칙 (6)을 다음과 같이 수정한다면 고유축(eigenaxis) 회전이라는 최단 각경로에 의한 자세제어가 가능하다고 한다.

 

\[ \mathbf{M}_G^b= [\omega_{ib}^b \times ][I_G ] \omega_{ib}^b +K \mathbf{q}_{e1:3} -D \omega_{ib}^b \tag{21} \]

 

여기서

 

\[ D=d[I_G ],\ \ \ K=k[I_G ],\ \ \ d \gt 0,\ k \gt 0 \]

 

이다. 그런데 증명이 명확하지 않은 것 같아서 구체적인 내용은 생략한다.