A step-by-step collaborative positioning method, system, device and medium

Abstract

The present invention relates to the technical field of cooperative positioning, and provides a step-by-step cooperative positioning method, system, device and medium. The method of the present invention includes: arbitrarily selecting a node in the unmanned cluster as the master node, and the remaining nodes as slave nodes, calculating the geometric factor between the slave node and the master node, and selecting the slave node with the smallest geometric factor as the cooperative positioning target node; selecting an angle between a master node, the cooperative positioning target node and other master nodes, and selecting the number of master nodes according to the navigation accuracy to obtain the selected master nodes; solving the high-precision navigation information of the cooperative positioning target node, and using the number and position information of the selected master nodes, and calculating the navigation information of the cooperative positioning target node according to the centralized cooperative positioning algorithm. Through step-by-step cooperative positioning, the present invention effectively reduces the demand of the cooperative positioning system for communication capacity, and at the same time greatly reduces the computational complexity of cooperative positioning.

Description

Technical Field

[0001] The present invention relates to the technical field of collaborative positioning of unmanned devices, and provides a step-by-step collaborative positioning method, system, device and medium. Background Art

[0002] Due to the differences in the accuracy of the navigation devices installed on different unmanned boats in an unmanned boat cluster, or some unmanned aerial vehicles and unmanned boats having no high-precision navigation information due to satellite navigation limitations, generally, through centralized collaborative positioning, the high-precision navigation information of some unmanned aerial vehicles and unmanned boats in the unmanned aerial vehicle and unmanned boat cluster is shared throughout the unmanned aerial vehicle and unmanned boat cluster, thereby improving the navigation information quality of unmanned aerial vehicles and unmanned boats with low-precision navigation information. In the centralized collaborative positioning of an unmanned boat cluster, relative observations need to be performed between unmanned boats, which poses relatively high requirements for communication resources and power energy. In addition, the computational workload of the centralized collaborative positioning of an unmanned boat cluster is very large, posing relatively high requirements for the computing capabilities of unmanned aerial vehicles and unmanned boats. Summary of the Invention

[0003] The present invention aims to at least solve one of the technical problems existing in the related art. For this reason, the present invention provides a step-by-step collaborative positioning method, system, device and medium, which realizes step-by-step correction of the navigation devices of unmanned boats with low-precision navigation information in an unmanned boat cluster. Each collaborative positioning only includes one unmanned boat with low-precision navigation information and a sufficient number of unmanned boats with high-precision navigation information.

[0004] The present invention provides a step-by-step collaborative positioning method, including:

[0005] S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the collaborative positioning target node;

[0006] S2: Select a master node and the collaborative positioning target node to connect and obtain a first side. Calculate the included angle between the connection lines of the collaborative positioning target node and other master nodes and the first side. Select the number of master nodes according to the navigation accuracy to obtain the selected master nodes;

[0007] S3: Solve the high-precision navigation information of the collaborative positioning target node. Use the number and position information of the selected master nodes, and according to the centralized collaborative positioning algorithm, calculate the navigation information of the collaborative positioning target node;

[0008] S4: Determine whether all slave nodes have performed collaborative positioning. If all slave nodes have performed collaborative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes have performed collaborative positioning, execute step S5;

[0009] S5: Determine whether the formation of the unmanned cluster has changed. If it has changed, execute step S6; if not, execute step S7;

[0010] S6: Set the collaborative positioning target node as the master node and return to step S1;

[0011] S7: Set the collaborative positioning target node as the master node, re - select the slave node with the smallest geometric factor as the collaborative positioning target node, and return to step S2.

[0012] For a step - by - step collaborative positioning method provided by the present invention, the calculation formula of the geometric factor is:

[0013]

[0014] Where, is the geometric factor of the th slave node, is the slave node ordinal number, is the total number of slave nodes, , is the matrix composed of the unit direction vectors of all master nodes relative to the th slave node, is the matrix transpose.

[0015] For a step - by - step collaborative positioning method provided by the present invention, the navigation accuracy includes high accuracy, medium accuracy, and low accuracy:

[0016] When the navigation accuracy is low accuracy, select a master node with an included angle of and a master node with an included angle of as the selected master nodes;

[0017] When the navigation accuracy is medium accuracy, select a master node with an included angle of , a master node with an included angle of and a master node with an included angle of as the selected master nodes;

[0018] When the navigation accuracy is high accuracy, select a master node with an included angle of , a master node with an included angle of , a master node with an included angle of and a master node with an included angle of as the selected master nodes.

[0019] For a step - by - step collaborative positioning method provided by the present invention, the steps of solving the high - precision navigation information of the collaborative positioning target node include:

[0020] S10: Calculate the centralized cooperative localization state transition matrix of slave nodes:

[0021]

[0022]

[0023] where is the node state transition matrix of the th slave node relative to the th master node, is the ordinal number of the selected master node, is the total number of selected master nodes, , is the time-related constant of the th master node, is the centralized cooperative localization state transition matrix of the th slave node;

[0024] S20: Calculate the centralized cooperative localization process noise driving matrix of slave nodes:

[0025]

[0026]

[0027] where is the process noise driving matrix of the th slave node relative to the th master node, is the identity matrix, is the centralized cooperative localization process noise driving matrix of the th slave node;

[0028] S30: Calculate the centralized cooperative localization process system noise matrix of slave nodes:

[0029]

[0030]

[0031] where is the system noise sequence of the th slave node relative to the th master node, is the system Gaussian white noise, is the system flicker noise, is the matrix transpose, is the centralized cooperative localization system noise sequence of the th slave node;

[0032] S40: Calculate the centralized collaborative positioning state vector according to the system state equation:

[0033]

[0034] Among them, is the centralized collaborative positioning state vector of the th slave node, is the first derivative.

[0035] According to a step-by-step collaborative positioning method provided by the present invention, the steps of the centralized collaborative positioning algorithm include:

[0036] S100: Calculate the observation vector of the slave node:

[0037]

[0038] Among them, is the centralized observation vector of the th slave node, the th slave node's observation vector relative to the th master node;

[0039] S200: Calculate the slave node observation matrix :

[0040]

[0041] S300: According to the observation vector of the slave node and the slave node observation matrix, recalculate the position observation vector of the slave node:

[0042]

[0043] Among them, is the updated position observation vector, is the collaborative positioning state vector of the th slave node relative to the th master node, is the th slave node's observation vector relative to the th master node, is the th slave node's collaborative positioning state vector relative to the th master node, , is the noise matrix;

[0044] S400: Update the observation vector:

[0045]

[0046] Among them, is the observation vector of the th slave node after update.

[0047] According to a step-by-step collaborative positioning method provided by the present invention, the method for determining whether the formation of the unmanned cluster changes is as follows: when the included angle of the selected master node remains unchanged, it is regarded that the formation has not changed; when the included angle of the selected master node changes, it is regarded that the formation has changed.

[0048] According to a step-by-step collaborative positioning method provided by the present invention, when the time from the start of step S1 reaches an integer multiple of the inertial navigation protection 1 nautical mile time, restart S1.

[0049] The present invention also provides a step-by-step collaborative positioning system, including:

[0050] Node selection module: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the collaborative positioning target node;

[0051] Navigation information calculation module: Select a master node and connect it with the collaborative positioning target node to obtain the first side. Calculate the included angle between the connection line between the collaborative positioning target node and other master nodes and the first side. Select the number of master nodes according to the navigation accuracy to obtain the selected master nodes; Solve the high-precision navigation information of the collaborative positioning target node, and use the number and position information of the selected master nodes to calculate the navigation information of the collaborative positioning target node according to the centralized collaborative positioning algorithm;

[0052] First judgment module: Judge whether all slave nodes perform collaborative positioning. If all slave nodes perform collaborative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes perform collaborative positioning, execute the second judgment module;

[0053] Second judgment module: Judge whether the formation of the unmanned cluster changes. If it changes, execute the first selection module; If it does not change, execute the second selection module;

[0054] First selection module: Take the collaborative positioning target node as the master node and return to the node selection module;

[0055] Second selection module: Take the collaborative positioning target node as the master node, re-select the slave node with the smallest geometric factor as the collaborative positioning target node, and return to the step of calculating navigation information module.

[0056] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, the steps of any one of the above-described step-by-step cooperative positioning methods are implemented.

[0057] The present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of any one of the above-described step-by-step cooperative positioning methods are implemented.

[0058] One or more of the above technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0059] A step-by-step cooperative positioning method, system, device, and medium provided by the present invention effectively reduce the communication capacity requirement of the cooperative positioning system and greatly reduce the computational complexity of cooperative positioning by step-by-step cooperative positioning, that is, only one slave node is preferably selected to participate in cooperative positioning each time, and the master node participating in cooperative positioning is also preferably selected. It can provide navigation information guarantee to meet the operation task requirements for an unmanned boat cluster and greatly improve the usability of cooperative positioning.

[0060] Additional aspects and advantages of the present invention will be given in part in the following description, become apparent in part from the following description, or be learned through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments or the prior art description. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0062] Figure 1 is a schematic flow chart of a step-by-step cooperative positioning method provided by the present invention.

[0063] Figure 2 is a structural block diagram of a step-by-step cooperative positioning system provided by the present invention.

[0064] Figure 3 is a schematic structural diagram of the electronic device provided by the present invention.

[0065] Reference numerals:

[0066] 810, processor; 820, communication interface; 830, memory; 840, communication bus. DETAILED DESCRIPTION

[0067] To make the objectives, technical solutions, and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the scope of protection of the present invention. The following embodiments are used to illustrate the present invention, but cannot be used to limit the scope of the present invention.

[0068] In the description of this specification, the description with reference to terms such as "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the embodiments of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in a suitable manner in any one or more embodiments or examples. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples.

[0069] The following will be combined with Figures 1 to 3 to describe the present invention.

[0070] Embodiment

[0071] Suppose there are several unmanned boats with high-precision navigation information in a step-by-step cooperative positioning system, which are referred to as master nodes here, and several unmanned boats with low-precision navigation information, which are referred to as slave nodes here. For the convenience of derivation, it is assumed here that the slave nodes have reduced navigation accuracy due to satellite navigation interference. Generally, the low-precision inertial navigation index configured for unmanned boats is about 2 to 4 hours per nautical mile. The navigation accuracy index of 1 nautical mile can meet most of the operation requirements. Therefore, it is only necessary to correct the inertial navigation system error of the slave nodes by cooperative positioning within 2 to 4 hours.

[0072] As Figure 1 shown, Figure 1 is a schematic flow chart of a step-by-step cooperative positioning method provided by the present invention. It includes the following steps:

[0073] S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the cooperative positioning target node;

[0074] S2: Select a master node and connect it to the collaborative positioning target node to obtain the first edge. Calculate the angles between the connections of the collaborative positioning target node and other master nodes and the first edge. Select the number of master nodes according to the navigation accuracy to obtain the selected master nodes;

[0075] S3: Solve the high-precision navigation information of the collaborative positioning target node. Use the number and position information of the selected master nodes, and calculate the navigation information of the collaborative positioning target node according to the centralized collaborative positioning algorithm;

[0076] S4: Determine whether all slave nodes have performed collaborative positioning. If all slave nodes have performed collaborative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes have performed collaborative positioning, execute step S5;

[0077] S5: Determine whether the formation of the unmanned cluster has changed. If it has changed, execute step S6; if it has not changed, execute step S7;

[0078] S6: Take the collaborative positioning target node as the master node and return to step S1;

[0079] S7: Take the collaborative positioning target node as the master node, re-select the slave node with the smallest geometric factor as the collaborative positioning target node, and return to step S2.

[0080] Specifically, the calculation formula of the geometric factor is:

[0081]

[0082] where is the geometric factor of the th slave node, is the slave node ordinal number, is the total number of slave nodes, , is the matrix composed of the unit direction vectors of all master nodes relative to the th slave node, is the matrix transpose.

[0083] The accuracy of collaborative positioning is mainly related to the position accuracy of the master node, the relative observation accuracy between nodes, and the geometric dilution of precision. When the position accuracy of the master node and the relative observation accuracy between nodes are certain, the smaller the geometric dilution of precision, the higher the collaborative positioning accuracy. Therefore, select the slave node corresponding to the smallest

[0084] as the first slave node participating in collaborative positioning.

[0085] Specifically, the navigation accuracy includes high precision, medium precision, and low precision: When the navigation accuracy is low precision, select an angle of The main node and an included angle of The main node, as the selected main node;

[0086] When the navigation accuracy is medium accuracy, select a main node with an included angle of A main node with an included angle of A main node with an included angle of The main node, as the selected main node;

[0087] When the navigation accuracy is high accuracy, select a main node with an included angle of A main node with an included angle of A main node with an included angle of A main node with an included angle of The main node, as the selected main node.

[0088] Specifically, the steps for solving the high-precision navigation information of the cooperative positioning target node include:

[0089] S10: Calculate the centralized cooperative positioning state transition matrix of the slave nodes:

[0090]

[0091]

[0092] Among them, Is the node state transition matrix of the th slave node relative to the th main node, Is the ordinal number of the selected main node, Is the total number of selected main nodes, , Is the time-related constant of the th main node, Is the th centralized cooperative positioning state transition matrix of the slave nodes;

[0093] S20: Calculate the centralized cooperative positioning process noise driving matrix of the slave nodes:

[0094]

[0095]

[0096] Among them, Is the process noise driving matrix of the th slave node relative to the th main node, Is the identity matrix, Is the The process noise driving matrix for the slave nodes in the centralized collaborative localization;

[0097] S30: Calculate the system noise matrix for the centralized collaborative localization of the slave nodes:

[0098]

[0099]

[0100] where, is the system noise sequence of the th slave node relative to the th master node, is the system Gaussian white noise, is the system flicker noise, is the matrix transpose, the th system noise sequence for the centralized collaborative localization of the slave nodes;

[0101] S40: Calculate the centralized collaborative localization state vector according to the system state equation:

[0102]

[0103] where, is the centralized collaborative localization state vector of the th slave node, is the first derivative.

[0104] Specifically, the steps of the centralized collaborative localization algorithm include:

[0105] S100: Calculate the observation vector of the slave nodes:

[0106]

[0107] where, is the centralized observation vector of the th slave node, the th slave node's observation vector relative to the th master node;

[0108] S200: Calculate the slave node observation matrix :

[0109]

[0110] S300: Recalculate the position observation vector of the slave nodes according to the observation vector and the observation matrix of the slave nodes:

[0111]

[0112] Among them, is the updated position observation vector, is the collaborative positioning status vector of the th slave node relative to the th master node, is the th slave node relative to the th master node's observation vector, is the th slave node relative to the th master node's collaborative positioning status vector, , is the noise matrix;

[0113] S400: Update the observation vector:

[0114]

[0115] Among them, is the observation vector of the th updated slave node.

[0116] The relative observations added to the observation vector can increase the constraints on the navigation state of each unmanned boat. The increase in effective observation information will inevitably reduce the state estimation error and the state estimation error covariance. From the centralized collaborative positioning calculation process, it can be seen that there are many matrix operations involved. The step-by-step collaborative positioning transforms the original collaborative positioning matrix, and the size of the transformed matrix is not greater than , greatly reducing the computational complexity.

[0117] Specifically, the method for judging whether the formation of the unmanned cluster has changed is as follows: when the included angle of the selected master node remains unchanged, it is considered that the formation has not changed; when the included angle of the selected master node changes, it is considered that the formation has changed.

[0118] Specifically, when the time elapsed since the start of step S1 reaches an integer multiple of the inertial navigation guarantee time of 1 nautical mile, restart S1.

[0119] In view of the situation where there are both low-precision navigation unmanned boats and high-precision navigation information unmanned boats in the unmanned boat cluster collaborative positioning system, the present invention proposes a design scheme for a step-by-step collaborative positioning system. This scheme uses step-by-step collaborative positioning, that is, only one slave node is preferably selected to participate in collaborative positioning each time, and at the same time, the master node participating in collaborative positioning is preferably selected, thereby effectively reducing the demand for communication capacity of the collaborative positioning system and greatly reducing the computational complexity of collaborative positioning. It can provide navigation information guarantee that meets the requirements of the operation task for the unmanned boat cluster and greatly improve the usability of collaborative positioning.

[0120] Such as Figure 2As shown Figure 2 is a structural block diagram of a step-by-step collaborative positioning system provided by the present invention. The system includes:

[0121] Selection node module: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the collaborative positioning target node;

[0122] Navigation information calculation module: Select a master node and the collaborative positioning target node to form a first side. Calculate the angles between the lines connecting the collaborative positioning target node and other master nodes and the first side. Select the number of master nodes according to the navigation accuracy to obtain the selected master nodes. Solve the high-precision navigation information of the collaborative positioning target node, and use the number and position information of the selected master nodes to calculate the navigation information of the collaborative positioning target node according to the centralized collaborative positioning algorithm;

[0123] First judgment module: Judge whether all slave nodes perform collaborative positioning. If all slave nodes perform collaborative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes perform collaborative positioning, execute the second judgment module;

[0124] Second judgment module: Judge whether the formation of the unmanned cluster changes. If it changes, execute the first selection module; if it does not change, execute the second selection module;

[0125] First selection module: Use the collaborative positioning target node as the master node and return to the selection node module;

[0126] Second selection module: Use the collaborative positioning target node as the master node, re-select the slave node with the smallest geometric factor as the collaborative positioning target node, and return to the step of calculating navigation information module.

[0127] Next, a step-by-step collaborative positioning device provided by the present invention will be described. The step-by-step collaborative positioning device described below can be mutually referred to the step-by-step collaborative positioning method described above.

[0128] Figure 3 Illustrates a schematic physical structure diagram of an electronic device, as Figure 3 shown. The electronic device may include: a processor 810, a communication interface 820, a memory 830, and a communication bus 840. Among them, the processor 810, the communication interface 820, and the memory 830 communicate with each other through the communication bus 840. The processor 810 can call the logical instructions in the memory 830 to execute a step-by-step collaborative positioning method, and the method includes:

[0129] S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the cooperative positioning target node;

[0130] S2: Select a master node and connect it to the cooperative positioning target node to obtain the first side. Calculate the angles between the lines connecting the cooperative positioning target node and other master nodes and the first side. Select the number of master nodes according to the navigation accuracy to obtain the selected master nodes;

[0131] S3: Solve the high-precision navigation information of the cooperative positioning target node. Use the number and position information of the selected master nodes, and according to the centralized cooperative positioning algorithm, calculate the navigation information of the cooperative positioning target node;

[0132] S4: Determine whether all slave nodes have performed cooperative positioning. If all slave nodes have performed cooperative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes have performed cooperative positioning, execute step S5;

[0133] S5: Determine whether the formation of the unmanned cluster has changed. If it has changed, execute step S6; if it has not changed, execute step S7;

[0134] S6: Use the cooperative positioning target node as the master node and return to step S1;

[0135] S7: Use the cooperative positioning target node as the master node, re-select the slave node with the smallest geometric factor as the cooperative positioning target node, and return to step S2.

[0136] In addition, when the logical instructions in the above-mentioned memory 830 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disks, or optical discs that can store program codes.

[0137] On the other hand, the present invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium. The computer program includes program instructions. When the program instructions are executed by a computer, the computer is capable of executing a step-by-step collaborative positioning method provided by each of the above methods. The method includes:

[0138] S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the collaborative positioning target node;

[0139] S2: Select a master node and connect it with the collaborative positioning target node to obtain a first side. Calculate the angles between the connections of the collaborative positioning target node and other master nodes and the first side. Select the number of master nodes according to the navigation accuracy to obtain the selected master nodes;

[0140] S3: Solve the high-precision navigation information of the collaborative positioning target node. Use the number and position information of the selected master nodes, and calculate the navigation information of the collaborative positioning target node according to the centralized collaborative positioning algorithm;

[0141] S4: Determine whether all slave nodes have performed collaborative positioning. If all slave nodes have performed collaborative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes have performed collaborative positioning, execute step S5;

[0142] S5: Determine whether the formation of the unmanned cluster has changed. If it has changed, execute step S6; if it has not changed, execute step S7;

[0143] S6: Use the collaborative positioning target node as the master node, and return to step S1;

[0144] S7: Use the collaborative positioning target node as the master node, re-select the slave node with the smallest geometric factor as the collaborative positioning target node, and return to step S2.

[0145] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they may be located in one place, or may be distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art can understand and implement it without creative labor.

[0146] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on such an understanding, the essence of the above technical solution, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0147] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

[0148] It should be noted that the embodiments of the present disclosure can be implemented by hardware, software, or a combination of software and hardware. The hardware part can be implemented using dedicated logic: the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated design hardware. Those skilled in the art can understand that the above devices and methods can be implemented using computer-executable instructions and / or included in processor control code, for example, such code is provided on a programmable memory or a data carrier such as an optical or electronic signal carrier.

[0149] In addition, although the operations of the method of the present disclosure are described in a specific order in the drawings, this does not require or imply that these operations must be performed in that specific order, or that all the operations shown must be performed to achieve the desired result. On the contrary, the steps depicted in the flowchart can be changed in the order of execution. Additionally or alternatively, some steps can be omitted, multiple steps can be combined into one step for execution, and / or one step can be decomposed into multiple steps for execution. It should also be noted that the features and functions of two or more devices according to the present disclosure can be embodied in one device. Conversely, the features and functions of one device described above can be further divided and embodied by multiple devices.

[0150] Although the present disclosure has been described with reference to several specific embodiments, it should be understood that the present disclosure is not limited to the specific embodiments disclosed. The present disclosure is intended to cover various modifications and equivalent arrangements included within the spirit and scope of the appended claims.

Claims

1. A step-by-step collaborative positioning method, characterized in that, It includes the following steps: S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the cooperative positioning target node. The calculation formula of the geometric factor is: Among them, is the geometric factor of the th slave node, is the slave node ordinal number, is the total number of slave nodes, , is the matrix composed of the unit direction vectors of all master nodes relative to the th slave node, is the matrix transpose; S2: Select a master node and the cooperative positioning target node to obtain the first side. Calculate the angles between the lines connecting the cooperative positioning target node and other master nodes and the first side. Select the number of master nodes according to the navigation accuracy to obtain the selected master nodes. The navigation accuracy includes high accuracy, medium accuracy, and low accuracy: When the navigation accuracy is low, select a main node with an included angle of and a main node with an included angle of as the selected main node; When the navigation accuracy is medium accuracy, select a main node with an included angle of , a main node with an included angle of , and a main node with an included angle of as the selected main nodes; When the navigation accuracy is high, select a main node with an included angle of , a main node with an included angle of , a main node with an included angle of and a main node with an included angle of as the selected main nodes; S3: Solve the high-precision navigation information of the cooperative positioning target node. Use the number and position information of the selected master nodes, and calculate the navigation information of the cooperative positioning target node according to the centralized cooperative positioning algorithm. S4: Determine whether all slave nodes have performed cooperative positioning. If all slave nodes have performed cooperative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes have performed cooperative positioning, execute step S5; S5: Determine whether the formation of the unmanned cluster has changed. If it has changed, execute step S6; if it has not changed, execute step S7; S6: Use the cooperative positioning target node as the master node and return to step S1; S7: Use the cooperative positioning target node as the master node, re-select the slave node with the smallest geometric factor as the cooperative positioning target node, and return to step S2.

2. The step-by-step collaborative positioning method according to claim 1, wherein The steps of solving the high-precision navigation information of the cooperative positioning target node include: S10: Calculate the centralized cooperative positioning state transition matrix of the slave nodes: Among them, is the node state transition matrix of the th slave node relative to the th master node, is the selected master node ordinal number, is the total number of selected master nodes, , is the time-related constant of the th master node, is the centralized cooperative localization state transition matrix of the th slave node; S20: Calculate the centralized cooperative positioning process noise driving matrix of the slave nodes: Among them, is the -th process noise driving matrix of the -th slave node relative to the master node, is the identity matrix, is the -th centralized cooperative localization process noise driving matrix of the slave node; S30: Calculate the centralized cooperative positioning process system noise matrix of the slave nodes: Among them, is the th system noise sequence of the th slave node relative to the master node, is the system Gaussian white noise, is the system flicker noise, is the matrix transpose, is the centralized cooperative positioning system noise sequence of the th slave node; S40: Calculate the centralized cooperative positioning state vector according to the system state equation: Among them, is the th centralized collaborative positioning state vector of slave nodes, is the first derivative of.

3. The step-by-step collaborative positioning method according to claim 2, wherein The steps of the centralized cooperative positioning algorithm include: S100: Calculate the observation vector of the slave nodes: Among them, is the centralized observation vector of the th slave node, and is the observation vector of the th th slave node relative to the th master node; S200: Calculate the observation matrix of the slave node : ； S300: Recalculate the position observation vector of the slave nodes according to the observation vector of the slave nodes and the slave node observation matrix: Among them, is the updated position observation vector, is the collaborative positioning status vector of the th slave node relative to the th master node, is the observation vector of the th slave node relative to the th master node, is the collaborative positioning status vector of the th slave node relative to the th master node, , is the noise matrix; S400: Update the observation vector: Among them, is the observation vector of the th slave node after update.

4. A step-by-step collaborative positioning method according to claim 1, characterized in that The method for determining whether the formation of the unmanned cluster has changed is: when the angles of the selected master nodes remain unchanged, it is considered that the formation has not changed; when the angles of the selected master nodes change, it is considered that the formation has changed.

5. A step-by-step cooperative positioning method according to claim 1, characterized in that When the time since the start of step S1 reaches an integer multiple of the inertial navigation protection 1 nautical mile time, restart S1.

6. A step-by-step collaborative positioning system for performing a step-by-step collaborative positioning method according to any one of claims 1 to 5, characterized in that It includes: Node selection module: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes. Calculate the geometric factor between the slave nodes and the master nodes, and select the slave node with the smallest geometric factor as the cooperative positioning target node; Calculation Navigation Information Module: Select a main node and connect it to the collaborative positioning target node to obtain the first edge, calculate the angles between the connections of the collaborative positioning target node and other main nodes and the first edge, select the number of main nodes according to the navigation accuracy to obtain the selected main nodes; solve the high-precision navigation information of the collaborative positioning target node, and use the number and position information of the selected main nodes to calculate the navigation information of the collaborative positioning target node according to the centralized collaborative positioning algorithm; First Judgment Module: Judge whether all slave nodes perform collaborative positioning. If all slave nodes perform collaborative positioning, the system positioning of the unmanned cluster is completed. If not all slave nodes perform collaborative positioning, execute the second judgment module; Second Judgment Module: Judge whether the formation of the unmanned cluster changes. If it changes, execute the first selection module; if it does not change, execute the second selection module; First Selection Module: Take the collaborative positioning target node as the main node and return to the selection node module; Second Selection Module: Take the collaborative positioning target node as the main node, re-select the slave node with the smallest geometric factor as the collaborative positioning target node, and return to the step of calculating navigation information module.

7. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein, When the processor executes the computer program, it implements the steps of a step-by-step collaborative positioning method as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of a step-by-step collaborative positioning method as described in any one of claims 1 to 5.

Images