Point cloud map construction method based on error state Kalman filtering algorithm

Abstract

The invention provides a point cloud map construction method based on an error state Kalman filtering algorithm. The method comprises the following steps: acquiring first data acquired by a laser odometer and second data acquired by a visual odometer; and performing data synchronization association on the first data and the second data to obtain the associated first data and the associated second data. And fusing the associated first data and the associated second data by adopting error state Kalman filtering to obtain a fused odometer. And carrying out nonlinear optimization on the fused odometer by adopting the factor graph of the GTSAM library to obtain an optimized odometer. And combining the optimized odometer with various sensor data to obtain a multi-source point cloud map. Nonlinear optimization can better reduce errors caused by a front end in an agricultural bumpy environment, and the precision and robustness of the fused odometer are improved. A factor graph is adopted for nonlinear optimization, the problem of large-scale nonlinear optimization can be solved, and a multi-source point cloud map provides support for automatic operation, environment monitoring and precise agricultural management.

Description

Technical Field

[0001] The present invention relates to the technical field of map construction, and in particular to a point cloud map construction method based on an error state Kalman filter algorithm. Background Art

[0002] With the development of agricultural modernization, the demand for intelligent management of agricultural plantations such as fruit and tea gardens in hilly and mountainous areas is growing. Among them, precise navigation and positioning technology is the basis for realizing intelligent management. However, the environment of fruit and tea gardens is complex and changeable, and there are challenges such as tree occlusion and terrain undulations, making it difficult for traditional navigation and positioning technology to meet the requirements of high precision and robustness. At present, SLAM (Simultaneous Localization and Mapping) technology has attracted much attention because it can realize autonomous navigation and map construction in unknown environments. In complex environments such as fruit and tea gardens, SLAM technology can help robots or intelligent devices realize autonomous navigation, path planning, environmental perception and other functions, thereby improving agricultural production efficiency and management level.

[0003] Existing SLAM technologies mainly include laser SLAM, visual SLAM, and inertial navigation SLAM. However, single-sensor SLAM technology often has limitations in practical applications. For example, although laser SLAM is not sensitive to environmental changes, it is easily affected by occlusion in complex environments; visual SLAM can obtain rich environmental information, but it is prone to failure in conditions such as lighting changes and texture loss; inertial navigation SLAM has the problem of cumulative errors, making it difficult to ensure long-term navigation accuracy.

[0004] Therefore, in view of the limitations of existing technologies, a point cloud map construction method is needed that can combine the advantages of multiple sensors and has high accuracy and strong robustness. Summary of the invention

[0005] In order to overcome the problems existing in the related art, the purpose of the present invention is to provide a point cloud map construction method based on the error state Kalman filter algorithm, which can combine the advantages of multiple sensors, and has high accuracy and strong robustness. In addition, in complex environments such as fruit and tea gardens, this method can overcome challenges such as tree occlusion and terrain undulations, realize autonomous navigation and map construction, which is of great significance for improving agricultural production efficiency and management level.

[0006] A point cloud map construction method based on an error state Kalman filter algorithm, comprising:

[0007] Acquire first data collected by a laser odometer and second data collected by a visual odometer;

[0008] Synchronously associating the first data with the second data to obtain associated first data and associated second data;

[0009] Using an error state Kalman filter to fuse the associated first data and the associated second data to obtain a fused odometer;

[0010] The fusion odometer is nonlinearly optimized using a factor graph of a GTSAM library to obtain an optimized odometer;

[0011] The optimized odometer is combined with multiple sensor data to obtain a multi-source point cloud map.

[0012] In a preferred technical solution of the present invention, the step of synchronously associating the first data with the second data to obtain the associated first data and the associated second data includes:

[0013] Time alignment of multiple sensors;

[0014] The plurality of sensors are spatially aligned, wherein the plurality of sensors include lidar, camera, and inertial measurement unit.

[0015] In a preferred technical solution of the present invention, the time alignment of the multiple sensors includes:

[0016] Using a robot operating system to compare message timestamps of the laser radar, the camera, and the inertial measurement unit;

