Multi-vehicle cooperative positioning method based on distance and angle of arrival composite measurement

Abstract

The application discloses a multi-submersible cooperative positioning method based on distance and angle of arrival composite measurement, and relates to the field of underwater multi-submersible cooperative positioning. The application introduces direction measurement as a supplement on the basis of distance measurement, and based on composite measurement information, realizes distributed data fusion of each submersible under the premise of less communication energy consumption, alleviates the influence of factors such as cumulative error in inertial measurement of the submersible and underwater harsh environment on the positioning performance of the submersible, and achieves the purpose of improving the positioning precision of the submersible.

Description

Technical Field

[0001] This invention relates to the field of underwater multi-submarine cooperative positioning, and in particular to a multi-submarine cooperative positioning method based on a composite measurement of distance and angle of arrival. Background Technology

[0002] With the continuous development of underwater unmanned vehicle (UUV) technology and equipment, UUVs are increasingly playing a role in various marine applications. Typically, UUVs operate based on their known position. However, due to the harsh and complex underwater environment and the lack of GPS positioning signals, UUV positioning has always been a pressing problem. Existing UUV positioning methods usually rely on anchor nodes pre-deployed in relevant sea areas as position references for positioning, with typical systems like the long baseline system. However, given the current deployment of marine equipment in my country, the anchor nodes available for UUV positioning are sparse and have limited coverage. Furthermore, the harsh underwater environment often leads to anchor node failure. Therefore, UUVs frequently need to perform positioning in the absence of anchor nodes. However, due to the cumulative error in inertial measurement, the lack of anchor nodes causes positioning errors to increase indefinitely. How to improve the positioning accuracy of UUVs in the absence of anchor nodes is a problem that urgently needs research and solutions. With the increasing trend of using multiple UUVs simultaneously, multi-UUV collaboration can provide higher position estimation accuracy when anchor nodes are lacking. Nevertheless, due to the lack of auxiliary information from anchor nodes, different submersibles can only estimate their positions based on limited measurements between them. Therefore, fully utilizing the available measurement information among multiple submersibles and designing corresponding cooperative positioning methods is an important approach to improving submersible positioning accuracy.

[0003] In summary, this invention addresses the challenges of submersible operations lacking anchor nodes by proposing a multi-submersible collaborative positioning method based on a combination of distance and angle of arrival measurements. Unlike traditional positioning methods that primarily rely on relative distance measurements between submersibles, this invention supplements distance measurement with angle of arrival measurements and employs distributed collaborative data fusion based on the composite measurement information to improve positioning accuracy.

[0004] Domestic patent application number 202210102709.7, entitled "A Cooperative Positioning Method for UUV Clusters with Integrated Ultra-Short Baseline Systems," provides a two-stage filtering method based on distance measurement and an ultra-short baseline system, which can be used to improve the cooperative positioning of master-slave submersible clusters. Domestic patent application number 202111610043.8, entitled "A Cooperative Navigation and Positioning Method for AUV Clusters in Deep Water Operations with Dual Master AUV Switching," provides a cooperative positioning method for master-slave submersible clusters that improves the positioning accuracy of slave AUVs by periodically surfacing the master AUV for position correction. Domestic patent application number 202210213443.3, entitled "A Distributed Small Platform Underwater Pure Azimuth Positioning Method Based on Neural Networks," provides a single submersible positioning method that uses the azimuth information between a reference node and the submersible to be positioned. Although all the above methods address submersible positioning problems, they all use relative distance or azimuth measurements alone in their design and do not specifically consider the impact of accumulated errors in submersible inertial measurements on positioning.

[0005] Therefore, most existing technologies are geared towards submersible positioning in environments with anchor nodes; most technologies use different measurement information individually, with limited use of combined information. The impact of cumulative errors in submersible inertial measurements on submersible positioning is not adequately considered, and no specific solutions have been designed to address this issue.

[0006] Therefore, those skilled in the art are dedicated to developing a multi-submarine cooperative positioning method based on a composite measurement of distance and angle of arrival. Through organic collaboration among multiple submarine vehicles, distributed data fusion is achieved, mitigating the impact of accumulated errors in inertial measurement and harsh underwater environments on submarine positioning performance, thereby improving positioning accuracy. Summary of the Invention

