AUV (Autonomous Underwater Vehicle) cooperative positioning method based on intersection of multiple ranging circles

Abstract

The invention relates to an AUV (Autonomous Underwater Vehicle) cooperative positioning method based on multi-ranging circle intersection. The invention relates to the technical field of underwater AUV (Autonomous Underwater Vehicle) cooperative positioning. The method comprises the following steps: establishing a kinematic model and a measurement model of an AUV; obtaining a distance measurement circle equation at the current moment and current control input, and jointly resolving an equation set constructed by a historical distance measurement circle; removing wrong candidate points; and determining error covariance matrixes of all candidate solutions and cross covariance matrixes among the candidate solutions to obtain a joint covariance matrix of all position estimations, and performing optimal weighted fusion on the candidate solutions by using linear least squares to obtain optimal position estimation and corresponding estimation error covariance. According to the method, a plurality of potential position estimation values are creatively generated through combined solution of historical distance measurement equations, fuzzy solutions are systematically removed according to the shape of an uncertain region, an accurate inner point set is obtained, and the method has high state estimation precision and low time complexity while estimation consistency is guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater AUV collaborative positioning, and is an AUV collaborative positioning method based on the intersection of multiple ranging circles. Background Art

[0002] Autonomous underwater vehicles (AUVs) play a vital role in search and rescue, area exploration, and other underwater missions. Compared to a single AUV, multiple AUVs working together offer significant efficiency and economic advantages in exploring unknown areas and performing search and rescue operations. Accurate positioning information is a crucial prerequisite for these multi-AUV collaborations to accomplish these tasks. However, achieving precise underwater positioning for AUVs is extremely challenging due to the complex underwater environment and limited sensor capabilities.

[0003] Currently, there are three main types of AUV positioning methods: autonomous positioning, long-baseline / buoy positioning, and collaborative positioning. Autonomous positioning requires the AUV to be equipped with sensors such as a high-precision inertial measurement unit and a Doppler odometer to maintain long-term autonomous positioning accuracy. This is costly and the navigation system is bulky. In long-baseline or buoy-based positioning solutions, the AUV calculates its position by measuring the distance from multiple pre-calibrated acoustic beacons. However, the deployment and maintenance costs of the beacons are high, and the AUV's maneuverability is limited by the beacon area. In collaborative positioning solutions, an autonomous surface vehicle (ASV) periodically transmits communication packets to an underwater AUV. The AUV then obtains its distance from the ASV, as well as statistical information such as the ASV's absolute position estimate and the corresponding error covariance carried in the packets. State estimation algorithms such as the extended Kalman filter are then used to estimate its position. In this approach, the AUV effectively uses the ASV as a mobile beacon. Collaborative positioning is cost-effective and efficient, and has become one of the most promising technologies for achieving accurate and reliable underwater positioning for AUVs.

[0004] Commonly used underwater AUV collaborative localization methods can be categorized into three categories: those based on the extended Kalman filter, the sigma point Kalman filter, and the particle filter. The extended Kalman filter and its dual form, the extended information filter, perform Taylor linearization on the state and measurement equations, then fuse prior and measurement information using conventional linear Kalman filters and information filters. These filters can suffer from severe estimation inconsistency when the Taylor expansion points are inaccurate or the equations are highly nonlinear. Estimation inconsistency occurs when the estimated error variance of the filter output is less than the estimated actual error variance, leading to misconceptions about the AUV's position and potentially hazardous planning and control behaviors. Sigma point Kalman filter-based methods tend to approximate the statistical moments of random vectors after nonlinear propagation rather than linearizing the nonlinear function. This alleviates the estimation inconsistency issue to some extent, but does not fully resolve it. These mainstream AUV collaborative localization methods based on the Kalman filter (eKF, information filter, and sigma point Kalman filter) all suffer from estimation inconsistency. Particle filtering uses a large number of particles to capture the actual uncertain areas of the state, fundamentally solving the problem of inconsistent estimation when the prior and noise density are known. However, its computational complexity is too large to be widely used on AUVs with limited processing capabilities.

[0005] In addition to the three methods mentioned above, a positioning method based on two-circle intersection has also been applied to AUV collaborative positioning. This method relies on the intersection of two ranging circles to directly solve for the AUV's position, hence the name "two-circle intersection method." Compared to the three existing methods, the two-circle intersection method offers better estimation consistency because the estimated value is obtained by precisely solving the equation without introducing linearization or statistical linearization errors. However, the two-circle intersection method also has a significant disadvantage: limited estimation accuracy. This approach uses only two ranging circles to generate a set of position estimates, and then selects a unique solution by correlating it with data from past solutions. This results in extremely low information utilization efficiency (the final position estimate utilizes information from only two ranging circles). Summary of the Invention

