A Robot Environmental Perception Method and System Based on Multi-Sensor Fusion

Abstract

This invention relates to the field of robot environmental interaction technology, and discloses a robot environmental perception method and system based on multi-sensor fusion. The method includes: receiving robot environmental perception commands; detecting the current environment using the target robot and the robot environmental perception commands to obtain a first frame of data; constructing an optimal ground plane model; acquiring a set of non-ground obstacle point cloud coordinates; predicting a set of obstacle prediction positions; scanning the current environment for obstacles to obtain a second frame of tracking list; acquiring destination coordinates, an obstacle safety function set, and a complete obstacle trajectory set based on the second frame of tracking list; identifying the completed mobile robot based on the destination coordinates, the obstacle safety function set, and the complete obstacle trajectory set; and completing robot environmental perception based on multi-sensor fusion using the completed mobile robot. This invention can improve the robot's perception and navigation efficiency, obstacle avoidance accuracy, and autonomous perception intelligence in complex dynamic environments.

Description

Technical Field

[0001] This invention relates to the field of robot environment interaction technology, and in particular to a robot environment perception method and system based on multi-sensor fusion. Background Technology

[0002] Multi-sensor fusion is a technology that integrates and analyzes data from multiple sensors of different or similar types to obtain more accurate, reliable, and comprehensive information than from a single sensor. A robot is an automated machine capable of sensing its environment, making decisions, and performing actions. Environmental perception refers to the process by which a robot monitors, analyzes, and understands its surroundings in real time using its onboard sensors.

[0003] In current applications of robot environmental perception, multi-sensor fusion methods still face numerous challenges. In dynamic scenarios, fluctuations in sensor data caused by factors such as obstacle movement, lighting changes, and human interference make it difficult for existing fusion algorithms to adapt to environmental changes in real time, resulting in insufficient stability of perception results. Furthermore, existing technologies largely focus on fusion at the environmental perception level and have not yet established an effective mechanism for efficiently mapping the fusion results to robot perception decisions, leading to delayed robot perception responses and limited perception efficiency. Therefore, improving the perception and navigation efficiency, obstacle avoidance accuracy, and autonomous perception intelligence of robots in complex dynamic environments is an urgent technical problem to be solved. Summary of the Invention

[0004] This invention provides a robot environmental perception method based on multi-sensor fusion and a computer-readable storage medium. Its main purpose is to improve the robot's perception and navigation efficiency, obstacle avoidance accuracy, and autonomous perception intelligence in complex dynamic environments.

[0005] To achieve the above objectives, the present invention provides a robot environment perception method based on multi-sensor fusion, comprising: The target robot and the current environment were identified. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. Receive robot environment perception instructions, use the target robot and robot environment perception instructions to detect the current environment, and obtain the first frame of data, which includes a point cloud coordinate set; Construct an optimal ground plane model based on the point cloud coordinate set, and obtain the point cloud coordinate set of non-ground obstacles based on the optimal ground plane model; Based on the coordinate set of non-ground obstacle point cloud, the predicted obstacle position set is predicted. The target robot and the predicted obstacle position set are used to scan the current environment for obstacles, and the second frame tracking list is obtained. Based on the second frame tracking list, obtain the destination coordinates, obstacle safety function set, and complete obstacle trajectory set. Based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set, confirm that the mobile robot has been completed. Based on the completed mobile robot, we can achieve robot environmental perception based on multi-sensor fusion.

[0006] Optionally, the construction of the optimal ground plane model based on the point cloud coordinate set includes: The maximum x-coordinate value, maximum y-coordinate value, and maximum y-coordinate value are obtained from the point cloud coordinate set, and a three-dimensional point cloud space is constructed based on the maximum x-coordinate value, maximum y-coordinate value, and maximum y-coordinate value; The three-dimensional point cloud space is divided using a preset voxel size to obtain a voxel set. Point cloud coordinates are extracted sequentially from the point cloud coordinate set. Based on the extracted point cloud coordinates and voxel size, the coordinate voxel index is calculated. The target voxel is identified in the voxel set according to the coordinate voxel index. The extracted point cloud coordinates are assigned to the target voxel to obtain the assigned voxel. Summarize the assigned voxels to obtain the assigned voxel set, wherein the assigned voxel set includes multiple assigned voxels, and each assigned voxel includes zero or one or more point cloud coordinates; Sequentially extract the allocated voxels from the allocated voxel set to obtain the voxel point cloud coordinate set of the extracted allocated voxels; determine whether point cloud coordinates exist in the extracted allocated voxels. If point cloud coordinates exist in the extracted assigned voxels, calculate the geometric centroid coordinates of the voxel point cloud coordinate set, and use the geometric centroid coordinates to replace the voxel point cloud coordinate set to obtain the updated point cloud coordinates. The point cloud coordinates are summarized and updated to obtain the downsampled point cloud coordinate set. The optimal ground plane model is then constructed based on the downsampled point cloud coordinate set.

[0007] Optionally, the construction of the optimal ground plane model based on the downsampled point cloud coordinate set includes: Random point cloud coordinate sets are extracted sequentially from the downsampled point cloud coordinate set, and then removed from the downsampled point cloud coordinate set to obtain the remaining point cloud coordinate set. A planar model is constructed based on the extracted random point cloud coordinate set. The planar distance between each remaining point cloud coordinate in the remaining point cloud coordinate set and the planar model is calculated to obtain a planar distance set. Planar distances are extracted sequentially from the planar distance set. If the extracted planar distance is less than a preset distance threshold, the remaining point cloud coordinates corresponding to the extracted planar distance are taken as the point cloud coordinates in the plane. Summarize the point cloud coordinates in the plane to obtain the point cloud coordinate set in the plane corresponding to the plane distance set, and calculate the number of point cloud coordinates in the plane based on the point cloud coordinate set in the plane. The point cloud coordinate set in the plane is removed from the remaining point cloud coordinate set to obtain the updated point cloud coordinate set, which is then used as the downsampled point cloud coordinate set. Return to the step of sequentially and randomly extracting random point cloud coordinate sets from the downsampled point cloud coordinate set; obtain the number of times the random point cloud coordinate sets are sequentially and randomly extracted from the downsampled point cloud coordinate set until the number of executions equals the preset number of normal executions; The number of point cloud coordinates in the plane is summarized to obtain the set of point cloud coordinates in the plane. The plane model corresponding to the largest number of point cloud coordinates in the set is taken as the best ground plane model.

[0008] Optionally, obtaining the point cloud coordinate set of non-ground obstacles based on the optimal ground plane model includes: Remove the in-plane point cloud coordinate set corresponding to the optimal ground plane model from the downsampled point cloud coordinate set to obtain the non-ground point cloud coordinate set. Extract non-ground point cloud coordinates from the non-ground point cloud coordinate set in sequence. Remove the extracted non-ground point cloud coordinates from the non-ground point cloud coordinate set to obtain the remaining non-ground point cloud coordinate set. Search for the nearest neighbor coordinate set from the remaining non-ground point cloud coordinate set. Calculate the nearest neighbor distance between the extracted non-ground point cloud coordinates and each nearest neighbor coordinate in the nearest neighbor coordinate set to obtain the nearest neighbor distance set, and calculate the nearest average distance based on the nearest neighbor distance set; Summarize the nearest average distances to obtain the nearest average distance set. Calculate the mean and standard deviation of the mean distances based on the nearest average distance set. Calculate the standard deviation multiple threshold based on the mean and standard deviation of the mean distances. The nearest average distance is extracted sequentially from the nearest average distance set. If the extracted nearest average distance is greater than the standard deviation multiple threshold, the non-ground point cloud coordinates corresponding to the extracted nearest average distance are taken as outlier noise coordinates. The coordinates of outliers are summarized to obtain the outlier coordinate set. The outlier coordinate set is then removed from the non-ground point cloud coordinate set to obtain the non-ground obstacle point cloud coordinate set.

[0009] Optionally, the step of predicting the obstacle prediction location set based on the non-ground obstacle point cloud coordinate set includes: Clustering is performed on the non-ground obstacle point cloud coordinate set to obtain obstacle clusters, wherein the obstacle clusters include multiple obstacle clusters, and each obstacle cluster includes multiple non-ground obstacle point cloud coordinates; Obstacle clusters are extracted sequentially from the obstacle cluster set. The three-dimensional bounding box parameters and obstacle centroid coordinates are determined based on the point cloud coordinates of multiple non-ground obstacles in the obstacle cluster. The three-dimensional bounding box parameters include: maximum horizontal value, minimum horizontal value, maximum vertical value, minimum vertical value, maximum vertical value, and minimum vertical value. A 3D bounding box is constructed using 3D bounding box parameters, and the obstacle dimensions are calculated based on the 3D bounding box. The obstacle dimensions include: obstacle length, obstacle width, and obstacle height. Based on the obstacle's center coordinates and size, the obstacle's static characteristic parameters are identified, and the obstacle's static characteristic parameters are summarized to obtain the obstacle's static characteristic parameter set; An obstacle tracker set is created based on the set of static feature parameters of obstacles, wherein each obstacle tracker corresponds one-to-one with the static feature parameters of obstacles; The obstacle tracker set is stored to obtain the first frame tracking list, which includes multiple tracked obstacles; For each tracked obstacle in the first frame tracking list, position prediction is performed to obtain the obstacle prediction position set.

