CN103727937B - 一种基于星敏感器的舰船姿态确定方法 - Google Patents

Abstract

一种基于星敏传感器的舰船姿态确定方法，包括以下步骤：步骤1：建立赤道直角坐标系O－XYZ、站心地平坐标系T－X^HY^HZ^H、载体坐标系T－X^zY^zZ^z、像平面直角坐标系o－xy、和像空间坐标系c－xyz；步骤2:星敏感器姿态测量的坐标变换，利用旋转矩阵完成星体在不同坐标系中的坐标值的变换，包括：赤道直角坐标系O－XYZ到站心地平坐标系T－X^HY^HZ^H的转换、和像空间坐标系c－xyz到载体坐标系T－X^zY^zZ^z的转换；步骤3：利用用欧拉角和四元数法得到舰船姿态模型；步骤4：利用QUEST方法进行最优四元数姿态矩阵的求解，得到舰船的航向、纵摇角、和横摇角。

Description

一种基于星敏感器的舰船姿态确定方法

技术领域

本发明涉及一种姿态确定方法，特别是涉及船舶的姿态确定方法。

背景技术

当前，在国内，星敏感器多应用于空间运载体，且多为单一孔径，应用于地面运载体的尚不多见。在国外，多孔径星敏感器及应用于地面运载体的星敏感器已有相关文献报道。

我军现有天文导航技术，以天体高度(或顶距)和天体方位作为观测量，高度是水平以上仰起的角度，方位是在水平面内度量的，无论高度观测还是方位观测都离不开水平基准，受角分级惯性水平基准误差制约，天文定位精度为千米量级，水平基准精度限制已成为天文导航向高精度方向发展的主要瓶颈。创立水平基准精度无关的天文导航理论，在此基础上研制高精度星图匹配导航技术设备并开展试验验证，不仅是天文导航技术向高精度方向发展的迫切要求。同时也是规避卫星导航及地基无线电导航易受攻击和操控的潜在危险，确保导航技术的战时可用性，实现高精度自主导航的必然要求。是我国新型舰艇等武器系统和武备平台技术发展的现实而又紧迫的客观需求。

我国的天文导航技术发展滞后。在民用航海领域，遵循国际海事组织相关法规，民用船舶将天文航海的理论和方法作为驾驶员任职必备的技能加以考核，将国际海事组织强制配备的六分仪、方位仪、天文钟等仪器以及航海天文历、航海天文历附表、天体高度方位表、太阳方位表等作为天文导航的基本装备。船舶天文定位的基本方法是，观测天体与 水天线之间夹角的同时记录测天时间，修正大气折光差、眼高差、视差以及面状天体的半径差得到天体地心真高度，用测天时间由《航海天文历》查算天体格林时角和赤纬，由船舶估计位置经度和天体格林时角计算天体地方时角，以天体地方时角、天体赤纬、船舶估计位置纬度为查表引数，由《天体高度方位表》查算天体计算高度和计算方位，按计算方位和天体地心真高度与天体计算高度之差确定天文位置线，用移线原理或异顶差原理将不同地点的位置线转换为同一地点，最后用作图或计算方法由多条对应于同一地点的天文位置线确定船位的经度和纬度，从而得到船舶的地表二维位置。船舶天文定向表现为利用天体测定指向仪器的零位误差(天测罗经差)。天测罗经差的基本方法是，用方位仪测定天体罗经方位的同时记录测天时间，用测天时间确定天体真方位，用天体的真方位和罗经方位确定罗经零位误差。

我国从六十年代中后期开始研制现代天文导航设备。国内从事舰船天文导航技术研发的单位，主要包括海军大连舰艇学院、华中光电技术研究所等。北京航空航天大学、长春光机所、中科院国家天文台、哈尔滨工程大学、南京航空航天大学等单位在天文导航相关技术领域具有一定技术积累。这些单位先后研制的天文导航系统，跨入了现代天文导航技术设备行列，但其精度基本上还处在国外上世纪七十年代的水平。我国目前实现的天文导航系统多用于空间载体的姿态测量，以及舰船夜间测星定位和航向测定，高精度舰船天文导航设备尚属空白。与国外先进水平相比较，我国现有天文导航技术存在很大差距，导航定位精度存在1到2个数量级的差距。

发明内容

针对现有技术中存在的缺陷，本发明提供一种基于星敏感器的舰船 姿态确定方法。

技术方案1：一种基于星敏感器的舰船姿态确定方法，其特征在于包括以下步骤：

