쿼터니언 기반 자세제어

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

 

$$\begin{array}{}\text{(1)}& {\overrightarrow{M}}_G={\overline{I}}_G\cdot \frac{{}^b{d}^i{\overrightarrow{\omega }}^b}{dt}+{}^i{\overrightarrow{\omega }}^b\times ({\overline{I}}_G\cdot {}^i{\overrightarrow{\omega }}^b)\end{array}$$

 

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

 

 

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

 

$$\begin{array}{}\text{(2)}& [I_G]{\dot{\omega }}_{ib}^b=-[{\omega }_{ib}^b\times ][I_G]{\omega }_{ib}^b+{\mathbf{M}}_G^b\end{array}$$

 

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

 

$$\begin{array}{}\text{(3)}& [{\omega }_{ib}^b\times ]=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]\end{array}$$

 

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

 

 

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

 

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

 

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

 

$$\begin{array}{}\text{(5)}& {\mathbf{q}}_I=\left[\begin{array}{c}1\\ 0\\ 0\\ 0\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(6)}& {\mathbf{M}}_G^b=K{\mathbf{q}}_{e1:3}-D{\omega }_{ib}^b\end{array}$$

 

여기서 $K>0$, $D>0$ 는 게인 행렬이고, ${\mathbf{q}}_{e1:3}$ 는 쿼터니언 오차 ${\mathbf{q}}_e$ 의 벡터부를 의미한다.

 

$$\begin{array}{}\text{(7)}& {\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}}_e$ 의 미분은 식 (4)에서 ${\mathbf{q}}_{cmd}$ 를 상수(constant)라고 가정하고 구한다.

 

$$\begin{array}{}\text{(8)}& 0={\dot{\mathbf{q}}}_b^i\otimes {\mathbf{q}}_e+{\mathbf{q}}_b^i\otimes {\dot{\mathbf{q}}}_e\end{array}$$

 

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

 

$$\begin{array}{r}\text{(9)}& {\dot{\mathbf{q}}}_e& =-{\left({\mathbf{q}}_b^i\right)}^{-1}\otimes {\dot{\mathbf{q}}}_b^i\otimes {\mathbf{q}}_e\\ & =-\frac{1}{2}{\left({\mathbf{q}}_b^i\right)}^{-1}\otimes {\mathbf{q}}_b^i\otimes {\overline{\omega }}_{ib}^b\otimes {\mathbf{q}}_e\\ \\ & =-\frac{1}{2}{\overline{\omega }}_{ib}^b\otimes {\mathbf{q}}_e\end{array}$$

 

여기서

 

$$\begin{array}{}\text{(10)}& {\overline{\omega }}_{ib}^b=\left[\begin{array}{c}0\\ {\omega }_{ib}^b\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(11)}& {\dot{\mathbf{q}}}_b^i=\frac{1}{2}{\mathbf{q}}_b^i\otimes {\overline{\omega }}_{ib}^b\end{array}$$

 

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

 

$$\begin{array}{r}\text{(12)}& {\dot{\mathbf{q}}}_e& =-\frac{1}{2}[{\overline{\omega }}_{ib}^b]{\mathbf{q}}_e\\ & =-\frac{1}{2}\left[\begin{array}{cc}0& -({\omega }_{ib}^b{)}^T\\ {\omega }_{ib}^b& [{\omega }_{ib}^b\times ]\end{array}\right]\left[\begin{array}{c}{q}_{e0}\\ {\mathbf{q}}_{e1:3}\end{array}\right]\\ \\ & =-\frac{1}{2}\left[\begin{array}{c}-{\mathbf{q}}_{e1:3}^T{\omega }_{ib}^b\\ q_{e0}{\omega }_{ib}^b+[{\omega }_{ib}^b\times ]{\mathbf{q}}_{e1:3}\end{array}\right]\end{array}$$

 

또는

 

$$\begin{array}{r}\text{(13)}& & {\dot{q}}_{e0}=\frac{1}{2}({\omega }_{ib}^b{)}^T{\mathbf{q}}_{e1:3}\\ & {\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{array}$$

 

이다.

 

 

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

 

$$\begin{array}{}\text{(14)}& [I_G]{\dot{\omega }}_{ib}^b=-[{\omega }_{ib}^b\times ][I_G]{\omega }_{ib}^b+K{\mathbf{q}}_{e1:3}-D{\omega }_{ib}^b\end{array}$$

 

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

 

$$\begin{array}{r}\text{(15)}& 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)\\ & =\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{array}$$

 

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

 

$$\begin{array}{}\text{(16)}& \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\end{array}$$

 

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

 

$$\begin{array}{r}\text{(17)}& \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)\\ & -[(q_{e0}-1){\mathbf{q}}_{e1:3}^T]\left[\begin{array}{c}-{\mathbf{q}}_{e1:3}^T{\omega }_{ib}^b\\ q_{e0}{\omega }_{ib}^b+[{\omega }_{ib}^b\times ]{\mathbf{q}}_{e1:3}\end{array}\right]\\ \\ & =-({\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{array}$$

 

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

 

$$\begin{array}{}\text{(18)}& K^{-1}=c_1[I_G]+c_{2}I,c_1>0,c_2>0\end{array}$$

 

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

 

$$\begin{array}{r}\text{(19)}& \dot{V}& =-({\omega }_{ib}^b{)}^T(c_1[I_G]+c_{2}I)[{\omega }_{ib}^b\times ][I_G]{\omega }_{ib}^b\\ & -({\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{array}$$

 

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

또한 식 (19)에 의하면 $c_1=0$ 인 경우에도, 또는 $c_2=0$ 인 경우에도 $\dot{V}<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)의 자세제어 법칙을 다음과 같이 수정할 수 있다.

 

$$\begin{array}{}\text{(20)}& {\mathbf{M}}_G^b=sgn(q_{e0})K{\mathbf{q}}_{e1:3}-D{\omega }_{ib}^b\end{array}$$

 

여기서

 

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

 

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

 

$$\begin{array}{}\text{(21)}& {\mathbf{M}}_G^b=[{\omega }_{ib}^b\times ][I_G]{\omega }_{ib}^b+K{\mathbf{q}}_{e1:3}-D{\omega }_{ib}^b\end{array}$$

 

여기서

 

$$D=d[I_G],K=k[I_G],d>0,k>0$$

 

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