[0010] Optionally, the step of using the target robot and the predicted location set of obstacles to perform obstacle scanning on the current environment to obtain a second frame tracking list includes: The target robot performs a dynamic obstacle scan of the current environment to obtain a second frame of data, which includes multiple obstacles detected in the current frame. The current observation location set is determined from the second frame of data, where each obstacle detected in the current frame corresponds one-to-one with the current observation location; The tracked obstacles are extracted sequentially from the tracking list of the first frame, and the predicted position of the target obstacle is confirmed from the set of predicted obstacle positions based on the extracted tracked obstacles. The predicted location of the target obstacle is combined with multiple obstacles detected in the current frame to obtain a matching combination set. The matching combination set includes multiple matching combinations, and each matching combination includes the predicted location of the target obstacle and the current observation location. The association cost set is calculated based on the matching combination set, where the association cost corresponds one-to-one with the matching combination; The minimum association cost is identified from the association cost set. If the minimum association cost is less than the preset maximum association distance threshold, the matching combination corresponding to the minimum association cost is taken as the optimal matching combination. The optimal matching combination is removed from multiple current frame detected obstacles and the first frame tracking list respectively to obtain multiple updated detected obstacles and updated first frame tracking list. The multiple updated detected obstacles are taken as multiple current frame detected obstacles, and the updated first frame tracking list is taken as the first frame tracking list. The step of extracting tracking obstacles from the first frame tracking list in sequence is returned until all tracking obstacles in the first frame tracking list are extracted. If the minimum association cost is greater than the maximum association distance threshold, then the matching combination corresponding to the minimum association cost is regarded as an unmatched combination; The best matching combinations and unmatched combinations are summarized separately to obtain the best matching combination set and the unmatched combination set. The new obstacle set and the lost tracking obstacle set are identified from the unmatched combination set. The number of lost tracking obstacles is calculated based on the lost tracking obstacle set and the number of lost tracking obstacles is saved to the tracking list of the first frame to obtain the unupdated tracking list. Obtain the obstacle set at the current observation position based on the optimal matching combination set. Calculate the centroid coordinate set of the current observed obstacles based on the obstacle set at the current observation position. Input the centroid coordinate set of the current observed obstacles into a pre-constructed Kalman filter to obtain the updated tracking dataset. Initialize each new obstacle in the new obstacle set to obtain the initialization parameter set. Update the unupdated tracking list using the updated tracking dataset and the initialization parameter set to obtain the second frame tracking list, which includes multiple updated tracking obstacles.

[0011] Optionally, obtaining the destination coordinates, obstacle safety function set, and complete obstacle track set based on the second frame tracking list includes: The latest state vector is obtained by sequentially extracting the updated tracking obstacles from the multiple updated tracking obstacles in the tracking list of the second frame; Once the prediction time period set is identified, perform the following operations on each prediction time period in the set: Calculate the next moment's predicted state vector based on the predicted time period and the latest state vector, obtain the next moment's predicted obstacle position based on the next moment's predicted state vector, summarize the next moment's predicted obstacle positions, and obtain the next moment's predicted obstacle position set corresponding to the predicted time period set. Based on the extracted updated tracking obstacles, obtain the historical obstacle location set. Sort the historical obstacle location set and the next moment's predicted obstacle location set in chronological order to obtain the historical obstacle location sequence and the predicted obstacle location sequence. Historical obstacle trajectories and predicted obstacle trajectories are generated based on historical obstacle location sequences and predicted obstacle location sequences. The historical obstacle trajectories and predicted obstacle trajectories are then spliced ​​together to obtain complete obstacle trajectories. The latest predicted coordinates of obstacles are determined based on the complete obstacle trajectory. The radius of the obstacle is obtained based on the extracted updated tracking obstacle. The current coordinates of the target robot and the destination coordinates are obtained. Based on the latest predicted obstacle coordinates, obstacle radius, and the current coordinates of the target robot, an obstacle safety function is constructed. The obstacle safety functions and the complete obstacle trajectory are then summarized to obtain the obstacle safety function set and the complete obstacle trajectory set.

[0012] Optionally, the step of confirming the completion of the mobile robot based on the destination coordinates, the obstacle safety function set, and the complete obstacle trajectory set includes: A two-dimensional grid map is constructed based on the current environment. The globally optimal movement path is then constructed based on the two-dimensional grid map, the complete obstacle track set, the current coordinates of the target robot, and the destination coordinates. Construct constraints, input the obstacle safety function set and constraints into the pre-constructed objective function, and obtain the objective function to be optimized; The objective function to be optimized is optimized to obtain a robot control command sequence. The robot control command sequence includes multiple robot control commands, and each robot control command corresponds one-to-one with the updated tracking obstacle. The robot control commands include linear velocity and angular velocity. The target robot is moved according to the robot control command sequence and the global optimal movement path. The movement coordinates are obtained. When the movement coordinates are equal to the destination coordinates, the target robot whose movement coordinates are equal to the destination coordinates is regarded as the robot whose movement has been completed.

[0013] Optionally, the obstacle safety function is represented as follows: in, Represents the obstacle safety function. Indicates the current coordinates of the target robot. Indicates the latest predicted obstacle coordinates. This indicates the preset robot radius. Indicates the radius of the obstacle. This indicates the preset safety margin. This represents the Euclidean norm.

[0014] To achieve the above objectives, the present invention also provides a robot environmental perception system based on multi-sensor fusion, comprising: The sensor configuration module is used to identify the target robot and the current environment. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. The robot environment perception module is used to receive robot environment perception instructions, use the target robot and robot environment perception instructions to detect the current environment, and obtain the first frame of data. The first frame of data includes a point cloud coordinate set, an optimal ground plane model is constructed based on the point cloud coordinate set, and a point cloud coordinate set of non-ground obstacles is obtained based on the optimal ground plane model. The obstacle extraction module is used to predict the predicted location set of obstacles based on the coordinate set of non-ground obstacle point cloud. The target robot and the predicted location set of obstacles are used to scan the current environment for obstacles and obtain the second frame tracking list. The robot perception and execution module is used to obtain the destination coordinates, obstacle safety function set, and complete obstacle trajectory set based on the second frame tracking list. Based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set, it confirms that the mobile robot has completed its operation and performs robot environmental perception based on multi-sensor fusion.

[0015] To address the above problems, the present invention also provides an electronic device, the electronic device comprising: Memory, storing at least one instruction; The processor executes the instructions stored in the memory to implement the robot environmental perception method based on multi-sensor fusion described above.

[0016] To address the aforementioned problems, the present invention also provides a computer-readable storage medium storing at least one instruction, which is executed by a processor in an electronic device to implement the aforementioned robot environmental perception method based on multi-sensor fusion.

[0017] To address the problems described in the background art, this invention identifies a target robot and the current environment. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. This invention clearly defines the target robot and its multiple sensors, providing a prerequisite for subsequent environmental detection and data acquisition using the advantages of different sensors. Different sensors can perceive environmental information from multiple dimensions, receive robot environmental perception commands, and use the target robot and these commands to detect the current environment, obtaining a first frame of data. This first frame includes a point cloud coordinate set. The point cloud data of this invention can accurately describe the 3D spatial position and shape information of objects in the environment, providing rich data support for subsequent operations such as building a ground plane model and identifying obstacles. An optimal ground plane model is constructed based on the point cloud coordinate set, and a point cloud coordinate set of non-ground obstacles is obtained based on this optimal ground plane model. This invention's construction of the optimal ground plane model can distinguish the ground portion of the environment from other objects, achieving preliminary environmental segmentation, which is helpful... This invention simplifies subsequent obstacle detection and analysis processes, improving processing efficiency. Based on a non-ground obstacle point cloud coordinate set, it predicts obstacle location sets. Using the target robot and the predicted obstacle location sets, it scans the current environment for obstacles, obtaining a second-frame tracking list. By predicting obstacle location sets, this invention allows the robot to understand the possible movement directions and ranges of obstacles in advance, thus avoiding potentially dangerous areas during path planning and improving robot safety and operational efficiency. Based on the second-frame tracking list, it obtains destination coordinates, obstacle safety function sets, and a complete obstacle trajectory set. Based on these, it confirms the completed mobile robot. This invention, by acquiring destination coordinates, obstacle safety function sets, and a complete obstacle trajectory set, comprehensively considers factors such as the robot's target position, obstacle safety, and obstacle movement trajectories, providing more comprehensive information for the robot's path planning and action decisions. Based on the completed mobile robot, it achieves robot environmental perception based on multi-sensor fusion. Therefore, this invention can improve the robot's navigation efficiency, obstacle avoidance accuracy, and autonomous interactive intelligence in complex dynamic environments. Attached Figure Description

[0018] Figure 1 This is a flowchart illustrating a robot environment perception method based on multi-sensor fusion according to an embodiment of the present invention. Figure 2 A functional block diagram of a robot environment perception system based on multi-sensor fusion provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the structure of an electronic device that implements the robot environment perception method based on multi-sensor fusion, according to an embodiment of the present invention.

[0019] Explanation of reference numerals in the attached figures: 10. Electronic device; 11. Processor; 12. Memory; 13. Bus.

[0020] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0021] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0022] This application provides a robot environment perception method based on multi-sensor fusion. The executing entity of the multi-sensor fusion-based robot environment perception method includes, but is not limited to, at least one of the following electronic devices that can be configured to execute the method provided in this application: a server, a terminal, etc. In other words, the multi-sensor fusion-based robot environment perception method can be executed by software or hardware installed on a terminal device or a server device, and the software can be a blockchain platform. The server includes, but is not limited to, a single server, a server cluster, a cloud server, or a cloud server cluster.

[0023] Reference Figure 1 The diagram shown is a flowchart illustrating a robot environment perception method based on multi-sensor fusion according to an embodiment of the present invention. In this embodiment, the robot environment perception method based on multi-sensor fusion includes: S1. Identify the target robot and the current environment. The target robot includes: a 3D LiDAR, an inertial measurement unit, and a vision sensor.

[0024] It should be explained that the target robot refers to a multi-sensor fusion robot platform with autonomous mobility. Its main body is fixedly equipped with a 3D LiDAR, an inertial measurement unit (IMU), and a vision sensor for synchronous perception and data acquisition of the current environment. The current environment refers to the actual 3D physical space in which the target robot is located at the moment it receives environmental perception commands. This space includes static obstacles, dynamic objects, and passable areas. The 3D LiDAR is an active ranging sensor mounted on the target robot and moving with it. It acquires the 3D coordinates of various reflection points in the current environment by emitting and receiving laser beams to generate a point cloud coordinate set with geometric structure information. The IMU is a sensor fixed to the target robot, used to measure the robot's three-axis acceleration and three-axis angular velocity in real time to provide attitude and motion state estimation. The vision sensor is a sensor fixed to the target robot, used to acquire visible light images of the current environment.

