Master-slave nonlinear compensation cooperative navigation method under loss of acoustic ranging data

Abstract

The application discloses a master-slave type nonlinear compensation cooperative navigation method under acoustic ranging data loss, and belongs to the field of underwater vehicle navigation and positioning. Specifically, for a plurality of autonomous underwater vehicles including a master node and slave nodes, each slave node outputs real-time attitude and speed information at k-1 moment; position calculation at k moment is carried out and is used as one-step prediction of the position state; the master node outputs real-time self position at l moment, and an observation equation of relative distance of the master node and the slave nodes is established; the master node sends observation information to each slave node; and through a random parameter gamma conforming to Bernoulli distribution l , the slave node judges whether the observation information of the master node is received or not; if not, acoustic ranging data loss occurs, and state prediction of the position of the slave node at the next moment is carried out; if yes, an observation matrix compensation amount is calculated and the observation matrix is corrected, extended Kalman filtering is carried out, and the cooperative correction position of the slave node is obtained; until navigation ends; and the application improves cooperative positioning precision.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of underwater vehicle navigation and positioning, and relates to a master-slave nonlinear compensation cooperative navigation method under acoustic ranging data loss. BACKGROUND

[0002] Compared with single underwater vehicle, multi-autonomous underwater vehicle cooperative operation not only improves efficiency and reliability, but also shows high quality and broad application potential in many fields such as military reconnaissance, ocean surveying and mapping, and resource exploration.

[0003] High-precision navigation capability is the key to performing multi-autonomous underwater vehicle cooperative combat tasks. Currently, common navigation methods include geophysical field matching navigation, inertial navigation and acoustic navigation. However, these methods are difficult to independently complete the navigation and positioning of autonomous underwater vehicles.

[0004] As a new navigation strategy for multi-autonomous underwater vehicle cluster operation, cooperative navigation not only helps to reduce the cost of cooperative combat system, but also improves the high-precision strike capability of the overall combat group, and has advantages such as high robustness, information sharing, task cooperation and long-time navigation.

[0005] Currently, the most commonly used mode of cooperative navigation is master-slave cooperative navigation, which is suitable for multi-unmanned surface vehicle, multi-unmanned aerial vehicle, unmanned ground vehicle and multi-robot system. The application adopts one master and one slave for introduction. The master node is equipped with a global navigation satellite system or a high-precision inertial navigation system (hereinafter referred to as inertial navigation) and a Doppler log, as well as an ultra-short baseline. The slave node is equipped with a low-precision inertial navigation system and a Doppler log, receives the position and relative distance information of the master node, and uses a data fusion algorithm to significantly improve the navigation accuracy of the slave node. However, the cooperative navigation accuracy is affected by many factors, including communication environment quality, sensor accuracy and reliability, and navigation algorithm. Among them, the information fusion algorithm is the core, and the model of the cooperative navigation system is nonlinear, so the extended Kalman filter algorithm and the nonlinear filter algorithm derived therefrom are widely used in cooperative navigation information fusion.

[0006] Prior art 1: "A master-slave cooperative positioning method of autonomous underwater vehicle combined navigation system" with publication number CN111595348A uses the distance measurement information of the master and slave AUVs as the observation quantity to cooperatively position the slave AUV and correct the distance between the two. Document 2: "An improved master-slave cooperative navigation technology research" in the 45th volume, 3rd issue in October 2022 restructures the system state equation and the measurement equation, uses extended Kalman filtering for filtering solution, and improves the navigation and positioning accuracy of the slave vehicle. Document 3: "Double leader AUV cooperative navigation algorithm based on distance measurement" in the 36th volume, 7th issue in July 2016 proposes a double leader AUV cooperative navigation algorithm to correct the position error of the leader AUV and converge to the mean value of the positioning error, and the positioning error of the follower AUV is effectively corrected. Document 4: "Cooperative localization for autonomous underwater vehicles" in the 28th volume, 6th issue in 2009 uses a distributed acoustic navigation algorithm, AUVs perform acoustic ranging and data exchange with each other, and realize cooperative positioning for large area and long time tasks. Document 5: "A new adaptive extended Kalman filter for cooperative localization" in the 54th volume, 1st issue in 2017 designs an adaptive extended Kalman filter cooperative positioning algorithm, predicts the covariance matrix based on the online expectation maximization method, and improves the cooperative positioning accuracy.

