阅读说明：本技术 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法 (Underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation ) 是由 夏琳琳 刘瑞敏 蒙跃 张晶晶 于 2021-06-03 设计创作，主要内容包括：本发明公开了一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法,属于水下导航领域本发明方法包括：首先利用水下偏振模式依赖太阳位置信息的原理,测量不同地点、深度和时间的水下偏振角,同时,利用偏振光传感器准确测得水下潜航器的速度、姿态、位置及航向角信息；其次利用场景的偏振信息,研究拓扑节点的识别方法和特征表达方法,以便更好地进行场景识别和目标探测；最后,考虑到水下滤波器的实用性,提出自适应无迹卡尔曼滤波(AUKF)数据融合策略,以改进的最大无偏后验噪声统计估计器在线估计未知系统噪声等参数,应用于水下潜航器的同时定位和地图构建(SLAM),即实现AUKF-SLAM。本发明可显著提高组合导航的定位定向精度。(The invention discloses an underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation, belonging to the field of underwater navigation and comprising the following steps: firstly, measuring underwater polarization angles of different places, depths and time by using the principle that an underwater polarization mode depends on sun position information, and simultaneously, accurately measuring the speed, attitude, position and course angle information of an underwater vehicle by using a polarized light sensor; secondly, researching an identification method and a feature expression method of the topological nodes by using polarization information of the scene so as to better identify the scene and detect the target; and finally, in consideration of the practicability of the underwater filter, providing an Adaptive Unscented Kalman Filtering (AUKF) data fusion strategy, estimating parameters such as unknown system noise and the like on line by using an improved maximum unbiased posteriori noise statistics estimator, and applying the parameters to simultaneous positioning and map construction (SLAM) of the underwater vehicle, namely realizing AUKF-SLAM. The invention can obviously improve the positioning and orientation precision of the integrated navigation.)

1. An underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation is characterized by comprising the following five steps:

step 1: selecting the attitude, the speed and the position of the underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;

step 2: calculating a sun vector by adopting a least square method of a polarization state through an orientation algorithm of a polarization sensor, and determining the position of the sun;

and step 3: analyzing visual scene feature extraction and matching problems by using image polarization information through a topological node identification algorithm;

and 4, step 4: according to the speed output of the inertial navigation system, establishing a speed inertial navigation measurement model and a polarized light nonlinear measurement model;

and 5: and integrating the steps, and establishing an integrated navigation system model through data fusion to construct an environment map.

2. An underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation is characterized in that a nonlinear equation of a kinematic model of a submersible vehicle established in step 1 is as follows:

in the formula above, the first and second groups of the compound,andthe position abscissa, the position ordinate, the zenith coordinate and the course coordinate of the underwater vehicle are respectively, u is a control input vector, and w is a zero-mean Gaussian-distribution process noise vector.

3. An underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation is characterized in that in step 2, a sun vector is calculated by a polarization state least square method through an orientation algorithm of a polarization sensor, and the position of the sun is determined;

in the formula above, the first and second groups of the compound,a heading angle representing the direction of a probe on the vehicle,the difference in the angles of the elements is represented,is an estimate of the position of the sun;

the sun position is thus determined from the estimated values:

in the formula above, the first and second groups of the compound,the position of the sun, t the measurement time, arcalist (. cndot.) the arc length.

4. An underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation is characterized by comprising the following steps of 3: the steps of constructing the visual scene environment feature map are as follows:

1) according to the position information of the identification image, determining a target sample, namely an image sample of a region corresponding to the center of each node, and labeling, wherein in the navigation topological graph, n nodes contain m image samples, and the image target corresponding to each node is labeled as:

the above formula I_iIs the feature vector of the ith image sample, y_i∈[1,2,…,n]Is the purpose of a nodeLabeling;

2) training target samples in adjacent topological nodes according to a large-boundary neighbor algorithm in machine learning to obtain a new measurement matrix M, so that the target samples with the same labels are more compact, and the target samples with different labels are looser;

3) the distance metric between any two target samples is defined as:

d_M(I_i,I_i+j)＝(I_i-I_i+j)^TM(I_i-I_i+j)

