Method for visual fusion lidar slam positioning navigation and unmanned aerial vehicle system thereof

Abstract

The application discloses a kind of visual fusion laser radar SLAM positioning navigation method and unmanned aerial vehicle system thereof, comprising obtaining the first fusion pose and motion information generated by three-dimensional laser SLAM system;Second fusion pose and motion information generated by visual SLAM system are obtained;First fusion pose and motion information are compared with second fusion pose and motion information, obtain third comparison error, compare third comparison error with system calibration error;When the third comparison error is greater than system calibration error, switch to visual SLAM system, for the navigation of unmanned aerial vehicle.The method and system provided by the application can enable unmanned aerial vehicle to make motion decision in complex environment when encountering heavy rain, thick smoke or thick fog and other bad weather, improve the accuracy and stability of unmanned aerial vehicle, convenient to use, and improve safety at the same time.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles (UAVs), and in particular relates to a method for vision fusion laser radar SLAM positioning and navigation and an UAV system thereof. Background Art

[0002] Due to its advantages of high precision, wide viewing angle, and wide ranging range, some existing drones currently use lidar technology for navigation. However, the applicant's research has found that the laser light from lidar has low attenuation and a long transmission distance in clear weather, while in bad weather such as heavy rain, thick smoke, and dense fog, the attenuation increases sharply, significantly affecting the laser's transmission distance. Therefore, when a drone using lidar technology encounters heavy rain, thick smoke, or dense fog, the data obtained by the lidar will be inaccurate, resulting in the drone being unable to determine its own position and attitude, which in turn makes it difficult for the drone to understand the complex environment and make movement decisions.

[0003] Therefore, how to enable drones to make movement decisions in complex environments when encountering severe weather such as heavy rain, thick smoke or fog is a technical problem that needs to be solved urgently. Summary of the Invention

[0004] The embodiment of the present invention provides a method for vision fusion lidar SLAM positioning and navigation and a UAV system thereof, which can achieve the effect that the UAV can still make motion decisions in complex environments when encountering severe weather such as heavy rain, thick smoke or dense fog.

[0005] On the one hand, an embodiment of the present invention provides a method for vision fusion lidar SLAM positioning and navigation for unmanned aerial vehicle positioning and navigation, comprising the following steps:

[0006] (1) Obtain the first fusion pose and motion information generated by the 3D laser SLAM system;

[0007] (2) Obtain the second fusion pose and motion information generated by the visual SLAM system;

[0008] (3) comparing the first fused pose and motion information with the second fused pose and motion information to obtain a third comparison error, and comparing the third comparison error with the system calibration error;

[0009] (4) When the third comparison error is greater than the system calibration error, switch to the visual SLAM system for navigation of the UAV.

[0010] Preferably, in the above embodiment, further comprising

[0011] (5) In step (4), further: when the third comparison error is greater than the system calibration error, switch to the visual SLAM system to obtain the third fused pose and motion information for the UAV positioning and navigation; or when the third comparison error is less than or equal to the system calibration error, obtain the second fused pose and motion information and continue to use the laser SLAM system for the UAV positioning and navigation;

[0012] (6) Obtaining map update information of the 3D laser SLAM system and / or obtaining map update information of the visual SLAM system;

[0013] (7) Obtaining the location information specified by the user, and generating the best route based on the location information specified by the user, the map update information, the third fusion pose and motion information or the second fusion pose and motion information, performing real-time positioning and autonomous navigation of the UAV, and displaying the location of the UAV and controlling the flight of the UAV;

[0014] In the embodiment of the present invention, the system calibration error is a system preset value.

[0015] Preferably, the three-dimensional laser SLAM system in step (1) generates the first fused pose and motion information and map update information further comprising the following steps:

[0016] (1-1) Obtain the first IMU pose and motion information of the current frame collected by the first IMU unit, and obtain the point cloud data information of the current frame collected by the three-dimensional laser radar;

[0017] (1-2), filtering and preprocessing the collected point cloud data;

[0018] (1-3) Solve the filtered point cloud data to obtain the lidar pose and motion information of the current frame and the local map of the surrounding environment of the current frame in the laser system, and update the map in the laser system;

[0019] (1-4), fuse all local maps to generate a global map;

[0020] (1-5) combining the lidar pose and motion information of the previous frame with the lidar pose and motion information of the current frame to generate a first motion trajectory;

[0021] (1-6), combining the first IMU pose and motion information of the previous frame with the first IMU pose and motion information of the current frame to generate a second motion trajectory;

[0022] (1-7), comparing the first motion trajectory and the second motion trajectory to obtain a first comparison error, and comparing the first comparison error with a laser SLAM calibration error, wherein the laser SLAM calibration error is a system preset value;

[0023] (1-8) According to the comparison result of the first comparison error and the laser SLAM calibration error, the position and motion information of the first IMU and the laser radar are fused, and combined with the currently updated global map model to obtain the first fused position and motion information of the current frame;

[0024] (1-9), output the first fused pose and motion information and global map information.

[0025] Preferably, the filtering and preprocessing of the collected point cloud data in step 1-2 further includes the following steps:

[0026] (1-2-1) Filter the collected point cloud data;

[0027] (1-2-2), pre-processing the filtered point cloud data;

[0028] (1-2-3) Update the temporary local map based on the preprocessed point cloud data.

[0029] Preferably, the step (1-6) of comparing the first comparison error with the laser SLAM calibration error further comprises the following steps:

[0030] (1-6-1) If the first comparison error is greater than the laser SLAM calibration error, the first IMU pose and motion information of the current frame is replaced with the lidar pose and motion information of the current frame;

[0031] (1-6-2) If the first comparison error is less than or equal to the laser SLAM calibration error, the first IMU pose and motion information of the current frame is stored.

[0032] Preferably, the visual SLAM system operation in step (2) to generate the second fused pose and motion information further comprises the following steps:

[0033] (2-1) Obtain the second IMU pose and motion information of the current frame collected by the second IMU unit, and obtain the image pixel information of the current frame collected by the camera; in this embodiment, the camera is a binocular camera;

[0034] (2-2) performing front-end feature matching and tracking on the image pixel information of the current frame;

[0035] (2-3) triangulate the feature points, calculate the camera position and motion information of the current frame and the local map of the surrounding environment of the current frame in the visual system, and update the map in the visual system;

[0036] (2-4) combining the camera position and motion information of the previous frame with the camera position and motion information of the current frame to generate a third motion trajectory;

[0037] (2-5) combining the second IMU pose and motion information of the previous frame with the second IMU pose and motion information of the current frame to generate a fourth motion trajectory;

[0038] (2-6), comparing the third motion trajectory and the fourth motion trajectory to obtain a second comparison error, and comparing the second comparison error with a visual SLAM calibration error, wherein the visual SLAM calibration error is a system preset value;

