Mobile robot navigation method based on multi-modal fusion environment perception

Abstract

The application belongs to the technical field of mobile robots, and relates to a mobile robot navigation method based on multi-modal fusion environment perception, which comprises the following steps: firstly, a two-dimensional laser radar arranged on a robot chassis is used to complete the pre-construction of a static global map of an environment; then, a depth image collected by a depth camera arranged on the robot chassis and an upper computer is subjected to denoising pretreatment, and after being converted into a three-dimensional point cloud, the depth image is subjected to operations such as cutting, down-sampling, filtering and obstacle segmentation, and the point cloud after the operation is converted into two-dimensional pseudo-laser radar data consistent with the data format of the laser radar; finally, a multi-layer cost map is constructed based on the laser radar perception data, the static global map and the two-dimensional pseudo-laser radar data, and then fused, and the existing navigation function package of ROS is used to complete the fused navigation. The application can effectively avoid various irregular three-dimensional obstacles, expand the application scene of the mobile robot, and has engineering application value and low-cost platform deployment practicability.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot technology and relates to a mobile robot navigation method based on multimodal fusion environmental perception. Background Technology

[0002] Most mainstream indoor service mobile robots currently rely on two-dimensional LiDAR for environmental perception, which has significant blind spots and makes it difficult to fully identify irregular three-dimensional obstacles. Because these robots carry their payloads on their chassis, their overall height is relatively high, and a single-plane perception method cannot effectively avoid unstructured obstacles. For example, when a robot passes through an area with a table, the two-dimensional LiDAR can only identify the table legs and cannot perceive obstacles at the tabletop height, easily leading to collisions between the payload and the tabletop when the robot passes under the table.

[0003] Given the stringent cost and computing power requirements of service robots, 3D LiDAR is difficult to implement in engineering applications. While depth cameras can acquire spatial 3D information at low cost, their accuracy and stability are poor, and the acquired data cannot be directly used for obstacle perception. At the same time, depth camera data and LiDAR data have significant heterogeneity in coordinate system definition, data dimensions, and formats, making it difficult to directly achieve multi-source information fusion. Furthermore, its data format is incompatible with the ROS navigation stack, making it unsuitable for direct map building and navigation planning. Summary of the Invention

[0004] To address the aforementioned technical problems in the existing technology, this invention proposes a mobile robot navigation method based on multimodal fusion environmental perception, the specific technical solution of which is as follows: A mobile robot navigation method based on multimodal fusion environmental perception includes: Step 1: Pre-build a static global map of the environment using a 2D LiDAR deployed on the chassis of the service mobile robot; Step 2: Perform denoising preprocessing on the depth images acquired by the depth cameras located at the front of the robot and deployed on its chassis and host computer respectively. After converting them into 3D point clouds, perform cropping, downsampling, filtering, and obstacle segmentation operations. Then, convert the processed point cloud into 2D pseudo-LiDAR data with the same data format as the LiDAR. Step 3: Based on LiDAR perception data, static global map and 2D pseudo LiDAR data, construct a multi-layer cost map and fuse them to obtain a final map. Then, use the navigation function package of the robot operating system ROS to realize path planning and navigation.

[0005] Furthermore, the perspectives of the depth camera on the chassis and the depth camera on the host computer intersect.

[0006] Furthermore, in step 2, the depth image is denoised using median filtering: for each pixel in the image, a 5×5 neighborhood is selected, and the median value of the sorted pixels in the neighborhood is used to replace the value of the neighborhood center point.

[0007] Furthermore, in step 2, the pixel coordinates and corresponding depth values ​​of the depth image are used in conjunction with the camera pose parameters to calculate the three-dimensional coordinates of each pixel in the spatial coordinate system, thereby converting the depth image into a three-dimensional point cloud.

[0008] Furthermore, in step 2, the 3D point cloud is specifically clipped as follows: the 3D point cloud is height filtered to retain only the portion from 0 to H, where H is the sum of the robot height and the safety distance; the 3D point cloud is depth filtered to retain only the points within the depth range of 0.3 meters to 6 meters.

