Autonomous navigation and obstacle avoidance method for delivery robot based on multi-sensor fusion

Abstract

The present application relates to the technical field of autonomous mobile robots, and particularly relates to a delivery robot autonomous navigation obstacle avoidance method based on multi-sensor fusion, which comprises the following steps: through data fusion of a depth camera and a laser radar, dynamic targets such as pedestrians and vehicles are identified, a filter algorithm is used to predict the future short-term motion trajectory of the dynamic targets, a risk corridor over time is generated, emergency stop or deadlock is avoided, and work efficiency and traffic smoothness are greatly improved; by using the characteristics that the laser radar is not sensitive to transparent objects and the ultrasonic wave is sensitive to the transparent objects, a space-time consistency cross-validation mechanism is established, false positives and false negatives are effectively eliminated, the robot can safely pass through a complex indoor environment, and the application boundary is greatly expanded; by fitting a ground plane through a depth camera and analyzing point cloud height changes in real time, obstacles that cannot be expressed by a two-dimensional navigation map can be identified; and an optimal trajectory is generated by minimizing a multi-target cost function, so that the path safety and smoothness are ensured.

Description

Technical Field

[0001] This invention relates to the field of autonomous mobile robot technology, and in particular to an autonomous navigation and obstacle avoidance method for cargo robots based on multi-sensor fusion. Background Technology

[0002] Current navigation systems for delivery robots suffer from significant drawbacks in complex real-world industrial and commercial environments due to their reliance on single or limited sensing technologies. These drawbacks severely restrict their reliability and large-scale application, including the following: Many current delivery robots rely on a single type of sensor for environmental perception, such as using only LiDAR or cameras. This results in limited perception capabilities when facing complex environments (such as changing light, transparent obstacles, and dynamic obstacles), leading to misjudgments or missed detections. Traditional methods often struggle to accurately predict the trajectory of dynamic obstacles (such as pedestrians and other mobile devices), resulting in delayed obstacle avoidance or unreasonable path planning. Transparent obstacles (such as glass doors and plastic films) are almost invisible to sensors like LiDAR, making them difficult to identify effectively with traditional methods and increasing the risk of collisions. In complex terrain, such as dangerous areas like pits, cliffs, or the edges of steps, traditional methods often fail to detect them accurately, increasing the risk of robots falling or getting stuck. Traditional two-dimensional grid maps primarily store obstacle location information and lack semantic information about dynamic obstacles, transparent obstacles, and dangerous areas on the road surface, limiting the flexibility and safety of path planning. To address this, the present invention proposes an autonomous navigation and obstacle avoidance method for cargo robots based on multi-sensor fusion. Summary of the Invention

[0003] The purpose of this invention is to solve the problems in the background art by proposing an autonomous navigation and obstacle avoidance method for cargo robots based on multi-sensor fusion.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] A multi-sensor fusion-based autonomous navigation and obstacle avoidance method for cargo delivery robots includes:

[0006] S1. Construct a hierarchical heterogeneous sensing module group: Configure a heterogeneous sensor array consisting of a depth camera, a single-line LiDAR, an ultrasonic ranging module, and a wheel electromagnetic encoder on the cargo robot body; collect multi-source heterogeneous sensing data through the heterogeneous sensor array;

[0007] S2. Preprocess the acquired multi-source heterogeneous sensing data and extract environmental features from the preprocessed data in parallel. The preprocessing includes time synchronization and spatial registration of the acquired multi-source heterogeneous sensing data, followed by filtering, noise reduction, and coordinate system unification. Environmental feature extraction includes extracting dynamic target contour features and ground 3D geometric features from depth camera data, extracting high-precision obstacle distance and contour features from single-line lidar data, and extracting near-field obstacle reflection features from ultrasonic data.

[0008] S3. Based on the extracted environmental features, collaborative perception and judgment are performed through feature complementarity and cross-validation mechanisms to generate a three-dimensional semantic map layer including dynamic obstacle prediction trajectory, transparent obstacle confidence area and road edge danger zone. The three-dimensional semantic map layer is then fused with the basic two-dimensional grid map constructed based on LiDAR point cloud to form an enhanced environmental semantic map.

[0009] S4. Based on the enhanced environmental semantic map, a local path planning method is used to generate a safe and smooth navigation trajectory for the delivery robot;

[0010] S5. The delivery robot moves along the navigation trajectory and uses real-time odometer data provided by the wheel electromagnetic encoder for trajectory tracking control, outputting motor control commands to drive the delivery robot to move.

[0011] S6. During the movement, steps S1 to S5 are executed repeatedly to achieve continuous perception of the surrounding environment, online updates of the enhanced environmental semantic map, and real-time replanning of the navigation path until the navigation task is completed.

