A fuzzy adaptive cooperative navigation information fusion method

Abstract

The application discloses a fuzzy adaptive cooperative navigation information fusion method, and mainly aims at cooperative positioning of a cluster composed of low-cost multi-carriers. A local-cooperative information fusion model of a cooperative navigation system is established, position information and ranging information are exchanged between the communicable carriers in the cluster, dimension expansion is realized based on single-point positioning information, a fuzzy logic adaptive controller is established in the cooperative navigation information fusion stage, the mean value and covariance of cooperative measurement new information are fully utilized, fuzzy rules are established, and noise related parameters of an iterative cubature Kalman filter are adaptively controlled, so that robust high-precision positioning of the low-cost cluster cooperative navigation is realized.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of cooperative navigation, and particularly relates to a fuzzy self-adaptive cooperative navigation information fusion method. BACKGROUND

[0002] In recent years, water unmanned surface vehicles (USVs) have attracted extensive attention in military and civilian fields, such as military reconnaissance, environmental detection, material transportation, disaster rescue, and fishery catching. Due to the high efficiency and flexibility, the USV cluster technology has become a hot research topic.

[0003] Navigation and positioning information is crucial for USVs. Especially in the USV cluster system, the cluster needs high-precision position, velocity, and other information to participate in task planning, precision control, and obstacle avoidance. Currently, the satellite navigation and positioning system (GNSS) and the inertial navigation and positioning system (INS) are the most commonly used navigation systems for USVs. GNSS can provide USVs with positioning accuracy up to centimeters, but in some challenging environments, such as multipath and few observation satellites, GNSS is difficult to provide navigation results. In addition, high-precision GNSS also faces the problem of high cost. INS, as a completely autonomous navigation system, can provide continuous, high-precision velocity, position, and attitude results, but due to its error accumulation characteristics, it cannot work alone for a long time. In order to fully utilize the advantages of USV cluster and obtain more reliable navigation and positioning results, the cooperative navigation and positioning information fusion technology of relative measurement information between USVs becomes a key factor. After obtaining local position estimation information through information fusion of a single carrier equipped with low-cost navigation equipment, the position information and USV-to-USV ranging are broadcast to other USVs within the communication range, and then the single-point position positioning accuracy is further improved through multi-source information fusion technology. Compared with the single USV navigation and positioning method using its own measurement information, the cooperative navigation and positioning technology can utilize the observation information between the clusters to correct the navigation and positioning results, thereby improving the absolute navigation accuracy. However, due to the influence of harsh environments such as water, the measurement information is affected by noise, making the measurement noise distribution non-Gaussian. In addition, the divergence caused by model error is also a key factor leading to the divergence of information fusion results. When the measurement does not provide enough accurate information for system state estimation, it will directly lead to the divergence and instability of the system positioning results.

[0004] Therefore, real-time dynamic adjustment of the noise characteristics in the cluster cooperative navigation and positioning process is an important direction to improve the accuracy and reliability of low-cost USV cluster cooperative positioning. SUMMARY

[0005] To solve the above problems, the application discloses a fuzzy adaptive cooperative navigation information fusion method, which realizes position information and ranging information interaction between communicable carriers in a cluster by establishing a local-cooperative information fusion model of a cooperative navigation system, expands dimensions of cooperative information on the basis of single-point positioning information, simultaneously establishes a fuzzy logic adaptive controller in a cooperative navigation information fusion stage, fully utilizes a cooperative measurement innovation mean value and a covariance, establishes a fuzzy rule, and adaptively controls noise related parameters of an iterative cubature Kalman filter, so as to realize robust high-precision positioning of a low-cost cluster cooperative navigation.

[0006] To achieve the above object, the technical scheme of the application is as follows:

[0007] A fuzzy adaptive cooperative navigation information fusion method comprises the following steps:

[0008] S1: a local-cooperative information fusion model of a cooperative navigation system is established;

[0009] The S1 specifically comprises the following steps:

[0010] S1-1: local information fusion and position estimation in a single-point positioning stage are completed;

[0011] In the single-point positioning stage, information fusion is completed by using inertial navigation speed, position, attitude information and satellite navigation pseudorange measurement information of a current carrier, and single-body navigation positioning information without cooperative information is output.

[0012] S1-2: an information fusion model in a cooperative navigation stage is established;

