Master-slave USV collaborative navigation method based on corner network condition adjustment

Abstract

The invention discloses a master-slave USV collaborative navigation method based on corner net condition adjustment, and belongs to the technical field of autonomous underwater navigation, and the method comprises the following steps: using a strapdown inertial navigation system to obtain a navigation state of a USV to be positioned at an initial moment, receiving a current position shared by other USV data in a USV formation by the USV to be positioned, converting the current position into a two-dimensional coordinate, and carrying out navigation on the two-dimensional coordinate; the USV to be positioned obtains the relative distance measurement information # imgabs0 # and the relative azimuth angle # imgabs1 # between the USV to be positioned and other USVs according to the equipped sensor, establishes a state equation and a measurement equation for describing the USV to be positioned, establishes a corner net condition adjustment mathematical model, processes measurement vectors, obtains a measurement vector adjustment value and a variance matrix thereof, sets an observation noise variance matrix, and obtains the measurement vector adjustment value of the USV to be positioned according to the measurement information # imgabs0 # and the relative azimuth angle # imgabs1 # of the USV to be positioned. And measurement updating is carried out in combination with DVL speed measurement information. By adopting the corner net condition adjustment-based master-slave USV collaborative navigation method disclosed by the invention, high-precision estimation and collaborative correction of the navigation state of the USV to be positioned can be realized.

Description

Technical Field

[0001] This invention relates to the field of autonomous underwater navigation technology, and in particular to a master-slave USV cooperative navigation method based on edge-angle network condition adjustment. Background Technology

[0002] With the increasing demand for highly competitive and dynamic complex environments, the operational mode of Unmanned Surface Vehicles (USVs) has shifted from single-vehicle to platoon-based. USV platoons achieve collaborative operations through real-time sharing of sensor information distributed across all vessels, with precise positioning being the foundation of this data sharing. To meet the requirements of high-precision navigation and positioning, spatial mapping relationships between USVs are used as measurement information, combined with data fusion technology to achieve collaborative navigation and improve overall performance. In satellite-denied environments, internal navigation information exchange can effectively suppress positioning error divergence.

[0003] Traditional master-slave USV cooperative navigation estimates the position of the slave USV by fusing high-precision positioning information from the master USV with relative distance information between them. In recent years, researchers have proposed utilizing navigation sensor information onboard the USVs (such as DVL velocity measurement and relative attitude information between vessels) to expand the dimensions of measurement information, thereby improving the observability and robustness of the formation cooperative navigation system. Whether based on relative attitude information or combined with navigation sensor information, the accuracy and stability of measurement information are crucial to cooperative correction performance. In particular, external interactive measurement information such as distance and angle measurements between USVs is easily affected by the environment, making it difficult to guarantee its performance. Therefore, researching methods to improve the accuracy of measurement information and combine it with DVL velocity information for cooperative navigation is of great significance for achieving high-precision USV navigation state estimation and cooperative correction. Summary of the Invention

[0004] The purpose of this invention is to provide a master-slave USV cooperative navigation method based on edge-angle network condition adjustment. This method improves the accuracy of corresponding measurement information by estimating the adjustment values ​​of edge and angle measurements between vessels, and further improves the accuracy of navigation state estimation by combining DVL velocity information, thereby achieving the goal of improving the navigation and positioning performance of USV formations in satellite denial environments.

[0005] To achieve the above objectives, this invention provides a master-slave USV cooperative navigation method based on edge-corner network conditional adjustment, comprising the following steps:

[0006] S1. Use a strapdown inertial navigation system to obtain the navigation state of the USV at the initial moment of positioning, select the Northeast-Heaven-Earth coordinate system as the navigation coordinate system, and construct a 13-dimensional system state variable X(t).

[0007] S2. The USV to be located receives its current latitude and longitude position (λ,L) from other USVs in the USV formation and converts it into coordinates in a two-dimensional Cartesian coordinate system.

