Mobile robot obstacle avoidance method based on multi-source perception data fusion

Abstract

The invention discloses a mobile robot obstacle avoidance method based on multi-source sensing data fusion, and the method comprises the steps: collecting robot sensing data through a laser radar, a camera and an IMU, and carrying out the preprocessing of the data; inputting the preprocessed data into the trained YOLO model to obtain two-dimensional bounding boxes, category labels and confidence coefficients of various obstacles; obtaining the three-dimensional position of the obstacle based on the two-dimensional bounding box, and obtaining an enhanced obstacle list based on the three-dimensional position; constructing a Nav2 dynamic cost map, and generating a robot global path based on the enhanced obstacle list; performing local path optimization on the global path of the robot to generate a local track of the robot; and driving the robot to move according to the local trajectory. According to the method, the limitation of a single sensor is overcome, and meanwhile, the reasonability and reliability of obstacle avoidance of the robot are improved by introducing semantic cost.

Description

Technical Field

[0001] This invention relates to the field of robot obstacle avoidance technology, and in particular to a method for obstacle avoidance of mobile robots based on multi-source perception data fusion. Background Technology

[0002] With the widespread application of mobile robots in complex and dynamic scenarios such as warehousing and logistics, service reception, and security patrol, extremely high demands are placed on their autonomous obstacle avoidance capabilities. Single sensors have inherent limitations, for example: LiDAR: It can accurately measure distances, but it cannot identify the specific type of obstacle (such as pedestrians, vehicles, glass doors), and its detection effect on objects with high reflectivity or light absorption is poor.

[0003] Cameras: They can provide rich texture and semantic information, but they are greatly affected by lighting and do not have rangefinding capabilities.

[0004] IMU: It can provide high-frequency information on the robot's own posture and acceleration, but it has cumulative drift error.

[0005] Furthermore, existing obstacle avoidance algorithms, such as the traditional dynamic window method, are usually based solely on geometric information and lack semantic understanding of the environment, which can easily lead to irrational or "panic" obstacle avoidance decisions by the robot. While Nav2, as a mature robot navigation framework, provides comprehensive navigation logic, its default perception layer is usually relatively simple.

[0006] Therefore, there is an urgent need for a real-time dynamic obstacle avoidance method that can integrate multi-source heterogeneous sensor data and inject advanced semantic information to achieve a more intelligent, human-like, and safer method. Summary of the Invention

[0007] The purpose of this invention is to provide a mobile robot obstacle avoidance method based on multi-source perception data fusion to solve the problems mentioned in the background art.

[0008] To achieve the above objectives, this invention provides a mobile robot obstacle avoidance method based on multi-source perception data fusion, comprising the following steps: S1. Collect robot perception data through lidar, camera and IMU, and preprocess the data; S2. Input the preprocessed data into the trained YOLO model to obtain the two-dimensional bounding boxes, category labels and confidence scores of various obstacles in the image; S3. Obtain the three-dimensional position of the obstacle based on the two-dimensional bounding box, and obtain an enhanced obstacle list based on the three-dimensional position; S4. Construct the Nav2 dynamic cost map and generate the robot's global path based on the enhanced obstacle list; S5. Optimize the local path of the robot's global path to generate the robot's local trajectory; S6. Convert the robot's local trajectory into linear velocity and angular velocity commands, and send the angular velocity commands to the robot's underlying motion controller to drive the robot to move.

[0009] Preferably, step S1 specifically includes: S11. Perform joint temporal and spatial calibration of the lidar, camera, and IMU to ensure that the data is aligned in time and coordinate system. S12. Acquire robot perception data collected by lidar, camera and IMU; S13. Using the high-frequency angular velocity and linear acceleration data collected by the IMU, motion compensation is performed on the point cloud distortion caused by the LiDAR's own motion during the scanning process to obtain a clean point cloud without distortion. S14. Perform distortion correction and white balance processing on the camera images; S15. Project the clean point cloud obtained in step S13 onto the image coordinate system through the camera-LiDAR extrinsic parameter matrix, so that each three-dimensional laser point corresponds to an image pixel.

[0010] Preferably, for each two-dimensional bounding box output by the YOLO model, all three-dimensional laser points falling within the box are found. Using the three-dimensional coordinates of these points, the precise three-dimensional position, size, and point cloud cluster of the obstacle in the robot coordinate system are calculated, resulting in an enhanced obstacle list containing category, 3D position, 3D size, confidence level, and point cloud cluster.

