Unmanned aerial vehicle dynamic path real-time optimization method based on multi-sensor fusion

Abstract

The present application relates to the technical field of unmanned aerial vehicle path optimization, in particular to a dynamic path real-time optimization method for unmanned aerial vehicle based on multi-sensor fusion, comprising the following steps: S100, data acquisition; three-dimensional point cloud data of cable trench environment and unmanned aerial vehicle motion state data are synchronously collected; S200, data processing; the point cloud data and the unmanned aerial vehicle motion state data are jointly processed by using a heterogeneous extended Kalman filtering algorithm based on a VINS framework, and unmanned aerial vehicle state estimation and three-dimensional positioning solution are carried out through a state transition model and an observation model. The present application introduces the laser radar reflection intensity as a "material property dimension", forming a multi-dimensional perception system of "space + material + motion"; the reflection intensity information of the laser radar is used to distinguish the object material, and additional positioning constraints are provided in the area without obvious visual features, such as smooth cable surface, so as to avoid "positioning failure when there is no feature point".

Description

Technical Field

[0001] This invention relates to the field of UAV path optimization technology, specifically a real-time dynamic path optimization method for UAVs based on multi-sensor fusion. Background Technology

[0002] Many existing path planning methods rely on static maps, which typically require building the entire environment model in advance or employing grid-based planning methods. Static path planning performs poorly in narrow and complex cable trench environments, especially when the cable trench structure is complex and contains sudden obstacles, making it unable to quickly adjust flight paths. Therefore, this method lacks the ability to cope with dynamic changes, easily leading to low inspection efficiency and even increasing the risk of equipment damage.

[0003] Traditional positioning methods often rely on GPS signals for spatial positioning, but GPS signals are frequently unavailable or unstable in narrow, enclosed environments such as cable trenches. Furthermore, single-sensor visual SLAM is prone to positioning drift in low-light tunnels due to feature point extraction failures, and inertial sensors also suffer from insufficient accuracy or drift issues, leading to decreased navigation accuracy. This reliance on a single sensor makes it susceptible to environmental changes, especially in complex structures like tunnels, where signal interference and positioning errors often significantly reduce the positioning accuracy of UAVs. Summary of the Invention

[0004] This invention addresses the problems in existing technologies by providing a real-time dynamic path optimization method for unmanned aerial vehicles (UAVs) based on multi-sensor fusion. The specific technical solution is as follows:

[0005] The real-time optimization method for UAV dynamic paths based on multi-sensor fusion includes the following steps:

[0006] S100, data acquisition;

[0007] Simultaneously acquire 3D point cloud data of the cable trench environment and UAV motion status data;

[0008] S200, Data Processing;

[0009] A heterogeneous extended Kalman filter algorithm based on the VINS framework is used to jointly process point cloud data and UAV motion state data. The state estimation and 3D localization solution of the UAV are performed through the state transition model and the observation model.

[0010] S300, dynamic path optimization;

[0011] Based on real-time point cloud data, the flight trajectory is optimized using displacement increment formulas and obstacle avoidance formulas.

[0012] As a further technical solution of the present invention, in step S100:

[0013] The three-dimensional point cloud data includes depth information, RGB color information, spatial coordinate information, and reflection intensity information;

[0014] The depth information and RGB color information are both collected by a depth camera. The depth information is used to identify tunnel walls, cable supports and dynamic obstacles and to determine the distance between the tunnel walls and the obstacles. The RGB color information is matched with the depth information to assign color to the point cloud and identify the appearance features of the obstacles.

[0015] The spatial coordinate information is obtained synchronously with the lidar through depth information calculation and is used to construct the three-dimensional contours of objects such as cable trench walls and cable supports.

[0016] The reflection intensity information is obtained through lidar, and the intensity value of the laser reflected back to the sensor is recorded to distinguish the material of the object.

[0017] As a further technical solution of the present invention, the UAV motion state data includes acceleration information and angular velocity information, both of which are acquired through an IMU.

[0018] As a further technical solution of the present invention, in step S200, the state transition model is as follows:

[0019] ;

[0020] in, This is the state transition function. This is the current state. For the IMU's acceleration and angular velocity, Process noise, This represents the state at the next moment.

[0021] As a further technical solution of the present invention, in step S200, the observation model is:

[0022] ;

[0023] in, For observation models, For the observed values, To observe noise.

