COMMUNICATION DELAY COMPENSATION METHOD AND SYSTEM BASED ON AUTONOMOUS ROBOT

Abstract

The disclosure provides a communication delay compensation method and a communication delay compensation system based on an autonomous robot, where the method includes the following steps: establishing a state equation based on a system model of an AUV positioning system; acquiring an included angle between a direction vector of AUV to an observation station and a velocity vector of AUV based on the system model; establishing an observation equation according to the state equation and the included angle; establishing an extended Kalman filter equation based on the system model, the included angle and the observation equation; and calculating a position information predicted value at the current time by using the extended Kalman filter equation to complete communication delay compensation.

Claims

1. A communication delay compensation method based on an autonomous robot, comprising following steps: establishing a state equation based on a system model of an AUV positioning system; acquiring an included angle between a direction vector of AUV to an observation station and a velocity vector of AUV based on the system model; establishing an observation equation according to the state equation and the included angle; establishing an extended Kalman filter equation based on the system model, the included angle and the observation equation; and calculating a position information predicted value at a current time by using the extended Kalman filter equation to complete communication delay compensation.

2. The communication delay compensation method based on the autonomous robot according to claim 1, wherein the state equation comprises: $X_k=f\left(X_{k-1},u_{k-1}\right)+{}_{k-1}{W}_k;$ in the formula, X.sub.k represents an n-dimensional state vector at k point of time; X.sub.k-1 represents an n-dimensional state vector at k-1 point of time; u.sub.k-1 represents a system input; .sub.k-1 represents a system noise input matrix from k-1 point of time to k point of time; W.sub.k represents a system noise vector at k-1 point of time; and f represents a nonlinear state function of a system state.