[0012] Furthermore, in step S2, the process of extracting dynamic target contour features and ground 3D geometric features from depth camera data includes:

[0013] S21. For the RGB image output by the depth camera, a target detection neural network is used to perform real-time detection and bounding box localization on the RGB image output by the depth camera.

[0014] S22. Combine the synchronously acquired depth image, project the bounding box onto the 3D point cloud to obtain the 3D point cloud cluster of the dynamic target;

[0015] S23. Perform Euclidean clustering segmentation on the 3D point cloud cluster and calculate its centroid coordinates; by using the change of centroid coordinates between consecutive frames, use Kalman filtering or particle filtering algorithms to estimate the instantaneous velocity and direction of motion of the dynamic target.

[0016] S24. Perform ground plane segmentation on the 3D point cloud data acquired by the depth camera, and use the random sampling consensus algorithm to fit a ground reference plane that represents the mathematical model of the ground plane.

[0017] Furthermore, in step S3, the process of generating the predicted trajectory of the dynamic obstacle includes:

[0018] S31. The obstacle distance information detected by the single-line lidar in the current frame is fused and spatially matched and associated with the centroid coordinates of the dynamic target three-dimensional point cloud cluster calculated in step S23.

[0019] S32. Based on the historical sequence of the dynamic target centroid coordinates after successful data association, predict its future short-term trajectory.

[0020] S33. Encapsulate the predicted trajectory into a dynamic obstacle region with time attributes, and expand the space based on the physical size of the dynamic target with its predicted location point as the center to form a risk corridor that changes over time, and mark it in the enhanced environmental semantic map.

[0021] Furthermore, in step S3, the process of generating the transparent obstacle confidence region includes:

[0022] S34. Real-time monitoring of single-line lidar data. For lidar scanning beam, if no obstacle return point is detected in any detection direction, and that direction is within the passable area of ​​the delivery robot, then the spatial area corresponding to that direction is marked as a lidar sensing void area.

[0023] S35. Synchronously query the readings of the ultrasonic ranging modules arranged in the corresponding direction of the delivery robot, and obtain the measured distance value of the ultrasonic modules in the detection direction corresponding to the cavity area perceived by the lidar.

[0024] S36. Map the measurement points of the ultrasonic module to the coordinate system of the single-line lidar to obtain the mapped measurement points. If the mapped measurement points are located within the spatial range of the lidar's perceived cavity area, they are determined to be spatially consistent. Check the timestamp difference between the ultrasonic measurement data and the lidar's perceived cavity area data. If the timestamp difference is less than the preset time synchronization threshold, they are determined to be time-synchronized. When the lidar's perceived cavity area and the ultrasonic measurement data simultaneously meet the conditions of spatial consistency and time synchronization, and the ultrasonic measurement distance value is less than the first safe distance threshold, it is determined that there is a high-confidence transparent obstacle in the area, and the area is marked as a transparent obstacle confidence area in the enhanced environmental semantic map.

[0025] Furthermore, in step S3, the process of generating the road edge danger zone includes:

[0026] S37. Based on the fitted ground reference plane, analyze the point cloud data provided by the depth camera in real time and traverse the point cloud in the region of interest in front of the depth camera.

[0027] S38. Calculate the vertical distance of each point cloud to the ground reference plane. If the vertical distance of a point cloud exceeds the preset depression threshold, it is determined to be a pit danger area. If the point cloud in front has a cliff-like gap relative to the ground reference plane, it is determined to be a cliff or step edge danger area.

[0028] S39. Mark the identified pothole danger zones and cliff edge danger zones as impassable road edge danger zones in the enhanced environmental semantic map.

[0029] Furthermore, in step S3, the process of fusing the 3D semantic map layer with the basic 2D raster map constructed based on LiDAR point clouds to form an enhanced environmental semantic map includes:

[0030] S3A uses point cloud data from continuous scanning by a single-line LiDAR and employs real-time localization and map building algorithms to generate a two-dimensional grid map with the initial position of the delivery robot as the origin. Each grid cell in the map stores an occupancy probability value, indicating the possibility of an obstacle at that position.

[0031] S3B: The generated dynamic obstacle prediction trajectory, transparent obstacle confidence area, and road edge danger zone, etc., are uniformly projected into the same coordinate system of the two-dimensional raster map according to the coordinate transformation relationship determined in the spatial registration process of step S2.

[0032] S3C: For each grid cell in a two-dimensional grid map, update its occupancy probability or additional semantic attributes according to the semantic information type projected onto the grid cell area using different rules;

[0033] S3D ultimately produces an enhanced environmental semantic map that overlays dynamic, transparent, and hazardous area semantic information onto a basic two-dimensional raster map, which is then used for path planning.

[0034] Furthermore, in step S4, the local path planning method generates the optimal trajectory by minimizing a multi-objective cost function, where the multi-objective cost function is a weighted sum of multiple cost terms, including:

[0035] (1) The cost of deviation between the path and the global reference path;

[0036] (2) The minimum distance cost between the path and all transparent obstacle confidence zones or road edge danger zones;

[0037] (3) The cost of the spatiotemporal overlap of risk corridors generated by path and dynamic obstacle prediction;

[0038] (4) The smoothness cost of the path itself;

[0039] (5) The cost of kinematic feasibility of the delivery robot.

[0040] Compared with existing technologies, the beneficial effects of this invention are as follows: By configuring multiple heterogeneous sensors such as depth cameras and single-line LiDAR, multi-source heterogeneous data is collected and preprocessed to extract environmental features, enabling comprehensive and accurate perception of surrounding environmental information; based on the extracted environmental features, a three-dimensional semantic map layer is generated through collaborative perception and judgment, and then fused with a basic two-dimensional grid map to form an enhanced environmental semantic map, presenting key information such as dynamic obstacle prediction trajectories and transparent obstacle confidence regions, providing rich basis for path planning; in the path planning stage, the local path planning method generates the optimal trajectory by minimizing the multi-objective cost function, comprehensively analyzing multiple factors such as path deviation and distance from obstacles to ensure path safety and smoothness; this invention achieves closed-loop optimization of perception, decision-making, and control without excessively relying on expensive sensors, providing key technical support for the large-scale, high-reliability application of delivery robots in complex indoor and outdoor environments such as warehousing and logistics. Attached Figure Description

[0041] Figure 1 This is a flowchart of the autonomous navigation and obstacle avoidance method for cargo robots based on multi-sensor fusion proposed in this invention. Detailed Implementation

[0042] 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. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0043] Reference Figure 1 An autonomous navigation and obstacle avoidance method for cargo delivery robots based on multi-sensor fusion includes:

[0044] S1. Construct a hierarchical heterogeneous perception module group: Configure a heterogeneous sensor array on the cargo robot body, consisting of a depth camera, a single-line LiDAR, an ultrasonic ranging module, and a wheeled electromagnetic encoder. (The depth camera is installed at a height of 0.6 to 1.2 meters above the ground, with a pitch angle tilted downwards by 5 to 20 degrees to ensure that its field of view can effectively cover the forward travel area and the ground area; the single-line LiDAR is installed at a similar height to the depth camera, and its scanning plane is parallel to the ground or at a slight angle; the ultrasonic ranging module is arranged in a ring array around the robot chassis, with its detection direction slightly tilted downwards to cover the near-field blind spots of the LiDAR and depth camera.) The wheel-type electromagnetic encoder is directly embedded inside the servo motor of the drive wheel of the delivery robot, and is rigidly connected to the motor output shaft on the same axis, ensuring that the actual rotation angle and speed of the drive wheel can be accurately detected without gaps or slippage. Its signal is directly transmitted to the main control computing unit of the delivery robot through a dedicated high-speed interface, ensuring a continuous supply of high-frequency, low-latency odometer pulse data, providing accurate body kinematic feedback for the trajectory calculation of the delivery robot. Multi-source heterogeneous sensing data is collected through a heterogeneous sensor array, including depth image information of the robot's surrounding environment, two-dimensional laser point cloud information, ultrasonic ranging information, and odometer information of the delivery robot itself.

