Distributed cluster collaborative navigation system and method based on data link networking and ranging

Abstract

The present invention provides a distributed cluster collaborative navigation system and method based on data link networking and ranging. The system includes: module M1: navigation systems for each carrier node; module M2: data links for each carrier node; and module M3: collaborative navigation information processor. The method includes: step S1: determining the information source weight of each collaborative navigation node based on the navigation node error; step S2: determining the distributed cluster networking strategy based on the information source weight of each node; step S3: performing ranging and communication on each navigation node through data link networking; and step S4: fusing the information of each navigation node to obtain distributed cluster collaborative navigation information for heterogeneous navigation systems. Step S1 is implemented in module M1, steps S2 and S3 are implemented in module M2, and step S4 is implemented in module M3. The present invention ensures the navigation accuracy of the distributed cluster in various complex environments and reduces the cost of a single carrier navigation system.

Description

Technical Field

[0001] The present invention relates to the technical field of distributed cluster collaborative navigation, and in particular to a distributed cluster collaborative navigation system and method based on data link networking and ranging. Background Art

[0002] At present, various navigation methods such as inertial navigation, satellite navigation, astronomical navigation, geomagnetic navigation, and visual navigation are emerging in an endless stream. Various types of combined navigation using two or more navigation methods have also been widely used in various scenarios in life such as aerospace, aviation, unmanned driving, underwater navigation, and indoor positioning.

[0003] Patent document CN101795221A (application number: CN201010103932.0) discloses a network topology structure and combined multiple access system design method for a swarm formation, belonging to the fields of aviation data link, radio navigation, and aircraft autonomy technology. The present invention provides a multiple access system and network topology structure suitable for swarm formation network communication and measurement, meeting the swarm formation's passive detection and positioning of moving targets and coordinated strike missions. The present invention provides a digital signal processor (DSP), FPGA device, and radio frequency channel on the intermediate frequency signal processing circuit board of the swarm link terminal that can implement the combined multiple access system proposed in the present invention and support the swarm formation network topology proposed in the present invention.

[0004] In order to make up for the shortcomings of a single navigation system, traditional combined navigation often adds multiple sensors to a navigation system to achieve complementary advantages, which usually increases the cost of a single carrier navigation system. At the same time, the application scenarios of distributed carrier clusters such as aircraft clusters, underwater unmanned submersible clusters, and vehicle clusters are also increasing.

[0005] Therefore, the navigation mode of the carrier cluster needs to be transformed from single-carrier independent navigation to distributed cluster collaborative navigation. The "super sensing system" based on the information fusion technology of multiple navigation devices of multiple carriers and the integration of information collection, transmission and processing into one can significantly enhance the perception ability and improve the degree of information sharing among users in the cluster, so as to better meet the navigation needs of the cluster. Summary of the Invention

[0006] In view of the defects in the prior art, the purpose of the present invention is to provide a distributed cluster collaborative navigation system and method based on data link networking and ranging.

[0007] The distributed cluster collaborative navigation system based on data link networking and ranging provided by the present invention includes:

[0008] Module M1: navigation system of each carrier node;

[0009] Module M2: data link of each carrier node;

[0010] Module M3: Collaborative navigation information processor.

[0011] The distributed cluster collaborative navigation system based on data link networking and ranging provided by the present invention is implemented by a distributed cluster collaborative navigation method based on data link networking and ranging, and the method comprises the following steps:

[0012] Step S1: Determine the information source weight of each collaborative navigation node based on the error of each navigation node;

[0013] Step S2: Determine the distributed cluster networking strategy based on the information source weight of each node;

[0014] Step S3: measuring the distance and communicating with each navigation node through data link networking;

[0015] Step S4: Fusing the information of each navigation node to obtain the distributed cluster collaborative navigation information of the heterogeneous navigation system.

[0016] Preferably, step S1 is implemented in the distributed cluster collaborative navigation system module M1 based on data link networking and ranging, steps S2 and S3 are implemented in the distributed cluster collaborative navigation system module M2 based on data link networking and ranging, and step S4 is implemented in the distributed cluster collaborative navigation system module M3 based on data link networking and ranging.

