Multi-sensor fusion unmanned aerial vehicle positioning and attitude determination system and method

Abstract

The invention discloses a multi-sensor fusion unmanned aerial vehicle positioning and attitude determination system and method. The system deeply fuses data of a millimeter wave radar, a real-time dynamic differential positioning technology and an inertial measurement unit; the method comprises an initialization stage S1 and a loop execution stage S2, and specifically comprises the steps of S21, data acquisition and synchronization; s22, performing state prediction based on an inertial measurement unit (IMU); s23, preprocessing the measurement data; s24, state updating is carried out based on the real-time dynamic differential positioning unit RTK and the millimeter wave radar unit MMW; s25, calculating a relative position; and S26, outputting. According to the method, high-precision and high-reliability absolute positioning and attitude determination are realized through a fusion algorithm, accurate and robust relative position estimation is completed through complementation of multiple sensors, maintenance of relatively good pose estimation and real-time calculation of an accurate relative geometrical relationship between the unmanned aerial vehicle and a target, so that high-update-rate state output is completed, and real-time control requirements are met.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) application technology, and in particular to a multi-sensor fusion system and method for UAV positioning and attitude determination. Background Technology

[0002] With the development of drone technology, its application in fields such as power line inspection, surveying and mapping, and logistics is becoming increasingly widespread. At the same time, it puts forward higher requirements for the performance of navigation systems. Especially in the scenario of power transmission line inspection, drones need to fly close and accurately along the power lines in environments with strong electromagnetic interference, obstacles such as towers, and dense layers of conductors.

[0003] Currently, there are multiple technical solutions for drone navigation and positioning:

[0004] (1) GNSS: The positioning accuracy of standard GNSS is at the meter level, which cannot meet the requirements of close-range precision operations;

[0005] (2) Inertial Navigation System (INS): Based on IMU, it provides high-frequency attitude and position updates, but there is drift error that accumulates over time, and it must rely on external information for correction;

[0006] (3) RTK technology: By receiving differential correction signals, GNSS positioning accuracy can be improved to the centimeter level, providing high-precision absolute position. However, its update frequency is relatively low, and it may fail, decrease in accuracy, or fail to fix the solution in areas with severe signal obstruction such as canyons, urban buildings, dense forests, or strong interference environments.

[0007] (4) Visual / LiDAR positioning: Visual positioning is easily affected by light and weather, and has poor robustness to environments with sparse or repetitive textures. It is also costly and its performance will significantly decrease in environments such as rain, fog, and dust. It also poses a challenge for stable detection of thin cables.

[0008] (5) Millimeter-wave radar: It has good all-weather operation capability, is resistant to rain, fog, dust and light interference, and has good detection effect on metal targets such as power lines. However, traditional millimeter-wave radar is mainly used for ranging or simple obstacle avoidance. Its angular resolution is relatively limited and it is easily affected by multipath effects in complex environments. It is difficult to provide complete and high-precision absolute pose and accurate relative pose information when used alone.

[0009] When current UAV navigation and positioning solutions are applied in the power sector, their accuracy and reliability are insufficient. In complex environments, it is difficult to ensure both high accuracy and high reliability at the same time. Traditional navigation systems mainly focus on the absolute attitude of the UAV itself. For application scenarios that require precise maintenance of the relative distance and angle with specific targets, such as slender wires, such as line inspection, there is a lack of direct, accurate, and robust measurement and estimation methods. At the same time, they cannot meet the high dynamic control requirements of UAVs in flight. Summary of the Invention

[0010] To overcome the above problems, the purpose of this invention is to provide a multi-sensor fusion system and method for unmanned aerial vehicle (UAV) positioning and attitude determination. The system deeply integrates data from millimeter-wave radar (MMW), real-time dynamic differential positioning (RTK) technology, and inertial measurement unit (IMU), enabling it to simultaneously meet the requirements of high-precision absolute positioning, high-precision attitude determination, high reliability, and precise relative positioning of specific targets in complex environments. This provides key, high-quality input parameters for UAVs to perform refined operations, especially autonomous power line inspection, thereby improving operational accuracy, efficiency, and safety.

