Mobile robot sensing and navigation integrated super-dynamic obstacle avoidance method and device in unknown environment

Abstract

The application discloses a mobile robot sensing and guiding integrated super-dynamic obstacle avoidance method and device in an unknown environment. The method comprises the following steps: a mobile robot acquires unknown environment obstacle point cloud information through a laser radar; a clustering method is used to identify a dynamic obstacle; a history motion trajectory of the dynamic obstacle is recorded, including coordinate and time information; a motion model is constructed by using the history motion trajectory, and a future dynamic obstacle trajectory for a period of time is predicted; a mobile obstacle avoidance trajectory of the mobile robot is planned according to the future dynamic obstacle trajectory, and dynamic obstacle avoidance is realized. The application can realize mobile robot obstacle avoidance planning in a dynamic scene, so as to improve the efficiency and safety of robot path planning.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot obstacle avoidance technology, and particularly relates to a mobile robot sensing and guidance integrated super-dynamic obstacle avoidance method and device in an unknown environment. BACKGROUND

[0002] In the research of mobile robot related technology, path planning is an important part. Path planning refers to determining the optimal path of a robot moving in space under the given environment and task requirements. The goal of path planning is to make the robot reach the target point from the starting point, and to satisfy various constraint conditions on the entire path, and to optimize certain performance indicators such as shortest path, minimum time, minimum energy consumption, etc.

[0003] Identifying and predicting dynamic obstacles in an unknown environment and achieving dynamic obstacle avoidance can make the movement of the robot more safe and efficient. In robot navigation, dynamic obstacles around the robot are perceived and identified in real time, and the position, velocity and other parameters of the obstacles are considered, as well as the map information of the surrounding environment, the future movement of the obstacles is predicted, and the planning algorithm can be combined with the movement trajectory of the obstacles to re-plan, reduce braking and collision in dynamic scenes, and improve stability and efficiency.

[0004] In an unknown scene containing static obstacles, current path planning algorithms can effectively generate driving trajectories, complete static obstacle avoidance and planning from the starting point to the end point, but they ignore the unknown dynamic obstacles that may exist in the scene. Considering the fast movement speed of dynamic obstacles, the current algorithm may fail to handle dynamic situations. Specifically, for dynamic obstacles that gradually approach the robot, the current planning algorithm does not consider dynamic prediction and treats them as static obstacles, generating a planning path based on the visible obstacle position at the last moment. At the next moment, the obstacle position changes and approaches the robot, and since the robot cannot quickly and timely re-plan, braking or collision occurs; if the obstacle gradually moves away from the robot, the current planning algorithm tends to generate a path in front of the obstacle position. As the dynamic obstacle moves forward, the robot needs to constantly re-plan, consuming a lot of time and being extremely inefficient; for dynamic obstacles with sudden changes in motion state, since the real-time re-planning of the current algorithm is limited, the robot collides with the dynamic obstacle, resulting in failure of obstacle avoidance planning, etc.

[0005] Some research has begun to try to use neural networks to predict the trajectory of dynamic obstacles, but neural network training is based on known data sets, and when dealing with unknown dynamic obstacles, the generalization is limited and the effect is not ideal. SUMMARY

[0006] Therefore, the application provides a mobile robot sensing and guiding integrated super dynamic obstacle avoidance method in an unknown environment, which does not depend on a known data set and can obtain the trajectory of an unknown dynamic obstacle, thereby realizing obstacle avoidance.

[0007] The application is implemented as follows:

[0008] The application provides a mobile robot sensing and guiding integrated super dynamic obstacle avoidance method in an unknown environment, which includes the following steps:

[0009] Step one, the obstacle information is unknown, and the mobile robot obtains point cloud data containing obstacles in a time period T in the environment through a laser radar;

[0010] Step two, the point cloud data is parsed to obtain the coordinates and timestamp information of the obstacles;

[0011] Step three, the coordinate data of the obstacles is clustered for the first time to obtain the cluster center points of the obstacles, and the corresponding timestamp information is combined to form a three-tuple data ; is the two-dimensional coordinate of the cluster center point, t is the time;

[0012] Step four, the two-dimensional coordinate of the cluster center point of each obstacle is clustered for the second time to obtain an obstacle cluster; the obstacles in the same cluster are considered to belong to the same obstacle;

[0013] Step five, for each obstacle cluster, the distance between the cluster center points is used to identify a dynamic obstacle;

[0014] Step six, the two-dimensional coordinate of the cluster center point of the identified dynamic obstacle is used to model the motion trajectory of the dynamic obstacle, and the model is used to predict the future trajectory of the dynamic obstacle after the time period T;

[0015] Step seven, according to the future trajectory of the dynamic obstacle, the moving obstacle avoidance trajectory of the mobile robot is planned to realize dynamic obstacle avoidance.

[0016] Preferably, in step seven, when the moving obstacle avoidance trajectory of the mobile robot is planned, the distance constraint between the path points corresponding to the same time of the mobile robot and the predicted trajectory of the dynamic obstacle is greater than a given safety distance.

[0017] Preferably, the clustering in steps three and four uses a density-based clustering method DBSCAN with noise.

[0018] Preferably, after obtaining the point cloud data in step one, further inflation processing is performed.

[0019] Preferably, in step five, the distance uses the Euclidean distance of the two-dimensional coordinates in the cluster center points.

[0020] Preferably, in step five, the dynamic obstacle is identified by the distance between the cluster center points as:

[0021] If there are two points in the same obstacle cluster with a distance greater than the set distance threshold, the obstacle cluster is considered to correspond to a dynamic obstacle.

[0022] Preferably, in step six, the dynamic obstacle motion trajectory is modeled using the two-dimensional coordinates of the identified cluster center points of the dynamic obstacle as:

[0023] A polynomial of order is used as the motion trajectory model, and the two-dimensional coordinates of the identified cluster center points of the dynamic obstacle are used for motion trajectory fitting; the motion trajectory model is: n

[0024]

[0025]

[0026] wherein, is the fitting curve of and x t is the fitting curve of and is the fitting curve of and t and is the coefficient of the fitting curve.

[0027] The application also provides a mobile robot sensing and guidance integrated super dynamic obstacle avoidance device in an unknown environment, which executes the above-mentioned mobile robot sensing and guidance integrated super dynamic obstacle avoidance method in an unknown environment; the device comprises: a point cloud data acquisition and analysis module, a dynamic obstacle identification module, a dynamic obstacle trajectory fitting module, a dynamic obstacle trajectory prediction module, and a dynamic obstacle avoidance planning module.

[0028] The point cloud data acquisition and analysis module is used to acquire point cloud data containing obstacles in a period T of the environment through a laser radar arranged on the mobile robot under the condition that obstacle information is unknown; the point cloud data is analyzed to obtain coordinate and timestamp information of the obstacles.

[0029] The dynamic obstacle identification module is used to perform first clustering on the coordinate data of the obstacles to obtain cluster center points of each obstacle, and combine the corresponding timestamp information to form a three-tuple data . is the two-dimensional coordinate of the cluster center point, and t ​​​​The time is obtained; the two-dimensional coordinates of the cluster center points of each obstacle are clustered for the second time to obtain an obstacle cluster; the obstacle clusters in the same cluster are considered to belong to the same obstacle; and the distance between the cluster center points is used to identify a dynamic obstacle for each obstacle cluster.

[0030] The dynamic obstacle trajectory fitting module is configured to model a dynamic obstacle motion trajectory using the two-dimensional coordinates of the cluster center points of the identified dynamic obstacles.

[0031] The dynamic obstacle trajectory prediction module is configured to predict a dynamic obstacle trajectory after a time period T using the modeling result of the motion trajectory.

[0032] The dynamic obstacle avoidance planning module is configured to plan a mobile robot motion obstacle avoidance trajectory according to the dynamic obstacle trajectory, so as to achieve dynamic obstacle avoidance.

[0033] Preferably, the dynamic obstacle avoidance planning module plans the mobile robot motion obstacle avoidance trajectory while imposing a distance constraint between path points corresponding to the same time of the mobile robot and the predicted trajectory of the dynamic obstacle, so that the distance is greater than a given safety distance.

[0034] Preferably, the dynamic obstacle identification module adopts a density-based clustering DBSCAN algorithm with noise.

[0035] Advantages:

[0036] (1) The present application obtains an obstacle cluster by clustering radar point cloud data multiple times, identifies a dynamic obstacle by giving a distance threshold based on the characteristic that a dynamic obstacle has a large moving distance within a period of time, and predicts a future trajectory based on a historical trajectory of the dynamic obstacle. The present application only considers the planar motion of a mobile robot, and only two-dimensional data is required for the coordinates obtained by analyzing data and clustering, so that the calculation is convenient, and real-time obstacle avoidance planning can be achieved.

[0037] In addition, compared with a planning algorithm for only static scenes, the present method considers the future trajectory of a dynamic obstacle, directly plans an obstacle avoidance path based on the obstacle position at a future time, avoids the obstacle, reduces the time required for re-planning, avoids possible collisions, and is more safe and efficient.

[0038] (2) In a preferred embodiment, the present application combines a predicted trajectory to construct a dynamic obstacle avoidance constraint and adds it to a trajectory solving optimization problem, so as to solve and generate a mobile robot motion trajectory that can achieve dynamic obstacle avoidance, thereby breaking through the previous limitation of only static obstacle avoidance.

[0039] (3) In a preferred embodiment, after the point cloud data is collected, the application converts it into an expanded point cloud data format. The expansion process can complete the shape and size of the obstacle, facilitate identification, and be conducive to the judgment of the safety distance when the robot path is planned in step seven, thereby ensuring the safety distance between the robot and the obstacle.

[0040] (4) The method provided by the application can update and correct the predicted trajectory in real time through continuous reception of point cloud data, and has good obstacle avoidance effect. BRIEF DESCRIPTION OF DRAWINGS

[0041] Figure 1 An unknown environment map set by the method provided by the application;

[0042] Figure 2 A flowchart of the mobile robot sensing and guiding integrated super-dynamic obstacle avoidance method in the unknown environment of the application;

[0043] Figure 3 A point cloud clustering predicted trajectory schematic diagram of the method provided by the application;

[0044] Figure 4 A dynamic obstacle avoidance constraint schematic diagram of the method provided by the application;

[0045] Figure 5 A robot planning process diagram of the method provided by the application;

[0046] Figure 6 A composition block diagram of the mobile robot sensing and guiding integrated super-dynamic obstacle avoidance device in the unknown environment of the application. DETAILED DESCRIPTION

[0047] The application will be described in detail below with reference to the accompanying drawings and examples.

[0048] The application provides a mobile robot sensing and guiding integrated super-dynamic obstacle avoidance scheme in an unknown environment. The scheme obtains an obstacle cluster through multiple clustering, identifies dynamic obstacles by giving a distance threshold based on the characteristics that dynamic obstacles have a large moving distance within a period of time, and predicts the future trajectory of the dynamic obstacles by using the historical trajectory of the dynamic obstacles.

[0049] Figure 1 The motion scene of the mobile robot and the dynamic obstacle is shown. There are multiple dynamic obstacles in the scene, which have different sizes and motion directions. Figure 2 The flowchart of the mobile robot sensing and guiding integrated super-dynamic obstacle avoidance method in the unknown environment in the embodiment of the application is shown in FIG. 1, which includes the following steps: Figure 2

[0050] ​Step 1: Obstacle information is unknown. The mobile robot uses LiDAR to acquire point cloud data containing obstacles in the environment over a period of time T.

[0051] In this step, the shape and movement of obstacles are unknown to the robot; the mobile robot is only given a starting point and a target point for path planning. The mobile robot obtains point cloud data of the unknown environment using LiDAR, and this point cloud data contains obstacle information.

[0052] Preferably, this step further converts the point cloud data into an expanded point cloud data format. The expansion process can complete the shape and size of the obstacle, making it easier to identify. It also helps to determine the safe distance during robot path planning in step seven, ensuring a safe distance between the robot and the obstacle.

[0053] Step 2: Analyze the point cloud data obtained in Step 1 to obtain the coordinates and timestamps of a series of obstacles. This invention only considers two-dimensional coordinates and timestamps.

[0054] Step 3: Perform the first clustering of the obstacle coordinate data to obtain the cluster center points of each obstacle, and combine them with the corresponding timestamp information to form triplet data. ; The two-dimensional coordinates of the cluster center point t For time.

[0055] The purpose of this step is to find the center point of each obstacle. For example... Figure 3 The small black dots in the diagram represent the obstacles. For static obstacles, there is no movement during the time interval T, and the center points cluster together. For dynamic obstacles, there is movement during the time interval T, and the center points will show a movement trajectory.

[0056] Clustering algorithms can include DBSCAN clustering (Density-Based Spatial Clustering of Applications with Noise), K-means clustering, Expectation-Maximum (EM) clustering of Gaussian Mixture Models (GMM), mean-shift clustering, and agglomerative hierarchical clustering. DBSCAN clustering is the preferred choice because it does not require prior knowledge of the number of clusters, is highly efficient, and is particularly suitable for datasets with clearly densely connected regions.

[0057] Step 4: Perform a second clustering on the two-dimensional coordinates of the cluster center points of each obstacle to obtain obstacle clusters; obstacle clusters in the same cluster are considered to belong to the same obstacle.

[0058] In this step, the center points of obstacles that are close to each other are grouped into a cluster, and each cluster is considered to correspond to one obstacle.

[0059] Step five, dynamic obstacle recognition: for each obstacle cluster, the dynamic obstacle is recognized by the distance between the cluster center points.

[0060] In this step, for each obstacle cluster, the distance between the cluster center points is calculated, which can be calculated by the Euclidean distance defined in equation (1).

[0061]

[0062] wherein, and are the coordinates of two center points in each obstacle cluster. Here, only the and coordinates are taken for calculation.

[0063] If there are two points in the same obstacle cluster with a distance greater than a set distance threshold , the obstacle cluster is considered as a dynamic obstacle, and the historical trajectory coordinates and time stamp of the dynamic obstacle are recorded. If all the two-point distances in the same obstacle cluster are less than the distance threshold , the current obstacle cluster is considered as a static obstacle.

[0064] Step six, using the two-dimensional coordinates of the cluster center points of the recognized dynamic obstacle to model the trajectory, to obtain the change relationship of the x coordinates with t , and the change relationship of the y coordinates with t , and then using the built model to predict the future dynamic obstacle trajectory after the time period T.

[0065] In this embodiment, the clustering-based dynamic obstacle detection is not limited to straight-line motion in the horizontal or vertical direction, and a n order polynomial is used as the motion model to fit the historical trajectory of the two-dimensional coordinates of the cluster center points of the recognized dynamic obstacle; the motion model is:

[0066]

[0067]

[0068] wherein, is the fitting curve of x and t , is the fitting curve of and , and t is the fitting curve of and , and

[0069] In the case of Figure 1In the shown scenario, the motion model defining the obstacle can be a 2nd order polynomial:

[0070]

[0071]

[0072] In combination with the dynamic obstacle history trajectory, the motion trajectory model can be obtained by fitting a polynomial through the least square method. The least square method minimizes the sum of the squares of the residuals between the predicted values and the observed values of the model, selects the best fitting curve, and the function can approximate the given data points.

[0073] Based on the obtained motion model, the future dynamic obstacle trajectory after the time period T can be predicted. As shown in Figure 3 The red arrow identifies the actual motion direction of the dynamic obstacle obtained in the experiment, and the blue one is the predicted trajectory.

[0074] Step seven, according to the future dynamic obstacle trajectory, the mobile robot moving obstacle avoidance trajectory is planned to realize dynamic obstacle avoidance.

[0075] In this step, the future trajectory of the dynamic obstacle is known, and the static obstacle information and various constraints can be further combined to construct a trajectory solving optimization problem, so as to plan the mobile robot moving obstacle avoidance trajectory. For specific planning process, please refer to the article Zhou X, Wang Z, Wen X, et al. Decentralized spatial-temporal trajectory planning for multicopter swarms[J]. arXiv preprint arXiv:2106.12481, 2021.

[0076] Preferably, in the planning of the mobile robot moving obstacle avoidance trajectory, a dynamic obstacle avoidance constraint term is constructed to constrain the distance between the path points corresponding to the same time of the mobile robot and the predicted trajectory of the dynamic obstacle, so that the distance between the corresponding path points is greater than a given safety distance, thereby ensuring that the mobile robot maintains a safe distance from the dynamic obstacle, and further obtaining a safe moving obstacle avoidance trajectory.

