A navigation method based on adaptive prediction and compensation of observation outliers

Abstract

A navigation method based on adaptive prediction compensation of observation outliers belongs to the technical field of underwater mobile node navigation and positioning. The present invention solves the problem of poor navigation and positioning effects of existing methods. The master node of the present invention is equipped with a global navigation satellite system, a Doppler odometer and an ultra-short baseline; the slave node is equipped with a low-precision inertial navigation and a Doppler odometer, and the slave node receives the position and relative distance information of the master node. In the case of abnormal observation data, the observation anomaly compensation method is used to compensate for the outliers, and the extended Kalman filter algorithm is used to correct the self-navigation position state of the slave node, reducing the impact of observation outliers on low-precision node navigation, significantly improving the navigation accuracy and collaborative positioning accuracy of the slave node, meeting the requirements of navigation accuracy, and ensuring the effect of navigation and positioning. The method of the present invention can be applied to underwater mobile node navigation and positioning.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater moving node navigation and positioning, and in particular relates to a navigation method based on adaptive prediction and compensation of observed outliers. Background Art

[0002] Compared with the autonomous navigation of a single underwater mobile node, the underwater mobile node navigation assisted by other nodes has the advantages of high robustness, information sharing, task collaboration and long-term navigation, and has broad application prospects in the field of underwater mobile node navigation.

[0003] High-precision navigation is key to executing coordinated combat missions involving multiple underwater mobile nodes. Currently, common navigation methods include geophysical field matching navigation, inertial navigation, and acoustic navigation. However, these methods struggle to independently complete the navigation and positioning of underwater mobile nodes.

[0004] Moving-node navigation, assisted by navigation information from other nodes, involves transmitting information such as absolute position, heading, and timestamps between moving nodes through underwater acoustic communication. Using the relative distance between nodes as a constraint, information fusion is performed using an extended Kalman filter to correct positioning errors for each moving node. Moving-node navigation assisted by adjacent nodes can be categorized into two types: parallel and master-slave. Parallel navigation involves each moving node being equipped with navigation equipment of equal precision, and collaborative navigation is achieved through information sharing and inter-vessel distance constraints. This approach offers the advantage of low cost, but the disadvantage is the difficulty in achieving stable collaborative positioning. Low-precision node (slave) navigation assisted by a high-precision node (master) involves a slave node (equipped with low-precision navigation equipment) periodically receiving positioning information from the master node (equipped with high-precision navigation equipment) via acoustic sensors, measuring the relative distance between the two nodes, and using an extended Kalman filter to estimate and correct the slave node's positioning error, thereby improving the slave node's positioning accuracy. This approach offers the advantage of consistently improving the positioning accuracy of low-precision nodes and has become a major research direction in the field of low-precision node navigation. However, due to the complex and changing underwater environment, acoustic communication channels are subject to problems such as packet loss, multipath effects, and unknown noise parameters. These factors can lead to outliers in relative ranging data, and the fusion of observation outliers can reduce the accuracy of collaborative navigation positioning. Therefore, there is an urgent need for outlier identification and prediction compensation algorithms for low-precision node navigation assisted by the master node.

[0005] In the patent application "A Master-Slave Collaborative Localization Method for an Autonomous Underwater Vehicle Integrated Navigation System," published with publication number CN111595348A, a slave AUV corrects its dead-reckoning positioning error by fusing ranging information from the master and slave AUVs as observations. In the paper "Research on an Improved Master-Slave Collaborative Navigation Technology," ranging information between slave vessels is used as observations to reconstruct the system's state equations and measurement equations, improving the accuracy of the slave's position solution in traditional master-slave collaborative navigation and positioning algorithms based on ranging information. The paper "AUV Cooperative Localization Method Based on Motion Radius Vector in the Presence of Unknown Currents" uses an approximate method to simulate ocean currents as unknown constant perturbations or unknown time-varying perturbations with randomly changing directions, and employs a Kalman filter to estimate and compensate for current errors. The paper "Cooperative Localization of Multiple UUVs with Communication Delays - A Real-Time Update Method Based on Path Prediction" proposes a non-uniformly spaced real-time update positioning method based on stateless backpropagation and the master AUV's track prediction. Although existing methods use ranging information as observation quantity and adopt nonlinear filtering algorithm to realize the position correction of low-precision nodes, they do not consider the impact of observation outliers on navigation positioning accuracy and do not perform corresponding compensation, resulting in poor navigation positioning effect of existing methods. Summary of the Invention

