Underwater vehicle motion state estimation method based on whisker sensor array

Abstract

The invention discloses an underwater vehicle motion state estimation method based on a whisker sensor array. The underwater vehicle motion state estimation method comprises the steps of S1, collecting time sequence voltage signals of all whisker sensor nodes in the whisker sensor array; s2, performing sliding window variance calculation on the time sequence voltage signal to obtain a sliding variance signal; s3, constructing a whisker sensor node state conversion rule to determine the current state of each whisker sensor; s4, constructing a jet line detection alliance strategy of the underwater vehicle to obtain a jet line detection alliance and sensor feature node transfer sequence; s5, acquiring a jet line estimation optimization model according to the jet line detection alliance, and acquiring a jet line sequence according to the jet line estimation optimization model; s6, acquiring sensor array observation data of the underwater vehicle according to the jet line sequence and the sensor feature node transfer sequence; s7, constructing a state space vector equation and a state observation equation of Kalman filtering based on the CTRV motion model; and S8, based on a Kalman filtering algorithm, estimating the motion state of the underwater vehicle according to the state space vector equation and the state observation equation of Kalman filtering. The problem that the motion state of the underwater vehicle cannot be accurately predicted due to the fact that traditional wake flow detection technologies such as acoustics and optics are very limited due to the development of a stealth technology and a noise reduction technology of the modern underwater vehicle and the influence of actual underwater complex natural environment interference and artificial interference is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater vehicle motion state estimation, and in particular to an underwater vehicle motion state estimation method based on a whisker sensor array. Background Art

[0002] The commonly used underwater vehicle wake detection methods are: acoustic detection, optical detection, remote sensing detection, etc. Acoustic detection has a long detection distance, but in shallow seas with complex terrain, underwater reverberation will cause great interference to acoustic detection. Optical detection has good directionality and high sensitivity, but the laser has a high degree of loss in water. Remote sensing detection can detect and search a wide area at a certain height, but it is difficult to detect deep-water targets.

[0003] At present, with the development of stealth technology and noise reduction technology of modern underwater vehicles and their susceptibility to interference from complex natural environments and artificial interference in the actual underwater environment, traditional acoustic, optical and other wake detection technologies have become very limited in their effectiveness and are unable to accurately predict the motion state of underwater vehicles. Therefore, new methods and approaches need to be studied for the detection of underwater objects. Summary of the invention

[0004] The present invention provides a method for estimating the motion state of an underwater vehicle based on a whisker sensor array to overcome the above technical problems.

[0005] In order to achieve the above object, the technical solution of the present invention is:

[0006] A method for estimating the motion state of an underwater vehicle based on a whisker sensor array specifically comprises the following steps:

[0007] S1: setting the whisker sensor array and collecting the time series voltage signal of each whisker sensor node in the whisker sensor array;

[0008] S2: Perform sliding window variance calculation on the time series voltage signal to obtain a sliding variance signal;

[0009] S3: constructing a state transition rule of a whisker sensor node according to the sliding variance signal to determine the current state of each whisker sensor;

[0010] The current state includes a dormant state, an active state, and a waiting state;

[0011] S4: According to the current state of each whisker sensor, a jet line detection coalition strategy of the underwater vehicle is constructed to obtain the jet line detection coalition and sensor feature node transfer sequence;

[0012] S5: obtaining a jet line estimation optimization model according to a jet line detection alliance, and obtaining a jet line sequence according to the jet line estimation optimization model;

[0013] S6: acquiring sensor array observation data of the underwater vehicle according to the jet line sequence and the sensor characteristic node transfer sequence;

[0014] The sensor array observation data includes the heading angle observation value, yaw angular velocity observation value, vehicle speed observation value and position observation value of the underwater vehicle;

[0015] S7: Based on the CTRV motion model, the state space vector equation and state observation equation of the Kalman filter are constructed according to the sensor array observation data;

[0016] S8: Based on the Kalman filter algorithm, the motion state of the underwater vehicle is estimated according to the state space vector equation and state observation equation of the Kalman filter.

[0017] Furthermore, S2 specifically includes the following steps:

[0018] S21: define the sampling period of the whisker sensor node as T, and obtain discrete voltage data u(t) according to the sampled continuous voltage signal u(t) of the original whisker sensor node. t＝kT ,k=1,2,……;

[0019] And define the discrete voltage data u(t) sampled at time t = kT t＝kT Represented as the kth discrete signal u(kT);

[0020] And according to the discrete voltage data u(t) t＝kT Obtain the time series voltage signal sequence of the whisker sensor node, that is, D(kT) = D(t) | t＝kT ; where D(t)| t＝kT represents the sliding variance of the kth discrete signal u(kT) at time t=kT;

[0021] S22: setting the data window size of the sliding mathematical expectation to n, and performing sliding window variance calculation on the time series voltage signal in the time series voltage signal sequence to obtain a sliding variance signal;

[0022] And the expression for calculating the sliding window variance is

[0023]

[0024] Where: E(kT) represents the sliding mathematical expectation obtained according to the kth discrete voltage signal; D(kT) represents the sliding variance of the kth discrete voltage signal.

[0025] Furthermore, the state transition rule of the whisker sensor node constructed in S3 is specifically:

[0026] S31: define the node state of the whisker sensor node;

[0027] And the node state includes a dormant state S 1 , activation state S 2 And the waiting state S 3 ;

[0028] Set the signal threshold and sliding variance D(kT) of the whisker sensor node to maintain time T;

[0029] The maintenance time T includes a first maintenance time T1 and a second maintenance time T2;

[0030] And the signal threshold includes a signal amplitude threshold T h , wake-up time threshold T t + And the sleep time threshold T t - ;

[0031] S32: Setting the initial node state of the whisker sensor node to the dormant state S 1 , and obtain the sliding variance signal D(kT) of the whisker sensor node;

[0032] Determine the sliding variance signal D(kT) of the whisker sensor node and the signal amplitude threshold T h size;

[0033] If the sliding variance signal D(kT) of the whisker sensor node is ≤T h When the timer starts and reaches the first maintenance time T1, the node state of the whisker sensor node is changed from the sleep state S 1 Update to waiting state S 3 , and continue to execute S33;

[0034] If the sliding variance signal D(kT) of the whisker sensor node is greater than T h When the timer starts and reaches the second maintenance time T2, it is determined whether the second maintenance time T2 is equal to the wake-up time threshold T t + size;

[0035] And when the second holding time T2>T t + When the node state of the whisker sensor node changes from the sleep state S 1 Or wait state S 3 Update to active state S 2 ;

[0036] When the second maintenance time T2≤T t +When the node state of the whisker sensor node remains in the dormant state S 1 ;

[0037] S33: Determine whether the first maintenance time T1 exceeds the sleep time threshold T t - ;

[0038] If the first maintenance time T1 exceeds the sleep time threshold T t - When the node state of the whisker sensor node changes from the waiting state S 3 Update to sleep state S 1 ;

[0039] If the first maintenance time T1 does not exceed the sleep time threshold T t - When the node state of the whisker sensor node continues to remain in the waiting state S 3 .

[0040] Furthermore, the S4 specifically includes the following steps:

[0041] S41: Jet line detection alliance strategy based on underwater vehicles, and according to the activated state S 2 The whisker sensor node below obtains the jet line detection alliance;

[0042] The underwater vehicle jet line detection alliance strategy includes detection alliance establishment rules, alliance member joining rules and alliance member exit rules;

[0043] Detection alliance establishment rules: When a certain whisker sensor node is activated, confirm whether multiple whisker sensor nodes adjacent to the whisker sensor node are also in an activated state;

[0044] If yes, the whisker sensor node and its adjacent whisker sensor nodes are defined as an alliance relationship and serve as alliance member nodes; if no, the whisker sensor node and its adjacent whisker sensor nodes are defined as non-alliance relationship and the whisker sensor nodes adjacent to the whisker sensor node are served as non-alliance member nodes;

[0045] Get the current jet line detection alliance according to the alliance member nodes;

[0046] The alliance member joining rule: confirm whether the neighboring nodes of each alliance member node in the current jet line detection alliance are activated;

[0047] wherein the neighbor nodes do not include the whisker sensor nodes in the current jet line detection alliance;