[0011] The technical solution adopted in this invention is:

[0012] A multi-sensor fusion-based unmanned aerial vehicle (UAV) localization and attitude determination system includes a millimeter-wave radar unit, a real-time dynamic differential positioning unit, an inertial measurement unit, a data synchronization unit, and a data processing and fusion unit.

[0013] The millimeter-wave radar unit transmits millimeter waves and receives the target's echo, outputting target detection information, including target range, azimuth, and elevation angle;

[0014] The real-time dynamic differential positioning unit receives GNSS signals and differential correction data, and calculates the global position, velocity and positioning status with centimeter-level accuracy;

[0015] The inertial measurement unit measures the three-axis angular velocity and three-axis acceleration of the UAV body at high frequency;

[0016] The data synchronization unit provides a unified high-precision timestamp for the data from the millimeter-wave radar unit, the real-time dynamic differential positioning unit, and the inertial measurement unit.

[0017] The data processing and fusion unit runs subsequent algorithms through the core processor and communicates with the UAV flight control system through a physical interface.

[0018] As a further description of the present invention, the physical interface of the data processing and fusion unit adopts the PSDK interface.

[0019] A method for localization and attitude determination of unmanned aerial vehicles (UAVs) applied to multi-sensor fusion includes an initialization phase S1 and a loop execution phase S2.

[0020] The initialization phase S1 includes the following:

[0021] S11: Load the pre-calibrated sensor intrinsic and extrinsic parameters;

[0022] S12: Perform initial alignment of the inertial measurement unit (IMU), and estimate the initial attitude and zero bias;

[0023] S13: Connect the real-time dynamic differential positioning unit RTK and wait for it to stabilize to obtain the initial high-precision position;

[0024] S14: Initialize the state vector X and covariance matrix Cov of the fusion filter;

[0025] The specific process of the cyclic execution phase includes:

[0026] S21: Data acquisition and synchronization, obtain the latest measurement data and corresponding timestamps of each sensor at the current moment;

[0027] S22: State prediction based on inertial measurement unit (IMU);

[0028] S23: Measurement data preprocessing;

[0029] S24: Status update based on real-time dynamic differential positioning unit RTK and millimeter-wave radar unit MMW;

[0030] S25: Relative position calculation;

[0031] S26: Output, outputs the fused high-frequency absolute state and the calculated relative position parameters to the flight control system or for data recording.

[0032] As a further description of the present invention, the fusion filter in S14 adopts EKF or UKF;

[0033] The state vector X includes position P, velocity V, attitude Att (represented by quaternion q), and IMU bias. , , represented as:

[0034] .

[0035] As a further description of the present invention, the state prediction process in S22 is as follows:

[0036] S221: Read data from the inertial measurement unit (IMU) to compensate for known deviations and calibration errors;

[0037] S222: Using the compensated angular velocity and specific force, the state after fusion from the previous moment is obtained through the inertial navigation differential equation. Predict the state at the current moment ;

[0038] S223: Predict the state covariance matrix based on the state transition model and the IMU noise model. .

[0039] As a further description of the present invention, S23 includes real-time dynamic differential positioning unit (RTK) data processing and millimeter-wave radar unit (MMW) data processing.

[0040] The process of RTK data processing by the real-time dynamic differential positioning unit is as follows:

[0041] S231: Check RTK positioning status and remove invalid data;

[0042] S232: Convert geographic coordinates to navigation coordinate system;

[0043] S233: Determine the measurement noise covariance .

[0044] The process of processing millimeter-wave radar unit (MMW) data is as follows:

[0045] S234: Filter and cluster the raw radar point cloud;

[0046] S235: Identify point clusters related to the target traverse and extract the target's measurement information. , including distance Azimuth Pitch angle ;

[0047] S236: Perform necessary tracking and multipath suppression processing on the target extracted in S235;

[0048] S237: Assess measurement confidence and determine measurement noise covariance. .

[0049] As a further description of the present invention, the state update process in S24 is as follows:

[0050] S241: Check if there is valid RTK measurement data or MMW measurement data at the current time. Available;