[0011] Preferably, the construction of the Nav2 dynamic cost map in step S4 includes: constructing a Nav2 local planner and a Nav2 global planner. The Nav2 global planner includes a static layer and a dynamic obstacle layer. The static layer directly generates a robot global path through a clean point cloud or laser SLAM. The dynamic obstacle layer optimizes the robot global path generated by the static layer based on an enhanced obstacle list.

[0012] Preferably, the dynamic obstacle layer optimizes the robot's global path generated by the static layer based on an enhanced obstacle list, including: The dynamic obstacle layer receives the enhanced obstacle list obtained in step S3 in real time. For static obstacles, its point cloud clusters are marked as high-cost regions; for dynamic obstacles, their current position is marked as a high-cost region, and the Kalman filter method is used to predict their short-term motion trajectory based on their historical position information. Regions on the predicted trajectory are marked as cost regions that decay over time, and different cost values ​​are set according to the object category.

[0013] Preferably, the cost of dynamic obstacle displacement decreases dynamically with time and motion state, and the update formula is: ; In the formula, Indicates the dynamic obstacle in time Location in time cost map The cost, Indicates the initial generation value. This is the attenuation coefficient.

[0014] Preferably, step S5 specifically includes: S51. Introduce semantic cost and construct a local trajectory optimization objective function. The formula is: ; In the formula, This represents the deviation between the trajectory and the global path. The weights represent the deviations between the trajectory and the global path. The distance cost to static obstacles, For the distance cost weight to static obstacles, The distance cost to dynamic obstacles, For distance cost weights with dynamic obstacles, As a cost to trajectory smoothness, For trajectory smoothness cost weights, For semantic cost weights, For semantic cost; The semantic cost formula is: ; In the formula, For the trajectory to the 1st The closest distance to each obstacle The preset safe distance for this type of obstacle. This represents the semantic weight of the category; The S52 and Nav2 local planners receive the robot's global path generated by the Nav2 global planner and perform local obstacle avoidance optimization based on the objective function to generate the robot's local trajectory.

[0015] Preferably, step S5 specifically includes: S51. Introduce semantic cost and directional cost to construct a local trajectory optimization objective function. The formula is: + ; In the formula, This represents the deviation between the trajectory and the global path. The weights represent the deviations between the trajectory and the global path. The distance cost to static obstacles, For the distance cost weight to static obstacles, The distance cost to dynamic obstacles, For distance cost weights with dynamic obstacles, As a cost to trajectory smoothness, For trajectory smoothness cost weights, For semantic cost weights, For semantic cost, As the weight of directional cost, As a directional cost; The semantic cost formula is: ; In the formula, For the trajectory to the 1st The closest distance to each obstacle The semantic weight of this category, Preset safe distance for this type of obstacle; The formula for directional cost is: ; In the formula, For obstacles Visibility weight, The directional angle of the line connecting the robot and the obstacle. To control orientation sensitivity; The S52 and Nav2 local planners receive the robot's global path generated by the Nav2 global planner and perform local obstacle avoidance optimization based on the objective function to generate the robot's local trajectory.

[0016] Therefore, the present invention employs the above-mentioned obstacle avoidance method for mobile robots based on multi-source perception data fusion, which has the following beneficial effects: (1) By fusing robot perception data collected by lidar, camera and IMU, the limitations of a single sensor are overcome. Even if lidar detection fails (e.g. facing glass) or camera vision degrades (e.g. sudden change in illumination), the system can still provide reliable information through another sensor, which greatly enhances the robustness of the system. (2) By incorporating semantic information into the cost map and planner, the robot is able to distinguish between “dangerous” and “relatively safe” obstacles and make more human-like and scenario-compliant decisions, such as politely avoiding pedestrians instead of braking suddenly.

[0017] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0018] Figure 1 The method flow of this invention embodiment Figure 1 ; Figure 2 The method flow of this invention embodiment Figure 2 . Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can be arranged and designed in various different configurations, and therefore should not be construed as limiting the present invention.

[0020] Example Reference Figures 1-2 This invention provides a method for obstacle avoidance in mobile robots based on multi-source perception data fusion, comprising the following steps: S1. Collect robot perception data using LiDAR, cameras, and IMU, and preprocess the data. Specific steps include: S11. Perform joint temporal and spatial calibration of the lidar, camera, and IMU to ensure that the data is aligned in time and coordinate system.

[0021] S12. Acquire robot perception data collected by LiDAR, camera and IMU.

[0022] S13. Using the high-frequency angular velocity and linear acceleration data collected by the IMU, motion compensation is performed on the point cloud distortion caused by the LiDAR's own motion during the scanning process to obtain a clean point cloud without distortion.

[0023] S14. Perform distortion correction and white balance processing on the camera images to provide high-quality input for YOLO detection.

[0024] S15. Project the clean point cloud obtained in step S13 onto the image coordinate system through the camera-LiDAR extrinsic parameter matrix, so that each three-dimensional laser point corresponds to an image pixel.

[0025] The camera-LiDAR extrinsic parameter matrix is ​​a 4×4 transformation matrix used to describe the rigid body transformation from the LiDAR coordinate system to the camera coordinate system, including rotation and translation components. This matrix is ​​a key parameter in sensor fusion, ensuring that point cloud data is aligned with image pixels.

[0026] S2. Input the preprocessed data into the trained YOLOv11 model to obtain the two-dimensional bounding boxes, category labels, and confidence scores of various obstacles in the image.

[0027] S3. Obtain the 3D position of the obstacle based on the 2D bounding box, and generate an enhanced obstacle list based on the 3D position. The specific process includes: For each 2D bounding box output by the YOLOv11 model In the image coordinate system, through the camera-LiDAR extrinsic parameter matrix Using a 4×4 transformation matrix (including rotation R and translation t) and a camera intrinsic parameter matrix K, find all 3D laser points falling within the bounding box, and transform all laser points falling within the bounding box from the lidar coordinate system to the image coordinate system. Using the 3D coordinates of these points, calculate the precise 3D position, size, and point cloud cluster of the obstacle in the robot coordinate system, and obtain an enhanced obstacle list containing category, 3D position, 3D size, confidence level, and point cloud cluster.

[0028] The calculation of three-dimensional position includes: Let the set of laser points falling within the bounding box be . ,in, The coordinates of the obstacle are shown in the lidar coordinate system. The three-dimensional position of the obstacle (i.e., the coordinates of the bounding box center in the robot coordinate system) can be calculated from the centroid of the point cloud cluster, using the formula: .

[0029] Calculating three-dimensional dimensions includes: The three-dimensional dimensions (length, width, and height) of an obstacle can be calculated using the extreme differences of the point cloud cluster along the three axes, as shown in the formula: ; Point cloud cluster extraction: point cloud clusters That is, all items falling into the bounding box laser point set It can be used for obstacle representation and updating in subsequent cost maps.

[0030] S4. Construct the Nav2 dynamic cost map and generate the robot's global path based on the enhanced obstacle list.

[0031] The construction of the Nav2 dynamic cost map includes: building a Nav2 local planner and a Nav2 global planner. The Nav2 global planner includes a static layer and a dynamic obstacle layer. The static layer directly generates a global path for the robot using a clean point cloud or laser SLAM. The dynamic obstacle layer optimizes the global path generated by the static layer based on an enhanced obstacle list.

[0032] Specifically, the dynamic obstacle layer optimizes the global robot path generated by the static layer based on the enhanced obstacle list, including: The dynamic obstacle layer receives the enhanced obstacle list obtained in step S3 in real time. For static obstacles, its point cloud clusters are marked as high-cost regions; for dynamic obstacles, their current position is marked as a high-cost region, and the Kalman filter method is used to predict their short-term motion trajectory based on their historical position information. Regions on the predicted trajectory are marked as cost regions that decay over time, with different cost values ​​assigned based on object category. For example, the cost of "pedestrian" is higher than that of "wheelbarrow," while "grass" or "carpet" regions can be set as low-cost traversable zones.

[0033] The cost of dynamic obstacle displacement decays dynamically with time and motion state, and the update formula is: ; In the formula, Indicates the dynamic obstacle in time Location in time cost map The cost, Indicates the initial generation value. This is the attenuation coefficient, which can be adaptively adjusted according to factors such as obstacle type and movement speed.

[0034] ; In the formula, The base attenuation rate is set according to the obstacle type; for example, it can be set to 0.5 for pedestrians and 0.8 for strollers. The current velocity of the obstacle is represented by Kalman filtering or the position difference between consecutive frames; For reference speed, take 1.0 m / s; The maximum obstacle speed allowed by the system; This is the velocity influence factor, used to adjust the contribution of velocity to decay, and is usually taken as 0.1 to 0.3.

