Freight robot autonomous navigation obstacle avoidance method based on multi-sensor fusion

Abstract

The invention relates to the technical field of autonomous mobile robots, in particular to a freight robot autonomous navigation obstacle avoidance method based on multi-sensor fusion, which comprises the following steps of: identifying dynamic targets such as pedestrians and vehicles through data fusion of a depth camera and a laser radar, and predicting a future short-term movement track of the dynamic targets by utilizing a filtering algorithm; a risk corridor over time is generated, so that emergency braking or deadlock is avoided, and the operation efficiency and the traffic smoothness are greatly improved; a space-time consistency cross validation mechanism is established by utilizing the characteristics that the laser radar is insensitive to transparent objects and ultrasonic waves are sensitive to the transparent objects, so that false alarm and missing alarm are effectively eliminated, the robot can safely pass through a complex indoor environment, and the application boundary is greatly expanded; a depth camera is used for fitting a ground plane and analyzing point cloud height change in real time, so that obstacles which cannot be expressed by a two-dimensional navigation map can be identified; and an optimal track is generated by minimizing a multi-target cost function, so that the path is ensured to be safe and smooth.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of autonomous mobile robots, and in particular to an autonomous navigation and obstacle avoidance method for delivery robots based on multi-sensor fusion. BACKGROUND

[0002] The navigation system of existing delivery robots has prominent defects in complex real industrial and commercial environments due to reliance on single or limited perception technology, which seriously restricts its reliability and large-scale application, including the following aspects: current many delivery robots rely on a single type of sensor for environment perception, such as using only lidar or camera, which leads to limited perception ability when facing complex environments (such as light changes, transparent obstacles, dynamic obstacles, etc.), and is prone to misjudgment or omission; traditional methods often have difficulty in accurately predicting the motion trajectory of dynamic obstacles (such as pedestrians, other mobile devices), resulting in untimely obstacle avoidance or unreasonable path planning for robots; transparent obstacles (such as glass doors, plastic films, etc.) are almost invisible to sensors such as lidar, and traditional methods are difficult to effectively identify, which can easily cause collision accidents; in complex terrain, such as dangerous areas such as pits, cliffs or step edges, traditional methods often have difficulty in accurate detection, increasing the risk of robot falling or falling into; traditional two-dimensional grid maps mainly store the position information of obstacles, lack of expression of semantic information such as dynamic obstacles, transparent obstacles and dangerous areas on the road surface, limiting the flexibility and safety of path planning. Therefore, the present application proposes an autonomous navigation and obstacle avoidance method for delivery robots based on multi-sensor fusion. SUMMARY

[0003] The purpose of the present application is to solve the problems in the background art, and an autonomous navigation and obstacle avoidance method for delivery robots based on multi-sensor fusion is proposed.

[0004] In order to achieve the above purpose, the present application adopts the following technical solutions: The autonomous navigation and obstacle avoidance method for delivery robots based on multi-sensor fusion comprises: S1, constructing a layered heterogeneous perception module group: configuring a heterogeneous sensor array composed of a depth camera, a single-line lidar, an ultrasonic ranging module and a wheeled electromagnetic encoder on the body of the delivery robot; collecting multi-source heterogeneous perception data through the heterogeneous sensor array; S2, pre-process the collected multi-source heterogeneous perception data, and extract environmental features from the pre-processed data in parallel; wherein the pre-processing includes time synchronization and spatial registration of the collected multi-source heterogeneous perception data, and filtering and denoising and coordinate system unification processing are performed respectively; the environmental feature extraction includes extracting dynamic target contour features and ground three-dimensional geometric features from depth camera data, extracting high-precision obstacle distance and contour features from single-line laser radar data, and extracting near-field obstacle reflection features from ultrasonic data; S3, based on the extracted environmental features, collaborative perception judgment is performed through a feature complementary and cross-validation mechanism to generate a three-dimensional semantic map layer including dynamic obstacle prediction trajectory, transparent obstacle confidence area and road edge danger zone, and the three-dimensional semantic map layer is fused with a basic two-dimensional grid map constructed based on laser radar point cloud 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 delivery robot navigation trajectory; S5, the delivery robot executes the navigation trajectory for movement, and combines the real-time odometer data provided by the wheeled electromagnetic encoder for trajectory tracking control, outputs motor control instructions, and drives the delivery robot to move; S6, in the moving process, steps S1 to S5 are executed in a loop to realize continuous perception of the surrounding environment, online update of the enhanced environmental semantic map, and real-time re-planning of the navigation path until the navigation task is completed.