[0051] S242: If measurement data is available If available, construct a measurement model and establish measurement values. With predicted state Functional relationship between Calculate the Kalman gain using the EKF or UKF algorithm. Using measurement residuals Update state estimation and update the state covariance. ;

[0052] If no valid measurement data is available, skip the state update step. , .

[0053] As a further description of the present invention, the relative position calculation process of S25 is as follows:

[0054] S251: Using the latest fused absolute position and attitude of the UAV, combined with the coordinates of the target point in the radar coordinate system of the current millimeter-wave radar unit (MMW), the coordinates of the target point in the UAV's body coordinate system are calculated through coordinate transformation. and coordinates in the global coordinate system ;

[0055] S252: Calculate the key relative geometric parameters between the UAV and the target guideline.

[0056] As a further description of the present invention, the relative geometric parameters in S252 include lateral horizontal distance, vertical height difference, line-of-sight distance, and relative observation angle.

[0057] As a further description of the present invention, the multi-sensor fusion data calculation process includes a world coordinate system W, a UAV body coordinate system B, a radar coordinate system R, and an IMU coordinate system I.

[0058] The origin of the world coordinate system W is set at a fixed point around the mission area for RTK output and final absolute position / velocity representation;

[0059] The origin of the UAV body coordinate system B is the UAV's centroid, used for inertial measurement unit (IMU) measurements to represent the UAV's attitude.

[0060] The origin of the IMU coordinate system I is the center position of the IMU sensor, and it is rotated relative to the UAV body coordinate system B through a pre-calibrated rotation matrix. Translation vector Fixed association;

[0061] The origin of the radar coordinate system R is the phase center position of the radar antenna, used to represent the original measurement values ​​of the millimeter-wave radar unit, and is connected to the UAV body coordinate system B via a pre-calibrated rotation matrix. Translation vector Fixed association.

[0062] The beneficial effects of this invention are:

[0063] This invention relates to a multi-sensor fusion system for unmanned aerial vehicle (UAV) positioning and attitude determination. The system deeply integrates data from millimeter-wave radar (MMW), real-time dynamic differential positioning (RTK) technology, and inertial measurement unit (IMU). This enables the system to simultaneously meet the requirements of high-precision absolute positioning, high-precision attitude determination, high reliability, and precise relative positioning of specific targets in complex environments. It provides key, high-quality input parameters for UAVs to perform refined operations, especially autonomous power line inspection, thereby improving operational accuracy, efficiency, and safety.

[0064] This invention discloses a method for multi-sensor fusion-based unmanned aerial vehicle (UAV) localization and attitude determination. This method utilizes optimized fusion algorithms, such as Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF), to tightly integrate high-frequency dynamic information from the Inertial Measurement Unit (IMU), centimeter-level absolute position information from Real-Time Kinematic (RTK) technology, and precise relative ranging capabilities from millimeter-wave radar (MMW). This is particularly effective for targets such as power lines in power applications, achieving high-precision and high-reliability absolute positioning and attitude determination. Even during brief RTK interruptions or radar interference, the complementary use of multiple sensors maintains good pose estimation. Simultaneously, by directly measuring with the MMW radar and combining it with the UAV's precise pose, the method calculates the accurate relative geometric relationship between the UAV and the target in real time, achieving accurate and robust relative position estimation. The fused state estimation, including pose and relative position, can be output at a high frequency, resulting in a high update rate state output that meets real-time control requirements. Attached Figure Description

[0065] Figure 1 This is a structural diagram of the multi-sensor fusion unmanned aerial vehicle localization and attitude determination system proposed in this invention.

[0066] Figure 2 This is an overall flowchart of the method for localization and attitude determination of unmanned aerial vehicles applied to multi-sensor fusion proposed in this invention;

[0067] Figure 3 The flowchart for S22, the method for localization and attitude determination of unmanned aerial vehicles proposed in this invention, is shown below.

[0068] Figure 4 This is a flowchart illustrating the S23 RTK data processing method for a multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system proposed in this invention.