[0008] S3. The USV to be located obtains relative distance measurement information between itself and other USVs based on its equipped sensors. and relative azimuth

[0009] S4. Establish the state equations describing the USV to be located based on the SINS error equations.

[0010] S5. Combining the measurement information between the master and slave USVs and the DVL speed measurement information carried by the slave USV, establish the measurement equations of the cooperative navigation system to describe the USV to be located.

[0011] S6. Establish a mathematical model for the conditional adjustment of edge-angle network;

[0012] S7. Use the edge-angle network conditional adjustment mathematical model established in S6 to process the measurement vectors and obtain the adjusted values ​​of the measurement vectors and their variance matrix.

[0013] S8. Use the adjusted value of the measurement vector obtained in S7 to replace the real observation vector, set the observation noise variance matrix using the measurement vector variance matrix, and update the measurement by combining it with DVL velocity measurement information.

[0014] Preferably, the 13-dimensional system state variable X(t) constructed in S1 is as follows:

[0015]

[0016] Where, φ=[φ E ,φ N ,φ U ] is the misalignment angle, δv=[δv E ,δv N [] represents the velocity error, δp = [δL, δλ] represents the latitude and longitude errors, and ε = [ε] x ,ε y ,ε z [This refers to the zero-point error of the three-axis gyroscope drift.] This represents the zero-position error of the triaxial accelerometer.

[0017] Preferably, the latitude and longitude coordinates (λ, L) of other USVs in S2 need to be transformed according to the arc length approximation criterion to obtain the x-axis and y-axis coordinates of a two-dimensional Cartesian coordinate system. Specifically:

[0018]

[0019] Where (λ0,L0) is the initial position of the USV to be located, and RN R is the radius of curvature of the zonal loop. M Let be the radius of curvature of the meridian.

[0020] Preferably, the relative distance measurement information in S3 Specifically, it is expressed as follows:

[0021]

[0022] Among them, v i For the ranging error, r i The actual distance between the primary USV and the secondary USV is specifically expressed as:

[0023]

[0024] Among them, (x mi ,y mi ) is the position coordinate of the main USV, (x s ,y s () represents the position coordinates from the USV;

[0025] relative azimuth Specifically, it is expressed as follows:

[0026]

[0027] Among them, α i The true value of the relative azimuth angle between the master USV and the slave USV is defined as the angle between the line connecting the centroids of the master USV and the slave USV and the x-axis. i This represents the angle measurement error.

[0028] Preferably, the state equation describing the USV to be located in S4 is as follows:

[0029]

[0030] in, Let X(t) be the time derivative of the state vector X(t), F(t) be the state transition matrix, G(t) be the noise driving matrix, and W(t) be the system noise vector.

[0031] Preferably, the state transition matrix F(t) in S4 is as follows:

[0032]

[0033]

[0034] Among them, w ie R is the angular velocity of Earth's rotation. N R is the radius of curvature of the zonal loop. M v is the radius of curvature of the meridian. EFor the geographical eastward velocity, v N For the geographic northward velocity, f = [f E ,f N ,f U To output the projection of the force in the northeast-northeast celestial coordinate system, Let ψ be the attitude transformation matrix, θ be the heading angle, θ be the pitch angle, and γ be the roll angle.

[0035] Preferably, the noise driving matrix G(t) in S4 is as follows:

[0036]

[0037] The system noise vector W(t) is specifically:

[0038] W(t) = [w gx ,w gy ,w gz ,w ax ,w ay ,w az ] T (4.14)

[0039] Among them, w gx ,w gy ,w gz For three-axis gyroscope drift white noise, w ax ,w ay ,w az The triaxial angular velocity meter has zero white noise bias.

[0040] Preferably, the measurement equation describing the USV to be located in S5 is as follows:

[0041] Z(t)=H(t)X(t)+V(t) (5.1)

[0042] Where H(t) is the system measurement matrix and V(t) is the measurement noise vector;

[0043] The system measurement matrix H(t) is specifically as follows:

[0044]

[0045] The noise vector V(t) is specifically measured as follows:

[0046]

[0047] V1(t)=-[Δd 1x ,Δd 1y ] T (5.6)

[0048] V2(t)=-[Δd 2x ,Δd2y ] T (5.7)

[0049]

[0050] Where r1 and r2 are the true distances between the main USV1 and the main USV2 and the slave USV respectively; α1 and α2 are the true azimuth angles between the main USV1 and the main USV2 and the slave USV respectively; v1 and v2 are the ranging errors, and w1 and w2 are the angle measurement errors.

[0051] Preferably, the mathematical model for the conditional adjustment of the edge-angle network established by S6 is as follows:

[0052] A·V m +W=[A1,A2]V m +[W1,W2] T =0 (6.1)

[0053] Among them, V m For the correction of the observations,

[0054]

[0055] W1=S3·sin(180°-β1-β3)-S2·sin(β1+β2) (6.3)

[0056]

[0057] W2=S1·sin(180°-β1-β3)-S2·sin(β3-β2) (6.5)

[0058] Among them, β1 and β2 are the relative azimuth angles between the main USV1 and the main USV2 and the slave USV respectively, β3 is the relative azimuth angle between the main USV1 and the main USV2, S1 and S2 are the relative distances between the main USV1, the main USV2 and the slave USV, and S3 is the relative distance between the main USV1 and the main USV2.

[0059] Preferably, the adjustment value of the measurement vector in S7 Specifically:

[0060]

[0061] Measurement vector adjustment value The variance matrix is ​​specifically:

[0062]

[0063] in, For the adjustment value of the measurement vector The cofactor matrix is ​​as follows:

[0064]

[0065] Among them, N AA =AQ m A T The coefficient matrix of the conditional adjustment equation;

[0066] The variance is expressed as a unit weight, specifically:

[0067]

[0068] Where r represents redundant observations and the number of condition equations.

[0069] Therefore, the master-slave USV cooperative navigation method based on edge-corner network condition adjustment described above has the following beneficial effects:

[0070] (1) This invention considers the impact of USV cooperative navigation ranging and angle measurement errors on the system's navigation and positioning performance, improves the accuracy of measurement information based on measurement adjustment theory, and accurately estimates the measurement variance information based on the adjustment results;

[0071] (2) This invention establishes an integrated filtering model that can realize the adjustment of measurement information and perform EKF cooperative navigation state estimation based on the adjusted measurement information and corresponding variance information;

[0072] (3) Compared with conventional cooperative navigation algorithms, this invention not only has a faster convergence speed, but also higher estimation accuracy.

[0073] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0074] Figure 1 This is a flowchart of a master-slave USV cooperative navigation method based on edge-angle network condition adjustment according to the present invention;

[0075] Figure 2 This is a schematic diagram of the USV formation measurement vector of a master-slave USV cooperative navigation method based on edge-angle network condition adjustment according to the present invention.

[0076] Figure 3 This is a schematic diagram of the corner network conditional adjustment of the master-slave USV cooperative navigation method based on corner network conditional adjustment according to the present invention. Detailed Implementation

[0077] The following detailed description of embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0078] Example

