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

Abstract

The invention discloses an unmanned aerial vehicle visual positioning flight optimization system based on multi-source data fusion, and belongs to the technical field of unmanned aerial vehicle navigation and control. The system comprises a data acquisition module used for acquiring RGB-D image data and multiple physical data; the environment modeling module is used for constructing a local three-dimensional environment map; the path planning module is used for generating an obstacle avoidance flight path; the instruction generation module is used for converting the obstacle avoidance flight path into an attitude control instruction and an accelerator instruction; the state prediction module is used for obtaining an unmanned aerial vehicle prediction state vector at the next moment; the positioning switching module is used for generating compensated fusion positioning data; and the flight control module is used for executing the attitude control instruction and the accelerator instruction. Through the multi-source data fusion and state prediction compensation mechanism, the problems of positioning interruption and trajectory divergence caused by loss of visual signals and unstable flight caused by sudden change of multi-source coordinate switching in the prior art are solved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) navigation and control technology, and in particular to a UAV visual positioning flight optimization system based on multi-source data fusion. Background Art

[0002] With the increasing demand for operations in urban canyons, indoor environments, and areas with limited signal availability, drones must rely on multiple sensor sources, including vision, inertial navigation, UWB, and cellular networks, for autonomous positioning and flight. Existing visual SLAM systems use monocular or binocular cameras combined with inertial navigation to estimate pose and generate obstacle avoidance trajectories through path planning algorithms. However, when visual features are sparse or illumination changes suddenly, the system output can jump and drift, causing positioning errors to accumulate rapidly.

[0003] In the existing technology, visual SLAM still relies on a single visual source in the event of GPS failure, and there is a problem of "loss of visual signal means positioning interruption and trajectory divergence"; at the same time, the accuracy of different sensor coordinate systems varies greatly, and position and posture mutations occur during switching, which makes smooth transition impossible and flight safety difficult to guarantee. Summary of the Invention

[0004] The embodiments of the present application provide a UAV visual positioning flight optimization system based on multi-source data fusion, which solves the problems in the prior art such as positioning interruption caused by loss of visual signals, trajectory divergence, and unstable flight caused by sudden changes in multi-source coordinate switching, thereby achieving continuous, high-precision positioning and stable flight control in complex environments.

[0005] The embodiment of the present application provides a UAV visual positioning flight optimization system based on multi-source data fusion, including: a data acquisition module: for acquiring RGB-D image data and multiple physical data in real time;

[0006] Environmental modeling module: used to fuse the RGB-D image data and multiple physical data to construct a local three-dimensional environmental map;

[0007] Path planning module: used to generate obstacle avoidance flight paths based on local 3D environment maps;

[0008] Instruction generation module: used to convert the obstacle avoidance flight path into attitude control instructions and throttle instructions;

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

[0010] Positioning switching module: used to generate compensated fusion positioning data through UWB+4G positioning data and predicted state vector when the UAV visual signal is lost;

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

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

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

[0014] Extract lidar point cloud data and ultrasonic ranging data based on the acquired multi-physical data;

[0015] The depth information of consecutive frames is registered by iterative closest point algorithm, the depth information in consecutive multiple frames of RGB-D image data is registered, and the depth images at different times are aligned;

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

[0017] Mapping the color information onto the preliminary three-dimensional point cloud map to form a local three-dimensional point cloud map with texture information;

[0018] The laser radar point cloud data and ultrasonic ranging data are weightedly fused with the local three-dimensional point cloud map to obtain a local three-dimensional environment map.

[0019] Furthermore, the steps of weighted fusion include:

[0020] The weighted fusion calculation formula is used to perform weighted fusion of the lidar point cloud data, ultrasonic ranging data and the local three-dimensional point cloud map;

[0021] The calculation method of weighted fusion is:

[0022] ;

[0023] Where, is the ambient light intensity, is the point cloud density, is the light intensity threshold, is the point cloud density threshold, is the light sensitivity coefficient, where , is the upper limit of ambient light intensity, is the lower limit of ambient light intensity, is the density sensitivity coefficient, where , is the upper limit of point cloud density, is the lower limit of the point cloud density.

[0024] Furthermore, the steps of generating an obstacle avoidance flight path based on the local three-dimensional environment map include:

[0025] Perform voxel processing on the local three-dimensional environment map to generate a three-dimensional grid map;

[0026] The occupancy probability of each grid is obtained through the occupancy probability calculation formula;

[0027] The occupation probability calculation formula is:

[0028] ;