3. The communication delay compensation method based on the autonomous robot according to claim 1, wherein a method for acquiring the included angle comprises following steps: establishing a Doppler measurement equation according to the system model, and acquiring the included angle through a backward deduction of the Doppler measurement equation; $\{\begin{array}{c}{\stackrel{}{f}}_k=f_k+e_k\\ f_k=f_s\left(1+\frac{v_k^T\left(u-s_k\right)}{c.Math.u-s_k.Math.}\right)=f_s\left(1+\frac{.Math.v_k.Math.}{c}\cos {}_k\right)\end{array},$ wherein f.sub.s represents a frequency of an acoustic signal emitted by a target; f.sub.k represents a frequency of an acoustic signal theoretically received by the target; {tilde over (f)}.sub.k represents a frequency of an acoustic signal actually received by the target; e.sub.k represents frequency noise; c represents a propagation speed of a propagation signal in water; v.sub.k represents a driving speed of AUV; u represents a position of the observation station; s.sub.k represents a position of AUV at k point of time; .sub.k represents the included angle between the direction vector of AUV to the observation station and the velocity vector of AUV; T represents a vector transposed symbol.

4. The communication delay compensation method based on the autonomous robot according to claim 3, wherein the observation equation comprises: $Z_k^{}=d_1^2+d_t^2-2d_1^2{d}_t^2\cos {}_k,$ wherein d represents a distance of AUV from the observation station at a last point of time; .sub.t represents a communication delay time; and d.sub.t represents a distance traveled by AUV within .sub.t duration.

5. The communication delay compensation method based on the autonomous robot according to claim 4, wherein a method for calculating the position information predicted value comprises: according to distance information between AUV and the observation station, calculating position information of AUV, acquiring a position information measured value of AUV at the current time, and at the same time, acquiring a position information predicted value of AUV at the last time and a position information prediction error of AUV at the last time; calculating a position information observed value of AUV at the current time by using the observation equation and according to the position information measured value of AUV at the current time; and calculating the position information predicted value of AUV at the current time by using the extended Kalman filter equation and according to the position information observed value of AUV at the current time, the position information predicted value of AUV at the last time and the position information prediction error of AUV at the last time, wherein the position information predicted value of AUV at the current time is the position information predicted value.

6. The communication delay compensation method based on the autonomous robot according to claim 5, wherein calculating a next position information predicted value by using the extended Kalman filter equation according to the position information predicted value of AUV at the last time, wherein a step comprises: ${\stackrel{}{X}}_{k|k-1}=f\left({\stackrel{}{X}}_{k-1},u_{k-1}\right),$ wherein {circumflex over (X)}.sub.k|k-1 represents the next position information predicted value; {circumflex over (X)}.sub.k-1 is the position information predicted value of AUV; calculating a next position information prediction error by using the extended Kalman filter equation and according to the position information prediction error of AUV at the last time, wherein a step comprises: $P_{k|k-1}={}_{k|k-1}{P}_{k-1}{}_{k|k-1}^T+{}_{k-1}{Q}_{k-1}{}_{k-1}^T,$ wherein P.sub.k|k-1 represents the next position information prediction error; P.sub.k-1 represents the position information prediction error of AUV at the last time; Q.sub.k-1 represents a position observation error at the last time; .sub.k|k-1.sup.T represents a state transition matrix, T represents a vector transposed symbol, and .sub.k-1 represents a system noise input matrix; calculating a filtering gain according to the next position information prediction error by using the extended Kalman filter equation, wherein a step comprises: $K_k=P_{k|k-1}{H_k^T\left(H_k{P}_{k|k-1}{H}_k^T+R_k\right)}^{-1},$ wherein K.sub.k represents the filtering gain; P.sub.k|k-1 represents the next position information prediction error; H.sub.k represents an observation matrix at time k; and R.sub.k represents a system noise error; calculating the position information predicted value at the current time by using the extended Kalman filter equation and according to the next position information predicted value, the filtering gain and the position information observed value at the current time, wherein a step comprises: ${\stackrel{}{X}}_k={\stackrel{}{X}}_{k|k-1}+K_k\left(Z_k-{\stackrel{}{Z}}_{k|k-1}\right),$ wherein {circumflex over (X)}.sub.k represents position information predicted value at the current time; {circumflex over (X)}.sub.k|k-1 represents the next position information predicted value; K.sub.k represents the filtering gain, Z.sub.k represents a position observation predicted value at the current time; and {circumflex over (Z)}.sub.k|k-1 represents the position information predicted value of AUV at the last time; calculating the position information prediction error at the current time by using the extended Kalman filter equation and according to the next position information prediction error, wherein a step comprises: $P_k=\left(I-K_k{H}_k\right)P_{k|k-1},$ wherein P.sub.k represents the position information prediction error at the current time; I represents unit matrix; H.sub.k represents the observation matrix at time k; and P.sub.k|k-1 represents the next position information prediction error.

7. A communication delay compensation system based on an autonomous robot, comprising: an AUV positioning system, further comprising a first construction module, an acquisition module, a second construction module, a third construction module and a prediction module; the first construction module is used for establishing a state equation based on a system model of an AUV positioning system; the acquisition module is used for acquiring an included angle between a direction vector of AUV to an observation station and a velocity vector of AUV based on the system model; the second construction module is used for establishing an observation equation according to the state equation and the included angle; the third construction module is used for establishing an extended Kalman filter equation based on the system model, the included angle and the observation equation; and the prediction module is used for calculating a position information predicted value at the current time by using the extended Kalman filter equation to complete communication delay compensation.

8. The communication delay compensation system based on the autonomous robot according to claim 7, wherein the state equation comprises: $X_k=f\left(X_{k-1},u_{k-1}\right)+{}_{k-1}{W}_k,$ wherein X.sub.k represents an n-dimensional state vector at k point of time; X.sub.k-1 represents an n-dimensional state vector at k-1 point of time; u.sub.k-1 represents a system input; .sub.k-1 represents a system noise input matrix from k-1 point of time to k point of time; W.sub.k represents a system noise vector at k-1 point of time; and f represents a nonlinear state function of a system state.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to explain the technical scheme of this disclosure more clearly, the drawings needed in the embodiments are briefly introduced below. Obviously, the drawings in the following description are only some embodiments of this disclosure. For ordinary technicians in this field, other drawings may be obtained according to these drawings without paying creative labor.

[0045] FIG. 1 is a flow chart of a method according to an embodiment of the present disclosure.

[0046] FIG. 2 is a schematic diagram of communication delay of a positioning system according to an embodiment of the present disclosure.

[0047] FIG. 3 is a motion diagram of Autonomous Underwater Vehicle (AUV) according to an embodiment of the present disclosure.

[0048] FIG. 4 is a geometric diagram of a positioning target based on Doppler measurement according to an embodiment of the present disclosure.

[0049] FIG. 5 is a structural schematic diagram of the system according to an embodiment of the present disclosure.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0050] In the following, the technical scheme in the embodiment of the disclosure will be clearly and completely described with reference to the drawings in the embodiment of the disclosure. Obviously, the described embodiment is only a part of the embodiment of the disclosure, but not the whole embodiment. Based on the embodiments in this disclosure, all other embodiments obtained by ordinary technicians in this field without creative work belong to the protection scope of this disclosure.