[0006] In response to the shortcomings of the existing technology, the present invention provides an AUV collaborative positioning method based on the intersection of multiple ranging circles. By jointly solving multiple historical ranging equations to generate a potential position estimation set, it effectively overcomes the estimation inconsistency problem caused by traditional filtering methods when dealing with nonlinearity. This method adopts a two-step fuzzy solution elimination method. First, it eliminates indistinguishable decompositions based on the shape of the uncertainty region, then screens high-confidence inliers through density clustering, and finally optimally fuses all inliers by explicitly modeling the correlation between inlier estimates. Compared with Kalman filtering methods, this algorithm has the advantages of independence from initial estimation, higher positioning accuracy and estimation consistency, and can quickly restore positioning performance after sensor anomalies or communication interruptions.

[0007] The present invention provides.

[0008] The present invention provides the following technical solutions: A collaborative positioning method for AUVs based on the intersection of multiple ranging circles, comprising the following steps: Step 1: Establish the kinematic model and measurement model of AUV; Step 2: Get the current distance circle equation and the current control input, and add them to the historical data queue. When the queue length exceeds the set value, remove the earliest element; Step 3: Solve the equations constructed by the historical ranging circle together. The obtained solutions are potential candidate points of the true position, and these solutions are added to the candidate solution set. Step 4: Eliminate incorrect candidate points and retain solutions that are clustered near the true location; Step 5: Determine the error covariance matrix of all candidate solutions and the cross-covariance matrix between candidate solutions to obtain the joint covariance matrix of all position estimates. Use linear least squares to perform optimal weighted fusion on the candidate solutions to obtain the optimal position estimate and the corresponding estimation error covariance matrix.

[0009] Preferably, the method further comprises: Establish the ASV's kinematic model and measurement model, input the ASV's initial position estimate, covariance matrix, time step, and time series correlation coefficient matrix; use Kalman filtering to update the ASV's position in time; if GNSS measurement is received, use Kalman filtering to fuse the ASV's position information and GNSS measurement information to obtain a new ASV position estimate, covariance matrix, and time series correlation coefficient matrix; if the current time is the broadcast time, send the ASV's position estimate, covariance matrix, and time series correlation coefficient matrix to the AUV for subsequent calculations on the AUV side.

[0010] Preferably, the step 1 is specifically: Establish the AUV two-dimensional kinematic model as the dynamic model of the system:

[0011] in, represents the horizontal coordinate of the AUV in the predefined northeast sky coordinate system, are the starboard speed and forward speed of the AUV, represents the heading angle of the AUV, is the sampling time; After the AUV receives the broadcast from the ASV, the measurement model of the received ranging measurement value is:

[0012] in, represents the 2D projected range measurement value obtained by using the actual 3D range value and the depth difference between the ASV and AUV, represents zero-mean Gaussian white ranging noise, Indicates the ASV broadcast time, Represents The relevant receiving time is related to , no distinction will be made in the future, and all will be used Indicates the time of ranging measurement and the broadcast time of the corresponding ASV; The state vector of the AUV is defined as , the state space model is obtained as:

[0013] in, Indicates process noise The noisy control input includes the starboard speed noise and the forward speed noise. and heading angle noise , [ ] indicates that ASV is at the time of broadcast The state space model of ASV is as follows

[0014] in, Represents the GNSS position measurement matrix.

[0015] Preferably, the step 2 is specifically as follows: The equation of each ranging circle is:

[0016] in, express The position of the AUV at the moment; 、 Respectively represent ASV at time and Position estimation; Indicates that the AUV is arrive The displacement estimate, , Indicates the ranging value; time The past distance measurement circle is "translated" to the current moment Form a new transformed current circle, the translation distance is exactly the displacement , the intersection of the past distance measurement circle and the current distance measurement circle is the current potential position solution; the moment is defined by the following set The intersection of the distance measuring circle and the current distance measuring circle:

[0017] Among them, the subscript Indicates at time The previous circle and at time The current circle, and Represents a two-dimensional circle intersection function:

[0018] in, is the intersection set, which can have one or two elements; is the number of the solution, which can be 1 or 2.

[0019] Preferably, the step 3 is specifically: When calculating the intersection of more circles, at time A set of position solutions can be obtained ,in = .

[0020] Preferably, the step 4 is specifically as follows: First, the discrimination condition is used to eliminate the candidate points generated by the intangent or circumtangent of the two ranging circles. Each candidate point is traversed and the number of candidate points contained in the circle with the point as the center and the length calculated online as the radius is counted. The area with the largest number of points is selected as the pure inner point set.

[0021] Preferably, the step 5 is specifically as follows: Step 5.1: Calculate the autocovariance matrix of the candidate points, specifically: By Performing first-order linearization, we get

[0022] in, is the Jacobian matrix, is the estimated error of the intersection position, The autocovariance matrix of is expressed as:

[0023]

[0024]

[0025]

[0026] in, is a timing-related term, This is the time series correlation coefficient matrix sent by ASV. Respectively indicate time and The ranging noise covariance matrix, and the arrive The cumulative process noise covariance matrix of ; Single-step process noise covariance matrix It is expressed as follows:

[0027]

[0028] in, , and Respectively indicate time The measured starboard speed, forward speed and heading angle, , and represents the corresponding error variance; then Calculated as =