[0029] Where, Grid cells The probability of occupation, is the grid height value, is the grid height threshold, is the sensitivity coefficient, is a natural constant;

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

[0031] The buffer radius is obtained by the buffer radius calculation formula, and the buffer zone is obtained based on the buffer radius;

[0032] The calculation formula for the buffer zone radius is:

[0033] ;

[0034] Where, is the maximum flight speed of the drone, is the safety factor, is the buffer zone radius;

[0035] Mark the marked obstacle area and the buffer zone around the obstacle area as an impassable area;

[0036] Based on the current position and target position of the UAV and the information of the impassable area, an improved path search method of the RRT* algorithm is used for path planning.

[0037] Furthermore, the steps of performing path planning using the improved path search method of the RRT* algorithm include:

[0038] The improved path search method is to additionally consider the path smoothness factor and energy consumption factor each time a new node is expanded through the RRT* algorithm;

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

[0040] ;

[0041] Where, is the total length of the planned path, calculated by accumulating the Euclidean distance between nodes. is the reference path length, is the path smoothness factor, To estimate energy consumption, is the reference energy consumption, 、 、 are weight coefficients respectively.

[0042] Furthermore, the steps of converting the obstacle avoidance flight path into attitude control instructions and throttle instructions include:

[0043] Decomposing the obstacle avoidance flight path into continuous path points and corresponding desired velocity vectors;

[0044] The required acceleration vector is obtained based on the desired velocity vector of each path point and the current velocity vector of the drone;

[0045] 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;

[0046] Based on the desired torque and desired thrust, the nonlinear mapping function is used to convert the desired attitude angle and desired total thrust of the UAV into , the desired attitude angle includes the roll angle , pitch angle , yaw angle :

[0047] ;

[0048] Where, 、 、 is the three-axis component of the desired acceleration in the body coordinate system, is the acceleration due to gravity, For drone quality, The current yaw angle of the drone.

[0049] Furthermore, the step of obtaining the predicted state vector of the drone at the next moment according to the drone's dynamic model, attitude control instructions, and throttle instructions includes:

[0050] Obtain the current state vector of the drone, which includes the position vector, velocity vector, and attitude quaternion of the drone;

[0051] Based on the acquired attitude control command and throttle command, a preset six-degree-of-freedom dynamic differential equation group of the UAV is used, with the current state vector, the attitude control command and the throttle command as input variables;

[0052] Solving the dynamic differential equations by a numerical integration algorithm to obtain the instantaneous rate of change of the state vector;

[0053] According to the instantaneous rate of change of the current state vector and the state vector, the predicted state vector of the drone at the next moment is obtained.

[0054] Furthermore, when the UAV visual signal is lost, the steps of generating compensated fused positioning data using UWB+4G positioning data and predicted state vector include:

[0055] The Frobenius norm calculation formula is used to calculate the Frobenius norm of the 6×6 pose covariance matrix of the UAV visual output in real time:

[0056] ;

[0057] Where, is the 6×6 visual SLAM pose covariance matrix, For the matrix Rank covariance elements of the columns;

[0058] When the Frobenius norm value is not less than the preset norm threshold, it is determined that the drone’s visual signal is lost;

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

[0060] ;

[0061] Where, is the total number of feature points that are successfully matched between the current frame and the map, is the total number of candidate feature points extracted for the current frame, For the The pixel coordinates of the feature points in the image plane, For the The 3D reprojected coordinates of feature points in the local map, is the pixel reprojection error tolerance;

[0062] When the number of feature point matches is less than the preset matching threshold, it is determined that the drone's visual signal is lost;

[0063] When the drone's visual signal is lost, the compensated fused positioning data is generated through UWB+4G positioning data and the predicted state vector.

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

[0065] When the drone's visual signal is lost, it receives UWB positioning data and 4G network-assisted positioning data in real time;

[0066] The received UWB positioning data is smoothed using the improved Matérn3 / 2 kernel function through Gaussian process regression.

[0067] The smoothed UWB positioning data is weightedly fused with the 4G network-assisted positioning data to generate preliminary fused positioning data;

[0068] The preliminary fused positioning data is fused with the UAV predicted state vector through an extended Kalman filter to obtain compensated fused positioning data.

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

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

[0071] Based on the current position error and attitude error of the UAV, the attitude control command is fine-tuned through the fuzzy adaptive PID controller. The angular velocity command in the attitude control command is generated by the following formula:

[0072] ;