[0048] If so, the neighbor node is used as a new member node of the jet line detection alliance to update the current jet line detection alliance;

[0049] If not, the neighbor node is not considered as a new member node of the jet line detection alliance;

[0050] The alliance member exit rule: The node state of the current jet line detection alliance is confirmed to be in the dormant state S 1 , then the whisker sensor node is regarded as a non-alliance member node and exits the current jet line detection alliance to update the current jet line detection alliance;

[0051] S42: Based on the jet line detection alliance strategy of the underwater vehicle, the newly activated tentacle sensor nodes are screened to obtain the sensor characteristic node transfer sequence:

[0052] The newly activated whisker sensor node is a new member node of the jet line detection alliance;

[0053] The node screening method is:

[0054] Project the position of each whisker sensor node in the current jet line detection alliance onto the straight line where the jet line vector of the underwater vehicle is located to obtain the projection point;

[0055] Define the direction of the underwater vehicle jet line vector as the positive direction;

[0056] If the projection point of the newly added member node is behind the projection point of any alliance member node in the current jet line detection alliance, the newly added alliance member node will be used as a sensor feature node close to the spacecraft thruster;

[0057] And the coordinates of the sensor feature nodes are marked as

[0058] in Indicates that the nth whisker sensor node is The time instants are marked as the coordinates of the sensor feature nodes, and Respectively represent the position abscissa and position ordinate of the sensor feature node;

[0059] According to the sensor feature nodes, the sensor feature node transfer sequence is recorded as And the corresponding sensor feature node transfer sequence is recorded as

[0060] Furthermore, the S5 specifically includes the following steps:

[0061] S51: Obtain a jet line estimation optimization model according to the jet line detection alliance, and the expression of the jet line estimation optimization model is:

[0062]

[0063] Where: P(kT) represents any point on the line where the underwater vehicle jet line is located; V(kT) represents the normal vector of the line where the underwater vehicle jet line is located; Φ(V(kT), P(kT)) represents the loss function of weighted orthogonal regression; N k represents the number of sensor nodes in the jet line detection alliance; ρ i (kT) represents the weight of the whisker sensor node, and ρ i (kT) = ρ(D i (kT), μ, λ); μ, λ represent the parameters determined by the structure of the whisker sensor and the environment respectively; P i represents the coordinates of the whisker sensor node; x i,min ,x i,max ,y i,min ,y i,max They represent the minimum value of the horizontal coordinate, the maximum value of the horizontal coordinate, the minimum value of the vertical coordinate, and the maximum value of the vertical coordinate of all the whisker sensor nodes in the jet line detection alliance respectively; p x (kT),p y (kT) represents the abscissa and ordinate components of P(kT);

[0064] S52: Obtain a jet line sequence according to the jet line estimation optimization model, and the expression of the jet line sequence is:

[0065] J={J(kT)|k=1,2,......}

[0066] J(kT)=(P(kT) T ,V(kT) T ) T

[0067] Where: J represents the jet line sequence; J(kT) represents any jet line vector in the jet line sequence; P(kT) T ,V(kT) T They represent the transpose of P(kT) and V(kT) respectively.

[0068] Further, in S6, sensor array observation data of the underwater vehicle is obtained according to the jet line sequence and the sensor characteristic node transfer sequence;

[0069] The heading angle observation value θ of the underwater vehicle z The expression of (kT) is

[0070]

[0071] L(kT)=(l x(kT),l y (kT)) T

[0072] L'(kT)=(l x '(kT),l y '(kT)) T =(-l x (kT),-l y (kT)) T

[0073] Where: L(kT) represents the direction vector of the jet line vector at each moment in the jet line sequence; l x (kT),l y (kT) represents the horizontal and vertical components of L(kT) respectively; L'(kT) represents the opposite direction vector of the jet line direction;

[0074] The yaw angular velocity observation value ω of the underwater vehicle z The expression of (kT) is

[0075]

[0076] Where: T represents the sampling period;

[0077] The vehicle speed observation value of the underwater vehicle The expression is

[0078]

[0079] Where: t d n represents the transfer direction vector of the sensor feature node; α(kT) represents the vector t d n With the jet line sequence The angle sequence of the direction vector of each jet line in , and d(kT) represents the intermediate sequence, and

[0080] The position observation value of the underwater vehicle The expression is

[0081]

[0082] h p(kT)=( h x(kT), h y(kT)) T

[0083] Where: h p(kT) represents the foot coordinate, which is the characteristic node The jet line sequence with a time interval of s periods before time t The foot of the perpendicular line to the straight line where each jet line lies; h x(kT), h y(kT) represents the horizontal and vertical coordinates of the foot of perpendicular respectively; They represent the average values ​​of the perpendicular coordinates.

[0084] Furthermore, the S7 specifically includes the following steps:

[0085] S71: Based on the sensor array observation data, the state vector X(kT) of the underwater vehicle at time kT is expressed as

[0086] X(kT)=(x(kT) y(kT) v(kT) θ(kT) ω(kT)) T

[0087] Among them, x(kT), y(kT), v(kT), θ(kT), ω(kT) represent the position observation value, vehicle velocity observation value, heading angle observation value and yaw angular velocity observation value of the sensor array of the underwater vehicle at time kT respectively;

[0088] S72: Based on the CTRV motion model, the CTRV state space vector equation is constructed according to the state vector of the underwater vehicle, and its expression is:

[0089]

[0090] Where: X((k+1)T) represents the state space vector of the underwater vehicle at time (k+1)T; r(kT) represents the state vector noise; X(kT) represents the state vector of the underwater vehicle at time kT;

[0091] S73: Perform local linearization processing on the CTRV state space vector equation to obtain the state transfer matrix A(kT) of the underwater vehicle, which is expressed as follows:

[0092]

[0093] S74: Based on the Jacobi matrix A(kT), the CTRV state space vector equation is rewritten as the state space vector equation of the Kalman filter, which is expressed as

[0094] X((k+1)T)=A(kT)X(kT)+υ(kT)

[0095] Where: υ(kT) represents state noise;

[0096] S75: Obtain the state observation equation based on the state space vector equation of Kalman filtering, which is expressed as follows:

[0097] Y(kT)=HX(kT)+w(kT)

[0098] Where: Y(kT) represents the theoretical observation value of the underwater vehicle; H represents the 5*5 unit matrix; w(kT) represents the observation noise of the state component, and w(kT) = (w x (kT),w y (kT),w v (kT),w θ (kT),w ω (kT)).

[0099] Furthermore, the S8 specifically includes the following steps:

[0100] S81: Define the initial state vector of the state space vector X(kT) involved in the Kalman filter iteration as X + (kT), the initial covariance matrix is ​​P + (kT);

[0101] S82: According to the initial state vector X + (kT) and the initial covariance matrix P + (kT), obtain the prior estimate of the underwater vehicle state vector X - ((k+1)T) and the prior estimate P of the initial covariance matrix - ((k+1)T), which is expressed as

[0102] X - ((k+1)T)=A(kT)X + (kT)

[0103] P - ((k+1)T)=A T (kT)P + (kT)A(kT)+Q(kT)

[0104] Where: A T (kT) represents the transpose of A(kT); Q(kT) represents the covariance matrix corresponding to the state noise when using the CTRV model, and

[0105]

[0106] They represent the variance of the underwater vehicle linear acceleration and yaw acceleration noise respectively;

[0107] S83: According to the prior estimate P of the covariance matrix - ((k+1)T) obtains the Kalman gain, which is expressed as

[0108]

[0109] Where: R((k+1)T) represents the covariance matrix of the state observation equation, and They represent the observation noise w of each state component at time ((k+1)T) respectively. x ((k+1)T),w y ((k+1)T),w v ((k+1)T),w θ ((k+1)T),w ω The variance of ((k+1)T);

[0110] S84: Confirm the actual observation value vector Z((k+1)T) of the underwater vehicle at the current moment, which is expressed as

[0111]

[0112] Where: Represents each observation component updated according to the state observation equation; x - ((k+1)T),y - ((k+1)T),v - ((k+1)T) represents the prior estimate X - ((k+1)T) obtained prior value;