[0069] Figure 5 The following is a flowchart of the S23 MMW data processing method for the localization and attitude determination system of unmanned aerial vehicles applied to multi-sensor fusion proposed in this invention;

[0070] Figure 6Here is the S24 state update flowchart of the method for localization and attitude determination system of unmanned aerial vehicles applied to multi-sensor fusion proposed in this invention;

[0071] Figure 7 This is a schematic diagram of coordinate transformation in embodiment four of the method proposed in this invention for the localization and attitude determination system of unmanned aerial vehicles using multi-sensor fusion. Detailed Implementation

[0072] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the protection scope of the present invention.

[0073] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0074] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0075] This invention is described in detail with reference to the schematic diagrams. When detailing the embodiments of this invention, for ease of explanation, the cross-sectional views illustrating the device structure may be partially enlarged, not adhering to the usual scale. Furthermore, the schematic diagrams are merely examples and should not be construed as limiting the scope of protection of this invention. In actual fabrication, the three-dimensional spatial dimensions of length, width, and depth should be included.

[0076] Furthermore, in the description of this invention, it should be noted that the terms "upper," "lower," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. These terms are used solely for the convenience of describing the invention and for simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the invention. In addition, the terms "first," "second," or "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0077] Unless otherwise explicitly specified and limited, the terms "installation," "connection," and "joining" in this invention should be interpreted broadly. For example, they can refer to fixed connections, detachable connections, or integral connections; similarly, they can refer to mechanical connections, electrical connections, or direct connections, or indirect connections through an intermediate medium, or internal connections between two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0078] like Figures 1-7 As shown, it illustrates a specific embodiment of the present invention:

[0079] Example 1

[0080] The multi-sensor fusion-based unmanned aerial vehicle (UAV) localization and attitude determination system includes a millimeter-wave radar (MMW) unit, a real-time dynamic differential positioning (RTK) unit, an inertial measurement unit (IMU), a data synchronization unit, and a data processing and fusion unit.

[0081] The millimeter-wave radar unit (MMW) transmits millimeter waves and receives the echoes from the target. In actual power application scenarios, the targets are mainly power lines and ground wires. It outputs target detection information, including target distance, azimuth angle, and elevation angle.

[0082] The real-time dynamic differential positioning unit (RTK) receives GNSS signals and differential correction data, and calculates the global position, velocity, and positioning status with centimeter-level accuracy.

[0083] The inertial measurement unit (IMU) measures the three-axis angular velocity and three-axis acceleration of the UAV body at high frequency;

[0084] The data synchronization unit provides a unified high-precision timestamp for the data from the millimeter-wave radar unit (MMW), the real-time dynamic differential positioning unit (RTK), and the inertial measurement unit (IMU).

[0085] The data processing and fusion unit runs subsequent algorithms through the core processor and communicates with the UAV flight control system through a physical interface.

[0086] Specifically, the physical interface of the data processing and fusion unit adopts the PSDK interface.

[0087] In this embodiment, as Figure 1 As shown, the system deeply integrates data from millimeter-wave radar (MMW), real-time dynamic differential positioning (RTK) technology, and inertial measurement unit (IMU) to simultaneously meet the requirements of high-precision absolute positioning, high-precision attitude determination, high reliability, and precise relative positioning of specific targets in complex environments. This provides key, high-quality input parameters for UAVs to perform refined operations, especially autonomous power line line inspection, thereby improving operational accuracy, efficiency, and safety.

[0088] Example 2

[0089] A method for localization and attitude determination of unmanned aerial vehicles (UAVs) applied to multi-sensor fusion includes an initialization phase S1 and a loop execution phase S2.

[0090] The initialization phase S1 includes the following:

[0091] S11: Load the pre-calibrated sensor intrinsic and extrinsic parameters;

[0092] S12: Perform initial alignment of the inertial measurement unit (IMU), and estimate the initial attitude and zero bias;

[0093] S13: Connect the real-time dynamic differential positioning unit RTK and wait for it to stabilize to obtain the initial high-precision position;

[0094] S14: Initialize the state vector X and covariance matrix Cov of the fusion filter;