步骤1：建立赤道直角坐标系O－XYZ、站心地平坐标系T－X^HY^HZ^H、载体坐标系T－X^zY^zZ^z、像平面直角坐标系o－xy、和像空间坐标系c－xyz；

步骤2:星敏感器姿态测量的坐标变换，利用旋转矩阵完成星体在不同坐标系中的坐标值的变换，包括：赤道直角坐标系O－XYZ到站心地平坐标系T－X^HY^HZ^H的转换、和像空间坐标系c－xyz到载体坐标系T－X^zY^zZ^z的转换；

步骤3：利用用欧拉角和四元数法得到舰船姿态模型；

步骤4：利用QUEST方法进行最优四元数姿态矩阵的求解，得到舰船的航向、纵摇角、和横摇角。

技术方案2：基于星敏感器的舰船姿态确定方法，其特征在于：

赤道直角坐标系O－XYZ：原点O位于地心，X轴指向春分点γ，Z轴垂直于天赤道指向天北极，Y轴在赤道面上，与X、Z轴构成右手直角坐标系；

站心地平坐标系T－X^HY^HZ^H：以站心T即观测点为原点，Z^H轴与站心T处的垂线重合，指向天顶为正，X^H轴为站心T所在子午圈切线，指向北为正，Y^H轴指向东，与Z^H轴、X^H轴构成左手直角坐标系；

载体坐标系T－X^zY^zZ^z：原点为舰船对称面、基本面、中船肋骨面三面的交点，X^Z轴为艏艉线，指向舰首为正；Y^z轴与舰船艏艉线垂直，并与其在同一平面上，指向右舷为正；Z^z轴与甲板平面垂直，向上为正；

像平面直角坐标系o－xy：原点o位于CCD面阵的中心，x、y轴分别平行于CCD阵列的两条相互垂直的边；

像空间坐标系c－xyz：取星敏感器光学系统的焦点c作为原点，z轴与视轴oc重合，向上为正方向；x和y轴分别平行于像平面坐标系的相应轴，方向一致。

技术方案3：基于星敏感器的舰船姿态确定方法，其特征在于：所述的星敏感器姿态测量的坐标变换的具体流程为：