above formula, d_MIs the Euclidean distance of two nodes in the reconstructed topological node space, d_M(I_i,I_i+j) In a linear relationship with the metric matrix, I_iAnd I_i+jA target neighbor sample;

4) solving the optimal solution of the following objective function by using the measurement matrix M:

let y_ilE {0,1} represents whether the characteristic vectors of the samples belong to the same node, if the characteristic vectors belong to the same node, the characteristic vectors are 1, otherwise, the characteristic vectors are 0; λ denotes the weight parameter, ε_ijlIs the total number of non-target samples

5) And (4) obtaining the expression and measurement of the topological graph on the motion environment through the steps, and completing the construction of the visual scene environment characteristic map.

5. An underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation is characterized by comprising the following steps of 4: the combined model of velocity inertial navigation measurement and nonlinear state polarized light measurement is established as follows:

the state model of the integrated navigation system is as follows:

above formula, F_IMU(t)、F_polar(t) represents a state coefficient matrix, G_IMU(t)、G_polar(t) denotes the system noise drive matrix, W_IMU(t)、W_polar(t) represents a system noise matrix;

the measurement equation of the integrated navigation system is as follows:

of the above formula, H_IMU(t)、H_polar(t) a non-linear function, U, representing the measurement equation_IMU(t)、U_polar(t) represents the measurement noise of the inertial sensor and the polarized light sensor, respectively.

6. An underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation is characterized by comprising the following steps of 5: performing data fusion through an AUKF-SLAM algorithm, establishing a combined navigation system model, and specifically constructing an environment map by the following steps:

1) when the time-varying noise statistics is estimated, old data is forgotten gradually, but the UKF algorithm does not have the change of the noise statistics, so the time-varying noise statistics estimator can be designed by using an attenuation memory weighting index method under the condition of unknown and time-varying noise statistics parameters by improving the algorithm, and a weighting coefficient is selected:

β_i＝β_i-1b,0<b<1,

can be written as:

β_i＝d_kb^i-1,i＝1,…,k，

thus, the recursive formula of the decaying memory time-varying noise statistic estimatorThe expression is as follows:

in the formula above, the first and second groups of the compound,in order to be the covariance of the system noise,for observing noise covariance, K is Kalman gain, W_iAre the weights of the mean and the covariance,satisfies the UKF sequence;

2) the AUKF algorithm calculates the self-adaptive factor coefficient lambda by introducing an attenuation factor formula_iThen using the coefficient to correct the covariance P of the predictable term_k|k-1Thereby, divergence of the filter is suppressed and convergence of the filter is ensured, and the convergence condition is as follows:

for attenuation factorFormula calculation of adaptive factor coefficient lambda_i：

Of the above formula, λ₀Tr (-) represents the matrix tracking for the initial value, S is the adjustable coefficient,is a residual sequence;

factor lambda in the algorithm_iThe larger the value, the smaller the proportion of information produced, resulting in a residual vector effect v_kThe more pronounced.

3) After noise reduction, performing data association on the acquired attitude, position, speed, course and the like by adopting a nearest neighbor data association algorithm; if the node target samples in the navigation topological graph are adjacent, merging is carried out, namely association is successful, and the underwater vehicle carries out time updating and measurement updating once; otherwise, adding the new metric as a new feature to the current state vector, namely performing state augmentation, and then continuing to perform merging, namely data association, of adjacent target samples, wherein the operations are as follows:

in the formula above, the first and second groups of the compound,is a new feature vector.

Through the improvement, the time updating and the measurement updating are carried out on the filter:

and (3) time updating:

measurement updating:

in the formula above, the first and second groups of the compound,andthe previous state vector, covariance vector and measurement vector of state at time k-1, x, respectively, of the system_i,k∣k-1、z_i,k∣k-1Values obtained for Sigma point sampling, W_i ^(m)And W_i ^(c)Weight, Q, of the system mean and covariance, respectively_k-1Covariance as system noise，Andare all observed covariances obtained in the system measurement update, R is the covariances of the observed noise, K_kIn order to be a matrix of the filter gains,andrespectively, the current state vector and covariance vector of the system.

