Step-by-step cooperative positioning method, system, device and medium

Abstract

The invention relates to the technical field of cooperative positioning, and provides a step-by-step cooperative positioning method, system and device and a medium. The method comprises the following steps: randomly selecting a node as a main node in an unmanned cluster, taking other nodes as slave nodes, calculating geometric factors of the slave nodes and the main node, and selecting the slave node with the minimum geometric factor as a cooperative positioning target node; selecting a main node, cooperatively positioning included angles between a target node and other main nodes, selecting the number of the main nodes according to navigation precision, and obtaining the selected main node; and solving high-precision navigation information of the co-location target node, and calculating the navigation information of the co-location target node by using the number and position information of the selected main nodes according to a centralized co-location algorithm. Through step-by-step cooperative positioning, the demand of the cooperative positioning system for the communication capacity is effectively reduced, and meanwhile, the calculation complexity of cooperative positioning is greatly reduced.

Description

Technical Field

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

[0002] Due to the differences in the accuracy of navigation equipment installed on different unmanned boats or the lack of high-precision navigation information due to the limitation of satellite navigation, the high-precision navigation information of some unmanned boats and unmanned boats in the unmanned boat and unmanned boat cluster can generally be shared in the entire unmanned boat and unmanned boat cluster through centralized collaborative positioning, thereby improving the navigation information quality of unmanned boats and unmanned boats with low-precision navigation information. In the centralized collaborative positioning of unmanned boat and unmanned boat clusters, relative observation is required between unmanned boats and unmanned boats, which places high demands on communication resources and power energy. In addition, the centralized collaborative positioning of unmanned boat clusters requires a large amount of calculation, which places high demands on the computing power of unmanned boats and unmanned boats. Summary of the invention

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

[0004] The present invention provides a step-by-step collaborative positioning method, comprising: S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes, calculate the geometric factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; S2: Select a master node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other master nodes and the first edge, select the number of master nodes according to the navigation accuracy, and obtain the selected master node; S3: solving high-precision navigation information of the collaboratively positioned target node, using the number and position information of the selected master nodes, and calculating the navigation information of the collaboratively positioned target node according to a centralized collaborative positioning algorithm; S4: Determine 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 step S5; S5: Determine whether the formation of the unmanned group has changed, if so, execute step S6; if not, execute step S7; S6: taking the collaborative positioning target node as the main node, and returning to step S1; S7: The collaborative positioning target node is used as the master node, and the slave node with the smallest geometric factor is reselected as the collaborative positioning target node, and the process returns to step S2.

[0005] According to a step-by-step collaborative positioning method provided by the present invention, the calculation formula of the geometric factor is: in, For the The geometric factor of the slave nodes, is the slave node ordinal number, is the total number of slave nodes, , For all master nodes relative to the A matrix consisting of unit direction vectors from the nodes, Transpose the matrix.

[0006] According to a step-by-step collaborative positioning method provided by the present invention, the navigation accuracy includes high accuracy, medium accuracy and low accuracy: When the navigation accuracy is low, select an angle of The main node and an angle is The master node of is used as the selected master node; When the navigation accuracy is medium, select an angle of The main node of The main node and an angle is The master node of is used as the selected master node; When the navigation accuracy is high, select an angle of The main node of The main node of The main node and an angle is ’s master node as the selected master node.

[0007] According to a step-by-step collaborative positioning method provided by the present invention, the step of solving high-precision navigation information of the collaboratively positioned target node comprises: S10: Calculate the centralized co-location state transition matrix of the slave node: in, For the The slave node is relative to The node state transfer matrix of the master nodes, is the ordinal number of the selected master node, is the total number of selected master nodes, , For the The time-dependent constant of the master node, For the The centralized co-location state transfer matrix of slave nodes; S20: Calculate the noise driving matrix of the centralized co-localization process from the node: in, For the The slave node is relative to The process noise driving matrix of the master nodes, is the identity matrix, For the A noise driven matrix for the centralized co-location process of slave nodes; S30: Calculate the system noise matrix of the centralized collaborative positioning process from the node: in, For the The slave node is relative to The system noise sequence of the master nodes, is the system Gaussian white noise, is the system flicker noise, is the matrix transpose, For the Noise sequence of a centralized collaborative positioning system with 10 slave nodes; S40: Calculate the centralized collaborative positioning state vector according to the system state equation: in, For the The slave nodes centrally coordinate the location state vector, for The first derivative of .

