Unmanned Swarm Formation Positioning Method in GNSS Denied Environment

Abstract

An embodiment of the present invention discloses a method for positioning an unmanned swarm formation in a GNSS-denied environment. The method comprises obtaining first position information based on measurement information from each formation member, obtaining distance measurement results and initial positioning information between each formation member via a data link, obtaining estimated positioning information for each formation member based on the first position information and distance measurement results, obtaining a position quantity to be corrected based on the estimated positioning information and initial positioning information, and obtaining target positioning information for each formation member based on the position quantity to be corrected. Thus, by combining multi-source data and assisting a primary carrier, the collaborative positioning accuracy of an unmanned swarm formation can be improved, ensuring the operational safety of the formation.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to an unmanned cluster formation positioning method in a GNSS-denied environment. Background Art

[0002] Unmanned swarm formations have demonstrated a significant impact in industries such as industry, agriculture, and scientific research due to their multiple advantages, including high efficiency, robustness, adaptability, economy, concealment, and flexible expansion. Among them, unmanned swarm collaborative positioning technology is the key to achieving efficient collaboration among multiple unmanned systems (such as drones, unmanned underwater vehicles, etc.) within a group. This technology determines their respective and relative positions through mutual collaboration, which is crucial for improving the operational capabilities and efficiency of unmanned systems in complex environments, and is particularly suitable for scenarios with limited navigation infrastructure. However, the current mainstream unmanned equipment formation positioning technology relies on a combined navigation method of inertial navigation systems and global navigation satellite systems. When the global navigation satellite system signal is blocked or interfered with, the combined navigation fails, resulting in a failure in the relative positioning of the unmanned equipment, affecting the safe operation of the formation. Summary of the Invention

[0003] In view of this, an embodiment of the present invention provides an unmanned cluster formation positioning method in a GNSS-denied environment, which can improve the collaborative positioning accuracy of the unmanned cluster formation and ensure the operational safety of the formation.

[0004] In a first aspect, an embodiment of the present invention provides an unmanned cluster formation positioning method in a GNSS-denied environment, the method comprising:

[0005] Acquire first position information based on measurement information of each formation member, where the first position information is position information of each formation member in a rectangular coordinate system with the main carrier as the origin;

[0006] Obtain distance measurement results and initial positioning information between each formation member through data link;

[0007] Obtaining estimated positioning information of each formation member based on the first position information and the distance measurement result;

[0008] Acquire a position quantity to be corrected based on the positioning estimation information and the initial positioning information;

[0009] The target positioning information of each formation member is obtained according to the position quantity to be corrected.

[0010] In some embodiments, obtaining the first position information according to the measurement information of each formation member includes:

[0011] Acquiring measurement information of each formation member through a data link device, wherein the measurement information is the spherical coordinates of each formation member in a spherical coordinate system with the main carrier as the origin;

[0012] Acquiring first position information according to measurement information of each formation member;

[0013] The first position information is obtained by calculating using the following formula:

[0014]

[0015] Among them, P i_detect is the coordinate of formation member i in the rectangular coordinate system with the main carrier as the origin, (φ i ,ψ i ,r i ) is the measurement information of formation member i, φ i is the azimuth of formation member i in the spherical coordinate system, ψ i is the pitch angle of formation member i in the spherical coordinate system, r i is the distance between formation member i and the main carrier, -r i cosψ i sinφ i is the x-axis coordinate of formation member i in the rectangular coordinate system of the main carrier, r i cosψ i cosφ i is the y-axis coordinate of formation member i in the rectangular coordinate system of the main carrier, r i sinψ i is the z-axis coordinate of formation member i in the rectangular coordinate system of the main carrier body.

[0016] In some embodiments, obtaining estimated positioning information of each formation member based on the first position information and the distance measurement result includes:

[0017] Acquire the initial position information of each formation member through the data link, wherein the initial position information is the position information of each formation member;

[0018] Acquire a distance residual according to the initial position information and the distance measurement result;

[0019] Acquire a position residual amount according to the initial position information and the first position information;

[0020] Determine the second position information of each formation member according to the distance residual and the position residual;

[0021] The estimated positioning information of each formation member is obtained based on the second position information.

[0022] In some embodiments, the distance residual is calculated using the following formula:

[0023]

[0024] in, is the difference between the actual measured distance and the initial distance between formation members i and j, P i is the initial position information of formation member i, P j is the initial position information of formation member j, |P i -P j | is the initial distance obtained based on the initial position information between formation members i and j, l ij is the distance measurement result between formation members i and j, i = 1, ..., n-1, j = i + 1, ...n;

[0025] The position residual is calculated by the following formula:

[0026]

[0027] in, is the difference between the actual measured position and the initial position of formation member i in the rectangular coordinate system of the main carrier body, P i is the initial position information of formation member i, P i_detect is the second position information of formation member i.

[0028] In some embodiments, determining the second position information of each formation member according to the distance residual and the position residual includes:

[0029] Determine a set of position information of all formation members according to the distance residual and the position residual;

[0030] determining second position information of each formation member according to the position information set;

[0031] The location information set is obtained by calculating the following formula:

[0032]

[0033] Among them, P * is the position information set of all formation members in the main carrier body coordinate system, w ij and w k are weight coefficients determined by the measurement noise, and argmin is a mathematical formula used to find the parameter value that minimizes the formula. is the weighted square of the distance residuals between formation members, is the weighted square of the position residual of each formation member, i = 1,…,n-1; j = i+1,…n, k = 1,…,n.

