Navigation positioning method for slave nodes under assistance of master node

Abstract

The invention discloses a master node assisted slave node navigation positioning method, relates to a master node assisted slave node navigation method, and belongs to the field of underwater vehicle navigation positioning. The invention aims to solve the problem of poor low-precision node positioning effect caused by the fact that the influence of a node navigation path and a relative navigation path between interactive nodes on the low-precision node positioning precision is not considered when low-precision node positioning error correction is realized by adopting a non-linear filtering algorithm in the prior art. The invention provides an improved navigation algorithm of a low-precision moving node under the assistance of a high-precision node. The method comprises the following steps: screening observation data, adaptively predicting a state covariance matrix in one step through selection of observability so as to improve the requirement of slave node navigation precision, establishing a slave node navigation system model and a process framework under the assistance of a master node, and correcting a self-navigation position state of a low-precision node by adopting an extended Kalman filtering improved algorithm. And the low-precision node positioning precision is improved.

Description

Technical Field

[0001] The invention relates to a navigation method of a slave node assisted by a master node, and belongs to the field of underwater vehicle navigation and positioning. Background Art

[0002] Underwater multi-node information sharing to complete tasks will be an important tool for exploring the ocean due to its easy operation and low loss cost. High-precision navigation is a prerequisite for underwater mobile nodes to complete tasks such as ocean exploration. Since electromagnetic waves will decay rapidly in water, the global satellite navigation system applicable to land can only be used when the underwater mobile node floats close to the water surface, and is not suitable for underwater navigation. Therefore, underwater mobile node navigation technology has become a hot research issue. Underwater mobile node navigation includes single-node navigation and multi-node navigation information interactive sharing navigation.

[0003] Single underwater moving node navigation includes geophysical field navigation, inertial navigation and Doppler log combined navigation, etc. Geophysical field navigation refers to the use of sensors to detect the distribution characteristics of the earth's magnetic field, gravity field, and seabed topography in space, and match them with the prior reference map to achieve autonomous positioning. The combined navigation of inertial navigation and Doppler log is still based on the dead reckoning idea, that is, the position change within the sampling interval is solved according to the navigation speed measured by the Doppler log at the current moment and the attitude angle measured by the inertial navigation system, and then combined with the position calculation at the previous moment to obtain the positioning result at that moment. The above two single-node navigation methods have their own shortcomings. Geophysical field navigation is limited by the scene, while the positioning error of the combined navigation of inertial navigation and Doppler log will accumulate over time.

[0004] Multiple nodes achieve navigation through information interaction because of its advantages such as higher overall navigation accuracy and strong stability, which can effectively avoid the shortcomings of single-node navigation. Multi-node navigation refers to the transmission of information such as the position, heading and timestamp of each node between nodes through underwater acoustic communication, with the relative distance between nodes as a constraint, and the extended Kalman filter is used to perform information fusion to achieve positioning error correction of each node. The present invention discusses the master-slave multi-node navigation in the context of high-precision nodes (master nodes) assisting low-precision nodes (slave nodes); the master node is a ship, beacon or submarine, etc.; the slave node is a UUV, ROV, etc.; UUV is an autonomous underwater vehicle, and ROV is an unmanned remotely controlled submersible;

[0005] The slave node navigation assisted by the master node is that the slave node receives the position of the master node (equipped with high-precision positioning) and the measured relative distance between the two nodes regularly through underwater acoustic communication, and then solves the difference between the relative distance between the one-step predicted nodes and the measured relative distance according to the position of the master node and the one-step predicted position of the slave node, obtains the residual amount containing the one-step predicted error combination of the slave node state, separates the error of each one-step predicted state amount contained in the residual amount through an algorithm and compensates it to the one-step predicted state amount to achieve low-precision node error correction. The present invention adopts one master and one slave for introduction, the master node is equipped with a global navigation satellite system or a high-precision inertial navigation system (hereinafter referred to as inertial navigation) and a Doppler odometer, and an ultra-short baseline; the slave node is equipped with a low-precision inertial navigation and a Doppler odometer, receives the position and relative distance information of the master node, and uses a data fusion algorithm to significantly improve the navigation accuracy of the slave node; however, due to, ① when the attitude angle and angular velocity change significantly, only the position, velocity and attitude angle of the previous moment are used to predict the state of the current moment, and the prediction result cannot accurately reflect the real changes of these parameters between adjacent sampling moments due to the rapid changes of velocity and attitude angle. Therefore, the current position state covariance matrix calculated in this way cannot accurately match the uncertainty of the actual state; ② The master-slave relative distance measurement, as the system observation quantity, is an indirect reflection of the state quantity rather than a direct measurement. Due to this characteristic, in most cases, the residual quantity obtained by observation cannot fully cover all the error components in the one-step prediction, and cannot completely compensate for the error in the one-step prediction; therefore, for some paths, the extended Kalman filter algorithm whose observation quantity is the relative distance between nodes cannot effectively correct the accumulated error of low-precision nodes, and the algorithm needs to be improved.