[0006] The purpose of the present invention is to solve the problem of poor navigation and positioning effect of existing methods, and to propose a navigation method based on adaptive prediction compensation of observation outliers.

[0007] The technical solution adopted by the present invention to solve the above technical problems is: a navigation method based on adaptive prediction and compensation of observed outliers, the method specifically comprising the following steps:

[0008] Step 1: The master node and each slave node navigate according to their respective trajectories;

[0009] Step 2: Each slave node outputs its own attitude information and velocity information at time k-1, k ≥ 2, where the attitude information is the heading angle θ under the load system output by the slave node inertial navigation k-1 The unit of heading angle is rad, and the speed information is the forward speed output by the Doppler log. and right speed The units of forward speed and rightward speed are both m / s;

[0010] Step 3: Establish a one-step prediction of the position state of each slave node based on the posture and speed information of each slave node at time k-1 and the state variables of each slave node at time k-1 and the forecast error covariance matrix

[0011] Step 4: Determine whether the k moment is the moment of information interaction between nodes;

[0012] If time k is not the time of information exchange between nodes, set k=k+1 and return to step 2;

[0013] If time k is the time of information exchange between nodes, execute step 5;

[0014] Step 5: The master node outputs its own position and pulse signal at time k, and the slave node calculates the relative distance between the master and slave nodes based on the delay time of the received pulse signal; and establishes the observation equation of the relative distance between the master node and each slave node based on the master node's own position output and the calculated relative distance;

[0015] According to the established observation equation, it is judged whether the relative distance calculated from the node is an outlier. If the relative distance calculated from the node is an outlier, step 6 is executed; otherwise, step 8 is executed directly using the relative distance calculated from the node.

[0016] Step 6: Calculate the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at time k, and calculate the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at each previous information interaction moment;

[0017] According to the difference between the calculated relative azimuth angle and the heading angle of the slave node, the information interaction moment used to compensate the relative distance calculated from the slave node at time k is selected from the previous information interaction moments;

[0018] Step 7: Use the relative distance between the master and slave nodes corresponding to the information exchange moment selected in Step 6 to compensate the relative distance calculated from the slave node at time k to obtain the compensated relative distance, and then use the compensated relative distance to execute Step 8;

[0019] Step 8: Take the one-step prediction of the slave node position at time k as the state quantity, and perform extended Kalman filtering based on the relative distance to obtain the corrected slave node position;

[0020] And let time k=k+1, return to step 2 and execute until the navigation is completed.

[0021] Furthermore, the specific process of step three is:

[0022] Take any slave node as an example

[0023]

[0024] Among them, C θk-1 represents the rotation matrix, V k-1 represents the velocity vector measured by the Doppler log, Δt is the sampling interval from the node position state, x k-1 is the position estimate from the node at time k-1. If time k-1 is the moment of information interaction between nodes, then If the k-1 moment is not the moment of information exchange between nodes, then

[0025] Forecast error covariance matrix for:

[0026]

[0027] Among them, P k-1 is the k-1 moment estimation error covariance matrix. If k-1 moment is the moment of information interaction between nodes, then If the k-1 moment is not the moment of information exchange between nodes, then F k is the state transition function at x k-1 The Jacobian matrix at G, the superscript T represents the transpose of the matrix, k is the state transition function at u k-1 The Jacobian matrix at Q k is the noise covariance matrix.

[0028] Furthermore, the Jacobian matrix F k and G k They are:

[0029]

[0030] Furthermore, the specific process of step five is as follows:

[0031] Take any slave node as an example

[0032] Step 5.1: Establish the observation equation based on the master node's own position at time k and the relative distance calculated by the slave node:

[0033]

[0034] Among them, ||·||2 represents the matrix 2 norm, z k is the relative distance calculated from the node at time k, is the position coordinate of the master node in the navigation system at time k, h(·) is the observation function, η k is the ranging noise at time k, is the position coordinate of the slave node at time k;

[0035] Step 52: According to z k Calculation parameters

[0036]

[0037] Among them, S k is the residual at time k, Indicates the relative distance between the master and slave nodes for one-step prediction, b k is a constant;

[0038] Step 53: Compare parameters The relationship with the decision threshold ε:

[0039] like Then the relative distance data at time k calculated from the node is abnormal;

[0040] when Then the relative distance data at time k calculated from the node is normal.

[0041] Furthermore, the relative distance between the master and slave nodes predicted in one step is for:

[0042]

[0043] in, is the one-step prediction from the node position state at time k.

[0044] Furthermore, the constant b k for:

[0045]

[0046] Among them, z k-ΔT is the relative distance calculated from the node at time k-ΔT, It represents the relative distance between the master and slave nodes after the estimation at time k-ΔT. is the position coordinate of the master node in the navigation system at time k-ΔT, is the position state estimate of the slave node at time k-ΔT, ΔT is the time interval for the master node to send observation information, and ΔT ≥ Δt.

[0047] Furthermore, the decision threshold ε is:

[0048]

[0049] in, represents the relative azimuth angle between the master and slave nodes at time k; σ x,k|k-ΔTrepresents the easting error variance inferred from the node position between adjacent information interaction moments, σ y,k|k-ΔT represents the northing error variance inferred from the node position between adjacent information interaction moments, σ r,k represents the ranging noise η at time k k The standard deviation of .

[0050] Furthermore, in step 6, based on the difference between the calculated relative azimuth angle and the heading angle of the slave node, an information interaction moment for compensating the relative distance calculated from the slave node at time k is selected from the previous information interaction moments; the specific process is:

[0051] Step 61: Initialize i=1;

[0052] Step 6.2: Calculate the index value z′ of the relative distance between the master and slave nodes at the time of information exchange k-iΔT k-iΔT :

[0053]

[0054] Among them, z′ k-iΔT The index value representing the relative distance between the master and slave nodes at the time of information exchange k-iΔT, Indicates the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at time k, is the relative azimuth angle between the master and slave nodes at time k, θ k is the heading angle from the node at time k, φ k-iΔT It represents the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at the time of information exchange k-iΔT, and δ is the set threshold;

[0055] Step 6.3. If z′ k-iΔT =1, then the relative distance between the master and slave nodes at the k-iΔT information interaction time is used to compensate for the relative distance between the master and slave nodes at the current time, let i=i+1, and return to step 62;

[0056] If z′ k-iΔT =0, the relative distance between the master and slave nodes at the k-iΔT information interaction moment cannot be used to compensate for the relative distance between the master and slave nodes at the current moment, and the iteration is stopped. Let i = N+1 for the last iteration, and determine the information interaction moments k, k-ΔT, k-2ΔT, ..., k-NΔT used to compensate for the relative distance between the master and slave nodes at the current moment.

[0057] Furthermore, the relative distance between the master and slave nodes corresponding to the information interaction moment selected in step 6 is used to compensate the relative distance calculated from the node at time k to obtain the compensated relative distance; the specific process is:

[0058]

[0059] in, is the relative distance between the master and slave nodes after compensation, is the residual at time k-ΔT, a1 is The weight of is the residual at time k-2ΔT, a2 is The weight of is the residual at time k-NΔT, a N yes The weight of

[0060]

[0061] in, represents the velocity vector measured by the Doppler log of the slave node at time i, and are the forward velocity and right velocity output by the Doppler log at time i.

[0062] Furthermore, the specific process of step eight is as follows:

[0063] Kalman gain K k for:

[0064]

[0065] Among them, the superscript -1 represents the inverse of the matrix, H k represents the measurement matrix; R k =[σ r,k ];

[0066] Position state estimation for:

[0067]

[0068] State estimation error covariance matrix for:

[0069]

[0070] where I2 is the 2×2 identity matrix.

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

[0072] The master node of the present invention is equipped with a global navigation satellite system, a Doppler odometer and an ultra-short baseline; the slave node is equipped with a low-precision inertial navigation and a Doppler odometer, and the slave node receives the position and relative distance information of the master node. In the case of abnormal observation data, an observation abnormality compensation method is used to compensate for the abnormal value, and an extended Kalman filter algorithm is used to correct the self-navigation position state of the slave node, thereby reducing the influence of the observation abnormal value on the low-precision node navigation, significantly improving the navigation accuracy and collaborative positioning accuracy of the slave node, meeting the navigation accuracy requirements, and ensuring the effect of navigation positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is a flow chart of a navigation method based on adaptive prediction and compensation of observed outliers of the present invention;

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

