阅读说明：本技术 一种基于天顶点矢量的偏振/vio三维姿态确定方法 (polarization/VIO three-dimensional attitude determination method based on zenith vector ) 是由 杨健 李晶 王善澎 郭雷 于 2021-11-22 设计创作，主要内容包括：本发明涉及一种基于天顶点矢量的偏振/VIO三维姿态确定方法,主要包括以下步骤：首先,利用图像式偏振传感器测量的偏振角信息和偏振度信息获得太阳矢量并确定太阳在图像像素坐标系下的位置；然后,根据太阳、中性点和天顶点三点共线的特点以及当前太阳天顶角信息,求解出偏振传感器坐标系下天顶矢量；再次,选择惯导误差状态和相机位姿为估计状态向量,建立包括天顶矢量、太阳矢量和视觉残差信息的量测模型；最终,设计视觉两帧误差相关自适应因子,对天顶矢量、太阳矢量和视觉残差信息进行加权融合,解算三维姿态信息。本发明基于偏振信息与天顶点检测可以同时提供水平姿态与航向的修正,保障稀疏特征环境下的三维姿态估计精度,提高环境适应性。(The invention relates to a polarization/VIO three-dimensional attitude determination method based on zenith vectors, which mainly comprises the following steps: firstly, obtaining a sun vector by utilizing polarization angle information and polarization degree information measured by an image type polarization sensor and determining the position of the sun under an image pixel coordinate system; then, solving a zenith vector under a polarization sensor coordinate system according to the collinear characteristics of three points of the sun, the neutral point and the zenith point and the information of the current sun zenith angle; thirdly, selecting an inertial navigation error state and a camera pose as estimated state vectors, and establishing a measurement model comprising a zenith vector, a sun vector and visual residual error information; finally, designing a visual two-frame error-related adaptive factor, performing weighted fusion on zenith vectors, solar vectors and visual residual error information, and calculating three-dimensional attitude information. The invention can simultaneously provide correction of horizontal attitude and course based on polarization information and zenith detection, guarantee the three-dimensional attitude estimation precision in the sparse characteristic environment and improve the environmental adaptability.)

1. A polarization/VIO three-dimensional attitude determination method based on zenith vectors is characterized by comprising the following implementation steps:

step 1, obtaining a sky polarization degree distribution image and a polarization angle distribution image under an image pixel coordinate system by using an image type polarization sensor, and detecting a neutral point based on the polarization degree distribution imageNAnd finding the position coordinates in the image pixel coordinate system (u _N ,v _N ) (ii) a Calculating polarization vectors of two observation directions based on polarization angle distribution imageAndto obtain the sun vector under the coordinate system of the polarization sensors ^b Determining the position coordinates of the sun in the image pixel coordinate system (u _s ,v _s )；

Step 2, utilizing the collinear relation among the sun, the neutral point and the zenith in the polarization degree distribution image, and the zenith angleWith the sun vector under the polarization sensor coordinate systems ^b And zenith vectorz ^b The included angles are the same, and the zenith angle of the sun under the navigation system is calculated by utilizing the solar astronomical calendar and the geographic longitude and latitudeCalculating zenith vector under polarization sensor coordinate systemz ^b ；

Step 3, selecting inertial navigation error states and camera poses at different moments as estimated state vectors, and establishing zenith vectors under a polarization sensor coordinate systemz ^b Sun vector under polarization sensor coordinate systems ^b Measuring the three-dimensional posture of the visual residual error information under the coordinate system of the visual camera;

step 4, designing self-adaptive factors related to visual two-frame errors in VIO (visual image of object) and aligning zenith vectors under a polarization sensor coordinate systemz ^b Sun vector under polarization sensor coordinate systems ^b And carrying out weighted fusion on the visual residual information under the coordinate system of the visual camera, and calculating the three-dimensional attitude information.

2. The method of claim 1 for determining polarization/VIO three-dimensional pose based on zenith vectors, wherein: in the step 1, a neutral point is detected based on the polarization degree imageNPosition coordinates in the image pixel coordinate system (u _N ,v _N ) And converting it into homogeneous coordinate to represent neutral point vectorn ^b According to the orthogonal relation of the sun vector and the polarization vector, the deviation of two observation directions is utilizedVibration vectorAndresolving to obtain a sun vectors ^b And further determining the projection position of the sun in the polarization degree image pixel coordinate system, wherein the position coordinate of the sun in the image pixel coordinate system is assumed to be (u _s ,v _s ) Converted to normalized homogeneous coordinates as:

