정적 자세결정: Davenport의 q-방법

앞서 살펴본 TRIAD는 두 개의 측정 단위벡터를 이용하여 우주비행체의 자세를 결정하였다 (https://pasus.tistory.com/302). 만약 두 개 이상의 단위벡터를 측정할 수 있다면 모든 측정 벡터를 이용할 수 있도록 TRIAD 방법을 개선해야 할 것이다. 예를 들면 많은 항성을 동시에 추적할 수 있는 별센서를 사용한다면 다수의 단위벡터가 측정된다.

 

 

Wahba는 센서의 불확실성으로 인해 발생하는 오류를 최소화하는 방식으로 다수의 측정 단위벡터를 처리하는 방법에 대한 문제를 정립했다.

\(n\) 개의 측정 단위벡터를 \(\hat{s}_k, \ k=1, ..., n\) 이라고 하자. 그리고 이 벡터를 기준좌표계 \(\{a\}\) 와 동체좌표계 \(\{b\}\) 로 표현한 벡터를 각각 \(\mathbf{s}_k^a\) 와 \(\mathbf{s}_k^b\) 로 표기한다. 그러면 좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 DCM \(C_b^a\) 를 이용하여 두 벡터의 관계식을 다음과 같이 쓸 수 있다.

 

\[ \mathbf{s}_k^a= C_b^a \mathbf{s}_k^b, \ \ \ k=1, ..., n \tag{1} \]

 

여기서 \(\mathbf{s}_k^b\) 는 우주비행체의 온보드 센서로 측정한 단위벡터이고, \(\mathbf{s}_k^a\) 는 기준좌표계에서 '수학모델'로 계산된 단위벡터이다.

Wahba는 기준좌표계에서 측정된 단위벡터와 동체좌표계에서 기준좌표계로 변환된 측정 단위벡터의 차이를 최소화하는 DCM \(C_b^a\) 를 구하는 최소화 문제를 제안하였다.

 

\[ \arg \min_{C_b^a} ⁡J(C_b^a )=\frac{1}{2} \sum_{k=1}^n w_k \vert \mathbf{s}_k^a-C_b^a \mathbf{s}_k^b \vert^2 \tag{2} \]

 

여기서 \(w_k\) 는 \(k\) 번째 벡터의 중요성을 나타내는 가중값(weight)이다. 식 (2)를 Wahba의 자세결정 문제라고 한다.

 

 

측정값에 오류가 없이 완벽하다면 \(C_b^a\) 는 모든 벡터 쌍에 대해 동일할 것이며 \(J(C_b^a )\) 의 최소값은 \(0\) 이 될 것이다. 하지만 현실 세계의 불확실성으로 인해 \(J(C_b^a )\) 는 양(\(+\))의 값을 갖게 될 것이므로 \(J(C_b^a )\) 값, 즉 오차를 최소화하는 방식으로 \(C_b^a\) 를 계산하는 것이 필요하다.

Wahba의 문제를 해결하기 위한 여러가지 방법이 제안되었는데, 예를 들면 Davenport의 q-방법, QUEST(QUaternion Estimator), SVD(Singular Value Decomposition) 방법, FOAM(Fast Optimal Attitude Matrix) 알고리즘 등이 있다. 여기서는 Wahba의 문제를 쿼터니언 기반으로 푼 Davenport의 q-방법에 대해서 알아본다.

먼저 식 (2)의 \(J(C_b^a )\) 를 전개하면 다음과 같다.

 

\[ \begin{align} J (C_b^a ) &= \frac{1}{2} \sum_{k=1}^n w_k \left( \mathbf{s}_k^a-C_b^a \mathbf{s}_k^b \right)^T \left( \mathbf{s}_k^a-C_b^a \mathbf{s}_k^b \right) \tag{3} \\ \\ &= \frac{1}{2} \sum_{k=1}^n w_k \left( (\mathbf{s}_k^a )^T \mathbf{s}_k^a-(\mathbf{s}_k^a )^T C_b^a \mathbf{s}_k^b -(\mathbf{s}_k^b )^T (C_b^a )^T \mathbf{s}_k^a+(\mathbf{s}_k^b )^T (C_b^a )^T C_b^a \mathbf{s}_k^b \right) \end{align} \]

 

여기서 \((\mathbf{s}_k^a )^T \mathbf{s}_k^a=1\), \((\mathbf{s}_k^b )^T \mathbf{s}_k^b=1\), \((C_b^a )^T C_b^a=I\) 이므로 위 식은 다음과 같이 된다.

 

\[ \begin{align} J(C_b^a ) &= \frac{1}{2} \sum_{k=1}^n w_k \left( 2-2(\mathbf{s}_k^a )^T C_b^a \mathbf{s}_k^b \right) \tag{4} \\ \\ &= \sum_{k=1}^n w_k - \sum_{k=1}^n w_k (\mathbf{s}_k^a )^T C_b^a \mathbf{s}_k^b \end{align} \]

 

위 식 우변의 첫번째 항은 \(C_b^a\) 의 함수가 아니므로 \(J(C_b^a )\) 를 최소화하기 위해서는 두번째 항을 최대화하면 된다. 따라서 최소화 문제는 다음과 같이 최대화 문제로 바뀐다.

 

\[ \max_{C_b^a } \bar{J} (C_b^a )= \sum_{k=1}^n w_k (\mathbf{s}_k^a )^T C_b^a \mathbf{s}_k^b \tag{5} \]

 

Davenport는 DCM 대신에 쿼터니언을 사용하였다. DCM과 쿼터니언 사이에는 변환 관계식이 있기 때문에 식 (5)를 다음과 같이 쓸 수 있다.

 

\[ \max_{\mathbf{q} } \bar{J} (\mathbf{q})= \sum_{k=1}^n w_k (\mathbf{s}_k^a )^T C_b^a(\mathbf{q}) \mathbf{s}_k^b \tag{6} \]

 

여기서 \(\mathbf{q}\) 는 좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 쿼터니언 \(\mathbf{q}_b^a\) 를 의미하며 \(C_b^a (\mathbf{q})\) 는 다음과 같다.

 

\[ C_b^a (\mathbf{q})= q_0^2 I+ \mathbf{q}_{1:3} \mathbf{q}_{1:3}^T+2q_0 [\mathbf{q}_{1:3} \times ]+[\mathbf{q}_{1:3} \times ]^2 \tag{7} \]

 

 

방향코사인행렬, 오일러각, 그리고 쿼터니언 | 박성수 | 딥웨이브- 교보ebook

 

쿼터니언 \(\mathbf{q}\) 는 실수부와 벡터부로 구성되는데 구성 요소는 다음과 같고,

 

\[ \mathbf{q}= \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}= \begin{bmatrix} q_0 \\ \mathbf{q}_{1:3} \end{bmatrix} \tag{8} \]

 