首先通过星像的像平面坐标(x,y)直接获得相应的像空间坐标(x,y,-f，然后通过坐标变换矩阵R_Z得到星像在载体坐标系中的坐标(X^Z,Y^Z,Z^Z)；

然后将像平面坐标进行标定参数修正，再根据星图上星像间的角距关系进行星体辨识，从预先载入系统的星库中找到对应星体的相关信息，经过相关计算和变换后得到星体的赤道直角坐标(X,Y,Z)，最后，通过坐标变换矩阵R_H得到星体对应的站心地平坐标(X^H,Y^H,Z^H)；

最后是实现从站心地平坐标系到载体坐标系的转换，令载体坐标系的初始位置与站心坐标系重合，舰船的姿态角的定义如下：H为航向，是舰船艏艉线TX^Z在平面X^HY^H上的投影与TX^H的夹角，自正北算起，顺时针为正，P为纵摇角，是舰船艏艉线TX^Z与其在平面X^HY^H上投影的夹角，舰艏抬起为正，R为横摇角，是轴TY^Z与其在平面X^HY^H上投影的夹角，甲板右舷上扬为正，当舰船存在摆动偏离初始位置时，将站心坐标系绕TZ^H顺时针旋转角度H，再绕旋转后的TY^H逆时针旋转角度P，最后再绕第二次旋转后的TX^H顺时针旋转角度R，得到载体坐标系T－X^zY^zZ^z。则有站心地平坐标系T－X^HY^HZ^H到载体坐标系T－X^zY^zZ^z之间的坐标转换矩阵R_C：

$R_c=\left(\begin{array}{ccc}\cos P\cos H& \cos P\sin H& \sin P\\ -\sin R\sin P\cos H-\cos R\sin H& -\sin R\sin P\sin H+\cos R\cos H& \sin R\cos P\\ -\cos R\sin P\cos H+\sin R\sin H& -\cos R\sin P\sin H-\sin R\cos H& \cos R\cos P\end{array}\right).$

本发明的有益效果是：克服了现有技术的缺陷，提高了舰船姿态确定的精度和适应性。

附图说明

图1是赤道直角坐标系和站心地平坐标系示意图。

图2是载体坐标系示意图。

图3是像平面坐标系和像空间坐标系示意图。

图4是坐标变换流程图。

具体实施方式

下面结合附图，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

如图1-4所示：

1、基本坐标系的定义及变换

（1）赤道直角坐标系O－XYZ（如图1）：原点O位于地心，X轴指向春分点γ，Z轴垂直于天赤道指向天北极，Y轴在赤道面上，与X、Z轴构成右手直角坐标系。

（2）站心地平坐标系T－X^HY^HZ^H（如图1）：以站心T即观测点为原点，Z^H轴与站心T处的垂线重合，指向天顶为正，X^H轴为站心T所在子午圈切线，指向北为正，Y^H轴指向东，与Z^H轴、X^H轴构成左手直角坐标系。

（3）载体坐标系T－X^zY^zZ^z（如图2）：原点为舰船对称面、基本面、中船肋骨面三面的交点，X^Z轴为艏艉线，指向舰首为正；Y^z轴与舰船艏艉线垂直，并与其在同一平面上，指向右舷为正；Z^z轴与甲板平面垂直，向上为正。由于坐标系具有平移不变性，我们可以认为载体坐标系原点和站心重合，统一用T表示。

（4）像平面直角坐标系o－xy（如图3）：原点o位于CCD面阵的中心，x、y轴分别平行于CCD阵列的两条相互垂直的边。

（5）像空间坐标系c－xyz（如图3）：取星敏感器光学系统的焦点c作为原点，z轴与视轴oc重合，向上为正方向；x和y轴分别平行于像平面坐标系的相应轴，方向一致。对于像空间坐标系来说有左手系和右手系之分，可以根据需要进行选择，但应在使用过程中保持一致，图3中，轴系的正方向按左手定则确定，即为左手系。像点在此坐标系中的坐标值为(x,y,-f)，其中x,y为像点的像平面坐标系的坐标，而f为星敏感器的焦距，任何像点的z坐标都等于-f。

星体在不同坐标系中的坐标值是不同的，它们之间的变换关系通常可利用旋转矩阵来完成。直角坐标系有六种不同形式的旋转矩阵变换，可以将其绕每个坐标轴进行旋转，也可以使每个坐标轴反向。

（1）赤道直角坐标系到站心地平坐标系的转换。

由图1可知，已知站心T（也就是观测点）在赤道直角坐标系中的球面坐标，将赤道直角坐标系绕Z轴逆时针旋转λ，再将坐标系绕旋转后的Y轴逆时针旋转，最后将经过两次旋转的坐标系的X轴反向，就得到了站心地平坐标系T－X^HY^HZ^H，转换矩阵R_H可表示为：

则矢量在站心地平坐标系中的坐标可表示为：

$\stackrel{\→}{S}=\left(\begin{array}{c}{X}^H\\ Y^H\\ Z^H\end{array}\right)=R_H\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)\·\·\·\left(2\right)$

（2）像空间坐标系到载体坐标系的转换。

对星敏感器拍摄的星图进行处理，经过质心提取获得星像点在像平面上的坐标(x,y)，则在像空间坐标系的坐标为(x,y,-f)，f为星敏感器焦距。星敏感器一旦完成在舰船上的安装，像空间坐标系c－xyz和载体坐标系T－X^zY^zZ^z之间的坐标转换矩阵就已经确定，并且可以精确求得，不妨设为R_Z，星像点在载体坐标系T－X^zY^zZ^z中的坐标可表示为：

$\stackrel{\→}{P}=R_z\left(\begin{array}{c}x\\ y\\ -f\end{array}\right)\·\·\·\left(3\right)$

星敏感器姿态测量的坐标变换过程如图4所示：

星敏感器获取的星体像平面坐标是整个系统的观测数据，接下来分3个流程进行坐标变换。

第一个流程比较简单，首先通过星像的像平面坐标(x,y)可以直接获得相应的像空间坐标(x,y,-f)，然后通过坐标变换矩阵RZ得到星像在载体坐标系中的坐标(X^Z,Y^Z,Z^Z)。

第二个流程首先将像平面坐标进行标定参数修正，再根据该帧星图上星像间的几何位置关系（角距关系）进行星体辨识，从预先载入系统的星库中找到对应星体的相关信息，经过相关计算和变换后得到星体的赤道直角坐标(X,Y,Z)，最后，通过坐标变换矩阵R_H得到星体对应的站心地平坐标(X^H,Y^H,Z^H)。

