Map construction method and system based on laser vision dynamic weighted fusion

Abstract

The invention provides a map construction method and system based on laser vision dynamic weighted fusion, and belongs to the technical field of data processing, and the method comprises the steps: obtaining an RGB image and a depth image through a vision sensor, and obtaining point cloud data through a laser radar; aligning the depth image with the RGB image, and removing invalid depth pixels; based on an ORB-SLAM2 algorithm, performing visual SLAM, extracting features of the depth image and the RGB image, and outputting sparse visual point cloud data; based on a Gmapping algorithm, performing laser radar SLAM, and outputting a 2D occupied grid map; projecting the sparse visual point cloud data into a 2D occupation grid map, and calculating the semantic occupation probability of each grid; calculating the geometric occupancy probability of each grid through an anti-sensor model; performing weighted fusion on the semantic occupancy probability and the geometric occupancy probability, and calculating a fusion occupancy probability; and generating a fusion map according to the fusion occupation probability.

Description

Technical Field

[0001] The present invention relates to the field of data processing technology, and in particular to a map construction method and system based on laser vision dynamic weighted fusion. Background Art

[0002] Building environmental maps is crucial in a variety of fields, including robotics, autonomous driving, augmented reality (AR), and smart manufacturing. Environmental maps are not only the foundation for machine perception and decision-making, but also improve system efficiency, safety, and intelligence.

[0003] Currently, environmental maps are primarily constructed using LiDAR (LiDAR) scanning, which offers high-precision distance measurement and strong anti-interference capabilities, particularly in complex environments. LiDAR generates three-dimensional point cloud data by transmitting a laser beam and receiving reflected signals, enabling precise perception of the surrounding structure. This technology is widely used in autonomous driving, robotic navigation, intelligent manufacturing, and other fields.

[0004] However, LiDAR lacks information in the height direction and can only scan a planar environment. Its performance is poor when facing complex environments. The environmental map constructed by LiDAR scanning may have structural missing, affecting the map accuracy. Summary of the Invention

[0005] The main purpose of the present invention is to provide a map construction method and system based on laser vision dynamic weighted fusion to solve the problem that the laser radar in the existing technology lacks information in the height direction, can only scan a planar environment, has poor performance when facing complex environments, and the environmental map constructed by laser radar scanning may have structural missing, affecting the map accuracy.

[0006] To achieve the above objectives, according to one aspect of the present invention, a map construction method based on laser vision dynamic weighted fusion is provided, comprising:

[0007] S1: Obtain RGB images and depth images through visual sensors, and obtain point cloud data through lidar;

[0008] S2: Align the depth image with the RGB image and remove invalid depth pixels;

[0009] S3: Perform visual SLAM based on the ORB-SLAM2 algorithm, extract features of the depth image and the RGB image, and output sparse visual point cloud data;

[0010] S4: Perform lidar SLAM based on the Gmapping algorithm and output a 2D occupancy grid map;

[0011] S5: Projecting the sparse visual point cloud data into the 2D occupancy grid map and calculating the semantic occupancy probability of each grid;

[0012] S6: Calculate the geometric occupancy probability of each grid through the inverse sensor model;

[0013] S7: performing weighted fusion on the semantic occupancy probability and the geometric occupancy probability to calculate a fused occupancy probability;

[0014] S8: Generate a fusion map according to the fusion occupancy probability.

[0015] Furthermore, the S3 specifically includes:

[0016] S301: Using the ORB algorithm, extracting feature points from the RGB image, and searching the depth image for depth information corresponding to the feature points determined based on the SLAM algorithm;

[0017] S302: Determine the depth confidence of each data point based on the depth information;

[0018] S303: Normalize the depth confidence of each data point to obtain a depth confidence weight of each data point;

[0019] S304: Using a nearest neighbor search algorithm, matching feature points of the current frame with those of the previous frame;

[0020] S305: Using the matched feature points, the PnP algorithm is used to estimate the camera pose.

[0021] S306: configuring an inertial measurement unit (IMU) in the visual sensor, and calculating a relative motion estimate of the visual sensor by pre-integrating the angular velocity and acceleration of the IMU between two consecutive image frames;

[0022] S307: Based on the relative motion estimation of the visual sensor and the camera pose estimation, local BA optimization is used to optimize the camera pose and data point positions. The depth confidence weight and IMU residual term are introduced into the optimization process to jointly optimize the vision and IMU residuals, suppress pure vision pose drift, and perform pose correction on the point cloud data of the visual sensor.

