Positioning method and electronic equipment

Abstract

The embodiment of the invention discloses a positioning method and electronic equipment, and the method comprises the steps: obtaining first position information according to the measurement information of each formation member, and obtaining a distance measurement result and initial positioning information between the formation members through a data link; and obtaining positioning estimation information of each formation member according to the first position information and the distance measurement result, obtaining a to-be-corrected position quantity according to the positioning estimation information and the initial positioning information, and obtaining target positioning information of each formation member according to the to-be-corrected position quantity. Therefore, through multi-source data combination and main carrier assistance, the unmanned cluster formation cooperative positioning precision can be improved, and the operation safety of the formation is guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a positioning method and electronic equipment. Background Art

[0002] Unmanned swarm formations have shown great influence in the fields of industry, agriculture, and scientific research due to their multiple advantages such as 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 to improving the operational capabilities and efficiency of unmanned systems in complex environments, and is especially suitable for scenarios with limited navigation infrastructure. However, the current mainstream unmanned equipment formation positioning technology relies on the combined navigation method of the inertial navigation system and the global navigation satellite system. When the global navigation satellite system signal is blocked or interfered with, the combined navigation fails, resulting in the failure of 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 a positioning method and an electronic device, which can improve the collaborative positioning accuracy of an unmanned cluster formation and ensure the operational safety of the formation.

[0004] In a first aspect, an embodiment of the present invention provides a positioning method, the method comprising:

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

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

[0007] Obtaining location estimation information of each formation member according to the first position information and the distance measurement result;

[0008] Acquire the position quantity to be corrected according to 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, acquiring the first position information according to the measurement information of each formation member includes:

[0011] Acquire 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] Acquire first position information according to measurement information of each formation member;

[0013] The first position information is obtained by calculating 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 the location estimation information of each formation member according to 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 location estimation information of each formation member is obtained according to the second location information.

[0022] In some embodiments, the distance residual is calculated by 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] Determine 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 residual between each formation member, 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, acquiring the location estimation information of each formation member according to the second location information includes:

[0035] Obtaining a rotation matrix, wherein the rotation matrix is ​​used to transform the main carrier body rectangular coordinate system into 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] Acquire location estimation information of each formation member according to the third position information and the own 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 altitude of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the altitude 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 member in the celestial direction 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 by the following formula:

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

[0047] The meridian radius is calculated by 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 flattening 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 by 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 position of formation member i to be corrected, Δλ i is the longitude position to be corrected of formation member i, ΔH i is the height position of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i under 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 location 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, and 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 perform the method described in the first aspect.

[0061] The technical solution of the embodiment of the present invention obtains the first position information according to the measurement information of each formation member, obtains the distance measurement results and initial positioning information between each formation member through the data link, obtains the positioning estimation information of each formation member according to the first position information and the distance measurement results, obtains the position quantity to be corrected according to the positioning estimation information and the initial positioning information, and obtains the target positioning information of each formation member according to the position quantity to be corrected. Therefore, through the combination of multi-source data and the assistance of the main carrier, the collaborative positioning accuracy of the unmanned cluster formation can be improved to ensure 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 a positioning system according to an embodiment of the present invention;

[0064] Figure 2 is a flow chart of a positioning method 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 is a schematic diagram of a positioning device 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 embodiments, but the present application is not limited to these embodiments. In the detailed description of the present application below, some specific details are described in detail. It is possible for those skilled in the art to fully understand the present application without the description of these details. In order to avoid confusing the essence of the present application, known methods, processes, flows, components and circuits are not described in detail.

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

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

[0073] In the description of this application, it should be understood that the terms "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance. In addition, in the description of this application, unless otherwise specified, the meaning of "plurality" is two or more.

[0074] Unmanned cluster collaborative positioning technology refers to the technology of mutual cooperation between multiple unmanned systems in a group 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 cluster collaborative positioning technology is a continuously developing field. It is of great significance to improve the operational capability 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 a positioning system according to an embodiment of the present invention. Figure 1In the illustrated embodiment, taking the formation members as aircraft as an example, the positioning system includes a first formation member 11, a second formation member 12, a third formation member 13 and a main carrier 14. The first formation member 11, the second formation member 12 and the third formation member 13 are connected to the main carrier 14 through a data link. Among them, the first formation member 11, the second formation member 12, the third formation member 13 and the main carrier 14 are all equipped with data link equipment and a navigation system. The data link equipment in the first formation member 11, the second formation member 12 and the third formation member 13 is used to measure the relative distance between each formation member, and the data link equipment of the main carrier 14 is used to measure the coordinates of each formation member in the spherical coordinate system of the main carrier body. Among them, the main carrier is an electronic device with a high-precision navigation device and a data link device with both angle measurement and distance measurement functions. The formation members refer to multiple unmanned systems in an unmanned cluster formation, such as unmanned aerial vehicles, unmanned underwater vehicles, etc., which are not specified in the 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 respective 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 first formation member 11, the second formation member 12 and the third formation member 13 obtain the distance measurement results between each formation member and the initial positioning information of each formation member through their 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 through 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 2FIG. 1 is a flow chart of a positioning method according to an embodiment of the present invention. Figure 2 The specific implementation of the positioning method 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, and 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 FIG. 1 is a 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 the first position information according to 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 and obtained by 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, r i cosψ i cosφ iis 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 the 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 itself. The data link equipment of the formation member itself 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 itself. 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 location estimation information of each formation member according to 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 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 in combination with the second position information. Finally, based on the third position information and the navigation information of the main carrier itself, 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: Acquire the initial position information of each formation member through the data link.

