CN104374388A - 一种基于偏振光传感器的航姿测定方法 - Google Patents

Abstract

本发明公开了一种基于偏振光传感器的航姿测定方法，采用设备包括三轴陀螺仪、三轴加速度计、偏振光传感器、GPS和飞控计算机。采用互补滤波器算法对各传感器数据进行融合，通过加速度计测量数据修正陀螺仪角速率的俯仰角和滚转角误差，偏振光传感器测量数据修正陀螺仪角速率的航向角误差，提高飞行器的姿态测量精度。本发明相比于传统航姿参考系统，具有不受电磁干扰、静动态环境测量精度高等优点。

Description

一种基于偏振光传感器的航姿测定方法

技术领域

本发明属于飞行器姿态测量与估计技术领域，涉及一种基于偏振光传感器的航姿测定方法。

背景技术

航姿参考系统(AHRS)能够为飞行器提供航向角、滚转角和俯仰角信息。它一般由多个轴向传感器组成，目前主要有两种组合：一种由三轴陀螺仪、三轴加速度计和三轴磁强计组成，另一种由三轴陀螺仪、三轴加速度计和GPS组成。但是以上两种方法的测量均存在各自的缺点：第一种组合的磁强计容易受到周围磁场和其他机载电子设备的影响，从而导致航向误差增大；第二种方法的GPS在静态时不能提供航向角，高机动时易丢星，同样将导致航向误差的增大。为了弥补以上缺点，本发明加入了偏振光传感器，提出基于偏振光传感器的航姿测定方法。

发明内容

本发明旨在采用偏振光传感器测量的偏振光方位角去修正陀螺仪的测量数据，提高姿态捷联矩阵的精确性，已达到提高姿态测量精度的目的。

本发明采用如下技术方案：

一种基于偏振光传感器的航姿测定方法，采用的设备包括三轴陀螺仪、三轴加速度计、偏振光传感器、GPS和飞控计算机。三轴陀螺仪测量飞行器三轴角速率，三轴加速度计测量飞行器的三轴加速度。偏振光传感器测量偏振光方位角。GPS提供当地时间、飞行器所在位置、速度信息。飞控计算机需要实时处理各种传感器传回的数据，还要将处理的结果发送给飞行器的控制单元，以实现对飞行器的机构控制，同时也承担将数据发送到地面站和接受地面控制指令的任务，因此必须考虑信息处理的实时性和数据融合算法的精简程度。通过加速度计测量的数据校正陀螺仪角速率的俯仰角、滚转角误差，偏振光传感器测量数据修正陀螺仪角速率的航向角误差，提高飞行器的姿态测量精度。

该方法具体步骤如下：

(1)采集三轴加速度计、偏振光传感器和GPS的输出数据，确定飞行器的初始滚转角φ、俯仰角θ和航向角ψ，建立导航坐标系到机体坐标系的初始姿态矩阵和机体坐标系到偏振光传感器坐标系的姿态矩阵

(2)根据飞行器所属时区，通过天文历计算方法估算当地太阳高度角h_s、方位角A_s，然后计算得到太阳方向矢量在导航坐标系下的投影

$a_{\mathrm{sun}}^n=\left[\cos \left(\mathrm{hs}\right)\sin \left(A_s\right)\cos \left(\mathrm{hs}\right)\cos \left(A_s\right)\sin \left(\mathrm{hs}\right)\right]---\left(1\right)$

(3)根据偏振光传感器坐标系、机体坐标系、导航坐标系之间关系和瑞利散射原理得到偏振光传感器坐标系下的入射光最大偏振方向矢量的观测值：

$\left[\begin{array}{c}\sin {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ \cos {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ 0\end{array}\right]=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]C_b^m{C}_n^b{a}_{\mathrm{sun}}^n=\left[\begin{array}{c}{A}_1\\ A_2\\ 0_{1\×3}\end{array}\right]C_b^m{C}_n^b{a}_{\mathrm{sun}}^n=\left[\begin{array}{c}{A}_1{C}_b^m{C}_n^b{a}_{\mathrm{sun}}^n\\ A_2{C}_b^m{C}_n^b{a}_{\mathrm{sun}}^n\\ 0\end{array}\right]---\left(2\right)$

(4)采集偏振光传感器输出的偏振方位角ψ_pl，在偏振光传感器坐标系下，计算入射光的最大偏振方向矢量的测量值： $\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{\mathrm{pl}}\\ \cos {\mathrm{\ψ}}_{\mathrm{pl}}\\ 0\end{array}\right].$

(5)计算航向误差校正矢量，入射光最大偏振方向矢量的观测值与测量值的偏差即为航向角误差，其值为两个矢量的叉乘：