[0017] Counting the time offsets between the laser radar, the camera, and the inertial measurement unit;

[0018] A time synchronizer is used to adjust the time relationship between the data of the laser radar, the camera and the inertial measurement unit.

[0019] In a preferred technical solution of the present invention, the spatial alignment of the multiple sensors includes:

[0020] Using the coordinate system of the inertial measurement unit as the standard coordinate system;

[0021] Calibrate the laser radar based on the standard coordinate system to obtain a first transformation matrix of the inertial measurement unit relative to the laser radar;

[0022] Calibrate the camera based on the standard coordinate system to obtain a second transformation matrix of the inertial measurement unit relative to the camera;

[0023] Multiplying the first transformation matrix by the laser point cloud data to transform the laser point cloud data in the coordinate system of the laser radar into the standard coordinate system;

[0024] The second transformation matrix is ​​multiplied by the camera data to transform the camera data in the camera coordinate system into the standard coordinate system.

[0025] In a preferred technical solution of the present invention, the factor graph of the GTSAM library is used to perform nonlinear optimization on the fusion odometer to obtain the optimized odometer, including:

[0026] Representing the system state as a set of state variables, wherein the state variables include robot posture and feature point positions;

[0027] The observation data and the motion model are represented as a constraint factor group; wherein the constraint factor group includes a plurality of constraint factors, each of which corresponds to a constraint function;

[0028] Adding the constraint functions corresponding to all the constraint factors to obtain a final optimization function;

[0029] The value of the final optimization function is updated in an iterative manner until the value of the final optimization function converges.

[0030] In a preferred technical solution of the present invention, the multi-source point cloud map includes a global laser map, a local dense map and a fine crop map.

[0031] In a preferred technical solution of the present invention, the optimized odometer is combined with multiple sensor data to obtain a multi-source point cloud map, including:

[0032] The global laser map, local dense map and fine crop map are calculated according to the following formulas:

[0033]

[0034] in, is the global laser map, is a local dense map, For fine crop maps; is the laser map of the i-th frame, is the local point cloud map of the i-th frame, is the fine point cloud map of the i-th frame; T i is the optimized odometer, is the inverse matrix of the laser global pose, is the inverse matrix of the local visual pose, is the inverse matrix of the binocular fine pose.

[0035] In a preferred technical solution of the present invention, before the data synchronization association is performed on the first data and the second data, the method further includes:

[0036] assigning a first weight to the first data;

[0037] A second weight is assigned to the second data; wherein the first weight is greater than the second weight.

[0038] In a preferred technical solution of the present invention, the process further includes:

[0039] Construct a global low-precision map and local high-precision sub-maps;

[0040] Define the hierarchical factor graph optimization objective function to jointly optimize the global pose and local features:

[0041]

[0042] in, is the pose estimate of the i-th node in the global map, is the corresponding prior constraint; is the observation model of the jth feature point in the local map, is the actual measured value; global is the global pose weight, λ local is the local feature weight, v is the real-time motion speed of the robot;

[0043] Dynamically adjust the global posture weight and the local feature weight according to the real-time movement speed of the robot;

[0044] The construction density of local submaps is adjusted through an adaptive resolution strategy.

[0045] In a preferred technical solution of the present invention, before combining the optimized odometer with multiple sensor data, the method further includes:

[0046] The Mask-RCNN instance segmentation network is used to detect dynamic objects in the fruit and tea garden environment in real time and output the dynamic region segmentation probability P mask ;

[0047] According to the dynamic area segmentation probability and the local geometric gradient of the point cloud, the dynamic point cloud removal threshold is calculated:

[0048]

[0049] Among them, P mask is the dynamic region segmentation probability, is the local geometric gradient of the point cloud, γ dynamic is the dynamic point cloud rejection threshold, S max is the maximum gradient amplitude of the scene, α is the dynamic area weight, and β is the geometric gradient weight;

[0050] If the dynamic point cloud rejection threshold is greater than a preset threshold, the corresponding dynamic point cloud is determined to be dynamic interference data, and the dynamic point cloud belonging to the dynamic interference data is rejected.