[0023] S308: Segment the dynamic objects in real time through a lightweight YOLOv5 network, remove feature points in the dynamic area, and retain the static background to obtain the sparse visual point cloud data.

[0024] Furthermore, the S4 specifically includes:

[0025] S401: Estimate the robot's position on the map using particle filtering based on the point cloud data acquired by the lidar.

[0026] S402: Use a motion model to describe how the robot moves in space and update the position of the particles based on the control input;

[0027] S403: Differentiating point cloud data of adjacent frames to distinguish static obstacles from dynamic objects, and removing point cloud data in areas where dynamic objects are located;

[0028] S404: Using a scan matching algorithm, calculate the matching degree between the position of each particle and the current laser scan;

[0029] S405: During the particle filtering process, the weight of the particle is updated according to the result of each laser matching;

[0030] S406: Resampling is performed based on the weights of the particles to generate a new particle set;

[0031] S407: Generate a 2D occupancy grid map according to the particle set.

[0032] Furthermore, the S5 specifically includes:

[0033] S501: Projecting the sparse visual point cloud data into the 2D occupancy grid map;

[0034] S502: Detect the semantic category of each data point in real time using a lightweight YOLOv5 network to determine the semantic label of each data point;

[0035] S503: Counting the semantic labels of the data points in each grid;

[0036] S504: Calculate the weighted probability of each semantic tag appearing based on the depth confidence weight of each data point;

[0037] S505: Based on the Poisson distribution model, the semantic occupancy probability of each grid is calculated according to the weighted probability of each semantic label appearing.

[0038] Furthermore, the semantic labels include: sheep house, sheep flock, obstacle and ground.

[0039] Furthermore, the S6 specifically includes:

[0040] S601: When the laser radar scans a grid, the logarithmic probability is updated based on whether the end point of the laser beam hits an obstacle;

[0041] S602: Based on the Sigmoid function, convert the logarithmic probability of each grid into the geometric occupancy probability.

[0042] Furthermore, the S7 specifically includes:

[0043] S701: Assume that the semantic occupancy probability and the geometric occupancy probability are conditionally independent under a given grid state;

[0044] S702: Setting a priori probability according to the site environment;

[0045] S703: Perform weighted fusion on the semantic occupancy probability and the geometric occupancy probability through Bayesian fusion to calculate a fused occupancy probability.

[0046] Furthermore, the S8 is specifically as follows:

[0047] It is determined whether the fusion occupancy probability of each grid is greater than the preset occupancy probability; if so, the grid is set to an occupied state; otherwise, the grid is set to an idle state to form the fusion map.

[0048] Furthermore, the map construction method based on laser vision dynamic weighted fusion also includes:

[0049] S9: When an environment change is detected, local map reconstruction is triggered.

[0050] According to one aspect of the present invention, a map construction system based on laser vision dynamic weighted fusion is provided, comprising:

[0051] processor;

[0052] The memory stores computer-readable instructions, which, when executed by the processor, implement the above-mentioned map construction method based on dynamic weighted fusion of laser vision.

[0053] By applying the technical solution of the present invention, the visual sensor provides rich detailed information, especially in the performance of height information and complex scenes, while the lidar provides reliable geometric structure data. By combining the data of visual and lidar sensors, it is possible to make up for the shortcomings of a single sensor and improve the accuracy and completeness of the environmental map.

[0054] In addition to the above-described objects, features and advantages, the present invention has other objects, features and advantages. The present invention will be further described in detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The accompanying drawings, which constitute part of the present invention, are intended to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are intended to explain the present invention and do not constitute an undue limitation of the present invention. In the accompanying drawings:

[0056] Figure 1A flow chart of a map construction method based on laser vision dynamic weighted fusion provided by an embodiment of the present invention is shown.

[0057] Figure 2 A structural schematic diagram of a map construction method based on laser vision dynamic weighted fusion provided by an embodiment of the present invention is shown.

[0058] Figure 3 A structural diagram of a map construction system based on laser vision dynamic weighted fusion provided by an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0059] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0060] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0061] It should be noted that the terms "first," "second," and the like in the specification and claims of the present invention and the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate to facilitate the embodiments of the present invention described herein. In addition, the terms "including," "having," and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to such processes, methods, products, or apparatuses.