[0095] The specific process of the cyclic execution phase includes:

[0096] S21: Data acquisition and synchronization, obtain the latest measurement data and corresponding timestamps of each sensor at the current moment;

[0097] S22: State prediction based on inertial measurement unit (IMU);

[0098] S23: Measurement data preprocessing;

[0099] S24: Status update based on real-time dynamic differential positioning unit RTK and millimeter-wave radar unit MMW;

[0100] S25: Relative position calculation;

[0101] S26: Output, outputs the fused high-frequency absolute state and the calculated relative position parameters to the flight control system or for data recording.

[0102] Specifically, the fusion filter in S14 adopts EKF or UKF;

[0103] The state vector X includes position P, velocity V, attitude Att (represented by quaternion q), and IMU bias. , , represented as:

[0104] .

[0105] In this embodiment, as Figure 2As shown, this method, through the design of optimized fusion algorithms such as Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF), tightly integrates the high-frequency dynamic information of the Inertial Measurement Unit (IMU), the centimeter-level absolute position information of Real-Time Kinematic (RTK) technology, and the precise relative ranging capability of the Millimeter-Wave Radar (MMW). Especially for targets such as power lines in power application scenarios, it achieves high-precision and high-reliability absolute positioning and attitude determination. Even when the RTK is briefly interrupted or the radar is interfered with, it can maintain good pose estimation through multi-sensor complementarity. Simultaneously, by directly measuring with the MMW radar and combining it with its precise own pose, it calculates the precise relative geometric relationship between the UAV and the target in real time, completing accurate and robust relative position estimation. The fused state estimation, including pose and relative position, can be output at a high frequency, thus achieving a high update rate state output to meet real-time control requirements.

[0106] In this embodiment, in addition to EKF and UKF, higher-order filtering methods such as volumetric Kalman filter (CKF) and quadrature Kalman filter (QKF) can be used to obtain better nonlinear processing capabilities in certain situations. For strong non-Gaussian noise or situations requiring multimodal estimation, particle filters can be considered, but the computational complexity needs to be weighed.

[0107] In this embodiment, the extrinsic parameters of the sensors, such as the installation deviation of the IMU and radar relative to the UAV body, can also be included in the state vector for online self-calibration. Alternatively, the model parameters of the target guideline, such as the catenary parameters and local direction vector, can be included in the state vector for online estimation, reducing the dependence on prior maps.

[0108] Example 3

[0109] Based on the above embodiment 2, the process of each step in the cyclic execution phase S2 of the method will be further explained.

[0110] Specifically, the state prediction process in S22 is as follows:

[0111] S221: Read data from the inertial measurement unit (IMU) to compensate for known deviations and calibration errors;

[0112] S222: Using the compensated angular velocity and specific force, the state after fusion from the previous moment is obtained through the inertial navigation differential equation. Predict the state at the current moment ;

[0113] S223: Predict the state covariance matrix based on the state transition model and the IMU noise model. .

[0114] In this embodiment, as Figure 3 As shown, this process uses data from the inertial measurement unit (IMU) to perform inertial navigation calculations. The inertial navigation differential equation used is a well-known model that considers the Earth's rotation and gravity, thereby predicting the UAV's state, including its position, velocity, attitude, and uncertainties. The uncertainties are represented by covariance.

[0115] Specifically, S23 includes real-time dynamic differential positioning unit (RTK) data processing and millimeter-wave radar unit (MMW) data processing;

[0116] The process of RTK data processing by the real-time dynamic differential positioning unit is as follows:

[0117] S231: Check RTK positioning status and remove invalid data;

[0118] S232: Convert geographic coordinates to navigation coordinate system;

[0119] S233: Determine the measurement noise covariance .

[0120] In this embodiment, as Figure 4 As shown, this data processing procedure describes how to determine whether data is available based on the positioning status of the Dynamic Differential Positioning Unit (RTK) and how to set its weight in the fusion, i.e., through noise covariance.

[0121] The process of processing millimeter-wave radar unit (MMW) data is as follows:

[0122] S234: Filter and cluster the raw radar point cloud;