[0034] In some embodiments, obtaining estimated positioning information of each formation member based on the second position information includes:

[0035] Obtaining a rotation matrix, wherein the rotation matrix is ​​used to convert the main carrier body rectangular coordinate system to the northeast celestial coordinate system;

[0036] Acquire third position information according to the second position information and the rotation matrix;

[0037] Obtain the main carrier's own navigation information;

[0038] Obtaining estimated positioning information of each formation member based on the third position information and the self-navigation information;

[0039] The third position information is obtained by calculating using the following formula:

[0040]

[0041] in, is the third position information of each formation member, is the second position information of each formation member, It is the rotation matrix from the main carrier body rectangular coordinate system to the northeast celestial coordinate system.

[0042] In some embodiments, the positioning estimation information is obtained by calculating the following formula:

[0043]

[0044] Among them, (L i ,λ i , H i ) is the positioning estimation information, the L i is the latitude of formation member i, λ i is the longitude of formation member i, H i is the elevation of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the elevation of the main carrier, p N is the north coordinate of formation member i in the northeast celestial coordinate system, p E is the east coordinate of formation member i in the northeast celestial coordinate system, p U is the coordinate of the formation members in the northeast celestial coordinate system, R mh is the meridian radius at the latitude of formation member i, R nh is the radius of the circle at the latitude of formation member i;

[0045] The meridian radius is calculated using the following formula:

[0046] R nh =r ec (1.0+f ec sin 2 L0)+H0

[0047] The meridian radius is calculated using the following formula:

[0048] R mh =r ec (1.0-2f ec +3f ec sin 2 L0)+H0

[0049] Among them, r ec is the major radius of the Earth, f ec is the oblateness of the earth, H0 is the altitude of the main carrier, and L0 is the latitude of the main carrier.

[0050] In some embodiments, the position value to be corrected is calculated using the following formula:

[0051] ΔL i =L i -L i_nav0

[0052] Δλ i =λ i -λ i_nav0

[0053] ΔH i =H i -H i_nav0

[0054] Where, ΔL i is the latitude of formation member i to be corrected, Δλ i is the longitude position of formation member i to be corrected, ΔH i is the height of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i in the navigation system, λ i_nav0 is the longitude value provided by formation member i under the navigation system, H i_nav0 is the elevation value provided by formation member i under the navigation system, i = 1,…,n.

[0055] In some embodiments, the target positioning information is obtained by calculating the following formula:

[0056] L i_t =L i_t ′+ΔL i

[0057] λ i_t =λ i_t ′+Δλ i

[0058] H i_t =H i_t ′+ΔH i

[0059] Among them, (L i_t ,λ i_t , H i_t ) is the target positioning information, L i_t ′ is the latitude value of formation member i at a certain time t, λ i_t ′ is the longitude value of formation member i at a certain time t, H i_t ′ is the elevation value of formation member i at a certain time t, and formation member i is (L i_t ,λ i_t , H i_t ) as its position at time t for subsequent navigation calculations.

[0060] In a second aspect, an embodiment of the present invention provides an electronic device comprising: a processor, a storage medium and a bus, wherein the storage medium stores machine-readable instructions executable by the processor. When the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the machine-readable instructions to implement the method described in the first aspect.

[0061] The technical solution of the embodiments of the present invention obtains first position information based on the measurement information of each formation member, obtains distance measurement results and initial positioning information between each formation member via a data link, obtains estimated positioning information for each formation member based on the first position information and distance measurement results, obtains a position quantity to be corrected based on the estimated positioning information and initial positioning information, and obtains target positioning information for each formation member based on the position quantity to be corrected. Thus, by combining multi-source data and assisting the main carrier, the collaborative positioning accuracy of unmanned swarm formations can be improved, ensuring the operational safety of the formation. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] The above and other objects, features and advantages of the present invention will become more apparent through the following description of the embodiments of the present invention with reference to the accompanying drawings, in which:

[0063] Figure 1 is a schematic diagram of an unmanned cluster formation positioning system in a GNSS-denied environment according to an embodiment of the present invention;

[0064] Figure 2 is a flowchart of an unmanned cluster formation positioning method in a GNSS-denied environment according to an embodiment of the present invention;

[0065] Figure 3 is a schematic diagram of formation member measurement information according to an embodiment of the present invention;

[0066] Figure 4 is a flow chart of obtaining positioning estimation information of each formation member according to an embodiment of the present invention;

[0067] Figure 5 is a flow chart of correcting positioning information according to an embodiment of the present invention;

[0068] Figure 6 2 is a schematic diagram of an unmanned cluster formation positioning device in a GNSS-denied environment according to an embodiment of the present invention;

[0069] Figure 7 is a schematic diagram of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0070] The present application is described below based on the following embodiments, but the present application is not limited to these embodiments. In the detailed description of the present application below, certain specific details are described in detail. Those skilled in the art can fully understand the present application without the description of these details. To avoid obscuring the essence of the present application, well-known methods, processes, procedures, components, and circuits are not described in detail.

[0071] Furthermore, persons of ordinary skill in the art will appreciate that the figures provided herein are for illustration purposes only and are not necessarily drawn to scale.

[0072] Unless the context clearly requires otherwise, words like “include”, “comprising” and the like throughout this application should be interpreted as including rather than exclusive or exhaustive; that is, as meaning “including but not limited to”.

[0073] In the description of this application, it should be understood that the terms "first", "second", etc. are used for descriptive purposes only and should not be understood to indicate or imply relative importance. In addition, in the description of this application, unless otherwise specified, "plurality" means two or more.

