Unmanned aerial vehicle visual positioning flight optimization system based on multi-source data fusion

Abstract

The application discloses a UAV visual positioning flight optimization system based on multi-source data fusion, and belongs to the technical field of UAV navigation and control. The system comprises: a data acquisition module for acquiring RGB-D image data and multiple physical data; an environment modeling module for constructing a local three-dimensional environment map; a path planning module for generating an obstacle-avoiding flight path; an instruction generation module for converting the obstacle-avoiding flight path into attitude control instructions and throttle instructions; a state prediction module for obtaining a UAV predicted state vector at the next moment; a positioning switching module for generating compensated fusion positioning data; and a flight control module for executing the attitude control instructions and the throttle instructions. Through the multi-source data fusion and state prediction compensation mechanism, the application solves the problems of positioning interruption, trajectory divergence and flight instability caused by visual signal loss, track divergence and multi-source coordinate switching mutation in the prior art.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned aerial vehicle navigation and control technology, and particularly relates to an unmanned aerial vehicle visual positioning flight optimization system based on multi-source data fusion. BACKGROUND

[0002] With the increasing demand for operations in urban canyons, indoor and signal-limited areas, unmanned aerial vehicles must rely on multiple sensors such as vision, inertial navigation, UWB, and cellular networks to complete autonomous positioning and flight. Existing visual SLAM systems estimate pose by combining monocular or binocular cameras with inertial navigation, and generate obstacle avoidance trajectories through path planning algorithms. However, when the visual features are sparse or the light changes abruptly, the system output will jump and drift, leading to rapid accumulation of positioning errors.

[0003] In the prior art, visual SLAM still relies on a single visual source in GPS failure scenarios, resulting in the problem of "visual signal loss leading to positioning interruption and trajectory divergence". At the same time, different sensor coordinate systems have large accuracy differences, resulting in position and attitude mutations when switching, which cannot be smoothly transitioned, and flight safety cannot be guaranteed. SUMMARY

[0004] The embodiments of the present application provide an unmanned aerial vehicle visual positioning flight optimization system based on multi-source data fusion, which solves the problems of visual signal loss leading to positioning interruption and trajectory divergence, and flight instability caused by sudden switching of multi-source coordinates in the prior art, and realizes continuous and high-precision positioning and stable flight control in complex environments.

[0005] The embodiments of the present application provide an unmanned aerial vehicle visual positioning flight optimization system based on multi-source data fusion, which includes: a data acquisition module for acquiring real-time RGB-D image data and multiple physical data;

[0006] An environment modeling module is configured to fuse the RGB-D image data and the multiple physical data to construct a local three-dimensional environment map;

[0007] A path planning module is configured to generate an obstacle avoidance flight path based on the local three-dimensional environment map;

[0008] An instruction generation module is configured to convert the obstacle avoidance flight path into attitude control instructions and throttle instructions;

[0009] A state prediction module is configured to obtain a predicted state vector of the unmanned aerial vehicle at the next moment according to a dynamics model of the unmanned aerial vehicle and the attitude control instructions and the throttle instructions;

[0010] A positioning switching module is configured to generate compensated fusion positioning data by using UWB+4G positioning data and the predicted state vector when the visual signal of the unmanned aerial vehicle is lost;

[0011] The steps for generating compensated fused positioning data from UWB+4G positioning data and predicted state vectors include:

[0012] The Frobenius norm of the 6×6 pose covariance matrix of the UAV's visual output is calculated in real time using the Frobenius norm calculation formula:

[0013] ;

[0014] In the formula, The 6×6 visual SLAM pose covariance matrix is... For the matrix of the first Line 1 The covariance elements of the column;

[0015] When the Frobenius norm value is not less than the preset norm threshold, the drone's visual signal is determined to be lost.

[0016] Count the number of successful matches between the current image frame and the map point cloud:

[0017] ;