[0123] S235: Identify point clusters related to the target traverse and extract the target's measurement information. , including distance Azimuth Pitch angle ;

[0124] S236: Perform necessary tracking and multipath suppression processing on the target extracted in S235;

[0125] S237: Assess measurement confidence and determine measurement noise covariance. .

[0126] In this embodiment, as Figure 5 As shown, the purpose of this data processing is to extract reliable target measurement information from the point cloud for fusion.

[0127] Specifically, the state update process in S24 is as follows:

[0128] S241: Check if there is valid RTK measurement data or MMW measurement data at the current time. Available;

[0129] S242: If measurement data is available If available, construct a measurement model and establish measurement values. With predicted state Functional relationship between Calculate the Kalman gain using the EKF or UKF algorithm. Using measurement residuals Update state estimation and update the state covariance. ;

[0130] If no valid measurement data is available, skip the state update step. , .

[0131] In this embodiment, the measurement model of the Real-Time Dynamic Differential Positioning (RTK) unit is typically linear, meaning there is a linear relationship between the observed position and velocity. The measurement model of the millimeter-wave radar (MMW) unit is non-linear and requires prediction of the UAV pose. Radar installation parameters And the position information of the target guideline (from prediction) to calculate the theoretical radar measurement value. .

[0132] In this embodiment, as Figure 6 As shown, this process demonstrates how to use effective sensor measurements to correct the state predicted by the IMU, resulting in more accurate fusion results.

[0133] Specifically, the relative position calculation process of S25 is as follows:

[0134] S251: Using the latest fused absolute position and attitude of the UAV, combined with the coordinates of the target point in the radar coordinate system of the current millimeter-wave radar unit (MMW), the coordinates of the target point in the UAV's body coordinate system are calculated through coordinate transformation. and coordinates in the global coordinate system ;

[0135] S252: Calculate the key relative geometric parameters between the UAV and the target guideline.

[0136] Specifically, the relative geometric parameters in S252 include lateral horizontal distance, vertical height difference, line-of-sight distance, and relative observation angle.

[0137] Example 4

[0138] In the specific implementation of Embodiments 2 and 3 above, coordinate transformation is required to obtain accurate state information of the UAV. Therefore, Embodiment 4 describes the coordinate transformation, such as... Figure 7 As shown.

[0139] Specifically, the multi-sensor fusion data calculation process includes the world coordinate system W, the UAV body coordinate system B, the radar coordinate system R, and the IMU coordinate system I.

[0140] The origin of the world coordinate system W is set at a fixed point around the mission area, used for RTK output and final absolute position / velocity representation.

[0141] In this embodiment, the world coordinate system W can be represented as NED (North-East-Down) or ENU (East-North-Up), where the axis is set as follows:

[0142] NED: X-axis points north, Y-axis points east, and Z-axis points to the Earth's center.

[0143] ENU: X-axis points east, Y-axis points north, and Z-axis points vertically upward.

[0144] The origin of the UAV body coordinate system B is the UAV's centroid, used for inertial measurement unit (IMU) measurements to represent the UAV's attitude.

[0145] In this embodiment, the coordinate system B of the UAV body is set as follows: the X-axis points forward along the nose, the Y-axis points to the right along the right wing, and the Z-axis points downward perpendicular to the fuselage.

[0146] The origin of the IMU coordinate system I is the center position of the IMU sensor, and it is rotated relative to the UAV body coordinate system B through a pre-calibrated rotation matrix. Translation vector Fixed association;

[0147] In this embodiment, the I-axis of the IMU coordinate system is defined by the IMU sensitive axis.

[0148] The origin of the radar coordinate system R is the phase center position of the radar antenna, used to represent the original measurement values ​​of the millimeter-wave radar unit, and is connected to the UAV body coordinate system B via a pre-calibrated rotation matrix. Translation vector Fixed association.

[0149] In this embodiment, the radar coordinate system R-axis is set as follows: the X-axis points to the center of the radar beam, i.e., the forward position, and the Y / Z axes are determined according to the right-hand rule.

