Cluster cooperative positioning result optimization method based on good state characteristic evaluation

Abstract

The invention discloses a cluster co-location result optimization method based on good state characteristic evaluation, which comprises the following steps of: dividing roles of nodes in a cluster into co-location reference nodes and co-location demand nodes according to whether the nodes are currently located or not, and generating all combinations of the location reference nodes; calculating a position matrix of each positioning reference node combination, and evaluating the good state characteristics of the matrix; cooperative positioning is carried out by using different positioning reference node combinations, and positioning result fusion is carried out by taking an evaluation result of matrix good state characteristics as a weight, so that the success rate and precision of cooperative positioning are improved. According to the invention, reliable positioning information can be provided when satellite navigation and scene matching navigation fail, and the task execution capability of a cluster system in a complex environment is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to a cluster collaborative positioning result optimization method based on well-behaved characteristic evaluation. Background Art

[0002] Inspired by the behavior of biological groups in nature, people have proposed the concept of cluster systems and applied it to the field of scientific research. A cluster system is composed of multiple nodes with certain communication capabilities. The nodes here can be drones, unmanned vehicles, and robots. They obtain information about the surrounding environment through their own sensors, and then exchange local information with their neighboring nodes through the communication network, thereby realizing mutual coordination and cooperation of the entire system. For cluster systems, positioning is the basic capability to ensure the implementation of their tasks. It can provide accurate geographic location information so that each node can plan paths and avoid obstacles according to its real-time location. At present, the use environment of cluster systems is becoming increasingly complex. When facing unfavorable factors such as satellite denial and building obstruction, some or even all nodes are difficult to autonomously locate through satellite navigation; at the same time, the cost of scene matching navigation is high and its adaptability to specific terrain is poor. Therefore, it is necessary to give full play to the advantages of autonomous coordination and complementary capabilities of cluster systems. In the case of failure of satellite navigation and scene matching navigation, the collaborative positioning of unlocated nodes can be achieved through the mutual redundancy of sensor information within the cluster, thereby ensuring the implementation of cluster systems in complex environments. Summary of the invention

[0003] In order to overcome the shortcomings of the prior art, the present invention provides a cluster collaborative positioning result optimization method based on good-state characteristic evaluation, which divides the roles of each node in the cluster according to whether it is currently positioned, and divides it into collaborative positioning reference nodes and collaborative positioning demand nodes, and generates all combinations of positioning reference nodes; calculates the position matrix of each positioning reference node combination, and evaluates the good-state characteristics of the matrix; uses different positioning reference node combinations for collaborative positioning, and uses the evaluation results of the matrix good-state characteristics as weights to fuse the positioning results, thereby improving the success rate and accuracy of collaborative positioning. The present invention can provide reliable positioning information when satellite navigation and scene matching navigation fail, and improve the task execution capability of the cluster system in complex environments.

[0004] The technical solution adopted by the present invention to solve the technical problem is as follows:

[0005] Step 1: Co-localization principle and error propagation mechanism;

[0006] Step 1-1: The combined navigation device of each node in the cluster system includes an inertial measurement unit, GPS, and a barometric altimeter, and some nodes are equipped with a scene matching navigation unit; each node is equipped with a data link communication module to achieve collaborative information exchange and relative distance information measurement;

[0007] The conditions for using cluster system co-location are:

[0008] 1) There are at least three normally positioned nodes in the cluster, and their absolute position coordinates are known;

[0009] 2) The relative distance information between nodes is known;

[0010] 3) The height information of the co-location reference node and the co-location demand node are both known;

[0011] Step 1-2: Principle of co-location;

[0012] Assume that points A, B, and C represent currently locatable nodes, and their corresponding north celestial east coordinates are (X1, Y1, Z1), (X2, Y2, Z2), and (X3, Y3, Z3), respectively. Point D represents the node that needs to be co-located. The relative distances between point D and points A, B, and C are measured by the data link as d1, d2, and d3. Draw a sphere with points A, B, and C as the center and d1, d2, and d3 as the radius. At the same time, draw a circle parallel to the OXZ plane at the known height of point D. Ideally, the three spheres and the circle will intersect at point D.

