CN104374388A

CN104374388A - Flight attitude determining method based on polarized light sensor

Info

Publication number: CN104374388A
Application number: CN201410628075.4A
Authority: CN
Inventors: 金仁成; 华宗治; 芮杨; 陈文�; 褚金奎; 孙会生
Original assignee: Dalian University of Technology
Current assignee: Dalian University of Technology
Priority date: 2014-11-10
Filing date: 2014-11-10
Publication date: 2015-02-25
Anticipated expiration: 2034-11-10
Also published as: CN104374388B

Abstract

The invention discloses a method for measuring heading attitude based on a polarized light sensor. The equipment used includes a three-axis gyroscope, a three-axis accelerometer, a polarized light sensor, a GPS and a flight control computer. Complementary filter algorithm is used to fuse the data of each sensor, the pitch angle and roll angle error of the gyroscope angular rate is corrected by the accelerometer measurement data, and the heading angle error of the gyroscope angular rate is corrected by the polarized light sensor measurement data, so as to improve the attitude measurement of the aircraft precision. Compared with the traditional attitude reference system, the present invention has the advantages of being free from electromagnetic interference and having high measurement accuracy of static and dynamic environments.

Description

A Heading and Attitude Measurement Method Based on Polarized Light Sensor

技术领域technical field

本发明属于飞行器姿态测量与估计技术领域，涉及一种基于偏振光传感器的航姿测定方法。The invention belongs to the technical field of aircraft attitude measurement and estimation, and relates to a method for measuring attitude of an aircraft based on a polarized light sensor.

背景技术Background technique

航姿参考系统(AHRS)能够为飞行器提供航向角、滚转角和俯仰角信息。它一般由多个轴向传感器组成，目前主要有两种组合：一种由三轴陀螺仪、三轴加速度计和三轴磁强计组成，另一种由三轴陀螺仪、三轴加速度计和GPS组成。但是以上两种方法的测量均存在各自的缺点：第一种组合的磁强计容易受到周围磁场和其他机载电子设备的影响，从而导致航向误差增大；第二种方法的GPS在静态时不能提供航向角，高机动时易丢星，同样将导致航向误差的增大。为了弥补以上缺点，本发明加入了偏振光传感器，提出基于偏振光传感器的航姿测定方法。Attitude Heading Reference System (AHRS) can provide heading angle, roll angle and pitch angle information for the aircraft. It is generally composed of multiple axial sensors. At present, there are two main combinations: one is composed of a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer, and the other is composed of a three-axis gyroscope and a three-axis accelerometer. And GPS composition. However, the above two methods of measurement have their own shortcomings: the magnetometer of the first combination is easily affected by the surrounding magnetic field and other airborne electronic equipment, resulting in an increase in heading error; It cannot provide the heading angle, and it is easy to lose the star during high maneuvering, which will also lead to an increase in heading error. In order to make up for the above shortcomings, the present invention adds a polarized light sensor, and proposes a heading attitude measurement method based on the polarized light sensor.

发明内容Contents of the invention

本发明旨在采用偏振光传感器测量的偏振光方位角去修正陀螺仪的测量数据，提高姿态捷联矩阵的精确性，已达到提高姿态测量精度的目的。The invention aims to use the polarized light azimuth angle measured by the polarized light sensor to correct the measurement data of the gyroscope, improve the accuracy of the attitude strapdown matrix, and achieve the purpose of improving the attitude measurement accuracy.

本发明采用如下技术方案：The present invention adopts following technical scheme:

一种基于偏振光传感器的航姿测定方法，采用的设备包括三轴陀螺仪、三轴加速度计、偏振光传感器、GPS和飞控计算机。三轴陀螺仪测量飞行器三轴角速率，三轴加速度计测量飞行器的三轴加速度。偏振光传感器测量偏振光方位角。GPS提供当地时间、飞行器所在位置、速度信息。飞控计算机需要实时处理各种传感器传回的数据，还要将处理的结果发送给飞行器的控制单元，以实现对飞行器的机构控制，同时也承担将数据发送到地面站和接受地面控制指令的任务，因此必须考虑信息处理的实时性和数据融合算法的精简程度。通过加速度计测量的数据校正陀螺仪角速率的俯仰角、滚转角误差，偏振光传感器测量数据修正陀螺仪角速率的航向角误差，提高飞行器的姿态测量精度。The invention discloses a method for measuring heading and attitude based on a polarized light sensor. The equipment used includes a three-axis gyroscope, a three-axis accelerometer, a polarized light sensor, a GPS and a flight control computer. The three-axis gyroscope measures the three-axis angular rate of the aircraft, and the three-axis accelerometer measures the three-axis acceleration of the aircraft. The polarized light sensor measures the azimuth angle of polarized light. GPS provides local time, aircraft location, and speed information. The flight control computer needs to process the data returned by various sensors in real time, and also send the processed results to the control unit of the aircraft to realize the mechanism control of the aircraft, and also undertake the responsibility of sending data to the ground station and receiving ground control commands. Therefore, the real-time information processing and the simplification of the data fusion algorithm must be considered. The pitch angle and roll angle errors of the gyroscope angular rate are corrected by the data measured by the accelerometer, and the heading angle error of the gyroscope angular rate is corrected by the data measured by the polarized light sensor, so as to improve the attitude measurement accuracy of the aircraft.

该方法具体步骤如下：The specific steps of the method are as follows:

(1)采集三轴加速度计、偏振光传感器和GPS的输出数据，确定飞行器的初始滚转角φ、俯仰角θ和航向角ψ，建立导航坐标系到机体坐标系的初始姿态矩阵和机体坐标系到偏振光传感器坐标系的姿态矩阵 (1) Collect the output data of the three-axis accelerometer, polarized light sensor and GPS, determine the initial roll angle φ, pitch angle θ and heading angle ψ of the aircraft, and establish the initial attitude matrix from the navigation coordinate system to the body coordinate system and the attitude matrix from the body coordinate system to the polarization sensor coordinate system

(2)根据飞行器所属时区，通过天文历计算方法估算当地太阳高度角h_s、方位角A_s，然后计算得到太阳方向矢量在导航坐标系下的投影 (2) According to the time zone of the aircraft, estimate the local solar altitude h _s and azimuth A _s through the calculation method of the astronomical calendar, and then calculate the projection of the sun direction vector in the navigation coordinate system

${a a}_{sun the sun}^{n no} = = [[cos cos ((hs hs)) sin sin (({A A}_{s the s})) cos cos ((hs hs)) cos cos (({A A}_{s the s})) sin sin ((hs hs))]] - - - - - - ((11))$

(3)根据偏振光传感器坐标系、机体坐标系、导航坐标系之间关系和瑞利散射原理得到偏振光传感器坐标系下的入射光最大偏振方向矢量的观测值：(3) According to the relationship between the polarized light sensor coordinate system, the body coordinate system, the navigation coordinate system and the Rayleigh scattering principle, the observed value of the maximum polarization direction vector of the incident light in the polarized light sensor coordinate system is obtained:

$[\begin{matrix} sin sin {\overset{~ ~}{ψ ψ}}_{pl pl} \\ cos cos {\overset{~ ~}{ψ ψ}}_{pl pl} \\ 00 \end{matrix}] = = [\begin{matrix} 00 & - - 11 & 00 \\ 11 & 00 & 00 \\ 00 & 00 & 00 \end{matrix}] {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} = = [\begin{matrix} {A A}_{11} \\ {A A}_{22} \\ 00_{11 \times \times 33} \end{matrix}] {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} = = [\begin{matrix} {A A}_{11} {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} \\ {A A}_{22} {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} \\ 00 \end{matrix}] - - - - - - ((22))$

(4)采集偏振光传感器输出的偏振方位角ψ_pl，在偏振光传感器坐标系下，计算入射光的最大偏振方向矢量的测量值： $[\begin{matrix} \sin ψ_{pl} \\ \cos ψ_{pl} \\ 0 \end{matrix}] .$ (4) Collect the polarization azimuth ψ _pl output by the polarized light sensor, and calculate the measured value of the maximum polarization direction vector of the incident light in the polarized light sensor coordinate system: $[\begin{matrix} \sin ψ_{pl} \\ \cos ψ_{pl} \\ 0 \end{matrix}] .$