[0074] Unmanned swarm collaborative positioning technology refers to the technology that allows multiple unmanned systems in a group to collaborate to determine their respective positions and relative positions to each other. The unmanned system refers to an intelligent joint system composed of a certain number of similar or heterogeneous unmanned equipment, control systems, and human-machine interfaces. Specifically, the unmanned equipment includes drones, unmanned vehicles, unmanned ships, and unmanned underwater vehicles, etc., which are not limited in the embodiments of the present invention. The research and application of unmanned swarm collaborative positioning technology is an evolving field that is of great significance for improving the operational capabilities and efficiency of unmanned systems in complex environments. This technology is particularly suitable for situations where navigation infrastructure is limited.

[0075] Figure 1 Schematic diagram of an unmanned cluster formation positioning system in a GNSS-denied environment according to an embodiment of the present invention. Figure 1In the illustrated embodiment, taking aircraft as formation members, the unmanned swarm formation positioning system for a GNSS-denied environment includes a first swarm member 11, a second swarm member 12, a third swarm member 13, and a main carrier 14. These first, second, and third swarm members 11, 12, 13 communicate with the main carrier 14 via a data link. Each of these swarm members 11, 12, 13, and the main carrier 14 is equipped with data link equipment and a navigation system. The data link equipment in the first, second, and third swarm members 11, 12, 13 is used to measure the relative distances between the swarm members, while the data link equipment in the main carrier 14 is used to measure the coordinates of each swarm member in the spherical coordinate system of the main carrier. The main carrier is an electronic device equipped with high-precision navigation equipment and a data link device capable of both angle and distance measurement. The formation members refer to multiple unmanned systems in the unmanned swarm formation, such as drones and unmanned underwater vehicles, and are not specified in this embodiment of the present invention. These formation members have their own navigation equipment and data link equipment, and can cooperate with each other in the group to determine their own positions and their relative positions to each other.

[0076] Among them, when the main carrier 14 is located at a fixed geographical location, the main carrier 14 can obtain its own high-precision navigation information at the geographical location. When the main carrier 14 is in a moving state, the main carrier 14 can obtain its own navigation information in real time, and analyze the positioning information of each formation for the mobile main carrier based on the real-time navigation information.

[0077] Specifically, the main carrier 14 obtains the measurement information of each formation member through its own data link equipment and obtains the first position information through calculation. At the same time, the main carrier 14 obtains the distance measurement results between each formation member and the initial positioning information of each formation member through its own data link equipment through the data link. The main carrier 14 obtains the estimated positioning information of each formation member based on the first position information and the distance measurement results, and obtains the position quantity to be corrected based on the estimated positioning information and the initial positioning information. The main carrier 14 sends the position quantity to be corrected to the first formation member 11, the second formation member 12, and the third formation member 13 via the data link for correction of the positioning information of each formation member.

[0078] Among them, the first formation member 11, the second formation member 12 and the third formation member 13 can be realized by other terminal devices such as unmanned aerial vehicles or unmanned underwater vehicles.

[0079] The main carrier 14 is realized by an electronic device integrating a high-precision navigation system and a data link communication module.

[0080] Figure 2 FIG. 1 is a flow chart of an unmanned cluster formation positioning method in a GNSS-denied environment according to an embodiment of the present invention. Figure 2 The specific implementation of the unmanned cluster formation positioning method in a GNSS-denied environment shown includes the following steps:

[0081] Step S100: Acquire first position information according to measurement information of each formation member.

[0082] Specifically, the main carrier measures the measurement information of each formation member through its own data link equipment. The measurement information is the spherical coordinates of each formation member in a spherical coordinate system with the main carrier as the origin.

[0083] Figure 3 Schematic diagram of formation member measurement information according to an embodiment of the present invention. Figure 3 As described above, the origin O of the main carrier body spherical coordinate system is located at the center of the main carrier, the Oy axis points to the right side of the main carrier, the Oy axis is perpendicular to the Ox axis and points to the head of the main carrier, and the Oz axis, Ox axis and Oy axis form a right-handed spherical coordinate system. In this embodiment, taking formation member i as an example, the spherical coordinates of formation member i in the spherical coordinate system with the main carrier as the origin are (φ i ,ψ i ,r i ),φ i is the azimuth of formation member i in the spherical coordinate system, ψ i is the pitch angle of formation member i in the spherical coordinate system, r i is the distance between formation member i and the main carrier.

[0084] The main carrier obtains first position information based on the measurement information of each formation member. The first position information is the position information of each formation member in a rectangular coordinate system with the main carrier as the origin. The first position information is calculated using the following formula:

[0085]

[0086] Among them, P i_detect is the coordinate of formation member i in the rectangular coordinate system with the main carrier as the origin, (φ i ,ψ i ,r i ) is the measurement information of formation member i, φ i is the azimuth of formation member i in the spherical coordinate system, ψ i is the pitch angle of formation member i in the spherical coordinate system, r i is the distance between formation member i and the main carrier, -r i cosψ i sinφ i is the x-axis coordinate of formation member i in the rectangular coordinate system of the main carrier, ri cosψ i cosφ i is the y-axis coordinate of formation member i in the rectangular coordinate system of the main carrier, r i sinψ i is the z-axis coordinate of formation member i in the rectangular coordinate system of the main carrier body.

[0087] Step S200: Obtain distance measurement results and initial positioning information between each formation member through the data link.