[0013] Assume that the north celestial east coordinates of point D are (X, Y, Z), which satisfies the following equations:

[0014]

[0015] Since the Y coordinates of points A, B, C, and D can be obtained indirectly through the pressure altitude, we can get:

[0016]

[0017] Perform two differences to eliminate the higher-order terms and obtain the following system of linear equations:

[0018]

[0019] Written in matrix form:

[0020] Qw=v

[0021] in

[0022]

[0023]

[0024] If the inverse matrix of matrix Q exists, solve for the X and Z coordinates of point D, that is, vector w:

[0025] w=Q -1 v

[0026] The co-localization result comes from solving the linear equation system Qw=v, where the matrix Q is related to the position of the reference node, and the vector v is related to the distance information between the nodes and the position of the reference node;

[0027] Step 2: Co-location scheme based on well-behaved characteristic evaluation;

[0028] Step 2-1: The collaborative positioning solution includes node positioning state judgment, collaborative positioning reference node combination generation, reference node combination position matrix well-behaved characteristics evaluation, collaborative positioning result solution and fusion. The information flow is as follows:

[0029] During the flight, the flight control of each node comprehensively judges the current positioning status of the aircraft based on the satellite positioning status and the scene matching positioning status. When the satellite is positioned normally or the node is equipped with scene matching navigation and successfully positioned under satellite denial, the flag is set to 1, otherwise it is set to 0; the flight control sends the positioning status of the aircraft to the cluster cooperative controller, and the cooperative controller determines whether the aircraft needs to be cooperatively positioned and sends the positioning status of the aircraft to the data link; the data link broadcasts the positioning status of the aircraft to other nodes in the cluster, and measures the relative distance information between the aircraft and other nodes, and transmits it back to the cooperative controller; the cooperative controller runs the cooperative positioning algorithm based on the well-behaved characteristic evaluation, and outputs the cooperative positioning result to the flight control; the flight control determines whether the cooperative positioning result is available based on the received cooperative positioning success flag, and if available, it is sent to the navigation device for combination;

[0030] Step 2-2: Co-location algorithm based on well-behaved characteristic evaluation;

[0031] Step 2-2-1: When the local machine needs to perform collaborative positioning, the co-controller extracts the positioning status of each node in the cluster from the inter-machine collaborative information γ i , and extract the effective relative distance d between the local machine and other nodes from the data link ranging information i , where i=1,2,...,n is the serial number of each node in the cluster, and n is the total number of nodes in the cluster;

[0032] Step 2-2-2: The co-controller determines the location of each node in the cluster according to its location status γ i Filter the positioning reference nodes and generate the combination of the serial numbers of all feasible positioning reference nodes, recorded as matrix C∈R m×3 , where m is the number of positioning reference node combinations, and 3 is the number of nodes required for a single collaborative positioning;

[0033] Step 2-2-3: Extract the location information of the positioning reference node from the inter-machine collaborative information (X i ,Y i ,Z i ), and calculate the position matrix Q of the positioning reference node combination k, where k = 1, 2, ..., m;

[0034] Step 2-2-4: Calculate the matrix Q corresponding to each set of positioning reference nodes k The condition number κ(Q k ), evaluate the well-conditioned properties of each matrix:

[0035]

[0036] Among them, κ max >1 is the threshold set according to the collaborative positioning accuracy requirement;

[0037] Step 2-2-5: If the matrix Q k is a well-conditioned matrix, then the combination of the kth positioning reference node is valid. If the matrix Q k If it is an ill-conditioned matrix, the combination of the kth positioning reference node is invalid and is removed from the matrix C;

[0038] Step 2-2-6: The positioning reference node combination matrix at this time is recorded as in is the number of actually available positioning reference node combinations;

[0039] Step 2-2-7: Traverse the sequence combination matrix of the positioning reference node For each row in , the collaborative positioning algorithm is called multiple times, and the weighted average of multiple positioning results is performed based on the matrix well-conditioned characteristics, that is:

[0040]

[0041] in, w k is the collaborative positioning result based on the kth positioning reference node combination, This is the co-localization result after fusion.

