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

CN121979254ACN 121979254 ACN121979254 ACN 121979254ACN-121979254-A

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. According to the invention, through synchronous fusion of multi-source data, the visual image, the laser point cloud and the IMU data are integrated, the characteristic information can be effectively extracted and the pose positioning is optimized in a complex environment, the adaptability deficiency of the traditional system in a dynamic environment is solved, the motion error and the positioning error of the visual and point cloud data are jointly analyzed through error optimization, and the positioning precision and the system robustness are improved. The method further combines environment modeling and time sequence analysis, can detect the motion state of the obstacle in real time, adjust the track priority, dynamically avoid the obstacle, improve the response speed and accuracy of navigation and obstacle avoidance, and ensure the stable operation and safety of the aircraft in a variable environment.

Inventors

HUANG CHUN
YIN LEI
XIA KE
ZHANG JIAN
TENG YONG
FAN GUOPENG

Assignees

诚芯智联(武汉)科技技术有限公司

Dates

Publication Date: 20260505
Application Date: 20251231

Claims (10)

1. Unmanned aerial vehicle location and dynamic obstacle avoidance system based on multisensor fuses, its characterized in that, the system includes: the multi-source fusion module is used for collecting visual images, laser point clouds and IMU data, extracting visual features and point cloud features, calculating the pre-integration motion quantity, synchronously fusing all data in time, generating multi-source fusion data and transmitting the multi-source fusion data to the error optimization module; The error optimization module is used for calculating visual errors of the visual features and the point cloud features based on the multi-source fusion data, analyzing the motion errors of the pre-integration motion quantity, jointly optimizing the two types of errors, generating a pose positioning set and transmitting the pose positioning set to the environment modeling module; the environment modeling module is used for dividing grids for the local environment based on the pose positioning set, calculating the change of the mass center of the point cloud, judging the speed and the direction of the obstacle by combining the displacement threshold of the obstacle, generating local environment data and transmitting the local environment data to the time sequence analysis module; The time sequence analysis module is used for acquiring all sensing signals based on the local environment data, convoluting and analyzing the energy change amplitude, calculating the signal stability of multiple time periods, generating a time sequence interference table and transmitting the time sequence interference table to the dynamic programming module; and the dynamic programming module is used for acquiring a track programming demand analysis candidate track set based on the time sequence interference table, evaluating collision risk for multiple tracks, adjusting the track priority in combination with signal stability and generating a dynamic obstacle avoidance result.
2. The multi-sensor fusion-based unmanned aerial vehicle positioning and dynamic obstacle avoidance system according to claim 1, wherein the multi-source fusion data comprises visual images, laser point clouds and IMU data, the pose positioning set comprises visual errors, point cloud characteristic errors, pre-integral motion quantity errors and joint optimization results, the local environment data comprises division grids, point cloud centroid changes, obstacle speeds and obstacle displacement directions, the time sequence interference list comprises multi-source sensing signals, energy change amplitudes and signal stability, and the dynamic obstacle avoidance results comprise candidate track sets, collision risk assessment and track priority adjustment results.
3. The unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 1, wherein the multi-source fusion module is specifically: The data analysis sub-module is used for acquiring image frames of the vision sensor, point cloud echoes of the laser radar and acceleration and angular velocity sequences of the IMU, calculating a difference value between a vision frame interval and a sampling period of the IMU according to the same time axis, and synchronously aligning the starting time of multiple data sources to generate a multi-source synchronous data set; The feature extraction sub-module is used for extracting pixel gradient distribution and angular point coordinates based on the image frames of the multi-source synchronous data set, calculating gray level change rate as a visual feature vector, extracting reflection intensity and coordinate calculation space density gradient of point cloud data as a point cloud feature vector, and corresponding to the two types of features to obtain cross-modal feature data; And the time synchronization sub-module is used for calling the cross-modal characteristic data and the IMU acceleration and angular velocity sequence, calculating the pre-integral motion quantity between adjacent moments, correcting the time drift error, analyzing the weighted translation and the gesture change rate, and carrying out time domain weighted fusion on the vision and the point cloud characteristics to generate multi-source fusion data.
4. The unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion of claim 1, wherein the error optimization module is specifically: the visual error sub-module is used for extracting feature point coordinates of adjacent frames and calculating space differences between corresponding points based on the space matching relation between visual features and point cloud features in the multi-source fusion data, analyzing visual residual errors, and carrying out weighted average on the residual errors of each matching point set to generate a visual error result; The motion error sub-module is used for calling the visual error result, acquiring the pre-integral motion quantity in the multi-source fusion data, accumulating time, analyzing the deviation degree of the motion error ratio and the posture change quantity of the adjacent frames, calculating the motion error ratio, correcting the abnormal integral by combining the posture change reference value, and calculating the accumulated deviation again to generate a motion error coefficient; And the joint optimization sub-module is used for calculating the weight ratio of the two types of errors in the time domain and the space domain according to the visual error result and the motion error coefficient, adjusting the covariance of a plurality of error items according to the weight ratio, executing weighted error vector iteration convergence operation, screening a state vector set with the optimal error, and generating a pose positioning set.
5. The unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 4, wherein the attitude change reference value calculates an angular velocity mean value and an angular acceleration mean value in each time window by extracting attitude angle change sequences in a plurality of time windows, and analyzes time integration results of the two.
6. The unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion of claim 1, wherein the environmental modeling module is specifically: the grid dividing sub-module extracts space coordinates of continuous time periods based on the pose positioning set, analyzes three-dimensional grids in a local space according to coordinate point distribution, calculates the point cloud number of multiple grids, screens out invalid empty grids, judges local area boundaries according to point cloud density differences between adjacent grids, and generates local grid distribution data; The mass center calculation sub-module is used for calling the local grid distribution data and the point cloud coordinates in the pose positioning set, calculating a space coordinate mean value, analyzing the geometric center position of a single grid, comparing the space offset of the mass center coordinates of the same grid in continuous frames, analyzing the mass center change vector of each grid and generating a grid mass center change result; And the obstacle judging submodule calculates the ratio of the centroid offset of the adjacent frames to the time interval as a time displacement rate according to the change result of the grid centroid, judges the movement state of the obstacle according to the difference between the time displacement rate and the obstacle displacement threshold, extracts the offset direction vector and calculates the movement speed distribution, and generates local environment data.
7. The unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to claim 6, wherein the obstacle displacement threshold value is obtained by extracting point clouds without obvious motion change in a plurality of frames, calculating a time sequence difference of centroid coordinates, obtaining a displacement distribution range of a static area, normalizing the range and extracting the upper limit of an offset stable interval.
8. The unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion of claim 1, wherein the timing analysis module is specifically: the signal acquisition sub-module is used for acquiring time sequence values of all sensing signals based on the local environment data, extracting amplitude and frequency distribution of multiple signals in the same time window, performing time synchronization, calculating the difference of the signal amplitudes at adjacent moments, recording the change rate and generating multi-source signal difference diversity; The convolution calculation sub-module is used for calling the multi-source signal difference diversity, executing convolution operation on the change sequences of the multi-sensor signals in a plurality of time periods, calculating the energy sum of convolution results, extracting an energy change interval, analyzing the energy fluctuation rate as an energy transfer characteristic among signals and generating energy fluctuation rate distribution; The stability evaluation sub-module calculates the standard deviation of the signal fluctuation rate in multiple time periods according to the energy fluctuation rate distribution, compares the standard deviation with a stability threshold value, extracts the time of a stable interval, counts the average value of the signal fluctuation rate in multiple intervals, calculates the change trend, and generates a time sequence interference table; the stability threshold is set according to a variance range by acquiring a signal sequence during steady-state operation and calculating the mean and variance of the multiple signal fluctuation rates.
9. The unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion of claim 1, wherein the dynamic programming module is specifically: the demand analysis sub-module is used for extracting the track planning demand parameters in the target area based on the time sequence interference table, screening path data meeting the track length and the course limit, counting the corner change value of each path and recording the path node distance to obtain a track characteristic parameter set; The risk assessment sub-module is used for calling the track characteristic parameter set, performing distance comparison on node distances of any two paths, screening path combinations with cross risks, counting the number of the cross nodes, recording node distance values, calculating path collision risk indexes and generating a collision risk assessment result; And the priority adjustment sub-module is used for carrying out sequencing calculation on priority parameters of multiple paths according to the collision risk assessment result, classifying the priority parameters into risk classes according to collision risk values, carrying out priority correction according to signal stability values, and generating a dynamic obstacle avoidance result.
10. The unmanned aerial vehicle positioning and dynamic obstacle avoidance method based on multi-sensor fusion, which is characterized in that the unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion according to any one of claims 1-9 is executed, and comprises the following steps: s1, acquiring visual images, laser point clouds and IMU data, extracting visual features and point cloud features, calculating pre-integral motion quantity, and performing time synchronous fusion on all data to generate multi-source fusion data; s2, calculating visual errors of visual features and point cloud features based on the multi-source fusion data, analyzing motion errors of pre-integration motion amounts, and jointly optimizing the two errors to generate a pose positioning set; s3, dividing grids for the local environment based on the pose positioning set, calculating the centroid change of the point cloud, judging the speed and the direction of the obstacle by combining the displacement threshold of the obstacle, and generating local environment data; s4, based on the local environment data, acquiring all sensing signals, convolutionally analyzing energy variation amplitude, calculating signal stability of multiple time periods, and generating a time sequence interference table; And S5, acquiring a track planning demand analysis candidate track set based on the time sequence interference table, evaluating collision risk for multiple tracks, and adjusting track priority in combination with signal stability to generate a dynamic obstacle avoidance result.