[0008] According to a step-by-step collaborative positioning method provided by the present invention, the centralized collaborative positioning algorithm steps include: S100: Calculate the observation vector from the node: in, For the The concentrated observation vector of slave nodes, No. The slave node is relative to The observation vector of the master node; S200: Calculate the slave node observation matrix: in, For the The concentrated observation vector of slave nodes, No. The slave node is relative to The observation vector of the master node; S300: Recalculate the position observation vector of the slave node according to the observation vector of the slave node and the slave node observation matrix: in, is the updated position observation vector, For the The slave node is relative to The co-location state vector of the master nodes, For the The slave node is relative to The observation vector of the master node, For the The slave node is relative to The co-location state vector of the master nodes, , is the noise matrix; S400: Update observation vector: in, After the update The observation vector of each slave node.

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

[0010] 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 guarantee time of 1 nautical mile, S1 is restarted.

[0011] The present invention also provides a step-by-step collaborative positioning system, comprising: 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 factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; Calculate navigation information module: select a main node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other main nodes and the first edge, select the number of main nodes according to the navigation accuracy, and obtain the selected main node; solve the high-precision navigation information of the co-located target node, and use the number and position information of the selected main nodes to calculate the navigation information of the co-located target node according to the centralized co-located algorithm; The first judgment module: judges whether all slave nodes are co-located. If all slave nodes are co-located, the system positioning of the unmanned cluster is completed. If not all slave nodes are co-located, the second judgment module is executed; The second judgment module is used to judge whether the formation of the unmanned cluster has changed, and if so, the first selection module is executed; if not, the second selection module is executed; The first selection module: taking the co-location target node as the main node and returning to the node selection module; The second selection module: taking the co-location target node as the master node, reselecting the slave node with the smallest geometric factor as the co-location target node, and returning to the step of calculating the navigation information module.

[0012] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of a step-by-step collaborative positioning method as described in any one of the above are implemented.

[0013] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of any of the above-described step-by-step collaborative positioning methods.

[0014] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects: The present invention provides a step-by-step collaborative positioning method, system, device and medium, which, through step-by-step collaborative positioning, only selects one slave node to participate in the collaborative positioning each time, and selects the master node to participate in the collaborative positioning at the same time, thereby effectively reducing the demand of the collaborative positioning system for communication capacity, and greatly reducing the computational complexity of the collaborative positioning, and can provide navigation information guarantee that meets the requirements of the operation task for the unmanned boat cluster, thereby greatly improving the availability of the collaborative positioning.

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

[0016] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0017] Figure 1 It is a flow chart of a step-by-step collaborative positioning method provided by the present invention.

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

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

[0020] Reference numerals: 810, processor; 820, communication interface; 830, memory; 840, communication bus. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical scheme and advantages of the present invention clearer, the technical scheme in the present invention will be clearly and completely described below. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in the field without creative work are 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.

[0022] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the embodiment 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 may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0023] Combine the following Figures 1 to 3 The present invention is described.

[0024] Example Assume that there are several unmanned boats with high navigation information accuracy in the step-by-step collaborative positioning system, which are called master nodes, and several unmanned boats with low navigation information accuracy, which are called slave nodes. For the convenience of derivation, it is assumed that the navigation accuracy of the slave nodes is reduced due to interference with satellite navigation. Generally speaking, the low-precision inertial navigation indicators configured for unmanned boats are about 1 nautical mile per 2 to 4 hours. The navigation accuracy indicator of 1 nautical mile can meet most operational requirements. Therefore, it is only necessary to use collaborative positioning on the slave nodes within 2 to 4 hours to correct the inertial navigation system errors.

[0025] like Figure 1 As shown, Figure 1 A schematic flow chart of a step-by-step collaborative positioning method provided by the present invention. The method comprises 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 factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; S2: Select a master node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other master nodes and the first edge, select the number of master nodes according to the navigation accuracy, and obtain the selected master node; S3: solving high-precision navigation information of the collaboratively positioned target node, using the number and position information of the selected master nodes, and calculating the navigation information of the collaboratively positioned target node according to a centralized collaborative positioning algorithm; S4: Determine 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 step S5; S5: Determine whether the formation of the unmanned group has changed, if so, execute step S6; if not, execute step S7; S6: taking the collaborative positioning target node as the main node, and returning to step S1; S7: The collaborative positioning target node is used as the master node, and the slave node with the smallest geometric factor is reselected as the collaborative positioning target node, and the process returns to step S2.