[0113] S85: Obtain the current a posteriori estimate X of the motion state vector of the underwater vehicle based on the actual observation value vector Z((k+1)T) and combined with the Kalman gain + ((k+1)T), which is expressed as

[0114] X + ((k+1)T)=X - ((k+1)T)+K((k+1)T)[Z((k+1)T)-HX - ((k+1)T)]

[0115] And the current posterior estimate X + ((k+1)T) is used as the estimation of the motion state of the underwater vehicle at the current iteration;

[0116] When the next iteration needs to be performed, step S86 is executed;

[0117] S86: Based on the prior estimate P of the initial covariance matrix - ((k+1)T) obtains the updated covariance matrix, which is expressed as

[0118] P +((k+1)T)=(IK((k+1)T)H((k+1)T))P - ((k+1)T)

[0119] The current a posteriori estimate of the underwater vehicle motion state vector X + ((k+1)T) and the updated covariance matrix are used as inputs of the Kalman filter algorithm again, and S82 to S85 are repeated.

[0120] Beneficial effects: The present invention provides a method for estimating the motion state of an underwater vehicle based on a whisker sensor array, which uses the signal of the whisker sensor array to observe the motion direction of the underwater vehicle, and combines the transfer of activated sensors in the sensor array to observe the running speed and trajectory of the underwater vehicle, and then estimates the motion state of the underwater vehicle through a Kalman filter algorithm based on the CTRV uniform speed constant turning rate model, thereby solving the problem that the underwater vehicle is easily affected by the actual complex underwater natural environment and artificial interference, and greatly improving the prediction accuracy of the motion state of the underwater vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0121] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0122] Figure 1 It is a flow chart of the method for estimating the motion state of an underwater vehicle based on a whisker sensor array of the present invention;

[0123] Figure 2 Schematic diagram of the state transition rules of the whisker sensor node in this embodiment;

[0124] Figure 3 is a schematic diagram of the jet line detection alliance in this embodiment;

[0125] Figure 4 Schematic diagram of the jet line vector in this embodiment;

[0126] Figure 5 is a schematic diagram of jet line estimation based on orthogonal regression in this embodiment;

[0127] Figure 6 This is a first exemplary schematic diagram of a node transfer direction in this embodiment;

[0128] Figure 7 This is a second exemplary schematic diagram of the node transfer direction in this embodiment;

[0129] Figure 8 This is a schematic diagram of obtaining the observed value of the aircraft speed in this embodiment;

[0130] Fig. 9 Schematic diagram of the observation data timing in this embodiment;

[0131] Fig.10 Schematic diagram of the topological connection relationship of sensor nodes in this embodiment;

[0132] Fig.11 This is a core block diagram of the underwater vehicle motion state estimation method based on the whisker sensor array in this embodiment. DETAILED DESCRIPTION

[0133] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0134] This embodiment provides a method for estimating the motion state of an underwater vehicle based on a whisker sensor array. Figure 1 As shown, the specific steps include:

[0135] S1: setting the whisker sensor array and collecting the time series voltage signal of each whisker sensor node in the whisker sensor array;

[0136] Specifically, after being disturbed by the flow field environment, the whisker sensor will undergo a certain degree of physical oscillation, which will cause the sensing unit in the sensor to deform accordingly and generate a signal with a certain degree of fluctuation. Since the closer the whisker sensor is to the excitation source in the axial direction and the closer it is to the center line of the jet in the radial direction, the greater the force of the jet on the whisker sensor, which is reflected in the signal as the greater the degree of fluctuation of the voltage signal. Therefore, the magnitude of the disturbance to the sensor can be characterized by analyzing the variance of the sensor output signal, and the whisker sensors can be distributed in a certain space range underwater at a certain density. The array formed is called a whisker sensor array. The whisker sensor array can be used to sense the intensity distribution of the underwater flow field, and then sense the vehicle in the underwater environment;

[0137] S2: Since the signal output by the sensor is a time series signal, the sliding window variance method is used to calculate the variance, and the sliding window variance calculation is performed on the time series voltage signal to obtain a sliding variance signal;

[0138] The specific steps include:

[0139] S21: define the sampling period of the whisker sensor node as T, and obtain discrete voltage data u(t) according to the sampled continuous voltage signal u(t) of the original whisker sensor node. t＝kT ,k=1,2,……;

[0140] And define the discrete voltage data u(t) sampled at time t = kT t＝kT Represented as the kth discrete signal u(kT);

[0141] And according to the discrete voltage data u(t) t＝kT Obtain the time series voltage signal sequence of the whisker sensor node, that is, D(kT) = D(t) | t＝kT ; where D(t)| t＝kT represents the sliding variance of the kth discrete signal u(kT) at time t=kT;

[0142] In this embodiment, it is assumed that the sampling period of the sensor is T. After sampling, the original continuous voltage signal u(t) is converted into discrete voltage data. For the convenience of description, this embodiment assumes that the system sampling period T remains unchanged and only considers the discrete signal after sampling. The signal u(t) obtained by sampling at the specified time t=kT is t＝kT It is equivalent to the kth discrete signal u(kT), where k = 1, 2, ..., and the same method will be used to represent the timing of discrete signals or data in the future. For example, D(kT) represents the sliding variance result calculated at time t = kT, which is equivalent to D(t)| t＝kT ;

[0143] S22: setting the data window size of the sliding mathematical expectation to n, and performing sliding window variance calculation on the time series voltage signal in the time series voltage signal sequence to obtain a sliding variance signal;

[0144] And the expression for calculating the sliding window variance is

[0145]

[0146] Where: E(kT) represents the sliding mathematical expectation obtained from the kth discrete voltage signal; D(kT) represents the sliding variance of the kth discrete voltage signal. After calculating the sliding variance, this embodiment converts the discrete voltage data into discrete variance data to describe the degree of change of the sensor signal;

[0147] S3: constructing a state transition rule of a whisker sensor node according to the sliding variance signal to determine the current state of each whisker sensor;

[0148] The current state includes a dormant state, an active state, and a waiting state;

[0149] Specifically, Figure 2 As shown, the state transition rule of the whisker sensor node constructed is:

[0150] S31: define the node state of the whisker sensor node;

[0151] And the node state includes a dormant state S 1 , activation state S 2 And the waiting state S 3 ;

[0152] Set the signal threshold and sliding variance D(kT) of the whisker sensor node to maintain time T;

[0153] The maintenance time T includes a first maintenance time T1 and a second maintenance time T2;

[0154] And the signal threshold includes a signal amplitude threshold T h , wake-up time threshold T t + And the sleep time threshold T t - ;

[0155] S32: Setting the initial node state of the whisker sensor node to the dormant state S 1 , and obtain the sliding variance signal D(kT) of the whisker sensor node;

[0156] Determine the sliding variance signal D(kT) of the whisker sensor node and the signal amplitude threshold T h size;

[0157] If the sliding variance signal D(kT) of the whisker sensor node is ≤T h When the timer starts and reaches the first maintenance time T1, the node state of the whisker sensor node is changed from the sleep state S 1 Update to waiting state S 3 , and continue to execute S33;

[0158] If the sliding variance signal D(kT) of the whisker sensor node is greater than T h When the timer starts and reaches the second maintenance time T2, it is determined whether the second maintenance time T2 is equal to the wake-up time threshold T t + size;

[0159] And when the second holding time T2>T t + When the node state of the whisker sensor node changes from the sleep state S 1 Or wait state S 3 Update to active state S2 ;

[0160] When the second maintenance time T2≤T t + When the node state of the whisker sensor node remains in the dormant state S 1 ;

[0161] S33: Determine whether the first maintenance time T1 exceeds the sleep time threshold T t - ;

[0162] If the first maintenance time T1 exceeds the sleep time threshold T t - When the node state of the whisker sensor node changes from the waiting state S 3 Update to sleep state S 1 ;

[0163] If the first maintenance time T1 does not exceed the sleep time threshold T t - When the node state of the whisker sensor node continues to remain in the waiting state S 3 .