[0025] S2. Receive the robot's environmental perception command, use the target robot and the robot's environmental perception command to detect the current environment, and obtain the first frame of data, which includes a point cloud coordinate set.

[0026] It should be explained that robot environmental perception commands refer to instructions issued by humans to direct the target robot to perform specified perception, movement, or manipulation actions in the current environment. A point cloud coordinate set refers to a data set obtained by scanning the current environment with a 3D LiDAR, which discretizes the geometric features of the environmental surface in the form of 3D coordinate points. Each point contains at least the spatial coordinate information of the horizontal, vertical, and triangular axes.

[0027] S3. Construct the optimal ground plane model based on the point cloud coordinate set, and obtain the point cloud coordinate set of non-ground obstacles based on the optimal ground plane model.

[0028] In detail, the construction of the optimal ground plane model based on the point cloud coordinate set includes: The maximum x-coordinate value, maximum y-coordinate value, and maximum y-coordinate value are obtained from the point cloud coordinate set, and a three-dimensional point cloud space is constructed based on the maximum x-coordinate value, maximum y-coordinate value, and maximum y-coordinate value; The three-dimensional point cloud space is divided using a preset voxel size to obtain a voxel set. Point cloud coordinates are extracted sequentially from the point cloud coordinate set. Based on the extracted point cloud coordinates and voxel size, the coordinate voxel index is calculated. The target voxel is identified in the voxel set according to the coordinate voxel index. The extracted point cloud coordinates are assigned to the target voxel to obtain the assigned voxel. Summarize the assigned voxels to obtain the assigned voxel set, wherein the assigned voxel set includes multiple assigned voxels, and each assigned voxel includes zero or one or more point cloud coordinates; Extract allocated voxels sequentially from the allocated voxel set, and determine whether point cloud coordinates exist in the extracted allocated voxels; If point cloud coordinates exist in the extracted assigned voxels, obtain the voxel point cloud coordinate set of the extracted assigned voxels, calculate the geometric centroid coordinates of the voxel point cloud coordinate set, and replace the voxel point cloud coordinate set with the geometric centroid coordinates to obtain the updated point cloud coordinates. The point cloud coordinates are summarized and updated to obtain the downsampled point cloud coordinate set. The optimal ground plane model is then constructed based on the downsampled point cloud coordinate set.

[0029] It should be explained that the maximum x-coordinate value, maximum y-coordinate value, and maximum vertical coordinate value refer to the maximum values ​​extracted from the x-coordinate, y-coordinate, and vertical coordinates of all point cloud coordinates in the point cloud coordinate set, respectively. The steps for constructing a 3D point cloud space based on the maximum x-coordinate value, maximum y-coordinate value, and maximum vertical coordinate value are as follows: Obtain the minimum x-coordinate value, minimum y-coordinate value, and minimum vertical coordinate value from the point cloud coordinate set; use these minimum values ​​as the origin of the 3D point cloud space; and use the maximum values ​​as the diagonal vertices of the 3D point cloud space, thus forming an axis-aligned cuboid space that completely encloses the point cloud coordinate set. The minimum x-coordinate value, minimum y-coordinate value, and minimum vertical coordinate value refer to the minimum values ​​extracted from the x-coordinate, y-coordinate, and vertical coordinates of all point cloud coordinates in the point cloud coordinate set, respectively. The voxel size refers to the pre-defined side length of the cube mesh, used to uniformly discretize the 3D point cloud space into several equal-sized cube meshes. The specific value of the voxel size is determined by the operator based on the actual environment and relevant experience. A detailed example is provided below: For instance, an indoor service robot uses 3D LiDAR for navigation and obstacle avoidance in an office environment. It needs to reliably detect and avoid obstacles such as chair legs, small trash cans, and pedestrians. The chair leg has a diameter of approximately 0.05 meters, the small trash can is approximately 0.3 meters wide, and the pedestrian's body thickness is approximately 0.3 meters. Based on actual needs, the specific value of the voxel size can be defined as 0.1 meters. The principle is that a voxel size of 0.1 meters is smaller than the minimum feature size of most obstacles to be detected indoors. For example, for a small structure like a chair leg with a diameter of 0.05 meters, although a 0.1-meter voxel size cannot accurately delineate its complete cross-section, under typical point cloud density, multiple neighboring point clouds on the chair leg will fall into a series of spatially adjacent voxels. These occupied voxels will form a continuous, linear, or columnar connected voxel set in three-dimensional space. This unique spatial arrangement and distribution pattern provides clear clues for subsequent clustering or recognition algorithms, enabling them to reliably infer the existence of an obstacle entity with a small cross-sectional area but extending longitudinally. It also clearly preserves the outlines of small trash cans and pedestrian bodies, ensuring that the basic geometry of the obstacle to be detected remains recognizable in the voxelized representation. At the same time, compared to choosing a smaller voxel size, such as 0.02 meters, 0.1 meters can avoid generating too many voxels while ensuring the detectability of the obstacle, thereby saving memory and computing resources and meeting the requirements of real-time robot operation.

[0030] Understandably, a voxel set refers to the collection of all cubic meshes obtained by uniformly dividing a 3D point cloud space according to voxel dimensions. Each mesh is called a voxel and has a unique 3D integer index. The step of calculating the coordinate voxel index based on the extracted point cloud coordinates and voxel dimensions is as follows: subtract the minimum coordinate value of the corresponding axis from the x, y, and y coordinates of the extracted point cloud coordinates to obtain the offset coordinates (the minimum value of the offset coordinates is 0, and the maximum value is the difference between the original maximum and minimum coordinate values). Divide the x, y, and y coordinates of the offset coordinates by the voxel dimensions to obtain floating-point numbers. Convert the floating-point numbers to integer indices using a floor function to obtain the coordinate voxel index. This invention, through the coordinate offset correction step, ensures that the voxel indices of all point cloud coordinates can be accurately mapped to the corresponding voxels in the preset 3D point cloud space.

[0031] For example, the x-coordinate of the point cloud coordinates is 5.2 meters, the minimum coordinate value of the corresponding axis is 1.0 meters, the maximum coordinate value is 6.0 meters, and the voxel size is 0.1 meters. After offset, the x-coordinate is 5.2 - 1.0 = 4.2 meters, and the voxel index is 4.2 ÷ 0.1 = 42 (rounded down). The number of voxels on this axis is (6.0 - 1.0) ÷ 0.1 = 50, and the index range is 0-49. 42 is within this range and will not exceed the spatial boundary.

[0032] It should also be explained that a target voxel refers to a unique cubic mesh in a voxel set that matches the coordinate voxel index. An assigned voxel refers to a target voxel into which at least one point cloud coordinate has been assigned. An assigned voxel set refers to the set of all assigned voxels. Geometric centroid coordinates refer to the three-dimensional coordinates obtained by taking the arithmetic mean of the x, y, and y coordinates of all point cloud coordinates within an assigned voxel, used to represent the spatial average position of that voxel. The replacement of the corresponding point cloud coordinates in the extracted assigned voxels using geometric centroid coordinates means deleting all corresponding point cloud coordinates in the assigned voxel and using the geometric centroid coordinates as the only updated point cloud coordinates, thereby achieving downsampling. Updated point cloud coordinates refer to the point cloud coordinates after the corresponding point cloud coordinates in the assigned voxel have been replaced. Downsampled point cloud coordinate set refers to the set of all downsampled point cloud coordinates. Voxel point cloud coordinate set refers to the set of corresponding point cloud coordinates in the extracted assigned voxels.

[0033] In detail, the construction of the optimal ground plane model based on the downsampled point cloud coordinate set includes: Random point cloud coordinate sets are extracted sequentially from the downsampled point cloud coordinate set, and then removed from the downsampled point cloud coordinate set to obtain the remaining point cloud coordinate set. A planar model is constructed based on the extracted random point cloud coordinate set. The planar distance between each remaining point cloud coordinate in the remaining point cloud coordinate set and the planar model is calculated to obtain a planar distance set. Planar distances are extracted sequentially from the planar distance set. If the extracted planar distance is less than a preset distance threshold, the remaining point cloud coordinates corresponding to the extracted planar distance are taken as the point cloud coordinates in the plane. Summarize the point cloud coordinates in the plane to obtain the point cloud coordinate set in the plane corresponding to the plane distance set, and calculate the number of point cloud coordinates in the plane based on the point cloud coordinate set in the plane. The point cloud coordinate set in the plane is removed from the remaining point cloud coordinate set to obtain the updated point cloud coordinate set, which is then used as the downsampled point cloud coordinate set. Return to the step of sequentially and randomly extracting random point cloud coordinate sets from the downsampled point cloud coordinate set; obtain the number of times the random point cloud coordinate sets are sequentially and randomly extracted from the downsampled point cloud coordinate set until the number of executions equals the preset number of normal executions; The number of point cloud coordinates in the plane is summarized to obtain the set of point cloud coordinates in the plane. The plane model corresponding to the largest number of point cloud coordinates in the set is taken as the best ground plane model.