[0093] Specifically, each formation member sends the initial positioning information to the main carrier through a data link, and the initial positioning information is obtained through the navigation system of each formation member. The navigation system usually includes high-precision navigation equipment such as a global navigation satellite system receiver and an inertial navigation system. In this embodiment, each formation member will continuously obtain geographic navigation results through its own navigation system, and send this information to the main carrier through a data link for centralized processing. The main carrier can track and locate the positions of the formation members based on this information.

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

[0095] Specifically, the main carrier obtains a distance residual amount according to the initial position information and the distance measurement result, and the distance residual amount is calculated and obtained 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, and the position information set is calculated and obtained by 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 residual between each formation member, 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, through 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), (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 of the above position information set can be used to obtain the four position information corresponding to (i=1, j=2), (i=2, j=3), (i=3, j=4), and (i=4, j=5) when k=1, and select the minimum value of the four position information. Similarly, the minimum value of the position information when k=2, 3, 4, and 5 can be obtained. That is, the minimum values ​​of the five position information can be obtained by 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 according to the position information set, where the position information set includes the minimum position information corresponding to each formation member.

[0112] Step S360: Obtaining estimated positioning information of each formation member according to the second position information.

[0113] Specifically, in a GNSS-denied environment, the main carrier first obtains the rotation matrix from the main carrier's main rectangular coordinate system to the northeast celestial coordinate system through INS, radio, vision and other technical means, 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 FIG. 1 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: Obtain 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 calculated and obtained by 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: Obtain the main carrier's own navigation information.

[0122] Specifically, the main carrier can obtain its own high-precision navigation information through various technical means that can work in a GNSS (Global Navigation Satellite System) denial environment, such as INS (Inertial Navigation System), radio and vision. The self-navigation information includes the longitude, latitude and altitude of the main carrier itself.

[0123] Step S364: Obtain the estimated positioning information of each formation member according to the third position information and the own 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, and the estimated positioning information is obtained by calculating 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 altitude of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the altitude 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 member in the celestial direction 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 by the following formula:

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

[0129] The meridian radius is calculated by 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 flattening of the earth, H0 is the altitude of the main carrier, and L0 is the latitude of the main carrier.

[0132] Step S400: obtaining a 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 to be corrected is calculated by 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 position of formation member i to be corrected, Δλ i is the longitude position to be corrected of formation member i, ΔH i is the height position of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i under 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: acquiring target positioning information of each formation member according to the position quantity to be corrected.

[0140] Specifically, the main carrier sends the position quantity to be corrected to each formation member through the data link, and each formation member corrects the real-time positioning information according to the position quantity to be corrected to determine the target positioning information and sends it to the main carrier through the data link. The real-time positioning information is obtained after each formation member receives the amount to be corrected sent back by 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] The embodiment of the present invention obtains the first position information according to the measurement information of each formation member, obtains the distance measurement results and initial positioning information between each formation member through the data link, obtains the positioning estimation information of each formation member according to the first position information and the distance measurement results, obtains the position quantity to be corrected according to the positioning estimation information and the initial positioning information, and obtains the target positioning information of each formation member according to the position quantity to be corrected. Therefore, through the combination of multi-source data and the assistance of the main carrier, the collaborative positioning accuracy of the unmanned cluster formation can be improved to ensure the operational safety of the formation.

[0147] Figure 6 Schematic diagram of a positioning device according to an embodiment of the present invention. Figure 6 As shown, the positioning device of the embodiment of the present invention includes a first position module 61, which is used to obtain first position information according to the measurement information of each formation member, wherein the first position 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 according to the first position information and the distance measurement result; a to-be-corrected position quantity acquisition module 64, which is used to obtain the to-be-corrected position quantity according to 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 according to the to-be-corrected position quantity.

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

[0149] A measurement information acquisition module, used to acquire the 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, used for acquiring first position information according to measurement information of each formation member;

[0151] The first position information is obtained by calculating 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 acquire 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 itself;

[0156] A distance residual module, used for obtaining a distance residual according to the initial position information and the distance measurement result;

[0157] A position residual module, used for obtaining a position residual amount according to the initial position information and the first position information;

[0158] A second position information acquisition module, 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 the positioning estimation information of each formation member according to the second position information.