第三个流程是实现从站心地平坐标系到载体坐标系的转换。令载体坐标系的初始位置与站心坐标系重合，舰船的姿态角的定义如下：H为航向，是舰船艏艉线TX^Z在平面X^HY^H上的投影与TX^H的夹角，自正北算 起，顺时针为正。P为纵摇角，是舰船艏艉线TX^Z与其在平面X^HY^H上投影的夹角，舰艏抬起为正。R为横摇角，是轴TY^Z与其在平面X^HY^H上投影的夹角，甲板右舷上扬为正。当舰船存在摆动偏离初始位置时，将站心坐标系绕TZ^H顺时针旋转角度H，再绕旋转后的TY^H逆时针旋转角度P，最后再绕第二次旋转后的TX^H顺时针旋转角度R，得到载体坐标系T－X^zY^zZ^z。则有站心地平坐标系T－X^HY^HZ^H到载体坐标系T－X^zY^zZ^z之间的坐标转换矩阵R_C：

$R_c=\left(\begin{array}{ccc}\cos P\cos H& \cos P\sin H& \sin P\\ -\sin R\sin P\cos H-\cos R\sin H& -\sin R\sin P\sin H+\cos R\cos H& \sin R\cos P\\ -\cos R\sin P\cos H+\sin R\sin H& -\cos R\sin P\sin H-\sin R\cos H& \cos R\cos P\end{array}\right)---\left(4\right)$

我们可以将R_C称为姿态矩阵。通过上述流程我们显然可知，对一帧星图上的所有星像来说，它们在站心地平坐标系中的坐标和载体坐标系中的坐标全部已知，所以姿态矩阵R_C精确可求，从而实现了载体的三轴姿态求解。

2、四元数描述法

四元数是具有四个元素的超复数，它可以描述一个坐标系或一个矢量相对于某一坐标系的旋转，定义为：

$q=\left[\begin{array}{c}{q}_{13}\\ q_4\end{array}\right]\·\·\·\left(5\right)$

其中，q₁₃称为四元数的矢量部分，实数q₄称为四元数的标量部分。

$q_{13}=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right]=e\sin \left(\frac{\mathrm{\α}}{2}\right)\·\·\·\left(6\right)$

$q_4=\cos \left(\frac{\mathrm{\α}}{2}\right)\·\·\·\left(7\right)$

式中e—表示旋转轴方向的单位向量；

α—旋转角。

它的代数形式为：

q＝q₁i+q₂j+q₃k+q₄……….…………（8）

规范化四元数的四个元素满足正交条件

q₁ ²+q₂ ²+q₃ ²+q₄ ²＝1……………………（9）

由于满足了归一化约束条件，它的逆q^-1可表示为

$q^{-1}=\frac{1}{q}=\frac{q^{*}}{{\mathrm{qq}}^{*}}=\frac{q^{*}}{{q_2}^2+{q_2}^2+{q_3}^2+{q_4}^2}=\left[\begin{array}{c}-q_{13}\\ q_4\end{array}\right]\·\·\·\left(10\right)$

可见，四元数的逆与四元数的共轭相同。若对坐标系连续进行了两次旋转，用四元数表示分别为q和p，则总的旋转可以用四元数的乘法来表示

$\⊗p=\left[q\right]p-\left\{p\right\}q\·\·\·\left(11\right)$

其中，算子表示四元数的乘法

$\left[\begin{array}{cc}{q}_4{I}_{3\×3}+[q_{13}\×]& q_{13}\\ -q_{13}^T& q_4\end{array}\right]\·\·\·\left(12\right)$

$\left[\begin{array}{cc}{p}_4{I}_{3\×3}-[p_{13}\×]& p_{13}\\ -p_{13}^T& p_4\end{array}\right]\·\·\·\left(13\right)$

[q₁₃×]是q₁₃的反对称阵，表示为

$[q_{13}\×]=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]\·\·\·\left(14\right)$

假设坐标系经过3-1-2转换，则坐标系的每次转换对应的四元数：

第一次转动：

第二次转动：

第三次转动： $q^{\′\′\′}={\left[\begin{array}{cccc}0& \sin \frac{\mathrm{\θ}}{2}& 0& \cos \frac{\mathrm{\θ}}{2}\end{array}\right]}^T\·\·\·\left(17\right)$

三次转换的合成：

$q=q^{\′}\⊗q^{\′\′}\⊗q^{\′\′\′}\·\·\·\left(18\right)$

用四元数表示的坐标系转换：

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]=R\left(q\right)\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}{q_1}^2-{q_2}^2-{q_3}^2+{q_4}^2& 2(q_1{q}_1-q_3{q}_4)& 2(q_1{q}_3+q_2{q}_4)\\ 2(q_1{q}_2+q_3{q}_4)& -{q_1}^2+{q_2}^2-{q_3}^2+{q_4}^2& 2(q_3{q}_2-q_1{q}_4)\\ 2(q_1{q}_3-q_2-q_4)& 2(q_3{q}_2+q_1{q}_4)& -{q_1}^2-{q_2}^2+{q_3}^2+{q_4}^2\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(19\right)$

3、舰船姿态模型

上文是对欧拉角和四元数描述方法的论述，当用欧拉角和四元数表示相同的坐标系转换时，两个姿态矩阵的对应项相等，则从（4）和（19）姿态矩阵中可以提取出舰船的姿态为：

$H=\arctan \left(\frac{2(q_1{q}_2+q_3{q}_4)}{{q_1}^2-{q_2}^2-{q_3}^2+{q_4}^2}\right)\·\·\·\left(20\right)$

P＝arctan(2q₁q₃-2q₂q₄)……………………（21）

$R=\arctan \left(\frac{2(q_3{q}_2-q_1{q}_4)}{-{q_1}^2-{q_2}^2+{q_3}^2+{q_4}^2}\right)\·\·\·\left(22\right)$

其中H为航向，取值范围为0～360，所以求解时需要根据q₁q₂+q₃q₄和q₁ ²-q₂ ²-q₃ ²+q₄ ²的正负来判断H的取值象限。

4、姿态求解

本项目采用QUEST方法进行最优四元数姿态矩阵的求解，通过求解特征值直接解决Wahba问题。

在这里，我们将姿态矩阵R_C由来表示。由上文分析，要确定进而确定姿态角，最根本的就是求解四元数[q₁,q₂,q₃,q₄]^T。设从星图中共提取出N颗星，为第i颗恒星在舰船本体坐标系T-X^ZY^ZZ^Z中方向的单位矢量，为识别得到的对应恒星在站心地平坐标系T-X^HY^HZ^H中的单位矢量，则测量模型为

${\stackrel{\→}{P}}_i=\stackrel{\→}{C}{\stackrel{\→}{S}}_i+\mathrm{\Δ}{b}_i\·\·\·\left(23\right)$

Δb_i为星敏感器的测量噪声。根据Wahba损失函数，满足的姿态矩阵是使指标函数

$J=\frac{1}{2}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{a}_i|{\stackrel{\→}{P}}_i-\stackrel{\→}{C}{\stackrel{\→}{S}}_i{|}^2\·\·\·\left(24\right)$

达到最小解。式中a_i为加权系数，对加权系数进行归一化处理，使姿态矩阵问题转化为使增益函数