[0164] In this embodiment, since the environmental noise is generally random and has little correlation, the environmental noise should cause the sensor array to generate a signal briefly, and the correlation between sensors is small. In this regard, three thresholds are set, namely, the signal amplitude threshold T h , wake-up time threshold T t + And the sleep time threshold T t - In this embodiment, a signal whose variance and holding time are greater than a certain threshold is defined as a valid signal, that is, when the variance signal D(kT)>T h When the variance signal is greater than the threshold T h The maintenance time T exceeds the wake-up time threshold T t + , the sensor state changes to the activated state S 2 The activated variance signal participates in the subsequent calculation and estimation of the motion state of the spacecraft. When the signal amplitude is lower than the threshold T h When the sensor enters the waiting state S 3 , start timing, when the maintenance time T exceeds the sleep time threshold T t - When the sensor state changes to the sleep state S 1 ,The signals generated by the sensors in the waiting state also participate in the subsequent calculations and also in the estimation of the motion state of the aircraft;

[0165] S4: According to the current state of each whisker sensor, a jet line detection coalition strategy of the underwater vehicle is constructed to obtain the jet line detection coalition and sensor feature node transfer sequence;

[0166] like Figure 3 As shown, the specific steps include:

[0167] S41: Considering that the jet generated by the underwater vehicle excites the whisker sensor array, it is in the activated state S 2 The whisker sensor nodes below are spatially concentrated;

[0168] Based on the jet line detection alliance strategy of underwater vehicles, and according to the activated state S 2 The whisker sensor node below obtains the jet line detection alliance;

[0169] The underwater vehicle jet line detection alliance strategy includes detection alliance establishment rules, alliance member joining rules and alliance member exit rules;

[0170] Detection alliance establishment rules: When a certain whisker sensor node is activated, confirm whether multiple whisker sensor nodes adjacent to the whisker sensor node are also in an activated state;

[0171] If yes, the whisker sensor node and its adjacent whisker sensor nodes are defined as an alliance relationship and serve as alliance member nodes; if no, the whisker sensor node and its adjacent whisker sensor nodes are defined as non-alliance relationship and the whisker sensor nodes adjacent to the whisker sensor node are served as non-alliance member nodes;

[0172] Get the current jet line detection alliance according to the alliance member nodes;

[0173] The alliance member joining rule: confirm whether the neighboring nodes of each alliance member node in the current jet line detection alliance are activated;

[0174] wherein the neighbor nodes do not include the whisker sensor nodes in the current jet line detection alliance;

[0175] If so, the neighbor node is used as a new member node of the jet line detection alliance to update the current jet line detection alliance;

[0176] If not, the neighbor node is not considered as a new member node of the jet line detection alliance;

[0177] The alliance member exit rule: The node state of the current jet line detection alliance is confirmed to be in the dormant state S 1 , then the whisker sensor node is regarded as a non-alliance member node and exits the current jet line detection alliance to update the current jet line detection alliance;

[0178] S42: Based on the jet line detection alliance strategy of the underwater vehicle, the newly activated tentacle sensor nodes are screened to obtain the sensor characteristic node transfer sequence:

[0179] The newly activated whisker sensor node is a new member node of the jet line detection alliance;

[0180] The node screening method is:

[0181] Project the position of each whisker sensor node in the current jet line detection alliance onto the straight line where the jet line vector of the underwater vehicle is located to obtain the projection point;

[0182] Define the direction of the underwater vehicle jet line vector as the positive direction;

[0183] If the projection point of the newly added member node is behind the projection point of any alliance member node in the current jet line detection alliance, the newly added alliance member node will be used as a sensor feature node close to the spacecraft thruster;

[0184] In this embodiment, when the underwater vehicle moves in the sensor array, it will activate sensors in sequence on its motion trajectory, so that new members join the jet line detection alliance. The position of the newly activated sensor has a certain correlation with the position of the vehicle. Considering that the vehicle is moving, the sensor newly added to the jet line detection alliance is not necessarily a sensor near the vehicle's propeller. Therefore, it is necessary to screen the newly activated sensors. The screening method is: project all node positions on the straight line where the jet line vector is located, with the direction of the jet line vector as the positive direction. If the projection point of the newly added member is behind the projection point of any member, it can be considered that the newly added member node is a node close to the vehicle's propeller, which is called a sensor feature node. This process is shown in the figure. Figure 6 At the moment shown, node N is the current feature node. When the vehicle moves forward, nodes A and B are also activated, as shown in Figure 7 As shown in the figure, at this time, since the projection of node B on the jet line is at the rear of all the node projections, node B is considered to be a new feature node. The transfer direction of the feature node is from N to B;

[0185] And the coordinates of the sensor feature nodes are marked as

[0186] in Indicates that the nth whisker sensor node is The time instants are marked as the coordinates of the sensor feature nodes, and Respectively represent the position abscissa and position ordinate of the sensor feature node;

[0187] According to the sensor feature nodes, the sensor feature node transfer sequence is recorded as And the corresponding sensor feature node transfer sequence is recorded as

[0188] S5: obtaining a jet line estimation optimization model according to a jet line detection alliance, and obtaining a jet line sequence according to the jet line estimation optimization model;

[0189] like Figure 4 As shown, the specific steps include:

[0190] S51: Assume that at time t=kT, the coordinates of point P are P(kT)=(p x (kT),p y (kT)) T , is any point on the straight line where the jet line lies, where p x (kT),p y (kT) are the horizontal and vertical components of the coordinates. Since they determine the position of the jet line in the coordinate system, P(kT) is called the anchor point of the jet line. The vector V(kT) that is 90° ahead of the jet line direction vector L(kT) is selected as the normal vector of the line where the vector L(kT) is located. According to the right-hand rule, V(kT) = (v x (kT),v y (kT)) T To indicate. x (kT),v y (kT) respectively represent (components of the normal vector), then a jet line can be represented by the vector J(kT) = (P(kT) T ,V(kT) T ) T to describe;

[0191] Direction vector of the jet line vector

[0192] The equation of the line on which the jet line lies can be expressed using the point method: ((x,y)-P(kT)) T V(kT) T =0;

[0193] Assume that at time kT, the number of sensor nodes in the jet line detection alliance is N k The coordinates of the sensor node in the Cartesian coordinate system are P i =(x i ,y i ) T ,i=1,2,...,N k , where x i ,y iThey represent the horizontal and vertical coordinates of the sensor in the geodetic coordinate system. The variance of the signal of the i-th sensor node is D i (kT), the purpose of this implementation is to estimate the current jet line vector based on the known size of the sensor node variance signal and the location of the sensor node. By adopting the idea of ​​weighted orthogonal regression, the sensor node is regarded as a coordinate point with weights, and weighted orthogonal regression is performed on these coordinate points. The specific process is as follows:

[0194] The weight of the sensor node is defined as ρ i (kT) = ρ(D i (kT), μ, λ), the weight is a function of the variance, where μ and λ are parameters determined by the sensor structure and environment, respectively, and participate in and affect the calculation of the weight. i (kT) and variance D i (kT) is positively correlated, that is, the larger the variance of the sensor signal, the more violent the sensor fluctuation. Therefore, the closer the sensor node is to the excitation source, the larger the weight; the closer the sensor node is to the jet centerline, the larger the weight; μ represents the structural parameters, which are determined by the structure of the sensor, such as the length, thickness and material of the sensor; λ represents the environmental parameters, which are determined by the environment, such as liquid density, depth, etc.; using the geodetic reference system, the coordinates of the sensor node are the sample points in the reference system, then the error of each sample point in the orthogonal regression can be expressed as:

[0195] e i (kT)=||V(kT)V(kT) T (P i -P(kT))|| 2

[0196] Among them, P i represents the coordinates of sensor node i;

[0197] Its geometric meaning is the square of the vertical distance from the sample point to the jet line;

[0198] After considering the weights, the loss function of weighted orthogonal regression is:

[0199]

[0200] Among them, N k represents the number of sensor nodes in the jet line detection alliance;

[0201] Since for the point method, the fixed point in the expression can be taken arbitrarily on the line, and the modulus of the normal vector can be taken arbitrarily, here we can limit the fixed point to the maximum range of the coordinates of the jet line detection alliance, and the normal vector is a unit vector, so the problem of estimating the jet line is transformed into an optimization problem:

[0202] The jet line estimation optimization model is obtained according to the jet line detection alliance, and the expression of the jet line estimation optimization model is:

[0203]

[0204] Where: P(kT) represents any point on the line where the underwater vehicle jet line is located; V(kT) represents the normal vector of the line where the underwater vehicle jet line is located; Φ(V(kT), P(kT)) represents the loss function of weighted orthogonal regression; N k represents the number of sensor nodes in the jet line detection alliance; ρ i (kT) represents the weight of the whisker sensor node, and ρ i (kT) = ρ(D i (kT), μ, λ); μ, λ represent the parameters determined by the structure of the whisker sensor and the environment respectively; P i represents the coordinates of the whisker sensor node; x i,min ,x i,max ,y i,min ,y i,max They represent the minimum value of the horizontal coordinate, the maximum value of the horizontal coordinate, the minimum value of the vertical coordinate, and the maximum value of the vertical coordinate of all the whisker sensor nodes in the jet line detection alliance respectively; p x (kT),p y (kT) represents the abscissa and ordinate components of P(kT);

[0205] The weighted orthogonal regression process in this embodiment can be geometrically explained as finding a straight line that minimizes the weighted sum of the distances from all sample points to the straight line. In the process of jet line estimation, the sensor nodes provide fixed sample point coordinates, and the variance of the sensor signal provides the weight of the sample point. Since the sensor nodes and sensor signal variances involved in the orthogonal regression are both determined by the jet generated by the aircraft, the estimated jet line can reflect the direction and position information of the aircraft jet. The schematic diagram of jet line estimation using orthogonal regression is shown in FIG. Figure 5 shown.

[0206] S52: Obtain a jet line sequence according to the jet line estimation optimization model, and the expression of the jet line sequence is:

[0207] J={J(kT)|k=1,2,......}

[0208] J(kT)=(P(kT) T ,V(kT) T ) T

[0209] Where: J represents the jet line sequence; J(kT) represents any jet line vector in the jet line sequence; P(kT) T ,V(kT) T They represent the transpose of P(kT) and V(kT) respectively.

[0210] S6: acquiring sensor array observation data of the underwater vehicle according to the jet line sequence and the sensor characteristic node transfer sequence;

[0211] The sensor array observation data includes the heading angle observation value, yaw angular velocity observation value, vehicle speed observation value and position observation value of the underwater vehicle;

[0212] Specifically, since the jet line vector sequence contains the heading and yaw angular velocity information of the aircraft during motion, the characteristic node transfer sequence contains the position and velocity information of the aircraft during motion. For the jet line vector sequence, this embodiment can observe the yaw angular velocity of the aircraft through the data in the sequence. For the data J={J(kT)|k=1,2,......} in the jet line sequence, this embodiment focuses on the direction vector L(kT)=(l x (kT),l y (kT)) T Since the opposite direction of the jet exit direction reflects the navigation direction of the aircraft, the opposite direction of the jet line direction is used in this embodiment.

[0213] L'(kT)=(l x '(kT),-l y '(kT)) T =(-l x (kT),-l y (kT)) T As the observed value of the vehicle's navigation angle;

[0214] The heading angle observation value θ of the underwater vehicle z The expression of (kT) is

[0215]

[0216] L(kT)=(l x (kT),l y (kT)) T

[0217] L'(kT)=(l x '(kT),l y '(kT)) T =(-l x (kT),-l y (kT)) T

[0218] Where: L(kT) represents the direction vector of the jet line vector at each moment in the jet line sequence; l x (kT),l y (kT) represents the horizontal and vertical components of L(kT) respectively; L'(kT) represents the opposite direction vector of the jet line direction;

[0219] In this embodiment, the normal vectors of the jet line sequence are processed in this way, and a sequence of angle information of the jet line sequence can be obtained: θ z ={θ z (kT)|k=1,2,.....};

[0220] Calculate the rate of change of the jet line vector angle:

[0221] Then we get the data sequence of the jet line vector angle change rate: ω z ={ω z (kT)|k=1,2,......}; Because the angular velocity information is available only when the angle information changes, the data of this sequence starts from time t=2. The sequence of the angle information of the jet line sequence calculated above is: θ z ={θ z (kT)|k=1,2,.....} and the data sequence of the rate of change of the jet line vector angle: ω z ={ω z (kT)|k=1,2,......}are respectively the observation values ​​of the sensor array for the navigation angle and angular velocity of the spacecraft;

[0222] The function of the node transfer sequence is to estimate the speed of the vehicle. Assuming that the characteristic node is Transfer to its neighbor node In the time period [ t k n-1 T, t k n T], m jet line vector estimations occurred, and a sequence containing m jet line estimation data was obtained:

[0223] The transfer direction vector of the feature node is defined as t d n = t P n - t P n-1 ,vector t d n and the jet line sequence The angle between the direction vectors of each jet line in is

[0224] make Get a new sequence

[0225] The average speed of the vehicle during the characteristic node transfer is calculated as

[0226]

[0227] The calculated As the observed value of the spacecraft speed, all the calculated average speeds form the sequence It is called the sequence of vehicle velocity observations;

[0228] The above process of finding the average speed can be understood geometrically as moving the node in the direction of the time period After projecting onto each microelement of the arc of the spacecraft trajectory and then averaging, the average value of the spacecraft linear velocity in this time period is obtained, as shown in Figure 8 shown.

[0229] In this embodiment, when a feature node is transferred, it means that a spacecraft is passing near the new feature node. The transfer direction of the feature node and the jet line vector are used to indicate the position of the spacecraft when waking up the feature node. like The jet line at time Draw a perpendicular line from the characteristic node to the straight line where the jet line is located, and the coordinates of the foot of the perpendicular reflect the position where the spacecraft passes;

[0230] Due to the low update frequency of the aircraft position observation value, the estimation of the aircraft position mainly uses the prior estimate value. The a posteriori estimate value is used only when the aircraft position observation value is updated. In addition, the use of jet lines will interfere with the estimation of the aircraft state to a certain extent. Before the position observation value participates in the a posteriori estimate, the interference of environmental noise should be reduced as much as possible. This implementation adopts the method of averaging to reduce interference: for the characteristic node In this implementation, the jet line sequence with a time interval of s cycles before the moment is taken Find the foot of the perpendicular line of the characteristic node respectively, and the coordinates of the foot of the perpendicular are

[0231]

[0232] in h p(kT)=( h x(kT), h y(kT)) T ;

[0233] Average its coordinates

[0234] get As the sensor array's observations of the aircraft's position. All calculated aircraft position observations form the sequence It is called the spacecraft position observation sequence;

[0235] In summary, the data observed by the sensor array include:

[0236] The sequence of vehicle heading observations θ z ={θ z (kT)|k=1,2,.....}

[0237] The sequence of spacecraft angular velocity observations ω z ={ω z (kT)|k=1,2,......}

[0238] Aircraft velocity observation sequence

[0239] Aircraft position

[0240] like Fig. 9 The above observation value update moments can be represented on a timing diagram:

[0241] And because of the time Essentially, it is the sampling time point when “a feature node transfer occurs”, so the time series Contained in {kT|k=1,2,......}, so the update of the observation values ​​of the spacecraft speed and position is more sparse in time series than the update of the angle and angular velocity observation values. The update frequency of the spacecraft angle observation value is consistent with the sampling frequency of the system, and because the calculation of angular velocity requires the difference of the angle observation results of two adjacent moments, the angular velocity observation value is updated from the moment k=2;

[0242] S7: Based on the CTRV motion model, the state space vector equation and state observation equation of the Kalman filter are constructed according to the sensor array observation data;

[0243] The specific steps include:

[0244] S71: In the CTRV motion model, it is assumed that the spacecraft does not change in depth, that is, the spacecraft moves on a two-dimensional horizontal plane. This implementation uses the CTRV model (constant turning rate uniform circular motion model) to describe the motion state of the spacecraft on the two-dimensional plane. The CTRV model believes that the motion state of the spacecraft can be described by five parameters, namely, position (horizontal and vertical coordinates) p(kT) = (x(kT), y(kT)) T, linear velocity v(kT), angle θ(kT), angular velocity ω(kT), then according to the sensor array observation data, the state vector X(kT) of the underwater vehicle at time kT is expressed as