[0034] It should be explained that the random point cloud coordinate set refers to the set of point cloud coordinates extracted from the current downsampled point cloud coordinate set in each iteration through random sampling, used to fit the candidate plane. It should be noted that the random point cloud coordinate set includes three non-collinear point cloud coordinates. These three non-collinear point cloud coordinates are selected randomly from the current downsampled point cloud coordinate set using a random sample consensus algorithm to fit an initial planar model. If the three points in the random point cloud coordinate set are collinear, a plane cannot be uniquely determined. The random sample consensus algorithm is existing technology and will not be elaborated here. The remaining point cloud coordinate set refers to the set of all point cloud coordinates remaining after removing the random point cloud coordinate set from the current downsampled point cloud coordinate set, used for subsequent planar distance calculations and statistical analysis of point cloud coordinates within the plane. The step of constructing a planar model based on the extracted random point cloud coordinate set is as follows: substituting the extracted random point cloud coordinate set into the planar equation for solution, to obtain the planar model. The planar distance refers to the distance calculated using the point-to-plane distance formula. The planar distance set refers to the set composed of all planar distances. The distance threshold refers to the maximum vertical distance pre-set for determining whether a point in the point cloud data belongs to a candidate plane when fitting a ground plane model based on a random sampling consensus algorithm. The specific value of the distance threshold is determined by the operator based on the actual environment and relevant neighborhood experience. A detailed example is given: For instance, an indoor service robot is navigating in an office environment. The minimum ground clearance of the target robot is known to be 0.1 meters. Office floors are mostly flat tiles or short-pile carpets. The ranging accuracy of a LiDAR within 5 meters is approximately ±0.02 meters. Therefore, based on actual needs, the specific value of the distance threshold can be defined as 0.05 meters. The principle is that the vertical distance between the lowest point of the robot's chassis and the ground is 0.1 meters. Theoretically, the target robot can safely pass any object with a height lower than this clearance without collision. In-plane point cloud coordinates refer to the remaining point cloud coordinates whose planar distance is less than the distance threshold. The in-plane point cloud coordinate set refers to the set of all corresponding in-plane point cloud coordinates in a planar model. It is used to quantitatively evaluate the degree of fit between the current planar model and the sensor observation data by statistically analyzing the number of coordinate points in the in-plane point cloud coordinate set. The more in-plane point cloud coordinates in the in-plane point cloud coordinate set, the higher the support of the current planar model. The number of in-plane point cloud coordinates refers to the total number of in-plane point cloud coordinates contained in the in-plane point cloud coordinate set. The execution count refers to the actual number of completed iterations when randomly extracting random point cloud coordinate sets from the downsampled point cloud coordinate set. The normal execution count refers to the pre-set maximum number of iterations used to control the number of loops. For example, the normal execution count is 500. In a real robot navigation environment, the ground may be partially occluded or contain noise points, and the ground coverage may dynamically change.Setting the number of executions to 500 ensures a high success rate for ground plane fitting in most practical scenarios, while avoiding computational delays caused by excessive iterations (e.g., greater than 5000 iterations), which could impact system real-time performance. Updating the point cloud coordinate set refers to removing the current point cloud coordinates from the remaining point cloud coordinate sets, creating a new point cloud set that serves as the input downsampled point cloud coordinate set for the next iteration. The set of point cloud coordinate counts within the plane refers to the collection of the number of point cloud coordinates obtained in each iteration.

[0035] It should be noted that this invention can distinguish the ground portion of the environment from other objects by constructing an optimal ground plane model, thus achieving preliminary environmental segmentation. This helps simplify the subsequent obstacle detection and analysis process, improves processing efficiency, and, by obtaining the point cloud coordinate set of non-ground obstacles through the optimal ground plane model, accurately identifies obstacles in the environment, eliminates ground interference, and provides more precise targets for subsequent obstacle prediction and tracking.

[0036] Specifically, obtaining the point cloud coordinate set of non-ground obstacles based on the optimal ground plane model includes: Remove the in-plane point cloud coordinate set corresponding to the optimal ground plane model from the downsampled point cloud coordinate set to obtain the non-ground point cloud coordinate set. Extract non-ground point cloud coordinates from the non-ground point cloud coordinate set in sequence. Remove the extracted non-ground point cloud coordinates from the non-ground point cloud coordinate set to obtain the remaining non-ground point cloud coordinate set. Search for the nearest neighbor coordinate set from the remaining non-ground point cloud coordinate set. Calculate the nearest neighbor distance between the extracted non-ground point cloud coordinates and each nearest neighbor coordinate in the nearest neighbor coordinate set to obtain the nearest neighbor distance set, and calculate the nearest average distance based on the nearest neighbor distance set; Summarize the nearest average distances to obtain the nearest average distance set. Calculate the mean and standard deviation of the mean distances based on the nearest average distance set. Calculate the standard deviation multiple threshold based on the mean and standard deviation of the mean distances. The nearest average distance is extracted sequentially from the nearest average distance set. If the extracted nearest average distance is greater than the standard deviation multiple threshold, the non-ground point cloud coordinates corresponding to the extracted nearest average distance are taken as outlier noise coordinates. The coordinates of outliers are summarized to obtain the outlier coordinate set. The outlier coordinate set is then removed from the non-ground point cloud coordinate set to obtain the non-ground obstacle point cloud coordinate set.

[0037] It should be explained that the non-ground point cloud coordinate set refers to the set of point cloud coordinates remaining after removing all point cloud coordinates marked as in-plane coordinates by the optimal ground plane model from the downsampled point cloud coordinate set. This set is used to separate passable areas from potential obstacles in the current environment and serves as the data foundation for subsequent obstacle clustering, tracking, and obstacle avoidance. A non-ground point cloud coordinate refers to any single 3D point in the non-ground point cloud coordinate set, used for outlier detection point by point. The remaining non-ground point cloud coordinate set refers to the set of non-ground point cloud coordinates remaining after the current non-ground point cloud coordinates have been extracted and removed. Searching for the nearest neighbor coordinate set from the remaining non-ground point cloud coordinate set refers to using a KD-tree to search for the nearest neighbor coordinate set from the remaining non-ground point cloud coordinate set. The nearest neighbor coordinate set is the set of 3D points with the smallest distance to the current non-ground point cloud coordinate, obtained by searching the remaining non-ground point cloud coordinate set using a KD-tree. The nearest neighbor distance is the Euclidean distance between the current non-ground point cloud coordinate and its nearest neighbor coordinate, used to characterize the local point cloud density of the current non-ground point cloud coordinate in the current region. The nearest neighbor distance set refers to the set of all nearest neighbor distances. The nearest average distance is the average of all nearest neighbor distances in the nearest neighbor distance set, reflecting the average distance between points in the local area where the non-ground point cloud coordinates are located. The nearest average distance set is the set of nearest average distances formed by calculating the nearest average distance for each non-ground point cloud coordinate in the non-ground point cloud coordinate set. The mean and standard deviation of the mean distance refer to the mean and standard deviation of the nearest average distance set, respectively. The standard deviation multiple threshold is the value obtained by adding twice the mean of the mean distance to the standard deviation of the mean distance. Outlier coordinates are non-ground point cloud coordinates whose nearest average distance is greater than the standard deviation multiple threshold. The outlier coordinate set is the set of all outlier coordinates. The non-ground obstacle point cloud coordinate set is the set of three-dimensional points remaining after removing the outlier coordinate set from the non-ground point cloud coordinate set, constituting the final effective point cloud data for obstacle recognition and clustering.

[0038] S4. Based on the coordinate set of non-ground obstacle point cloud, predict the obstacle prediction position set, and use the target robot and the obstacle prediction position set to scan the current environment for obstacles, and obtain the second frame tracking list.

[0039] Specifically, the prediction of the obstacle prediction location set based on the non-ground obstacle point cloud coordinate set includes: Clustering is performed on the non-ground obstacle point cloud coordinate set to obtain obstacle clusters, wherein the obstacle clusters include multiple obstacle clusters, and each obstacle cluster includes multiple non-ground obstacle point cloud coordinates; Obstacle clusters are extracted sequentially from the obstacle cluster set. The three-dimensional bounding box parameters and obstacle centroid coordinates are determined based on the point cloud coordinates of multiple non-ground obstacles in the obstacle cluster. The three-dimensional bounding box parameters include: maximum horizontal value, minimum horizontal value, maximum vertical value, minimum vertical value, maximum vertical value, and minimum vertical value. A 3D bounding box is constructed using 3D bounding box parameters, and the obstacle dimensions are calculated based on the 3D bounding box. The obstacle dimensions include: obstacle length, obstacle width, and obstacle height. Based on the obstacle's center coordinates and size, the obstacle's static characteristic parameters are identified, and the obstacle's static characteristic parameters are summarized to obtain the obstacle's static characteristic parameter set; An obstacle tracker set is created based on the set of static feature parameters of obstacles, wherein each obstacle tracker corresponds one-to-one with the static feature parameters of obstacles; The obstacle tracker set is stored to obtain the first frame tracking list, which includes multiple tracked obstacles; For each tracked obstacle in the first frame tracking list, position prediction is performed to obtain the obstacle prediction position set.

[0040] It should be explained that the clustering operation on the non-ground obstacle point cloud coordinate set refers to using a clustering algorithm to perform density connectivity analysis on the non-ground obstacle point cloud coordinates in the non-ground obstacle point cloud coordinate set, and classifying non-ground obstacle point cloud coordinates with a spatial distance less than a preset clustering threshold into the same cluster. The clustering threshold is a pre-set distance value used to determine the maximum spatial distance between two non-ground obstacle point cloud coordinates that belong to the same object. For example, the clustering threshold is 0.2 meters. If the clustering threshold is too small (e.g., greater than 0.1 meters), the same object may be divided into multiple small clusters due to sparse point cloud or noise, increasing the number of tracking targets and algorithm complexity; if the clustering threshold is too large (e.g., greater than 0.5 meters), multiple independent and close objects (e.g., pedestrians walking side by side) may be mistakenly merged into a single cluster, resulting in missed detection of obstacles and distortion of trajectory prediction. Therefore, 0.2 meters is a trade-off between typical point cloud density and object spacing. An obstacle cluster refers to the set of all obstacle clusters obtained after the clustering operation. The maximum horizontal value, minimum horizontal value, maximum vertical value, minimum vertical value, maximum vertical value, and minimum vertical value refer to the maximum, minimum, maximum, minimum, maximum, and minimum coordinates of all non-ground obstacle point cloud coordinates within an obstacle cluster, respectively, along the horizontal axis. A 3D bounding box is an axis-aligned cuboid formed by the 3D bounding box parameters, used to geometrically enclose the corresponding obstacle cluster. The steps for calculating obstacle dimensions based on the 3D bounding box are as follows: the maximum horizontal value minus the minimum horizontal value is the obstacle length; the maximum vertical value minus the minimum vertical value is the obstacle width; and the maximum vertical value minus the minimum vertical value is the obstacle height. The obstacle static feature parameter set refers to the collection of obstacle static feature parameters corresponding to all obstacle clusters. Storing the obstacle tracker set refers to storing the obstacle tracker set using a list. The obstacle tracker set is a collection composed of all obstacle trackers. The obstacle tracker creation steps are as follows: Based on the centroid coordinates and obstacle size of the obstacle in the previous frame, initialize the state vector and noise covariance matrix in the Kalman filter (or its extended algorithms EKF, UKF), and assign a unique ID, thereby instantly generating a tracker that corresponds one-to-one with the current detection. Tracked obstacles refer to obstacles imported into the first frame tracking list and assigned a unique ID; their state is updated by the corresponding obstacle tracker. The step of predicting the position of each tracked obstacle in the first frame tracking list involves using the Kalman filter to perform state prediction on each tracked obstacle in the first frame tracking list to obtain the predicted obstacle position. The step of using the Kalman filter to perform state prediction on each tracked obstacle in the first frame tracking list to obtain the predicted obstacle position is as follows: First, define the state vector of the tracked obstacle, including the centroid coordinates in three-dimensional space and the corresponding axial velocity.A state transition matrix is ​​constructed based on a uniform motion model, where the time interval is determined by the sensor sampling frequency. During prediction, the predicted state of the current frame is calculated using the optimal estimated state vector from the previous frame through the state transition matrix. Simultaneously, the state covariance matrix is ​​updated by incorporating the process noise covariance matrix to reflect prediction uncertainty. Finally, coordinate components are extracted from the predicted state to obtain the predicted obstacle position. The above process of predicting obstacle positions can be implemented using existing technologies and will not be elaborated further here.