(5)计算航向误差校正矢量，入射光最大偏振方向矢量的观测值与测量值的偏差即为航向角误差，其值为两个矢量的叉乘：(5) Calculate the heading error correction vector, the deviation between the observed value and the measured value of the maximum polarization direction vector of the incident light is the heading angle error, and its value is the cross product of two vectors:

${e e}_{ψ ψ} = = [\begin{matrix} sin sin {\overset{~ ~}{ψ ψ}}_{pl pl} \\ cos cos {\overset{~ ~}{ψ ψ}}_{pl pl} \\ 00 \end{matrix}] \times \times [\begin{matrix} sin sin {ψ ψ}_{pl pl} \\ cos cos {ψ ψ}_{pl pl} \\ 00 \end{matrix}] - - - - - - ((33))$

(6)采集三轴加速度数据g_b，去掉加速度计测量值中的向心加速度，得到重力加速度矢量参考值。(6) Collect the triaxial acceleration data g _b , remove the centripetal acceleration in the measured value of the accelerometer, and obtain the reference value of the gravitational acceleration vector.

${g g}_{r r} = = {g g}_{b b} + + {ω ω}_{en en}^{n no} \times \times V V - - - - - - ((44))$

其中速度V由GPS得到，由V经过公式计算得到。where the velocity V is obtained by GPS, Calculated by V through the formula.

(7)计算俯仰、滚转误差校正矢量，即重力加速度的观测值(的第三列)与实际测量参考值g_r单位矢量的差值，其值为两矢量的叉乘：(7) Calculate the pitch and roll error correction vectors, that is, the observed value of the acceleration of gravity ( The difference between the third column of ) and the actual measurement reference value g _r unit vector, its value is the cross product of the two vectors:

${e e}_{φθ φθ} = = [\begin{matrix} {C C}_{1313} \\ {C C}_{23 twenty three} \\ {C C}_{3333} \end{matrix}] \times \times \frac{{g g}_{r r}}{| | {g g}_{r r} | |} - - - - - - ((55))$

(8)进行互补滤波，通过反馈控制校正陀螺仪测量角速率，从而提高航向角、俯仰角、滚转角的精度。(8) Perform complementary filtering, and correct the angular rate measured by the gyroscope through feedback control, thereby improving the accuracy of heading angle, pitch angle, and roll angle.

ω＝ω_b+k_ψPe_ψ+k_ψIdtΣe_ψ+k_φθPe_φθ+k_φθIdtΣe_φθ (6)ω＝ω _b +k _ψP e _ψ +k _ψI dtΣe _ψ +k _φθP e _φθ +k _φθI dtΣe _φθ (6)

根据加速度计和偏振光传感器的响应时间对其采用不同的滤波系数，其中，k_ψP、k_φθP的大小决定了互补滤波器的截止频率，k_ψI、k_φθI的大小决定了消除静态偏差的时间。Different filter coefficients are used according to the response time of the accelerometer and polarized light sensor, among which, the size of k _ψP and k _φθP determines the cut-off frequency of the complementary filter, and the size of k _ψI and k _φθI determines the time to eliminate static deviation .

(9)通过四元数法更新，得到新的姿态矩阵和姿态角。(9) Update by quaternion method to get a new attitude matrix and attitude angle.

(10)重复(1)至(9)过程，实现系统实时输出飞行器的航姿信息。(10) Repeat the process from (1) to (9) to realize the real-time output of the attitude information of the aircraft by the system.

本发明的有益效果是：The beneficial effects of the present invention are:

1、相比于三轴陀螺仪、三轴加速度计和三轴磁强计组合，本发明不受周围环境地磁和机载设备的电磁干扰；相比于三轴陀螺仪，三轴加速度计和GPS组合,本发明不受飞行器运动状态的影响。1. Compared with the combination of a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer, the present invention is free from the electromagnetic interference of the surrounding geomagnetism and airborne equipment; compared with a three-axis gyroscope, the three-axis accelerometer and Combined with GPS, the present invention is not affected by the motion state of the aircraft.