[0035] This design allows fast-moving obstacles to decay at a faster rate, consistent with their movement characteristics of leaving the original area in a short time; while slow or static obstacles decay more slowly, maintaining a high-cost zone for a longer period to ensure safety.

[0036] Meanwhile, the Nav2 dynamic cost map periodically cleans up areas with costs below a threshold every Δt to ensure the map's real-time performance and consistency.

[0037] S5. Optimize the robot's global path to generate a local trajectory. The specific process includes: S51. In the objective function for robot trajectory optimization, in addition to the original cost terms such as path length, distance to static obstacles, and velocity smoothness, a semantic cost term is added, with the formula as follows: ; In the formula, This represents the deviation between the trajectory and the global path. The weights represent the deviations between the trajectory and the global path. The distance cost to static obstacles, For the distance cost weight to static obstacles, The distance cost to dynamic obstacles, For distance cost weights with dynamic obstacles, As a cost to trajectory smoothness, For trajectory smoothness cost weights, For semantic cost weights, For semantic cost; The semantic cost formula is: ; In the formula, For the trajectory to the 1st The closest distance to each obstacle The semantic weight of this category, Preset safe distance for this type of obstacle.

[0038] Specifically, different safe distances are preset for different types of obstacles and defined as d_safe_class. For example, d_safe_person > d_safe_cart. When the distance between the trajectory and the obstacle is less than d_safe_class, the semantic penalty cost increases sharply.

[0039] The S52 and Nav2 local planners receive the robot's global path generated by the Nav2 global planner and perform local obstacle avoidance optimization based on the objective function to generate the robot's local trajectory.

[0040] S6. Convert the robot's local trajectory into linear velocity and angular velocity commands, and send the angular velocity commands to the robot's underlying motion controller to drive the robot to move. After the robot moves, the new sensor data re-enters the process, forming a real-time closed loop of "perception-fusion-planning-execution".

[0041] Example 2 Unlike Example 1, this example introduces both semantic and directional costs when optimizing the local path of the robot's global path. The objective function is: + ; In the formula, This represents the deviation between the trajectory and the global path. The weights represent the deviations between the trajectory and the global path. The distance cost to static obstacles, For the distance cost weight to static obstacles, The distance cost to dynamic obstacles, For distance cost weights with dynamic obstacles, As a cost to trajectory smoothness, For trajectory smoothness cost weights, For semantic cost weights, For semantic cost, As the weight of directional cost, As a directional cost; The semantic cost formula is: ; In the formula, For the trajectory to the 1st The closest distance to each obstacle The semantic weight of this category, Preset safe distance for this type of obstacle; The formula for directional cost is: ; In the formula, For obstacles Visibility weight, The directional angle of the line connecting the robot and the obstacle. To control orientation sensitivity.

[0042] In this embodiment, a directional cost is introduced to encourage the robot to detour around the pedestrian from the side and rear. For example, when approaching a pedestrian, the robot is encouraged to detour from the side and rear of the pedestrian's field of vision, rather than cutting in from directly in front.

[0043] Therefore, the present invention adopts the above-mentioned obstacle avoidance method for mobile robots based on multi-source perception data fusion, which overcomes the limitations of a single sensor. At the same time, by introducing semantic costs, the rationality and reliability of robot obstacle avoidance are improved.

[0044] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A method for obstacle avoidance in mobile robots based on multi-source perception data fusion, characterized in that, Including the following steps: S1. Collect robot perception data through lidar, camera and IMU, and preprocess the data; S2. Input the preprocessed data into the trained YOLO model to obtain the two-dimensional bounding boxes, category labels and confidence scores of various obstacles in the image; S3. Obtain the three-dimensional position of the obstacle based on the two-dimensional bounding box, and obtain an enhanced obstacle list based on the three-dimensional position; S4. Construct the Nav2 dynamic cost map and generate the robot's global path based on the enhanced obstacle list; S5. Optimize the local path of the robot's global path to generate the robot's local trajectory; S6. Convert the robot's local trajectory into linear velocity and angular velocity commands, and send the angular velocity commands to the robot's underlying motion controller to drive the robot to move.