[0051] In order to make the above objects, features and advantages of this disclosure more obvious and easier to understand, the disclosure will be further described in detail with the attached drawings and specific embodiments.

Embodiment 1

[0052] As shown in FIG. 1, FIG. 1 is a flow chart of a method according to an embodiment of the present disclosure, and steps of the method include: [0053] S1, establishing a state equation based on a system model of Autonomous Underwater Vehicle (AUV) positioning system. [0054] based on the system model of AUV, establishing the state equation:

[00010]$X_k=f\left(X_{k-1},u_{k-1}\right)+{}_{k-1}{W}_k,$ [0055] in the formula, X.sub.k represents an n-dimensional state vector at k point of time; X.sub.k-1 represents an n-dimensional state vector at k-1 point of time; u.sub.k-1 represents a system input; .sub.k-1 represents a system noise input matrix from k-1 point of time to k point of time; W.sub.k represents a system noise vector at k-1 point of time; and f represents a nonlinear state function of a system state.

[0056] In this embodiment, considering that the communication delay in the underwater positioning system may affect the positioning accuracy, if the distance between the AUV and the observation station is directly substituted into the observation equation as measurement information, a large error is introduced to affect the navigation performance. In this scenario, this embodiment introduces Doppler measurement information and uses an EKF filtering method based on a reconstructed measurement equation to compensate the navigation error caused by the delay. The communication delay of the positioning system is shown in FIG. 2. [0057] S2, acquiring an included angle between a direction vector of AUV to an observation station and a velocity vector of AUV based on the system model (the motion schematic of AUV is shown in FIG. 3) is obtained, including the following steps: [0058] as shown in FIG. 4, a Doppler measurement equation of AUV is obtained based on a geometric diagram of the target positioning by the mobile sensor with Doppler measurement.