[0006] Existing document 1: "A master-slave collaborative positioning method for an autonomous underwater vehicle integrated navigation system" with announcement number CN111595348A, the slave AUV corrects its own dead reckoning positioning error by fusing the master and slave AUV ranging information as observation quantity. Document 2: October 2022, Volume 45, Issue 3: "A Study on an Improved Master-Slave Collaborative Navigation Technology" uses the ranging information of the slave boat as the observation quantity to reconstruct the system state equation and measurement equation, and improve the position solution accuracy of the slave boat in the traditional master-slave collaborative navigation positioning algorithm based on ranging information. Document 3: July 2016, Volume 40, Pages 1229-1244: "Cooperative navigation of AUVs via acoustic communication networking: field experience with the Typhoon vehicles" compensates for underwater acoustic communication packet loss and improves the positioning accuracy of underwater node navigation caused by communication packet loss. Reference 4: "Cooperative Localization and Unknown Currents Estimation Using Multiple Autonomous Underwater Vehicles", Volume 5, Issue 2, February 2020, proposed a method of fusing unscented Kalman filtering and linear Kalman filtering to achieve the estimation of location and ocean currents.

[0007] The above literature uses ranging information as the observation quantity and adopts nonlinear filtering algorithm to realize low-precision node positioning error correction, but none of them considers the influence of node navigation path and relative navigation path between interactive nodes on low-precision node positioning accuracy, resulting in poor low-precision node positioning effect. Summary of the invention

[0008] The purpose of the present invention is to solve the problem that the existing nonlinear filtering algorithm is used to realize the error correction of low-precision node positioning, but the influence of the node navigation path and the relative navigation path between the interactive nodes on the low-precision node positioning accuracy is not considered, resulting in poor low-precision node positioning effect, and a navigation positioning method for the slave node under the assistance of the master node is proposed.

[0009] A method for navigating and positioning a slave node assisted by a master node has the following specific processes:

[0010] Step 1: Before the slave node starts sailing, the slave node determines the initial position and initial attitude angle of the slave node according to the satellite navigation system and inertial navigation sensor on the slave node;

[0011] Step 2: The master node and the slave node navigate underwater according to the task requirements;

[0012] Step 3: Collect the attitude angle output by the inertial navigation sensor on the slave node and the speed data output by the Doppler odometer on the slave node at time k-1;

[0013] Step 4: The slave node uses the attitude angle, velocity data and position information of the slave node at time k-1 to calculate and update the position information of the slave node at time k, repeat steps 3 and 4 N times, and then execute step 5;

[0014] Where N = T' / Δt, T' is the time interval for the master node to send observation information, in seconds; Δt represents the data output time interval, Δt = 1, in seconds;

[0015] Step 5: The master node and the slave node perform the lth information exchange, l=1, to obtain the relative distance information between the master and slave nodes;

[0016] The relative distance information between the master and slave nodes is observation information;

[0017] Establish the observation equation of the relative distance between the master node and each slave node;

[0018] Step 6: Establish a random parameter γ that conforms to the Bernoulli distribution lT′ , according to the judgment condition S lT′ Determine the random parameter γ l The value of

[0019] When the random parameter γ lT′ When the value is 0, return to step 3;

[0020] When the random parameter γ lT′ When the value is 1, execute step 7;

[0021] Step 7: Compare the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at the current information interaction moment with the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at the last information interaction moment, and adaptively select the prediction error covariance matrix;

[0022] Step 8: Taking the relative distance between the master and slave nodes at the time of the lth information interaction as the observation quantity, and the one-step prediction of the slave node position at the time of the lth information interaction as the state quantity, the observation quantity and the state quantity are fused based on the extended Kalman filter to obtain the corrected position of the slave node at the time of information interaction;

[0023] Step 9: Let l=l+1, and repeat steps 3 to 8 until the navigation is completed.

[0024] The beneficial effects of the present invention are:

[0025] In view of the shortcomings of the prior art, the present invention proposes an improved navigation algorithm for low-precision moving nodes assisted by high-precision nodes. First, the observation data is screened, and then the state covariance matrix is ​​adaptively predicted in one step by selecting the observable degree, so as to improve the demand for slave node navigation accuracy, establish a slave node navigation system model and process framework assisted by the master node, and adopt an improved extended Kalman filter algorithm to correct the self-navigation position state of the low-precision node and improve the positioning accuracy of the low-precision node.