[0160] In some embodiments, the distance residual is calculated by 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, 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, used to determine the second position information of each formation member according to 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 kare 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 residual between each formation member, 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, used for acquiring third position information according to the second position information and the rotation matrix;

[0175] The self-navigation module is used to obtain the self-navigation information of the main carrier;

[0176] A positioning estimation information submodule, used for obtaining positioning estimation information of each formation member according to the third position information and the own 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 altitude of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the altitude 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, pU is the coordinate of the formation member in the celestial direction 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 by the following formula:

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

[0185] The meridian radius is calculated by 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 flattening 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 by 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 position of formation member i to be corrected, Δλ i is the longitude position to be corrected of formation member i, ΔH i is the height position of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i under 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 location 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] The embodiment of the present invention obtains the first position information according to the measurement information of each formation member, obtains the distance measurement results and initial positioning information between each formation member through the data link, obtains the positioning estimation information of each formation member according to the first position information and the distance measurement results, obtains the position quantity to be corrected according to the positioning estimation information and the initial positioning information, and obtains the target positioning information of each formation member according to the position quantity to be corrected. Therefore, through the combination of multi-source data and the assistance of the main carrier, the collaborative positioning accuracy of the unmanned cluster formation can be improved to ensure the operational safety of the formation.

[0199] Figure 7 Schematic diagram of an electronic device according to an embodiment of the present invention. Figure 7 As shown, Figure 7The 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 as described above to realize the processing of data and the control of other devices. The bus 73 connects the above-mentioned multiple components together, and at the same time 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, a keyboard, a modem, a network interface, a touch input device, a somatosensory input device, a 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] The solutions described in this specification and in the examples, if they involve the processing of personal information, will be processed on the premise of having a legal basis (such as obtaining the consent of the subject of personal information, or being necessary for the performance of a contract, etc.), and will only be processed within the scope of regulations or agreements. If a user refuses to process personal information other than the necessary information for basic functions, it will not affect the user's use of basic functions.

[0201] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, devices (equipment) or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware. Moreover, the present application may adopt a computer program product implemented on one or more computer-readable storage media (including but not limited to 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 flowchart can be implemented by computer program instructions.

[0203] These computer program instructions may be stored in a computer readable memory capable of directing 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 including an instruction device that implements the process Figure 1 A function specified in a process or multiple processes.

[0204] These computer program instructions may 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 for a computer to execute part or all of the above method embodiments.

[0206] That is, those skilled in the art can understand that all or part of the steps in the above-mentioned embodiment method can be completed by specifying relevant hardware through a program, and the program is stored in a storage medium, including several instructions for a device (which can be a single-chip microcomputer, chip, etc.) or a processor (processor) to execute all or part of the steps of the method described in each embodiment of the present application. 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.

[0207] The above description is only a preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A positioning method, characterized in that: The method comprises: Acquire first position information according to the measurement information of each formation member, where the first position information is the position information of each formation member in a rectangular coordinate system with the main carrier as the origin; Obtain the distance measurement results and initial positioning information between each formation member through the data link; Acquire location estimation information of each formation member according to the first position information and the distance measurement result; Acquire the position quantity to be corrected according to the positioning estimation information and the initial positioning information; The target positioning information of each formation member is obtained according to the position quantity to be corrected.

2. The method according to claim 1, characterized in that: The obtaining of the first position information according to the measurement information of each formation member includes: Acquire 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; Acquire 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 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.

3. The method according to claim 1, characterized in that The acquiring the 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 the second position information of each formation member according to the distance residual and the position residual; The positioning estimation information of each formation member is obtained according to the second position information.

4. The method according to claim 3, characterized in that The distance residual is calculated by 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 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.

5. The method according to claim 3, characterized in that: The determining of the second position information of each formation member according to the distance residual and the position residual comprises: Determine a set of position information of all formation members according to the distance residual and the position residual; Determine 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 residual between each formation member, is the weighted square of the position residual of each formation member, i=1,…,n-1; j=i+1,…n, k=1,…,n.

6. The method according to claim 5, characterized in that The acquiring of the location estimation information of each formation member according to the second location information includes: Obtaining a rotation matrix, wherein the rotation matrix is ​​used to transform the main carrier body rectangular coordinate system into 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; Acquire location estimation information of each formation member according to the third location information and the own navigation information; 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.

7. The method according to claim 6, 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 altitude of formation member i, L0 is the latitude of the main carrier, λ0 is the longitude of the main carrier, H0 is the altitude 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 member in the celestial direction 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 meridian radius 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 by 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 flattening of the earth, H0 is the altitude of the main carrier, and L0 is the latitude of the main carrier.

8. The method according to claim 7, characterized in that The position to be corrected is calculated by 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 position of formation member i to be corrected, Δλ i is the longitude position to be corrected of formation member i, ΔH i is the height position of formation member i to be corrected, L i_nav0 is the latitude value provided by formation member i under 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.

9. The method according to claim 8, 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.

10. An electronic device comprising a memory and a processor, characterized in that: The memory is used 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 9.

Images