[0075] Figure 3 This is the result diagram of the outlier recognition of the relative distance observation between the master and slave nodes in the present invention;

[0076] Figure 4 for Figure 3 The residual map corresponding to the recognition result;

[0077] Figure 5 This is a comparison chart of the impact of observed outliers on slave node collaborative positioning in the present invention. DETAILED DESCRIPTION

[0078] Specific implementation method 1: Combination Figure 1 This embodiment describes a navigation method based on adaptive prediction and compensation of observed outliers, which specifically includes the following steps:

[0079] Step 1: The master node can be a mobile beacon, a surface ship, etc., and the slave node can be an autonomous underwater vehicle. The master node and each slave node navigate according to their own trajectory;

[0080] Step 2: Each slave node outputs its own attitude information and velocity information at time k-1, k ≥ 2, where the attitude information is the heading angle θ under the load system output by the slave node inertial navigation k-1 The unit of heading angle is rad, and the speed information is the forward speed output by the Doppler log. and right speed The units of forward speed and rightward speed are both m / s;

[0081] The carrier system of the present invention takes the position of the centroid of the slave node as the origin O, and the positive directions of the x-axis, y-axis and z-axis are respectively in front of, to the right of and above the slave node;

[0082] Step 3: Establish a one-step prediction of the position state of each slave node based on the posture and speed information of each slave node at time k-1 and the state variable of each slave node at time k-1 (the state variable is the slave node position). and the forecast error covariance matrix

[0083] Step 4: Determine whether the k moment is the moment of information interaction between nodes;

[0084] If time k is not the time of information exchange between nodes, set k=k+1 and return to step 2;

[0085] If time k is the time of information exchange between nodes, execute step 5;

[0086] Step 5: The master node is equipped with a global navigation satellite system (or a high-precision inertial navigation system) and an ultra-short baseline. The master node outputs its own position and pulse signal at time k. The slave node calculates the relative distance between the master and slave nodes based on the delay time of the received pulse signal. The observation equation of the relative distance between the master node and each slave node is established based on the master node's own position output and the calculated relative distance.

[0087] According to the established observation equation, it is judged whether the relative distance calculated from the node is an outlier. If the relative distance calculated from the node is an outlier, step 6 is executed; otherwise, step 8 is executed directly using the relative distance calculated from the node.

[0088] Step 6: Calculate the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at time k, and calculate the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at each previous information interaction moment;

[0089] According to the difference between the calculated relative azimuth angle and the heading angle of the slave node, the information interaction moment used to compensate the relative distance calculated from the slave node at time k is selected from the previous information interaction moments;

[0090] Step 7: Use the relative distance between the master and slave nodes corresponding to the information exchange moment selected in Step 6 to compensate the relative distance calculated from the slave node at time k to obtain the compensated relative distance, and then use the compensated relative distance to execute Step 8;

[0091] Step 8: Take the one-step prediction of the slave node position at time k as the state quantity, and perform extended Kalman filtering based on the relative distance to obtain the corrected slave node position;

[0092] And let time k=k+1, return to step 2 and execute until the navigation is completed.

[0093] For a multi-autonomous underwater vehicle composed of a master node and slave nodes, 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 speed information at each moment in real time, with a sampling interval of Δt, and the value of Δt in the present invention is 1s; the position information at each moment is obtained by position estimation based on the attitude and speed information; 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 moment of information exchange in real time, with a coordination period of ΔT; then, the master node sends its own position and the relative distance information between the master and slave nodes to the slave node; the slave node determines whether the received master node information is an outlier. If it is determined to be an outlier, the outlier is compensated, and the relative distance between the master and slave nodes at the moment of information exchange is used as the observation quantity, and the position of the slave node at the moment of information exchange is used as the state quantity, and the modified observation matrix is ​​used to perform slave node extended Kalman filter estimation, and k is set to be k+1. The above process is repeated until the navigation is completed.

[0094] Specific embodiment 2: This embodiment differs from specific embodiment 1 in that the specific process of step 3 is as follows:

[0095] Take any slave node as an example

[0096]

[0097] Among them, u k-1 is the vector of the system input from the node at time k-1, w k-1 is the zero-mean Gaussian white noise output by the sensor at time k-1, f(·) is the state transition function, represents the rotation matrix, V k-1 represents the velocity vector measured by the Doppler log, Δt is the sampling interval from the node position state, x k-1 is the position estimate from the node at time k-1. If time k-1 is the moment of information interaction between nodes, then If the k-1 moment is not the moment of information exchange between nodes, then That is, it is obtained by recursion through formula (1);