[0045] S2. Preprocess the collected multi-source heterogeneous sensing data and extract environmental features in parallel from the preprocessed data. Preprocessing includes time synchronization and spatial registration of the collected multi-source heterogeneous sensing data, followed by filtering, noise reduction, and coordinate system unification (time synchronization uses hardware trigger signals or software synchronization based on Network Time Protocol (NTP) to ensure that the timestamp deviation of each sensor's data is less than 10 milliseconds; spatial registration uses the transformation matrix of each sensor relative to the base coordinate system of the delivery robot, obtained through pre-calibration, to unify all multi-source heterogeneous sensing data to the base coordinate system of the delivery robot). Environmental feature extraction includes extracting dynamic target contour features and ground 3D geometric features from depth camera data, extracting high-precision obstacle distance and contour features from single-line lidar data, and extracting near-field obstacle reflection features from ultrasonic data.

[0046] S3. Based on the extracted environmental features, collaborative perception and judgment are performed through feature complementarity and cross-validation mechanisms to generate a three-dimensional semantic map layer including dynamic obstacle prediction trajectory, transparent obstacle confidence area and road edge danger zone. The three-dimensional semantic map layer is then fused with the basic two-dimensional grid map constructed based on LiDAR point cloud to form an enhanced environmental semantic map.

[0047] S4. Based on the enhanced environmental semantic map, a local path planning method is used to generate a safe and smooth navigation trajectory for the delivery robot;

[0048] S5. The delivery robot moves along the navigation trajectory and uses real-time odometer data provided by the wheel electromagnetic encoder for trajectory tracking control, outputting motor control commands to drive the delivery robot to move.

[0049] S6. During the movement, steps S1 to S5 are executed repeatedly to achieve continuous perception of the surrounding environment, online updates of the enhanced environmental semantic map, and real-time replanning of the navigation path until the navigation task is completed.

[0050] It should be further explained that, in the specific implementation process, step S2, the process of extracting dynamic target contour features and ground 3D geometric features from depth camera data, includes:

[0051] S21. For the RGB images output by the depth camera, a target detection neural network is used to perform real-time detection and bounding box localization of the RGB images (including pedestrians and other mobile devices) output by the depth camera. The neural network is a YOLO or SSD architecture.

[0052] S22. Combine the synchronously acquired depth image, project the bounding box onto the 3D point cloud to obtain the 3D point cloud cluster of the dynamic target;

[0053] S23. Perform Euclidean clustering segmentation on the 3D point cloud cluster and calculate its centroid coordinates; by using the change of centroid coordinates between consecutive frames, use Kalman filtering or particle filtering algorithms to estimate the instantaneous velocity and direction of motion of the dynamic target.

[0054] S24. The 3D point cloud data acquired by the depth camera is segmented into ground planes, and a ground reference plane representing the mathematical model of the ground plane is fitted using the Random Sampling Consensus (RANSAC) algorithm. It is understood that the purpose of segmenting the point cloud data into ground planes is to separate points belonging to the ground from non-ground points such as obstacles and dynamic targets, so as to provide a basis for subsequent obstacle identification and traversable area analysis. In this invention, the Random Sampling Consensus (RANSAC) algorithm is specifically used for plane fitting. The process includes: randomly selecting three points from the point cloud to calculate a plane model, calculating the distance from all points in the point cloud to the plane, determining points whose distance is less than a preset distance threshold as interior points (i.e., ground points), repeating this process iteratively, and finally selecting the plane model with the most interior points as the ground plane, thereby obtaining a mathematical equation representing the ground plane, i.e., the ground reference plane.

[0055] It should be further explained that, in the specific implementation process, the process of generating the dynamic obstacle prediction trajectory, the transparent obstacle confidence area, and the road edge danger zone in step S3 includes:

[0056] S31. The obstacle distance information detected by the single-line lidar in the current frame is fused and spatially matched and associated with the centroid coordinates of the dynamic target three-dimensional point cloud cluster calculated in step S23 to verify the existence of the dynamic target and improve its position accuracy.

[0057] S32. Based on the historical sequence of the centroid coordinates of the dynamic target after successful data association, predict its motion trajectory in the near future. The predicted trajectory covers a predefined prediction time window starting from the current moment, represented as a series of predicted position points arranged in chronological order. Understandably, the near future refers to a time window sufficient for the delivery robot to make a reasonable avoidance decision, such as 1 to 3 seconds. The prediction is based on the dynamic target's motion state (such as position and velocity). Specifically, a uniform motion model is used for prediction: assuming the dynamic target maintains its current instantaneous velocity and moves in a straight line within the prediction time window, its future position is calculated according to the uniform linear motion displacement formula, thus forming a trajectory. For more complex scenarios, an acceleration-based motion model or a machine learning prediction model based on historical trajectories is used.

[0058] S33. Encapsulate the predicted trajectory into a dynamic obstacle region with time attributes, and expand the space based on the physical size of the dynamic target with its predicted location point as the center to form a risk corridor that changes over time, and mark it in the enhanced environmental semantic map.

[0059] Understandably, the specific implementation of forming a risk corridor over time is as follows: S331, the system takes the current estimated position of a dynamic obstacle (such as a pedestrian) as the starting point and its predicted trajectory as the center line; the predicted trajectory consists of a series of discrete prediction points distributed at fixed time intervals, each prediction point... Corresponding to a future moment ,in For the index of the prediction points; S332, for each prediction point Define a risk area, which is based on Centered on, with The circular region with radius is defined by the formula: (for non-circular obstacles, it is defined by their circumcircle or by polygon expansion based on their outline). In the formula, The radius of the dynamic obstacle risk corridor. For the physical dimensions of the dynamic target (the obstacle itself) (e.g., simplifying a pedestrian to a circle with a diameter of 0.5 meters, then...) (0.25 meters) These are adjustable weighting coefficients. The instantaneous velocity of the dynamic obstacle. The trace represents the predicted covariance matrix based on historical trajectory fitting, used to quantify the uncertainty of the prediction; the faster the speed, the higher the uncertainty, the larger the radius of the risk area, and the larger the safety margin reserved for the delivery robot; S333, connect the risk areas of each prediction point on the continuous time series in chronological order to form a three-dimensional (two-dimensional space + one-dimensional time) risk corridor whose cross-section changes with time.

[0060] S34. Real-time monitoring of single-line lidar data. For lidar scanning beam, if no obstacle return point is detected in any detection direction, and that direction is within the passable area of ​​the delivery robot, then the spatial area corresponding to that direction is marked as a lidar sensing void area.

[0061] S35. Synchronously query the readings of the ultrasonic ranging modules arranged in the corresponding direction of the delivery robot, and obtain the measured distance value of the ultrasonic modules in the detection direction corresponding to the cavity area perceived by the lidar.

[0062] S36. Map the measurement points of the ultrasonic module to the coordinate system of the single-line lidar to obtain the mapped measurement points. If the mapped measurement points are located within the spatial range of the lidar's sensing cavity area, they are considered spatially consistent. Check the timestamp difference between the ultrasonic measurement data and the lidar's sensing cavity area data. If the timestamp difference is less than a preset time synchronization threshold, they are considered time-synchronized. When the lidar's sensing cavity area and the ultrasonic measurement data simultaneously meet the conditions of spatial consistency and time synchronization, and the ultrasonic measurement distance is less than the first safe distance threshold, it is determined that there is a high-confidence transparent obstacle in the area, and the area is marked as a transparent obstacle confidence area in the enhanced environmental semantic map. Optionally, read the RGB image data of the depth camera within the transparent obstacle confidence area, and analyze the texture features and edges of the image in the area. The feature or optical reflection characteristics are compared with a preset feature library of transparent objects (such as glass). If the matching degree is higher than the confidence threshold, the confidence of the determination that the area is a transparent obstacle is increased; if the matching degree is low, the confidence is lowered or a secondary confirmation mechanism is initiated. It should be noted that, from a dimensional perspective, the lidar perception hole area is a spatial area defined by the return point of a specific obstacle not detected during lidar detection. It is essentially a description of a spatial category and does not have physical dimensions in the traditional sense. On the other hand, the ultrasonic measurement data, as the output of the ultrasonic ranging module, is a distance value with length as the dimension, which directly represents the spatial interval between the obstacle and the delivery robot. Although there is a difference between the two at the dimensional level, in the task of detecting transparent obstacles, the two can be analyzed collaboratively by constructing a specific analysis framework.

[0063] S37. Based on the fitted ground reference plane, analyze the point cloud data provided by the depth camera in real time and traverse the point cloud in the region of interest in front of the depth camera.

[0064] S38. Calculate the vertical distance of each point cloud to the ground reference plane. If the vertical distance of a point cloud exceeds the preset depression threshold, it is determined to be a pit danger area. If the point cloud in front has a cliff-like gap relative to the ground reference plane, it is determined to be a cliff or step edge danger area.

[0065] S39. Mark the identified pothole danger zones and cliff edge danger zones as impassable road edge danger zones in the enhanced environmental semantic map.

[0066] It should be further explained that, in the specific implementation process, the process of fusing the 3D semantic map layer with the basic 2D raster map constructed based on LiDAR point clouds in step S3 to form an enhanced environmental semantic map includes:

[0067] S3A, based on point cloud data continuously scanned by a single-line LiDAR, uses a Simultaneous Localization and Mapping (SLAM) algorithm to generate a two-dimensional grid map with the initial position of the delivery robot as the origin. Each grid cell in this map stores an occupancy probability value, indicating the likelihood of an obstacle at that location. It is understood that SLAM is one of the core technologies in the field of autonomous mobile robots. Through probability estimation methods, it enables the robot to simultaneously infer its own trajectory (localization) and gradually build a spatial model of the environment (mapping) when moving in an unknown environment. In this invention, the purpose of applying the SLAM algorithm is to create an accurate two-dimensional grid map based on geometric information. This map will serve as the basic coordinate system for subsequent semantic information fusion. The specific application process is as follows: when the delivery robot starts, its initial position is used as the origin of the map coordinate system.

[0068] As the delivery robot moves, the real-time localization and mapping algorithm continuously receives odometry information from the wheeled electromagnetic encoder (used for preliminary estimation of the delivery robot's movement) and simultaneously acquires the environmental point cloud scanned by the single-line LiDAR. By comparing the current frame of LiDAR scan data with existing map features or the previous frame of scan data (e.g., using Iterative Closest Point (ICP) or its variants), the algorithm more accurately calculates the robot's pose changes (translation and rotation) relative to the environment, thereby correcting any cumulative errors that may be generated by the odometry.

[0069] Based on each step of localization estimation, the obstacle information currently detected by the laser scan is converted into a grid map. In the generated grid map, each grid cell stores an occupancy probability value (such as between 0 and 1), which represents the probability that the location is occupied by an obstacle. For example, the closer the value is to 1, the higher the certainty that the grid cell is an obstacle; the closer it is to 0, the higher the certainty that the area is free space; and about 0.5 indicates that the state is unknown.

[0070] When the delivery robot revisits an area it has previously visited, the real-time localization and mapping algorithm can complete scene reproduction and recognition, and significantly correct the cumulative error of the entire movement trajectory and map through location re-identification technology, thereby ensuring the global consistency of the map.

[0071] S3B: The generated dynamic obstacle prediction trajectory, transparent obstacle confidence area, and road edge danger zone, etc., are uniformly projected into the same coordinate system of the two-dimensional raster map according to the coordinate transformation relationship determined in the spatial registration process of step S2.

[0072] S3C: For each grid cell in the two-dimensional grid map, based on the semantic information type projected onto the grid cell area, different rules are used to update its occupancy probability or additional semantic attributes: When any grid cell is located within the risk corridor of the predicted trajectory of a dynamic obstacle, a dynamic obstacle label is assigned to the grid cell, and a high occupancy probability with timeliness is assigned, which decays over time.

[0073] If there is a grid in the two-dimensional grid map that is within the confidence region of a transparent obstacle, the occupancy probability of that grid is directly set to the maximum value and a transparent obstacle attribute label is attached. The obstacle information represented by this label takes precedence over the original observation value of the single-line lidar.

[0074] For grid cells located in dangerous zones at the edge of the road surface, set the occupancy probability of the grid cell to the maximum value and attach an impassable attribute label;

[0075] S3D ultimately produces an enhanced environmental semantic map that overlays dynamic, transparent, and hazardous area semantic information onto a basic two-dimensional raster map, which is then used for path planning.

[0076] It should be further explained that, in the specific implementation process, in step S4, the local path planning method generates the optimal trajectory by minimizing a multi-objective cost function, wherein the multi-objective cost function... The weighted sum of multiple cost terms, including:

[0077] (1) The cost of deviation between the path and the global reference path;

[0078] (2) The minimum distance cost between the path and all transparent obstacle confidence zones or road edge danger zones;

[0079] (3) The cost of the spatiotemporal overlap of risk corridors generated by path and dynamic obstacle prediction;

[0080] (4) The smoothness cost of the path itself;

[0081] (5) The kinematic feasibility cost of the delivery robot;

[0082] Multi-objective cost function for:

[0083] In the formula, This cost is the deviation between the local path and the global reference path. This cost ensures that the local planning does not deviate from the global strategic objective. Assume that the local path consists of N pose points. For the first Individual pose points Calculate the Euclidean distance from it to the nearest point on the global reference path. The total deviation cost is the sum of the squares of all pose point deviations divided by N, which is the average deviation: ;

[0084] The minimum distance cost between the path and the confidence zone of transparent obstacles or the danger zone at the edge of the road surface is given; the cost increases sharply when the distance is less than a safety threshold; for each pose point on the path... Calculate the minimum distance from the nearest transparent obstacle confidence zone or road edge danger zone. Define a safe distance ,when At that time, When it is 0, At that time, It increases exponentially with decreasing distance to ensure the robot resolutely stays away from danger: In the formula, k is a constant greater than 1 (e.g., k=2), used to control the sharpness of cost growth;

[0085] The cost of spatiotemporal overlap of risk corridors generated by path and dynamic obstacle prediction, where the overlap cost is calculated based on temporal overlap and spatial intrusion depth: for each pose point on the path There is an estimated arrival time. The system checks the time. Is the point located within the risk corridor of any dynamic obstacle? If the estimated time of the pose point... If a point is not within the effective timeframe of any risk corridor, then the spatiotemporal overlap cost of that point is: If the estimated time of the pose point Within a certain risk corridor's timeframe, calculate that point. Distance to the corresponding time point on the center line of the risk corridor (i.e., the predicted trajectory) The depth of penetration is If the value is negative, it is taken as 0. The cost of spatiotemporal overlap is then expressed as: In the formula, Let M be the radius of the dynamic obstacle risk corridor, M be the number of pose points on the path that overlap with the risk corridor in time, and m be the adjustment coefficient. This cost term The planned paths are encouraged to deviate from the future trajectories of dynamic obstacles in both time and space;

[0086] The smoothness of the path itself is at a cost, penalizing sharp turns to ensure ride comfort and control stability. This cost is typically measured by the curvature of the path point sequence or the rate of change of the steering angle; for example, calculating the change in the angle between two vectors formed by three consecutive path points. The smoothness cost is: ;

[0087] The kinematic feasibility cost for the delivery robot ensures that the generated path conforms to the delivery robot's kinematic constraints (such as minimum turning radius, maximum speed, or acceleration). For example, it checks whether the curvature between adjacent points on the path exceeds the maximum curvature that the delivery robot can achieve, and if it does, a large penalty term is applied. The weighting coefficients for each cost term can be used to balance different performance indicators such as path safety, efficiency, and smoothness, enabling the robot's behavior to adapt to different application scenarios.

[0088] Furthermore, the formulas mentioned above are all dimensionless calculations, derived from software simulation using a large amount of collected data to approximate the real situation. The weighting coefficients and preset thresholds in the formulas are set by those skilled in the art based on actual conditions or obtained through large-scale data simulation. The magnitude of the weighting coefficient is a specific value obtained by quantifying each parameter to facilitate subsequent comparison. The magnitude of the weighting coefficient depends on the amount of sample data and the processing coefficients initially set by those skilled in the art for each set of sample data. As long as it does not affect the proportional relationship between the parameter and the quantified value, it is acceptable.

[0089] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to interchangeably. The focus of each embodiment is on its differences from other embodiments. In particular, the apparatus embodiments are described simply because they are fundamentally based on the method embodiments; relevant details can be found in the descriptions of the method embodiments.

[0090] For ease of description, the above devices are described separately by function as various units. Of course, in implementing this application, the functions of each unit can be implemented in one or more software and / or hardware.

[0091] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0092] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0093] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0094] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0095] Secondly: The accompanying drawings of the embodiments disclosed in this invention only involve the structures involved in the embodiments disclosed in this invention. Other structures can refer to the general design. In the absence of conflict, the same embodiment and different embodiments of this invention can be combined with each other.