[0041] The predicted location of an obstacle is an estimate of the location where an obstacle will appear in the next moment (future). This estimate is a three-dimensional centroid coordinate.

[0042] In detail, the step of using the target robot and the predicted location set of obstacles to perform obstacle scanning on the current environment to obtain the second frame tracking list includes: The target robot performs a dynamic obstacle scan of the current environment to obtain a second frame of data, which includes multiple obstacles detected in the current frame. The current observation location set is determined from the second frame of data, where each obstacle detected in the current frame corresponds one-to-one with the current observation location; The tracked obstacles are extracted sequentially from the tracking list of the first frame, and the predicted position of the target obstacle is confirmed from the set of predicted obstacle positions based on the extracted tracked obstacles. The predicted location of the target obstacle is combined with multiple obstacles detected in the current frame to obtain a matching combination set. The matching combination set includes multiple matching combinations, and each matching combination includes the predicted location of the target obstacle and the current observation location. The association cost set is calculated based on the matching combination set, where the association cost corresponds one-to-one with the matching combination; The minimum association cost is identified from the association cost set. If the minimum association cost is less than the preset maximum association distance threshold, the matching combination corresponding to the minimum association cost is taken as the optimal matching combination. The optimal matching combination is removed from multiple current frame detected obstacles and the first frame tracking list respectively to obtain multiple updated detected obstacles and updated first frame tracking list. The multiple updated detected obstacles are taken as multiple current frame detected obstacles, and the updated first frame tracking list is taken as the first frame tracking list. The step of extracting tracking obstacles from the first frame tracking list in sequence is returned until all tracking obstacles in the first frame tracking list are extracted. If the minimum association cost is greater than the maximum association distance threshold, then the matching combination corresponding to the minimum association cost is regarded as an unmatched combination; The best matching combinations and unmatched combinations are summarized separately to obtain the best matching combination set and the unmatched combination set. The new obstacle set and the lost tracking obstacle set are identified from the unmatched combination set. The number of lost tracking obstacles is calculated based on the lost tracking obstacle set and the number of lost tracking obstacles is saved to the tracking list of the first frame to obtain the unupdated tracking list. Obtain the obstacle set at the current observation position based on the optimal matching combination set. Calculate the centroid coordinate set of the current observed obstacles based on the obstacle set at the current observation position. Input the centroid coordinate set of the current observed obstacles into a pre-constructed Kalman filter to obtain the updated tracking dataset. Initialize each new obstacle in the new obstacle set to obtain the initialization parameter set. Update the unupdated tracking list using the updated tracking dataset and the initialization parameter set to obtain the second frame tracking list, which includes multiple updated tracking obstacles.

[0043] It should be explained that the second frame data refers to all detection information obtained by the target robot in performing a dynamic obstacle scan of the current environment at the next sampling time, including at least multiple obstacles detected in the current frame and their corresponding current observation positions. The current observation position set refers to the set composed of all current observation positions. The current observation position refers to the three-dimensional centroid coordinates of the obstacle detected in the current frame in the robot coordinate system in the second frame data. The target obstacle prediction position refers to the three-dimensional centroid coordinates extracted from the obstacle prediction position set that have the same tracking ID as the currently extracted tracked obstacle. The combination of the target obstacle prediction position with multiple obstacles detected in the current frame refers to the operation of combining the target obstacle prediction position with each of the multiple obstacles detected in the current frame in pairs. For example, the predicted position of the target obstacle A is P1 = (2.5m, 3.8m, 0.7m). Multiple obstacles detected in the current frame include {obstacle B, obstacle C, obstacle D}. After pairwise combination of the predicted position of the target obstacle with each of the multiple obstacles detected in the current frame, the matching combination set is {(P1, B), (P1, C), (P1, D)}.

[0044] Understandably, the association cost refers to the Euclidean distance between the predicted position of the target obstacle and the obstacle detected in the current frame. The association cost set refers to the set of association costs calculated for each pair of predicted target obstacle positions and current observation positions in the matching combination set, used to quantify the matching similarity. The minimum association cost is the association cost with the smallest value in the association cost set, used to determine the optimal matching combination. The maximum association distance threshold is a pre-set value used to determine whether the minimum association cost is valid. If the minimum association cost is less than or equal to the maximum association distance threshold, it is considered a valid match; otherwise, it is considered an invalid match. For example, with a maximum association distance threshold of 1 meter, in typical indoor and outdoor navigation scenarios, the maximum displacement of a dynamic obstacle (such as a pedestrian) within two adjacent frame perception periods (usually 0.1 to 0.5 seconds) generally does not exceed 1.0 meter. Setting the maximum association distance threshold to 1.0 meter can cover the inter-frame displacement of most continuously moving obstacles, avoiding trajectory association failure due to rapid obstacle movement. The inter-frame displacement refers to the actual spatial distance moved by the same dynamic obstacle (such as a pedestrian) between two adjacent perceptions (i.e., two adjacent frame point clouds or images). The optimal matching combination refers to the matching combination corresponding to the minimum association cost. Updating detected obstacles refers to the set of current-frame detected obstacles remaining after removing the optimal matching combination from multiple current-frame detected obstacles. Updating the first-frame tracking list refers to the list of tracking obstacles remaining after removing the tracking obstacle corresponding to the optimal matching combination from the first-frame tracking list. Unmatched combinations refer to the matching combination corresponding to the minimum association cost.

[0045] Importantly, the optimal matching combination set refers to the set consisting of all optimal matching combinations. The unmatched combination set refers to the set consisting of all unmatched combinations. The steps for identifying the new obstacle set and the lost tracking obstacle set from the unmatched combination set are as follows: Unmatched combinations in the unmatched combination set containing only the current observation position but no corresponding predicted position are added to the new obstacle set; unmatched combinations in the unmatched combination set containing only the predicted position but no corresponding current observation position are added to the lost tracking obstacle set. The number of losses refers to the number obtained by adding 1 to the original loss count for each tracking obstacle in the lost tracking obstacle set, and this number of losses is written back to the field of the unupdated tracking list. The unupdated tracking list refers to the list that has completed the update of the number of losses. The current observation position obstacle set refers to the set of all current observation positions extracted from the optimal matching combination set. The current observation obstacle centroid coordinate set refers to the set consisting of the centroid coordinates of all current observation obstacles. The steps for obtaining the current observation obstacle centroid coordinates are the same as the steps for obtaining the obstacle centroid coordinates, and will not be repeated here. The updated tracking dataset refers to the set of tracker parameters output by the Kalman filter after inputting the current set of observed obstacle centroid coordinates into it, and then correcting for state and updating covariance. Initializing each new obstacle in the new obstacle set involves assigning a globally unique new ID to each obstacle, using its current observed position as the initial state vector, setting the initial velocity vector to zero, and constructing a corresponding Kalman filter based on the obstacle's size and initial covariance matrix (which are the initialization parameters in the Kalman filter). The initialized obstacle set refers to the set of new obstacles that has already been initialized. The initialized parameter set refers to the set of Kalman filters and IDs that correspond one-to-one with the initialized obstacle set. Updating refers to correcting the state of the best-matching tracker combination in the unupdated tracking list using the updated tracking dataset, and appending the initialized parameter set to the unupdated tracking list to form a complete second-frame tracking list. Updated tracking obstacles refer to the tracking obstacles in the second-frame tracking list that have been corrected by the Kalman filter.

[0046] S5. Based on the second frame tracking list, obtain the destination coordinates, obstacle safety function set, and complete obstacle trajectory set. Based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set, confirm that the mobile robot has been completed.

[0047] Specifically, obtaining the destination coordinates, obstacle safety function set, and complete obstacle track set based on the second frame tracking list includes: The latest state vector is obtained by sequentially extracting the updated tracking obstacles from the multiple updated tracking obstacles in the tracking list of the second frame; Once the prediction time period set is identified, perform the following operations on each prediction time period in the set: Calculate the next moment's predicted state vector based on the predicted time period and the latest state vector, obtain the next moment's predicted obstacle position based on the next moment's predicted state vector, summarize the next moment's predicted obstacle positions, and obtain the next moment's predicted obstacle position set corresponding to the predicted time period set. Based on the extracted updated tracking obstacles, obtain the historical obstacle location set. Sort the historical obstacle location set and the next moment's predicted obstacle location set in chronological order to obtain the historical obstacle location sequence and the predicted obstacle location sequence. Historical obstacle trajectories and predicted obstacle trajectories are generated based on historical obstacle location sequences and predicted obstacle location sequences. The historical obstacle trajectories and predicted obstacle trajectories are then spliced ​​together to obtain complete obstacle trajectories. The latest predicted coordinates of obstacles are determined based on the complete obstacle trajectory. The radius of the obstacle is obtained based on the extracted updated tracking obstacle. The current coordinates of the target robot and the destination coordinates are obtained. Based on the latest predicted obstacle coordinates, obstacle radius, and the current coordinates of the target robot, an obstacle safety function is constructed. The obstacle safety functions and the complete obstacle trajectory are then summarized to obtain the obstacle safety function set and the complete obstacle trajectory set.