[0051] The beneficial effects of the present invention are:

[0052] The point cloud map construction method based on the error state Kalman filter algorithm provided by the present invention includes obtaining the first data collected by the laser odometer and the second data collected by the visual odometer. The data collection frequencies of the laser odometer and the visual odometer are different, and the first data and the second data generated can complement each other. The first data and the second data are synchronously associated, and the multi-sensor data are time-aligned and space-aligned to reduce the data error, thereby improving the accuracy of data fusion. The laser odometer has a small cumulative error in a large scene and is not affected by the outdoor lighting environment, but does not have a loop detection module and a nonlinear optimization module. The visual odometer has a loop detection module and a nonlinear optimization module, but the disadvantage is that the accuracy of the large scene is poor and depends on the outdoor lighting conditions. The error state Kalman filter is used to fuse the associated first data and the associated second data to obtain a fused odometer. The factor graph of the GTSAM library is used to perform nonlinear optimization on the fused odometer to obtain an optimized odometer. Nonlinear optimization can better reduce the error caused by the front end in the bumpy agricultural environment and improve the accuracy and robustness of the fused odometer. Using factor graphs for nonlinear optimization can solve large-scale nonlinear optimization problems. The optimized odometer is combined with data from multiple sensors to obtain a multi-source point cloud map. The multi-source point cloud map is not only rich in details, but also has high-precision spatial positioning capabilities. The multi-source point cloud map provides support for subsequent automated operations, environmental monitoring, and precision agricultural management. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 is a flow chart of a point cloud map construction method based on an error state Kalman filter algorithm of the present invention;

[0054] Figure 2 is a flow chart of time alignment of multiple sensors according to the present invention;

[0055] Figure 3 It is a flow chart of the present invention for performing nonlinear optimization on the fused odometer using the factor graph of the GTSAM library. DETAILED DESCRIPTION

[0056] The preferred embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to make the present invention more thorough and complete, and to fully convey the scope of the present invention to those skilled in the art.

[0057] Example 1

[0058] like Figure 1 As shown, this embodiment provides a point cloud map construction method based on an error state Kalman filter algorithm, comprising:

[0059] S1: Acquire first data collected by a laser odometer and second data collected by a visual odometer.

[0060] S2: Synchronously associate the first data with the second data to obtain associated first data and associated second data.

[0061] S3: Using an error state Kalman filter to fuse the associated first data and the associated second data to obtain a fused odometer.

[0062] S4: Use the factor graph of the GTSAM library to perform nonlinear optimization on the fused odometer to obtain an optimized odometer.

[0063] S5: Combining the optimized odometer with multiple sensor data to obtain a multi-source point cloud map.

[0064] Before synchronously associating the first data with the second data, the method further includes:

[0065] S21': assigning a first weight to the first data.

[0066] S22': assign a second weight to the second data; wherein the first weight is greater than the second weight.

[0067] The first data and the second data are directly input into the SLAM system. The SLAM system of this embodiment is a loosely coupled system. This embodiment takes the first weight as 0.7 and the second weight as 0.3 as an example. The first weight represents the confidence of the first data corresponding to the laser odometer, and the second weight represents the confidence of the second data corresponding to the visual odometer. Generally, the laser odometer is more emphasized, that is, the confidence of the first data collected by the laser odometer is higher than the confidence of the second data collected by the visual odometer.

[0068] The step of synchronously associating the first data with the second data to obtain the associated first data and the associated second data includes:

[0069] S21: Time alignment of multiple sensors.

[0070] S22: spatially aligning the multiple sensors, wherein the multiple sensors include a lidar, a camera, and an inertial measurement unit.

[0071] like Figure 2 As shown, the time alignment of multiple sensors includes:

[0072] S211: Using a robot operating system to compare message timestamps of the laser radar, the camera, and the inertial measurement unit.

[0073] S212: Count the time offsets between the laser radar, the camera, and the inertial measurement unit.

[0074] S213: Use a time synchronizer to adjust the time relationship between the data of the laser radar, the camera, and the inertial measurement unit.

