A 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 network condition adjustment, belonging to the technical field of autonomous underwater navigation. The method comprises the following steps: using a strapdown inertial navigation system to obtain the navigation state of a USV to be positioned at an initial moment; the USV to be positioned receives the current position shared by data of other USVs in a USV formation and converts the current position into two-dimensional coordinates; the USV to be positioned obtains relative distance measurement information and relative azimuth angle between the USV to be positioned and other USVs according to equipped sensors; establishes a state equation and a measurement equation describing the USV to be positioned; establishes a corner network condition adjustment mathematical model; processes measurement vectors to obtain measurement vector adjustment values ​​and variance matrices thereof; sets an observation noise variance matrix; and performs measurement update in combination with DVL speed measurement information. The invention adopts the master-slave USV collaborative navigation method based on corner network condition adjustment to achieve high-precision estimation and collaborative correction of the navigation state of the USV to be positioned.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous underwater navigation, and in particular to a master-slave USV collaborative navigation method based on corner network condition adjustment. Background Art

[0002] With the growing demand for highly confrontational, dynamic, and complex environments, unmanned surface vehicles (USVs) are shifting their operational model from single vessels to formations. USV formations achieve collaborative operations through real-time sharing of sensor information distributed across each vessel, with precise positioning as the foundation for this data sharing. To meet the demand for high-precision navigation and positioning, collaborative navigation is achieved by utilizing spatial mapping relationships between USVs as measurement information, combined with data fusion technology, to improve overall performance. In satellite-denied environments, internal navigation information exchange effectively suppresses positioning error divergence.

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

[0004] The purpose of the present invention is to provide a master-slave USV collaborative navigation method based on angle network condition adjustment, which improves the accuracy of corresponding measurement information by estimating the adjustment value of side and angle measurement between vessels, and further improves the navigation state estimation accuracy by combining DVL speed measurement information, thereby achieving the purpose of improving the navigation and positioning performance of USV formation in a satellite denial environment.

[0005] To achieve the above object, the present invention provides a master-slave USV collaborative navigation method based on corner network condition adjustment, comprising the following steps:

[0006] S1. Use the strapdown inertial navigation system to obtain the navigation state of the USV to be positioned at the initial moment, select the northeastern sky geographic coordinate system as the navigation coordinate system, and construct the 13-dimensional system state variable X(t);

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

[0008] S3. The USV to be positioned obtains the relative distance measurement information between it and other USVs based on the equipped sensors and relative azimuth

[0009] S4. Establish a state equation describing the USV to be positioned based on the SINS error equation;

[0010] 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 the measurement equation of the collaborative navigation system to describe the USV to be positioned;

[0011] S6. Establishing a mathematical model for corner network condition adjustment;

[0012] S7, utilize 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;

[0013] S8. Use the measurement vector adjustment value obtained in S7 to replace the true 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.

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

[0015]

[0016] 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.

[0017] Preferably, the longitude and latitude 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 the two-dimensional plane rectangular coordinate system. Specifically:

[0018]

[0019] Among them, (λ0, L0) is the initial position of the USV to be positioned, RN is the radius of curvature of the y-axis circle, R M is the meridian curvature radius.

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

[0021]

[0022] Among them, v i is the ranging error, r i The actual distance between the master USV and the slave USV is expressed as:

[0023]

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

[0025] Relative azimuth Specifically expressed as:

[0026]

[0027] Among them, α i The true value of the relative azimuth between the master USV and the slave USV is 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.

[0028] Preferably, the state equation describing the USV to be positioned in S4 is specifically:

[0029]

[0030] 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.

[0031] Preferably, the state transfer matrix F(t) in S4 is specifically:

[0032]

[0033]

[0034] Among them, w ie is the angular velocity of the Earth's rotation, R N is the radius of curvature of the y-axis circle, R M is the meridian curvature radius, v Eis 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 geographic coordinate system, is the attitude transformation matrix, ψ is the heading angle, θ is the pitch angle, and γ is the roll angle.

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