[0048] It should be explained that the latest state vector refers to the Kalman filter output from the extracted and updated tracked obstacles, containing information on the current 3D position, 3D linear velocity, and 3D acceleration. The prediction time period set refers to a pre-defined set of discrete time sequences used to predict the future state of obstacles segment by segment. The step of calculating the next-moment predicted state vector based on the prediction time period and the latest state vector is as follows: within the prediction time period, the pre-constructed state transition matrix and the latest state vector are multiplied to obtain the next-moment predicted state vector. The state transition matrix refers to a 6×6 or 9×9 constant velocity transition matrix corresponding to a uniform motion model, used to linearly map the current latest state vector to the predicted state at the next moment. The next-moment obstacle prediction position refers to the 3D position extracted from the next-moment predicted state vector. The next-moment obstacle prediction position set refers to the set of next-moment obstacle prediction positions obtained after performing predictions on all prediction time periods in the prediction time period set. The historical obstacle position set refers to the set of observed 3D positions stored within the updated tracked obstacles, accumulated from previous frames. The historical obstacle position sequence refers to a temporal queue formed by sorting the historical obstacle position sets in ascending order of timestamps. The predicted obstacle position sequence refers to a temporal queue formed by sorting the predicted obstacle positions for the next moment according to the chronological order of the predicted time period. The historical obstacle trajectory is a broken line formed by connecting the historical obstacle position sequences in three-dimensional space in chronological order. The predicted obstacle trajectory is also a broken line formed by connecting the predicted obstacle position sequences in three-dimensional space in chronological order. The complete obstacle trajectory is a continuous spatial curve formed by splicing the end of the historical obstacle trajectory with the beginning of the predicted obstacle trajectory, used to describe the complete movement path of the obstacle in the past and future. The latest obstacle prediction coordinates are the coordinates of the end point of the complete obstacle trajectory. The step of obtaining the obstacle radius based on the extracted updated tracking obstacles is as follows: obtain the obstacle size corresponding to the updated tracking obstacle, and take half of the maximum value of the obstacle size as the obstacle radius. The target robot's current coordinates refer to the three-dimensional position output by the real-time positioning system at the moment of prediction. The destination coordinates are three-dimensional coordinates pre-planned by humans. The obstacle safety function set refers to the set of obstacle safety functions constructed one by one for all updated tracking obstacles. A complete obstacle track set refers to the collection of complete obstacle tracks corresponding to all updated tracking obstacles.

[0049] Specifically, the step of confirming the completed mobile robot based on the destination coordinates, the obstacle safety function set, and the complete obstacle trajectory set includes: A two-dimensional grid map is constructed based on the current environment. The globally optimal movement path is then constructed based on the two-dimensional grid map, the complete obstacle track set, the current coordinates of the target robot, and the destination coordinates. Construct constraints, input the obstacle safety function set and constraints into the pre-constructed objective function, and obtain the objective function to be optimized; The objective function to be optimized is optimized to obtain a robot control command sequence. The robot control command sequence includes multiple robot control commands, and each robot control command corresponds one-to-one with the updated tracking obstacle. The robot control commands include linear velocity and angular velocity. The target robot is moved according to the robot control command sequence and the global optimal movement path. The movement coordinates are obtained. When the movement coordinates are equal to the destination coordinates, the target robot whose movement coordinates are equal to the destination coordinates is regarded as the robot whose movement has been completed.

[0050] It should be explained that constructing the globally optimal movement path based on the 2D grid map, the complete obstacle track set, the target robot's current coordinates, and the destination coordinates refers to using a path planning algorithm (such as the A* algorithm) to plan the movement path based on the 2D grid map, the complete obstacle track set, the target robot's current coordinates, and the destination coordinates, thus obtaining the globally optimal movement path. The constraints refer to a set of nonlinear inequalities generated by the obstacle safety function set, requiring that the Euclidean distance between the robot and the predicted position of any obstacle in the prediction time domain is not less than the sum of the corresponding obstacle radius and the safety margin, while the linear velocity and angular velocity satisfy the robot's kinematics. The objective function refers to a pre-constructed quadratic performance index, including a position error term, a velocity smoothing term, and a control energy term, used to quantify the path tracking quality. The objective function to be optimized refers to the nonlinear programming expression formed by coupling the objective function and the constraints through Lagrange multipliers, a function for the solver to optimize. Optimizing the objective function to be optimized refers to using sequential quadratic programming to optimize the objective function to be optimized, minimizing the objective function under the constraints, and outputting a discrete-time sequence of robot control commands. Linear velocity refers to the translational speed along the horizontal axis of the robot body in the robot control command. Angular velocity refers to the rotational speed about the vertical axis of the robot body in the robot control command. Movement coordinates refer to the latest three-dimensional position output in real time by the positioning system after the target robot completes one step movement according to the robot control command. Constructing a two-dimensional grid map based on the current environment refers to using simultaneous localization and mapping (SLAM) technology based on LiDAR to construct a two-dimensional grid map of the environment in real time through scan matching, pose map optimization, and grid occupancy probability updates. Constructing a two-dimensional grid map is existing technology and will not be elaborated further here.

[0051] In detail, the obstacle safety function is represented as follows: in, Represents the obstacle safety function. Indicates the current coordinates of the target robot. Indicates the latest predicted obstacle coordinates. This indicates the preset robot radius. Indicates the radius of the obstacle. This indicates the preset safety margin. This represents the Euclidean norm.

[0052] It should be explained that the robot radius refers to a pre-set value used to simplify the space occupied by the robot into a circular protective domain. The robot radius can be obtained from the target robot's design drawings. The safety margin refers to a pre-set additional distance that must be maintained between the robot and obstacles to compensate for positioning errors, control lag, and sensor noise, ensuring that collisions will not occur even in the presence of uncertainties. For example, the safety margin is 0.1 meters. This invention sets the safety margin to 0.1 meters to match the voxel size with the safety margin, ensuring consistency and coordination between obstacle representation and safety constraints in path planning and control.

[0053] S6. Based on the completed mobile robot, complete the robot's environmental perception based on multi-sensor fusion.

[0054] Importantly, the environmental perception achieved using the completed mobile robot demonstrates that the robot can accurately avoid obstacles and reach its destination in complex environments, realizing effective perception between the robot and its environment and improving the efficiency and success rate of task execution. Simultaneously, the entire process fully leverages the advantages of multi-sensor fusion, achieving more accurate environmental perception and more intelligent decision-making through data complementarity and collaborative processing from different sensors, thus verifying the effectiveness and practicality of multi-sensor fusion technology in robot environmental perception.

[0055] To address the problems described in the background art, this invention identifies a target robot and the current environment. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. This invention clearly defines the target robot and its multiple sensors, providing a prerequisite for subsequent environmental detection and data acquisition using the advantages of different sensors. Different sensors can perceive environmental information from multiple dimensions, receive robot environmental perception commands, and use the target robot and these commands to detect the current environment, obtaining a first frame of data. This first frame includes a point cloud coordinate set. The point cloud data of this invention can accurately describe the 3D spatial position and shape information of objects in the environment, providing rich data support for subsequent operations such as building a ground plane model and identifying obstacles. An optimal ground plane model is constructed based on the point cloud coordinate set, and a point cloud coordinate set of non-ground obstacles is obtained based on this optimal ground plane model. This invention's construction of the optimal ground plane model can distinguish the ground portion of the environment from other objects, achieving preliminary environmental segmentation, which is helpful... This invention simplifies subsequent obstacle detection and analysis processes, improving processing efficiency. Based on a non-ground obstacle point cloud coordinate set, it predicts obstacle location sets. Using the target robot and the predicted obstacle location sets, it scans the current environment for obstacles, obtaining a second-frame tracking list. By predicting obstacle location sets, this invention allows the robot to understand the possible movement directions and ranges of obstacles in advance, thus avoiding potentially dangerous areas during path planning and improving robot safety and operational efficiency. Based on the second-frame tracking list, it obtains destination coordinates, obstacle safety function sets, and a complete obstacle trajectory set. Based on these, it confirms the completed mobile robot. This invention, by acquiring destination coordinates, obstacle safety function sets, and a complete obstacle trajectory set, comprehensively considers factors such as the robot's target position, obstacle safety, and obstacle movement trajectories, providing more comprehensive information for the robot's path planning and action decisions. Based on the completed mobile robot, it achieves robot environmental perception based on multi-sensor fusion. Therefore, this invention can improve the robot's navigation efficiency, obstacle avoidance accuracy, and autonomous interactive intelligence in complex dynamic environments.

[0056] like Figure 2 The diagram shown is a functional block diagram of a robot environmental perception system based on multi-sensor fusion provided in an embodiment of the present invention.

[0057] The robot environment perception system 100 based on multi-sensor fusion described in this invention can be installed in an electronic device. Depending on the functions implemented, the robot environment perception system 100 based on multi-sensor fusion may include a sensor configuration module 101, a robot environment perception module 102, an obstacle extraction module 103, and a robot perception execution module 104. The module described in this invention can also be called a unit, which refers to a series of computer program segments that can be executed by the processor of an electronic device and can perform a fixed function, and which are stored in the memory of the electronic device. The sensor configuration module 101 is used to identify the target robot and the current environment. The target robot includes a three-dimensional lidar, an inertial measurement unit, and a vision sensor. The robot environment perception module 102 is used to receive robot environment perception instructions, use the target robot and robot environment perception instructions to detect the current environment, and obtain a first frame of data. The first frame of data includes a point cloud coordinate set, an optimal ground plane model is constructed based on the point cloud coordinate set, and a non-ground obstacle point cloud coordinate set is obtained based on the optimal ground plane model. The obstacle extraction module 103 is used to predict the obstacle prediction position set based on the non-ground obstacle point cloud coordinate set, and use the target robot and the obstacle prediction position set to scan the current environment for obstacles to obtain the second frame tracking list. The robot perception and execution module 104 is used to obtain the destination coordinates, obstacle safety function set and complete obstacle trajectory set based on the second frame tracking list, confirm the completed mobile robot based on the destination coordinates, obstacle safety function set and complete obstacle trajectory set, and complete the robot environment perception based on the completed mobile robot.