[0024] As a further technical solution of the present invention Defined as:

[0025]

[0026] in, For location, For speed, For attitude quaternions, , These are the biases for the accelerometer and gyroscope, respectively.

[0027] As a further technical solution of the present invention, in step S300, the displacement increment formula is as follows:

[0028] ;

[0029] in, This represents the displacement increment at the next moment. At the current speed, For time step, For the target location, The gradient of the repulsive potential field formed by the obstacles. This is the drag coefficient.

[0030] As a further technical solution of the present invention, in step S300, the obstacle avoidance formula is as follows:

[0031] ;

[0032] in, The distance to the obstacle. For path curvature, It is a function of obstacle distance, ensuring that the maximum speed is not exceeded during obstacle avoidance. .

[0033] The beneficial effects of this invention are as follows:

[0034] (1) "Dimensional expansion" of multi-sensor fusion to break through the environmental limitations of a single sensor;

[0035] By introducing the "material property dimension" of lidar reflection intensity, a multi-dimensional perception system of "space + material + motion" is formed;

[0036] The reflection intensity information of lidar is used to distinguish the material of objects and provide additional positioning constraints in areas without obvious visual features, such as smooth cable surfaces, to avoid "positioning failure when there are no feature points".

[0037] The depth camera and LiDAR form a "redundant perception". When the depth camera fails due to low light, the spatial coordinates and reflection intensity data of the LiDAR can be seamlessly replaced to ensure that environmental perception is not interrupted. This "hardware-level complementarity" is designed specifically for the unstable lighting scene of cable trench.

[0038] (2) Data processing "algorithm upgrade" to dynamically adapt to the positioning logic of complex environments;

[0039] This scheme achieves highly robust positioning by improving the heterogeneous EKF, introducing lidar reflection intensity as an additional observation term, adding a "reflection intensity residual term" to the EKF observation model, and using material reflection characteristics to constrain the positioning results, thus solving the problem of "unconstrained drift" in weak texture areas.

[0040] Simultaneously, a dynamic observation weight adjustment mechanism is designed: the sensor weights are adjusted in real time according to the environment to achieve "whoever is reliable is trusted"—this "adaptive trust mechanism" allows positioning to maintain centimeter-level accuracy even in scenarios such as sudden changes in lighting and occlusion, in order to adapt to the narrow and enclosed environment of cable trenches.

[0041] (3) "Scene customization" of path planning, with dual constraints of energy and safety in narrow spaces;

[0042] Dynamic optimization is achieved through the meticulous design of mathematical models. In the displacement increment formula, the dynamic balance between the "gravity of flying towards the target" and the "repulsive force of obstacles" ensures that the path "neither detours nor collides". It is especially suitable for long-distance inspection of cable trenches. Compared with the traditional algorithm that "detours around obstacles", it can effectively improve the endurance.

[0043] Meanwhile, the obstacle avoidance formula directly links the path curvature to the distance to the obstacle, so that the closer the distance, the stricter the curvature restriction. Through the combination of physical constraints and mathematical models, it avoids fuselage collisions caused by sharp turns in narrow spaces. Attached Figure Description

[0044] Figure 1 A flowchart of a real-time dynamic path optimization method for UAVs based on multi-sensor fusion is shown. Detailed Implementation

[0045] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the embodiments.

[0046] The real-time optimization method for UAV dynamic paths based on multi-sensor fusion includes the following steps:

[0047] S100, Data Acquisition: Synchronously acquire 3D point cloud data of the cable trench environment and UAV motion status data; wherein, the 3D point cloud data includes depth information, RGB color information, spatial coordinate information and reflection intensity information; the UAV motion status data includes acceleration information and angular velocity information.

[0048] This step involves collecting two types of core data in real time to form a dual-dimensional information source for the drone: "perceiving the environment + perceiving itself".

[0049] in:

[0050] Depth information, which is acquired by a depth camera, is used to identify tunnel walls, cable supports and dynamic obstacles and to determine the distance between the object and the obstacle. Specifically, the depth camera acquires the depth information of each pixel in the scene, i.e. the distance from the object to the camera, through active or passive means, and generates a depth map. Each pixel value in the depth map represents the distance value of the corresponding position.

[0051] The acquisition of depth information includes, but is not limited to, the use of RGBD cameras, TOF cameras, binocular vision cameras, structured light cameras, hybrid technology cameras, etc. For example, in this embodiment, the depth camera used is an RGBD camera with a resolution of 1280×720 and a frame rate of 30fps.

