Mobile robot simultaneous positioning and mapping method based on area array laser and depth camera

Abstract

The invention relates to the field of mobile robots, in particular to a mobile robot simultaneous positioning and mapping method based on area array laser and a depth camera, and the method comprises the following steps: 1, data collection and preprocessing; step 2, feature extraction; step 3, data fusion; 4, constructing a map; and step 5, positioning and map updating. According to the invention, through advantage complementation of the area array laser and the depth camera, the positioning and mapping precision is improved; object features extracted by the depth camera are used for constructing a semantic map, so that the robot can understand object meanings in the environment, and rich semantic information is provided. A dynamic object detection and tracking mechanism enables the robot to cope with changes in the environment in real time, and the adaptability of the dynamic environment is improved; and carrying out optimization design of links such as data acquisition, processing, fusion and map construction.

Description

Technical Field

[0001] The present invention relates to the field of mobile robots, and in particular to a method for simultaneous positioning and mapping of mobile robots based on area array lasers and depth cameras. Background Art

[0002] With the rapid development of science and technology, mobile robots have been widely used in many fields, such as logistics distribution, indoor cleaning, industrial inspection, etc.; in these application scenarios, mobile robots need to have accurate positioning and efficient environmental mapping capabilities to achieve complex operations such as autonomous navigation and task execution.

[0003] Traditional positioning and mapping methods have many limitations. Solutions based on a single sensor, such as relying solely on LiDAR, have a certain degree of accuracy in distance measurement, but are prone to measurement errors in complex indoor scenes, such as when there are a large number of reflective objects, narrow passages or dynamic obstacles, resulting in inaccurate positioning and incomplete mapping. When using depth cameras alone, their depth information is greatly affected by factors such as lighting and object material. In dim light or direct strong light, the reliability of depth data is greatly reduced, and it is also difficult to meet the needs of high-precision positioning and mapping.

[0004] In recent years, multi-sensor fusion technology has gradually become a research hotspot, but most of the existing fusion methods have failed to give full play to the advantages of area array lasers and depth cameras. There are problems such as untimely data processing, rough fusion algorithms, and inability to adapt to complex dynamic environments.

[0005] Therefore, those skilled in the art provide a method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera to solve the problems raised in the above background technology. Summary of the invention

[0006] In order to solve the above technical problems, the present invention provides a method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera.

[0007] The following steps are involved:

[0008] Step 1: Data acquisition and preprocessing: including an array laser sensor and a depth camera. The array laser sensor acquires two-dimensional laser point cloud data and performs noise filtering and sensitivity adjustment; the depth camera acquires depth images and performs illumination compensation and bilateral filtering;

[0009] Step 2: Feature extraction: extract geometric feature points from the two-dimensional laser point cloud data and extract object features from the depth image;

[0010] Step 3: Data fusion: Use the fusion algorithm based on extended Kalman filter to fuse the geometric feature points and object features, establish the state equation of the mobile robot, and use the covariance matrix to weigh the two sensor data;

[0011] Step 4: Map construction: construct a raster map and a semantic map. The raster map reflects the distribution of obstacles, and the semantic map contains the semantic information of objects.

[0012] Step 5: Positioning and map update: Use the iterative closest point algorithm combined with the extended Kalman filter for positioning, and update the grid map and semantic map according to environmental changes.

[0013] Preferably, during the acquisition process, the area array laser sensor uses a built-in noise filter to preliminarily remove noise points caused by laser reflection, scattering, etc., and at the same time uses a dynamic threshold adjustment algorithm to adjust the laser receiving sensitivity in real time according to factors such as ambient light intensity and target reflectivity, to ensure the acquisition of a clear and stable laser point cloud.

[0014] Preferably, the depth camera adopts an adaptive illumination compensation algorithm to automatically adjust the camera exposure time and gain according to parameters such as the average brightness and contrast of the image to improve the quality of the depth data, and performs bilateral filtering on the collected depth image to remove salt and pepper noise in the image and smooth the mutation of the depth value to obtain a preprocessed depth image.

[0015] Preferably, a curvature-based feature extraction algorithm is used from the preprocessed two-dimensional laser point cloud data to identify geometric feature points such as corners and edges in the environment, and the curvature value of each point in the point cloud is calculated, and the points with curvature greater than a set threshold are marked as feature points.

