Cooperative positioning system and method for unmanned underwater vehicle (UUV) cluster based on ranging and information interaction

Abstract

Some embodiments of the disclosure provide a cooperative positioning system and method for an unmanned underwater vehicle (UUV) cluster based on ranging and information interaction. In some examples, the cooperative positioning system includes one master UUV, multiple slave UUVs, and a cooperative positioning apparatus. In some examples, the cooperative positioning method includes following steps. A master UUV and a slave UUV perform internal clock disciplining. The master UUV periodically packages a location of the master UUV and a covariance matrix of location estimation into a cooperative positioning data packet and broadcasts the cooperative positioning data packet to slave UUVs of a cluster. The slave UUV receives the cooperative positioning data packet. A cooperative positioning apparatus of the salve UUV constructs a state and a measurement equation according to data. The cooperative positioning apparatus calculates and obtains a cooperative positioning result by using an extended Kalman filter (EKF).

Claims

1. A cooperative positioning system for an unmanned underwater vehicle (UUV) cluster based on ranging and information interaction, comprising: (1) a master UUV, carrying a Doppler velocity log (DVL), a high-precision inertial navigation system (INS), a first satellite unit module, a first underwater acoustic communication machine, a first main control unit module, a first power system, a first data processing module, and a first clock module, wherein: the DVL is configured to lock water and water bottom and is configured to measure a velocity of the master UUV; the high-precision INS is configured to estimate a location and a corresponding motion posture of the master UUV; the first satellite unit module is configured to receive satellite location update and communication with a control center when the master UUV emerges from sea; the first underwater acoustic communication machine is configured to support underwater communication between the master UUV and a slave UUV and to enable the master UUV to report a status to the control center and receive an instruction and data of the control center; the first main control unit module is configured to control floating, diving, and task switching of the master UUV; the first power system is configured to output power required by the master UUV; the first data processing module is configured to output a location of the master UUV with reference to data of the DVL and the INS and to generate a cooperative positioning data packet; and the first clock module is configured to provide a clock reference; (2) a plurality of slave UUVs, each slave UUV carries a low-precision INS, a second satellite unit module, a second underwater acoustic communication machine, a second main control unit module, a second power system, a second data processing module, and a second clock module; wherein: the low-precision INS is configured to output a positioning result and a corresponding motion posture of the slave UUV; the second satellite unit module is configured to receive satellite location update and communication with the control center when the slave UUV emerges from sea; the second underwater acoustic communication machine is configured to support underwater communication between the slave UUV and the master UUV and to receive the cooperative positioning data packet; the second main control unit module is configured to control floating, diving, and task switching of the slave UUV and has an integrated function of communication and positioning; the second power system is configured to output power required by the slave UUV; the second data processing module is configured to process internal data of the slave UUV; and the second clock module is configured to provide another clock reference; and (3) a cooperative positioning apparatus installed on the slave UUV and configured to execute a cooperative positioning algorithm and to output a cooperative positioning result, wherein: the cooperative positioning apparatus comprises a primary navigation unit and a secondary navigation unit, the primary navigation unit is configured to perform location estimation by using data of the INS of the slave UUV, and the secondary navigation unit is configured to perform filtering processing by using acoustic ranging and the cooperative positioning data packet; the cooperative positioning apparatus provides two interfaces outwards: a communication interface in connection with the data processing module and a communication interface in connection with the underwater acoustic communication machine; interface communication uses a serial communication mode consisting of a ground cable, a transmitting TX, and a receiving RX, and uses a software handshaking XON/XOFF mode to establish a communication connection; and when communication is required, a receive end sends XON to a transmit end to start to receive data, and the transmit end starts to send data after receiving XON; or when no communication is required, the receive end feeds back XOFF to end communication; (3-1) the communication interface in connection with the data processing module: inputs of the slave UUV, at a frequency of 1 Hz by default, longitude, latitude, velocity, heading, depth, and timestamp data that are acquired by the INS to the cooperative positioning apparatus, and the cooperative positioning apparatus outputs longitude and latitude data obtained after cooperative positioning to the slave UUV; information interaction between the cooperative positioning apparatus and the slave UUV is as follows: the cooperative positioning apparatus sends XON to the second data processing module to start to receive the information when needing status information, and sends XOFF to end communication until there is no need; the second data processing module starts to send status information comprising longitude and latitude after receiving XON of the cooperative positioning apparatus, and stops sending until receiving XOFF; the second data processing module sends XON to the cooperative positioning apparatus when needing the cooperative positioning result, and sends XOFF to stop communication until the result is not needed; and the cooperative positioning apparatus starts to send the cooperative positioning result after receiving XON of the slave UUV, and stops sending until receiving XOFF; and (3-2) the communication interface in connection with the underwater acoustic communication machine: after receiving the cooperative positioning data packet of the master UUV, the second underwater acoustic communication machine inputs, to the cooperative positioning apparatus, parsed packet information and a time of arrival that is of the cooperative positioning data packet and that is recorded by the underwater acoustic communication machine; information interaction between the cooperative positioning apparatus and the second acoustic communication receiver is as follows: the cooperative positioning apparatus sends XON to the second underwater acoustic communication machine when needing information received by the second underwater acoustic communication machine, and sends XOFF until the information is not needed; and the second underwater acoustic communication machine receives, after receiving XON, the cooperative positioning data packet of the master UUV and inputs cooperative positioning data packet and a time of arrival of the cooperative positioning data packet to the cooperative positioning apparatus, and stops sending until receiving XOFF.