[0062] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0063] Reference Manual Figure 1 , which shows a flow chart of a map construction method based on laser vision dynamic weighted fusion provided by an embodiment of the present invention.

[0064] Reference Manual Figure 2 , which shows a structural diagram of a map construction method based on laser vision dynamic weighted fusion provided by an embodiment of the present invention.

[0065] The embodiment of the present invention provides a map construction method based on laser vision dynamic weighted fusion, which can be mainly applied to large-scale sheep farms. The map construction method includes:

[0066] S1: Obtain RGB images and depth images through visual sensors, and obtain point cloud data through lidar.

[0067] Optionally, the visual sensor may be an RGB camera and a depth camera.

[0068] S2: Align the depth image with the RGB image and remove invalid depth pixels.

[0069] Specifically, camera calibration is used to obtain the internal and external parameters of the RGB and depth cameras. These parameters are then used to map each pixel in the depth image to a pixel in the RGB image to ensure spatial consistency between the two. Invalid depth pixel removal is achieved by checking whether the pixel values ​​in the depth image are valid (for example, zero or invalid pixels), and whether there is noise or depth values ​​outside the measurement range. These invalid or abnormal depth values ​​are removed to improve data quality.

[0070] S3: Based on the ORB-SLAM2 algorithm, perform visual SLAM, extract features of depth images and RGB images, and output sparse visual point cloud data.

[0071] Among them, ORB-SLAM2 is a vision-based SLAM (Simultaneous Localization and Mapping) algorithm that uses ORB (Oriented FAST and Rotated BRIEF) feature extraction and matching technology to efficiently construct sparse three-dimensional maps and perform positioning in dynamic environments. It calculates the relative motion of the camera by extracting and matching feature points in the image, and updates the camera's pose in real time. ORB-SLAM2 supports monocular, stereo, and RGB-D camera inputs, can operate stably for long periods of time in large-scale environments, and has closed-loop detection and map optimization capabilities. The traditional ORB-SLAM2 algorithm mainly relies on visual information for pose estimation. When there are insufficient feature points in the environment or when rapid motion occurs, purely visual SLAM is prone to problems such as pose drift and mismatching.

[0072] In a possible implementation, S3 specifically includes sub-steps S301 to S308:

[0073] S301: Use the ORB algorithm to extract feature points from the RGB image, and search the depth image for depth information corresponding to the feature points determined based on the SLAM algorithm.

[0074] S302: Determine the depth confidence of each data point based on the depth information:

[0075]

[0076] Among them, C i represents the depth confidence of the i-th data point, exp represents the exponential function with a natural constant as the base, d i Represents the depth information of the i-th data point, μ d represents the depth mean, σ d Indicates the depth standard deviation.

[0077] In this embodiment of the present invention, depth confidence is calculated based on the difference between the depth value and the depth mean. Data points with larger differences have lower confidence, while data points with smaller differences have higher confidence. This can suppress noise or outliers in the depth data, especially when depth measurements are inaccurate or interfered with. By assigning lower weights to these unreliable data points, the overall data quality and stability are improved, thereby improving the accuracy and robustness of subsequent image processing, pose estimation, and map construction.

[0078] S303: Normalize the depth confidence of each data point to obtain a depth confidence weight of each data point.

[0079] S304: Use the nearest neighbor search algorithm to match the feature points of the current frame with those of the previous frame.

[0080] The nearest neighbor search algorithm is used to find the points in a dataset that are most similar or closest to a given query point. It calculates the distance between the query point and each point in the dataset and returns the closest point or points. Common distance metrics include Euclidean distance and Manhattan distance. The nearest neighbor search algorithm is a well-established state of the art and will not be further described in this article.

[0081] S305: Using the matched feature points, the PnP algorithm is used to estimate the camera pose.

[0082] The Perspective-n-Point (PnP) algorithm is a technique for calculating camera pose. Given multiple feature points in an image and their corresponding 3D coordinates, the PnP algorithm can estimate the camera's position and orientation. By solving the camera's projection matrix, the PnP algorithm provides accurate camera pose estimation for visual SLAM systems. It is often combined with robust estimation methods such as RANSAC to improve stability and accuracy in noisy data. The PnP algorithm is a well-established state-of-the-art technology and will not be further elaborated in this article.