[0039] (2-7) According to the comparison result of the second comparison error and the visual SLAM calibration error, the second IMU and camera pose and motion information are fused, and the second fused pose and motion information of the current frame in the visual system map is obtained in combination with the visual system map information;

[0040] Preferably, the step 2-6 of comparing the second comparison error with the visual SLAM calibration error further comprises the following steps:

[0041] (2-6-1) If the second comparison error is greater than the visual SLAM calibration error, the second IMU pose and motion information of the current frame is replaced with the camera pose and motion information of the current frame;

[0042] (2-6-2) If the second comparison error is less than or equal to the visual SLAM calibration error, the second IMU pose and motion information of the current frame is stored.

[0043] Preferably, the step (3) of comparing the third comparison error with the system calibration error further includes the following steps:

[0044] (3-1) If the third comparison error is greater than the system calibration error, switch to the visual SLAM system. When there is an external GPS signal, combine the GPS received signal to obtain the second fusion pose and motion information with the earth coordinate system;

[0045] (3-2) If the third comparison error is less than or equal to the system calibration error, the usage weights of the three-dimensional laser SLAM system and the visual SLAM system are calculated according to the weight algorithm. When there is an external GPS signal, the third fusion posture and motion information with the earth coordinate system is obtained in combination with the GPS receiving signal.

[0046] Preferably, if the third comparison error in step (3-1) is greater than the system calibration error, the following steps are further included:

[0047] (3-1-1), comparing the third comparison error with the system switching error;

[0048] (3-1-2) If the third comparison error is less than or equal to the system switching error, the second fused pose and motion information is replaced with the first fused pose and motion information and stored in the visual SLAM system;

[0049] (3-1-3) If the third comparison error is greater than the system switching error, go to step (4) and switch to the visual SLAM system. When there is an external GPS signal, the second fused pose and motion information with the earth coordinate system is obtained by combining the GPS receiving signal.

[0050] On the other hand, an embodiment of the present invention provides an unmanned aerial vehicle (UAV) system based on visual fusion three-dimensional lidar SLAM technology, including a three-dimensional laser SLAM system for UAV positioning and navigation, a visual SLAM system for UAV positioning and navigation, a flight control unit for controlling the UAV, and a system processing unit for processing data of each system, wherein the three-dimensional laser SLAM system provides a first fusion posture and motion information, the visual SLAM system provides a second fusion posture and motion information, the system processing unit compares the first fusion posture and motion information with the second fusion posture and motion information, obtains a third comparison error, and compares the third comparison error with a system calibration error preset by the system; when the third comparison error is greater than the system calibration error preset by the system, the current navigation system is switched to the visual SLAM system for positioning and navigation of the UAV.

[0051] Preferably, the system processing unit includes a system calibration module, a system switching module and a system fusion module, wherein:

[0052] The system calibration module is used to correct the error of the visual SLAM system in real time. The system calibration module compares the first fused pose and motion information with the second fused pose and motion information to obtain a third comparison error, and compares the third comparison error with a preset system calibration error;

[0053] The system switching module is used for switching between the visual SLAM system and the three-dimensional laser SLAM system;

[0054] The system fusion module is used to fuse the position and motion information of the visual SLAM system and the three-dimensional laser SLAM system;

[0055] When the third comparison error is greater than the system calibration error, the third comparison error is compared with the preset system switching error; if the third comparison error is less than or equal to the system calibration error, the first fused pose and motion information and the second fused pose and motion information are output to the system fusion module;

[0056] If the third comparison error is less than or equal to the system switching error, the system calibration module replaces the second fused pose and motion information with the first fused pose and motion information, and outputs the first fused pose and motion information and the second fused pose and motion information to the system fusion module;

[0057] If the third comparison error is greater than the system switching error, the first fused pose and motion information and the second fused pose and motion information are output to the system switching module, and the system switching module only uses the second fused pose and motion information output by the visual SLAM system to locate and navigate the UAV;

[0058] The system fusion module calculates the usage weights of the 3D laser SLAM system and the visual SLAM system according to the results sent by the system calibration module and the weight algorithm, and fuses them to obtain the fused pose motion information.

[0059] Preferably, the UAV system of visual fusion three-dimensional lidar SLAM technology described in the embodiment of the present invention also includes a multi-sensor fusion module, which performs fusion according to the fused pose motion information and updated global three-dimensional map information sent by the system fusion module. When there are external GPS and / or RTK signals, the GPS and / or RTK information are simultaneously fused to obtain fused pose and motion information with the earth coordinate system.

[0060] Preferably, the three-dimensional laser SLAM system further includes a laser information acquisition unit and a laser information processing unit, wherein:

[0061] The laser information acquisition unit is used to collect information for the three-dimensional laser SLAM system;

[0062] The laser information processing unit is used to process the information collected by the laser information collection unit.

[0063] Preferably, the laser information acquisition unit further includes a first IMU unit and at least one laser radar, wherein:

[0064] The first IMU unit is used to provide predicted pose and motion information for the three-dimensional laser SLAM system;

[0065] The laser radar is used to collect point cloud data of the surrounding environment.

[0066] Preferably, the laser radar is a multi-line three-dimensional laser scanning radar.

[0067] Preferably, the method of providing the predicted pose and motion information for the 3D laser SLAM system further includes the first IMU unit collecting mileage information, converting the mileage information into UAV pose change information through the UAV inertial odometer kinematic model, and sending it to the Bayesian filter to preliminarily calculate the predicted pose and motion information. The predicted pose of the Bayesian filter is corrected by the system observation model, which effectively improves the accuracy of the UAV pose estimated by the filter.

[0068] Preferably, the laser information processing unit further includes a point cloud processing module, a radar data processing module, a radar system storage module, a radar system map update module and a global map fusion module, wherein:

[0069] The point cloud processing module is used to process point cloud data;

[0070] The radar data processing module is used to process data of the radar system;

[0071] The radar system storage module is used to store information generated by the laser information processing unit;

[0072] The radar system map update module is used to update the map information of the three-dimensional laser SLAM system 100 in real time;

[0073] The global map fusion module is used to fuse all local map information to generate global map information, so that the UAV can plan a navigation path in the global map information.

[0074] Preferably, the point cloud processing module further includes a filtering module and a pre-processing module, wherein:

[0075] The filtering module is used to filter the point cloud data collected by the laser radar;

[0076] The pre-processing module is used to perform preliminary processing on the filtered point cloud data to obtain temporary local map information.

[0077] Preferably, the filtering process includes denoising the point cloud data, removing abnormal points and reducing the amount of redundant point cloud data.

[0078] Preferably, the radar data processing module further includes a radar solution module, a radar system trajectory generation module, a radar system trajectory comparison module, a radar system calibration module, and a lidar fusion module, wherein:

[0079] The radar solution module is used to solve the filtered point cloud data to obtain the corresponding lidar pose and motion information and the local map information of the surrounding environment of the current frame;

[0080] The radar system trajectory generation module is used to generate the motion trajectory of the UAV in two consecutive frames by combining the posture and motion information of two consecutive frames;

[0081] The radar system trajectory comparison module is used to compare the motion trajectory and the error;

[0082] The radar system calibration module is used to correct the error of the first IMU unit in real time;

[0083] The lidar fusion module is used to fuse the posture and motion information output by the first IMU unit and the lidar.

[0084] Preferably, the visual SLAM system further includes a visual information acquisition unit and a visual information processing unit, wherein:

[0085] The visual information acquisition unit is used to collect information for the visual SLAM system;

[0086] The visual information processing unit is used to process the information collected by the visual information collection unit.

[0087] Preferably, the visual information acquisition unit further includes a second IMU unit and at least one set of binocular cameras, wherein:

[0088] The second IMU unit is used to provide predicted pose and motion information for the visual SLAM system;

[0089] The at least one set of binocular cameras is used to capture image pixel information of the surrounding environment.

[0090] Preferably, providing the predicted pose and motion information for the visual SLAM system also includes that the second IMU unit first collects mileage information, converts the mileage information into UAV pose change information through the UAV inertial odometer kinematic model, and sends it to the Bayesian filter to preliminarily calculate the predicted pose and motion information.

[0091] Preferably, the visual information processing unit includes a feature processing module, a visual data processing module, a visual system storage module and a visual system map updating module, wherein:

[0092] The feature processing module is used to process image features;

[0093] The visual data processing module is used to process data of the visual system;

[0094] The visual system storage module is used to store information generated by the visual information processing unit;

[0095] The visual system map update module is used to update the map information of the visual SLAM system in real time.

[0096] Preferably, the feature processing module further includes a feature matching and tracking module and a feature point processing module, wherein:

[0097] The feature matching and tracking module is used to track the image pixel information collected by the binocular camera in real time and match the image pixel information of two consecutive frames;

[0098] The feature point processing module is used to perform feature point processing on features.

[0099] Preferably, the method for processing the feature points is as follows:

[0100] A 6-dimensional vector multi-keyframe matching point triangulation feature point depth acquisition method is used to represent the 3D position of an environmental feature point, including the position of the drone's camera and the direction and depth information of the environmental feature points where the drone is located; the environment is continuously updated, and when the feature estimation covariance is less than a set threshold, the 6-dimensional representation of the environmental feature points where the drone is located is converted into 3-dimensional Euclidean coordinates. By establishing multiple feature points in the same frame image, the representation of the feature points belonging to the same frame is reduced to a camera posture plus multiple depths, so as to effectively reduce the length of the drone system state.

[0101] Preferably, the visual data processing module further includes a visual solution module, a visual system trajectory generation module, a visual system trajectory comparison module, a visual system calibration module, and a visual binocular fusion module, wherein:

[0102] The visual solution module is used to solve the processed environmental feature point information into camera posture and motion information and local map information of the current frame surrounding environment;

[0103] The visual system trajectory generation module is used to generate the motion trajectory of the UAV in two consecutive frames by combining the posture and motion information of two consecutive frames;

[0104] The visual system trajectory comparison module is used to compare motion trajectories and errors;

[0105] The visual system calibration module is used to correct the error of the second IMU unit in real time;

[0106] The visual binocular fusion module is used to fuse the posture and motion information output by the second IMU unit and the binocular camera.

[0107] Preferably, the flight control unit includes a global planning module, a local planning module and a bottom control module, wherein:

[0108] The global planning module is used to plan the optimal navigation path of the global map;

[0109] The local planning module is used to plan the global optimal path based on the real-time local map information obtained after preprocessing;

[0110] The bottom control module is used to control and distribute the UAV.

[0111] The method and system provided by the present invention simultaneously use a visual system and a three-dimensional laser system based on a system fusion module. When a deviation occurs in the navigation positioning of one of the systems, it can seamlessly switch to the other system through the system switching module, thereby improving the environmental adaptability and stability of the UAV. At the same time, the system processing unit compares and corrects the data of the two systems in real time, making the system positioning and navigation more accurate and applicable to a wider range of scenarios.

[0112] The beneficial effects of the present invention are as follows:

[0113] 1. When the drone encounters severe weather such as heavy rain, thick smoke or fog, it can still make movement decisions in complex environments, which improves the accuracy and stability of the drone, makes it easier to use, and improves safety.

[0114] 2. By switching between the two systems, it is possible to prevent the entire drone from stopping operating when one of the systems fails.

[0115] 3. It can be flown indoors or in any condition where there is no satellite GPS signal reception. It is safe and reliable, not easily interfered with by wireless signals, and can fly autonomously without any communication connection.

[0116] 4. Achieve high-reliability hovering and autonomous flight of UAVs, and independently plan routes quickly and fly to the destination.

[0117] 5. The system's lidar is a self-luminous sensor that can fly in complete darkness. BRIEF DESCRIPTION OF THE DRAWINGS

[0118] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0119] Figure 1 This is a general flow chart of the method for visual fusion lidar SLAM positioning and navigation of a drone in an embodiment of the present invention.

[0120] Figure 2 This is a flowchart of the operation of the three-dimensional laser SLAM system in an embodiment of the present invention.

[0121] Figure 3 This is a flowchart of the operation of the visual SLAM system in an embodiment of the present invention.

[0122] Figure 4 This is a module diagram of the drone system based on vision fusion 3D lidar SLAM positioning and navigation in an embodiment of the present invention. DETAILED DESCRIPTION

[0123] In order to make the purpose, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application generally described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of this application.

[0124] On the one hand, see Figure 1 The embodiment of the present invention provides a method for visual fusion laser radar SLAM positioning and navigation for unmanned aerial vehicle positioning and navigation, comprising the following steps:

[0125] (1) Obtain the first fusion pose and motion information generated by the 3D laser SLAM system;

[0126] (2) Obtain the second fusion pose and motion information generated by the visual SLAM system;

[0127] (3) comparing the first fused pose and motion information with the second fused pose and motion information to obtain a third comparison error, and comparing the third comparison error with the system calibration error;

[0128] (4) When the third comparison error is greater than the system calibration error, switch to the visual SLAM system for navigation of the UAV.

[0129] In the above embodiment, further comprising:

[0130] (5) In step (4), further: when the third comparison error is greater than the system calibration error, switch to the visual SLAM system to obtain the third fused pose and motion information for the UAV positioning and navigation; or when the third comparison error is less than or equal to the system calibration error, obtain the second fused pose and motion information and continue to use the laser SLAM system for the UAV positioning and navigation;

