A center-symmetric polytope Kalman filtering AUV velocity estimation method based on T-N-L structure

Abstract

The application discloses a center-symmetrical polytope Kalman filtering AUV velocity estimation method based on a T-N-L structure. First, the state space model of the autonomous underwater vehicle is identified according to experimental data, then the reachable set of the AUV system state is predicted and corrected in combination with the properties of the center-symmetrical polytope and the Kalman filtering thought, the velocity interval of the autonomous underwater vehicle is calculated through the corrected reachable set of the system, and the effective monitoring of the velocity is realized. The method is suitable for autonomous underwater vehicles of various configurations and motion conditions, compared with the prior art, the application provides a system effective velocity estimation method, the method adopted by the application can generate more accurate velocity estimation value and more compact velocity estimation interval, not only the accuracy of the velocity state monitoring is improved, but also the behavior of the robot in the complex underwater environment is ensured to be more stable and predictable, the overall reliability and stability of the underwater robot system are significantly enhanced.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of autonomous underwater vehicle state estimation, and particularly relates to a central symmetric polytope Kalman filter AUV velocity estimation method based on a T-N-L structure. BACKGROUND

[0002] The ocean, as the largest natural field on earth, contains rich resources such as minerals, oil, and natural gas. However, the seabed environment is extremely complex, with high pressure, low temperature, and strong corrosion for years, which brings great challenges to the exploration and development of seabed resources. Although traditional underwater robots have achieved certain results in shallow sea areas, their operational flexibility and stability in deep sea exploration still have significant limitations. In this context, autonomous underwater vehicles (AUVs) have emerged as a new generation of intelligent marine equipment. With its high autonomy, excellent environmental adaptability, and long-term deep sea operation characteristics, AUV is gradually becoming the core equipment in seabed resource exploration, environmental monitoring, and marine research.

[0003] AUV must ensure its own safety while working in complex marine environments without cables, which requires monitoring the state of AUV. However, the sensors for monitoring AUV usually have high prices, and in order to reduce costs, state estimation technology is proposed to achieve accurate monitoring of AUV state. The development of this technology not only effectively reduces the hardware cost of the equipment, but also improves its economic efficiency and operability in practical applications. However, due to environmental influences, the results of state estimation may have some deviation from the true value. In order to alleviate the influence of uncertainty, robust state estimation methods have been widely studied. Robust state estimation methods are usually divided into two categories: stochastic methods and set-membership estimation methods. Traditional stochastic methods require the assumption that uncertainty follows a known probability distribution during AUV state estimation, which may not be true in practical applications. Set-membership estimation methods achieve state estimation by constructing a set containing all possible state values, which assumes that uncertainty is unknown but bounded, making the conditions more relaxed and meeting the actual needs.

[0004] Set-membership estimation methods can be classified by different sets, such as ellipsoids, polytopes, intervals, and central symmetric polytopes. Among existing geometric bodies, central symmetric polytopes have received widespread attention due to their simplicity in obtaining linear mappings and Minkowski sums. In the field of AUV state estimation, Doppler velocity log used for velocity measurement is not only expensive, but also has limited measurement accuracy in certain environments. SUMMARY

[0005] The application aims to provide a center-symmetrical polytope Kalman filtering AUV velocity estimation method based on a T-N-L structure.

[0006] The application achieves the goal through the following technical solutions.

[0007] A center-symmetrical polytope Kalman filtering AUV velocity estimation method has the following specific steps:

[0008] Step one: based on the "Beaver-II" AUV platform constructed in the laboratory, the state space model of the AUV is systematically analyzed;

[0009] Step two: based on the T-N-L structure and the properties of the center-symmetrical polytope, the reachable set of the "Beaver-II" AUV system state is predicted according to the state space model in step one;

[0010] Step three: the reachable set of the "Beaver-II" AUV system state in step two is corrected;

[0011] Step four: according to the optimal reachable set obtained in step three, the state interval of the "Beaver-II" AUV system is calculated according to the properties of the minimum interval envelope and the state space model obtained in step one;