[0007] The above documents use distance measurement information as the observation quantity and use a nonlinear filtering algorithm to realize cooperative positioning correction, but do not consider that the nonlinear algorithm will introduce errors, and do not perform corresponding error compensation, resulting in poor cooperative positioning effect. SUMMARY

[0008] The present application proposes a master-slave nonlinear compensation cooperative navigation method under acoustic ranging data loss to overcome the shortcomings of the prior art. In the case of observation data loss, a master-slave cooperative navigation system model and process framework are established to compensate for nonlinear errors and improve navigation accuracy. An extended Kalman filter algorithm with compensation observation matrix is used to correct the self-navigation position state of the slave node and improve the cooperative positioning accuracy.

[0009] The master-slave nonlinear compensation cooperative navigation method under acoustic ranging data loss comprises the following steps:

[0010] Step one, for a plurality of autonomous underwater vehicles including a master node and a plurality of slave nodes, each node navigates according to its own arbitrary trajectory;

[0011] Step two: each slave node outputs the attitude and velocity information of each slave node at time k-1 in real time;

[0012] Specifically, the body heading angle θ of the slave node inertial navigation output k-1 , the forward velocity of Doppler log output and the right velocity k≥2.

[0013] Step three: each slave node uses the attitude and velocity information at time k-1 to calculate the position at time k, and performs one step prediction on the slave node position state at time k, and loops to time l-1;

[0014] l=k+a / b; a is the position state update frequency of the slave node; b is the frequency of the master node sending observation information; b≥a.

[0015] The specific process of position calculation is as follows:

[0016]

[0017] The position state of the slave node at time k is x k =[x k ,y k ] T , x k , y k are the east and north position coordinates of the slave node in the navigation system; Δt is the data output time interval;

[0018] Then, the inferred position of the slave node is taken as the state variable, and one step prediction of the position state of the slave node x k / k-1 is established:

[0019]

[0020] Wherein, f(·) is the state transition function; is the position state estimation of the slave node at time k-1;

[0021] is the vector of the system input slave node at time k-1; process noise w k-1 is the 0-mean Gaussian white noise output by the sensor at time k-1;

[0022] The prediction error covariance matrix P k / k-1 is:

[0023]

[0024] The position state estimation is

[0025]

[0026] State estimation error covariance matrix P k is:

[0027] P k = P k / k-1 (5)

[0028] F k is the Jacobian matrix of the state function at P k-1 is the estimation error covariance matrix at k-1, G k is the Jacobian matrix of the state function at u k-1 Q k is the noise covariance matrix;

[0029] Step four: the master node outputs its own position at time l in real time, and establishes the observation equation of the relative distance between the master node and each slave node;

[0030] The observation equation is established based on the relative distance between the master node and the slave node at time l:

[0031]

[0032] where ||·||2 represents the matrix 2-norm; z l is the observation vector at time l; is the position coordinate of the master node navigation system at time l; h(·) is the observation function; η l is the ranging noise at time l; x l is the position coordinate of the slave node at time l;

[0033] Step five: the master node sends observation information including its own position and the relative distance between the master node and the slave node to each slave node respectively;

[0034] Step six: a random parameter γ l satisfying Bernoulli distribution is established to determine whether the slave node receives the observation information of the master node at time l. If yes, proceed to step seven, otherwise, the acoustic ranging data is lost, and return to step three to perform state prediction of the slave node position at the next time;

[0035] When γ l = 1, it means that the slave node receives the observation information at time l; when γ l = 0, it means that the slave node does not receive the observation information at time l, and the acoustic ranging data is lost.