$e_{\mathrm{\ψ}}=\left[\begin{array}{c}\sin {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ \cos {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ 0\end{array}\right]\×\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{\mathrm{pl}}\\ \cos {\mathrm{\ψ}}_{\mathrm{pl}}\\ 0\end{array}\right]---\left(3\right)$

(6)采集三轴加速度数据g_b，去掉加速度计测量值中的向心加速度，得到重力加速度矢量参考值。

$g_r=g_b+{\mathrm{\ω}}_{\mathrm{en}}^n\×V---\left(4\right)$

其中速度V由GPS得到，由V经过公式计算得到。

(7)计算俯仰、滚转误差校正矢量，即重力加速度的观测值(的第三列)与实际测量参考值g_r单位矢量的差值，其值为两矢量的叉乘：

$e_{\mathrm{\φ\θ}}=\left[\begin{array}{c}{C}_{13}\\ C_{23}\\ C_{33}\end{array}\right]\×\frac{g_r}{\left|g_r\right|}---\left(5\right)$

(8)进行互补滤波，通过反馈控制校正陀螺仪测量角速率，从而提高航向角、俯仰角、滚转角的精度。

ω＝ω_b+k_ψPe_ψ+k_ψIdtΣe_ψ+k_φθPe_φθ+k_φθIdtΣe_φθ    (6)

根据加速度计和偏振光传感器的响应时间对其采用不同的滤波系数，其中，k_ψP、k_φθP的大小决定了互补滤波器的截止频率，k_ψI、k_φθI的大小决定了消除静态偏差的时间。

(9)通过四元数法更新，得到新的姿态矩阵和姿态角。

(10)重复(1)至(9)过程，实现系统实时输出飞行器的航姿信息。

本发明的有益效果是：

1、相比于三轴陀螺仪、三轴加速度计和三轴磁强计组合，本发明不受周围环境地磁和机载设备的电磁干扰；相比于三轴陀螺仪，三轴加速度计和GPS组合,本发明不受飞行器运动状态的影响。

2、本发明采用互补滤波器算法解算飞行器姿态，相比于扩展卡尔曼滤波器，不需要偏振光传感器的精确误差模型，而且计算量小，能够实现长时间稳定地输出高精度姿态数据，尤其适合微型飞控系统。

附图说明

图1为本发明的主要坐标系图。

图2为本发明的航姿测定方法工作流程图。

图3为本发明的原理框图。

具体实施方式

下面结合附图和具体技术方案对本发明的具体实施方式作进一步阐述。

如图1所示，本发明涉及的坐标系有：地平坐标系，导航坐标系，机体坐标系，偏振光传感器坐标系。其中导航坐标系选取东北天坐标系，同时为了减少坐标系之间的转换，选取地平坐标系与导航坐标系重合，为东北天坐标系，偏振光传感器坐标系与机体坐标系重合。设偏振光传感器坐标系的Y轴为其体轴，则偏振光传感器测量的偏振方位角ψ_pl为入射光的最大偏振方向矢量在偏振光传感器坐标系的OXY平面的投影与Y轴的夹角。

在本发明的航姿参考系统中，三轴陀螺仪能够测量无人机机体的三轴角速度矢量，可根据它的测量信息解算出无人机的俯仰角、滚转角、航向角信息，短时间测量精度高，但长时间测量精度会受到温度漂移的影响；三轴加速度传感器能够测量无人机机体的三轴加速度矢量，可根据它的测量信息解算出无人机的俯仰角、滚转角信息，长时间测量精度高，但短时间测量精度会受到机体振动的影响；偏振光传感器能够测量入射光方向的偏振方位角，没有误差积累，长时间测量精度高，但短时间测量精度不如陀螺仪。由以上分析可知，陀螺仪与加速度计、偏振光存在测量相同的量，并且在频域特性上互补，所以采互补滤波器算法解算飞行器的姿态。

结合图2和图3，以下是该方法的具体步骤：

1、采集三轴加速度计、偏振光传感器和GPS输出数据，确定飞行器的初始滚转角φ、俯仰角θ和航向角ψ。建立导航坐标系到机体坐标系的初始姿态矩阵和机体坐标系到偏振光传感器坐标系的姿态矩阵

飞行器起飞之前一般为静止状态，加速度计测量值可认为只有重力加速度，不存在运动加速度，相当于重力加速度在飞行器机体三轴的投影，由于重力始终与航向平面始终垂直，所以不能得到航向角，可以解算得到飞行器的初始俯仰角、滚转角：

$\begin{array}{c}\mathrm{\θ}={\tan }^{-1}\frac{(-g_{\mathrm{mx}})}{\sqrt{{g_{\mathrm{my}}}^2+{g_{\mathrm{mz}}}^2}}\\ \mathrm{\φ}={\tan }^{-1}\frac{g_{\mathrm{my}}}{g_{\mathrm{mz}}}\end{array}---\left(7\right)$

根据瑞利散射原理，入射光的最大偏振光矢量与太阳子午线垂直，可以解算得到飞行器的初始航向角：

$\mathrm{\ψ}={\mathrm{\ψ}}_{\mathrm{pl}}+A_s\±\frac{\mathrm{\π}}{2}---\left(8\right)$

其中，A_s为太阳方位角，根据GPS输出的飞行器位置和当地时间通过天文历计算方法估算得到。方位角采用在大地测量中的方位，方位角以正北方为起算点，按顺时针方向度量，取值范围为0°～360°，这种定义与一般偏振光文献中定义的天文测量中的方位值有所区别。

导航坐标系到机体坐标系的初始姿态矩阵为：

$\begin{array}{c}{C}_n^b=\left[\begin{array}{ccc}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θco\; s\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\φ}\sin \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]\\ =\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\\ =\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\end{array}---\left(9\right)$