wherein the content of the first and second substances,representing the solar altitude in the polarization sensor coordinate system,Krepresenting a camera internal reference matrix.

3. The method of claim 1 for determining polarization/VIO three-dimensional pose based on zenith vectors, wherein: in the step 2, the neutral point vector under the polarization sensor coordinate system obtained in the step 1 is usedn ^b And sun vectors ^b Information, determining the plane passing through the sun, the neutral point and the zenith point under the coordinate system of the polarization sensor, and recording as the planeα；

Zenith angle of sun under navigation systemObtained by solar astronomical almanac and geographical latitude:

wherein the content of the first and second substances,indicating the solar altitude under the navigation system,Lin the case of the geographic latitude, the latitude,solar declination, and omega is the solar hour angle;

in the polarisation sensor coordinate systems ^b The shaft is a rotating shaft and a zenith angleObtaining conical surfaces as semi-axial angleszUsing the conical surfacezAnd plane surfaceαThe intersecting line can obtain the zenith vector under the polarization sensor coordinate systemz ^b ：

Wherein the content of the first and second substances,is a zenith angle and represents a conical surfacezThe half-axis angle of (a) is,z ^b representing a zenith vector under a polarization sensor coordinate system to be solved;

selecting the navigation system as the next zenith vectorAnd obtaining a predicted zenith vector according to the attitude information provided by the visual inertial odometer, and recording the predicted zenith vector as:

wherein the content of the first and second substances,and representing a posture conversion matrix from a navigation system provided by the visual inertial odometer to a polarization sensor coordinate system, and removing the ambiguity of the acquired zenith vector under the polarization sensor coordinate system by using the predicted zenith vector to determine a unique zenith vector.

4. The method of claim 1 for determining polarization/VIO three-dimensional pose based on zenith vectors, wherein: in the step 3, the inertial navigation error state in VIO is adoptedWhereinR ^6×1Representing a 6 x 1 vector, and camera states at different timesEstablishing a system estimation state vector:

wherein the content of the first and second substances,the error of the attitude angle of the carrier is represented,representing the zero-bias of the gyroscope,is shown asiThe error of the attitude angle of the camera at the moment,is shown asiA temporal camera position error;

establishing a system measurement model comprising the zenith vector, the sun vector and the vision measurement residual error information obtained in the step 2 as follows:

wherein the content of the first and second substances,representing the transformation matrix of the attitude from the navigation system to the polarization sensor coordinate system,z ^v which represents a measure of the visual residual error,a visual measurement Jacobian matrix is represented,is indicative of the visual metrology noise,the measurement noise of the sun vector is represented,representing zenith vector measurement noise.

5. The method of claim 1 for determining polarization/VIO three-dimensional pose based on zenith vectors, wherein: in step 4, a process noise variance matrix Q array and a measurement noise variance matrix R array of the adaptive factor adjusting system are designed according to the change of the difference value between two visual frames in the VIO:

wherein the content of the first and second substances,bis a factor of forgetting to forget,b ^k+1represents the power of a forgetting factor;is shown askTime of day camerayThe direction position error, namely the position increment of the advancing direction of the unmanned aerial vehicle,an adaptation factor representing an inter-frame error according to two frames before and after the vision,Qan array sumRThe array adjustment process is as follows:

wherein the content of the first and second substances,an estimate representing a measured noise variance matrix;a variance matrix estimate representing process noise;to measure innovation;H _k+1is a measuring array;K _k+1representing a Kalman filtering gain; phi _{k k+1,}Representing a state transition matrix;P _k to representkThe time state covariance.

Technical Field

The invention belongs to the field of polarization combined navigation, and particularly relates to a polarization/VIO three-dimensional attitude determination method based on zenith vectors, which provides a new thought for researching polarization navigation three-dimensional attitude determination.

Background