2、本发明采用互补滤波器算法解算飞行器姿态，相比于扩展卡尔曼滤波器，不需要偏振光传感器的精确误差模型，而且计算量小，能够实现长时间稳定地输出高精度姿态数据，尤其适合微型飞控系统。2. The present invention uses a complementary filter algorithm to solve the attitude of the aircraft. Compared with the extended Kalman filter, the precise error model of the polarization sensor is not required, and the calculation amount is small, and it can output high-precision attitude data stably for a long time. Especially suitable for miniature flight control system.

附图说明Description of drawings

图1为本发明的主要坐标系图。Fig. 1 is the main coordinate system figure of the present invention.

图2为本发明的航姿测定方法工作流程图。Fig. 2 is a working flow chart of the attitude determination method of the present invention.

图3为本发明的原理框图。Fig. 3 is a functional block diagram of the present invention.

具体实施方式Detailed ways

下面结合附图和具体技术方案对本发明的具体实施方式作进一步阐述。The specific embodiments of the present invention will be further elaborated below in conjunction with the accompanying drawings and specific technical solutions.

如图1所示，本发明涉及的坐标系有：地平坐标系，导航坐标系，机体坐标系，偏振光传感器坐标系。其中导航坐标系选取东北天坐标系，同时为了减少坐标系之间的转换，选取地平坐标系与导航坐标系重合，为东北天坐标系，偏振光传感器坐标系与机体坐标系重合。设偏振光传感器坐标系的Y轴为其体轴，则偏振光传感器测量的偏振方位角ψ_pl为入射光的最大偏振方向矢量在偏振光传感器坐标系的OXY平面的投影与Y轴的夹角。As shown in FIG. 1 , the coordinate systems involved in the present invention include: horizon coordinate system, navigation coordinate system, body coordinate system, and polarized light sensor coordinate system. The navigation coordinate system is the northeast sky coordinate system, and in order to reduce the conversion between coordinate systems, the horizon coordinate system is selected to coincide with the navigation coordinate system, which is the northeast sky coordinate system, and the polarization sensor coordinate system coincides with the body coordinate system. Let the Y axis of the polarized light sensor coordinate system be its body axis, then the polarization azimuth ψ _pl measured by the polarized light sensor is the angle between the projection of the maximum polarization direction vector of the incident light on the OXY plane of the polarized light sensor coordinate system and the Y axis .

在本发明的航姿参考系统中，三轴陀螺仪能够测量无人机机体的三轴角速度矢量，可根据它的测量信息解算出无人机的俯仰角、滚转角、航向角信息，短时间测量精度高，但长时间测量精度会受到温度漂移的影响；三轴加速度传感器能够测量无人机机体的三轴加速度矢量，可根据它的测量信息解算出无人机的俯仰角、滚转角信息，长时间测量精度高，但短时间测量精度会受到机体振动的影响；偏振光传感器能够测量入射光方向的偏振方位角，没有误差积累，长时间测量精度高，但短时间测量精度不如陀螺仪。由以上分析可知，陀螺仪与加速度计、偏振光存在测量相同的量，并且在频域特性上互补，所以采互补滤波器算法解算飞行器的姿态。In the attitude reference system of the present invention, the three-axis gyroscope can measure the three-axis angular velocity vector of the UAV body, and can calculate the pitch angle, roll angle, and heading angle information of the UAV according to its measurement information. The measurement accuracy is high, but the long-term measurement accuracy will be affected by temperature drift; the three-axis acceleration sensor can measure the three-axis acceleration vector of the drone body, and can calculate the pitch angle and roll angle information of the drone based on its measurement information , the long-term measurement accuracy is high, but the short-term measurement accuracy will be affected by the vibration of the body; the polarized light sensor can measure the polarization azimuth angle of the incident light direction without error accumulation, and the long-term measurement accuracy is high, but the short-term measurement accuracy is not as good as the gyroscope . From the above analysis, it can be seen that the gyroscope, the accelerometer and the polarized light measure the same quantity, and they are complementary in frequency domain characteristics, so the complementary filter algorithm is used to solve the attitude of the aircraft.

结合图2和图3，以下是该方法的具体步骤：Combining Figure 2 and Figure 3, the following are the specific steps of the method:

1、采集三轴加速度计、偏振光传感器和GPS输出数据，确定飞行器的初始滚转角φ、俯仰角θ和航向角ψ。建立导航坐标系到机体坐标系的初始姿态矩阵和机体坐标系到偏振光传感器坐标系的姿态矩阵 1. Collect the output data of the three-axis accelerometer, polarized light sensor and GPS, and determine the initial roll angle φ, pitch angle θ and heading angle ψ of the aircraft. Establish the initial attitude matrix from the navigation coordinate system to the body coordinate system and the attitude matrix from the body coordinate system to the polarization sensor coordinate system

飞行器起飞之前一般为静止状态，加速度计测量值可认为只有重力加速度，不存在运动加速度，相当于重力加速度在飞行器机体三轴的投影，由于重力始终与航向平面始终垂直，所以不能得到航向角，可以解算得到飞行器的初始俯仰角、滚转角：The aircraft is generally in a static state before take-off, and the accelerometer measurement value can be regarded as only the acceleration of gravity, and there is no acceleration of motion, which is equivalent to the projection of the acceleration of gravity on the three axes of the aircraft body. Since the gravity is always perpendicular to the heading plane, the heading angle cannot be obtained. The initial pitch angle and roll angle of the aircraft can be obtained by solving:

$\begin{matrix} θ θ = = {tan the tan}^{- - 11} \frac{((- - {g g}_{mx mx}))}{\sqrt{{g g}_{my my}^{22} + + {g g}_{mz mz}^{22}}} \\ φ φ = = {tan the tan}^{- - 11} \frac{{g g}_{my my}}{{g g}_{mz mz}} \end{matrix} - - - - - - ((77))$

根据瑞利散射原理，入射光的最大偏振光矢量与太阳子午线垂直，可以解算得到飞行器的初始航向角：According to the principle of Rayleigh scattering, the maximum polarized light vector of the incident light is perpendicular to the sun meridian, and the initial heading angle of the aircraft can be calculated as:

$ψ ψ = = {ψ ψ}_{pl pl} + + {A A}_{s the s} &PlusMinus; &PlusMinus; \frac{π π}{22} - - - - - - ((88))$

其中，A_s为太阳方位角，根据GPS输出的飞行器位置和当地时间通过天文历计算方法估算得到。方位角采用在大地测量中的方位，方位角以正北方为起算点，按顺时针方向度量，取值范围为0°～360°，这种定义与一般偏振光文献中定义的天文测量中的方位值有所区别。Among them, A _s is the azimuth of the sun, which is estimated by the calculation method of the astronomical calendar according to the position of the aircraft output by the GPS and the local time. The azimuth adopts the azimuth in geodetic surveying. The azimuth takes true north as the starting point and is measured in a clockwise direction. The value range is 0° to 360°. Azimuth values vary.

导航坐标系到机体坐标系的初始姿态矩阵为：The initial attitude matrix from the navigation coordinate system to the body coordinate system for:

$\begin{matrix} {C C}_{n no}^{b b} = = [\begin{matrix} cos cos φ φ cos cos ψ ψ + + sin sin φ φ sin sin θ θ sin sin ψ ψ & - - cos cos φ φ sin sin ψ ψ + + sin sin φ φ sin sin θco sψ θco sψ & - - sin sin φ φ cos cos θ θ \\ cos cos θ θ sin sin ψ ψ & cos cos θ θ cos cos ψ ψ & sin sin θ θ \\ sin sin φ φ cos cos ψ ψ - - cos cos φ φ sin sin θ θ sin sin ψ ψ & - - sin sin φ φ sin sin ψ ψ - - cos cos φ φ sin sin θ θ cos cos ψ ψ & cos cos φ φ cos cos θ θ \end{matrix}] \\ = = [\begin{matrix} {q q}_{00}^{22} + + {q q}_{11}^{22} - - {q q}_{22}^{22} - - {q q}_{33}^{22} & 22 (({q q}_{11} {q q}_{22} - - {q q}_{00} {q q}_{33})) & 22 (({q q}_{11} {q q}_{33} + + {q q}_{00} {q q}_{22})) \\ 22 (({q q}_{11} {q q}_{22} + + {q q}_{00} {q q}_{33})) & {q q}_{00}^{22} - - {q q}_{11}^{22} + + {q q}_{22}^{22} - - {q q}_{33}^{22} & 22 (({q q}_{22} {q q}_{33} - - {q q}_{00} {q q}_{11})) \\ 22 (({q q}_{11} {q q}_{33} - - {q q}_{00} {q q}_{22})) & 22 (({q q}_{22} {q q}_{33} + + {q q}_{00} {q q}_{11})) & {q q}_{00}^{22} - - {q q}_{11}^{22} - - {q q}_{22}^{22} + + {q q}_{33}^{22} \end{matrix}] \\ = = [\begin{matrix} {C C}_{1111} & {C C}_{1212} & {C C}_{1313} \\ {C C}_{21 twenty one} & {C C}_{22 twenty two} & {C C}_{23 twenty three} \\ {C C}_{3131} & {C C}_{3232} & {C C}_{3333} \end{matrix}] \end{matrix} - - - - - - ((99))$