[0017] Preferably, the step S1 includes:

[0018] If the network nodes are pure inertial navigation, an inertial navigation error accumulation prediction model is established, and the weight of the collaborative navigation information source is determined based on the accumulated error;

[0019] A single node inertial navigation error accumulation prediction model is established based on the attitude error after initial alignment, the bound velocity error, and the position error. The velocity error caused by the initial attitude error after the start of navigation is extrapolated and predicted. The change value of the inertial navigation velocity error between the start of navigation time T1 and the current time T2 is taken as:

[0020]

[0021]

[0022]

[0023] According to the velocity error extrapolation, the change in the initial attitude error inertial position error between the start of navigation time T1 and the current time T2 is taken as:

[0024]

[0025]

[0026]

[0027] Where: f N 、f U 、f E The north-east velocity increment is 2.5 ms; N 、φ U 、φ E is the initial attitude error of the north sky east;

[0028] Position error caused by initial velocity error:

[0029]

[0030]

[0031]

[0032] Where: δV N0 , δV U0 , δV E0 is the initial north celestial east velocity error;

[0033] The extrapolated predicted inertial position error at the current moment is:

[0034]

[0035]

[0036]

[0037]

[0038] Where: δD N0 , δD U0 , δD E0 is the position error of the initial binding, δD Sum is the position synthesis error;

[0039] The inverse of the inertial navigation error predicted by each node is used to form a diagonal matrix as the weight matrix of the collaborative navigation information source If the node is a high-precision location node formed by combined navigation, the weight of the node is 1.

[0040] Preferably, the step S2 includes:

[0041] The distributed cluster networking strategy is a multi-level distributed collaborative networking strategy. The strategy is:

[0042] 3 to 4 nodes form a network cluster, and the data link network within the cluster is used for collaborative navigation;

[0043] 3 to 4 cluster head nodes are networked to form a data link subnet for collaborative navigation;

[0044] 3 to 4 data link subnets form the upper level data link network collaborative navigation;

[0045] Carry out time-slot communication and ranging for network data links at all levels, and decentralize the distributed structure.

[0046] Preferably, step S3 includes:

[0047] The relative distance between two nodes is measured and data communication between two nodes is realized through the networking data link. The data link communication adopts a time slot design, which is divided into intra-cluster business transmission time slot, inter-cluster business transmission time slot, intra-cluster time calibration time slot, and inter-cluster time calibration time slot.

[0048] Each node in the data link adopts omnidirectional transmission and reception mode in its own business time slot;

[0049] Use multi-channel omnidirectional reception in their respective non-business time slots;

[0050] In the cluster time calibration time slot, the cluster members perform intra-cluster time synchronization and distance measurement;

[0051] In the inter-cluster timing slot, time synchronization and distance measurement between clusters are performed.

[0052] Preferably, step S4 includes:

[0053] Each heterogeneous navigation system selects different information source weight matrices P according to the navigation system accuracy. x To realize cluster collaborative navigation, the information fusion algorithm of distributed cluster collaborative navigation of heterogeneous navigation system is the least square method, and the measurement equation is:

[0054] L(t)=HX(t)+Δ

[0055] Where, are the three-axis position errors of each node in the cluster in the geographic coordinate system, n is the number of cluster nodes; L(t) is the observation matrix, the number of ranging values, which changes in real time with the number of cluster nodes n; H is the observation coefficient matrix; Δ is the observation noise matrix;

[0056] The weighted and minimum constraint condition for the inertial navigation position error of each node in the cluster network is designed as follows:

[0057] G T P x D a X=0

[0058] Where G T It is by H TThe 6×3n-dimensional matrix composed of the eigenvectors corresponding to the zero eigenvalues ​​of PH; x Collaborative navigation information source weight matrix; D a The error conversion matrix for converting the longitude and latitude errors into the Earth-centered Earth-fixed coordinate system;

[0059] The inertial navigation error state is estimated as:

[0060] X=(H T RH+P x GG T P x ) -1 H T RL

[0061] Where R is the observation noise covariance matrix.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] (1) The present invention utilizes data link networking communication and ranging between carriers in a distributed cluster to achieve information interaction and complementary advantages, constructs a distributed cluster collaborative navigation information fusion algorithm, fuses the navigation system information of each carrier in the cluster, realizes cluster collaborative navigation, ensures the navigation accuracy of the distributed cluster in various complex environments, and reduces the cost of a single carrier navigation system;

[0064] (2) The system of the present invention does not require a central node, all nodes are equivalent, and has strong adaptability. It is still applicable when all carrier navigation systems in the cluster are pure inertial navigation systems, ensuring that the cluster can still effectively guarantee the navigation accuracy of the entire cluster in an environment with electromagnetic interference and obstruction. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Other features, objects and advantages of the present invention will become more apparent upon reading the detailed description of non-limiting embodiments with reference to the following drawings:

[0066] Figure 1 This is a structural diagram of a distributed cluster collaborative navigation system based on data link networking and ranging according to an embodiment of the present invention;

[0067] Figure 2 This is a flowchart of the implementation steps of a distributed cluster collaborative navigation system based on data link networking and ranging according to an embodiment of the present invention;

[0068] Figure 3 This is a multi-level distributed collaborative networking strategy diagram of an embodiment of the present invention. DETAILED DESCRIPTION

[0069] The present invention will be described in detail below with reference to specific embodiments. The following examples 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 skilled in the art, several changes and improvements can be made without departing from the scope of the present invention. These all fall within the scope of protection of the present invention.

[0070] Example:

[0071] The present invention utilizes data link networking communication and ranging between carriers in a distributed cluster to achieve information interaction and complementary advantages, constructs a distributed cluster collaborative navigation information fusion algorithm, fuses the navigation system information of each carrier in the cluster, and realizes cluster collaborative navigation.

[0072] Figure 1 This is a structural diagram of a distributed cluster collaborative navigation system based on data link networking and ranging according to an embodiment of the present invention, including:

[0073] Module M1: navigation system of each carrier node;

[0074] Module M2: data link of each carrier node;

[0075] Module M3: Collaborative navigation information processor.

[0076] Figure 2 The implementation steps of a distributed cluster collaborative navigation system based on data link networking and ranging according to an embodiment of the present invention include:

[0077] Step S1: Determine the information source weight of each node in collaborative navigation based on the navigation system error of each node. Step S1 is implemented in module M1.

[0078] If the networking nodes are pure inertial navigation, an inertial navigation error accumulation prediction model is established, and the weight of the collaborative navigation information source is determined based on the accumulated error.

[0079] A single node inertial navigation error accumulation prediction model is established based on the attitude error after initial alignment, the bound velocity error and the position error.

[0080] The velocity error caused by the initial attitude error after the start of navigation is extrapolated and predicted. The change in the inertial navigation velocity error between the start of navigation time T1 and the current time T2 is taken as:

[0081]

[0082]

[0083]

[0084] According to the velocity error extrapolation, the change in the initial attitude error inertial position error between the start of navigation time T1 and the current time T2 is taken as:

[0085]

[0086]

[0087]

[0088] Where: f N 、f U 、f E is the 2.5ms north-east velocity increment, φ N 、φ U 、φ E is the initial attitude error of the North Celestial East.

[0089] Position error caused by initial velocity error:

[0090]

[0091]

[0092]

[0093] Where: δV N0 , δV U0 , δV E0 is the initial north celestial east velocity error.

[0094] The extrapolated predicted inertial position error at the current moment is:

[0095]

[0096]

[0097]

[0098]

[0099] Where: δD N0 , δD U0 , δD E0 is the position error of the initial binding, δD Sum is the position composite error.

[0100] The inverse of the inertial navigation error predicted by each node forms a diagonal matrix which can be used as the weight matrix of the collaborative navigation information source

[0101] If the node is a high-precision location node formed by combined navigation, the weight of the node is 1.