[0131] (6) Obtaining map update information of the 3D laser SLAM system and / or obtaining map update information of the visual SLAM system;

[0132] (7) Obtaining the location information specified by the user, and generating the best route based on the location information specified by the user, the map update information, the third fusion pose and motion information or the second fusion pose and motion information, performing real-time positioning and autonomous navigation of the UAV, and displaying the location of the UAV and controlling the flight of the UAV;

[0133] In the embodiment of the present invention, the system calibration error is a system preset value.

[0134] Furthermore, in the embodiment of the present invention, the step (1) of obtaining the first fusion pose and motion information generated by the three-dimensional laser SLAM system further includes the following steps, please refer to Figure 2 :

[0135] (1-1) Obtain the first IMU pose and motion information of the current frame collected by the first IMU unit, and obtain the point cloud data information of the current frame collected by the three-dimensional laser radar;

[0136] (1-2), filtering and preprocessing the collected point cloud data;

[0137] (1-3) Solve the filtered point cloud data to obtain the lidar pose and motion information of the current frame and the local map of the surrounding environment of the current frame in the laser system, and update the map in the laser system;

[0138] (1-4), fuse all local maps to generate a global map;

[0139] (1-5) combining the lidar pose and motion information of the previous frame with the lidar pose and motion information of the current frame to generate a first motion trajectory;

[0140] (1-6), combining the first IMU pose and motion information of the previous frame with the first IMU pose and motion information of the current frame to generate a second motion trajectory;

[0141] (1-7), comparing the first motion trajectory and the second motion trajectory to obtain a first comparison error, and comparing the first comparison error with a laser SLAM calibration error, wherein the laser SLAM calibration error is a system preset value;

[0142] (1-8) According to the comparison result of the first comparison error and the laser SLAM calibration error, the position and motion information of the first IMU and the laser radar are fused, and combined with the currently updated global map model to obtain the first fused position and motion information of the current frame;

[0143] (1-9), output the first fused pose and motion information and global map information.

[0144] Furthermore, in the embodiment of the present invention, the map update information of the three-dimensional laser SLAM system is obtained in step (6) by solving the point cloud data after filtering, and the local map information of the surrounding environment of the current frame in the laser system is updated according to the local map information.

[0145] Furthermore, the embodiment of the present invention also includes step (1-8): fusing all local map information to generate a global map.

[0146] Furthermore, in the embodiment of the present invention, in step (1-7), obtaining the first fused pose and motion information of the current frame includes outputting the first fused pose and motion information in combination with the currently updated global map information;

[0147] Furthermore, in an embodiment of the present invention, the filtering and preprocessing of the collected point cloud data in step (1-2) further includes the following steps:

[0148] (1-2-1) Filter the collected point cloud data;

[0149] (1-2-2), pre-processing the filtered point cloud data;

[0150] (1-2-3) Update the temporary local map based on the preprocessed point cloud data.

[0151] The step (1-6) of comparing the first comparison error with the laser SLAM calibration error further includes the following steps:

[0152] (1-6-1) If the first comparison error is greater than the laser SLAM calibration error, the first IMU pose and motion information of the current frame is replaced with the lidar pose and motion information of the current frame;

[0153] (1-6-2) If the first comparison error is less than or equal to the laser SLAM calibration error, the first IMU pose and motion information of the current frame is stored.

[0154] The visual SLAM system in step (2) generates the second fusion pose and motion information as follows: Figure 3 As shown, the following steps are also included:

[0155] (2-1) Obtain the second IMU position and motion information of the current frame collected by the second IMU unit, and obtain the image pixel information of the current frame collected by the camera; in this embodiment, it is a binocular camera.

[0156] (2-2) performing front-end feature matching and tracking on the image pixel information of the current frame;

[0157] (2-3) triangulate the feature points, calculate the camera position and motion information of the current frame and the local map of the surrounding environment of the current frame in the visual system, and update the map in the visual system;

[0158] (2-4) combining the camera position and motion information of the previous frame with the camera position and motion information of the current frame to generate a third motion trajectory;

[0159] (2-5) combining the second IMU pose and motion information of the previous frame with the second IMU pose and motion information of the current frame to generate a fourth motion trajectory;

[0160] (2-6), comparing the third motion trajectory and the fourth motion trajectory to obtain a second comparison error, and comparing the second comparison error with a visual SLAM calibration error, wherein the visual SLAM calibration error is a system preset value;

[0161] (2-7) According to the comparison result of the second comparison error and the visual SLAM calibration error, the second IMU and camera posture and motion information are fused, and the second fused posture and motion information of the current frame in the visual system map is obtained in combination with the visual system map information.

[0162] Furthermore, in an embodiment of the present invention, the step (2-6) of comparing the second comparison error with the visual SLAM calibration error further includes the following steps:

[0163] (2-6-1) If the second comparison error is greater than the visual SLAM calibration error, the second IMU pose and motion information of the current frame is replaced with the camera pose and motion information of the current frame;

[0164] (2-6-2) If the second comparison error is less than or equal to the visual SLAM calibration error, store the second IMU pose and motion information of the current frame;

[0165] Furthermore, in an embodiment of the present invention, the step (3) of comparing the third comparison error with the system calibration error further includes the following steps:

[0166] (3-1), if the third comparison error is greater than the system calibration error, go to step (4), switch to the visual SLAM system, and combine the GPS reception signal to obtain the second fusion pose and motion information with the earth coordinate system;

[0167] (3-2) If the third comparison error is less than or equal to the system calibration error, the usage weights of the three-dimensional laser SLAM system and the visual SLAM system are calculated according to the weight algorithm. When there is an external GPS signal, the third fusion posture and motion information with the earth coordinate system is obtained in combination with the GPS receiving signal.

[0168] If the third comparison error in step (3-1) is greater than the system calibration error, the following steps are further included:

[0169] (3-1-1), comparing the third comparison error with the system switching error;

[0170] (3-1-2) If the third comparison error is less than or equal to the system switching error, the second fused pose and motion information is replaced with the first fused pose and motion information and stored in the visual SLAM system;

[0171] (3-1-3) If the third comparison error is greater than the system switching error, go to step (4) and switch to the visual SLAM system. When there is an external GPS signal, the second fused pose and motion information with the earth coordinate system is obtained by combining the GPS receiving signal.