[0042] Preferably, the normally positioned node refers to a node that is not satellite-rejected, carries scene matching navigation, and is normally matched and positioned.

[0043] The beneficial effects of the present invention are as follows:

[0044] In view of the problem that the cluster collaborative positioning results are very sensitive to information source errors and have large positioning errors under certain circumstances, the present invention selects the actual available positioning reference node combination through the method of evaluating the well-conditioned characteristics of the position matrix, and performs weighted averaging on the collaborative positioning results under multiple groups of positioning reference nodes, thereby outputting a more accurate collaborative positioning result. Through the above technology, reliable positioning information can be provided when satellite navigation and scene matching navigation fail, and the task execution capability of the cluster system in complex environments can be improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a schematic diagram of the collaborative positioning principle of the present invention.

[0046] Figure 2 Co-locate workflow for the present invention.

[0047] Figure 3 Schematic diagram of inter-node ranging error according to an embodiment of the present invention.

[0048] Figure 4 This is the plane flight trajectory of each node in the cluster system according to the embodiment of the present invention.

[0049] Figure 5 The collaborative positioning error before optimization is an embodiment of the present invention.

[0050] Figure 6 This is the collaborative positioning error after optimization in the embodiment of the present invention. DETAILED DESCRIPTION

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

[0052] When some nodes in the cluster system cannot locate themselves, they need to use the status and distance information of the current locatable nodes for collaborative positioning. Since there are errors in the distance information between nodes and the positioning results of the locatable nodes, if the combination of locatable nodes selected is not appropriate, the output collaborative positioning results are very sensitive to the above errors, resulting in unreliable collaborative positioning results and significantly increased errors, which is not conducive to the cluster system to carry out tasks such as formation flying and collaborative detection.

[0053] Therefore, the technical problem to be solved by the present invention is how to optimize the collaborative positioning method of the cluster system, reduce the sensitivity of the positioning result to the information source error, and achieve reliable positioning of the cluster system in a complex environment.

[0054] In order to solve the above technical problems, the present invention provides a cluster collaborative positioning result optimization method based on good performance evaluation. According to whether it is currently positioned, the role of each node in the cluster is divided into collaborative positioning reference nodes and collaborative positioning demand nodes, and all combinations of positioning reference nodes are generated; the position matrix of each positioning reference node combination is calculated, and the good performance characteristics of the matrix are evaluated; different positioning reference node combinations are used for collaborative positioning, and the positioning results are integrated with the evaluation results of the matrix good performance characteristics as weights, thereby improving the success rate and accuracy of collaborative positioning. Figure 2 The specific contents include:

[0055] 1. Co-location principle and error propagation mechanism;

[0056] The combined navigation device of each node in the cluster system generally includes an inertial measurement unit, GPS, barometric altimeter, etc., and some nodes are equipped with a scene matching navigation unit; each node is equipped with a data link communication module to achieve collaborative information interaction and relative distance information measurement. Based on the above hardware equipment, the following are the conditions for the use of cluster system collaborative positioning:

[0057] 1) There are at least three nodes in the cluster that are normally positioned (satellite is not rejected, or equipped with scene matching navigation and normal matching positioning), and their absolute position coordinates are known;

[0058] 2) The relative distance information between nodes is known;

[0059] 3) The altitude information of the collaborative positioning reference node and the collaborative positioning demand node are both known (barometric altitude).

[0060] The principle of co-location is as follows Figure 1 As shown, points A, B, and C represent currently locatable nodes, and their corresponding north celestial east coordinates are (X1, Y1, Z1), (X2, Y2, Z2), and (X3, Y3, Z3), respectively. Point D represents the node that needs to be co-located. The relative distances between point D and points A, B, and C are measured by the data link as d1, d2, and d3. A sphere is drawn with points A, B, and C as the center and d1, d2, and d3 as the radius. At the same time, a circle is drawn parallel to the OXZ plane at the known height of point D. Ideally, the three spheres and the circle will intersect at point D.

[0061] Assume that the north celestial east coordinates of point D are (X, Y, Z), which satisfies the following set of equations:

[0062]

