A polarization fusion orientation method for occluded environments

Abstract

This paper discloses a polarization fusion orientation method for occluded environments. First, a polarization camera is used to capture a polarized image of the sky. The heading angle output by the E-vector polarization orientation method is used as the state variable, and the heading angle output by the fitted symmetry axis polarization orientation method is used as the observation variable. The heading angle fusion is achieved using a multi-frequency variational Bayesian strong tracking volumetric Kalman filter (MF-VBSTCKF). Different methods are selected to update the optimal heading angle at the sampling point and during the sampling interval. This method solves the problem that polarized light cannot achieve high-precision and high-robust heading angle measurement in occluded environments. It can improve the accuracy and robustness of polarization light orientation methods in occluded environments while ensuring high-frequency output of heading angles.

Description

Technical Field

[0001] The present invention relates to the technical field of polarized light orientation and information fusion, and in particular to a polarization fusion orientation method for an occlusion environment. Background Art

[0002] Navigation technology is crucial for providing continuous, safe, and reliable navigation services. Inertial Navigation Systems (INS) and Global Navigation Satellite Systems (GNSS) are currently the most widely used navigation systems. However, in certain situations, they can become unreliable. For example, INS navigation errors accumulate over time, and GNSS signals are susceptible to electromagnetic interference. With in-depth research into animal navigation mechanisms, polarization-based heading technology, due to its superior performance, including long flight time, high precision, and resistance to electromagnetic interference, has been adopted by various unmanned platforms, including unmanned vehicles and drones.

[0003] While significant progress has been made in polarization-based orientation methods, such as the E-vector polarization-based orientation method, which offers high accuracy and real-time performance under clear weather conditions, and the fitted symmetry axis polarization-based orientation method, which offers strong anti-interference capabilities in obscured environments, these methods still have limitations. In particular, the lack of consideration for different obstructions, such as clouds, trees, and buildings, prevents polarization compasses from leveraging their real-time and high-precision advantages in obscured environments. Consequently, achieving high-accuracy and real-time polarization-based orientation in obscured environments is difficult. Summary of the Invention

[0004] Purpose of the invention: In order to solve the problem in the prior art that polarized light compasses are difficult to achieve high-precision and high-real-time heading angle measurement in an obstructed environment, the present invention provides a polarization fusion orientation method for an obstructed environment.

[0005] Technical solution: A polarization fusion orientation method for occluded environments, including the following steps:

[0006] Step 1: Use a polarization camera to obtain a sky polarization image. Based on the sky polarization image, calculate the heading angle using the E-vector polarization orientation method and the symmetry axis fitting polarization orientation method. When using the symmetry axis fitting polarization orientation method, the sampling frequency is relatively low due to the need to redraw the image.

[0007] Step 2: The heading angle output by the E-vector polarization orientation method is used as the state quantity, and the heading angle output by the symmetric axis polarization orientation method is used as the observation quantity. The state quantity and the observation quantity are input into the multi-frequency variational Bayesian strong tracking cubature Kalman filter to perform heading angle fusion.

[0008] Step 3: Given the observation matrix, at the low-frequency observation sampling point, both the high-frequency state and the low-frequency observation exist. The observation matrix is used to calculate the residual and estimation error, and then the optimal heading angle at this time is updated through the multi-frequency variational Bayesian strong tracking cubature Kalman filter.

[0009] Given a state transfer matrix, during a low-frequency observation sampling interval, the low-frequency observation does not exist. The state transfer matrix is used to update the estimated error, the updated estimated error is used to update the residual, and the updated residual is used to update the optimal heading angle.

[0010] Furthermore, in step 1, in the E vector polarization orientation method, the polarization information of the zenith area is first calculated, and the solar azimuth angle in the navigation coordinate system is obtained by consulting the solar ephemeris. Then, the heading angle is obtained based on the plane formed by the incident light E vector being perpendicular to the observation direction and the sun vector in the Rayleigh scattering theory;

[0011] In the method of fitting the symmetric axis polarization orientation, the polarization information of all regions is calculated, the polarization angle image is redrawn, and the points with AOP values close to 90° are selected to fit the solar meridian. Finally, the angle α between the solar meridian and the carrier axis in the camera coordinate system is determined. s ,Depend on α s Calculate the heading angle;