[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 is the three-axis gyro drift white noise, w ax ,w ay ,w az is the zero-bias white noise of the triaxial angular velocity sensor.

[0040] Preferably, the measurement equation describing the USV to be positioned in S5 is specifically:

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

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

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

[0044]

[0045] The measurement noise vector V(t) is specifically:

[0046]

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

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

[0049]

[0050] 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.

[0051] Preferably, the corner grid condition adjustment mathematical model established in S6 is specifically:

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

[0053] Among them, V m is the observation correction number,

[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 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.

[0059] Preferably, the vector adjustment value is measured in S7 Specifically:

[0060]

[0061] Measure vector adjustment values The variance matrix of is specifically:

[0062]

[0063] in, is the measured vector adjustment value The cofactor matrix of is:

[0064]

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

[0066] is the unit weight variance, specifically:

[0067]

[0068] Among them, r is the redundant observation value and the number of conditional equations.

[0069] Therefore, the present invention adopts the above-mentioned master-slave USV collaborative navigation method based on corner network condition adjustment, which has the following beneficial effects:

[0070] (1) The present invention takes into account the problem that USV collaborative navigation ranging and angle measurement errors affect the system navigation and positioning performance, improves measurement information precision based on measurement adjustment theory and accurately estimates measurement variance information according to adjustment results;

[0071] (2) The present invention establishes an integrated filtering model that can realize measurement information adjustment and perform EKF collaborative navigation state estimation based on the measurement information after adjustment and the corresponding variance information;

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

[0073] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 This is a flow chart of a master-slave USV collaborative navigation method based on corner network condition adjustment according to the present invention;

[0075] Figure 2 A schematic diagram of USV formation measurement vectors of a master-slave USV collaborative navigation method based on corner network condition adjustment according to the present invention;

[0076] Figure 3 The present invention provides a schematic diagram of the corner network condition adjustment of a master-slave USV collaborative navigation method based on the corner network condition adjustment. DETAILED DESCRIPTION

[0077] The following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but rather merely represents selected embodiments of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort shall fall within the scope of protection of the present invention.

[0078] Example

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

[0080] 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 celestial geographic coordinate system as the navigation coordinate system, and construct the 13-dimensional system state variable X(t).

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

[0082]

[0083] 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, is the zero position error of the three-axis gyroscope drift, is the zero position error of the triaxial accelerometer.

[0084] S2. The USV to be positioned receives its 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.

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

[0086]

[0087] Among them, (λ0, L0) is the initial position of the USV to be positioned, R N is the radius of curvature of the y-axis circle, R M is the meridian curvature radius.

[0088] S3. The USV to be positioned obtains the relative distance measurement information between it and other USVs based on the equipped sensors and relative azimuth

[0089] Combine Figure 2 , relative distance measurement information Specifically expressed as:

[0090]

[0091] Among them, v i is the ranging error, r i The actual distance between the master USV and the slave USV is expressed as:

[0092]

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

[0094] Relative azimuth Specifically expressed as:

[0095]

[0096] Among them, α i The true value of the relative azimuth between the master USV and the slave USV is 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.

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

[0098] The state equation describing the USV to be positioned is specifically:

[0099]

[0100] 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.

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

[0102]

[0103]

[0104] Among them, w ie is the angular velocity of the Earth's rotation, R N is the radius of curvature of the y-axis circle, R M is the meridian curvature radius, v Eis 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 geographic coordinate system, is the attitude transformation matrix, ψ is the heading angle, θ is the pitch angle, and γ is the roll angle.

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

[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 is the three-axis gyro drift white noise, w ax ,w ay ,w az is the zero-bias white noise of the triaxial angular velocity sensor.

[0110] 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 the measurement equation of the collaborative navigation system to describe the USV to be positioned.

[0111] The measurement equation describing the USV to be positioned is:

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

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

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

[0115]

[0116] The measurement noise vector V(t) is specifically:

[0117]

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

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

[0120]

[0121] 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.

[0122] S6. Establish a mathematical model for corner network conditional adjustment.

[0123] Combine Figure 3 The mathematical model of corner network condition adjustment is as follows:

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

[0125] Among them, V m is the observation correction number,

[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 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.

[0131] S7. Process the measurement vector using the corner grid conditional adjustment mathematical model established in S6 to obtain the measurement vector adjustment value and its variance matrix. Use the weighted least squares algorithm to obtain the optimal solution for the observation correction number. The least squares criterion followed by the conditional adjustment is:

[0132]

[0133] Let the Lagrange multiplier vector be The Lagrangian function is constructed as:

[0134]

[0135] Use Lagrange multiplier method to solve the correction number V m , and then get the measurement vector adjustment value Specifically:

[0136]

[0137] Measure vector adjustment values The variance matrix of is specifically:

[0138]

[0139] in, is the measured vector adjustment value The cofactor matrix of is:

[0140]

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

[0142] is the unit weight variance, specifically:

[0143]

[0144] Among them, r is the redundant observation value and the number of conditional equations.

[0145] S8. Use the measurement vector adjustment value obtained in S7 to replace the true observation vector, estimate the variance matrix representing the absolute accuracy of the observation value based on the adjustment result, and then 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.

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

[0147] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit the same. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solutions of the present invention with equivalents, and these modifications or equivalent replacements 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 northeastern sky geographic coordinate system as the navigation coordinate system, and construct the 13-dimensional system state variable ; S2, the USV to be positioned receives the current longitude and latitude position of itself from data shared by other USVs in the USV formation ( ), and convert 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 the measurement equation of the collaborative navigation system to describe the USV to be positioned; S6. Establishing a mathematical model for corner network condition adjustment; The established mathematical model for corner network condition adjustment is as follows: （6.1） in, is the observation correction number, ; （6.2） （6.3） （6.4） （6.5） in, and The relative azimuths between the master USV1, master USV2 and the slave USV, is the relative azimuth between the main USV1 and the main USV2, and The relative distance between the main USV1, main USV2 and the slave USV, is the relative distance between the main USV1 and the main USV2; 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 ; Measure vector adjustment values Specifically: （7.1） Measure vector adjustment values The variance matrix of is specifically: （7.2） in, is the measured vector adjustment value The cofactor matrix of is: （7.3） in, is the coefficient matrix of the conditional adjustment equation; is the unit weight variance, specifically: （7.4） in, is the redundant observation value, the number of conditional equations; S8. Use the measurement vector adjustment value obtained in S7 to replace the true 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 kind of master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, is characterized in that, 13-dimensional system state variables constructed in S1 Specifically: （1.1） in, is the misalignment angle, is the speed error, are the latitude error and longitude error, is the zero position error of the three-axis gyroscope drift, is the zero position error of the triaxial accelerometer.

3. a kind of master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, is characterized in that, The latitude and longitude coordinates of other USVs in S2 ( ) According to the arc length approximation criterion, coordinate transformation is performed to obtain a two-dimensional plane rectangular coordinate system axis, Axis coordinates, specifically: （2.1） in,( ) is the initial position of the USV to be positioned, is the radius of curvature of the Maoyou circle, is the meridian curvature radius.

4. a kind of master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, is characterized in that, Relative distance measurement information in S3 Specifically expressed as: （3.1） in, is the ranging error, The actual distance between the master USV and the slave USV is expressed as: （3.2） in,( ) is the position coordinate of the main USV, ( ) is the position coordinate of the slave USV; Relative azimuth Specifically expressed as: （3.3） in, The true value of the relative azimuth between the master USV and the slave USV is defined as the line connecting the centroids of the master USV and the slave USV and x The angle between the axes, is the angle measurement error.

5. a kind of master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 1, is characterized in that, The state equation describing the USV to be positioned in S4 is specifically: （4.1） in, is the state vector The time derivative of is the state transition matrix, is the noise driving matrix, is the system noise vector.

6. a kind of master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 5, is characterized in that, State transition matrix in S4 Specifically: （4.2） （4.3） （4.4） （4.5） （4.6） （4.7） （4.8） （4.9） （4.10） （4.11） （4.12） in, is the Earth's rotation angular velocity, is the radius of curvature of the Maoyou circle, is the meridian curvature radius, is the geographic eastward velocity, is the geographic northing velocity, To output the projection of the comparison in the Northeastern geographic coordinate system, is the posture transformation matrix, is the heading angle, is the pitch angle, is the roll angle.

7. a kind of master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 6, is characterized in that, S4 noise driving matrix Specifically: （4.13） System noise vector Specifically: （4.14） in, is the three-axis gyroscope drift white noise, is the zero-bias white noise of the triaxial angular velocity sensor.

8. a kind of master-slave USV collaborative navigation method based on corner network condition adjustment according to claim 3, is characterized in that, The measurement equation describing the USV to be positioned in S5 is specifically: （5.1） in, is the system measurement matrix, is the measurement noise vector; System Measurement Matrix Specifically: （5.2） （5.3） （5.4） Measurement noise vector Specifically: （5.5） （5.6） （5.7） （5.8） in, and The true distances between the master USV1, master USV2 and the slave USV respectively; and The true azimuth angles between the master USV1, master USV2 and the slave USV respectively; and is the ranging error, and is the angle measurement error.

Images