[0036] Step seven: after Taylor expansion of the relative distance observation equation between the master node and the slave node at time l and retaining the first order term, the linearization error is obtained, and the partial derivative of the linearization error to the one-step predicted position of the slave node is calculated to obtain the observation matrix compensation amount, and then the observation matrix is corrected;

[0037] Linearization error is:

[0038]

[0039] Observation matrix compensation amount AH l is:

[0040]

[0041] The modified observation matrix formula is:

[0042]

[0043] Wherein, is a one-step prediction of the observation. is the Jacobian matrix of the observation equation at x l / l-1 , is the first-order term of the Taylor expansion of the observation equation at x l / l-1 . x l / l-1 , y l / l-1 is the east and north position coordinates of the slave node one-step predicted at time l; is the east and north position coordinates of the master node at time l; x l , y l is the east and north position coordinates of the slave node at time l.

[0044] Step eight: taking the relative distance between the master and slave nodes at time l as the observation, the one-step prediction of the slave node position at time l as the state, using the modified observation matrix, performing extended Kalman filtering to obtain the cooperative correction position of the slave node;

[0045] The one-step prediction of the slave node position at time l x l / l-1 is:

[0046]

[0047] The prediction error covariance matrix P l / l-1 is:

[0048]

[0049] The Kalman gain K l is:

[0050]

[0051] The cooperative correction position state estimate is:

[0052]

[0053] The state estimation error covariance matrix P l is:

[0054] P l = (I2 - K l H l )P l / l-1 (12)

[0055] where R l is the covariance matrix of the observation noise η l ; I2 is a 2x2 identity matrix.

[0056] Step nine: return to step two, repeat steps two to eight until the navigation ends;

[0057] The present application has the advantages of:

[0058] The present application provides a master-slave nonlinear compensation collaborative navigation method under acoustic ranging data loss, establishes a collaborative navigation framework under ranging information loss, considers the influence of linearization error on position estimation, and improves the collaborative positioning precision by simultaneously compensating the observation matrix algorithm. BRIEF DESCRIPTION OF DRAWINGS

[0059] Figure 1 Fig. 1 is a flowchart of the master-slave nonlinear compensation collaborative navigation method under acoustic ranging data loss according to the present application;

[0060] Figure 2 Fig. 2 is a navigation track diagram of the master node and the slave node according to the present application;

[0061] Figure 3 Fig. 3 is a collaborative positioning error comparison diagram of the slave node according to the present application;

[0062] Figure 4 Fig. 4 is a linearization error comparison diagram of the observation equation according to the present application;

[0063] Figure 5 Fig. 5 is a diagram showing the influence of linearization error on the collaborative positioning of the slave node according to the present application. DETAILED DESCRIPTION

[0064] In order to facilitate those skilled in the art to understand and implement the present application, the present application will be further described in detail below in combination with the drawings and examples. Obviously, the described examples are only partial examples of the present application, but not all examples, and all other examples obtained by those skilled in the art without creative labor on the basis of the examples in the present application should belong to the protection scope of the present application.

[0065] The present application provides a master-slave nonlinear compensation collaborative navigation method under acoustic ranging data loss, uses the relative distance between the master node and the slave node as the observation quantity, judges whether the observation information is received, and uses the compensation observation matrix extended Kalman filter to online collaboratively correct the position of the slave node.

[0066] As Figure 1 shown, comprising the following steps:

[0067] Step one, for a plurality of autonomous underwater vehicles including a master node and several slave nodes, each node sails according to its own trajectory;

[0068] Step two: each slave node outputs the attitude and velocity information of each slave node at time k-1 in real time;

[0069] Specifically, the body course angle θ k-1 (output by the slave node inertial navigation system) (unit: rad), the forward speed V (output by the Doppler log) and the right speed V (both units: m / s); k≥2.