[0083] S306: An inertial measurement unit (IMU) is configured in the visual sensor, and a relative motion estimate of the visual sensor is calculated by pre-integrating the angular velocity and acceleration of the IMU between two consecutive image frames.

[0084] S307: Based on the relative motion estimation of the visual sensor and the camera pose estimation, local BA optimization is used to optimize the camera pose and data point positions. In the optimization process, depth confidence weights and IMU residual terms are introduced to jointly optimize the vision and IMU residuals, suppress pure vision pose drift, and perform pose correction on the point cloud data of the visual sensor.

[0085] The joint optimization objective function of local BA optimization is specifically:

[0086]

[0087]

[0088]

[0089]

[0090]

[0091] Where J represents the joint optimization objective function, λ vision Represents the weight coefficient of the visual reprojection error term, ε vision represents the visual reprojection error term, β i represents the depth confidence weight of the i-th data point, e ik represents the visual reprojection error of the i-th data point in the k-th frame image, z ik Indicates the position of the i-th data point in the k-th frame image, T k Indicates the camera pose when acquiring the kth frame image, X i represents the three-dimensional position of the i-th data point in the world coordinate system, h represents the reprojection calculation function, h(T k ,X i ) represents the reprojected position of the i-th data point in the k-th frame image, λIMU Represents the weight coefficient of the IMU residual term, ε IMU Represents the IMU residual term, e IMU,k Indicates the IMU residual when acquiring the k-th frame image, Indicates the pose predicted by the IMU when acquiring the k-th frame image.

[0092] In an embodiment of the present invention, by introducing depth confidence weights and IMU residual terms in the local BA optimization and jointly optimizing the visual and IMU residuals, the pose drift problem common in traditional visual SLAM can be effectively suppressed. The depth confidence weight can give more reliable depth data a greater optimization weight, thereby improving the accuracy of the pose and data point position. The IMU information provides additional motion constraints to help maintain the stability of the system in dynamic environments or low-texture areas, and reduce the error accumulation that may occur in pure visual SLAM. Through this joint optimization method, the complementary advantages of vision and IMU data are fully utilized, thereby improving the overall accuracy, robustness and positioning accuracy of the system, especially in the case of rapid motion or large environmental changes.

[0093] S308: Use the lightweight YOLOv5 network to segment dynamic objects in real time, remove feature points in the dynamic area, retain the static background, and obtain sparse visual point cloud data.

[0094] YOLOv5 is a deep learning-based object detection model, part of the YOLO (You Only Look Once) family of algorithms. It can detect and classify multiple objects in an image in real time and return the bounding box location of each object. YOLOv5 boasts high speed and accuracy, is capable of processing high-resolution images, and is widely used in fields such as autonomous driving, video surveillance, and robotic perception. Its lightweight and optimized design enables efficient operation in embedded systems and real-time applications. YOLOv5 is a highly mature existing technology and will not be elaborated upon in this article.

[0095] In dynamic environments, dynamic objects (such as pedestrians and vehicles) often lead to mismatches or inaccurate map updates, as their movement can interfere with the stability and consistency of feature points. By segmenting dynamic objects and removing their feature points in real time using YOLOv5, the SLAM algorithm can be prevented from interfering with these dynamic objects, ensuring that only feature points from the static background are used for pose estimation and map construction, thereby improving map stability and accuracy.

[0096] S4: Perform lidar SLAM based on the Gmapping algorithm and output a 2D occupancy grid map.

[0097] Gmapping is an algorithm for SLAM mapping based on LiDAR data. It uses particle filtering technology to estimate the robot's position in the environment and gradually construct a 2D occupancy grid map. The Gmapping algorithm processes LiDAR scan data to identify and model static obstacles in the environment, supporting real-time map generation and updates. Its high efficiency and stability make it widely used in mobile robots and autonomous driving systems, particularly for mapping tasks in two-dimensional environments. Traditional Gmapping algorithms assume that all obstacles in the environment are static and therefore cannot effectively distinguish between dynamic and static objects. In environments with moving objects (such as pedestrians, vehicles, and animals), these dynamic objects are incorrectly included in the map, resulting in inaccurate map updates and affecting subsequent path planning and navigation.

[0098] In a possible implementation, S4 specifically includes sub-steps S401 to S407:

[0099] S401: Estimate the robot's position on the map through particle filtering based on the point cloud data obtained by the lidar.