机体坐标系到偏振光传感器坐标系的姿态矩阵为：Attitude matrix from the body coordinate system to the polarization sensor coordinate system for:

${C C}_{b b}^{m m} = = [\begin{matrix} 11 & 00 & 00 \\ 00 & 11 & 00 \\ 00 & 00 & 11 \end{matrix}] - - - - - - ((1010))$

2、采集GPS数据，根据GPS输出的飞行器位置和当地时间通过天文历计算方法估算得到太阳高度角h_s和太阳方位角A_s,然后计算得到太阳方向矢量在导航坐标系下的投影 2. Collect GPS data, estimate the sun altitude angle h _s and sun azimuth angle A _s through the almanac calculation method according to the aircraft position and local time output by GPS, and then calculate the projection of the sun direction vector in the navigation coordinate system

${a a}_{sun the sun}^{n no} = = [[cos cos ((hs hs)) sin sin (({A A}_{s the s})) cos cos ((hs hs)) cos cos (({A A}_{s the s})) sin sin ((hs hs))]] - - - - - - ((1111))$

3.根据偏振光传感器坐标系、机体坐标系、导航坐标系之间关系和瑞利散射原理得到偏振光传感器坐标系下的入射光最大偏振方向矢量的观测值。3. According to the relationship between the polarized light sensor coordinate system, the body coordinate system, the navigation coordinate system and the principle of Rayleigh scattering, the observed value of the maximum polarization direction vector of the incident light in the polarized light sensor coordinate system is obtained.

在偏振光传感器坐标系下，根据瑞利散射原理，入射光的最大偏振方向矢量垂直于观测方向矢量与太阳方向矢量所在的平面，可表示为：In the coordinate system of the polarized light sensor, according to the principle of Rayleigh scattering, the maximum polarization direction vector of the incident light is perpendicular to the plane where the observation direction vector and the sun direction vector are located, which can be expressed as:

$[\begin{matrix} sin sin {\overset{~ ~}{ψ ψ}}_{pl pl} \\ cos cos {\overset{~ ~}{ψ ψ}}_{pl pl} \\ 00 \end{matrix}] = = [\begin{matrix} 00 & - - 11 & 00 \\ 11 & 00 & 00 \\ 00 & 00 & 00 \end{matrix}] {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} = = [\begin{matrix} {A A}_{11} \\ {A A}_{22} \\ 00_{11 \times \times 33} \end{matrix}] {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} = = [\begin{matrix} {A A}_{11} {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} \\ {A A}_{22} {C C}_{b b}^{m m} {C C}_{n no}^{b b} {a a}_{sun the sun}^{n no} \\ 00 \end{matrix}] - - - - - - ((1212))$

4、采集偏振光传感器输出的偏振方位角ψ_pl，在偏振光传感器坐标系下，计算入射光的最大偏振方向矢量的测量值： $[\begin{matrix} \sin ψ_{pl} \\ \cos ψ_{pl} \\ 0 \end{matrix}] .$ 4. Collect the polarization azimuth ψ _pl output by the polarization sensor, and calculate the measured value of the maximum polarization direction vector of the incident light in the coordinate system of the polarization sensor: $[\begin{matrix} \sin ψ_{pl} \\ \cos ψ_{pl} \\ 0 \end{matrix}] .$