[0026] Specifically, the calculation formula of the geometric factor is: in, For the The geometric factor of the slave nodes, is the slave node ordinal number, is the total number of slave nodes, , For all master nodes relative to the A matrix consisting of unit direction vectors from the nodes, Transpose the matrix.

[0027] 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 precision factor. When the position accuracy of the master node and the relative observation accuracy between nodes are constant, the smaller the geometric precision factor, the higher the collaborative positioning accuracy. The slave node corresponding to the smallest value is used as the first slave node to participate in the co-location.

[0028] Specifically, the navigation accuracy includes high accuracy, medium accuracy and low accuracy: When the navigation accuracy is low, select an angle of The main node and an angle is The master node of is used as the selected master node; When the navigation accuracy is medium, select an angle of The main node of The main node and an angle is The master node of is used as the selected master node; When the navigation accuracy is high, select an angle of The main node of The main node of The main node and an angle is ’s master node as the selected master node.

[0029] Specifically, the step of solving the high-precision navigation information of the collaboratively positioned target node includes: S10: Calculate the centralized co-location state transition matrix of the slave node: in, For the The slave node is relative to The node state transfer matrix of the master nodes, is the ordinal number of the selected master node, is the total number of selected master nodes, , For the The time-dependent constant of the master node, For the The centralized co-location state transfer matrix of slave nodes; S20: Calculate the noise driving matrix of the centralized co-localization process from the node: in, For the The slave node is relative to The process noise driving matrix of the master nodes, is the identity matrix, For the A noise driven matrix for the centralized co-location process of slave nodes; S30: Calculate the system noise matrix of the centralized collaborative positioning process from the node: in, For the The slave node is relative to The system noise sequence of the master nodes, is the system Gaussian white noise, is the system flicker noise, is the matrix transpose, No. Noise sequence of a centralized collaborative positioning system with 10 slave nodes; S40: Calculate the centralized collaborative positioning state vector according to the system state equation: in, For the The slave nodes centrally coordinate the location state vector, for The first derivative of .

[0030] Specifically, the centralized collaborative positioning algorithm steps include: S100: Calculate the observation vector from the node: in, For the The concentrated observation vector of slave nodes, No. The slave node is relative to The observation vector of the master node; S200: Calculate the slave node observation matrix: in, For the The concentrated observation vector of slave nodes, No. The slave node is relative to The observation vector of the master node; S300: Recalculate the position observation vector of the slave node according to the observation vector of the slave node and the slave node observation matrix: in, is the updated position observation vector, For the The slave node is relative to The co-location state vector of the master nodes, For the The slave node is relative to The observation vector of the master node, For the The slave node is relative to The co-location state vector of the master nodes, , is the noise matrix; S400: Update observation vector: in, After the update The observation vector of each slave node.

[0031] The relative observation added to the observation vector can add constraints to the navigation state of each unmanned boat. The increase of effective observation information will inevitably reduce the state estimation error and the covariance of the state estimation error. From the centralized collaborative positioning calculation process, it can be seen that many matrix operations are involved. The step-by-step collaborative positioning converts the original collaborative positioning matrix into a matrix size no larger than , which greatly reduces the amount of calculation.

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

[0033] Specifically, when the time from the start of step S1 reaches an integral multiple of the inertial navigation guarantee time of 1 nautical mile, S1 is restarted.

[0034] Aiming at the situation that low-precision navigation unmanned boats and high-precision navigation information unmanned boats co-exist in the collaborative positioning system of an unmanned boat cluster, the present invention proposes a design scheme of a step-by-step collaborative positioning system. The scheme adopts step-by-step collaborative positioning, that is, only one slave node is selected to participate in the collaborative positioning each time, and the master node participating in the collaborative positioning is selected at the same time, thereby effectively reducing the demand of the collaborative positioning system for communication capacity and greatly reducing the computational complexity of the collaborative positioning. It can provide the unmanned boat cluster with navigation information guarantee that meets the requirements of the operation task, and greatly improves the availability of the collaborative positioning.