[0029] Step 5.2: Calculate the error covariance matrix between different candidate points: Position estimation The estimated error of is written as:

[0030] set up ; Cross-covariance matrix:

[0031]

[0032] The joint covariance matrix of all position estimates in is expressed as

[0033] in, It is the set of candidate points retained after screening in the second step; Step 5.3: Perform weighted fusion on the information and use For all position estimates in , the following linear least squares problem is established:

[0034]

[0035] The solution to the least squares problem is:

[0036] The estimated error covariance is and position estimation This is the output value of the algorithm.

[0037] An AUV collaborative positioning system based on multi-range circle intersection, the system comprising: A model building module, wherein the model building module builds a kinematic model and a measurement model of the AUV; A data acquisition module, which obtains the current distance circle equation and the current control input, and adds them to the historical data queue. When the queue length exceeds a set value, the earliest element is removed; A solution module, which solves the system of equations constructed by the historical ranging circles, and the obtained solutions are potential real position candidate points, and these solutions are added to the candidate solution set; A fuzzy point elimination module, which eliminates incorrect candidate points and retains solutions that are clustered near the true position; An estimation module determines the error covariance matrix of all candidate solutions and the cross-covariance matrix between the candidate solutions to obtain a joint covariance matrix of all position estimates, and uses linear least squares to perform optimal weighted fusion on the candidate solutions to obtain the optimal position estimate and the corresponding estimation error covariance matrix.

[0038] A computer-readable storage medium stores a computer program, which is executed by a processor to implement an AUV collaborative positioning method based on the intersection of multiple ranging circles.

[0039] A computer device includes a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, an AUV collaborative positioning method based on the intersection of multiple ranging circles is implemented.

[0040] The present invention has the following beneficial effects: Compared with the prior art, the present invention has the following advantages: This paper innovatively proposes to generate multiple potential position estimates by jointly solving historical ranging equations. Based on the shape of the uncertainty region, the ambiguity of the solution is systematically removed to obtain an accurate set of interior points. An optimal fusion algorithm is then developed to fuse these interior points to provide a precise position estimate that incorporates multiple ranging and position inference information. This approach achieves high state estimation accuracy and low time complexity while ensuring consistent estimates.

[0041] In practical applications, when faced with nonlinearities in measurement equations, Kalman filter-based methods approximate the posterior density to a Gaussian distribution, potentially leading to serious estimation inconsistencies. The two-circle intersection rule suffers from low estimation accuracy due to its poor information utilization. Compared to mainstream Kalman filter methods, this method offers improved estimation accuracy and consistency, effectively addressing the estimation inconsistencies caused by measurement nonlinearities. It also consumes fewer computational resources than particle filters, resulting in high computational efficiency.

[0042] Compared with existing Kalman filter-based methods, this method has the following three major advantages. (1) Initial estimation independence: The estimation error of this algorithm framework can converge quickly without the need for precise initial estimation. (2) Higher positioning accuracy and consistency: Compared with Kalman filter-based methods, this method can provide better estimation accuracy and consistency and effectively handle measurement nonlinearity problems. (3) Fast recovery capability: After temporary abnormal underwater problems such as sensor failure, ranging outliers, and model uncertainty, this method can quickly restore normal functions. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the specific embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0044] Figure 1 Shown are the root mean square error curves of different algorithms under different initial errors; Figure 2 Shown are plots of the position root mean square error of different algorithms used for accuracy evaluation; Figure 3 Shown are line plots of the normalized squared estimation errors of different algorithms used for consistency assessment; Figure 4 Shown is a graph showing the change of RMSE over time for different algorithms after encountering underwater problems. Figure 5 Displayed are position error graphs for different algorithms; Figure 6 Shown is a flow chart of the present invention. DETAILED DESCRIPTION

[0045] The technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described are only some embodiments of the present invention, not all embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0046] The present invention is described in detail below with reference to specific embodiments. Specific embodiment one: according to Figures 1 to 6 As shown, the specific optimization technical solution adopted by the present invention to solve the above technical problems is: the present invention relates to an AUV collaborative positioning method based on the intersection of multiple ranging circles.

[0048] The present invention provides an AUV collaborative positioning method based on the intersection of multiple ranging circles, the method comprising the following steps: Step 1: Establish the kinematic model and measurement model of AUV; Step 2: Get the current distance circle equation and the current control input, and add them to the historical data queue. When the queue length exceeds the set value, remove the earliest element; Step 3: Solve the equations constructed by the historical ranging circle together. The obtained solutions are potential candidate points of the true position, and these solutions are added to the candidate solution set. Step 4: Eliminate incorrect candidate points and retain solutions that are clustered near the true location; Step 5: Determine the error covariance matrix of all candidate solutions and the cross-covariance matrix between candidate solutions to obtain the joint covariance matrix of all position estimates. Use linear least squares to perform optimal weighted fusion on the candidate solutions to obtain the optimal position estimate and the corresponding estimation error covariance matrix. Specific embodiment two: The difference between the second embodiment of the present application and the first embodiment is that: The method further comprises: Establish the ASV's kinematic model and measurement model, input the ASV's initial position estimate, covariance matrix, time step, and time series correlation coefficient matrix; use Kalman filtering to update the ASV's position in time; if GNSS measurement is received, use Kalman filtering to fuse the ASV's position information and GNSS measurement information to obtain a new ASV position estimate, covariance matrix, and time series correlation coefficient matrix; if the current time is the broadcast time, send the ASV's position estimate, covariance matrix, and time series correlation coefficient matrix to the AUV for subsequent calculations on the AUV side. Specific embodiment three: The only difference between the third embodiment of the present application and the second embodiment is that: Establish the AUV two-dimensional kinematic model as the dynamic model of the system:

[0051] in, represents the horizontal coordinate of the AUV in the predefined northeast sky coordinate system, are the starboard speed and forward speed of the AUV, represents the heading angle of the AUV, is the sampling time; After the AUV receives the broadcast from the ASV, the measurement model of the received ranging measurement value is:

[0052] in, represents the 2D projected range measurement value obtained by using the actual 3D range value and the depth difference between the ASV and AUV, represents zero-mean Gaussian white ranging noise, Indicates the ASV broadcast time, Represents The relevant receiving time is related to , no distinction will be made in the future, and all will be used Indicates the time of ranging measurement and the broadcast time of the corresponding ASV; The state vector of the AUV is defined as , the state space model is obtained as:

[0053] in, Indicates process noise The noisy control input includes the starboard speed noise and the forward speed noise. and heading angle noise , [ ] indicates that ASV is at the time of broadcast The state space model of ASV is as follows

[0054] in, Represents the GNSS position measurement matrix. Specific embodiment four: The only difference between the fourth embodiment of the present application and the third embodiment is that: The step 2 is specifically as follows: The equation of each ranging circle is:

[0056] in, express The position of the AUV at the moment; 、 Respectively represent ASV at time and Position estimation; Indicates that the AUV is arrive The displacement estimate, , Indicates the ranging value; time The past distance measurement circle is "translated" to the current moment Form a new transformed current circle, the translation distance is exactly the displacement , the intersection of the past distance measurement circle and the current distance measurement circle is the current potential position solution; the moment is defined by the following set The intersection of the distance measuring circle and the current distance measuring circle:

[0057] Among them, the subscript Indicates at time The previous circle and at time The current circle, and Represents a two-dimensional circle intersection function:

[0058] in, is the intersection set, which can have one or two elements; is the number of the solution, which can be 1 or 2. Specific embodiment five: The only difference between the fifth embodiment of the present invention and the fourth embodiment is that: The step 3 is specifically as follows: When calculating the intersection of more circles, at time A set of position solutions can be obtained ,in = . Specific embodiment six: The only difference between the sixth embodiment of the present invention and the fifth embodiment is that: The step 4 is specifically as follows: First, the discrimination condition is used to eliminate the candidate points generated by the intangent or circumtangent of the two ranging circles. Each candidate point is traversed and the number of candidate points contained in the circle with the point as the center and the length calculated online as the radius is counted. The area with the largest number of points is selected as the pure inner point set. Specific embodiment seven: The only difference between the seventh embodiment of the present invention and the sixth embodiment is that: The step 5 is specifically as follows: Step 5.1: Calculate the autocovariance matrix of the candidate points, specifically: By Performing first-order linearization, we get

[0062] in, is the Jacobian matrix, is the estimated error of the intersection position, The autocovariance matrix of is expressed as:

[0063]

[0064]

[0065]

[0066] in, is a timing-related term, This is the time series correlation coefficient matrix sent by ASV. Respectively indicate time and The ranging noise covariance matrix, and the arrive The cumulative process noise covariance matrix of ; Single-step process noise covariance matrix It is expressed as follows:

[0067]

[0068] in, , and Respectively indicate time The measured starboard speed, forward speed and heading angle, , and represents the corresponding error variance; then Calculated as =

[0069] Step 5.2: Calculate the error covariance matrix between different candidate points: Position estimation The estimated error of is written as:

[0070] set up ; Cross-covariance matrix:

[0071]

[0072] The joint covariance matrix of all position estimates in is expressed as

[0073] in, It is the set of candidate points retained after screening in the second step; Step 5.3: Perform weighted fusion on the information and use For all position estimates in , the following linear least squares problem is established:

[0074]

[0075] The solution to the least squares problem is:

[0076] The estimated error covariance is and position estimation This is the output value of the algorithm. Specific embodiment eight: The only difference between the eighth embodiment of the present invention and the seventh embodiment is that: The present invention provides an AUV collaborative positioning system based on the intersection of multiple ranging circles, the system comprising: A model building module, wherein the model building module builds a kinematic model and a measurement model of the AUV; A data acquisition module, which obtains the current distance circle equation and the current control input, and adds them to the historical data queue. When the queue length exceeds a set value, the earliest element is removed; A solution module, which solves the system of equations constructed by the historical ranging circles, and the obtained solutions are potential real position candidate points, and these solutions are added to the candidate solution set; A fuzzy point elimination module, which eliminates incorrect candidate points and retains solutions that are clustered near the true position; An estimation module determines the error covariance matrix of all candidate solutions and the cross-covariance matrix between the candidate solutions to obtain a joint covariance matrix of all position estimates, and uses linear least squares to perform optimal weighted fusion on the candidate solutions to obtain the optimal position estimate and the corresponding estimation error covariance matrix. Specific embodiment nine: The only difference between the ninth embodiment of the present invention and the eighth embodiment is that: The present invention provides a computer-readable storage medium on which a computer program is stored. The program is executed by a processor to implement an AUV collaborative positioning method based on the intersection of multiple ranging circles. Specific embodiment ten: The only difference between the tenth embodiment of the present invention and the ninth embodiment is that: The present invention provides a computer device, comprising a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, an AUV collaborative positioning method based on the intersection of multiple ranging circles is implemented. Specific embodiment eleven: The only difference between the eleventh embodiment of the present invention and the tenth embodiment is that: In order to overcome the problems of existing methods, the present invention provides an AUV collaborative positioning method based on the intersection of multiple ranging circles. By jointly solving multiple historical ranging equations, a potential position estimation set is generated, which effectively overcomes the estimation inconsistency problem caused by traditional filtering methods when dealing with nonlinearity. This method adopts a two-step fuzzy solution elimination method. First, it eliminates indistinguishable decompositions based on the shape of the uncertainty region, then screens high-confidence inliers through density clustering, and finally optimally fuses all inliers by explicitly modeling the correlation between inlier estimates. Compared with Kalman filtering methods, this algorithm has the advantages of independence from initial estimation, higher positioning accuracy and estimation consistency, and can quickly restore positioning performance after sensor anomalies or communication interruptions.

[0081] The present invention provides an AUV collaborative positioning method based on the intersection of multiple ranging circles, comprising: ASV side: Step 1: Establish the kinematic model and measurement model of ASV Step 2: Input the initial position estimate, covariance matrix, time step, and temporary correlation coefficient matrix of the ASV.

[0082] Step 3: Use Kalman filtering to update the ASV position in time.

[0083] Step 4: If GNSS measurements are received, use Kalman filtering to update the measurements with the GNSS information to obtain a new ASV position estimate, covariance matrix, and temporary correlation coefficient matrix.

[0084] Step 5: If the current time is the broadcast time, the ASV position estimate, covariance matrix, and temporary correlation coefficient matrix are sent to the AUV for subsequent calculations on the AUV side.

[0085] AUV side: Step 1: Establish the kinematic model and measurement model of AUV; Step 2: Obtain the current odometry circle equation and the current control input, and add them to the historical data queue. Remove the oldest element when the queue length exceeds the set value. If there is no odometry data update, perform an EKF time update, and extrapolate the current position estimate and error covariance matrix based on dead reckoning and the previous position estimate and error covariance matrix.

[0086] Step 3: Jointly solve the system of equations constructed by the historical ranging circle. The obtained solutions are potential true position candidate points, and these solutions are added to the candidate solution set.

[0087] Step 4: Eliminate incorrect candidate points and retain solutions clustered near the true location. This is a two-step process. First, use a discriminant condition to eliminate candidate points generated by in- or circumscribing the two ranging circles. Then, traverse each candidate point and count the number of candidate points within a circle with the radius calculated online and centered at that point. Select the region with the largest number of points as the pure inlier set.

[0088] Step 5: By linearizing the ranging circle equation, we derive the Jacobian matrix of the candidate point error with respect to the ASV position error, ranging noise, displacement error, and other errors, thus obtaining the autocovariance matrix of the candidate point error. We then calculate the cross-covariance matrix of the position estimation errors between different candidates, thereby obtaining the joint covariance matrix of all position estimates. Finally, we use linear least squares to perform a weighted fusion of this information to obtain the optimal position estimate and the corresponding estimation error covariance.

[0089] The position information of the AUV can be converted into a two-dimensional horizontal model as the dynamic model of the system as shown below.

[0090]

[0091] in represents the horizontal coordinate of the AUV in the predefined Northeastern Universe (ENU) coordinate system, are the starboard speed and forward speed of the AUV, represents the heading angle of the AUV, is the sampling time.

[0092] After the AUV receives the broadcast from the ASV, the measurement model of the received ranging measurement value is:

[0093] in represents the 2D projected range measurement value obtained by using the actual 3D range value and the depth difference between the ASV and AUV, represents zero-mean Gaussian white ranging noise, Indicates the ASV broadcast time, Represents The relevant receiving time is related to , no distinction will be made in the future, and all will be used Indicates the time of ranging measurement and the broadcast time of the corresponding ASV.

[0094] Furthermore, the state vector of the AUV is defined as Then, the state space model is obtained as

