AUV speed estimation method based on centrosymmetric multi-cell Kalman filter

Abstract

The invention discloses an AUV speed estimation method based on a centrosymmetric multi-cell Kalman filter, and the method comprises the steps: carrying out the systematic analysis of a state space model of an autonomous underwater robot, calculating and optimizing a reachable set of the system state of the underwater robot through combining the property of a centrosymmetric multi-cell body and the thought of Kalman filtering, and carrying out the calculation of the reachable set of the system state of the underwater robot. The speed interval of the underwater robot is calculated through the optimal reachable set, and the speed of the underwater robot is effectively monitored. The method is suitable for autonomous underwater robots of various configurations and different motion states, and compared with the prior art, the method provided by the invention not only can accurately estimate the speed of the underwater robot, but also can provide interval estimation of the speed, so that the effective monitoring of the speed state is realized, and the reliability of the underwater robot is improved. And the reliability and the stability of the autonomous underwater robot are obviously improved.

Description

Technical Field

[0001] The invention belongs to the field of autonomous underwater robot state estimation, and in particular relates to an AUV speed estimation method based on a centrosymmetric polyhedral Kalman filter. Background Art

[0002] The ocean is the most vast natural environment on Earth, and it is rich in mineral resources, oil, and natural gas. However, the seabed environment is complex and changeable, usually under high pressure, low temperature, and highly corrosive conditions. These extreme environments pose huge challenges to the exploration and exploitation of seabed resources. Although traditional underwater robots have made several breakthroughs in shallow waters, their flexibility and stability in deep-sea exploration are far from enough. In the face of these problems, autonomous underwater robots (AUVs) have emerged as a new generation of intelligent marine equipment. Relying on their high degree of autonomy, excellent environmental adaptability, and ability to perform deep-sea missions for a long time, AUVs are gradually becoming key technical equipment for seabed resource exploration, environmental monitoring, and marine scientific research.

[0003] AUVs work unmanned and without cables in complex marine environments and must ensure their own safety, which requires continuous monitoring of the state of the AUV. Since the sensors used for monitoring are often expensive, in order to reduce the overall cost, state estimation technology has been developed to make it possible to monitor the state of the AUV more economically. The advancement of this technology not only significantly reduces the hardware cost, but also enhances the economy and practical operability of the equipment. However, uncertainty factors in reality may cause the state estimation results to deviate from the actual situation. In order to alleviate the impact of this uncertainty, robust state estimation methods have been widely studied. These methods are mainly divided into two types: stochastic methods and set membership estimation methods. Traditional stochastic methods assume that uncertainty follows a Gaussian distribution when estimating the state of the AUV, but this assumption is often not applicable in practice. In contrast, the set membership estimation method only assumes that the uncertainty is unknown but bounded, providing a more flexible and practical solution.

[0004] The set membership estimation method estimates by creating a set that includes all possible state values. According to the different sets, the set membership estimation method can be divided into many categories. Among them, the centrosymmetric polytope has attracted widespread attention due to its simplicity in obtaining linear mappings and Minkowski sums. In the field of AUV state estimation, the Doppler velocimeter used for speed measurement is not only expensive, but also has limited measurement accuracy in certain environments. Summary of the invention

[0005] The purpose of the present invention is to provide an AUV speed estimation method based on a centrosymmetric polyhedral Kalman filter, which can not only accurately estimate the speed of the underwater robot, but also provide an interval estimation of the speed, thereby realizing effective monitoring of the speed state and significantly improving the reliability and stability of the autonomous underwater robot.

[0006] The purpose of the present invention is achieved through the following technical solutions:

[0007] An AUV velocity estimation method based on a centrosymmetric polytope Kalman filter, the specific steps are as follows:

[0008] Step 1: Analyze the state space model of the autonomous underwater robot system;

[0009] Step 2: According to the state space model formula in step 1, the reachable set of the underwater robot system state is calculated using the central symmetric polyhedral definition, Minkowski sum property, and linear mapping property;

[0010] Step 3: Optimize the reachable set of the underwater robot system state in step 2;

[0011] Step 4: According to the optimal reachable set obtained in step 3, the speed interval of the underwater robot system is calculated according to the properties of the minimum interval envelope and the state space model obtained in step 1;

[0012] Step 5: According to the speed range of the underwater robot system obtained in step 4, analyze the speed estimation result of the underwater robot system.

[0013] Furthermore, the step 1 is specifically as follows:

[0014] Based on the "UVIC-I" AUV platform, the state space model of AUV is systematically analyzed, and the state space model is obtained through identification as follows:

[0015]

[0016] Among them, x represents the state vector, u represents the control input vector, d represents the error vector of system identification, y represents the measurement output vector, v represents the noise vector caused by sensor accuracy, C is a known constant matrix, and A, B, and D are obtained from experimental data through system identification;

[0017] Since the motion state of the AUV varies within a preset range, the state quantity of the system is also bounded during the entire operation; although the specific values ​​of the initial value, identification error and measurement noise are unknown, they are all bounded variables and can be expressed in the following form:

[0018]

[0019] in, are the initial value x0, the identification error w and the boundary of the measurement noise ν respectively;

[0020] According to the definition of centrosymmetric polytope, formula (2) is expressed as:

[0021]

[0022] in,

[0023] Furthermore, the centrosymmetric polyhedron is defined as:

[0024] S-order centrosymmetric polytope It is a hypercube B s =[-1,+1] s The affine transformation of is written as follows:

[0025]

[0026] in, represents the Minkowski sum operator, c is The center vector of G is The generator matrix of .

[0027] Furthermore, the step 2 is specifically as follows:

[0028] According to the properties of Minkowski sum, linear mapping, and the x in the state space model formula (1), + =Ax+Bu+d and in formula (3) Predicted reachable set of “UVIC-I” AUV It is expressed as:

[0029]

[0030] The predicted reachable set Written in the form of a centrosymmetric polyhedron:

[0031]

[0032] According to the definition of centrosymmetric polytope, formula (6) can be expressed as:

[0033]

[0034] Among them, δ is a one-dimensional unit vector;

[0035] According to the properties of Minkowski sum, the properties of linear mapping, y=Cx+v in the state space model formula (1) and Measurement strips of the “UVIC-I” AUV It is expressed as:

[0036]

[0037] There exists a one-dimensional unit vector Γ such that the measurement strip It is expressed as:

[0038] y-Cx=G v Γ (9)

[0039] Based on the order reduction algorithm, predict the reachable set (7) and measuring strips (8), the reachable set of system states is obtained by calculation as follows:

[0040] c=Ac - +Bu - +Ly-LCAc - -LCBu - (10)

[0041]

[0042] Furthermore, the property of the Minkowski sum is:

[0043] Two centrosymmetric polytopes and The Minkowski sum of is still a centrosymmetric polytope and satisfies the following equation:

[0044]

[0045] The properties of the linear mapping are:

[0046] Given a centrosymmetric polytope and a matrix Π, whose linear transformation about the matrix is ​​still a centrally symmetric polyhedron and satisfies the following equation:

[0047] Π<c,G> = <Πc,ΠG> (14)

[0048] The order reduction algorithm is:

[0049] For a centrosymmetric polyhedron Given an integer q, satisfying n<q<s, rearrange the columns of matrix G in descending order of their Euclidean norm to form a new matrix Make Among them, G a is the first qn column of G, and G b is a diagonal matrix that satisfies

[0050] Furthermore, the step 3 is specifically as follows:

[0051] According to the generation matrix (11) of the reachable set of system states, we know that the reachable set contains the modified matrix L to be determined. In fact, different L will form reachable sets of different shapes and sizes. Therefore, it is very important to choose the most appropriate correction matrix L. L directly affects the reachable set The size of the reachable set affects the accuracy of the speed estimation. The size criterion J is such that The volume is as small as possible to obtain an accurate speed range, as follows:

[0052] Define the size criterion J = tr(G T G), where G is a reachable set The generator matrix of , according to the generator matrix (11) of the reachable set of system states, is:

[0053] J=tr[(Q d -LCQ d )(I-LC) T ]+tr(LQ v L T )+tr[(A-LCA)P - (A-LCA) T ] (15)

[0054] in,

[0055]

[0056]

[0057]

[0058] In order to optimize the size criterion J, based on the matrix operation rules, we derive formula (15):

[0059]

[0060] According to formula (16), since the size criterion J is a convex function of the generator matrix G and is also expressed in the form of a positive definite quadratic form, it must have a unique minimum value; let have to:

[0061] L=(AP - A T +Q d )C T Y - -1(17)

[0062] in,

[0063] Y - =C(AP - A T +Q d )C T +Q v

[0064] Based on formula (17), the optimal correction matrix L is obtained * Make the reachable set The volume of the optimized reachable set is as small as possible. as follows:

[0065] c=Ac - +Bu - +L * y * C - -L * CBu - (18)

[0066]

[0067] Furthermore, the step 4 is specifically as follows:

[0068] According to the properties of the minimum interval envelope and the state space model (1), the interval of the system state is obtained as follows:

[0069]

[0070] Among them, x - (i) and x + (i) represent the upper and lower bounds of the i-th component of the system state x, respectively, and c(i) represents the optimal reachable set middle

[0071] Heart * The i-th component of G(i,j) represents the reachable set Generator Matrix G * The i-th row and j-th column of .

[0072] Furthermore, the property of the minimum interval envelope is:

[0073] For a centrosymmetric polytope of order s Its minimum interval envelope It is obtained from the following formula:

[0074]

[0075] Among them, z - (i), z+ (i) and c(i) represent z - , z + and the i-th component of c, G(i,j) represents the i-th row and j-th column of G.

[0076] Furthermore, in step 4, we obtain the state interval formula (20) of the underwater robot system. When the component of the state is speed, the speed interval of the underwater robot system can be obtained. This speed interval can ensure that the actual speed value of the underwater robot system must be within the estimated interval, because the set membership estimation method is based on the worst-case assumption of the environment and noise. This method not only effectively reduces the dependence on high-cost sensors, but also significantly enhances the robustness and stability of the system in complex marine environments. In summary, this method has broad application prospects and high practical value, and is suitable for a variety of complex underwater operation scenarios.

[0077] The beneficial effects of the present invention are:

[0078] 1. The present invention proposes an AUV velocity estimation method based on a centrosymmetric polyhedral Kalman filter. This method is applicable to autonomous underwater robots of various configurations and motion conditions, has wide applicability and versatility, and greatly expands the scope of application.

[0079] 2. Compared with traditional methods, the Kalman filter AUV speed estimation method based on centrosymmetric polyhedron proposed in the present invention can not only accurately estimate the speed of the underwater robot, but also provide interval estimation of the speed, thereby realizing effective monitoring of the speed state and significantly improving the reliability and stability of the autonomous underwater robot.

[0080] 3. The AUV velocity estimation method proposed in the present invention can effectively cope with various environmental noises and uncertainties, and has high robustness. Whether in a still water environment or in a dynamic and complex marine environment, the method can maintain high state estimation performance.

[0081] 4. The AUV velocity estimation method proposed in this invention provides a more cost-effective solution with higher measurement accuracy in a variety of environments. This method reduces the reliance on expensive hardware through advanced algorithm optimization, and can maintain stable velocity estimation, especially when the measurement accuracy is easily affected by environmental factors, thereby reducing the operating cost of the overall system and significantly improving the flexibility of application. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 It is a flow chart of the Kalman filter AUV velocity estimation method based on centrosymmetric polyhedron;

[0083] Figure 2This is the overall structure diagram of the "UVIC-I" AUV system;

[0084] Figure 3 It is the estimated result of the diving speed range of the “UVIC-I” AUV system;

[0085] Figure 4 It is the estimated result of the pitch angular velocity range of the “UVIC-I” AUV system. DETAILED DESCRIPTION

[0086] The present invention is further described below in conjunction with the accompanying drawings.

[0087] The present invention provides an AUV velocity estimation method based on a centrosymmetric polytope Kalman filter, and the relevant definitions and properties required are as follows:

[0088] To simplify the notation, we omit the symbol k representing the discrete moment, where the subscript + is used to represent the moment k+1 and the subscript - is used to represent the moment k-1.

[0089] Definition 1: Definition of centrosymmetric polytope, centrosymmetric polytope of order s It is a hypercube B s =[-1,+1] s The affine transformation of can be written as follows:

[0090]

[0091] in, represents the Minkowski sum operator, c is The center vector of G is The generator matrix of .

[0092] Property 1: Matrix operation rules. For matrices A, B, C, X with appropriate dimensions, the following equation holds:

[0093]

[0094] Property 2: Minkowski sum, two centrosymmetric polytopes and The Minkowski sum of is still a centrosymmetric polytope and satisfies the following equation:

[0095]

[0096] Property 3: Linear mapping, given a centrally symmetric polyhedron and a matrix Π, whose linear transformation about the matrix is ​​still a centrally symmetric polyhedron and satisfies the following equation:

[0097]

[0098] Property 4: Reduced-order algorithm, for a centrally symmetric polyhedron Given an integer q, satisfying n<q<s, the columns of the matrix G can be rearranged in descending order of their Euclidean norm to form a new matrix Make Among them, G a is the first qn column of G, and G b is a diagonal matrix that satisfies

[0099] Property 5: Minimum interval envelope, for a centrosymmetric polyhedron of order s Its minimum interval envelope It can be obtained by the following formula:

[0100]

[0101] Among them, z - (i), z + (i) and c(i) represent z - , z + and the i-th component of c, G(i,j) represents the i-th row and j-th column of G.

[0102] The specific structure of the "UVIC-I" AUV of the present invention is as follows Figure 2 As shown, the electronic cabin is located on the top of the "UVIC-I" AUV, which is used to accommodate various electronic components to ensure protection and stable operation in the underwater environment. A manipulator for underwater operations is installed at the front end of the AUV, enabling it to complete delicate operations such as underwater grabbing and manipulation. In addition, the "UVIC-I" AUV is equipped with a variety of sensors, such as a depth gauge, a digital compass, etc., to ensure the accuracy and stability of attitude control during diving, thereby ensuring efficient execution and safety of operations. The specific implementation scheme of the present invention is as follows:

[0103] The "UVIC-I" AUV was experimented in a water pool. During the experiment, the "UVIC-I" AUV started and reached the preset stable state, and continued to operate in this state, showing a stable speed and attitude. The diving depth Z, diving speed w, pitch angle θ and pitch angular velocity q of the "UVIC-I" AUV on the vertical plane are mainly controlled and realized by the thrusts T1 and T2 generated by the vertical thrusters. According to the experimental data, the vertical state space model of the "UVIC-I" AUV is identified as follows:

[0104]

[0105] in,

[0106]

[0107] Given that the motion state of the AUV varies within a preset range, the state quantity of the system is also bounded during the entire operation. Although the specific values ​​of the initial value, identification error, and measurement noise are unknown, they are all bounded variables and can be expressed in the following form:

[0108]

[0109] in, are the initial value x0, the identification error w and the boundary of the measurement noise ν respectively.

[0110] According to the definition of centrosymmetric polytope, assumption (28) can be expressed as:

[0111]

[0112] in

[0113] According to the Minkowski sum formula (24), the linear mapping formula (25), the state space model equation x + =Ax+Bu+d and the assumptions in (29) Predicted reachable set of “UVIC-I” AUV It can be expressed as follows:

[0114]

[0115] The predicted reachable set (30) is written in the form of a centrosymmetric polytope:

[0116]

[0117] According to the definition of centrosymmetric polytope, (31) can be expressed as:

[0118]

[0119] Where δ is a one-dimensional unit vector.

[0120] According to the Minkowski sum formula (24), the linear mapping formula (25), the state space model equation y = Cx + v and the assumptions (29) Measurement strips of the “UVIC-I” AUV It can be expressed as follows:

[0121]

[0122] There exists a one-dimensional unit vector Γ such that the measurement strip It can be expressed as follows:

[0123] y-Cx=G v Γ (34)

[0124] Based on the predicted reachable set and measuring strips (33), we can obtain the reachable set of system states by calculating The specific steps are as follows:

[0125] Substituting (32) into (34), the following equation holds:

[0126] C[AG - G d ]δ=yG v Γ-CAc - -CBu - (35)

[0127] At the same time, (32) is expressed as follows:

[0128] x=Ac - +Bu - +LC[AG - G d ]δ+(I-LC)[AG - G d ]δ (36)

[0129] Among them, L is the correction matrix to be determined.

[0130] Substituting (35) into (36), the following equation holds:

[0131] x=Ac - +Bu - +Ly-LG v Γ-LCAc - -LCBu - +(I-LC)[AG - G d ]δ (37)

[0132] Since δ and Γ are both unit vectors, then is also a unit vector. According to the definition of centrosymmetric polytopes, Minkowski and formula (24), the order reduction algorithm and the linear mapping formula (25), formula (37) can be expressed as follows:

[0133]

[0134] c=Ac - +Bu - +Ly-LCAc - -LCBu - (39)

[0135]

[0136] make The reachable set of the “UVIC-I” AUV system status can be obtained.

[0137] Define the size criterion J = tr(G T G), where G is a reachable set The generator matrix of , according to (40), we can know:

[0138] J=tr[(Q d -LCQ d )(I-LC) T ]+tr(LQ v L T )+tr[(A-LCA)P - (A-LCA) T ] (42)

[0139] in,

[0140]

[0141] In order to optimize the size criterion J, based on the matrix operation rules, we derive (42):

[0142]

[0143] According to formula (43), since the size criterion J is a convex function of the generator matrix G and is also in the form of a positive definite quadratic form, it must have a unique minimum value. Let We can get:

[0144] L=(AP - A T +Q d )C T Y - -1 (44)

[0145] in,

[0146] Y - =C(AP - A T +Q d )C T +Q v

[0147] Based on formula (44), the optimal correction matrix L can be obtained * Make the reachable set The volume of the optimized reachable set is as small as possible. as follows:

[0148] c=Ac - +Bu - +L * y * C - -L * CBu - (45)

[0149]

[0150] According to the minimum interval envelope property formula (26) and the state space model (27), the interval of the system state can be obtained as follows:

[0151]

[0152] Among them, x - (i) and x + (i) represent the upper and lower bounds of the i-th component of the system state x, respectively, and c(i) represents the optimal reachable set Centerc * The i-th component of G(i,j) represents the reachable set Generator Matrix G * The i-th row and j-th column of .

[0153] For the state space model (27), the target speed corresponds to the first and second components of the system state x, that is, i = 1, 2. According to formula (47), the speed range can be expressed as:

[0154]

[0155]

[0156] Based on the speed range calculated by formulas (48) and (49), effective speed monitoring can be achieved.

[0157] The sampling of the "UVIC-I" AUV starts from the moment of startup, with a sampling frequency of 5Hz, that is, data is collected every 0.2 seconds. Since both the diving depth Z and the pitch angle θ can be measured, the measurement matrix Through the identification and analysis of experimental data, the parameter matrices A, B, and D are obtained as follows:

[0158]

[0159] According to the feedback of experimental data, the magnitude of identification error and measurement noise can be obtained as follows:

[0160]

[0161] Based on the above conditions, the estimated results of the diving speed and pitch angular velocity of the "UVIC-I" AUV system are as follows: Figure 3-4 As shown, the black star-shaped broken line represents the true value of the diving speed and pitch angular velocity of the "UVIC-I" AUV at each moment, and the two red dotted lines represent the upper boundary and lower boundary of the interval of the diving speed and pitch angular velocity determined by the calculation method adopted by the present invention, respectively. The cyan area within the red boundary shows the optimal reachable set of the "UVIC-I" AUV, and the green star-shaped broken line represents the estimated value of the diving speed and pitch angular velocity of the "UVIC-I" AUV at each moment calculated by the Kalman filter method. Figure 3-4 It can be seen that the Kalman filter method can only make a point estimate of the speed, that is, a single estimated value of the speed is obtained at each sampling moment. This estimation method is difficult to fully express the range or uncertainty of the speed change when facing scenes with large noise or uncertain factors. The calculation method adopted by the present invention can provide an interval estimate of the speed, which can not only give the optimal estimate of the speed, but also provide the upper and lower limits of its possible fluctuations. When the system state has a large uncertainty, the interval estimate can more comprehensively reflect the actual range of speed changes, thereby improving the adaptability and reliability of the system in complex environments.

[0162] In summary, the present invention aims at the problem that Doppler velocimeter is expensive and has limited measurement accuracy in certain environments, and proposes an AUV speed estimation method based on centrosymmetric polyhedral Kalman filter. First, the state space model of the autonomous underwater robot is systematically analyzed, and then the reachable set of the underwater robot system state is calculated and optimized by combining the properties of the centrosymmetric polyhedral and the idea of ​​Kalman filtering. The speed range of the underwater robot is calculated through the optimal reachable set, and its speed is effectively monitored, ensuring the stability and reliability of the AUV.

[0163] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for AUV velocity estimation based on a centrosymmetric polytope Kalman filter, characterized in that: The specific steps are as follows: Step 1: Analyze the state space model of the autonomous underwater robot system; Step 2: According to the state space model formula in step 1, the reachable set of the underwater robot system state is calculated using the central symmetric polyhedral definition, Minkowski sum property, and linear mapping property; Step 3: Optimize the reachable set of the underwater robot system state in step 2; Step 4: According to the optimal reachable set obtained in step 3, the speed interval of the underwater robot system is calculated according to the properties of the minimum interval envelope and the state space model obtained in step 1; Step 5: According to the speed range of the underwater robot system obtained in step 4, analyze the speed estimation result of the underwater robot system.

2. The AUV velocity estimation method based on a centrosymmetric polytope Kalman filter according to claim 1, characterized in that: The step 1 is specifically as follows: Based on the "UVIC-I" AUV platform, the state space model of AUV is systematically analyzed, and the state space model is obtained through identification as follows: Among them, x represents the state vector, u represents the control input vector, d represents the error vector of system identification, y represents the measurement output vector, v represents the noise vector caused by sensor accuracy, C is a known constant matrix, and A, B, and D are obtained from experimental data through system identification; Since the motion state of the AUV varies within a preset range, the state quantity of the system is also bounded during the entire operation; although the specific values ​​of the initial value, identification error and measurement noise are unknown, they are all bounded variables and can be expressed in the following form: in, are the initial value x0, the identification error w and the boundary of the measurement noise ν respectively; According to the definition of centrosymmetric polytope, formula (2) is expressed as: in, 3. The AUV velocity estimation method based on a centrosymmetric polytope Kalman filter according to claim 2, characterized in that: The centrosymmetric polytope is defined as: S-order centrosymmetric polytope It is a hypercube B s =[-1,+1] s The affine transformation of is written as follows: in, represents the Minkowski sum operator, c is The center vector of G is The generator matrix of .

4. The AUV velocity estimation method based on a centrosymmetric polytope Kalman filter according to claim 1, characterized in that: The step 2 is specifically as follows: According to the properties of Minkowski sum, the properties of linear mapping, and the x in the state space model formula (1), + =Ax+Bu+d and in formula (3) Predicted reachable set of "UVIC-I" AUV It is expressed as: The predicted reachable set Written in the form of a centrosymmetric polyhedron: According to the definition of centrosymmetric polytope, formula (6) can be expressed as: Among them, δ is a one-dimensional unit vector; According to the properties of Minkowski sum, the properties of linear mapping, y=Cx+v in the state space model formula (1) and Measurement strips of the "UVIC-I" AUV It is expressed as: There exists a one-dimensional unit vector Γ such that the measurement strip It is expressed as: y-Cx=G v C (9) Based on the order reduction algorithm, predict the reachable set and measuring strips (8), the reachable set of system states is obtained by calculation as follows: c=Ac - +Bu - +Ly-LCAc - -LCBu - (10) 5. The AUV velocity estimation method based on a centrosymmetric polytope Kalman filter according to claim 3, characterized in that: The properties of the Minkowski sum are: Two centrosymmetric polytopes and The Minkowski sum of is still a centrosymmetric polytope and satisfies the following equation: The properties of the linear mapping are: Given a centrosymmetric polytope and a matrix Π, whose linear transformation about the matrix is ​​still a centrally symmetric polyhedron and satisfies the following equation: Π<c,G> = <Πc,ΠG> (14) The order reduction algorithm is: For a centrosymmetric polyhedron Given an integer q, satisfying n<q<s, rearrange the columns of matrix G in descending order of their Euclidean norm to form a new matrix Make Among them, G a is the first qn column of G, and G b is a diagonal matrix that satisfies 6. The AUV velocity estimation method based on a centrosymmetric polytope Kalman filter according to claim 1, characterized in that: The step 3 is specifically as follows: According to the generation matrix (11) of the reachable set of system states, L directly affects the reachable set The size of the reachable set affects the accuracy of the speed estimation. The size criterion J is such that The volume is as small as possible to obtain an accurate speed range, as follows: Define the size criterion J = tr(G T G), where G is a reachable set The generator matrix of , according to the generator matrix (11) of the reachable set of system states, is: J=tr[(Q d -LCQ d )(I-LC) T ]+tr(LQ v L T )+tr[(A-LCA)P - (A-LCA) T ](15) in, In order to optimize the size criterion J, based on the matrix operation rules, we derive formula (15): According to formula (16), since the size criterion J is a convex function of the generator matrix G and is also expressed in the form of a positive definite quadratic form, it must have a unique minimum value; let have to: in, AND - =C(AP - TO T +Q d )C T +Q v Based on formula (17), the optimal correction matrix L is obtained * Make the reachable set The volume of the optimized reachable set is as small as possible. as follows: c=Ac - +Bu - +L * yL * CAc - -L * CBu - (18) 7. The AUV velocity estimation method based on a centrosymmetric polytope Kalman filter according to claim 1, characterized in that: The step 4 is specifically as follows: According to the properties of the minimum interval envelope and the state space model (1), the interval of the system state is obtained as follows: Among them, x - (i) and x + (i) represent the upper and lower bounds of the i-th component of the system state x, respectively, and c(i) represents the optimal reachable set Centerc * The i-th component of G(i,j) represents the reachable set Generator Matrix G * The i-th row and j-th column of .

8. The AUV velocity estimation method based on a centrosymmetric polytope Kalman filter according to claim 7, characterized in that: The properties of the minimum interval envelope are: For a centrosymmetric polytope of order s Its minimum interval envelope It is obtained from the following formula: Among them, z - (i), z + (i) and c(i) represent z - , z + and the i-th component of c, G(i,j) represents the i-th row and j-th column of G.

Images