정적 자세결정: 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$ 를 이용하여 두 벡터의 관계식을 다음과 같이 쓸 수 있다.

 

$$\begin{array}{}\text{(1)}& {\mathbf{s}}_k^a=C_b^a{\mathbf{s}}_k^b,k=1,...,n\end{array}$$

 

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

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

 

$$\begin{array}{}\text{(2)}& \arg \underset{C_b^a}{min}J(C_b^a)=\frac{1}{2}\sum _{k=1}^n{w}_k|{\mathbf{s}}_k^a-C_b^a{\mathbf{s}}_k^b{|}^2\end{array}$$

 

여기서 $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{array}{r}\text{(3)}& J(C_b^a)& =\frac{1}{2}\sum _{k=1}^n{w}_k{({\mathbf{s}}_k^a-C_b^a{\mathbf{s}}_k^b)}^T({\mathbf{s}}_k^a-C_b^a{\mathbf{s}}_k^b)\\ & =\frac{1}{2}\sum _{k=1}^n{w}_k(({\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)\end{array}$$

 

여기서 $({\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{array}{r}\text{(4)}& J(C_b^a)& =\frac{1}{2}\sum _{k=1}^n{w}_k(2-2({\mathbf{s}}_k^a{)}^T{C}_b^a{\mathbf{s}}_k^b)\\ & =\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{array}$$

 

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

 

$$\begin{array}{}\text{(5)}& \underset{C_b^a}{max}\overline{J}(C_b^a)=\sum _{k=1}^n{w}_k({\mathbf{s}}_k^a{)}^T{C}_b^a{\mathbf{s}}_k^b\end{array}$$

 

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

 

$$\begin{array}{}\text{(6)}& \underset{\mathbf{q}}{max}\overline{J}(\mathbf{q})=\sum _{k=1}^n{w}_k({\mathbf{s}}_k^a{)}^T{C}_b^a(\mathbf{q}){\mathbf{s}}_k^b\end{array}$$

 

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

 

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

 

 

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

 

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

 

$$\begin{array}{}\text{(8)}& \mathbf{q}=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}{q}_0\\ {\mathbf{q}}_{1:3}\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(9)}& [{\mathbf{q}}_{1:3}\times ]=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{r}\text{(10)}& \left[\begin{array}{c}0\\ C_b^a(\mathbf{q}){\mathbf{s}}_k^b\end{array}\right]& =\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{s}}_k^b\end{array}\right]\otimes {\mathbf{q}}^{\ast }\\ & =\left[\begin{array}{cc}{q}_0& -{\mathbf{q}}_{1:3}^T\\ {\mathbf{q}}_{1:3}& q_{0}I+[{\mathbf{q}}_{1:3}\times ]\end{array}\right]\left[\begin{array}{cc}0& -({\mathbf{s}}_k^b{)}^T\\ {\mathbf{s}}_k^b& [{\mathbf{s}}_k^b\times ]\end{array}\right]\left[\begin{array}{c}{q}_0\\ -{\mathbf{q}}_{1:3}\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{r}\text{(11)}& ({\mathbf{s}}_k^a{)}^T{C}_b^a(\mathbf{q}){\mathbf{s}}_k^b& =\left[\begin{array}{cc}0& ({\mathbf{s}}_k^a{)}^T\end{array}\right]\left[\begin{array}{c}0\\ C_b^a(\mathbf{q}){\mathbf{s}}_k^b\end{array}\right]\\ & =\left[\begin{array}{cc}0& ({\mathbf{s}}_k^a{)}^T\end{array}\right]\left[\begin{array}{cc}{q}_0& -{\mathbf{q}}_{1:3}^T\\ {\mathbf{q}}_{1:3}& q_{0}I+[{\mathbf{q}}_{1:3}\times ]\end{array}\right]\left[\begin{array}{cc}0& -({\mathbf{s}}_k^b{)}^T\\ {\mathbf{s}}_k^b& [{\mathbf{s}}_k^b\times ]\end{array}\right]\left[\begin{array}{c}{q}_0\\ -{\mathbf{q}}_{1:3}\end{array}\right]\\ \\ & =\left[\begin{array}{cc}{q}_0& ({\mathbf{q}}_{1:3}{)}^T\end{array}\right]\left[\begin{array}{cc}0& ({\mathbf{s}}_k^a{)}^T\\ {\mathbf{s}}_k^a& -[{\mathbf{s}}_k^a\times ]\end{array}\right]\left[\begin{array}{cc}0& ({\mathbf{s}}_k^b{)}^T\\ {\mathbf{s}}_k^b& -[{\mathbf{s}}_k^b\times ]\end{array}\right]\left[\begin{array}{c}{q}_0\\ {\mathbf{q}}_{1:3}\end{array}\right]\\ \\ & ={\mathbf{q}}^T\left[\begin{array}{cc}({\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{array}\right]\mathbf{q}\end{array}$$

 

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

 

$$\begin{array}{r}\text{(12)}& \overline{J}(\mathbf{q})& =\sum _{k=1}^n{w}_k{\mathbf{q}}^T\left[\begin{array}{cc}({\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{array}\right]\mathbf{q}\\ & ={\mathbf{q}}^T\left(\sum _{k=1}^n{w}_k\left[\begin{array}{cc}({\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{array}\right]\right)\mathbf{q}\\ \\ & ={\mathbf{q}}^{T}K\mathbf{q}\end{array}$$

 

여기서

 

$$\begin{array}{}\text{(13)}& K=\sum _{k=1}^n{w}_k\left[\begin{array}{cc}({\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{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(14)}& B=\sum _{k=1}^n{w}_k{\mathbf{s}}_k^a({\mathbf{s}}_k^b{)}^T\end{array}$$

 

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

 

$$\begin{array}{r}\text{(15)}& & \sum _{k=1}^n{w}_k({\mathbf{s}}_k^a{)}^T{\mathbf{s}}_k^b=tr(B)\\ & \sum _{k=1}^n-w_k[{\mathbf{s}}_k^a\times ]{\mathbf{s}}_k^b=\left[\begin{array}{c}{B}_{32}-B_{23}\\ -B_{31}+B_{13}\\ B_{21}-B_{12}\end{array}\right]=\mathbf{z}\\ \\ & \sum _{k=1}^n-w_k({\mathbf{s}}_k^a{)}^T[{\mathbf{s}}_k^b\times ]=\sum _{k=1}^n{w}_k{([{\mathbf{s}}_k^b\times ]{\mathbf{s}}_k^a)}^T\\ \\ & =\sum _{k=1}^n-w_k([{\mathbf{s}}_k^a\times ]{\mathbf{s}}_k^b{)}^T\\ \\ & ={\left[\begin{array}{c}{B}_{32}-B_{23}\\ -B_{31}+B_{13}\\ B_{21}-B_{12}\end{array}\right]}^T={\mathbf{z}}^T\\ \\ & \sum _{k=1}^n{w}_k({\mathbf{s}}_k^a({\mathbf{s}}_k^b{)}^T+[{\mathbf{s}}_k^a\times ][{\mathbf{s}}_k^b\times ])=B+B^T-tr(B)I\end{array}$$

 

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

 

$$\begin{array}{}\text{(16)}& K=\left[\begin{array}{cc}tr(B)& {\mathbf{z}}^T\\ \mathbf{z}& B+B^T-tr(B)I\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{r}\text{(17)}& & \underset{\mathbf{q}}{max}\overline{J}(\mathbf{q})={\mathbf{q}}^{T}K\mathbf{q}\\ & \text{subject to }{\mathbf{q}}^T\mathbf{q}=1\end{array}$$

 

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

 

$$\begin{array}{}\text{(18)}& {\overline{J}}^{\prime }(\mathbf{q})={\mathbf{q}}^{T}K\mathbf{q}+\lambda (1-{\mathbf{q}}^T\mathbf{q})\end{array}$$

 

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

 

$$\begin{array}{}\text{(19)}& K\mathbf{q}=\lambda \mathbf{q}\end{array}$$

 

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

 

$$\begin{array}{r}\text{(20)}& max\overline{J}(\mathbf{q})& ={\mathbf{q}}_{max}^{T}K{\mathbf{q}}_{max}\\ & ={\mathbf{q}}_{max}^T{\lambda }_{max}{\mathbf{q}}_{max}\\ \\ & ={\lambda }_{max}{\mathbf{q}}_{max}^T{\mathbf{q}}_{max}\\ \\ & ={\lambda }_{max}\end{array}$$

 

마지막으로 ${\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$ 가 다음과 같이 주어졌다고 하자.

 

$$\begin{array}{}\text{(21)}& {\mathbf{s}}_1^a=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],{\mathbf{s}}_2^a=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(22)}& C_b^a=\left[\begin{array}{ccc}0.9254& 0.0180& 0.3785\\ 0.1632& 0.8826& -0.4410\\ -0.3420& 0.4698& 0.8138\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(23)}& {\mathbf{s}}_1^b=\left[\begin{array}{c}0.9254\\ 0.0180\\ 0.3785\end{array}\right],{\mathbf{s}}_2^b=\left[\begin{array}{c}-0.3420\\ 0.4698\\ 0.8138\end{array}\right]\end{array}$$

 

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

 

$$B=\left[\begin{array}{ccc}0.9254& 0.0180& 0.3785\\ 0& 0& 0\\ -0.3420& 0.4698& 0.8138\end{array}\right]$$

 

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

 

$$K=\left[\begin{array}{cccc}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{array}\right]$$

 

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

 

$$\mathbf{q}=\left[\begin{array}{c}-0.9515\\ -0.2393\\ -0.1893\\ -0.0381\end{array}\right]\text{또는}\mathbf{q}=\left[\begin{array}{c}0.9515\\ 0.2393\\ 0.1893\\ 0.0381\end{array}\right]$$

 

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

 

$$C_b^a=\left[\begin{array}{ccc}0.9254& 0.0180& 0.3785\\ 0.1632& 0.8826& -0.4410\\ -0.3420& 0.4698& 0.8138\end{array}\right]$$

 

 

 

다음은 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);