[0012] According to φ, α s Calculate the heading angles of the two methods respectively:

[0013]

[0014] in, is the heading angle obtained by the E vector polarization orientation method, φ is the polarization angle, is the solar azimuth in the navigation coordinate system, is the heading angle obtained by fitting the symmetric axis polarization orientation method.

[0015] Furthermore, in step 2, the specific method of heading angle fusion is:

[0016] Assuming that both high-frequency state variables and low-frequency observation variables exist, a multi-frequency variational Bayesian strong tracking cubature Kalman filter is used to approximate the joint posterior distribution of the state variables and the observation noise variance through the variational Bayesian method. The joint posterior distribution is expressed as the product of the Gaussian distribution and the inverse gamma distribution:

[0017]

[0018] Among them, p(x k , R k|z 1：k ) is the joint posterior distribution at time k, P k is the filter estimate covariance at time k, x k , z k represent the state quantity and observation quantity at time k respectively, represents the estimated state quantity at time k, λ k , μ k is the inverse gamma distribution parameter, N(·) represents Gaussian distribution, IG(·) represents inverse gamma distribution, and the observation noise variance R k It can be calculated by the following formula:

[0019] R k =(λ k -n-1) -1 μ k (4)

[0020] Where n is the dimension of the observation, λ k and μ k Updated by the following formula:

[0021] λ k =1+λ k / k-1 (5)

[0022]

[0023] Among them, λ k / k-1 and μ k / k-1 is the inverse gamma distribution parameter from time k-1 to time k, m = 2n is the number of volume points, j = 1, 2, ..., m, z k,j is the observation value of the j-th volume point;

[0024] At the same time, the multi-frequency variational Bayesian strong tracking cubature Kalman filter introduces the fading factor τ k To adjust P in real time k / k-1 ,

[0025]

[0026] Among them, P k / k-1 represents the filter estimation covariance from time k-1 to time k, x k / k-1，j represents the predicted value of the state quantity of the j-th volume point from time k-1 to time k, represents the predicted value of the state quantity from time k-1 to time k, Q k is the state noise variance, τ k Expressed as:

[0027]

[0028] tr(·) represents the trace of the matrix, Vk is the covariance matrix of the residuals, z k / k-1，j represents the predicted value of the observation value of the j-th volume point from time k-1 to time k, represents the predicted value of the observation from time k-1 to time k, ρ is the forgetting factor, which takes ρ=0.95, γ k is the residual, expressed as:

[0029]

[0030] Among them, H k Represents the observation matrix.

[0031] Furthermore, in step 3, at the low-frequency observation sampling point, both the high-frequency state quantity and the low-frequency observation quantity exist, and the calculation formulas for the residual, estimation error and optimal heading angle are:

[0032] Among them, γ k , β k are the residual and estimation error at time k, respectively, x k , z k Represent the status at time k respectively

[0033]

[0034] states and observables, represents the estimated value of the state quantity at time k, represents the predicted value of the state quantity from time k-1 to time k, H k represents the observation matrix, K k is the Kalman filter gain, Represents the predicted value of the observation from time k-1 to time k.

[0035] Furthermore, in step 3, during the low-frequency observation sampling interval, the low-frequency observation does not exist, and the calculation formulas for the estimation error, residual and optimal heading angle are:

[0036]

[0037] Among them, β k+1 , γ k+1 are the estimation error and residual at time k+1, F k is the state transition matrix of the system, represents the estimated state quantity at time k+1, Represents the predicted value of the state quantity from time k to time k+1, ∈ k+1 =diag([∈ 1，k ,∈ 2，k ,…∈ n，k+1 ]) is the adjustment factor.

[0038] Beneficial Effects: Compared with the existing technology, the present invention provides a polarization fusion orientation method for occluded environments. It combines the E-vector polarization orientation method and the symmetry axis fitting polarization orientation method. It solves the problems of poor accuracy of the E-vector polarization orientation method and poor real-time performance of the symmetry axis fitting polarization orientation method in occluded environments. It enables the polarized light compass to achieve high-precision and high-real-time orientation even in occluded environments, thereby improving the robustness of the polarized compass.