[0016] Preferably, for the preprocessed depth image, a deep learning-based target detection and segmentation algorithm is used to identify various objects in the image, such as furniture, doors and windows, and extract their following features: contour, texture and other features. The deep learning model is pre-trained on a large number of indoor scene image data sets.

[0017] Preferably, during the fusion process, the measurement noise characteristics of the two sensors are different, and the covariance matrix is ​​used to model the measurement noise. According to the statistical analysis of the actual measurement data, the parameters of the covariance matrix are adjusted in real time to ensure that the fusion algorithm can accurately weigh the data of the two sensors and improve the accuracy of positioning and mapping.

[0018] Preferably, the grid map divides the robot's motion space into a series of grids of equal size, and determines whether each grid is occupied by an obstacle based on the position information of each point in the fused data, thereby constructing a grid map reflecting the distribution of environmental obstacles. During the construction process, a dynamic grid update algorithm is adopted. When the robot moves and obtains new fused data, only the affected grids are updated, thereby improving the efficiency of map construction.

[0019] Preferably, the semantic map combines the object features extracted by the depth camera and the fused position information to semantically annotate objects in the environment and construct a map containing rich semantic information. For example, the detected sofa is annotated as a "sitting area" and the doors and windows are annotated as "traffic areas".

[0020] Preferably, before the robot is started, the array laser and the depth camera are accurately synchronized in time and calibrated in space. By adopting a high-precision clock synchronization module, it is ensured that the data collected by the two sensors strictly correspond in time. The intrinsic and extrinsic parameters of the camera are accurately calibrated using tools such as a checkerboard calibration board to ensure the spatial consistency of the measurement data of the two sensors and reduce measurement errors caused by sensor asynchrony and uncalibrated.

[0021] Preferably, during the data collection and processing, a dynamic object detection and tracking mechanism is set up, and a motion detection algorithm based on the optical flow method is used in combination with depth information to identify dynamic objects in the environment, such as pedestrians, moving furniture, etc. When constructing a map, the area where the dynamic objects are located is marked as a special state to avoid misjudging them as fixed obstacles. At the same time, during the positioning process, the impact of dynamic objects on the robot's motion is considered, and the robot's path planning is adjusted in time to improve the system's adaptability to dynamic environments.

[0022] Technical effects and advantages of the present invention:

[0023] 1. High-precision positioning and mapping: Through the complementary advantages of area array lasers and depth cameras, as well as sophisticated fusion algorithms, high-precision positioning and mapping can be achieved in complex indoor and outdoor environments. Whether there are reflective objects, large lighting changes, or narrow passages, the environment features can be accurately identified and accurate maps can be constructed, providing a reliable basis for the robot's autonomous navigation.

[0024] 2. Rich semantic information: Object features extracted by the depth camera are used to build semantic maps, enabling the robot to understand the meaning of objects in the environment and perform more intelligent tasks. For example, when cleaning indoors, it can automatically identify and avoid valuables, and when in logistics distribution, it can quickly find the target shelf.

[0025] 3. Adaptability to dynamic environments: The dynamic object detection and tracking mechanism enables the robot to respond to changes in the environment in real time, avoiding positioning errors and collision accidents caused by dynamic obstacles, and improving the robot's operating safety and reliability in dynamic environments such as crowded places and factory workshops.

[0026] 4. Efficient algorithm process: The optimized design of data collection, processing, fusion and map construction improves the overall operation efficiency of the system, reduces data processing time, enables the robot to respond to environmental changes more quickly, and improves the fluency of autonomous navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a schematic diagram of the overall process of a method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera provided in an embodiment of the present application;

[0028] Figure 2 It is a schematic diagram of the framework of data collection and preprocessing in the simultaneous positioning and mapping method of a mobile robot based on an array laser and a depth camera provided in an embodiment of the present application. DETAILED DESCRIPTION

[0029] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. The embodiments of the present invention are provided for the purpose of illustration and description, and are not intended to be exhaustive or to limit the present invention to the disclosed forms. Many modifications and variations will be apparent to those of ordinary skill in the art. The embodiments are selected and described in order to better illustrate the principles and practical applications of the present invention, and to enable those of ordinary skill in the art to understand the present invention and thereby design various embodiments with various modifications suitable for specific uses.

[0030] Example 1

[0031] See also Figures 1-2 In this embodiment, a method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera is provided, comprising the following steps:

[0032] Step 1: Data collection and preprocessing