[0088] Specifically, the main carrier obtains the distance measurement results and initial positioning information between each formation member through the data link. The data link refers to a link for intercommunication of data, which allows information to be transmitted between the main carrier and the formation crew. The distance measurement results are obtained through the data link equipment of each formation member. The data link equipment of the formation member has wireless communication and ranging functions and can directly obtain the distance measurement results between each formation member. The initial positioning information is obtained through the navigation system of each formation member. The initial positioning information includes the longitude, latitude and altitude of the formation member itself. Each formation member sends the distance measurement results and initial positioning information to the main carrier through the data link.

[0089] Step S300: Obtaining estimated positioning information of each formation member based on the first position information and the distance measurement result.

[0090] Specifically, in a GNSS (Global Navigation Satellite System) denied environment, the main carrier obtains the initial position information of the formation members through the data link, and based on the initial position information, the distance measurement result and the first position information, uses the optimization algorithm to iteratively calculate the second position information of each formation member. Then, the main carrier uses INS (Inertial Navigation System), radio, vision and other technical means to obtain the rotation matrix from the main carrier's main body rectangular coordinate system to the northeast celestial coordinate system, and calculates the third position information of the formation members in the northeast celestial coordinate system based on the second position information. Finally, based on the third position information and the main carrier's own navigation information, the positioning estimation information of each formation member is calculated.

[0091] Figure 4 FIG. 1 is a flow chart of an embodiment of the present invention for obtaining the location estimation information of each formation member. Figure 4 As shown, the specific implementation includes the following steps:

[0092] Step S310: Obtain the initial position information of each formation member through the data link.

[0093] Specifically, each formation member transmits the initial positioning information to the main carrier via a data link. This initial positioning information is obtained by each formation member's own navigation system. This navigation system typically includes high-precision navigation equipment such as a Global Navigation Satellite System receiver and an inertial navigation system. In this embodiment, each formation member continuously obtains geographic navigation results through its own navigation system and transmits this information to the main carrier via a data link for centralized processing. The main carrier can use this information to track and locate the formation members' positions.

[0094] Step S320: Obtain a distance residual according to the initial position information and the distance measurement result.

[0095] Specifically, the primary carrier obtains a distance residual according to the initial position information and the distance measurement result, and the distance residual is calculated by the following formula:

[0096]

[0097] in, is the difference between the actual measured distance and the initial distance between formation members i and j, P i is the initial position information of formation member i, P j is the initial position information of formation member j, |P i -P j | is the initial distance obtained based on the initial position information between formation members i and j, l ij is the distance measurement result between formation members i and j, i = 1, ..., n-1, j = i + 1, ...n;

[0098] Step S330: Obtain a position residual according to the initial position information and the first position information.

[0099] Specifically, the main carrier obtains a position residual amount according to the initial position information and the first position information, and the position residual amount is calculated and obtained by the following formula:

[0100]

[0101] in, is the difference between the actual measured position and the initial position of formation member i in the rectangular coordinate system of the main carrier body, P i is the initial position information of formation member i, P i_detect is the second position information of formation member i.

[0102] Step S340: Determine a set of position information of all formation members according to the distance residual and the position residual.

[0103] Specifically, the main carrier determines the position information set of all formation members according to the distance residual and the position residual. The position information set is obtained by calculating the following formula:

[0104]

[0105] Among them, P * is the position information set of all formation members in the main carrier body coordinate system, w ij and w k are weight coefficients determined by the measurement noise, and argmin is a mathematical formula used to find the parameter value that minimizes the formula. is the weighted square of the distance residuals between formation members, is the weighted square of the position residual of each formation member, i = 1,…,n-1; j = i+1,…n, k = 1,…,n.

[0106] In this embodiment, taking five formation members as an example, the formation members include formation member 1, formation member 2, formation member 3, formation member 4, and formation member 5. The initial position information corresponding to formation member 1 is P1, the initial position information corresponding to formation member 2 is P2, the initial position information corresponding to formation member 3 is P3, the initial position information corresponding to formation member 4 is P4, and the initial position information corresponding to formation member 5 is P5.

[0107] Therefore, according to P1-P5, the above formula It can be calculated That is, we can get the corresponding values ​​of (i=1, j=2), (i=2, j=3), (i=3, j=4), and (i=4, j=5).

[0108] At the same time, according to the above formula Can get arrive The value of .

[0109] Furthermore, the calculation formula for the above position information set can be used to obtain the four position information corresponding to k = 1, (i = 1, j = 2), (i = 2, j = 3), (i = 3, j = 4), and (i = 4, j = 5), and the minimum value of the four position information can be selected. Similarly, the minimum value of the position information can be obtained when k = 2, 3, 4, and 5. In other words, the minimum values ​​of the five position information can be obtained using the above formula, and these five minimum values ​​constitute the position information set.

[0110] Step S350: Determine the second position information of each formation member according to the position information set.

[0111] Specifically, the main carrier may determine the second position information corresponding to each formation member based on the position information set, where the position information set includes the minimum position information corresponding to each formation member.

[0112] Step S360: Obtain estimated positioning information of each formation member based on the second position information.

[0113] Specifically, in a GNSS-denied environment, the main carrier first obtains the rotation matrix from the main carrier's rectangular coordinate system to the northeast celestial coordinate system through INS, radio, and vision technologies, and uses the rotation matrix and the second position information to calculate the third position information of each formation member. Based on the third position information and its own navigation information, the positioning estimation information of each formation member is calculated.

[0114] Figure 5 This is a flow chart of correcting positioning information according to an embodiment of the present invention. Figure 5 As shown, the specific implementation includes the following steps:

[0115] Step S361: Obtain the rotation matrix.

[0116] Specifically, the main carrier can obtain the rotation matrix from the main carrier body rectangular coordinate system to the northeast celestial coordinate system through various technical means that can work in a GNSS-denied environment, such as INS, radio and vision.

[0117] Step S362: Acquire third position information according to the second position information and the rotation matrix.

[0118] Specifically, the main carrier obtains third position information according to the second position information and the rotation matrix, and the third position information is obtained by calculating using the following formula:

[0119]

[0120] in, is the third position information of each formation member, is the second position information of each formation member, It is the rotation matrix from the main carrier body rectangular coordinate system to the northeast celestial coordinate system.

[0121] Step S363: Acquire the main carrier's own navigation information.

[0122] Specifically, the primary carrier can obtain high-precision navigation information through various technical means, such as INS (Inertial Navigation System), radio, and vision, which can operate in GNSS (Global Navigation Satellite System)-denied environments. This navigation information includes the primary carrier's own longitude, latitude, and altitude.

[0123] Step S364: Obtain estimated positioning information of each formation member based on the third position information and the self-navigation information.

[0124] Specifically, the main carrier obtains the estimated positioning information of each formation member according to the third position information and the own navigation information. The estimated positioning information is obtained by calculating using the following formula:

[0125]

[0126] Among them, (L i ,λ i , H i ) is the positioning estimation information, the L i is the latitude of formation member i, λ i is the longitude of formation member i, H i is the elevation of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the elevation of the main carrier, p N is the north coordinate of formation member i in the northeast celestial coordinate system, p E is the east coordinate of formation member i in the northeast celestial coordinate system, p U is the coordinate of the formation members in the northeast celestial coordinate system, R mh is the meridian radius at the latitude of formation member i, R nh is the radius of the circle at the latitude of formation member i;

[0127] The meridian radius is calculated using the following formula:

[0128] R nh =r ec (1.0+f ec sin 2 L0)+H0

[0129] The meridian radius is calculated using the following formula:

[0130] R mh =r ec (1.0-2f ec +3f ec sin 2L0)+H0

[0131] Among them, r ec is the major radius of the Earth, f ec is the oblateness of the earth, H0 is the altitude of the main carrier, and L0 is the latitude of the main carrier.

[0132] Step S400: Obtain the position quantity to be corrected according to the positioning estimation information and the initial positioning information.

[0133] Specifically, the main carrier obtains the position quantity to be corrected based on the positioning estimation information and the initial positioning information. The initial positioning information is obtained at the same time as the main carrier obtains the measurement information of the unmanned cluster formation members in the main carrier body coordinate system through the data link equipment.

[0134] The position value to be corrected is obtained by calculation using the following formula:

[0135] ΔL i =L i -L i_nav0

[0136] Δλ i =λ i -λ i_nav0

[0137] ΔH i =H i -H i_nav0

[0138] Where, ΔL i is the latitude of formation member i to be corrected, Δλ i is the longitude position of formation member i to be corrected, ΔH i is the height of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i in the navigation system, λ i_nav0 is the longitude value provided by formation member i under the navigation system, H i_nav0 is the elevation value provided by formation member i under the navigation system, i = 1,…,n.

[0139] Step S500: Obtain target positioning information of each formation member according to the position quantity to be corrected.

[0140] Specifically, the main carrier transmits the position value to be corrected to each formation member via a data link. Each formation member corrects the real-time positioning information based on the position value to be corrected, determines the target positioning information, and transmits it to the main carrier via the data link. The real-time positioning information is obtained only after each formation member receives the position value to be corrected transmitted back via the data link.

[0141] The target positioning information is obtained by calculating the following formula:

[0142] L i_t =L i_t ′+ΔL i

[0143] λ i_t =λ i_t ′+Δλ i

[0144] H i_t =H i_t ′+ΔH i

[0145] Among them, (L i_t ,λ i_t , H i_t ) is the target positioning information, L i_t ′ is the latitude value of formation member i at a certain time t, λ i_t ′ is the longitude value of formation member i at a certain time t, H i_t ′ is the elevation value of formation member i at a certain time t, and formation member i is (L i_t ,λ i_t , H i_t ) as its position at time t for subsequent navigation calculations.

[0146] This embodiment of the present invention obtains first position information based on the measurement information of each formation member, obtains distance measurement results and initial positioning information between each formation member via a data link, obtains estimated positioning information for each formation member based on the first position information and distance measurement results, obtains a position quantity to be corrected based on the estimated positioning information and initial positioning information, and obtains target positioning information for each formation member based on the position quantity to be corrected. Thus, by combining multi-source data and assisting the main carrier, the collaborative positioning accuracy of unmanned swarm formations can be improved, ensuring the operational safety of the formation.

[0147] Figure 6 Schematic diagram of an unmanned cluster formation positioning device in a GNSS-denied environment according to an embodiment of the present invention. Figure 6As shown, the unmanned cluster formation positioning device in a GNSS-denied environment of an embodiment of the present invention includes a first positioning module 61, which is used to obtain first positioning information based on the measurement information of each formation member, where the first positioning information is the position information of each formation member in a rectangular coordinate system with the main carrier as the origin; a data result acquisition module 62, which is used to obtain the distance measurement results and initial positioning information between each formation member through a data link; a navigation estimation module 63, which is used to obtain the positioning estimation information of each formation member based on the first positioning information and the distance measurement results; a to-be-corrected position quantity acquisition module 64, which is used to obtain the to-be-corrected position quantity based on the positioning estimation information and the initial positioning information; and a correction module 65, which is used to obtain the target positioning information of each formation member based on the to-be-corrected position quantity.