The polarization navigation technology is a biologically inspired navigation means, and has the advantages of being passive and not accumulating errors along with time. Most of the polarization information obtained according to the Rayleigh scattering model provides course constraint for the integrated navigation system, and the three-dimensional attitude cannot be solved only by the polarization information. The polarization navigation usually needs to be combined with the inertial navigation to complete three-dimensional attitude determination, but the horizontal attitude error of the combined method is easy to accumulate and has poor dynamic property. The Visual Inertial Odometer (VIO) completes the attitude estimation of the carrier by extracting feature points in the natural environment and combining an inertial navigation technology, but the estimation capability of the visual inertial odometer to the course is poor, and meanwhile, the problem of inaccurate attitude estimation is easy to occur in the scene of environmental feature loss.

The current method for integrated navigation by using visual navigation, inertial navigation and image type polarization navigation comprises the following steps: the patent discloses an unmanned aerial vehicle pose estimation method based on visual inertial polarized light fusion (application number: 202010623718.1), which only utilizes polarized navigation to provide course constraint and does not consider the problem of inaccurate estimation of visual navigation attitude under sparse characteristics; the thesis "micro-inertia/polarization vision based integrated orientation method" provides course constraint for an integrated navigation system by using polarization navigation, and cannot correct horizontal attitude; compared with the method provided by the invention, the method depending on the double neutral point vectors is easy to be disturbed by weather, and can not ensure that two neutral point vectors are detected under certain weather conditions; the patent discloses a three-dimensional attitude obtaining method (application number: 201210005641.7) based on atmospheric polarization mode spatial features, the method completes three-dimensional attitude determination by collecting spatial positions of significant feature points, the method has large calculation amount due to the fact that all sampling points need to be traversed, and the requirement of navigation resolving frequency cannot be met for carriers such as unmanned aerial vehicles with limited airborne computing resources; the patent 'a carrier three-dimensional attitude acquisition method based on horizon and polarized light' (application number: 201810062481.7) solves the horizontal attitude by extracting the horizon, then provides three-dimensional attitude information according to the polarization calculation course information combination, and does not fully utilize the polarization navigation information to solve the three-dimensional attitude. The patent 'three-dimensional attitude information fast resolving method based on double polarized light vectors' (application number: 201711137596. X) only utilizes polarized light information to complete three-dimensional attitude resolving, so the dynamic property is poorer than that of the polarization/VIO three-dimensional attitude determining method provided by the patent.

Disclosure of Invention

The invention solves the technical problem of dynamic three-dimensional attitude determination of a carrier in a GNSS rejection environment, overcomes the defect that most of sky polarized light information can only provide course constraint for a combined navigation system, provides a new idea of using polarized information to perform three-dimensional attitude determination, and ensures the precision of VIO three-dimensional attitude determination in a sparse characteristic environment.

The technical solution of the invention is as follows: a polarization/VIO three-dimensional attitude determination method based on zenith vectors mainly comprises the following implementation steps:

step 1, obtaining a sky polarization degree distribution image and a polarization angle distribution image under an image pixel coordinate system by using an image type polarization sensor, and detecting a neutral point based on the polarization degree imageN，Obtaining the coordinates of the location in the image pixel coordinate system (u _N ,v _N ) (ii) a Obtaining polarization vectors of two observation directions based on polarization angle imageAndcalculating the sun vector under the coordinate system of the polarization sensors ^b Determining the position coordinates of the sun in the image pixel coordinate system (u _s ,v _s )；

Step 2, according to the collinearity of the sun, the neutral point and the zenith in the polarization degree distribution image, and the zenith angleWith the sun vector under the polarization sensor coordinate systems ^b Included angle with zenith vectorz ^b Similarly, the zenith angle of the sun under the navigation system is calculated by utilizing the solar astronomical calendar and the geographic longitude and latitudeCalculating zenith vector under polarization sensor coordinate systemz ^b ；

Step 3, selecting the inertial navigation error state and the camera pose as states, and establishing a zenith vector under a coordinate system combined with the polarization sensorz ^b Sun vector under polarization sensor coordinate systems ^b And a three-dimensional posture measuring model of visual residual error information under a visual camera coordinate system;