[0026] An improved navigation algorithm for low-precision moving nodes assisted by high-precision nodes establishes a slave node navigation framework based on the relative distance between master and slave nodes as the observation quantity; the relative distance between nodes is screened, and the observation quantity that can ensure that the estimated error is less than the one-step prediction error is selected; at the same time, the one-step prediction state covariance matrix is ​​adapted through the relative azimuth angle of the master and slave nodes at the moment of information interaction and the heading angle of the slave node, thereby improving the extended Kalman filter algorithm and improving the positioning accuracy of the slave node.

[0027] The present invention takes the relative distance between the master node and the slave node as the observation quantity, determines whether the received observation information is an outlier, compensates for the observed outlier, and uses the extended Kalman filter to collaboratively correct the position of the slave node online. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 This is a flow chart of an improved navigation algorithm for low-precision moving nodes assisted by high-precision nodes in the present invention;

[0029] Figure 2 It is the navigation trajectory diagram of the master and slave nodes in the present invention;

[0030] Figure 3 The relative azimuth angle of the master and slave nodes and the heading angle of the slave node in the present invention and the difference diagram between the two;

[0031] Figure 4 This is a comparison diagram of slave node positioning errors assisted by the master node in the present invention. DETAILED DESCRIPTION

[0032] Specific implementation method 1: In this implementation method, a navigation and positioning method of a slave node assisted by a master node is as follows:

[0033] Step 1: Before the slave node starts sailing, the slave node determines the initial position and initial attitude angle of the slave node according to the satellite navigation system and inertial navigation sensor on the slave node;

[0034] Step 2: The master node and the slave node navigate underwater according to the task requirements (for example, to detect underwater pipelines, the slave node follows a certain trajectory to detect underwater pipelines);

[0035] Step 3: Collect the attitude angle output by the inertial navigation sensor on the slave node and the speed data output by the Doppler odometer on the slave node at time k-1;

[0036] Step 4: The slave node uses the attitude angle, velocity data and position information of the slave node at time k-1 to calculate and update the position information of the slave node at time k, repeat steps 3 and 4 N times, and then execute step 5;

[0037] Where N = T' / Δt, T' is the time interval for the master node to send observation information, in seconds; Δt represents the data output time interval, Δt = 1, in seconds;

[0038] Step 5: The master node and the slave node perform the lth information exchange, l=1, and obtain the relative distance information between the master and slave nodes (delay time multiplied by the speed of sound equals the distance);

[0039] The relative distance information between the master and slave nodes is observation information;

[0040] Establish the observation equation of the relative distance between the master node and each slave node;

[0041] Step 6: Establish a random parameter γ that conforms to the Bernoulli distribution lT′ , according to the judgment condition S lT′ Determine the random parameter γ l The value of

[0042] When the random parameter γ lT′ When the value is 0, it returns to step 3 to predict the state of the slave node position at the next moment;

[0043] When the random parameter γ lT′ When the value is 1, execute step 7;

[0044] Step 7: Compare the relative azimuth of the master and slave nodes at the current information exchange moment (the relative azimuth is known if the positions of the master and slave nodes are known) and the heading angle of the slave node with the relative azimuth of the master and slave nodes and the heading angle of the slave node at the last information exchange moment, and adaptively select the prediction error covariance matrix;

[0045] Step 8: Taking the relative distance between the master and slave nodes at the time of the lth information interaction as the observation quantity, and the one-step prediction of the slave node position at the time of the lth information interaction as the state quantity, the observation quantity and the state quantity are fused based on the extended Kalman filter to obtain the corrected position of the slave node at the time of information interaction (the position of the slave node);

[0046] Step nine: Let l=l+1, and repeat steps three to eight (second information interaction, third information interaction, fourth information interaction) until the navigation is completed.

[0047] The master node is a ship, a beacon or a submarine, etc.;

[0048] The slave node is a UUV or the like.

[0049] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that: in step 3, the attitude angle output from the inertial navigation sensor on the node and the speed data output from the Doppler odometer on the node at time k-1 are collected; the specific process is:

[0050] Construct the carrier coordinate system: the right direction from the node is the X axis, the forward direction from the node is the Y axis, and the vertical X and Y plane upward is the Z axis;

[0051] The inertial navigation sensor on the slave node obtains the attitude angle in the carrier coordinate system, and extracts the heading angle θ from the attitude angle m,k-1 ;