[0012] Step five: according to the state interval of the underwater robot system obtained in step four, the straight sailing velocity estimation result of the "Beaver-II" AUV system is calculated.

[0013] Further, the step 1 is specifically:

[0014] Based on the "Beaver-II" AUV platform constructed in the laboratory, the state space model of the AUV is systematically analyzed, and the state space model thereof is obtained through identification as follows:

[0015]

[0016] Wherein, x represents a state vector, u represents a control input vector, w represents an error vector of system identification, y represents a measured output vector, v represents a noise vector caused by sensor accuracy, E, C, D1 and D2 are known constant matrices, and A and B are obtained from experimental data through system identification;

[0017] The historical data of the error generated in the identification process and the sensor measurement noise are analyzed; after removing the abnormal values with large amplitudes, the initial value is determined, and the boundary of the identification error and the measurement noise satisfies the following conditions:

[0018]

[0019] where, are the initial value x0, the identification error w and the measurement noise v, respectively, and

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

[0021]

[0022] where,

[0023] Further, the definition of the centrally symmetric polytope is as follows:

[0024] s-order centrally symmetric polytope is an affine transformation of hypercube B s = [-1, +1] s , and the formula is as follows:

[0025]

[0026] where, denotes the Minkowski sum operator, c is the central vector of B , and G is the generating matrix of B

[0027] Further, the step 2 is specifically:

[0028] Based on the T-N-L structure, formula (1) is rewritten in the following form:

[0029]

[0030] where

[0031]

[0032] According to the T-N-L formula (5), we have:

[0033] x + = (TE+NC)x + = TAx+TBu+Ny + +TD1w-ND2v + (7)

[0034] According to the formula (7) and the order reduction algorithm of the centrally symmetric polytope property, we have where

[0035]

[0036] Every iteration step The dimension of the reachable set will increase, according to the order-reduction property, the reachable set in formula (8) is order-reduced, thus obtaining wherein is the predicted reachable set of the system state.

[0037] Further, the order-reduction algorithm of the central symmetric polytope property is as follows:

[0038] For a central symmetric polytope Given an integer q (n < q < s), the columns of the matrix G are rearranged in descending order of their Euclidean norms to form a new matrix such that wherein G a is the first q-n columns of G, and G b is a diagonal matrix satisfying

[0039]

[0040] Further, the step 3 is specifically as follows:

[0041] According to formula (1), the x + in the predicted reachable set formula obtained in step 2 not only satisfies but also satisfies wherein is a strip obtained from the measurement equation in formula (1), and the expression is as follows:

[0042]

[0043] When the intersection of and is difficult to calculate, thus a central symmetric polytope called the corrected reachable set will be used to contain the intersection of and , that is, to reduce the difficulty of operation;

[0044] The central c + (Λ + ) of the corrected reachable set + and the generating matrix G + (Λ + ) are as follows:

[0045]

[0046] wherein, Λ is a to-be-determined correction matrix;

[0047] In order to obtain the accurate speed range, the Frobenius norm is used as the optimization criterion of Λ, and the reachable set is modified by optimization. The generator matrix G + (Λ + )’s Frobenius norm, so The volume is as small as possible to obtain the optimal correction matrix as follows:

[0048]

[0049] Based on formula (14), the optimal corrected reachable set is: as follows:

[0050]

[0051] Through the reachable set Through the optimization calculation, the accurate speed range is obtained, thereby realizing effective speed monitoring.

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

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

[0054]

[0055] Among them, x - (i) and x + (i) represents the upper and lower bounds of the i-th component of the system state x, and c(i) represents the corrected reachable set center The i-th component of G(i,j) represents the corrected reachable set Generating Matrix The i-th row and j-th column of .

[0056] Furthermore, in step 4, when the component of the state is speed, the speed range of the underwater robot system is obtained; when the environment and noise are in the worst case, the speed range can ensure that the actual speed value of the underwater robot system must be within the estimated range.

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