[0095] in Indicates process noise The noisy control input includes the starboard speed noise and the forward speed noise. and heading angle noise . Indicates ASV at the time of broadcast Similarly, the state space model of ASV is as follows

[0096] in Represents the GNSS position measurement matrix.

[0097] The equation of each ranging circle in step 1 is

[0098] in express The position of the AUV at the moment; 、 Respectively represent ASV at time and The position estimate of , which is the center of the ranging circle; Indicates that the AUV is arrive The displacement estimate of , that is, the distance from the historical ranging circle to the center of the current ranging circle, is obtained by dead reckoning; , Represents the distance measurement value, that is, the radius of the distance measurement circle, which is calculated by the projection of the three-dimensional distance measurement value.

[0099] In step 2, the moment The past distance measurement circle is "translated" to the current moment Form a new transformed current circle, the translation distance is exactly the displacement Then, the intersection of the past ranging circle and the current ranging circle is the current potential position solution.

[0100] Furthermore, the moment is defined by the following set The intersection of the distance measuring circle and the current distance measuring circle.

[0101]

[0102] Among them, the subscript Indicates at time The previous circle and at time The current circle, and represents the two-dimensional circle intersection function, which is of the form

[0103] is the intersection set, which can have one or two elements; is the number of the solution, which can be 1 or 2.

[0104] If we calculate the intersection of more circles, at time A set of position solutions can be obtained ,in = .

[0105] In step 3, the position solution obtained in step 2 is eliminated in two steps, specifically: In step 3.1, use the following formula to eliminate the candidate points generated by the inscription or circumscription of the two ranging circles.

[0106]

[0107] The inequality on the left represents the internal case, and the inequality on the right represents the external case. is the distance between the centers of the two measuring circles, is the larger radius value of the current and historical ranging circles, is the smaller radius value between the current and historical ranging circles, is the threshold parameter for comprehensive positioning error and noise , , ,in They represent the position estimation error variance, ranging error variance and cumulative displacement position error variance of ASV, respectively.

[0108] After executing step 3.1, a temporary internal point set will be obtained ,in and .

[0109] Step 3.2: Solve the set from the position A point is randomly selected as an interior point in , =1 or 2. Defines a new model , where the threshold Determine using the following formula:

[0110] in They represent the position estimation error variance, ranging error variance and cumulative displacement position error variance of ASV, Represents an adjustment scalar greater than 1.

[0111] Traverse each remaining candidate point and calculate the number of inliers that meet the above model. Finally, we get the pure inlier set containing the largest number of inliers. .

[0112] In step 4, the candidate point information is weighted and fused to obtain the optimal position estimate, specifically: Step 4.1, calculate the autocovariance matrix of the candidate points, specifically: By Performing first-order linearization, we get

[0113] in is the Jacobian matrix, is the estimated error of the intersection position. Therefore, The autocovariance matrix of can be expressed as

[0114] in,

[0115]

[0116] in is a temporary correlation term, which can be calculated as follows:

[0117] in, Temporary correlation coefficient matrix sent by ASV.

[0118] Respectively indicate time and The ranging noise covariance matrix, and the arrive The cumulative process noise covariance matrix of . Among them, the single-step process noise covariance matrix It can be expressed as follows:

[0119] in

[0120] in , and Respectively indicate time The measured starboard speed, forward speed and heading angle, , and represents the corresponding error variance. Then It can be calculated as =

[0121] Step 4.2, calculate the error covariance matrix between different candidate points, specifically: Position estimation The estimation error can be written as:

[0122] Assumptions .

[0123] Obviously, the estimation error and have similar terms, so there is correlation. The cross-covariance matrix can be expressed as follows.

[0124]

[0125] in

[0126] Due to the characteristics of white noise, and Not relevant.

[0127] In summary, The joint covariance matrix of all position estimates in can be expressed as

[0128] in, It is the set of candidate points retained after screening in the second step.

[0129] Step 4.3, perform weighted fusion on the information. For all position estimates in , the following linear least squares problem can be formulated.

[0130]

[0131] in,

[0132] The solution to the least squares problem is,

[0133] The estimated error covariance is and position estimation This is the output value of the algorithm.

[0134] This paper innovatively proposes to generate multiple potential position estimates by jointly solving historical ranging equations. Based on the shape of the uncertainty region, the ambiguity of the solution is systematically removed to obtain an accurate set of interior points. An optimal fusion algorithm is then developed to fuse these interior points to provide a precise position estimate that incorporates multiple ranging and position inference information. This approach achieves high state estimation accuracy and low time complexity while ensuring consistent estimates.

[0135] This paper provides an AUV collaborative localization algorithm based on the intersection of multiple ranging circles. In practical applications, Kalman filter-based methods approximate the posterior density to a Gaussian distribution, which can lead to serious estimation inconsistencies due to the nonlinearity of the measurement equation. The two-circle intersection rule suffers from low estimation accuracy due to its poor information utilization. Compared to mainstream Kalman filter methods, this method offers improved estimation accuracy and consistency, effectively addresses the estimation inconsistencies caused by measurement nonlinearity, and consumes fewer computational resources than particle filters, resulting in high computational efficiency.