[0073] Where, is the UAV attitude error, 、 、 is the PID gain, and the gain adaptation rule is ,in is the error sensitivity coefficient, is the basic gain, is the attitude error change rate;

[0074] The fine-tuned attitude control commands 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. The state prediction module combines the UAV's six-degree-of-freedom dynamics model with real-time attitude control commands and throttle commands to calculate the instantaneous rate of change of the state vector and infer the predicted state vector at the next moment, thereby achieving accurate prediction and control of the flight trajectory, effectively solving the problem of positioning jump and drift caused by loss of visual signals in existing technologies.

[0077] 2. Through the multi-source positioning method of vision / UWB / 4G / dynamic prediction, it seamlessly switches to fusion positioning mode when the drone's visual data is lost, thereby achieving continuous stability of drone positioning in extreme environments and solving the problem of positioning interruption caused by missing features or motion blur of visual signals.

[0078] 3. The flight control module compares the fused positioning data with multiple physical data to generate the attitude control and throttle commands from the command generation module, calculates the position error and attitude error 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, thereby enhancing flight stability and safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 A schematic diagram of the structure of a UAV visual positioning flight optimization system based on multi-source data fusion provided in an embodiment of the present application. DETAILED DESCRIPTION

[0080] The embodiments of the present application provide a UAV visual positioning flight optimization system based on multi-source data fusion, which solves the problems in the prior art caused by visual signal loss, i.e. positioning interruption, trajectory divergence, and multi-source coordinate switching mutations leading to unstable flight. By integrating visual, inertial, and UWB+4G positioning data in real time and realizing coordinate system alignment and error compensation, continuous, high-precision positioning and smooth flight control in complex environments are achieved.

[0081] In order to better understand the above technical solution, the above technical solution will be described in detail below with reference to the accompanying drawings and specific implementation methods.

[0082] like Figure 1 , which is a flow chart of a UAV visual positioning flight optimization system based on multi-source data fusion provided by an embodiment of the present application, 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 air pressure altitude data acquired through a barometer;

[0083] Environmental modeling module: used to fuse the RGB-D image data and multiple physical data to build a local three-dimensional environmental map and identify obstacles and landing platform signs;

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

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

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

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

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

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

[0090] The environment modeling module receives the RGB-D image data obtained by the data acquisition module, obtains an RGB-D image data sequence, and extracts depth information and color information from each frame of the RGB-D image data;

[0091] Extract lidar point cloud data and ultrasonic ranging data based on the acquired multi-physical data;

[0092] The depth information of consecutive frames is registered by iterative closest point algorithm, the depth information in consecutive multiple frames of RGB-D image data is registered, and the depth images at different times are aligned;

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

[0094] Mapping the color information onto the preliminary three-dimensional point cloud map to form a local three-dimensional point cloud map with texture information;

[0095] The laser radar point cloud data and ultrasonic ranging data are weightedly fused with the local three-dimensional point cloud map to obtain a local three-dimensional environment map.

[0096] Furthermore, the steps of weighted fusion include:

[0097] The weighted fusion calculation formula is used to perform weighted fusion of the lidar point cloud data, ultrasonic ranging data and the local three-dimensional point cloud map;

[0098] The calculation method of weighted fusion is:

[0099] ;

[0100] Where, is the ambient light intensity (unit: lux), is the point cloud density (unit: points / m³), is the light intensity threshold (unit: lux), is the point cloud density threshold (unit: points / m³), is the light sensitivity coefficient (unit: ),in, , is the upper limit of ambient light intensity, is the lower limit of ambient light intensity, is the density sensitivity coefficient (unit: m³ / points), where , is the upper limit of point cloud density, is the lower limit of the point cloud density.

[0101] Furthermore, the steps of generating an obstacle avoidance flight path based on the local three-dimensional environment map include:

[0102] Perform voxel processing on the local three-dimensional environment map to generate a three-dimensional grid map;

[0103] The occupancy probability of each grid is obtained through the occupancy probability calculation formula;

[0104] The occupation probability calculation formula is:

[0105] ;

[0106] Where, Grid cells The probability of occupation, is the grid height value, is the grid height threshold, is the sensitivity coefficient, is a natural constant;

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

[0108] The buffer radius is obtained by the buffer radius calculation formula, and the buffer zone is obtained based on the buffer radius;

[0109] The calculation formula for the buffer zone radius is:

[0110] ;

[0111] Where, is the maximum flight speed of the drone, is the safety factor, is the buffer zone radius;