[0100] S402: Use a motion model to describe how the robot moves in space and update the position of the particles based on the control input.

[0101] S403: Differentiating point cloud data of adjacent frames to distinguish static obstacles from dynamic objects, and removing point cloud data in areas where dynamic objects are located.

[0102] In this embodiment of the present invention, by differentiating point cloud data from adjacent frames to distinguish between static obstacles and dynamic objects, and removing point cloud data in areas containing dynamic objects, only static obstacles are included in the final map, thus avoiding errors and inconsistencies caused by dynamic objects and ensuring map reliability, especially in complex or dynamic environments. This process can effectively reduce mismatches and incorrect mapping, improve positioning and path planning accuracy, and ultimately enhance the overall performance and robustness of the system.

[0103] S404: Using a scan matching algorithm, calculate the matching degree between the position of each particle and the current laser scan.

[0104] S405: During the particle filtering process, the weight of the particle is updated according to the result of each laser matching.

[0105] S406: Resampling is performed based on the weights of the particles to generate a new particle set.

[0106] S407: Generate a 2D occupancy grid map based on the particle set.

[0107] In an embodiment of the present invention, scan matching and particle weight updating further optimize pose estimation and map accuracy, ensuring that the robot can generate accurate 2D occupancy grid maps in real time and efficiently in complex environments, thereby improving the accuracy of navigation and path planning and enhancing the robustness of the system.

[0108] S5: Project the sparse visual point cloud data into a 2D occupancy grid map and calculate the semantic occupancy probability of each grid.

[0109] In a possible implementation, S5 specifically includes sub-steps S501 to S505:

[0110] S501: Projecting the sparse visual point cloud data into a 2D occupancy grid map.

[0111] It should be noted that by projecting sparse visual point cloud data into a 2D occupancy grid map, the three-dimensional visual information can be converted into a two-dimensional form suitable for map representation, so that the visual data can be effectively combined with the map data obtained by the lidar.

[0112] S502: Detect the semantic category of each data point in real time through a lightweight YOLOv5 network to determine the semantic label of each data point.

[0113] Optionally, the semantic tags include: sheep shed, sheep flock, obstacle, and ground.

[0114] S503: Count the semantic labels of the data points in each grid.

[0115] It's important to note that by counting the semantic labels of the data points within each grid, we can aggregate the semantic information within the region and further analyze its characteristics. This step helps us accurately understand the composition of each grid, identify and distinguish different object categories in the region, and thus improve the accuracy of environmental perception and the semantic level of the map.

[0116] S504: Calculate the weighted probability of each semantic tag appearing based on the depth confidence weight of each data point:

[0117]

[0118] Among them, p uvj represents the weighted probability of the jth semantic label in the grid (u, v), β uvi represents the depth confidence weight of the i-th data point in the grid (u, v), x uvij The statistical parameter indicating whether the i-th data point in the grid (u, v) belongs to the j-th semantic label. uvij = 1, it means that the i-th data point in the grid (u, v) belongs to the j-th semantic label.uvij = 0, it means that the i-th data point in the grid (u, v) does not belong to the j-th semantic label, N uv Represents the total number of data points within the grid (u,v).

[0119] It should be noted that by incorporating the depth confidence weights of data points to calculate the weighted probability of semantic label occurrence, we can increase the contribution of data points with high-confidence depth information to map generation while reducing the interference of unreliable data. This ensures that the final semantic occupancy probability is more accurate and effectively improves map quality, especially in dynamic or noisy environments.

[0120] S505: Based on the Poisson distribution model and the weighted probability of occurrence of each semantic label, the semantic occupancy probability of each grid is calculated:

[0121]

[0122] Among them, Ps uv represents the semantic occupancy probability of grid (u,v), ω j represents the category weight of the jth semantic label, λ represents the semantic density coefficient of the Poisson distribution, and m represents the total number of categories of semantic labels.

[0123] In this embodiment of the present invention, semantic occupancy probabilities are calculated based on a Poisson distribution model. This allows for the reasonable quantification of the probability of occurrence of different semantic labels within a grid. This allows each grid in the map to not only reflect whether an object occupies that area, but also assign different weights to different categories of objects. This method enables the system to generate more detailed and reliable semantic information, helping to improve robots' environmental understanding and decision-making capabilities, especially in complex or changing environments.