[0009] Further, in step 2, a voxel filter is used to downsample the 3D point cloud, and the voxel size is set so that only one mean point is retained in each cube region with a side length of voxel value; a noise filter is used to remove discrete point cloud noise, and the noise filter radius and minimum number of neighboring points are set. The specific judgment rule is: if a point in the point cloud contains fewer points in its neighborhood of the noise filter radius than the minimum number of neighboring points, then the point is judged as a noise point and removed.

[0010] Further, in step 2, the normal direction is obtained by sampling the point cloud within the normal radius of each point. All points whose normals are within 20° of the +Z axis are marked as ground, and other points are marked as candidate obstacles. Then, the point clouds of candidate obstacles are clustered, and points with more than 10 consecutive clustered points are identified as obstacles, i.e., obstacle point clouds are extracted. The extracted obstacle point clouds are then projected onto the horizontal plane of the 2D LiDAR installation height in the robot coordinate system. Based on the angular resolution and scanning range of the 2D LiDAR, the projected points are sampled, distances are calculated, and filtered to finally generate pseudo-LiDAR data consistent with the format of the real 2D LiDAR.

[0011] Furthermore, step 3 specifically includes: Step 3.1: Read the static global map pre-built by the 2D LiDAR and convert the grid status in the map into a value: if the map corresponding to the grid position is black, it means that the position is occupied, and a value of 254 is assigned; if the map corresponding to the grid position is white, it means that the position is idle, and a value of 0 is assigned. Step 3.2: Construct three obstacle layer cost maps based on the 2D LiDAR data, the pseudo LiDAR data obtained by processing the images acquired by the chassis depth camera in Step 2, and the pseudo LiDAR data obtained by processing the images acquired by the host computer depth camera in Step 2, respectively. When constructing each obstacle layer cost map, first initialize a 2D grid map with all zeros. Each time the 2D LiDAR scan data is updated, for each obstacle point in the point cloud, set the grid value of its location to 254, and then draw a line segment connecting the obstacle point and the LiDAR location to obtain the LiDAR scan line. Use the Bresenham algorithm to calculate the grids that the scan line passes through and set them as idle with a cost value of 0. Step 3.3: Based on the four-layer cost map generated in Steps 3.1 and 3.2, perform map fusion and cost inflation operations: overlay the four-layer cost maps onto the same main map using the maximum value method; perform cost inflation operations on the main map, and after the inflation operation is completed, the final fused cost map is obtained. Step 3.4: Using the move_base navigation package in the robot operating system ROS, construct the global cost map and the local cost map respectively, and perform path planning using a hierarchical planning strategy.

[0012] Furthermore, in step 3.3, during the expansion operation, the grid is divided into the following four categories and assigned corresponding values ​​based on the distance of the robot to the nearest obstacle: Fatal: An obstacle exists within the grid, costing 254. Embedded class: The distance to the obstacle is less than the radius Ri of the robot's inscribed circle, with a cost of 253; Buffer type: The distance to the obstacle is between the robot's inscribed circle radius Ri and expansion radius Re, and the cost decays exponentially. Free class: The distance to the obstacle is greater than the expansion radius Re, and the cost value is 0.

[0013] Further, in step 3.4, the path planning includes: Global path planning uses the A* algorithm to generate the optimal global path for the robot from the starting point to the target point. Local path planning employs the TEB algorithm to correct the path in real time and ensure that the robot's trajectory meets its own motion constraints.

[0014] Compared with existing indoor mobile robot perception and navigation methods, this invention can effectively avoid various irregular three-dimensional obstacles, expand the application scenarios of mobile robots, and has both engineering application value and low-cost platform deployment practicality. Attached Figure Description

