CN113739795B - Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation - Google Patents
Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation Download PDFInfo
- Publication number
- CN113739795B CN113739795B CN202110620366.9A CN202110620366A CN113739795B CN 113739795 B CN113739795 B CN 113739795B CN 202110620366 A CN202110620366 A CN 202110620366A CN 113739795 B CN113739795 B CN 113739795B
- Authority
- CN
- China
- Prior art keywords
- underwater
- measurement
- navigation
- vector
- polarized light
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000013507 mapping Methods 0.000 title claims abstract description 14
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 12
- 230000010287 polarization Effects 0.000 claims abstract description 28
- 230000004927 fusion Effects 0.000 claims abstract description 10
- 239000013598 vector Substances 0.000 claims description 60
- 238000005259 measurement Methods 0.000 claims description 41
- 239000011159 matrix material Substances 0.000 claims description 29
- 239000000523 sample Substances 0.000 claims description 16
- 230000000007 visual effect Effects 0.000 claims description 13
- 230000006870 function Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 230000003416 augmentation Effects 0.000 claims description 4
- 238000012549 training Methods 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 3
- 238000010801 machine learning Methods 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 2
- 230000006872 improvement Effects 0.000 claims description 2
- 238000002372 labelling Methods 0.000 claims description 2
- 238000000605 extraction Methods 0.000 claims 1
- 238000005562 fading Methods 0.000 claims 1
- 230000004807 localization Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 6
- 238000011160 research Methods 0.000 description 6
- 238000001514 detection method Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 239000011664 nicotinic acid Substances 0.000 description 4
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000002238 attenuated effect Effects 0.000 description 2
- 239000000969 carrier Substances 0.000 description 2
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000009189 diving Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses an underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation, which belongs to the field of underwater navigation and comprises the following steps: firstly, measuring underwater polarization angles of different places, depths and time by utilizing the principle that an underwater polarization mode depends on solar position information, and simultaneously, accurately measuring speed, attitude, position and course angle information of an underwater vehicle by utilizing a polarized light sensor; secondly, researching a recognition method and a feature expression method of topological nodes by utilizing polarization information of a scene so as to better recognize the scene and detect a target; finally, in consideration of the practicability of the underwater filter, a self-Adaptive Unscented Kalman Filter (AUKF) data fusion strategy is provided, parameters such as unknown system noise and the like are estimated on line by an improved maximum unbiased posterior noise statistics estimator, and the method is applied to simultaneous localization and mapping (SLAM) of the underwater vehicle, namely AUKF-SLAM is realized. The invention can obviously improve the positioning and orientation precision of the integrated navigation.
Description
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 of different scenes.
Background
Bionic polarized light navigation is an emerging navigation technology, has the advantages of good navigation performance, strong concealment and low interference, and is paid attention to by a plurality of students in recent years. In the existing researches, the research of sky polarized light is mainly focused on land, and the research of sea surface and underwater diving is very little, however, the bionic polarized light navigation also has potential of being applied in water. Since the underwater polarization mode is stable, the polarization distribution of the underwater scattered light contains the position information of the sun direction of the underwater emissivity peak, many underwater organisms navigate through the perception of polarization information using the ocean scattered light polarization distribution mode. The invention provides an underwater polarized light navigation technology, which assists inertial navigation and realizes autonomous underwater passive navigation.
The simultaneous localization and mapping (SLAM) technology is applied to underwater navigation, and the purpose of the SLAM technology is to enable the underwater vehicle not to depend on priori underwater environment information, but to extract needed information from sensors carried by the underwater vehicle, and to construct a map through an environment model and environment characteristics. At present, the common navigation calculation is focused on the expression by using the motion environment, namely, the image is taken as a characteristic and semantic information is taken as a drive, but the motion space measurement is ignored, so the invention focuses on reconstructing the navigation topology of the underwater motion environment, and improves the positioning and orientation precision of navigation.
In conclusion, the invention takes inertial navigation as a main body and polarized light bionic navigation as an auxiliary body, improves the identification expression of the node characteristics in the aspect of constructing a map, and designs a polarized visual image enhancement model by using a polaroid and a camera. Analyzing scene polarization, and facilitating scene identification and target identification detection; the AUKF-SLAM data fusion strategy is adopted, so that the stability and 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 the navigation positioning of the 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/inertia/vision combined navigation, which adopts a polarized light/inertia/vision combined navigation mode to construct the underwater combined navigation system. The polarized light compass is used as an orientation result, polarized visual scene image information is utilized, characteristics of topological nodes are used as identification and expression methods, self-positioning of underwater navigation is achieved, and meanwhile an environment map is built. The AUKF data fusion model is provided and applied to synchronous positioning and map building of the underwater vehicle, so that more accurate navigation orientation positioning accuracy 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: the method comprises the steps of selecting the posture, the speed and the position of an underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;
step 2: calculating a solar 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;
step 3: constructing a visual scene environment characteristic map by using image polarization information through a topological node identification algorithm;
step 4: according to the speed output of the inertial navigation system, a speed inertial navigation measurement model and a polarized light nonlinear measurement model are established;
step 5: and integrating the steps, establishing a combined navigation system model through data fusion, and constructing an environment map.
Step 1: in SLAM research algorithms, it is critical to the vector construction model. The method comprises the steps of selecting the posture, the speed and the position of an underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;
the nonlinear equation of the motion model is:
the upper part of the device is provided with a plurality of grooves,is->The position abscissa, the position ordinate, the zenith coordinate and the heading coordinate of the submarine are respectively, u is a control input vector, and w is a zero-mean Gaussian distributed process noise vector;
step 2: calculating a solar vector by adopting a least square method of a polarization state through an orientation algorithm of a polarization sensor;
the upper part of the device is provided with a plurality of grooves,heading angle indicating the direction of the probe on the submarine, +.>Representing the element angle difference; />Is an estimate of the sun position;
thereby determining the sun position from the estimate:
the upper part of the device is provided with a plurality of grooves,the sun position, t is the measurement time, and arclist (·) is the arc length;
step 3: providing information for autonomous navigation by taking topological nodes as carriers, carrying out positioning calculation, identifying a scene and a detection target by utilizing polarization information, and constructing a visual scene environment characteristic map;
according to the position information of the identification image, determining a target sample (an image sample of a corresponding area of the center of each node), and marking a mark, wherein in the navigation topological graph, n nodes comprise m image samples in total, and the corresponding image targets of each node are marked as follows:
above, I i Feature vector, y, for the ith image sample i ∈[1,2,…,n]A target label for a node;
training target samples in adjacent topological nodes according to a large boundary neighbor algorithm in machine learning to obtain a new metric matrix M, so that the target samples with the same label are more compact, and the target samples with different labels are more loose;
defining a distance measure between any two target samples as follows:
d M (I i ,I i+j )＝(I i -I i+j ) T M(I i -I i+j )
above, d M D is Euclidean distance of two nodes in the reconstructed topological node space M (I i ,I i+j ) Linear relation with metric matrix, I i And I i+j A target neighbor sample;
finally, solving an optimal solution of the following objective function by using a metric matrix M:
let y il E {0,1} indicates whether the feature vectors of the samples belong to the same node, if so, the feature vectors are 1, otherwise, the feature vectors are 0; lambda represents the weight parameter epsilon ijl Is the total number of non-target samples. Thus the most can be obtainedObtaining the expression and measurement of the topological graph on the motion environment by using the optimal solution;
step 4: establishing a combined model of speed 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:
step 5: and carrying out navigation data fusion of the underwater autonomous underwater vehicle by adopting an AUKF algorithm through heading information provided by polarized light navigation, speed information provided by inertial navigation and position information provided by visual navigation, and carrying out SLAM, wherein the steps are as follows:
1) Reading the speed, position and course observation data of the submarine provided by the inertial navigation sensor, the visual odometer and the polarized light sensor respectively;
2) Selecting weighting coefficients
β i ＝d k b i-1 ,i＝1,…,k，
3) Calculating adaptive factor coefficients
Adaptive factor coefficient lambda i Expressed as:
above, lambda 0 For initial values, tr (·) represents matrix tracking, S is an adjustable coefficient,is a residual sequence;
4) Attenuated memory time-varying noise statistical estimator
Recurrence formula of attenuation memory time-varying noise statistical estimatorThe expression is as follows:
the upper part of the device is provided with a plurality of grooves,is the system noise covariance +>To observe the noise covariance, K is the Kalman gain, W i Weights for mean and covariance, +.>The UKF sequence is satisfied;
5) The convergence condition of the filter is as follows:
6) Adopting a nearest neighbor data association algorithm to carry out data association on the acquired gesture, position, speed and course; if nodes (target samples) in the navigation topological graph are adjacent, merging, namely, successful association is achieved, and the submarine is subjected to time updating and measurement updating once at the moment; 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 association) of adjacent target samples, as follows;
the upper part of the device is provided with a plurality of grooves,is a new feature vector.
And (5) updating time:
and (5) measurement and update:
the upper part of the device is provided with a plurality of grooves,is->The measurement vectors, x, are the previous state vector, covariance vector and state at time k-1 of the system respectively i,k∣k-1 、z i,k∣k-1 Values obtained for Sigma point samples, +.>And->Weights of mean and covariance of system, Q k-1 Is the covariance of the system noise, +.>And->All are observed covariances obtained in system measurement update, R is covariance of observed noise, K k For the filter gain matrix +.>And->The current state vector and covariance vector of the system are respectively.
Finally, underwater synchronous positioning and mapping are completed.
Compared with the prior art, the invention has the advantages that:
(1) According to the invention, by utilizing the polarized light bionic navigation principle, the support of a geophysical field database is not needed, and autonomous navigation can be completed in an underwater complex unstructured environment by researching an underwater polarized sensor orientation algorithm. The navigation mode has high concealment and strong robustness.
(2) Compared with the application scene of land SLAM, the underwater environment is dim and complex, and has a plurality of limitations on the SLAM system based on the vision sensor. Therefore, the invention fully utilizes the image information of polarized vision, provides position data for the identification of the topological nodes of the visual scene, obviously improves the positioning and orientation precision of the combined navigation and effectively suppresses the positioning cumulative error in the autonomous navigation process.
(3) At present, a Kalman filter is mostly adopted in data fusion of a navigation system, and the great disadvantage is that the estimation accuracy of navigation error parameters is seriously dependent on the accuracy of an established model. The typical UKF algorithm is a type of nonlinear filter that avoids linearization of the state or measurement equation, but its implementation principle has limitations and is not suitable for underwater SLAM. The invention provides an AUKF-SLAM algorithm, which can effectively inhibit the divergence of a filter and improve the rapid tracking capability of the filter under the conditions of unknown underwater noise 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 the principle of underwater polarized light/inertial/visual integrated navigation;
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 invention will be described in detail below with reference to the drawings and the detailed description.
Step 1: in SLAM research algorithms, it is critical to the vector construction model. The method comprises the steps of selecting the posture, the speed and the position of an underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;
the motion law of the underwater vehicle is quite complex, the motion is simplified, and only plane motion with two degrees of freedom of a horizontal plane and a vertical plane is considered. The augmentation state of the motion model is defined as follows:
above, X v Is a state vector matrix of the current gesture in the submarine, x i I=0,..Node, wherein X v The definition is as follows:
X v ＝[x y z y] T
the above formula, x, y, z are the position coordinates, and y is the course angle of the submarine in the horizontal reference coordinate system.
Since the corresponding error covariance matrix is extended, the augmented error covariance matrix can be expressed as:
above, P v,v For covariance of current pose, P v,j And P i,j Covariance of the current state of the j and the previous state of the i, j are respectively represented;
the system state equation is:
above, A is a transfer matrix, x v The system state estimated value at the moment v is represented by a system state estimated value at the moment v, B is an input weighting matrix, u is a control input vector, w is a zero-mean Gaussian distribution process noise vector, and the covariance matrix is represented by Q v 。
Establishing a relational expression of the covariance of the current attitude error and the current attitude state vector, namely
According to the definition of the system state vector, a motion model nonlinear equation is established:
the upper part of the device is provided with a plurality of grooves,is->The position abscissa, the position ordinate, the zenith coordinate and the heading coordinate of the submarine are respectively, u is a control input vector, and w is a zero-mean Gaussian distributed process noise vector;
step 2: the underwater mode in the underwater polarization mode E-vector angle is less sensitive to disturbance (including surface waves), so that the position of the sun can be relied on, and the course angle and the pitch angle of the sun can be determined according to the position of the sun, so that the position of an observer can be accurately estimated. The step calculates the solar vector by adopting a least square method of the polarization state through an orientation algorithm of the polarization sensor;
with the Mueller-Stokes form of polarized light, a beam of polarized light is represented by a wave vector(representing wave number and propagation direction of polarized light) and Stokes vector +.>(representing the intensity and polarization state of polarized light) represents 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 ,η t Definition, assuming that the Taiyang light is unpolarized, S i ＝(1,0,0,0) T Thus, according to the snell's law of refraction, light (k t ,x t ,S t ) The expression is as follows:
x t ＝x i ＝n×k i /||n×k i ||
S t ＝M R S i
above, S i And S is t Stokes vectors of the incident angle and the exit angle, respectively, where S i ＝(1 0 0 0) T . By means of the above, through a coordinate transformation matrix M R→S Obtaining S t In the underwater transmitted sunlight model, the scattering matrix is denoted as M S Wave vector k of scattered light s And k is equal to t The pointing detector is the same in size and is specifically shown as follows:
S s ＝M S M R→S S t
the Stokes vector is then:
S d ＝M S→D M S M R→S M R S i
extracting a polarization angle by using Stokes vector, and calculating an initial estimated value of the position of the sun in the sky angle as
L of the above type 1 The norm functions to eliminate outliers caused by measurement noise, which, in the underwater scattering model, heading angle representing the direction of the probe on the submarine, then:
representing the element angle difference, τ represents the polarization angle:
to eliminate minimization of model errors, the sun position is estimated using the least squares method:
thereby finally determining the sun position according to the estimated value:
in the above formula, t is the measurement time, and arclist (·) is the arc length;
step 3: after the sun altitude angle and azimuth angle are determined through the polarization mode of the scattering model, the step is to provide information through autonomous navigation by taking topological nodes as carriers, perform positioning calculation, identify scenes and detection targets by utilizing polarization information, and analyze the problem of building the environment characteristic map of the visual scene;
the topology node characteristic recognition schematic diagram, as shown in fig. 3, is a comparison diagram before and after training a target sample, is critical in the recognition of the topology node when constructing a navigation topology diagram of an underwater environment, and can show high similarity of environmental characteristics of some adjacent nodes in a complex underwater environment, so that if frames in a current node and frames in a previous node have too much similarity, two adjacent nodes are combined, and the memory of a 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 corresponding area in the center of each node), and labeling, wherein in the navigation topological graph, n nodes comprise m image samples in total, and the corresponding image targets of each node are as follows:
above, I i Feature vector for the ith image sample，y i ∈[1,2,…,n]Is the target 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 metric matrix M, so that the target samples with the same label are more compact, and the target samples with different labels are more loose. Defining a distance measure between any two target samples as follows:
d M (I i ,I i+j )＝(I i -I i+j ) T M(I i -I i+j )
above, d M D is Euclidean distance of two nodes in the reconstructed topological node space M (I i ,I i+j ) Linear relation with metric matrix, I i And I i+j A target neighbor sample;
finally, solving an optimal solution of the following objective function by using a metric matrix M:
let y il E {0,1} indicates whether the feature vectors of the samples belong to the same node, if so, the feature vectors are 1, otherwise, the feature vectors are 0; lambda represents the weight parameter epsilon ijl Is the total number of non-target samples. Therefore, the optimal solution can be obtained, and the expression and the measurement of the topological graph on the motion environment can be obtained;
step 4: establishing a combined model of speed 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:
the upper part of the device is provided with a plurality of grooves,for inertial navigation attitude angle error δv E ,δv N ,δv u Is the speed error, delta L, delta lambda, delta h is the position error, epsilon bx ,ε by ,ε bz Is the constant error of random drift of the gyro, epsilon rx ,ε ry ,ε n First order Markov process for random drift of gyroscopes, < >>Random errors for the accelerometer;
and establishing state equations of all subsystems of inertial navigation and polarized light, wherein the state equations are respectively as follows:
above, F IMU (t)、F polar (t) represents a state coefficient matrix, G IMU (t)、G polar (t) represents a system noise driving 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)
above, H IMU (t)、H polar (t) represents a nonlinear function of the measurement equation, U IMU (t)、U polar (t) represents measurement noise of the inertial sensor and the polarized light sensor, respectively, wherein:
in summary, the measurement equation of the integrated navigation system is:
step 5: integrating the steps, establishing a combined navigation system model through data fusion, and constructing an environment map.
The invention provides an AUKF algorithm for carrying out SLAM research (AUKF-SLAM), namely integrating the steps, and carrying out navigation data fusion of an underwater autonomous underwater vehicle through heading information provided by polarized light navigation, speed information provided by inertial navigation and position information provided by visual navigation. The basic principle of the AUKF-SLAM algorithm is shown in figure 4, and the specific steps are as follows:
when estimating the time-varying noise statistics, old data should be forgotten gradually, but the UKF algorithm does not have the change of noise statistics, so the time-varying noise statistics estimator can be designed by using the attenuation memory weighted exponential method under the condition of unknown and time-varying noise statistics parameters by improving the algorithm. Selecting a weighting coefficient:
β i ＝β i-1 b,0<b<1,
can be written as:
β i ＝d k b i-1 ,i＝1,…,k，
thus, attenuated memory time-varying noise statistical estimationRecursive formula of deviceThe expression is as follows:
the upper part of the device is provided with a plurality of grooves,is the system noise covariance +>To observe the noise covariance, K is the Kalman gain, W i Weights for mean and covariance, +.>The UKF sequence is satisfied;
AUKF algorithm calculates self-adaptive factor coefficient lambda by introducing attenuation factor formula i The coefficient is then used to correct the covariance P of the predictable term k|k-1 Thereby suppressing divergence of the filter and ensuring convergence of the filter. The convergence conditions are as follows:
for the attenuation factor formula, calculating the adaptive factor coefficient lambda i ：
Above, lambda 0 For initial values, tr (·) represents matrix tracking, S is an adjustable coefficient,is a residual sequence;
the factor lambda in the algorithm i The larger the value, the smaller the proportion of information generated, resulting in residual vector effect v k The 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 abrupt state change;
after noise reduction, adopting a nearest neighbor data association algorithm to carry out data association on the acquired gesture, position, speed, course and the like; if nodes (target samples) in the navigation topological graph are adjacent, merging, namely, successful association is achieved, and the submarine is subjected to time updating and measurement updating once at the moment; otherwise, the new metric is added as a new feature to the current state vector, i.e., state augmentation is performed, and then merging (i.e., data correlation) of neighboring target samples continues to be performed. The operation is as follows:
the upper part of the device is provided with a plurality of grooves,is a new feature vector.
Through the improvement, the filter is subjected to time updating and measurement updating:
and (5) updating time:
and (5) measurement and update:
/>
the upper part of the device is provided with a plurality of grooves,is->The measurement vectors, x, are the previous state vector, covariance vector and state at time k-1 of the system respectively i,k∣k-1 、z i,k∣k-1 The value obtained for Sigma Point sampling, W i (m) And W is i (c) Weights of mean and covariance of system, Q k-1 Is the covariance of the system noise, +.>And->All are observed covariances obtained in system measurement update, R is covariance of observed noise, K k For the filter gain matrix +.>And->The current state vector and covariance vector of the system respectively.
In summary, the AUKF-SLAM algorithm provides a feasible and novel method for simultaneous underwater positioning and mapping in an unknown environment.
Claims (4)
1. The underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation is characterized by comprising the following five steps:
step 1: the method comprises the steps of selecting the posture, the speed and the position of an underwater vehicle as a system state, and establishing a kinematic model of the underwater vehicle;
step 2: calculating a solar 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;
step 3: analyzing the problems of visual scene feature extraction and matching by using image polarization information through a topological node recognition algorithm;
step 4: establishing a speed inertial navigation measurement model and a polarized light nonlinear measurement model according to the speed output of the inertial navigation system;
step 5: integrating the steps, establishing a combined navigation system model through data fusion, and constructing an environment map;
the method for calculating the sun vector and determining the sun position by adopting a least square method of the polarization state through an orientation algorithm of the polarization sensor in the step 2 comprises the following steps:
the upper part of the device is provided with a plurality of grooves,heading angle indicating the direction of the probe on the submarine, +.>Representing the angle difference of the elements>Is an estimate of the sun position;
thereby determining the sun position from the estimate:
the upper part of the device is provided with a plurality of grooves,the sun position, t is the measurement time, and arclist (·) is the arc length;
step 3: the method for constructing the visual scene environment characteristic map comprises the following steps of:
1) According to the position information of the identification image, determining a target sample, namely an image sample of a corresponding area of the center of each node, and labeling, wherein in the navigation topological graph, n nodes comprise m image samples in total, and the corresponding image targets of each node are as follows:
above, I i Feature vector, y, for the ith image sample i ∈[1,2,…,n]A target label for a node;
2) Training target samples in adjacent topological nodes according to a large boundary neighbor algorithm in machine learning to obtain a new metric matrix M, so that the target samples with the same label are more compact, and the target samples with different labels are more loose;
3) Defining a distance measure between any two target samples as follows:
d M (I i ,I i+j )＝(I i -I i+j ) T M(I i -I i+j )
above, d M D is Euclidean distance of two nodes in the reconstructed topological node space M (I i ,I i+j ) Linear relation with metric matrix, I i And I i+j A target neighbor sample;
4) Solving an optimal solution of the following objective function by using a metric matrix M:
let y il E {0,1} indicates whether the feature vectors of the samples belong to the same node, if so, the feature vectors are 1, otherwise, the feature vectors are 0; lambda represents the weight parameter epsilon ijl Is the total number of non-target samples;
5) And obtaining the expression and measurement of the topological graph on the motion environment by the steps, and completing the construction of the visual scene environment characteristic map.
2. The method for underwater synchronous positioning and mapping based on polarized light/inertia/vision integrated navigation according to claim 1, wherein the nonlinear equation of the kinematic model of the submarine established in step 1 is:
the upper part of the device is provided with a plurality of grooves,is->The position abscissa, the position ordinate, the zenith coordinate and the heading coordinate of the submarine are respectively, u is a control input vector, and w is a zero-mean Gaussian distribution process noise vector.
3. The method for underwater synchronous positioning and mapping based on polarized light/inertia/vision combined navigation according to claim 1, wherein the following steps are carried out: 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:
above, F IMU (t)、F polar (t) represents a state coefficient matrix, G IMU (t)、G polar (t) represents a system noise driving matrix, W IMU (t)、W polar (t) represents a system noise matrix;
the measurement equation of the integrated navigation system is:
above, H IMU (t)、H polar (t) represents a nonlinear function of the measurement equation, U IMU (t)、U polar (t) represents measurement noise of the inertial sensor and the polarized light sensor, respectively.
4. The method for underwater synchronous positioning and mapping based on polarized light/inertia/vision combined navigation according to claim 1, wherein the following steps are carried out: data fusion is carried out through an AUKF-SLAM algorithm, a combined navigation system model is established, and the specific steps of building an environment map are as follows:
1) When estimating the time-varying noise statistics, old data should be forgotten gradually, but the UKF algorithm does not have the change of noise statistics, so the time-varying noise statistics estimator can be designed by using an attenuation memory weighted exponential method under the condition of unknown and time-varying noise statistics parameters by improving the algorithm, and the weighting coefficients are selected:
β i ＝β i-1 b,0<b<1,
can be written as:
β i ＝d k b i-1 ,i＝1,…,k，
therefore, the recurrence formula of the fading memory time-varying noise statistics estimatorThe expression is as follows:
the upper part of the device is provided with a plurality of grooves,is the system noise covariance +>To observe the noise covariance, K is the Kalman gain, W i Weights for mean and covariance, +.>Satisfy UKF sequence；
2) AUKF algorithm calculates self-adaptive factor coefficient lambda by introducing attenuation factor formula i The coefficient is then used to correct the covariance P of the predictable term k|k-1 Thereby suppressing divergence of the filter and ensuring convergence of the filter, the convergence conditions are as follows:
for the attenuation factor formula, calculating the adaptive factor coefficient lambda i ：
Above, lambda 0 For initial values, tr (·) represents matrix tracking, S is an adjustable coefficient,is a residual sequence;
the factor lambda in the algorithm i The larger the value, the smaller the proportion of information generated, resulting in residual vector effect v k The more pronounced;
3) After noise reduction, adopting a nearest neighbor data association algorithm to carry out data association on the acquired gesture, position, speed, course and the like; if the node target samples in the navigation topological graph are adjacent, merging, namely, successful association is achieved, and the submarine is subjected to one-time updating and measurement updating; 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 of adjacent target samples, i.e. data association, as follows:
the upper part of the device is provided with a plurality of grooves,is a new feature vector;
through the improvement, the filter is subjected to time updating and measurement updating:
and (5) updating time:
and (5) measurement and update:
the upper part of the device is provided with a plurality of grooves,is->The measurement vectors, x, are the previous state vector, covariance vector and k-1 time state of the system respectively i,k∣k-1 、z i,k∣k-1 Values obtained for Sigma point samples, +.>And->Weights of mean and covariance of system, Q k-1 Is the covariance of the system noise, +.>And->All are observed covariances obtained in system measurement update, R is covariance of observed noise, K k For the filter gain matrix +.>And->The current state vector and covariance vector of the system are respectively.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110620366.9A CN113739795B (en) | 2021-06-03 | 2021-06-03 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110620366.9A CN113739795B (en) | 2021-06-03 | 2021-06-03 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113739795A CN113739795A (en) | 2021-12-03 |
| CN113739795B true CN113739795B (en) | 2023-10-20 |
Family
ID=78728422
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110620366.9A Active CN113739795B (en) | 2021-06-03 | 2021-06-03 | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113739795B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114353774B (en) * | 2022-01-17 | 2024-04-30 | 青岛智海牧洋科技有限公司 | Underwater light pattern compass device |
| CN115014313B (en) * | 2022-05-30 | 2023-08-22 | 中北大学 | Polarized light compass heading error processing method based on parallel multi-scale |
| CN114910080B (en) * | 2022-07-15 | 2022-10-21 | 北京航空航天大学 | Three-dimensional navigation attitude determination method based on underwater downlink radiation light intensity and polarized light field |
| CN116182855B (en) * | 2023-04-28 | 2023-07-07 | 北京航空航天大学 | Combined navigation method of compound eye-simulated polarized vision unmanned aerial vehicle under weak light and strong environment |
| CN116222580B (en) * | 2023-05-09 | 2023-07-25 | 北京航空航天大学 | Underwater polarization orientation method based on cross-medium refraction interference compensation correction |
Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5270883A (en) * | 1991-08-29 | 1993-12-14 | Mitsubishi Denki Kabushiki Kaisha | Magnetic read/write circuit |
| CN103712621A (en) * | 2013-12-23 | 2014-04-09 | 大连理工大学 | Method for determining attitude of polarized light and infrared sensor auxiliary inertial navigation system |
| CN106767752A (en) * | 2016-11-25 | 2017-05-31 | 北京航空航天大学 | A kind of Combinated navigation method based on polarization information |
| CN107589748A (en) * | 2017-08-21 | 2018-01-16 | 江苏科技大学 | AUV autonomous navigation methods based on UnscentedFastSLAM algorithms |
| CN108362288A (en) * | 2018-02-08 | 2018-08-03 | 北方工业大学 | Polarized light S L AM method based on unscented Kalman filtering |
| CN108387236A (en) * | 2018-02-08 | 2018-08-10 | 北方工业大学 | Polarized light S L AM method based on extended Kalman filtering |
| CN108827288A (en) * | 2018-04-12 | 2018-11-16 | 东北电力大学 | A kind of dimensionality reduction strapdown inertial navigation system Initial Alignment Method and system based on dual quaterion |
| CN110672131A (en) * | 2019-11-19 | 2020-01-10 | 北方工业大学 | UKF (unscented Kalman Filter) alignment method for inertial/polarized light integrated navigation system under large misalignment angle |
| CN111024076A (en) * | 2019-12-30 | 2020-04-17 | 北京航空航天大学 | Underwater combined navigation system based on bionic polarization |
| CN111045454A (en) * | 2019-12-30 | 2020-04-21 | 北京航空航天大学 | Unmanned aerial vehicle self-driving instrument based on bionic autonomous navigation |
| CN111504312A (en) * | 2020-07-02 | 2020-08-07 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle pose estimation method based on visual inertial polarized light fusion |
| CN112129288A (en) * | 2020-11-24 | 2020-12-25 | 中国人民解放军国防科技大学 | Position and orientation estimation method and system based on polarized light/geomagnetic heading constraint |
| CN112444245A (en) * | 2020-11-17 | 2021-03-05 | 大连理工大学 | Insect-imitated vision integrated navigation method based on polarized light, optical flow vector and binocular vision sensor |
| CN112697138A (en) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | Factor graph optimization-based bionic polarization synchronous positioning and composition method |
| CN113834480A (en) * | 2021-11-26 | 2021-12-24 | 北京航空航天大学 | Self-positioning method of compound eye-imitating polarization sensor based on scattering angle weight distribution |
| CN215767102U (en) * | 2021-12-20 | 2022-02-08 | 中北大学 | Airborne inertial/polarized light/optical flow/visual combined navigation device |
| CN116255988A (en) * | 2023-05-11 | 2023-06-13 | 北京航空航天大学 | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2776216B1 (en) * | 2011-11-11 | 2022-08-31 | iRobot Corporation | Robot apparautus and control method for resuming operation following a pause. |
| US10962625B2 (en) * | 2013-10-22 | 2021-03-30 | Polaris Sensor Technologies, Inc. | Celestial positioning system and method |
| US10417469B2 (en) * | 2016-05-07 | 2019-09-17 | Morgan E. Davidson | Navigation using self-describing fiducials |
-
2021
- 2021-06-03 CN CN202110620366.9A patent/CN113739795B/en active Active
Patent Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5270883A (en) * | 1991-08-29 | 1993-12-14 | Mitsubishi Denki Kabushiki Kaisha | Magnetic read/write circuit |
| CN103712621A (en) * | 2013-12-23 | 2014-04-09 | 大连理工大学 | Method for determining attitude of polarized light and infrared sensor auxiliary inertial navigation system |
| CN106767752A (en) * | 2016-11-25 | 2017-05-31 | 北京航空航天大学 | A kind of Combinated navigation method based on polarization information |
| CN107589748A (en) * | 2017-08-21 | 2018-01-16 | 江苏科技大学 | AUV autonomous navigation methods based on UnscentedFastSLAM algorithms |
| CN108362288A (en) * | 2018-02-08 | 2018-08-03 | 北方工业大学 | Polarized light S L AM method based on unscented Kalman filtering |
| CN108387236A (en) * | 2018-02-08 | 2018-08-10 | 北方工业大学 | Polarized light S L AM method based on extended Kalman filtering |
| CN108827288A (en) * | 2018-04-12 | 2018-11-16 | 东北电力大学 | A kind of dimensionality reduction strapdown inertial navigation system Initial Alignment Method and system based on dual quaterion |
| CN110672131A (en) * | 2019-11-19 | 2020-01-10 | 北方工业大学 | UKF (unscented Kalman Filter) alignment method for inertial/polarized light integrated navigation system under large misalignment angle |
| CN111024076A (en) * | 2019-12-30 | 2020-04-17 | 北京航空航天大学 | Underwater combined navigation system based on bionic polarization |
| CN111045454A (en) * | 2019-12-30 | 2020-04-21 | 北京航空航天大学 | Unmanned aerial vehicle self-driving instrument based on bionic autonomous navigation |
| CN111504312A (en) * | 2020-07-02 | 2020-08-07 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle pose estimation method based on visual inertial polarized light fusion |
| CN112444245A (en) * | 2020-11-17 | 2021-03-05 | 大连理工大学 | Insect-imitated vision integrated navigation method based on polarized light, optical flow vector and binocular vision sensor |
| CN112129288A (en) * | 2020-11-24 | 2020-12-25 | 中国人民解放军国防科技大学 | Position and orientation estimation method and system based on polarized light/geomagnetic heading constraint |
| CN112697138A (en) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | Factor graph optimization-based bionic polarization synchronous positioning and composition method |
| CN113834480A (en) * | 2021-11-26 | 2021-12-24 | 北京航空航天大学 | Self-positioning method of compound eye-imitating polarization sensor based on scattering angle weight distribution |
| CN215767102U (en) * | 2021-12-20 | 2022-02-08 | 中北大学 | Airborne inertial/polarized light/optical flow/visual combined navigation device |
| CN116255988A (en) * | 2023-05-11 | 2023-06-13 | 北京航空航天大学 | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation |
Non-Patent Citations (5)
| Title |
|---|
| Polarized light-aided visual-inertial navigation system:global heading measurements and graph optimization-based multi-sensor fusion;Xia LL 等;《Measurement science and technology》;第33卷(第5期);055111 * |
| 仿生导航技术综述;胡小平;毛军;范晨;张礼廉;何晓峰;韩国良;范颖;;导航定位与授时(04);7-16 * |
| 基于多视图几何的惯性/立体视觉组合导航方法研究;孔祥龙 等;《中国优秀硕士学位论文全文数据库社会科学I辑》(第1期);G112-9 * |
| 多方向偏振光实时定位样机的设计与搭建;褚金奎;张慧霞;王寅龙;时超;;光学精密工程(02);37-43 * |
| 天空偏振光导航辅助的双目视觉-惯性;刘瑞敏;《中国优秀硕士学位论文全文数据库信息科技辑》(第1期);1138-1625 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113739795A (en) | 2021-12-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113739795B (en) | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation | |
| CN108362281B (en) | Long-baseline underwater submarine matching navigation method and system | |
| CN111288989B (en) | Visual positioning method for small unmanned aerial vehicle | |
| Eustice et al. | Exactly sparse delayed-state filters | |
| CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
| CN110132284B (en) | Global positioning method based on depth information | |
| CN103674021A (en) | Integrated navigation system and method based on SINS (Strapdown Inertial Navigation System) and star sensor | |
| Zhang et al. | NavNet: AUV navigation through deep sequential learning | |
| CN114019552A (en) | Bayesian multi-sensor error constraint-based location reliability optimization method | |
| Wu et al. | Research on the relative positions-constrained pattern matching method for underwater gravity-aided inertial navigation | |
| CN113686299B (en) | Marine dynamic target positioning and moving speed prediction method | |
| CN115574816A (en) | Bionic vision multi-source information intelligent perception unmanned platform | |
| CN116448100A (en) | Multi-sensor fusion type offshore unmanned ship SLAM method | |
| CN112797985A (en) | Indoor positioning method and indoor positioning system based on weighted extended Kalman filtering | |
| CN115930977A (en) | Method and system for positioning characteristic degradation scene, electronic equipment and readable storage medium | |
| CN114355409A (en) | Water surface target motion estimation method | |
| CN114608568A (en) | Multi-sensor-based information instant fusion positioning method | |
| CN112945233B (en) | Global drift-free autonomous robot simultaneous positioning and map construction method | |
| CN113624227A (en) | Contour line iteration gravity matching algorithm based on Mahalanobis distance | |
| CN117687059A (en) | Vehicle positioning method and device, electronic equipment and storage medium | |
| CN117419719A (en) | IMU-fused three-dimensional laser radar positioning and mapping method | |
| CN117664124A (en) | Inertial guidance and visual information fusion AGV navigation system and method based on ROS | |
| CN113029173A (en) | Vehicle navigation method and device | |
| CN116380079A (en) | Underwater SLAM method for fusing front-view sonar and ORB-SLAM3 | |
| CN114459474B (en) | Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |