Unmanned aerial vehicle positioning and dynamic obstacle avoidance system and method based on multi-sensor fusion

Abstract

The invention relates to the technical field of three-dimensional positions, in particular to an unmanned aerial vehicle positioning and dynamic obstacle avoidance system and method based on multi-sensor fusion, and the system comprises a multi-source fusion module, an error optimization module, an environment modeling module, a time sequence analysis module and a dynamic planning module. According to the invention, through synchronous fusion of multi-source data and integration of visual images, laser point cloud and IMU data, feature information can be effectively extracted and pose positioning can be optimized in a complex environment, the problem of insufficient adaptability of a traditional system in a dynamic environment is solved, and motion errors and positioning errors of visual and point cloud data are jointly analyzed through error optimization, so that the accuracy of the system is improved. And the positioning precision and the system robustness are improved. And environment modeling and time sequence analysis are further combined, so that the motion state of the obstacle can be detected in real time, the flight path priority can be adjusted, the obstacle can be dynamically avoided, the response speed and accuracy of navigation and obstacle avoidance are improved, and stable operation and safety of the aircraft in a changing environment are ensured.

Description

Technical Field

[0001] This invention relates to the field of three-dimensional positioning technology, and in particular to a UAV positioning and dynamic obstacle avoidance system and method based on multi-sensor fusion. Background Technology

[0002] The field of 3D positioning technology involves the detection, control, and adjustment of the position and attitude of objects in space. It primarily studies how to achieve stable positioning and motion control of aircraft, robots, and vehicles in 3D space through measurement and control methods. Core aspects include spatial coordinate measurement, heading angle and attitude calculation, trajectory planning, and adaptive control in dynamic environments. Its applications are wide-ranging, covering aerospace, intelligent unmanned systems, autonomous driving, and robot control. It typically relies on inertial measurement units, satellite navigation systems, visual ranging sensors, radar ranging, and fusion algorithms to construct a complete spatial positioning and control system.

[0003] Traditional UAV positioning and dynamic obstacle avoidance systems refer to systems that determine the UAV's flight path and avoid obstacles using a single sensor. They typically rely on global positioning signals to obtain the UAV's latitude, longitude, and altitude information, and combine this with gyroscopes and accelerometers to calculate attitude parameters. During obstacle avoidance, infrared or ultrasonic ranging devices are used to detect the distance to obstacles ahead, and then course corrections or speed adjustments are triggered based on preset thresholds to avoid obstacles. In complex environments, these systems usually achieve navigation and obstacle avoidance control through preset path planning and fixed threshold judgment methods.

[0004] Current technologies rely on a single sensor to acquire position information and use preset path planning and fixed thresholds for obstacle avoidance. This approach is susceptible to dynamic changes in complex environments and struggles to cope with non-static obstacles or sudden changes in interference signals, leading to reduced positioning and obstacle avoidance accuracy in changing environments. Furthermore, traditional positioning systems largely depend on satellite positioning and inertial measurement units for attitude calculation, failing to effectively integrate multi-source data. This results in poor adaptability in dynamic environments, especially in situations with strong signal interference or blind spots, making it difficult for the positioning and control system to maintain stable operation, thus limiting the aircraft's safety and control capabilities. Summary of the Invention

[0005] To address the technical problems existing in the prior art, embodiments of the present invention provide a UAV positioning and dynamic obstacle avoidance system and method based on multi-sensor fusion. The technical solution is as follows: On the one hand, a UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion is provided, which includes: The multi-source fusion module acquires visual images, laser point clouds, and IMU data, extracts visual features and point cloud features, calculates pre-integrated motion, and fuses all data synchronously over time to generate multi-source fusion data, which is then transmitted to the error optimization module. The error optimization module calculates visual errors based on the multi-source fusion data, analyzes motion errors based on pre-integrated motion, jointly optimizes the two types of errors, generates a pose localization set, and transmits it to the environment modeling module. The environment modeling module, based on the pose localization set, divides the local environment into grids and calculates the centroid change of the point cloud. It combines the obstacle displacement threshold to determine the speed and direction of the obstacle, generates local environment data, and transmits it to the time series analysis module. The time series analysis module acquires all sensor signals based on the local environmental data and performs convolutional analysis on the energy change amplitude, calculates the signal stability over multiple time periods, generates a time series interference table, and transmits it to the dynamic programming module. The dynamic programming module, based on the time-series interference table, obtains a set of candidate tracks for track planning requirements analysis, assesses the collision risk of multiple tracks, adjusts track priorities in conjunction with signal stability, and generates dynamic obstacle avoidance results.

[0006] As a further aspect of the present invention, the multi-source fusion data includes visual images, laser point clouds, and IMU data; the pose localization set includes visual errors, point cloud feature errors, pre-integrated motion errors, and joint optimization results; the local environment data includes grid division, point cloud centroid changes, obstacle velocities, and obstacle displacement directions; the temporal interference table includes multi-source sensor signals, energy change amplitude, and signal stability; and the dynamic obstacle avoidance results include candidate trajectory sets, collision risk assessment, and trajectory priority adjustment results.

[0007] As a further aspect of the present invention, the multi-source fusion module specifically comprises: The data analysis submodule collects image frames from the visual sensor, point cloud echoes from the LiDAR, and acceleration and angular velocity sequences from the IMU. It calculates the difference between the visual frame interval and the IMU sampling period based on the same time axis, synchronizes and aligns the start times of multiple data sources, and generates a multi-source synchronized dataset. The feature extraction submodule extracts pixel gradient distribution and corner coordinates from image frames of the multi-source synchronous dataset, calculates the gray-level change rate as a visual feature vector, extracts the reflection intensity and coordinates of point cloud data to calculate the spatial density gradient as a point cloud feature vector, and obtains cross-modal feature data by corresponding the two types of features. The time synchronization submodule calls the cross-modal feature data and IMU acceleration and angular velocity sequences, calculates the pre-integrated motion between adjacent time points and corrects the time drift error, analyzes the weighted translation and attitude change rate, performs temporal weighted fusion of visual and point cloud features, and generates multi-source fusion data.

[0008] As a further aspect of the present invention, the error optimization module specifically comprises: The visual error submodule extracts the coordinates of feature points in adjacent frames and calculates the spatial difference between corresponding points based on the spatial matching relationship between visual features and point cloud features in the multi-source fusion data. It analyzes the visual residuals, performs a weighted average of the residuals of each set of matching points, and generates visual error results. The motion error submodule calls the visual error result, obtains the pre-integrated motion amount in the multi-source fusion data and performs time accumulation, analyzes the deviation from the pose change amount of adjacent frames to calculate the motion deviation ratio, corrects the abnormal integral and recalculates the accumulated deviation in combination with the pose change reference value, and generates motion error coefficients. The joint optimization submodule calculates the weight ratio of the two types of errors in the time and spatial domains based on the visual error results and the motion error coefficients. It adjusts the covariance of multiple error terms according to the weight ratio, performs weighted error vector iterative convergence operation, selects the state vector set with the best error, and generates a pose localization set.

[0009] As a further aspect of the present invention, the attitude change reference value is obtained by extracting the attitude angle change sequence within multiple time windows, calculating the mean angular velocity and mean angular acceleration within each time window, and analyzing the time integration results of the two.

[0010] As a further aspect of the present invention, the environment modeling module specifically comprises: The grid division submodule extracts the spatial coordinates of continuous time periods based on the pose positioning set, analyzes the three-dimensional grid according to the distribution of coordinate points in the local space, calculates the point cloud number of multiple grids and filters out invalid empty grids, determines the boundary of the local area according to the point cloud density difference between adjacent grids, and generates local grid distribution data. The centroid calculation submodule calls the local grid distribution data and the point cloud coordinates in the pose positioning set to calculate the mean of spatial coordinates, analyze the geometric center position of a single grid, compare the spatial offset of the centroid coordinates of the same grid in consecutive frames, analyze the centroid change vector of each grid, and generate the grid centroid change result. The obstacle determination submodule calculates the ratio of the centroid offset of adjacent frames to the time interval based on the grid centroid change results. It determines the obstacle's motion state based on the difference between the time displacement rate and the obstacle displacement threshold, extracts the offset direction vector, calculates the motion velocity distribution, and generates local environmental data.

[0011] As a further aspect of the present invention, the obstacle displacement threshold is obtained by extracting point clouds that have not undergone significant motion changes in multiple frames, calculating the time series difference of centroid coordinates, obtaining the displacement distribution range of the static region, and then normalizing the range and extracting the upper limit of the offset stable interval.

[0012] As a further aspect of the present invention, the time series analysis module specifically comprises: The signal acquisition submodule acquires the time series values ​​of all sensor signals based on the local environmental data, extracts the amplitude and frequency distribution of multiple signals within the same time window and synchronizes them in time, calculates the difference in signal amplitude at adjacent times and records the rate of change, and generates a multi-source signal difference set. The convolution calculation submodule calls the multi-source signal difference set, performs convolution operation on the change sequence of multi-sensor signals in multiple time periods, calculates the energy sum of the convolution result and extracts the energy change interval, analyzes the energy fluctuation rate as the energy transfer feature between signals, and generates the energy fluctuation rate distribution. The stability assessment submodule calculates the standard deviation of signal volatility over multiple time periods based on the energy volatility distribution and compares it with the stability threshold. It extracts the time of the stable interval, calculates the average value of signal volatility over multiple intervals and calculates the trend of change, and generates a time-series interference table. The stability threshold is set by collecting signal sequences during steady-state operation, calculating the mean and variance of the volatility of multiple signals, and then setting the threshold based on the variance range.

[0013] As a further aspect of the present invention, the dynamic programming module specifically comprises: The requirement analysis submodule extracts the trajectory planning requirement parameters within the target area based on the time-series interference table, filters the path data that meets the trajectory length and heading restrictions, counts the turning angle change value of each path and records the path node spacing, and obtains the trajectory feature parameter set. The risk assessment submodule calls the set of trajectory feature parameters, performs distance comparison on the node spacing of any two paths, filters path combinations with intersection risks, counts the number of intersection nodes and records the node spacing value, calculates the path collision risk index, and generates collision risk assessment results. The priority adjustment submodule sorts and calculates the priority parameters of the multipath based on the collision risk assessment results, classifies them into risk levels according to the collision risk value, and corrects the priority based on the signal stability value to generate dynamic obstacle avoidance results.

[0014] On the other hand, the UAV localization and dynamic obstacle avoidance method based on multi-sensor fusion, which is executed based on the aforementioned UAV localization and dynamic obstacle avoidance system based on multi-sensor fusion, includes the following steps: S1: Acquire visual images, laser point clouds and IMU data, extract visual features and point cloud features and calculate pre-integrated motion, fuse all data synchronously in time, and generate multi-source fused data; S2: Based on the multi-source fusion data, calculate the visual error of visual features and point cloud features, analyze the motion error of pre-integrated motion, jointly optimize the two types of errors, and generate a pose localization set. S3: Based on the pose localization set, the local environment is divided into grids and the centroid change of the point cloud is calculated. The speed and direction of the obstacle are determined by combining the obstacle displacement threshold, and local environment data is generated. S4: Based on the local environmental data, acquire all sensor signals and perform convolution analysis on the energy change amplitude, calculate the signal stability over multiple time periods, and generate a time-series interference table; S5: Based on the time-series interference table, obtain the candidate trajectory set for trajectory planning requirement analysis, assess the collision risk of multiple trajectories, adjust the trajectory priority in combination with signal stability, and generate dynamic obstacle avoidance results.

[0015] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following: By synchronously fusing multi-source data, integrating visual images, laser point clouds, and IMU data, feature information can be effectively extracted and pose localization optimized in complex environments. This addresses the insufficient adaptability of traditional systems in dynamic environments. Through error optimization and joint analysis of motion and localization errors in visual and point cloud data, positioning accuracy and system robustness are improved. Furthermore, by combining environmental modeling and temporal analysis, the motion state of obstacles can be detected in real time, and track priorities can be adjusted to dynamically avoid obstacles. This improves the reaction speed and accuracy of navigation and obstacle avoidance, ensuring the stable operation and safety of the aircraft in changing environments. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying 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.

[0017] Figure 1 This is a system schematic diagram of the present invention; Figure 2 This is a schematic diagram of the system framework of the present invention; Figure 3 This is a flowchart of the multi-source fusion module in this invention; Figure 4 This is a flowchart of the error optimization module in this invention; Figure 5 This is a flowchart of the environment modeling module in this invention; Figure 6 This is a flowchart of the timing analysis module in this invention; Figure 7This is a flowchart of the dynamic programming module in this invention; Figure 8 This is a flowchart of the method of the present invention. Detailed Implementation

[0018] The technical solution of the present invention will now be described with reference to the accompanying drawings.

[0019] In embodiments of the present invention, words such as "exemplarily," "for example," etc., are used to indicate that something is an example, illustration, or description. Any embodiment or design described as "exemplary" in the present invention should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of the word "exemplary" is intended to present the concept in a concrete manner. Furthermore, in embodiments of the present invention, the meaning expressed by "and / or" can be both, or either one.

[0020] In the embodiments of this invention, the terms "image" and "picture" may sometimes be used interchangeably. It should be noted that, without emphasizing the distinction between them, they convey the same meaning. Similarly, the terms "of," "corresponding (relevant)," and "corresponding" may sometimes be used interchangeably. It should be noted that, without emphasizing the distinction between them, they convey the same meaning.

[0021] In this embodiment of the invention, sometimes a subscript such as W1 may be written in a non-subscript form such as W1. When the difference is not emphasized, the meaning they express is the same.

[0022] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0023] This invention provides a UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion, such as... Figure 1-2 The diagram shown illustrates a UAV localization and dynamic obstacle avoidance system based on multi-sensor fusion. This system includes: The multi-source fusion module acquires visual images, laser point clouds, and IMU data, extracts visual features and point cloud features, calculates pre-integrated motion, and fuses all data synchronously over time to generate multi-source fusion data, which is then transmitted to the error optimization module. The error optimization module calculates visual errors based on multi-source fusion data, calculates motion errors based on visual features and point cloud features, analyzes motion errors based on pre-integrated motion, jointly optimizes the two types of errors, generates a pose localization set, and transmits it to the environment modeling module. The environment modeling module, based on the pose localization set, divides the local environment into grids and calculates the centroid changes of the point cloud. It combines the obstacle displacement threshold to determine the speed and direction of the obstacle, generates local environment data, and transmits it to the time series analysis module. The time series analysis module acquires all sensor signals based on local environmental data and performs convolutional analysis on the energy change amplitude, calculates the signal stability over multiple time periods, generates a time series interference table, and passes it to the dynamic programming module. The dynamic programming module, based on the temporal interference table, obtains a set of candidate tracks for track planning requirements analysis, assesses collision risks for multiple tracks, adjusts track priorities based on signal stability, and generates dynamic obstacle avoidance results.

[0024] The multi-source fusion data includes visual images, laser point clouds, and IMU data. The pose localization set includes visual errors, point cloud feature errors, pre-integrated motion errors, and joint optimization results. The local environment data includes grid division, point cloud centroid changes, obstacle velocities, and obstacle displacement directions. The temporal interference table includes multi-source sensor signals, energy change amplitude, and signal stability. The dynamic obstacle avoidance results include candidate trajectory sets, collision risk assessment, and trajectory priority adjustment results.

[0025] Specifically, such as Figure 2 , 3 As shown, the multi-source fusion module specifically consists of: The data analysis submodule collects image frames from the visual sensor, point cloud echoes from the LiDAR, and acceleration and angular velocity sequences from the IMU. It calculates the difference between the visual frame interval and the IMU sampling period based on the same time axis, synchronizes and aligns the start times of multiple data sources, and generates a multi-source synchronized dataset. The raw data was input from a drone performing 3D scanning modeling of urban buildings. The drone's visual sensor acquired image frames at a rate of 30 frames per second, with the first frame timestamped at 10.000 seconds, resulting in a visual frame interval of 33.33 milliseconds. The lidar acquired point cloud echoes at a frequency of 10 times per second, with the first echo timestamped at 10.015 seconds, and an acquisition interval of 100 milliseconds. The inertial measurement unit (IMU) acquired acceleration and angular velocity sequences at a frequency of 200 Hz, with the first data point timestamped at 10.002 seconds, and a sampling period of 5 milliseconds. First, the start times of each data source were aligned. The latest start timestamp among all data sources was selected as the global synchronization start time. In this example, the visual sensor's start time was 10.000 seconds, the IMU's start time was 10.002 seconds, and the lidar's start time was 10.015 seconds. Comparing the three timestamps, 10.015 seconds is the latest start time; therefore, the global synchronization start time is set to 10.015 seconds. All data collected before 10.015 seconds is discarded, including visual frames with a timestamp of 10.000 seconds, and IMU data with timestamps of 10.002 seconds, 10.007 seconds, and 10.012 seconds. Next, the specific difference between the visual frame interval and the IMU sampling period is calculated and recorded. The visual frame interval is 33.33 milliseconds, and the IMU sampling period is 5 milliseconds; the difference between the two is 33.33 milliseconds. 5 = 28.33 milliseconds. This difference is used for subsequent timestamp alignment verification. Finally, a multi-source synchronization dataset is generated based on the global synchronization start time. Starting from 10.015 seconds, a new timestamp based on the synchronization clock is assigned to each data point from each data source. The first synchronization data packet has a timestamp of 10.015 seconds, and this packet will contain LiDAR point cloud data with a timestamp of 10.015 seconds. Since the next frame acquisition time of the visual sensor is 10.033 seconds, and the IMU has new samples at 10.017 seconds, 10.022 seconds, 10.027 seconds, and 10.032 seconds, in subsequent processing, these sensor data at different time points are associated with a unified synchronization timestamp through interpolation or nearest-neighbor matching. For example, taking the acquisition node of the LiDAR as the reference, the visual frame at 10.033 seconds and all IMU data between 10.017 seconds and 10.115 seconds are associated with the LiDAR data at 10.015 seconds and 10.115 seconds to form a series of datasets with a unified time stamp.

[0026] The feature extraction submodule extracts pixel gradient distribution and corner coordinates from image frames of multi-source synchronous datasets, calculates the gray-level change rate as a visual feature vector, extracts the reflection intensity and coordinates of point cloud data to calculate the spatial density gradient as a point cloud feature vector, and matches the two types of features to obtain cross-modal feature data. The generated multi-source synchronization dataset is invoked. Taking the synchronization data unit with a timestamp of 10.115 seconds as an example, this unit contains an image frame, a cluster of point cloud data, and the corresponding IMU sequence. First, visual features are extracted for this image frame. When extracting the pixel gradient distribution, the pixel with coordinates (256, 312) in the image is selected, and its gray value is 150. The gray values ​​of the pixels in its surrounding 3x3 neighborhood are examined. For example, the gray value of the pixel to its right is 158, and the gray value of the pixel below it is 142. Then the gradient component of this point in the horizontal direction is 158. 150 = 8, the gradient component in the vertical direction is 150. 142 = 8. The gradient magnitude of this pixel is... This calculation is repeated for all corner points in the image to form a gradient distribution. For corner point coordinate extraction, the gradient changes within the neighborhood of each pixel are analyzed. A pixel is identified as a corner point when the gradient magnitude in all directions is higher than a preset gradient threshold (e.g., 10.0). For example, if the gradient magnitude of point (256, 312) is between 10.5 and 12.0 in all eight directions, then that point is recorded as a corner point. Finally, the corner point coordinates (256, 312) are combined with its gradient magnitude of 11.31 to form part of the visual feature vector, i.e., [256, 312, 11.31]. Simultaneously, features are extracted from the point cloud data. From the point cloud data with a timestamp of 10.115 seconds, a point P1 with coordinates (10.5, 20.1, 5.3) and a reflectance intensity of 180 is selected. To calculate its spatial density gradient, the number of neighboring points within a 0.5-meter radius around it is first determined, which is counted as 25. Similarly, at another point P2, located 0.1 meters from P1, with coordinates (10.6, 20.1, 5.3) and a reflection intensity of 175, there are 22 neighboring points within the same radius. The spatial density gradient is the difference in the number of neighboring points between two points divided by the distance between the two points, i.e., (25... 22) / 0.1=30. The feature vector of this point cloud region consists of the average reflection intensity and spatial density gradient, i.e., [(180+175)÷2, 30]=[177.5, 30]. Finally, the visual feature vector and the point cloud feature vector are mapped. Using the intrinsic and extrinsic parameter calibration matrix carried by the UAV, the point cloud coordinates (10.5, 20.1, 5.3) are projected onto the image plane. If the image coordinates after projection are (257, 311), and the pixel distance between these coordinates and the previously extracted visual corner coordinates (256, 312) is less than the set matching distance threshold (e.g., 2 pixels), then these two features are associated. Thus, a cross-modal feature data is generated, which contains the pairing of visual feature [256, 312, 11.31] and point cloud feature [177.5, 30].

[0027] The time synchronization submodule calls cross-modal feature data and IMU acceleration and angular velocity sequences, calculates the pre-integrated motion between adjacent time points and corrects time drift errors, analyzes weighted translation and attitude change rates, performs temporal weighted fusion of visual and point cloud features, and generates multi-source fusion data. The generated cross-modal feature data, along with the corresponding IMU acceleration and angular velocity sequences, are retrieved. Taking the cross-modal feature data from two consecutive times, T1=10.115 and T2=10.148 seconds, as an example, the time interval is 33 milliseconds. During this period, the IMU generates six sets of acceleration and angular velocity readings with a period of 5 milliseconds. First, the pre-integrated motion between adjacent time points is calculated, and the time drift error is corrected. The six sets of angular velocity sequences between T1 and T2 are integrated to obtain the attitude change during this time period, for example, the total rotation is (0.02, -0.01, 0.05) radians. The acceleration sequence is integrated once to obtain the velocity change, and twice to obtain the displacement change, thus obtaining the pre-integrated motion, for example, the displacement is (0.15, 0.02, -0.01) meters. The time drift error rate is obtained through experimental calibration. The specific calibration process is as follows: the IMU device is continuously run in a stationary state for 3600 seconds, and its output timestamps are synchronously recorded using a high-precision atomic clock. The cumulative drift is calculated by comparing the difference between the IMU timestamp and the atomic clock timestamp.

[0028] Table 1. Experimental Data for IMU Time Drift Error Calibration

[0029] As shown in Table 1, the total cumulative drift during the 3600-second test was 60.0 milliseconds. Therefore, the time drift error rate is calculated to be approximately 60.0 ÷ 3600 ≈ 0.0167. Applying this error rate to the current 33-millisecond time interval, the corrected time interval is... Milliseconds. Recalculate the integral motion using the corrected time interval.

[0030] Next, the weighted translation and attitude change rates are analyzed, and temporal weighted fusion is performed. By matching cross-modal features at times T1 and T2, displacement estimates based on vision and point cloud are calculated, for example, (0.17, 0.01, -0.02) meters. There are two displacement estimates: (0.15, 0.02, -0.01) meters obtained from IMU pre-integration, and (0.17, 0.01, -0.02) meters obtained from the visual point cloud. Weights are set according to the current motion state of the UAV. When the absolute mean of the UAV's angular velocity (e.g., 0.8 radians / second during this period) is below 1.5 radians / second, it is considered slow motion. In this case, the displacement estimate weight of the visual point cloud is set to 0.6, and the displacement estimate weight of the IMU is set to 0.4. These weight values ​​are determined experimentally based on the principle of minimizing the error between the fusion results and the ground truth system (such as Vicon) at different motion speeds. The fused translational displacement on the X-axis is 0.15 × 0.4 + 0.17 × 0.6 = 0.06 + 0.102 = 0.162 meters. The same calculation is performed on the Y and Z axes to generate the fused displacement and attitude. This high-precision displacement and attitude information is then integrated with the feature data at that moment to generate a single data unit of multi-source fused data.

[0031] Specifically, such as Figure 2 , 4 As shown, the error optimization module specifically consists of: The visual error submodule extracts the coordinates of feature points in adjacent frames and calculates the spatial difference between corresponding points based on the spatial matching relationship between visual features and point cloud features in multi-source fusion data. It analyzes the visual residuals, performs a weighted average of the residuals of each set of matching points, and generates visual error results. The generated multi-source fusion data is used, specifically processing two consecutive data units with timestamps of 10.148 seconds and 10.181 seconds. First, matching feature points between these two image frames are extracted, and the difference in their spatial coordinates is calculated. In one data unit, a feature point P2 has its 3D spatial coordinates at 10.148 seconds determined by point cloud data to be (10.500, 20.100, 5.300) meters. At the subsequent 10.181 seconds, the same point P3 is matched using a feature tracking algorithm, and its new spatial coordinates are (10.665, 20.110, 5.281) meters. Simultaneously, the calculated pose changes of the UAV between these two times are used, namely, translation (0.162, 0.014, -0.016) meters and rotation (0.02, -0.01, 0.05) radians. The coordinates of point P2 (10.500, 20.100, 5.300) meters are spatially transformed based on this pose change. First, a rotation transformation is applied, followed by a translation transformation. The predicted coordinates P2 at 10.181 seconds are calculated to be (10.664, 20.111, 5.282) meters. Then, the spatial difference between the predicted coordinates P2 and the actual observed coordinates P3, i.e., the visual residual, is calculated. This is done by finding the Euclidean distance between the two coordinates, specifically by summing the squares of the components of the coordinate difference (-0.001, 0.001, 0.001) meters and then taking the square root, resulting in a residual of approximately 0.00173 meters. This process is repeated for all 50 valid feature matching point pairs between frames, resulting in a set of 50 residual values. For example, the residual calculation results for two other matching points are 0.00215 meters and 0.00190 meters, respectively. Next, a weighted average is calculated for all 50 residuals of the matched point set. The weights are set with reference to the depth values ​​of the feature points in the camera coordinate system, specifically the reciprocal of the depth. Points with smaller depth values ​​have higher measurement accuracy and are assigned higher weights. The depth values ​​of the aforementioned three points are 5.30 meters, 8.10 meters, and 4.50 meters, respectively, and their corresponding unnormalized weights are 1 / 5.30 = 0.1887, 1 / 8.10 = 0.1235, and 1 / 4.50 = 0.2222. The unnormalized weights of all 50 points are summed, resulting in a total of 8.532. Then, the normalized weights are obtained by dividing the unnormalized weight of each point by this sum; for example, the normalized weights of the first three points are 0.0221, 0.0145, and 0.0261, respectively. Finally, the residual value of each point is multiplied by its corresponding normalized weight, and all products are summed. The sum of all 50 points is 0.00190 meters, which is the final visual error result.

[0032] The motion error submodule calls the visual error results, obtains the pre-integrated motion amount in the multi-source fusion data and performs time accumulation, analyzes the deviation from the pose change amount in adjacent frames to calculate the motion deviation ratio, corrects the abnormal integral and recalculates the accumulated deviation in combination with the pose change reference value, and generates motion error coefficients. The generated visual error result of 0.00190 meters was used, and the pre-integrated motion of the Inertial Measurement Unit (IMU) over 10 consecutive frame intervals in the multi-source fusion data was obtained, with a total duration of approximately 0.33 seconds. First, the IMU pre-integrated rotation vectors over these 10 frame intervals were accumulated over time. The total rotation amount obtained after accumulation was set to (0.063, -0.033, 0.152) radians along the three axes. Then, the deviation between this accumulated rotation amount and the total attitude change calculated from visual and point cloud feature matching within the same time period was analyzed. The total attitude change calculated based on visual point cloud features was set to (0.065, -0.030, 0.155) radians. The difference between the two was calculated, yielding a deviation vector of (-0.002, -0.003, -0.003) radians. Next, the motion deviation ratio was calculated, defined as the ratio of the magnitude of the deviation vector to the magnitude of the visual point cloud attitude change vector. The magnitude of the deviation vector, i.e., the square root of the sum of the squares of its components, is approximately 0.00469. The magnitude of the visual point cloud attitude change vector is approximately 0.1707. Therefore, the motion deviation ratio is calculated as 0.00469 / 0.1707 = 0.0275. This motion deviation ratio is then compared to the attitude change baseline. This baseline was determined through a series of offline calibration experiments, in which the UAV operated under different motion states, and its motion deviation ratio was compared with the true value of the high-precision motion capture system. Experimental data shows that under rapid rotation and other intense motions, 95% of the motion deviation ratios are below 0.050; therefore, 0.050 is set as the attitude change baseline. The currently calculated deviation ratio of 0.0275 is lower than this baseline value, indicating that the IMU integration did not experience any anomalies during this time period. If the calculated deviation ratio is 0.061, it is considered an abnormal integration because it is higher than 0.050. In this case, the cumulative rotation of the IMU will be directly corrected using the attitude change amount (0.065, -0.030, 0.155 radians) of the visual point cloud, and the bias estimate of the IMU gyroscope will be adjusted based on the portion of the deviation ratio that exceeds the reference value by 0.011. In this embodiment, since the integration is normal, no correction is required. Finally, a motion error coefficient is generated based on the motion deviation ratio. The calculation method is to add the value 1 to the motion deviation ratio to obtain a motion error coefficient of 1.0275.

[0033] The joint optimization submodule calculates the weight ratio of the two types of errors in the time and spatial domains based on the visual error results and motion error coefficients. It adjusts the covariance of multiple error terms according to the weight ratio, performs iterative convergence operation of the weighted error vector, selects the state vector set with the best error, and generates the pose localization set. Based on the generated visual error result of 0.00190 meters and motion error coefficient of 1.0275, the complete state vector of the UAV is optimized. First, the weight ratio of these two types of errors in the optimization process is calculated. The weight information for the visual error is related to the reciprocal of the square of the visual error, approximately 1 divided by the square of 0.00190, or approximately 277000. The weight information for the motion error is related to the reciprocal of the square of the portion of the motion error coefficient that deviates from 1, approximately 1 divided by the square of 0.0275, or approximately 1322. The weight ratio of the two is calculated as 277000 / 1322 = 209.5. This ratio is used to adjust the covariance of multiple error terms. The state vector contains components such as the UAV's position, attitude, and velocity. In its covariance matrix, the diagonal element values ​​corresponding to the position component are updated based on new visual observations and set to the square of the visual error result, i.e., 3.61 multiplied by 10 to the power of -6. The covariance related to the attitude component is adjusted according to the motion error coefficient. Subsequently, an iterative convergence operation of the weighted error vector is performed. The goal of this operation is to find an optimal state vector that minimizes the total weighted error combining all visual and motion residuals. During the iteration, an error vector containing all 50 visual residuals and 10 IMU motion residuals is constructed, and its Jacobian matrix relative to the state vector is calculated. The linear equations are solved using the aforementioned weight information to obtain the update amount of the state vector. This update amount is added to the current state vector to obtain a new state vector, and the total weighted error is recalculated. For example, the initial total error is 0.85, which decreases to 0.52 after the first iteration. This iterative process continues until the change in the total weighted error between two consecutive iterations is less than a preset convergence threshold of 1 x 10^-5. This threshold was determined through experimental data analysis; when the error change is below this value, the accuracy of the state vector does not significantly improve. The total error is set to 0.47158 after the 5th iteration and 0.47157 after the 6th iteration, with a difference of 0.00001, satisfying the convergence condition, and the iteration stops. The state vector set obtained when the iteration stops is the optimal state vector set selected by error. Finally, the pose information at the current moment is extracted from this optimal state vector set to generate a new element in the pose localization set. For time 10.181 seconds, the final output pose is position (10.664, 20.111, 5.282) meters and attitude quaternion (0.999, 0.010, -0.005, 0.025).

[0034] Specifically, such as Figure 2 , 5 As shown, the environment modeling module specifically consists of: The grid division submodule extracts spatial coordinates for continuous time periods based on the pose localization set, analyzes the 3D grid in the local space according to the distribution of coordinate points, calculates the point cloud number of multiple grids and filters out invalid empty grids, determines the boundary of the local area based on the point cloud density difference between adjacent grids, and generates local grid distribution data. The generated pose localization set is retrieved, and 30 consecutive frames (approximately 1 second in time, from 10.181 seconds to 11.181 seconds) of UAV spatial coordinates and associated point cloud data are extracted from it. First, based on the 3D spatial distribution of all point cloud coordinates within this time period, a local 3D grid map is analyzed and constructed. The extent of this local space is determined by the maximum and minimum values ​​of all point cloud coordinates: the X-axis range is 5.0 meters to 25.0 meters, the Y-axis range is 10.0 meters to 30.0 meters, and the Z-axis range is 0.0 meters to 15.0 meters. This space is divided along each axis with a side length of 0.5 meters, forming a series of cubic grids; the local space contains a total of 48,000 grids. All 250,000 point cloud data points collected within this time period are then assigned to their respective grids. For example, a point with spatial coordinates (10.664, 20.111, 5.282) meters has its X-axis index calculated by dividing (10.664 - 5.0) by 0.5 and rounding down, resulting in 11; its Y-axis index is calculated by dividing (20.111 - 10.0) by 0.5, resulting in 20; and its Z-axis index is calculated by dividing (5.282 - 0.0) by 0.5 and rounding down, resulting in 10. Therefore, this point is assigned to the grid with index (11, 20, 10). After traversing all point clouds, the number of point clouds in each grid is counted. Next, the grids are filtered. A threshold of 5 is set for the number of point clouds in an invalid empty grid. This threshold is based on statistical analysis of the random noise point density generated by the lidar sensor at a target distance of 50 meters. Within a 0.125 cubic meter space (i.e., the volume of one grid), the number of noise points is less than 5 in 99.8% of cases. When a raster has fewer than 5 points, it is considered an invalid empty raster and is filtered out. For example, if the raster (15, 25, 18) has 3 points, it is filtered out; if the raster (11, 20, 10) has 158 points, it is retained. Then, the boundary of the local area is determined based on the point cloud density difference between adjacent rasteres. Select a valid raster, for example, the raster (11, 20, 10) with 158 points, and check the point cloud counts of its 26 adjacent rasteres. If its adjacent raster (12, 20, 10) has 8 points, then the point cloud density difference between the two is calculated as 158-8=150. The point cloud density difference threshold is set to 100. This threshold is set with reference to scan data of typical scenes such as building edges and vegetation edges, where the point cloud count difference between rasteres 0.5 meters apart on both sides of an entity edge usually exceeds 100. Since the calculated density difference of 150 is greater than the threshold of 100, it is determined that there is a local region boundary between these two rasters. All raster pairs identified as boundaries are recorded, and finally, local raster distribution data containing all valid rasters and their point cloud counts, along with labeled boundary relationships, is generated.

[0035] The centroid calculation submodule calls the local grid distribution data and the point cloud coordinates in the pose localization set, calculates the mean of spatial coordinates, analyzes the geometric center position of a single grid, compares the spatial offset of the centroid coordinates of the same grid in consecutive frames, analyzes the centroid change vector of each grid, and generates the grid centroid change results. The generated local grid distribution data and the point cloud coordinates at the corresponding time in the pose localization set are retrieved. Taking the data at time 10.181 seconds as an example, the geometric center and centroid of the grid (11, 20, 10) are analyzed. The geometric center of this grid is the average of the center points of its six faces, and its coordinates are calculated using the grid index, which is (10.75, 20.25, 5.25) meters. To calculate its centroid, the spatial coordinates of all 158 point clouds within this grid are extracted and their arithmetic mean is calculated. For example, three points within this grid are randomly selected, with coordinates of point one (10.664, 20.111, 5.282) meters, point two (10.831, 20.352, 5.198) meters, and point three (10.705, 20.289, 5.330) meters. The X, Y, and Z coordinates of all 158 points are summed. The sum of the X coordinates is 1701.13, the sum of the Y coordinates is 3200.48, and the sum of the Z coordinates is 830.56. Therefore, the centroid coordinates at this moment are (10.767, 20.256, 5.257) meters, with X-axis coordinates of 1701.13 / 158, Y-axis coordinates of 3200.48 / 158, and Z-axis coordinates of 830.56 / 158. Next, the spatial offset of the centroid coordinates of the same grid in consecutive frames is compared. At 10.214 seconds in the next frame, the UAV shifts, and the centroid is calculated again for the grid (11, 20, 10) fixed in the world coordinate system. The number of point clouds falling into this grid now becomes 165. Through the same calculation process, the new centroid coordinates are (10.771, 20.298, 5.256) meters. Next, the centroid change vector of the raster is analyzed. This vector is obtained by calculating the difference in centroid coordinates between two time points. The X component of the centroid change vector is 10.771 - 10.767 = 0.004, the Y component is 20.298 - 20.256 = 0.042, and the Z component is 5.256 - 5.257 = -0.001. Therefore, the centroid change vector is (0.004, 0.042, -0.001) meters. For all rasters in the local raster distribution data that are determined to be valid in the previous and current frames, this process of calculating centroid coordinates and change vectors is repeated. The indices of all rasters and their corresponding centroid change vectors are integrated to generate the raster centroid change result.

[0036] The obstacle determination submodule calculates the ratio of the centroid offset of adjacent frames to the time interval based on the change results of the grid centroid, and determines the obstacle motion state based on the difference between the time displacement rate and the obstacle displacement threshold. It also extracts the offset direction vector, calculates the motion velocity distribution, and generates local environmental data. Based on the generated centroid change results of the raster, the motion state of the raster (11, 20, 10) is determined. First, the centroid offset of the raster between adjacent frames 10.181 seconds and 10.214 seconds is calculated, which is the magnitude of the centroid change vector (0.004, 0.042, -0.001) meters. This offset is obtained by calculating the square root of the sum of the squares of its components, which is approximately 0.0422 meters. The time interval is 10.214 - 10.181 = 0.033 seconds. Dividing the centroid offset by the time interval, the time displacement rate is calculated, which is 0.0422 / 0.033 = 1.28 meters per second. Next, the motion state of the obstacle is determined based on the difference between the time displacement rate and a preset obstacle displacement threshold. The obstacle displacement threshold is set to 0.1 meters per second. This threshold is set based on experimental data from long-term observation of a known static target. In the experiment, targets such as building exteriors, stationary trees, and parked vehicles were observed continuously for 60 seconds. Due to positioning drift and lidar measurement noise, the centroids of static targets also exhibited slight displacements. Statistical data showed that the maximum time displacement rate of these static targets did not exceed 0.081 m / s. To provide sufficient error margin, the obstacle displacement threshold was set to 0.1 m / s. The currently calculated time displacement rate was 1.28 m / s, which is greater than the obstacle displacement threshold of 0.1 m / s. Therefore, the object in grid (11, 20, 10) was determined to be a dynamic obstacle. If another grid had a calculated time displacement rate of 0.05 m / s, the object in that grid was determined to be static because it was less than 0.1 m / s. For grids determined to be dynamic obstacles, their offset direction vector was extracted, i.e., the centroid change vector (0.004, 0.042, -0.001) was normalized to obtain a direction vector of approximately (0.095, 0.995, -0.024). The obstacle's velocity is calculated as a time displacement rate of 1.28 meters per second. This calculation is repeated for all grid cells identified as dynamic obstacles to obtain a set of velocity values. The motion status of all grid cells, the velocity of dynamic obstacles, and their direction vectors are then integrated to generate local environmental data.

[0037] Specifically, such as Figure 2 , 6 As shown, the time series analysis module specifically consists of: The signal acquisition submodule acquires the time series values ​​of all sensor signals based on local environmental data, extracts the amplitude and frequency distribution of multiple signals within the same time window and synchronizes them in time, calculates the difference in signal amplitude at adjacent times and records the rate of change, and generates a multi-source signal difference set. Based on the generated local environmental data, upon detecting a dynamic obstacle with a velocity of 1.28 m / s within the grid (11, 20, 10), the acquisition of multi-source sensor signals from the UAV itself is triggered. This process acquires the signal-to-noise ratio (SNR) output from the GPS receiver and the Z-axis accelerometer signal output from the inertial measurement unit (IMU), forming two sets of time-series values. Within a 100-millisecond time window, specifically from 11.200 to 11.300 seconds, the amplitude and frequency distributions of these two signals are extracted. For the 10 Hz GPS signal, this window contains 2 data points; for the 500 Hz IMU signal, it contains 51 data points. By reading the timestamp associated with each data point, the positions of all data points are aligned onto a unified time axis, achieving time synchronization between different signals. Subsequently, the difference in signal amplitude between adjacent moments is calculated, and its rate of change is recorded. At 11:200, the carrier-to-noise ratio (CNR) of the GPS signal is 45.0 dB-Hertz. At the next sampling point, 11:300, this value drops to 42.0 dB-Hertz. The amplitude difference is calculated by subtracting the former from the latter, i.e., 42.0 - 45.0 = -3.0 dB-Hertz. The time interval is 11:300 - 11:200 = 0.1 seconds. Therefore, the rate of change during this time interval is calculated by dividing the amplitude difference by the time interval, i.e., -3.0 / 0.1 = -30.0 dB-Hertz per second. For the inertial measurement unit (IMU) signal, at 11:200, the Z-axis acceleration reading is -9.81 m / s², and at the immediately following sampling point, 11:202, the reading is -9.91 m / s². The amplitude difference is -9.91 minus -9.81, resulting in -0.1 m / s². The time interval is 11.202 - 11.200 = 0.002 seconds. Its rate of change is -0.1 / 0.002 = -50.0 meters per cubic second. This difference and rate of change calculation process is repeated for all adjacent signal sampling points within this 100-millisecond time window and for multiple subsequent consecutive windows. The time series of the GPS carrier-to-noise ratio change rate and the inertial measurement unit acceleration change rate are integrated to generate a multi-source signal differential set.

[0038] The convolution calculation submodule calls the multi-source signal difference diversity to perform convolution operations on the change sequences of multi-sensor signals over multiple time periods, calculates the sum of energy of the convolution results and extracts the energy change interval, analyzes the energy fluctuation rate as a feature of energy transfer between signals, and generates an energy fluctuation rate distribution. The generated multi-source signal differential set is invoked, which contains the variation sequences of the GPS carrier-to-noise ratio rate of change and the inertial measurement unit (IMU) acceleration rate of change over multiple consecutive 100-millisecond time intervals. One time interval, from 11.200 to 11.300 seconds, is selected for processing. This sequence contains 50 values, with the first few values ​​being -50.0, -40.0, 20.0, 15.0, etc. A one-dimensional convolution operation is performed on this variation sequence. A convolution kernel of length 3 is defined with values ​​[0.5, 1.0, 0.5]. The convolution operation is performed by covering the first three values ​​of the rate of change sequence with the kernel, multiplying the corresponding elements, and summing the results to obtain the first value of the convolution result sequence. Specifically, -50.0 * 0.5 + (-40.0) * 1.0 + 20.0 * 0.5 = -55.0. The convolution kernel is then moved one position to the right, covering the second, third, and fourth values ​​of the sequence. The multiplication and summation calculation is repeated to obtain the second convolution result: -40.0*0.5 + 20.0*1.0 + 15.0*0.5 = 7.5. This sliding calculation process is repeated for the entire sequence to obtain a new convolution result sequence, with the first few values ​​being -55.0, 7.5, 12.5, etc. Next, the total energy of the convolution results within this time period is calculated. The total energy is obtained by calculating the sum of the squares of each value in the convolution result sequence. Taking the first three values ​​as an example, the partial energy sum is the square of -55.0, plus the square of 7.5, plus the square of 12.5, i.e., 3025 + 56.25 + 156.25 = 3237.5. The total energy calculated for the complete inertial measurement unit convolution sequence within this 100-millisecond time period is 150,000. Perform the same convolution and energy calculation on the inertial measurement unit rate of change sequence for the next 100-millisecond time interval (11.300 to 11.400 seconds), resulting in a total energy of 180,000. Analyze the energy volatility, which is calculated as the difference between the total energy of two consecutive time intervals, divided by the total energy of the previous time interval. Therefore, the energy volatility is (180,000 - 150,000) / 150,000 = 0.20. Repeat this process for all sensor signals and all consecutive time intervals to generate a set of energy volatility values, for example, [0.20, -0.10, 0.35, 0.15, -0.05, 0.25]. This sequence is the energy volatility distribution.

[0039] The stability assessment submodule calculates the standard deviation of signal volatility over multiple time periods based on the energy volatility distribution and compares it with the stability threshold. It extracts the time of the stable interval, calculates the average value of signal volatility over multiple intervals and calculates the trend of change, and generates a time-series interference table. The stability threshold is set by collecting signal sequences during steady-state operation, calculating the mean and variance of the volatility of multiple signals, and then setting the threshold based on the variance range. The stability of the signal is evaluated based on the generated energy volatility distribution, i.e., the numerical sequence [0.20, -0.10, 0.35, 0.15, -0.05, 0.25]. First, the standard deviation of the signal volatility over six consecutive time periods (total duration 600 milliseconds) is calculated. The first step is to calculate the mean of the sequence: sum all the values ​​in the sequence, i.e., 0.20 + (-0.10) + 0.35 + 0.15 + (-0.05) + 0.25 = 0.80, then divide by the number 6 to obtain an average of approximately 0.133. The second step is to calculate the variance, which is the average of the squares of the differences between each value in the sequence and the mean. Specifically, the squares of (0.20 - 0.133) are added to the squares of (-0.10 - 0.133) until the last value, and the sum of all squares is divided by 6 to obtain a variance of approximately 0.0256. The third step is to calculate the standard deviation. Taking the square root of the variance 0.0256, the standard deviation is approximately 0.160. Next, this standard deviation is compared to a stability threshold, set at 0.20. This threshold was determined based on extensive experimental data collected under different electromagnetic environments. In a laboratory environment without known interference, analysis of 500 samples showed that the 99th percentile of the standard deviation of energy fluctuation rate was 0.18. In an urban canyon environment with weak interference, this value was 0.45. Near strong interference sources such as GPS spoofers, this value rose to 0.82. To effectively identify fluctuations significant to baseline noise, the threshold was set to 0.20. The currently calculated standard deviation is 0.160, which is less than the threshold of 0.20; therefore, this 600-millisecond time period (from 11.200 seconds to 11.800 seconds) is considered a stable interval. If the standard deviation for another time period is 0.28, since it is greater than 0.20, that time period is considered an interference interval. Extract all time periods identified as stable intervals and calculate the average volatility within each interval. For the first stable interval (11.200 seconds to 11.800 seconds), the average volatility is 0.133. Set the average volatility of the subsequently discovered second stable interval (12.500 seconds to 13.100 seconds) to 0.095. Calculate the trend of the average value changes for these two stable intervals by subtracting the former from the latter, i.e., 0.095 - 0.133 = -0.038. Finally, integrate all time periods, their stability determination results (stable or disturbed), the calculated standard deviation, and the average volatility of the stable intervals to generate a time-series disturbance table.

[0040] Specifically, such as Figure 2 , 7 As shown, the dynamic programming module is specifically as follows: The requirement analysis submodule extracts the trajectory planning requirement parameters within the target area based on the time-series interference table, filters the path data that meets the trajectory length and heading restrictions, counts the turning angle change value of each path and records the path node spacing, and obtains the trajectory feature parameter set. Based on the generated temporal interference table, the trajectory planning requirements parameters within the target area are extracted. First, the starting point coordinates of the UAV are obtained from the mission instructions as (0, 0, 10) meters, and the target endpoint coordinates are obtained as (100, 50, 10) meters. The upper limit of the trajectory length is set to 200 meters, which is calculated based on the UAV's battery life and mission time constraints. Simultaneously, the maximum heading deflection angle of a single flight segment is set to 30 degrees, a limit based on experimental data of the UAV's maximum maneuverability at rated speed. Subsequently, three preliminary candidate path data are generated: Path A, Path B, and Path C. Path A consists of the node sequence (0, 0, 10), (60, -10, 10), and (100, 50, 10), with a total length of 214.5 meters. Path B consists of the node sequence (0, 0, 10), (50, 10, 10), and (100, 50, 10), with a total length of 180.3 meters. Path C consists of the node sequence (0, 0, 10), (40, 30, 10), and (100, 50, 10), with a total length of 192.1 meters. These three paths are filtered. Path A, with a length of 214.5 meters, exceeds the 200-meter limit and is therefore eliminated. Paths B and C are both within the limit. Next, the turning angle change value for each remaining path is calculated. For path B, the first segment vector is (50, 10, 0), and the second segment vector is (50, 40, 0). By calculating the dot product of the two vectors and dividing by the product of their respective moduli, then taking the inverse cosine, the turning angle is found to be 22.1 degrees, less than the 30-degree limit. For path C, the first segment vector is (40, 30, 0), and the second segment vector is (60, 20, 0). Using the same method, the turning angle is calculated to be 31.0 degrees, greater than the 30-degree limit, therefore path C is also eliminated. Finally, only path B is retained. To increase path diversity, a new path D is generated by fine-tuning the nodes of path B. Its node sequence is [(0, 0, 10), (55, 15, 10), (100, 50, 10)], with a total length of 183.5 meters and a turning angle of 18.9 degrees, satisfying all constraints. The distance between the path nodes of paths B and D is recorded. The lengths of the two segments of path B are 51.0 meters and 64.0 meters, respectively. The lengths of the two segments of path D are 57.0 meters and 59.2 meters, respectively. The node coordinates, turning angle changes, and node distances of paths B and D are integrated to obtain the set of track feature parameters.

[0041] The risk assessment submodule calls the trajectory feature parameter set, performs distance comparison on the node spacing of any two paths, filters path combinations with intersection risks, counts the number of intersection nodes and records the node spacing value, calculates the path collision risk index, and generates collision risk assessment results. The generated set of track feature parameters, containing detailed data for path B and path D, is invoked. First, a distance comparison is performed between the nodes of these two paths. This comparison is not based on the numerical value of the node spacing, but rather on an assessment of the spatial proximity of the two paths. The first segment of path B (from node (0, 0, 10) to (50, 10, 10)) and the first segment of path D (from node (0, 0, 10) to (55, 15, 10)) are selected. Since the two segments share the same starting point, there is an inherent risk of intersection. The second segment of path B (from node (50, 10, 10) to (100, 50, 10)) and the second segment of path D (from node (55, 15, 10) to (100, 50, 10)) are compared, and the shortest distance between the two segments (spatial line segments) is calculated. This calculation is performed by parameterizing the two line segments and solving for the minimum value of the distance function. The calculated shortest distance between the two line segments is 4.5 meters. A crossover risk distance threshold of 5.0 meters is set, determined based on the UAV's dimensions (wingspan 2 meters) plus a safety redundancy (3 meters). Since 4.5 meters is less than 5.0 meters, a crossover risk is determined between path B and path D. Next, the number of crossover nodes is counted. In this example, there is a closest point pair between the second segments of the two paths, and the distance is less than the threshold, so the number of crossover nodes is recorded as 1. Simultaneously, the node spacing at this crossover point is recorded as 4.5 meters. Subsequently, the path collision risk index is calculated. This index is calculated as the square of the ratio of the crossover risk distance threshold to the actual shortest node spacing. The calculation process is: (5.0 divided by 4.5) squared, the result is approximately 1.23. This definition of the index results in a non-linear, rapid increase in the risk index value as the spacing between the two paths decreases. If the shortest distance between the two paths is greater than 5.0 meters, the collision risk index is set to 0. Repeat this process for all possible path combinations (in this example, only path B and path D), and integrate the path combinations, the number of intersection nodes, the node spacing value, and the calculated path collision risk index 1.23 to generate the collision risk assessment result.

[0042] The priority adjustment submodule sorts and calculates the priority parameters of multiple paths based on the collision risk assessment results, classifies them into risk levels according to the collision risk value, and corrects the priorities according to the signal stability value to generate dynamic obstacle avoidance results. Based on the generated collision risk assessment results (the collision risk index for path B and path D is 1.23), the priority parameters of the multipaths are calculated and ranked. First, initial priority parameters are assigned to paths B and D, calculated based on the track length as the difference between the upper limit of length (200 meters) and the actual track length. The initial priority for path B is 200. 180.3 = 19.7. The initial priority of path D is 200. 183.5 = 16.5. Therefore, the initial ranking is path B over path D. Next, the paths are divided into different risk levels based on their collision risk values.

[0043] Table 2 Collision Risk Level Classification Table

[0044] As shown in Table 2, the risk level classification criteria were determined through statistical analysis of collision accident rates at different distances in historical flight data. The calculated collision risk index of 1.23 falls within the (1.00, 2.00) range, corresponding to a risk level of Level 3 (high risk), with a priority penalty coefficient of 0.50. This penalty coefficient is applied to path D, which initially has a lower priority, and its priority is corrected to 16.5 × 0.50 = 8.25. The priority of path B remains unchanged at 19.7. Subsequently, priority correction is performed based on the signal stability values ​​in the time-series interference table. The average signal volatility of 0.133 is extracted from the time-series interference table for the stable interval (11.200 seconds to 11.800 seconds). A stability correction coefficient is defined, calculated as a value of 1 minus the product of the average signal volatility and the path angle complexity. The path angle complexity is defined as the ratio of the total path angle to the maximum allowable total angle (60 degrees). The angle complexity of path B is 22.1 ÷ 60 ≈ 0.368. The cornering complexity of path D is 18.9 ÷ 60 = 0.315. The stability correction factor for path B is 1. (0.133 × 0.368) ≈ 0.951. The stability correction coefficient for path D is 1. (0.133 × 0.315) ≈ 0.958. Applying this correction factor to their respective priorities, the final priority of path B is 19.7 × 0.951 ≈ 18.73. The final priority of path D is 8.25 × 0.958 ≈ 7.90. Sort all the final priorities of the paths to obtain the final path selection order. This is the generated dynamic obstacle avoidance result.

[0045] Please see Figure 8 The UAV localization and dynamic obstacle avoidance method based on multi-sensor fusion is executed based on the aforementioned UAV localization and dynamic obstacle avoidance system based on multi-sensor fusion, and includes the following steps: S1: Acquire visual images, laser point clouds and IMU data, extract visual features and point cloud features and calculate pre-integrated motion, fuse all data synchronously in time, and generate multi-source fused data; S2: Based on multi-source fusion data, calculate visual errors from visual features and point cloud features, analyze motion errors from pre-integrated motion, jointly optimize the two types of errors, and generate a pose localization set. S3: Based on the pose localization set, the local environment is divided into grids and the centroid change of the point cloud is calculated. Combined with the obstacle displacement threshold, the speed and direction of the obstacle are determined and the local environment data is generated. S4: Based on local environmental data, acquire all sensor signals and perform convolution analysis on energy change amplitude, calculate signal stability over multiple time periods, and generate a time-series interference table; S5: Based on the temporal interference table, obtain the candidate trajectory set for trajectory planning requirement analysis, assess the collision risk of multiple trajectories, adjust the trajectory priority in combination with signal stability, and generate dynamic obstacle avoidance results.

[0046] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion, characterized in that, The system includes: The multi-source fusion module acquires visual images, laser point clouds, and IMU data, extracts visual features and point cloud features, calculates pre-integrated motion, and fuses all data synchronously over time to generate multi-source fusion data, which is then transmitted to the error optimization module. The error optimization module calculates visual errors based on the multi-source fusion data, analyzes motion errors based on pre-integrated motion, jointly optimizes the two types of errors, generates a pose localization set, and transmits it to the environment modeling module. The environment modeling module, based on the pose localization set, divides the local environment into grids and calculates the centroid change of the point cloud. It combines the obstacle displacement threshold to determine the speed and direction of the obstacle, generates local environment data, and transmits it to the time series analysis module. The time series analysis module acquires all sensor signals based on the local environmental data and performs convolutional analysis on the energy change amplitude, calculates the signal stability over multiple time periods, generates a time series interference table, and transmits it to the dynamic programming module. The dynamic programming module, based on the time-series interference table, obtains a set of candidate tracks for track planning requirements analysis, assesses the collision risk of multiple tracks, adjusts track priorities in conjunction with signal stability, and generates dynamic obstacle avoidance results.

2. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The multi-source fusion data includes visual images, laser point clouds, and IMU data. The pose localization set includes visual errors, point cloud feature errors, pre-integrated motion errors, and joint optimization results. The local environment data includes grid division, point cloud centroid changes, obstacle velocities, and obstacle displacement directions. The temporal interference table includes multi-source sensor signals, energy change amplitude, and signal stability. The dynamic obstacle avoidance results include candidate trajectory sets, collision risk assessment, and trajectory priority adjustment results.

3. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The multi-source fusion module is specifically: The data analysis submodule collects image frames from the visual sensor, point cloud echoes from the LiDAR, and acceleration and angular velocity sequences from the IMU. It calculates the difference between the visual frame interval and the IMU sampling period based on the same time axis, synchronizes and aligns the start times of multiple data sources, and generates a multi-source synchronized dataset. The feature extraction submodule extracts pixel gradient distribution and corner coordinates from image frames of the multi-source synchronous dataset, calculates the gray-level change rate as a visual feature vector, extracts the reflection intensity and coordinates of point cloud data to calculate the spatial density gradient as a point cloud feature vector, and obtains cross-modal feature data by corresponding the two types of features. The time synchronization submodule calls the cross-modal feature data and IMU acceleration and angular velocity sequences, calculates the pre-integrated motion between adjacent time points and corrects the time drift error, analyzes the weighted translation and attitude change rate, performs temporal weighted fusion of visual and point cloud features, and generates multi-source fusion data.

4. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The error optimization module specifically comprises: The visual error submodule extracts the coordinates of feature points in adjacent frames and calculates the spatial difference between corresponding points based on the spatial matching relationship between visual features and point cloud features in the multi-source fusion data. It analyzes the visual residuals, performs a weighted average of the residuals of each set of matching points, and generates visual error results. The motion error submodule calls the visual error result, obtains the pre-integrated motion amount in the multi-source fusion data and performs time accumulation, analyzes the deviation from the pose change amount of adjacent frames to calculate the motion deviation ratio, corrects the abnormal integral and recalculates the accumulated deviation in combination with the pose change reference value, and generates motion error coefficients. The joint optimization submodule calculates the weight ratio of the two types of errors in the time and spatial domains based on the visual error results and the motion error coefficients. It adjusts the covariance of multiple error terms according to the weight ratio, performs weighted error vector iterative convergence operation, selects the state vector set with the best error, and generates a pose localization set.

5. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 4, characterized in that, The attitude change baseline value is obtained by extracting the attitude angle change sequence within multiple time windows, calculating the mean angular velocity and mean angular acceleration within each time window, and analyzing the time integral results of the two.

6. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The environment modeling module is specifically as follows: The grid division submodule extracts the spatial coordinates of continuous time periods based on the pose positioning set, analyzes the three-dimensional grid according to the distribution of coordinate points in the local space, calculates the point cloud number of multiple grids and filters out invalid empty grids, determines the boundary of the local area according to the point cloud density difference between adjacent grids, and generates local grid distribution data. The centroid calculation submodule calls the local grid distribution data and the point cloud coordinates in the pose positioning set to calculate the mean of spatial coordinates, analyze the geometric center position of a single grid, compare the spatial offset of the centroid coordinates of the same grid in consecutive frames, analyze the centroid change vector of each grid, and generate the grid centroid change result. The obstacle determination submodule calculates the ratio of the centroid offset of adjacent frames to the time interval based on the grid centroid change results. It determines the obstacle's motion state based on the difference between the time displacement rate and the obstacle displacement threshold, extracts the offset direction vector, calculates the motion velocity distribution, and generates local environmental data.

7. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 6, characterized in that, The obstacle displacement threshold is obtained by extracting point clouds that do not undergo significant motion changes in multiple frames, calculating the time series difference of centroid coordinates, obtaining the displacement distribution range of the static region, normalizing the range, and extracting the upper limit of the stable offset interval.

8. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The time series analysis module specifically includes: The signal acquisition submodule acquires the time series values ​​of all sensor signals based on the local environmental data, extracts the amplitude and frequency distribution of multiple signals within the same time window and synchronizes them in time, calculates the difference in signal amplitude at adjacent times and records the rate of change, and generates a multi-source signal difference set. The convolution calculation submodule calls the multi-source signal difference set, performs convolution operation on the change sequence of multi-sensor signals in multiple time periods, calculates the energy sum of the convolution result and extracts the energy change interval, analyzes the energy fluctuation rate as the energy transfer feature between signals, and generates the energy fluctuation rate distribution. The stability assessment submodule calculates the standard deviation of signal volatility over multiple time periods based on the energy volatility distribution and compares it with the stability threshold. It extracts the time of the stable interval, calculates the average value of signal volatility over multiple intervals and calculates the trend of change, and generates a time-series interference table. The stability threshold is set by collecting signal sequences during steady-state operation, calculating the mean and variance of the volatility of multiple signals, and then setting the threshold based on the variance range.

9. The UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 1, characterized in that, The dynamic programming module is specifically as follows: The requirement analysis submodule extracts the trajectory planning requirement parameters within the target area based on the time-series interference table, filters the path data that meets the trajectory length and heading restrictions, counts the turning angle change value of each path and records the path node spacing, and obtains the trajectory feature parameter set. The risk assessment submodule calls the set of trajectory feature parameters, performs distance comparison on the node spacing of any two paths, filters path combinations with intersection risks, counts the number of intersection nodes and records the node spacing value, calculates the path collision risk index, and generates collision risk assessment results. The priority adjustment submodule sorts and calculates the priority parameters of the multipath based on the collision risk assessment results, classifies them into risk levels according to the collision risk value, and corrects the priority based on the signal stability value to generate dynamic obstacle avoidance results.

10. A method for UAV localization and dynamic obstacle avoidance based on multi-sensor fusion, characterized in that, The execution of the UAV positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to any one of claims 1-9 includes the following steps: S1: Acquire visual images, laser point clouds and IMU data, extract visual features and point cloud features and calculate pre-integrated motion, fuse all data synchronously in time, and generate multi-source fused data; S2: Based on the multi-source fusion data, calculate the visual error of visual features and point cloud features, analyze the motion error of pre-integrated motion, jointly optimize the two types of errors, and generate a pose localization set. S3: Based on the pose localization set, the local environment is divided into grids and the centroid change of the point cloud is calculated. The speed and direction of the obstacle are determined by combining the obstacle displacement threshold, and local environment data is generated. S4: Based on the local environmental data, acquire all sensor signals and perform convolution analysis on the energy change amplitude, calculate the signal stability over multiple time periods, and generate a time-series interference table; S5: Based on the time-series interference table, obtain the candidate trajectory set for trajectory planning requirement analysis, assess the collision risk of multiple trajectories, adjust the trajectory priority in combination with signal stability, and generate dynamic obstacle avoidance results.

Images