A uuv cooperative positioning method considering master node positioning error
By establishing a measurement model and a state-extended dimension model, and considering the master node positioning error, the problem of inaccurate sub-node positioning in the UUV collaborative positioning system was solved, and higher precision sub-node positioning was achieved.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- HARBIN INST OF TECH
- Filing Date
- 2024-04-25
- Publication Date
- 2026-07-14
AI Technical Summary
In UUV cooperative positioning systems, the positional error of the master node is ignored, resulting in unsatisfactory positioning performance of the child nodes.
Establish a measurement model, a state expansion model, and a collaborative measurement model. Consider the positioning error of the master node, and use the relationship between the expanded state variables and the measurement values for filtering and updating to improve the positioning accuracy of the child nodes.
This effectively improves the positioning accuracy of child nodes, providing high-precision location information for path planning and task allocation in the subsequent collaborative system.
Smart Images
Figure CN118463991B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to underwater positioning systems, and more particularly to the field of cooperative positioning technology for underwater unmanned vehicles. Background Technology
[0002] Unmanned Underwater Vehicles (UUVs) are capable of performing underwater combat, resource exploration, and underwater reconnaissance missions. Their high autonomy, maneuverability, and compact size have garnered significant attention from major military powers worldwide. As UUV technology matures, the difficulty and complexity of their tasks have increased considerably, including seabed topography mapping, search and rescue of distressed vessels, and underwater target detection and identification. The independent operation of a single UUV is no longer sufficient to meet these requirements. In the 1970s, German physicist Hermann Haken first proposed the concept of collaboration, pointing out that individuals can effectively integrate resources through cooperation, achieving a "1+1>2" effect. Subsequently, Japanese scholars proposed the concept of cooperative localization in their research on multi-robot localization, which has become one of the most promising research directions in the field of navigation, and has been widely researched and applied in areas such as satellite positioning, wireless sensor network localization, and robot cooperative localization.
[0003] With the development of underwater acoustic communication technology, underwater unmanned vehicles can now perform ranging and communication through underwater acoustic technology. Therefore, UUV swarm collaboration technology based on underwater acoustic communication networks has received increasing attention. UUV swarm collaboration technology is characterized by wide spatial distribution and multiple functions, which can extend the working range of individual UUVs, improve work efficiency, and accomplish complex tasks that are impossible or difficult for individual UUVs to complete.
[0004] In a master-slave UUV cooperative system, the principle of cooperative localization is that child nodes use the position information of the master node and the ranging information between the master and child nodes to correct their own localization. Traditional cooperative localization models assume the master node's position information is accurate, ignoring its own positional errors. However, although the master node's position information can be obtained by combining a high-precision inertial navigation system (INS) with a Doppler velocity log (DVL), a certain positional error still exists. This error is transmitted to the child nodes through the ranging model, resulting in unsatisfactory cooperative localization performance for the child nodes. Summary of the Invention
[0005] To address the above problems, this invention provides a UUV cooperative positioning method that considers the positioning error of the master node. The method is implemented based on a master-slave UUV cooperative positioning system, which includes a master node and slave nodes. The method includes the following steps:
[0006] Step 1: Establish a measurement model for collecting measurement values;
[0007] The measurement value is the relative distance between the master node and the child node;
[0008] Step 2: Establish a state dimension expansion model in the geocentric-geofixed coordinate system. The state dimension expansion model is used to expand the state variables of the child nodes to obtain the expanded state variables.
[0009] The state dimension expansion model is also used to expand the dimension of the state covariance and the noise covariance;
[0010] Step 3: Establish a collaborative measurement model in the geocentric-geofixed coordinate system. The collaborative measurement model is used to obtain the relationship between the expanded state variables and the measurement values with the help of the expanded state covariance and the expanded noise covariance.
[0011] Step 4: Based on the relationship between the expanded state quantity and the measurement value, filter and update the state information of the child node to obtain the positioning result of the child node.
[0012] Furthermore, in step 1, the measurement model is:
[0013]
[0014] in, This represents the coordinates of child node i. Let r represent the coordinates of the master node n. i,n Represents the measured value, ε r This indicates the distance measurement error.
[0015] Furthermore, in step 2, the expanded state variables are represented as follows:
[0016] X k =[φδvδp b g b a δv m δp m ] T
[0017] Where φ represents the three-dimensional attitude error, δv represents the three-dimensional velocity error, δp represents the three-dimensional position error, and b g This indicates that the gyroscope has zero bias across its three axes, b a This indicates that the accelerometer has zero bias across its three axes, δv mThe three-dimensional velocity error of the master node, δp m This represents the three-dimensional position error of the master node.
[0018] Furthermore, in step 2, the expanded state covariance is expressed as:
[0019]
[0020] in, This represents the covariance matrix of the current node. This represents the covariance matrix of the velocity error and position error of the master node.
[0021] Furthermore, in step 2, the expanded noise covariance is expressed as:
[0022]
[0023] in, This represents the noise matrix of the current node, 0 6×6 This represents a zero matrix with 6 rows and 6 columns.
[0024] In step 3, the collaborative measurement model is:
[0025] Z = HX + w ρ
[0026] Where Z represents the pseudorange value, H represents the measurement transition matrix, X represents the state vector, and w ρ This indicates measurement noise.
[0027] The measurement transfer matrix is represented as follows:
[0028] H = [0 1×6 B s A s 0 1×9 B m A m ] 1×21
[0029] Among them, A s Let A represent the transformation matrix of the current node. m B represents the transformation matrix of the master node; s B represents the Jacobian matrix representing the ranging information of the current node with respect to the current node's position. m This represents the Jacobian matrix representing the distance measurement information of the current node with respect to the position of the master node.
[0030] The present invention also provides a computer device, the device including a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor performs any of the methods described above.
[0031] The present invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the method described in any one of the preceding claims.
[0032] The beneficial effects of this invention are as follows:
[0033] The collaborative positioning scheme proposed in this invention is more in line with the actual situation, namely, the master node inevitably has positioning errors. Based on this, this invention effectively improves the positioning accuracy of the child nodes by establishing a new state expansion model and a collaborative measurement model, providing high-precision location information for subsequent collaborative system path planning, task allocation and other aspects. Attached Figure Description
[0034] Figure 1 This is a motion trajectory diagram of the main node and child nodes in the simulation experiment described in Implementation Method 10;
[0035] Figure 2 The ranging information in the simulation experiment described in Implementation Method Ten;
[0036] Figure 3 The positioning error of the master node in the simulation experiment described in Implementation Method 10;
[0037] Figure 4 The positioning error of the sub-nodes using different algorithms in the simulation experiment described in Implementation Method 10. Detailed Implementation
[0038] Implementation Method 1: A UUV cooperative positioning method considering master node positioning error. The method is implemented based on a master-slave UUV cooperative positioning system, which includes a master node and slave nodes. The method includes the following steps:
[0039] Step 1: Establish a measurement model for collecting measurement values;
[0040] The measurement value is the relative distance between the master node and the child node;
[0041] Step 2: Establish a state dimension expansion model in the geocentric-geofixed coordinate system. The state dimension expansion model is used to expand the state variables of the child nodes to obtain the expanded state variables.
[0042] The state dimension expansion model is also used to expand the dimension of the state covariance and the noise covariance;
[0043] Step 3: Establish a collaborative measurement model in the geocentric-geofixed coordinate system. The collaborative measurement model is used to obtain the relationship between the expanded state variables and the measurement values with the help of the expanded state covariance and the expanded noise covariance.
[0044] Step 4: Based on the relationship between the expanded state quantity and the measurement value, filter and update the state information of the child node to obtain the positioning result of the child node.
[0045] This method expands the dimensionality of the state variables of child nodes by establishing a measurement model and a state expansion model, and establishes a collaborative measurement model to achieve filtering and updating of the state information of child nodes, so as to take into account the influence of the positioning error of the master node, and finally obtain a more accurate positioning result.
[0046] Implementation Method Two: This implementation method further defines the UUV cooperative positioning method considering master node positioning error described in Implementation Method One. In this implementation method, the measurement model in step 1 is:
[0047]
[0048] in, This represents the coordinates of child node i. Let r represent the coordinates of the master node n. i,n Represents the measured value, ε r This indicates the distance measurement error.
[0049] Furthermore, in step 2, the expanded state variables are represented as follows:
[0050] X k =[φδvδp b g b a δv m δp m ] T
[0051] Where φ represents the three-dimensional attitude error, δv represents the three-dimensional velocity error, δp represents the three-dimensional position error, and b g This indicates that the gyroscope has zero bias across its three axes, b a This indicates that the accelerometer has zero bias across its three axes, δv m The three-dimensional velocity error of the master node, δp m This represents the three-dimensional position error of the master node.
[0052] As can be seen from the formula, the positional accuracy of the master node also affects the reliability of the distance observation. Assume that the master node's positioning result has a perturbation of δx on the x-axis, and the true coordinates are x... n The relationship between them is as follows:
[0053]
[0054] Let r i,n Taking the partial derivative with respect to δx, we get
[0055]
[0056] And thus obtain
[0057]
[0058] The above equation shows that when there is a disturbance in the x-axis coordinate, an equivalent disturbance is introduced into the distance observation equation. Similarly, disturbances in the y and z-axis coordinates will also introduce disturbances into the distance observation equation. This indicates that even if the distance observations are highly reliable, errors in the master node's position information can indirectly introduce noise, leading to poor coordination.
[0059] Implementation Method 3: This implementation method further defines the UUV cooperative positioning method considering master node positioning error described in Implementation Method 1. In this implementation method, when expanding the dimensionality of the child node state variables in step 2, the state expansion model is:
[0060] X k =[φδvδp b g b a δv m δp m ] T
[0061] Where φ represents the three-dimensional attitude error, δv represents the three-dimensional velocity error, δp represents the three-dimensional position error, and b g This indicates that the gyroscope has zero bias across its three axes, b a This indicates that the accelerometer has zero bias across its three axes, δv m The three-dimensional velocity error of the master node, δp m This represents the three-dimensional position error of the master node.
[0062] In step 2, the geocentric coordinate system o is first defined. e x e y e z e The origin is located at the Earth's center, o e x e and o e y e The axis lies in the plane of the Earth's equator, where o e x e Pointing towards the Prime Meridian, o e z e The axis is the Earth's rotation axis, pointing towards the North Pole, and the coordinate system is fixed to the Earth;
[0063] Before dimension expansion, the state variables of child nodes are defined as follows:
[0064] X k =[φδvδp b g b a ] T
[0065] Where φ represents the three-dimensional attitude error, δv represents the three-dimensional velocity error, δp represents the three-dimensional position error, and b g This indicates that the gyroscope has zero bias across its three axes, b a This indicates that the accelerometer has zero bias across its three axes.
[0066] The conversion relationships between geographic coordinates (λ, L, h) and geocentric rectangular coordinates (x, y, z) for the same location are as follows:
[0067]
[0068] Among them, R N denoted by , and e represents the Earth's eccentricity.
[0069] Solving for the total differential on both sides of the above equation, since the range of nodal movement is small in a short time, R can be considered as... N Since it is a constant value, we get:
[0070]
[0071] Implementation Method Four: This implementation method further defines the UUV cooperative positioning method considering master node positioning error described in Implementation Method One. In this implementation method, the expanded state covariance in step 2 is expressed as:
[0072]
[0073] in, This represents the covariance matrix of the current node. This represents the covariance matrix of the velocity error and position error of the master node.
[0074] The expanded state covariance is used to describe the uncertainty of state estimation in the collaborative measurement model.
[0075] Implementation Method 5: This implementation method further defines the UUV cooperative positioning method considering master node positioning error described in Implementation Method 1. In this implementation method, the expanded noise covariance in step 2 is expressed as:
[0076]
[0077] in, This represents the noise matrix of the current node, 0 6×6 This represents a zero matrix with 6 rows and 6 columns.
[0078] The expanded noise covariance is used to describe the characteristics of measurement noise in the cooperative measurement model.
[0079] Implementation Method Six: This implementation method further defines the UUV cooperative positioning method considering master node positioning error described in Implementation Method One. In this implementation method, the cooperative measurement model in step 3 is:
[0080] Z = HX + w ρ
[0081] Where Z represents the pseudorange value, H represents the measurement transition matrix, X represents the state vector, and w ρ This indicates measurement noise.
[0082] In step 3, the relationship between the expanded state variables and the measured values is obtained through the pseudorange measurement equation, which is:
[0083] δρ=ρ SINS -ρ meas +w ρ
[0084] Where, ρ SINS ρ represents the pseudorange calculated based on the positions of the child node inertial navigation systems and the position information of the master node. meas This represents the ranging information output by the hydrophone, w ρ This indicates ranging noise.
[0085] In the Earth-centered Earth-fixed system, the position coordinates of the master node obtained through INS / DVL combination are: The actual position coordinates are (x m ,y m ,z m The calculated position coordinates of the child node inertial navigation system are: The actual position coordinates of the child node (x) s ,y s ,z s ),but:
[0086]
[0087]
[0088] Therefore, the nonlinear measurement equation for the pseudorange is obtained:
[0089]
[0090] Define it as follows:
[0091]
[0092]
[0093] Among them, [δx s δy s δzs ] T For the position error of the child node inertial navigation system solution output, [δx m δy m δz m The position error is calculated and output by the INS / DVL combination of the master node.
[0094] For ρ meas exist Performing a Taylor expansion at this point, ignoring second-order and higher-order terms, we get:
[0095]
[0096] Define it as follows:
[0097]
[0098] The pseudorange measurement equation can then be expressed as:
[0099]
[0100] In summary, the pseudorange measurement equation is expressed as:
[0101] Z = HX + w ρ .
[0102] Implementation Method Seven: This implementation method further defines the UUV cooperative positioning method considering master node positioning error described in Implementation Method Six. In this implementation method, the measurement transfer matrix is represented as:
[0103] H = [0 1×6 B s A s 0 1×9 B m A m ] 1×21
[0104] Among them, A s Let A represent the transformation matrix of the current node. m B represents the transformation matrix of the master node; s B represents the Jacobian matrix representing the ranging information of the current node with respect to the current node's position. m This represents the Jacobian matrix representing the distance measurement information of the current node with respect to the position of the master node.
[0105] Specifically, A s A m These represent the positional errors [δL, δλ, δh] of the current node and the master node in the geographic coordinate system, respectively. T Position error converted to geocentric rectangular coordinates [δxδyδz] T The transformation matrix; B sThe ρ represents the ranging information received by the hydrophone from the current node. meas Regarding the current node's inertial navigation calculation position Jacobian matrix, B m The ρ represents the ranging information received by the hydrophone from the current node. meas Regarding the position of the master node obtained by combining INS / DVL. The Jacobian matrix.
[0106] Implementation Method 8: This implementation method provides a computer device, including a memory and a processor. The memory stores a computer program. When the processor runs the computer program stored in the memory, the processor executes the method described in any one of Implementation Methods 1 to 7.
[0107] Implementation Method Nine: This implementation method provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the method described in any one of Implementation Methods One to Seven.
[0108] Implementation Method 10: The above implementation methods are illustrated below with examples, such as... Figures 1 to 4 As shown, to verify the effectiveness of the present invention, a UUV cooperative positioning method considering the master node positioning error was simulated using software.
[0109] The simulation conditions are as follows: there are two master node UUVs and one child node UUV in the simulation. The starting points of the two master node UUVs are (0m, 0m) and (100m, 200m) respectively, and the starting point of the child node UUV is (300m, 0m). The actual speeds, headings, and trajectories of the three UUVs are attached. Figure 1 As shown, Node1 and Node3 are the master nodes, and Node2 is the child node. In the simulation, the gyroscope drift of the low-precision INS of the UUV equipment in the child node is set to 3° / h, the accelerometer bias to 50μg, and the underwater acoustic ranging noise is set to zero-mean white noise with a Gaussian distribution and a standard deviation of 2m. The simulation duration is set to 1800s. The ranging error in the simulation is shown in the attached figure. Figure 2 As shown.
[0110] The master node uses an INS / DVL combination for positioning, and the positioning error is shown in the attached figure. Figure 3 As shown in the figure, it can be seen that the master node inevitably has a positioning error.
[0111] Appendix Figure 4 The positioning errors estimated for child nodes using different cooperative positioning methods are given. Algorithm 1 represents a cooperative positioning method that does not consider the positioning error of the master node, while Algorithm 2 represents a cooperative positioning method proposed in this invention that considers the positioning error of the master node.
[0112] From the appendix Figure 4 As can be seen, due to the inherent positioning error of the master node, Algorithm 1, which fails to consider this factor, results in a larger positioning error for the child nodes. Algorithm 2, by considering and modeling the positioning error of the master node, achieves better positioning performance. Calculations of the root mean square error (RMSE) show that Algorithm 1 has an RMSE of 4.1m and a maximum positioning error of 9.6m; Algorithm 2 has an RMSE of 3.5m and a maximum positioning error of 8.4m. In conclusion, the algorithm proposed in this invention exhibits superior positioning performance.
Claims
1. A UUV cooperative positioning method considering master node positioning error, characterized in that, The method is based on a master-slave UUV cooperative positioning system, which includes a master node and slave nodes. The method includes the following steps: Step 1: Establish a measurement model for collecting measurement values; The measurement value is the relative distance between the master node and the child node; In step 1, the measurement model is: in, Represents child nodes i coordinates Indicates the master node n coordinates Indicates the measured value. Indicates the distance measurement error; Step 2: Establish a state dimension expansion model in the geocentric-geofixed coordinate system. The state dimension expansion model is used to expand the state variables of the child nodes to obtain the expanded state variables. The state dimension expansion model is also used to expand the dimension of the state covariance and the noise covariance; In step 2, the expanded state variables are represented as follows: in, Indicates the three-dimensional attitude error. Indicates the three-dimensional velocity error. Indicates three-dimensional position error. This indicates that the gyroscope has zero bias across its three axes. This indicates that the accelerometer has zero bias across all three axes. This represents the three-dimensional velocity error of the master node. This represents the three-dimensional position error of the master node; Step 3: Establish a collaborative measurement model in the geocentric-geofixed coordinate system. The collaborative measurement model is used to obtain the relationship between the expanded state variables and the measurement values with the help of the expanded state covariance and the expanded noise covariance. Step 4: Based on the relationship between the expanded state quantity and the measurement value, filter and update the state information of the child node to obtain the positioning result of the child node.
2. The UUV cooperative positioning method considering master node positioning error according to claim 1, characterized in that, In step 2, the expanded state covariance is expressed as: in, Represents the covariance matrix of the current node. This represents the covariance matrix of the velocity error and position error of the master node.
3. The UUV cooperative positioning method considering master node positioning error according to claim 1, characterized in that, In step 2, the expanded noise covariance is expressed as: in, This represents the noise matrix of the current node. This represents a zero matrix with 6 rows and 6 columns.
4. The UUV cooperative positioning method considering master node positioning error according to claim 1, characterized in that, In step 3, the collaborative measurement model is: in, Z Indicates the pseudo-range value. H Represents the measurement transition matrix. X Represents a state vector. This indicates measurement noise.
5. A UUV cooperative positioning method considering master node positioning error according to claim 4, characterized in that, The measurement transfer matrix is represented as follows: in, This represents the transformation matrix of the current node. The transformation matrix representing the master node; This represents the Jacobian matrix representing the ranging information of the current node with respect to the current node's position. This represents the Jacobian matrix representing the distance measurement information of the current node with respect to the position of the master node.
6. A computer device, including a memory and a processor, characterized in that, The memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor executes the method according to any one of claims 1-5.
7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the method of any one of claims 1-5.