[0245] X(kT)=(x(kT) y(kT) v(kT) θ(kT) ω(kT)) T

[0246] Among them, x(kT), y(kT), v(kT), θ(kT), ω(kT) represent the position observation value, vehicle velocity observation value, heading angle observation value and yaw angular velocity observation value of the sensor array of the underwater vehicle at time kT respectively;

[0247] S72: Based on the CTRV motion model, the CTRV state space vector equation is constructed according to the state vector of the underwater vehicle, and its expression is:

[0248]

[0249] Where: X((k+1)T) represents the state space vector of the underwater vehicle at time (k+1)T; r(kT) represents the state vector noise; X(kT) represents the state vector of the underwater vehicle at time kT;

[0250] S73: Perform local linearization processing on the CTRV state space vector equation to obtain the state transfer matrix A(kT) of the underwater vehicle, which is expressed as follows:

[0251]

[0252] S74: Based on the Jacobi matrix A(kT), the CTRV state space vector equation is rewritten as the state space vector equation of the Kalman filter, and its expression is

[0253] X((k+1)T)=A(kT)X(kT)+υ(kT)

[0254] Where: υ(kT) represents state noise;

[0255] The noise introduced in the CTRV model in this embodiment mainly comes from two sources: the linear acceleration and yaw acceleration noise of the underwater vehicle. This embodiment assumes that the linear acceleration and yaw acceleration satisfy the mean value of 0 and the variances are respectively and The Gaussian distribution of the state noise is given by the CTRV model, and the corresponding covariance matrix is:

[0256] Q(kT)=

[0257]

[0258] Where: They represent the variance of the underwater vehicle linear acceleration and yaw acceleration noise respectively;

[0259] This embodiment uses the state space expression obtained by the CTRV model. If the motion state X(kT) of the aircraft at a certain moment is known, in an ideal situation without considering noise (so the noise υ(kT) is not considered in the prediction step), the motion state X((k+1)T)=A(kT)X(kT) of the aircraft at the next moment can be predicted. However, the fact is that the operation of the aircraft will always be affected by noise (in the CTRV model, since only the linear velocity and angular velocity are used, the linear acceleration and angular acceleration are also regarded as noise). Therefore, there is an error in predicting the motion state of the aircraft simply by the state space expression. In order to reduce the error, it is necessary to correct the predicted motion state by the observed motion state, and the algorithm to achieve this correction is the Kalman filter algorithm;

[0260] S75: After the state space vector equation describes the theoretical operating state of the spacecraft, an observation equation is also needed to describe what kind of observation value will be obtained in theory under this operating state, namely the observation equation.

[0261] Since the observed values ​​of the vehicle's motion state already have the form of the components in the state vector required by the CTRV model, we define the observation equation as:

[0262] Y(kT)=HX(kT)+w(kT)

[0263] Where: Y(kT) represents the theoretical observation value of the underwater vehicle; H represents the observation matrix which is a 5*5 unit matrix; w(kT) represents the observation noise of the state component, and

[0264] w(kT)=(w x (kT),w y (kT),w v (kT),w θ (kT),w ω (kT));

[0265] The covariance matrix corresponding to the observation equation is

[0266]

[0267] in They refer to the variance of the observation noise of each state component respectively;

[0268] Note that the meaning of the observation equation is: theoretically, the observation result Y(kT) can be obtained based on the state vector X(kT) at the current moment;

[0269] S8: Based on the Kalman filter algorithm, the motion state of the underwater vehicle is estimated according to the state space vector equation and state observation equation of the Kalman filter;

[0270] Since Kalman filtering is an iterative algorithm, an initial value is required to start the iterative process, which includes the following steps:

[0271] S81: Define the initial state vector of the state space vector X(kT) involved in the Kalman filter iteration as X + (kT), the initial covariance matrix is ​​P + (kT);

[0272] Specifically, it is assumed that when the jet line detection alliance is established, the coordinates of the latest feature node are

[0273] The corresponding time is In t 0 The angle of the jet line at the moment is θ 0 =θ(t 0 ), the velocity observation value calculated when the feature node is activated is v 0 =v(t 0 ), the observed value of angular velocity ω 0 =ω(t 0 ), then in this embodiment, X + (kT)| k＝0 =(x 0 ,y 0 ,v 0 ,θ 0 ,ω 0 ) T As the initial value of the Kalman filter state vector iteration, X + (kT) is X + (kT)| k＝0 The abbreviation of

[0274] In this embodiment, the covariance matrix P is defined + (kT)| k＝0 =ε is the initial value of the Kalman filter covariance matrix iteration, where ε is a small non-zero positive number, P + (kT) is P + (kT)| k＝0 The abbreviation of

[0275] S82: According to the initial state vector X + (kT) and the initial covariance matrix P + (kT), obtain the prior estimate of the underwater vehicle state vector X -((k+1)T) and the prior estimate P of the initial covariance matrix - ((k+1)T), which is expressed as

[0276] X - ((k+1)T)=A(kT)X + (kT)

[0277] P - ((k+1)T)=A T (kT)P + (kT)A(kT)+Q(kT)

[0278] Where: A T (kT) represents the transpose of A(kT); Q(kT) represents the covariance matrix corresponding to the state noise when using the CTRV model;

[0279] S83: According to the prior estimate P of the covariance matrix - ((k+1)T) obtains the Kalman gain, which is expressed as

[0280]

[0281] Where: R((k+1)T) represents the covariance matrix of the state observation equation at the time ((k+1)T);

[0282] S84: Confirm the actual observation value vector Z((k+1)T) of the underwater vehicle at the current moment. Since the components of the actual observation value are not updated synchronously, when an observation component is updated, the updated observation component is used. If no update occurs, the corresponding prior estimation component obtained in step S82 is used, that is:

[0283]

[0284] Where: Represents each observation component updated according to the state observation equation; x - ((k+1)T),y - ((k+1)T),v - ((k+1)T) represents the prior estimate X - ((k+1)T) obtained prior value;

[0285] S85: Obtain the current a posteriori estimate X of the motion state vector of the underwater vehicle based on the actual observation value vector Z((k+1)T) and combined with the Kalman gain + ((k+1)T), which is expressed as

[0286] X + ((k+1)T)=X -((k+1)T)+K((k+1)T)[Z((k+1)T)-HX - ((k+1)T)]

[0287] And the current posterior estimate X + ((k+1)T) is used as the estimation of the motion state of the underwater vehicle at the current iteration;

[0288] When the next iteration needs to be performed, step S86 is executed;

[0289] S86: Based on the prior estimate P of the initial covariance matrix - ((k+1)T) obtains the updated covariance matrix, which is expressed as

[0290] P + ((k+1)T)=(IK((k+1)T)H((k+1)T))P - ((k+1)T)

[0291] The current a posteriori estimate of the underwater vehicle motion state vector X + ((k+1)T) and the updated covariance matrix are used as inputs of the Kalman filter algorithm again, and S82 to S85 are repeated.

[0292] This embodiment provides a method for estimating the motion state of an underwater vehicle based on a whisker sensor array. Fig.11 As shown, this embodiment uses the whisker sensor to collect disturbances in the two-dimensional underwater flow field environment. When the underwater vehicle passes through the array and the jet generated by its propeller excites the whisker sensor network, the electrical signal of the excited whisker sensor is collected, and the variance of the signal is calculated to characterize the intensity of the flow field environment disturbance. Then, the sensor position and signal strength are subjected to a weighted orthogonal regression algorithm to obtain a jet line that can characterize the direction and position of the vehicle jet, which is used to estimate the navigation angle and angular velocity of the vehicle. At the same time, the position information of the sensor node and the jet line are used to estimate the linear velocity and position of the vehicle. In addition, the CTRV model is used to describe the planar motion state of the underwater vehicle, and the Kalman filter is used to predict and update the motion state parameters of the vehicle.

[0293] This embodiment also includes the following concept definitions:

[0294] A. Propulsion method of the aircraft: The propeller used in the aircraft targeted by the present invention should be a propeller, which can generate a jet during propulsion to excite the sensor.

