[发明专利]一种基于星敏感器的舰船姿态确定方法

摘要

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

主权项

一种基于星敏感器的舰船姿态确定方法，其特征在于包括以下步骤：步骤1：建立赤道直角坐标系O－XYZ、站心地平坐标系T－XHYHZH、载体坐标系T－XzYzZz、像平面直角坐标系o－xy、像空间坐标系c－xyz；步骤2:星敏感器姿态测量的坐标变换，利用旋转矩阵完成星体在不同坐标系中的坐标值的变换，包括：赤道直角坐标系O－XYZ到站心地平坐标系T－XHYHZH的转换、和像空间坐标系c－xyz到载体坐标系T－XzYzZz的转换；步骤3：利用欧拉角和四元数法得到舰船姿态模型；步骤4：利用QUEST方法进行最优四元数姿态矩阵的求解，得到舰船的航向、纵摇角、和横摇角，赤道直角坐标系O－XYZ：原点O位于地心，X轴指向春分点γ，Z轴垂直于天赤道指向天北极，Y轴在赤道面上，与X、Z轴构成右手直角坐标系；站心地平坐标系T－XHYHZH：以载体坐标系原点T即观测点为原点，ZH轴与载体坐标系原点T处的垂线重合，指向天顶为正，XH轴为载体坐标系原点T所在子午圈切线，指向北为正，YH轴指向东，与ZH轴、XH轴构成左手直角坐标系；载体坐标系T－XzYzZz：原点为舰船对称面、基本面、中船肋骨面三面的交点，XZ轴为艏艉线，指向舰首为正；Yz轴与舰船艏艉线垂直，并与其在同一平面上，指向右舷为正；Zz轴与甲板平面垂直，向上为正；像平面直角坐标系o－xy：原点o位于CCD面阵的中心，x、y轴分别平行于CCD阵列的两条相互垂直的边；像空间坐标系c－xyz：取星敏感器光学系统的焦点c作为原点，z轴与视轴oc重合，向上为正方向；x和y轴分别平行于像平面坐标系的x轴、y轴，方向一致，所述的星敏感器姿态测量的坐标变换的具体流程为：首先通过星像的像平面坐标(x,y)直接获得相应的像空间坐标(x,y,‑f)，然后通过坐标变换矩阵RZ得到星像在载体坐标系中的坐标(XZ,YZ,ZZ)；然后将像平面坐标进行标定参数修正，再根据星图上星像间的角距关系进行星体辨识，从预先载入系统的星库中找到对应星体的相关信息，经过相关计算和变换后得到星体的赤道直角坐标(X,Y,Z)，最后，通过坐标变换矩阵RH得到星体对应的站心地平坐标(XH,YH,ZH)；最后是实现从站心地平坐标系到载体坐标系的转换，令载体坐标系的初始位置与站心坐标系位置重合，舰船的姿态角的定义如下：H为航向角，是舰船艏艉线TXZ在平面XHYH上的投影与TXH的夹角，自正北算起，顺时针为正，P为纵摇角，是舰船艏艉线TXZ与其在平面XHYH上投影的夹角，舰艏抬起为正，R为横摇角，是轴TYZ与其在平面XHYH上投影的夹角，甲板右舷上扬为正，当舰船存在摆动偏离初始位置时，将站心坐标系绕TZH顺时针旋转角度H，再绕旋转后的TYH逆时针旋转角度P，最后再绕第二次旋转后的TXH顺时针旋转角度R，得到载体坐标系T－XzYzZz，则有站心地平坐标系T－XHYHZH到载体坐标系T－XzYzZz之间的坐标转换矩阵RC；RC=cosPcosHcosPsinHsinP-sinRsinPcosH-cosRsinH-sinRsinPsinH+cosRcosHsinRcosP-cosRsinPcosH+sinRsinH-cosRsinPsinH-sinRcosHcosRcosP;]]>所述赤道直角坐标系到站心地平坐标系的转换为：已知载体坐标系原点T在赤道直角坐标系中的球面坐标将赤道直角坐标系绕Z轴逆时针旋转λ，再将坐标系绕旋转后的Y轴逆时针旋转最后将经过两次旋转的坐标系的X轴反向，就得到了站心地平坐标系T－XHYHZH，转换矩阵RH可表示为：则矢量在站心地平坐标系中的坐标可表示为：S→=XHYHZH=RHXYZ...(2);]]>所述像空间坐标系到载体坐标系的转换为：对星敏感器拍摄的星图进行处理，经过质心提取获得星像点在像平面上的坐标(x,y)，则在像空间坐标系的坐标为(x,y,‑f)，f为星敏感器焦距，星敏感器一旦完成在舰船上的安装，像空间坐标系c－xyz和载体坐标系T－XzYzZz之间的坐标转换矩阵就已经确定，并且可以精确求得，不妨设为RZ，星像点在载体坐标系T－XzYzZz中的坐标可表示为：P→=RZxy-f...(3);]]>所述四元数法包括：四元数是具有四个元素的超复数，它可以描述一个坐标系或一个矢量相对于某一坐标系的旋转，定义为：q=q13q4...(5)]]>其中，q13称为四元数的矢量部分，实数q4称为四元数的标量部分，q13=q1q2q3=esin(α2)...(6)]]>q4=cos(α2)...(7)]]>式中e—表示旋转轴方向的单位向量；α—旋转角，它的代数形式为：q＝q1i+q2j+q3k+q4……….…………(8)规范化四元数的四个元素满足正交条件q12+q22+q32+q42＝1……………………(9)由于满足了归一化约束条件，它的逆q‑1可表示为q-1=1q=q*qq*=q*q12+q22+q32+q42=-q13q4...(10)]]>可见，四元数的逆与四元数的共轭相同，若对坐标系连续进行了两次旋转，用四元数表示分别为q和p，则总的旋转可以用四元数的乘法来表示q⊗p=[q]p={p}q...(11)]]>其中，算子表示四元数的乘法[q]=q4I3×3+[q13×]q13-q13Tq4...(12)]]>{p}=p4I3×3-[p13×]p13-p13Tp4...(13)]]>[q13×]是q13的反对称阵，表示为[q13×]=0-q3q2q30-q1-q2q10...(14)]]>假设坐标系经过3‑1‑2转换，则坐标系的每次转换对应的四元数：第一次转动：第二次转动：第三次转动：三次转换的合成：q=q′⊗q′′⊗q′′′...(18)]]>用四元数表示的坐标系转换：x′y′z′=R(q)xyz=q12-q22-q32+q422(q1q2-q3q4)2(q1q3+q2q4)2(q1q2+q3q4)-q12+q22-q32+q422(q3q2-q1q4)2(q1q3-q3q4)2(q3q2+q1q4)-q12-q22+q32+q42xyz---(19);]]>所述舰船姿态模型为：H=arctan(2(q1q2+q3q4)q12-q22-q32+q42)...(20)]]>P＝arctan(2q1q3‑2q2q4)……………………(21)R=arctan(2(q3q2-q1q4)-q12-q22+q32+q42)...(22)]]>其中H为航向，取值范围为0°～360°，所以求解时需要根据q1q2+q3q4和q12‑q22‑q32+q42的正负来判断H的取值象限；所述利用QUEST方法进行最优四元数姿态矩阵的求解，得到舰船的航向、纵摇角、和横摇角包括：我们将姿态矩阵RC由来表示，由上文分析，要确定进而确定姿态角，最根本的就是求解四元数[q1,q2,q3,q4]T，设从星图中共提取出N颗星，为第i颗恒星在舰船本体坐标系T‑XZYZZZ中方向的单位矢量，为识别得到的对应恒星在站心地平坐标系T‑XHYHZH中的单位矢量，则测量模型为P→i=C→S→i+Δbi...(23)]]>△bi为星敏感器的测量噪声，根据Wahba损失函数，满足的姿态矩阵是使指标函数J=12Σi=1Nai|P→i-C→S→i|2...(24)]]>达到最小解，式中ai为加权系数，对加权系数进行归一化处理，使姿态矩阵问题转化为使增益函数g(C→)=Σi=1NaiP→iTC→S→i=tr(C→B→T)...(25)]]>取最大值问题，其中增益函数可以化成关于姿态四元数的一个二次型函数g(q→)=q→TK→q→...(26)]]>K→=T→-σI→Z→Z→Tσ...(27)]]>其中在N个单位参考矢量S1,…,SN中，只要有两个矢量不共线，则的特征值互异，且存在唯一的姿态四元数解使二次型函数取最大值，将此唯一解记为就是所求的满足关系式的最优姿态四元数解，利用拉格朗日乘子法，可解得的计算公式qopt=1γ2+|X→|2X→γ...(28)]]>其中γ≡(λmax+σ)α‑△，β＝λmax‑σ，λmax为拉格朗日乘子，它是方程λ4‑(a+b)λ2‑cλ+(ab+cσ‑d)＝0………………(29)的解，方程中的参数分别定义为：a＝σ2‑k，利用Newton‑Raphson方法进行迭代求解λmax，初值设定为1，由λmax进一步求得代入(20)、(21)和(22)可求得姿态角，航向H、纵摇角P、横摇角R。