[0005] Further, in step S2, the process of extracting dynamic target contour features and ground three-dimensional 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 positioning on the RGB image output by the depth camera; S22, project the bounding box into the three-dimensional point cloud by synchronously acquiring the depth image to obtain the three-dimensional point cloud cluster of the dynamic target; S23, perform Euclidean clustering segmentation on the three-dimensional point cloud cluster, and calculate the centroid coordinates; by the centroid coordinate change between consecutive multiple frames, a Kalman filter or particle filter algorithm is used to estimate the instantaneous motion speed and motion direction of the dynamic target; S24, perform ground plane segmentation on the three-dimensional point cloud data acquired by the depth camera, and use the random sample consensus algorithm to fit the ground reference plane representing the mathematical model of the ground plane.

[0006] Further, in step S3, the process of generating a dynamic obstacle prediction trajectory includes: S31, the distance information of the obstacle detected by the fusion single-line laser radar in the current frame is matched in space and data associated with the center of mass coordinates of the dynamic target three-dimensional point cloud cluster calculated in step S23; S32, based on the history sequence of the center of mass coordinates of the dynamic target after successful data association, the motion trajectory of the dynamic target in the short term is predicted; S33, the predicted trajectory is packaged as a dynamic obstacle area with time attribute, and a risk corridor is formed according to the physical size of the dynamic target by taking the predicted position point as the center and expanding in space, and is marked in the enhanced environment semantic map.

[0007] Further, in step S3, the process of generating the transparent obstacle confidence area includes: S34, the data of the single-line laser radar is monitored in real time, and for the laser radar scanning beam, if no obstacle return point is detected in any detection direction, and the direction is located in the passable area of the delivery robot, the space area corresponding to the direction is marked as a laser radar perception hole area; S35, the reading of the ultrasonic ranging module arranged in the corresponding direction of the delivery robot is synchronously queried to obtain the measured distance value of the ultrasonic module in the detection direction corresponding to the laser radar perception hole area; S36, the measured point of the ultrasonic module is mapped to the coordinate system of the single-line laser radar to obtain the mapped measured point; if the mapped measured point is located within the spatial range of the laser radar perception hole area, it is determined to be spatially consistent; the time stamp difference between the ultrasonic measurement data and the laser radar perception hole area data is checked, and if the time stamp difference is less than a preset time synchronization threshold, it is determined to be time-synchronized; when the laser radar perception hole area and the ultrasonic measurement data simultaneously satisfy the conditions of spatial consistency and time synchronization, and the ultrasonic measurement distance value is less than the first safety 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 environment semantic map.

[0008] Further, in step S3, the process of generating the road edge danger zone includes: S37, based on the fitted ground reference plane, the point cloud data provided by the depth camera is analyzed in real time, and the point cloud in the front interest area of the depth camera is traversed; S38, the vertical distance of each point cloud to the ground reference plane is calculated, and if the vertical distance of a piece of continuous point cloud exceeds a preset depression threshold, it is determined to be a pit danger area; if the front point cloud appears a cliff-like absence relative to the ground reference plane, it is determined to be a cliff or step edge danger area; S39, the identified pit danger area and cliff edge danger area are marked as an impassable road edge danger zone in the enhanced environment semantic map.

[0009] Further, in step S3, the process of fusing the three-dimensional semantic map layer with the basic two-dimensional grid map constructed based on the lidar point cloud includes: S3A, based on the point cloud data of continuous scanning of single-line lidar, an instant positioning and map construction algorithm is used 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 obstacles at that location. S3B, the three-dimensional semantic information of the generated dynamic obstacle prediction trajectory, transparent obstacle confidence area, and road edge danger zone is projected into the same coordinate system of the two-dimensional grid map through the coordinate transformation relationship determined in the spatial registration process of step S2. S3C, for each grid in the two-dimensional grid map, according to the type of semantic information projected onto the grid area, different rules are used to update its occupancy probability or additional semantic attributes. S3D, finally, an enhanced environmental semantic map is obtained, which superimposes dynamic, transparent, and dangerous area semantic information on the basic two-dimensional grid map, and is used for path planning.