[0039] By introducing a fading factor to adjust the output of the gain matrix, rapid tracking of the system state is achieved. Furthermore, using the system state and the observation noise variance as the estimated variables, a variational Bayesian learning iterative approximation is performed before each recursive state estimation to obtain the posterior distribution of the observation noise, thereby improving the heading angle estimation accuracy.

[0040] A multi-frequency data fusion algorithm based on residual compensation is introduced to solve the data fusion problem caused by different sampling frequencies. At the low-frequency data sampling interval, the high-frequency data is updated and residual compensation is performed, thereby achieving seamless fusion and output of the heading angle. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 This is a flow chart of the polarization fusion orientation method for occluded environments;

[0042] Figure 2 is a schematic diagram of a multi-frequency variational Bayesian strong tracking cubature Kalman filter;

[0043] Figure 3 is a heading angle error diagram of the present invention in a static occlusion environment;

[0044] Figure 4 This is a heading angle error diagram of the present invention in a dynamic occlusion environment. DETAILED DESCRIPTION

[0045] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments.

[0046] like Figure 1 As shown, a polarization fusion orientation method for an occluded environment includes the following steps:

[0047] Step 1: Use a polarization camera to obtain a sky polarization image. Based on the sky polarization image, calculate the heading angle using the E-vector polarization orientation method and the fitting symmetry axis polarization orientation method. When using the fitting symmetry axis polarization orientation method, the sampling frequency is relatively low due to the need to redraw the image.

[0048] In this embodiment, in the E-vector polarization orientation method, the polarization information of the zenith area is first calculated using the sky polarization image, and an effective observation point G is selected. Its observation direction can be expressed in the incident light coordinate system i as:

[0049]

[0050] Where O represents the position of the polarization compass;

[0051] In the incident light coordinate system i, the direction of the incident light vector E can be expressed by the polarization angle φ:

[0052]

[0053] Sun Vector In the navigation coordinate system, it can be expressed as:

[0054]

[0055] in, and h s It is the azimuth and altitude angle of the sun in the navigation coordinate system calculated by the solar ephemeris solution;

[0056] Then according to the Rayleigh scattering model, the incident light E vector is perpendicular to the plane formed by the observation direction and the sun vector, so:

[0057]

[0058] Where c is a constant that can make the modulos of both sides equal, yes The resulting antisymmetric matrix is, is the direction cosine matrix from the navigation coordinate system to the incident light coordinate system, The heading angle express;

[0059]

[0060] Finally, the heading angle of the E vector polarization orientation method is Obtained by the following formula:

[0061]

[0062] Where φ is the polarization angle, is the solar azimuth in the navigation coordinate system.

[0063] The method of fitting the symmetry axis polarization orientation is to calculate the polarization information of all regions, and based on the significant characteristics of the AOP distribution with the solar meridian as the symmetry axis and showing an antisymmetric distribution, extract the polarization information of points around the solar meridian; next, redraw the AOP image and select points with AOP values close to 90°. Finally, these points are projected onto a two-dimensional plane, and the solar meridian is fitted using the least squares method to finally determine the angle α between the solar meridian and the carrier body axis in the camera coordinate system. s, the heading angle of the symmetric axis polarization orientation method It can be expressed as:

[0064]

[0065] in, is the heading angle obtained by the E vector polarization orientation method, φ is the polarization angle, is the solar azimuth in the navigation coordinate system, is the heading angle obtained by fitting the symmetric axis polarization orientation method.

[0066] Step 2: Take the heading angle output by the E vector polarization orientation method as the state quantity, and the heading angle output by the fitting symmetry axis polarization orientation method as the observation quantity, and input the state quantity and observation quantity as follows: Figure 2 The multi-frequency variational Bayesian strong tracking cubature Kalman filter (MF-VBSTCKF) shown in Figure 1 performs heading angle fusion.

[0067] The specific method of heading angle fusion is:

[0068] Assuming that both high-frequency state variables and low-frequency observation variables exist, a multi-frequency variational Bayesian strong tracking cubature Kalman filter is used to approximate the joint posterior distribution of the state variables and the observation noise variance through the variational Bayesian method. The joint posterior distribution is expressed as the product of the Gaussian distribution and the inverse gamma distribution:

[0069]

[0070] Among them, p(x k , R k |z 1：k ) is the joint posterior distribution at time k, P k is the filter estimate covariance at time k, x k , z k represent the state quantity and observation quantity at time k respectively, represents the estimated state quantity at time k, λ k , μ k is the inverse gamma distribution parameter, N(·) represents Gaussian distribution, IG(·) represents inverse gamma distribution, and the observation noise variance R k It can be calculated by the following formula:

[0071] R k =(λ k -n-1) -1 μ k (9)

[0072] Where n is the dimension of the observation, λ k and μ k Updated by the following formula:

[0073] λ k =1+λ k / k-1 (10)

[0074]

[0075] Among them, λ k / k-1 and μ k / k-1 is the inverse gamma distribution parameter from time k-1 to time k, m = 2n is the number of volume points, j = 1, 2, ..., m, z k,j is the observation value of the j-th volume point;

[0076] At the same time, the multi-frequency variational Bayesian strong tracking cubature Kalman filter introduces the fading factor τ k To adjust P in real time k / k-1 , thereby achieving rapid tracking of state quantities,

[0077]

[0078] Among them, P k / k-1 represents the filter estimation covariance from time k-1 to time k, x k / k-1，j represents the predicted value of the state quantity of the j-th volume point from time k-1 to time k, represents the predicted value of the state quantity from time k-1 to time k, Q k is the state noise variance, τ k Expressed as:

[0079]

[0080] tr(·) represents the trace of the matrix, V k is the covariance matrix of the residuals, z k / k-1，j represents the predicted value of the observation value of the j-th volume point from time k-1 to time k, represents the predicted value of the observation from time k-1 to time k, ρ is the forgetting factor, which takes ρ=0.95, γ k is the residual, expressed as:

[0081]

[0082] Among them, H k Represents the observation matrix.

[0083] Step 3: For low-frequency observation sampling points and intervals, choose to use different methods to update the optimal heading angle. Because there are no low-frequency observations at the sampling interval, the observation noise variance cannot be updated using equations (9) and (11). Therefore, different update methods need to be selected according to different situations.

[0084] (a) At the low-frequency observation sampling point, both high-frequency state variables and low-frequency observations exist. The observation matrix can be obtained based on the observation function. The observation matrix is used to calculate the residual and estimation error, and then the optimal heading angle at this time is updated through the multi-frequency variational Bayesian strong tracking cubature Kalman filter.

[0085] Since both high-frequency state variables and low-frequency observation variables exist, the residual can be expressed by formula (15), and the estimated error β k Expressed as:

[0086]

[0087] When low-frequency observations exist, the optimal heading angle update formula is:

[0088]

[0089] Among them, γ k , β k are the residual and estimation error at time k, respectively, x k , z k represent the state quantity and observation quantity at time k respectively, represents the estimated value of the state quantity at time k, represents the predicted value of the state quantity from time k-1 to time k, H k represents the observation matrix, K k is the Kalman filter gain, Represents the predicted value of the observation from time k-1 to time k.

[0090] (b) During the low-frequency observation sampling interval, the low-frequency observation does not exist, and the observation noise variance cannot be updated through equations (9) and (11), R k+1 will tend to infinity, and the Kalman gain K at time k+1 k+1 will tend to zero, so the estimation error at time k+1 can be expressed as:

[0091]

[0092] Among them, F k is the state transition matrix of the system, It is the predicted value of the state quantity from time k to time k+1.

[0093] Since the observed quantity z k+1 Does not exist, and the residual cannot be updated by formula (15). In this case, the estimated error is used to update the residual:

[0094]

[0095] As shown above, when low-frequency observations do not exist, the estimated error can be continuously updated according to Equation (18), and the residual can be expressed by the estimated error according to Equation (19). In the conventional form of filtering, the optimal heading angle can be expressed by Equation (17). However, in the low-frequency observation sampling interval, the size of the Kalman gain is unknown and its value is theoretically very small. Therefore, the estimation formula of the optimal heading angle can be adjusted to

[0096]