[0136] Compared with existing Kalman filter-based methods, this method has the following three major advantages. (1) Initial estimation independence: The estimation error of this algorithm framework can converge quickly without the need for precise initial estimation. (2) Higher positioning accuracy and consistency: Compared with Kalman filter-based methods, this method can provide better estimation accuracy and consistency and effectively handle measurement nonlinearity problems. (3) Fast recovery capability: After temporary abnormal underwater problems such as sensor failure, ranging outliers, and model uncertainty, this method can quickly restore normal functions.

[0137] To verify the performance advantages of the multi-circle intersection method, this proposal conducts three simulation tests on the existing extended Kalman filter, cubature Kalman filter, two-circle intersection method, and multi-circle intersection method: (1) Testing the dependence of different algorithms on initial state estimation. (2) Testing the positioning accuracy of different algorithms. (3) Testing the recoverability of different algorithms.

[0138] In Test 1, the root mean square error of the two-circle intersection method, extended Kalman filter, and cubature Kalman filter when the initial error is 10m, 25m, 50m, and 100m is plotted as follows Figure 1As shown in the figure (the areas covered by the maximum and minimum RMS error curves for each algorithm in all cases are indicated by semi-transparent areas of the corresponding colors). Clearly, the green area has the smallest area, indicating that the multi-circle intersection algorithm can achieve rapid convergence despite various initial errors and outperforms the other two mainstream algorithms.

[0139] In Test 2, the estimation accuracy and consistency of the four algorithms were evaluated. The position root mean square error and normalized estimation error square of all compared algorithms were plotted as follows: Figure 2 、 Figure 3 As shown in the figure, it is clear that the estimation consistency of the two-circle intersection method and the multi-circle intersection method is significantly better than that of the Kalman filter method over time. In addition, the estimation accuracy of the multi-circle intersection method is the highest among all the compared algorithms.

[0140] In Test 3, the resiliency of all algorithms was evaluated, focusing on their ability to recover after encountering specific underwater problems (mainly considering packet loss and ranging outliers). The root mean square error and squared estimation error curves of each algorithm in this case were plotted as follows: Figure 4 As shown in the figure, during packet loss, the position estimation error of each algorithm increases. Once broadcast and ranging data are received again, the multi-circle intersection method can recalculate the estimated error in less than a minute, while the KF-like filter takes almost four times as long. A similar phenomenon also occurs after ranging anomalies. Clearly, the multi-circle intersection method is significantly more resilient than the other algorithms.

[0141] The advantages of the multi-circle intersection method have been theoretically verified through simulation experiments. In order to truly demonstrate the advantages and disadvantages of this algorithm, actual lake tests were also conducted on the lake surface: Three vessels equipped with onboard computers, DVLs, GPS, compasses, and acoustic modems sailed on the lake, acting as two ASVs and one AUV. All vehicles used acoustic modems to operate underwater for acoustic ranging and communication. GPS provided a precise position reference for all vehicles, and the two ASVs broadcast an acoustic data packet every 5 seconds. In addition, considering that large outliers in ranging measurements may occur in more challenging scenarios, two sudden outliers were simulated at 754s and 950s, respectively, forcing the ranging value to change from 173 meters to 100 meters and from 110 meters to 180 meters, thereby verifying the algorithm's recoverability.

[0142] The experiment plotted the position error curves of all algorithms as follows Figure 5 As shown in the figure, the estimation accuracy of the extended Kalman filter, cubic Kalman filter, and multi-circle intersection method is comparable, while the two-circle intersection method has a relatively low estimation accuracy. The recoverability of the multi-circle intersection method is significantly better than that of the Kalman filter and two-circle intersection methods. This fully demonstrates the practicality of the multi-circle intersection method.

[0143] The above is only a preferred embodiment of a method for collaborative positioning of AUVs based on the intersection of multiple ranging circles. The scope of protection of a method for collaborative positioning of AUVs based on the intersection of multiple ranging circles is not limited to the above embodiment. All technical solutions based on this concept fall within the scope of protection of the present invention. It should be noted that for those skilled in the art, various improvements and variations that do not depart from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A collaborative positioning method for AUVs based on the intersection of multiple ranging circles, characterized by: The method comprises the following steps: Step 1: Establish the kinematic model and measurement model of AUV; Step 2: Get the current distance circle equation and the current control input, and add them to the historical data queue. When the queue length exceeds the set value, remove the earliest element; Step 3: Solve the equations constructed by the historical ranging circle together. The obtained solutions are potential candidate points of the true position, and these solutions are added to the candidate solution set. Step 4: Eliminate incorrect candidate points and retain solutions that are clustered near the true location; Step 5: Determine the error covariance matrix of all candidate solutions and the cross-covariance matrix between candidate solutions to obtain the joint covariance matrix of all position estimates. Use linear least squares to perform optimal weighted fusion on the candidate solutions to obtain the optimal position estimate and the corresponding estimation error covariance matrix.