[0010] Further, in step S4, the local path planning method generates an 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 deviation cost of the path from the global reference path; (2) the minimum distance cost of the path from all transparent obstacle confidence areas or road edge danger zones; (3) the overlap cost of the path with the risk corridor generated by the dynamic obstacle prediction in space and time; (4) the smoothness cost of the path itself; (5) the kinematic feasibility cost of the delivery robot.

[0011] Compared with the prior art, the application has the beneficial effects that: by configuring various heterogeneous sensors such as a depth camera, a single-line laser radar and the like, multi-source heterogeneous data are collected and preprocessed, and environmental features are extracted, so that the surrounding environment information can be comprehensively and accurately perceived; based on the extracted environmental features, a three-dimensional semantic map layer is generated through collaborative perception determination, and is fused with a basic two-dimensional grid map to form an enhanced environmental semantic map, so that key information such as a dynamic obstacle prediction trajectory and a transparent obstacle confidence area is presented, and rich basis is provided for path planning; in the path planning link, a local path planning method generates an optimal trajectory by minimizing a multi-objective cost function, and various factors such as path deviation and distance from an obstacle are comprehensively analyzed to ensure path safety and smoothness; the application realizes closed-loop optimization of the perception, decision and control links under the premise of not excessively relying on expensive sensors, and provides key technical support for large-scale and high-reliability application of the delivery robot in complex indoor and outdoor environments such as warehouses and logistics. BRIEF DESCRIPTION OF DRAWINGS

[0012] Figure 1 A flowchart of the autonomous navigation and obstacle avoidance method of the delivery robot based on multi-sensor fusion proposed by the application. DETAILED DESCRIPTION

[0013] The technical solutions in the embodiments of the application will be clearly and completely described below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, rather than all the embodiments of the application. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the application.