[0148] In some embodiments, the first location module 61 includes:

[0149] A measurement information acquisition module is used to obtain measurement information of each formation member through a data link device, wherein the measurement information is the spherical coordinates of each formation member in a spherical coordinate system with the main carrier as the origin;

[0150] A first coordinate conversion module, configured to obtain first position information based on measurement information of each formation member;

[0151] The first position information is obtained by calculating using the following formula:

[0152]

[0153] Among them, P i_detect is the coordinate of formation member i in the rectangular coordinate system with the main carrier as the origin, (φ i ,ψ i ,r i ) is the measurement information of formation member i, φ i is the azimuth of formation member i in the spherical coordinate system, ψ i is the pitch angle of formation member i in the spherical coordinate system, r i is the distance between formation member i and the main carrier, -r i cosψ i sinφ i is the x-axis coordinate of formation member i in the rectangular coordinate system of the main carrier, r i cosψ i cosφ i is the y-axis coordinate of formation member i in the rectangular coordinate system of the main carrier, r i sinψ i is the z-axis coordinate of formation member i in the rectangular coordinate system of the main carrier body.

[0154] In some embodiments, the navigation estimation module 63 includes:

[0155] An initial position acquisition module is used to obtain the initial position information of each formation member through a data link, wherein the initial position information is the position information of each formation member;

[0156] A distance residual module, configured to obtain a distance residual according to the initial position information and the distance measurement result;

[0157] A position residual module, configured to obtain a position residual amount according to the initial position information and the first position information;

[0158] A second position information acquisition module is used to determine the second position information of each formation member according to the distance residual and the position residual;

[0159] A positioning estimation information acquisition module is used to obtain positioning estimation information of each formation member based on the second position information.

[0160] In some embodiments, the distance residual is calculated using the following formula:

[0161]

[0162] in, is the difference between the actual measured distance and the initial distance between formation members i and j, P i is the initial position information of formation member i, P j is the initial position information of formation member j, |P i -P j | is the initial distance obtained based on the initial position information between formation members i and j, l ij is the distance measurement result between formation members i and j, i = 1, ..., n-1, j = i + 1, ...n;

[0163] The position residual is calculated by the following formula:

[0164]

[0165] in, is the difference between the actual measured position and the initial position of formation member i in the rectangular coordinate system of the main carrier body, P i is the initial position information of formation member i, P i_detect is the second position information of formation member i.

[0166] In some embodiments, the second location information acquisition module includes:

[0167] A position information set acquisition module is used to determine the position information set of all formation members according to the distance residual and the position residual;

[0168] a position information extraction module, configured to determine second position information of each formation member based on the position information set;

[0169] The location information set is obtained by calculating the following formula:

[0170]

[0171] Among them, P * is the position information set of all formation members in the main carrier body coordinate system, w ij and w k are weight coefficients determined by the measurement noise, and argmin is a mathematical formula used to find the parameter value that minimizes the formula. is the weighted square of the distance residuals between formation members, is the weighted square of the position residual of each formation member, i = 1,…,n-1; j = i+1,…n, k = 1,…,n.

[0172] In some embodiments, the positioning estimation information acquisition module includes:

[0173] A rotation matrix acquisition module, used to acquire a rotation matrix, wherein the rotation matrix is ​​used to convert the rectangular coordinate system of the main carrier body into the northeast celestial coordinate system;

[0174] A second coordinate conversion module, configured to obtain third position information according to the second position information and the rotation matrix;

[0175] Self-navigation module, used to obtain the main carrier's self-navigation information;

[0176] a positioning estimation information submodule, configured to obtain positioning estimation information of each formation member based on the third position information and the self-navigation information;

[0177] The third position information is obtained by calculating using the following formula:

[0178]

[0179] in, is the third position information of each formation member, is the second position information of each formation member, It is the rotation matrix from the main carrier body rectangular coordinate system to the northeast celestial coordinate system.

[0180] In some embodiments, the positioning estimation information is obtained by calculating the following formula:

[0181]

[0182] Among them, (L i ,λ i , H i ) is the positioning estimation information, the L i is the latitude of formation member i, λ i is the longitude of formation member i, H i is the elevation of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the elevation of the main carrier, p N is the north coordinate of formation member i in the northeast celestial coordinate system, p E is the east coordinate of formation member i in the northeast celestial coordinate system, p U is the coordinate of the formation members in the northeast celestial coordinate system, R mh is the meridian radius at the latitude of formation member i, R nh is the radius of the circle at the latitude of formation member i;

[0183] The meridian radius is calculated using the following formula:

[0184] R nh =r ec (1.0+f ec sin 2 L0)+H0

[0185] The meridian radius is calculated using the following formula:

[0186] R mh =r ec (1.0-2f ec +3f ec sin 2 L0)+H0

[0187] Among them, r ec is the major radius of the Earth, f ec is the oblateness of the earth, H0 is the altitude of the main carrier, and L0 is the latitude of the main carrier.

[0188] In some embodiments, the position value to be corrected is calculated using the following formula:

[0189] ΔL i =L i -L i_nav0

[0190] Δλ i =λ i -λ i_nav0

[0191] ΔH i =H i -H i_nav0

[0192] Where, ΔL i is the latitude of formation member i to be corrected, Δλ i is the longitude position of formation member i to be corrected, ΔH i is the height of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i in the navigation system, λ i_nav0 is the longitude value provided by formation member i under the navigation system, H i_nav0 is the elevation value provided by formation member i under the navigation system, i = 1,…,n.