step 4, designing self-adaptive factors related to visual two-frame errors in VIO, and aiming at zenith vectors under a polarization sensor coordinate system and solar vectors under the polarization sensor coordinate systems ^b Carrying out weighted fusion on the visual residual error information under a visual camera coordinate system, and resolving to obtain three-dimensional attitude information;

neutral points are detected in step 1 from polarization degree-based imagesNPosition coordinates (u _N ,v _N ) And converting it into homogeneous coordinate to represent neutral point vectorn ^b . According to the vertical relation between the sun vector and the polarization vector, the polarization vectors of two observation directions are utilizedAndresolving to obtain a sun vectors ^b To determine image pixel coordinatesCoordinate of sun position under the coordinate system (u _s ,v _s ) Converted to normalized homogeneous coordinates as:

wherein the content of the first and second substances,representing the solar altitude in the polarization sensor coordinate system,Krepresenting a camera internal reference matrix.

In step 2, according to the neutral point vector under the polarization sensor coordinate system obtained in step 1n ^b And sun vectors ^b Information, determining the plane passing through the sun, the neutral point and the zenith point under the coordinate system of the polarization sensor, and recording as the planeα。

Zenith angle of sun under navigation systemObtained by solar astronomical almanac and geographical latitude:

wherein the content of the first and second substances,indicating the solar altitude under the navigation system,Lin the case of the geographic latitude, the latitude,δdeclination of the sun, and omega is the solar hour angle.

In the polarisation sensor coordinate systems ^b The shaft is a rotating shaft and a zenith angleObtaining conical surfaces as semi-axial angleszUsing the conical surfacezAnd plane surfaceαThe intersecting line can obtain the zenith vector under the polarization sensor coordinate systemz ^b ：

Wherein the content of the first and second substances,is a zenith angle and represents a conical surfacezThe half-axis angle of (a) is,z ^b representing the zenith vector under the polarization sensor coordinate system to be solved. Obviously, the cone and the plane have two intersecting lines, namely, the solved zenith vector has ambiguity.

The general selection navigation system has the following zenith vector ofAnd obtaining a predicted zenith vector according to the attitude information provided by the visual inertial odometer, and recording the predicted zenith vector as:

wherein the content of the first and second substances,and representing a posture conversion matrix from a navigation system provided by the visual inertial odometer to a polarization sensor coordinate system, and removing the ambiguity of the acquired zenith vector under the polarization sensor coordinate system by using the predicted zenith vector to determine a unique zenith vector.

In step 3, the inertial navigation error state（R ^6×1Representing a 6 x 1 vector) and camera states at different timesEstablishing a system estimation state vector:

wherein the content of the first and second substances,the error of the attitude angle of the carrier is represented,representing the zero-bias of the gyroscope,is shown asiThe error of the attitude angle of the camera at the moment,is shown asiTemporal camera position error. Establishing a system measurement model comprising the zenith vector, the sun vector and the vision measurement residual error information obtained in the step 2 as follows:

wherein the content of the first and second substances,representing the transformation matrix of the attitude from the navigation system to the polarization sensor coordinate system,z ^v which represents a measure of the visual residual error,a visual measurement Jacobian matrix is represented,which is indicative of the corresponding measured noise,the measurement noise of the sun vector is represented,representing zenith vector measurement noise.

The overall measurement equation of the system is as follows:

wherein the content of the first and second substances,zrepresenting systematic measurements, from sun vector measurementsz ^s Zenith vector measurementz ^b And visual residual measurementz ^v The components of the composition are as follows,Ha measurement matrix representing the combination of the three,vrepresenting the noise of the three measurements.

In step 4, an adaptive factor is designed according to the change of the difference value between two VIO frames, so as to improve the filtering precision of the system. The method comprises the following specific steps:

the adaptive factor is designed as follows:

wherein the content of the first and second substances,bis a factor of forgetting to forget,is shown askTime of day camerayAnd (4) direction position error, namely the advancing direction position increment of the unmanned aerial vehicle.An adaptation factor representing an inter-frame error according to two frames before and after the vision. The new filtering process is obtained as follows:

combining the above equation of state and measurement equationRepresenting the mean of the process noise at time k. In the extended kalman filtering process, the following can be obtained:

wherein the content of the first and second substances,is composed ofkEstimate of the measured noise variance matrix at time +1,is composed ofkEstimation of the system noise variance matrix at time + 1.Is composed ofkMeasuring innovation at +1 moment;H _k+1is composed ofkMeasuring array at +1 moment;K _k+1to representk+1 time kalman filter gain; phi _{k k+1,}Represents fromkIs timed tokState transition matrix at +1 moment;P _k to representkThe time state covariance.

Compared with the prior art, the invention has the advantages that:

(1) the image type polarization sensor is used for detecting based on zenith and neutral points, horizontal attitude constraint can be provided, a new idea is provided for three-dimensional attitude determination by a polarization navigation technology, and the image type polarization sensor is not limited to the polarization navigation technology and only can provide course constraint;

(2) because the vision measurement is influenced by environmental characteristic points and illumination, measurement information is lost and the precision is reduced, meanwhile, the polarization image sensor is reduced in the polarization measurement precision due to environmental factors such as direct sunlight, reflected light and the like, and an adaptive factor is designed according to the error between two visual frames to complete the fusion of three posture constraint information, so that a three-dimensional posture estimation result is obtained.

Drawings

FIG. 1 is a flow chart of a method for determining polarization/VIO three-dimensional attitude based on zenith vectors according to the present invention;

FIG. 2 is a schematic diagram of solving the zenith vector under the polarization sensor coordinate system by utilizing the constraint formed by the plane of the sun vector under the polarization sensor coordinate system and the conical surface with the zenith angle as a semi-axis angle according to the unchanged included angle between the zenith angle of the sun under the navigation system and the zenith vector and the sun vector under the polarization sensor coordinate system.

Detailed Description

The following description of the specific implementation steps of the present invention with reference to the accompanying drawings 1 and 2 and examples is as follows:

step 1, obtaining a sky polarization degree distribution image and a polarization angle distribution image under an image pixel coordinate system by using an image type polarization sensor, and detecting a neutral point based on the polarization degree imageN，Obtaining the coordinates of the location in the image pixel coordinate system (u _N ,v _N ) (ii) a Obtaining polarization vectors of two observation directions based on polarization angle imageAndcalculating the sun vector under the coordinate system of the polarization sensors ^b Determining the position coordinates of the sun in the image pixel coordinate system (u _s ,v _s )；

Step 2, as shown in figure 2, according to the collinearity of the sun, the neutral point and the zenith point in the polarization degree distribution image, the included angle between the zenith angle and the sun vector and the zenith vector under the polarization sensor coordinate system is the same, and the zenith angle of the sun under the navigation system is calculated and obtained by utilizing the solar astronomical calendar and the geographical latitude and longitudeCalculating zenith vector under polarization sensor coordinate systemz ^b ；

Step 3, selecting the inertial navigation error state and the camera pose as states, and establishing a zenith vector under a coordinate system combined with the polarization sensorz ^b Sun vector under polarization sensor coordinate systems ^b And a three-dimensional posture measuring model of visual residual error information under a camera coordinate system;

and 4, aiming at the acquired zenith vector, sun vector and visual residual error information, because the visual inertial odometer is influenced by environmental characteristic points and illumination, measurement information is lost and the precision is reduced, meanwhile, the polarization measurement precision is reduced due to environmental factors such as direct solar light, reflected light and the like of the polarization image sensor, and an adaptive factor related to the error between two frames of VIO is designed to be timely adjusted to avoid error accumulation caused by inaccuracy of one measurement information.

The specific implementation steps are as follows:

in step 1, a sky polarization degree distribution image and a polarization angle distribution image under an image pixel coordinate system are obtained through an image type polarization sensor, and according to the characteristic that the polarization degree at a neutral point of the polarization degree distribution image is 0, the position coordinate of the neutral point of the image pixel coordinate system is determined (u _N ,v _N ) Normalizing the coordinates to represent a neutral point vectorn ^b . Obtaining polarization vectors of two observation directions based on polarization angle distribution imageAndresolving to obtain the sun vector under the coordinate system of the polarization sensors ^b . Position coordinates of the sun under the image pixel coordinate system according to the projection relation (u _s ,v _s ) Conversion to homogeneous coordinates may result in:

wherein the content of the first and second substances,representing the solar altitude in the polarization sensor coordinate system,Krepresenting a camera internal reference matrix.