[0096] In conclusion, the above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for autonomous navigation and obstacle avoidance of a cargo delivery robot based on multi-sensor fusion, characterized in that: S1. Construct a hierarchical heterogeneous sensing module group: Configure a heterogeneous sensor array consisting of a depth camera, a single-line LiDAR, an ultrasonic ranging module, and a wheel electromagnetic encoder on the cargo robot body; collect multi-source heterogeneous sensing data through the heterogeneous sensor array; S2. Preprocess the acquired multi-source heterogeneous sensing data and extract environmental features from the preprocessed data in parallel. The preprocessing includes time synchronization and spatial registration of the acquired multi-source heterogeneous sensing data, followed by filtering, noise reduction, and coordinate system unification. Environmental feature extraction includes extracting dynamic target contour features and ground 3D geometric features from depth camera data, extracting high-precision obstacle distance and contour features from single-line lidar data, and extracting near-field obstacle reflection features from ultrasonic data. S3. Based on the extracted environmental features, collaborative perception and judgment are performed through feature complementarity and cross-validation mechanisms. Specifically, the motion state of dynamic targets is estimated based on the contour features of dynamic targets, and their short-term future motion trajectory is predicted. Transparent obstacles are identified by performing spatiotemporal consistency cross-validation between the perception data of single-line lidar and the perception data of ultrasonic ranging modules. Based on the three-dimensional geometric features of the ground, dangerous areas at the road edge are identified by analyzing the height change from the point cloud to the ground plane. A three-dimensional semantic map layer is generated, including the predicted trajectory of dynamic obstacles, the confidence area of ​​transparent obstacles, and the dangerous areas at the road edge. The three-dimensional semantic map layer is then fused with the basic two-dimensional grid map constructed based on lidar point clouds to form an enhanced environmental semantic map. S4. Based on the enhanced environmental semantic map, a local path planning method is used to generate a safe and smooth navigation trajectory for the delivery robot; wherein, the local path planning generates the optimal trajectory by minimizing a multi-objective cost function; S5. The delivery robot moves along the navigation trajectory and uses real-time odometer data provided by the wheel electromagnetic encoder for trajectory tracking control, outputting motor control commands to drive the delivery robot to move. S6. During the movement, steps S1 to S5 are executed repeatedly to achieve continuous perception of the surrounding environment, online updates of the enhanced environmental semantic map, and real-time replanning of the navigation path until the navigation task is completed.