[0052] The Doppler log on the slave node outputs the forward velocity at time k-1 in the carrier coordinate system. and right speed k≥2.

[0053] The other steps and parameters are the same as those in the first embodiment.

[0054] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that: in step four, the slave node uses the attitude angle, speed data and position information of the slave node at time k-1 to calculate and update the position information of the slave node at time k; the specific process is:

[0055] Step 41: Calculate the position vector of the slave node at time k according to the position calculation equation; the specific process is:

[0056]

[0057] Among them, x k =[x′ k ,y′ k ] T represents the true position vector of node k at the moment, x′ k , y′ k Respectively represent the east and north position coordinates of the slave node in the navigation coordinate system; x k-1 Represents the position vector from node k-1 at the moment; Indicates the actual heading angle θ k-1 The rotation matrix of represents the actual velocity vector from node k-1, represents the forward speed from node k-1, Indicates the rightward speed from node k-1; the superscript T indicates transposition; the unit of heading angle is rad; the unit of speed is m / s;

[0058] Construct the navigation coordinate system: take the center of the carrier coordinate system as the origin, due east as the X axis, due north as the Y axis, and the celestial direction perpendicular to the X and Y planes as the Z axis;

[0059] Step 4.2: Using the noisy velocity vector measured by the Doppler measuring instrument and the noisy heading angle measured by the inertial navigation sensor, a one-step prediction of the slave node position state is established according to the position estimation equation. The expression is:

[0060]

[0061] Where, f(·) represents the state transition function;

[0062] It indicates the position estimate of the slave node at time k-1;

[0063] represents the system input vector from the node at time k-1;

[0064] represents the process noise at time k-1, which is the zero-mean Gaussian white noise output by the inertial navigation sensor on the slave node at time k-1; represents the process noise of the forward velocity at time k-1, represents the process noise of the rightward velocity at time k-1, represents the process noise of the heading angle at time k-1;

[0065] represents the heading angle with noise measured by the inertial navigation sensor at time k-1;

[0066] represents the noisy velocity vector measured by the Doppler measurement instrument at time k-1;

[0067] Step 43: Get the k-time prediction error covariance matrix The expression is:

[0068]

[0069] Among them, Q k is the noise covariance matrix at time k-1, E[] means to find the expectation, w k represents the process noise at time k, Indicates w k Find the transpose, diag[] represents a diagonal matrix, represents the forward velocity noise variance, represents the rightward velocity noise variance, represents the noise variance of the heading angle;

[0070] is the k-1 time estimation error covariance matrix; F k is the state function in The Jacobian matrix at G k is the state function at u k-1 The Jacobian matrix at .

[0071] The other steps and parameters are the same as those in the first or second embodiment.

[0072] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that: the state function is The Jacobian matrix F at k It is expressed as:

[0073]

[0074] in, represents the state estimation of the slave node at the k-1th time of measurement, and f(·) represents the state transition function.

[0075] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0076] Specific implementation method 5: This implementation method is different from any one of the specific implementation methods 1 to 4 in that: the state function is in u k-1 The Jacobian matrix G at k It is expressed as:

[0077]

[0078] Among them, u m,k-1 represents the system input vector from the node at the measured k-1 time, and f(·) represents the state transfer function.

[0079] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0080] Specific implementation method 6: This implementation method is different from any one of specific implementation methods 1 to 5 in that: in step 5, the master node and the slave node perform the lth information exchange, l=1, and obtain the relative distance information between the master and slave nodes (the delay time (in seconds) multiplied by the speed of sound equals the distance);

[0081] The relative distance information between the master and slave nodes is observation information;

[0082] Establish the observation equation of the relative distance between the master node and each slave node;

[0083] The specific process is:

[0084] The master node sends information including the master node's own position and acoustic pulse to each slave node (the first information exchange between the master node and the slave node), and calculates the relative distance between the master and slave nodes based on the delay time of the acoustic pulse and the underwater sound speed (the speed at which sound propagates underwater) (delay time multiplied by the sound speed equals distance);

[0085] The relative distance information between the master and slave nodes is observation information;

[0086] Establish the observation equation of the relative distance between the master node and each slave node.

[0087] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0088] Specific implementation method 7: This implementation method is different from any one of the specific implementation methods 1 to 6 in that: the observation equation for the relative distance between the master node and each slave node is established; the specific process is:

[0089] The observation equation is established based on the relative distance between the master and slave nodes at the time of the lth information interaction:

[0090]