In step 2, according to the neutral point vector under the polarization sensor coordinate system obtained in step 1n ^b And sun vectors ^b Determining the plane passing through the sun, the neutral point and the zenith point under the coordinate system of the polarization sensor, and recording as the planeα。

Zenith angle of sun under navigation systemObtained by solar astronomical almanac and geographical latitude:

wherein the content of the first and second substances,indicating the solar altitude under the navigation system,Lin the case of the geographic latitude, the latitude,δdeclination of the sun, and omega is the solar hour angle.

In the polarisation sensor coordinate systems ^b The shaft is a rotating shaft and a zenith angleObtaining conical surfaces as semi-axial angleszUsing the conical surfacezAnd plane surfaceαThe intersecting line can obtain the zenith vector under the polarization sensor coordinate systemz ^b ：

Wherein the content of the first and second substances,is a zenith angle and represents a conical surfacezThe half-axis angle of (a) is,z ^b representing the zenith vector under the polarization sensor coordinate system to be solved. Obviously, the cone and the plane have two intersecting lines, namely, the solved zenith vector has ambiguity.

The zenith vector is typically chosen to beAnd obtaining a predicted zenith vector according to the attitude information provided by the visual inertial odometer, and recording the predicted zenith vector as:

wherein the content of the first and second substances,and representing a posture conversion matrix from a navigation system provided by the visual inertial odometer to a polarization sensor coordinate system, and removing the ambiguity of the acquired zenith vector under the polarization sensor coordinate system by using the predicted zenith vector to determine a unique zenith vector.

In step 3, the inertial navigation error state（R ^6×1Representing a 6 x 1 vector) and camera states at different timesEstablishing a system estimation state vector:

wherein the content of the first and second substances,the error of the attitude angle of the carrier is represented,representing the zero-bias of the gyroscope,is shown asiThe error of the attitude angle of the camera at the moment,is shown asiTime cameraA position error. Establishing a system measurement model comprising the zenith vector, the sun vector and the vision measurement residual error information obtained in the step 2 as follows:

wherein the content of the first and second substances,representing the transformation matrix of the attitude from the navigation system to the polarization sensor coordinate system,z ^v which represents a measure of the visual residual error,a visual measurement Jacobian matrix is represented,is indicative of the visual metrology noise,the measurement noise of the sun vector is represented,representing zenith vector measurement noise;

the overall measurement equation of the system is as follows:

wherein the content of the first and second substances,zrepresenting systematic measurements, from sun vector measurementsz ^s Zenith vector measurementz ^b And visual residual measurementz ^v The components of the composition are as follows,Ha measurement matrix representing the combination of the three,vrepresenting the noise of the three measurements.

In step 4, an adaptive factor is designed according to the change of the difference value between two visual frames in the VIO, so that the filtering precision of the system is improved. The method comprises the following specific steps:

the adaptive factor is designed as follows:

wherein the content of the first and second substances,bis a factor of forgetting to forget,is shown askTime of day camerayAnd (4) direction position error, namely the advancing direction position increment of the unmanned aerial vehicle.An adaptation factor representing an inter-frame error according to two frames before and after the vision.

The new filtering process is obtained as follows:

combining the above equation of state and measurement equationRepresenting the mean of the process noise at time k. In the extended kalman filtering process, the following can be obtained:

wherein the content of the first and second substances,is composed ofkEstimate of the measured noise variance matrix at time +1,is composed ofkEstimation of the system noise variance matrix at time + 1.Is composed ofkMeasuring innovation at +1 moment;H _k+1is composed ofkMeasuring array at +1 moment;K _k+1to representk+1 time kalman filter gain; phi _{k k+1,}Represents fromkIs timed tokState transition matrix at +1 moment;P _k to representkThe time state covariance.

The self-adaptive physical significance lies in that when the error between two frames of the visual inertial odometer is greater than a certain threshold value, the threshold value is set as a function of the position increment of the unmanned aerial vehicle, namely, the error between the two frames is not greater than the distance moved in the corresponding time of the unmanned aerial vehicle, the system measurement noise is adjusted, and then the optimal estimation result is achieved.

Those skilled in the art will appreciate that the invention may be practiced without these specific details. The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.