Description

Unmanned aerial vehicle positioning and dynamic obstacle avoidance system and method based on multi-sensor fusion Technical Field 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. Background The technical field of three-dimensional position relates to detection, control and adjustment of the position and the gesture of an object in space, and mainly researches on how to realize stable positioning and motion control of aircrafts, robots, vehicles and the like in three-dimensional space through measurement and control means. The core matters include space coordinate measurement, course angle and gesture calculation, motion track planning, self-adaptive control in dynamic environment and the like. The method has wide application range, covers the directions of aerospace, intelligent unmanned systems, automatic driving, robot control and the like, and generally builds a complete space positioning and controlling system by means of an inertial measurement unit, a satellite navigation system, a visual ranging sensor, a radar ranging and fusion algorithm and the like. The traditional unmanned aerial vehicle positioning and dynamic obstacle avoidance system is a system for determining a flight path of an unmanned aerial vehicle and avoiding obstacles through a single sensor, generally relies on global satellite positioning signals to obtain longitude and latitude and altitude information of the unmanned aerial vehicle, combines a gyroscope and an accelerometer to calculate attitude parameters, adopts an infrared ranging device or an ultrasonic ranging device to detect a distance of a front obstacle in the obstacle avoidance process, and then triggers course correction or speed adjustment according to a preset threshold value to avoid obstacles. The navigation and obstacle avoidance control of the system is generally realized in a complex environment through a preset path planning and fixed threshold judgment mode. In the prior art, a single sensor is relied on to acquire position information and obstacle avoidance is carried out through preset path planning and fixed threshold judgment, the mode is easily affected by dynamic change in a complex environment, and non-static obstacles or suddenly-changed interference signals in the environment are difficult to deal with, so that the positioning and obstacle avoidance precision of a system in a changed environment is reduced. In addition, the traditional positioning system is mostly dependent on a satellite positioning and inertial measurement unit to perform attitude calculation, so that multisource data cannot be effectively integrated, the adaptability of the positioning system in a dynamic environment is poor, and particularly under the condition that signal interference is strong or dead zones exist, the positioning and control system is difficult to ensure stable operation, so that the safety and control capability of an aircraft are limited. Disclosure of Invention In order to solve the technical problems in the prior art, the embodiment of the invention provides an unmanned aerial vehicle positioning and dynamic obstacle avoidance system and method based on multi-sensor fusion. The technical scheme is as follows: in one aspect, an unmanned aerial vehicle positioning and dynamic obstacle avoidance system based on multi-sensor fusion is provided, the system comprising: the multi-source fusion module is used for collecting visual images, laser point clouds and IMU data, extracting visual features and point cloud features, calculating the pre-integration motion quantity, synchronously fusing all data in time, generating multi-source fusion data and transmitting the multi-source fusion data to the error optimization module; The error optimization module is used for calculating visual errors of the visual features and the point cloud features based on the multi-source fusion data, analyzing the motion errors of the pre-integration motion quantity, jointly optimizing the two types of errors, generating a pose positioning set and transmitting the pose positioning set to the environment modeling module; the environment modeling module is used for dividing grids for the local environment based on the pose positioning set, calculating the change of the mass center of the point cloud, judging the speed and the direction of the obstacle by combining the displacement threshold of the obstacle, generating local environment data and transmitting the local environment data to the time sequence analysis module; The time sequence analysis module is used for acquiring all sensing signals based on the local environment data, convoluting and analyzing the energy change amplitude, calculating the signal stability of multiple time periods, generating a time sequence interference table and transmitting the time sequence interf