[0018] In the formula, This represents the total number of feature points that successfully matched the map in the current frame. This represents the total number of candidate feature points extracted in the current frame. For the first The pixel coordinates of each feature point in the image plane. For the first The 3D reprojection coordinates of each feature point in the local map For pixel reprojection error tolerance;

[0019] When the number of matching feature points is less than the preset matching threshold, the drone's visual signal is determined to be lost.

[0020] When the drone's visual signal is lost, compensated fused positioning data is generated by combining UWB+4G positioning data with the predicted state vector.

[0021] Flight control module: used to execute attitude control commands and throttle commands based on compensated fused positioning data and multiple physical data.

[0022] Furthermore, the step of fusing the RGB-D image data and multiple physical data to construct a local three-dimensional environment map includes:

[0023] Receive the acquired RGB-D image data and extract depth and color information from each frame of RGB-D image data;

[0024] Based on the acquired multiple physical data, lidar point cloud data and ultrasonic ranging data are extracted;

[0025] The depth information of consecutive frames is registered by the iterative nearest point algorithm, and the depth information in multiple consecutive frames of RGB-D image data is registered to align the depth images at different times.

[0026] The registered depth information is downsampled and fused using a voxel grid map algorithm to generate a preliminary 3D point cloud map.

[0027] The color information is mapped onto the preliminary 3D point cloud map to form a local 3D point cloud map with texture information.

[0028] The lidar point cloud data and ultrasonic ranging data are weighted and fused with the local 3D point cloud map to obtain a local 3D environment map.

[0029] Further steps for weighted fusion include:

[0030] The lidar point cloud data, ultrasonic ranging data and local 3D point cloud map are weighted and fused using a weighted fusion calculation formula.

[0031] The weighted fusion calculation method is as follows:

[0032] ;

[0033] In the formula, For ambient light intensity, Point cloud density, The light intensity threshold, The point cloud density threshold. Let be the light sensitivity coefficient, where , This represents the upper limit of ambient light intensity. This represents the lower limit of ambient light intensity. Let be the density sensitivity coefficient, where , This represents the upper limit of point cloud density. This represents the lower limit of point cloud density.

[0034] Furthermore, the steps for generating an obstacle avoidance flight path based on a local 3D environment map include:

[0035] Voxelization is performed on the local 3D environment map to generate a 3D raster map;

[0036] The occupancy probability of each grid cell is obtained using the occupancy probability calculation formula.

[0037] The formula for calculating the occupancy probability is:

[0038] ;

[0039] In the formula, For grid cells The probability of occupancy, This is the grid height value. This is the grid height threshold. This is the sensitivity coefficient. It is a natural constant;

[0040] If the occupancy probability of a grid cell is not less than the preset occupancy threshold, then the grid cell is marked as an obstacle area.

[0041] The buffer radius is obtained through the buffer radius calculation formula, and the buffer is then determined based on the buffer radius.

[0042] The formula for calculating the buffer radius is:

[0043] ;

[0044] In the formula, The maximum flight speed of the drone, For safety reasons, The radius of the buffer zone;

[0045] Mark the marked obstacle area and the surrounding buffer zone as a non-passable area;

[0046] Based on the current and target locations of the UAV, as well as the information on the impassable area, an improved path search method using the RRT* algorithm is employed for path planning.

[0047] Furthermore, the steps for path planning using the improved path search method based on the RRT* algorithm include:

[0048] The improved path search method additionally considers the path smoothness factor and energy consumption factor each time a new node is expanded using the RRT* algorithm;

[0049] When selecting the optimal parent node, the optimal path is determined by minimizing the objective function:

[0050] ;

[0051] In the formula, The total length of the planned path is calculated by summing the Euclidean distances between nodes. For reference path length, For path smoothness factor, To estimate energy consumption, For reference energy consumption, , , These are the weighting coefficients.

[0052] Furthermore, the steps for converting the obstacle avoidance flight path into attitude control commands and throttle commands include:

[0053] The obstacle avoidance flight path is decomposed into continuous path points and corresponding desired velocity vectors.