[0058] In detail, the modules in the robot environment perception system 100 based on multi-sensor fusion described in this embodiment of the invention employ the same methods as described above during use. Figure 1 The method uses the same techniques as the multi-sensor fusion-based robot environmental perception method described in the article and can produce the same technical effects, so it will not be repeated here.

[0059] like Figure 3 The diagram shown is a structural schematic of an electronic device for implementing a robot environmental perception method based on multi-sensor fusion, according to an embodiment of the present invention.

[0060] The electronic device 1 may include a processor 10, a memory 11 and a bus 12, and may also include a computer program stored in the memory 11 and executable on the processor 10, such as a robot environmental perception method program based on multi-sensor fusion.

[0061] The memory 11 includes at least one type of readable storage medium, such as flash memory, portable hard drive, multimedia card, card-type memory (e.g., SD or DX memory), magnetic memory, magnetic disk, optical disk, etc. In some embodiments, the memory 11 can be an internal storage unit of the electronic device 1, such as a portable hard drive. In other embodiments, the memory 11 can be an external storage device of the electronic device 1, such as a plug-in portable hard drive, smart media card (SMC), secure digital card (SD), flash card, etc., equipped on the electronic device 1. Furthermore, the memory 11 includes both internal storage units and external storage devices of the electronic device 1. The memory 11 can be used not only to store application software and various types of data installed on the electronic device 1, such as code for a robot environmental perception method program based on multi-sensor fusion, but also to temporarily store data that has been output or will be output.

[0062] In some embodiments, the processor 10 may be composed of integrated circuits, such as a single packaged integrated circuit or multiple integrated circuits with the same or different functions, including combinations of one or more central processing units (CPUs), microprocessors, digital processing chips, graphics processors, and various control chips. The processor 10 is the control unit of the electronic device, connecting various components of the entire electronic device through various interfaces and lines. It executes programs or modules stored in the memory 11 (e.g., a robot environmental perception method program based on multi-sensor fusion) and calls data stored in the memory 11 to perform various functions of the electronic device 1 and process data.

[0063] The bus 12 can be a peripheral component interconnect (PCI) bus or an extended industry standard architecture (EISA) bus, etc. The bus 12 can be divided into an address bus, a data bus, a control bus, etc. The bus 12 is configured to realize the connection and communication between the memory 11 and at least one processor 10, etc.

[0064] Figure 3 Only electronic devices with components are shown; those skilled in the art will understand that... Figure 3The structure shown does not constitute a limitation on the electronic device 1, and may include fewer or more components than shown, or combine certain components, or have different component arrangements.

[0065] For example, although not shown, the electronic device 1 may also include a power supply (such as a battery) to power the various components. Preferably, the power supply can be logically connected to the at least one processor 10 through a power management device, thereby enabling functions such as charging management, discharging management, and power consumption management. The power supply may also include one or more DC or AC power supplies, recharging devices, power fault detection circuits, power converters or inverters, power status indicators, and other arbitrary components. The electronic device 1 may also include various sensors, Bluetooth modules, Wi-Fi modules, etc., which will not be described in detail here.

[0066] Furthermore, the electronic device 1 may also include a network interface. Optionally, the network interface may include a wired interface and / or a wireless interface (such as a Wi-Fi interface, a Bluetooth interface, etc.), which is typically used to establish communication connections between the electronic device 1 and other electronic devices.

[0067] Optionally, the electronic device 1 may further include a user interface, which may be a display, an input unit (such as a keyboard), and optionally, a standard wired interface or a wireless interface. Optionally, in some embodiments, the display may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, or an OLED (Organic Light-Emitting Diode) touchscreen, etc. The display may also be appropriately referred to as a screen or display unit, used to display information processed in the electronic device 1 and to display a visual user interface.

[0068] The robot environment perception method program based on multi-sensor fusion stored in the memory 11 of the electronic device 1 is a combination of multiple instructions. When run in the processor 10, it can achieve the following: The target robot and the current environment were identified. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. Receive robot environment perception instructions, use the target robot and robot environment perception instructions to detect the current environment, and obtain the first frame of data, which includes a point cloud coordinate set; Construct an optimal ground plane model based on the point cloud coordinate set, and obtain the point cloud coordinate set of non-ground obstacles based on the optimal ground plane model; Based on the coordinate set of non-ground obstacle point cloud, the predicted obstacle position set is predicted. The target robot and the predicted obstacle position set are used to scan the current environment for obstacles, and the second frame tracking list is obtained. Based on the second frame tracking list, obtain the destination coordinates, obstacle safety function set, and complete obstacle trajectory set. Based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set, confirm that the mobile robot has been completed. Based on the completed mobile robot, we can achieve robot environmental perception based on multi-sensor fusion.

[0069] Specifically, the processor 10's implementation method for the above instructions can be found in [reference needed]. Figures 1 to 3 The descriptions of the relevant steps in the corresponding embodiments are not repeated here.

[0070] Furthermore, if the modules / units integrated in the electronic device 1 are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. The computer-readable storage medium can be volatile or non-volatile. For example, the computer-readable medium may include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, or a read-only memory (ROM).

[0071] The present invention also provides a computer-readable storage medium storing a computer program, which, when executed by a processor of an electronic device, can perform the following: The target robot and the current environment were identified. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. Receive robot environment perception instructions, use the target robot and robot environment perception instructions to detect the current environment, and obtain the first frame of data, which includes a point cloud coordinate set; Construct an optimal ground plane model based on the point cloud coordinate set, and obtain the point cloud coordinate set of non-ground obstacles based on the optimal ground plane model; Based on the coordinate set of non-ground obstacle point cloud, the predicted obstacle position set is predicted. The target robot and the predicted obstacle position set are used to scan the current environment for obstacles, and the second frame tracking list is obtained. Based on the second frame tracking list, obtain the destination coordinates, obstacle safety function set, and complete obstacle trajectory set. Based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set, confirm that the mobile robot has been completed. Based on the completed mobile robot, we can achieve robot environmental perception based on multi-sensor fusion.

[0072] In the embodiments provided by this invention, it should be understood that the disclosed devices, systems, and methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative, and actual implementations may have other classification methods.

[0073] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0074] Furthermore, the functional modules in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or in the form of hardware plus software functional modules.

[0075] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention.

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

Claims

1. A robot environment perception method based on multi-sensor fusion, characterized in that, The method includes: The target robot and the current environment were identified. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. Receive robot environment perception instructions, use the target robot and robot environment perception instructions to detect the current environment, and obtain the first frame of data, which includes a point cloud coordinate set; Construct an optimal ground plane model based on the point cloud coordinate set, and obtain the point cloud coordinate set of non-ground obstacles based on the optimal ground plane model; Based on the coordinate set of non-ground obstacle point cloud, the predicted obstacle position set is predicted. The target robot and the predicted obstacle position set are used to scan the current environment for obstacles, and the second frame tracking list is obtained. Based on the second frame tracking list, obtain the destination coordinates, obstacle safety function set, and complete obstacle trajectory set. Based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set, confirm that the mobile robot has been completed. Based on the completed mobile robot, we can achieve robot environmental perception based on multi-sensor fusion.

2. The robot environment perception method based on multi-sensor fusion as described in claim 1, characterized in that, The construction of the optimal ground plane model based on the point cloud coordinate set includes: Obtain the maximum x-coordinate value, maximum y-coordinate value, and maximum vertical coordinate value from the point cloud coordinate set, and construct a three-dimensional point cloud space based on the maximum x-coordinate value, maximum y-coordinate value, and maximum vertical coordinate value; The three-dimensional point cloud space is divided using a preset voxel size to obtain a voxel set. Point cloud coordinates are extracted sequentially from the point cloud coordinate set. Based on the extracted point cloud coordinates and voxel size, the coordinate voxel index is calculated. The target voxel is identified in the voxel set according to the coordinate voxel index. The extracted point cloud coordinates are assigned to the target voxel to obtain the assigned voxel. Summarize the assigned voxels to obtain the assigned voxel set, wherein the assigned voxel set includes multiple assigned voxels, and each assigned voxel includes zero or one or more point cloud coordinates; Extract allocated voxels sequentially from the allocated voxel set, and determine whether point cloud coordinates exist in the extracted allocated voxels; If point cloud coordinates exist in the extracted assigned voxels, obtain the voxel point cloud coordinate set of the extracted assigned voxels, calculate the geometric centroid coordinates of the voxel point cloud coordinate set, and replace the voxel point cloud coordinate set with the geometric centroid coordinates to obtain the updated point cloud coordinates. The point cloud coordinates are summarized and updated to obtain the downsampled point cloud coordinate set. The optimal ground plane model is then constructed based on the downsampled point cloud coordinate set.

3. The robot environment perception method based on multi-sensor fusion as described in claim 2, characterized in that, The construction of the optimal ground plane model based on the downsampled point cloud coordinate set includes: Random point cloud coordinate sets are extracted sequentially from the downsampled point cloud coordinate set, and then removed from the downsampled point cloud coordinate set to obtain the remaining point cloud coordinate set. A planar model is constructed based on the extracted random point cloud coordinate set. The planar distance between each remaining point cloud coordinate in the remaining point cloud coordinate set and the planar model is calculated to obtain a planar distance set. Planar distances are extracted sequentially from the planar distance set. If the extracted planar distance is less than a preset distance threshold, the remaining point cloud coordinates corresponding to the extracted planar distance are taken as the point cloud coordinates in the plane. Summarize the point cloud coordinates in the plane to obtain the point cloud coordinate set in the plane corresponding to the plane distance set, and calculate the number of point cloud coordinates in the plane based on the point cloud coordinate set in the plane. The point cloud coordinate set in the plane is removed from the remaining point cloud coordinate set to obtain the updated point cloud coordinate set, which is then used as the downsampled point cloud coordinate set. Return to the step of sequentially and randomly extracting random point cloud coordinate sets from the downsampled point cloud coordinate set; obtain the number of times the random point cloud coordinate sets are sequentially and randomly extracted from the downsampled point cloud coordinate set until the number of executions equals the preset number of normal executions; The number of point cloud coordinates in the plane is summarized to obtain the set of point cloud coordinates in the plane. The plane model corresponding to the largest number of point cloud coordinates in the set is taken as the best ground plane model.