[0015] Figure 1This is a flowchart of a mobile robot navigation method based on multimodal fusion environmental perception according to an embodiment of the present invention. Figure 2 This is a schematic diagram of the sensor deployment of a mobile robot according to an embodiment of the present invention; Figure 3 These are comparison images of depth image filtering and denoising before and after, according to an embodiment of the present invention. Figure 4 This is an RGB image of the experimental scene in an embodiment of the present invention; Figure 5 yes Figure 4 A schematic diagram of a 3D point cloud obtained by direct image conversion; Figure 6 yes Figure 4 Schematic diagrams of 3D and 2D point clouds of obstacles in the image; Figure 7 This is a schematic diagram illustrating the assignment of expansion layer cost according to an embodiment of the present invention; Figure 8 This is a schematic diagram of the construction of a multi-layer cost map according to an embodiment of the present invention. Detailed Implementation

[0016] To make the objectives, technical solutions, and technical effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0017] like Figure 1 As shown in the figure, the mobile robot navigation method based on multimodal fusion environmental perception proposed in this embodiment of the invention is mainly applicable to service mobile robots in indoor scenarios, and the specific implementation includes the following steps: Step 1: Pre-build a static global map of the environment using a 2D LiDAR deployed on the chassis of the service mobile robot.

[0018] The service mobile robot consists of two parts: a chassis and a host computer. The chassis integrates a drive system, a power system, a motion control system, and major sensors, while the host computer is responsible for implementing core service functions and is equipped with a small number of auxiliary sensors.

[0019] Using a 2D LiDAR as the robot's primary sensor offers advantages such as long detection range and good stability. Two depth cameras, mounted separately on the chassis and the front of the host computer, are selected as auxiliary sensors specifically for detecting 3D obstacles along the robot's path. Specifically, for example... Figure 2As shown, the 2D LiDAR is mounted on the upper part of the chassis, specifically deployed between the other structures of the chassis and the chassis top plate. A lightweight support structure connects the chassis top plate to the lower main structure, minimizing the obstruction of the radar's sensing area by the support structure, especially in the robot's direction of travel. A depth camera with an upward-facing view is mounted on the lower front of the chassis, and a depth camera with a downward-facing view is mounted on the upper front of the host computer. The perspectives of the two depth cameras intersect to achieve full coverage sensing of the area directly in front of the robot.

[0020] Step 2: Perform denoising preprocessing on the depth images acquired by the depth cameras located at the front of the robot and deployed on its chassis and host computer respectively. After converting them into 3D point clouds, perform operations such as cropping, downsampling, filtering, and obstacle segmentation. Finally, convert the processed point cloud into 2D pseudo-LiDAR data consistent with the LiDAR data format. Specifically, this includes the following steps: Step 2.1: Depth image filtering and denoising.

[0021] Depth image noise often appears as salt-and-pepper noise at obstacle edges. Median filtering is used for noise reduction. For each pixel in the image, a 5×5 neighborhood is selected, and the median value of the sorted pixels within that neighborhood replaces the value of the neighborhood's center point. The depth images before and after denoising are shown below. Figure 3 As shown.

[0022] Step 2.2: Convert the depth image into a 3D point cloud.

[0023] By utilizing the pixel coordinates and corresponding depth values ​​of a depth image, combined with known camera pose parameters, the 3D coordinates of each pixel in the spatial coordinate system are calculated, thereby converting the depth image into a 3D point cloud and achieving unification with the coordinate system of LiDAR data. For example... Figure 4 Taking the scene shown as an example, the depth image acquired in this scene is converted into a 3D point cloud and visualized using RViz, as shown below. Figure 5 As shown.

[0024] Step 2.3: Cropping of the 3D point cloud.

[0025] Height filtering is applied to the 3D point cloud, retaining only the portion from 0 to H, where H is the robot height plus a safety distance, typically 1.5m. Depth filtering is applied to the 3D point cloud, retaining only points within the effective depth range of 0.3m to 6m. Points that are too close can be considered to be occluded by internal robot components, while points that are too far away have larger errors but will not affect navigation, and are therefore filtered out.

