A navigation and positioning method for slave nodes assisted by a master node

Abstract

A navigation and positioning method for a slave node assisted by a master node. The present invention relates to a navigation method for a slave node assisted by a master node, and the present invention belongs to the field of underwater vehicle navigation and positioning. 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. 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 observability, thereby improving the demand for slave node navigation accuracy, establishing a slave node navigation system model and process framework assisted by a master node, and using an improved extended Kalman filter algorithm to correct the self-navigation position state of the low-precision node, thereby improving the positioning accuracy of the low-precision node.

Description

Technical Field

[0001] The present 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 will be a crucial tool for ocean exploration due to its ease of operation and low cost of loss. High-precision navigation is a prerequisite for underwater mobile nodes to accomplish tasks such as ocean exploration. However, due to the rapid attenuation of electromagnetic waves in water, global satellite navigation systems used on land can only be used when underwater mobile nodes are near the surface and are not suitable for underwater navigation. Therefore, underwater mobile node navigation technology has become a hot research topic. Underwater mobile node navigation includes single-node navigation and multi-node navigation with interactive information sharing.

[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 spatial distribution characteristics of the Earth's magnetic field, gravity field, and seabed topography, and match them with a priori reference maps to achieve autonomous positioning. Inertial navigation and Doppler log combined navigation is still based on the concept of dead reckoning, that is, the position change within the sampling interval is solved based on 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 inertial navigation and Doppler log combined navigation will accumulate over time.

[0004] Multiple nodes realize 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 use of underwater acoustic communication between nodes to transmit information such as the position, heading and timestamp of each node, 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 which high-precision nodes (master nodes) assist 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 regularly receives the position of the master node (equipped with high-precision positioning) and the relative distance measured between the two nodes through underwater acoustic communication, and then calculates the difference between the one-step predicted relative distance between the nodes and the measured relative distance based on the position of the master node and the one-step predicted position of the slave node, obtains a residual containing the one-step predicted error combination of the slave node state, separates the error of each one-step predicted state quantity contained in the residual quantity through an algorithm and compensates it to the one-step predicted state quantity 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, as well as 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 the following reasons: ① when the attitude angle and angular velocity change significantly, the state at the current moment is predicted only based on the position, velocity and attitude angle of the previous moment, and the rapid change of velocity and attitude angle causes the prediction result to be unable to accurately reflect the true change of these parameters between adjacent sampling moments. Therefore, the calculated current position state covariance matrix cannot accurately match the uncertainty of the actual state. Secondly, the master-slave relative ranging, as a system observation, is an indirect reflection of the state rather than a direct measurement. Due to this characteristic, in most cases, the residual obtained through observation cannot fully cover all error components in the one-step prediction and cannot fully compensate for the error in the one-step prediction. Therefore, for some paths, the extended Kalman filter algorithm, which uses the relative distance between nodes as the observation, 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," Publication No. CN111595348A, describes a slave AUV correcting its own dead-reckoning positioning error by fusing ranging information from the master and slave AUVs as observations. Document 2: "Research on an Improved Master-Slave Collaborative Navigation Technology," published in Volume 45, Issue 3, October 2022, uses ranging information from the slave vessel as observations to reconstruct the system's state equations and measurement equations, improving the accuracy of the slave vessel's position solution in traditional master-slave collaborative navigation and positioning algorithms based on ranging information. Document 3: "Cooperative navigation of AUVs via acoustic communication networking: field experience with the Typhoon vehicles," published in Volume 40, Pages 1229-1244, July 2016, compensates for underwater acoustic communication packet loss, improving the positioning accuracy of underwater node navigation due to communication packet loss. Reference 4: "Cooperative Localization and Unknown Currents Estimation Using Multiple Autonomous Underwater Vehicles" in Volume 5, Issue 2, February 2020, proposed a method of fusing unscented Kalman filtering and linear Kalman filtering to achieve the estimation of position 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. However, none of them considers the impact of node navigation path and relative navigation path between interacting nodes on the 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 correct the low-precision node positioning error, 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. 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 with the assistance of a master node has the following specific steps:

[0010] Step 1: Before the slave node starts sailing, the slave node determines its initial position and initial attitude angle 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 mission requirements;

[0012] Step 3: Collect the attitude angle output by the inertial navigation sensor on the node and the speed data output by the Doppler odometer on the 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, where 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 angles of the master and slave nodes and the heading angles of the slave nodes at the current information interaction moment with the relative azimuth angles of the master and slave nodes and the heading angles of the slave nodes at the last information interaction moment, and adaptively select the prediction error covariance matrix;

[0022] Step 8: Using the relative distance between the master and slave nodes at the time of the first information interaction as the observation quantity and the one-step prediction of the slave node position at the time of the first 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 the 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 response to the shortcomings of the existing technology, 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 observability, thereby improving the demand for slave node navigation accuracy. A slave node navigation system model and process framework are established under the assistance of the master node. The extended Kalman filter improved algorithm is used 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 and the heading angle of the slave node at the moment of information exchange, thereby improving the extended Kalman filter algorithm and enhancing the positioning accuracy of the slave node.

[0027] The present invention uses 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 and the difference diagram between the two in the present invention;

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

[0032] Specific implementation method 1: This implementation method is a master node assisted navigation and positioning method of the slave node. The specific process is as follows:

[0033] Step 1: Before the slave node starts sailing, the slave node determines its initial position and initial attitude angle 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 should follow a specific trajectory to detect the underwater pipelines);