[0091] Among them, z lT′ represents the observed value at time lT′; h(·) is the observation function; x lT′ represents the position coordinates of the slave node at time lT′; represents the position coordinates of the master node obtained by the global navigation satellite system carried by the master node at time lT′; ||·||2 represents the matrix 2 norm; η lT′ represents the ranging noise at time lT′, T′ is the time interval for the master node to send observation information, l is the lth information interaction, the mean is zero Gaussian white noise, and the variance is

[0092] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0093] Specific implementation eight: This implementation differs from any one of the specific implementations one to seven in that: in step six, a random parameter γ conforming to the Bernoulli distribution is established. lT′ , according to the judgment condition S lT′ Determine the random parameter γ lT′ The value of

[0094] When the random parameter γ lT′ When the value is 0, it returns to step 3 to predict the state of the slave node position at the next moment;

[0095] When the random parameter γ lT′ When the value is 1, execute step 7;

[0096] The specific process is:

[0097] Random parameter γ lT′ Build as:

[0098]

[0099] Among them, S lT′ represents the residual, z lT′ represents the observed value at time lT′; represents the relative distance between nodes predicted in one step at time lT′, represents the one-step prediction from the node position state at time lT′, represents the position coordinates of the master node obtained by the global navigation satellite system carried by the master node at time lT′;

[0100] represents the state error of the slave node predicted in one step at time lT′, From the prediction error covariance matrix The vector consisting of the diagonal elements of ;

[0101] When γ lT′ =1, indicating that the error after receiving the fusion information at time lT′ from the slave node is less than the error before estimation, and step 7 is executed;

[0102] The fused information is the information obtained by fusing the observation information and the state variables;

[0103] When γ lT′ =0, indicating that the error after estimation of the fusion information received from the slave node at time lT′ is greater than or equal to the error before estimation, and step 3 is executed.

[0104] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0105] Specific implementation method 9: This implementation method is different from any one of specific implementation methods 1 to 8 in that: in step 7, the relative azimuth angle of the master and slave nodes at the current information exchange moment (the relative azimuth angle is known if the positions of the master and slave nodes are known) and the heading angle of the slave node are compared with the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at the last information exchange moment, and the prediction error covariance matrix is ​​adaptively selected;

[0106] The specific process is:

[0107] The prediction error covariance matrix at the current information interaction moment The selection principles are:

[0108]

[0109] in, Represents the prediction error covariance matrix The trace of ; I2 is the 2×2 identity matrix; · represents the vector dot product;

[0110] φ lT′ Indicates the difference between the relative azimuth of the master and slave nodes and the heading angle of the slave node at time lT′. represents the relative azimuth of the master and slave nodes at time lT′, θ lT′ represents the heading angle from the node at time lT′;

[0111] φ (l-1)T′ Represents the difference between the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at time (l-1)T′;

[0112] δ represents the threshold for judging whether the previous observation can be used to predict the current observation, and is generally taken as 10.

[0113] The other steps and parameters are the same as those in Specific Implementation 1 to 8-1.

[0114] Specific implementation method ten: This implementation method is different from any one of specific implementation methods one to nine in that: in step eight, the relative distance between the master and slave nodes at the time of the first information interaction is used as the observation quantity, and the one-step prediction of the position of the slave node at the time of the first information interaction is used as the state quantity. The observation quantity and the state quantity are fused based on the extended Kalman filter to obtain the corrected position of the slave node at the time of information interaction (the position of the slave node); the specific process is:

[0115] Step 81: One-step prediction of node position at the time of information interaction for:

[0116]

[0117] In the formula, represents the optimal position at time lT′-1, u lT′-1 represents the optimal system input at time lT′-1, w lT′-1 represents the process noise at time lT′-1;

[0118] Step 82: Obtain the prediction error covariance matrix at time lT′ The expression is:

[0119]

[0120] In the formula, F lT′ Indicates that the state function at time lT′ is The Jacobian matrix at , represents the estimated error covariance matrix at time lT′-1, G lT′ Indicates that the state function at time lT′ is k-1The Jacobian matrix at Q lT′ represents the noise covariance matrix at time lT′;

[0121] Step 83: Prediction error covariance matrix based on time lT′ Get the Kalman gain K lT′ ; The expression is:

[0122]

[0123] In the formula, H lT′ Indicates that the measurement function at time lT′ is The Jacobian matrix at , represents the measurement noise covariance matrix at time lT′;

[0124] Step 84: Based on Kalman gain K lT′ and the one-step prediction of the node position at the lth information interaction time Get position state estimate The expression is:

[0125]

[0126] Step 85: Prediction error covariance matrix based on time lT′ and the Kalman gain K lT′ , obtain the state estimation error covariance matrix The expression is:

[0127]

[0128] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 9.

[0129] The following examples are used to verify the beneficial effects of the present invention:

[0130] Embodiment 1:

[0131] For a multi-autonomous underwater vehicle consisting of only one master node and one slave node, each node sails according to a preset trajectory; the slave node is equipped with an inertial navigation system and a Doppler log, and outputs attitude and velocity information at time k-1 in real time, with a sampling interval of Δt; position calculation is performed to obtain position information at time k; the master node is equipped with a global navigation satellite system and an ultra-short baseline, and outputs its own position and the relative distance between the master and slave nodes at the time of the lth information interaction in real time, and the information interaction cycle between nodes is T;

[0132] Then, the master node sends its own position and the relative distance between the master and slave nodes to the slave node; the slave node determines whether the master node information received at the lth coordination moment is an abnormal value. If it is determined to be an abnormal value, it compensates for the abnormal value;

[0133] Finally, the relative distance between the master and slave nodes at the time of the lth information interaction is taken as the observation quantity, the position of the slave node at the time of the lth information interaction is taken as the state quantity, and the corrected observation matrix is ​​used to locate the slave node with the assistance of the master node navigation information; let k = lT′+1, and repeat until the navigation ends;

[0134] The present invention is verified by MATLAB simulation:

[0135] Simulation conditions:

[0136] 1) Node motion parameters:

[0137] High-precision nodes:

[0138] Initial position: (0m, 1000m);

[0139] Speed: forward speed 2m / s, right speed 0m / s, vertical speed is not considered;

[0140] Azimuth: θ m =35°

[0141] Low-precision nodes:

[0142] Initial position: (500m, 500m);

[0143] Speed: forward speed 2m / s, right speed 0m / s, vertical speed is not considered;

[0144] Azimuth: θ s =45°

[0145] 2) Low-precision node autonomous navigation sensor error:

[0146] DVL: speed measurement error zero mean Gaussian white noise, variance

[0147] INS: Azimuth velocity error zero mean Gaussian white noise, variance

[0148] 3) Ranging error between high-precision nodes and low-precision nodes:

[0149] The ranging errors are all zero-mean Gaussian white noise with a variance of

[0150] 4) Kalman filter parameters:

[0151] Initial state error covariance matrix: P0 = diag[0m,0m] 2

[0152] Process noise covariance matrix: Q k=diag[0.1m / s,0.1m / s,0.0005°] 2

[0153] Measurement noise covariance matrix: R k =diag[0.1m] 2

[0154] 5) Other parameters:

[0155] The low-precision node navigation simulation time is 32000s, the sampling frequency is: 1Hz, and the underwater acoustic communication cycle for receiving high-precision node navigation information is 30s;

[0156] By utilizing the method of the present invention, a slave node collaborative navigation positioning result is obtained.

[0157] Among them, Figure 2 As shown, the navigation trajectory diagrams of the master and slave nodes are obtained;

[0158] Figure 3 It is the relative azimuth of the master and slave nodes and the heading angle of the slave node and the difference between the two. It can be seen from the figure that in the entire voyage of the slave node, except for the first 5 times, the difference between the relative azimuth of the master and slave nodes and the heading angle of the slave node has a certain change. With the extended Kalman filter, the relative azimuth of the master and slave nodes and the heading angle difference of the slave node at the subsequent information interaction time remain basically unchanged. Therefore, it is necessary to perform one-step prediction covariance matrix adaptation to ensure the improvement of the positioning accuracy of the slave node;

[0159] Figure 4 This is a comparison chart of the navigation positioning error of the slave node assisted by the master node. It can be seen from the figure that the average value of the whole positioning error is reduced to 58.7 meters, which shows a significant performance advantage compared to the average positioning error of 585.1 meters of the traditional extended Kalman filter algorithm.

[0160] The present invention may also have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. A navigation and positioning method for a slave node assisted by a master node, characterized in that: The specific process of the method is: Step 1: Before the slave node starts sailing, the slave node determines the initial position and initial attitude angle of the slave node according to the satellite navigation system and inertial navigation sensor on the slave node; Step 2: The master node and the slave node navigate underwater according to the task requirements; Step 3: Collect the attitude angle output from the inertial navigation sensor on the node and the speed data output from the Doppler odometer on the node at time k-1; Step 4: The slave node uses the attitude angle, velocity data and position information of the slave node at time k-1 to calculate and update the position information of the slave node at time k, repeat steps 3 and 4 N times, and then execute step 5; Where N = T' / Δt, T' is the time interval for the master node to send observation information, in seconds; Δt represents the data output time interval, Δt = 1, in seconds; Step 5: The master node and the slave node perform the lth information exchange, l=1, to obtain the relative distance information between the master and slave nodes; The relative distance information between the master and slave nodes is observation information; Establish the observation equation of the relative distance between the master node and each slave node; Step 6: Establish a random parameter γ that conforms to the Bernoulli distribution lT′ , according to the judgment condition S lT′ Determine the random parameter γ l The value of When the random parameter γ lT′ When the value is 0, return to step 3; When the random parameter γ lT′ When the value is 1, execute step 7; Step 7: Compare the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at the current information interaction moment with the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at the last information interaction moment, and adaptively select the prediction error covariance matrix; Step 8: Taking the relative distance between the master and slave nodes at the time of the lth information interaction as the observation quantity, and the one-step prediction of the slave node position at the time of the lth information interaction as the state quantity, the observation quantity and the state quantity are fused based on the extended Kalman filter to obtain the corrected position of the slave node at the time of information interaction; Step 9: Let l=l+1, and repeat steps 3 to 8 until the navigation is completed.