[0063] Since the Y coordinates of points A, B, C, and D can be obtained indirectly through the pressure altitude, we can get

[0064]

[0065] Differencing the above system of equations twice can eliminate the higher-order terms and obtain the following system of linear equations:

[0066]

[0067] Written in matrix form:

[0068] Qw=v

[0069] in

[0070]

[0071]

[0072] If the inverse matrix of matrix Q exists, the X and Z coordinates of point D can be solved, that is, the vector w:

[0073] w=Q -1 v

[0074] From the above positioning principle, we can know that the collaborative positioning result comes from solving the linear equation group Qw=v, where the matrix Q is related to the position of the reference node, and the vector v is related to the distance information between the nodes and the position of the reference node. In actual flight, there are errors in the position of the reference node and the distance information measured by the data link, which leads to a certain deviation between Q and v and their true values, thus affecting the solution of the equation group.

[0075] The positioning result of the cluster collaborative positioning reference node is the north celestial east coordinate relative to the origin. Under the condition of satellite availability, the horizontal position error is generally: less than or equal to 10m (1σ), and under the condition of satellite denial, the horizontal position error of positioning through scene matching is generally: less than or equal to 30m (1σ). Combined with the magnitude of the north celestial east position, it can be seen that the error has a relatively small impact on the position matrix Q and a relatively large impact on the vector v. On the other hand, the error of the data link ranging information is: less than or equal to 30m for a single group, and this error is only reflected in the vector v. Since collaborative positioning requires at least three sets of ranging information, it can be seen from the principle of geometry that the actual impact on the positioning result is greater than the sum of the three sets of ranging errors.

[0076] In view of this situation, the influence of the error in vector v on the accuracy of collaborative positioning is analyzed in detail. The matrix condition number κ(Q) is introduced to measure the sensitivity of the solution of the system of equations to the error in vector v, that is, the well-conditioned characteristic. When the condition number κ(Q) is close to 1, the position matrix Q is a well-conditioned matrix, and the positioning result is less affected by the error in vector v; when the condition number κ(Q) is larger, the position matrix Q is an ill-conditioned matrix, and the positioning result is more affected by the error in vector v. In order to ensure the reliability of the positioning result, the position matrix Q corresponding to the selected positioning reference node should have a well-conditioned characteristic, otherwise it is not suitable for collaborative positioning solution.

[0077] 2. Co-location scheme based on good-state characteristic evaluation;

[0078] The collaborative positioning scheme mainly includes node positioning state judgment, collaborative positioning reference node combination generation, reference node combination position matrix goodness evaluation, collaborative positioning result solution and fusion, etc. Its information flow is summarized as follows:

[0079] During the flight, the flight control of each node comprehensively judges the current positioning status of the aircraft based on the satellite positioning status and the scene matching positioning status. When the satellite is positioned normally or the node is equipped with scene matching navigation and successfully positioned under satellite denial, the flag is set to 1, otherwise it is set to 0. The flight control sends the positioning status of the aircraft to the cluster collaborative controller (co-controller). The co-controller determines whether the aircraft needs to be collaboratively positioned and sends the positioning status of the aircraft to the data link. The data link broadcasts the positioning status of the aircraft to other nodes in the cluster, measures the relative distance information between the aircraft and other nodes, and transmits it back to the co-controller. The co-controller runs the collaborative positioning algorithm based on the well-behaved characteristic evaluation and outputs the collaborative positioning result to the flight control. The flight control determines whether the collaborative positioning result is available based on the received collaborative positioning success flag. If available, it is sent to the navigation device for combination.

[0080] The specific details of the collaborative positioning algorithm based on well-conditioned characteristic evaluation are as follows:

[0081] 1) When the local machine needs to perform collaborative positioning, the co-controller extracts the positioning status of each node in the cluster from the inter-machine collaborative information γ i , and extract the effective relative distance d between the local machine and other nodes from the data link ranging information i , where i=1,2,...,n is the serial number of each node in the cluster, and n is the total number of nodes in the cluster;

[0082] 2) The co-controller determines the location of each node in the cluster according to its location status γ i Filter the positioning reference nodes and generate the combination of the serial numbers of all feasible positioning reference nodes, recorded as matrix C∈R m×3 , where m is the number of positioning reference node combinations, and 3 is the number of nodes required for a single collaborative positioning;