[0013] In the cooperative navigation stage, information output by single-point navigation positioning, position information and ranging information of other carriers in the cluster after information transmission are fully utilized to establish a nonlinear state model and a measurement model:

[0014] x k =f(x k-1 )+w k-1

[0015] z k =h(x k )+v k

[0016] w k ~N(0,Q)

[0017] v k ~N(0,R)

[0018] Wherein, x k represents a system state vector at k time, z k represents measurement at k time, w kQ represents process noise, and Q is the corresponding process noise covariance matrix; v k R represents measurement noise, and R is the measurement noise covariance matrix. f(·) and h(·) represent state nonlinear equations and measurement nonlinear equations, respectively.

[0019] S2: A fuzzy logic adaptive controller (FLAC) is established in the cooperative navigation stage to dynamically and adaptively control and adjust the noise intensity in the internal model of the filter;

[0020] In order to prevent the volume Kalman filter from diverging, a fuzzy logic adaptive controller is established to adjust the noise intensity, that is, the covariance matrix of the fuzzy adaptive volume Kalman filter is defined as:

[0021] R k = ρ(α)R

[0022] Q k = ρ(α)Q

[0023] In the formula, ρ(α) = α -2(k+1) is an exponential factor, and α is an exponential factor coefficient and α ≥ 1.

[0024] S3: Under the control of the fuzzy logic adaptive model described in S2, an iterative volume Kalman filter is used for information fusion

[0025] The S3 specifically includes the following steps:

[0026] S3-1: Generate a volume point framework and perform time updating

[0027]

[0028] In the formula, P k-1|k-1 is the optimal estimation state covariance matrix at k-1, ξ is a volume point coefficient term, is the optimal estimation state vector of the system at k-1, x i，k-1 is a time prediction update volume point, i = 1..2n is the volume point number, and n is the system state dimension.

[0029]

[0030]

[0031] In the formula, x is a one-step prediction state quantity at k, P k|k-1 is a one-step prediction state covariance at k, and ω is a weighting vector.

[0032] S3-2: Combine the cooperative information to expand the state dimension, generate a new volume point approximation framework, and perform measurement iterative updating

[0033]

[0034] P A，k|k-1 = diag(P k|k-1 , P p，k-1 )

[0035] wherein Ξ p，k-1 is the set of cooperative position information, P p，k-1 is the set of covariance information corresponding to the cooperative position information, is the state vector after dimension expansion.

[0036]

[0037] wherein χ i，k|k-1 is the volume point after dimension expansion of the measurement at time k

[0038]

[0039]

[0040]

[0041]

[0042]

[0043] wherein j is the iteration number, is the state estimation vector after each iteration, ζ j is the iteration scaling factor, and the step length is updated by using the multi-dimensional scaling method.

[0044] Final optimal estimation of the state of the monomer carrier at time k and the optimal estimation covariance matrix P k|k :

[0045]

[0046]

[0047] Further, in S2, a fuzzy logic adaptive controller is established in the cooperative navigation stage to dynamically and adaptively control and adjust the noise intensity in the internal model of the filter, a multi-output-single-output fuzzy controller is designed by using the fuzzy control theory, wherein the mean of the residual and the covariance of the residual are used as the input of the fuzzy controller:

[0048]

[0049]

[0050] The index factor a of the fuzzy controller is defined as:

[0051]

[0052] The fuzzy rule R is defined as:

[0053]

[0054] Then b i = g i (·),

[0055] Where the fuzzy set is

[0056]

[0057] Further, in the S2, a fuzzy logic adaptive controller is established in the cooperative navigation stage, and the noise intensity in the filter internal model is dynamically and adaptively controlled and adjusted. A multi-output-single-output fuzzy controller is designed by using the fuzzy control theory, fuzzy rules are set, and the residual error covariance is calculated and compared with the theoretical covariance obtained by the cubature Kalman filter. The residual error mean is compared with 0.

[0058] When is greater than the theoretical value and the residual error mean deviates from 0, the filter tends to be unstable, at this time, the index factor a of the fuzzy controller takes a larger value, and a is proportional to b i ;

[0059] When is very large and the mean is not 0, it is considered that there is a great possibility of abnormality in the measurement value, at this time, the index factor a takes a smaller value so as not to completely trust the filter.

[0060] Further, in the S3-2, the state is extended by combining the single-body position information with the cooperative information:

[0061]

[0062] P A，k|k-1 = diag(P k|k-1 , P p，k-1 )

[0063] Where Ξ p，k-1 is a set of cooperative position information, that is,

[0064] is a set of position information of m cooperative carriers at k-1 time in the current carrier communication range.

[0065] The set of position covariance information of m cooperating carriers at time k-1 within the current carrier communication range.

[0066] The beneficial effects of this invention are:

[0067] Compared with existing technologies, this invention, targeting low-cost multi-carrier clusters, considers fully integrating collaborative location information within the cluster to establish a local-cooperative information fusion model for the collaborative navigation system. Based on individual positioning information, it combines the position information and relative distance information of other carriers within the communication range, and uses the innovation mean and innovation covariance to establish a fuzzy controller to achieve adaptive control of the relevant noise parameters of the iterative capacitive Kalman filter, thereby realizing information fusion for collaborative navigation. This provides a technical reference for achieving robust high-precision positioning in low-cost cluster collaborative navigation. Attached Figure Description

[0068] Figure 1 A schematic diagram of the collaborative navigation system for unmanned surface vessels in an embodiment of the present invention.

[0069] Figure 2 A schematic diagram of iterative capacitive Kalman filtering based on fuzzy control in an embodiment of the present invention.

[0070] Figure 3 A schematic diagram of the membership function of the residual covariance matrix in an embodiment of the present invention.

[0071] Figure 4 A schematic diagram of the membership function of the residual mean in an embodiment of the present invention.

[0072] Figure 5 A schematic diagram of the membership function of the exponential factor in an embodiment of the present invention. Detailed Implementation

[0073] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0074] As shown in the figure, the fuzzy adaptive cooperative navigation information fusion method of the present invention includes the following steps:

[0075] S1: Establish a local-cooperative information fusion model for the collaborative navigation system. The specific steps include the following steps S1-1 and S1-2:

[0076] like Figure 1 The diagram shown is a flowchart of the collaborative navigation system for unmanned surface vessels (USVs) swarms disclosed in this invention.

[0077] S1-1: Complete the local information fusion and location estimation in the single-point positioning stage;

[0078] During the single-point positioning phase, each surface vessel uses the velocity, position, and attitude information provided by its own inertial navigation system and the pseudorange measurement information provided by the satellite navigation system to complete information fusion and output the single-vessel navigation and positioning information of each vessel at the current moment without cooperative information;

[0079] S1-2: Establish an information fusion model for the collaborative navigation phase;

[0080] During the cooperative navigation phase, the information output from single-point navigation and positioning, the position information of other carriers within the cluster synchronized after information transmission, and ranging information are fully utilized to establish a nonlinear state model and measurement model:

[0081] x k =f(x) k-1 )+w k-1

[0082] z k =h(x k )+v k

[0083] w k ~N(0,Q)

[0084] v k ~N(0,R)

[0085] Where, x k z represents the system state vector at time k. k The quantity representing time k, w k V represents process noise, and Q is its corresponding process noise covariance matrix; k Let R represent the measurement noise, and let f(·) and h(·) represent the state nonlinear equation and the measurement nonlinear equation, respectively.

[0086] Each surface vessel is equipped with communication and ranging equipment, and broadcasts its current position and ranging information periodically. Using the current surface vessel as the central vessel, it receives position information from other surface vessels (cooperating vessels) within its communication range. n and location covariance P n1 .

[0087] like Figure 2 The diagram shown is a schematic of the iterative capacitive Kalman filter based on fuzzy control disclosed in this invention.

[0088] S2: Establish a fuzzy logic adaptive controller (FLAC) during the cooperative navigation phase to dynamically and adaptively adjust the noise intensity in the internal model of the filter.

[0089] In order to prevent the volume Kalman filter from diverging, a fuzzy logic adaptive controller is established to adjust the noise intensity, that is, the covariance matrix of the fuzzy adaptive volume Kalman filter is defined as:

[0090] R k = ρ(α)R

[0091] Q k = ρ(α)Q

[0092] wherein ρ(α) = α -2(k+1) is an exponential factor, and α is an exponential factor coefficient and α ≥ 1.

[0093] S3: under the control of the fuzzy logic adaptive model in S2, information fusion is performed by using an iterative volume Kalman filter