[0007] In view of the above-mentioned deficiencies of the prior art, the technical problems to be solved by the present invention are: how to use composite distance and angle of arrival measurements to improve the positioning accuracy of submersibles; how to effectively use distributed cooperation among multiple submersibles to improve the positioning accuracy of multiple submersibles; and how to design a prediction and correction mechanism for submersible positions to mitigate the impact of accumulated errors in inertial measurement on cooperative positioning.

[0008] To achieve the above objectives, the present invention provides a multi-submarine cooperative positioning method based on a combination of distance and angle of arrival measurements, comprising the following steps:

[0009] Step 1: After sensor calibration and initialization of their own position information, each submersible begins underwater navigation;

[0010] Step 2, at sailing time t < T thr At that time, each submersible used only inertial positioning, among which, T thrThe value of depends on the error accumulation rate of the inertial measurement equipment;

[0011] Step 3, at the sailing time t=T thr At that time, communication links are established between the submersibles, and collaborative positioning between the submersibles begins;

[0012] Step 4: Particle characterization of each submersible based on its own position information at time (t-1) Each submersible predicts its own position at time (t), and receives information from its neighboring submersibles j∈S. i (S i The acoustic signals emitted by the collection of neighboring underwater vehicles are analyzed, and the angle of arrival is calculated using a receiving array. Each submersible's predicted position at time (t) Predicted position values ​​of neighboring submersible j Calculate the estimated azimuth from neighboring submersible j to itself. And establish the relationship with the heading angle. Use of each submersible The heading angle estimate at time (t) is used as the submersible's own heading angle estimate; in the prediction of each submersible's position information, the heading angle estimate from the previous time step is used. And the estimation of cumulative uncertainty Obtain the heading particles used for prediction And through particles and Particle representation that predicts its own position at time (t);

[0013] Step 5: Each submersible will broadcast its predicted position at time (t) along with the corresponding uncertainty matrix, which will be used by neighboring submersibles to generate relative position representations.

[0014] Step 6: At time (t), each submersible calculates a relative position representation for its own localization based on the position information received from neighboring submersibles broadcast.

[0015] Step 7: Each submersible uses the position information predicted in Step 4 and the relative position information based on neighboring submersibles in Step 6 to calculate its own position estimate at time (t).

[0016] Furthermore, steps 4 to 7 should be executed once at each moment of the submarine's cooperative positioning. That is, at time (t), each submarine sequentially executes steps 4 to 7 to obtain a position estimate at time (t); at time (t+1), each submarine should re-execute steps 4 to 7 to obtain a position estimate at time (t+1), and so on until the submarine's navigation ends.

[0017] Furthermore, step 4 includes the following steps:

[0018] Step 4.1: Obtain the angle of arrival measurements between the submersibles;

[0019] Submarine j, a neighbor of submarine i, emits an acoustic signal. The signal reaches submarine i and is received by the acoustic array on board submarine i. The angle of arrival is calculated by the time difference of arrival of the acoustic signal to different array elements. Assuming the acoustic array element spacing of submersible i is l, the underwater sound speed is s, and the time difference between sound waves arriving at adjacent elements is Δt, then... The angle of arrival measurements for submersible i and other neighboring submersibles were obtained using the same method.

[0020] Step 4.2: Calculate the navigation attitude angles of submersible i;

[0021] The estimated position of submersible i at time (t-1) is The predicted position of the submersible i at time (t) is obtained using inertial measurement values ​​acquired locally from the inertial measurement unit.

[0022]

[0023]

[0024]

[0025] in, and These are the measured values ​​of speed, heading angle, pitch angle, and depth of submersible i at the corresponding time, where τ represents the time length between adjacent time points; and the predicted position of submersible j at time (t). The same method can be used to obtain it;

[0026] Using the predicted positions of submersible i and submersible j, calculate the estimated bearing from submersible j to submersible i.

[0027] From the perspective of submersible j, calculate the estimated heading angle of submersible i at time (t). The heading angle estimate of submersible i at time (t) can be obtained using the same method from the perspective of other neighboring submersibles of submersible i.

[0028] The estimated heading angle of submersible i at time (t) is Among them, S i |S represents the set of neighboring submersibles of submersible i. i | represents the number of neighboring submersibles of submersible i;

[0029] Step 4.3: Predict the position information of submersible i;

[0030] Assume that the particle representation of the position information of the submersible i at time (t-1) is as follows: in K represents the total number of particles. For grains The weights are then used to represent the particle characterization of the predicted position of the submersible i at time (t). The following can be calculated:

[0031]

[0032]

[0033]

[0034] in, and These are the measurement variances of speed, pitch angle, and depth, respectively. This represents the accumulated uncertainty in the process of measuring and estimating the heading angle;

[0035] Calculate particles The corresponding weight is

[0036] Furthermore, step 5 is as follows:

[0037] Predicted position of submersible i Uncertainty matrix middle and express Uncertainty in the horizontal direction;

[0038] The particle characterization of the position information obtained by the submersible i at time (t-1) is as follows: but in,

[0039] Furthermore, step 6 includes the following steps:

[0040] Step 6.1: Perform coarse correction on the position information of neighboring submersible j;

[0041] Submarine i received the information broadcast by submarine j and Subsequently, preliminary particle characterization of the latent device j at time (t) is obtained by sampling from the Gaussian distribution.

[0042] Submarine i predicts its own position Measurement of the relative distance between submersibles i and j Particle characterization for obtaining relative position information used for coarse correction of the submersible j position. The calculation is as follows:

[0043]

[0044]

[0045]

[0046]

[0047] in, The variance of the angle of arrival measurements between submersibles, the value of which controls the coverage area of ​​the particles;

[0048] Based on particle characterization Particle characterization at position j of the submersible can be calculated. corresponding weights in, h is taken as the standard deviation of the relative distance measurement;

[0049] Particle characterization Normalization and resampling are performed to obtain the particle characterization of the position information of the submersible j after coarse correction.

[0050] Step 6.2: Calculate the relative distance between the submersibles;

[0051] Assuming that submersible i sends a ranging signal to submersible j at time (t1) and receives the returned ranging signal at time (t), the relative distance between submersible i and submersible j can be calculated as follows:

[0052] Due to the navigation depth of submersible i and submersible j and Both can be obtained through depth sensors, so the horizontal distance between submersible i and submersible j can be calculated as follows:

[0053] Submarine i calculates relative distance based on angle of arrival measurements. The compensation value for the ranging error is calculated.

[0054] When submersible i travels in the direction that shortens the distance between it and submersible j, the distance between submersible i and submersible j is... When submersible i travels in the direction that increases the distance between it and submersible j, the distance between submersible i and submersible j is:

[0055] Step 6.3: Generate relative position representation;

[0056] Particle characterization of submersible i after obtaining the position information of submersible j After determining its relative distance to the submersible j, the relative position representation required for self-positioning can be calculated. as follows:

[0057]

[0058]

[0059]

[0060]

[0061] in, For the range variance, ∈ controls the coverage area of ​​the particles;

[0062] Step 6.4: Align the relative position representations of different neighboring submersibles;

[0063] The relative position representations of all submersibles are aligned at time (t), so the relative position representation of submersible j obtained at time (t1) can be used. Align;

[0064] Therefore, the particle representation of the relative position information for the positioning of submersible i, calculated based on the position information of submersible j, is as follows: in

[0065] Furthermore, step 7 includes the following steps:

[0066] Step 7.1: Update the predicted particles from Step 4 using the relative position representation based on different neighboring submersibles. weight Among them, S i Represents the set of all neighboring submersibles of submersible i.

[0067] For the particle set after weight update Normalization and resampling are performed to obtain the particle representation of the position information of the latent device i at time (t).

[0068] Step 7.2: Calculate the estimated position of submersible i at time (t).

[0069] Furthermore, the submersible is equipped with an inertial measurement unit, a depth sensor, an underwater acoustic communication device, a ranging device, and an angle measuring array.

[0070] Furthermore, the inertial measurement unit is used to acquire the attitude and motion information of the submersible.

[0071] Furthermore, the depth sensor is used to obtain the submersible's navigation depth.

[0072] Furthermore, the underwater acoustic communication device is used for data transmission between the submersibles during collaborative positioning.

[0073] In a preferred embodiment of the present invention, the present invention addresses the current development status of limited coverage of marine positioning systems and provides a multi-submarine cooperative positioning method based on a composite measurement of distance and angle of arrival under conditions without anchor node assistance, in response to the location information requirements of multi-submarine cooperative operation.

[0074] The cooperative positioning algorithm provided by this invention is designed for a cluster of submersibles consisting of several submersibles. In addition to the control, power and other modules and systems necessary for their own operation, each submersible also needs to be equipped with an inertial measurement unit, a depth sensor, an underwater acoustic communication device, a ranging device and an angle measuring array. The inertial measurement unit is used to acquire the attitude and motion information of each submersible, the depth sensor is used to acquire the navigation depth of each submersible, the underwater acoustic communication device is used to support data transmission between submersibles during cooperative positioning, and the ranging device and angle measuring array are used to acquire the relative distance and angle of arrival measurements between submersibles.

[0075] The cooperative positioning method provided by this invention is based on the Bayesian theoretical framework. It estimates the position variables of the submersibles by solving the probability distribution of each submersible's position variables and using a Bayesian estimator. Furthermore, the calculation process is implemented using particles, ensuring the method's adaptability to the nonlinearity and non-Gaussianity of underwater positioning problems. Since the submersible's speed can be obtained with high measurement accuracy, and the cumulative error in the submersible's pitch angle can be handled in the same way as the cumulative error in its heading angle, the technical solution description of this invention focuses on the handling method for the cumulative error in the heading angle, while the handling method for the cumulative error in the pitch angle is the same. Since the method for measuring the angle of arrival in the pitch direction between submersibles is the same as in the horizontal direction, the technical solution description of this invention focuses on the use of the horizontal angle of arrival measurement, while the handling method for the pitch direction is the same.

[0076] Compared with the prior art, the present invention has the following obvious substantive features and significant advantages:

[0077] This invention addresses submersible operation scenarios lacking anchor nodes and proposes a multi-submersible collaborative positioning method based on combined distance and azimuth measurements. Unlike traditional positioning methods that primarily rely on single measurements between submersibles, this invention introduces azimuth measurements as a supplement to distance measurements. Based on the composite measurement information, through organic collaboration among multiple submersibles, it achieves distributed data fusion of each submersible with minimal communication energy consumption. This mitigates the impact of accumulated errors in submersible inertial measurements and harsh underwater environments on submersible positioning performance, thereby improving submersible positioning accuracy.

[0078] The following will further explain the concept, specific structure, and technical effects of the present invention in conjunction with the accompanying drawings, so as to fully understand the purpose, features, and effects of the present invention. Attached Figure Description

[0079] Figure 1 This is a flowchart illustrating the execution of a preferred embodiment of the present invention. Detailed Implementation

[0080] The following description, with reference to the accompanying drawings, illustrates several preferred embodiments of the present invention to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0081] In the accompanying drawings, components with the same structure are indicated by the same numerical designation, and components with similar structures or functions are indicated by similar numerical designations. The dimensions and thicknesses of each component shown in the drawings are arbitrary, and the present invention does not limit the dimensions and thicknesses of each component. To make the illustrations clearer, the thickness of some components has been appropriately exaggerated in the drawings.