[0058] 1. This paper proposes a centrosymmetric polyhedral Kalman filter AUV velocity estimation method based on TNL structure. This method is applicable to autonomous underwater vehicles of various configurations and motion conditions, has wide applicability and versatility, and greatly expands the scope of application.

[0059] 2. Compared with traditional methods, the AUV velocity estimation method based on T-N-L structure central symmetric polytope Kalman filter proposed in the application can generate more accurate velocity estimation value and more compact velocity estimation interval, not only improving the accuracy of velocity state monitoring, but also ensuring the behavior of the robot in complex underwater environment more stable and predictable, significantly enhancing the overall reliability and stability of the underwater robot system.

[0060] 3. The AUV velocity estimation method based on T-N-L structure central symmetric polytope Kalman filter proposed in the application can effectively cope with various environmental noise and uncertainty factors, and has high robustness. Whether in still water environment or in dynamic complex marine environment, the method can maintain high state estimation performance.

[0061] 4. The AUV velocity estimation method proposed in the application provides a more cost-effective solution with higher measurement accuracy in various environments. Through advanced algorithm optimization, the method reduces the dependence on expensive hardware, especially in cases where measurement accuracy is easily affected by environmental factors, the method can still maintain stable velocity estimation, thereby reducing the operating cost of the overall system and significantly improving the flexibility of application. BRIEF DESCRIPTION OF DRAWINGS

[0062] Figure 1 The flowchart of the AUV velocity estimation method based on T-N-L structure central symmetric polytope Kalman filter is shown in the figure.

[0063] Figure 2 The structure diagram of the "Beaver-II" AUV system is shown in the figure.

[0064] Figure 3 The motion trajectory diagram of the "Beaver-II" AUV system is shown in the figure.

[0065] Figure 4 The estimation result of the straight sailing speed interval of the "Beaver-II" AUV system is shown in the figure. DETAILED DESCRIPTION

[0066] The application will be further described below in conjunction with the accompanying drawings.

[0067] The definitions, properties and lemmas needed in the application are as follows:

[0068] In order to simplify the symbols, we omit the discrete time moment k, where the subscript - is used to represent the moment k-1, and the subscript + is used to represent the moment k+1.

[0069] Definition 1 (central symmetric polytope definition): s-order central symmetric polytope is a hypercube B s = [-1, +1]s An affine transformation of a zonotope Z can be written in the form

[0070]

[0071] where, denotes the Minkowski sum operator, c is the center vector of Z, and G is the generating matrix of Z.

[0072] Property 1 (Central Symmetric Zonotope Property): For a central symmetric zonotope and the following property holds:

[0073]

[0074] where K is a constant matrix, c, c1, c2 e R n , G, G1, G2 e R n×s ,

[0075] Property 2 (Reduced Order Algorithm): For a central symmetric zonotope Given an integer q (n < q < s), the columns of the matrix G can be rearranged in descending order of their Euclidean norms to form a new matrix such that where G a is the first q-n columns of G, and G b is a diagonal matrix satisfying

[0076] Property 3 (Minimum Interval Envelope): For a central symmetric zonotope of order s its minimum interval envelope can be obtained from the following equation:

[0077]

[0078] where z - (i), z + (i), and c(i) denote the i-th component of z - , z + , and c, respectively, and G(i,j) represents the i-th row and j-th column of G.

[0079] Lemma 1: Given matrices X e R a×b , Y e R b×c , and Z e R a×c , if rank(Y) = c, then the general solution to the equation XY = Z is:

[0080]

[0081] where S is a matrix in R a×b is an arbitrary matrix.

[0082] Lemma 2: Given a centrally symmetric polytope and a strip their intersection is a centrally symmetric polytope containing a center c(Λ) and a generator matrix G(Λ) as follows:

[0083]

[0084] where K is a known matrix, e and δ are known vectors, Δ = diag(δ), and Λ is a to-be-determined correction matrix.

[0085] The embodiment of the application is a specific implementation method of the Kalman filter AUV velocity estimation method based on a T-N-L structure centrally symmetric polytope, and the method comprises the following steps:

[0086] The size of the "Beaver-II" AUV is 0.8mx0.5mx0.45m, the mass in air is 50kg, and the working time is 5 hours. Figure 2 As shown in FIG. 1, the "Beaver-II" AUV is equipped with a digital compass and a depth sensor, which are used to measure the heading angle and diving depth of the AUV, respectively. The power cabin and the electronic cabin are installed symmetrically on the left and right sides of the upper part of the underwater robot, the digital compass is fixed on the upper part of the power cabin and the electronic cabin, and the depth gauge is installed on the lower part of the underwater robot.

[0087] The specific steps are as follows:

[0088] Firstly, the "Beaver-II" AUV was tested in a pool. During the experiment, the "Beaver-II" AUV started from the start to reach the preset stable state and continued to run in the state, showing stable speed and attitude, and the trajectory diagram is as shown in FIG. 2. Figure 2 The straight sailing of the "Beaver-II" AUV mainly relies on the thrust of the two main propellers, and the heading control is completed by adjusting the thrust difference of the two main propellers. The thrust of the two main propellers is determined by the control voltage, and the corresponding thrust models are as follows:

[0089] The relationship between the control voltage and the thrust of the left propeller is as follows:

[0090] Forward: T1 = 6.7785*V - 4.0768 (26)

[0091] Backward: T1 = 7.5499*V + 5.878 (27)

[0092] Right thruster control voltage and thrust relationship:

[0093] Forward: T2 = 6.7703*V - 4.22 (28)

[0094] Backward: T2 = 7.0137*V + 7.4725 (29)

[0095] Where V is the control voltage, ranging from -5V to 5V.

[0096] Then, according to the pool experiment data, the model of "Beaver-II" AUV horizontal plane motion state space is identified. According to the straight speed p, the bow angle ψ, the left thruster thrust T1 and the right thruster thrust T2, the state space model of AUV horizontal plane model is derived as follows:

[0097] Ex + = Ax + Bu + D1ω (30)

[0098] Where

[0099]

[0100] The bow angle ψ of "Beaver-II" AUV can be obtained by digital compass, and the measurement output equation of the system is:

[0101] y = Cx + D2υ (32)

[0102] Where

[0103] C = D2 = I ny (33)

[0104] Based on T-N-L structure, the state space model (30) is expressed as follows:

[0105]

[0106] Where

[0107]

[0108] Here, we give the design method of T and N, first rewrite (35) as the following form:

[0109]

[0110] Since E is full rank, according to Lemma 1, the general solution of [T N] in equation (36) is:

[0111]

[0112] According to (37), T, N can be obtained as follows:

[0113]

[0114] where

[0115]

[0116] After determining the parameters of T, N, we can obtain the reachable set of x

[0117] x + = (TE+NC)x + = TAx+TBu+Ny + +TD1w-ND2ν + (40)

[0118] According to formula (40) and the central symmetric polytope property, we can obtain where

[0119]

[0120] Considering that the dimension of x will increase by one step per iteration, according to the order reduction property, we can reduce the order of the reachable set in (41) , and thus obtain where is the predicted reachable set of the system state.

[0121] From the state space model (30), we can obtain that x + not only satisfies but also satisfies where is the strip obtained from the measurement equation in the state space model (30), and its expression is as follows:

[0122]

[0123] Note that However, the intersection of and is difficult to calculate, so we will use a central symmetric polytope called the correction reachable set to contain the intersection of and , that is, to reduce the difficulty of operation. According to Lemma 2, the center c + (Λ + ) and the generating matrix G + (Λ + ) of the correction reachable set are

[0124]

[0125] where Λ is the correction matrix to be determined.

[0126] In fact, different Λ will form different shape and size of the corrected reachable set Therefore, it is crucial to choose the most appropriate correction matrix Λ. Λ can directly affect the size of the corrected reachable set, and the size of the corrected reachable set will further affect the accuracy of the speed estimation. In order to get accurate speed interval, this paper adopts Frobenius norm as the optimization criterion of Λ, by optimizing the Frobenius norm of the generating matrix G + (Λ + ) of the corrected reachable set , make the volume of as small as possible, as follows:

[0127] According to the definition of centrally symmetric polytope, the generating matrix G + (Λ + ) of the corrected reachable set is rewritten as follows:

[0128]

[0129] where,

[0130] Based on this, the Frobenius norm of G + (Λ + ) can be obtained by the following formula:

[0131]

[0132] In order to optimize the Frobenius norm of G + (Λ + ), we take the derivative of (48):

[0133]

[0134] According to (49), since the Frobenius norm of G + (Λ + ) is a convex function related to Λ + , and at the same time it is in the form of positive definite quadratic form, it must exist a unique minimum. Let get the optimal correction matrix as follows:

[0135]

[0136] Based on (50), the optimal corrected reachable set can be obtained as follows

[0137]

[0138] Through the reachable set Through the optimization calculation, we can obtain the accurate speed range, thereby realizing the effective monitoring of the speed.

[0139] According to the minimum interval envelope and state space model, the interval of the system state can be obtained as follows:

[0140]

[0141] Among them, x - (i) and x + (i) represents the upper and lower bounds of the i-th component of the system state x, and c(i) represents the corrected reachable set center The i-th component of G(i,j) represents the corrected reachable set Generating Matrix The i-th row and j-th column of .

[0142] For the state space model, the direct flight speed corresponds to the first component of the system state x, i.e., i = 1. Then, according to (56), the interval of the direct flight speed can be expressed as:

[0143]

[0144] Based on the direct flight speed interval calculated based on the minimum interval envelope (3), effective speed monitoring can be achieved.

[0145] Sampling starts at the startup moment, with a sampling frequency of 5 Hz, that is, data is collected every 0.2 seconds, and a total of 200 cycles of data are obtained. Based on the identification analysis of these data, the parameter matrices A and B are obtained:

[0146]

[0147] By analyzing the system data, the boundaries of the identification error and measurement noise are:

[0148]

[0149] The linear system based on TNL structure can be expressed as follows:

[0150]

[0151] in

[0152]

[0153] Based on the above conditions, the direct flight speed range of the "Beaver-II" AUV is as follows: Figure 4As shown, two red dotted lines represent the upper and lower bounds of the straight speed interval determined by the calculation method adopted by the present application, and two blue dotted lines show the upper and lower bounds of the straight speed interval determined by the traditional speed estimation method without T-N-L structure. In addition, the cyan region within the red boundary shows the corrected reachable set of the "Beaver-II" AUV. The black star-shaped broken line accurately represents the true value of the straight speed of the underwater robot at each time. Figure 4 It can be seen that the true value of the straight speed of the "Beaver-II" AUV is completely contained in the interval calculated by the method proposed in the present application. At the same time, compared with the speed estimation method without T-N-L structure, the speed interval calculated by the method proposed in the present application is obviously more compact, which shows that the method proposed in the present application not only improves the accuracy of speed state monitoring, but also significantly enhances the overall reliability and stability of the underwater robot system by ensuring that the behavior of the robot in complex underwater environment is more stable and predictable.

[0154] In summary, the present application proposes a T-N-L structure based central symmetric polytope Kalman filtering AUV speed estimation method aiming at the problem that the Doppler speedometer is expensive and the measurement accuracy is limited in some environments. First, the state space model of the AUV is identified according to the experimental data, and then the reachable set of the AUV system state is predicted and corrected by combining the properties of the central symmetric polytope and the idea of Kalman filtering, and the straight speed interval of the AUV is calculated through the corrected reachable set of the system, so as to realize the effective monitoring of the speed of the AUV and ensure the stability and reliability of the AUV.