[0077] Repeat steps one to seven above to continuously update the predicted trajectory and replan until the mobile robot reaches the given target position from the starting position.

[0078] The present application is for the scene with dynamic and static obstacles. The point cloud information of the obstacle is obtained by the laser radar, the predicted trajectory after the dynamic obstacle identification and trajectory prediction is published to the planning node, the mobile trajectory is planned after the control instruction is published to the chassis, and the dynamic obstacle avoidance motion planning of the robot is completed.

[0079] Experimental verification is carried out based on the above method. The experimental scene is set, and the shape and motion of the dynamic obstacle is as shown in Figure 1 The maximum speed of the dynamic obstacle is 0.472 m / s, and the maximum speed of the robot is 0.3 m / s. Only the starting point and target point of the robot path planning are given, and the robot moves from (0.5, 0.5) to (6.0, 4.5) with the left lower corner of the map as the origin. The planning result of the mobile robot in the current scene by using the method of the application is as shown in Figure 5 The robot successfully avoids the dynamic obstacle and reaches the given target point.

[0080] A plurality of different experimental scenes are set, and the online solving time of the trajectory and the collision process with the dynamic obstacle are recorded. It can be known that the online solving time of the method is extremely short, about 1.44 ms, and the success rate is greater than 80%. Compared with the planning algorithm only for static scenes, the method considers the future trajectory of the dynamic obstacle, directly plans the obstacle avoidance path based on the obstacle position at the future time, avoids the obstacle, reduces the time required for re-planning, and avoids possible collision, which is more safe and efficient.

[0081] Based on the above method, the application further provides a mobile robot sensing and guiding integrated super-dynamic obstacle avoidance device in an unknown environment, as shown in Figure 6 The device comprises a point cloud data acquisition and analysis module, a dynamic obstacle identification module, a dynamic obstacle trajectory fitting module, a dynamic obstacle trajectory prediction module and a dynamic obstacle avoidance planning module.

[0082] The point cloud data acquisition and analysis module is used to acquire point cloud data containing obstacles in a time T in the environment through a laser radar arranged on the mobile robot under the condition that the obstacle information is unknown; the point cloud data is analyzed to obtain the coordinates and timestamp information of the obstacles;

[0083] The dynamic obstacle identification module is used to first cluster the coordinate data and time information of the obstacles to obtain the cluster center points of the obstacles, and combine the corresponding timestamp information to form a three-tuple data ; The two-dimensional coordinates of the cluster center points are t , and the time is. The two-dimensional coordinates of the cluster center points of the obstacles are secondly clustered to obtain an obstacle cluster. The obstacles in the same cluster are considered to belong to the same obstacle. For each obstacle cluster, the dynamic obstacle is identified through the distance between the cluster center points. As long as there is a pair of cluster center points with a distance greater than a distance threshold d , the obstacle corresponding to the obstacle cluster is determined as a dynamic obstacle.

[0084] In a preferred embodiment, the two clustering operations preferably use the DBSCAN algorithm.

[0085] The dynamic obstacle trajectory fitting module is used to model the motion trajectory of dynamic obstacles using the two-dimensional coordinates of the cluster center points of the identified dynamic obstacles, and obtain... x Coordinates t The relationship of change, and y Coordinates t The changing relationship. Motion trajectory modeling can be achieved using... n The polynomial is of order 1, preferably a second-order polynomial.

[0086] The dynamic obstacle trajectory prediction module is used to predict the dynamic obstacle trajectory after a time period T using the motion trajectory modeling results.

[0087] The dynamic obstacle avoidance planning module is used to plan the mobile robot's movement obstacle avoidance trajectory based on the dynamic obstacle trajectory, thereby achieving dynamic obstacle avoidance.

[0088] In a preferred embodiment, when planning the obstacle avoidance trajectory of the mobile robot, a distance constraint is imposed between the path points corresponding to the predicted trajectories of the mobile robot and the dynamic obstacle at the same time, such that the distance is greater than a given safe distance.

[0089] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A hyper-dynamic obstacle avoidance method integrating sensing and guidance for mobile robots in unknown environments, characterized in that, The method includes the following steps: Step 1: Obstacle information is unknown. The mobile robot acquires point cloud data containing obstacles in the environment over a period of time T using LiDAR and performs dilation processing. Step 2: Analyze the point cloud data to obtain the coordinates and timestamp information of the obstacles; Step 3: Perform the first clustering of the obstacle coordinate data to obtain the cluster center points of each obstacle, and combine them with the corresponding timestamp information to form triplet data. ; The two-dimensional coordinates of the cluster center point t For time; Step 4: Perform a second clustering on the two-dimensional coordinates of the cluster center points of each obstacle to obtain obstacle clusters; obstacle clusters in the same cluster are considered to belong to the same obstacle; Step 5: For each obstacle cluster, identify dynamic obstacles by the Euclidean distance between the two-dimensional coordinates of the cluster center points: if there is a distance between two points in the same obstacle cluster that is greater than a set distance threshold, then the obstacle cluster is considered to correspond to a dynamic obstacle. Step Six: Adopt n A polynomial of order T is used as the motion trajectory model. The two-dimensional coordinates of the cluster centers of the identified dynamic obstacles are used to fit the motion trajectory, thereby achieving dynamic obstacle trajectory modeling. The established model is then used to predict the future dynamic obstacle trajectory after a time period T. The motion trajectory model is as follows: in, for x and t The fitted curve, for and t The fitted curve, as well as The coefficients of the fitted curve; Step 7: Based on the future dynamic obstacle trajectory, plan the mobile robot's obstacle avoidance trajectory to achieve dynamic obstacle avoidance.

2. The method as described in claim 1, characterized in that, In step seven, when planning the mobile robot's obstacle avoidance trajectory, a distance constraint is imposed between the path points corresponding to the predicted trajectories of the mobile robot and the dynamic obstacle at the same time, so that the distance is greater than a given safe distance.

3. The method as described in claim 1, characterized in that, The clustering in steps three and four employs the density-based clustering method DBSCAN with noise.

4. A mobile robot with integrated sensing and guidance for ultra-dynamic obstacle avoidance in unknown environments, characterized in that, The device performs the integrated sensing and guidance ultra-dynamic obstacle avoidance method for mobile robots in unknown environments as described in claim 1; the device includes: a point cloud data acquisition and analysis module, a dynamic obstacle recognition module, a dynamic obstacle trajectory fitting module, a dynamic obstacle trajectory prediction module, and a dynamic obstacle avoidance planning module; The point cloud data acquisition and parsing module is used to acquire point cloud data containing obstacles in the environment over a period of time T using a lidar installed on the mobile robot when the obstacle information is unknown; and to parse the point cloud data to obtain the coordinates and timestamp information of the obstacles. The dynamic obstacle recognition module is used to perform the first clustering of the obstacle coordinate data to obtain the cluster center points of each obstacle, and combine them with the corresponding timestamp information to form triplet data. ; The two-dimensional coordinates of the cluster center point t For time; perform a second clustering on the two-dimensional coordinates of the cluster center points of each obstacle to obtain obstacle clusters; obstacle clusters in the same cluster are considered to belong to the same obstacle; for each obstacle cluster, identify dynamic obstacles by the distance between the cluster center points; The dynamic obstacle trajectory fitting module is used to model the motion trajectory of dynamic obstacles using the two-dimensional coordinates of the cluster center points of the identified dynamic obstacles. The dynamic obstacle trajectory prediction module is used to predict the dynamic obstacle trajectory after time period T using the motion trajectory modeling results. The dynamic obstacle avoidance planning module is used to plan the mobile robot's movement obstacle avoidance trajectory based on the dynamic obstacle trajectory, thereby achieving dynamic obstacle avoidance.

5. The apparatus as described in claim 4, characterized in that, When planning the mobile robot's obstacle avoidance trajectory, the dynamic obstacle avoidance planning module imposes distance constraints on the path points corresponding to the predicted trajectories of the mobile robot and dynamic obstacles at the same time, ensuring that the distance is greater than a given safe distance.

6. The apparatus as claimed in claim 4, characterized in that, The dynamic obstacle recognition module uses the DBSCAN algorithm, a density-based clustering algorithm with noise.

Images