[0026] Step 2.4: Point cloud downsampling and filtering.

[0027] A voxel filter is used to downsample the 3D point cloud. The voxel size (voxel_value) is set to 0.02m, meaning that only one mean point is retained within each 0.02m cube region. This preserves obstacle spatial information while reducing the computational and storage requirements of subsequent processing steps. A noise filter is then used to remove noise from the discrete point cloud. The noise filter radius (noise_filter_radius) is set to 0.05m, and the minimum number of neighborhood points (noise_filter_min_neighbors) is 5. The specific decision rule is: if a point in the point cloud contains fewer than 5 points within its 0.05m radius neighborhood, then that point is identified as a noise point and removed.

[0028] Step 2.5: Obstacle point cloud extraction.

[0029] Because some ground surfaces are not uniformly flat and some obstacles are very low, point cloud height coordinate segmentation cannot effectively distinguish between ground and obstacles. A normal filtering method is used to segment the ground: below a certain height, the normal direction is obtained by sampling the point cloud within the normal radius (normal_radius) of each point. Here, the normal radius is 0.05m. All points where the angle between the normal and the +Z axis is within 20° are marked as ground, and other points are marked as candidate obstacles. Then, the candidate obstacle point clouds are clustered. Only when there are more than 10 consecutive points after clustering are they identified as obstacles, i.e., the obstacle point cloud is extracted. This further reduces the interference of unstable factors such as impulse noise. The 3D point cloud processed by steps 2.3 to 2.5 is as follows: Figure 6 The image shows a white point cloud with medium to small grain size.

[0030] Step 2.6: Data Dimension Transformation.

[0031] The extracted obstacle point cloud is converted into 2D pseudo-LiDAR data to ensure compatibility with the existing Robot Operating System (ROS) 2D navigation stack interface. The core of the conversion process is to project the 3D point cloud data onto the horizontal plane at the installation height of the 2D LiDAR in the robot coordinate system. Then, according to the "angular resolution + scanning range" rules of the 2D LiDAR, the projected points are sampled, distances are calculated, and filters are applied to generate pseudo-LiDAR data consistent with the format of a real 2D LiDAR. This process can be implemented through the ` / pointcloud_to_laserscan` node in ROS. The 2D pseudo-LiDAR data obtained after step 2.6 is shown below. Figure 6 The point cloud is shown as having a medium to large grain size.

[0032] Step 3: Information Fusion and Navigation Planning Based on Multi-Level Cost Maps. Cost maps of different levels are constructed based on LiDAR perception data, a static global map, and 2D pseudo-LiDAR data. These multi-level cost maps are then fused to obtain a final map. Path planning and navigation are then implemented using existing ROS navigation packages. This step utilizes a multi-level cost map strategy, employing a hierarchical architecture to achieve refined representation of environmental information. Each layer of the cost map represents an information source, and each grid cell stores a value between 0 and 255 to represent the robot's cost at that point.

[0033] Step 3 specifically includes the following steps: Step 3.1: Construct a static layer cost map.

[0034] The system reads a pre-built static global map from a 2D LiDAR sensor and assigns values ​​based on the grid status: if the map corresponding to a grid location is black, it indicates that the location is occupied, and a value of 254 is assigned; if the map corresponding to a grid location is white, it indicates that the location is idle, and a value of 0 is assigned. The static layer is only built during initialization and is not updated afterward.

[0035] Step 3.2: Construct cost maps for three obstacle layers.

[0036] Three obstacle layer cost maps are constructed based on two-dimensional LiDAR data, pseudo LiDAR data obtained by processing images acquired by the chassis depth camera in step 2, and pseudo LiDAR data obtained by processing images acquired by the host computer depth camera in step 2, respectively. The construction principles of the three are exactly the same.

[0037] Initialize a 2D grid map with all zeros. Each time the 2D radar scan data is updated, for each obstacle point in the point cloud, set the cost of the grid it belongs to to 254. Then draw a line segment connecting the obstacle point to the point where the lidar is located to obtain the radar scan line. Use the Bresenham algorithm to calculate the grids that the scan line passes through and set them as idle with a cost of 0.