[0098] Forecast error covariance matrix for:

[0099]

[0100] Among them, P k-1 is the k-1 moment estimation error covariance matrix. If k-1 moment is the moment of information interaction between nodes, then If the k-1 moment is not the moment of information exchange between nodes, then Fk is the state transition function at x k-1 The Jacobian matrix at G, the superscript T represents the transpose of the matrix, k is the state transition function at u k-1 The Jacobian matrix at Q k is the noise covariance matrix,

[0101] Other steps and parameters are the same as those in the first embodiment.

[0102] The one-step prediction method for the position state of each slave node is the same. This embodiment uses any slave node as an example for illustration, so the parameters involved in this embodiment refer to the parameters corresponding to the same slave node. Each slave node adopts the one-step prediction method for the position state of this embodiment.

[0103] Specific embodiment 3: This embodiment is different from specific embodiment 1 or 2 in that the Jacobian matrix F k and G k They are:

[0104]

[0105] Other steps and parameters are the same as those in the first or second embodiment.

[0106] Specific embodiment 4: This embodiment differs from any one of specific embodiments 1 to 3 in that the specific process of step 5 is as follows:

[0107] Take any slave node as an example

[0108] Step 5.1: Establish the observation equation based on the master node's own position at time k and the relative distance calculated by the slave node:

[0109]

[0110] Among them, ||·||2 represents the matrix 2 norm, z k is the relative distance calculated from the node at time k, is the position coordinate of the master node in the navigation system at time k, h(·) is the observation function, η k is the ranging noise at time k, is the position coordinate of the slave node at time k (an unknown quantity that needs to be estimated using formula (14));

[0111] The navigation system of the present invention refers to the northeast celestial coordinate system;

[0112] Step 52: According to z k Calculation parameter S k :

[0113]

[0114] Among them, S k is the residual at time k, Indicates the relative distance between the master and slave nodes for one-step prediction, b k is a constant;

[0115] Step 53: Compare parameters The relationship with the decision threshold ε:

[0116] like Then the relative distance data at time k calculated from the node is abnormal;

[0117] when Then the relative distance data at time k calculated from the node is normal.

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

[0119] At any information exchange moment, the method for determining whether the relative distance data calculated by each slave node is abnormal is the same. This embodiment uses any slave node as an example for illustration, so the parameters involved in this embodiment refer to the parameters corresponding to the same slave node. The process of this embodiment can be repeated for the relative distance data calculated by each slave node.

[0120] Specific embodiment 5: This embodiment differs from specific embodiments 1 to 4 in that the relative distance between the master and slave nodes predicted in one step is for:

[0121]

[0122] in, is the one-step prediction from the node position state at time k.

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

[0124] Specific embodiment 6: This embodiment differs from any one of specific embodiments 1 to 5 in that the constant b k for:

[0125]

[0126] Among them, z k-ΔT is the relative distance calculated from the node at time k-ΔT, It represents the relative distance between the master and slave nodes after the estimation at time k-ΔT. is the position coordinate of the master node in the navigation system at time k-ΔT, is the position state estimate of the slave node at time k-ΔT, ΔT is the time interval for the master node to send observation information, and ΔT ≥ Δt.

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

[0128] Specific embodiment 7: This embodiment differs from any one of specific embodiments 1 to 6 in that the decision threshold ε is:

[0129]

[0130] in, represents the relative azimuth between the master and slave nodes at time k (obtained based on the position of the slave node at time k predicted in one step and the position output by the master node); σ x,k|k-ΔT represents the easting error variance inferred from the node position between adjacent information interaction moments, σ y,k|k-ΔT represents the northing error variance inferred from the node position between adjacent information interaction moments, σ r,k represents the ranging noise η at time k k The standard deviation of .

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

[0132] Eastward error variance σ x,k|k-ΔT and northing error variance σ y,k|k-ΔT according to get.

[0133] Specific embodiment eight: This embodiment differs from any one of specific embodiments one to seven in that, in step six, based on the difference between the calculated relative azimuth angle and the heading angle of the slave node, an information interaction moment for compensating the relative distance calculated from the slave node at time k is selected from the previous information interaction moments; the specific process is as follows:

[0134] Step 61: Initialize i=1;

[0135] Step 6.2: Calculate the index value z′ of the relative distance between the master and slave nodes at the time of information exchange k-iΔT k-iΔT :

[0136]

[0137] Among them, z′ k-iΔT The index value representing the relative distance between the master and slave nodes at the time of information exchange k-iΔT, Indicates the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at time k, is the relative azimuth angle between the master and slave nodes at time k, θ k is the heading angle from the node at time k, φk-iΔT It represents the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at the time of information exchange k-iΔT. δ is the set threshold value, which is 10.

[0138] Step 6.3. If z′ k-iΔT =1, then the relative distance between the master and slave nodes at the k-iΔT information interaction time is used to compensate for the relative distance between the master and slave nodes at the current time, let i=i+1, and return to step 62;

[0139] If z′ k-iΔT =0, the relative distance between the master and slave nodes at the k-iΔT information interaction moment cannot be used to compensate for the relative distance between the master and slave nodes at the current moment, and the iteration is stopped. Let i = N+1 for the last iteration, and determine the information interaction moments k, k-ΔT, k-2ΔT, ..., k-NΔT used to compensate for the relative distance between the master and slave nodes at the current moment.

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

[0141] Taking any slave node as an example, the index value of the relative distance between the master node and the slave node at each historical information exchange moment is calculated. Based on the index value, a value is then selected to compensate for the relative distance calculated for the slave node at the current information exchange moment. Similarly, the method of selecting relative distance data for compensation for each slave node is the method of this embodiment.

[0142] Specific embodiment nine: This embodiment differs from any one of specific embodiments one to eight in that the relative distance between the master and slave nodes corresponding to the information interaction moment selected in step six is ​​used to compensate the relative distance calculated from the node at time k to obtain the compensated relative distance; the specific process is as follows:

[0143]

[0144] in, is the relative distance between the master and slave nodes after compensation, is the residual at time k-ΔT, a1 is The weight of is the residual at time k-2ΔT, a2 is The weight of is the residual at time k-NΔT, a N yes The weight of

[0145] It should be noted that if there is a moment in the selected information interaction moment where the original relative distance is an abnormal value, then at the information interaction moment corresponding to the abnormal value, the relative distance data used to calculate the residual amount in formula (11) should be the compensated relative distance data;

[0146]

[0147] in, represents the velocity vector measured by the Doppler log of the slave node at time i, and are the forward velocity and right velocity output by the Doppler log at time i.

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

[0149] The compensation method of this embodiment is used for the abnormal relative distance data of each slave node.

[0150] Specific embodiment 10: This embodiment differs from any one of specific embodiments 1 to 9 in that the specific process of step 8 is as follows:

[0151] Kalman gain K k for:

[0152]

[0153] Among them, the superscript -1 represents the inverse of the matrix, H k represents the measurement matrix; R k =[σ r,k ];

[0154] The measurement matrix is ​​the measurement equation in The Jacobian matrix at is as follows:

[0155]

[0156] Position state estimation (i.e., the result of node position correction):

[0157]

[0158] When the compensation process is completed, the z k for

[0159] State estimation error covariance matrix for:

[0160]

[0161] where I2 is the 2×2 identity matrix.

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

[0163] Experimental part

[0164] The method of the present invention is verified by MATLAB simulation for a multi-autonomous underwater vehicle consisting of only one master node and one slave node.

[0165] Simulation conditions:

[0166] 1) Node motion parameters:

[0167] High-precision nodes:

[0168] Initial position: (-4500m, 0m);

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

[0170] Azimuth: Φ z =Φ m sin(ω1t),Φ m =0.5°, ω1=1 / 520;

[0171] Low-precision nodes:

[0172] Initial position: (0m, 0m);

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

[0174] Azimuth: φ z =φ m sin(ω2t)+φ0,φ m =0.05°, ω2=1 / 1200, φ0=1°;

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

[0176] DVL: The speed measurement error is zero-mean Gaussian white noise with variance

[0177] INS: Azimuth velocity error is zero-mean Gaussian white noise with variance σ θ =(0.0005°) 2 ;

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

[0179] The ranging errors are all Gaussian white noise with zero mean and variance σ r =(0.1m) 2 ;

[0180] 4) Kalman filter parameters:

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

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

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