[0124] S6: Calculate the geometric occupancy probability of each grid through the inverse sensor model.

[0125] In a possible implementation, S6 specifically includes sub-steps S601 to S602:

[0126] S601: When the laser radar scans a grid, the logarithmic probability is updated according to whether the end point of the laser beam hits an obstacle.

[0127] When the end point of the laser beam hits an obstacle, add the logarithmic probability:

[0128]

[0129] Among them, l uv Represents the logarithmic probability of the grid (u, v), log represents the logarithmic function, p hit Indicates the probability of hitting, 0.5 <p hit <1.

[0130] When the end point of the laser beam misses an obstacle, reduce the logarithmic probability:

[0131]

[0132] Among them, p free represents the idle probability, 0 <p free <0.5.

[0133] It's important to note that by updating the logarithmic probability of each grid cell based on the LiDAR scan results, the occupancy status of that grid cell can be dynamically adjusted based on the actual measurement (whether or not the laser beam hit an obstacle). If the laser beam hits an obstacle, the logarithmic probability increases, indicating that the area is more likely to be occupied. If it misses, the logarithmic probability decreases, indicating that the area is likely unoccupied. This method, by gradually adjusting the logarithmic probability, allows the map to accurately reflect environmental changes in real time, especially by providing smooth transitions between different scan results, thereby improving map reliability and accuracy.

[0134] S602: Based on the Sigmoid function, the logarithmic probability of each grid is converted into geometric occupancy probability:

[0135]

[0136] Among them, Pa uv Represents the geometric occupancy probability of the grid (u,v).

[0137] It's important to note that converting log-probability to geometric occupancy probability using the sigmoid function converts the log-probability value (whether positive or negative) into a probability value between 0 and 1, making the occupancy status of each grid more intuitive and easier to understand. This conversion helps generate a smoother occupancy probability map, allowing the system to clearly determine which areas are occupied by obstacles and which areas are vacant.

[0138] S7: Perform weighted fusion of the semantic occupancy probability and the geometric occupancy probability to calculate the fused occupancy probability.

[0139] In a possible implementation, S7 specifically includes sub-steps S701 to S703:

[0140] S701: Assume that the semantic occupancy probability and the geometric occupancy probability are conditionally independent under a given grid state.

[0141] S702: Setting a priori probability according to the site environment.

[0142] Among them, those skilled in the art can set the size of the prior probability according to the actual site environment, and the present invention does not limit it.

[0143] S703: Perform weighted fusion of the semantic occupancy probability and the geometric occupancy probability through Bayesian fusion to calculate the fused occupancy probability:

[0144]

[0145] Among them, P uv represents the fusion occupancy probability of the grid (u, v), and P0 represents the prior probability.

[0146] Among them, Bayesian fusion is a probabilistic method based on Bayes' theorem, which is used to combine information from different sources to generate more accurate and consistent results.

[0147] In an embodiment of the present invention, by performing a Bayesian weighted fusion of the semantic occupancy probability and the geometric occupancy probability, the different information sources provided by vision and lidar can be effectively combined to generate a more accurate and comprehensive environmental map. Assuming that the semantic occupancy probability and the geometric occupancy probability are conditionally independent under a given grid state can simplify the fusion process and reduce computational complexity. On this basis, the prior probability is set according to the environmental conditions to further improve the system's adaptability to different scenarios. By combining the occupancy probabilities of the two, the Bayesian fusion method retains the details in the visual information (such as object type and scene recognition) and incorporates the geometric structure information provided by the lidar, thereby optimizing the judgment of the occupancy state and enhancing the accuracy and robustness of the map. This method can provide more reliable positioning, navigation, and decision support in dynamic and complex environments.

[0148] S8: Generate a fusion map based on the fusion occupancy probability.

[0149] In one possible implementation, S8 specifically includes determining whether the fused occupancy probability of each grid is greater than a preset occupancy probability. If so, the grid is set to an occupied state. Otherwise, the grid is set to an idle state to form a fused map.

[0150] Among them, those skilled in the art can set the size of the preset occupancy probability according to actual conditions, and the present invention does not limit it.

[0151] In this embodiment of the present invention, by generating a fused map based on the fused occupancy probability, the true state of the environment can be more accurately reflected. By setting a preset occupancy probability threshold, the system can intelligently determine whether each grid is occupied, thereby effectively distinguishing between obstacles and free areas.