[0035] like Figure 2 As shown, Figure 2 A structural block diagram of a step-by-step collaborative positioning system provided by the present invention, the system comprising: 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 factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; Calculate navigation information module: select a main node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other main nodes and the first edge, select the number of main nodes according to the navigation accuracy, and obtain the selected main node; solve the high-precision navigation information of the co-located target node, and use the number and position information of the selected main nodes to calculate the navigation information of the co-located target node according to the centralized co-located algorithm; The first judgment module: judges whether all slave nodes are co-located. If all slave nodes are co-located, the system positioning of the unmanned cluster is completed. If not all slave nodes are co-located, the second judgment module is executed; The second judgment module is used to judge whether the formation of the unmanned cluster has changed, and if so, the first selection module is executed; if not, the second selection module is executed; The first selection module: taking the co-location target node as the main node and returning to the node selection module; The second selection module: taking the co-location target node as the master node, reselecting the slave node with the smallest geometric factor as the co-location target node, and returning to the step of calculating the navigation information module.

[0036] A step-by-step collaborative positioning device provided by the present invention is described below. The step-by-step collaborative positioning device described below and the step-by-step collaborative positioning method described above can refer to each other.

[0037] Figure 3 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 3 As shown, the electronic device may include: a processor 810, a communication interface 820, a memory 830 and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other through the communication bus 840. The processor 810 may call the logic instructions in the memory 830 to execute a step-by-step collaborative positioning method, which includes: S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes, calculate the geometric factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; S2: Select a master node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other master nodes and the first edge, select the number of master nodes according to the navigation accuracy, and obtain the selected master node; S3: solving high-precision navigation information of the collaboratively positioned target node, using the number and position information of the selected master nodes, and calculating the navigation information of the collaboratively positioned target node according to a centralized collaborative positioning algorithm; S4: Determine 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 step S5; S5: Determine whether the formation of the unmanned group has changed, if so, execute step S6; if not, execute step S7; S6: taking the collaborative positioning target node as the main node, and returning to step S1; S7: The collaborative positioning target node is used as the master node, and the slave node with the smallest geometric factor is reselected as the collaborative positioning target node, and the process returns to step S2.

[0038] In addition, the logic instructions in the above-mentioned memory 830 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0039] On the other hand, the present invention further provides a computer program product, the computer program product comprising a computer program stored on a non-transitory computer-readable storage medium, the computer program comprising program instructions, when the program instructions are executed by a computer, the computer can execute a step-by-step collaborative positioning method provided by the above methods, the method comprising: S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes, calculate the geometric factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; S2: Select a master node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other master nodes and the first edge, select the number of master nodes according to the navigation accuracy, and obtain the selected master node; S3: solving high-precision navigation information of the collaboratively positioned target node, using the number and position information of the selected master nodes, and calculating the navigation information of the collaboratively positioned target node according to a centralized collaborative positioning algorithm; S4: Determine 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 step S5; S5: Determine whether the formation of the unmanned group has changed, if so, execute step S6; if not, execute step S7; S6: taking the collaborative positioning target node as the main node, and returning to step S1; S7: The collaborative positioning target node is used as the master node, and the slave node with the smallest geometric factor is reselected as the collaborative positioning target node, and the process returns to step S2.

[0040] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0041] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for 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.

[0042] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

[0043] It should be noted that the embodiments of the present disclosure may be implemented by hardware, software, or a combination of software and hardware. The hardware portion may be implemented using dedicated logic: the software portion may 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 will appreciate that the above-described apparatus and methods may be implemented using computer executable instructions and / or contained in a processor control code, such as a programmable memory or a data carrier such as an optical or electronic signal carrier providing such code.

[0044] In addition, although the operation of the method of the present disclosure is described in a particular order in the accompanying drawings, this does not require or imply that these operations must be performed in this particular order, or that all the operations shown must be performed to achieve the desired result. On the contrary, the steps depicted in the flow chart can change 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 a device described above can be further divided into being embodied by multiple devices.

[0045] 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: The following steps are involved: S1: Select any number of nodes in the unmanned cluster as master nodes, and the remaining nodes as slave nodes, calculate the geometric factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; S2: Select a master node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other master nodes and the first edge, select the number of master nodes according to the navigation accuracy, and obtain the selected master node; S3: solving high-precision navigation information of the collaboratively positioned target node, using the number and position information of the selected master nodes, and calculating the navigation information of the collaboratively positioned target node according to a centralized collaborative positioning algorithm; S4: Determine 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 step S5; S5: Determine whether the formation of the unmanned group has changed, if so, execute step S6; if not, execute step S7; S6: taking the co-located target node as the main node, and returning to step S1; S7: The collaborative positioning target node is used as the master node, and the slave node with the smallest geometric factor is reselected as the collaborative positioning target node, and the process returns to step S2.