[0075] Time alignment means ensuring that the data collected by different sensors have the same time reference. By calibrating the sensor's timestamp, the data from different sensors are synchronized in time. Time alignment helps to avoid data errors and improve the accuracy of data fusion, ensuring that the data acquired by the SLAM system at the same time is consistent.

[0076] Spatial alignment is to ensure that the data collected by different sensors can be correctly matched and aligned in space. Different sensors may have different positions, postures or coordinate systems. The task of spatial alignment is to adjust these parameters so that the data from different sensors can correspond to the same position in a common spatial coordinate system.

[0077] In a multi-sensor system, the data collected by each sensor generally has different time differences. For example, the sampling frequency of IMU is 10Hz-200Hz, the sampling frequency of LiDAR is 10Hz, and the sampling frequency of camera is 30Hz. Due to the difference in time sampling, time differences will occur. These time differences are very small, only about tens of milliseconds, but when the data from different sensors are integrated, different timestamps will cause information dislocation and inaccurate fusion. Therefore, multi-sensor timestamp synchronization is required to eliminate differences and ensure that the data from different sensors are consistent in time, so that the data can be matched and fused in the correct time sequence.

[0078] ROS is used for multi-sensor timestamp soft synchronization. The principle of ROS timestamp synchronization is based on coordinating the messages of multiple sensors to ensure that they are consistent in time. By comparing the timestamps of sensor messages, the system calculates the time offset between different sensors and adjusts the time relationship of the data through the time synchronizer. The ROS time system is used as the time base of the entire system.

[0079] When the data from the lidar, camera, and inertial measurement unit are input into the ROS system, the data is input into the buffer, and the alignment data between the data is found within the minimum time difference. This embodiment takes the minimum time difference of 0.1 second as an example.

[0080] The spatial alignment of the multiple sensors comprises:

[0081] S221: Using the coordinate system of the inertial measurement unit as the standard coordinate system.

[0082] S222: Calibrate the laser radar based on the standard coordinate system to obtain a first transformation matrix of the inertial measurement unit relative to the laser radar.

[0083] S223: Calibrate the camera based on the standard coordinate system to obtain a second transformation matrix of the inertial measurement unit relative to the camera.

[0084] S224: Multiply the laser point cloud data by the first transformation matrix to transform the laser point cloud data in the coordinate system of the laser radar into the standard coordinate system.

[0085] S225: Multiply the second transformation matrix by the camera data to transform the camera data in the camera coordinate system into the standard coordinate system.

[0086] The spatial alignment of the ROS system requires unifying the three coordinate systems of the lidar, camera, and inertial measurement unit, selecting the coordinate system of the inertial measurement unit as the standard coordinate system, and converting the observation data of the lidar and camera to the standard coordinate system. By calibrating the sensors, the transformation matrix T of the inertial measurement unit relative to the lidar is obtained. 1 , the transformation matrix T of the inertial measurement unit relative to the stereo camera 2 , and the transformation matrix T of the inertial measurement unit relative to the depth camera 3 The data transmitted between sensors, such as laser point cloud {L}, is transformed by relative pose T 1 *{L}, synchronized to the IMU (Inertial Measurement Unit) coordinate system.

[0087] The point cloud map construction method based on the error state Kalman filter algorithm provided in this embodiment includes obtaining the first data collected by the laser odometer and the second data collected by the visual odometer. The data collection frequencies of the laser odometer and the visual odometer are different, and the first data and the second data generated can complement each other. The first data and the second data are synchronously associated, and the multi-sensor data are time-aligned and spatially aligned to reduce the data error, thereby improving the accuracy of data fusion. The laser odometer has a small cumulative error in large scenes and is not affected by the outdoor lighting environment, but there is no loop detection module and a nonlinear optimization module. The visual odometer has a loop detection module and a nonlinear optimization module, but the disadvantage is that the accuracy of large scenes is poor and depends on outdoor lighting conditions. The error state Kalman filter is used to fuse the associated first data and the associated second data to obtain a fused odometer. The factor graph of the GTSAM library is used to perform nonlinear optimization on the fused odometer to obtain an optimized odometer. Nonlinear optimization can better reduce the error caused by the front end in the bumpy agricultural environment and improve the accuracy and robustness of the fused odometer. Using factor graphs for nonlinear optimization can solve large-scale nonlinear optimization problems. The optimized odometer is combined with data from multiple sensors to obtain a multi-source point cloud map. The multi-source point cloud map is not only rich in details, but also has high-precision spatial positioning capabilities. The multi-source point cloud map provides support for subsequent automated operations, environmental monitoring, and precision agricultural management.