[0052] RGB color information, which is collected by a depth camera and matched with depth information, is used to assign color to the point cloud, making it easier to identify the appearance features of obstacles and to distinguish "what objects look like", such as black cable brackets and gray cement walls.

[0053] Spatial coordinate information, which is obtained synchronously with the lidar through depth information calculation, is used to construct the three-dimensional contours of objects such as cable trench walls and cable supports; the lidar transmits data through Ethernet communication with a scanning frequency of 10Hz, providing redundant environmental perception data when the point cloud quality of the RGBD camera deteriorates due to low light.

[0054] Reflection intensity information, which is obtained through lidar, records the intensity value of the laser reflected back to the sensor, and is used to distinguish the material of the object;

[0055] Acceleration and angular velocity information are acquired through an IMU (Inertial Measurement Unit); the IMU is a six-axis sensor, namely a three-axis accelerometer and a three-axis gyroscope, which is integrated into the UAV.

[0056] In the S100, the depth camera excels at "detail recognition," such as the appearance of obstacles; the lidar excels at "low-light robustness," such as being able to stably output point clouds even when the light in cable trenches is dim; the two complement each other to solve the "blindness" problem of a single sensor in complex environments.

[0057] The IMU outputs motion data at 100-1000Hz, far exceeding the frame rate of environmental sensors, and can capture the drone's minute movements in real time, providing a "high-frequency dynamic benchmark" for subsequent positioning.

[0058] By using multi-dimensional data such as depth, RGB, spatial coordinates, reflection intensity, and motion parameters, sufficient "raw materials" are provided for subsequent fusion processing, avoiding perception failure caused by the lack of a single data point.

[0059] S200, Data Processing: The heterogeneous extended Kalman filter (EKF) algorithm based on the VINS (Visual Inertial Navigation) framework is used to jointly process point cloud data and UAV motion state data. State estimation and 3D positioning are performed through state transition and observation models.

[0060] This step is based on the VINS framework and improves the traditional EKF algorithm into a "heterogeneous extended Kalman filter". By fusing the point cloud data of the S100 and the IMU data, it outputs the precise status of the UAV and the optimized environmental information; the UAV's status here refers to its position, velocity, and attitude.

[0061] The state transition model is as follows:

[0062] ;

[0063] in, This is the state transition function. This is the current state. For the IMU's acceleration and angular velocity, Process noise, The state at the next moment;

[0064] Based on the current state Through the acceleration and angular velocity of the IMU Predicting the state at the next moment For example, "Current speed is 1 m / s, position moves 1 m after 1 second"; while process noise It reflects and compensates for prediction errors, such as airflow disturbances.

[0065] The observation model is as follows:

[0066] ;

[0067] in, For observation models, For the observed values, To observe noise;

[0068] "Observations" With predicted state If the predicted position deviates significantly from the actual position measured by the lidar, the position is corrected; for example, "the predicted position is at x=5m, the actual position is at x=5.2m, so the position is corrected to 5.1m".

[0069] Defined as:

[0070]

[0071] in: For location, For speed, For attitude quaternions, , These are the biases for the accelerometer and gyroscope, respectively.

[0072] In step S200, the fusion algorithm introduces lidar reflection intensity as an auxiliary observation item based on the existing VINS-EKF, which solves the limitations of a single sensor. When GPS fails, there is no need to rely on satellites, and in low light, there is no need to rely on vision. The reflection intensity supplements the positioning constraints in weak texture areas, and the positioning error can be controlled at the centimeter level.

[0073] Simultaneously, a dynamic observation weight adjustment mechanism is designed to ensure that "data from reliable sensors is given priority." For example, the weight of lidar is increased in low light conditions to avoid the collapse of the overall positioning due to the failure of a single sensor. This adapts to the perception degradation caused by lighting and occlusion in tunnels and solves the limitations of a single sensor in the tunnel environment.

[0074] S300, Dynamic Path Planning: Based on real-time point cloud data, the flight trajectory is optimized using a displacement increment formula and an obstacle avoidance formula. The displacement increment formula is as follows:

[0075] ;

[0076] in, This represents the displacement increment at the next moment. At the current speed, For time step, For the target location, The gradient of the repulsive potential field formed by the obstacles. This is the drag coefficient.