Technical Field

The invention relates to an underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation, belongs to the field of underwater navigation, and meets the requirements of technical precision, reliability, concealment and sea area applicability of underwater navigation in different scenes.

Background

The bionic polarized light navigation as a new navigation technology has the advantages of good navigation performance, strong concealment and low interference, and is concerned by a plurality of scholars in recent years. In the existing research, the research on polarized sky light, whether sunlight or moonlight, is mainly focused on land, and the research on sea surface and underwater diving navigation is very little, however, the bionic polarized light navigation also has potential in water application. Since the underwater polarization mode is stable, and the polarization distribution of the underwater scattered light contains position information of the sun direction of the underwater radiance peak, many underwater living beings navigate by sensing the polarization information by using the marine scattered light polarization distribution mode. The invention provides an underwater polarized light navigation technology, which is used for assisting inertial navigation and realizing underwater autonomous passive navigation.

The simultaneous localization and mapping (SLAM) technology is applied to underwater navigation and has important research significance, the underwater vehicle is enabled not to depend on prior underwater environment information, required information is extracted from a sensor carried by the underwater vehicle, and a map is constructed through an environment model and environment characteristics. At present, the common navigation solution focuses on expressing by utilizing a motion environment, namely, images are used as characteristics, semantic information is used as driving, but motion space measurement is omitted, so that the navigation solution constructs the navigation topology of the motion environment in water, and the positioning and orientation accuracy of navigation is improved.

In conclusion, the method takes inertial navigation as a main body and polarized light bionic navigation as an auxiliary body, improves the identification expression of node characteristics in the aspect of constructing a map, and designs a polarized visual image enhancement model by utilizing a polarizing film and a camera. The scene polarization analysis is beneficial to scene identification and target identification detection; by adopting an AUKF-SLAM data fusion strategy, the stability and the self-adaptive capacity of a filtering algorithm can be enhanced, navigation errors can be corrected, and the method plays a vital role in future underwater environment detection and resource exploration.

Disclosure of Invention

For navigation positioning of an underwater vehicle, in order to overcome the defects of an inertial navigation system, the invention provides an underwater synchronous positioning and mapping method based on polarized light/inertial/visual integrated navigation, and an underwater integrated navigation system is constructed by adopting a polarized light/inertial/visual integrated navigation mode. The polarized light compass is used as an orientation result, the image information of the polarized visual scene is utilized, the characteristics of the topological nodes are used as an identification and expression method, the self-positioning of underwater navigation is realized, and meanwhile, an environment map is constructed. And an AUKF data fusion model is provided and applied to synchronous positioning and mapping of the underwater vehicle, so that more accurate navigation orientation positioning precision is obtained.

The technical scheme is as follows: the invention provides an underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation, which comprises the following five steps:

step 1: selecting the attitude, the speed and the position of the underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;

step 2: calculating a sun vector by adopting a least square method of a polarization state through an orientation algorithm of a polarization sensor, and determining the position of the sun;

and step 3: constructing a visual scene environment characteristic map by using image polarization information through a topological node identification algorithm;

and 4, step 4: according to the speed output of the inertial navigation system, establishing a speed inertial navigation measurement model and a polarized light nonlinear measurement model;

and 5: and integrating the steps, and establishing an integrated navigation system model through data fusion to construct an environment map.

Step 1: in SLAM research algorithm, the method is important for constructing a model for a vector. Selecting the attitude, the speed and the position of the underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;

the nonlinear equation of the motion model is as follows:

in the formula above, the first and second groups of the compound,andthe position abscissa, the position ordinate, the zenith coordinate and the course coordinate of the underwater vehicle are respectively, u is a control input vector, and w is a zero-mean Gaussian-distribution process noise vector;

step 2: calculating a sun vector by a least square method of a polarization state through an orientation algorithm of a polarization sensor;

in the formula above, the first and second groups of the compound,a heading angle representing the direction of a probe on the vehicle,representing an element angle difference;is an estimate of the position of the sun;

the sun position is thus determined from the estimated values:

in the formula above, the first and second groups of the compound,the position of the sun, t is the measurement time, and arcidist (-) is the arc length;

and step 3: the method comprises the steps that information is provided for autonomous navigation through a topological node as a carrier, positioning calculation is carried out, a scene and a detection target are identified through polarization information, and a visual scene environment characteristic map is constructed;

according to the position information of the identification image, determining a target sample (an image sample of a region corresponding to the center of each node), marking a mark, wherein in the navigation topological graph, n nodes contain m image samples, and the image target corresponding to each node is marked as:

the above formula I_iIs the feature vector of the ith image sample, y_i∈[1,2,…,n]Target labels for nodes;

then, training target samples in adjacent topological nodes according to a large-boundary neighbor algorithm in machine learning to obtain a new measurement matrix M, so that the target samples with the same label are more compact, and the target samples with different labels are looser;

the distance metric between any two target samples is defined as:

d_M(I_i,I_i+j)＝(I_i-I_i+j)^TM(I_i-I_i+j)

above formula, d_MIs the Euclidean distance of two nodes in the reconstructed topological node space, d_M(I_i,I_i+j) In a linear relationship with the metric matrix, I_iAnd I_i+jA target neighbor sample;

and finally, solving the optimal solution of the following objective function by using the measurement matrix M:

let y_ilE {0,1} represents whether the characteristic vectors of the samples belong to the same node, if the characteristic vectors belong to the same node, the characteristic vectors are 1, otherwise, the characteristic vectors are 0; λ denotes the weight parameter, ε_ijlThe total number of non-target samples. Therefore, an optimal solution can be obtained, and the expression and measurement of the topological graph on the motion environment are obtained;

and 4, step 4: establishing a combined model of velocity inertial navigation measurement and nonlinear state polarized light measurement;

the state model of the integrated navigation system is as follows:

the measurement equation of the integrated navigation system is as follows:

and 5: navigation data fusion of the underwater autonomous underwater vehicle is carried out by adopting an AUKF algorithm through course information provided by polarized light navigation, speed information provided by inertial navigation and position information provided by visual navigation, SLAM is carried out, and the steps are as follows:

1) reading speed, position and course observation data of the underwater vehicle respectively provided by an inertial navigation sensor, a visual odometer and a polarized light sensor;

2) selecting weighting coefficients

β_i＝d_kb^i-1,i＝1,…,k，

3) Calculating adaptive factor coefficients

Coefficient of adaptive factor lambda_iExpressed as:

of the above formula, λ₀Tr (-) represents the matrix tracking for the initial value, S is the adjustable coefficient,is a residual sequence;

4) attenuation memory time-varying noise statistic estimator

Recursive formula of attenuation memory time-varying noise statistic estimatorThe expression is as follows:

in the formula above, the first and second groups of the compound,in order to be the covariance of the system noise,for observing noise covariance, K is Kalman gain, W_iAre the weights of the mean and covariance,satisfies the UKF sequence;

5) through the noise reduction processing of the filter, the convergence condition of the filter is as follows:

6) performing data association on the acquired attitude, position, speed and course by adopting a nearest neighbor data association algorithm; if the nodes (target samples) in the navigation topological graph are adjacent, merging is carried out, namely association is successful, and the underwater vehicle carries out time updating and measurement updating once; otherwise, adding the new metric as a new feature to the current state vector, namely performing state augmentation, and then continuing to perform merging (namely data association) of adjacent target samples, as follows;

in the formula above, the first and second groups of the compound,is a new feature vector.

And (3) time updating:

measurement updating:

in the formula above, the first and second groups of the compound,andthe previous state vector, covariance vector and measurement vector of state at time k-1, x, respectively, of the system_i,k∣k-1、z_i,k∣k-1Is the value obtained by sampling the Sigma points,andweight, Q, of the system mean and covariance, respectively_k-1Is the covariance of the system noise and,andare all observed covariances obtained in the system measurement update, R is the covariances of the observed noise, K_kIn order to be a matrix of the filter gains,andrespectively, the current state vector and covariance vector of the system.

And finally, completing underwater synchronous positioning and mapping.

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