[0088] Example 2

[0089] This embodiment provides a point cloud map construction method based on an error state Kalman filter algorithm. This embodiment only describes the differences from Embodiment 1, such as Figure 3 As shown, the factor graph of the GTSAM library is used to perform nonlinear optimization on the fusion odometer to obtain the optimized odometer, including:

[0090] S41: Representing the system state as a state variable group, wherein the state variables include the robot posture and feature point positions.

[0091] S42: Representing the observed data and the motion model as a constraint factor group; wherein the constraint factor group includes a plurality of constraint factors, and each constraint factor corresponds to a constraint function.

[0092] S43: Add the constraint functions corresponding to all the constraint factors to obtain a final optimization function.

[0093] S44: updating the value of the final optimization function in an iterative manner until the value of the final optimization function converges.

[0094] Factor graph optimization is a method for solving large-scale nonlinear optimization problems. Observation data and motion models are represented as a set of factors to constrain the relationship between variables. The ROS system of the present invention includes three constraint factors, namely, laser odometer factor ZL, visual odometer factor ZV and loop factor Loop. In factor graph optimization, each factor represents a constraint condition of the system. The goal of optimization is to minimize the sum of all constraint functions, that is, the final optimization function, so that the state variables of the system reach the optimal solution.

[0095] The factor graph is optimized in an iterative manner. Each iteration updates the state variables to optimize the value of the constraint function until it converges to the optimal solution or reaches the stopping condition, thereby improving the consistency and continuity of the optimization process.

[0096] The multi-source point cloud map includes a global laser map, a local dense map and a fine crop map. According to the development needs of the forest, fruit and tea garden inspection equipment in the Lingnan region, and in view of the lack of suitable technology in the intelligent movement of hilly and mountainous inspection equipment and the low degree of automation, this paper combines the hilly and mountainous multi-kinetic chassis mobile equipment, based on the error iterative Kalman filter algorithm, and integrates the laser odometer, visual odometer and IMU sensor to construct a SLAM algorithm suitable for agricultural, forestry and tea garden inspection robots. At the same time, it constructs a global laser map and a local visual dense map to assist the robot in completing the inspection task.

[0097] The role of SLAM in agriculture is not only based on navigation, but also requires observation of local maps. Sometimes, in order to observe crops, it is also necessary to build a detailed three-dimensional crop model. Therefore, it is necessary to build a multi-source point cloud map in the agricultural environment, including a global laser map, a local dense map and a detailed crop map.

[0098] The optimized odometer is combined with multiple sensor data to obtain a multi-source point cloud map, including:

[0099] The global laser map, local dense map and fine crop map are calculated according to the following formulas:

[0100]

[0101] in, is the global laser map, is a local dense map, For fine crop maps; is the laser map of the i-th frame, is the local point cloud map of the i-th frame, is the fine point cloud map of the i-th frame; T i is the optimized odometer, is the inverse matrix of the laser global pose, is the inverse matrix of the local visual pose, is the inverse matrix of the binocular fine pose.

[0102] The updating of the value of the final optimization function in an iterative manner includes:

[0103] Construct the first constraint function corresponding to the laser constraint factor:

[0104]

[0105] in, is the laser confinement factor, for The set of nearest points on all local maps; For the forward position, is the total point set of the i-th laser radar frame, represents the forward posture when the first constraint function reaches the minimum value, |||| 2 represents the square of the 2-norm.