[0083] 3) Extract the location information of the positioning reference node from the inter-machine collaborative information (X i ,Y i ,Z i ), and calculate the position matrix Q of the positioning reference node combination k , where k = 1, 2, ..., m;

[0084] 4) Calculate the matrix Q corresponding to each group of positioning reference nodes k The condition number κ(Q k ), evaluate the well-conditioned properties of each matrix:

[0085]

[0086] Among them, κ max >1 is the threshold set according to the collaborative positioning accuracy requirement;

[0087] 5) If the matrix Q kis a well-conditioned matrix, then the kth combination of positioning reference nodes is valid. If the matrix Q k If it is an ill-conditioned matrix, the combination of the kth positioning reference node is invalid and is removed from the matrix C;

[0088] 6) The positioning reference node combination matrix at this time is recorded as in is the number of actually available positioning reference node combinations;

[0089] 7) Traverse the sequence combination matrix of the positioning reference node For each row in , the collaborative localization algorithm in the previous section is called multiple times, and the weighted average of multiple localization results is performed based on the matrix well-conditioned characteristics, that is,

[0090]

[0091] in, w k is the collaborative positioning result based on the kth positioning reference node combination, This is the co-localization result after fusion.

[0092] Example:

[0093] 1: Start, comprehensively judge the current positioning status of the machine based on the satellite positioning status and scene matching positioning status, and broadcast the information to other nodes;

[0094] 2: The unlocated node selects the positioning reference nodes according to the positioning status of other nodes in the group, and generates a combination of all positioning reference nodes;

[0095] 3: Calculate the position matrix of each group of positioning reference nodes and evaluate the well-conditioned characteristics of the matrix;

[0096] 4: Determine whether the combination of positioning reference nodes is feasible based on the well-behaved characteristics of the matrix;

[0097] 5: Traverse all feasible positioning reference node combinations, call the collaborative positioning algorithm, and perform weighted averaging of multiple positioning results based on the matrix well-conditioned characteristics;

[0098] 6: Output the collaborative localization result after fusion.

[0099] The following describes it in detail with reference to specific embodiments.

[0100] A cluster system with 6 nodes is selected for simulation. The nodes are represented by ①-⑥. Nodes ③ and ④ are set as nodes that need to be co-located, and the remaining nodes are positioning reference nodes. In order to simulate the ranging information in the real environment, the ranging error that satisfies the normal distribution is introduced, such as Figure 3 shown. Figure 4The plane flight trajectory of each node of the cluster system is given. When turning and changing formations, the position of the positioning reference node is close to collinearity, that is, it enters the singular state of collaborative positioning. The collaborative positioning results before and after optimization are shown in Figure 2. Figure 5 and Figure 6 As shown, when collaborative positioning is impossible, the positioning error is represented by 0. It can be seen that after using the optimization method proposed in the present invention, the success rate and positioning accuracy of cluster collaborative positioning are greatly improved.

Claims

1. A cluster collaborative positioning result optimization method based on good-state characteristic evaluation, characterized in that: The steps include: Step 1: Co-localization principle and error propagation mechanism; Step 1-1: The combined navigation device of each node in the cluster system includes an inertial measurement unit, GPS, and a barometric altimeter, and some nodes are equipped with a scene matching navigation unit; each node is equipped with a data link communication module to achieve collaborative information exchange and relative distance information measurement; The conditions for using cluster system co-location are: 1) There are at least three normally positioned nodes in the cluster, and their absolute position coordinates are known; 2) The relative distance information between nodes is known; 3) The height information of the co-location reference node and the co-location demand node are both known; Step 1-2: Principle of co-location; Assume that points A, B, and C represent currently locatable nodes, and their corresponding north celestial east coordinates are (X1, Y1, Z1), (X2, Y2, Z2), and (X3, Y3, Z3), respectively. Point D represents the node that needs to be co-located. The relative distances between point D and points A, B, and C are measured by the data link as d1, d2, and d3. Draw a sphere with points A, B, and C as the center and d1, d2, and d3 as the radius. At the same time, draw a circle parallel to the OXZ plane at the known height of point D. Ideally, the three spheres and the circle will intersect at point D. Assume that the north celestial east coordinates of point D are (X, Y, Z), which satisfies the following equations: Since the Y coordinates of points A, B, C, and D can be obtained indirectly through the pressure altitude, we can get: Perform two differences to eliminate the higher-order terms and obtain the following system of linear equations: Written in matrix form: Qw=v in If the inverse matrix of matrix Q exists, solve for the X and Z coordinates of point D, that is, vector w: w=Q -1 v The co-localization result comes from solving the linear equation system Qw=v, where the matrix Q is related to the position of the reference node, and the vector v is related to the distance information between the nodes and the position of the reference node; Step 2: Co-location scheme based on well-behaved characteristic evaluation; Step 2-1: The collaborative positioning solution includes node positioning state judgment, collaborative positioning reference node combination generation, reference node combination position matrix well-behaved characteristics evaluation, collaborative positioning result solution and fusion. The information flow is as follows: During the flight, the flight control of each node comprehensively judges the current positioning status of the aircraft based on the satellite positioning status and the scene matching positioning status. When the satellite is positioned normally or the node is equipped with scene matching navigation and successfully positioned under satellite denial, the flag is set to 1, otherwise it is set to 0; the flight control sends the positioning status of the aircraft to the cluster cooperative controller, and the cooperative controller determines whether the aircraft needs to be cooperatively positioned and sends the positioning status of the aircraft to the data link; the data link broadcasts the positioning status of the aircraft to other nodes in the cluster, and measures the relative distance information between the aircraft and other nodes, and transmits it back to the cooperative controller; the cooperative controller runs the cooperative positioning algorithm based on the well-behaved characteristic evaluation, and outputs the cooperative positioning result to the flight control; the flight control determines whether the cooperative positioning result is available based on the received cooperative positioning success flag, and if available, it is sent to the navigation device for combination; Step 2-2: Co-location algorithm based on well-behaved characteristic evaluation; Step 2-2-1: When the local machine needs to perform collaborative positioning, the co-controller extracts the positioning status of each node in the cluster from the inter-machine collaborative information γ i , and extract the effective relative distance d between the local machine and other nodes from the data link ranging information i , where i=1,2,...,n is the serial number of each node in the cluster, and n is the total number of nodes in the cluster; Step 2-2-2: The co-controller determines the location of each node in the cluster according to its location status γ i Filter the positioning reference nodes and generate the combination of the serial numbers of all feasible positioning reference nodes, recorded as matrix C∈R m×3 , where m is the number of positioning reference node combinations, and 3 is the number of nodes required for a single collaborative positioning; Step 2-2-3: Extract the location information of the positioning reference node from the inter-machine collaborative information (X i ,Y i ,Z i ), and calculate the position matrix Q of the positioning reference node combination k , where k = 1, 2, ..., m; Step 2-2-4: Calculate the matrix Q corresponding to each set of positioning reference nodes k The condition number κ(Q k ), evaluate the well-conditioned properties of each matrix: Among them, κ max >1 is the threshold set according to the collaborative positioning accuracy requirement; Step 2-2-5: If the matrix Q k is a well-conditioned matrix, then the kth combination of positioning reference nodes is valid. If the matrix Q k If it is an ill-conditioned matrix, the combination of the kth positioning reference node is invalid and is removed from the matrix C; Step 2-2-6: The positioning reference node combination matrix at this time is recorded as in is the number of actually available positioning reference node combinations; Step 2-2-7: Traverse the sequence combination matrix of the reference node For each row in , the collaborative positioning algorithm is called multiple times, and the weighted average of multiple positioning results is performed based on the matrix well-conditioned characteristics, that is: in, w k is the collaborative positioning result based on the kth positioning reference node combination, This is the co-localization result after fusion.

2. The cluster collaborative positioning result optimization method based on good-state characteristic evaluation according to claim 1, characterized in that: The normally positioned node refers to a node that is not rejected by the satellite, is equipped with scene matching navigation and is normally matched and positioned.

Images