2. The cooperative positioning system according to claim 1, wherein a working mode of the cooperative positioning system is as follows: the master UUV fuses the data of the DVL and the high-precision INS, outputs a high-precision reference location of the master UUV, packages key information into the cooperative positioning data packet, and broadcasts the cooperative positioning data packet to the slave UUVs in the cluster at a fixed time interval by using the first underwater acoustic communication machine, wherein the key information comprises the location of the master UUV, information about a state covariance matrix, and a sending timestamp; and the low-precision INS of the slave UUV has a high update frequency, and uses data that is acquired by the low-precision INS and that comprises longitude and latitude as that of the primary navigation unit in the cooperative positioning apparatus; the secondary navigation unit in the cooperative positioning apparatus has a low update frequency; after receiving the cooperative positioning data packet of the master UUV, the second underwater acoustic communication machine records a time of arrival of the cooperative positioning data packet, and inputs the time of arrival and parsed content of the cooperative positioning data packet to the cooperative positioning apparatus; and the cooperative positioning apparatus performs cooperative positioning processing according to information input by the second underwater acoustic communication machine, and outputs updated positioning information to the slave UUV to complete cooperative positioning.

3. A cooperative positioning method using the cooperative positioning system according to claim 1, comprising following steps: step (1): performing, by a master UUV and a slave UUV, internal clock disciplining before being launched into water, keeping clock synchronization, and starting underwater work after disciplining is completed; and starting, by a first satellite unit module of the master UUV and a second satellite unit module of the slave UUV, to work, and initializing positioning information of the master UUV and the slave UUV; step (2): after a stable working state is entered, generating, by a first data processing module of the master UUV, a cooperative positioning data packet at a fixed period T, wherein the cooperative positioning data packet comprises longitude L.sub.M,k, latitude B.sub.M,k, a depth d.sub.M,k, a velocity v.sub.M,k, a heading .sub.M,k, a positioning standard deviation in an X direction .sub.Mx,k, a positioning standard deviation in a Y direction .sub.My,k, and a packet time of emission t.sub.k.sup.e, and a subscript M represents the master UUV and a subscript k represents a k.sup.th positioning timeslot; and broadcasting the cooperative positioning data packet to slave UUVs in a cluster; step (3): after receiving the cooperative positioning data packet, recording, by a second underwater acoustic communication machine, a time of arrival t.sub.k.sup.a of the cooperative positioning data packet, parsing information in the cooperative positioning data packet, and processing the longitude and the latitude of the master UUV in the cooperative positioning data packet to convert the longitude and the latitude into an XY coordinate system location (x.sub.M,k,y.sub.M,k)=LB2XY(L.sub.M,k,B.sub.M,k) wherein LB2XY(,) represents a conversion operation between the longitude and the latitude and an XY coordinate system; and obtaining, by a second data processing module, data of an INS, comprising longitude L.sub.S,k, latitude B.sub.S,k, a depth d.sub.S,k, a velocity v.sub.S,k, a heading .sub.S,k, and angular acceleration .sub.S,k that are of the slave UUV, wherein a subscript S represents the slave UUV, processing the longitude and the latitude of the slave UUV in the data to convert the longitude and the latitude into an XY coordinate system location (x.sub.S,k,y.sub.S,k)=LB2XY(L.sub.S,k, B.sub.S,k), and inputting, to a cooperative positioning apparatus, a location of the master UUV, a location of the slave UUV, the parsed information of the cooperative positioning data packet, and remaining output information of the second data processing module of the slave UUV; and step (4): establishing, by the cooperative positioning apparatus, a state transition equation and a measurement equation, and performing filtering processing by using the parsed information in the cooperative positioning data packet to complete cooperative positioning, comprising: step (4-1): establishing a state vector and the state transition equation, wherein: when the slave UUV is in a state of steady state operation, a motion status .sub.S,k of the slave UUV at the k.sup.th positioning timeslot is expressed as a combination .sub.S,k=[x.sub.S,k,y.sub.S,k,.sub.S,k].sup.T of the XY coordinate system location and the heading, wherein [.Math.].sup.T represents a transpose operation; under an update frequency of the fixed period T, an estimation status of the slave UUV at the k.sup.th positioning timeslot, .sub.S,k=[x.sub.S,k,y.sub.S,k,.sub.S,k].sup.T, and a motion equation is expressed as $\{\begin{array}{c}{\stackrel{\_}{x}}_{S,k}=x_{S,k-1}+T^{}{v}_{S,k-1}\cos {}_{S,k-1}\\ {\stackrel{\_}{y}}_{S,k}=y_{S,k-1}+T^{}{v}_{S,k-1}\sin {}_{S,k-1}\\ {\stackrel{\_}{}}_{S,k}={}_{S,k-1}+T^{}{}_{S,k-1}\end{array};$ a current state is combined with a state cached in a preorder to construct a combined state vector .sub.k[.sub.S,k.sup.T, .sub.S,k-1.sup.T, . . . , .sub.S,k-N+1.sup.T].sup.T, wherein N represents a cached order, and a corresponding covariance matrix is P.sub.k; a combined motion prediction model is .sub.k=f(.sub.k-1,u.sub.S,k-1)+n.sub.k, wherein u.sub.S,k-1 represents a motion velocity of the slave UUV as a measurement input, and n.sub.k represents an estimation error, which follows Gaussian distribution; a state equation f(,) is a nonlinear function about the state vector, with a Jacobi matrix of F.sub.k, and linear processing of the prediction model is expressed as .sub.k=F.sub.k.sub.k-1F.sub.k.sup.T+Bu.sub.S,k-1B.sup.T+n.sub.k, wherein B represents an input control matrix; a covariance matrix P.sub.k corresponding to .sub.k is expressed as P.sub.k=F.sub.kP.sub.k-1F.sub.k.sup.T+Q.sub.k, wherein Q.sub.k is an introduced state transition error, $Q_k=\left[\begin{array}{ccc}{\left(T^{}\right)}^2{}_{v_{S,k-1}}^2{\cos }^2{}_{S,k-1}& {\left(T^{}\right)}^2{}_{v_{S,k-1}}^2\cos {}_{S,k-1}\sin {}_{S,k-1}& 0\\ {\left(T^{}\right)}^2{}_{v_{S,k-1}}^2\sin {}_{S,k-1}\cos {}_{S,k-1}& {\left(T^{}\right)}^2{}_{v_{S,k-1}}^2{\sin }^2{}_{S,k-1}& 0\\ 0& 0& {}_{{}_{S,k-1}}^2\end{array}\right],$ .sub.v.sub.S,k and .sub..sub.S,k respectively represent a standard deviation of velocity measurement and a standard deviation of heading measurement of the slave UUV; and after an augmentation operation is completed once on the state vector, .sub.k=[.sub.k.sup.T,.sub.k-1.sup.T].sup.T, and the covariance matrix of the state vector is synchronously augmented and is modified and expressed as ${\stackrel{\_}{P}}_k=\left[\begin{array}{ccc}{Q}_k& -F_k^T{Q}_k& 0\\ -Q_k{F}_k& F_k{Q}_k{F}_k^T+P_{k-1}\left[1,1\right]& P_{k-1}\left[1,2\right]\\ 0& P_{k-1}\left[2,1\right]& P_{k-1}\left[2,2\right]\end{array}\right],$ wherein P.sub.k-1[1,1], P.sub.k-1[1,2], P.sub.k-1[2,1], and P.sub.k-1[2,2] respectively represent upper left, upper right, lower left, and lower right submatrices of a covariance matrix P.sub.k-1; step (4-2): augmenting the state vector, comprising: adding, by the cooperative positioning apparatus, the received information .sub.M,k of the master UUV on a basis of an original status to construct an augmented state vector .sub.k=[.sub.M,k.sup.T, .sub.M,k-2.sup.T, . . . , .sub.M,1.sup.T, .sub.k.sup.T].sup.T, wherein a corresponding covariance matrix is {tilde over (P)}.sub.k, {tilde over (P)}.sub.K=P.sub.k+P.sub.M,kP.sub.M,K1, and P.sub.M,k represents a state covariance matrix of the master UUV; step (4-3): establishing the measurement equation, comprising: calculating a propagation range r.sub.k=c(t.sub.k.sup.at.sub.k.sup.e) according to the parsed time of emission t.sub.k.sup.e and a locally recorded time of arrival t.sub.k.sup.a, wherein c is an equivalent sound velocity; at the k.sup.th positioning timeslot, expressing spatial location information of the slave UUV as (x.sub.S,k, y.sub.S,k, d.sub.S,k), and expressing spatial location information of the master UUV as (x.sub.M,k, y.sub.M,k, d.sub.M,k); and constructing a ranging equation $h\left({\stackrel{}{}}_k\right)=\sqrt{{\left({\stackrel{\_}{x}}_{S,k}-x_{M,k}\right)}^2+{\left({\stackrel{}{y}}_{S,k}-y_{M,k}\right)}^2+{\left(d_{S,k}-d_{M,k}\right)}^2}$ with respect to the state vector, wherein let measurement be affected by a white Gaussian noise .sub.S,k with a zero mean, a measurement model is expressed as z.sub.k=h (.sub.k)+.sub.S,k=r.sub.k, and a measurement deviation matrix is R.sub.k; step (4-4): filtering update, comprising: introducing innovation gain based on an extended Kalman filter (EKF) to absorb innovation, to update a predicted state, wherein an updated predicted state {circumflex over ()}.sub.k=.sub.k+K.sub.k(z.sub.kh(.sub.k)+H.sub.k.sub.k), K.sub.k is innovation gain at a timeslot k, h(.sub.k) represents the ranging equation, and H.sub.k is a Jacobi matrix of the measurement equation; obtaining innovation gain K by using a measurement matrix and a measurement deviation, and K.sub.k={tilde over (P)}.sub.k+H.sub.k.sup.T(H.sub.k{tilde over (P)}.sub.kH.sub.k.sup.T+R.sub.k).sup.1; and synchronizing a covariance matrix {circumflex over (P)}.sub.k=(IK.sub.kH.sub.k){tilde over (P)}.sub.k of the updated state based on the updated state and the innovation gain, wherein I is a unit matrix; and step (4-5): obtaining a positioning result by using ranging information, comprising: extracting an updated location ({circumflex over (x)}.sub.S,k, .sub.S,k) of the slave UUV from {circumflex over ()}.sub.k, and comparing the updated location with (x.sub.S,k, y.sub.S,k) in step (3), wherein when a difference of the two is less than or equal to a set threshold, updating is normal, the updated location is converted into latitude and longitude coordinates ({circumflex over (L)}.sub.S,k,{circumflex over (B)}.sub.S,k)=XY2LB({circumflex over (x)}.sub.S,k,.sub.S,k), and a cooperative positioning result is output, or when the difference between the two is greater than the set threshold, an updated result at this time is discarded; or when the cooperative positioning apparatus fails to receive input data of the underwater acoustic communication machine, extracting a location (x.sub.S,k,y.sub.S,k) of the slave UUV from .sub.k, converting the location into latitude and longitude coordinates (L.sub.S,k,B.sub.S,k)=XY2LB(x.sub.S,k,y.sub.S,k), and outputting a cooperative positioning result.

Description

BRIEF DESCRIPTION OF THE DRAWING

(1) Illustrative embodiments of the present disclosure are described in detail below with reference to the attached drawing FIGURE.

(2) The FIGURE is a schematic diagram of logic of cooperative positioning processing of a master UUV and a slave UUV according to an embodiment of the disclosure.

DETAILED DESCRIPTION

(3) The following describes some non-limiting exemplary embodiments of the invention with reference to the accompanying drawings. The described embodiments are merely a part rather than all of the embodiments of the invention. All other embodiments obtained by a person of ordinary skill in the art based on the embodiments of the disclosure shall fall within the scope of the disclosure.

(4) In some embodiments, a cooperative positioning system for a UUV cluster based on ranging and information interaction may include a master UUV, a slave UUV, and a cooperative positioning apparatus.

(5) Optionally, the cooperative positioning system may include the following components.

(6) (1) One master UUV, which may be an AUV or the like, carrying a DVL, a high-precision INS, a first satellite unit module, a first underwater acoustic communication machine, a first main control unit module, a first power system, a first data processing module, and a first clock module.

(7) The DVL locks the water and the water bottom, and is configured to measure a velocity of the master UUV.

(8) The high-precision INS is configured to estimate a location and a corresponding motion posture of the master UUV.

(9) The first satellite unit module is configured to receive satellite location update and communication with a control center when the master UUV emerges from the sea.

(10) The first underwater acoustic communication machine is configured to support underwater communication between the master UUV and the slave UUV, and enable the master UUV to report a status to the control center and receives an instruction and data of the control center

(11) The first main control unit module is configured to control floating, diving, and task switching of the master UUV.

(12) The first power system is configured to output power required by the master UUV.

(13) The first data processing module is configured to output a location of the master UUV with reference to data of the DVL and the INS, and generate a cooperative positioning data packet.

(14) The first clock module is configured to provide a clock reference.

(15) (2) three slave UUVs, which may be small-scale AUVs and the like. Each slave UUV carries a low-precision INS, a second satellite unit module, a second underwater acoustic communication machine, a second main control unit module, a second power system, a second data processing module, and a second clock module.

(16) The low-precision INS outputs a positioning result and a corresponding motion posture of the slave UUV.

(17) The second satellite unit module is configured to receive satellite location update and communication with the control center when the slave UUV emerges from the sea.

(18) The second underwater acoustic communication machine is configured to support underwater communication between the slave UUV and the master UUV, and configured to receive the cooperative positioning data packet.

(19) The second main control unit module is configured to control floating, diving, and task switching of the slave UUV, and has an integrated function of communication and positioning.

(20) The second power system is configured to output power required by the slave UUV.

(21) The second data processing module is configured to process internal data of the slave UUV.

(22) The second clock module is configured to provide another clock reference.

(23) The high-precision INS carried by the master UUV and the low-precision INS carried by the slave UUV have relatively high and low precision.

(24) (3) the cooperative positioning apparatus, installed on the slave UUV, configured to execute a cooperative positioning algorithm and output a cooperative positioning result.

(25) The cooperative positioning apparatus may include a primary navigation unit and a secondary navigation unit, the primary navigation unit performs location estimation by using data of the INS of the slave UUV, and the secondary navigation unit performs filtering processing by using acoustic ranging and the cooperative positioning data packet.

(26) The cooperative positioning apparatus provides two interfaces outwards: a communication interface in connection with the data processing module and a communication interface in connection with the underwater acoustic communication machine. Interface communication uses a serial communication mode consisting of a ground cable, a transmitting TX, and a receiving RX, and uses a software handshaking XON/XOFF mode to establish a communication connection. When communication is required, a receive end sends XON to a transmit end to start to receive data, and the transmit end starts to send data after receiving XON. Or when no communication is required, the receive end feeds back XOFF to end communication.

(27) (3-1) The communication interface in connection with the data processing module: the slave UUV inputs, at a frequency of 1 Hz by default, longitude, latitude, velocity, heading, depth, and timestamp data that are acquired by the INS to the cooperative positioning apparatus, and the cooperative positioning apparatus outputs longitude and latitude data obtained after cooperative positioning to the slave UUV.

(28) Information interaction between the cooperative positioning apparatus and the slave UUV is as follows: the cooperative positioning apparatus sends XON to the second data processing module to start to receive the information when needing status information, and sends XOFF to end communication until there is no need. The second data processing module starts to send status information including longitude and latitude after receiving XON of the cooperative positioning apparatus, and stops sending until receiving XOFF. The second data processing module sends XON to the cooperative positioning apparatus when needing the cooperative positioning result, and sends XOFF to stop communication until the result is not needed. The cooperative positioning apparatus starts to send the cooperative positioning result after receiving XON of the slave UUV, and stops sending until receiving XOFF.

(29) (3-2) The communication interface in connection with the underwater acoustic communication machine: after receiving the cooperative positioning data packet of the master UUV, the second underwater acoustic communication machine inputs, to the cooperative positioning apparatus, parsed packet information and a time of arrival that is of the cooperative positioning data packet and that is recorded by the underwater acoustic communication machine.

(30) Information interaction between the cooperative positioning apparatus and the second acoustic communication receiver is as follows: the cooperative positioning apparatus sends XON to the second underwater acoustic communication machine when needing information received by the second underwater acoustic communication machine, and sends XOFF until the information is not needed. The second underwater acoustic communication machine receives, after receiving XON, the cooperative positioning data packet of the master UUV and inputs cooperative positioning data packet and a time of arrival of the cooperative positioning data packet to the cooperative positioning apparatus, and stops sending until receiving XOFF.

(31) A working mode of the cooperative positioning system is as follows:

(32) The master UUV fuses the data of the DVL and the high-precision INS, outputs a high-precision reference location of the master UUV, packages key information into the cooperative positioning data packet, and broadcasts the cooperative positioning data packet to the slave UUVs in the cluster at a fixed time interval by using the first underwater acoustic communication machine. The key information may include the location of the master UUV, information about a state covariance matrix, and a sending timestamp.

(33) The low-precision INS of the slave UUV has a high update frequency, and uses data that is acquired by the low-precision INS and that may include longitude and latitude as that of the primary navigation unit in the cooperative positioning apparatus. The secondary navigation unit in the cooperative positioning apparatus has a low update frequency. After receiving the cooperative positioning data packet of the master UUV, the second underwater acoustic communication machine records a time of arrival of the cooperative positioning data packet, and inputs the time of arrival and parsed content of the cooperative positioning data packet to the cooperative positioning apparatus. The cooperative positioning apparatus performs cooperative positioning processing according to information input by the second underwater acoustic communication machine, and outputs updated positioning information to the slave UUV to complete cooperative positioning.

(34) The FIGURE shows logic of cooperative positioning processing of a master UUV and a slave UUV. As shown in the FIGURE, three slave UUVs use a same cooperative positioning method. For an i.sup.th slave UUV. i{1, 2, 3}. The cooperative positioning method may include following steps. Step (1): The master UUV (expressed by a subscript M) and the i.sup.th slave UUV (expressed by a subscript Si) perform internal clock disciplining before being launched into water, keep clock synchronization, and start underwater work after disciplining is completed. A first satellite unit module and a second satellite unit module start to work, and respectively initialize positioning information of the master UUV and the i.sup.th slave UUV. Step (2): After a stable working state is entered, a first data processing module of the master UUV generates a cooperative positioning data packet at a fixed period T. At a k.sup.th positioning timeslot, the cooperative positioning data packet may include longitude L.sub.M,k, latitude B.sub.M,k, a depth d.sub.M,k, a velocity v.sub.M,k, a heading .sub.M,k, a positioning standard deviation in an X direction .sub.Mx,k, a positioning standard deviation in a Y direction .sub.My,k, and a packet time of emission t.sub.k.sup.e. Broadcasts the cooperative positioning data packet to slave UUVs in a cluster.

(35) The cooperative positioning data packet may include a 1-byte start header SYN of the data packet, a 1-byte valid packet length LEN, 4-byte longitude M_LON, 4-byte latitude M_LAT, a 2-byte depth M_DEP, a 2-byte velocity M_VEL, a 2-byte heading M_HEAD, a 4-byte positioning standard deviation in an X direction and in a Y direction M_UNCX and M_UNCY, a 4-byte time of emission M_TIMET, and 2-byte frame check code FCC. A format of the cooperative positioning data packet is shown in the following table, and is then broadcast to the slave UUVs in the cluster.

(36) TABLE-US-00001 SYN LEN M_LON M_LAT M_DEP M_VEL M_HEAD M_UNCX M_UNCY M_TIMET FCC 1 1 4 4 2 2 2 2 2 4 2

(37) Step (3): A second data processing module of the i.sup.th slave UUV obtains data of an INS, processes longitude and latitude of the slave UUV in the data to convert the longitude and the latitude into an XY coordinate system location (x.sub.Si,k, y.sub.Si,k)=LB2XY(L.sub.Si,k,B.sub.Si,k) LB2XY( ) represents a conversion operation between the longitude and the latitude and an XY coordinate system, and packages and inputs the location and remaining information to the cooperative positioning apparatus. A format of data input from the second data processing module of the slave UUV to the cooperative positioning processing apparatus is shown in the table below, including 4-byte longitude INS_LON, 4-byte latitude INS_LAT, a 2-byte depth INS_DEP, a 2-byte velocity INS_VEL, a 2-byte heading INS_HEAD, and a 4-byte time INS_TIME.

(38) TABLE-US-00002 SYN INS_LON INS_LAT INS_DEP INS_VEL INS_HEAD INS_TIME 1 4 4 2 2 2 4

(39) After receiving the positioning packet, an underwater acoustic communication machine of the i.sup.th slave UUV records a time of arrival t.sub.i,k.sup.a of the positioning packet, parses information in the cooperative positioning data packet, processes the longitude and the latitude of the master UUV in the cooperative positioning data packet to convert the longitude and the latitude into an XY coordinate system location (x.sub.M,k, y.sub.M,k)=LB2XY(L.sub.M,k, B.sub.M,k), and inputs packaged data to the cooperative positioning processing apparatus after data packaging is completed. A format of the data input from an acoustic communication machine to the cooperative positioning processing apparatus is shown in the table below, including the 4-byte longitude M_LON, the 4-byte latitude M_LAT, the 2-byte depth M_DEP, the 2-byte velocity M_VEL, the 2-byte heading M_HEAD, the 4-byte positioning standard deviation in the X direction and in the Y direction M_UNCX and M_UNCY, the 4-byte time of emission M_TIMET, and a 4-byte time of arrival M_TIMER that are of the master UUV and that are obtained through parsing.

(40) TABLE-US-00003 SYN M_LON M_LAT M_DEP M_VEL M_HEAD M_UNCX M_UNCY M_TIMET M_TIMER 1 4 4 2 2 2 2 2 4 4 Step (4): The cooperative positioning apparatus establishes a state transition equation and a measurement equation, and performs filtering processing by using the parsed information in the cooperative positioning data packet to complete cooperative positioning. The method may include following steps. Step (4-1): establishing a state vector and the state transition equation.

(41) When the i.sup.th slave UUV is in a state of steady state operation, a motion status .sub.Si,k of the slave UUV at the k.sup.th positioning timeslot is expressed as a combination .sub.Si,k=[x.sub.Si,k, y.sub.Si,k, .sub.Si,k].sup.T of the XY coordinate system location and the heading. [.Math.].sup.T represents a transpose operation.

(42) Under an update frequency of the fixed period T, an estimation status of the i.sup.th slave UUV at the k.sup.th positioning timeslot .sub.Si,k=[x.sub.Si,k, y.sub.Si,k, .sub.Si,k].sup.T, and a motion equation is expressed as

(43) $\{\begin{array}{c}{\stackrel{\_}{x}}_{\mathrm{Si},k}=x_{\mathrm{Si},k-1}+T^{}{v}_{\mathrm{Si},k-1}\cos {}_{\mathrm{Si},k-1}\\ {\stackrel{\_}{y}}_{\mathrm{Si},k}=y_{\mathrm{Si},k-1}+T^{}{v}_{\mathrm{Si},k-1}\sin {}_{\mathrm{Si},k-1}\\ {\stackrel{\_}{}}_{\mathrm{Si},k}={}_{\mathrm{Si},k-1}+T^{}{}_{\mathrm{Si},k-1}\end{array}.$

(44) A current state is combined with a state cached in a preorder to construct a combined state vector .sub.i,k=[.sub.Si,k.sup.T, .sub.Si,k-1.sup.T, .sub.Si,k-N+1.sup.T].sup.T. N represents a cached order, and a corresponding covariance matrix is P.sub.i,k. A combined motion prediction model .sub.i,k=f(.sub.i,k-1,u.sub.Si,k-1)+n.sub.i,k. u.sub.Si,k-1 represents a motion velocity of the slave UUV as a measurement input, and n.sub.k represents an estimation error, which follows Gaussian distribution. A state equation f(,) is a nonlinear function about the state vector, with a Jacobi matrix of F.sub.i,k, and linear processing of the prediction model is expressed as .sub.i,k=F.sub.i,k.sub.i,k-1F.sub.i,k.sup.T+B.sub.iu.sub.Si,k-1B.sub.i.sup.T+n.sub.i,k. B.sub.i represents an input control matrix.

(45) A covariance matrix P.sub.k corresponding to .sub.k is expressed as P.sub.i,k=F.sub.i,kP.sub.i,k-1F.sub.i,k.sup.T+Q.sub.i,k. Q.sub.i,k is an introduced state transition error,

(46) $Q_{i,k}=\left[\begin{array}{ccc}{\left(T^{}\right)}^2{}_{v_{\mathrm{Si},k-1}}^2{\cos }^2{}_{\mathrm{Si},k-1}& {\left(T^{}\right)}^2{}_{v_{\mathrm{Si},k-1}}^2\cos {}_{\mathrm{Si},k-1}\sin {}_{\mathrm{Si},k-1}& 0\\ {\left(T^{}\right)}^2{}_{v_{\mathrm{Si},k-1}}^2\sin {}_{\mathrm{Si},k-1}\cos {}_{\mathrm{Si},k-1}& {\left(T^{}\right)}^2{}_{v_{\mathrm{Si},k-1}}^2{\sin }^2{}_{\mathrm{Si},k-1}& 0\\ 0& 0& {}_{{}_{\mathrm{Si},k-1}}^2\end{array}\right],$
.sub.v.sub.Si,k and .sub..sub.Si,k respectively represent a standard deviation of velocity measurement and a standard deviation of heading measurement of the slave UUV.

(47) After an augmentation operation is completed once on the state vector, .sub.i,k=[.sub.i,k.sup.T, .sub.i,k-1].sup.T, and the covariance matrix of the state vector is synchronously augmented and is modified and expressed as

(48) ${\stackrel{\_}{P}}_{i,k}=\left[\begin{array}{ccc}{Q}_{i,k}& -F_{i,k}^T{Q}_{i,k}& 0\\ -Q_{i,k}{F}_{i,k}& F_{i,k}{Q}_{i,k}{F}_{i,k}^T+P_{i,k-1}\left[1,1\right]& P_{i,k-1}\left[1,2\right]\\ 0& P_{i,k-1}\left[2,1\right]& P_{i,k-1}\left[2,2\right]\end{array}\right].$
P.sub.i,k-1[1,1], P.sub.i,k-1[1,2], P.sub.i,k-1[2,1] and P.sub.i,k-1[2,2] respectively represent upper left, upper right, lower left, and lower right submatrices of a covariance matrix P.sub.i,k-1. Step (4-2): augmenting the state vector, including:

(49) The cooperative positioning apparatus adds the received information .sub.M,k of the master UUV on a basis of an original status to construct an augmented state vector .sub.i,k=[.sub.M,k.sup.T, .sub.M,k-2.sup.T, . . . , .sub.M,1.sup.T, .sub.i,k.sup.T].sup.T. A corresponding covariance matrix is {tilde over (P)}.sub.i,k, {tilde over (P)}.sub.i,k=P.sub.i,k+P.sub.M,kP.sub.M,k-1, and P.sub.M,k represents a state covariance matrix of the master UUV. Step (4-3): establishing the measurement equation, including:

(50) Calculating a propagation range r.sub.i,k=c(t.sub.i,k.sup.at.sub.k.sup.e) according to the parsed time of emission t.sub.k.sup.e and the locally recorded time of arrival t.sub.i,k.sup.a. c is an equivalent sound velocity.

(51) At the k.sup.th positioning timeslot, expressing spatial location information of the i.sup.th slave UUV as (x.sub.Si,k, y.sub.Si,k, d.sub.Si,k), and expressing spatial location information of the master UUV as (x.sub.M,K, y.sub.M,k, d.sub.M,k).

(52) Constructing a ranging equation

(53) $h\left({\stackrel{}{}}_{i,k}\right)=\sqrt{{\left({\stackrel{\_}{x}}_{\mathrm{Si},k}-x_{M,k}\right)}^2+{\left({\stackrel{}{y}}_{\mathrm{Si},k}-y_{M,k}\right)}^2+{\left(d_{\mathrm{Si},k}-d_{M,k}\right)}^2}$
with respect to the state vector. Let measurement be affected by a white Gaussian noise .sub.Si,k with a zero mean, a measurement model is expressed as z.sub.i,k=h(.sub.i,k)+.sub.Si,k=r.sub.i,k, and a measurement deviation matrix is R.sub.i,k. Step (4-4): filtering update.

(54) Because measurement about the state vector refers to a nonlinear function, linear processing may be performed on measurement to obtain a Jacobi matrix H of measurement, including: introducing innovation gain K based on an EKF with reference to Step (4-1) to Step (4-3) to absorb innovation, to update a predicted state {circumflex over ()}.sub.i,k=.sub.i,k+K.sub.i,k(z.sub.i,kh(.sub.i,k)+H.sub.i,k.sub.i,k). {circumflex over ()}.sub.i,k is an updated predicted state, K.sub.i,k is innovation gain at a timeslot k, h(.sub.i,k) represents the ranging equation, and H.sub.i,k is a Jacobi matrix of the measurement equation.

(55) Obtaining the innovation gain K by using a measurement matrix and a measurement deviation, and K.sub.i,k={tilde over (P)}.sub.i,k+H.sub.i,k.sup.T(H.sub.i,k{tilde over (P)}.sub.i,kH.sub.i,k.sup.T+R.sub.i,k).sup.1.

(56) Synchronizing a covariance matrix {circumflex over (P)}.sub.i,k=(IK.sub.i,kH.sub.i,k){tilde over (P)}.sub.i,k of the updated state based on the updated state and the innovation gain. I is a unit matrix. Step (4-5): obtaining a positioning result by using ranging information, including:

(57) Extracting an updated location ({circumflex over (x)}.sub.Si,k, .sub.Si,k) of the i.sup.th slave UUV from {circumflex over ()}.sub.i,k, and comparing the updated location with (x.sub.Si-INS,k,y.sub.Si-INS,k). In Step (3). When a difference of the two is less than a set threshold, updating is normal, the updated location is converted into latitude and longitude coordinates ({circumflex over (L)}.sub.Si,k,{circumflex over (B)}.sub.Si,k)=XY2LB({circumflex over (x)}.sub.Si,k,.sub.Si,k), and a cooperative positioning result is output, or when the difference between the two is greater than the set threshold, an updated result at this time is discarded. Or when the cooperative positioning apparatus fails to receive input data of the underwater acoustic communication machine, extracting a location (x.sub.Si,k,y.sub.Si,k) of the slave UUV from .sub.i,k, converting the location into latitude and longitude coordinates (L.sub.Si,k,B.sub.Si,k)=XY2LB(x.sub.Si,k,y.sub.Si,k), and outputting a cooperative positioning result.

(58) Various embodiments of the disclosure may have one or more of the following effects. In some embodiments, the disclosure provides a cooperative positioning system for a UUV cluster based on ranging and information interaction for an existing requirement of cooperative positioning of a master-slave UUV cluster. When positioning uncertainty of a master vehicle is considered, cooperative positioning in the cluster is performed based on ranging and information interaction. In other embodiments, the disclosure may implement a connotation of cooperative positioning that if master and slave underwater vehicles may implement information sharing with each other, the slave vehicle may improve navigation and positioning precision of the slave vehicle by using positioning information of the master vehicle, thereby improving overall positioning capability of a cluster. In further embodiments, a cooperative positioning model between a master vehicle and a slave vehicle may be constructed by using ranging and information interaction between the vehicles, and overall positioning performance of the cluster may be improved with reference to a filtering method. In comparison with conventional methods, in the present disclosure, positioning uncertainty of the master UUV may be considered. The master UUV may package the key information into the cooperative positioning data packet and may send the cooperative positioning data packet to the slave UUV. The slave UUV may establish a new state vector by using both state information of the master UUV and the state information of the slave UUV, update the state vector after receiving the cooperative positioning data packet, and obtain the cooperative positioning result, which may further reduce impact of the positioning uncertainty of the master UUV on overall positioning performance of the cluster.

(59) Many different arrangements of the various components depicted, as well as components not shown, are possible without departing from the spirit and scope of the present disclosure. Embodiments of the present disclosure have been described with the intent to be illustrative rather than restrictive. Alternative embodiments will become apparent to those skilled in the art that do not depart from its scope. A skilled artisan may develop alternative means of implementing the aforementioned improvements without departing from the scope of the present disclosure.

(60) It will be understood that certain features and subcombinations are of utility and may be employed without reference to other features and subcombinations and are contemplated within the scope of the claims. Unless indicated otherwise, not all steps listed in the various FIGURES need be carried out in the specific order described.