2. The method for navigating and positioning a slave node with the assistance of a master node according to claim 1, characterized in that: In the step 3, the attitude angle output from the inertial navigation sensor on the slave node and the speed data output from the Doppler odometer on the slave node at time k-1 are collected; the specific process is: Construct the carrier coordinate system: the right direction from the node is the X axis, the forward direction from the node is the Y axis, and the vertical X and Y plane upward is the Z axis; Obtain the attitude angle from the inertial navigation sensor on the node in the carrier coordinate system, and extract the heading angle θ from the attitude angle m,k-1 ; The Doppler log on the slave node outputs the forward velocity at time k-1 in the carrier coordinate system. and right speed 3. The method for navigating and positioning a slave node with the assistance of a master node according to claim 2, characterized in that: In step 4, the slave node uses the attitude angle, velocity data and position information of the slave node at time k-1 to calculate and update the position information of the slave node at time k; the specific process is: Step 41: Calculate the position vector of the slave node at time k according to the position calculation equation; the specific process is: Among them, x k =[x′ k ,y′ k ] T represents the real position vector of node k at the moment, x′ k , y′ k Respectively represent the east and north position coordinates of the slave node in the navigation coordinate system; x k-1 Represents the position vector from node k-1 at the moment; Indicates the actual heading angle θ k-1 The rotation matrix of represents the actual velocity vector from node k-1, represents the forward speed from node k-1, represents the rightward velocity from node k-1; the superscript T represents transposition; Construct the navigation coordinate system: take the center of the carrier coordinate system as the origin, due east as the X axis, due north as the Y axis, and the celestial direction perpendicular to the X and Y planes as the Z axis; Step 4.2: Using the noisy velocity vector measured by the Doppler measuring instrument and the noisy heading angle measured by the inertial navigation sensor, a one-step prediction of the slave node position state is established according to the position estimation equation. The expression is: Where, f(·) represents the state transition function; It indicates the position estimate of the slave node at time k-1; represents the system input vector from the node at time k-1; represents the process noise at k-1 time; represents the process noise of the forward velocity at time k-1, represents the process noise of the rightward velocity at time k-1, represents the process noise of the heading angle at time k-1; represents the heading angle with noise measured by the inertial navigation sensor at time k-1; represents the noisy velocity vector measured by the Doppler measurement instrument at time k-1; Step 43: Get the k-time prediction error covariance matrix The expression is: Among them, Q k is the noise covariance matrix at time k-1, E[] means to find the expectation, w k represents the process noise at time k, Indicates w k Find the transpose, diag[] represents a diagonal matrix, represents the forward velocity noise variance, represents the rightward velocity noise variance, represents the noise variance of the heading angle; is the k-1 time estimation error covariance matrix; F k is the state function in The Jacobian matrix at G k is the state function at u k-1 The Jacobian matrix at .

4. The method for navigating and positioning a slave node with the assistance of a master node according to claim 3, characterized in that: The state function is The Jacobian matrix F at k It is expressed as: in, represents the state estimation of the slave node at the k-1th time of measurement, and f(·) represents the state transition function.

5. The method for navigating and positioning a slave node with the assistance of a master node according to claim 4, characterized in that: The state function is in u k-1 The Jacobian matrix G at k It is expressed as: Among them, u m,k-1 represents the system input vector from the node at the measured k-1 time, and f(·) represents the state transfer function.

6. The method for navigating and positioning a slave node with the assistance of a master node according to claim 5, characterized in that: In step 5, the master node and the slave node perform the lth information exchange, where l=1, to obtain the relative distance information between the master and slave nodes; The relative distance information between the master and slave nodes is observation information; Establish the observation equation of the relative distance between the master node and each slave node; The specific process is: The master node sends information including the master node's own position and acoustic pulses to each slave node, and calculates the relative distance between the master and slave nodes based on the delay time of the acoustic pulses and the underwater sound speed; The relative distance information between the master and slave nodes is observation information; Establish the observation equation of the relative distance between the master node and each slave node.