$g\left(\stackrel{\→}{C}\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{a}_i{\stackrel{\→}{P}}_i^T\stackrel{\→}{C}{\stackrel{\→}{S}}_i=\mathrm{tr}\left(\stackrel{\→}{C}{\stackrel{\→}{B}}^T\right)\·\·\·\left(25\right)$

取最大值问题。其中增益函数可以化成关于姿态四元数的一个二次型函数

$g\left(\stackrel{\→}{q}\right)={\stackrel{\→}{q}}^T\stackrel{\→}{K}\stackrel{\→}{q}\·\·\·\left(26\right)$

$\stackrel{\→}{K}=\left[\begin{array}{cc}\stackrel{\→}{T}-\mathrm{\σ}\stackrel{\→}{I}& \stackrel{\→}{Z}\\ {\stackrel{\→}{Z}}^T& \mathrm{\σ}\end{array}\right]\·\·\·\left(27\right)$

其中

在N个单位参考矢量S₁,…,S_N中，只要有两个矢量不共线，则的特征值互异，且存在唯一的姿态四元数解使二次型函数取最大值。将此唯一解记为就是所求的满足关系式的最优姿态四元数解。利用拉格朗日乘子法，可解得的计算公式

$q_{\mathrm{opt}}=\frac{1}{\sqrt{{\mathrm{\γ}}^2+|\stackrel{\→}{X}}{|}^2}\left[\begin{array}{c}\stackrel{\→}{X}\\ \mathrm{\γ}\end{array}\right]\·\·\·\left(28\right)$

其中 $\stackrel{\→}{X}\≡(\mathrm{\α}\stackrel{\→}{I}+\mathrm{\β}\stackrel{\→}{T}+{\stackrel{\→}{T}}^2)\stackrel{\→}{Z},$ $\mathrm{\γ}\≡({\mathrm{\λ}}_{\max }+\mathrm{\σ})\mathrm{\α}-\mathrm{\Δ},$ $\mathrm{\α}={\mathrm{\λ}}_{\max }^2-{\mathrm{\σ}}^2+k,$ $\mathrm{\β}={\mathrm{\λ}}_{\max }-\mathrm{\σ},$ $\mathrm{\Δ}=\det \left(\stackrel{\→}{T}\right),$ λ_max为拉格朗日乘子，它是方程

λ⁴-(a+b)λ²-cλ+(ab+cσ-d)＝0………………（29） 的解，方程中的参数分别定义为：

$a={\mathrm{\σ}}^2-k,$ ${\mathrm{\σ}}^2+{\stackrel{\→}{Z}}^{T}Z,$ $c=\mathrm{\Δ}+{\stackrel{\→}{Z}}^T\stackrel{\→}{T}\stackrel{\→}{Z},$ $d={\stackrel{\→}{Z}}^T{\stackrel{\→}{T}}^2\stackrel{\→}{Z}$

利用Newton-Raphson方法进行迭代求解λ_max，初值设定为1，由λ_max进一步求得，代入（20）、（21）和（22）可求得姿态角H（航向）、P（纵摇角）、R（横摇角）。

Claims (1)

1.一种基于星敏感器的舰船姿态确定方法，其特征在于包括以下步骤：

步骤1：建立赤道直角坐标系O－XYZ、站心地平坐标系T－X^HY^HZ^H、载体坐标系T－X^zY^zZ^z、像平面直角坐标系o－xy、像空间坐标系c－xyz；

步骤2:星敏感器姿态测量的坐标变换，利用旋转矩阵完成星体在不同坐标系中的坐标值的变换，包括：赤道直角坐标系O－XYZ到站心地平坐标系T－X^HY^HZ^H的转换、和像空间坐标系c－xyz到载体坐标系T－X^zY^zZ^z的转换；

步骤3：利用欧拉角和四元数法得到舰船姿态模型；

步骤4：利用QUEST方法进行最优四元数姿态矩阵的求解，得到舰船的航向、纵摇角、和横摇角，

赤道直角坐标系O－XYZ：原点O位于地心，X轴指向春分点γ，Z轴垂直于天赤道指向天北极，Y轴在赤道面上，与X、Z轴构成右手直角坐标系；

站心地平坐标系T－X^HY^HZ^H：以载体坐标系原点T即观测点为原点，Z^H轴与载体坐标系原点T处的垂线重合，指向天顶为正，X^H轴为载体坐标系原点T所在子午圈切线，指向北为正，Y^H轴指向东，与Z^H轴、X^H轴构成左手直角坐标系；

载体坐标系T－X^zY^zZ^z：原点为舰船对称面、基本面、中船肋骨面三面的交点，X^Z轴为艏艉线，指向舰首为正；Y^z轴与舰船艏艉线垂直，并与其在同一平面上，指向右舷为正；Z^z轴与甲板平面垂直，向上为正；

像平面直角坐标系o－xy：原点o位于CCD面阵的中心，x、y轴分别平行于CCD阵列的两条相互垂直的边；

像空间坐标系c－xyz：取星敏感器光学系统的焦点c作为原点，z轴与视轴oc重合，向上为正方向；x和y轴分别平行于像平面坐标系的x轴、y轴，方向一致，

所述的星敏感器姿态测量的坐标变换的具体流程为：

首先通过星像的像平面坐标(x,y)直接获得相应的像空间坐标(x,y,-f)，然后通过坐标变换矩阵R_Z得到星像在载体坐标系中的坐标(X^Z,Y^Z,Z^Z)；

然后将像平面坐标进行标定参数修正，再根据星图上星像间的角距关系进行星体辨识，从预先载入系统的星库中找到对应星体的相关信息，经过相关计算和变换后得到星体的赤道直角坐标(X,Y,Z)，最后，通过坐标变换矩阵R_H得到星体对应的站心地平坐标(X^H,Y^H,Z^H)；

最后是实现从站心地平坐标系到载体坐标系的转换，令载体坐标系的初始位置与站心坐标系位置重合，舰船的姿态角的定义如下：H为航向角，是舰船艏艉线TX^Z在平面X^HY^H上的投影与TX^H的夹角，自正北算起，顺时针为正，P为纵摇角，是舰船艏艉线TX^Z与其在平面X^HY^H上投影的夹角，舰艏抬起为正，R为横摇角，是轴TY^Z与其在平面X^HY^H上投影的夹角，甲板右舷上扬为正，当舰船存在摆动偏离初始位置时，将站心坐标系绕TZ^H顺时针旋转角度H，再绕旋转后的TY^H逆时针旋转角度P，最后再绕第二次旋转后的TX^H顺时针旋转角度R，得到载体坐标系T－X^zY^zZ^z，则有站心地平坐标系T－X^HY^HZ^H到载体坐标系T－X^zY^zZ^z之间的坐标转换矩阵R_C；

$R_C=\left(\begin{array}{ccc}\cos P\cos H& \cos P\sin H& \sin P\\ -\sin R\sin P\cos H-\cos R\sin H& -\sin R\sin P\sin H+\cos R\cos H& \sin R\cos P\\ -\cos R\sin P\cos H+\sin R\sin H& -\cos R\sin P\sin H-\sin R\cos H& \cos R\cos P\end{array}\right);$

所述赤道直角坐标系到站心地平坐标系的转换为：已知载体坐标系原点T在赤道直角坐标系中的球面坐标将赤道直角坐标系绕Z轴逆时针旋转λ，再将坐标系绕旋转后的Y轴逆时针旋转最后将经过两次旋转的坐标系的X轴反向，就得到了站心地平坐标系T－X^HY^HZ^H，转换矩阵R_H可表示为：

则矢量在站心地平坐标系中的坐标可表示为：

$\stackrel{\→}{S}=\left(\begin{array}{c}{X}^H\\ Y^H\\ Z^H\end{array}\right)=R_H\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)\mathrm{...}\left(2\right);$