[0184] 5) Observe abnormal value parameters during information interaction between nodes:

[0185] The observation anomaly occurrence time is set as: random noise with a mean of 10m and a standard deviation of 30m and random noise with a mean of -10m and a standard deviation of 30m are selected with equal probability.

[0186] 6) Other parameters:

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

[0188] By using the method of the present invention, the navigation positioning result of the slave node under the assistance of the master node is obtained. Figure 2 As shown, the navigation trajectory diagram of the master and slave nodes is obtained; Figure 3 and Figure 4 This is the outlier identification diagram of the relative distance observation between the master and slave nodes. As can be seen from the figure, during the entire slave node voyage, the number of information exchanges between nodes is 43 times, the number of outliers is 14 times, the number of identified outliers is 13 times, and the identification rate is 92.86%; Figure 5 This is a comparison chart of the positioning errors of slave node navigation. It can be seen from the figure that the average positioning error of the entire voyage of the slave node navigation assisted by the master node after the wild value compensation is 4.24m, and the average positioning error of the entire voyage when there is no wild value is 4.14m, that is, the positioning error of the method of the present invention is at the same order of magnitude as that when there is no wild value, indicating that the method of the present invention greatly reduces the impact of observed wild values ​​on the slave node navigation assisted by the master node.

[0189] The above examples are merely illustrative of the calculation model and process of the present invention and are not intended to limit the embodiments of the present invention. Persons skilled in the art will readily appreciate that other variations or modifications based on the above description are possible. This list of embodiments is not exhaustive; however, any obvious variations or modifications derived from the technical solution of the present invention remain within the scope of protection of the present invention.

Claims

1. A navigation method based on adaptive prediction and compensation of observed outliers, characterized in that: The method specifically comprises the following steps: Step 1: The master node and each slave node navigate according to their respective trajectories; Step 2: Each slave node outputs its own attitude information and velocity information at time k-1, k ≥ 2, where the attitude information is the heading angle θ under the load system output by the slave node inertial navigation k-1 The unit of heading angle is rad, and the speed information is the forward speed output by the Doppler log. and right speed The units of forward speed and rightward speed are both m / s; Step 3: Establish a one-step prediction of the position state of each slave node based on the posture and speed information of each slave node at time k-1 and the state variables of each slave node at time k-1 and the forecast error covariance matrix Step 4: Determine whether the k moment is the moment of information interaction between nodes; If time k is not the time of information exchange between nodes, set k=k+1 and return to step 2; If time k is the time of information exchange between nodes, execute step 5; Step 5: The master node outputs its own position and pulse signal at time k, and the slave node calculates the relative distance between the master and slave nodes based on the delay time of the received pulse signal; and establishes the observation equation of the relative distance between the master node and each slave node based on the master node's own position output and the calculated relative distance; According to the established observation equation, it is judged whether the relative distance calculated from the node is an outlier. If the relative distance calculated from the node is an outlier, step 6 is executed; otherwise, step 8 is executed directly using the relative distance calculated from the node. Step 6: Calculate the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at time k, and calculate the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at each previous information interaction moment; According to the difference between the calculated relative azimuth angle and the heading angle of the slave node, the information interaction moment used to compensate the relative distance calculated from the slave node at time k is selected from the previous information interaction moments; Step 7: Use the relative distance between the master and slave nodes corresponding to the information exchange moment selected in Step 6 to compensate the relative distance calculated from the slave node at time k to obtain the compensated relative distance, and then use the compensated relative distance to execute Step 8; Step 8: Take the one-step prediction of the slave node position at time k as the state quantity, and perform extended Kalman filtering based on the relative distance to obtain the corrected slave node position; And let time k=k+1, return to step 2 and execute until the navigation is completed.

2. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 1, characterized in that: The specific process of step three is: Take any slave node as an example in, represents the rotation matrix, V k-1 represents the velocity vector measured by the Doppler log, Δt is the sampling interval from the node position state, x k-1 is the position estimate from the node at time k-1. If time k-1 is the moment of information interaction between nodes, then If the k-1 moment is not the moment of information exchange between nodes, then Forecast error covariance matrix for: Among them, P k-1 is the k-1 moment estimation error covariance matrix. If k-1 moment is the moment of information interaction between nodes, then If the k-1 moment is not the moment of information exchange between nodes, then F k is the state transition function at x k-1 The Jacobian matrix at G, the superscript T represents the transpose of the matrix, k is the state transition function at u k-1 The Jacobian matrix at Q k is the noise covariance matrix.

3. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 2, characterized in that: The Jacobian matrix F k and G k They are:

4. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 3, characterized in that: The specific process of step five is: Take any slave node as an example Step 5.1: Establish the observation equation based on the master node's own position at time k and the relative distance calculated by the slave node: Among them, ||·||2 represents the matrix 2 norm, z k is the relative distance calculated from the node at time k, is the position coordinate of the master node in the navigation system at time k, h(·) is the observation function, η k is the ranging noise at time k, is the position coordinate of the slave node at time k; Step 52: According to z k Calculation parameters S k =S k -b k (6) Among them, S k is the residual at time k, Indicates the relative distance between the master and slave nodes for one-step prediction, b k is a constant; Step 53: Compare parameters The relationship with the decision threshold ε: like Then the relative distance data at time k calculated from the node is abnormal; when Then the relative distance data at time k calculated from the node is normal.

5. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 4, characterized in that: The relative distance between the master and slave nodes predicted in one step for: in, is the one-step prediction from the node position state at time k.

6. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 5, characterized in that: The constant b k for: Among them, z k-ΔT is the relative distance calculated from the node at time k-ΔT, It represents the relative distance between the master and slave nodes after the estimation at time k-ΔT. is the position coordinate of the master node in the navigation system at time k-ΔT, is the position state estimate of the slave node at time k-ΔT, ΔT is the time interval for the master node to send observation information, and ΔT ≥ Δt.

7. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 6, characterized in that: The decision threshold ε is: in, represents the relative azimuth angle between the master and slave nodes at time k; σ x,k|k-ΔT represents the easting error variance inferred from the node position between adjacent information interaction moments, σ y,k|k-ΔT represents the northing error variance inferred from the node position between adjacent information interaction moments, σ r,k represents the ranging noise η at time k k The standard deviation of .

8. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 7, characterized in that: In step 6, based on the difference between the calculated relative azimuth and the heading angle of the slave node, an information interaction moment for compensating the relative distance calculated from the slave node at time k is selected from the previous information interaction moments; the specific process is: Step 61: Initialize i=1; Step 6.2: Calculate the index value z′ of the relative distance between the master and slave nodes at the time of information exchange k-iΔT k-iΔT : Among them, z′ k-iΔT The index value representing the relative distance between the master and slave nodes at the time of information exchange k-iΔT, Indicates the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at time k, is the relative azimuth angle between the master and slave nodes at time k, θ k is the heading angle from the node at time k, φ k-iΔT It represents the difference between the relative azimuth angle of the master and slave nodes and the heading angle of the slave node at the time of information exchange k-iΔT, and δ is the set threshold; Step 6.

3. If z′ k-iΔT =1, then the relative distance between the master and slave nodes at the k-iΔT information interaction time is used to compensate for the relative distance between the master and slave nodes at the current time, let i=i+1, and return to step 62; If z′ k-iΔT =0, the relative distance between the master and slave nodes at the k-iΔT information interaction moment cannot be used to compensate for the relative distance between the master and slave nodes at the current moment, and the iteration is stopped. Let i = N+1 for the last iteration, and determine the information interaction moments k, k-ΔT, k-2ΔT, ..., k-NΔT used to compensate for the relative distance between the master and slave nodes at the current moment.

9. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 8, characterized in that: The relative distance between the master and slave nodes corresponding to the information interaction moment selected in step 6 is used to compensate the relative distance calculated from the slave node at time k to obtain the compensated relative distance; the specific process is: in, is the relative distance between the master and slave nodes after compensation, is the residual at time k-ΔT, a1 is The weight of is the residual at time k-2ΔT, a2 is The weight of is the residual at time k-NΔT, a N yes The weight of in, represents the velocity vector measured by the Doppler log of the slave node at time i, and are the forward velocity and right velocity output by the Doppler log at time i.

10. The navigation method based on adaptive prediction and compensation of observed outliers according to claim 9, characterized in that: The specific process of step eight is as follows: Kalman gain K k for: Among them, the superscript -1 represents the inverse of the matrix, H k represents the measurement matrix; R k =[σ r,k ]; Position state estimation for: State estimation error covariance matrix for: where I2 is the 2×2 identity matrix.

Images