2. The autonomous navigation and obstacle avoidance method for a cargo delivery robot based on multi-sensor fusion according to claim 1, characterized in that: In step S2, the process of extracting dynamic target contour features and ground 3D geometric features from depth camera data includes: S21. For the RGB image output by the depth camera, a target detection neural network is used to perform real-time detection and bounding box localization on the RGB image output by the depth camera. S22. Combine the synchronously acquired depth image, project the bounding box onto the 3D point cloud to obtain the 3D point cloud cluster of the dynamic target; S23. Perform Euclidean clustering segmentation on the 3D point cloud cluster and calculate its centroid coordinates; by using the change of centroid coordinates between consecutive frames, use Kalman filtering or particle filtering algorithms to estimate the instantaneous velocity and direction of motion of the dynamic target. S24. Perform ground plane segmentation on the 3D point cloud data acquired by the depth camera, and use the random sampling consensus algorithm to fit a ground reference plane that represents the mathematical model of the ground plane.

3. The autonomous navigation and obstacle avoidance method for a cargo delivery robot based on multi-sensor fusion according to claim 1 or 2, characterized in that: In step S3, the process of generating the predicted trajectory of dynamic obstacles includes: S31. The obstacle distance information detected by the single-line lidar in the current frame is fused and spatially matched and associated with the centroid coordinates of the dynamic target three-dimensional point cloud cluster calculated in step S23. S32. Based on the historical sequence of the dynamic target centroid coordinates after successful data association, predict its future short-term trajectory. S33. Encapsulate the predicted trajectory into a dynamic obstacle region with time attributes, and expand the space based on the physical size of the dynamic target with its predicted location point as the center to form a risk corridor that changes over time, and mark it in the enhanced environmental semantic map.

4. The autonomous navigation and obstacle avoidance method for a cargo delivery robot based on multi-sensor fusion according to claim 1, characterized in that: In step S3, the process of generating the confidence region for the transparent obstacle includes: S34. Real-time monitoring of single-line lidar data. For lidar scanning beam, if no obstacle return point is detected in any detection direction, and that direction is within the passable area of ​​the delivery robot, then the spatial area corresponding to that direction is marked as a lidar sensing void area. S35. Synchronously query the readings of the ultrasonic ranging modules arranged in the corresponding direction of the delivery robot, and obtain the measured distance value of the ultrasonic modules in the detection direction corresponding to the cavity area perceived by the lidar. S36. Map the measurement points of the ultrasonic module to the coordinate system of the single-line lidar to obtain the mapped measurement points. If the mapped measurement points are located within the spatial range of the lidar's perceived cavity area, they are determined to be spatially consistent. Check the timestamp difference between the ultrasonic measurement data and the lidar's perceived cavity area data. If the timestamp difference is less than the preset time synchronization threshold, they are determined to be time-synchronized. When the lidar's perceived cavity area and the ultrasonic measurement data simultaneously meet the conditions of spatial consistency and time synchronization, and the ultrasonic measurement distance value is less than the first safe distance threshold, it is determined that there is a high-confidence transparent obstacle in the area, and the area is marked as a transparent obstacle confidence area in the enhanced environmental semantic map.