[0070] Step three: each slave node uses the attitude and velocity information at time k-1 to calculate the position at time k, and performs state prediction on the slave node position at time k, and loops to time l-1;

[0071] l=k+a / b; a is the state update frequency of the slave node position; b is the frequency of the master node sending observation information (its own position and the relative distance between master and slave); b≥a.

[0072] The specific process is as follows:

[0073]

[0074] The position state of the slave node at time k is x k =[x k ,y k ] T , x k , y k are the east and north position coordinates of the slave node in the navigation system; Δt is the data output time interval; T represents the matrix transpose.

[0075] Then, the slave node performs state prediction, and the inferred slave node position is taken as the state variable to establish one-step prediction x k / k-1 of the slave node position state at time k:

[0076]

[0077] The prediction error covariance matrix P k / k-1 is:

[0078]

[0079] The position state estimation is

[0080]

[0081] State estimation error covariance matrix P k is:

[0082] P k = P k / k -1 (5)

[0083] where f(·) is the state transition function; is the position estimate of the slave node at time k-1; is the system input vector from the slave node at time k-1; process noise is the 0-mean Gaussian white noise of the sensor output at time k-1; the noise covariance matrix is P k is the estimation error covariance matrix at time k-1; F k is the Jacobian matrix of the state function at ; G k is the Jacobian matrix of the state function at u k-1 .

[0084]

[0085]

[0086] Step four: the master node is equipped with a global navigation satellite system and an ultra-short baseline, and outputs the position of the master node at time l in real time, and establishes an observation equation of the relative distance between the master node and each slave node;

[0087] An observation equation is established based on the relative distance between the master node and the slave node at time l:

[0088]

[0089] where ||·||2 represents the matrix 2-norm; z l is the observation vector at time l; is the position coordinate of the master node navigation system at time l; h(·) is the observation function; η l is the ranging noise at time l; x l is the position coordinate of the slave node at time l;

[0090] Step five: the master node sends observation information including the position of the master node and the relative distance between the master node and each slave node to each slave node;

[0091] Step six: the slave node judges whether the master node information at time l is received, if the master node information at time l is received, step seven is performed, otherwise, step three is returned;

[0092] A random parameter γ l is established, which conforms to a Bernoulli distribution, and γ l=1, indicating that the observation information at this moment has been received from the node, and step seven is performed; when γ l =0 indicates that the slave node did not receive the observation information at this moment, the acoustic ranging data is lost, and the process returns to step three to predict the state of the slave node's position at the next moment.

[0093] Step 7: After Taylor expansion of the observation equation for the relative distance between the master and slave nodes at time l, the linearization error after retaining the first-order terms is calculated, and the partial derivative of the linearization error with respect to the one-step predicted position of the slave node is calculated to obtain the observation matrix compensation amount, and then the observation matrix is ​​corrected.

[0094] The linearization error is:

[0095]

[0096] It is a one-step prediction of the observed quantity.

[0097] Observation matrix compensation quantity ΔH l for:

[0098]

[0099] The corrected observation matrix formula is as follows:

[0100]

[0101] in, For the observation equation in x l / l-1 Jacobian matrix at the location, The observation equation in x l / l-1 The first-order term in the Taylor expansion. l / l-1 y l / l-1 These are the east and north coordinates predicted one step from the node at time l; The coordinates of the master node's east and north positions at time l; x l y l These are the coordinates of the node's east and north positions at time l.

[0102] Step 8: Using the relative distance between the master and slave nodes at time l as the observation and the one-step prediction of the slave node position at time l as the state variable, use the corrected observation matrix to perform extended Kalman filtering to obtain the cooperative correction position of the slave nodes.

[0103] Position state prediction: The state is predicted in one step as equation (2) at time l, and the prediction error covariance matrix is ​​equation (3);

[0104] Position state update: Kalman gain K l for:

[0105]

[0106] Cooperative correction of position state estimation is:

[0107]

[0108] State estimation error covariance matrix P l is:

[0109] P l = (I2- K l H l ) P l / l-1 (14)

[0110] wherein, K k is the Kalman gain; R l is the covariance matrix of observation noise η l , I2 is a 2*2 unit matrix; (·) -1 is the inverse operation of matrix; is the one-step prediction of the observation.

[0111] Step nine: repeat steps two to eight until the navigation ends.

[0112] Embodiment:

[0113] For a multi-autonomous underwater vehicle composed of only one master node and one slave node, each node navigates according to an arbitrary trajectory; the slave node carries an inertial navigation system and a Doppler log, and outputs real-time attitude and speed information at time k-1, with a frequency of a Hz; position calculation is performed to obtain position information at time k; the master node carries a global navigation satellite system and an ultra-short baseline, and outputs real-time position of the master node itself and relative distance between the master node and the slave node at time l, with a frequency of b Hz;

[0114] Then, the master node sends its own position and the relative distance between the master node and the slave node to the slave node; the slave node judges whether the master node information at time l is received, if not, it indicates that the data is lost, and returns to perform position calculation and state prediction again; if it is received, the linearized error after Taylor expansion of the relative distance equation between the master node and the slave node at time l is calculated, and the partial derivative of the linearized error to the one-step predicted position of the slave node is calculated to obtain the observation matrix compensation, which is used to correct the observation matrix;

[0115] Finally, using the modified observation matrix, the slave node extended Kalman filter cooperative positioning is performed with the relative distance between the master node and the slave node at time l as the observation and the position of the slave node at time l as the state; let k = l + 1, and repeat until the navigation ends;

[0116] MATLAB simulation is performed to verify the invention:

[0117] The motion parameters of the slave node are as follows:

[0118] Initial position: 500 m east, 500 m north;

[0119] Forward velocity: 2 m / s, right velocity: 0 m / s;

[0120] Heading angle: θ s = 0.01 sin(0.4π / 180t).

[0121] Master node motion parameters are as follows:

[0122] Initial position: 300 m east, 500 m north;

[0123] Forward velocity: 2 m / s, right velocity: 0 m / s;

[0124] Heading angle: θ m = 0.2 sin(0.8π / 180t).

[0125] Slave node sensor parameters are as follows:

[0126] Inertial navigation: heading angle error is set to 0 mean Gaussian white noise, variance δ θ = (0.0005°) 2 ;

[0127] Doppler log: velocity error is set to 0 mean Gaussian white noise, variance

[0128] Master node sensor parameters are as follows:

[0129] Ultra-short baseline: distance measurement error is set to 0 mean Gaussian white noise, variance δ r = (0.01 m) 2 .

[0130] Extended Kalman filter parameters are as follows:

[0131] Initial estimation error covariance matrix: P0 = diag[0 m, 0 m] 2 ;

[0132] Process noise covariance matrix: Q k = diag[0.01 m / s, 0.01 m / s, 0.0005°] 2 ;

[0133] Measurement noise covariance matrix: R k = diag[0.01 m] 2 .

[0134] Other parameters are as follows:

[0135] Simulation duration: 2400s; sampling frequency: 5Hz; underwater acoustic communication period: 10s.

[0136] By using the method, the slave node cooperative navigation positioning result is obtained. Figure 2 As shown in the figure, the navigation trajectory diagram of the master node and the slave node is obtained. Figure 3 is the comparison diagram of slave node cooperative positioning error of compensation observation matrix method and extended Kalman filter, from the diagram, it can be known that the cooperative positioning error of the compensation observation matrix method is less than that of the extended Kalman filter, and the positioning error is reduced from 5.18m to 4.03m; Figure 4 is the comparison diagram of observation equation linearization error under the compensation observation matrix method and the extended Kalman filter, from the diagram, it can be known that the linearization error is reduced by 51.68% through the compensation observation matrix method; Figure 5 is the comparison diagram of the influence of linearization error on cooperative navigation positioning under the compensation observation matrix method and the extended Kalman filter, from the diagram, it can be known that the influence of the compensation observation matrix method on cooperative positioning is obviously reduced.