7. The method for navigating and positioning a slave node with the assistance of a master node according to claim 6, characterized in that: The observation equation of the relative distance between the master node and each slave node is established; the specific process is: The observation equation is established based on the relative distance between the master and slave nodes at the time of the lth information interaction: Among them, z lT′ represents the observed value at time lT′; h(·) is the observation function; x lT′ represents the position coordinates of the slave node at time lT′; represents the position coordinates of the master node obtained by the global navigation satellite system carried by the master node at time lT′; ||·||2 represents the matrix 2 norm; η lT′ represents the ranging noise at time lT′, T′ is the time interval for the master node to send observation information, l is the lth information interaction, the mean is zero Gaussian white noise, and the variance is 8. The method for navigating and positioning a slave node with the assistance of a master node according to claim 7, characterized in that: In step 6, a random parameter γ is established that conforms to the Bernoulli distribution. lT′ , according to the judgment condition S lT′ Determine the random parameter γ lT′ The value of When the random parameter γ lT′ When the value is 0, return to step 3; When the random parameter γ lT′ When the value is 1, execute step 7; The specific process is: Random parameter γ lT′ Build as: Among them, S lT′ represents the residual, z lT′ represents the observed value at time lT′; represents the relative distance between nodes predicted in one step at time lT′, represents the one-step prediction from the node position state at time lT′, represents the position coordinates of the master node obtained by the global navigation satellite system carried by the master node at time lT′; represents the state error of the slave node predicted in one step at time lT′, From the prediction error covariance matrix The vector consisting of the diagonal elements of ; When γ lT′ =1, indicating that the error after receiving the fusion information at time lT′ from the slave node is less than the error before estimation, and step 7 is executed; The fused information is the information obtained by fusing the observation information and the state variables; When γ lT′ =0, indicating that the error after estimation of the fusion information received from the slave node at time lT′ is greater than or equal to the error before estimation, and step 3 is executed.

9. The method for navigating and positioning a slave node with the assistance of a master node according to claim 8, characterized in that: In the step 7, the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at the current information interaction moment are compared with the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at the last information interaction moment, and the prediction error covariance matrix is ​​adaptively selected; The specific process is: The prediction error covariance matrix at the current information interaction moment The selection principles are: in, Represents the prediction error covariance matrix The trace of ; I2 is the 2×2 identity matrix; · represents the vector dot product; φ lT′ Indicates the difference between the relative azimuth of the master and slave nodes and the heading angle of the slave node at time lT′. represents the relative azimuth of the master and slave nodes at time lT′, θ lT′ represents the heading angle from the node at time lT′; φ (l-1)T′ Represents the difference between the relative azimuth angle of the master node and the slave node and the heading angle of the slave node at time (l-1)T′; δ represents the threshold, which is usually set to 10.

10. The method for navigating and positioning a slave node with the assistance of a master node according to claim 9, characterized in that: In step eight, the relative distance between the master and slave nodes at the time of the first information interaction is used as the observation quantity, and the one-step prediction of the position of the slave node at the time of the first information interaction is used as the state quantity. The observation quantity and the state quantity are fused based on the extended Kalman filter to obtain the corrected position of the slave node at the time of information interaction. The specific process is: Step 81: One-step prediction of node position at the time of information interaction for: In the formula, represents the optimal position at time lT′-1, u lT′-1 represents the optimal system input at time lT′-1, w lT′-1 represents the process noise at time lT′-1; Step 82: Obtain the prediction error covariance matrix at time lT′ The expression is: In the formula, F lT′ Indicates that the state function at time lT′ is The Jacobian matrix at , represents the estimated error covariance matrix at time lT′-1, G lT′ Indicates that the state function at time lT′ is k-1 The Jacobian matrix at Q lT′ represents the noise covariance matrix at time lT′; Step 83: Prediction error covariance matrix based on time lT′ Get the Kalman gain K lT′ ; The expression is: In the formula, H lT′ Indicates that the measurement function at time lT′ is The Jacobian matrix at R lT′ represents the measurement noise covariance matrix at time lT′; Step 84: Based on Kalman gain K lT′ and the one-step prediction of the node position at the lth information interaction time Get position state estimate The expression is: Step 85: Prediction error covariance matrix based on time lT′ and the Kalman gain K lT′ , obtain the state estimation error covariance matrix The expression is:

Images