[0150] The transformation relationship between the above coordinate systems is as follows: the point or vector transformation between each coordinate system needs to be completed using the corresponding rotation matrix (based on the UAV attitude q or calibration parameters) and translation vector.

[0151] For example, from a point in radar coordinate system R Points transformed to world coordinate system W :

[0152] ,

[0153] in: It refers to the location of the drone within the W series. It is a rotation matrix from the B system to the W system.

[0154] In summary, according to Examples 2, 3, and 4, this method proposes a tightly coupled fusion method based on Kalman filtering. Instead of simply post-processing or selectively using sensor data, it simultaneously estimates the absolute pose, velocity, and IMU deviation of the UAV in a unified state vector, and uses the measurements from RTK and MMW radar to jointly correct the state prediction, thus possessing a tightly coupled fusion framework.

[0155] To address the nonlinear measurement characteristics of MMW radar, an accurate measurement model was established. This model correlates the predicted UAV state with radar measurements and takes into account sensor installation errors. It effectively integrates the relative measurement information of the radar into the absolute pose estimation and is used for accurate relative positioning.

[0156] Through fusion, RTK can effectively correct long-term IMU drift, IMU high-frequency data can fill the measurement gap between RTK and radar and improve the output frequency, MMW radar can provide important external observation information when RTK fails, especially relative position and attitude constraints, and enhance the system's availability in adverse weather conditions. The fusion algorithm can also estimate and compensate for IMU random errors, such as zero bias, online. By using the high-precision self-pose after fusion and the processed radar measurements, high-precision, high-frequency relative position parameters of the target can be calculated, achieving accurate relative positioning.

[0157] The fusion algorithm of this method dynamically adjusts the corresponding measurement noise covariance matrix based on the real-time positioning status of RTK and the confidence level of radar measurement, thereby improving the robustness and accuracy of the fusion system.

[0158] In this embodiment, a multi-rate fusion strategy can be adopted to process the differences in update frequency of different sensor data more precisely, and more complex fault detection and isolation logic can be designed to identify and process abnormal sensor data.

[0159] In this embodiment, visual or LiDAR information is further fused on the basis of MMW+RTK+IMU. For example, visual assistance is used for wire recognition, and LiDAR provides a denser scene point cloud to form a more powerful multimodal fusion system.

[0160] The preferred embodiments of the present invention have been described in detail above with reference to the accompanying drawings. However, the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those skilled in the art, various changes can be made without departing from the spirit of the present invention.

[0161] Many other changes and modifications can be made without departing from the concept and scope of this invention. It should be understood that this invention is not limited to the specific embodiments, and the scope of this invention is defined by the appended claims.

Claims

1. A multi-sensor fusion-based unmanned aerial vehicle (UAV) localization and attitude determination system, characterized in that, It includes a millimeter-wave radar unit, a real-time dynamic differential positioning unit, an inertial measurement unit, a data synchronization unit, and a data processing and fusion unit. The millimeter-wave radar unit transmits millimeter waves and receives the target's echo, outputting target detection information, including target range, azimuth, and elevation angle; The real-time dynamic differential positioning unit receives GNSS signals and differential correction data, and calculates the global position, velocity and positioning status with centimeter-level accuracy; The inertial measurement unit measures the three-axis angular velocity and three-axis acceleration of the UAV body at high frequency; The data synchronization unit provides a unified high-precision timestamp for the data from the millimeter-wave radar unit, the real-time dynamic differential positioning unit, and the inertial measurement unit. The data processing and fusion unit runs subsequent algorithms through the core processor and communicates with the UAV flight control system through a physical interface.

2. The multi-sensor fusion unmanned aerial vehicle positioning and attitude determination system according to claim 1, characterized in that, The physical interface of the data processing and fusion unit adopts the PSDK interface.