[0172] On the other hand, the embodiment of the present invention also provides a UAV system based on vision fusion laser radar SLAM positioning and navigation Figure 4 The module diagram of the unmanned aerial vehicle system based on visual fusion 3D laser radar SLAM positioning and navigation in an embodiment of the present invention includes a 3D laser SLAM system 100, a visual SLAM system 200, a flight control unit 300, and a system processing unit 400. The 3D laser SLAM system 100 is used for unmanned aerial vehicle positioning and navigation. The 3D laser SLAM system 100 provides first fused pose and motion information, the visual SLAM system 200 is used to optimize the 3D laser SLAM system and provide unmanned aerial vehicle navigation when the 3D laser SLAM system is not applicable. The visual SLAM system 200 provides second fused pose and motion information, the flight control unit 300 is used to control the unmanned aerial vehicle, and the system processing unit 400 is used to process data from each system. The system processing unit 400 compares the first fused pose and motion information with the second fused pose and motion information to obtain a third comparison error, and compares the third comparison error with a system calibration error preset by the system. When the third comparison error is greater than the system calibration error preset by the system, the current navigation system is switched to the visual SLAM system for unmanned aerial vehicle positioning and navigation. The system processing unit 400 includes a system calibration module 410, a system switching module 420 and a system fusion module 430.

[0173] The system calibration module 410 is used to correct the error of the visual SLAM system 200 in real time. The system calibration module compares the first fused pose and motion information with the second fused pose and motion information to obtain a third comparison error, and compares the third comparison error with a preset system calibration error;

[0174] The system switching module 420 is used for switching between the visual SLAM system 200 and the three-dimensional laser SLAM system 100;

[0175] The system fusion module 430 is used to fuse the posture and motion information of the visual SLAM system 200 and the three-dimensional laser SLAM system 100;

[0176] When the third comparison error is greater than the system calibration error, the third comparison error is compared with the preset system switching error; if the third comparison error is less than or equal to the system calibration error, the first fused pose and motion information and the second fused pose and motion information are output to the system fusion module;

[0177] If the third comparison error is less than or equal to the system switching error, the system calibration module replaces the second fused pose and motion information with the first fused pose and motion information, and outputs the first fused pose and motion information and the second fused pose and motion information to the system fusion module;

[0178] If the third comparison error is greater than the system switching error, the first fused pose and motion information and the second fused pose and motion information are output to the system switching module, and the system switching module only uses the second fused pose and motion information output by the visual SLAM system to locate and navigate the UAV;

[0179] The system fusion module calculates the usage weights of the 3D laser SLAM system and the visual SLAM system according to the results sent by the system calibration module and the weight algorithm, and fuses them to obtain the fused pose motion information.

[0180] Furthermore, in an embodiment of the present invention, a multi-sensor fusion module 440 is also included, which performs fusion based on the fused pose and motion information and updated global three-dimensional map information sent by the system fusion module. When there are external GPS and / or RTK signals, the GPS and / or RTK information are fused at the same time to obtain fused pose and motion information with the earth coordinate system.

[0181] Furthermore, in an embodiment of the present invention, the three-dimensional laser SLAM system 100 also includes a laser information acquisition unit 110 and a laser information processing unit 120, wherein the laser information acquisition unit 110 is used to collect information for the three-dimensional laser SLAM system 100, and the laser information processing unit 120 is used to process the information collected by the laser information acquisition unit.

[0182] The laser information acquisition unit 110 further includes a first IMU unit 111 and at least one laser radar 112, wherein:

[0183] The first IMU unit 111 is used to provide predicted pose and motion information for the three-dimensional laser SLAM system 100;

[0184] The laser radar 112 is used to collect point cloud data of the surrounding environment.

[0185] The laser radar 112 is a multi-line three-dimensional laser scanning radar.

[0186] The method of providing predicted posture and motion information for the three-dimensional laser SLAM system 100 also includes the first IMU unit 111 first collecting mileage information, converting the mileage information into drone posture change information through the drone inertial odometer kinematic model, and sending it to the Bayesian filter to preliminarily calculate the predicted posture and motion information.

[0187] Furthermore, in an embodiment of the present invention, the laser information processing unit 120 further includes a point cloud processing module 130, a radar data processing module 140, a radar system storage module 150, a radar system map update module 160 and a global map fusion module 170, wherein:

[0188] The point cloud processing module 130 is used to process point cloud data;

[0189] The radar data processing module 140 is used to process data from the radar system;

[0190] The radar system storage module 150 is used to store information generated by the laser information processing unit;

[0191] The radar system map update module 160 is used to update the map information of the three-dimensional laser SLAM system 100 in real time;

[0192] The global map fusion module 170 is used to fuse all local map information to generate global map information, so that the UAV can plan a navigation path in the global map information.

[0193] The point cloud processing module 130 further includes a filtering module 131 and a pre-processing module 132, wherein:

[0194] The filtering module 131 is used to filter the point cloud data collected by the laser radar 112;

[0195] The pre-processing module 132 is used to perform preliminary processing on the filtered point cloud data to obtain temporary local map information.

[0196] Furthermore, in an embodiment of the present invention, the filtering process includes denoising the point cloud data, removing abnormal points, and reducing the amount of redundant point cloud data.

[0197] The radar data processing module 140 further includes a radar solution module 141, a radar system trajectory generation module 142, a radar system trajectory comparison module 143, a radar system calibration module 144, and a lidar fusion module 145, wherein:

[0198] The radar solution module 141 is used to solve the filtered point cloud data to obtain the corresponding lidar pose and motion information and the local map information of the surrounding environment of the current frame;

[0199] The radar system trajectory generation module 142 is used to generate the motion trajectory of the UAV in two consecutive frames by combining the posture and motion information of two consecutive frames;

[0200] The radar system trajectory comparison module 143 is used to compare the motion trajectory and the error;

[0201] The radar system calibration module 144 is used to correct the error of the first IMU unit 111 in real time;

[0202] The lidar fusion module 145 is used to fuse the position and motion information output by the first IMU unit 111 and the lidar.

[0203] Furthermore, in an embodiment of the present invention, the visual SLAM system 200 also includes a visual information acquisition unit 210 and a visual information processing unit 220, wherein the visual information acquisition unit 210 is used to collect information for the visual SLAM system 200, and the visual information processing unit 220 is used to process the information collected by the visual information acquisition unit.

[0204] The visual information acquisition unit 210 further includes a second IMU unit 211 and at least one set of binocular cameras 212, wherein:

[0205] The second IMU unit 211 is used to provide predicted pose and motion information for the visual SLAM system 200;

[0206] The at least one set of binocular cameras 212 is used to capture image pixel information of the surrounding environment.

[0207] The provision of predicted pose and motion information for the visual SLAM system 200 also includes the second IMU unit 211 first collecting mileage information, converting the mileage information into UAV pose change information through the UAV inertial odometer kinematic model, and sending it to the Bayesian filter to preliminarily calculate the predicted pose and motion information.

[0208] The visual information processing unit 220 includes a feature processing module 230, a visual data processing module 240, a visual system storage module 250 and a visual system map updating module 260, wherein:

[0209] The feature processing module 230 is used to process image features;

[0210] The visual data processing module 240 is used to process data of the visual system;

[0211] The visual system storage module 250 is used to store information generated by the visual information processing unit;

[0212] The visual system map update module 260 is used to update the map information of the visual SLAM system 200 in real time.

[0213] The feature processing module 230 further includes a feature matching and tracking module 231 and a feature point processing module 232, wherein:

[0214] The feature matching and tracking module 231 is used to track the image pixel information collected by the binocular camera 212 in real time and match the image pixel information of two consecutive frames with features;

[0215] The feature point processing module 232 is used to process features into feature points.

[0216] The method of feature point processing is as follows:

[0217] A 6-dimensional vector multi-keyframe matching point triangulation feature point depth acquisition method is used to represent the 3D position of an environmental feature point, including the position of the drone's camera and the direction and depth information of the environmental feature points where the drone is located; the environment is continuously updated, and when the feature estimation covariance is less than a set threshold, the 6-dimensional representation of the environmental feature points where the drone is located is converted into 3-dimensional Euclidean coordinates. By establishing multiple feature points in the same frame image, the representation of the feature points belonging to the same frame is reduced to a camera posture plus multiple depths, so as to effectively reduce the length of the drone system state.

[0218] The visual data processing module 240 further includes a visual solution module 241, a visual system trajectory generation module 242, a visual system trajectory comparison module 243, a visual system calibration module 244, and a visual binocular fusion module 245, wherein:

[0219] The visual solution module 241 is used to solve the processed environmental feature point information into camera posture and motion information and local map information of the surrounding environment of the current frame;

[0220] The visual system trajectory generation module 242 is used to generate the motion trajectory of the UAV in two consecutive frames by combining the posture and motion information of two consecutive frames;

[0221] The visual system trajectory comparison module 243 is used to compare the motion trajectory and the error;

[0222] The visual system calibration module 244 is used to correct the error of the second IMU unit 211 in real time;

[0223] The visual binocular fusion module 245 is used to fuse the posture and motion information output by the second IMU unit 211 and the binocular camera 212.

[0224] The flight control unit 300 includes a global planning module 310, a local planning module 320 and a bottom control module 330, wherein:

[0225] The global planning module 310 is used to plan the optimal navigation path of the global map;

[0226] The local planning module 320 is used to plan the global optimal path based on the real-time local map information obtained after preprocessing;

[0227] The bottom control module 330 is used to control and distribute the UAVs.

[0228] The overall operation of a UAV system based on vision fusion 3D lidar SLAM positioning technology in this embodiment is as follows:

[0229] The 3D laser SLAM system 100 and the visual SLAM system 200 operate independently and generate first fused pose and motion information and second fused pose and motion information respectively, which are output to the system calibration module 410;

[0230] The system calibration module 410 compares the first fused pose and motion information with the second fused pose and motion information to obtain a third comparison error, and compares the third comparison error with a preset system calibration error;

[0231] If the third comparison error is greater than the system calibration error, the third comparison error is compared with a preset system switching error; if the third comparison error is less than or equal to the system calibration error, the first fused pose and motion information and the second fused pose and motion information are output to the system fusion module 430;

[0232] If the third comparison error is less than or equal to the system switching error, the system calibration module 410 replaces the second fused pose and motion information with the first fused pose and motion information, and outputs the first fused pose and motion information and the second fused pose and motion information to the system fusion module 430;

[0233] If the third comparison error is greater than the system switching error, the first fused pose and motion information and the second fused pose and motion information are output to the system switching module 420, and the system switching module 420 only uses the second fused pose and motion information output by the visual SLAM system 200 to locate the position of the UAV;

[0234] The multi-sensor fusion module 440 performs fusion based on the fused pose and motion information sent by the system fusion module and the updated global 3D map information. When there is an external GPS and / or RTK signal, the multi-sensor fusion module 440 also fuses the GPS and / or RTK information to obtain a third fused pose and motion information with an earth coordinate system.

[0235] The global planning module 310 plans the optimal navigation path for the UAV to reach the target point based on the user-specified location, map update information, and the third fused pose and motion information or the second fused pose and motion information, i.e., the shortest and obstacle-free navigation path, and outputs all data to the local planning module 320;

[0236] The local planning module 320 plans a local optimal navigation path based on the real-time local map information sent by the pre-processing module 132 in the laser information processing unit and the data output by the global planning module 310, compares it with the optimal navigation path to obtain the optimal path, and outputs the optimal path to the bottom control module 330;

[0237] The bottom control module 330 controls the drone according to the optimal path and controls the drone’s flight speed, angle, and direction.

[0238] The drone received the command and began to fly.

[0239] The three-dimensional laser SLAM system 100 operates as follows:

[0240] The first IMU unit 111 obtains the mileage information of the 3D laser SLAM system 100, and converts the mileage information into the UAV posture change information through the UAV inertial odometry kinematic model. The information is sent to the Bayesian filter to preliminarily calculate the first IMU posture and motion information of the current frame and output it to the radar system trajectory generation module 142. The multi-line 3D laser scanning radar collects the point cloud data information of the current frame and outputs it to the filtering module 131;

[0241] The filtering module 131 performs filtering processing on the collected point cloud data, such as noise reduction, removal of abnormal points, and reduction of redundant point cloud data volume, and outputs the filtered point cloud data to the radar solution module 141 and the pre-processing module 132 respectively;

[0242] The pre-processing module 132 performs preliminary processing on the filtered point cloud data to obtain temporary local map information and outputs it to the local planning module 320;

[0243] The radar solution module 141 solves the filtered point cloud data to obtain the lidar pose and motion information of the current frame and the local map information of the surrounding environment of the current frame in the laser system, and outputs the lidar pose and motion information of the current frame to the radar system trajectory generation module 142, and outputs the local map information of the surrounding environment of the current frame in the laser system to the laser system map update module 160;

[0244] The laser system map update module 160 updates the local map information of two consecutive frames in real time and outputs it to the global map fusion module 170;

[0245] The global map fusion module 170 fuses all local map information to generate global map information. When the UAV takes off from the same location next time, it can directly use the global map information and output the global map information to the lidar fusion module 145 and the global planning module 310.

[0246] The radar system trajectory generation module 142 combines the lidar pose and motion information of the previous frame with the lidar pose and motion information of the current frame to generate a first motion trajectory, combines the first IMU pose and motion information of the previous frame with the first IMU pose and motion information of the current frame to generate a second motion trajectory, and outputs the first motion trajectory and the second motion trajectory to the radar system trajectory comparison module 143;

[0247] The radar system trajectory comparison module 143 compares the first motion trajectory and the second motion trajectory to obtain a first comparison error, compares the first comparison error with a preset laser SLAM calibration error, and outputs the comparison result to the radar system calibration module 144;