4. The robot environment perception method based on multi-sensor fusion as described in claim 3, characterized in that, The acquisition of the non-ground obstacle point cloud coordinate set based on the optimal ground plane model includes: Remove the in-plane point cloud coordinate set corresponding to the optimal ground plane model from the downsampled point cloud coordinate set to obtain the non-ground point cloud coordinate set. Extract non-ground point cloud coordinates from the non-ground point cloud coordinate set in sequence. Remove the extracted non-ground point cloud coordinates from the non-ground point cloud coordinate set to obtain the remaining non-ground point cloud coordinate set. Search for the nearest neighbor coordinate set from the remaining non-ground point cloud coordinate set. Calculate the nearest neighbor distance between the extracted non-ground point cloud coordinates and each nearest neighbor coordinate in the nearest neighbor coordinate set to obtain the nearest neighbor distance set, and calculate the nearest average distance based on the nearest neighbor distance set; Summarize the nearest average distances to obtain the nearest average distance set. Calculate the mean and standard deviation of the mean distances based on the nearest average distance set. Calculate the standard deviation multiple threshold based on the mean and standard deviation of the mean distances. The nearest average distance is extracted sequentially from the nearest average distance set. If the extracted nearest average distance is greater than the standard deviation multiple threshold, the non-ground point cloud coordinates corresponding to the extracted nearest average distance are taken as outlier noise coordinates. The coordinates of outliers are summarized to obtain the outlier coordinate set. The outlier coordinate set is then removed from the non-ground point cloud coordinate set to obtain the non-ground obstacle point cloud coordinate set.

5. The robot environment perception method based on multi-sensor fusion as described in claim 4, characterized in that, The method of predicting obstacle prediction locations based on non-ground obstacle point cloud coordinates includes: Clustering is performed on the non-ground obstacle point cloud coordinate set to obtain obstacle clusters, wherein the obstacle clusters include multiple obstacle clusters, and each obstacle cluster includes multiple non-ground obstacle point cloud coordinates; Obstacle clusters are extracted sequentially from the obstacle cluster set. The three-dimensional bounding box parameters and obstacle centroid coordinates are determined based on the point cloud coordinates of multiple non-ground obstacles in the obstacle cluster. The three-dimensional bounding box parameters include: maximum horizontal value, minimum horizontal value, maximum vertical value, minimum vertical value, maximum vertical value, and minimum vertical value. A 3D bounding box is constructed using 3D bounding box parameters, and the obstacle dimensions are calculated based on the 3D bounding box. The obstacle dimensions include: obstacle length, obstacle width, and obstacle height. Based on the obstacle's center coordinates and size, the obstacle's static characteristic parameters are identified, and the obstacle's static characteristic parameters are summarized to obtain the obstacle's static characteristic parameter set; An obstacle tracker set is created based on the set of static feature parameters of obstacles, wherein each obstacle tracker corresponds one-to-one with the static feature parameters of obstacles; The obstacle tracker set is stored to obtain the first frame tracking list, which includes multiple tracked obstacles; For each tracked obstacle in the first frame tracking list, position prediction is performed to obtain the obstacle prediction position set.

6. The robot environment perception method based on multi-sensor fusion as described in claim 5, characterized in that, The process of scanning the current environment using the target robot and the predicted location set of obstacles to obtain a second frame tracking list includes: The target robot performs a dynamic obstacle scan of the current environment to obtain a second frame of data, which includes multiple obstacles detected in the current frame. The current observation location set is determined from the second frame of data, where each obstacle detected in the current frame corresponds one-to-one with the current observation location; The tracked obstacles are extracted sequentially from the tracking list of the first frame, and the predicted position of the target obstacle is confirmed from the set of predicted obstacle positions based on the extracted tracked obstacles. The predicted location of the target obstacle is combined with multiple obstacles detected in the current frame to obtain a matching combination set. The matching combination set includes multiple matching combinations, and each matching combination includes the predicted location of the target obstacle and the current observation location. The association cost set is calculated based on the matching combination set, where the association cost corresponds one-to-one with the matching combination; The minimum association cost is identified from the association cost set. If the minimum association cost is less than the preset maximum association distance threshold, the matching combination corresponding to the minimum association cost is taken as the optimal matching combination. The optimal matching combination is removed from multiple current frame detected obstacles and the first frame tracking list respectively to obtain multiple updated detected obstacles and updated first frame tracking list. The multiple updated detected obstacles are taken as multiple current frame detected obstacles, and the updated first frame tracking list is taken as the first frame tracking list. The step of extracting tracking obstacles from the first frame tracking list in sequence is returned until all tracking obstacles in the first frame tracking list are extracted. If the minimum association cost is greater than the maximum association distance threshold, then the matching combination corresponding to the minimum association cost is regarded as an unmatched combination; The best matching combinations and unmatched combinations are summarized separately to obtain the best matching combination set and the unmatched combination set. The new obstacle set and the lost tracking obstacle set are identified from the unmatched combination set. The number of lost tracking obstacles is calculated based on the lost tracking obstacle set and the number of lost tracking obstacles is saved to the tracking list of the first frame to obtain the unupdated tracking list. Obtain the obstacle set at the current observation position based on the optimal matching combination set. Calculate the centroid coordinate set of the current observed obstacles based on the obstacle set at the current observation position. Input the centroid coordinate set of the current observed obstacles into a pre-constructed Kalman filter to obtain the updated tracking dataset. Initialize each new obstacle in the new obstacle set to obtain the initialization parameter set. Update the unupdated tracking list using the updated tracking dataset and the initialization parameter set to obtain the second frame tracking list, which includes multiple updated tracking obstacles.

7. The robot environment perception method based on multi-sensor fusion as described in claim 6, characterized in that, The process of obtaining the destination coordinates, obstacle safety function set, and complete obstacle track set based on the second frame tracking list includes: The latest state vector is obtained by sequentially extracting the updated tracking obstacles from the multiple updated tracking obstacles in the tracking list of the second frame; Once the prediction time period set is identified, perform the following operations on each prediction time period in the set: Calculate the next moment's predicted state vector based on the predicted time period and the latest state vector, obtain the next moment's predicted obstacle position based on the next moment's predicted state vector, summarize the next moment's predicted obstacle positions, and obtain the next moment's predicted obstacle position set corresponding to the predicted time period set. Based on the extracted updated tracking obstacles, obtain the historical obstacle location set. Sort the historical obstacle location set and the next moment's predicted obstacle location set in chronological order to obtain the historical obstacle location sequence and the predicted obstacle location sequence. Historical obstacle trajectory and predicted obstacle trajectory are generated based on historical obstacle location sequence and predicted obstacle location sequence. The historical obstacle trajectory and predicted obstacle trajectory are then spliced ​​together to obtain the complete obstacle trajectory. The latest predicted coordinates of obstacles are determined based on the complete obstacle trajectory. The radius of the obstacle is obtained based on the extracted updated tracking obstacle. The current coordinates of the target robot and the destination coordinates are obtained. Based on the latest predicted obstacle coordinates, obstacle radius, and the current coordinates of the target robot, an obstacle safety function is constructed. The obstacle safety functions and the complete obstacle trajectory are then summarized to obtain the obstacle safety function set and the complete obstacle trajectory set.

8. The robot environment perception method based on multi-sensor fusion as described in claim 7, characterized in that, The process of confirming the completion of the mobile robot based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set includes: A two-dimensional grid map is constructed based on the current environment. The globally optimal movement path is then constructed based on the two-dimensional grid map, the complete obstacle track set, the current coordinates of the target robot, and the destination coordinates. Construct constraints, input the obstacle safety function set and constraints into the pre-constructed objective function, and obtain the objective function to be optimized; The objective function to be optimized is optimized to obtain a robot control command sequence. The robot control command sequence includes multiple robot control commands, and each robot control command corresponds one-to-one with the updated tracking obstacle. The robot control commands include linear velocity and angular velocity. The target robot is moved according to the robot control command sequence and the global optimal movement path. The movement coordinates are obtained. When the movement coordinates are equal to the destination coordinates, the target robot whose movement coordinates are equal to the destination coordinates is regarded as the robot whose movement has been completed.

9. The robot environment perception method based on multi-sensor fusion as described in claim 8, characterized in that, The obstacle safety function is represented as follows: in, Represents the obstacle safety function. Indicates the current coordinates of the target robot. Indicates the latest predicted obstacle coordinates. This indicates the preset robot radius. Indicates the radius of the obstacle. This indicates the preset safety margin. This represents the Euclidean norm.

10. A robot environmental perception system based on multi-sensor fusion, characterized in that, The system includes: The sensor configuration module is used to identify the target robot and the current environment. The target robot includes a 3D LiDAR, an inertial measurement unit, and a vision sensor. The robot environment perception module is used to receive robot environment perception instructions, use the target robot and robot environment perception instructions to detect the current environment, and obtain the first frame of data. The first frame of data includes a point cloud coordinate set, an optimal ground plane model is constructed based on the point cloud coordinate set, and a point cloud coordinate set of non-ground obstacles is obtained based on the optimal ground plane model. The obstacle extraction module is used to predict the predicted location set of obstacles based on the coordinate set of non-ground obstacle point cloud. The target robot and the predicted location set of obstacles are used to scan the current environment for obstacles and obtain the second frame tracking list. The robot perception and execution module is used to obtain the destination coordinates, obstacle safety function set, and complete obstacle trajectory set based on the second frame tracking list. Based on the destination coordinates, obstacle safety function set, and complete obstacle trajectory set, it confirms that the mobile robot has completed its operation and performs robot environmental perception based on multi-sensor fusion.

Images