Clustered uav positioning method based on millimeter wave radar

Abstract

The application provides a millimeter wave radar-based cluster unmanned aerial vehicle positioning method and system, a storage medium and an electronic device, and relates to the field of unmanned cluster positioning. The application first uses millimeter wave radar technology to solve the unmanned aerial vehicle cluster positioning problem, and can realize high-precision distance, azimuth, pitch and speed measurement. Compared with existing wireless positioning technology, the measurement information is more abundant and the measurement accuracy is higher. At the same time, the application proposes a solution to the coordinate unification problem of satellite navigation positioning and cluster relative positioning, which is convenient for the design of formation controller to ensure that the cluster maintains a specific formation geometry at a specific position.

Description

Technical Field

[0001] This invention relates to the field of unmanned swarm positioning, specifically to a method, system, storage medium, and electronic device for swarm UAV positioning based on millimeter-wave radar. Background Technology

[0002] In recent years, unmanned aerial vehicles (UAVs) have gradually become practical. However, due to their inherent performance limitations, individual UAVs face significant challenges when encountering complex environments and tasks. Therefore, UAV technology development is trending towards swarm collaboration. A UAV swarm is an autonomous intelligent system composed of a certain number of UAVs that utilizes information exchange and feedback, incentives and responses to achieve coordinated behavior, dynamically adapt to the environment, and jointly complete specific tasks. With its collective intelligence advantage far exceeding that of individual UAVs, UAV swarms have become a research hotspot in both military and civilian fields, potentially generating significant military, social, and economic benefits.

[0003] Coordinated formation is the technological foundation for UAV swarm mission execution. Multiple UAVs plan their routes and maintain a specific geometric formation to adapt to mission requirements, reducing their energy demands and enabling more efficient mission completion. Currently, UAVs widely use satellite navigation and positioning technology, but in denied scenarios, satellite navigation and positioning suffers from poor stability and unreliability. In such situations, relative positioning technology can be used to measure the relative positions of individual UAVs within the swarm. Regarding measurement methods, existing UAV swarms primarily use wireless positioning technology to achieve relative positioning.

[0004] For example, wireless positioning technologies based on continuous wave signals, such as Bluetooth, WiFi (Wireless Fidelity), and ZigBee, typically use Signal Strength Indication Information (RSSI) or Channel State Information (CSI) to estimate distance, but they suffer from problems such as small coverage area, high susceptibility to noise interference, unstable positioning, and low accuracy.

[0005] For example, ultra-wideband (UWB) positioning technology uses the arrival time of multiple received narrow pulse signals and employs TDOA (Time Difference of Arrival) and TOF (Time of Fight) algorithms to achieve positioning accuracy down to the centimeter level. However, its positioning accuracy is affected by the time synchronization accuracy between drones and the number of drones involved in the positioning process, resulting in high implementation complexity. Summary of the Invention

[0006] (a) Technical problems to be solved

[0007] To address the shortcomings of existing technologies, this invention provides a cluster UAV positioning method, system, storage medium, and electronic device based on millimeter-wave radar, which solves the technical problem of low measurement accuracy in traditional wireless positioning technologies.

[0008] (II) Technical Solution

[0009] To achieve the above objectives, the present invention provides the following technical solution:

[0010] A swarm UAV localization method based on millimeter-wave radar, using a lead-wingman formation mode, wherein the method is executed by the lead aircraft, and includes:

[0011] S1. Based on the set of critical path points of the planned trajectory and the motion model of the wingmen, estimate the geometric topology of the swarm formation at the future measurement time t to obtain the expected value of the relative position of the wingmen; where the planned trajectory of each UAV refers to the flight trajectory from the take-off point to the mission target that is collaboratively planned according to the target mission.

[0012] S2. Before the measurement time t, the aircraft's own position coordinates in the WGS84 coordinate system are calculated in real time by combining the historical position coordinates before navigation failure and the measurement information of the inertial measurement unit.

[0013] S3. Upon reaching measurement time t, use millimeter-wave radar to measure the wingmen in the group, obtain the position of the wingmen relative to the lead aircraft, and obtain the measurement value of the relative position of the wingmen in the group.

[0014] S4. Perform correlation processing on the expected and measured values ​​of the relative positions of the cluster wingmen to establish the correspondence between the wingman number and its relative position measurement value;