[0097] Among them, β k+1 , γ k+1 are the estimation error and residual at time k+1, Represents the estimated value of the state quantity at time k+1, ∈ k+1 =diag([∈ 1,k ,∈ 2，k ,…∈ n，k+1 ]) is the adjustment factor, which is equivalent to the Kalman gain.

[0098] To verify the effectiveness of this method, the following comparative experiment was conducted. The polarization compass used in the experiment consists of a polarization camera and an NVIDIA Jetson TX2 development board. The heading angle of the reference system was obtained from the SPAN-KVH1750, a high-precision, compact fiber-optic integrated navigation system with a heading accuracy of 0.035° and a sampling frequency of 4 Hz. Figure 1 This is a flow chart of the polarization fusion orientation method for occluded environments, where the sampling frequency of the E-vector polarization orientation method is 4 Hz, and the sampling frequency of the fitting symmetry axis polarization orientation method is 1 Hz. Figure 2 is a schematic diagram of the multi-frequency variational Bayesian strong tracking cubature Kalman filter, T E and T S are the sampling periods of the E-vector polarization orientation method and the fitted symmetry axis polarization orientation method, respectively. To demonstrate the effectiveness of the present invention, comparative experimental results are given using a single polarization orientation method and two polarization orientation methods using different fusion algorithms under different occlusion environments. The fusion algorithms used for comparison include: multi-frequency cubic Kalman filter (MF-CKF), multi-frequency strong tracking cubic Kalman filter (MF-STCKF), and multi-frequency variational Bayesian strong tracking cubic Kalman filter (MF-VBSTCKF). Figure 3 is a diagram of the heading angle error of various methods in the static occlusion environment of the present invention, Figure 4 This is a diagram of the heading angle error of various methods in the present invention under dynamic occlusion environment. Figure 3 、 Figure 4It can be seen that due to the influence of occlusion, the heading angle obtained by the E-vector polarization orientation method varies greatly, reducing the heading accuracy. The heading angle obtained by the fitting symmetric axis polarization orientation method can maintain high accuracy even in the presence of occlusion, but the output frequency is low. By performing residual compensation on the state quantity in the low-frequency data interval, the output frequency of all fusion algorithms is consistent with the E-vector polarization orientation method. Among the three fusion methods, MF-CKF has the worst optimal heading angle estimation accuracy. MF-STCKF can better track the target state by introducing a strong tracking fading factor, but due to the lack of real-time estimation and update of observation noise, errors still exist. MF-VBSTCKF utilizes the advantages of the variational Bayesian method in adaptive estimation of time-varying noise. While maintaining high tracking accuracy, it can significantly improve the heading angle estimation performance and reduce the heading error. The above shows that the proposed polarization fusion orientation method for occluded environments can achieve high-precision and high-robustness heading angle measurement.

[0099] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A polarization fusion orientation method for an obstructed environment, characterized in that: The following steps are involved: Step 1: Use a polarization camera to obtain a sky polarization image. Based on the sky polarization image, calculate the heading angle using the E-vector polarization orientation method and the symmetry axis fitting polarization orientation method. When using the symmetry axis fitting polarization orientation method, the sampling frequency is relatively low due to the need to redraw the image. Step 2: The heading angle output by the E-vector polarization orientation method is used as the state quantity, and the heading angle output by the symmetric axis polarization orientation method is used as the observation quantity. The state quantity and the observation quantity are input into the multi-frequency variational Bayesian strong tracking cubature Kalman filter to perform heading angle fusion. Step 3: Given the observation matrix, at the low-frequency observation sampling point, both the high-frequency state and the low-frequency observation exist. The observation matrix is used to calculate the residual and estimation error, and then the optimal heading angle at this time is updated through the multi-frequency variational Bayesian strong tracking cubature Kalman filter. Given a state transfer matrix, during a low-frequency observation sampling interval, the low-frequency observation does not exist. The state transfer matrix is used to update the estimated error, the updated estimated error is used to update the residual, and the updated residual is used to update the optimal heading angle.