Claims

1. A master-slave nonlinear compensation cooperative navigation method in the case of acoustic ranging data loss, characterized in that, The specific steps are as follows: Step 1: For a multi-autonomous underwater vehicle with a master node and several slave nodes traveling along an arbitrary path, Each slave node outputs attitude and velocity information in real time at all times, and performs... The position was calculated at a given time, and... At each step, a prediction is made based on the node's position and state, and the process is repeated until... time; The position estimation process is as follows: From node Position state at time ; , East and north position coordinates of the from node in the navigation system; Data output time interval; Course angle of the from node output by the inertial navigation system; Forward speed output by the Doppler log; Rightward speed output by the Doppler log; ; Then, the calculated slave node position is taken as a state quantity, and one-step prediction of the slave node position state is established : wherein, is a state transition function; is the position state estimate of the node at the instant is the input vector from the node system at time instant k; process noise is a 0-mean Gaussian white noise of the sensor output at time instant k; Prediction error covariance matrix is: Position state estimation is: State estimation error covariance matrix is: is the Jacobian matrix of the state function at is the state function is the time estimate error covariance matrix, is the Jacobian matrix of the state function at is the process noise covariance matrix;​​ Step two: when The master node outputs its position in real time and establishes the observation equation of the relative distance between the master node and each slave node, and sends the observation information to each slave node. Observation equation: wherein, represents the matrix 2-norm; is is the observation vector at time is is the position coordinate of the master node East at time is the observation function; is is the ranging noise at time is is the position coordinate of the slave node East at time Step three: Establishing random parameters in accordance with Bernoulli distribution , judging whether the slave node receives the observation information of the master node at the moment, if yes, proceeding to step four, otherwise, the acoustic ranging data is lost, then returning to step one to predict the state of the slave node position at the next moment; , judging whether the slave node receives the observation information of the master node at the moment, if yes, proceeding to step four, otherwise, the acoustic ranging data is lost, then returning to step one to predict the state of the slave node position at the next moment; Step four: adding the compensation quantity to the observation matrix After Taylor expansion of the relative distance observation equation of master and slave nodes at the moment and retaining to the first order term, the linearized error is obtained, the partial derivative of the linearized error to the one-step predicted position of the slave node is calculated, the observation matrix compensation quantity is obtained, and then the observation matrix is corrected; linearization error is: Observation matrix compensation amount is: The observation matrix formula is revised as: ; wherein, is a one-step prediction of the observation; is a Jacobian matrix of the observation equation at , is a first order term of a Taylor expansion of the observation equation at , , is east and north position coordinates of the slave node at time step is north position coordinate of the master node at time step is north position coordinate of the slave node at time step Step five: with The relative distance between the master node and the slave node at the moment is the observation, The one-step prediction of the slave node position at the moment is the state quantity, the extended Kalman filter is performed using the modified observation matrix to obtain the cooperative correction position of the slave node; the above steps are repeated until the navigation ends. Kalman gain is: Coordinated correction of position state estimates is: State estimation error covariance matrix is: wherein is an observation noise covariance matrix; is an identity matrix.

2. The master-slave nonlinear compensation cooperative navigation method in the case of acoustic ranging data loss according to claim 1, characterized in that, The step one, ; is the frequency of the node position state update; is the frequency of the master node sending observation information; .

3. The master-slave nonlinear compensation cooperative navigation method in the case of acoustic ranging data loss according to claim 1, characterized in that, The step three, when , indicates that the observation information at is received from the node; when , indicates that the observation information at is not received from the node, and the acoustic ranging data is lost.

4. The master-slave nonlinear compensation cooperative navigation method in the case of acoustic ranging data loss according to claim 1, characterized in that, In step five, A one-step prediction from node positions is: Prediction error covariance matrix is: 。

Images