CN104374388A - Flight attitude determining method based on polarized light sensor - Google Patents

Abstract

The invention discloses a flight attitude determining method based on a polarized light sensor. According to the invention, adopted equipment comprises a three-axis gyroscope, a three-axis accelerometer, the polarized light sensor, a GPS and a flight control computer. A complementary filter algorithm is used to fuse sensor data; pitch angle and rolling angle errors of the angular speed of the gyroscope are revised through measured data of the accelerometer; and an course angle error of the angular speed of the gyroscope is revised through measured data of the polarized light sensor, so that the attitude measurement precision of an aircraft is improved. Compared with a traditional flight attitude reference system, the flight attitude determining method has the advantages that the flight attitude determining method is not interfered by electromagnetism, the measurement precision of static and dynamic environments is high and the like.

Description

A kind of boat appearance assay method based on polarized light sensor

Technical field

The invention belongs to aircraft attitude measurement and estimation technique field, relate to a kind of boat appearance assay method based on polarized light sensor.

Background technology

Attitude heading reference system (AHRS) can provide course angle, roll angle and angle of pitch information for aircraft.It is generally made up of multiple axial sensor, mainly contains two kinds of combinations at present: one is made up of three-axis gyroscope, three axis accelerometer and three axis magnetometer, and another kind is made up of three-axis gyroscope, three axis accelerometer and GPS.But all there is respective shortcoming in the measurement of above two kinds of methods: the magnetometer of the first combination is easily subject to the impact of surrounding magnetic field and other air environments, thus causes course error to increase; The GPS of second method can not provide course angle when static state, high motor-driven time easily lose star, will the increase of course error be caused equally.In order to make up above shortcoming, present invention adds polarized light sensor, proposing the boat appearance assay method based on polarized light sensor.

Summary of the invention

The polarized light position angle that the present invention is intended to adopt polarized light sensor to measure is gone to revise gyrostatic measurement data, improves the accuracy of attitude strap-down matrix, has reached the object improving attitude measurement accuracy.

The present invention adopts following technical scheme:

Based on a boat appearance assay method for polarized light sensor, the equipment of employing comprises three-axis gyroscope, three axis accelerometer, polarized light sensor, GPS and flight control computer.Three-axis gyroscope measures aircraft tri-axis angular rate, and the 3-axis acceleration of aircraft measured by three axis accelerometer.Polarized light sensor measures polarized light position angle.GPS provides local time, aircraft position, velocity information.The data that flight control computer needs the various sensor of process in real time to pass back, also the result of process to be sent to the control module of aircraft, to realize the mechanism controls to aircraft, also bear data being sent to land station and accepting ground steering order of task, that therefore must consider the real-time of information processing and data anastomosing algorithm simplifies degree simultaneously.By the angle of pitch, the roll angle error of the Data correction gyroscope angular speed of accelerometer measures, the course angle error of polarized light sensor measurement data correction gyroscope angular speed, improves the attitude measurement accuracy of aircraft.

The method concrete steps are as follows:

(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

$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) 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 polarized light sensor coordinate system, body axis system, navigational coordinate system:

$\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) 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: $\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{\mathrm{pl}}\\ \cos {\mathrm{\ψ}}_{\mathrm{pl}}\\ 0\end{array}\right].$

(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_{\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) 3-axis acceleration data g is gathered _b, remove the centripetal acceleration in acceleration measuring value, obtain acceleration of gravity vector reference value.

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

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

(7) calculate pitching, roll error correcting vector, namely acceleration of gravity observed reading ( the 3rd row) with actual measurement reference value g _rthe difference of unit vector, its value is the multiplication cross of two vectors:

$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) 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)

Response time according to accelerometer and polarized light sensor adopts different filter factors to it, wherein, and k _{ψ P}, k _{φ θ P}size determine the cutoff frequency of complementary filter, k _{ψ I}, k _{φ θ I}size determine and eliminate time of static deviation.