[0248] If the first comparison error is greater than the laser SLAM calibration error, the radar system calibration module 144 replaces the first IMU pose and motion information of the current frame with the lidar pose and motion information of the current frame. If the first comparison error is less than or equal to the laser SLAM calibration error, the first IMU pose and motion information of the current frame is stored.

[0249] The posture and motion information of the first IMU and the lidar are output to the lidar fusion module 145. The lidar fusion module 145 fuses the posture and motion information of the first IMU and the lidar according to the fusion algorithm and combines it with the currently updated global map model to obtain the first fused posture and motion information of the current frame, and outputs the first fused posture and motion information of the current frame to the system calibration module 410.

[0250] The visual SLAM system 200 operates as follows:

[0251] The second IMU unit 211 obtains the mileage information of the visual SLAM system 200, and converts the mileage information into the UAV posture change information through the UAV inertial odometer kinematic model. The information is sent to the Bayesian filter to preliminarily calculate the second IMU posture and motion information of the current frame and output it to the visual system trajectory generation module 242. The binocular camera 212 collects the image pixels of the current frame and outputs them to the feature matching and tracking module 231;

[0252] The feature matching and tracking module 231 tracks the image pixel information collected by the binocular camera 212 in real time and matches the image pixel information of two consecutive frames and outputs the matched feature points to the feature point processing module 232;

[0253] The feature point processing module 232 performs feature point triangulation, specifically using a 6-dimensional vector multi-keyframe matching point triangulation feature point depth acquisition method to represent the three-dimensional position of an environmental feature point, including the position of the drone's camera and the direction and depth information of the environmental feature point in which the drone is located; continuously updating the environment, and when the feature estimation covariance is less than a certain set threshold, converting the 6-dimensional representation of the environmental feature point in which the drone is located into a 3-dimensional Euclidean coordinate, establishing multiple feature points in the same frame image, and reducing the representation of feature points belonging to the same frame to a form of a camera pose plus multiple depths, thereby effectively reducing the length of the drone system state. The processed environmental feature point information is output to the visual solution module 241;

[0254] The visual solution module 241 solves the processed environmental feature point information to obtain the camera pose and motion information of the current frame and the local map of the surrounding environment of the current frame in the visual system, and outputs the camera pose and motion information of the current frame to the visual system trajectory generation module 242, and outputs the local map information of the surrounding environment of the current frame in the visual system to the visual system map update module 260;

[0255] The visual system map update module 260 updates the map information of the visual SLAM system 200 in real time and outputs the map information to the visual binocular fusion module 245;

[0256] The visual system trajectory generation module 242 combines the camera pose and motion information of the previous frame with the camera pose and motion information of the current frame to generate a third motion trajectory, combines the second IMU pose and motion information of the previous frame with the second IMU pose and motion information of the current frame to generate a fourth motion trajectory, and outputs the third motion trajectory and the fourth motion trajectory to the visual system trajectory comparison module 243;

[0257] The visual system trajectory comparison module 243 compares the third motion trajectory and the fourth motion trajectory to obtain a second comparison error, compares the second comparison error with a preset visual SLAM calibration error, and outputs the comparison result to the visual system calibration module 244;

[0258] If the second comparison error is greater than the visual SLAM calibration error, the visual system calibration module 244 replaces the second IMU pose and motion information of the current frame with the camera pose and motion information of the current frame. If the second comparison error is less than or equal to the visual SLAM calibration error, the second IMU pose and motion information of the current frame is stored.

[0259] The posture and motion information of the second IMU and the binocular are output to the visual binocular fusion module 245. The visual binocular fusion module 245 fuses the posture and motion information of the second IMU and the camera according to the fusion algorithm, and combines the visual system map information to obtain the second fused posture and motion information of the current frame in the visual system map, and outputs the second fused posture and motion information of the current frame to the system calibration module 410.

[0260] The method and system provided by the present invention simultaneously use a visual system and a three-dimensional laser system based on a system fusion module. When a deviation occurs in the navigation positioning of one of the systems, it can seamlessly switch to the other system through the system switching module, thereby improving the environmental adaptability and stability of the UAV. At the same time, the system processing unit compares and corrects the data of the two systems in real time, making the system positioning and navigation more accurate and applicable to a wider range of scenarios.

[0261] At the same time, the laser SLAM system of the present invention adopts a real-time calibration strategy of laser radar + inertial odometer to realize real-time correction of odometer error, effectively improving the accuracy of drone positioning and map construction of the SLAM algorithm.

[0262] In addition, compared with traditional methods, the feature point depth acquisition method of triangulation of multiple keyframe matching points does not require manual feature extraction or optical flow image extraction, does not require the construction of feature descriptors, does not require inter-frame feature matching, and does not require complex geometric calculations. It can enable new drones to continuously learn in depth and improve the overall reliability of the system.

[0263] The above is a detailed introduction to the visual fusion three-dimensional lidar SLAM positioning method and its drone system provided by the present invention. For general technicians in this field, based on the ideas of the embodiments of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A method for visual fusion laser radar SLAM positioning and navigation, used for unmanned aerial vehicle positioning and navigation, characterized in that: The following steps are involved: (1) obtaining a first fused pose and motion information generated by a three-dimensional laser SLAM system; (2) obtaining a second fused pose and motion information generated by a visual SLAM system; (3) comparing the first fused pose and motion information with the second fused pose and motion information to obtain a third comparison error, and comparing the third comparison error with a system calibration error; (4) when the third comparison error is greater than the system calibration error, switching the current navigation system to the visual SLAM system for navigation of the UAV; The step (3) of comparing the third comparison error with the system calibration error also includes the following steps: (3-1) if the third comparison error is greater than the system calibration error, proceed to step (4), switch to the visual SLAM system, and when there is an external GPS signal, combine the GPS receiving signal to obtain the second fused posture and motion information with the earth coordinate system; (3-2) if the third comparison error is less than or equal to the system calibration error, calculate the usage weights of the three-dimensional laser SLAM system and the visual SLAM system according to the weight algorithm, and when there is an external GPS signal, combine the GPS receiving signal to obtain the third fused posture and motion information with the earth coordinate system.

2. The method for visual fusion laser radar SLAM positioning and navigation according to claim 1, characterized in that: The method further includes the following steps: (5) in step (4), further comprising: when the third comparison error is greater than the system calibration error, switching to the visual SLAM system to obtain the third fused posture and motion information for positioning and navigation of the UAV; or when the third comparison error is less than or equal to the system calibration error, obtaining the second fused posture and motion information, and continuing to use the laser SLAM system for positioning and navigation of the UAV; (6) obtaining map update information of the three-dimensional laser SLAM system and / or obtaining map update information of the visual SLAM system; (7) obtaining the user-specified position information, and generating the best route based on the user-specified position information, map update information, the third fused posture and motion information or the second fused posture and motion information, performing real-time positioning and autonomous navigation of the UAV, and displaying the position of the UAV and controlling the flight of the UAV.