[0094] The S3 specifically comprises the following steps:

[0095] S3-1: a volume point framework is generated, and time updating is performed

[0096]

[0097] wherein P k-1|k-1 is an optimal estimation state covariance matrix at k-1, ξ is a volume point coefficient term, is an optimal estimation state vector of the system at k-1, x i，k-1 is a time prediction updated volume point, i = 1..2n is a volume point number, and n is a system state dimension.

[0098]

[0099]

[0100] wherein x is a one-step prediction state quantity at k, P k|k-1 is a one-step prediction state covariance at k, and ω is a weighting vector.

[0101] S3-2: combined with cooperative information, state extension is performed, a new volume point approximation framework is generated, and measurement iterative updating is performed

[0102]

[0103] P A，k|k-1 = diag(P k|k-1 , P p，k-1 )

[0104] wherein Ξ p，k-1 is a cooperative position information set, P p，k-1 is a covariance information set corresponding to the cooperative position information, The state vector after the dimension expansion.

[0105]

[0106] Wherein, χ i，k|k-1 The measurement volume point after the dimension expansion at the k moment

[0107]

[0108]

[0109]

[0110]

[0111]

[0112] Wherein, j is the iteration number, The state estimation vector after each iteration, ζj is the iteration scaling factor, and a multi-dimensional scaling method is used for step length updating.

[0113] The final optimal estimation of the state of the monomer carrier at the k moment The optimal estimation covariance matrix P k|k :

[0114]

[0115]

[0116] Further, in S2, a fuzzy logic adaptive controller is established in the cooperative navigation stage, the noise intensity in the filter internal model is dynamically and adaptively controlled and adjusted, a multi-output-single-output fuzzy controller is designed by using the fuzzy control theory.

[0117] As Figures 3-5 The residual covariance, residual mean and control factor membership function of the fuzzy controller in the embodiment of the present application are shown in the schematic diagram, wherein S=small, M=medium, L=large, and Z=0.

[0118] Wherein, the mean of the residual and the covariance of the residual are taken as the fuzzy controller inputs:

[0119]

[0120]

[0121] The exponential factor α of the fuzzy controller is defined as:

[0122]

[0123] The fuzzy rule R is defined as:

[0124]

[0125] Then b i =g i (·), where the fuzzy set is

[0126]

[0127] Further, in S2, a fuzzy logic adaptive controller is established in the cooperative navigation stage to dynamically and adaptively control and adjust the noise intensity in the filter internal model. A multi-output-single-output fuzzy controller is designed by using fuzzy control theory, fuzzy rules are set, and the residual error covariance is calculated and compared with the theoretical covariance obtained by the cubature Kalman filter. The residual error mean is compared with 0.

[0128] When is greater than the theoretical value and the residual error mean deviates from 0, the filter tends to be unstable, at which time the exponential factor a of the fuzzy controller takes a larger value, and a is proportional to b i .

[0129] When is very large and the mean is not 0, it is considered that there is a great possibility of abnormality in the measurement value, at which time the exponential factor a takes a smaller value so as not to completely trust the filter.

[0130] Here, nine fuzzy rules are established, for example:

[0131] When the residual error covariance correlation is L and the residual error mean is Z, a is L.

[0132] When the residual error covariance correlation is Z and the residual error mean is L, a is Z.

[0133] Further, in S3-2, state extension is performed by combining single-body position information with cooperative information:

[0134]

[0135] P A，k|k-1 = diag(P k|k-1 , P p，k-1 )

[0136] where Ξ p，k-1 is a cooperative position information set, i.e.:

[0137] is the position information set of m cooperative carriers at k-1 time in the current carrier communication range.

[0138] A set of position covariance information of the m cooperative carriers at the k-1 time instant within the current carrier communication range.

[0139] It should be noted that the above content only illustrates the technical idea of the present application, and cannot limit the protection scope of the present application. For ordinary skilled in the art, without departing from the principle of the present application, a number of improvements and refinements can be made, which fall within the protection scope of the claims of the present application.

Claims