2. The polarization fusion orientation method for an obstructed environment according to claim 1, characterized in that: In step 1, in the E vector polarization orientation method, the polarization information of the zenith area is first calculated, and the solar azimuth angle in the navigation coordinate system is obtained by consulting the solar ephemeris. Then, the heading angle is obtained based on the plane formed by the incident light E vector being perpendicular to the observation direction and the sun vector in the Rayleigh scattering theory; In the method of fitting the symmetric axis polarization orientation, the polarization information of all regions is calculated, the polarization angle image is redrawn, and the points with AOP values close to 90° are selected to fit the solar meridian. Finally, the angle α between the solar meridian and the carrier axis in the camera coordinate system is determined. s ,Depend on α s Calculate the heading angle; According to φ, α s Calculate the heading angles of the two methods respectively: in, is the heading angle obtained by the E vector polarization orientation method, φ is the polarization angle, is the solar azimuth in the navigation coordinate system, is the heading angle obtained by fitting the symmetric axis polarization orientation method.

3. The polarization fusion orientation method for an obstructed environment according to claim 1 or 2, characterized in that: In step 2, the specific method of heading angle fusion is: Assuming that both high-frequency state variables and low-frequency observation variables exist, a multi-frequency variational Bayesian strong tracking cubature Kalman filter is used to approximate the joint posterior distribution of the state variables and the observation noise variance through the variational Bayesian method. The joint posterior distribution is expressed as the product of the Gaussian distribution and the inverse gamma distribution: Among them, p(x k , R k |z 1：k ) is the joint posterior distribution at time k, P k is the filter estimate covariance at time k, x k , z k represent the state quantity and observation quantity at time k respectively, represents the estimated state quantity at time k, λ k , μ k is the inverse gamma distribution parameter, N(·) represents Gaussian distribution, IG(·) represents inverse gamma distribution, and the observation noise variance R k It can be calculated by the following formula: R k =(λ k -n-1) -1 m k (4) Where n is the dimension of the observation, λ k and μ k Updated by the following formula: l k =1+λ k / k-1 (5) Among them, λ k / k-1 and μ k / k-1 is the inverse gamma distribution parameter from time k-1 to time k, m = 2n is the number of volume points, j = 1, 2, ..., m, z k，j is the observation value of the j-th volume point; At the same time, the multi-frequency variational Bayesian strong tracking cubature Kalman filter introduces the fading factor τ k To adjust P in real time k / k-1 , Among them, P k / k-1 represents the filter estimation covariance from time k-1 to time k, x k / k-1，j represents the predicted value of the state quantity of the j-th volume point from time k-1 to time k, represents the predicted value of the state quantity from time k-1 to time k, Q k is the state noise variance, τ k Expressed as: tr(·) represents the trace of the matrix, V k is the covariance matrix of the residuals, z k / k-1，j represents the predicted value of the observation value of the j-th volume point from time k-1 to time k, represents the predicted value of the observation from time k-1 to time k, ρ is the forgetting factor, which takes ρ=0.95, γ k is the residual, expressed as: Among them, H k Represents the observation matrix.

4. The polarization fusion orientation method for an obstructed environment according to claim 1 or 2, characterized in that: In step 3, at the low-frequency observation sampling point, both the high-frequency state quantity and the low-frequency observation quantity exist. The calculation formulas for the residual, estimation error and optimal heading angle are: Among them, γ k , β k are the residual and estimation error at time k, respectively, x k , z k represent the state quantity and observation quantity at time k respectively, represents the estimated value of the state quantity at time k, represents the predicted value of the state quantity from time k-1 to time k, H k represents the observation matrix, K k is the Kalman filter gain, Represents the predicted value of the observation from time k-1 to time k.

5. The polarization fusion orientation method for an obstructed environment according to claim 1 or 2, characterized in that: In step 3, during the low-frequency observation sampling interval, the low-frequency observation does not exist, and the calculation formulas for the estimation error, residual and optimal heading angle are: Among them, β k+1 , γ k+1 are the estimation error and residual at time k+1, F k is the state transition matrix of the system, represents the estimated state quantity at time k+1, Represents the predicted value of the state quantity from time k to time k+1, ∈ k+1 =diag([∈ 1，k ,∈ 2，k ,…∈ n，k+1 ]) is the adjustment factor.

Images