[0106] This application uses the method of point cloud residual calculation based on laser keyframes to construct the cost function. Due to the use of efficient IKD tree, all scanning points are used instead of line and surface feature points. Using a sliding window approach, the local map M is constructed by combining the previous k laser radar frames i-1 , is the i-th laser radar frame The following formula is used to construct the local map of the i-1th frame:

[0107]

[0108] in, is the point set of the previous k frames, is the first k laser pose estimates, M i-1 is the local map of the i-1th frame.

[0109] After constructing the local map M of the i-1th frame i-1 After that, we need to use the ikd tree and KNN algorithm to find All in M i-1 The nearest neighbor point set on Construct the first constraint function. Optimize the forward pose through constraints Achieve laser optimization factor.

[0110] After constructing the first constraint function corresponding to the laser constraint factor, the method further includes:

[0111] Construct the second constraint function corresponding to the camera constraint factor:

[0112]

[0113] in, is the camera constraint factor, represents the i-th feature point, Represents the spatial point set of the i-th camera frame; represents the pose estimation of the i-th frame, KD is the adjustment factor, Represents the pose estimate when the second constraint function reaches the minimum value.

[0114] The present invention uses the reprojection error of the visual key frame feature points to construct the residual to construct the cost function of the visual odometer factor. Input system, the feature points of the current frame and PnP form the spatial point set of the i-th camera frame Combine the spatial point set of the i-th camera frame and the pose estimate of the i-th frame to calculate the reprojected pixel points:

[0115]

[0116] in, is the i-th reprojected pixel.

[0117] The present invention maintains a sliding window of the laser radar keyframe for the feature graph, which ensures limited computational complexity. In addition, the present invention uses incremental ISAM to calculate each constraint in real time, and the new robot state will be added as a node in the factor graph.

[0118] This embodiment uses the factor graph of the GTSAM library to perform nonlinear optimization on the fusion odometer to obtain the optimized odometer, including representing the system state as a state variable group, and the state variables include the robot posture and the position of the feature point. The observation data and the motion model are represented as a constraint factor group; wherein the constraint factor group includes multiple constraint factors, and each constraint factor corresponds to a constraint function. The constraint functions corresponding to all constraint factors are added to obtain the final optimization function. The value of the final optimization function is updated in an iterative manner until the value of the final optimization function converges. Factor graph optimization is a method for solving large-scale nonlinear optimization problems. The observation data and the motion model are represented as a set of factors for the relationship between the constraint variables. The ROS system of the present invention includes three constraint factors, namely, the laser odometer factor ZL, the visual odometer factor ZV and the loop factor Loop. In the factor graph optimization, each factor represents a constraint condition of the system. The goal of the optimization is to minimize the sum of all constraint functions, that is, the final optimization function, so that the state variables of the system reach the optimal solution. The factor graph is optimized in an iterative manner. Each iteration updates the state variables to optimize the value of the constraint function until it converges to the optimal solution or reaches the stopping condition, thereby improving the consistency and continuity of the optimization process.

[0119] Example 3

[0120] This embodiment only describes the differences from Embodiment 1.

[0121] After the value of the final optimization function converges, the method further includes:

[0122] Construct a global low-precision map and local high-precision sub-maps;

[0123] Define the hierarchical factor graph optimization objective function to jointly optimize the global pose and local features:

[0124]

[0125] in, is the pose estimate of the i-th node in the global map, is the corresponding prior constraint; is the observation model of the jth feature point in the local map, is the actual measured value; global is the global pose weight, λ local is the local feature weight, v is the real-time motion speed of the robot;

[0126] Dynamically adjust the global posture weight and the local feature weight according to the real-time movement speed of the robot;

[0127] The construction density of local submaps is adjusted through an adaptive resolution strategy.

[0128] Before combining the optimized odometer with multiple sensor data, the method further includes:

[0129] The Mask-RCNN instance segmentation network is used to detect dynamic objects in the fruit and tea garden environment in real time and output the dynamic region segmentation probability P mask ;

[0130] According to the dynamic area segmentation probability and the local geometric gradient of the point cloud, the dynamic point cloud removal threshold is calculated:

[0131]