[0033] Area array laser sensor: Use high-resolution area array laser to scan the robot's surroundings at a certain frequency to obtain two-dimensional laser point cloud data of the environment. During the acquisition process, the built-in noise filter is used to preliminarily remove noise points caused by laser reflection, scattering, etc. At the same time, the dynamic threshold adjustment algorithm is used to adjust the laser receiving sensitivity in real time according to factors such as ambient light intensity and target reflectivity to ensure the acquisition of clear and stable laser point clouds;

[0034] Depth camera: Run the depth camera synchronously to obtain the depth image of the environment; in view of the problem that the depth camera is easily affected by light, an adaptive light compensation algorithm is used to automatically adjust the camera exposure time and gain according to the average brightness, contrast and other parameters of the image to improve the quality of the depth data. Perform bilateral filtering on the collected depth image to remove the salt and pepper noise in the image and smooth the sudden change of the depth value to obtain the pre-processed depth image;

[0035] Step 2: Feature extraction

[0036] From the pre-processed 2D laser point cloud data, the curvature-based feature extraction algorithm is used to identify geometric feature points such as corners and edges in the environment. By calculating the curvature value of each point in the point cloud, the points with curvature greater than the set threshold are marked as feature points. These feature points usually correspond to prominent locations such as corners and object contours in the environment, providing key geometric information for subsequent positioning and mapping;

[0037] For the pre-processed depth images, we use the deep learning-based object detection and segmentation algorithm to identify various objects in the images, such as furniture, doors and windows, and extract their contours, textures and other features. The deep learning model is pre-trained on a large number of indoor scene image data sets and can accurately distinguish different objects. The extracted features will help build a map with more semantic information.

[0038] Step 3: Data Fusion

[0039] The extended Kalman filter (EKF)-based fusion algorithm is used to fuse the geometric feature points extracted by the array laser and the object features extracted by the depth camera. First, the state equation of the mobile robot is established, including state variables such as position and posture, as well as control input variables such as speed and angular velocity. The array laser measurement data is used as the main observation value, and the depth camera data is used as the auxiliary observation value. The EKF algorithm is used to estimate the state of the robot and continuously update the optimal value of the state variable.

[0040] In the fusion process, the covariance matrix is ​​used to model the measurement noise, taking into account the different measurement noise characteristics of the two sensors. According to the statistical analysis of the actual measurement data, the parameters of the covariance matrix are adjusted in real time to ensure that the fusion algorithm can accurately weigh the data of the two sensors and improve the accuracy of positioning and mapping.

[0041] Step 4: Map construction

[0042] Based on the fused data, two types of maps are constructed: raster maps and semantic maps;

[0043] Grid map: The robot's motion space is divided into a series of grids of equal size. Based on the position information of each point in the fused data, it is determined whether each grid is occupied by an obstacle, thereby constructing a grid map that reflects the distribution of environmental obstacles. During the construction process, a dynamic grid update algorithm is used. When the robot moves and obtains new fused data, only the affected grids are updated to improve the efficiency of map construction;

[0044] Semantic map: Combine the object features extracted by the depth camera with the fused position information to semantically annotate objects in the environment and build a map containing rich semantic information. For example, the detected sofa is annotated as a "sitting area" and the doors and windows are annotated as "traffic areas". The semantic map will provide more intuitive and useful information for the robot's autonomous navigation and task planning.

[0045] Step 5: Positioning and map update

[0046] Positioning: During the robot's movement, the fused data is used to continuously update the robot's position and posture estimation. The current position of the robot is determined by matching the currently collected data with the features in the constructed map and using an algorithm based on iterative closest point (ICP). At the same time, the EKF algorithm is used to optimize the position estimation and improve the accuracy and stability of positioning.

[0047] Map update: When the robot discovers a new environment area or the environment changes, such as new obstacles or object movement, the grid map and semantic map are updated in a timely manner; using the newly collected data, the corresponding grid and semantic annotations are modified according to the above map construction method to ensure that the map always reflects the latest environmental information.

[0048] Among them, in order to improve the real-time performance and adaptability of the system, the present invention also includes the following optimization measures:

[0049] Sensor synchronization and calibration: Before the robot starts, the array laser and depth camera are accurately synchronized in time and calibrated in space. A high-precision clock synchronization module is used to ensure that the data collected by the two sensors are strictly corresponding in time. The intrinsic and extrinsic parameters of the camera are accurately calibrated using tools such as the checkerboard calibration board to ensure the spatial consistency of the measurement data of the two sensors and reduce measurement errors caused by sensor asynchrony and uncalibrated.

[0050] Adaptation to dynamic environments: During data collection and processing, a dynamic object detection and tracking mechanism is set up. A motion detection algorithm based on optical flow is used in combination with depth information to identify dynamic objects in the environment, such as pedestrians and moving furniture. When building a map, the area where the dynamic objects are located is marked as a special state to avoid misjudging them as fixed obstacles. During the positioning process, the impact of dynamic objects on the robot's motion is considered, and the robot's path planning is adjusted in a timely manner to improve the system's ability to adapt to dynamic environments.

[0051] When using the present invention, the staff first performs sensor synchronization and calibration work during the robot initialization phase; places a checkerboard calibration plate at a certain distance in front of the robot, starts the area array laser and the depth camera, obtains the laser point cloud data and the depth image of the calibration plate respectively according to the calibration procedure, calculates the intrinsic and extrinsic parameters of the camera through a specific algorithm, and completes the spatial calibration; at the same time, uses a high-precision clock synchronization module to synchronize the clocks of the two sensors to ensure the consistency of data acquisition time.

[0052] In the data collection and preprocessing stage, the area array laser sensor scans the surrounding environment at a frequency of 20 times per second (adjustable according to actual needs) to obtain two-dimensional laser point cloud data. During a scan, for example, if a metal object with strong reflectivity is encountered, the noise filter judges and removes noise points based on parameters such as the reflected light intensity and the spot size; if the ambient light suddenly weakens, the dynamic threshold adjustment algorithm automatically increases the laser receiving sensitivity to ensure the clarity and stability of the point cloud data. The depth camera synchronously collects depth images. When the average brightness of the image is low, the adaptive illumination compensation algorithm will extend the camera exposure time, increase the gain, and improve the quality of the depth data. The collected depth image is processed by bilateral filtering to remove salt and pepper noise and obtain a usable depth image.

[0053] In the feature extraction stage, for the two-dimensional laser point cloud data, the curvature of each point is calculated. When the curvature is greater than 0.5 (empirical threshold, which can be optimized according to the actual scene), the point is marked as a feature point. The set of these feature points reflects the geometric features of the environment, such as the corners and edges of objects. The depth image enters the target detection and segmentation model based on deep learning. The model is trained based on a large number of indoor scene images such as the COCO dataset, and can accurately identify objects such as sofas and tables, and extract their contours, textures and other features.

[0054] Data fusion adopts an EKF-based algorithm, with the robot's initial position and posture as the initial values ​​of the state variables, the array laser measurement data as the main observation value, and the depth camera data as the auxiliary observation value; in the fusion process, according to the previous statistical analysis of the measurement noise of the two sensors, the initial parameters of the covariance matrix are set, and as the robot moves, new data is continuously collected, the covariance matrix is ​​adjusted in real time to optimize the fusion effect.

[0055] When constructing the map, we first construct a grid map, dividing the robot's motion space into grids with a side length of 0.1 meters (adjustable as needed), and then determine the state of each grid based on the fused data to construct an obstacle distribution map. On this basis, we combine the object features extracted by the depth camera to construct a semantic map, marking the identified sofa as a "sitting area" and doors and windows as "passage areas".

[0056] In the positioning and map updating phase, every time the robot collects new data during its movement, it uses the ICP algorithm to match it with the features in the constructed map to determine the current position, and combines it with the EKF algorithm to optimize the position estimate. When a new environment area is discovered, such as entering an unexplored room, the robot immediately updates the grid map and semantic map according to the above map construction method, incorporating new obstacle and object information into it.

[0057] During the entire operation process, the dynamic object detection and tracking mechanism runs continuously; if a pedestrian is detected moving in front of the robot, the optical flow method is combined with depth information to determine its movement direction and speed. When building the map, the area where the pedestrian is located is marked as a dynamic area. During positioning, the path planning is adjusted to avoid pedestrians, ensuring the safe and efficient operation of the robot.

[0058] The electrical components appearing in this article are all electrically connected to an external main controller and 220V AC power, and the main controller can be a conventional known device that controls a computer, etc. The specific implementation method of the present disclosure omits the detailed description of known functions and known components. To ensure the compatibility of the equipment, the operating methods used are consistent with the parameters of marketed equipment.