\([\mathbf{q}_{1:3} \times ]\) 는 \(\mathbf{q}_{1:3}\) 의 빗대칭 행렬(skew-symmetric matrix)로서 다음과 같이 정의된다.

 

\[ [\mathbf{q}_{1:3} \times ]= \begin{bmatrix} 0 & -q_3 & q_2 \\ q_3 & 0 & -q_1 \\ -q_2 & q_1 & 0 \end{bmatrix} \tag{9} \]

 

쿼터니언을 이용한 좌표 변환 식에 의하면 식 (6)의 \(C_b^a (\mathbf{q}) \mathbf{s}_k^b\) 부분은 다음과 같이 쿼터니언 식으로 표현할 수 있다 (https://pasus.tistory.com/86).

 

\[ \begin{align} \begin{bmatrix} 0 \\ C_b^a (\mathbf{q}) \mathbf{s}_k^b \end{bmatrix} &= \mathbf{q} \otimes \begin{bmatrix} 0 \\ \mathbf{s}_k^b \end{bmatrix} \otimes \mathbf{q}^* \tag{10} \\ \\ &= \begin{bmatrix} q_0 & -\mathbf{q}_{1:3}^T \\ \mathbf{q}_{1:3} & q_0 I+[\mathbf{q}_{1:3} \times ] \end{bmatrix} \begin{bmatrix} 0 & -(\mathbf{s}_k^b )^T \\ \mathbf{s}_k^b & [\mathbf{s}_k^b \times ] \end{bmatrix} \begin{bmatrix} q_0 \\ -\mathbf{q}_{1:3} \end{bmatrix} \end{align} \]

 

여기서 \(\mathbf{q}^*\) 는 켤레 쿼터니언(quaternions conjugate)이다. 식 (10)을 이용하면 식 (6)의 우변을 다음과 같이 전개할 수 있다.

 

\[ \begin{align} (\mathbf{s}_k^a )^T C_b^a (\mathbf{q}) \mathbf{s}_k^b &= \begin{bmatrix} 0 & (\mathbf{s}_k^a )^T \end{bmatrix} \begin{bmatrix} 0 \\ C_b^a (\mathbf{q}) \mathbf{s}_k^b \end{bmatrix} \tag{11} \\ \\ &= \begin{bmatrix} 0 & (\mathbf{s}_k^a )^T \end{bmatrix} \begin{bmatrix} q_0 & -\mathbf{q}_{1:3}^T \\ \mathbf{q}_{1:3} & q_0 I+[\mathbf{q}_{1:3} \times ] \end{bmatrix} \begin{bmatrix} 0 & -(\mathbf{s}_k^b )^T \\ \mathbf{s}_k^b & [\mathbf{s}_k^b \times ] \end{bmatrix} \begin{bmatrix} q_0 \\ -\mathbf{q}_{1:3} \end{bmatrix} \\ \\ &= \begin{bmatrix} q_0 & (\mathbf{q}_{1:3})^T \end{bmatrix} \begin{bmatrix} 0 & (\mathbf{s}_k^a )^T \\ \mathbf{s}_k^a & -[\mathbf{s}_k^a \times ] \end{bmatrix} \begin{bmatrix} 0 & (\mathbf{s}_k^b )^T \\ \mathbf{s}_k^b & -[\mathbf{s}_k^b \times ] \end{bmatrix} \begin{bmatrix} q_0 \\ \mathbf{q}_{1:3} \end{bmatrix} \\ \\ &= \mathbf{q}^T \begin{bmatrix} (\mathbf{s}_k^a )^T \mathbf{s}_k^b & -(\mathbf{s}_k^a )^T [\mathbf{s}_k^b \times ] \\ -[\mathbf{s}_k^a \times ] \mathbf{s}_k^b & \mathbf{s}_k^a (\mathbf{s}_k^b )^T+[ \mathbf{s}_k^a \times ][\mathbf{s}_k^b \times] \end{bmatrix} \mathbf{q} \end{align} \]

 

식 (11)을 식 (6)에 대입하면 다음과 같이 된다.

 

\[ \begin{align} \bar{J} (\mathbf{q}) &= \sum_{k=1}^n w_k \mathbf{q}^T \begin{bmatrix} (\mathbf{s}_k^a )^T \mathbf{s}_k^b & -(\mathbf{s}_k^a )^T [\mathbf{s}_k^b \times ] \\ -[\mathbf{s}_k^a \times ] \mathbf{s}_k^b & \mathbf{s}_k^a (\mathbf{s}_k^b )^T+[ \mathbf{s}_k^a \times ][\mathbf{s}_k^b \times] \end{bmatrix} \mathbf{q} \tag{12} \\ \\ &= \mathbf{q}^T \left( \sum_{k=1}^n w_k \begin{bmatrix} (\mathbf{s}_k^a )^T \mathbf{s}_k^b & -(\mathbf{s}_k^a )^T [\mathbf{s}_k^b \times ] \\ -[\mathbf{s}_k^a \times ] \mathbf{s}_k^b & \mathbf{s}_k^a (\mathbf{s}_k^b )^T+[ \mathbf{s}_k^a \times ][\mathbf{s}_k^b \times] \end{bmatrix} \right) \mathbf{q} \\ \\ &= \mathbf{q}^T K \mathbf{q} \end{align} \]

 

여기서

 

\[ K = \sum_{k=1}^n w_k \begin{bmatrix} (\mathbf{s}_k^a )^T \mathbf{s}_k^b & -(\mathbf{s}_k^a )^T [\mathbf{s}_k^b \times ] \\ -[\mathbf{s}_k^a \times ] \mathbf{s}_k^b & \mathbf{s}_k^a (\mathbf{s}_k^b )^T+[ \mathbf{s}_k^a \times ][\mathbf{s}_k^b \times] \end{bmatrix} \tag{13} \]

 

이다. 계산량을 줄이기 위해서 행렬 \(K\) 를 좀더 가다듬어 보자. 행렬 \(B\) 를 다음과 같이 정의하면,

 

\[ B= \sum_{k=1}^n w_k \ \mathbf{s}_k^a (\mathbf{s}_k^b )^T \tag{14} \]

 

행렬 \(K\) 의 구성 요소는 각각 다음과 같이 행렬 \(B\) 로 표현할 수 있다.

 

\[ \begin{align} & \sum_{k=1}^n w_k (\mathbf{s}_k^a )^T \mathbf{s}_k^b = tr(B) \tag{15} \\ \\ & \sum_{k=1}^n -w_k [\mathbf{s}_k^a \times ] \mathbf{s}_k^b = \begin{bmatrix} B_{32}-B_{23} \\ -B_{31}+B_{13} \\ B_{21}-B_{12} \end{bmatrix} = \mathbf{z} \\ \\ & \sum_{k=1}^n -w_k (\mathbf{s}_k^a )^T [\mathbf{s}_k^b \times] = \sum_{k=1}^n w_k \left([\mathbf{s}_k^b \times ] \mathbf{s}_k^a \right)^T \\ \\ & \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \sum_{k=1}^n -w_k ([\mathbf{s}_k^a \times ] \mathbf{s}_k^b )^T \\ \\ & \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \begin{bmatrix} B_{32}-B_{23} \\ -B_{31}+B_{13} \\ B_{21}-B_{12} \end{bmatrix}^T = \mathbf{z}^T \\ \\ & \sum_{k=1}^n w_k \left( \mathbf{s}_k^a (\mathbf{s}_k^b )^T + [\mathbf{s}_k^a \times ][\mathbf{s}_k^b \times] \right) =B+B^T-tr(B) I \end{align} \]

 

식 (15)를 식 (13)에 대입하면 \(K\) 는 다음과 같이 된다.

 

\[ K= \begin{bmatrix} tr(B) & \mathbf{z}^T \\ \mathbf{z} & B+B^T-tr(B) I \end{bmatrix} \tag{16} \]

 

쿼터니언은 단위 길이 제약, \(\mathbf{q}^T \mathbf{q}=1\) 을 만족해야 하므로 식 (6)의 최대화 문제는 다음과 같이 제약조건을 갖는 최대화 문제로 표현해야 한다.

 

\[ \begin{align} & \max_{\mathbf{q}} \bar{J} (\mathbf{q}) = \mathbf{q}^T K \mathbf{q} \tag{17} \\ \\ & \mbox{subject to } \ \mathbf{q}^T \mathbf{q}=1 \end{align} \]

 

라그랑지 곱수(Lagrange multiplier) \(\lambda\) 를 도입하여 식 (17)을 제약조건이 없는 최대화 문제로 바꾸면 다음과 같다 (https://pasus.tistory.com/29).

 

\[ \bar{J}'(\mathbf{q})= \mathbf{q}^T K \mathbf{q}+ \lambda (1-\mathbf{q}^T \mathbf{q} ) \tag{18} \]

 

\(\bar{J}'\) 을 \(\mathbf{q}\) 에 대해 미분하고 \(0\) 으로 놓으면 \(\bar{J}(\mathbf{q})\) 의 최대값은 다음 식을 만족하는 \(\mathbf{q}\) 에서 얻을 수 있다.

 

\[ K \mathbf{q}= \lambda \mathbf{q} \tag{19} \]

 

식 (19)는 고유값 문제이므로 이제 최대화 문제는 고유값 문제가 되었으며 \(\bar{J}(\mathbf{q})\) 를 최대화하는 쿼터니언은 행렬 \(K\) 의 최대 고유값에 해당하는 고유벡터가 된다.

 

\[ \begin{align} \max \bar{J}( \mathbf{q}) &= \mathbf{q}_{max}^T K \mathbf{q}_{max} \tag{20} \\ \\ &= \mathbf{q}_{max}^T \lambda_{max} \mathbf{q}_{max} \\ \\ &= \lambda_{max} \mathbf{q}_{max}^T \mathbf{q}_{max} \\ \\ &= \lambda_{max} \end{align} \]

 

마지막으로 \(\mathbf{q}_{max}\) 에 해당하는 DCM \(C_b^a\) 를 식 (7)의 변환식을 이용해서 계산하면 된다.

Davenport의 q-방법을 요약하면 다음과 같다.

우선 \(4 \times 4\) 행렬 \(K\) 를 식 (14)와 (16)을 이용하여 계산한다. 그리고 \(K\) 의 고유값과 고유벡터를 구한다. 이어서 최대 고유값과 그에 해당하는 고유벡터를 선택한다. 이 고유벡터가 좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 최적 쿼터니언이다.

Davenport의 q-방법은 최적화 문제를 고유값 문제로 변환하여 풀기 때문에 계산 요구량 측면에서 단점이 될 수 있다.

예를 들어본다. 두 개 이상의 측정 벡터를 처리할 수 있는 q-방법으로는 적당한 예제가 아닐 수 있지만, TRIAD에서 들었던 예와 동일한 문제(https://pasus.tistory.com/302)를 풀어보겠다.

먼저 기준좌표계 \(\{a\}\) 에서 단위벡터 \(\hat{s}_1, \ \hat{s}_2\) 가 다음과 같이 주어졌다고 하자.

 

\[ \mathbf{s}_1^a= \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}, \ \ \ \ \ \mathbf{s}_2^a= \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \tag{21} \]

 

좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 참값 DCM은 3-2-1 오일러각 30도, 20도, 10도 회전을 통해서 다음과 같다고 하자.

 

\[ C_b^a= \begin{bmatrix} 0.9254 & 0.0180 & 0.3785 \\ 0.1632 & 0.8826 & -0.4410 \\ -0.3420 & 0.4698 & 0.8138 \end{bmatrix} \tag{22} \]

 

그러면 동체좌표계 \(\{b\}\) 에서 단위벡터 \(\hat{s}_1, \ \hat{s}_2\) 는 다음과 같다.

 

\[ \mathbf{s}_1^b= \begin{bmatrix} 0.9254 \\ 0.0180 \\ 0.3785 \end{bmatrix}, \ \ \ \ \ \mathbf{s}_2^b= \begin{bmatrix} -0.3420 \\ 0.4698 \\ 0.8138 \end{bmatrix} \tag{23} \]

 

이제 식 (21)과 (23)을 측정 단위벡터로 간주하고 q-방법을 사용하여 DCM \(C_b^a\) 를 계산해보자. 먼저 가중값을 모두 \(1\) 로 하면 식 (14)에 의해서 행렬 \(B\) 는 다음과 같다.

 

\[ B= \begin{bmatrix} 0.9254 & 0.0180 & 0.3785 \\ 0 & 0 & 0 \\ -0.3420 & 0.4698 & 0.8138 \end{bmatrix} \]

 

식 (16)에 의해서 행렬 \(K\) 는 다음과 같다.

 

\[ K= \begin{bmatrix} 1.7392 & 0.4698 & 0.7205 & -0.0180 \\ 0.4698 & 0.1116 & 0.0180 & 0.0365 \\ 0.7205 & 0.0180 & -1.7392 & 0.4698 \\ -0.0180 & 0.0365 & 0.4698 & -0.1116 \end{bmatrix} \]

 

행렬 \(K\)의 최대 고유값은 \(2.0\) 이며 그 때의 고유벡터가 최적의 쿼터니언이 된다.

 

\[ \mathbf{q} = \begin{bmatrix} -0.9515 \\ -0.2393 \\ -0.1893 \\ -0.0381 \end{bmatrix} \ \ \ \ \ \mbox{또는} \ \ \ \ \ \mathbf{q} = \begin{bmatrix} 0.9515 \\ 0.2393 \\ 0.1893 \\ 0.0381 \end{bmatrix} \]

 

식 (7)을 이용하여 DCM으로 변환하면 다음과 같아서 식 (22)로 주어지는 참값 DCM과 일치함을 알 수 있다.

 

\[ C_b^a= \begin{bmatrix} 0.9254 & 0.0180 & 0.3785 \\ 0.1632 & 0.8826 & -0.4410 \\ -0.3420 & 0.4698 & 0.8138 \end{bmatrix} \]

 

 

 

다음은 q-방법을 매트랩 코드로 작성한 것이다.

 

function q = davenport(weight, S_a, S_b)

% Davenport's q-method: static attitide determination
% input:
%       weight: weights of each vector
%       S_a = [s_a_1, ..., s_a_n] of s_hat_k, k=1,...,n in frame {a}
%       S_b = [s_b_1, ..., s_b_n] of s_hat_k, k=1,...,n in frame {b}
% output:
%       quaternion q^a_b
%
% coded by st.watermelon

% size
[m,n] = size(S_a);


% B
B = zeros(3,3);
for k=1:n
    B = B + weight(k)*S_a(:,k)*S_b(:,k)';
end

% K
K11 = trace(B);
K21 = [B(3,2)-B(2,3);
      -B(3,1)+B(1,3);
       B(2,1)-B(1,2)];
K22 = B + B' - K11 * eye(3);

K = [K11 K21'; 
     K21 K22];

% eigevnalue
[V, D] = eig(K);

% max quarternion
[m_lam, m_idx] = max(diag(D));
q = V(:, m_idx);