5、计算航向角误差校正矢量，入射光最大偏振方向矢量的观测值与测量值的偏差即为航向误差，其值近似为两个矢量的叉乘：5. Calculate the heading angle error correction vector. The deviation between the observed value and the measured value of the maximum polarization direction vector of the incident light is the heading error, and its value is approximately the cross product of two vectors:

${e e}_{ψ ψ} = = [\begin{matrix} sin sin {\overset{~ ~}{ψ ψ}}_{pl pl} \\ cos cos {\overset{~ ~}{ψ ψ}}_{pl pl} \\ 00 \end{matrix}] \times \times [\begin{matrix} sin sin {ψ ψ}_{pl pl} \\ cos cos {ψ ψ}_{pl pl} \\ 00 \end{matrix}] - - - - - - ((1313))$

${g g}_{r r} = = {g g}_{b b} + + {ω ω}_{en en}^{n no} \times \times V V - - - - - - ((1414))$

7、计算俯仰角、滚转角误差校正矢量，重力加速度的观测值(的第三列)与实际测量的参考值即为俯仰、滚转误差，其值近似为两矢量的叉乘：7. Calculate the pitch angle, roll angle error correction vector, and the observed value of the acceleration of gravity ( The third column of ) and the actual measured reference value are the pitch and roll errors, and their values are approximately the cross product of the two vectors:

${e e}_{φθ φθ} = = [\begin{matrix} {C C}_{1313} \\ {C C}_{23 twenty three} \\ {C C}_{3333} \end{matrix}] \times \times \frac{{g g}_{r r}}{| | {g g}_{r r} | |} - - - - - - ((1515))$

8、进行互补滤波，通过反馈控制校正陀螺仪测量角速率，从而提高航向角、俯仰角、滚转角的精度。8. Perform complementary filtering, and correct the angular rate measured by the gyroscope through feedback control, thereby improving the accuracy of heading angle, pitch angle, and roll angle.

ω＝ω_b+k_ψPe_ψ+k_ψIdtΣe_ψ+k_φθPe_φθ+k_φθIdtΣe_φθ (16)ω＝ω _b +k _ψP e _ψ +k _ψI dtΣe _ψ +k _φθP e _φθ +k _φθI dtΣe _φθ (16)

9、通过四元数法更新，得到新的姿态矩阵和姿态角。9. Update through the quaternion method to get a new attitude matrix and attitude angle.

将修正后的角速率ω带入四元数微分方程，使用四阶龙格库塔法解算得到新的四元数(q₀q₁q₂q₃)。Bring the corrected angular velocity ω into the quaternion differential equation, and use the fourth-order Runge-Kutta method to solve the new quaternion (q ₀ q ₁ q ₂ q ₃ ).

$[\begin{matrix} {\overset{. .}{q q}}_{00} \\ {\overset{. .}{q q}}_{11} \\ {\overset{. .}{q q}}_{22} \\ {\overset{. .}{q q}}_{33} \end{matrix}] = = \frac{11}{22} [\begin{matrix} 00 & - - {ω ω}_{x x} & - - {ω ω}_{y the y} & - - {ω ω}_{z z} \\ {ω ω}_{x x} & 00 & {ω ω}_{z z} & - - {ω ω}_{y the y} \\ {ω ω}_{y the y} & - - {ω ω}_{z z} & 00 & {ω ω}_{x x} \\ {ω ω}_{z z} & {ω ω}_{y the y} & - - {ω ω}_{x x} & 00 \end{matrix}] [\begin{matrix} {q q}_{00} \\ {q q}_{11} \\ {q q}_{22} \\ {q q}_{33} \end{matrix}] - - - - - - ((1717))$

将新的四元数q₀q₁q₂代入公式9更新姿态矩阵 Substitute the new quaternion q ₀ q ₁ q ₂ into formula 9 to update the attitude matrix

由解算出飞行器的姿态角：Depend on Solve the attitude angle of the aircraft:

$\begin{matrix} ψ ψ = = {tan the tan}^{- - 11} ((\frac{{C C}_{21 twenty one}}{{C C}_{22 twenty two}})) \\ φ φ = = {tan the tan}^{- - 11} ((- - \frac{{C C}_{1313}}{{C C}_{3333}})) \\ θ θ = = {sin sin}^{- - 11} (({C C}_{23 twenty three})) \end{matrix} - - - - - - ((1818))$

10、重复步骤1至步骤9过程，实现系统实时输出飞行器的姿态信息。10. Repeat the process from step 1 to step 9 to realize the real-time output of the attitude information of the aircraft by the system.

Claims

1., based on a boat appearance assay method for polarized light sensor, it is characterized in that following steps,

(1) gather the output data of three axis accelerometer, polarized light sensor and GPS, determine the initial roll angle φ of aircraft, pitching angle theta and course angle ψ, set up the initial attitude matrix that navigation coordinate is tied to body axis system with the attitude matrix of body axis system to polarized light sensor coordinate system

(2) time zone belonging to aircraft, estimates local sun altitude h by astronomical ephemeris computing method _s, position angle A _s, then calculate the projection of solar direction vector under navigational coordinate system according to following formula (1)

a_{sun}^{n} = [\cos (hs) \sin (A_{s}) \cos (hs) \cos (A_{s}) \sin (hs)] - - - (1)

(3) observed reading of the incident light maximum polarization direction vector under polarized light sensor coordinate system is obtained according to relation and Rayleigh scattering principle between the middle polarized light sensor coordinate system of following formula (2), body axis system, navigational coordinate system:

[\begin{matrix} \sin {\tilde{ψ}}_{p 1} \\ \cos {\tilde{ψ}}_{p 1} \\ 0 \end{matrix}] = [\begin{matrix} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] C_{b}^{m} C_{n}^{b} a_{sun}^{n} = [\begin{matrix} A_{1} \\ A_{2} \\ 0_{1 \times 3} \end{matrix}] C_{b}^{m} C_{n}^{b} a_{sun}^{n} = [\begin{matrix} A_{1} C_{b}^{m} C_{n}^{b} a_{sun}^{n} \\ A_{2} C_{b}^{m} C_{n}^{b} a_{sun}^{n} \\ 0 \end{matrix}] - - - (2)

(4) the polarization azimuth ψ that polarized light sensor exports is gathered _pl, under polarized light sensor coordinate system, calculate the measured value of the maximum polarization direction vector of incident light:

[\begin{matrix} \sin ψ_{p 1} \\ \cos ψ_{p 1} \\ 0 \end{matrix}];

(5) calculate course error correcting vector, the observed reading of incident light maximum polarization direction vector and the deviation of measured value are course angle error, and its value is the multiplication cross of two vectors:

e_{ψ} = [\begin{matrix} \sin {\tilde{ψ}}_{p 1} \\ \cos {\tilde{ψ}}_{p 1} \\ 0 \end{matrix}] \times [\begin{matrix} \sin ψ_{p 1} \\ \cos ψ_{p 1} \\ 0 \end{matrix}] - - - (3)

(6) 3-axis acceleration data g is gathered _bwith three-axis gyroscope data ω _b, remove the centripetal acceleration in acceleration measuring value, obtain acceleration of gravity vector reference value;

g_{r} = g_{b} + ω_{en}^{n} \times V - - - (4)

Its medium velocity V is obtained by GPS, obtained through formulae discovery by V.

(7) pitching, roll error correcting vector is calculated, i.e. the observed reading of acceleration of gravity and actual measurement reference value g _rthe difference of unit vector, its value is the multiplication cross of two vectors:

e_{φθ} = [\begin{matrix} C_{13} \\ C_{23} \\ C_{33} \end{matrix}] \times \frac{g_{r}}{| g_{r} |} - - - (5)

(8) carry out complementary filter, correct gyroscope survey angular speed by FEEDBACK CONTROL, thus improve the precision of course angle, the angle of pitch, roll angle;

ω＝ω _b+k _ψPe _ψ+k _ψIdtΣe _ψ+k _φθPe _φθ+k _φθIdtΣe _φθ(6)

(9) upgraded by Quaternion Method, obtain new attitude matrix and attitude angle;

(10) repeat (1) to (9) process, the system that realizes exports the boat appearance information of aircraft in real time.