[0014] Referring to Figure 1 The autonomous navigation and obstacle avoidance method of the delivery robot based on multi-sensor fusion comprises the following steps: S1, construct a layered heterogeneous perception module group: configure a heterogeneous sensor array composed of a depth camera, a single-line laser radar, an ultrasonic ranging module, and a wheeled electromagnetic encoder on the body of the delivery robot (the installation height of the depth camera is 0.6-1.2 meters from the ground, and the pitch angle is inclined downward by 5-20 degrees to ensure that the field of view angle can effectively cover the front travel area and the ground area; the installation height of the single-line laser radar is similar to that of the depth camera, and the scanning plane is parallel or slightly angled with the ground; the ultrasonic ranging module is arranged in a ring array around the robot chassis, and the detection direction is slightly inclined downward to cover the near-field blind area of the laser radar and the depth camera; the wheeled electromagnetic encoder is directly embedded in the drive wheel servo motor of the delivery robot, and is rigidly connected with the motor output shaft to ensure accurate detection of the actual rotation angle and speed of the drive wheel without gap and slip; the signal is transmitted directly to the main control computing unit of the delivery robot through a special high-speed interface to provide high-frequency and low-delay odometer pulse data for accurate kinematic feedback of the delivery robot's trajectory calculation); S2, pre-process the collected multi-source heterogeneous perception data, and extract environmental features from the pre-processed data in parallel; wherein the pre-processing includes time synchronization and spatial registration of the collected multi-source heterogeneous perception data, and filtering and denoising and coordinate system unification processing (time synchronization uses a hardware trigger signal or a software synchronization method based on the network time protocol (NTP) to ensure that the timestamp deviation of each sensor data is less than 10 milliseconds; spatial registration is to unify all multi-source heterogeneous perception data to the base coordinate system of the delivery robot through the transformation matrix of each sensor relative to the base coordinate system of the delivery robot); environmental feature extraction includes extracting dynamic target contour features and ground three-dimensional geometric features from depth camera data, extracting high-precision obstacle distance and contour features from single-line laser radar data, and extracting near-field obstacle reflection features from ultrasonic data; S3, based on the extracted environmental features, perform collaborative perception judgment through feature complementation and cross-validation mechanism to generate a three-dimensional semantic map layer including dynamic obstacle prediction trajectory, transparent obstacle confidence area, and road edge dangerous zone, and fuse the three-dimensional semantic map layer with the basic two-dimensional grid map based on laser radar point cloud to form an enhanced environmental semantic map; S4, based on the enhanced environmental semantic map, generate a safe and smooth delivery robot navigation trajectory using a local path planning method; S5, the delivery robot executes the navigation trajectory to move, and combines real-time odometer data provided by the wheeled electromagnetic encoder to perform trajectory tracking control, and outputs motor control instructions to drive the delivery robot to move; S6, during the moving process, steps S1 to S5 are cyclically executed to realize continuous perception of the surrounding environment, online updating of the enhanced environment semantic map, and real-time re-planning of the navigation path, until the navigation task is completed.

[0015] It needs to be further explained that, in the specific implementation process, in step S2, the process of extracting dynamic target contour features and ground three-dimensional geometric features from the 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 positioning on the RGB image output by the depth camera (including pedestrians, other mobile devices), wherein the neural network is YOLO or SSD architecture; S22, the bounding box is projected into the three-dimensional point cloud by combining the synchronously acquired depth image to obtain the three-dimensional point cloud cluster of the dynamic target; S23, the three-dimensional point cloud cluster is subjected to Euclidean clustering segmentation, and the centroid coordinates are calculated; by means of the centroid coordinate changes between continuous multiple frames, Kalman filtering or particle filtering algorithm is used to estimate the instantaneous motion speed and motion direction of the dynamic target; S24, the three-dimensional point cloud data acquired by the depth camera is subjected to ground plane segmentation, and a ground reference plane representing the mathematical model of the ground plane is fitted by using the random sample consensus algorithm; it can be understood that the purpose of the ground plane segmentation of the point cloud data is to separate the points belonging to the ground from the non-ground points such as obstacles and dynamic targets, and to provide a basis for subsequent obstacle identification and passable area analysis; in the present application, the random sample consensus (RANSAC) algorithm is used for plane fitting, and the process includes: three points are randomly selected from the point cloud to calculate a plane model, the distances of all points in the point cloud to the plane are calculated, the points with a distance less than a pre-set distance threshold are determined as inliers (i.e. ground points), and the process is repeated and iterated, and finally the plane model with the most inliers is selected as the ground plane, so that a mathematical equation representing the ground plane, i.e. the ground reference plane, is obtained.

[0016] It needs to be further explained that, in the specific implementation process, in step S3, the process of generating the dynamic obstacle prediction trajectory, the transparent obstacle confidence region and the road edge danger zone includes: S31, the obstacle distance information detected by the single-line laser radar in the current frame is matched 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 the position accuracy thereof; 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. 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. 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. S34, real-time monitoring of the data of the single-line laser radar, for the laser radar scanning beam, if no obstacle return point is detected in any detection direction, and the direction is located in the passable area of the delivery robot, the space region corresponding to the direction is marked as a laser radar perception hollow area; S35, synchronously querying the reading of the ultrasonic ranging module arranged in the corresponding direction of the delivery robot, to obtain the measured distance value of the ultrasonic module in the detection direction corresponding to the laser radar perception hollow area; S36, mapping the measurement point of the ultrasonic module to the coordinate system of the single-line laser radar to obtain a mapped measurement point; if the mapped measurement point is located within the spatial range of the laser radar perception hollow area, it is determined that the space is consistent; checking the time stamp difference between the ultrasonic measurement data and the laser radar perception hollow area data, if the time stamp difference is less than a preset time synchronization threshold, it is determined that the time is synchronized; when the laser radar perception hollow area and the ultrasonic measurement data simultaneously satisfy the conditions of space consistency and time synchronization, and the ultrasonic measurement distance value is less than a first safety distance threshold, it is determined that there is a high-confidence transparent obstacle in the region, and the region is marked as a transparent obstacle confidence region in the enhanced environmental semantic map; optionally, reading the RGB image data of the depth camera in the transparent obstacle confidence region, and comparing the texture features, edge features or optical reflection characteristics of the image in the region with a preset transparent object (such as glass) feature library; if the matching degree is higher than a confidence threshold, the confidence of the determination that the region is a transparent obstacle is enhanced; if the matching degree is low, the confidence is lowered or a secondary confirmation mechanism is started; it should be noted that, in terms of dimension, the laser radar perception hollow area is a definition of a specific space region in which no obstacle return point is detected during laser radar detection, which is essentially a description of a space category and does not have the traditional physical dimension; the ultrasonic measurement data, as the output result of the ultrasonic ranging module, is a distance value with length as the dimension, directly representing the spatial interval between the obstacle and the delivery robot; although there is a difference in dimension between the two, in the detection task of the transparent obstacle, a specific analysis framework can be constructed to realize the collaborative analysis of the two; S37, based on the fitted ground reference plane, real-time analyzing the point cloud data provided by the depth camera, and traversing the point cloud in the front interest region of the depth camera; S38, calculating the vertical distance of each point cloud to the ground reference plane, if the vertical distance of a continuous piece of point cloud exceeds a preset concave threshold, it is determined as a pit danger region; if the front point cloud appears a cliff-like absence relative to the ground reference plane, it is determined as a cliff or step edge danger region; S39, marking the identified pit danger region and cliff edge danger region as an impassable road edge danger zone in the enhanced environmental semantic map.

[0017] It needs to be further explained that in the specific implementation process, in step S3, the process of fusing the three-dimensional semantic map layer with the basic two-dimensional grid map constructed based on the laser radar point cloud to form a layer of enhanced environment semantic map includes: S3A, based on the point cloud data of the continuous scanning of the single-line laser radar, an instant positioning and map construction algorithm is used 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 representing the possibility of the presence of obstacles at that position. It can be understood that the instant positioning and map construction algorithm is one of the core technologies in the field of autonomous mobile robots. Through a probability estimation method, the robot can simultaneously infer its motion trajectory (positioning) and gradually construct a spatial model of the environment (mapping) while moving in an unknown environment. In this application, the purpose of applying the instant positioning and map construction (SLAM) algorithm is to create an accurate two-dimensional grid map based on geometric information, which will serve as the basis coordinate system for subsequent semantic information fusion. The specific application process is as follows: when the delivery robot starts, its initial position is taken as the origin of the map coordinate system; When the delivery robot moves, the instant positioning and map construction algorithm continuously receives the odometer information provided by the wheeled electromagnetic encoder (used to preliminarily estimate the motion of the delivery robot) and simultaneously acquires the environmental point cloud scanned by the single-line laser radar. By comparing the current frame of laser scan data with the existing map features or the previous frame of scan data (for example, using the Iterative Closest Point (ICP) algorithm or its variants), the pose change (translation and rotation) of the robot relative to the environment is more accurately calculated, thereby correcting the cumulative errors that may be generated by the odometer. Based on each step of positioning estimation, the obstacle information scanned by the current laser is converted into a grid map. In the generated grid map, each grid stores an occupancy probability value (such as between 0 and 1), representing the possibility of the position being occupied by obstacles. For example, the closer the value is to 1, the higher the certainty that the grid is an obstacle. The closer it is to 0, the higher the certainty that the area is a free space. Approximately 0.5 indicates an unknown state. When the delivery robot re-visits an area it has previously visited, the instant positioning and map construction algorithm can complete scene reproduction recognition and significantly correct the cumulative errors of the entire motion trajectory and map through position re-identification technology, thereby ensuring the global consistency of the map. S3B, the three-dimensional semantic information of the generated dynamic obstacle prediction trajectory, transparent obstacle confidence area, and road edge danger zone is projected into the same coordinate system of the two-dimensional grid map through the coordinate transformation relationship determined in the space registration process of step S2. S3C, for each grid in the two-dimensional grid map, different rules are adopted to update its occupancy probability or additional semantic attributes according to the type of semantic information projected onto the grid area: when any grid is located in the risk corridor of the dynamic obstacle prediction trajectory, the grid is labeled as a dynamic obstacle and is assigned a time-limited high occupancy probability which decays with the passage of prediction time; If there is a grid in the two-dimensional grid map that is in the transparent obstacle confidence area, the occupancy probability of the grid is directly set to the maximum value, and a transparent obstacle attribute label is added, wherein the obstacle information represented by the label takes priority over the original observation value of the single-line laser radar; For a grid located in the road edge danger zone, the occupancy probability of the grid is set to the maximum value, and an impassable attribute label is added; S3D, finally, an enhanced environment semantic map is obtained by superimposing dynamic, transparent and dangerous area semantic information on the basis two-dimensional grid map, which is used for path planning.

[0018] It needs to be further explained that in the specific implementation process, in step S4, the local path planning method generates an optimal trajectory by minimizing a multi-objective cost function, wherein the multi-objective cost function is the weighted sum of multiple cost terms, including: (1) path deviation cost from the global reference path; (2) minimum distance cost of the path from all transparent obstacle confidence areas or road edge danger zones; (3) overlap cost in space-time of the path and the risk corridor generated by the dynamic obstacle prediction; (4) smoothness cost of the path itself; (5) kinematic feasibility cost of the delivery robot; The multi-objective cost function is: , wherein is the path deviation cost from the global reference path, which ensures that the local planning does not deviate from the global strategic goal, and the local path is composed of N pose points , for the th pose point , the Euclidean distance from the nearest point on the global reference path is calculated , and the total deviation cost is the sum of the squares of the deviations of all pose points divided by N, i.e. the average deviation: ; is the minimum distance cost of the path from the transparent obstacle confidence area or the road edge danger zone, which sharply increases when the distance is less than the safety threshold; for each pose point on the pathCalculate 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; 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; 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: ; 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 weight coefficients of each cost item can balance different performance indexes such as safety, efficiency and smoothness of the path, so that the robot behavior can adapt to different application scenarios.

[0019] In addition, the formulas involved in the above are calculated by removing the dimension and taking the numerical value, which is obtained by collecting a large amount of data to simulate the closest real situation by software. The weight coefficients in the formula and the various preset thresholds in the analysis process are set by the person skilled in the art according to the actual situation or obtained by a large amount of data simulation. The size of the weight coefficient is a specific numerical value obtained by quantifying each parameter for subsequent comparison. The size of the weight coefficient depends on the amount of sample data and the corresponding processing coefficient preliminarily set by the person skilled in the art for each group of sample data. As long as it does not affect the proportional relationship between the parameters and the quantized numerical value.

[0020] Each embodiment in the specification is described in a progressive manner, and the same or similar parts between each embodiment can be referred to each other. Each embodiment mainly describes the differences from other embodiments. In particular, for the device embodiment, since it is basically based on the method embodiment, the description is relatively simple, and the related parts can be referred to the part of the method embodiment.

[0021] For the convenience of description, the above device is described by dividing into various units according to functions. Of course, the functions of each unit can be implemented in the same or multiple software and / or hardware when implementing the present application.

[0022] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented 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.

[0023] The present application is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 The device that implements the functions specified in one flow or multiple flows and / or blocks Figure 1 one flow or multiple flows and / or blocks

[0024] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the flow Figure 1 one or more flow or block Figure 1 one or more blocks or blocks specified in the flow.

[0025] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions executed on the computer or other programmable apparatus provide steps for implementing the flow Figure 1 one or more flow or block Figure 1 one or more blocks or blocks specified in the flow.

[0026] Secondly: the drawings of the embodiments disclosed in the present application only involve the structures related to the embodiments disclosed in the present application, other structures can refer to the general design, and in the case of no conflict, the same embodiments and different embodiments of the present application can be combined with each other; Finally: the above only describes the preferred embodiments of the present application, but the protection scope of the present application is not limited to this, any person skilled in the art can make equivalent replacement or change according to the technical scheme and the inventive concept of the present application within the technical range disclosed in the present application, which should be covered in the protection scope of the present application.

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 module arranged in the corresponding direction of the delivery robot, and obtain the measured distance value of the ultrasonic module 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