[0038] Step 3.3: Construct the expansion layer and fusion cost map.

[0039] The expansion layer does not directly acquire environmental information. Instead, it creates a virtual safety boundary around the obstacle based on the four-layer cost map constructed in the previous steps to ensure that the robot maintains a certain safe distance from the obstacle during navigation.

[0040] like Figure 7As shown, when the expansion layer assigns a cost value to the grid, it can be divided into four cases based on the distance of the robot to the nearest obstacle: Fatal: There is an obstacle inside the grid, and a collision will definitely occur, so the grid is assigned a cost value of 254; Embedded: The distance to the obstacle is less than the robot's inscribed circle radius Ri, and a collision will definitely occur, so the grid is assigned a cost value of 253; Buffer: The distance to the obstacle is greater than the inscribed circle radius Ri and less than the expansion radius Re, and the robot may not collide with the obstacle, so the cost value is assigned according to a negative exponential decay to keep the robot as far away from the area as possible; Free: The distance to the obstacle is greater than the expansion radius Re, and nothing will prevent the robot from going to the area, so the cost value is 0.

[0041] , Where r is the distance from the point to the nearest obstacle grid, and weight is a custom-defined gradient of cost descent; the larger the value, the faster the cost decreases. Since the cost is discrete, the cost calculated by the negative exponential function must be rounded during the inflation process.

[0042] The method of obtaining the fusion cost map is as follows Figure 8 As shown, the four binarized independent maps constructed in steps 3.1 and 3.2 are overlaid onto the same main map using the maximum value method. The inflation layer does not construct a separate map; instead, it performs a cost inflation operation directly on the main map. After inflation is complete, the fused cost map is obtained.

[0043] Step 3.4: Navigation Planning. After the cost map is constructed, navigation is performed using the nav_core framework within the move_base package. The navigation process requires the construction of both a global cost map and a local cost map, and a hierarchical planning strategy is employed for path planning: the global path planning relies on the A* algorithm to generate the optimal global path from the starting point to the target point; the local path planning is implemented using the TEB algorithm to achieve real-time path correction and ensure that the robot's trajectory meets its own motion constraints.

[0044] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any way. Although the implementation process of the present invention has been described in detail above, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A mobile robot navigation method based on multimodal fusion environmental perception, characterized in that, include: Step 1: Pre-build a static global map of the environment using a 2D LiDAR deployed on the chassis of the service mobile robot; Step 2: Perform denoising preprocessing on the depth images acquired by the depth cameras located at the front of the robot and deployed on its chassis and host computer respectively. After converting them into 3D point clouds, perform cropping, downsampling, filtering, and obstacle segmentation operations. Then, convert the processed point cloud into 2D pseudo-LiDAR data with the same data format as the LiDAR. Step 3: Based on LiDAR perception data, static global map and 2D pseudo LiDAR data, construct a multi-layer cost map and fuse them to obtain a final map. Then, use the navigation function package of the robot operating system ROS to realize path planning and navigation.

2. The mobile robot navigation method as described in claim 1, characterized in that, The depth cameras on the chassis and the depth cameras on the host computer have intersecting perspectives.

3. The mobile robot navigation method as described in claim 1, characterized in that, In step 2, the depth image is denoised using median filtering: for each pixel in the image, a 5×5 neighborhood is selected, and the median value of the sorted pixels in the neighborhood is used to replace the value of the neighborhood center point.

4. The mobile robot navigation method as described in claim 1, characterized in that, In step 2, the pixel coordinates and corresponding depth values ​​of the depth image are used in conjunction with the camera pose parameters to calculate the three-dimensional coordinates of each pixel in the spatial coordinate system, thereby converting the depth image into a three-dimensional point cloud.