(1) the invention utilizes the polarized light bionic navigation principle, does not need the support of a geophysical field database, and can complete autonomous navigation in an underwater complex unstructured environment by researching the orientation algorithm of the underwater polarization sensor. The navigation mode is high in concealment and strong in robustness.

(2) Compared with the application scenario of the land SLAM, the underwater environment is dim and complex, and the visual sensor-based SLAM system has many limitations. Therefore, the invention fully utilizes the image information of the polarization vision, provides position data for the identification of the topology nodes of the visual scene, obviously improves the positioning and orientation precision of the integrated navigation and effectively inhibits the positioning accumulative error in the autonomous navigation process.

(3) At present, a Kalman filter is mostly adopted for navigation system data fusion, and the great disadvantage is that the accuracy of the established model is seriously depended on for the estimation precision of navigation error parameters. The typical UKF algorithm is a nonlinear filter which avoids the linearization of state or measurement equations, but the implementation principle has limitation and is not suitable for underwater SLAM. The invention provides the AUKF-SLAM algorithm, which can effectively inhibit the divergence of the filter and improve the quick tracking capability of the filter under the condition of unknown underwater and time-varying noise in deep sea areas and the like, thereby improving the performance of the integrated navigation system.

Drawings

FIG. 1 is a schematic diagram of an underwater polarized light/inertial/visual integrated navigation principle;

FIG. 2 is an underwater polarization state scattering model;

FIG. 3 is a schematic diagram of topology node feature identification;

FIG. 4 is a basic schematic diagram of the AUKF-SLAM algorithm.

Detailed Description

The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

Step 1: in SLAM research algorithm, the method is important for constructing a model for a vector. Selecting the attitude, the speed and the position of the underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;

the motion rule of the underwater vehicle is very complex, the motion needs to be simplified, and only the plane motion of two degrees of freedom of a horizontal plane and a vertical plane is considered. The augmented state of the motion model is defined as follows:

of the above formula, X_vA state vector matrix, { x, { for the current attitude in the spacecraft_iI-0.. k-1} is a node of the state vector in the attitude map, where X is_vIs defined as:

X_v＝[x y z y]^T

in the above formula, x, y and z are position coordinates, and y is the heading angle of the underwater vehicle in the horizontal reference coordinate system.

Since the corresponding error covariance matrix is extended, the augmented error covariance matrix can be expressed as:

of the above formula, P_v,vIs the covariance of the current attitude, P_v,jAnd P_i,jRespectively representing the covariance of the jth current state and the ith, jth previous state;

the system state equation is:

in the above formula, A is a transfer matrix, x_vIs the estimated value of the system state at the moment v, B is an input weighting matrix, u is a control input vector, w is a zero mean Gaussian distributed process noise vector, and the covariance matrix is Q_v。

Establishing a relational expression of the covariance of the current attitude error and the current attitude state vector, i.e.

According to the definition of the system state vector, establishing a motion model nonlinear equation:

in the formula above, the first and second groups of the compound,andthe position abscissa, the position ordinate, the zenith coordinate and the course coordinate of the underwater vehicle are respectively, u is a control input vector, and w is a zero-mean Gaussian-distribution process noise vector;

step 2: the underwater mode in the E-vector angle of the underwater polarization mode is less sensitive to disturbances (including surface waves) and therefore can rely on the position of the sun and from this determine the heading and pitch angles of the sun to accurately estimate the position of the observer. Therefore, the sun vector is calculated by adopting a least square method of the polarization state through the orientation algorithm of the polarization sensor;

using the Mueller-Stokes form of polarized light, a beam of polarized light consisting of wave vectors(representing wave number and propagation direction of polarized light) and Stokes vector(representing the intensity and polarization state of polarized light) indicates that sunlight (k)_i,x_i,S_i) Refraction in water, air-water interface from surface normal n and actual refractive index η of air and water_i,η_tDefinition, S, assuming too much sunlight is unpolarized_i＝(1,0,0,0)^TThus transmitting light (k) according to snell's law of refraction_t,x_t,S_t) Is represented as follows:

x_t＝x_i＝n×k_i/||n×k_i||

S_t＝M_RS_i