[0054] The required acceleration vector is obtained based on the desired velocity vector at each path point and the current velocity vector of the UAV.

[0055] The acceleration vector is converted into the desired torque and desired thrust of the UAV in a spatial rectangular coordinate system through a nonlinear dynamic inverse control model.

[0056] Based on the desired torque and desired thrust, these are transformed into the desired attitude angle and desired total thrust of the UAV through a nonlinear mapping function. The desired attitude angle includes the roll angle. Pitch angle Yaw angle :

[0057] ;

[0058] In the formula, , , For the desired acceleration components in the three axes of the body coordinate system, It is the acceleration due to gravity. For the quality of drones, This is the current yaw angle of the drone.

[0059] Furthermore, based on the UAV's dynamic model and attitude control and throttle commands, the steps to obtain the predicted UAV state vector for the next moment include:

[0060] Obtain the current state vector of the UAV, which includes the UAV's position vector, velocity vector, and attitude quaternion;

[0061] Based on the acquired attitude control commands and throttle commands, the current state vector and the attitude control commands and throttle commands are used as input variables through a preset set of six-degree-of-freedom dynamic differential equations of the UAV.

[0062] The instantaneous rate of change of the state vector is obtained by solving the system of dynamic differential equations using a numerical integration algorithm.

[0063] Based on the current state vector and the instantaneous rate of change of the state vector, the predicted state vector of the UAV at the next moment is obtained.

[0064] Furthermore, the steps for generating compensated fused positioning data using UWB+4G positioning data and predicted state vectors include:

[0065] When the drone loses its visual signal, it receives UWB positioning data and 4G network-assisted positioning data in real time.

[0066] The received UWB positioning data is smoothed using Gaussian process regression with an improved Matérn3 / 2 kernel function;

[0067] The smoothed UWB positioning data is weighted and fused with 4G network-assisted positioning data to generate preliminary fused positioning data.

[0068] The preliminary fused positioning data and the UAV predicted state vector are fused using an extended Kalman filter to obtain compensated fused positioning data.

[0069] Furthermore, based on the compensated fused positioning data and multiple physical data, the steps for executing attitude control commands and throttle commands include:

[0070] Based on the obtained fused positioning data and multiple physical data, the UAV's position and attitude information in the fused positioning data are compared with the attitude control command and throttle command to obtain the UAV's current position error and attitude error.

[0071] Based on the current position and attitude errors of the UAV, a fuzzy adaptive PID controller is used to fine-tune the attitude control commands. The formula for generating the angular velocity command in the attitude control commands is as follows:

[0072] ;

[0073] In the formula, For the attitude error of the UAV, , , For PID gain, the gain adaptive rule is as follows: ,in For error sensitivity coefficient, Based on the gain, This represents the rate of change of attitude error.

[0074] The finely adjusted attitude control and throttle commands are sent to the drone for execution.

[0075] One or more technical solutions provided in the embodiments of this application have at least the following technical effects or advantages:

[0076] 1. By combining the UAV's six-degree-of-freedom dynamic model with real-time attitude control and throttle commands through the state prediction module, the instantaneous rate of change of the state vector is calculated, and the predicted state vector for the next moment is calculated. This achieves accurate prediction and control of the flight trajectory, effectively solving the problem of positioning jumps and drifts caused by the loss of visual signals in existing technologies.

[0077] 2. By using a multi-source localization method based on vision / UWB / 4G / dynamics prediction, the system can seamlessly switch to a fusion localization mode when UAV visual data is lost, thereby achieving continuous stability of UAV localization in extreme environments and solving the problem of localization interruption caused by missing features or motion blur in visual signals.

[0078] 3. The flight control module combines the fused positioning data with the multi-physical data comparison command generation module to generate attitude control and throttle commands, calculates position and attitude errors in real time, and uses a fuzzy adaptive PID controller to dynamically fine-tune the angular velocity command, thereby achieving closed-loop fine adjustment of the UAV's attitude and thrust, thus enhancing flight stability and safety. Attached Figure Description