[0035] Step 3: Collect the attitude angle output by the inertial navigation sensor on the node and the speed data output by the Doppler odometer on the 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, where l = 1, and obtain the relative distance information between the master and slave nodes (the 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, return to step 3 and 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 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 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 adaptively select the prediction error covariance matrix;

[0045] Step 8: Using the relative distance between the master and slave nodes at the time of the first information interaction as the observation quantity and the one-step prediction of the slave node position at the time of the first 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 the information interaction (the position of the slave node);

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

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

[0048] The slave node is a UUV, etc.

[0049] Specific embodiment 2: This embodiment differs from the specific embodiment 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] 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 ;

[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] Other steps and parameters are the same as those in the first embodiment.

[0054] Specific embodiment three: This embodiment differs from specific embodiment one or two in that: in step four, 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:

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

[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 of 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 velocity from node k-1, Indicates the rightward velocity 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 a 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 velocity vector with noise measured by the Doppler measurement instrument and the heading angle with noise 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 represents the position estimate from the 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 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 velocity vector with noise measured by the Doppler measurement instrument at time k-1;

[0067] Step 4.3. Obtain 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] Other steps and parameters are the same as those in the first or second embodiment.

[0072] Specific embodiment 4: This embodiment differs from any one of the specific embodiments 1 to 3 in that: the state function The Jacobian matrix F at k Expressed as:

[0073]

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

[0075] The other steps and parameters are the same as those in the first to third embodiments.

[0076] Specific embodiment 5: This embodiment differs from any one of the specific embodiments 1 to 4 in that: the state function is in u k-1 The Jacobian matrix G at k Expressed as:

[0077]

[0078] Among them, u m,k-1 represents the system input vector from the node at the k-1th moment of measurement, and f(·) represents the state transition function.

[0079] The other steps and parameters are the same as those in the first to fourth embodiments.

[0080] Specific embodiment 6: This embodiment differs from any one of specific embodiments 1 to 5 in that: in step 5, the master node and the slave node perform information exchange for the lth time, where 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 its own position and acoustic pulse to each slave node (the first information exchange between the master and slave nodes). The relative distance between the master and slave nodes is calculated based on the delay time of the acoustic pulse and the speed of sound underwater (the speed at which sound propagates underwater) (the delay time multiplied by the speed of sound equals the 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 the first to fifth embodiments.

[0088] Specific embodiment seven: This embodiment differs from any one of specific embodiments one to six 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 first information interaction:

[0090]

[0091] Among them, z lT′ represents the observed quantity at time lT′; h(·) is the observation function; x lT′ Indicates 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′, where 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 the first to sixth embodiments.

[0093] Specific embodiment eight: This embodiment differs from any one of specific embodiments one to seven in that: in step six, 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

[0094] When the random parameter γ lT′ When the value is 0, return to step 3 and 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 quantity 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 the first to seventh embodiments.

[0105] Specific embodiment nine: This embodiment differs from any one of specific embodiments one to eight in that: in step seven, the relative azimuth angles of the master and slave nodes at the current information exchange moment (the relative azimuth angles are 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 angles of the master and slave nodes and the heading angle of the slave node at the last information exchange moment, and a 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 angle of the master and slave nodes and the heading angle of the slave node at time lT′. represents the relative azimuth angle between 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 angles of the master and slave nodes 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 set to 10.

[0113] Other steps and parameters are the same as those in Specific Implementations 1 to 8-1.

[0114] Specific embodiment ten: This embodiment differs from any one of specific embodiments 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 slave node position 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 as follows:

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

[0116]

[0117] Where, 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] Where, F lT′ Indicates that the state function at time lT′ is The Jacobian matrix at , represents the estimation 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] Where 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 time of the lth information interaction Get position state estimate The expression is:

[0125]

[0126] Step 85: Based on the prediction error covariance matrix at 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 the specific implementation modes 1 to 9-1.

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

[0130] Example 1:

[0131] For a multi-autonomous underwater vehicle consisting of only one master node and one slave node, each node navigates according to a preset trajectory. The slave node is equipped with an inertial navigation system and a Doppler odometer, and outputs attitude and velocity information at time k-1 in real time, with a sampling interval of Δt. Position estimation is performed to obtain the 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 exchange in real time. The information exchange period 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 first collaboration moment is an outlier. If it is determined to be an outlier, it compensates for the outlier.

[0133] Finally, the relative distance between the master and slave nodes at the time of the lth information interaction is used as the observation quantity, and the position of the slave node at the time of the lth information interaction is used as the state quantity. 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 is completed.

[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, such as Figure 2 As shown, the navigation trajectory diagram of the master and slave nodes is obtained;

[0158] Figure 3 It is the relative azimuth of the master and slave nodes and the heading angle of the slave node, as well as the difference between the two. As can be seen from the figure, 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 difference between the relative azimuth of the master and slave nodes and the heading angle of the slave node remains basically unchanged at the subsequent information exchange moments. Therefore, it is necessary to perform one-step prediction covariance matrix adaptation to ensure the improvement of the slave node positioning accuracy;

[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 positioning error of the entire process 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 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 by: The specific process of the method is: Step 1: Before the slave node starts sailing, the slave node determines its initial position and initial attitude angle 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 mission requirements; Step 3: Collection 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 all times; Step 4: Exploit from the node The attitude angle, velocity data and position information of the slave node are calculated and updated at all times. Repeat steps 3 and 4 for the current location information. Next, execute step five; in , The time interval for the master node to send observation information, in seconds; Indicates the data output time interval, , in seconds; Step 5: The master node and the slave node Information interaction, , 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 random parameters that conform to the Bernoulli distribution , according to the judgment conditions Determine random parameters The value of When the random parameters When the value is 0, return to step 3; When the random parameters When the value is 1, execute step 7; Step 7: Compare the relative azimuth angles of the master and slave nodes and the heading angles of the slave nodes at the current information interaction moment with the relative azimuth angles of the master and slave nodes and the heading angles of the slave nodes at the last information interaction moment, and adaptively select the prediction error covariance matrix; Step 8: The relative distance between the master and slave nodes at the time of information interaction is the observation quantity. The one-step prediction of the slave node position at the time of 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. Step 9: Order , 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 step three, the collection 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 all times; 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 ; The Doppler log on the slave node outputs the data in the carrier coordinate system. Forward speed at time 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 utilizes The attitude angle, velocity data and position information of the slave node are calculated and updated at all times. Location information at the moment; The specific process is: Step 4.

1. Calculate the slave node based on the position calculation equation The position vector at the moment; the specific process is: （1） in, Represents a slave node The real position vector at the moment, 、 Respectively represent the easting and northing position coordinates of the slave node in the navigation coordinate system; Represents a slave node Position vector at the moment; Indicates the actual heading angle The rotation matrix of Represents a slave node The actual velocity vector at the moment, Represents a slave node The forward speed at time, Represents a slave node The rightward speed at the moment; the superscript T indicates transposition; Construct a 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 velocity vector with noise measured by the Doppler measurement instrument and the heading angle with noise 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: （2） in, represents the state transition function; express The moments are estimated from the positions of the nodes; express System input vector from the node at each moment; express Momentary process noise; express The process noise of the forward velocity at time t, express The process noise of the rightward speed at time t, express Process noise of the heading angle at each moment; express The noisy heading angle measured by the inertial navigation sensor at all times; express The velocity vector with noise measured by the Doppler measurement instrument at the moment; Step 4.

3. Obtain Moment prediction error covariance matrix , the expression is: （3） in, yes The moment noise covariance matrix, , Expressing hope, express Time process noise, Express Find the transpose, represents a diagonal matrix, represents the forward velocity noise variance, represents the rightward velocity noise variance, represents the noise variance of the heading angle; yes The moment estimation error covariance matrix; Is the state function in The Jacobian matrix at ; Is the state function in 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 at Expressed as: （6） in, Indicates the measurement The time is estimated from the state of the node, Represents the state transition function.

5. The method for navigation and positioning of a slave node assisted by a master node according to claim 4, characterized in that: The state function is The Jacobian matrix at Expressed as: （7） in, Indicates the measurement The system input vector from the node at time, Represents the state transition function.

6. The method for navigation and positioning of a slave node assisted by a master node according to claim 5, characterized in that: In step five, the master node and the slave node perform the Information interaction, , 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 its own position and acoustic pulse to each slave node, and calculates the relative distance between the master and slave nodes based on the delay time of the acoustic pulse and the speed of sound underwater. 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 navigation and positioning of a slave node assisted by 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: First The observation equation of the relative distance between the master and slave nodes at the time of information interaction is established: （8） in, express Observable quantity at a moment; is the observation function; express The position coordinates of the slave node at the moment; express The master node position coordinates obtained by the global navigation satellite system on board the master node at that moment; represents the matrix 2 norm; express The ranging noise at the moment, The time interval for the master node to send observation information, For the Information interaction, the mean is zero Gaussian white noise, the variance is .

8. The method for navigation and positioning of a slave node assisted by a master node according to claim 7, characterized in that: In step 6, random parameters that conform to the Bernoulli distribution are established , according to the judgment conditions Determine random parameters The value of When the random parameters When the value is 0, return to step 3; When the random parameters When the value is 1, execute step 7; The specific process is: Random parameters Build as: （9） in, represents the residual, , express moment observations; express Predict the relative distance between nodes one step at a time, , express One-step prediction of the node position state at time, express The master node position coordinates obtained by the global navigation satellite system on board the master node at that moment; express The state error of the slave node is predicted one step at a time. From the prediction error covariance matrix The vector consisting of the diagonal elements of ; when , indicating that the slave node receives If the error after the moment-by-moment fusion information estimation is less than the error before the estimation, proceed to step seven. The fused information is the information obtained by fusing the observation information and the state variables; when , indicating that the slave node receives If the error after the moment-by-moment fusion information estimation is greater than or equal to the error before the estimation, proceed to step three.

9. The method for navigation and positioning of a slave node assisted by 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: （10） in, Represents the prediction error covariance matrix traces; yes Identity matrix; Represents vector dot product; express The difference between the relative azimuth angles of the master and slave nodes and the heading angle of the slave node at the moment, , express The relative azimuth of the master and slave nodes at the moment, express The heading angle from the node at the moment; express The difference between the relative azimuth angles of the master and slave nodes and the heading angle of the slave node at that moment; Indicates the threshold, which is usually set to 10.

10. The method for navigation and positioning of a slave node assisted by a master node according to claim 9, characterized in that: In the step eight, The relative distance between the master and slave nodes at the time of information interaction is the observation quantity. The one-step prediction of the slave node position at the time of 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 as follows: Step 81: One-step prediction of node position at the time of information interaction for: Where, express The best position at the moment, express The optimal system input at time express The process noise at each moment; Step 82: Get Moment prediction error covariance matrix ; The expression is: Where, express The state function at the moment The Jacobian matrix at , express The moment estimation error covariance matrix, express The state function at the moment The Jacobian matrix at , express moment noise covariance matrix; Step 83: Based on Moment prediction error covariance matrix Get Kalman gain ; The expression is: （11） Where, express The time measurement function is The Jacobian matrix at , express The moment measurement noise covariance matrix; Step 84: Based on Kalman gain Hedi One-step prediction of node position at the time of information interaction , get the position state estimate ; The expression is: （12） Step 85: Based on Moment prediction error covariance matrix and Kalman gain , obtain the state estimation error covariance matrix ; The expression is: （13）。

Images