[0079] like Figure 1 As shown, this invention provides a master-slave USV cooperative navigation method based on edge-corner network condition adjustment, comprising the following steps:

[0080] S1. Use a strapdown inertial navigation system to obtain the navigation state of the USV at the initial moment of positioning. Select the Northeast-Heaven-Earth coordinate system as the navigation coordinate system and construct a 13-dimensional system state variable X(t).

[0081] The constructed 13-dimensional system state variable X(t) is as follows:

[0082]

[0083] Where, φ=[φ E ,φ N ,φ U ] is the misalignment angle, δv=[[δv E ,δv N [δp] represents the velocity error, and [δL, δλ] represents the latitude and longitude errors, respectively. This refers to the zero-position error of the three-axis gyroscope drift. This represents the zero-position error of the triaxial accelerometer.

[0084] S2. The USV to be located receives its current latitude and longitude position (λ,L) from other USVs in the USV formation and converts it into coordinates in a two-dimensional Cartesian coordinate system.

[0085] The latitude and longitude coordinates (λ, L) of other USVs need to be transformed according to the arc length approximation criterion to obtain the x-axis and y-axis coordinates in a two-dimensional Cartesian coordinate system. Specifically:

[0086]

[0087] Where (λ0,L0) is the initial position of the USV to be located, and R N R is the radius of curvature of the zonal loop. M Let be the radius of curvature of the meridian.

[0088] S3. The USV to be located obtains relative distance measurement information between itself and other USVs based on its equipped sensors. and relative azimuth

[0089] Combine Figure 2 Relative distance measurement information Specifically, it is expressed as follows:

[0090]

[0091] Among them, v i For the ranging error, r i The actual distance between the primary USV and the secondary USV is specifically expressed as:

[0092]

[0093] Among them, (x mi ,y mi ) is the position coordinate of the main USV, (x s ,y s () represents the position coordinates from the USV;

[0094] relative azimuth Specifically, it is expressed as follows:

[0095]

[0096] Among them, α i The true value of the relative azimuth angle between the master USV and the slave USV is defined as the angle between the line connecting the centroids of the master USV and the slave USV and the x-axis. i This represents the angle measurement error.

[0097] S4. Establish the state equation describing the USV to be located based on the SINS error equation.

[0098] The state equations describing the USV to be located are as follows:

[0099]

[0100] in, Let X(t) be the time derivative of the state vector X(t), F(t) be the state transition matrix, G(t) be the noise driving matrix, and W(t) be the system noise vector.

[0101] The state transition matrix F(t) is specifically:

[0102]

[0103]

[0104] Among them, w ie R is the angular velocity of Earth's rotation. N R is the radius of curvature of the zonal loop. M v is the radius of curvature of the meridian. EFor the geographical eastward velocity, v N For the geographic northward velocity, f = [f E ,f N ,f U To output the projection of the force in the northeast-northeast celestial coordinate system, Let ψ be the attitude transformation matrix, θ be the heading angle, θ be the pitch angle, and γ be the roll angle.

[0105] The noise driving matrix G(t) is specifically as follows:

[0106]

[0107] The system noise vector W(t) is specifically:

[0108] W(t) = [w gx ,w gy ,w gz ,w ax ,w ay ,w az ] T (4.14)

[0109] Among them, w gx ,w gy ,w gz For three-axis gyroscope drift white noise, w ax ,w ay ,w az The triaxial angular velocity meter has zero white noise bias.

[0110] S5. Combining the measurement information between the master and slave USVs and the DVL speed measurement information carried by the slave USV, establish the measurement equations of the cooperative navigation system to describe the USV to be located.

[0111] The measurement equations describing the USV to be located are as follows:

[0112] Z(t)=H(t)X(t)+V(t) (5.1)

[0113] Where H(t) is the system measurement matrix and V(t) is the measurement noise vector;

[0114] The system measurement matrix H(t) is specifically as follows:

[0115]

[0116] The noise vector V(t) is specifically measured as follows:

[0117]

[0118] V1(t)=-[Δd 1x ,Δd 1y ] T(5.6)

[0119] V2(t)=-[Δd 2x ,Δd 2y ] T (5.7)

[0120]

[0121] Where r1 and r2 are the true distances between the main USV1 and the main USV2 and the slave USV respectively; α1 and α2 are the true azimuth angles between the main USV1 and the main USV2 and the slave USV respectively; v1 and v2 are the ranging errors, and w1 and w2 are the angle measurement errors.

[0122] S6. Establish a mathematical model for the conditional adjustment of the edge-angle network.

[0123] Combine Figure 3 The specific mathematical model for the conditional adjustment of the edge-angle network is as follows:

[0124] A·V m +W=[A1,A2]V m +[W1,W2] T =0 (6.1)

[0125] Among them, V m For the correction of the observations,

[0126]

[0127] W1=S3·sin(180°-β1-β3)-S2·sin(β1+β2) (6.3)

[0128]

[0129] W2=S1·sin(180°-β1-β3)-S2·sin(β3-β2) (6.5)

[0130] Among them, β1 and β2 are the relative azimuth angles between the main USV1 and the main USV2 and the slave USV respectively, β3 is the relative azimuth angle between the main USV1 and the main USV2, S1 and S2 are the relative distances between the main USV1, the main USV2 and the slave USV, and S3 is the relative distance between the main USV1 and the main USV2.

[0131] S7. Using the edge-angle network conditional adjustment mathematical model established in S6, the measurement vectors are processed to obtain the adjusted values ​​of the measurement vectors and their variance matrix. The optimal solution for the observation corrections is obtained using the weighted least squares algorithm. The least squares criterion followed by conditional adjustment is:

[0132]

[0133] Let the Lagrange multiplier vector be... Construct the Lagrange function as follows:

[0134]

[0135] The correction V is solved using the Lagrange multiplier method. m This leads to the obtained measurement vector adjustment values. Specifically:

[0136]

[0137] Measurement vector adjustment value The variance matrix is ​​specifically:

[0138]

[0139] in, For the adjustment value of the measurement vector The cofactor matrix is ​​as follows:

[0140]

[0141] Among them, N AA =AQ m A T The coefficient matrix of the conditional adjustment equation;

[0142] The variance is expressed as a unit weight, specifically:

[0143]

[0144] Where r represents redundant observations and the number of condition equations.

[0145] S8. Use the adjusted value of the measurement vector obtained in S7 to replace the real observation vector. Estimate the variance matrix representing the absolute accuracy of the observation based on the adjustment results. Then, use the measurement vector variance matrix to set the observation noise variance matrix and combine it with DVL velocity measurement information to update the measurement.

[0146] Therefore, the present invention employs the above-mentioned master-slave USV cooperative navigation method based on edge-angle network condition adjustment, which can achieve high-precision estimation and cooperative correction of the navigation state of the USV (slave USV) to be located.

[0147] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A master-slave USV collaborative navigation method based on corner network condition adjustment, characterized in that: The following steps are involved: S1. Use the strapdown inertial navigation system to obtain the navigation state of the USV to be positioned at the initial moment, select the northeast sky geographic coordinate system as the navigation coordinate system, and construct the 13-dimensional system state variable X(t); S2, the USV to be positioned receives the current longitude and latitude position (λ, L) shared by other USVs in the USV formation, and converts it into coordinates in a two-dimensional rectangular coordinate system; S3: The USV to be positioned obtains the relative distance measurement information between it and other USVs based on the sensors it is equipped with. and relative azimuth S4. Establish a state equation describing the USV to be positioned based on the SINS error equation; S5. Combine the measurement information between the master and slave USVs and the speed measurement information of the DVL carried by the slave USV to establish a measurement equation describing the USV to be positioned in the collaborative navigation system; S6. Establishing a mathematical model for corner network condition adjustment; S7, using the corner grid condition adjustment mathematical model established in S6 to process the measurement vector and obtain the measurement vector adjustment value and its variance matrix S8. Use the measurement vector adjustment value obtained in S7 to replace the real observation vector, use the measurement vector variance matrix to set the observation noise variance matrix, and perform measurement update in combination with the DVL speed measurement information.

2. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, characterized in that, The 13-dimensional system state variable X(t) constructed in S1 is specifically: Where φ=[φ E ,φ N ,φ U ] is the misalignment angle, δv=[δv E ,δv N ] is the velocity error, δp=[δL,δλ] is the latitude error and longitude error, ε=[ε x ,ε y ,ε z ] is the zero position error of the three-axis gyroscope drift, is the zero position error of the triaxial accelerometer.

3. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, characterized in that, The longitude and latitude coordinates (λ, L) of other USVs in S2 are transformed according to the arc length approximation criterion to obtain the x-axis and y-axis coordinates of the two-dimensional plane rectangular coordinate system, which are specifically: Among them, (λ0, L0) is the initial position of the USV to be located, R N R is the radius of curvature of the yoke, M is the meridian curvature radius.

4. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, characterized in that, Relative distance measurement information in S3 Specifically expressed as: Among them, v i is the ranging error, r i The actual distance between the master USV and the slave USV is specifically expressed as: Among them, (x mi ,y mi ) is the position coordinate of the main USV, (x s ,y s ) is the position coordinate from USV; Relative azimuth Specifically expressed as: Among them, α i The true value of the relative azimuth between the master USV and the slave USV, defined as the angle between the center of mass of the master USV and the slave USV and the x-axis, w i is the angle measurement error.

5. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, characterized in that, The state equation describing the USV to be positioned in S4 is specifically: in, is the time derivative of the state vector X(t), F(t) is the state transfer matrix, G(t) is the noise driving matrix, and W(t) is the system noise vector.

6. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 5, characterized in that, The state transfer matrix F(t) in S4 is specifically: Among them, w ie is the angular velocity of the Earth’s rotation, R N R is the radius of curvature of the yoke, M is the meridian curvature radius, v E is the geographic eastward velocity, v N is the geographic north velocity, f=[f E ,f N ,f U ] is the projection of the output ratio in the northeastern sky geographic coordinate system, is the attitude transformation matrix, ψ is the heading angle, θ is the pitch angle, and γ is the roll angle.

7. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 6, characterized in that: The noise driving matrix G(t) in S4 is specifically: The system noise vector W(t) is specifically: W(t)=[w gx ,w ay ,w gz ,w ax ,w ay ,w az ] T (4.14) Among them, w gx ,w gy ,w gz is the three-axis gyro drift white noise, w ax ,w ay ,w az is the zero bias white noise of the three-axis angular velocity sensor.

8. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, characterized in that: The measurement equation describing the USV to be located in S5 is specifically: Z(t)=H(t)X(t)+V(t) (5.1) Where H(t) is the system measurement matrix, V(t) is the measurement noise vector; The system measurement matrix H(t) is specifically: The measurement noise vector V(t) is specifically: V1(t)=-[Δd 1x ,Δd 1y ] T (5.6) V2(t)=-[Δd 2x ,Δd 2y ] T (5.7) Among them, r1 and r2 are the true distances between the master USV1 and the master USV2 and the slave USV respectively; α1 and α2 are the true azimuths between the master USV1 and the master USV2 and the slave USV respectively; v1 and v2 are the ranging errors, and w1 and w2 are the angle measurement errors.

9. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, characterized in that: The mathematical model of corner network condition adjustment established by S6 is as follows: A·V m +W=[A1,A2]V m +[W1,W2] T =0 (6.1) Among them, V m is the observation correction number, W1=S3·sin(180°-β1-β3)-S2·sin(β1+β2) (6.3) W2=S1·sin(180°-β1-β3)-S2·sin(β3-β2) (6.5) Among them, β1 and β2 are the relative azimuths between the main USV1 and the main USV2 and the slave USV respectively, β3 is the relative azimuth between the main USV1 and the main USV2, S1 and S2 are the relative distances between the main USV1 and the main USV2 and the slave USV, and S3 is the relative distance between the main USV1 and the main USV2.

10. A master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 9, characterized in that: Measure vector adjustment values ​​in S7 Specifically: Measure vector adjustment values The variance matrix of is: in, is the measured vector adjustment value The cofactor matrix of is: Among them, N AA =AQ m A T is the coefficient matrix of the conditional adjustment equation; is the unit weight variance, specifically: Among them, r is the number of redundant observations and conditional equations.

Images