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

摘要

一种基于星敏传感器的舰船姿态确定方法，包括以下步骤：步骤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方法进行最优四元数姿态矩阵的求解，得到舰船的航向、纵摇角、和横摇角。

主权项

一种基于星敏感器的舰船姿态确定方法，其特征在于包括以下步骤：步骤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可表示为：则矢量在站心地平坐标系中的坐标可表示为：$\overrightarrow{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中的坐标可表示为：$\overrightarrow{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{\alpha }{2}\right)\mathrm{...}\left(6\right)$$q_4=cos\left(\frac{\alpha }{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\otimes p=\left[q\right]p=\left\{p\right\}q\mathrm{...}\left(11\right)$其中，算子表示四元数的乘法$\left[q\right]=\left[\begin{array}{cc}{q}_4{I}_{3\times 3}+[q_{13}\times ]& 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\times 3}-[p_{13}\times ]& p_{13}\\ -p_{13}^T& p_4\end{array}\right]\mathrm{...}\left(13\right)$[q₁₃×]是q₁₃的反对称阵，表示为$[q_{13}\times ]=\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^{\prime }\otimes q^{\prime \prime }\otimes q^{\prime \prime \prime }\mathrm{...}\left(18\right)$用四元数表示的坐标系转换：$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\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中的单位矢量，则测量模型为${\overrightarrow{P}}_i=\overrightarrow{C}{\overrightarrow{S}}_i+{\mathrm{\Delta b}}_i\mathrm{...}\left(23\right)$△b_i为星敏感器的测量噪声，根据Wahba损失函数，满足的姿态矩阵是使指标函数$J=\frac{1}{2}\underset{i=1}{\overset{N}{\Sigma }}{a}_i|{\overrightarrow{P}}_i-\overrightarrow{C}{\overrightarrow{S}}_i{|}^2\mathrm{...}\left(24\right)$达到最小解，式中a_i为加权系数，对加权系数进行归一化处理，使姿态矩阵问题转化为使增益函数$g\left(\overrightarrow{C}\right)=\underset{i=1}{\overset{N}{\Sigma }}{a}_i{\overrightarrow{P}}_i^T\overrightarrow{C}{\overrightarrow{S}}_i=tr\left(\overrightarrow{C}{\overrightarrow{B}}^T\right)\mathrm{...}\left(25\right)$取最大值问题，其中增益函数可以化成关于姿态四元数的一个二次型函数$g\left(\overrightarrow{q}\right)={\overrightarrow{q}}^T\overrightarrow{K}\overrightarrow{q}\mathrm{...}\left(26\right)$$\overrightarrow{K}=\left[\begin{array}{cc}\overrightarrow{T}-\sigma \overrightarrow{I}& \overrightarrow{Z}\\ {\overrightarrow{Z}}^T& \sigma \end{array}\right]\mathrm{...}\left(27\right)$其中在N个单位参考矢量S₁,…,S_N中，只要有两个矢量不共线，则的特征值互异，且存在唯一的姿态四元数解使二次型函数取最大值，将此唯一解记为就是所求的满足关系式的最优姿态四元数解，利用拉格朗日乘子法，可解得的计算公式$q_{opt}=\frac{1}{\sqrt{{\gamma }^2+|\overrightarrow{X}{|}^2}}\left[\begin{array}{c}\overrightarrow{X}\\ \gamma \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。