[0112] The method to obtain the buffer zone based on the buffer zone radius is:

[0113] Obstacle grid center point As the center of the sphere, construct a spherical buffer area:

[0114] ;

[0115] Where, is a set of three-dimensional points, is the coordinate of any point in three-dimensional space;

[0116] Set the marked obstacle area and the buffer zone around the obstacle area as an impassable area;

[0117] Based on the current position and target position of the UAV and the information of the impassable area, an improved path search method of the RRT* algorithm is used for path planning.

[0118] Furthermore, the steps of path planning using the improved path search method of the RRT* algorithm are as follows:

[0119] The improved path search method is to additionally consider the path smoothness factor and energy consumption factor each time a new node is expanded through the RRT* algorithm;

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

[0121] ;

[0122] Where, is the total length of the planned path (unit: m), calculated by accumulating the Euclidean distance between nodes. is the reference path length (unit: m), is the path smoothness factor (dimensionless), calculated as the cosine of the angle between adjacent path segments: , To estimate energy consumption (unit: J), integrate the flight power to obtain: ,in, is the UAV power function, is the reference energy consumption (unit: J), taking the gravitational potential energy benchmark value , 、 、 are weight coefficients respectively.

[0123] Furthermore, the steps of converting the obstacle avoidance flight path into attitude control instructions and throttle instructions include:

[0124] Decomposing the obstacle avoidance flight path into continuous path points and corresponding desired velocity vectors;

[0125] The required acceleration vector is obtained based on the desired velocity vector of each path point and the current velocity vector of the drone;

[0126] The method for obtaining the acceleration vector is:

[0127] Calculate the expected velocity vectors of adjacent path points:

[0128] ;

[0129] Where, For the The desired velocity vector for the segment path, For the The three-dimensional coordinates of the path points, For the The three-dimensional coordinates of the path points, is a fixed time step;

[0130] Read the current speed of the drone And take the difference to get the required acceleration vector:

[0131] ;

[0132] Where, For the The desired acceleration vector for the segment path, is the current velocity vector of the UAV;

[0133] 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;

[0134] Based on the desired torque and desired thrust, the nonlinear mapping function is used to convert the desired attitude angle and desired total thrust of the UAV into , the desired attitude angle includes the roll angle , pitch angle , yaw angle :

[0135] ;

[0136] Where, 、 、 is the expected acceleration in the three-axis component of the body coordinate system (unit: m / s²), is the acceleration due to gravity (unit: m / s²), is the mass of the drone (unit: kg), The current yaw angle of the drone (unit: rad).

[0137] Furthermore, the step of obtaining the predicted state vector of the drone at the next moment according to the drone's dynamic model, attitude control instructions, and throttle instructions includes:

[0138] Obtain the current state vector of the drone, which includes the position vector, velocity vector, and attitude quaternion of the drone;

[0139] Based on the acquired attitude control command and throttle command, a preset six-degree-of-freedom dynamic differential equation group of the UAV is used, with the current state vector, the attitude control command and the throttle command as input variables;

[0140] The six-degree-of-freedom dynamic differential equations of the UAV are:

[0141] ;

[0142] Where, is the drone position vector (unit: m), is the UAV velocity vector (unit: m / s), is the attitude quaternion, is the rotation matrix from the body coordinate system to the earth coordinate system, is the total thrust corresponding to the throttle command (unit: N), is the angular velocity converted from attitude control instructions (unit: rad / s), is the mass of the drone (unit: kg), is the air density (unit: kg / m³), is the aerodynamic drag coefficient, is the equivalent cross-sectional area (unit: m²);

[0143] Solving the dynamic differential equations by a numerical integration algorithm to obtain the instantaneous rate of change of the state vector;

[0144] According to the instantaneous rate of change of the current state vector and the state vector, the predicted state vector of the drone at the next moment is obtained;

[0145] The state prediction module outputs a complete state vector containing the predicted position, speed and attitude of the drone.

[0146] Furthermore, when the UAV visual signal is lost, the steps of generating compensated fused positioning data using UWB+4G positioning data and predicted state vector include:

[0147] The Frobenius norm calculation formula is used to calculate the Frobenius norm of the 6×6 pose covariance matrix of the UAV visual output in real time:

[0148] ;

[0149] Where, is the 6×6 visual SLAM pose covariance matrix, For the matrix Rank covariance elements of the columns;

[0150] When the Frobenius norm value is not less than the preset norm threshold, it is determined that the drone’s visual signal is lost;

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

[0152] ;