[0082] This invention discloses a multi-submarine cooperative positioning method based on combined distance and angle of arrival measurements, relating to the field of underwater multi-submarine cooperative positioning. Addressing the current limitations in the coverage of marine positioning systems, this invention, in the absence of anchor nodes, utilizes the collaborative efforts of multiple submarines and designs a distributed data fusion method based on the combined use of distance and angle of arrival measurement information between submarines. This effectively mitigates the impact of accumulated errors in inertial measurements during submarine navigation and the lack of anchor nodes on submarine positioning accuracy, thereby significantly improving the overall positioning accuracy of the multi-submarine system.

[0083] Figure 1 This invention provides a specific implementation process for a multi-submarine cooperative positioning method based on a combined distance and angle of arrival measurement. Since this invention addresses the positioning problem during multi-submarine cooperative mission execution, the number of submarine vehicles should be at least two in actual use.

[0084] To use the cooperative positioning method provided by this invention, each submersible, in addition to being equipped with the necessary control, power, and other modules and systems for its own operation, also needs to be equipped with an inertial measurement unit, a depth sensor, an underwater acoustic communication device, a ranging device, and an angle measuring array. Among them, the inertial measurement unit is used to acquire the attitude and motion information of each submersible, the depth sensor is used to acquire the navigation depth of each submersible, the underwater acoustic communication device is used to support data transmission between submersibles during cooperative positioning, and the ranging device and angle measuring array are used to acquire the relative distance and angle of arrival measurements between submersibles.