所述像空间坐标系到载体坐标系的转换为：对星敏感器拍摄的星图进行处理，经过质心提取获得星像点在像平面上的坐标(x,y)，则在像空间坐标系的坐标为(x,y,-f)，f为星敏感器焦距，星敏感器一旦完成在舰船上的安装，像空间坐标系c－xyz和载体坐标系T－X^zY^zZ^z之间的坐标转换矩阵就已经确定，并且可以精确求得，不妨设为R_Z，星像点在载体坐标系T－X^zY^zZ^z中的坐标可表示为：

$\stackrel{\→}{P}=R_Z\left(\begin{array}{c}x\\ y\\ -f\end{array}\right)\mathrm{...}\left(3\right);$

所述四元数法包括：四元数是具有四个元素的超复数，它可以描述一个坐标系或一个矢量相对于某一坐标系的旋转，定义为：

$q=\left[\begin{array}{c}{q}_{13}\\ q_4\end{array}\right]\mathrm{...}\left(5\right)$

其中，q₁₃称为四元数的矢量部分，实数q₄称为四元数的标量部分，

$q_{13}=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right]=esin\left(\frac{\mathrm{\α}}{2}\right)\mathrm{...}\left(6\right)$

$q_4=cos\left(\frac{\mathrm{\α}}{2}\right)\mathrm{...}\left(7\right)$

式中e—表示旋转轴方向的单位向量；

α—旋转角，

它的代数形式为：

q＝q₁i+q₂j+q₃k+q₄……….…………(8)

规范化四元数的四个元素满足正交条件

q₁ ²+q₂ ²+q₃ ²+q₄ ²＝1……………………(9)

由于满足了归一化约束条件，它的逆q^-1可表示为

$q^{-1}=\frac{1}{q}=\frac{q^{*}}{{\mathrm{qq}}^{*}}=\frac{q^{*}}{{q_1}^2+{q_2}^2+{q_3}^2+{q_4}^2}=\left[\begin{array}{c}-q_{13}\\ q_4\end{array}\right]\mathrm{...}\left(10\right)$

可见，四元数的逆与四元数的共轭相同，若对坐标系连续进行了两次旋转，用四元数表示分别为q和p，则总的旋转可以用四元数的乘法来表示

$q\⊗p=\[q\]p=\left\{p\right\}q\mathrm{...}\left(11\right)$

其中，算子表示四元数的乘法

$\[q\]=\left[\begin{array}{cc}{q}_4{I}_{3\×3}+\[q_{13}\×\]& q_{13}\\ -q_{13}^T& q_4\end{array}\right]\mathrm{...}\left(12\right)$

$\left\{p\right\}=\left[\begin{array}{cc}{p}_4{I}_{3\×3}-\[p_{13}\×\]& p_{13}\\ -p_{13}^T& p_4\end{array}\right]\mathrm{...}\left(13\right)$

[q₁₃×]是q₁₃的反对称阵，表示为

$\[q_{13}\×\]=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]\mathrm{...}\left(14\right)$

假设坐标系经过3-1-2转换，则坐标系的每次转换对应的四元数：

第一次转动：

第二次转动：

第三次转动：

三次转换的合成：

$q=q^{\′}\⊗q^{\′\′}\⊗q^{\′\′\′}\mathrm{...}\left(18\right)$