机体坐标系到偏振光传感器坐标系的姿态矩阵为：

$C_b^m=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(10\right)$

2、采集GPS数据，根据GPS输出的飞行器位置和当地时间通过天文历计算方法估算得到太阳高度角h_s和太阳方位角A_s,然后计算得到太阳方向矢量在导航坐标系下的投影

$a_{\mathrm{sun}}^n=\left[\cos \left(\mathrm{hs}\right)\sin \left(A_s\right)\cos \left(\mathrm{hs}\right)\cos \left(A_s\right)\sin \left(\mathrm{hs}\right)\right]---\left(11\right)$

3.根据偏振光传感器坐标系、机体坐标系、导航坐标系之间关系和瑞利散射原理得到偏振光传感器坐标系下的入射光最大偏振方向矢量的观测值。

在偏振光传感器坐标系下，根据瑞利散射原理，入射光的最大偏振方向矢量垂直于观测方向矢量与太阳方向矢量所在的平面，可表示为：

$\left[\begin{array}{c}\sin {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ \cos {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ 0\end{array}\right]=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]C_b^m{C}_n^b{a}_{\mathrm{sun}}^n=\left[\begin{array}{c}{A}_1\\ A_2\\ 0_{1\×3}\end{array}\right]C_b^m{C}_n^b{a}_{\mathrm{sun}}^n=\left[\begin{array}{c}{A}_1{C}_b^m{C}_n^b{a}_{\mathrm{sun}}^n\\ A_2{C}_b^m{C}_n^b{a}_{\mathrm{sun}}^n\\ 0\end{array}\right]---\left(12\right)$

4、采集偏振光传感器输出的偏振方位角ψ_pl，在偏振光传感器坐标系下，计算入射光的最大偏振方向矢量的测量值： $\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{\mathrm{pl}}\\ \cos {\mathrm{\ψ}}_{\mathrm{pl}}\\ 0\end{array}\right].$

5、计算航向角误差校正矢量，入射光最大偏振方向矢量的观测值与测量值的偏差即为航向误差，其值近似为两个矢量的叉乘：

$e_{\mathrm{\ψ}}=\left[\begin{array}{c}\sin {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ \cos {\widetilde{\mathrm{\ψ}}}_{\mathrm{pl}}\\ 0\end{array}\right]\×\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{\mathrm{pl}}\\ \cos {\mathrm{\ψ}}_{\mathrm{pl}}\\ 0\end{array}\right]---\left(13\right)$

(6)采集三轴加速度数据g_b，去掉加速度计测量值中的向心加速度，得到重力加速度矢量参考值。

$g_r=g_b+{\mathrm{\ω}}_{\mathrm{en}}^n\×V---\left(14\right)$

其中速度V由GPS得到，由V经过公式计算得到。

7、计算俯仰角、滚转角误差校正矢量，重力加速度的观测值(的第三列)与实际测量的参考值即为俯仰、滚转误差，其值近似为两矢量的叉乘：

$e_{\mathrm{\φ\θ}}=\left[\begin{array}{c}{C}_{13}\\ C_{23}\\ C_{33}\end{array}\right]\×\frac{g_r}{\left|g_r\right|}---\left(15\right)$

8、进行互补滤波，通过反馈控制校正陀螺仪测量角速率，从而提高航向角、俯仰角、滚转角的精度。

ω＝ω_b+k_ψPe_ψ+k_ψIdtΣe_ψ+k_φθPe_φθ+k_φθIdtΣe_φθ    (16)

根据加速度计和偏振光传感器的响应时间对其采用不同的滤波系数，其中，k_ψP、k_φθP的大小决定了互补滤波器的截止频率，k_ψI、k_φθI的大小决定了消除静态偏差的时间。

9、通过四元数法更新，得到新的姿态矩阵和姿态角。