[0132] Among them, P mask is the dynamic region segmentation probability, is the local geometric gradient of the point cloud, γ dynamic is the dynamic point cloud rejection threshold, S max is the maximum gradient amplitude of the scene, α is the dynamic area weight, and β is the geometric gradient weight;

[0133] If the dynamic point cloud rejection threshold is greater than a preset threshold, the corresponding dynamic point cloud is determined to be dynamic interference data, and the dynamic point cloud belonging to the dynamic interference data is rejected.

[0134] The sum of the dynamic area weight α and the geometric gradient weight β is 1. In this embodiment, the preset threshold is set to 0.5. Combining the label consistency detection algorithm with the polluted frame removal strategy can ensure the global consistency of the static map and avoid map errors introduced by dynamic interference.

[0135] Preferably, the first weight and the second weight are adjusted by dynamically adjusting the weights, including: (1) real-time collection of environmental parameters, including light intensity, texture richness, and laser occlusion ratio. (2) based on a fuzzy logic controller, dynamically calculating the fusion weight of the laser odometer and the visual odometer according to the environmental parameters. (3) when the light intensity is lower than the threshold or the texture is missing, the first weight is increased to above 0.8; when the laser occlusion ratio exceeds 50%, the second weight is increased to above 0.5.

[0136] Preferably, a sensor redundancy and fault tolerance mechanism is set up: (1) Real-time monitoring of the data confidence of the lidar, camera, and inertial measurement unit. (2) When the confidence of a sensor is lower than the threshold, the backup sensor switching logic is automatically triggered, and Kalman prediction interpolation is performed based on historical data. (3) When the lidar fails, the binocular visual odometer is activated and the GPS data is fused, and the positioning information is compensated through factor graph optimization.

[0137] Preferably, the ROS system of the present invention further includes an energy efficiency optimization module, which performs the following adjustments: (1) using sparse processing technology to downsample point cloud data in non-critical areas. (2) selectively triggering high power consumption modes of the laser radar and camera based on motion state detection. (3) reducing the computational load of the main processor by distributing factor graph optimization tasks through edge computing nodes.

[0138] This embodiment, until the value of the final optimization function converges, also includes constructing a global low-precision map and a local high-precision sub-map, defining a hierarchical factor graph optimization objective function, and jointly optimizing the global pose and local features. The global pose weight and local feature weight are dynamically adjusted according to the real-time motion speed of the robot, and the construction density of the local sub-map is adjusted through an adaptive resolution strategy. Before combining the optimized odometer with multiple sensor data, it also includes: using the Mask-RCNN instance segmentation network to detect dynamic objects in the fruit and tea garden environment in real time, and outputting the dynamic area segmentation probability P mask ; According to the dynamic area segmentation probability and the local geometric gradient of the point cloud, the dynamic point cloud rejection threshold is calculated. If the dynamic point cloud rejection threshold is greater than the preset threshold, the corresponding dynamic point cloud is determined to be dynamic interference data, and the dynamic point cloud belonging to dynamic interference data is rejected. Combining the label consistency detection algorithm with the contaminated frame rejection strategy can ensure the global consistency of the static map and avoid map errors introduced by dynamic interference.

[0139] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, device, article or method including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, device, article or method. In the absence of further restrictions, an element defined by the sentence "includes a ..." does not exclude the presence of other identical elements in the process, device, article or method including the element.

[0140] The above description is only a preferred embodiment of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A point cloud map construction method based on an error state Kalman filter algorithm, characterized in that: include: Acquire first data collected by a laser odometer and second data collected by a visual odometer; Synchronously associating the first data with the second data to obtain associated first data and associated second data; Using an error state Kalman filter to fuse the associated first data and the associated second data to obtain a fused odometer; The fusion odometer is nonlinearly optimized using a factor graph of a GTSAM library to obtain an optimized odometer; The optimized odometer is combined with multiple sensor data to obtain a multi-source point cloud map.

2. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 1 is characterized in that: The step of synchronously associating the first data with the second data to obtain the associated first data and the associated second data includes: Time alignment of multiple sensors; The plurality of sensors are spatially aligned, wherein the plurality of sensors include lidar, camera, and inertial measurement unit.

3. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 2 is characterized in that: The time alignment of multiple sensors includes: Using a robot operating system to compare message timestamps of the laser radar, the camera, and the inertial measurement unit; Counting the time offsets between the laser radar, the camera, and the inertial measurement unit; A time synchronizer is used to adjust the time relationship between the data of the laser radar, the camera and the inertial measurement unit.

4. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 2 is characterized in that: The spatial alignment of the multiple sensors comprises: Using the coordinate system of the inertial measurement unit as the standard coordinate system; Calibrate the laser radar based on the standard coordinate system to obtain a first transformation matrix of the inertial measurement unit relative to the laser radar; Calibrate the camera based on the standard coordinate system to obtain a second transformation matrix of the inertial measurement unit relative to the camera; Multiplying the first transformation matrix by the laser point cloud data to transform the laser point cloud data in the coordinate system of the laser radar into the standard coordinate system; The second transformation matrix is ​​multiplied by the camera data to transform the camera data in the camera coordinate system into the standard coordinate system.

5. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 1 is characterized in that: The method of using the factor graph of the GTSAM library to perform nonlinear optimization on the fusion odometer to obtain an optimized odometer includes: Representing the system state as a set of state variables, wherein the state variables include robot posture and feature point positions; The observation data and the motion model are represented as a constraint factor group; wherein the constraint factor group includes a plurality of constraint factors, each of which corresponds to a constraint function; Adding the constraint functions corresponding to all the constraint factors to obtain a final optimization function; The value of the final optimization function is updated in an iterative manner until the value of the final optimization function converges.

6. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 1 is characterized in that: The multi-source point cloud map includes a global laser map, a local dense map and a fine crop map.

7. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 6 is characterized in that: The optimized odometer is combined with multiple sensor data to obtain a multi-source point cloud map, including: The global laser map, local dense map and fine crop map are calculated according to the following formulas: in, is the global laser map, is a local dense map, For fine crop maps; is the laser map of the i-th frame, is the local point cloud map of the i-th frame, is the fine point cloud map of the i-th frame; T i is the optimized odometer, is the inverse matrix of the laser global pose, is the inverse matrix of the local visual pose, is the inverse matrix of the binocular fine pose.

8. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 5 is characterized in that: Before the data synchronization association is performed on the first data and the second data, the method further includes: assigning a first weight to the first data; A second weight is assigned to the second data; wherein the first weight is greater than the second weight.

9. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 5, characterized in that: After the value of the final optimization function converges, the method further includes: Construct a global low-precision map and local high-precision sub-maps; Define the hierarchical factor graph optimization objective function to jointly optimize the global pose and local features: in, is the pose estimate of the i-th node in the global map, is the corresponding prior constraint; is the observation model of the jth feature point in the local map, is the actual measured value; global is the global pose weight, λ local is the local feature weight, v is the real-time motion speed of the robot; Dynamically adjust the global posture weight and the local feature weight according to the real-time movement speed of the robot; The construction density of local submaps is adjusted through an adaptive resolution strategy.

10. The point cloud map construction method based on the error state Kalman filter algorithm according to claim 1, characterized in that: Before combining the optimized odometer with multiple sensor data, the method further includes: The Mask-RCNN instance segmentation network is used to detect dynamic objects in the fruit and tea garden environment in real time and output the dynamic region segmentation probability P mask ; According to the dynamic area segmentation probability and the local geometric gradient of the point cloud, the dynamic point cloud removal threshold is calculated: Among them, P mask is the dynamic region segmentation probability, is the local geometric gradient of the point cloud, γ dynamic is the dynamic point cloud rejection threshold, S max is the maximum gradient amplitude of the scene, α is the dynamic area weight, and β is the geometric gradient weight; If the dynamic point cloud rejection threshold is greater than a preset threshold, the corresponding dynamic point cloud is determined to be dynamic interference data, and the dynamic point cloud belonging to the dynamic interference data is rejected.

Images