5. The autonomous navigation and obstacle avoidance method for a cargo delivery robot based on multi-sensor fusion according to claim 1, characterized in that: In step S3, the process of generating a hazardous zone at the road edge includes: S37. Based on the fitted ground reference plane, analyze the point cloud data provided by the depth camera in real time and traverse the point cloud in the region of interest in front of the depth camera. S38. Calculate the vertical distance of each point cloud to the ground reference plane. If the vertical distance of a point cloud exceeds the preset depression threshold, it is determined to be a pit danger area. If the point cloud in front has a cliff-like gap relative to the ground reference plane, it is determined to be a cliff or step edge danger area. S39. Mark the identified pothole danger zones and cliff edge danger zones as impassable road edge danger zones in the enhanced environmental semantic map.

6. The autonomous navigation and obstacle avoidance method for a cargo delivery robot based on multi-sensor fusion according to claim 3, characterized in that: In step S3, the process of fusing the 3D semantic map layer with the basic 2D raster map constructed based on LiDAR point clouds to form an enhanced environmental semantic map includes: S3A uses point cloud data from continuous scanning by a single-line LiDAR and employs real-time localization and map building algorithms to generate a two-dimensional grid map with the initial position of the delivery robot as the origin. Each grid cell in this map stores an occupancy probability value. S3B: The generated dynamic obstacle prediction trajectory, transparent obstacle confidence area, and road edge danger zone, etc., are uniformly projected into the same coordinate system of the two-dimensional raster map according to the coordinate transformation relationship determined in the spatial registration process of step S2. S3C: For each grid cell in a two-dimensional grid map, update its occupancy probability or additional semantic attributes according to the semantic information type projected onto the grid cell area using different rules; S3D ultimately produces an enhanced environmental semantic map that overlays dynamic, transparent, and hazardous area semantic information onto a basic two-dimensional raster map, which is then used for path planning.

7. The autonomous navigation and obstacle avoidance method for a cargo delivery robot based on multi-sensor fusion according to claim 1, characterized in that: In step S4, the local path planning method generates the optimal trajectory by minimizing a multi-objective cost function, where the multi-objective cost function is a weighted sum of multiple cost terms, including: (1) The cost of deviation between the path and the global reference path; (2) The minimum distance cost between the path and all transparent obstacle confidence zones or road edge danger zones; (3) The cost of the spatiotemporal overlap of risk corridors generated by path and dynamic obstacle prediction; (4) The smoothness cost of the path itself; (5) The cost of kinematic feasibility of the delivery robot.

Images