2. The method according to claim 1, wherein: The method further comprises: Establish the ASV's kinematic model and measurement model, input the ASV's initial position estimate, covariance matrix, time step, and time series correlation coefficient matrix; use Kalman filtering to update the ASV's position in time; if GNSS measurement is received, use Kalman filtering to fuse the ASV's position information and GNSS measurement information to obtain a new ASV position estimate, covariance matrix, and time series correlation coefficient matrix; if the current time is the broadcast time, send the ASV's position estimate, covariance matrix, and time series correlation coefficient matrix to the AUV for subsequent calculations on the AUV side.

3. The method according to claim 2, wherein: The step 1 is specifically as follows: Establish the AUV two-dimensional kinematic model as the dynamic model of the system: in, represents the horizontal coordinate of the AUV in the predefined northeast sky coordinate system, are the starboard speed and forward speed of the AUV, represents the heading angle of the AUV, is the sampling time; After the AUV receives the broadcast from the ASV, the measurement model of the received ranging measurement value is: in, represents the 2D projected range measurement value obtained by using the actual 3D range value and the depth difference between the ASV and AUV, represents zero-mean Gaussian white ranging noise, Indicates the ASV broadcast time, Represents The relevant receiving time is related to , no distinction will be made in the future, and all will be used Indicates the time of ranging measurement and the broadcast time of the corresponding ASV; The state vector of the AUV is defined as , the state space model is obtained as: in, Indicates process noise The noisy control input includes the starboard speed noise and the forward speed noise. and heading angle noise , [ ] indicates that ASV is at the time of broadcast The state space model of ASV is as follows in, Represents the GNSS position measurement matrix.

4. The method according to claim 3, wherein: The step 2 is specifically as follows: The equation of each ranging circle is: in, express The position of the AUV at the moment; 、 Respectively represent ASV at time and Position estimation; Indicates that the AUV is arrive The displacement estimate, , Indicates the ranging value; time The past distance measurement circle is "translated" to the current moment Form a new transformed current circle, the translation distance is exactly the displacement , the intersection of the past distance measurement circle and the current distance measurement circle is the current potential position solution; the moment is defined by the following set The intersection of the distance measuring circle and the current distance measuring circle: Among them, the subscript Indicates at time The previous circle and at time The current circle, and Represents a two-dimensional circle intersection function: in, is the intersection set, which can have one or two elements; is the number of the solution, which can be 1 or 2.

5. The method according to claim 4, wherein: The step 3 is specifically as follows: When calculating the intersection of more circles, at time A set of position solutions can be obtained ,in = .

6. The method according to claim 5, wherein: The step 4 is specifically as follows: First, the discrimination condition is used to eliminate the candidate points generated by the intangent or circumtangent of the two ranging circles. Each candidate point is traversed and the number of candidate points contained in the circle with the point as the center and the length calculated online as the radius is counted. The area with the largest number of points is selected as the pure inner point set.

7. The method according to claim 4, wherein: The step 5 is specifically as follows: Step 5.1: Calculate the autocovariance matrix of the candidate points, specifically: By Performing first-order linearization, we get in, is the Jacobian matrix, is the estimated error of the intersection position, The autocovariance matrix of is expressed as: in, is a timing-related term, This is the time series correlation coefficient matrix sent by ASV. Respectively indicate time and The ranging noise covariance matrix, and the arrive The cumulative process noise covariance matrix of ; Single-step process noise covariance matrix It is expressed as follows: in, , and Respectively indicate time The measured starboard speed, forward speed and heading angle, , and represents the corresponding error variance; then Calculated as = Step 5.2: Calculate the error covariance matrix between different candidate points: Position estimation The estimated error of is written as: set up ; Cross-covariance matrix: The joint covariance matrix of all position estimates in is expressed as in, It is the set of candidate points retained after screening in the second step; Step 5.3: Perform weighted fusion on the information and use For all position estimates in , the following linear least squares problem is established: The solution to the least squares problem is: The estimated error covariance is and position estimation This is the output value of the algorithm.

8. An AUV collaborative positioning system based on multi-range circle intersection, characterized by: The system comprises: A model building module, wherein the model building module builds a kinematic model and a measurement model of the AUV; A data acquisition module, which obtains the current distance circle equation and the current control input, and adds them to the historical data queue. When the queue length exceeds a set value, the earliest element is removed; A solution module, which solves the system of equations constructed by the historical ranging circles, and the obtained solutions are potential real position candidate points, and these solutions are added to the candidate solution set; A fuzzy point elimination module, which eliminates incorrect candidate points and retains solutions that are clustered near the true position; An estimation module determines the error covariance matrix of all candidate solutions and the cross-covariance matrix between the candidate solutions to obtain a joint covariance matrix of all position estimates, and uses linear least squares to perform optimal weighted fusion on the candidate solutions to obtain the optimal position estimate and the corresponding estimation error covariance matrix.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: The program is executed by a processor to implement the method according to claims 1 to 7.

10. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the method of claims 1-7 is implemented.

Images