[0079] Figure 1 This is a schematic diagram of the structure of a UAV visual positioning and flight optimization system based on multi-source data fusion, provided in an embodiment of this application. Detailed Implementation

[0080] This application provides a UAV visual positioning flight optimization system based on multi-source data fusion, which solves the problems of visual signal loss leading to positioning interruption, trajectory divergence, and flight instability caused by sudden changes in multi-source coordinate switching in the prior art. By fusing visual, inertial, and UWB+4G positioning data in real time and realizing coordinate system alignment and error compensation, it achieves continuous, high-precision positioning and stable flight control in complex environments.

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

[0082] like Figure 1 The diagram shown is a flowchart of a UAV visual positioning flight optimization system based on multi-source data fusion provided in this application embodiment, including: a data acquisition module: used to acquire RGB-D image data and multiple physical data in real time through a binocular camera; the multiple physical data includes angular velocity data and acceleration data acquired through an inertial measurement unit, and barometric altitude data acquired through a barometer;

[0083] Environment modeling module: used to fuse the RGB-D image data and multiple physical data to construct a local 3D environment map and identify obstacles and landing platform markers;

[0084] Path planning module: Used to generate obstacle avoidance flight paths based on local 3D environment maps and obstacle information;

[0085] Command generation module: used to convert the obstacle avoidance flight path into attitude control commands and throttle commands;

[0086] State prediction module: used to obtain the predicted state vector of the UAV at the next moment based on the UAV's dynamic model, attitude control commands, and throttle commands;

[0087] Positioning switching module: used to generate compensated fused positioning data by combining UWB+4G positioning data and predicted state vector when the UAV's visual signal is lost;

[0088] The steps for generating compensated fused positioning data from UWB+4G positioning data and predicted state vectors include:

[0089] The Frobenius norm of the 6×6 pose covariance matrix of the UAV's visual output is calculated in real time using the Frobenius norm calculation formula:

[0090] ;

[0091] In the formula, The 6×6 visual SLAM pose covariance matrix is... For the matrix of the first Line 1 The covariance elements of the column;

[0092] When the Frobenius norm value is not less than the preset norm threshold, the drone's visual signal is determined to be lost.

[0093] Count the number of successful matches between the current image frame and the map point cloud:

[0094] ;

[0095] In the formula, This represents the total number of feature points that successfully matched the map in the current frame. This represents the total number of candidate feature points extracted in the current frame. For the first The pixel coordinates of each feature point in the image plane. For the first The 3D reprojection coordinates of each feature point in the local map For pixel reprojection error tolerance;

[0096] When the number of matching feature points is less than the preset matching threshold, the drone's visual signal is determined to be lost.

[0097] When the drone's visual signal is lost, compensated fused positioning data is generated by combining UWB+4G positioning data with the predicted state vector.

[0098] Flight control module: Used to execute attitude control commands and throttle commands to control the flight of the UAV based on compensated fused positioning data and multiple physical data.

[0099] Furthermore, the step of fusing the RGB-D image data and multiple physical data to construct a local three-dimensional environment map includes:

[0100] The environment modeling module receives RGB-D image data acquired by the data acquisition module, obtains RGB-D image data sequences, and extracts depth and color information from each frame of RGB-D image data.

[0101] Based on the acquired multiple physical data, lidar point cloud data and ultrasonic ranging data are extracted;

[0102] The depth information of consecutive frames is registered by the iterative nearest point algorithm, and the depth information in multiple consecutive frames of RGB-D image data is registered to align the depth images at different times.

[0103] The registered depth information is downsampled and fused using a voxel grid map algorithm to generate a preliminary 3D point cloud map.

[0104] The color information is mapped onto the preliminary 3D point cloud map to form a local 3D point cloud map with texture information.

[0105] The lidar point cloud data and ultrasonic ranging data are weighted and fused with the local 3D point cloud map to obtain a local 3D environment map.

[0106] Further steps for weighted fusion include:

[0107] The lidar point cloud data, ultrasonic ranging data and local 3D point cloud map are weighted and fused using a weighted fusion calculation formula.

[0108] The weighted fusion calculation method is as follows:

[0109] ;

[0110] In the formula, Ambient light intensity (unit: lux). Point cloud density (unit: points / m³). Light intensity threshold (unit: lux). Point cloud density threshold (unit: points / m³). Light sensitivity coefficient (unit: ),in, , This represents the upper limit of ambient light intensity. This represents the lower limit of ambient light intensity. Here, is the density sensitivity coefficient (unit: m³ / points), where, , This represents the upper limit of point cloud density. This represents the lower limit of point cloud density.

[0111] Furthermore, the steps for generating an obstacle avoidance flight path based on a local 3D environment map include:

[0112] Voxelization is performed on the local 3D environment map to generate a 3D raster map;

[0113] The occupancy probability of each grid cell is obtained using the occupancy probability calculation formula.

[0114] The formula for calculating the occupancy probability is:

[0115] ;

[0116] In the formula, For grid cells The probability of occupancy, This is the grid height value. This is the grid height threshold. This is the sensitivity coefficient. It is a natural constant;

[0117] If the occupancy probability of a grid cell is not less than the preset occupancy threshold, then the grid cell is marked as an obstacle area.

[0118] The buffer radius is obtained through the buffer radius calculation formula, and the buffer is then determined based on the buffer radius.

[0119] The formula for calculating the buffer radius is:

[0120] ;

[0121] In the formula, The maximum flight speed of the drone, For safety reasons, The radius of the buffer zone;

[0122] The method for obtaining the buffer based on the buffer radius is as follows:

[0123] Center point of obstacle grid Construct a spherical buffer zone with the center of the sphere as the center:

[0124] ;

[0125] In the formula, For a set of points in three-dimensional space, The coordinates of any point in three-dimensional space;

[0126] Set the marked obstacle area and the surrounding buffer zone as a non-passable area;

[0127] Based on the current and target locations of the UAV, as well as the information on the impassable area, an improved path search method using the RRT* algorithm is employed for path planning.

[0128] Furthermore, the steps for path planning using the improved path search method based on the RRT* algorithm are as follows:

[0129] The improved path search method additionally considers the path smoothness factor and energy consumption factor each time a new node is expanded using the RRT* algorithm;

[0130] When selecting the optimal parent node, the optimal path is determined by minimizing the objective function:

[0131] ;

[0132] In the formula, The total planned path length (in meters) is calculated by summing the Euclidean distances between nodes. Reference path length (unit: m). The path smoothness factor (dimensionless) is calculated as the cosine of the angle between adjacent path segments: , To estimate energy consumption (in J), the following is obtained by integrating the flight power: ,in, For the power function of the drone, For reference energy consumption (unit: J), the gravitational potential energy benchmark value is used. , , , These are the weighting coefficients.

[0133] Furthermore, the steps for converting the obstacle avoidance flight path into attitude control commands and throttle commands include:

[0134] The obstacle avoidance flight path is decomposed into continuous path points and corresponding desired velocity vectors.

[0135] The required acceleration vector is obtained based on the desired velocity vector at each path point and the current velocity vector of the UAV.

[0136] The method for obtaining the acceleration vector is as follows:

[0137] Calculate the expected velocity vector of adjacent path points:

[0138] ;

[0139] In the formula, For the first The expected velocity vector of the path segment. For the first The three-dimensional coordinates of the path points For the first The three-dimensional coordinates of the path points For fixed time steps;

[0140] Read the current speed of the drone Then, calculate the difference to obtain the required acceleration vector:

[0141] ;

[0142] In the formula, For the first The required acceleration vector of the path segment. This represents the current velocity vector of the drone.

[0143] The acceleration vector is converted into the desired torque and desired thrust of the UAV in a spatial rectangular coordinate system through a nonlinear dynamic inverse control model.

