Centrosymmetric multi-cell Kalman filtering AUV speed estimation method based on T-N-L structure

Abstract

The invention discloses a centrosymmetric multi-cell Kalman filtering AUV (Autonomous Underwater Vehicle) speed estimation method based on a T-N-L structure. Firstly, a state space model of the autonomous underwater robot is obtained through identification according to experimental data, then a reachable set of the state of the AUV system is predicted and corrected by combining the property of a centrosymmetric polytope and the idea of Kalman filtering, the speed interval of the autonomous underwater robot is calculated through the corrected reachable set of the system, and effective monitoring of the speed of the autonomous underwater robot is achieved. The method is suitable for autonomous underwater robots of various configurations and motion conditions, compared with the prior art, the systematic and effective speed estimation method is provided, the adopted method can generate a more accurate speed estimation value and a more compact speed estimation interval, and the speed estimation accuracy is improved. The accuracy of speed state monitoring is improved, and the overall reliability and stability of an underwater robot system are remarkably enhanced by ensuring that behaviors of the robot in a complex underwater environment are more stable and predictable.

Description

Technical Field

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

[0002] The ocean, as the most vast natural area on earth, is rich in minerals, oil, natural gas and other resources. However, the seabed environment is extremely complex and is in extreme conditions of high pressure, low temperature and strong corrosion all year round, which poses great challenges to the exploration and development of seabed resources. Although traditional underwater robots have achieved certain results in shallow waters, their operational flexibility and stability in deep-sea exploration are still significantly limited. Against this background, autonomous underwater robots (AUVs) have emerged as a new generation of intelligent marine equipment. With their high degree of autonomy, excellent environmental adaptability and long-term deep-sea operations, AUVs are gradually becoming core equipment in seabed resource exploration, environmental monitoring and marine research.

[0003] AUVs work unmanned and without cables in complex marine environments and must ensure their own safety, which requires monitoring the state of AUVs. However, sensors for monitoring AUVs are usually expensive. In order to reduce costs, state estimation technology has been proposed to accurately monitor the state of AUVs. The development of this technology not only effectively reduces the hardware cost of the equipment, but also improves its economy and operability in practical applications. However, due to the influence of the environment, the results of state estimation may have a certain deviation from the true value. In order to mitigate the impact of uncertainty, robust state estimation methods have been widely studied. Robust state estimation methods are generally divided into two categories, namely stochastic methods and set membership estimation methods. In the process of estimating the state of AUVs, traditional stochastic methods need to assume that uncertainty obeys a known probability distribution, which may not be true in practical applications. The set membership estimation method realizes state estimation by constructing a set containing all possible state values. It assumes that uncertainty is unknown but bounded, which makes the conditions more relaxed and meets practical needs.

[0004] Set membership estimation methods can be classified by different sets, such as ellipsoids, polyhedra, intervals, and centrosymmetric polytopes. Among the existing geometric bodies, centrosymmetric polytopes have attracted widespread attention due to their ease of 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 a centrosymmetric polytope Kalman filter AUV speed estimation method based on TNL structure. By predicting and correcting the reachable set of the robot system state, the speed is estimated in intervals, thereby achieving effective monitoring of the AUV speed and significantly improving the stability and reliability of the AUV.

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

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

[0008] Step 1: Based on the "Beaver-II" AUV platform built in the laboratory, a systematic analysis of the AUV state space model is conducted;

[0009] Step 2: Based on the properties of the TNL structure and the centrosymmetric polyhedron, the reachable set of the “Beaver-II” AUV system state is predicted according to the state space model in step 1;

[0010] Step 3: Correct the reachable set of the “Beaver-II” AUV system state in step 2;

[0011] Step 4: Based on the optimal reachable set obtained in step 3, 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 1;

[0012] Step 5: Calculate the straight-line speed estimation result of the "Beaver-II" AUV system according to the interval of the underwater robot system state obtained in step 4.

[0013] Further, the step 1 is specifically:

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

[0015]

[0016] Among them, 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;

[0017] The errors generated during the identification process and the historical data of sensor measurement noise were analyzed; after removing the outliers with large amplitudes, the initial values ​​were determined, and the boundaries of the identification error and measurement noise met the following conditions:

[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 central symmetric polyhedron is defined as follows:

[0024] S-order centrosymmetric polytope It is a hypercube B s =[-1,+1] s The affine transformation of is 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] Based on the TNL structure, formula (1) is rewritten as follows:

[0029]

[0030] in

[0031]

[0032] According to TNL formula (5), we get:

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

[0034] According to formula (7) and the order reduction algorithm of the centrosymmetric polytope property, we get in

[0035]

[0036] Each iteration step The dimension of will increase. According to the reduced-order property, the reachable set in formula (8) Perform reduction to obtain in That is the predicted reachable set of system states.

[0037] Furthermore, the order reduction algorithm of the centrosymmetric polytope property is as follows:

[0038] 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

[0039]

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

[0041] 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), and the expression is as follows:

[0042]

[0043] when hour and The intersection of is difficult to calculate, so we will use a centrosymmetric polytope called a corrected reachable set to include and The intersection of To reduce the difficulty of calculation;

[0044] Corrected reachable set Center of + (Λ + ) and the generator matrix G + (Λ + )for:

[0045]

[0046] in, Λ is the correction matrix to be determined;

[0047] In order to obtain an 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 that 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, thus realizing the effective monitoring of the speed.

[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 Generate 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. The present invention proposes a centrosymmetric polyhedral Kalman filter AUV velocity estimation method based on TNL structure. This method is suitable for autonomous underwater robots 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 centrosymmetric polyhedral Kalman filter AUV speed estimation method based on TNL structure proposed in the present invention can generate more accurate speed estimation values ​​and more compact speed estimation intervals, which 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 robot's behavior in complex underwater environments is more stable and predictable.

[0060] 3. The centrosymmetric polytope Kalman filter AUV velocity estimation method based on TNL structure proposed in the present invention can effectively deal with various environmental noises and uncertainties, and has high robustness. Whether in a still water environment or in a dynamic and complex marine environment, this method can maintain a high state estimation performance.

[0061] 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

[0062] Figure 1 It is a flow chart of the centrosymmetric polyhedral Kalman filter velocity AUV estimation method based on TNL structure;

[0063] Figure 2 This is the overall structural diagram of the "Beaver-II" AUV system;

[0064] Figure 3 This is the motion trajectory diagram of the "Beaver-II" AUV system;

[0065] Figure 4 This is the estimated result of the direct flight speed range of the “Beaver-II” AUV system. DETAILED DESCRIPTION

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

[0067] The definitions, properties and lemmas required for the present invention are as follows:

[0068] To simplify notation, we omit the discrete time instant k, where the subscript - is used to denote the instant k-1 and the subscript + is used to denote the instant k+1.

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

[0070]

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

[0072] Property 1 (property of centrosymmetric polyhedron): For centrosymmetric polyhedron and The following properties hold:

[0073]

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

[0075] Property 2 (reduction algorithm): For a centrally symmetric polyhedron Given an integer q (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

[0076] Property 3 (minimum interval envelope): For a centrosymmetric polytope of order s Its minimum interval envelope It can be obtained by the following formula:

[0077]

[0078] 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.

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

[0080]

[0081] Among them, S∈R a×b is an arbitrary matrix.

[0082] Lemma 2: Given a centrosymmetric polytope and a strip Their intersection can be represented by a centrosymmetric polyhedron Contains, its center c(Λ) and generator matrix G(Λ) are expressed as follows:

[0083]

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

[0085] The specific implementation method of the centrosymmetric polytope Kalman filter AUV velocity estimation method based on TNL structure of the present invention is as follows:

[0086] The dimensions of the "Beaver-II" AUV are 0.8mx0.5mx0.45m, its mass in air is 50kg, and its operating time is 5 hours. Figure 2 As shown in the figure, 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 compartment and the electronic compartment are installed symmetrically on the upper part of the underwater robot, the digital compass is fixed on the upper part of the power compartment and the electronic compartment, and the depth gauge is installed on the lower part of the underwater robot.

[0087] The specific steps are as follows:

[0088] First, the "Beaver-II" AUV was tested in a water pool. During the experiment, the "Beaver-II" AUV started to reach the preset stable state and continued to operate in this state, showing a stable speed and attitude. The trajectory diagram is shown in the figure below. Figure 2 As shown. The straight-line navigation of the "Beaver-II" AUV is mainly achieved by the thrust of the two main thrusters, and the heading control is completed by adjusting the thrust difference of the two main thrusters. The thrust of the two main thrusters is determined by the control voltage, and the corresponding thrust models are:

[0089] The relationship between the left thruster control voltage and thrust:

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

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

[0092] The relationship between the right thruster control voltage and thrust:

[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, based on the water tank test data, the model of the horizontal motion state space of the "Beaver-II" AUV is identified. According to the straight-line speed ρ, the heading angle ψ, the left thruster thrust T1 and the right thruster thrust T2, the state space model of the AUV horizontal model is derived as follows:

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

[0098] in

[0099]

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

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

[0102] in

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

[0104] Based on the TNL structure, the state space model (30) is expressed as follows:

[0105]

[0106] in

[0107]

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

[0109]

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

[0111]

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

[0113]

[0114] in

[0115]

[0116] After determining the parameters of T and N, according to the state space model (30), we can obtain:

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

[0118] According to formula (40) and the properties of centrosymmetric polytopes, we can get in

[0119]

[0120] Considering that each iteration The dimension of will increase. According to the reduction property, we can transform the reachable set in (41) into Perform reduction to obtain in That is the predicted reachable set of system states.

[0121] From the state space model (30), we can get x + Not only satisfy It will also satisfy in is the strip obtained by the measurement equation in the state-space model (30), and its expression is as follows:

[0122]

[0123] Notice However, and The intersection of is difficult to calculate, so we will use a centrosymmetric polytope called a corrected reachable set to include and The intersection of To reduce the difficulty of the operation. According to Lemma 2, the correction reachable set Center of + (Λ + ) and the generator matrix G + (Λ + )for

[0124]

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

[0126] In fact, different Λ will form corrected reachable sets of different shapes and sizes. Therefore, it is very important 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 obtain an accurate speed range, this paper adopts the Frobenius norm as the optimization criterion of Λ. The generator matrix G + (Λ + )'s Frobenius norm, so that The size is as small as possible, as follows:

[0127] According to the definition of centrosymmetric polytope, the generator matrix G of the modified reachable set is + (Λ + ) is rewritten as follows:

[0128]

[0129] in,

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

[0131]

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

[0133]

[0134] According to (49), since G + (Λ + ) is related to the Frobenius norm of Λ + If it is a convex function and it is also a positive definite quadratic form, then it must have a unique minimum value. Get the optimal correction matrix as follows:

[0135]

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

[0137]

[0138] Through the reachable set Through the optimization calculation, we can get the accurate speed range, so as to realize the effective monitoring of 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 Generate 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, that is, 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] The sampling work starts from the start-up time, and the sampling frequency is 5Hz, that is, data is collected every 0.2 seconds, and a total of 200 cycles of data are obtained. Based on the identification and 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, the two red dashed lines represent the upper and lower bounds of the straight-line speed interval determined by the calculation method adopted by the present invention, and the two blue dashed lines represent the upper and lower bounds of the straight-line speed interval determined by the traditional speed estimation method without TNL structure. In addition, the cyan area 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-line speed of the underwater robot at each moment. Figure 4 It can be seen that the true value of the straight-line speed of the "Beaver-II" AUV is completely included in the interval of the method proposed in the present invention. At the same time, compared with the speed estimation method without TNL structure, the speed interval calculated by the method proposed in the present invention is significantly more compact, which shows that the method proposed in the present invention 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 robot's behavior in complex underwater environments is more stable and predictable.

[0154] In summary, the present invention aims at the problem that Doppler velocimeter is expensive and has limited measurement accuracy in certain environments, and proposes a centrosymmetric polyhedral Kalman filter AUV speed estimation method based on TNL structure. Firstly, the state space model of AUV is identified according to experimental data, and then the reachable set of AUV system state is predicted and corrected by combining the properties of centrosymmetric polyhedral and the idea of ​​Kalman filter. The direct navigation speed range of AUV is calculated through the corrected reachable set of the system, so as to realize effective monitoring of its speed and ensure the stability and reliability of AUV.

[0155] 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 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 is conducted; 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-II" AUV system state; Step 3: Correct the reachable set of the "Beaver-II" AUV system state in step 2; Step 4: Based on the optimal reachable set obtained in step 3, 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 1; Step 5: Calculate the estimated straight-line speed of the "Beaver-II" AUV 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 TNL structure according to claim 1 is characterized in that: Described step 1 is specifically: Based on the "Beaver-II" AUV platform built in the laboratory, the state space model of AUV is systematically analyzed, and its state space model is obtained through identification as follows: Among them, 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; The errors generated during the identification process and the historical data of sensor measurement noise were analyzed; after removing the outliers with large amplitudes, the initial values ​​were determined, and the boundaries of the identification error and measurement noise met the following conditions: 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 method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of TNL structure according to claim 2 is characterized in that: The definition of the centrosymmetric polytope is 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 TNL structure according to claim 1, characterized in that: The step 2 is specifically as follows: Based on the TNL structure, formula (1) is rewritten as follows: in According to TNL formula (5), we get: x + =(TE+NC)x + =TAx+TBu+Ny + +TD1w-ND2ν + (7) According to formula (7) and the order reduction algorithm of the centrosymmetric polytope property, we get in Each iteration step The dimension of will increase. According to the reduced-order property, the reachable set in formula (8) Perform reduction to obtain in That is the predicted reachable set of system states.

5. The method for AUV velocity estimation based on a centrosymmetric polyhedral Kalman filter of TNL structure according to claim 4 is 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 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 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), expressed as follows: when hour and The intersection of is difficult to calculate, so we will use a centrosymmetric polytope called a corrected reachable set to include and The intersection of To reduce the difficulty of calculation; Corrected reachable set Center of + (Λ + ) and the generator matrix G + (Λ + )for: in, Λ is the correction matrix to be determined; In order to obtain an 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 that 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, thus realizing the effective monitoring of the speed.

7. 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 Generate Matrix The i-th row and j-th column of .

8. 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