[0015] S5. Send the relative position measurement value and the lead aircraft's own position coordinates to each wingman, so that the wingman's own position coordinates in the WGS84 coordinate system can be calculated at the wingman's location.

[0016] Preferably, when a satellite navigation denial occurs, the lead aircraft broadcasts a navigation failure message to the wingman, and the cluster switches to relative positioning mode to execute S1 to S5; otherwise, it maintains satellite navigation positioning mode.

[0017] Preferably, S1 includes:

[0018] S101. Perform a coordinate transformation from the WGS84 coordinate system to the geocentric rectangular coordinate system. After the transformation, obtain the set of position coordinates of the lead aircraft and all wingmen in the geocentric rectangular coordinate system at measurement time t; the transformation expression is as follows:

[0019]

[0020] Where (L,B,H) represent the geodetic longitude, geodetic latitude, and geodetic height of the critical path point in WGS84 coordinates, respectively, and (X,Y,Z) are the rectangular coordinates of the critical path point in the geocentric rectangular coordinate system.

[0021] S102. Perform coordinate transformation from the geocentric rectangular coordinate system to the aircraft geographic coordinate system. After the transformation is completed, obtain the coordinate set of the lead aircraft and all wingmen at the measurement time t in the geographic coordinate system.

[0022] Taking the lead aircraft as the origin of the carrier aircraft's geographic coordinate system, let its WGS84 coordinates be (L0, B0, H0) and its geocentric rectangular coordinates be (x0, y0, z0). Then the geocentric rectangular coordinates are (x0, y0, z0). w ,y w ,z w The location coordinates of the wingman in this geographic coordinate system. The calculation formula is as follows:

[0023]

[0024] Wherein, coordinate transformation matrix

[0025] S103. Based on the coordinate set, obtain the distance, bearing, and pitch of each wingman relative to the lead aircraft to obtain the expected value of the relative position of the group of wingmen at measurement time t.

[0026] Preferably, S4 includes:

[0027] A data association algorithm is used to associate the expected and measured relative positions of the cluster wingmen. When some wingmen cannot be associated with the relative position measurement, S3 is executed again for measurement. If, after reaching the maximum number of consecutive measurements, there are still wingmen that cannot be associated with the relative position measurement, then the expected relative position of this wingman is used as its measurement value.

[0028] Preferably, S5 includes:

[0029] S501, Perform the transformation from relative position to the aircraft coordinate system;

[0030] Let the relative position of the wingman measured by the lead aircraft be (r, a, θ). The wingman's position in the lead aircraft's coordinate system is represented by rectangular coordinates (x, a, θ). p ,y p ,z p The expression is represented as follows:

[0031]

[0032] Where r, a, and θ represent the measured distance, elevation, and azimuth, respectively;

[0033] S502. Perform coordinate transformation from the aircraft coordinate system to the aircraft geographic coordinate system;

[0034] The wingman's rectangular coordinates (x, y) in the lead aircraft's geographic coordinate system l ,y l ,z l ) is represented as:

[0035]

[0036] Where β, ε, and γ are the yaw angle, pitch angle, and roll angle of the aircraft, respectively;

[0037] S503. Perform coordinate transformation from the aircraft's geographic coordinate system to the geocentric rectangular coordinate system;

[0038] The coordinates of the lead aircraft's position at measurement time t are known. The coordinate rotation matrix L from the aircraft's geographic coordinate system to the geocentric coordinate system ba It can be expressed as the following formula:

[0039]

[0040] Among them, L D L E These are the rotation matrices of the D and E axes of the aircraft's geographic coordinate system and the geocentric Cartesian coordinate system, respectively.

[0041] After coordinate rotation and translation, the wingman's spatial coordinates (x, y, z) in the geocentric Cartesian coordinate system are represented as follows:

[0042]

[0043] S504. Perform coordinate transformation from the geocentric rectangular coordinate system to the WGS84 coordinate system. The transformation formula is as follows:

[0044]

[0045] Since variables B and H are nested, a recursive method or an approximate direct method is used to solve the problem, so as to finally obtain the wingman's own position coordinates in the WGS84 coordinate system.

[0046] A swarm UAV positioning system based on millimeter-wave radar, using a lead-wingman formation mode, wherein the system is executed by the lead aircraft, includes:

[0047] The estimation module is used to estimate the geometric topology of the swarm formation at a future measurement time t based on the set of critical path points of the planned trajectory and the swarm wingman motion model, so as to obtain the expected value of the relative position of the swarm wingmen; where the planned trajectory of each UAV refers to the flight trajectory from the take-off point to the mission target that is collaboratively planned according to the target mission.

[0048] The first calculation module is used to calculate the lead aircraft's own position coordinates in the WGS84 coordinate system in real time before the measurement time t, combining the historical position coordinates before navigation failure and the measurement information of the inertial measurement unit.

[0049] The measurement module is used to measure the wingmen of the group using millimeter-wave radar at measurement time t, and obtain the position of the wingmen relative to the leader, so as to obtain the measurement value of the relative position of the wingmen of the group.

[0050] The association module is used to associate the expected and measured values ​​of the relative positions of cluster wingmen and establish the correspondence between the wingman number and its relative position measurement value.

[0051] The second calculation module is used to send the relative position measurement value and the lead aircraft's own position coordinates to each wingman, so as to calculate the wingman's own position coordinates in the WGS84 coordinate system at the wingman's location.

[0052] A storage medium storing a computer program for positioning swarm drones based on millimeter-wave radar, wherein the computer program causes a computer to execute the swarm drone positioning method as described above.

[0053] An electronic device, comprising:

[0054] One or more processors; a memory; and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the programs including methods for performing the clustered drone positioning method as described above.

[0055] (III) Beneficial Effects

[0056] This invention provides a method, system, storage medium, and electronic device for swarm UAV positioning based on millimeter-wave radar. Compared with existing technologies, it has the following advantages:

[0057] This invention is the first to apply millimeter-wave radar technology to solve the problem of UAV swarm positioning, enabling high-precision measurement of distance, azimuth, pitch, and velocity. Compared to existing wireless positioning technologies, it provides richer measurement information and higher accuracy. Simultaneously, this invention addresses the coordinate unification issue between satellite navigation positioning and swarm relative positioning, facilitating the design of formation controllers to ensure the swarm maintains a specific formation geometry at specific locations. Attached Figure Description

[0058] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0059] Figure 1 A block diagram illustrating a clustered UAV positioning method based on millimeter-wave radar, provided as an embodiment of the present invention.

[0060] Figure 2 This is a schematic diagram of a cluster formation flight process provided in an embodiment of the present invention;

[0061] Figure 3 A flowchart illustrating a clustered UAV positioning method based on millimeter-wave radar, provided as an embodiment of the present invention. Detailed Implementation

[0062] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention are described clearly and completely. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0063] This application provides a method, system, storage medium, and electronic device for swarm drone positioning based on millimeter-wave radar. This solves the technical problem of low measurement accuracy in traditional wireless positioning technology, enables autonomous positioning of drone swarms in satellite navigation denied environments, and expands the application scenarios of drone swarms.

[0064] The technical solution in this application is to solve the above-mentioned technical problems, and the general idea is as follows:

[0065] This invention is the first to use radar instead of wireless technology to achieve cluster positioning. For example, the 4D millimeter-wave radar uses MIMO (Multiple-Input Multiple-Output) virtual aperture technology, which can measure the target's distance, azimuth, elevation angle and velocity in real time with high precision. It has the advantages of good environmental adaptability, long range, safety and reliability, miniaturization and low cost, and is suitable for relative positioning of UAV clusters.

[0066] Furthermore, considering that the purpose of measuring the position of swarm drones is to serve as input for the formation controller, which typically designs its control law based on the absolute coordinates of the planned trajectory (such as satellite navigation coordinates), and adjusts the drones' flight parameters according to the deviation of the measured position from the desired position, thereby enabling the swarm to maintain a specific formation geometry at a specific location. However, existing relative positioning technologies often only provide relative positional relationships or relative coordinates within the swarm coordinate system, causing inconvenience in formation controller design. Therefore, it is necessary to unify the position measurements from satellite navigation positioning and relative positioning into the same coordinate system to facilitate a unified formation controller design.

[0067] To better understand the above technical solutions, the following will provide a detailed explanation of the technical solutions in conjunction with the accompanying drawings and specific implementation methods.

[0068] Example:

[0069] like Figure 1 As shown, this embodiment of the invention provides a cluster UAV positioning method based on millimeter-wave radar, based on a lead-wingman formation mode. The method is executed by the lead aircraft and includes:

[0070] S1. Based on the set of critical path points of the planned trajectory and the motion model of the wingmen, estimate the geometric topology of the swarm formation at the future measurement time t to obtain the expected value of the relative position of the wingmen; where the planned trajectory of each UAV refers to the flight trajectory from the take-off point to the mission target that is collaboratively planned according to the target mission.

[0071] S2. Before the measurement time t, the aircraft's own position coordinates in the WGS84 coordinate system are calculated in real time by combining the historical position coordinates before navigation failure and the measurement information of the inertial measurement unit.

[0072] S3. Upon reaching measurement time t, use millimeter-wave radar to measure the wingmen in the group, obtain the position of the wingmen relative to the lead aircraft, and obtain the measurement value of the relative position of the wingmen in the group.

[0073] S4. Perform correlation processing on the expected and measured values ​​of the relative positions of the cluster wingmen to establish the correspondence between the wingman number and its relative position measurement value;

[0074] S5. Send the relative position measurement value and the lead aircraft's own position coordinates to each wingman, so that the wingman's own position coordinates in the WGS84 coordinate system can be calculated at the wingman's location.

[0075] This invention provides a simple and efficient swarm positioning method based on millimeter-wave radar measurement, enabling autonomous positioning of UAV swarms in satellite navigation denied environments and expanding the application scenarios of UAV swarms.

[0076] like Figure 2 As shown, in this embodiment of the invention, the UAV swarm adopts a typical lead-wingman formation. In addition to the mission payload, the lead aircraft carries a 4D millimeter-wave radar, satellite navigation unit, inertial measurement unit, communication unit, and computing unit, while the wingmen only carry the satellite navigation unit, communication unit, and computing unit. The UAV swarm takes off and establishes internal real-time communication. Based on the target mission, a flight path from the takeoff point to the mission objective is collaboratively planned. The flight path of each UAV is described by a series of key waypoints (geodetic longitude, geodetic latitude, geodetic altitude, and arrival time).

[0077] Next, we will combine Figure 3 The above plan is described in detail:

[0078] When satellite navigation denial (i.e., satellite navigation failure) occurs, the lead aircraft broadcasts a navigation failure message to the wingman, and the cluster switches to relative positioning mode to execute S1–S5; otherwise, it maintains satellite navigation positioning mode; wherein:

[0079] In step S1, based on the set of critical path points of the planned trajectory and the wingman motion model, the geometric topology of the formation at a future measurement time t is estimated to obtain the expected values ​​of the relative positions of the wingmen; including:

[0080] S101. Perform a coordinate transformation from the WGS84 coordinate system to the geocentric rectangular coordinate system. After the transformation, obtain the set of position coordinates of the lead aircraft and all wingmen in the geocentric rectangular coordinate system at measurement time t; the transformation expression is as follows:

[0081]

[0082] Where (L,B,H) represent the geodetic longitude, geodetic latitude, and geodetic height of the critical path point in WGS84 coordinates, respectively, and (X,Y,Z) are the rectangular coordinates of the critical path point in the geocentric rectangular coordinate system.

[0083] S102. Perform coordinate transformation from the geocentric rectangular coordinate system to the aircraft geographic coordinate system. After the transformation is completed, obtain the coordinate set of the lead aircraft and all wingmen at the measurement time t in the geographic coordinate system.

[0084] Since the commonly used geographic coordinate systems for carrier aircraft include the NED (Northeast Earth) coordinate system or the ENU (Northeast Sky) coordinate system, this embodiment of the invention will use the NED coordinate system as an example for explanation. Its origin is set at the center of mass of the carrier aircraft, N (North) is defined as the geographic north direction, E (East) is the tangent of the Earth's rotation, and D (Earth) is the perpendicular line from the carrier aircraft to the ground plane and points downwards.

[0085] At this point, taking the lead aircraft as the origin of the carrier's geographic coordinate system, let its WGS84 coordinates be (L0, B0, H0) and its geocentric rectangular coordinates be (x0, y0, z0). Then, the geocentric rectangular coordinates are (x0, y0, z0). w ,y w ,z w The location coordinates of the wingman in this geographic coordinate system. The calculation formula is as follows:

[0086]

[0087] Wherein, coordinate transformation matrix

[0088] S103. Based on the coordinate set, obtain the distance, bearing, and pitch of each wingman relative to the lead aircraft to obtain the expected value of the relative position of the group of wingmen at measurement time t.