above formula, S_iAnd S_tStokes vectors of the incident angle and the emergence angle, respectively, where S_i＝(1 0 0 0)^T. Using the above formula, matrix M is transformed by coordinates_R→STo obtain S_tIn the underwater transmission sunlight model, the scattering matrix is expressed as M_SWave vector k of scattered light_sAnd k is_tThe size is the same, and the pointing detector is specifically expressed as follows:

S_s＝M_SM_R→SS_t

then the Stokes vector is:

S_d＝M_S→DM_SM_R→SM_RS_i

extracting a polarization angle by using Stokes vectors, and calculating an initial estimation value of the position of the sun in the sky angle as

L of the above formula₁The norm effect is to eliminate outliers caused by measurement noise, which, in the underwater scattering model, a course angle representing the direction of the probe on the vehicle, then:

denotes the element angle difference, τ denotes the polarization angle:

to eliminate the minimization of model errors, the sun position is estimated using the least squares method:

the sun position is thus finally determined from the estimated values:

in the above formula, t is the measurement time, and arcidist (-) is the arc length;

and step 3: after the solar altitude angle and the solar azimuth angle are determined through the polarization mode of the scattering model, the step provides information for autonomous navigation through the topological nodes as a carrier, positioning calculation is carried out, and the problem of building a visual scene environment feature map is analyzed by utilizing polarization information to identify a scene and a detection target;

a schematic diagram of identifying characteristics of topology nodes, as shown in fig. 3, is a comparison diagram before and after training of a target sample, when a navigation topology map of an underwater environment is constructed, identification of the topology nodes is critical, and in a complex underwater environment, the environmental characteristics of some adjacent nodes may show high similarity, so that if a frame in a current node has too much similarity with a frame in a previous node, two adjacent nodes are merged, so that a memory of map can be saved;

the specific method comprises the following steps: according to the position information of the identification image, determining a target sample (an image sample of a region corresponding to the center of each node), marking a label, wherein in the navigation topological graph, n nodes contain m image samples, and the image target corresponding to each node is marked as:

the above formula I_iIs the feature vector of the ith image sample, y_i∈[1,2,…,n]The destination label of the node.

And then training target samples in adjacent topological nodes according to a large-boundary neighbor algorithm in machine learning to obtain a new measurement matrix M, so that the target samples with the same label are more compact, and the target samples with different labels are looser. The distance metric between any two target samples is defined as:

d_M(I_i,I_i+j)＝(I_i-I_i+j)^TM(I_i-I_i+j)

above formula, d_MIs the Euclidean distance of two nodes in the reconstructed topological node space, d_M(I_i,I_i+j) In a linear relationship with the metric matrix, I_iAnd I_i+jA target neighbor sample;

and finally, solving the optimal solution of the following objective function by using the measurement matrix M:

let y_ilE {0,1} represents whether the characteristic vectors of the samples belong to the same node, if the characteristic vectors belong to the same node, the characteristic vectors are 1, otherwise, the characteristic vectors are 0; λ represents a weight parameter, ε_ijlThe total number of non-target samples. Therefore, the optimal solution can be obtained, and the expression and degree of the topological graph to the motion environment can be obtainedAn amount;

and 4, step 4: establishing a combined model of velocity inertial navigation measurement and nonlinear state polarized light measurement;

in the integrated navigation, attitude angle error, speed error, position error and gyro random drift in an inertial navigation system are taken as state variables of the system, namely:

in the formula above, the first and second groups of the compound,for inertial navigation attitude angle error, δ v_E,δv_N,δv_uFor velocity error, δ L, δ λ, δ h are position errors, ε_bx,ε_by,ε_bzIs a constant error of the gyro random drift, epsilon_rx,ε_ry,ε_nA first order markov process for gyroscopic random drift,random error for the accelerometer;

establishing state equations of each subsystem of inertial navigation and polarized light, wherein the state equations respectively comprise:

above formula, F_IMU(t)、F_polar(t) represents a state coefficient matrix, G_IMU(t)、G_polar(t) denotes the system noise drive matrix, W_IMU(t)、W_polar(t) represents a system noise matrix;