[0102] Step S2: Determine the distributed cluster networking strategy. Step S2 is implemented in module M2.

[0103] Figure 3 This is a multi-level distributed collaborative networking strategy diagram for an embodiment of the present invention. The specific strategy is:

[0104] 3 to 4 nodes form a network cluster, and the data link network within the cluster is used for collaborative navigation;

[0105] 3 to 4 cluster head nodes are networked to form a data link subnet for collaborative navigation;

[0106] 3 to 4 data link subnets form the upper level data link network collaborative navigation;

[0107] Network data links at all levels use time-slot communication and ranging, and the distributed structure is decentralized to avoid excessive processing burden on single nodes or single clusters.

[0108] Step S3: Each carrier node measures distance and communicates via data link networking. Step S3 is implemented in module M2.

[0109] The networked data link measures the relative distance between nodes (one carrier represents one data link node) and enables data communication between them. Data link communication utilizes a time-slot design, divided into intra-cluster service transmission slots, inter-cluster service transmission slots, intra-cluster timing slots, and inter-cluster timing slots.

[0110] Each node in the data link adopts omnidirectional transmission and reception mode in its own business time slot; adopts multi-channel omnidirectional reception in its own non-business time slot; performs intra-cluster time synchronization and distance measurement between cluster members in the intra-cluster timing slot; performs inter-cluster time synchronization and distance measurement in the inter-cluster timing slot.

[0111] Step S4: Design a distributed cluster collaborative navigation information fusion algorithm for heterogeneous navigation systems. Step S4 is implemented in module M3.

[0112] Each heterogeneous navigation system selects different information source weight matrices P according to the navigation system accuracy. x Realize cluster collaborative navigation. The information fusion algorithm for distributed cluster collaborative navigation of heterogeneous navigation system is the least squares method.

[0113] Measurement equation:

[0114] L(t)=HX(t)+Δ

[0115] Where, are the three-axis position errors of each node in the cluster in the geographic coordinate system, n is the number of cluster nodes. L(t) is the observation matrix, i.e., the number of ranging values, which changes in real time with the number of cluster nodes n. H is the observation coefficient matrix, and Δ is the observation noise matrix.

[0116] The weighted and minimum constraint condition for the inertial navigation position error of each node in the cluster network is designed as follows:

[0117] G T P x D a X=0

[0118] Where G T It is by H T The 6×3n-dimensional matrix composed of the eigenvectors corresponding to the zero eigenvalues ​​of PH, P x Collaborative navigation information source weight matrix, D a The error conversion matrix for converting longitude and latitude errors to Earth-centered Earth-fixed coordinate system errors.

[0119] The inertial navigation error state is estimated as:

[0120] X=(H T RH+P x GG T P x ) -1 H T RL

[0121] Where R is the observation noise covariance matrix.

[0122] The present invention proposes a cluster networking collaborative navigation method based on constrained Kalman filtering for a pure inertial navigation system. The method utilizes the networking ranging between nodes in the cluster and designs networking constraints. Based on the constraints, state equations and measurement equations are constructed, and a new Kalman filter is designed. The inertial navigation error is recursively estimated in real time to correct the inertial navigation system, thereby suppressing the divergence of the pure inertial navigation error of each node and achieving pure inertial navigation high-precision positioning.

[0123] Those skilled in the art will appreciate that, in addition to implementing the system, device, and various modules provided by the present invention in purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, embedded microcontrollers, and the like by logically programming the method steps. Therefore, the system, device, and various modules provided by the present invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; the modules for implementing various functions can also be considered both software programs for implementing the method and structures within the hardware component.

[0124] The above describes specific embodiments of the present invention. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art may make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. The embodiments of this application and the features in the embodiments may be combined with each other in any manner unless there is a conflict.

Claims