[0085] This invention relates to a multi-submarine cooperative positioning method based on combined distance and angle of arrival measurements, comprising the following steps:

[0086] Step 1: After sensor calibration and initialization of their own position information, each submersible begins underwater navigation.

[0087] Step 2: When the sailing time t < T thr At that time, each submersible used only inertial positioning, among which, T thr The value of depends on the error accumulation rate of the inertial measurement equipment.

[0088] Step 3: At the sailing time t=T thr At that time, communication links were established between the various submersibles, and collaborative positioning between the submersibles began.

[0089] Step 4: Particle characterization of each submersible based on its own position information at time (t-1) Predict its own position information at time (t): Each submersible receives information from its neighboring submersible j∈S i (S i The acoustic signals emitted by the collection of neighboring underwater vehicles are analyzed, and the angle of arrival is calculated using a receiving array. Each submersible's predicted position at time (t) Predicted location values ​​of neighbor j Calculate the estimated azimuth from neighboring submersible j to itself. And establish the relationship with the heading angle. Use of each submersible The heading angle estimate at time (t) is used as the submersible's own heading angle estimate; in the prediction of each submersible's position information, the heading angle estimate from the previous time step is used. And the estimation of cumulative uncertainty Obtain the heading particles used for prediction And through particles and The particle characterization that predicts its own position information at time (t) avoids the direct use of inertial measurement values, thereby mitigating the impact of accumulated errors in inertial measurement on prediction.

[0090] Step 5: Each submersible will broadcast its predicted position at time (t) along with the corresponding uncertainty matrix, which will be used by neighboring submersibles to generate relative position representations.

[0091] Step 6: At time (t), each submersible calculates a relative position representation for its own localization based on the position information received from neighboring submersibles broadcast.

[0092] Step 7: Each submersible uses the position information predicted in Step 4 and the relative position information based on neighboring submersibles in Step 6 to calculate its own position estimate at time (t).

[0093] Steps 4 through 7 should be executed once at each moment of the submarine's cooperative positioning. That is, at time (t), each submarine sequentially executes steps 4 through 7 to obtain a position estimate at time (t); at time (t+1), each submarine should re-execute steps 4 through 7 to obtain a position estimate at time (t+1), and so on until the submarine's voyage ends.

[0094] The specific process of step 4 is as follows: Taking the position prediction of submersible i as an example, step 4 includes the following three parts:

[0095] Step 41: Obtain the angle measurement values ​​between the submersibles.

[0096] Consider a neighboring submersible j of submersible i emitting an acoustic signal. This signal reaches submersible i and is received by the acoustic array onboard submersible i. The angle of arrival is calculated by using the time difference of arrival of the acoustic signal at different array elements. Assuming the acoustic array element spacing of submersible i is l, the underwater sound speed is s, and the time difference between sound waves arriving at adjacent elements is Δt, then... The angle of arrival measurements of submersible i and other neighboring submersibles can also be obtained using the same method.

[0097] Step 42: Calculate the navigation attitude angle of submersible i.

[0098] Assume the estimated position of submersible i at time (t-1) is The predicted position of the submersible i at time (t) is obtained using inertial measurement values ​​acquired locally from the inertial measurement unit.

[0099]

[0100]

[0101]

[0102] in, and These are the measured values ​​of speed, heading angle, pitch angle, and depth of submersible i at the corresponding time points, respectively, where τ represents the time length between adjacent time points. The predicted position of submersible j at time (t) is also shown. The same method can also be used to obtain it.

[0103] Using the predicted positions of submersible i and submersible j, calculate the estimated bearing from submersible j to submersible i.

[0104] From the perspective of submersible j, calculate the estimated heading angle of submersible i at time (t). The heading angle estimate of submersible i at time (t) can also be obtained using the same method from the perspective of other neighboring submersibles of submersible i.

[0105] The estimated heading angle of submersible i at time (t) is Among them, S i |S represents the set of neighboring submersibles of submersible i. i | represents the number of neighboring submarines of submarine i.

[0106] Step 43: Predict the position information of submersible i.

[0107] Assume that the particle representation of the position information of the submersible i at time (t-1) is as follows: in K represents the total number of particles. For particles The weights are then used to represent the particle characterization of the predicted position of the submersible i at time (t). The following can be calculated:

[0108]

[0109]

[0110]

[0111] in, and These are the measurement variances of speed, pitch angle, and depth, respectively. This represents the accumulated uncertainty in the process of measuring and estimating the heading angle.

[0112] Calculate particles The corresponding weight is

[0113] Step 5 is as follows: Taking submersible i as an example, the predicted position value... The uncertainty matrix is ​​calculated in step 42. middle and express Uncertainty in the horizontal direction.

[0114] Suppose that the particle representation of the position information obtained by the submersible i at time (t-1) is as follows: but in,

[0115] The specific process of step 6 is as follows: Taking the self-localization of submersible i as an example, based on the position information received from submersible j, step 6 includes the following four parts:

[0116] Step 61: Perform coarse correction on the position information of neighboring submersible j.

[0117] Submarine i received the information broadcast by submarine j and Subsequently, preliminary particle characterization of the latent device j at time (t) is obtained by sampling from the Gaussian distribution.

[0118] Submarine i predicts its own position Measurement of the relative distance between submersibles i and j Particle characterization for obtaining relative position information used for coarse correction of the submersible j position. The calculation is as follows:

[0119]

[0120]

[0121]

[0122]

[0123] in, The variance of the angle of arrival measurements between submersibles is represented by ρ, and the value of ρ controls the coverage area of ​​the particles.

[0124] Based on particle characterization Particle characterization at position j of the submersible can be calculated. corresponding weights in, h is taken as the standard deviation of the relative distance measurement.

[0125] Particle characterization Normalization and resampling are performed to obtain the particle characterization of the position information of the submersible j after coarse correction.

[0126] Step 62: Calculate the relative distance between the submersibles.

[0127] To reduce the synchronization requirements between submersibles, this invention uses a two-way ranging method. Assuming submersible i sends a ranging signal to submersible j at time (t1) and receives the returned ranging signal at time (t), the relative distance between submersibles i and j is calculated as follows:

[0128] Due to the navigation depth of submersibles i and j and Both can be obtained through depth sensors, so the horizontal distance between submersibles i and j is calculated as follows:

[0129] Throughout the two-way ranging process, the movement of submersible i itself will cause ranging errors. Therefore, when calculating the relative distance of submersible i, the angle of arrival measurement value will be used as the basis. The compensation value for the ranging error is calculated.

[0130] When submersible i travels in the direction that shortens the distance between it and submersible j, the distance between submersible i and j is... When submersible i travels in the direction that increases the distance between it and submersible j, the distance between submersible i and j is...

[0131] Step 63: Generate a relative position representation.

[0132] Particle characterization of submersible i after obtaining the position information of submersible j After determining its relative distance to the submersible j, the relative position representation required for self-positioning can be calculated. as follows:

[0133]

[0134]

[0135]

[0136]

[0137] in, For the distance measurement variance, ∈ controls the coverage area of ​​the particles.

[0138] Step 64: Align the relative position representations of different neighboring submersibles.

[0139] Due to the long latency of underwater acoustic communication, the information broadcast by different neighboring submersibles arrives at submersible i at different times, resulting in different times corresponding to the calculated relative position representations. Assuming that the relative position representations of all submersibles are aligned at time (t), then the relative position representation of submersible j obtained at time (t1) can be used... Align them.

[0140] Therefore, the particle representation of the relative position information for the positioning of submersible i, calculated based on the position information of submersible j, is as follows: in

[0141] The specific process of step 7 is as follows:

[0142] Taking submersible i as an example, step 7 includes the following two parts:

[0143] Step 71: Update the particles predicted in Step 4 using the relative position representation based on different neighboring submersibles. weight Among them, S i Represents the set of all neighboring submersibles of submersible i.

[0144] For the particle set after weight update Normalization and resampling are performed to obtain the particle representation of the position information of the latent device i at time (t).

[0145] Step 72: Calculate the estimated position of submersible i at time (t).

[0146] Since the speed of a submersible can be obtained with high measurement accuracy, and the cumulative error in the pitch angle can be processed in the same way as the cumulative error in the heading angle, the description of the claims of this invention focuses on the processing method of the cumulative error in the heading angle of the submersible. The cumulative error in the pitch angle of the submersible can be processed in the same way and should also be within the scope of protection of the claims. Since the method of measuring the angle of arrival in the pitch direction between submersibles is the same as that in the horizontal direction, the description of the claims of this invention focuses on the use of the horizontal angle of arrival measurement. The pitch direction can be processed in the same way.

[0147] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A method for multi-vehicle cooperative positioning based on range and angle of arrival composite measurement, characterized in that, The method comprises the following steps: Step 1, after the sensor calibration and the self-position information initialization of each submersible, the submersible starts underwater navigation; Step 2, time of transit During the time of transit, each vehicle uses inertial positioning only, wherein, The value of the error accumulation rate depends on the error accumulation rate of the inertial measurement device; Step 3, at the time of sailing The communication link between the two submarines is established and the cooperative positioning between the two submarines is started. Step 4: Each submersible, based on its own situation at any given time... Particle representation of position information For oneself at all times Position information is used for prediction, and each submersible receives information from neighboring submersibles. The emitted sound signal Represents the set of neighboring submersibles, and uses a receiver array to calculate the angle of arrival measurement. ; Each submersible is based on its own time Location prediction value and neighbor's underwater vehicle Location prediction value Calculate from neighboring submersibles To its own estimated position And establish the relationship with the heading angle. Use of each submersible As itself at all times The estimated heading angle is used; in the prediction of the position information of each submersible, the estimated heading angle at the previous moment is used. and estimates of cumulative uncertainty Obtain the heading particles used for prediction. and through particles and Predict yourself at any moment Particle representation of position information; Step 5, each vehicle will broadcast the position prediction value at time and the corresponding uncertainty matrix for neighbor vehicles to generate relative position representation; Step 6, each vehicle computes at time Based on the received neighbor vehicle broadcasted position information, compute a relative position representation for own positioning; Step 7. Each vehicle uses the predicted position information from Step 4 and the relative position information from Step 6 based on neighboring vehicles to calculate its own position estimate at time t. Step 7. Each vehicle uses the predicted position information from Step 4 and the relative position information from Step 6 based on neighboring vehicles to calculate its own position estimate at time t.

2. The method of claim 1, wherein, The steps 4 to 7 should be executed once at each time instant when the vehicles are co-located, i.e. at time instant , each vehicle sequentially executes the steps 4 to 7 to obtain the position estimate at time instant ; at time instant , each vehicle should re- execute the steps 4 to 7 to obtain the position estimate at time instant , and so on until the end of the vehicles' journey.

3. The method of claim 1, wherein, The step 4 comprises the following steps: Step 4.1, obtaining the inter-submersible angle of arrival measurement value; Submarine Neighboring Submarine The acoustic signal was emitted and reached the submersible. Submarine The onboard acoustic array receives the signal and calculates the angle of arrival measurement value by measuring the time difference of arrival of the acoustic signal to different array elements. Hypothetical Submarine The spacing between the elements of the acoustic array is The speed of sound underwater is The time difference between the arrival of the sound wave and the adjacent array element is ,but Submarine The angle of arrival measurements were obtained using the same method as those of other neighboring submersibles. Step 4.2, calculating the vehicle's attitude angle of the vehicle submarine At time The position estimate at time The position prediction at time is obtained using inertial measurements locally acquired from an inertial measurement unit :​ , , , wherein, , , and are the position of the submersible at the corresponding time, the measured values of the velocity, the heading angle, the pitch angle and the depth, represents the length of time between adjacent times; the position of the submersible at time is predicted using the same method; and the position of the submersible Using the position prediction value of the submersible and the submersible , an azimuth estimation value from the submersible to the submersible is calculated ; From the submersible Angle, computational submarine At any moment estimated heading angle From the submersible Other neighboring submersibles' angles, submersibles At any moment The estimated heading angle can be obtained using the same method; submarine At time The heading angle estimate of the submarine is wherein, represents the set of neighbor submarines of the submarine represents the number of neighbor submarines of the submarine represents the set of neighbor submarines of the submarine represents the number of neighbor submarines of the submarine Step 4.3, predicting the position of the submersible the position information; Assume at time submarine The particle representation of the position information is where , represents the total number of particles, is the weight of a particle , then at time submarine The particle representation of the predicted position can be calculated as follows: , , , , ， , , where, , and are the measurement variances of the velocity, the pitch angle, the depth, respectively, denotes the accumulated uncertainty in the heading angle measurement and estimation process; Computing particles Corresponding weights are .

4. The method of claim 1, wherein, The step 5 is: Submarine Position prediction value Uncertainty matrix In And Indicates Uncertainty in the horizontal direction; At time Submarine The particle whose position information is located is characterized as Then , wherein .

5. The range and angle of arrival composite multi-vehicle cooperative positioning method of claim 1, wherein, The step 6 comprises the following steps: Step 6.

1. Coarse correction of position information of the neighbor ROV by the host ROV submarine Upon receiving the submarine broadcasted information and Afterwards, the submarine at time position of the preliminary particle representation ; submersible prediction based on its own position and the submersible relative distance measurement between and the submersible , resulting in a particle representation of relative position information for the submersible coarse correction of the submersible's position is calculated as follows: , , , , ， , , wherein, is the variance of the inter-vehicle angle of arrival measurement, the value of controls the coverage area of the particles; Particle characterization based Autonomous underwater vehicle Particle characterization of a position Respective weights Wherein, , Taking a standard deviation of relative distance measurements. Particle characterization Performing normalization and resampling, resulting in a coarsely corrected particle characterization of the vehicle's position information ; Step 6.2, calculating the inter-submersible relative distance measurement; Assume the submarine At time Send a ranging signal to the submarine At time Obtain the returned ranging signal, then calculate the relative distance of the submarine to the submarine is ; Due to the submersible With Submarine sailing depth and All of these can be obtained through depth sensors, thus enabling computational underwater vehicles. With Submarine The horizontal distance between them is ; Submarine Computing relative distance based on angle of arrival measurements Computing a compensation value for ranging error ; When the submersible is navigating in a direction that shortens the distance to the submersible , the distance between the submersible and the submersible is ; when the submersible is navigating in a direction that increases the distance to the submersible , the distance between the submersible and the submersible is ; Step 6.3, generating the relative position representation; submarine After obtaining the position information of the submarine , the particle representation and its own relative distance from the submarine , the relative position representation required for self-positioning can be calculated as follows: , , , , ， , wherein, to measure the variance of the range, control the coverage area of the particles; Step 6.4, aligning the relative position representations based on different neighbor submersibles; All the relative position representations of the submersible are at time aligned, then the time is obtained the relative position representation of the submersible is aligned; Thus, based on the position information of the submersible a relative position information for the positioning of the submersible is calculated, which is characterized by wherein , .

6. The method of claim 1, wherein, The step 7 comprises the following steps: Step 7.

1. Update the predicted particles from step 4 using a representation based on the relative position of different neighbor ROVs with weights where represents the ROV set of all neighbor ROVs of the ROV ; particle set after weight update normalization and resampling to obtain the vehicle at time particle representation of location information ; Step 7.2, calculating the submersible At time Position estimate .

7. The range and angle of arrival composite multi-vehicle cooperative positioning method of claim 1, wherein, The submersible is equipped with an inertial measurement unit, a depth sensor, an underwater acoustic communication machine, a ranging device and an angle measurement array.

8. The method of claim 7, wherein, The inertial measurement unit is used for obtaining the submersible attitude and motion information.

9. The method of claim 7, wherein, The depth sensor is used for obtaining the submersible navigation depth.

10. The method of claim 7, wherein, The underwater acoustic communication machine is used for the data transmission between the submersibles during the cooperative positioning.

Images