[0153] Where, is the total number of feature points that are successfully matched between the current frame and the map, is the total number of candidate feature points extracted for the current frame, For the The pixel coordinates of the feature points in the image plane, For the The 3D reprojected coordinates of feature points in the local map, is the pixel reprojection error tolerance;

[0154] When the number of feature point matches is less than the preset matching threshold, it is determined that the drone's visual signal is lost;

[0155] When the drone's visual signal is lost, the compensated fused positioning data is generated through UWB+4G positioning data and the predicted state vector.

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

[0157] When the drone's visual signal is lost, the 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 the improved Matérn3 / 2 kernel function through Gaussian process regression.

[0159] Among them, the improved Matérn3 / 2 kernel function is:

[0160] ;

[0161] Where, is the time difference (unit: s), is the signal variance (unit: m), is the time constant (unit: s), updated by UWB frequency Sure, ;

[0162] The smoothed UWB positioning data is weightedly fused with the 4G network-assisted positioning data to generate preliminary fused positioning data;

[0163] The preliminary fused positioning data is fused with the UAV predicted state vector through an extended Kalman filter to obtain compensated fused positioning data.

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

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

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

[0167] ;

[0168] Where, The current path point position coordinates output by the instruction generation module, The coordinates of the drone position after compensation and fusion positioning;

[0169] The calculation formula for the current attitude error of the drone is:

[0170] ;

[0171] Where, The quaternion of the drone's expected attitude given by the instruction generation module, The quaternion of the current attitude of the drone after compensation and fusion positioning;

[0172] Based on the current position error and attitude error of the UAV, the attitude control command is fine-tuned through the fuzzy adaptive PID controller. The angular velocity command in the attitude control command is generated by the following formula:

[0173] ;

[0174] Where, is the UAV attitude error, 、 、 is the PID gain, and the gain adaptation rule is ,in is the error sensitivity coefficient, is the basic gain, is the attitude error change rate;

[0175] The fine-tuned attitude control commands and throttle commands are sent to the drone for execution.

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

[0177] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts 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, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0178] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0179] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0180] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0181] Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if such changes and modifications fall within the scope of the claims and their equivalents, the present invention is intended to include such changes and modifications.

Claims

1. UAV visual positioning flight optimization system based on multi-source data fusion, characterized by: include: Data acquisition module: used to acquire RGB-D image data and multiple physical data in real time; Environmental modeling module: used to fuse the RGB-D image data and multiple physical data to construct a local three-dimensional environmental map; Path planning module: used to generate obstacle avoidance flight paths based on local 3D environment maps; Instruction generation module: used to convert the obstacle avoidance flight path into attitude control instructions and throttle instructions; State prediction module: used to obtain the predicted state vector of the drone at the next moment based on the drone's dynamic model, attitude control instructions, and throttle instructions; Positioning switching module: used to generate compensated fusion positioning data through UWB+4G positioning data and predicted state vector when the UAV visual signal is lost; Flight control module: used to execute attitude control commands and throttle commands based on compensated fusion positioning data and multiple physical data.

2. The UAV visual positioning flight optimization system based on multi-source data fusion as claimed in claim 1, characterized in that: The steps of fusing the RGB-D image data and multiple physical data to construct a local three-dimensional environment map include: Receive the acquired RGB-D image data and extract depth information and color information from each frame of RGB-D image data; Extract lidar point cloud data and ultrasonic ranging data based on the acquired multi-physical data; The depth information of consecutive frames is registered by iterative closest point algorithm, the depth information in consecutive multiple frames of RGB-D image data is registered, and the depth images at different times are aligned; The registered depth information is downsampled and fused using a voxel grid map algorithm to generate a preliminary 3D point cloud map. Mapping the color information onto the preliminary three-dimensional point cloud map to form a local three-dimensional point cloud map with texture information; The laser radar point cloud data and ultrasonic ranging data are weightedly fused with the local three-dimensional point cloud map to obtain a local three-dimensional environment map.

3. The UAV visual positioning flight optimization system based on multi-source data fusion as claimed in claim 2, characterized in that: The steps for weighted fusion include: The weighted fusion calculation formula is used to perform weighted fusion of the lidar point cloud data, ultrasonic ranging data and the local three-dimensional point cloud map; The calculation method of weighted fusion is: ； Where, is the ambient light intensity, is the point cloud density, is the light intensity threshold, is the point cloud density threshold, is the light sensitivity coefficient, where , is the upper limit of ambient light intensity, is the lower limit of ambient light intensity, is the density sensitivity coefficient, where , is the upper limit of point cloud density, is the lower limit of the point cloud density.