[0152] In a possible implementation, the map construction method based on laser vision dynamic weighted fusion further includes:

[0153] S9: When an environment change is detected, local map reconstruction is triggered.

[0154] In this embodiment of the present invention, by triggering local map reconstruction when the environment changes, it is possible to ensure that the map always reflects the latest state of the current environment. In a dynamic environment, changes in objects, the movement of obstacles, or the addition of new objects may cause the original map to become inaccurate. Local map reconstruction can effectively correct these changes, avoiding deviations or errors in the entire map. This local reconstruction approach reduces computational overhead by focusing only on the areas that have changed, thereby improving system efficiency and responsiveness.

[0155] From the above description, it can be seen that the map construction method based on dynamic weighted fusion of laser vision provided by the present invention can make up for the shortcomings of a single sensor and improve the accuracy and completeness of the environmental map by combining the data of vision and lidar sensors.

[0156] Reference Manual Figure 3 , which shows a structural diagram of a map construction system based on laser vision dynamic weighted fusion provided by an embodiment of the present invention.

[0157] The embodiment of the present invention provides a map construction system 20 based on laser vision dynamic weighted fusion, comprising:

[0158] Processor 201;

[0159] The memory 202 stores computer-readable instructions, and when the computer-readable instructions are executed by the processor 201, the above-mentioned map construction method based on laser vision dynamic weighted fusion is implemented.

[0160] From the above description, it can be seen that the map construction method based on dynamic weighted fusion of laser vision provided by the present invention can make up for the shortcomings of a single sensor and improve the accuracy and completeness of the environmental map by combining the data of vision and lidar sensors.

[0161] Unless otherwise specifically stated, the relative arrangement of the parts and steps, the numerical expressions and the numerical values ​​set forth in these embodiments do not limit the scope of the present invention. At the same time, it should be understood that, for ease of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship. The techniques, methods and equipment known to those of ordinary skill in the relevant art may not be discussed in detail, but where appropriate, the techniques, methods and equipment should be considered as part of the authorization specification. In all examples shown and discussed here, any specific values ​​should be interpreted as being merely exemplary and not as limiting. Therefore, other examples of the exemplary embodiments may have different values. It should be noted that similar numbers and letters represent similar items in the following figures, and therefore, once an item is defined in one figure, it does not need to be further discussed in subsequent figures.

[0162] For ease of description, spatially relative terms such as "above," "above," "on the upper surface of," and "upper" may be used herein to describe the spatial positional relationship of a device or feature to other devices or features as shown in the figures. It should be understood that spatially relative terms are intended to encompass different orientations of the device in use or operation in addition to the orientation depicted in the figures. For example, if a device in a drawing is inverted, a device described as "above" or "on top of" another device or structure would then be positioned as "below" or "below" the other device or structure. Thus, the exemplary term "above" can include both the "above" and "below" orientations. The device may also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatially relative descriptions used herein should be interpreted accordingly.

[0163] In the description of the present invention, it should be understood that the directions or positional relationships indicated by directional words such as "front, back, up, down, left, right", "horizontal, vertical, perpendicular, horizontal" and "top, bottom" are usually based on the directions or positional relationships shown in the accompanying drawings. They are only for the convenience of describing the present invention and simplifying the description. Unless otherwise specified, these directional words do not indicate or imply that the device or element referred to must have a specific direction or be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the scope of protection of the present invention; the directional words "inside and outside" refer to the inside and outside relative to the outline of each component itself.

[0164] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. A map construction method based on laser vision dynamic weighted fusion, characterized in that: include: S1: Obtain RGB images and depth images through visual sensors, and obtain point cloud data through lidar; S2: Align the depth image with the RGB image and remove invalid depth pixels; S3: Based on the ORB-SLAM2 algorithm, perform visual SLAM, extract features of the aligned depth image and RGB image, and output sparse visual point cloud data; S4: Based on the Gmapping algorithm, perform lidar SLAM and output a 2D occupancy grid map; S5: Projecting the sparse visual point cloud data into the 2D occupancy grid map, and calculating the semantic occupancy probability of each grid; S6: Calculate the geometric occupancy probability of each grid through the inverse sensor model; S7: performing weighted fusion on the semantic occupancy probability and the geometric occupancy probability to calculate a fused occupancy probability; S8: Generate a fusion map according to the fusion occupancy probability.