用四元数表示的坐标系转换：

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]=R\left(q\right)\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}{q_1}^2-{q_2}^2-{q_3}^2+{q_4}^2& 2\left(q_1{q}_2-q_3{q}_4\right)& 2\left(q_1{q}_3+q_2{q}_4\right)\\ 2\left(q_1{q}_2+q_3{q}_4\right)& -{q_1}^2+{q_2}^2-{q_3}^2+{q_4}^2& 2\left(q_3{q}_2-q_1{q}_4\right)\\ 2\left(q_1{q}_3-q_3{q}_4\right)& 2\left(q_3{q}_2+q_1{q}_4\right)& -{q_1}^2-{q_2}^2+{q_3}^2+{q_4}^2\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(19\right);$

所述舰船姿态模型为：

$H=arctan\left(\frac{2(q_1{q}_2+q_3{q}_4)}{{q_1}^2-{q_2}^2-{q_3}^2+{q_4}^2}\right)\mathrm{...}\left(20\right)$

P＝arctan(2q₁q₃-2q₂q₄)……………………(21)

$R=arctan\left(\frac{2(q_3{q}_2-q_1{q}_4)}{-{q_1}^2-{q_2}^2+{q_3}^2+{q_4}^2}\right)\mathrm{...}\left(22\right)$

其中H为航向，取值范围为0°～360°，所以求解时需要根据q₁q₂+q₃q₄和q₁ ²-q₂ ²-q₃ ²+q₄ ²的正负来判断H的取值象限；

所述利用QUEST方法进行最优四元数姿态矩阵的求解，得到舰船的航向、纵摇角、和横摇角包括：

我们将姿态矩阵R_C由来表示，由上文分析，要确定进而确定姿态角，最根本的就是求解四元数[q₁,q₂,q₃,q₄]^T，设从星图中共提取出N颗星，为第i颗恒星在舰船本体坐标系T-X^ZY^ZZ^Z中方向的单位矢量，为识别得到的对应恒星在站心地平坐标系T-X^HY^HZ^H中的单位矢量，则测量模型为

${\stackrel{\→}{P}}_i=\stackrel{\→}{C}{\stackrel{\→}{S}}_i+{\mathrm{\Δb}}_i\mathrm{...}\left(23\right)$

△b_i为星敏感器的测量噪声，根据Wahba损失函数，满足的姿态矩阵是使指标函数

$J=\frac{1}{2}\underset{i=1}{\overset{N}{\Σ}}{a}_i|{\stackrel{\→}{P}}_i-\stackrel{\→}{C}{\stackrel{\→}{S}}_i{|}^2\mathrm{...}\left(24\right)$

达到最小解，式中a_i为加权系数，对加权系数进行归一化处理，使姿态矩阵问题转化为使增益函数

$g\left(\stackrel{\→}{C}\right)=\underset{i=1}{\overset{N}{\Σ}}{a}_i{\stackrel{\→}{P}}_i^T\stackrel{\→}{C}{\stackrel{\→}{S}}_i=tr\left(\stackrel{\→}{C}{\stackrel{\→}{B}}^T\right)\mathrm{...}\left(25\right)$

取最大值问题，其中增益函数可以化成关于姿态四元数的一个二次型函数

$g\left(\stackrel{\→}{q}\right)={\stackrel{\→}{q}}^T\stackrel{\→}{K}\stackrel{\→}{q}\mathrm{...}\left(26\right)$

$\stackrel{\→}{K}=\left[\begin{array}{cc}\stackrel{\→}{T}-\mathrm{\σ}\stackrel{\→}{I}& \stackrel{\→}{Z}\\ {\stackrel{\→}{Z}}^T& \mathrm{\σ}\end{array}\right]\mathrm{...}\left(27\right)$

其中

在N个单位参考矢量S₁,…,S_N中，只要有两个矢量不共线，则的特征值互异，且存在唯一的姿态四元数解使二次型函数取最大值，将此唯一解记为就是所求的满足关系式的最优姿态四元数解，利用拉格朗日乘子法，可解得的计算公式

$q_{opt}=\frac{1}{\sqrt{{\mathrm{\γ}}^2+|\stackrel{\→}{X}{|}^2}}\left[\begin{array}{c}\stackrel{\→}{X}\\ \mathrm{\γ}\end{array}\right]\mathrm{...}\left(28\right)$

其中γ≡(λ_max+σ)α-△，β＝λ_max-σ， λ_max为拉格朗日乘子，它是方程

λ⁴-(a+b)λ²-cλ+(ab+cσ-d)＝0………………(29)

的解，方程中的参数分别定义为：

a＝σ²-k，

利用Newton-Raphson方法进行迭代求解λ_max，初值设定为1，由λ_max进一步求得代入(20)、(21)和(22)可求得姿态角，航向H、纵摇角P、横摇角R。