4. The UAV visual positioning flight optimization system based on multi-source data fusion as claimed in claim 1, characterized in that: Based on the local 3D environment map, the steps of generating an obstacle avoidance flight path include: Perform voxel processing on the local three-dimensional environment map to generate a three-dimensional grid map; The occupancy probability of each grid is obtained through the occupancy probability calculation formula; The occupation probability calculation formula is: ； Where, Grid cells The probability of occupation, is the grid height value, is the grid height threshold, is the sensitivity coefficient, is a natural constant; If the occupancy probability of a grid cell is not less than a preset occupancy threshold, the grid cell is marked as an obstacle area; The buffer radius is obtained by the buffer radius calculation formula, and the buffer zone is obtained based on the buffer radius; The calculation formula for the buffer zone radius is: ； Where, is the maximum flight speed of the drone, is the safety factor, is the buffer zone radius; Mark the marked obstacle area and the buffer zone around the obstacle area as an impassable area; Based on the current position and target position of the UAV and the information of the impassable area, an improved path search method of the RRT* algorithm is used for path planning.

5. The UAV visual positioning flight optimization system based on multi-source data fusion as claimed in claim 4, characterized in that: The steps of path planning using the improved path search method of the RRT* algorithm include: The improved path search method is to additionally consider the path smoothness factor and energy consumption factor each time a new node is expanded through the RRT* algorithm; When selecting the optimal parent node, the optimal path is determined by minimizing the objective function: ； Where, is the total length of the planned path, calculated by accumulating the Euclidean distance between nodes. is the reference path length, is the path smoothness factor, To estimate energy consumption, is the reference energy consumption, 、 、 are weight coefficients respectively.

6. The UAV visual positioning flight optimization system based on multi-source data fusion as claimed in claim 1, characterized in that: The steps for converting the obstacle avoidance flight path into attitude control commands and throttle commands include: Decomposing the obstacle avoidance flight path into continuous path points and corresponding desired velocity vectors; The required acceleration vector is obtained based on the desired velocity vector of each path point and the current velocity vector of the drone; 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, the nonlinear mapping function is used to convert the desired attitude angle and desired total thrust of the UAV into , the desired attitude angle includes the roll angle , pitch angle , yaw angle : ； Where, 、 、 is the three-axis component of the desired acceleration in the body coordinate system, is the acceleration due to gravity, For drone quality, The current yaw angle of the drone.

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

8. The UAV visual positioning flight optimization system based on multi-source data fusion as claimed in claim 1, characterized in that: When the drone's visual signal is lost, the steps to generate compensated fused positioning data using UWB+4G positioning data and the predicted state vector include: The Frobenius norm calculation formula is used to calculate the Frobenius norm of the 6×6 pose covariance matrix of the UAV visual output in real time: ； Where, is the 6×6 visual SLAM pose covariance matrix, For the matrix Rank covariance elements of the columns; When the Frobenius norm value is not less than the preset norm threshold, it is determined that the drone’s visual signal is lost; Count the number of successful matches between the current image frame and the map point cloud: ； Where, is the total number of feature points that are successfully matched between the current frame and the map, is the total number of candidate feature points extracted for the current frame, For the The pixel coordinates of the feature points in the image plane, For the The 3D reprojected coordinates of feature points in the local map, is the pixel reprojection error tolerance; When the number of feature point matches is less than the preset matching threshold, it is determined that the drone's visual signal is lost; When the drone's visual signal is lost, the compensated fused positioning data is generated through UWB+4G positioning data and the predicted state vector.

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

10. The UAV visual positioning flight optimization system based on multi-source data fusion as claimed in claim 1, characterized in that: The steps of executing attitude control commands and throttle commands based on the compensated fused positioning data and multiple physical data include: Based on the obtained fused positioning data and multiple physical data, the UAV position and attitude information in the fused positioning data are compared with the attitude control command and throttle command to obtain the current position error and attitude error of the UAV; Based on the current position error and attitude error of the UAV, the attitude control command is fine-tuned through the fuzzy adaptive PID controller. The angular velocity command in the attitude control command is generated by the following formula: ； Where, is the UAV attitude error, 、 、 is the PID gain, and the gain adaptation rule is ,in is the error sensitivity coefficient, is the basic gain, is the attitude error change rate; The fine-tuned attitude control commands and throttle commands are sent to the drone for execution.

Images