1. A distributed cluster collaborative navigation method based on data link networking and ranging, characterized in that: This is achieved through a distributed cluster collaborative navigation system based on data link networking and ranging. The distributed cluster collaborative navigation system based on data link networking and ranging includes: Module M1: navigation system of each carrier node; Module M2: data link of each carrier node; Module M3: collaborative navigation information processor; The distributed cluster collaborative navigation method based on data link networking and ranging performs the following steps: Step S1: Determine the information source weight of each collaborative navigation node based on the error of each navigation node; Step S2: Determine the distributed cluster networking strategy based on the information source weight of each node; Step S3: measuring the distance and communicating with each navigation node through data link networking; Step S4: Fusing the information of each navigation node to obtain the distributed cluster collaborative navigation information of the heterogeneous navigation system; The step S1 is implemented in module M1, the steps S2 and S3 are implemented in module M2, and the step S4 is implemented in module M3; The step S1 comprises: If the network nodes are pure inertial navigation, an inertial navigation error accumulation prediction model is established, and the weight of the collaborative navigation information source is determined based on the accumulated error; A single node inertial navigation error accumulation prediction model is established based on the attitude error after initial alignment, the bound velocity error, and the position error. The velocity error caused by the initial attitude error after the start of navigation is extrapolated and predicted. The change value of the inertial navigation velocity error between the start of navigation time T1 and the current time T2 is taken as: According to the velocity error, the change in the initial attitude error inertial position error between the start of navigation time T1 and the current time T2 is taken as: Where: f N 、f U 、f E The north-east velocity increment is 2.5 ms; N 、φ U 、φ E is the initial attitude error of the north sky east; Position error caused by initial velocity error: Where: δV N0 , δV U0 , δV E0 is the initial north celestial east velocity error; The extrapolated predicted inertial position error at the current moment is: Where: δD N0 , δD U0 , δD E0 is the position error of the initial binding, δD Sum is the position synthesis error; The inverse of the inertial navigation error predicted by each node is used to form a diagonal matrix as the weight matrix of the collaborative navigation information source If the node is a high-precision location node composed of combined navigation, the weight of the node is 1; The step S2 comprises: The distributed cluster networking strategy is a multi-level distributed collaborative networking strategy. The strategy is: 3 to 4 nodes form a network cluster, and the data link network within the cluster is used for collaborative navigation; 3 to 4 cluster head nodes are networked to form a data link subnet for collaborative navigation; 3 to 4 data link subnets form the upper level data link network collaborative navigation; Carry out time-slot communication and ranging for network data links at all levels, and decentralize the distributed structure; The step S3 comprises: The relative distance between two nodes is measured and data communication between two nodes is realized through the networking data link. The data link communication adopts a time slot design, which is divided into intra-cluster business transmission time slot, inter-cluster business transmission time slot, intra-cluster time calibration time slot, and inter-cluster time calibration time slot. Each node in the data link adopts omnidirectional transmission and reception mode in its own business time slot; Use multi-channel omnidirectional reception in their respective non-service time slots; In the cluster time calibration time slot, the cluster members perform intra-cluster time synchronization and distance measurement; In the inter-cluster timing slot, time synchronization and distance measurement between clusters are performed; The step S4 comprises: Each heterogeneous navigation system selects different information source weight matrices P according to the navigation system accuracy. x To realize cluster collaborative navigation, the information fusion algorithm of distributed cluster collaborative navigation of heterogeneous navigation system is the least square method, and the measurement equation is: L(t)=HX(t)+Δ Where, are the three-axis position errors of each node in the cluster in the geographic coordinate system, n is the number of cluster nodes; L(t) is the observation matrix, the number of ranging values, which changes in real time with the number of cluster nodes n; H is the observation coefficient matrix; Δ is the observation noise matrix; The weighted and minimum constraint condition for the inertial navigation position error of each node in the cluster network is designed as follows: G T P x D a X=0 Where G T It is by H T The 6×3n-dimensional matrix composed of the eigenvectors corresponding to the zero eigenvalues ​​of PH; x Collaborative navigation information source weight matrix; D a The error conversion matrix for converting the longitude and latitude errors into the Earth-centered Earth-fixed coordinate system; The inertial navigation error state is estimated as: X=(H T RH+P x GG T P x ) -1 H T RL Where R is the observation noise covariance matrix.

Images