[0295] B. Whisker sensor: The whisker sensor used in the present invention is based on the principle of a friction nanogenerator or a piezoelectric nanogenerator, and has a flexible shell and a hollow cavity structure. The hollow cavity is used to accommodate the power generation unit. When the external fluid environment changes due to the passage of an object, the flexible shell will deform under the action of the fluid, and then drive the internal power generation unit to generate an electrical signal. Generally speaking, the more severe the disturbance of the external fluid environment, the greater the variance of the electrical signal generated by the whisker sensor.

[0296] C. Whisker sensor array: Whisker sensors are distributed in a certain space underwater at a certain density, and the array formed is called a whisker sensor array. The whisker sensor array can be used to sense the intensity distribution of the underwater flow field, and then sense the vehicle in the underwater environment.

[0297] D. Jet: The flow field with a certain flow direction and velocity distribution generated by the propeller of the spacecraft is called a jet. The jet has a certain axial length and radial width.

[0298] E. Jet field: Generally, the closer the whisker sensor is to the excitation source in the axial direction and the closer it is to the center line in the radial direction, the greater the force of the jet on the whisker sensor. Since the jet is generally turbulent, the degree of shaking of the whisker sensor will also be greater, which is reflected in the signal generated by it as a larger signal variance. Different positions of the jet can cause the whisker sensor to produce different sizes of variance, and they show a certain statistical law. We use the mean of the variance to describe the intensity of the sensor excited at different positions in the jet, and call this jet space with intensity description the jet field.

[0299] F. Jet line: Within a certain distance near the excitation source, the movement of the spacecraft and the ocean current have little effect on the jet, and the jet is emitted in an approximate straight line. In the approximate case, the ray that best reflects the effective jet emission direction and best represents the position of the center axis of the effective jet field cylinder in space is called the jet line. Within the approximate range, this ray is used to describe the jet field. In theory, it is customary to use a vector passing through a certain point to describe the jet line in two-dimensional space. The vector that describes the direction of the jet line is called the jet line vector, and the point that describes the position of the jet line is called the anchor point of the jet line.

[0300] G. Sensor node: Each sensor in the sensor array is called a sensor node, or node for short.

[0301] H. Topological connection relationship of sensors: In order to establish the correlation relationship between sensors, we establish a topological connection relationship between sensors within a certain distance. In the topological graph, sensor nodes are represented by points, and the connection relationship is represented by a line segment connecting two points, such as Fig.10 As shown, the distance between interconnected nodes is defined as 1, and they are neighbor nodes to each other.

[0302] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for estimating the motion state of an underwater vehicle based on a whisker sensor array, characterized in that: The specific steps include: S1: setting the whisker sensor array and collecting the time series voltage signal of each whisker sensor node in the whisker sensor array; S2: Perform sliding window variance calculation on the time series voltage signal to obtain a sliding variance signal; S3: constructing a state transition rule of a whisker sensor node according to the sliding variance signal to determine the current state of each whisker sensor; The current state includes a dormant state, an active state, and a waiting state; S4: According to the current state of each whisker sensor, a jet line detection coalition strategy of the underwater vehicle is constructed to obtain the jet line detection coalition and sensor feature node transfer sequence; S5: obtaining a jet line estimation optimization model according to a jet line detection alliance, and obtaining a jet line sequence according to the jet line estimation optimization model; S6: acquiring sensor array observation data of the underwater vehicle according to the jet line sequence and the sensor characteristic node transfer sequence; The sensor array observation data includes the heading angle observation value, yaw angular velocity observation value, vehicle speed observation value and position observation value of the underwater vehicle; S7: Based on the CTRV motion model, the state space vector equation and state observation equation of the Kalman filter are constructed according to the sensor array observation data; S8: Based on the Kalman filter algorithm, the motion state of the underwater vehicle is estimated according to the state space vector equation and state observation equation of the Kalman filter.

2. The method for estimating the motion state of an underwater vehicle based on a whisker sensor array according to claim 1, characterized in that: S2 specifically includes the following steps: S21: define the sampling period of the whisker sensor node as T, and obtain discrete voltage data u(t) according to the sampled continuous voltage signal u(t) of the original whisker sensor node. t＝kT ,k=1,2,……; And define the discrete voltage data u(t) sampled at time t = kT t＝kT Represented as the kth discrete signal u(kT); And according to the discrete voltage data u(t) t＝kT Obtain the time series voltage signal sequence of the whisker sensor node, that is, D(kT) = D(t) | t＝kT ; where D(t)| t＝kT represents the sliding variance of the kth discrete signal u(kT) at time t=kT; S22: setting the data window size of the sliding mathematical expectation to n, and performing sliding window variance calculation on the time series voltage signal in the time series voltage signal sequence to obtain a sliding variance signal; And the expression for calculating the sliding window variance is Where: E(kT) represents the sliding mathematical expectation obtained according to the kth discrete voltage signal; D(kT) represents the sliding variance of the kth discrete voltage signal.

3. The method for estimating the motion state of an underwater vehicle based on a whisker sensor array according to claim 2, characterized in that: The state transition rule of the whisker sensor node constructed in S3 is specifically: S31: define the node state of the whisker sensor node; And the node state includes a dormant state S1, an active state S2 and a waiting state S3; Set the signal threshold and sliding variance D(kT) of the whisker sensor node to maintain time T; The maintenance time T includes a first maintenance time T1 and a second maintenance time T2; And the signal threshold includes a signal amplitude threshold T h , wake-up time threshold T t + And the sleep time threshold T t - ; S32: setting the initial node state of the whisker sensor node to the dormant state S1, and obtaining the sliding variance signal D(kT) of the whisker sensor node; Determine the sliding variance signal D(kT) of the whisker sensor node and the signal amplitude threshold T h size; If the sliding variance signal D(kT) of the whisker sensor node is ≤T h When the timing starts and reaches the first maintenance time T1, the node state of the whisker sensor node is updated from the sleep state S1 to the waiting state S3, and S33 is continued to be executed; If the sliding variance signal D(kT) of the whisker sensor node>T h When the timer starts and reaches the second maintenance time T2, it is determined whether the second maintenance time T2 is equal to the wake-up time threshold T t + size; And when the second holding time T2>T t + When , the node state of the whisker sensor node is updated from the dormant state S1 or the waiting state S3 to the active state S2; When the second maintenance time T2≤T t + When , the node state of the whisker sensor node remains in the dormant state S1; S33: Determine whether the first maintenance time T1 exceeds the sleep time threshold T t - ; If the first maintenance time T1 exceeds the sleep time threshold T t - When , the node state of the whisker sensor node is updated from the waiting state S3 to the dormant state S1; If the first maintenance time T1 does not exceed the sleep time threshold T t - , the node state of the whisker sensor node continues to maintain the waiting state S3.