将修正后的角速率ω带入四元数微分方程，使用四阶龙格库塔法解算得到新的四元数(q₀q₁q₂q₃)。

$\left[\begin{array}{c}{\stackrel{.}{q}}_0\\ {\stackrel{.}{q}}_1\\ {\stackrel{.}{q}}_2\\ {\stackrel{.}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(17\right)$

将新的四元数q₀q₁q₂代入公式9更新姿态矩阵

由解算出飞行器的姿态角：

$\begin{array}{c}\mathrm{\ψ}={\tan }^{-1}\left(\frac{C_{21}}{C_{22}}\right)\\ \mathrm{\φ}={\tan }^{-1}(-\frac{C_{13}}{C_{33}})\\ \mathrm{\θ}={\sin }^{-1}\left(C_{23}\right)\end{array}---\left(18\right)$

10、重复步骤1至步骤9过程，实现系统实时输出飞行器的姿态信息。

Claims (1)

1.一种基于偏振光传感器的航姿测定方法，其特征在于以下步骤，

(1)采集三轴加速度计、偏振光传感器和GPS的输出数据，确定飞行器的初始滚转角φ、俯仰角θ和航向角ψ，建立导航坐标系到机体坐标系的初始姿态矩阵和机体坐标系到偏振光传感器坐标系的姿态矩阵

(2)根据飞行器所属时区，通过天文历计算方法估算当地太阳高度角h_s、方位角A_s，然后依据下式(1)计算得到太阳方向矢量在导航坐标系下的投影

$a_{\mathrm{sun}}^n=\left[\cos \left(\mathrm{hs}\right)\sin \left(A_s\right)\cos \left(\mathrm{hs}\right)\cos \left(A_s\right)\sin \left(\mathrm{hs}\right)\right]---\left(1\right)$

(3)根据下式(2)中偏振光传感器坐标系、机体坐标系、导航坐标系之间关系和瑞利散射原理得到偏振光传感器坐标系下的入射光最大偏振方向矢量的观测值：

$\left[\begin{array}{c}\sin {\widetilde{\mathrm{\ψ}}}_{p1}\\ \cos {\widetilde{\mathrm{\ψ}}}_{p1}\\ 0\end{array}\right]=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]C_b^m{C}_n^b{a}_{\mathrm{sun}}^n=\left[\begin{array}{c}{A}_1\\ A_2\\ 0_{1\×3}\end{array}\right]C_b^m{C}_n^b{a}_{\mathrm{sun}}^n=\left[\begin{array}{c}{A}_1{C}_b^m{C}_n^b{a}_{\mathrm{sun}}^n\\ A_2{C}_b^m{C}_n^b{a}_{\mathrm{sun}}^n\\ 0\end{array}\right]---\left(2\right)$

(4)采集偏振光传感器输出的偏振方位角ψ_pl，在偏振光传感器坐标系下，计算入射光的最大偏振方向矢量的测量值： $\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{p1}\\ \cos {\mathrm{\ψ}}_{p1}\\ 0\end{array}\right];$

(5)计算航向误差校正矢量，入射光最大偏振方向矢量的观测值与测量值的偏差即为航向角误差，其值为两个矢量的叉乘：

$e_{\mathrm{\ψ}}=\left[\begin{array}{c}\sin {\widetilde{\mathrm{\ψ}}}_{p1}\\ \cos {\widetilde{\mathrm{\ψ}}}_{p1}\\ 0\end{array}\right]\×\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{p1}\\ \cos {\mathrm{\ψ}}_{p1}\\ 0\end{array}\right]---\left(3\right)$

(6)采集三轴加速度数据g_b和三轴陀螺仪数据ω_b，去掉加速度计测量值中的向心加速度，得到重力加速度矢量参考值；

$g_r=g_b+{\mathrm{\ω}}_{\mathrm{en}}^n\×V---\left(4\right)$

其中速度V由GPS得到，由V经过公式计算得到。

(7)计算俯仰、滚转误差校正矢量，即重力加速度的观测值与实际测量参考值g_r单位矢量的差值，其值为两矢量的叉乘：

$e_{\mathrm{\φ\θ}}=\left[\begin{array}{c}{C}_{13}\\ C_{23}\\ C_{33}\end{array}\right]\×\frac{g_r}{\left|g_r\right|}---\left(5\right)$

(8)进行互补滤波，通过反馈控制校正陀螺仪测量角速率，从而提高航向角、俯仰角、滚转角的精度；

ω＝ω_b+k_ψPe_ψ+k_ψIdtΣe_ψ+k_φθPe_φθ+k_φθIdtΣe_φθ  (6)

(9)通过四元数法更新，得到新的姿态矩阵和姿态角；

(10)重复(1)至(9)过程，实现系统实时输出飞行器的航姿信息。