[0144] Based on the desired torque and desired thrust, these are transformed into the desired attitude angle and desired total thrust of the UAV through a nonlinear mapping function. The desired attitude angle includes the roll angle. Pitch angle Yaw angle :

[0145] ;

[0146] In the formula, , , The desired acceleration is expressed as the three-axis components in the body coordinate system (unit: m / s²). This is the acceleration due to gravity (unit: m / s²). Mass of the drone (unit: kg) This is the current yaw angle of the drone (unit: rad).

[0147] Furthermore, based on the UAV's dynamic model and attitude control and throttle commands, the steps to obtain the predicted UAV state vector for the next moment include:

[0148] Obtain the current state vector of the UAV, which includes the UAV's position vector, velocity vector, and attitude quaternion;

[0149] Based on the acquired attitude control commands and throttle commands, the current state vector and the attitude control commands and throttle commands are used as input variables through a preset set of six-degree-of-freedom dynamic differential equations of the UAV.

[0150] The six-degree-of-freedom dynamic differential equations of the UAV are as follows:

[0151] ;

[0152] In the formula, This is the UAV's position vector (unit: m). This is the velocity vector of the UAV (unit: m / s). For attitude quaternions, This is the rotation matrix from the body coordinate system to the Earth coordinate system. This represents the total thrust (in N) corresponding to the throttle command. Angular velocity (unit: rad / s) for attitude control command conversion. Mass of the drone (unit: kg) Air density (unit: kg / m³). This is the aerodynamic drag coefficient. Equivalent cross-sectional area (unit: m²);

[0153] The instantaneous rate of change of the state vector is obtained by solving the system of dynamic differential equations using a numerical integration algorithm.

[0154] Based on the current state vector and the instantaneous rate of change of the state vector, the predicted state vector of the UAV at the next moment is obtained;

[0155] The state prediction module outputs a complete state vector containing the UAV's position prediction, velocity prediction, and attitude prediction.

[0156] Furthermore, the steps for generating compensated fused positioning data using UWB+4G positioning data and predicted state vectors include:

[0157] When the drone loses its visual signal, a backup positioning source is activated to receive UWB positioning data and 4G network-assisted positioning data in real time.

[0158] The received UWB positioning data is smoothed using Gaussian process regression with an improved Matérn3 / 2 kernel function;

[0159] The improved Matérn3 / 2 kernel function is as follows:

[0160] ;

[0161] In the formula, Time difference (unit: seconds) The signal variance is expressed in meters (m). The time constant (in seconds) is the update frequency from UWB. Sure, ;

[0162] The smoothed UWB positioning data is weighted and fused with 4G network-assisted positioning data to generate preliminary fused positioning data.

[0163] The preliminary fused positioning data and the UAV predicted state vector are fused using an extended Kalman filter to obtain compensated fused positioning data.

[0164] Furthermore, based on the compensated fused positioning data and multiple physical data, the steps for executing attitude control commands and throttle commands include:

[0165] Based on the obtained fused positioning data and multiple physical data, the UAV's position and attitude information in the fused positioning data are compared with the attitude control command and throttle command to obtain the UAV's current position error and attitude error.

[0166] The formula for calculating the current position error of the drone is:

[0167] ;

[0168] In the formula, The coordinates of the current path point output by the instruction generation module. To compensate for the drone's position coordinates after fusion positioning;

[0169] The formula for calculating the current attitude error of the UAV is:

[0170] ;

[0171] In the formula, The quaternion of the expected attitude of the UAV provided by the instruction generation module. To compensate for the current attitude quaternion of the UAV after fusion positioning;

[0172] Based on the current position and attitude errors of the UAV, a fuzzy adaptive PID controller is used to fine-tune the attitude control commands. The formula for generating the angular velocity command in the attitude control commands is as follows:

[0173] ;

[0174] In the formula, For the attitude error of the UAV, , , For PID gain, the gain adaptive rule is as follows: ,in For error sensitivity coefficient, Based on the gain, This represents the rate of change of attitude error.