(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.

The invention has the beneficial effects as follows:

1, compared to the combination of three-axis gyroscope, three axis accelerometer and three axis magnetometer, the present invention is not subject to the electromagnetic interference (EMI) of surrounding environment earth magnetism and airborne equipment; Compared to three-axis gyroscope, three axis accelerometer and GPS combination, the present invention is not by the impact of aircraft movements state.

2, the present invention adopts complementary filter algorithm to resolve attitude of flight vehicle, compared to extended Kalman filter, do not need the Accuracy Error model of polarized light sensor, and calculated amount is little, can realize exporting high-precision attitude data long-term and stably, especially be applicable to miniature flight control system.

Accompanying drawing explanation

Fig. 1 is main coordinate system figure of the present invention.

Fig. 2 is boat appearance assay method workflow diagram of the present invention.

Fig. 3 is theory diagram of the present invention.

Embodiment

Below in conjunction with accompanying drawing and concrete technical scheme, the specific embodiment of the present invention is further elaborated.

As shown in Figure 1, the coordinate system that the present invention relates to has: the horizontal system of coordinates, navigational coordinate system, body axis system, polarized light sensor coordinate system.Wherein navigational coordinate system chooses sky, northeast coordinate system, and simultaneously in order to reduce the conversion between coordinate system, choose the horizontal system of coordinates and overlap with navigational coordinate system, be sky, northeast coordinate system, polarized light sensor coordinate system overlaps with body axis system.If the Y-axis of polarized light sensor coordinate system is its body axle, then the polarization azimuth ψ of polarized light sensor measurement _plfor the maximum polarization direction vector of incident light is at the projection of OXY plane of polarized light sensor coordinate system and the angle of Y-axis.

In attitude heading reference system of the present invention, three-axis gyroscope can measure three axis angular rate vectors of unmanned plane body, the angle of pitch, roll angle, the course angle information of unmanned plane can be calculated according to its metrical information, short time measurement precision is high, but long-time measuring accuracy can be subject to the impact of temperature drift; 3-axis acceleration sensor can measure the 3-axis acceleration vector of unmanned plane body, and can calculate the angle of pitch, the roll angle information of unmanned plane according to its metrical information, long-time measuring accuracy is high, but short time measurement precision can be subject to the impact of body vibration; Polarized light sensor can measure the polarization azimuth in incident light direction, does not have the accumulation of error, and long-time measuring accuracy is high, but short time measurement precision is not as gyroscope.From analyzing above, gyroscope exists with accelerometer, polarized light measures identical amount, and complementary on frequency domain characteristic, so adopt the attitude that complementary filter algorithm resolves aircraft.

Composition graphs 2 and Fig. 3 are below the concrete steps of the method:

1, gather three axis accelerometer, polarized light sensor and GPS and export data, 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

Stationary state is generally before aircraft takeoff, acceleration measuring value can think to only have acceleration of gravity, there is not acceleration of motion, be equivalent to the projection of acceleration of gravity at aircraft body three axle, because gravity is vertical all the time with yaw plane all the time, so can not course angle be obtained, the initial pitch angle, the roll angle that obtain aircraft can be resolved:

$\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)$

According to Rayleigh scattering principle, the maximum Polarization Vector of incident light is vertical with sun meridian, can resolve the angle, initial heading obtaining aircraft:

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

Wherein, A _sfor solar azimuth, the position of aircraft exported according to GPS and local time are obtained by the estimation of astronomical ephemeris computing method.Position angle adopts orientation in geodetic surveying, and position angle is with the positive north for Fixed Initial Point, and measure in the direction of the clock, span is 0 ° ~ 360 °, and the orientation values in the astronomical surveing defined in this definition and general polarized light document is distinguished to some extent.

Navigation coordinate is tied to the initial attitude matrix of body axis system for:

$\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)$

Body axis system is to the attitude matrix of polarized light sensor coordinate system for:

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

2, gather gps data, the position of aircraft exported according to GPS and local time obtain sun altitude h by the estimation of astronomical ephemeris computing method _swith solar azimuth A _s, then calculate the projection of solar direction vector under navigational coordinate system

$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. the 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 polarized light sensor coordinate system, body axis system, navigational coordinate system.

Under polarized light sensor coordinate system, according to Rayleigh scattering principle, the maximum polarization direction vector normal of incident light, in the plane at observed ray vector and solar direction vector place, can be expressed as:

$\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, 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: $\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{\mathrm{pl}}\\ \cos {\mathrm{\ψ}}_{\mathrm{pl}}\\ 0\end{array}\right].$

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

$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) 3-axis acceleration data g is gathered _b, remove the centripetal acceleration in acceleration measuring value, obtain acceleration of gravity vector reference value.

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

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

7, the angle of pitch, roll angle error correction vector is calculated, the observed reading of acceleration of gravity ( the 3rd row) be pitching, roll error with the reference value of actual measurement, its value is approximately the multiplication cross of two vectors:

$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, 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 _φθ(16)

Response time according to accelerometer and polarized light sensor adopts different filter factors to it, wherein, and k _{ψ P}, k _{φ θ P}size determine the cutoff frequency of complementary filter, k _{ψ I}, k _{φ θ I}size determine and eliminate time of static deviation.

9, upgraded by Quaternion Method, obtain new attitude matrix and attitude angle.

Bring revised angular speed ω into quaternion differential equation, use Fourth order Runge-Kutta to resolve and obtain new hypercomplex number (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)$

By new hypercomplex number q ₀q ₁q ₂substitute into formula 9 and upgrade attitude matrix

By calculate the attitude angle of aircraft:

$\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, repeat step 1 to step 9 process, realize the attitude information that system exports aircraft in real time.

Claims (1)

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_{\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) 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:

$\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) 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: $\left[\begin{array}{c}\sin {\mathrm{\ψ}}_{p1}\\ \cos {\mathrm{\ψ}}_{p1}\\ 0\end{array}\right];$

(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_{\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) 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+{\mathrm{\ω}}_{\mathrm{en}}^n\×V---\left(4\right)$

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_{\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) 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.