[0077] The gravitational term, which is the force propelling the vehicle towards the target, represents the current velocity. ×Target Direction ×Time step To ensure that the drone does not deviate from the target direction, such as flying towards the end of the cable trench;

[0078] This is the repulsive force term, which represents the resistance to avoid obstacles; the drag coefficient is... × Obstacle repulsive force gradient: The closer the obstacle is, the greater the repulsive force gradient, which forces the drone to deviate away from the obstacle. For example, if there is an obstacle 1m in front, the repulsive force will push the drone to deviate to the side.

[0079] In summary, this means that the drone primarily flies directly to the target throughout its entire flight path, avoiding obstacles only when necessary, ensuring minimal energy consumption and reducing unnecessary detours. Compared to traditional algorithms that "avoid obstacles," this approach can further improve endurance and is particularly suitable for long-distance, multi-obstacle inspection scenarios such as cable trenches.

[0080] The obstacle avoidance formula is as follows:

[0081] ;

[0082] in, The distance to the obstacle. For path curvature, It is a function of obstacle distance, ensuring that the maximum speed is not exceeded during obstacle avoidance. .

[0083] Path curvature Distance from obstacles Correlation, meaning the closer the distance, the smaller the upper limit of curvature, ensuring that "the turning range matches the safe distance", ensuring that the drone "turns smoothly" in narrow spaces, avoiding loss of control of the drone due to sharp turns, and reducing the risk of equipment damage.

[0084] For example, at a distance of 0.5m from an obstacle, the curvature is limited to 0.3m. -1 That is, the turning radius is ≥3.3m, while ensuring that the speed does not exceed the maximum limit.

[0085] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it.

Claims

1. A real-time dynamic path optimization method for UAVs based on multi-sensor fusion, characterized in that, Includes the following steps: S100, data acquisition; Simultaneously collect three-dimensional point cloud data of the cable trench environment and UAV motion status data. The three-dimensional point cloud data includes depth information, RGB color information, spatial coordinate information and reflection intensity information. S200, Data Processing; A heterogeneous extended Kalman filter algorithm based on the VINS framework is used to jointly process point cloud data and UAV motion state data. The state estimation and 3D localization solution of the UAV are performed through the state transition model and the observation model. S300, dynamic path optimization; Based on real-time point cloud data, the flight trajectory is optimized using displacement increment formulas and obstacle avoidance formulas. The formula for the displacement increment is as follows: ； in, This represents the displacement increment at the next moment. At the current speed, For time step, For the target location, The gradient of the repulsive potential field formed by the obstacles. This is the drag coefficient; The obstacle avoidance formula is as follows: ； in, The distance to the obstacle. For path curvature, It is a function of obstacle distance, ensuring that the maximum speed is not exceeded during obstacle avoidance. .

2. The real-time dynamic path optimization method for UAVs based on multi-sensor fusion according to claim 1, characterized in that, In step S100: The depth information and RGB color information are both collected by a depth camera. The depth information is used to identify tunnel walls, cable supports and dynamic obstacles and to determine the distance between the tunnel walls and the obstacles. The RGB color information is matched with the depth information to assign color to the point cloud and identify the appearance features of the obstacles. The spatial coordinate information is obtained synchronously with the lidar through depth information calculation and is used to construct the three-dimensional contours of the cable trench wall and cable support objects. The reflection intensity information is obtained through lidar, and the intensity value of the laser reflected back to the sensor is recorded to distinguish the material of the object.

3. The real-time dynamic path optimization method for UAVs based on multi-sensor fusion according to claim 2, characterized in that, The UAV motion status data includes acceleration information and angular velocity information, both of which are acquired through an IMU.

4. The real-time dynamic path optimization method for UAVs based on multi-sensor fusion according to claim 1, characterized in that, In step S200, the state transition model is as follows: ； in, This is the state transition function. This is the current state. For the IMU's acceleration and angular velocity, Process noise, This represents the state at the next moment.

5. The real-time dynamic path optimization method for UAVs based on multi-sensor fusion according to claim 1, characterized in that, In step S200, the observation model is: ； in, For observation models, For the observed values, To observe noise.

6. The real-time dynamic path optimization method for UAVs based on multi-sensor fusion according to claim 4 or 5, characterized in that, Defined as: in, For location, For speed, For attitude quaternions, , These are the biases for the accelerometer and gyroscope, respectively.

Images