[0193] In some embodiments, the target positioning information is obtained by calculating the following formula:

[0194] L i_t =L i_t ′+ΔL i

[0195] λ i_t =λ i_t ′+Δλ i

[0196] H i_t =H i_t ′+ΔH i

[0197] Among them, (L i_t ,λ i_t , H i_t ) is the target positioning information, L i_t ′ is the latitude value of formation member i at a certain time t, λ i_t ′ is the longitude value of formation member i at a certain time t, H i_t ′ is the elevation value of formation member i at a certain time t, and formation member i is (L i_t ,λ i_t , H i_t ) as its position at time t for subsequent navigation calculations.

[0198] This embodiment of the present invention obtains first position information based on the measurement information of each formation member, obtains distance measurement results and initial positioning information between each formation member via a data link, obtains estimated positioning information for each formation member based on the first position information and distance measurement results, obtains a position quantity to be corrected based on the estimated positioning information and initial positioning information, and obtains target positioning information for each formation member based on the position quantity to be corrected. Thus, by combining multi-source data and assisting the main carrier, the collaborative positioning accuracy of unmanned swarm formations can be improved, ensuring the operational safety of the formation.

[0199] Figure 7Schematic diagram of an electronic device according to an embodiment of the present invention. Figure 7 As shown, Figure 7 The electronic device shown includes a general computer hardware structure, which includes at least a processor 71 and a memory 72. The processor 71 and the memory 72 are connected via a bus 73. The memory 72 is suitable for storing instructions or programs executable by the processor 71. The processor 71 can be an independent microprocessor or a collection of one or more microprocessors. Thus, the processor 71 executes the instructions stored in the memory 72, thereby executing the method flow of the embodiment of the present invention described above to process data and control other devices. The bus 73 connects the above-mentioned multiple components together and connects the above-mentioned components to the display controller 74 and the display device and the input / output (I / O) device 75. The input / output (I / O) device 75 can be a mouse, keyboard, modem, network interface, touch input device, somatosensory input device, printer, and other devices known in the art. Typically, the input / output device 75 is connected to the system via an input / output (I / O) controller 76.

[0200] Where the solutions described in this specification and in the examples involve the processing of personal information, such processing will be conducted with a legitimate basis (e.g., with the consent of the personal information subject or as necessary for the performance of a contract) and only within the prescribed or agreed scope. A user's refusal to process personal information other than that required for basic functions will not affect the user's use of these basic functions.

[0201] It will be understood by those skilled in the art that the embodiments of the present application may be provided as methods, devices (equipment), or computer program products. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-readable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0202] The present application is described with reference to flowcharts of methods, apparatuses (devices), and computer program products according to embodiments of the present application. It should be understood that each process in the flowcharts can be implemented by computer program instructions.

[0203] These computer program instructions may be stored in a computer-readable memory that can direct a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device that implements the process Figure 1 A function specified in a process or multiple processes.

[0204] These computer program instructions can also be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce the instructions for implementing the process Figure 1 A device that specifies functions in a process or multiple processes.

[0205] Another embodiment of the present invention relates to a non-volatile storage medium for storing a computer-readable program, wherein the computer-readable program is used to enable a computer to execute part or all of the above method embodiments.

[0206] That is, those skilled in the art will understand that all or part of the steps in the above-mentioned embodiments can be implemented by specifying relevant hardware through a program, which is stored in a storage medium and includes a number of instructions for causing a device (which may be a single-chip microcomputer, chip, etc.) or a processor to execute all or part of the steps of the methods described in the embodiments of the present application. The aforementioned storage medium includes: a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, etc., various media that can store program code.

[0207] The foregoing is merely a preferred embodiment of the present application and is not intended to limit the present application. Persons skilled in the art will readily appreciate that various modifications and variations are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present application are intended to be within the scope of protection of the present application.

Claims

1. A method for positioning unmanned swarm formations in a GNSS-denied environment, applicable to a main carrier, which is an electronic device equipped with a navigation device and a data link device with both angle and distance measurement functions, characterized in that: The method comprises: Acquire first position information based on measurement information of each formation member, where the first position information is position information of each formation member in a rectangular coordinate system with the main carrier as the origin, and the measurement information is spherical coordinates of each formation member in a spherical coordinate system with the main carrier as the origin; Obtain distance measurement results and initial positioning information between each formation member through data link; Obtaining estimated positioning information of each formation member based on the first position information and the distance measurement result; Acquire a position quantity to be corrected based on the positioning estimation information and the initial positioning information; Obtaining target positioning information of each formation member according to the position quantity to be corrected; The acquiring of location estimation information of each formation member according to the first position information and the distance measurement result includes: Acquire the initial position information of each formation member through the data link, wherein the initial position information is the position information of each formation member; Acquire a distance residual according to the initial position information and the distance measurement result; Acquire a position residual amount according to the initial position information and the first position information; Determine a set of position information of all formation members according to the distance residual and the position residual; determining second position information of each formation member according to the position information set; The location information set is obtained by calculating the following formula: Among them, P * is the position information set of all formation members in the main carrier body coordinate system, w ij and w k are weight coefficients determined by the measurement noise, and argmin is a mathematical formula used to find the parameter value that minimizes the formula. is the weighted square of the distance residuals between formation members, is the weighted square of the position residual of each formation member, i=1,…,n-1; j=i+1,…n, k=1,…,n; Obtaining estimated positioning information of each formation member based on the second position information; The acquiring of the estimated positioning information of each formation member according to the second position information includes: Obtaining a rotation matrix, wherein the rotation matrix is ​​used to convert the main carrier body rectangular coordinate system to the northeast celestial coordinate system; Acquire third position information according to the second position information and the rotation matrix; Obtain the main carrier's own navigation information; The positioning estimation information of each formation member is obtained according to the third position information and the self-navigation information.

