航天器姿态的确定方法

一、简介

本文将介绍几种姿态确定的方法，用同一样例进行计算比较。读者可以根据实际情况进行选取调用。

二、TRIAD方法

在此需要引入新的坐标系：T坐标系

先将体坐标系和惯性坐标系转化为引入的T坐标系，转换方法如下：

s是sun（太阳），m是magnitude（磁场），即以太阳与磁场作为参考写出体坐标系和惯性坐标系的参数，再转换到T坐标系中。最后可以得到二者相对T坐标系的相对向量：

从而可以计算，得到DCM，代码如下：

v1B=[0.8273; 0.5541; -0.0920];
v2B=[-0.8285; 0.5522; -0.0955];
v1N=[-0.1517; -0.9669; 0.2050];
v2N=[-0.8393; 0.4494; -0.3044];
BT=[v1B, cross(v1B,v2B)/norm(cross(v1B,v2B)), cross(v1B,cross(v1B,v2B)/norm(cross(v1B,v2B)))];
NT=[v1N, cross(v1N,v2N)/norm(cross(v1N,v2N)), cross(v1N,cross(v1N,v2N)/norm(cross(v1N,v2N)))];
BN=BT*NT';
disp(BN)

三、瓦赫巴（Wahba）问题

瓦赫巴问题实际上是有N（N>1）个参数的情况下，如何求最佳姿态的问题。主要有两个方法：德文波特Q（Devenport`s Q）和QUEST。

3.1 德文波特 Q

德文波特Q的主要思想是利用四元数，先放一些主要步骤

到这一步，核心思想就是导出矩阵K，解其特征值，最大的特征值对应的特征向量即为四元数。再由四元数转化为DCM即可。

代码如下：

v1B=[0.8273; 0.5541; -0.0920];
v2B=[-0.8285; 0.5522; -0.0955];
v1N=[-0.1517; -0.9669; 0.2050];
v2N=[-0.8393; 0.4494; -0.3044];
w1=1;
w2=2;
B=w1*v1B*v1N'+w2*v2B*v2N';
S=B+B';
sigma=B(1,1)+B(2,2)+B(3,3);
Z=[B(2,3)-B(3,2); B(3,1)-B(1,3); B(1,2)-B(2,1)];
K1=[sigma, Z'];
K2=[Z, S-sigma*eye(3)];
K=[K1; K2];
[V, D]=eig(K);
if D(1,1)>D(2,2)&&D(1,1)>D(3,3)&&D(1,1)>D(4,4)
    eigenvector=V(:,1);
elseif D(2,2)>D(1,1)&&D(2,2)>D(3,3)&&D(2,2)>D(4,4)
    eigenvector=V(:,2);
elseif D(3,3)>D(1,1)&&D(3,3)>D(2,2)&&D(3,3)>D(4,4)
    eigenvector=V(:,3);
elseif D(4,4)>D(1,1)&&D(4,4)>D(2,2)&&D(4,4)>D(3,3)
    eigenvector=V(:,4);
end
Q=eigenvector;
C=[Q(1)^2+Q(2)^2-Q(3)^2-Q(4)^2, 2*(Q(2)*Q(3)+Q(1)*Q(4)), 2*(Q(2)*Q(4)-Q(1)*Q(3));
   2*(Q(2)*Q(3)-Q(1)*Q(4)) , Q(1)^2-Q(2)^2+Q(3)^2-Q(4)^2, 2*(Q(3)*Q(4)+Q(1)*Q(2));
   2*(Q(2)*Q(4)+Q(1)*Q(3)) , 2*(Q(3)*Q(4)-Q(1)*Q(2)),  Q(1)^2-Q(2)^2-Q(3)^2+Q(4)^2];
disp(C)

3.2 QUEST

与德文波特Q不同的是，QUEST采用思想是CRP，即经典罗德里格斯常数。整体思路类似，但优点是无需求解特征值。

牛顿拉夫森方法：

再用CRP转DCM方法转换，代码如下：

v1B=[0.8273; 0.5541; -0.0920];
v2B=[-0.8285; 0.5522; -0.0955];
v1N=[-0.1517; -0.9669; 0.2050];
v2N=[-0.8393; 0.4494; -0.3044];
w1=1;
w2=2;
B=w1*v1B*v1N'+w2*v2B*v2N';
S=B+B';
sigma=B(1,1)+B(2,2)+B(3,3);
Z=[B(2,3)-B(3,2); B(3,1)-B(1,3); B(1,2)-B(2,1)];
K1=[sigma, Z'];
K2=[Z, S-sigma*eye(3)];
K=[K1; K2];
lamda=w1+w2;
q=inv((lamda+sigma)*eye(3)-S)*Z;
dcm=[1+q(1)^2-q(2)^2-q(3)^2, 2*(q(1)*q(2)+q(3)), 2*(q(1)*q(3)-q(2));
     2*(q(2)*q(1)-q(3)), 1-q(1)^2+q(2)^2-q(3)^2,2*(q(2)*q(3)+q(1));
     2*(q(3)*q(1)+q(2)), 2*(q(3)*q(2)-q(1)), 1-q(1)^2-q(2)^2+q(3)^2]/(1+q'*q);
disp(dcm)

四、最佳线性姿态估计方法（OLAE）

最后是OLAE方法，他的一个巨大特点是将体坐标系和惯性坐标系都除以其范数进行标准化。

涉及Cayley变换，放出过程，不作详细说明。

v1B=[0.8273; 0.5541; -0.0920];
v2B=[-0.8285; 0.5522; -0.0955];
v1B=v1B/norm(v1B);
v2B=v2B/norm(v2B);
v1N=[-0.1517; -0.9669; 0.2050];
v2N=[-0.8393; 0.4494; -0.3044];
v1N=v1N/norm(v1N);
v2N=v2N/norm(v2N);
W=eye(6);
A=v1B+v1N;
B=v2B+v2N;
tilde_1=[0, -A(3), A(2);
         A(3), 0, -A(1);
         -A(2), A(1), 0];
tilde_2=[0, -B(3), B(2);
         B(3), 0, -B(1);
         -B(2), B(1), 0];
S=[tilde_1; tilde_2];
d=[v1B-v1N; v2B-v2N];
q=inv(S'*W*S)*S'*W*d;
dcm=[1+q(1)^2-q(2)^2-q(3)^2, 2*(q(1)*q(2)+q(3)), 2*(q(1)*q(3)-q(2));
     2*(q(2)*q(1)-q(3)), 1-q(1)^2+q(2)^2-q(3)^2,2*(q(2)*q(3)+q(1));
     2*(q(3)*q(1)+q(2)), 2*(q(3)*q(2)-q(1)), 1-q(1)^2-q(2)^2+q(3)^2]/(1+q'*q);
disp(dcm)

五、误差估计

先贴一下四个方法的计算结果，相当接近，精度要求很高。

如何评估误差呢？主要思想是用DCM转换为PRV。理论DCM乘以真实DCM的转置，得到Bi。将Bi从DCM转换为PRV（主旋转向量）。PRV主要由旋转角度与旋转方向决定。旋转方向是三个轴共同决定。最后再用PRV相关公式得到向量范数，即偏离角度。

BN=[ 0.969846, 0.17101, 0.173648;
    -0.200706, 0.96461, 0.17101;
    -0.138258, -0.200706, 0.969846];
BN_true=[0.963592, 0.187303, 0.190809;
         -0.223042, 0.956645, 0.187303;
         -0.147454, -0.223042, 0.963592];
Bi=BN*BN_true';
tr=Bi(1,1)+Bi(2,2)+Bi(3,3);
phi=acos(0.5*(tr-1));
e=(1/(2*sin(rad2deg(phi))))*[Bi(2,3)-Bi(3,2); Bi(3,1)-Bi(1,3); Bi(1,2)-Bi(2,1)];
disp(rad2deg(norm(e)))

六、结语

有多种方法可以对航天器姿态进行评估与确定。本文如有纰漏，烦请读者批评指正。