[0089] In step S2, before the measurement time t, the aircraft's own position coordinates in the WGS84 coordinate system are calculated in real time by combining the historical position coordinates before navigation failure and the measurement information of the inertial measurement unit.

[0090] In step S3, when the measurement time t is reached, the millimeter-wave radar is used to measure the position of the wingman relative to the lead aircraft, so as to obtain the measurement value of the relative position of the wingman in the group.

[0091] In step S4, the expected and measured values ​​of the relative positions of the cluster wingmen are correlated to establish a correspondence between the wingman number and its relative position measurement value; including:

[0092] Data association algorithms (such as nearest neighbor association algorithms) are used to associate the expected and measured relative positions of the cluster wingmen.

[0093] When some wingmen fail to associate with the relative position measurement value (for example, when the cluster of drones is densely distributed, they may fail to associate due to obstruction, interference, low radar resolution, etc.), S3 is re-executed to perform the measurement.

[0094] If, after reaching the maximum number of consecutive measurements, there are still wingmen that cannot be correlated with the relative position measurement value, then the expected relative position value of this wingman is taken as its measurement value.

[0095] In step S5, the relative position measurement value and the lead aircraft's own position coordinates are sent to each wingman to calculate the wingman's own position coordinates in the WGS84 coordinate system at the wingman's location; including:

[0096] S501, Perform the transformation from relative position to the aircraft coordinate system;

[0097] Let the relative position of the wingman measured by the lead aircraft be (r, a, θ). The wingman's position in the lead aircraft's coordinate system is represented by rectangular coordinates (x, a, θ). p ,y p ,z p The expression is represented as follows:

[0098]

[0099] Where r, a, and θ represent the measured distance, elevation, and azimuth, respectively;

[0100] This embodiment of the invention assumes that the direction of the radar antenna coincides with the longitudinal axis of the aircraft nose. If they do not coincide, it is necessary to compensate for the angle between the radar antenna axis and the aircraft axis. The origin of the aircraft coordinate system is taken at the center of mass of the aircraft. The X-axis is defined as the longitudinal axis of the aircraft nose, the Y-axis is defined as the positive direction of the right wing, and the Z-axis is determined by the right-hand screw rule, pointing downwards towards the fuselage.

[0101] S502. Perform coordinate transformation from the aircraft coordinate system to the aircraft geographic coordinate system;

[0102] The wingman's rectangular coordinates (x, y) in the lead aircraft's geographic coordinate system l ,y l ,z l ) is represented as:

[0103]

[0104] Where β, ε, and γ are the yaw angle, pitch angle, and roll angle of the aircraft, respectively;

[0105] S503. Perform coordinate transformation from the aircraft's geographic coordinate system to the geocentric rectangular coordinate system;

[0106] The coordinates of the lead aircraft's position at measurement time t are known. The coordinate rotation matrix L from the aircraft's geographic coordinate system to the geocentric coordinate system ba It can be expressed as the following formula:

[0107]

[0108] Among them, L D L E These are the rotation matrices of the D and E axes of the aircraft's geographic coordinate system and the geocentric Cartesian coordinate system, respectively.

[0109] After coordinate rotation and translation, the wingman's spatial coordinates (x, y, z) in the geocentric Cartesian coordinate system are represented as follows:

[0110]

[0111] S504. Perform coordinate transformation from the geocentric rectangular coordinate system to the WGS84 coordinate system. The transformation formula is as follows:

[0112]

[0113] Since variables B and H are nested, a recursive method or an approximate direct method is used to solve the problem, so as to finally obtain the wingman's own position coordinates in the WGS84 coordinate system.

[0114] At this point, the absolute position coordinates of all swarmed UAVs, described using a unified coordinate system (WGS84 coordinate system), are finally obtained. These coordinates can be used as input to the formation controller, which then designs a control law based on the absolute position coordinates. This allows the controller to adjust the UAVs' flight parameters according to the deviation between the measured position values ​​and the desired position values, thereby enabling the swarm to maintain a specific formation geometry at a specific location.

[0115] It should be noted that if the drone swarm is still in a satellite navigation denial state after completing a full round of S1 to S5, repeat the above S1 to S5 until satellite navigation returns to normal, and then switch back to satellite navigation positioning mode.

[0116] This invention provides a clustered UAV positioning system based on millimeter-wave radar, using a lead-wingman formation mode. The system is executed by the lead aircraft and includes:

[0117] The estimation module is used to estimate the geometric topology of the swarm formation at a future measurement time t based on the set of critical path points of the planned trajectory and the swarm wingman motion model, so as to obtain the expected value of the relative position of the swarm wingmen; where the planned trajectory of each UAV refers to the flight trajectory from the take-off point to the mission target that is collaboratively planned according to the target mission.

[0118] The first calculation module is used to calculate the lead aircraft's own position coordinates in the WGS84 coordinate system in real time before the measurement time t, combining the historical position coordinates before navigation failure and the measurement information of the inertial measurement unit.

[0119] The measurement module is used to measure the wingmen of the group using millimeter-wave radar at measurement time t, and obtain the position of the wingmen relative to the leader, so as to obtain the measurement value of the relative position of the wingmen of the group.

[0120] The association module is used to associate the expected and measured values ​​of the relative positions of cluster wingmen and establish the correspondence between the wingman number and its relative position measurement value.

[0121] The second calculation module is used to send the relative position measurement value and the lead aircraft's own position coordinates to each wingman, so as to calculate the wingman's own position coordinates in the WGS84 coordinate system at the wingman's location.

[0122] This invention provides a storage medium storing a computer program for swarm drone positioning based on millimeter-wave radar, wherein the computer program causes a computer to execute the swarm drone positioning method described above.

[0123] This invention provides an electronic device, comprising:

[0124] One or more processors; a memory; and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the programs including methods for performing the clustered drone positioning method as described above.

[0125] It is understood that the millimeter-wave radar-based swarm UAV positioning system, storage medium, and electronic device provided in the embodiments of the present invention correspond to the millimeter-wave radar-based swarm UAV positioning method provided in the embodiments of the present invention. The explanations, examples, and beneficial effects of the relevant contents can be referred to the corresponding parts of the swarm UAV positioning method, and will not be repeated here.

[0126] In summary, compared with existing technologies, it has the following beneficial effects:

[0127] This invention, for the first time, applies millimeter-wave radar technology to solve the problem of UAV swarm positioning, enabling high-precision measurement of distance, azimuth, pitch, and velocity. Compared to existing wireless positioning technologies, it provides richer measurement information and higher accuracy. Simultaneously, this invention addresses the coordinate unification issue between satellite navigation positioning and swarm relative positioning, facilitating the design of formation controllers to ensure the swarm maintains a specific formation geometry at specific locations.

[0128] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0129] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for swarm UAV positioning based on millimeter-wave radar, characterized in that, Based on the lead-wingman formation mode, this method is executed by the lead aircraft and includes: S1. Based on the set of critical path points of the planned trajectory and the motion model of the wingmen, estimate the geometric topology of the swarm formation at the future measurement time t to obtain the expected value of the relative position of the wingmen; where the planned trajectory of each UAV refers to the flight trajectory from the take-off point to the mission target that is collaboratively planned according to the target mission. S2. Before the measurement time t, the aircraft's own position coordinates in the WGS84 coordinate system are calculated in real time by combining the historical position coordinates before navigation failure and the measurement information of the inertial measurement unit. S3. Upon reaching measurement time t, use millimeter-wave radar to measure the wingmen in the group, obtain the position of the wingmen relative to the lead aircraft, and obtain the measurement value of the relative position of the wingmen in the group. S4. Perform correlation processing on the expected and measured values ​​of the relative positions of the cluster wingmen to establish the correspondence between the wingman number and its relative position measurement value; S5. Send the relative position measurement value and the lead aircraft's own position coordinates to each wingman, so that the wingman's own position coordinates in the WGS84 coordinate system can be calculated at the wingman's location. S4 includes: A data association algorithm is used to associate the expected and measured relative positions of the cluster wingmen. When some wingmen cannot be associated with the relative position measurement, S3 is executed again for measurement. If, after reaching the maximum number of consecutive measurements, there are still wingmen that cannot be associated with the relative position measurement, the expected relative position of this wingman is used as its measurement value. S5 includes: S501, Perform the transformation from relative position to the aircraft coordinate system; The lead aircraft was instructed to measure the relative position of the wingman. The wingman is positioned in the lead aircraft's coordinate system using rectangular coordinates. It can be expressed as the following formula: in, These represent the measured distance, elevation, and azimuth, respectively. S502. Perform coordinate transformation from the aircraft coordinate system to the aircraft geographic coordinate system; The rectangular coordinates of the wingman in the carrier aircraft's geographic coordinate system. Represented as: in, , , These are the aircraft's yaw angle, pitch angle, and roll angle, respectively. S503. Perform coordinate transformation from the aircraft's geographic coordinate system to the geocentric rectangular coordinate system; The coordinates of the lead aircraft's position at measurement time t are known. The coordinate rotation matrix from the aircraft's geographic coordinate system to the geocentric coordinate system It can be expressed as the following formula: in, , These are the rotation matrices of the D and E axes of the aircraft's geographic coordinate system and the geocentric Cartesian coordinate system, respectively. After coordinate rotation and translation, the wingman's spatial coordinates in the geocentric Cartesian coordinate system Represented as: ； S504. Perform coordinate transformation from the geocentric rectangular coordinate system to the WGS84 coordinate system. The transformation formula is as follows: Among them, due to variables and The solutions are nested and solved using recursion or an approximate direct method to finally obtain the wingman's own position coordinates in the WGS84 coordinate system.