5. The mobile robot navigation method as described in claim 1, characterized in that, In step 2, the 3D point cloud is specifically clipped as follows: the 3D point cloud is height filtered, retaining only the portion from 0 to H, where H is the sum of the robot height and the safety distance; the 3D point cloud is depth filtered, retaining only points within the depth range of 0.3 meters to 6 meters.

6. The mobile robot navigation method as described in claim 1, characterized in that, In step 2, a voxel filter is used to downsample the 3D point cloud. The voxel size is set so that only one mean point is retained in each cube region with a side length equal to the voxel value. A noise filter is used to remove discrete point cloud noise. The noise filter radius and minimum number of neighboring points are set. The specific judgment rule is: if a point in the point cloud contains fewer points in its neighborhood within the noise filter radius than the minimum number of neighboring points, then the point is judged as a noise point and removed.

7. The mobile robot navigation method as described in claim 1, characterized in that, In step 2, the normal direction is obtained by sampling the point cloud within the normal radius of each point. All points whose normals are within 20° of the +Z axis are marked as ground, and other points are marked as candidate obstacles. Then, the point clouds of candidate obstacles are clustered, and points with more than 10 consecutive points after clustering are identified as obstacles, i.e., obstacle point clouds are extracted. The extracted obstacle point clouds are then projected onto the horizontal plane of the 2D LiDAR installation height in the robot coordinate system. Based on the angular resolution and scanning range of the 2D LiDAR, the projected points are sampled, distances are calculated, and filtered to finally generate pseudo-LiDAR data consistent with the format of the real 2D LiDAR.

8. The mobile robot navigation method as described in claim 1, characterized in that, Step 3 specifically includes: Step 3.1: Read the static global map pre-built by the 2D LiDAR and convert the grid status in the map into a value: if the map corresponding to the grid position is black, it means that the position is occupied, and a value of 254 is assigned; if the map corresponding to the grid position is white, it means that the position is idle, and a value of 0 is assigned. Step 3.2: Construct three obstacle layer cost maps based on the 2D LiDAR data, the pseudo LiDAR data obtained by processing the images acquired by the chassis depth camera in Step 2, and the pseudo LiDAR data obtained by processing the images acquired by the host computer depth camera in Step 2, respectively. When constructing each obstacle layer cost map, first initialize a 2D grid map with all zeros. Each time the 2D LiDAR scan data is updated, for each obstacle point in the point cloud, set the grid value of its location to 254, and then draw a line segment connecting the obstacle point and the LiDAR location to obtain the LiDAR scan line. Use the Bresenham algorithm to calculate the grids that the scan line passes through and set them as idle with a cost value of 0. Step 3.3: Based on the four-layer cost map generated in Steps 3.1 and 3.2, perform map fusion and cost inflation operations: overlay the four-layer cost maps onto the same main map using the maximum value method; perform cost inflation operations on the main map, and after the inflation operation is completed, the final fused cost map is obtained. Step 3.4: Using the move_base navigation package in the robot operating system ROS, construct the global cost map and the local cost map respectively, and perform path planning using a hierarchical planning strategy.

9. The mobile robot navigation method as described in claim 8, characterized in that, In step 3.3, during the expansion operation, the grid is divided into the following four categories and assigned corresponding values ​​based on the distance of the robot to the nearest obstacle: Fatal: An obstacle exists within the grid, costing 254. Embedded class: The distance to the obstacle is less than the radius Ri of the robot's inscribed circle, with a cost of 253; Buffer type: The distance to the obstacle is between the robot's inscribed circle radius Ri and expansion radius Re, and the cost decays exponentially. Free class: The distance to the obstacle is greater than the expansion radius Re, and the cost value is 0.

10. The mobile robot navigation method as described in claim 8, characterized in that, In step 3.4, the path planning includes: Global path planning uses the A* algorithm to generate the optimal global path for the robot from the starting point to the target point. Local path planning employs the TEB algorithm to correct the path in real time and ensure that the robot's trajectory meets its own motion constraints.

Images