[0155] The above only describes the preferred embodiments of the present application and is not used to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A centrosymmetric polyhedral Kalman filter AUV velocity estimation method based on TNL structure, characterized by: The specific steps are as follows: Step 1: Based on the Beaver-II AUV platform built in the laboratory, a systematic analysis of the AUV state space model was conducted; The state-space model: Where x represents the state vector, u represents the control input vector, w represents the error vector of system identification, y represents the measurement output vector, ν represents the noise vector caused by sensor accuracy, E, C, D1, D2 are known constant matrices, and A and B are obtained from experimental data through system identification; Step 2: Based on the properties of the TNL structure and the centrosymmetric polyhedron, and according to the state space model in step 1, predict the reachable set of the Beaver-IIAUV system state; Based on the TNL structure, formula (1) is rewritten as follows: in According to TNL formula (5), we can get: x + =(TE+NC)x + =TAx+TBu+Ny + +TD1w-ND2ν + (7) According to formula (7) and the order reduction algorithm of the centrosymmetric polyhedral properties, we can get in Each iteration step The dimension of will increase, according to the reduction property, the reachable set in formula (8) Perform reduction to obtain in That is the predicted reachable set of the system state; Step 3: Correct the reachable set of the Beaver-IIAUV system state in step 2; Step 4: Based on the optimal reachable set obtained in step 3, the state interval of the Beaver-IIAUV system is calculated according to the properties of the minimum interval envelope and the state space model obtained in step 1; Step 5: Calculate the estimated straight-line speed of the Beaver-IIAUV system based on the interval of the underwater robot system state obtained in step 4.

2. The method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of a TNL structure according to claim 1, characterized in that: In step 1, A and B are obtained from experimental data through system identification. The errors generated during the identification process and the historical data of sensor measurement noise are analyzed. After removing outliers with large amplitudes, the initial values ​​are determined. The boundaries of the identification error and measurement noise meet the following conditions: in, are the bounds of the initial value x0, identification error w and measurement noise ν respectively; According to the definition of centrosymmetric polytope, formula (2) is expressed as: in, 3. The method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of TNL structure according to claim 2, characterized in that: The centrosymmetric polytope is defined as follows: S-order centrosymmetric polytope It is a hypercube B s =[-1,+1] s The affine transformation of is as follows: in, represents the Minkowski sum operator, c is The center vector of G is The generator matrix of .

4. The method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of a TNL structure according to claim 1, characterized in that: The order reduction algorithm for the centrosymmetric polytope property is as follows: For a centrosymmetric polyhedron Given an integer q (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 5. The method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of a TNL structure according to claim 1, characterized in that: The step 3 is specifically as follows: According to formula (1), the x in the predicted reachable set formula obtained in step 2 is + Not only satisfy It will also satisfy in is the strip obtained by the measurement equation in formula (1), which is expressed as follows: when hour and The intersection of is difficult to calculate, so we will use a centrosymmetric polyhedron called a corrected reachable set to include and The intersection of To reduce the difficulty of calculation; Corrected reachable set Center of c + (Λ + ) and the generator matrix G + (Λ + )for: in, Λ is the correction matrix to be determined; In order to obtain the accurate speed range, the Frobenius norm is used as the optimization criterion of Λ, and the reachable set is modified by optimization. The generator matrix G + (Λ + )’s Frobenius norm, so The volume is as small as possible to obtain the optimal correction matrix as follows: Based on formula (14), the optimal corrected reachable set is: as follows: Through the reachable set Through the optimization calculation, the accurate speed range is obtained, thereby realizing effective speed monitoring.

6. The method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of TNL structure according to claim 1, characterized in that: The step 4 is specifically as follows: According to the properties of the minimum interval envelope and formula (1), the interval of the system state is obtained as follows: Among them, x - (i) and x + (i) represents the upper and lower bounds of the i-th component of the system state x, and c(i) represents the corrected reachable set center The i-th component of G(i,j) represents the corrected reachable set Generating Matrix The i-th row and j-th column of .

7. The method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of TNL structure according to claim 1, characterized in that: In step 4, when the component of the state is speed, the speed range of the underwater robot system is obtained; when the environment and noise are in the worst case, the speed range can ensure that the actual speed value of the underwater robot system must be within the estimated range.

Images