1. A method for fuzzy adaptive cooperative navigation information fusion, characterized in that: The method comprises the following steps: S1: establishing a local-collaborative information fusion model of the cooperative navigation system; The S1 specifically comprises the following steps: S1-1: completing local information fusion and position estimation in the single-point positioning stage; In the single-point positioning stage, information fusion is completed by using the inertial navigation speed, position, and attitude information of the current carrier and the satellite navigation pseudo-range measurement information, and single-body navigation positioning information without collaborative information is output; S1-2: establishing an information fusion model in the cooperative navigation stage; In the cooperative navigation stage, the information output by the single-point navigation positioning, the position information of other carriers in the cluster after information transmission, and the ranging information are fully utilized to establish a nonlinear state model and a measurement model: x k = f(x k-1 ) + w k-1 z k = h(x k ) + v k w k ~N(0,Q) v k ~N(0,R) where x k represents the system state vector at time k, z k represents the measurement at time k, w k represents the process noise, Q is the corresponding process noise covariance matrix; v k represents the measurement noise, R is the measurement noise covariance matrix; f(·) and h(·) represent the state nonlinear equation and the measurement nonlinear equation, respectively S2: establishing a fuzzy logic adaptive controller in the cooperative navigation stage to dynamically and adaptively control and adjust the noise intensity in the internal model of the filter; In order to prevent the divergence of the cubature Kalman filter, the fuzzy logic adaptive controller is established to adjust the noise intensity, that is, the covariance matrix of the fuzzy adaptive cubature Kalman filter is defined as: R k = p(a)R Q k = p(a)Q wherein, define p(a) = a -2(k+1) is an exponential factor, a is an exponential factor coefficient and a > 1; S3: under the control of the fuzzy logic adaptive model in S2, an iterative cubature Kalman filter is used for information fusion; The S3 specifically comprises the following steps: S3-1: generating a cubature point framework for time updating where P k-1|k-1 is the optimal estimation state covariance matrix at k-1 time, ξ is the volume point coefficient term, is the optimal estimation state vector of the system at k-1 time, x i,k-1 is the volume point of the time prediction update, i = 1..2n is the volume point number, and n is the system state dimension. wherein, is the one-step predicted state quantity at time k, P k|k-1 is the one-step predicted state covariance at time k, and ω is a weighting vector. S3-2: combining the collaborative information to perform state extension and generating a new cubature point approximation framework for measurement iterative updating P A,k|k-1 = diag(P k|k-1 ,P p,k-1 ) wherein Ξ p,k-1 is a set of cooperative position information, P p,k-1 is a set of covariance information corresponding to the cooperative position information, is the extended state vector; where χ i,k|k-1 is the measured extended volume point at time k where j is the iteration number, is the state estimation vector after each iteration, ζ j is the iteration scaling factor, and a multidimensional scaling method is used for step size update. Final optimal estimate of the state of the cell carrier at time instant k with the optimal estimate covariance matrix P k|k :

2. The method according to claim 1, wherein: In the S2, the fuzzy logic adaptive controller is established in the cooperative navigation stage to dynamically and adaptively control and adjust the noise intensity in the internal model of the filter, the fuzzy control theory is used to design a multi-output-single-output fuzzy controller, and the mean of the residual error and the covariance of the residual error are used as the inputs of the fuzzy controller: The index factor a of the fuzzy controller is defined as: The fuzzy rule R is defined as: Then b i = g i (·), wherein the fuzzy set is 3. The method according to claim 1, wherein: In the S2, the fuzzy logic adaptive controller is established in the cooperative navigation stage, the noise intensity in the filter internal model is dynamically and adaptively controlled and adjusted, the multi-output-single-output fuzzy controller is designed by using the fuzzy control theory, the fuzzy rules are set, the residual error covariance is calculated, and the residual error mean Theoretical covariance calculated by the same volume Kalman filter is compared with the residual error mean 0 is compared; When greater than the theoretical value and the residual mean When deviating from 0, the filter tends to be unstable, at which time the exponential factor a of the fuzzy controller takes a larger value, and a is directly proportional to the deviation from 0. When When the variance is very large and the mean is not zero, it is considered that there is a high probability of an anomaly in the measured value, in which case the index factor a takes a small value so as not to trust the filter completely.

4. The method according to claim 1, wherein: In the S3-2, the state extension is performed by combining the single-body position information with the collaborative information: wherein Ξ p,k-1 is a set of collaborative location information, i.e. is a set of position information of m cooperative carriers at k-1 time within a current carrier communication range. A set of position covariance information of the m cooperative carriers at k-1 time within the current carrier communication range.

Images