[0059] Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field and related fields without creative work should fall within the scope of protection of the present invention. The structures, devices and operating methods not specifically described and explained in the present invention are implemented according to the conventional means in the field unless otherwise specified and limited.

Claims

1. A method for simultaneous positioning and mapping of a mobile robot based on array laser and depth camera, characterized in that: The following steps are involved: Step 1: Data acquisition and preprocessing: including an array laser sensor and a depth camera. The array laser sensor acquires two-dimensional laser point cloud data and performs noise filtering and sensitivity adjustment; the depth camera acquires depth images and performs illumination compensation and bilateral filtering; Step 2: Feature extraction: extract geometric feature points from the two-dimensional laser point cloud data and extract object features from the depth image; Step 3: Data fusion: Use the fusion algorithm based on extended Kalman filter to fuse the geometric feature points and object features, establish the state equation of the mobile robot, and use the covariance matrix to weigh the two sensor data; Step 4: Map construction: construct a raster map and a semantic map. The raster map reflects the distribution of obstacles, and the semantic map contains the semantic information of objects. Step 5: Positioning and map update: Use the iterative closest point algorithm combined with the extended Kalman filter for positioning, and update the grid map and semantic map according to environmental changes.

2. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: During the acquisition process, the area array laser sensor uses a built-in noise filter to preliminarily remove noise points caused by laser reflection and scattering. At the same time, it uses a dynamic threshold adjustment algorithm to adjust the laser receiving sensitivity in real time according to ambient light intensity and target reflectivity factors to ensure the acquisition of a clear and stable laser point cloud.

3. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: The depth camera adopts an adaptive illumination compensation algorithm to automatically adjust the camera exposure time and gain according to the average brightness and contrast parameters of the image to improve the quality of the depth data, and performs bilateral filtering on the collected depth image to remove salt and pepper noise in the image and smooth the sudden change of the depth value to obtain a preprocessed depth image.

4. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: From the preprocessed two-dimensional laser point cloud data, a curvature-based feature extraction algorithm is used to identify corner points and edge geometric feature points in the environment. By calculating the curvature value of each point in the point cloud, the points with curvature greater than the set threshold are marked as feature points.

5. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 4, characterized in that: For the preprocessed depth images, the deep learning-based target detection and segmentation algorithm is used to identify various objects in the image and extract their features: contour and texture features. The deep learning model is pre-trained on a large number of indoor scene image data sets.

6. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: During the fusion process, the covariance matrix is ​​used to model the measurement noise, taking into account the different measurement noise characteristics of the two sensors. According to the statistical analysis of the actual measurement data, the parameters of the covariance matrix are adjusted in real time to ensure that the fusion algorithm can accurately weigh the data of the two sensors and improve the accuracy of positioning and mapping.

7. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: The grid map divides the robot's motion space into a series of grids of different sizes. Based on the position information of each point in the fused data, it is determined whether each grid is occupied by an obstacle, thereby constructing a grid map reflecting the distribution of environmental obstacles. During the construction process, a dynamic grid update algorithm is adopted. When the robot moves and obtains new fused data, only the affected grids are updated to improve the efficiency of map construction.

8. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: The semantic map combines the object features extracted by the depth camera and the fused location information to semantically annotate objects in the environment and construct a map containing rich semantic information.

9. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: Before the robot is started, the array laser and depth camera are accurately synchronized in time and calibrated in space. By adopting a high-precision clock synchronization module, the data collected by the two sensors are ensured to be strictly corresponding in time. The checkerboard calibration board tool is used to accurately calibrate the camera's internal and external parameters to ensure the spatial consistency of the two sensors' measurement data and reduce measurement errors caused by sensor asynchrony and uncalibrated.

10. The method for simultaneous positioning and mapping of a mobile robot based on an array laser and a depth camera according to claim 1, characterized in that: During the data collection and processing process, a dynamic object detection and tracking mechanism is set up. The motion detection algorithm based on the optical flow method is used in combination with depth information to identify dynamic objects in the environment. When building the map, the area where the dynamic objects are located is marked as a special state to avoid misjudging them as fixed obstacles. At the same time, during the positioning process, the impact of dynamic objects on the robot's motion is considered, and the robot's path planning is adjusted in time to improve the system's adaptability to dynamic environments.

Images