[00011]$\{\begin{array}{c}{\stackrel{}{f}}_k=f_k+e_k\\ f_k=f_s\left(1+\frac{v_k^T\left(u-s_k\right)}{c.Math.u-s_k.Math.}\right)=f_s\left(1+\frac{.Math.v_k.Math.}{c}\cos {}_k\right)\end{array},$ [0059] where f.sub.s represents a frequency of an acoustic signal emitted by a target; f.sub.k represents a frequency of an acoustic signal theoretically received by the target; {tilde over (f)}.sub.k represents a frequency of an acoustic signal actually received by the target; e.sub.k represents frequency noise; c represents a propagation speed of a propagation signal in water; v.sub.k represents a driving speed of AUV; u represents a position of the observation station; s.sub.k represents a position of AUV at k point of time; .sub.k represents the included angle between the direction vector of AUV to the observation station and the velocity vector of AUV; T represents a vector transposed symbol.

[0060] Then, based on the transceiver frequency, an included angle of AUV is deduced from the Doppler measurement equation:

[00012]${}_k=\arccos \left(\frac{c}{.Math.v_k.Math.}\left(\frac{f_k}{f_s}-1\right)\right).$

[0061] Finally, according to the included angle, the measurement equation of EKF is reconstructed, and an updated observation equation is:

[00013]$Z_k^{}=d_1^2+d_t^2-2d_1^2{d}_t^2\cos {}_k,$ [0062] where Z.sub.k represents an Euclidean distance between AUV and the observation station at time t2, d represents a distance of AUV from the observation station at a last point of time; .sub.t represents a communication delay time; and d.sub.t represents a distance traveled by AUV within .sub.t duration, and .sub.k represents the included angle between the direction vector of AUV to the observation station and the velocity vector of AUV.

[0063] Thus, more accurate position information of AUV at a signal receiving time t2 is obtained, and compensation for communication delay positioning error is realized. [0064] S3, establishing an observation equation according to the state equation and the included angle:

[00014]$Z_k^{}=d_1^2+d_t^2-2d_1^2{d}_t^2\cos {}_k;$ [0065] in the formula, d represents a distance of AUV from the observation station at a last point of time; .sub.t represents a communication delay time; and d.sub.t represents a distance traveled by AUV within .sub.t duration.

[0066] Using the observation equation, calculating the distance observation value at the current time;

[00015]$z_k=h\left({\stackrel{}{X}}_{k|}\right)+{}_k^p,$ [0067] where z.sub.k represents an observation vector at time k, h({circumflex over (X)}.sub.k|) represents a nonlinear function of a system observation vector, and .sub.k.sup.p represents an observation noise vector at time k. [0068] S4, establishing an extended Kalman filter equation based on the system model, the included angle and the observation equation. The observation equation is updated according to the Doppler measurement information. [0069] S5, calculating a position information predicted value at the current time by using the extended Kalman filter equation to complete communication delay compensation.

[0070] According to distance information between AUV and the observation station, calculating position information of AUV, acquiring a position information measured value of AUV at the current time, and at the same time, acquiring a position information predicted value of AUV at the last time and a position information prediction error of AUV at the last time; [0071] calculating a position information observed value of AUV at the current time by using the observation equation and according to the position information measured value of AUV at the current time; and [0072] calculating the position information predicted value of AUV at the current time by using the extended Kalman filter equation and according to the position information observed value of AUV at the current time, the position information predicted value of AUV at the last time and the position information prediction error of AUV at the last time, where the position information predicted value of AUV at the current time is the position information predicted value.

[0073] Calculating the position predicted value of AUV at the current time by using the extended Kalman equation and according to the position information observed value of AUV at the current time, the position information predicted value of AUV at the last time and the position information prediction error of AUV at the last time, where a complete extended Kalman filtering process is as follows: [0074] a state prediction:

[00016]${\stackrel{}{X}}_{k|k-1}=f\left({\stackrel{}{X}}_{k-1},u_{k-1}\right),$ [0075] where {circumflex over (X)}.sub.k|k-1 represents the next position information predicted value; {circumflex over (X)}.sub.k-1 is the position information predicted value of AUV; [0076] calculating a next position prediction by using the extended Kalman filter equation and according to the position information prediction error of AUV at the last time. The established equation is:

[00017]$P_{k|k-1}={}_{k|k-1}{P}_{k-1}{}_{k|k-1}^T+{}_{k-1}{Q}_{k-1}{}_{k-1}^T,$ [0077] where P.sub.k|k-1 represents the next position information prediction error; P.sub.k-1 represents the position information prediction error of AUV at the last time; Q.sub.k-1 represents a position observation error at the last time; .sub.k|k-1.sup.T represents a state transition matrix, T represents a vector transposed symbol, and .sub.k-1 represents a system noise input matrix; as this example is a nonlinear problem, and .sub.k|k-1 and .sub.k-1 are Jacobian matrices of nonlinear function (.Math.) about {circumflex over (X)}.sub.k-1 and .sub.k-1, respectively, which are expressed as follows:

[00018]${}_{k|k-1}=\frac{f}{X_{K-1}}|X_{K-1}={\stackrel{}{X}}_{k-1}{}_{k-1}=\frac{f}{u_{k-1}}|u_{k-1}={\hat{u}}_{k-1};$ [0078] calculating a filtering gain according to the next position information prediction error by using the extended Kalman filter equation, and the established equation is as follows:

[00019]$K_k=P_{k|k-1}{H_k^T\left(H_k{P}_{k|k-1}{H}_k^T+R_k\right)}^{-1},$ [0079] where K.sub.k represents the filtering gain; P.sub.k|k-1 represents the next position information prediction error; H.sub.k represents an observation matrix at time k; and R.sub.k represents a system noise error; [0080] calculating the position information predicted value at the current time by using the extended Kalman filter equation and according to the next position information predicted value, the filtering gain and the position information observed value at the current time (the position information predicted value at the current time is the position information predicted value). The equation is established as follows:

[00020]${\stackrel{}{X}}_k={\stackrel{}{X}}_{k|k-1}+K_k\left(Z_k-{\stackrel{}{Z}}_{k|k-1}\right);$ [0081] in the formula, {circumflex over (X)}.sub.k represents position information predicted value at the current time; {circumflex over (X)}.sub.k|k-1 represents the next position information predicted value; K.sub.k represents the filtering gain, Z.sub.k represents a position observation predicted value at the current time; and {circumflex over (Z)}.sub.k|k-1 represents the position information predicted value of AUV at the last time; [0082] calculating a position information prediction error at the current time by using the Kalman filter equation and according to a next position information prediction error; the equation is established as follows:

[00021]$P_k=\left(I-K_k{H}_k\right)P_{k|k-1};$ [0083] in the formula, P.sub.k represents the position information prediction error at the current time; I represents unit matrix; H.sub.k represents the observation matrix at time k; and P.sub.k|k-1 represents the next position information prediction error.

Embodiment 2

[0084] As shown in FIG. 5, FIG. 5 is a schematic diagram of the system structure according to an embodiment of the disclosure, the system includes an Autonomous Underwater Vehicle (AUV) positioning system, also includes a first construction module, an acquisition module, a second construction module, a third construction module and a prediction module. The first construction module is used for establishing a state equation based on a system model of an AUV positioning system; the acquisition module is used for acquiring an included angle between a direction vector of AUV to an observation station and a velocity vector of AUV based on the system model; the second construction module is used for establishing an observation equation according to the state equation and the included angle; the third construction module is used for establishing an extended Kalman filter equation based on the system model, the included angle and the observation equation; and the prediction module is used for calculating a position information predicted value at the current time by using the extended Kalman filter equation to complete communication delay compensation.

[0085] The following explains in detail how the disclosure solves the technical problems in real life in combination with this embodiment.

[0086] The first construction module is used for establishing a state equation based on a system model of an AUV positioning system. [0087] based on the system model of AUV, establishing the state equation:

[00022]$X_k=f\left(X_{k-1},u_{k-1}\right)+{}_{k-1}{W}_k,$ [0088] in the formula, X.sub.k represents an n-dimensional state vector at k point of time; X.sub.k-1 represents an n-dimensional state vector at k-1 point of time; u.sub.k-1 represents a system input; .sub.k-1 represents a system noise input matrix from k-1 point of time to k point of time; W.sub.k represents a system noise vector at k-1 point of time; and f represents a nonlinear state function of a system state.

[0089] In this embodiment, considering that the communication delay in the underwater positioning system may negatively affect the positioning accuracy, if the distance between the AUV and the observation station is directly substituted into the observation equation as measurement data, a large error is introduced to affect the navigation performance. In this scenario, this embodiment introduces Doppler measurement data and uses an EKF filtering method based on the reconstructed measurement equation to compensate the navigation error caused by the delay. The communication delay of the positioning system is shown in FIG. 2.

[0090] The acquisition module is used for acquiring the included angle between a direction vector of AUV to the observation station and the velocity vector of AUV based on the system model (the motion schematic of AUV is shown in FIG. 3), and the steps include: [0091] as shown in FIG. 4, a Doppler measurement equation of AUV is obtained based on a geometric diagram of the target positioning by the mobile sensor with Doppler measurement.

[00023]$\{\begin{array}{c}{\stackrel{}{f}}_k=f_k+e_k\\ f_k=f_s\left(1+\frac{v_k^T\left(u-s_k\right)}{c.Math.u-s_k.Math.}\right)=f_s\left(1+\frac{.Math.v_k.Math.}{c}\cos {}_k\right)\end{array},$ [0092] where f.sub.s represents a frequency of an acoustic signal emitted by a target; f.sub.k represents a frequency of an acoustic signal theoretically received by the target; {tilde over (f)}.sub.k represents a frequency of an acoustic signal actually received by the target; e.sub.k represents frequency noise; c represents a propagation speed of a propagation signal in water; v.sub.k represents a driving speed of AUV; u represents a position of the observation station; s.sub.k represents a position of AUV at k point of time; .sub.k represents the included angle between the direction vector of AUV to the observation station and the velocity vector of AUV; T represents a vector transposed symbol.

[0093] Then, based on the transceiver frequency, an included angle of AUV is deduced from the Doppler measurement equation:

[00024]${}_k=\arccos \left(\frac{c}{.Math.v_k.Math.}\left(\frac{f_k}{f_s}-1\right)\right).$

[0094] Finally, according to the included angle, the measurement equation of EKF is reconstructed, and an updated observation equation is:

[00025]$Z_k^{}=d_1^2+d_t^2-2d_1^2{d}_t^2\cos {}_k,$ [0095] where Z.sub.k represents an Euclidean distance between AUV and the observation station at time t2, d represents a distance of AUV from the observation station at a last point of time; .sub.t represents a communication delay time; and d.sub.t represents a distance traveled by AUV within .sub.t duration, and .sub.k represents the included angle between the direction vector of AUV to the observation station and the velocity vector of AUV.

[0096] Thus, more accurate position information of AUV at a signal receiving time t2 is obtained, and compensation for communication delay positioning error is realized.

[0097] The second construction module is used for establishing an observation equation according to the state equation and the included angle.

[00026]$Z_k^{}=d_1^2+d_t^2-2d_1^2{d}_t^2\cos {}_k;$ [0098] in the formula, d represents a distance of AUV from the observation station at a last point of time; .sub.t represents a communication delay time; and d.sub.t represents a distance traveled by AUV within .sub.t duration.

[0099] Using the observation equation, calculating the distance observation value at the current time;

[00027]$z_k=h\left({\stackrel{}{X}}_{k|}\right)+{}_k^p,$ [0100] where z.sub.k represents an observation vector at time k, h({circumflex over (X)}.sub.k|) represents a nonlinear function of a system observation vector, and .sub.k.sup.p represents an observation noise vector at time k.

[0101] The third construction module is used for establishing an extended Kalman filter equation based on the system model, the included angle and the observation equation. The observation equation is updated according to the Doppler measurement information.

[0102] The prediction module is used for calculating a position information predicted value at the current time by using the extended Kalman filter equation to complete communication delay compensation.

[0103] According to distance data between AUV and the observation station, calculating position information of AUV, acquiring a position information measured value of AUV at the current time, and at the same time, acquiring a position information predicted value of AUV at the last time and a position information prediction error of AUV at the last time; [0104] calculating a position information observed value of AUV at the current time by using the observation equation and according to the position information measured value of AUV at the current time; and [0105] calculating the position information predicted value of AUV at the current time by using the extended Kalman filter equation and according to the position information observed value of AUV at the current time, the position information predicted value of AUV at the last time and the position information prediction error of AUV at the last time, where the position information predicted value of AUV at the current time is the position information predicted value. [0106] calculating the position predicted value of AUV at the current time by using the extended Kalman equation and according to the position information observed value of AUV at the current time, the position information predicted value of AUV at the last time and the position information prediction error of AUV at the last time, where a complete extended Kalman filtering process is as follows: [0107] a state prediction:

[00028]${\stackrel{}{X}}_{k|k-1}=f\left({\stackrel{}{X}}_{k-1},u_{k-1}\right),$ [0108] where {circumflex over (X)}.sub.k|k-1 represents the next position information predicted value; {circumflex over (X)}.sub.k-1 is the position information predicted value of AUV; [0109] calculating a next position prediction by using the extended Kalman filter equation and according to the position information prediction error of AUV at the last time. The established equation is:

[00029]$P_{k|k-1}={}_{k|k-1}{P}_{k-1}{}_{k|k-1}^T+{}_{k-1}{Q}_{k-1}{}_{k-1}^T,$ [0110] where P.sub.k|k-1 represents the next position information prediction error; P.sub.k-1 represents the position information prediction error of AUV at the last time; Q.sub.k-1 represents a position observation error at the last time; .sub.k|k-1.sup.T represents a state transition matrix, T represents a vector transposed symbol, and .sub.k-1 represents a system noise input matrix; as this example is a nonlinear problem, and .sub.k|k-1 and .sub.k-1 are Jacobian matrices of nonlinear function (.Math.) about {circumflex over (X)}.sub.k-1 and .sub.k-1, respectively, which are expressed as follows:

[00030]${}_{k|k-1}=\frac{f}{X_{K-1}}|X_{K-1}={\stackrel{}{X}}_{k-1}$${}_{k-1}=\frac{f}{u_{k-1}}|u_{k-1}={\hat{u}}_{k-1};$ [0111] calculating a filtering gain according to the next position information prediction error by using the extended Kalman filter equation, and the established equation is as follows:

[00031]$K_k=P_{k|k-1}{H_k^T\left(H_k{P}_{k|k-1}{H}_k^T+R_k\right)}^{-1},$ [0112] where K.sub.k represents the filtering gain; P.sub.k|k-1 represents the next position information prediction error; H.sub.k represents an observation matrix at time k; and R.sub.k represents a system noise error; [0113] calculating the position information predicted value at the current time by using the extended Kalman filter equation and according to the next position information predicted value, the filtering gain and the position information observed value at the current time (the position information predicted value at the current time is the position information predicted value). The equation is established as follows:

[00032]${\stackrel{}{X}}_k={\stackrel{}{X}}_{k|k-1}+K_k\left(Z_k-{\stackrel{}{Z}}_{k|k-1}\right);$ [0114] in the formula, {circumflex over (X)}.sub.k represents position information predicted value at the current time; {circumflex over (X)}.sub.k|k-1 represents the next position information predicted value; K.sub.k represents the filtering gain, Z.sub.k represents a position observation predicted value at the current time; and {circumflex over (Z)}.sub.k|k-1 represents the position information predicted value of AUV at the last time; [0115] calculating a position information prediction error at the current time by using the Extended Kalman filter equation and according to a next position information prediction error; the equation is established as follows:

[00033]$P_k=\left(I-K_k{H}_k\right)P_{k|k-1};$ [0116] in the formula, P.sub.k represents the position information prediction error at the current time; I represents unit matrix; H.sub.k represents the observation matrix at time k; and P.sub.k|k-1 represents the next position information prediction error.

[0117] The above-mentioned embodiment is only a description of the preferred mode of this disclosure, not a limitation on the scope of this disclosure. Without departing from the design spirit of this disclosure, various modifications and improvements made by ordinary technicians in this field to the technical scheme of this disclosure shall fall within the protection scope determined by the claims of this disclosure.