4. The method for estimating the motion state of an underwater vehicle based on a whisker sensor array according to claim 3, characterized in that: The S4 specifically comprises the following steps: S41: A jet line detection alliance strategy based on the underwater vehicle, and obtaining a jet line detection alliance according to the whisker sensor nodes in the activation state S2; The underwater vehicle jet line detection alliance strategy includes detection alliance establishment rules, alliance member joining rules and alliance member exit rules; Detection alliance establishment rules: When a certain whisker sensor node is activated, confirm whether multiple whisker sensor nodes adjacent to the whisker sensor node are also in an activated state; If yes, the whisker sensor node and its adjacent whisker sensor nodes are defined as an alliance relationship and serve as alliance member nodes; if no, the whisker sensor node and its adjacent whisker sensor nodes are defined as non-alliance relationship and the whisker sensor nodes adjacent to the whisker sensor node are served as non-alliance member nodes; Get the current jet line detection alliance according to the alliance member nodes; The alliance member joining rule: confirm whether the neighboring nodes of each alliance member node in the current jet line detection alliance are activated; wherein the neighbor nodes do not include the whisker sensor nodes in the current jet line detection alliance; If so, the neighbor node is used as a new member node of the jet line detection alliance to update the current jet line detection alliance; If not, the neighbor node is not considered as a new member node of the jet line detection alliance; The alliance member exit rule: if the node state of the whisker sensor node in the current jet line detection alliance is confirmed to be the dormant state S1, the whisker sensor node is regarded as a non-alliance member node and exits the current jet line detection alliance to update the current jet line detection alliance; S42: Based on the jet line detection alliance strategy of the underwater vehicle, the newly activated tentacle sensor nodes are screened to obtain the sensor characteristic node transfer sequence: The newly activated whisker sensor node is a new member node of the jet line detection alliance; The node screening method is: Project the position of each whisker sensor node in the current jet line detection alliance onto the straight line where the jet line vector of the underwater vehicle is located to obtain the projection point; Define the direction of the underwater vehicle jet line vector as the positive direction; If the projection point of the newly added member node is behind the projection point of any alliance member node in the current jet line detection alliance, the newly added alliance member node will be used as a sensor feature node close to the spacecraft thruster; And the coordinates of the sensor feature nodes are marked as in Indicates that the nth whisker sensor node is The time instants are marked as the coordinates of the sensor feature nodes, and Respectively represent the position abscissa and position ordinate of the sensor feature node; According to the sensor feature nodes, the sensor feature node transfer sequence is recorded as And the corresponding sensor feature node transfer sequence is recorded as 5. The method for estimating the motion state of an underwater vehicle based on a whisker sensor array according to claim 4, characterized in that: The S5 specifically includes the following steps: S51: Obtain a jet line estimation optimization model according to the jet line detection alliance, and the expression of the jet line estimation optimization model is: Where: P(kT) represents any point on the line where the underwater vehicle jet line is located; V(kT) represents the normal vector of the line where the underwater vehicle jet line is located; Φ(V(kT), P(kT)) represents the loss function of weighted orthogonal regression; N k represents the number of sensor nodes in the jet line detection alliance; ρ i (kT) represents the weight of the whisker sensor node, and ρ i (kT) = ρ(D i (kT), μ, λ); μ, λ represent the parameters determined by the structure of the whisker sensor and the environment respectively; P i represents the coordinates of the whisker sensor node; x i,min ,x i,max ,y i,min ,y i,max They represent the minimum value of the horizontal coordinate, the maximum value of the horizontal coordinate, the minimum value of the vertical coordinate, and the maximum value of the vertical coordinate of all the whisker sensor nodes in the jet line detection alliance respectively; p x (kT),p y (kT) represents the abscissa and ordinate components of P(kT); S52: Obtain a jet line sequence according to the jet line estimation optimization model, and the expression of the jet line sequence is J={J(kT)|k=1,2,...} J(kT)=(P(kT) T ,V(kT) T , T Where: J represents the jet line sequence; J(kT) represents any jet line vector in the jet line sequence; P(kT) T ,V(kT) T They represent the transpose of P(kT) and V(kT) respectively.

6. The method for estimating the motion state of an underwater vehicle based on a whisker sensor array according to claim 5, characterized in that: In S6, sensor array observation data of the underwater vehicle is obtained according to the jet line sequence and the sensor characteristic node transfer sequence; The heading angle observation value θ of the underwater vehicle z The expression of (kT) is L(kT)=(l x (kT),l y (kT)) T L'(kT)=(l x '(kT),l y '(kT)) T =(-l x (kT),-l y (kT)) T Where: L(kT) represents the direction vector of the jet line vector at each moment in the jet line sequence; l x (kT),l y (kT) represents the horizontal and vertical components of L(kT) respectively; L'(kT) represents the opposite direction vector of the jet line direction; The yaw angular velocity observation value ω of the underwater vehicle z The expression of (kT) is Where: T represents the sampling period; The vehicle speed observation value of the underwater vehicle The expression is Where: t d n represents the transfer direction vector of the sensor feature node; α(kT) represents the vector t d n With the jet line sequence The angle sequence of the direction vector of each jet line in , and d(kT) represents the intermediate sequence, and The position observation value of the underwater vehicle The expression is h p(kT)=( h x(kT), h y(kT)) T Where: h p(kT) represents the foot coordinate, which is the characteristic node The jet line sequence with a time interval of s periods before time t The foot of the perpendicular line to the straight line where each jet line lies; h x(kT), h y(kT) represents the horizontal and vertical coordinates of the foot of perpendicular respectively; They represent the average values ​​of the perpendicular coordinates.

7. The method for estimating the motion state of an underwater vehicle based on a whisker sensor array according to claim 6, characterized in that: The S7 specifically comprises the following steps: S71: Based on the sensor array observation data, the state vector X(kT) of the underwater vehicle at time kT is expressed as X(kT) = (x(kT) y(kT) v(kT) θ(kT) ω(kT)) T Among them, x(kT), y(kT), v(kT), θ(kT), ω(kT) represent the position observation value, vehicle velocity observation value, heading angle observation value and yaw angular velocity observation value of the sensor array of the underwater vehicle at time kT respectively; S72: Based on the CTRV motion model, the CTRV state space vector equation is constructed according to the state vector of the underwater vehicle, and its expression is: Where: X((k+1)T) represents the state space vector of the underwater vehicle at time (k+1)T; r(kT) represents the state vector noise; X(kT) represents the state vector of the underwater vehicle at time kT; S73: Perform local linearization processing on the CTRV state space vector equation to obtain the state transfer matrix A(kT) of the underwater vehicle, which is expressed as follows: S74: Based on the Jacobi matrix A(kT), the CTRV state space vector equation is rewritten as the state space vector equation of the Kalman filter, which is expressed as X((k+1)T)=A(kT)X(kT)+υ(kT) Where: υ(kT) represents state noise; S75: Obtain the state observation equation based on the state space vector equation of Kalman filtering, which is expressed as follows: Y(kT)=HX(kT)+w(kT) Where: Y(kT) represents the theoretical observation value of the underwater vehicle; H represents the 5*5 unit matrix; w(kT) represents the observation noise of the state component, and w(kT) = (w x (kT),w y (kT),w v (kT),w θ (kT),w ω (kT)).

8. The method for estimating the motion state of an underwater vehicle based on a whisker sensor array according to claim 7, characterized in that: The S8 specifically includes the following steps: S81: Define the initial state vector of the state space vector X(kT) involved in the Kalman filter iteration as X + (kT), the initial covariance matrix is ​​P + (kT); S82: According to the initial state vector X + (kT) and the initial covariance matrix P + (kT), obtain the prior estimate of the underwater vehicle state vector X - ((k+1)T) and the prior estimate P of the initial covariance matrix - ((k+1)T), whose expression is X - ((k+1)T)=A(kT)X + (kT) P - ((k+1)T)=A T (kT)P + (kT)A(kT)+Q(kT) Where: A T (kT) represents the transpose of A(kT); Q(kT) represents the covariance matrix corresponding to the state noise when using the CTRV model, and They represent the variance of the underwater vehicle linear acceleration and yaw acceleration noise respectively; S83: According to the prior estimate P of the covariance matrix - ((k+1)T) obtains the Kalman gain, which is expressed as Where: R((k+1)T) represents the covariance matrix of the state observation equation, and They represent the observation noise w of each state component at time ((k+1)T) respectively. x ((k+1)T),w y ((k+1)T),w v ((k+1)T),w θ ((k+1)T),w ω The variance of ((k+1)T); S84: Confirm the actual observation value vector Z((k+1)T) of the underwater vehicle at the current moment, which is expressed as Where: Represents each observation component updated according to the state observation equation; x - ((k+1)T),y - ((k+1)T),v - ((k+1)T) represents the prior estimate X - ((k+1)T) obtained prior value; S85: Obtain the current a posteriori estimate X of the motion state vector of the underwater vehicle based on the actual observation value vector Z((k+1)T) and combined with the Kalman gain + ((k+1)T), which is expressed as X + ((k+1)T)=X - ((k+1)T)+K((k+1)T)[Z((k+1)T)-HX - ((k+1)T)] And the current posterior estimate X + ((k+1)T) is used as the estimation of the motion state of the underwater vehicle at the current iteration; When the next iteration needs to be performed, step S86 is executed; S86: Based on the prior estimate P of the initial covariance matrix - ((k+1)T) obtains the updated covariance matrix, which is expressed as P + ((k+1)T)=(IK((k+1)T)H((k+1)T))P - ((k+1)T) The current a posteriori estimate of the underwater vehicle motion state vector X + ((k+1)T) and the updated covariance matrix are used as inputs of the Kalman filter algorithm again, and S82 to S85 are repeated.

Images