2. The method according to claim 1, characterized in that The obtaining of first position information according to measurement information of each formation member includes: Acquiring measurement information of each formation member through a data link device, wherein the measurement information is the spherical coordinates of each formation member in a spherical coordinate system with the main carrier as the origin; Acquiring first position information according to measurement information of each formation member; The first position information is obtained by calculating using the following formula: Among them, P k_detect is the coordinate of formation member k in the rectangular coordinate system with the main carrier as the origin, (φ k ,ψ k ,r k ) is the measurement information of formation member k, φ k is the azimuth of formation member k in the spherical coordinate system, ψ k is the pitch angle of formation member k in the spherical coordinate system, r k is the distance between formation member k and the main carrier, -r k cosψ k sinφ k is the x-axis coordinate of formation member k in the rectangular coordinate system of the main carrier, r k cosψ k cosφ k is the y-axis coordinate of formation member k in the rectangular coordinate system of the main carrier, r k sinψ k is the z-axis coordinate of formation member k in the rectangular coordinate system of the main carrier body.

3. The method according to claim 1, characterized in that The distance residual is calculated using the following formula: in, is the difference between the actual measured distance and the initial distance between formation members i and j, P i is the initial position information of formation member i, P j is the initial position information of formation member j, |P i -P j | is the initial distance obtained based on the initial position information between formation members i and j, l ij is the distance measurement result between formation members i and j, i = 1, ..., n-1, j = i + 1, ...n; The position residual is calculated by the following formula: in, is the difference between the actual measured position and the initial position of formation member k in the rectangular coordinate system of the main carrier body, P k is the initial position information of formation member k, P k_detect is the second position information of formation member k.

4. The method according to claim 1, wherein The third position information is obtained by calculating using the following formula: in, is the third position information of each formation member, is the second position information of each formation member, It is the rotation matrix from the main carrier body rectangular coordinate system to the northeast celestial coordinate system.

5. The method according to claim 4, characterized in that The positioning estimation information is obtained by calculating the following formula: Among them, (L i ,λ i , H i ) is the positioning estimation information, the L i is the latitude of formation member i, λ i is the longitude of formation member i, H i is the elevation of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the elevation of the main carrier, p N is the north coordinate of formation member i in the northeast celestial coordinate system, p E is the east coordinate of formation member i in the northeast celestial coordinate system, p U is the coordinate of the formation members in the northeast celestial coordinate system, R mh is the meridian radius at the latitude of formation member i, R nh is the radius of the circle at the latitude of formation member i; The radius of the circle is calculated by the following formula: <h2 style=";text-align:left;direction:ltr">R<h2 style=";text-align:left;direction:ltr"> nh <h2 style=";text-align:left;direction:ltr"> =r<h2 style=";text-align:left;direction:ltr"> ec <h2 style=";text-align:left;direction:ltr"> (1.0+f<h2 style=";text-align:left;direction:ltr"> ec <h2 style=";text-align:left;direction:ltr"> sin<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> L0)+H0 The meridian radius is calculated using the following formula: <h2 style=";text-align:left;direction:ltr">R<h2 style=";text-align:left;direction:ltr"> mh <h2 style=";text-align:left;direction:ltr"> =r<h2 style=";text-align:left;direction:ltr"> ec <h2 style=";text-align:left;direction:ltr"> (1.0-2f<h2 style=";text-align:left;direction:ltr"> ec <h2 style=";text-align:left;direction:ltr"> +3f<h2 style=";text-align:left;direction:ltr"> ec <h2 style=";text-align:left;direction:ltr"> sin<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> L0)+H0 Among them, r ec is the major radius of the Earth, f ec is the oblateness of the earth, H0 is the altitude of the main carrier, and L0 is the latitude of the main carrier.

6. The method according to claim 5, characterized in that The position value to be corrected is obtained by calculating the following formula: ΔL i =L i -L i_nav0 Dl i =λ i -l i_nav0 ΔH i =H i -H i_nav0 Where, ΔL i is the latitude of formation member i to be corrected, Δλ i is the longitude position of formation member i to be corrected, ΔH i is the height of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i in the navigation system, λ i_nav0 is the longitude value provided by formation member i under the navigation system, H i_nav0 is the elevation value provided by formation member i under the navigation system, i = 1,…,n.

7. The method according to claim 6, characterized in that The target positioning information is obtained by calculating the following formula: L i_t =L i_t ′+ΔL i l i_t =λ i_t ′+Dl i H i_t =H i_t ′+ΔH i Among them, (L i_t ,λ i_t , H i_t ) is the target positioning information, L i_t ′ is the latitude value of formation member i at a certain time t, λ i_t ′ is the longitude value of formation member i at a certain time t, H i_t ′ is the elevation value of formation member i at a certain time t, and formation member i is (L i_t ,λ i_t , H i_t ) as its position at time t for subsequent navigation calculations.

8. An electronic device comprising a memory and a processor, characterized in that: The memory is configured to store one or more computer program instructions, wherein the one or more computer program instructions are executed by the processor to implement the method according to any one of claims 1 to 7.

Images