2. The clustered UAV positioning method as described in claim 1, characterized in that, When a satellite navigation denial occurs, the lead aircraft broadcasts a navigation failure message to the wingman, and the cluster switches to relative positioning mode to execute S1~S5; otherwise, it maintains satellite navigation positioning mode.

3. The swarm drone positioning method as described in claim 1, characterized in that, S1 includes: S101. Perform a coordinate transformation from the WGS84 coordinate system to the geocentric rectangular coordinate system. After the transformation, obtain the set of position coordinates of the lead aircraft and all wingmen in the geocentric rectangular coordinate system at measurement time t; the transformation expression is as follows: in, These represent the geodetic longitude, geodetic latitude, and geodetic height of the critical path point in WGS84 coordinates. Here are the rectangular coordinates of the critical path points in the geocentric Cartesian coordinate system; S102. Perform coordinate transformation from the geocentric rectangular coordinate system to the aircraft geographic coordinate system. After the transformation is completed, obtain the coordinate set of the lead aircraft and all wingmen at the measurement time t in the geographic coordinate system. Taking the lead aircraft as the origin of the carrier aircraft's geographic coordinate system, let its WGS84 coordinates be... The geocentric rectangular coordinates are Then the rectangular coordinates in geocentric space are The wingman's location coordinates in this geographic coordinate system The calculation formula is as follows: Wherein, coordinate transformation matrix ; S103. Based on the coordinate set, obtain the distance, bearing, and pitch of each wingman relative to the lead aircraft to obtain the expected value of the relative position of the group of wingmen at measurement time t.

4. A clustered UAV positioning system based on millimeter-wave radar, characterized in that, Based on a lead-wingman formation mode, this system is used to execute the swarm UAV positioning method as described in any one of claims 1 to 3, including: The estimation module is used to estimate the geometric topology of the swarm formation at a future measurement time t based on the set of critical path points of the planned trajectory and the swarm wingman motion model, so as to obtain the expected value of the relative position of the swarm wingmen; where the planned trajectory of each UAV refers to the flight trajectory from the take-off point to the mission target that is collaboratively planned according to the target mission. The first calculation module is used to calculate the lead aircraft's own position coordinates in the WGS84 coordinate system in real time before the measurement time t, combining the historical position coordinates before navigation failure and the measurement information of the inertial measurement unit. The measurement module is used to measure the wingmen of the group using millimeter-wave radar at measurement time t, and obtain the position of the wingmen relative to the leader, so as to obtain the measurement value of the relative position of the wingmen of the group. The association module is used to associate the expected and measured values ​​of the relative positions of cluster wingmen and establish the correspondence between the wingman number and its relative position measurement value. The second calculation module is used to send the relative position measurement value and the lead aircraft's own position coordinates to each wingman, so as to calculate the wingman's own position coordinates in the WGS84 coordinate system at the wingman's location.

5. A storage medium, characterized in that, It stores a computer program for positioning swarm drones based on millimeter-wave radar, wherein the computer program causes the computer to execute the swarm drone positioning method as described in any one of claims 1 to 3.

6. An electronic device, characterized in that, include: One or more processors; Memory; And one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the programs including methods for performing the clustered UAV positioning method as described in any one of claims 1 to 3.

Images