3. The method for visual fusion laser radar SLAM positioning and navigation according to claim 2, characterized in that: The step (1) of obtaining the first fusion posture and motion information generated by the three-dimensional laser SLAM system also includes the following steps: (1-1) obtaining the first IMU posture and motion information of the current frame collected by the first IMU unit, and obtaining the point cloud data information of the current frame collected by the three-dimensional laser radar; (1-2) filtering and preprocessing the collected point cloud data; (1-3) solving the filtered point cloud data to obtain the laser radar posture and motion information of the current frame; (1-4) combining the laser radar posture and motion information of the previous frame with the laser radar posture and motion information of the current frame to generate A first motion trajectory; (1-5) combining the first IMU posture and motion information of the previous frame with the first IMU posture and motion information of the current frame to generate a second motion trajectory; (1-6) comparing the first motion trajectory and the second motion trajectory to obtain a first comparison error, and comparing the first comparison error with the laser SLAM calibration error, wherein the laser SLAM calibration error is a system preset value; (1-7) fusing the posture and motion information of the first IMU and the laser radar according to the comparison result of the first comparison error and the laser SLAM calibration error to obtain the first fused posture and motion information of the current frame.

4. The method for visual fusion laser radar SLAM positioning and navigation according to claim 2, characterized in that: The operation of the visual SLAM system in step (2) to generate the second fusion posture and motion information also includes the following steps: (2-1), obtaining the second IMU posture and motion information of the current frame collected by the second IMU unit, and obtaining the image pixel information of the current frame collected by the camera; (2-2), performing front-end feature matching and tracking on the image pixel information of the current frame; (2-3), triangulating the features, and solving to obtain the camera posture and motion information of the current frame and the local map of the surrounding environment of the current frame in the visual system, and updating the map in the visual system; (2-4), combining the camera posture and motion information of the previous frame with the camera posture of the current frame (2-5) generating a third motion trajectory based on the second IMU pose and motion information of the previous frame and the second IMU pose and motion information of the current frame; (2-6) comparing the third motion trajectory with the fourth motion trajectory to obtain a second comparison error, and comparing the second comparison error with the visual SLAM calibration error, wherein the visual SLAM calibration error is a system preset value; (2-7) fusing the second IMU and camera pose and motion information according to the comparison result of the second comparison error and the visual SLAM calibration error, and obtaining the second fused pose and motion information of the current frame in the visual system map in combination with the visual system map information.

5. The method for visual fusion laser radar SLAM positioning and navigation according to claim 4, characterized in that: Comparing the second comparison error with the visual SLAM calibration error in the step (2-6) also includes the following steps: (2-6-1) if the second comparison error is greater than the visual SLAM calibration error, replacing the second IMU pose and motion information of the current frame with the camera pose and motion information of the current frame; (2-6-2) if the second comparison error is less than or equal to the visual SLAM calibration error, storing the second IMU pose and motion information of the current frame.

6. The method for visual fusion laser radar SLAM positioning and navigation according to claim 5, characterized in that: If the third comparison error in step (3-1) is greater than the system calibration error, the following steps are also included: (3-1-1) comparing the third comparison error with the system switching error; (3-1-2) if the third comparison error is less than or equal to the system switching error, replacing the second fused posture and motion information with the first fused posture and motion information, and storing them in the visual SLAM system; (3-1-3) if the third comparison error is greater than the system switching error, entering step (4) and switching to the visual SLAM system. When there is an external GPS signal, the second fused posture and motion information with the earth coordinate system is obtained in combination with the GPS receiving signal.

7. A UAV system with vision fusion laser radar SLAM positioning and navigation, characterized by: The invention comprises a three-dimensional laser SLAM system (100) for positioning and navigating a drone, a visual SLAM system (200) for positioning and navigating a drone, a flight control unit (300) for controlling the drone, and a system processing unit (400) for processing data of each system, wherein the three-dimensional laser SLAM system (100) provides first fused posture and motion information, the visual SLAM system (200) provides second fused posture and motion information, the system processing unit (400) compares the first fused posture and motion information with the second fused posture and motion information to obtain a third comparison error, and compares the third comparison error with a system calibration error preset by the system; when the third comparison error is greater than the system calibration error preset by the system, the current navigation system is switched to the visual SLAM system for positioning and navigation of the drone; The system processing unit (400) includes a system calibration module (410), a system switching module (420), and a system fusion module (430); the system calibration module (410) is used to correct the error of the visual SLAM system (200) in real time; the system calibration module compares the first fused posture and motion information with the second fused posture and motion information to obtain a third comparison error, and compares the third comparison error with a preset system calibration error; the system switching module (420) is used to switch between the visual SLAM system (200) and the three-dimensional laser SLAM system (100); the system fusion module (430) is used to fuse the posture and motion information of the visual SLAM system (200) and the three-dimensional laser SLAM system (100); when the third comparison error is greater than the system calibration error, the third comparison error is compared with a preset system switching error; If the third comparison error is less than or equal to the system calibration error, the first fused pose and motion information and the second fused pose and motion information are output to the system fusion module; if the third comparison error is less than or equal to the system switching error, the system calibration module replaces the second fused pose and motion information with the first fused pose and motion information, and outputs the first fused pose and motion information and the second fused pose and motion information to the system fusion module; if the third comparison error is greater than the system switching error, the first fused pose and motion information and the second fused pose and motion information are output to the system switching module, and the system switching module only uses the second fused pose and motion information output by the visual SLAM system to locate and navigate the drone; the system fusion module calculates the usage weights of the three-dimensional laser SLAM system and the visual SLAM system according to the results sent by the system calibration module and the weight algorithm, and fuses them to obtain the fused pose and motion information.

8. The UAV system of vision fusion laser radar SLAM positioning and navigation as claimed in claim 7, characterized in that: The laser SLAM system also includes a laser system map update module for updating the map information therein according to the updated first fused pose and motion information, and a global map fusion module for fusing the local map information of all local laser SLAM systems to generate a global three-dimensional map; the visual SLAM system also includes a visual system map update module for updating the map information therein according to the updated second fused pose and motion information.

9. The UAV system of vision fusion laser radar SLAM positioning and navigation as claimed in claim 8, characterized in that: The system also includes a multi-sensor fusion module (440) for fusing the fused posture motion information and updated global three-dimensional map information sent by the system fusion module. When there is an external GPS and / or RTK signal, the GPS and / or RTK information is simultaneously fused to obtain fused posture information with an earth coordinate system.

Images