in summary, the state model of the integrated navigation system is:

the measurement model of the system is established as follows:

Y_IMU(t)＝H_IMU(t)+U_IMU(t)

Y_polar(t)＝H_polar(t)+U_polar(t)

of the above formula, H_IMU(t)、H_polar(t) a non-linear function, U, representing the measurement equation_IMU(t)、U_polar(t) represents the measurement noise of the inertial sensor and the polarized light sensor, respectively, wherein:

in summary, the measurement equation of the integrated navigation system is:

and 5: and integrating the steps, establishing a combined navigation system model through data fusion, and constructing an environment map.

The invention provides an AUKF algorithm for SLAM research (AUKF-SLAM), namely, the steps are integrated, navigation data fusion of an underwater autonomous underwater vehicle is carried out through course information provided by polarized light navigation, speed information provided by inertial navigation and position information provided by visual navigation, the algorithm solves the problems of precision reduction and divergence of the traditional UKF under the unknown and time-varying conditions, and introduces adaptive fading factors to correct prediction error covariance and adjust a filter gain matrix K, so that the divergence of a filter can be inhibited, and the rapid tracking capability is improved. The basic principle of the AUKF-SLAM algorithm is shown in FIG. 4, and the specific steps are as follows:

old data is gradually forgotten when the time-varying noise statistic is estimated, but the UKF algorithm does not have the change of the noise statistic, so the algorithm can be improved to design a time-varying noise statistic estimator by using an attenuation memory weighted index method under the condition of unknown and time-varying noise statistic parameters. Selecting a weighting coefficient:

β_i＝β_i-1b,0<b<1,

can be written as:

β_i＝d_kb^i-1,i＝1,…,k，

thus, the recursive formula of the decaying memory time-varying noise statistic estimatorThe expression is as follows:

in the formula above, the first and second groups of the compound,in order to be the covariance of the system noise,for observing noise covariance, K is Kalman gain, W_iAre the weights of the mean and the covariance,satisfies the UKF sequence;

the AUKF algorithm calculates the self-adaptive factor coefficient lambda by introducing an attenuation factor formula_iThen using the coefficient to correct the covariance P of the predictable term_k|k-1Thereby, divergence of the filter is suppressed and convergence of the filter is ensured. The convergence conditions are as follows:

calculating adaptive factor coefficient lambda for attenuation factor formula_i：

Of the above formula, λ₀Tr (-) represents the matrix tracking for the initial value, S is the adjustable coefficient,is a residual sequence;

factor lambda in the algorithm_iThe larger the value, the smaller the proportion of information produced, resulting in a residual vector effect v_kThe more pronounced. Therefore, the algorithm not only inhibits the divergence of the filter and improves the noise reduction capability of the filter, but also has strong tracking capability on the condition of sudden change of state;

after noise reduction, performing data association on the acquired attitude, position, speed, course and the like by adopting a nearest neighbor data association algorithm; if the nodes (target samples) in the navigation topological graph are adjacent, merging is carried out, namely association is successful, and the underwater vehicle carries out time updating and measurement updating once; otherwise, adding the new metric as a new feature to the current state vector, i.e. performing state augmentation, and then continuing to perform merging (i.e. data correlation) of adjacent target samples. The operation is as follows:

in the formula above, the first and second groups of the compound,is a new feature vector.

Through the improvement, the time updating and the measurement updating are carried out on the filter:

and (3) time updating:

measurement updating:

in the formula above, the first and second groups of the compound,andthe previous state vector, covariance vector and measurement vector of state at time k-1, x, respectively, of the system_i,k∣k-1、z_i,k∣k-1Values obtained for Sigma point sampling, W_i ^(m)And W_i ^(c)Weight, Q, of the system mean and covariance, respectively_k-1Is the covariance of the system noise and,andare all observed covariances obtained in the system measurement update, R is the covariances of the observed noise, K_kIn order to be a matrix of the filter gains,andrespectively, the current state vector and covariance vector of the system.

In conclusion, the AUKF-SLAM algorithm provides a feasible and novel method for simultaneously positioning and mapping underwater under unknown environment.