Cluster networking collaborative navigation method and system based on SSUKF
By using the super-sphere single-sampled sampling and traceless Kalman filtering (SSUKF) method in collaborative navigation technology, the problems of slow convergence, poor robustness and large computing volume in the prior art are solved, and more efficient collaborative navigation and reducing the computing burden are achieved.
Patent Information
- Application Number
- CN202411987215.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-12-31
- Publication Date
- 2025-04-25
Smart Images
Figure CN119879927A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of collaborative navigation technology, and in particular, to a cluster networking collaborative navigation method and system based on SSUKF, and more particularly, to a cluster networking collaborative navigation method and system based on hypersphere simplex sampling unscented Kalman filtering. Background Art
[0002] Under modern combat conditions, a single aircraft is limited by its size and weight during missions, and it is increasingly difficult for it to complete combat missions alone. Formation cluster collaborative operations can enable each combat unit to match each other and exert combat effectiveness beyond that of a single aircraft. Collaborative navigation technology is the basis for collaborative operations of multi-aircraft clusters. Aircraft clusters conduct collaborative navigation through information sharing, which can greatly improve the navigation accuracy and robustness of the entire cluster.
[0003] At present, collaborative navigation mostly uses data links to obtain the distance information between formation nodes, and fuses it with the inertial navigation results through extended Kalman filters (EKF) or unscented Kalman filters (UKF) to eliminate the inertial navigation errors. This method generally has the defects of slow filter convergence and poor robustness, and the extended Kalman filter and unscented Kalman filter have large computational complexity, which will impose a heavy burden on the aircraft embedded computing platform. Summary of the invention
[0004] In view of the defects in the prior art, the object of the present invention is to provide a cluster networking collaborative navigation method and system based on SSUKF.
[0005] According to the present invention, a cluster networking collaborative navigation method based on SSUKF is provided, comprising:
[0006] Step S1: determine the cluster networking nodes and measure the relative distance and relative angle between each node;
[0007] Step S2: Based on the measured relative distances and relative angles between the nodes, establish cluster networking collaborative navigation filtering state equations and measurement equations;
[0008] Step S3: constructing a hypersphere simplex sampling unscented Kalman filter based on the constructed cluster group cooperative navigation filter state equation and measurement equation, and initializing the hypersphere simplex sampling unscented Kalman filter;
[0009] Step S4: using the initialized hypersphere simplex sampling unscented Kalman filter to estimate the state variables;
[0010] Step S5: Correct the inertial navigation using the motion parameters in the estimated value of the state variable.
[0011] Preferably, step S1 includes: determining the nodes participating in the networking according to the geometric configuration, measuring the distance and angle relationship between each node in the networking through data link, millimeter wave radar and infrared sensor, and measuring the distance, azimuth and altitude between each node for collaborative navigation.
[0012] Preferably, the cluster networking collaborative navigation filtering state equation in step S2 includes:
[0013] X(k+1)=f(X(k))+w
[0014] Among them, X(k) represents the state variable at time k; X(k+1) represents the state variable at time k+1; w represents the system noise variable of the state equation;
[0015] X(k)=[P n (k), V n (k), φ(k), θ(k), ψ(k), df(k), dW(k)] T
[0016] Among them, P n =[x, y, z] is the position of the center of mass of the aircraft in the three directions of X, Y, and Z of the navigation coordinate system; V n =[v x , v y , v z ] is the velocity of the center of mass of the aircraft in the three directions of X, Y, and Z in the navigation coordinate system; φ is the carrier roll angle, θ is the carrier pitch angle, and ψ is the carrier yaw angle; df = [df x , df y , df z ] is the zero bias of the accelerometer in the three directions of X, Y, and Z of the carrier coordinate system; dW = [dW x , dW y , dW z ] is the constant drift of the gyroscope in the three directions of X, Y, and Z of the carrier coordinate system;
[0017] X(k+1)=[P n (k+1), V n (k+1), φ(k+1), θ(k), ψ(k+1), df(k+1), dW(k+1)] T
[0018] Among them, V n (k+1)=V n (k)+((f n (k)-(2ω ie +ω en )×V en )+G)×Δt
[0019]
[0020] φ(k+1)=φ(k)+(ω nbx -tanθ(k)(ω nby cosφ(k)-ω nbz sinφ(k)))×Δt
[0021] θ(k+1)=θ(k)+(ω nby sinφ(k)+ω nbz cosφ(k))×Δt
[0022] ψ(k+1)=ψ(k)+((ω nby cosφ(k)-ω nbz sinφ(k)) / cosθ(k))×Δt
[0023] df(k+1)=df(k)
[0024] dW(k+1)=dW(k)
[0025]
[0026] ω nb =[ω nbx ,ω nby ,ω nbz ] T is the component of the angular velocity of the aircraft carrier relative to the navigation coordinate system in the carrier coordinate system; ω ie The component of the Earth's rotation angular velocity in the navigation coordinate system, ω en is the component of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system; ω b is the angular velocity output by the gyroscope on the aircraft; f b It is the apparent acceleration output by the accelerometer on the aircraft; is the attitude matrix from the carrier coordinate system to the navigation coordinate system, is the attitude matrix from the navigation coordinate system to the carrier coordinate system; Δt is the calculation period; G is the local gravity acceleration; V en is the component of the velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system; ω nbx ,ω nbz ,ω nby After forming the vector, it is ω nb , is the component of the angular velocity of the aircraft carrier relative to the navigation coordinate system in the carrier coordinate system.
[0027] Preferably, the cluster networking collaborative navigation filtering measurement equation in step S2 includes:
[0028]
[0029] Among them, Z i is the measured value, h represents the nonlinear change from the state variable X to the observed variable Z;
[0030]
[0031] Among them, r i , α i , β i are the true values of the slant distance, altitude angle and azimuth angle of other aircraft in the formation and itself, respectively, i is the noise measured for it, and m is the number of other aircraft;
[0032] α i =arcsin(e by / r i )
[0033] β i =arctan(e bz / e bx )
[0034]
[0035] Among them, e by , e bz , e bx for e b The three components of the carrier are: φ is the carrier roll angle, θ is the carrier pitch angle, and ψ is the carrier yaw angle.
[0036] Preferably, step S3 comprises:
[0037]
[0038] in, It represents the estimated value of the initial value of the state variable, E represents the mathematical expectation, x0 represents the initial value of the state variable, and P0 represents the initial value of the covariance matrix.
[0039] Preferably, the step S4 comprises:
[0040] Step S4.1: Calculate the sampling points of the hypersphere single-row hypersphere simplex sampling unscented Kalman filter;
[0041] Step S4.2: Perform time update and measurement update based on the sampling points of the hypersphere single-row hypersphere simplex sampling unscented Kalman filter.
[0042] Preferably, the step S4.1 comprises:
[0043] Step S4.1.1: Select the center point W0 as: 0≤W0<1;
[0044] Step S4.1.2: Determine the weights of other Sigma points as:
[0045]
[0046] Where n represents the dimension of the state equation;
[0047] Step S4.1.3: Iteratively initialize the Sigma points:
[0048]
[0049] Step S4.1.4: For j = 2, 3, ... n, iterate the Sigma point by the following formula:
[0050]
[0051] Step S4.1.5: Calculate the sigma point:
[0052]
[0053] in, is the mean of the state variable x, and P is the covariance matrix.
[0054] Preferably,
[0055] The time update includes:
[0056] χ i，k|k-1 =f(χ i，k-1 )
[0057]
[0058] Z i，k|k-1 =h(χ i，k|k-1 )
[0059]
[0060] Among them, h represents the nonlinear change from state variable X to observation variable Z;
[0061] The measurement update includes:
[0062]
[0063]
[0064] in, is the estimated value of the filter state variable, P k is the filter state variable covariance.
[0065] Preferably, step S5 comprises: using a hypersphere simplex to sample an unscented Kalman filter state variable estimate The first 9 dimensions of the image are used to replace the output of the inertial navigation system, thereby realizing the correction of the inertial navigation system.
[0066] According to the present invention, a cluster networking collaborative navigation system based on SSUKF is provided, comprising:
[0067] Module M1: Determine the cluster networking nodes and measure the relative distance and relative angle between each node;
[0068] Module M2: Based on the measured relative distances and relative angles between nodes, establish cluster networking collaborative navigation filtering state equations and measurement equations;
[0069] Module M3: construct a hypersphere simplex sampling unscented Kalman filter based on the constructed cluster group cooperative navigation filter state equation and measurement equation, and initialize the hypersphere simplex sampling unscented Kalman filter;
[0070] Module M4: Estimate the state variables using the initialized hypersphere simplex sampling unscented Kalman filter;
[0071] Module M5: Use the motion parameters in the estimated value of the state variable to correct the inertial navigation.
[0072] Compared with the prior art, the present invention has the following beneficial effects:
[0073] 1. Based on the single use of data link ranging information as the observation quantity, the present invention adds the relative angle information of the infrared angle measuring sensors between clusters as a supplementary observation quantity, which can effectively improve the filter convergence speed and greatly enhance the robustness;
[0074] 2. The present invention adopts the hypersphere simplex sampling unscented Kalman filter (SSUKF) to greatly improve the calculation efficiency and reduce the calculation burden under the premise of ensuring the solution accuracy, which is convenient for application in the aircraft embedded computing platform. BRIEF DESCRIPTION OF THE DRAWINGS
[0075] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:
[0076] Figure 1 This is a flow chart of the cluster networking collaborative navigation method based on hypersphere simplex sampling unscented Kalman filtering.
[0077] Figure 2 Graph showing the update process of the unscented Kalman filter for hypersphere simplex sampling. DETAILED DESCRIPTION
[0078] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.
[0079] Example 1
[0080] According to a cluster networking collaborative navigation method and system based on a hypersphere simplex sampling unscented Kalman filter provided by the present invention, the problem of long-term accumulation of motion parameters is solved by utilizing the networking distance and angle measurement between nodes in the cluster, and a hypersphere simplex sampling strategy is designed. A hypersphere simplex sampling unscented Kalman filter is constructed to estimate the motion parameters and suppress the divergence of the motion parameters.
[0081] The cluster networking collaborative navigation method based on hypersphere simplex sampling unscented Kalman filtering is as follows: Figures 1 to 2 As shown, including:
[0082] Step S1: Determine the number of cluster networking nodes and measure the relative distance and relative angle between each node;
[0083] Step S2: Establish cluster networking collaborative navigation filtering state equation and measurement equation;
[0084] Step S3: filter initialization;
[0085] Step S4: estimating the state variables by using hypersphere simplex sampling unscented Kalman filtering;
[0086] Step S5: Correct the inertial navigation using the motion parameters in the estimated value of the state variable.
[0087] Specifically, in step S1, the nodes participating in the networking are determined according to the geometric configuration, and the distance and angle relationship between each node in the networking are measured through data link, millimeter wave radar, infrared sensor and other means, which are used as the measured distance, azimuth and altitude between each node of collaborative navigation for subsequent filtering.
[0088] Specifically, the step S2 establishes cluster networking collaborative navigation filtering state equations and measurement equations.
[0089] The state equation is as follows:
[0090] X(k+1)=f(X(k))+w
[0091] Among them, the state variable at time k is:
[0092] X(k)=[P n (k), V n(k), φ(k), θ(k), ψ(k), df(k), dW(k)] T
[0093] P n =[x, y, z] is the position of the center of mass of the aircraft in the three directions of X, Y, and Z of the navigation coordinate system; V n =[v x , v y , v z ] is the velocity of the center of mass of the aircraft in the three directions of X, Y, and Z in the navigation coordinate system; φ is the carrier roll angle, θ is the carrier pitch angle, and ψ is the carrier yaw angle; df = [df x , df y , df z ] is the zero bias of the accelerometer in the three directions of X, Y, and Z of the carrier coordinate system; dW = [dW x , dW y , dW z ] is the constant drift of the gyro in the three directions of X, Y, and Z of the carrier coordinate system; w is the system noise
[0094] State variables at time k+1:
[0095] X(k+1)=[P n (k+1), V n (k+1), φ(k+1), θ(k), ψ(k+1), df(k+1), dW(k+1)] T
[0096] in
[0097] V n (k+1)=V n (k)+((f n (k)-(2ω ie +ω en )×V en )+G)×Δt
[0098]
[0099] φ(k+1)=φ(k)+(ω nbx -tanθ(k)(ω nby cosφ(k)-ω nbz sinφ(k)))×Δt
[0100] θ(k+1)=θ(k)+(ω nby sinφ(k)+ω nbz cosφ(k))×Δt
[0101] ψ(k+ 1)=ψ(k)+((ω nby cosφ(k)-ω nbz sinφ(k)) / cosθ(k))×Δt
[0102] df(k+1)=df(k)
[0103] dW(k+1)=dW(k)
[0104]
[0105] ω nb =[ω nbx ,ω nby ,ω nbz ] T is the component of the angular velocity of the aircraft carrier relative to the navigation coordinate system in the carrier coordinate system; ω ie The component of the Earth's rotation angular velocity in the navigation coordinate system, ω en is the component of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system; ω b is the angular velocity output by the gyroscope on the aircraft. b It is the apparent acceleration output by the accelerometer on the aircraft; is the attitude matrix from the carrier coordinate system to the navigation coordinate system, is the attitude matrix from the navigation coordinate system to the carrier coordinate system; Δt is the calculation period.
[0106] The measurement equation is as follows:
[0107]
[0108] Among them, Z i The measured values obtained in step S1, namely the measured distance, azimuth and elevation between each node; r i , α i , β i are the true values of the slant distance, altitude angle and azimuth angle of other aircraft in the formation and itself, respectively, i is the noise measured for it, and m is the number of other aircraft.
[0109]
[0110] α i =arcsin(e by / r i )
[0111] β i =arctan(e bz / e bx )
[0112]
[0113]
[0114] Among them, φ is the carrier roll angle, θ is the carrier pitch angle, and ψ is the carrier yaw angle.
[0115] Specifically, the filter is initialized in step S3 as follows (this step is performed only once):
[0116]
[0117] Specifically, in step S4, the state variables are estimated by using a hypersphere simplex sampling unscented Kalman filter. The method is as follows:
[0118] 1) Calculate the sampling points (Sigma points) of the hypersphere simplex sampling unscented Kalman filter
[0119] a. Select the center point W0 as: 0≤W0<1.
[0120] b. Determine the weights of other Sigma points:
[0121]
[0122] Where n represents the dimension of the state equation, which is 15 here.
[0123] c. Iteratively initialize the Sigma points:
[0124]
[0125] d. For j = 2, 3, ... n, the Sigma point is iterated by the following formula:
[0126]
[0127] get Hereinafter referred to as σ i
[0128] e. Calculate the sigma point:
[0129]
[0130] in is the mean of the state variable x, and P is the covariance matrix
[0131] 2) Time update:
[0132] χ i，k|k-1 =f(χ i，k-1 )
[0133]
[0134] Z i，k|k-1 =h(χ i，k|k-1 )
[0135]
[0136] 3) Measurement update:
[0137]
[0138] That is the estimated value of the filter state variable, P k is the filter state variable covariance.
[0139] Specifically, step S5 uses the hypersphere simplex sampling unscented Kalman filter state variable estimation value The first 9 dimensions (i.e., P n (k), V n (k), φ(k), θ(k), ψ(k)) are used to replace the output result of the inertial navigation, thereby realizing the correction of the inertial navigation.
[0140] The present invention also provides a cluster networking collaborative navigation system based on SSUKF, and the cluster networking collaborative navigation system based on SSUKF can be realized by executing the process steps of the cluster networking collaborative navigation method based on SSUKF, that is, those skilled in the art can understand the cluster networking collaborative navigation method based on SSUKF as a preferred implementation of the cluster networking collaborative navigation system based on SSUKF.
[0141] Those skilled in the art know that, in addition to realizing the system and its various devices, modules, and units provided by the present invention in a purely computer-readable program code, it is entirely possible to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a hardware component, and the devices, modules, and units included therein for realizing various functions can also be regarded as structures within the hardware component; the devices, modules, and units for realizing various functions can also be regarded as both software modules for realizing the method and structures within the hardware component.
[0142] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.
Claims
1. A cluster networking collaborative navigation method based on SSUKF, characterized in that: include: Step S1: determine the cluster networking nodes and measure the relative distance and relative angle between each node; Step S2: Based on the measured relative distances and relative angles between the nodes, establish cluster networking collaborative navigation filtering state equations and measurement equations; Step S3: constructing a hypersphere simplex sampling unscented Kalman filter based on the constructed cluster group cooperative navigation filter state equation and measurement equation, and initializing the hypersphere simplex sampling unscented Kalman filter; Step S4: using the initialized hypersphere simplex sampling unscented Kalman filter to estimate the state variables; Step S5: Correct the inertial navigation using the motion parameters in the estimated value of the state variable.
2. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The step S1 includes: determining the nodes participating in the networking according to the geometric configuration, measuring the distance and angle relationship between each node in the networking through data links, millimeter wave radars and infrared sensors, and measuring the distance, azimuth and altitude between each node for collaborative navigation.
3. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The cluster networking collaborative navigation filtering state equation in step S2 includes: X(k+1)=f(X(k))+w Among them, X(k) represents the state variable at time k; X(k+1) represents the state variable at time k+1; w represents the system noise variable of the state equation; X(k)=[P n (k),V n (k), φ(k), θ(k), ψ(k), df(k), dW(k)] T Among them, P n =[x, y, z] is the position of the center of mass of the aircraft in the three directions of X, Y, and Z of the navigation coordinate system; V n =[v x , v y , v z ] is the velocity of the center of mass of the aircraft in the three directions of X, Y, and Z in the navigation coordinate system; φ is the carrier roll angle, θ is the carrier pitch angle, and ψ is the carrier yaw angle; df = [df x , df y , df z ] is the zero bias of the accelerometer in the three directions of X, Y, and Z of the carrier coordinate system; dW = [dW x , dW y , dW z ] is the constant drift of the gyroscope in the three directions of X, Y, and Z of the carrier coordinate system; X(k+1)=[P n (k+1),V n (k+1), φ(k+1), θ(k), ψ(k+1), df(k+1), dW(k+1)] T Among them, V n (k+1)=V n (k)+((f n (k)-(2ω ie +ω en )×V en )+G)×Δt φ(k+1)=φ(k)+(ω nbx -tanθ(k)(ω) nby cosφ(k)-ω nbz sinφ(k)))×Δt θ(k+1)=θ(k)+(ω nby sinφ(k)+ω nbz cosφ(k))×Δt ψ(k+1)=ψ(k)+((ω nby cosφ(k)-ω nbz sinφ(k)) / cosθ(k))×Δt df(k+1)=df(k) dW(k+1)=dW(k) ω nb =[ω nbx ,ω nby ,ω nbz ] T is the component of the angular velocity of the aircraft carrier relative to the navigation coordinate system in the carrier coordinate system; ω ie The component of the Earth's rotation angular velocity in the navigation coordinate system, ω en is the component of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system; ω b is the angular velocity output by the gyroscope on the aircraft; f b It is the apparent acceleration output by the accelerometer on the aircraft; is the attitude matrix from the carrier coordinate system to the navigation coordinate system, is the attitude matrix from the navigation coordinate system to the carrier coordinate system; Δt is the calculation period; G is the local gravity acceleration; V en is the component of the velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system; ω nbx ,ω nbz ,ω nby After forming the vector, it is ω nb , is the component of the angular velocity of the aircraft carrier relative to the navigation coordinate system in the carrier coordinate system.
4. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The cluster networking collaborative navigation filtering measurement equation in step S2 includes: Among them, Z i is the measured value, h represents the nonlinear change from the state variable X to the observed variable Z; Among them, r i , α i , β i are the true values of the slant distance, altitude angle and azimuth angle of other aircraft in the formation and itself, respectively, i is the noise measured for it, and m is the number of other aircraft; α i =arcsin(e by / r i ) β i =arctan(e bz / and bx ) Among them, e by , e bz , e bx for e b The three components of the carrier are: φ is the carrier roll angle, θ is the carrier pitch angle, and ψ is the carrier yaw angle.
5. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The step S3 comprises: in, It represents the estimated value of the initial value of the state variable, E represents the mathematical expectation, x0 represents the initial value of the state variable, and P0 represents the initial value of the covariance matrix.
6. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The step S4 comprises: Step S4.1: Calculate the sampling points of the hypersphere single-row hypersphere simplex sampling unscented Kalman filter; Step S4.2: Perform time update and measurement update based on the sampling points of the hypersphere single-row hypersphere simplex sampling unscented Kalman filter.
7. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The step S4.1 comprises: Step S4.1.1: Select the center point W0 as: 0≤W0<1; Step S4.1.2: Determine the weights of other Sigma points as: Where n represents the dimension of the state equation; Step S4.1.3: Iteratively initialize the Sigma points: Step S4.1.4: For j = 2, 3, ... n, iterate the Sigma points by the following formula: Step S4.1.5: Calculate the sigma point: in, is the mean of the state variable x, and P is the covariance matrix.
8. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The time update includes: x i,k|k-1 =f(x i,k-1 ) Z i,k|k-1 =h(χ i,k|k-1 ) Among them, h represents the nonlinear change from state variable X to observation variable Z; The measurement update includes: in, is the estimated value of the filter state variable, P k is the filter state variable covariance.
9. The SSUKF-based cluster networking collaborative navigation method according to claim 1, characterized in that: The step S5 comprises: using a hypersphere simplex to sample an unscented Kalman filter state variable estimate The first 9 dimensions of the image are used to replace the output of the inertial navigation system, thereby realizing the correction of the inertial navigation system.
10. A cluster networking collaborative navigation system based on SSUKF, characterized in that: include: Module M1: Determine the cluster networking nodes and measure the relative distance and relative angle between each node; Module M2: Based on the measured relative distances and angles between nodes, establish cluster networking collaborative navigation filtering state equations and measurement equations; Module M3: construct a hypersphere simplex sampling unscented Kalman filter based on the constructed cluster group cooperative navigation filter state equation and measurement equation, and initialize the hypersphere simplex sampling unscented Kalman filter; Module M4: Estimate the state variables using the initialized hypersphere simplex sampling unscented Kalman filter; Module M5: Use the motion parameters in the state variable estimate to correct the inertial navigation.