2. The map construction method based on laser vision dynamic weighted fusion according to claim 1 is characterized in that: The S3 specifically includes: S301: Using the ORB algorithm, extracting feature points from the RGB image, and searching the depth image for depth information corresponding to the feature points determined based on the SLAM algorithm; S302: Determine the depth confidence of each data point based on the depth information; S303: Normalizing the depth confidence of each data point to obtain a depth confidence weight of each data point; S304: Using a nearest neighbor search algorithm, matching feature points of the current frame with those of the previous frame; S305: Using the matched feature points, the PnP algorithm is used to estimate the camera pose. S306: configuring an inertial measurement unit (IMU) in the visual sensor, and calculating a relative motion estimate of the visual sensor by pre-integrating the angular velocity and acceleration of the IMU between two consecutive image frames; S307: Based on the relative motion estimation of the visual sensor and the camera pose estimation, local BA optimization is used to optimize the camera pose and data point positions. The depth confidence weight and IMU residual term are introduced into the optimization process to jointly optimize the vision and IMU residuals, suppress pure vision pose drift, and perform pose correction on the point cloud data of the visual sensor. S308: Segment the dynamic objects in real time through a lightweight YOLOv5 network, remove feature points in the dynamic area, retain the static background, and output the sparse visual point cloud data.

3. The map construction method based on laser vision dynamic weighted fusion according to claim 1 is characterized in that: The S4 specifically includes: S401: Estimate the position of the robot on the map through particle filtering based on the point cloud data acquired by the laser radar; S402: Using a motion model to describe how the robot moves in space, and updating the position of the particle according to the control input; S403: Differentiating point cloud data of adjacent frames to distinguish static obstacles from dynamic objects, and removing point cloud data in areas where the dynamic objects are located; S404: using a scan matching algorithm to calculate the matching degree between the position of each particle and the current laser scan; S405: During the particle filtering process, the weight of the particle is updated according to the result of each laser matching; S406: Resampling based on the weights of the particles to generate a new particle set; S407: Output the 2D occupancy grid map according to the particle set.

4. The map construction method based on laser vision dynamic weighted fusion according to claim 1 is characterized in that: The S5 specifically includes: S501: Projecting the sparse visual point cloud data into the 2D occupancy grid map; S502: Detecting the semantic category of each data point in real time using a lightweight YOLOv5 network to determine a semantic label for each data point; S503: Counting the semantic labels of each data point in the grid; S504: Calculating the weighted probability of occurrence of each semantic tag based on the depth confidence weight of each data point; S505: Calculating the semantic occupancy probability of each grid according to the weighted probability of occurrence of each semantic tag based on a Poisson distribution model.

5. The map construction method based on laser vision dynamic weighted fusion according to claim 4 is characterized in that: The semantic labels include: sheep house, sheep flock, obstacle and ground.

6. The map construction method based on laser vision dynamic weighted fusion according to claim 1 is characterized in that: The S6 specifically includes: S601: When the laser radar scans a grid, the logarithmic probability is updated according to whether the end point of the laser beam hits an obstacle; S602: Based on the Sigmoid function, convert the logarithmic probability of each grid into the geometric occupancy probability.

7. The map construction method based on laser vision dynamic weighted fusion according to claim 1 is characterized in that: The S7 specifically includes: S701: Assume that the semantic occupancy probability and the geometric occupancy probability are conditionally independent under a given grid state; S702: Setting a priori probability according to the site environment; S703: Perform weighted fusion on the semantic occupancy probability and the geometric occupancy probability through Bayesian fusion to calculate the fused occupancy probability.

8. The map construction method based on laser vision dynamic weighted fusion according to claim 1 is characterized in that: The S8 is specifically: Determine whether the fused occupancy probability of each grid is greater than a preset occupancy probability; if so, set the grid to an occupied state; otherwise, set the grid to an idle state to generate the fused map.

9. The map construction method based on laser vision dynamic weighted fusion according to claim 1 is characterized in that: Also includes: S9: When an environment change is detected, local map reconstruction is triggered.

10. A map construction system based on laser vision dynamic weighted fusion, characterized in that: include: processor; A memory having computer-readable instructions stored thereon, wherein when the computer-readable instructions are executed by the processor, the map construction method based on laser vision dynamic weighted fusion as described in any one of claims 1 to 9 is implemented.

Images