3. A method applied to the multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to any one of claims 1-2, characterized in that, It includes an initialization phase S1 and a loop execution phase S2. The initialization phase S1 includes the following: S11: Load the pre-calibrated sensor intrinsic and extrinsic parameters; S12: Perform initial alignment of the inertial measurement unit (IMU), and estimate the initial attitude and zero bias; S13: Connect the real-time dynamic differential positioning unit RTK and wait for it to stabilize to obtain the initial high-precision position; S14: Initialize the state vector X and covariance matrix Cov of the fusion filter; The specific process of the cyclic execution phase includes: S21: Data acquisition and synchronization, obtain the latest measurement data and corresponding timestamps of each sensor at the current moment; S22: State prediction based on inertial measurement unit (IMU); S23: Measurement data preprocessing; S24: Status update based on real-time dynamic differential positioning unit RTK and millimeter-wave radar unit MMW; S25: Relative position calculation; S26: Output, outputs the fused high-frequency absolute state and the calculated relative position parameters to the flight control system or for data recording.

4. The method for multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to claim 3, characterized in that, The fusion filter in S14 uses EKF or UKF; The state vector X includes position P, velocity V, attitude Att (represented by quaternion q), and IMU bias. , , represented as: 。 5. The method for multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to claim 3, characterized in that, The state prediction process in S22 is as follows: S221: Read data from the inertial measurement unit (IMU) to compensate for known deviations and calibration errors; S222: Using the compensated angular velocity and specific force, the state after fusion from the previous moment is obtained through the inertial navigation differential equation. Predict the state at the current moment ; S223: Predict the state covariance matrix based on the state transition model and the IMU noise model. .

6. The method for multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to claim 3, characterized in that, S23 includes real-time dynamic differential positioning unit (RTK) data processing and millimeter-wave radar unit (MMW) data processing. The process of RTK data processing by the real-time dynamic differential positioning unit is as follows: S231: Check RTK positioning status and remove invalid data; S232: Convert geographic coordinates to navigation coordinate system; S233: Determine the measurement noise covariance ; The process of processing millimeter-wave radar unit (MMW) data is as follows: S234: Filter and cluster the raw radar point cloud; S235: Identify point clusters related to the target traverse and extract the target's measurement information. , including distance Azimuth Pitch angle ; S236: Perform necessary tracking and multipath suppression processing on the target extracted in S235; S237: Assess measurement confidence and determine measurement noise covariance. .

7. The method for multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to claim 3, characterized in that, The state update process in S24 is as follows: S241: Check if there is valid RTK measurement data or MMW measurement data at the current time. Available; S242: If measurement data is available If available, construct a measurement model and establish measurement values. With predicted state Functional relationship between Calculate the Kalman gain using the EKF or UKF algorithm. Using measurement residuals Update state estimation and update the state covariance. ; If no valid measurement data is available, skip the state update step. , .

8. The method for multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to claim 3, characterized in that, The relative position calculation process for S25 is as follows: S251: Using the latest fused absolute position and attitude of the UAV, combined with the coordinates of the target point in the radar coordinate system of the current millimeter-wave radar unit (MMW), the coordinates of the target point in the UAV's body coordinate system are calculated through coordinate transformation. and coordinates in the global coordinate system ; S252: Calculate the key relative geometric parameters between the UAV and the target guideline.

9. The method for multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to claim 8, characterized in that, The relative geometric parameters in S252 include lateral horizontal distance, vertical height difference, line-of-sight distance, and relative observation angle.

10. The method for multi-sensor fusion-based unmanned aerial vehicle localization and attitude determination system according to claim 3, characterized in that, The multi-sensor fusion data calculation process includes the world coordinate system W, the UAV body coordinate system B, the radar coordinate system R, and the IMU coordinate system I. The origin of the world coordinate system W is set at a fixed point around the mission area for RTK output and final absolute position / velocity representation; The origin of the UAV body coordinate system B is the UAV's centroid, used for inertial measurement unit (IMU) measurements to represent the UAV's attitude. The origin of the IMU coordinate system I is the center position of the IMU sensor, and it is rotated relative to the UAV body coordinate system B through a pre-calibrated rotation matrix. Translation vector Fixed association; The origin of the radar coordinate system R is the phase center position of the radar antenna, used to represent the original measurement values ​​of the millimeter-wave radar unit, and is connected to the UAV body coordinate system B via a pre-calibrated rotation matrix. Translation vector Fixed association.

Images