[0175] The finely adjusted attitude control and throttle commands are sent to the drone for execution.

[0176] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0177] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0178] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0179] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1The steps of the function specified in one or more boxes.

[0180] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the invention.

[0181] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A UAV visual positioning and flight optimization system based on multi-source data fusion, characterized in that, include: Data acquisition module: used to acquire RGB-D image data and multiple physical data in real time; Environment modeling module: used to fuse the RGB-D image data and multiple physical data to construct a local 3D environment map; Path planning module: Used to generate obstacle avoidance flight paths based on local 3D environment maps; Command generation module: used to convert the obstacle avoidance flight path into attitude control commands and throttle commands; State prediction module: used to obtain the predicted state vector of the UAV at the next moment based on the UAV's dynamic model, attitude control commands, and throttle commands; Positioning switching module: used to generate compensated fused positioning data by combining UWB+4G positioning data and predicted state vector when the UAV's visual signal is lost; The steps for generating compensated fused positioning data from UWB+4G positioning data and predicted state vectors include: The Frobenius norm of the 6×6 pose covariance matrix of the UAV's visual output is calculated in real time using the Frobenius norm calculation formula: ； In the formula, The 6×6 visual SLAM pose covariance matrix is... For the matrix of the first Line number The covariance elements of the column; When the Frobenius norm value is not less than the preset norm threshold, the drone's visual signal is determined to be lost. Count the number of successful matches between the current image frame and the map point cloud: ； In the formula, This represents the total number of feature points that successfully matched the map in the current frame. This represents the total number of candidate feature points extracted in the current frame. For the first The pixel coordinates of each feature point in the image plane. For the first The 3D reprojection coordinates of each feature point in the local map For pixel reprojection error tolerance; When the number of matching feature points is less than the preset matching threshold, the drone's visual signal is determined to be lost. When the drone's visual signal is lost, compensated fused positioning data is generated by combining UWB+4G positioning data with the predicted state vector. Flight control module: used to execute attitude control commands and throttle commands based on compensated fused positioning data and multiple physical data.

2. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 1, characterized in that, The steps for constructing a local 3D environment map by integrating the RGB-D image data and multiple physical data include: Receive the acquired RGB-D image data and extract depth and color information from each frame of RGB-D image data; Based on the acquired multiple physical data, lidar point cloud data and ultrasonic ranging data are extracted; The depth information of consecutive frames is registered by the iterative nearest point algorithm, and the depth information in multiple consecutive frames of RGB-D image data is registered to align the depth images at different times. The registered depth information is downsampled and fused using a voxel grid map algorithm to generate a preliminary 3D point cloud map. The color information is mapped onto the preliminary 3D point cloud map to form a local 3D point cloud map with texture information. The lidar point cloud data and ultrasonic ranging data are weighted and fused with the local 3D point cloud map to obtain a local 3D environment map.

3. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 2, characterized in that, The steps for weighted fusion include: The lidar point cloud data, ultrasonic ranging data and local 3D point cloud map are weighted and fused using a weighted fusion calculation formula. The weighted fusion calculation method is as follows: ； In the formula, For ambient light intensity, Point cloud density, The light intensity threshold, The point cloud density threshold. Let be the light sensitivity coefficient, where , This represents the upper limit of ambient light intensity. This represents the lower limit of ambient light intensity. Let be the density sensitivity coefficient, where , This represents the upper limit of point cloud density. This represents the lower limit of point cloud density.

4. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 1, characterized in that, The steps for generating an obstacle avoidance flight path based on a local 3D environment map include: Voxelization is performed on the local 3D environment map to generate a 3D raster map; The occupancy probability of each grid cell is obtained using the occupancy probability calculation formula. The formula for calculating the occupancy probability is: ； In the formula, For grid cells The probability of occupancy, This is the grid height value. This is the grid height threshold. This is the sensitivity coefficient. It is a natural constant; If the occupancy probability of a grid cell is not less than the preset occupancy threshold, then the grid cell is marked as an obstacle area. The buffer radius is obtained through the buffer radius calculation formula, and the buffer is then determined based on the buffer radius. The formula for calculating the buffer radius is: ； In the formula, The maximum flight speed of the drone, For safety reasons, The radius of the buffer zone; Mark the marked obstacle area and the surrounding buffer zone as a non-passable area; Based on the current and target locations of the UAV, as well as the information on the impassable area, an improved path search method using the RRT* algorithm is employed for path planning.

5. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 4, characterized in that, The steps of the improved path search method using the RRT* algorithm for path planning include: The improved path search method additionally considers the path smoothness factor and energy consumption factor each time a new node is expanded using the RRT* algorithm; When selecting the optimal parent node, the optimal path is determined by minimizing the objective function: ； In the formula, The total length of the planned path is calculated by summing the Euclidean distances between nodes. For reference path length, For path smoothness factor, To estimate energy consumption, For reference energy consumption, , , These are the weighting coefficients.

6. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 1, characterized in that, The steps to convert the obstacle avoidance flight path into attitude control commands and throttle commands include: The obstacle avoidance flight path is decomposed into continuous path points and corresponding desired velocity vectors. The required acceleration vector is obtained based on the desired velocity vector at each path point and the current velocity vector of the UAV. The acceleration vector is converted into the desired torque and desired thrust of the UAV in a spatial rectangular coordinate system through a nonlinear dynamic inverse control model. Based on the desired torque and desired thrust, these are transformed into the desired attitude angle and desired total thrust of the UAV through a nonlinear mapping function. The desired attitude angle includes the roll angle. Pitch angle Yaw angle : ； In the formula, , , For the desired acceleration components in the three axes of the body coordinate system, It is the acceleration due to gravity. For the quality of drones, This is the current yaw angle of the drone.

7. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 1, characterized in that, The steps to obtain the predicted state vector of the UAV at the next moment, based on the UAV's dynamic model, attitude control commands, and throttle commands, include: Obtain the current state vector of the UAV, which includes the UAV's position vector, velocity vector, and attitude quaternion; Based on the acquired attitude control commands and throttle commands, the current state vector and the attitude control commands and throttle commands are used as input variables through a preset set of six-degree-of-freedom dynamic differential equations of the UAV. The instantaneous rate of change of the state vector is obtained by solving the system of dynamic differential equations using a numerical integration algorithm. Based on the current state vector and the instantaneous rate of change of the state vector, the predicted state vector of the UAV at the next moment is obtained.

8. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 1, characterized in that, The steps for generating compensated fused positioning data from UWB+4G positioning data and predicted state vectors include: When the drone loses its visual signal, it receives UWB positioning data and 4G network-assisted positioning data in real time. The received UWB positioning data is smoothed using Gaussian process regression with an improved Matérn3 / 2 kernel function; The smoothed UWB positioning data is weighted and fused with 4G network-assisted positioning data to generate preliminary fused positioning data. The preliminary fused positioning data and the UAV predicted state vector are fused using an extended Kalman filter to obtain compensated fused positioning data.

9. The UAV visual positioning and flight optimization system based on multi-source data fusion as described in claim 1, characterized in that, Based on the compensated fused positioning data and multiple physical data, the steps for executing attitude control commands and throttle commands include: Based on the obtained fused positioning data and multiple physical data, the UAV's position and attitude information in the fused positioning data are compared with the attitude control command and throttle command to obtain the UAV's current position error and attitude error. Based on the current position and attitude errors of the UAV, a fuzzy adaptive PID controller is used to fine-tune the attitude control commands. The formula for generating the angular velocity command in the attitude control commands is as follows: ； In the formula, For the attitude error of the UAV, , , For PID gain, the gain adaptive rule is as follows: ,in For error sensitivity coefficient, Based on the gain, This represents the rate of change of attitude error. The finely adjusted attitude control and throttle commands are sent to the drone for execution.

Images