2. The obstacle avoidance method for mobile robots based on multi-source perception data fusion according to claim 1, characterized in that, Step S1 specifically includes: S11. Perform joint temporal and spatial calibration of the lidar, camera, and IMU to ensure that the data is aligned in time and coordinate system. S12. Acquire robot perception data collected by lidar, camera and IMU; S13. Using the high-frequency angular velocity and linear acceleration data collected by the IMU, motion compensation is performed on the point cloud distortion caused by the LiDAR's own motion during the scanning process to obtain a clean point cloud without distortion. S14. Perform distortion correction and white balance processing on the camera images; S15. Project the clean point cloud obtained in step S13 onto the image coordinate system through the camera-LiDAR extrinsic parameter matrix, so that each three-dimensional laser point corresponds to an image pixel.

3. The obstacle avoidance method for mobile robots based on multi-source perception data fusion according to claim 1, characterized in that, Step S3 specifically includes: for each two-dimensional bounding box output by the YOLO model, find all three-dimensional laser points that fall within the box, and use the three-dimensional coordinates of these points to calculate the three-dimensional position, size, and point cloud cluster of the obstacle in the robot coordinate system, thereby obtaining an enhanced obstacle list that includes category, 3D position, 3D size, confidence level, and point cloud cluster.

4. The obstacle avoidance method for mobile robots based on multi-source perception data fusion according to claim 3, characterized in that: Step S4, constructing the Nav2 dynamic cost map, includes: constructing a Nav2 local planner and a Nav2 global planner. The Nav2 global planner includes a static layer and a dynamic obstacle layer. The static layer directly generates a robot global path using a clean point cloud or laser SLAM. The dynamic obstacle layer optimizes the robot global path generated by the static layer based on an enhanced obstacle list.

5. The obstacle avoidance method for a mobile robot based on multi-source perception data fusion according to claim 4, characterized in that, The dynamic obstacle layer optimizes the global robot path generated by the static layer based on an enhanced obstacle list, including: The dynamic obstacle layer receives the enhanced obstacle list obtained in step S3 in real time. For static obstacles, its point cloud clusters are marked as high-cost regions; for dynamic obstacles, their current position is marked as a high-cost region, and the Kalman filter method is used to predict their short-term motion trajectory based on their historical position information. Regions on the predicted trajectory are marked as cost regions that decay over time, and different cost values ​​are set according to the object category.

6. The obstacle avoidance method for mobile robots based on multi-source perception data fusion according to claim 5, characterized in that: The cost of dynamic obstacle displacement decays dynamically with time and motion state, and the update formula is: ； In the formula, Indicates the dynamic obstacle in time Location in time cost map The cost, Indicates the initial generation value. This is the attenuation coefficient.

7. The obstacle avoidance method for a mobile robot based on multi-source perception data fusion according to claim 6, characterized in that, Step S5 specifically includes: S51. Introduce semantic cost and construct a local trajectory optimization objective function. The formula is: ； In the formula, This represents the deviation between the trajectory and the global path. The weights represent the deviations between the trajectory and the global path. The distance cost to static obstacles, As a weight for the distance cost to static obstacles, The distance cost to dynamic obstacles, For the distance cost weight with dynamic obstacles, As a cost to trajectory smoothness, For trajectory smoothness cost weights, For semantic cost weights, For semantic cost; The semantic cost formula is: ； In the formula, For the trajectory to the 1st The closest distance to each obstacle The preset safe distance for this type of obstacle. This represents the semantic weight of the category; The S52 and Nav2 local planners receive the robot's global path generated by the Nav2 global planner and perform local obstacle avoidance optimization based on the objective function to generate the robot's local trajectory.

8. The obstacle avoidance method for a mobile robot based on multi-source perception data fusion according to claim 6, characterized in that, Step S5 specifically includes: S51. Introduce semantic cost and directional cost to construct a local trajectory optimization objective function. The formula is: + ； In the formula, This represents the deviation between the trajectory and the global path. The weights represent the deviations between the trajectory and the global path. The distance cost to static obstacles, As a weight for the distance cost to static obstacles, The distance cost to dynamic obstacles, For the distance cost weight with dynamic obstacles, As a cost to trajectory smoothness, For trajectory smoothness cost weights, For semantic cost weights, For semantic cost, As the weight of directional cost, As a directional cost; The semantic cost formula is: ； In the formula, For the trajectory to the 1st The closest distance to each obstacle The semantic weight of this category. Preset safe distance for this type of obstacle; The formula for directional cost is: ； In the formula, For obstacles Visibility weight, The directional angle of the line connecting the robot and the obstacle. To control orientation sensitivity; The S52 and Nav2 local planners receive the robot's global path generated by the Nav2 global planner and perform local obstacle avoidance optimization based on the objective function to generate the robot's local trajectory.

Images