2. A step-by-step collaborative positioning method according to claim 1, characterized in that: The calculation formula of the geometric factor is: in, For the The geometric factor of the slave nodes, is the slave node ordinal number, is the total number of slave nodes, , For all master nodes relative to the A matrix consisting of unit direction vectors from the nodes, Transpose the matrix.

3. A step-by-step collaborative positioning method according to claim 1, characterized in that: The navigation accuracy includes high accuracy, medium accuracy and low accuracy: When the navigation accuracy is low, select an angle of The main node and an angle is The master node of is used as the selected master node; When the navigation accuracy is medium, select an angle of The main node of The main node and an angle is The master node of is used as the selected master node; When the navigation accuracy is high, select an angle of The main node of The main node of The main node and an angle is ’s master node as the selected master node.

4. The step-by-step collaborative positioning method according to claim 1, characterized in that: The step of solving the high-precision navigation information of the collaboratively positioned target node comprises: S10: Calculate the centralized co-location state transition matrix of the slave node: in, For the The slave node is relative to The node state transfer matrix of the master nodes, is the ordinal number of the selected master node, is the total number of selected master nodes, , For the The time-dependent constant of the master node, For the The centralized co-location state transfer matrix of slave nodes; S20: Calculate the noise driving matrix of the centralized collaborative positioning process from the node: in, For the The slave node is relative to The process noise driving matrix of the master nodes, is the identity matrix, For the A noise driven matrix for the centralized co-location process of slave nodes; S30: Calculate the system noise matrix of the centralized collaborative positioning process from the node: in, For the The slave node is relative to The system noise sequence of the master nodes, is the system Gaussian white noise, is the system flicker noise, is the matrix transpose, For the Noise sequence of a centralized collaborative positioning system with 10 slave nodes; S40: Calculate the centralized collaborative positioning state vector according to the system state equation: in, For the The slave nodes centrally coordinate the location state vector, for The first derivative of .

5. A step-by-step collaborative positioning method according to claim 4, characterized in that: The centralized collaborative positioning algorithm steps include: S100: Calculate the observation vector from the node: in, For the The concentrated observation vector of slave nodes, No. The slave node is relative to The observation vector of the master node; S200: Calculate the slave node observation matrix: in, For the The concentrated observation vector of slave nodes, No. The slave node is relative to The observation vector of the master node; S300: Recalculate the position observation vector of the slave node according to the observation vector of the slave node and the slave node observation matrix: in, is the updated position observation vector, For the The slave node is relative to The co-location state vector of the master nodes, For the The slave node is relative to The observation vector of the master node, For the The slave node is relative to The co-location state vector of the master nodes, , is the noise matrix; S400: Update observation vector: in, After the update The observation vector of each slave node.

6. The step-by-step collaborative positioning method according to claim 1, characterized in that: The method for judging whether the formation of the unmanned cluster has changed is: when the angle of the selected master node remains unchanged, it is considered that the formation has not changed; when the angle of the selected master node changes, it is considered that the formation has changed.

7. A step-by-step collaborative positioning method according to any one of claims 1, characterized in that: When the time from the start of step S1 reaches an integral multiple of the inertial navigation guarantee time of 1 nautical mile, restart S1.

8. A step-by-step collaborative positioning system for executing a step-by-step collaborative positioning method as claimed in any one of claims 1 to 7, characterized in that: include: 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 factors of the slave nodes and the master node, and select the slave node with the smallest geometric factor as the collaborative positioning target node; Calculate navigation information module: select a main node and connect it with the co-located target node to obtain a first edge, calculate the angle between the line connecting the co-located target node and other main nodes and the first edge, select the number of main nodes according to the navigation accuracy, and obtain the selected main node; solve the high-precision navigation information of the co-located target node, and use the number and position information of the selected main nodes to calculate the navigation information of the co-located target node according to the centralized co-located algorithm; The first judgment module: judges whether all slave nodes are co-located. If all slave nodes are co-located, the system positioning of the unmanned cluster is completed. If not all slave nodes are co-located, the second judgment module is executed; The second judgment module is used to judge whether the formation of the unmanned cluster has changed, and if so, the first selection module is executed; if not, the second selection module is executed; The first selection module: taking the co-location target node as the main node and returning to the node selection module; The second selection module: taking the co-location target node as the master node, reselecting the slave node with the smallest geometric factor as the co-location target node, and returning to the step of calculating the navigation information module.

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

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of a step-by-step collaborative positioning method as claimed in any one of claims 1 to 7 are implemented.

Images