Trajectory tracking method and device, autonomous mobile device and storage medium

Abstract

The embodiment of the invention provides a trajectory tracking method and device, an autonomous mobile device and a storage medium, and relates to the technical field of autonomous mobile devices. The method comprises the steps of obtaining sensing data collected by a sensor; according to the sensing data, determining a target point of an obstacle, the target point being a point closest to an autonomous mobile device in the contour of the obstacle; according to the method, the trajectory of the dynamic obstacle is determined according to the target point of the obstacle, so that the dynamic obstacle in the working scene can be effectively detected and the trajectory of the dynamic obstacle is tracked, and reliable prior information (such as the trajectory of the dynamic obstacle) is provided for an operation decision (such as collision detection) of the autonomous mobile device. Therefore, it is ensured that the autonomous mobile device can make a timely safe response to the dynamic obstacle, and the operation safety of the autonomous mobile device is improved.

Description

Technical Field

[0001] The embodiments of the present application relate to the technical field of autonomous mobile devices, and in particular, to a trajectory tracking method, device, autonomous mobile device, and storage medium. Background Art

[0002] As autonomous mobile devices are gradually applied to more and more fields, their operating environments have become increasingly complex, often presenting complex situations such as mixed traffic of people and vehicles, and multi-device collaborative operations. These complex working conditions pose higher requirements for functions such as obstacle avoidance and obstacle bypassing of autonomous mobile devices. And functions such as obstacle avoidance and obstacle bypassing rely on obstacle detection.

[0003] In the field of autonomous mobile devices, since the general driving speed of autonomous mobile devices is relatively slow, whether it is a dynamic obstacle or a static obstacle, during the process of collision detection, obstacle stopping, and obstacle bypassing, the autonomous mobile device usually treats each detected obstacle frame as a static obstacle, updates it to the obstacle map in real time, and then provides decision-making information for the autonomous mobile device based on the obstacle map. However, the inventors have found in their research that currently, there is a lack of detection and tracking of dynamic obstacles in the field of autonomous mobile devices. When an autonomous mobile device operates in complex working conditions such as mixed traffic of people and vehicles and multi-device collaboration, it is difficult to make a timely safety response to dynamic obstacles moving in opposite directions or crossing each other, posing a risk of collision, thus threatening the safety of autonomous mobile devices or people in the working scenario.

[0004] In the field of autonomous driving related to autonomous mobile devices, since the vehicle driving speed is relatively fast, dynamic obstacle tracking is usually performed to ensure that the vehicle can respond to dynamic obstacles in a timely manner and drive safely. However, the inventors have found in their research that the tracking of dynamic obstacles in the field of autonomous driving generally combines traffic data, road conditions information, and perceived obstacle information, etc., and uses deep learning methods to predict the trajectory of dynamic obstacles. The algorithm is too complex and there are differences between the autonomous driving scenario and the working scenario of autonomous mobile devices. The dynamic obstacle detection in the field of autonomous driving is not suitable for implementation and application in the field of autonomous mobile devices.

[0005] Therefore, how to detect and track dynamic obstacles in the working scenario of autonomous mobile devices is an urgent problem to be solved in the field of autonomous mobile devices. Summary of the Invention

[0006] The embodiments of the present application propose a trajectory tracking method, device, autonomous mobile device, and storage medium to solve the above problems.

[0007] In a first aspect, an embodiment of the present application provides a trajectory tracking method, which includes: obtaining perception data collected by a sensor; determining a target point of an obstacle according to the perception data, where the target point is the point on the contour of the obstacle that is closest to the autonomous mobile device; and determining the trajectory of the dynamic obstacle according to the target point of the obstacle.

[0008] In a second aspect, an embodiment of the present application provides a trajectory tracking device, which includes: a data acquisition module for obtaining perception data collected by a sensor; a target point determination module for determining a target point of an obstacle according to the perception data, where the target point is the point on the contour of the obstacle that is closest to the autonomous mobile device; and a trajectory determination module for determining the trajectory of the dynamic obstacle according to the target point of the obstacle.

[0009] In a third aspect, an embodiment of the present application provides an autonomous mobile device, which includes a memory; one or more processors; and one or more application programs, where the one or more application programs are stored in the memory and are used to execute the method described above when called by the one or more processors.

[0010] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, in which program code is stored, and the program code is used to execute the method described above when called by a processor.

[0011] The trajectory tracking method, device, autonomous mobile device, and storage medium provided by the embodiments of the present application obtain the perception data collected by the sensor; determine the target point of the obstacle according to the perception data, where the target point is the point on the contour of the obstacle that is closest to the autonomous mobile device; and determine the trajectory of the dynamic obstacle according to the target point of the obstacle, so as to effectively detect the dynamic obstacles in the operation scenario and track the trajectories of the dynamic obstacles, providing reliable prior information (such as the trajectories of dynamic obstacles) for the operation decision-making (such as collision detection) of the autonomous mobile device, thereby ensuring that the autonomous mobile device can make timely and safe responses to dynamic obstacles and improving the safety of the operation of the autonomous mobile device. Description of the Drawings

[0012] To more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present application, and those skilled in the art can obtain other drawings without creative efforts based on these drawings.

[0013] Figure 1 The flowchart of the trajectory tracking method provided by an embodiment of the present application is shown;

[0014] Figure 2Shows a schematic diagram of the obstacle contour provided by an exemplary embodiment of the present application;

[0015] Figure 3 Shows a schematic diagram of a large static obstacle provided by an exemplary embodiment of the present application;

[0016] Figure 4 Shows a flowchart of the Kalman observation smoothing trajectory provided by an exemplary embodiment of the present application;

[0017] Figure 5 Shows a flowchart of the trajectory tracking method provided by another embodiment of the present application;

[0018] Figure 6 Shows a flowchart of the trajectory tracking method provided by yet another embodiment of the present application;

[0019] Figure 7 Shows a schematic diagram of the reliability verification of Kalman prediction points provided by an exemplary embodiment of the present application;

[0020] Figure 8 Shows a flowchart of the trajectory tracking method provided by an exemplary embodiment of the present application;

[0021] Figure 9 Shows a structural block diagram of a trajectory tracking device provided by an embodiment of the present application;

[0022] Figure 10 Shows a structural block diagram of an autonomous mobile device provided by an embodiment of the present application. Detailed implementation manners

[0023] In order to enable those skilled in the art of the present technology to better understand the solutions of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative efforts fall within the scope of protection of the present application.

[0024] The trajectory tracking method in the embodiments of the present application can be applied to an autonomous mobile device or a trajectory tracking device. The autonomous mobile device may include, but is not limited to, an Automated Guided Vehicle (AGV) and an Autonomous Mobile Robot (AMR). The autonomous mobile device can be adapted to various models such as differential drive, omnidirectional wheels, and Mecanum wheels, and supports multiple devices to cooperate in performing functions such as linear trajectory, curved trajectory, in-place spin, obstacle stopping, obstacle avoidance, state synchronization, and exception handling.

[0025] See Figure 1 , Figure 1 which is a flowchart of a trajectory tracking method provided by an embodiment of the present application. The trajectory tracking method may include steps S110 to step S130.

[0026] Step S110: Obtain the perception data collected by the sensor.

[0027] The sensor is used to sense the environmental information around the autonomous mobile device. The sensor may include, but is not limited to, lidar, millimeter wave radar, camera, etc. For example, in an embodiment of the present application, the sensor may be a lidar and a camera installed on the autonomous mobile device. Correspondingly, the perception data may include the point cloud information collected by the lidar and the photos taken by the camera.

[0028] Since it mainly involves planar trajectory tracking, it is only necessary to obtain the projection of the obstacle on the ground through the sensor. Correspondingly, the camera may be a fish-eye camera. After the photo is collected, it can be first synthesized through a panoramic synthesis algorithm so that the photo is presented as being taken at an angle perpendicular to the ground.

[0029] In an embodiment of the present application, the sensor (such as lidar, camera, etc.) has a preset acquisition frequency, and the acquisition frequency can be set by the developer according to actual needs. For example, it can be set to collect once every 50 milliseconds. This application does not limit this here.

[0030] In some embodiments, an object recognition algorithm in related technologies (such as a two-dimensional reconstruction algorithm based on deep learning) can be used to recognize the objects in the collected photos and convert them into point cloud information. The point cloud information converted from the photo can be merged with the point cloud information collected by other sensors (such as lidar) at the same moment, so as to obtain the perception data of this frame.

[0031] It should be noted that in an embodiment of the present application, only the perception information in front of, on the left side, and on the right side of the autonomous mobile device may be obtained. This is because the autonomous mobile device itself has an obstacle avoidance function. Other autonomous mobile devices coming from the rear can regard the autonomous mobile device in front as a dynamic obstacle and perform related obstacle avoidance, following, etc. operations. Therefore, there is no need to consider the obstacles behind the autonomous mobile device. In the subsequent steps, the perception data in front of, on the left side, and on the right side of the autonomous mobile device will be used as examples for explanation.

[0032] Step S120: Determine the target point of the obstacle according to the perception data. The target point is the point on the obstacle contour that is closest to the autonomous mobile device.

[0033] In some embodiments, the sensed data can be clustered to obtain at least one clustering cluster, and each clustering cluster corresponds to an obstacle; according to the points in the clustering cluster, the obstacle contour and the center of the obstacle contour are determined; according to the points in the clustering cluster, the center of the obstacle contour, the current position of the autonomous mobile device, and the end position of the current task, the target point of the obstacle is determined.

[0034] In some embodiments, the clustering conditions can be preset according to the distance between adjacent target points, and the sensed data is clustered according to the clustering conditions to obtain at least one clustering cluster.

[0035] In fact, each clustering cluster is the point cloud corresponding to the obstacle, so each clustering cluster represents an obstacle.

[0036] In some embodiments, the obstacle contour can be determined according to the maximum abscissa, minimum abscissa, maximum ordinate, and minimum ordinate in the clustering cluster.

[0037] Among them, the autonomous mobile device itself has a preset coordinate system (the coordinate system of the global map), and the origin and the direction of the coordinate axes of the preset coordinate system can be determined by the current task executed by the autonomous mobile device. It should be noted that the preset coordinate system is established on the horizontal ground, and the preset coordinate systems of different autonomous mobile devices can be different.

[0038] For example, assume that the minimum abscissa of a certain clustering cluster is x min , the maximum abscissa is x max , the minimum ordinate is y min , and the maximum ordinate is y max , then a rectangular obstacle contour can be obtained, and the four corner points of the rectangular obstacle contour are (x max , y max ), (x min , y max ), (x max , y min ), and (x min , y min ), and this rectangle is the obstacle contour corresponding to this clustering cluster. As Figure 2 shown, for obstacle A, the obstacle contour can be as shown by the dashed rectangular frame in Figure 2 .

[0039] In some embodiments, the mean of the minimum abscissa and the maximum abscissa in the clustering cluster can be used as the abscissa of the center of the obstacle contour; the mean of the minimum ordinate and the maximum ordinate in the clustering cluster can be used as the ordinate of the center of the obstacle contour.

[0040] For example, assume that the coordinates of the center of the obstacle contour are (centerX, centerY), the minimum abscissa in the cluster is x min , the maximum abscissa is x max , the minimum ordinate is y min , the maximum ordinate is y max , then the abscissa centerX of the center of the obstacle contour can be calculated by the following expression (1):

[0041] centerX = (x max + x min ) / 2 (1)

[0042] The ordinate centerY of the center of the obstacle contour can be calculated by the following expression (2):

[0043] centerY = (y max + y min ) / 2 (2)

[0044] In some embodiments, the first included angle can be determined according to the current position and the end position; according to the points in the cluster, the center of the obstacle contour, the current position and the first included angle, the target point of the obstacle can be determined.

[0045] The first included angle refers to the included angle between the line connecting the current position and the end position and the x-axis of the preset coordinate system. In some embodiments, the first included angle can be set as θ, the current position is (x1, y1), and the end position is (x2, y2), then the first included angle can be calculated by the following expression (3):

[0046]

[0047] For example, the point closest to the autonomous mobile device in the obstacle contour can be determined according to the included angle interval where the first included angle is located and the size relationship between the abscissa / ordinate of the current position and the abscissa / ordinate of the center of the obstacle contour.

[0048] The included angle interval refers to the angle interval for determining the orientation of the obstacle relative to the autonomous mobile device. In the embodiments of the present application, the included angle interval includes the first included angle interval, the second included angle interval, the third included angle interval, and the fourth included angle interval.

[0049] Among them, the first included angle interval can be [-π / 4, π / 4]; the second included angle interval can be (π / 4, 3π / 4); the third included angle interval can be (-3π / 4, -π / 4); the fourth included angle interval can be [3π / 4, -3π / 4].[[]END]]

[0050] It can be understood that when a dynamic obstacle may affect the operation of the autonomous mobile device, the point where the dynamic obstacle first collides with the autonomous mobile device should be the point closest to the autonomous mobile device. Therefore, it is necessary to determine the point closest to the autonomous mobile device among the four corner points of the obstacle contour as the target point.

[0051] In some embodiments, if the first included angle is within the first included angle range and the ordinate of the center is greater than the ordinate of the current position, the minimum abscissa in the clustering cluster is determined as the abscissa of the target point, and the minimum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0052] For example, if the first included angle is within the first included angle range and the value of centerY is greater than y1, x min can be used as the abscissa of the target point, and y min can be used as the ordinate of the target point. That is, the coordinates of the target point are (x min , y min ).

[0053] In some embodiments, if the first included angle is within the first included angle range and the ordinate of the center is less than or equal to the ordinate of the current position, the minimum abscissa in the clustering cluster is determined as the abscissa of the target point, and the maximum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0054] For example, if the first included angle is within the first included angle range and the value of centerY is less than or equal to y1, x min can be used as the abscissa of the target point, and y max can be used as the ordinate of the target point. That is, the coordinates of the target point are (x min , y max ).

[0055] In some embodiments, if the first included angle is within the second included angle range and the abscissa of the center is greater than the abscissa of the current position, the minimum abscissa in the clustering cluster is determined as the abscissa of the target point, and the minimum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0056] For example, if the first included angle is within the second included angle range and the value of centerX is greater than x1, x min can be used as the abscissa of the target point, and y min can be used as the ordinate of the target point. That is, the coordinates of the target point are (x min , y min ).

[0057] In some embodiments, if the first included angle is within the second included angle range and the abscissa of the center is less than or equal to the abscissa of the current position, the maximum abscissa in the clustering cluster is determined as the abscissa of the target point, and the minimum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0058] Exemplarily, if the first included angle is within the second included angle interval and the value of centerX is less than or equal to x1, x min can be used as the abscissa of the target point, and y min can be used as the ordinate of the target point, that is, the coordinates of the target point are (x min , y min ).

[0059] In some embodiments, if the first included angle is within the third included angle interval and the abscissa of the center is greater than the abscissa of the current position, the minimum abscissa in the clustering cluster is determined as the abscissa of the target point, and the maximum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0060] Exemplarily, if the first included angle is within the third included angle interval and the value of centerX is greater than x1, x min can be used as the abscissa of the target point, and y max can be used as the ordinate of the target point, that is, the coordinates of the target point are (x min , y max ).

[0061] In some embodiments, if the first included angle is within the third included angle interval and the abscissa of the center is less than or equal to the abscissa of the current position, the maximum abscissa in the clustering cluster is determined as the abscissa of the target point, and the maximum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0062] Exemplarily, if the first included angle is within the third included angle interval and the value of centerX is less than or equal to x1, x max can be used as the abscissa of the target point, and y max can be used as the ordinate of the target point, that is, the coordinates of the target point are (x max , y max ).

[0063] In some embodiments, if the first included angle is within the fourth included angle interval and the ordinate of the center is greater than the ordinate of the current position, the maximum abscissa in the clustering cluster is determined as the abscissa of the target point, and the minimum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0064] Exemplarily, if the first included angle is within the fourth included angle interval and the value of centerY is greater than y1, x max can be used as the abscissa of the target point, and y min can be used as the ordinate of the target point, that is, the coordinates of the target point are (x max , y min ).

[0065] In some embodiments, if the first is in the fourth included angle range and the ordinate of the center is less than or equal to the ordinate of the current position, the maximum abscissa in the clustering cluster is determined as the abscissa of the target point, and the maximum ordinate in the clustering cluster is determined as the ordinate of the target point.

[0066] For example, if the first included angle is within the fourth included angle range and the value of centerY is less than or equal to y1, x max can be used as the abscissa of the target point, and y max can be used as the ordinate of the target point. That is, the coordinates of the target point are (x max , y max ).

[0067] Step S130: Determine the trajectory of the dynamic obstacle according to the target point of the obstacle.

[0068] In some embodiments, the target points of the obstacle can be clustered to obtain at least one clustering cluster, and each clustering cluster forms the trajectory of an obstacle; determine the starting point and ending point of the trajectory of the obstacle; and determine the trajectory of the dynamic obstacle from the trajectories of the obstacles according to the starting point and ending point of the trajectory.

[0069] In some embodiments, the clustering conditions can be preset according to the distances between the target points, and the target points of the obstacle are clustered according to the clustering conditions to obtain at least one clustering cluster.

[0070] Among them, the clustering method can be the Density-Based Spatial Clustering of Applications with Noise (DBSCAN), which is not limited in this application. Since the acquisition interval of the perception data is very short, target points with close distances can be regarded as the same obstacle at different positions at different times (that is, it may have moved), and after clustering, it can be regarded as the trajectory of the obstacle.

[0071] It should be noted that obstacles can be divided into two types: static obstacles and dynamic obstacles.

[0072] Among them, static obstacles refer to obstacles whose positions do not change, such as buildings, fire hydrants, street lamps, etc. Since they do not move, there is no need to track them in real time, and only need to avoid them during the navigation of the autonomous mobile device. Generally, static obstacles in the environment are pre-marked in the global map of the autonomous mobile device.

[0073] Dynamic obstacles refer to obstacles whose positions change in real time, such as autonomous mobile devices, people, etc. Since they move, they need to be tracked in real time to avoid collisions with them.

[0074] In some embodiments, in order to reduce the data storage pressure and the computing pressure, before determining the trajectory start point and the trajectory end point of an obstacle, the trajectory of the static obstacle can be filtered out according to the total number of points in the obstacle trajectory, so as to try to retain only the trajectories of the dynamic obstacles that need to be tracked, reduce the storage pressure, and reduce the resource consumption and the amount of computation.

[0075] Specifically, it can be determined whether the number of points included in the trajectory of the obstacle is less than a number threshold. If the number of points included in the trajectory of the obstacle is less than the number threshold, the trajectory of the obstacle is deleted. The number threshold can be preset according to the tracking progress of the dynamic obstacle trajectory. For example, the number threshold can be 3.

[0076] It can be understood that for a static obstacle, no matter how the autonomous mobile device moves, the position of the static obstacle itself will not change. Therefore, even if there are errors in steps such as clustering trajectories or calculating the position of the target point by the autonomous mobile device, it is difficult for the number of target points of a certain static obstacle to be above the number threshold.

[0077] In some embodiments, the first point in the trajectory of the obstacle can be determined as the trajectory start point, and the last point in the trajectory of the obstacle can be determined as the trajectory end point.

[0078] In other embodiments, in order to further improve the calculation accuracy of the average speed of the obstacle, the coordinate mean of the first N points in the trajectory of the obstacle can be determined as the coordinate of the trajectory start point; the coordinate mean of the last N points in the trajectory of the obstacle can be determined as the coordinate of the trajectory end point. Here, N is a positive integer, and N can be preset according to actual requirements (such as the size of the operation scenario and the tracking accuracy requirements for the dynamic obstacle trajectory, etc.). For example, N can be 4.

[0079] In some embodiments, the average speed of the obstacle can be determined according to the trajectory start point and the trajectory end point; if the average speed of the obstacle is greater than or equal to the speed threshold, it is determined that the trajectory of the obstacle is the trajectory of a dynamic obstacle.

[0080] Since a static obstacle does not move (i.e., the speed is always 0), while the dynamic obstacles in the scenario where the autonomous mobile device operates move in real time (i.e., the average speed cannot always be 0), therefore, a speed threshold can be set to determine whether a certain obstacle moves, so as to screen out the dynamic obstacles and filter out the static obstacles.

[0081] It can be understood that due to certain errors that may occur in the foregoing steps of the autonomous mobile device, static obstacles with an average speed not equal to 0 may be detected. Therefore, a speed threshold can be set slightly greater than 0 to ensure that static obstacles with an average speed not equal to 0 detected due to errors are filtered out. Specifically, the speed threshold can be set by the developer according to the overall error of the autonomous mobile device, and the embodiments of the present application do not limit this here.

[0082] It can be understood that the above two methods can only filter out static obstacles with a relatively small size (that is, those that can be completely captured by the sensor and a relatively complete point cloud can be obtained). For larger static obstacles, such as walls, since the sensor can only obtain a part of the wall, when the autonomous mobile device moves, it can always obtain a part of the wall, and it may misjudge the obtained part as a dynamic obstacle. For example, as Figure 3 shown, when the autonomous mobile device ( Figure 3 the circle in) moves along the wall ( Figure 3 the rectangle in), it moves to different positions at different times, and it can always observe the "obstacle" shown in the shaded part of the figure moving along with the autonomous mobile device. During this process, the autonomous mobile device will collect target points of this "obstacle" and obtain its trajectory through clustering. However, in fact, the wall itself does not move, so the trajectory of this part of the "obstacle" needs to be removed.

[0083] In order to remove the trajectories of large static obstacles such as walls as described above to further improve the screening accuracy, in some embodiments, the first angle can be determined according to the current position of the autonomous mobile device and the end position of the current task; the second angle can be determined according to the starting point and the ending point of the trajectory; the absolute value of the difference between the first angle and the second angle can be determined; if the absolute value belongs to the specified angle interval, the trajectory corresponding to the absolute value is determined to be the trajectory of a dynamic obstacle.

[0084] The second angle refers to the angle between the line connecting the starting point and the ending point of the trajectory and the x-axis of the preset coordinate system. The specified angle interval refers to a preset angle interval for judging the similarity between the moving direction of the obstacle and the moving direction of the autonomous mobile device. For example, the specified angle interval can be [π / 4, 3π / 4].

[0085] Among them, when the absolute value of the difference between the first angle and the second angle is within the specified angle interval, it means that the moving direction of this obstacle is relatively similar to the moving direction of the robot itself, and it can be approximately considered that this obstacle is a large static obstacle (such as a wall), and the autonomous mobile device may be moving along the direction of this large static obstacle. Therefore, the trajectory of this large static obstacle can be removed.

[0086] Since errors may occur in steps such as the acquisition, clustering, and trajectory determination of perception data, there may be some unreasonable target points in the finally obtained trajectory. Among them, unreasonable may refer to that the position of a certain target point is significantly deviated from the normal range (for example, the distance from the previous and subsequent target points is too far).

[0087] In order to eliminate the above unreasonable target points, in some embodiments, for each frame, after obtaining the trajectory of the dynamic obstacle, the trajectory of the dynamic obstacle can also be smoothed by the Kalman filter algorithm.

[0088] Specifically, referring to Figure 4 , for the trajectory of each dynamic obstacle, the method of smoothing the trajectory of the dynamic obstacle by the Kalman filter algorithm may include:

[0089] First, set the coordinates of the first target point of the trajectory in the preset coordinate system as (startX, startY), and set the initial velocity of the first target point as 0, then the initial state X0 [startX, startY, 0, 0] of this trajectory can be obtained. Then set the initial covariance matrix of the Kalman filter as P0 (which can be estimated through the uncertainty of the system initial state and prior knowledge, or can be statistically analyzed through historical data, and is not limited here), and set as the predicted state for the first time, then there are the following expressions (4) and (5):

[0090]

[0091]

[0092] Among them, P'1 is the covariance of the prior estimate corresponding to the first target point, A1 is the state transition matrix corresponding to the first target point (the state transition matrix at each moment can be determined by the kinematic equation of the system), Q is the system noise covariance matrix (which can be determined by the characteristics and statistical analysis of the system noise), refers to the transpose of A1.

[0093] Then, use the second target point as the observation value Z1 for Kalman update.

[0094] Specifically, first update the Kalman gain K1 corresponding to the first target point according to expression (6):

[0095]

[0096] Among them, H1 is the transformation matrix from the state vector to the observation vector (which can be determined by the measurement equation of the system), and similarly, refers to the transpose of H1.

[0097] Then, update the optimal estimate X1 at the moment corresponding to the first target point according to expression (7):

[0098]

[0099] Finally, update the posterior estimate covariance matrix P1 according to expression (8) as the "initial covariance matrix" in the smoothing process of the second point:

[0100] P1 = (1 - K1H1)P'1 (8)

[0101] So far, the smoothing of the second point in the trajectory is completed. For other subsequent target points in the trajectory, after obtaining the optimal estimate X n and P n then, execute in sequence according to the above steps in chronological order until the last point is smoothed (i.e., the optimal estimate of the last point is obtained), and the dynamic obstacle trajectory smoothed by the Kalman filter algorithm can be obtained.

[0102] Based on steps S110 to S130, the dynamic obstacles in the operation scenario can be effectively detected and the trajectories of the dynamic obstacles can be tracked, providing reliable prior information (such as the trajectories of dynamic obstacles) for the operation decision-making (such as collision detection) of the autonomous mobile device, thereby ensuring that the autonomous mobile device can make timely and safe responses to dynamic obstacles and improving the safety of the operation of the autonomous mobile device.

[0103] See Figure 5 , in some embodiments, before determining the trajectory of the dynamic obstacle according to the target points of the obstacle, the trajectory tracking method further includes a target point real-time update step, that is, the following steps S210 to S230.

[0104] Step S210: Save the target points of the obstacle into the target point set, and each target point has a collection time.

[0105] Step S220: Determine the difference between the collection time of the current frame of perception data and the collection time of the target point.

[0106] Step S230: Delete the target points in the target point set whose difference is greater than the difference threshold.

[0107] Among them, the difference threshold can be set by the developer according to the actual situation, and the embodiments of the present application do not limit it here.

[0108] Based on steps S210 to S230, only the target points corresponding to the latest multiple frames of perception data can be saved, and the relatively old target points can be deleted, thereby reducing the data storage amount, reducing the data storage pressure, and avoiding meaningless resource consumption.

[0109] See Figure 6 , Figure 6 which is a flowchart of a trajectory tracking method provided by another embodiment of the present application. The trajectory tracking method may include steps S310 to step S330.

[0110] Step S310: Obtain the perception data collected by the sensor.

[0111] Step S320: Determine the target points of the obstacles according to the perception data.

[0112] For the specific implementation manners of steps S310 to step S320, please refer to the relevant descriptions in steps S110 to step S120, which will not be elaborated herein in the embodiments of the present application.

[0113] Step S330: Determine the set of target points according to the target points of the obstacles.

[0114] Step S340: Obtain the latest trajectory points of the dynamic obstacle according to the latest obtained target points of the obstacle and the set of target points, and obtain the trajectory of the dynamic obstacle.

[0115] For the specific description of determining the set of target points, please refer to the relevant descriptions in steps S210 to step S230, which will not be elaborated herein in the embodiments of the present application. After obtaining the set of target points, clustering the set of target points can obtain the trajectory of the obstacle, and then eliminating the trajectory of the static obstacle, and finally obtaining the trajectory of the dynamic obstacle.

[0116] In some embodiments, Kalman filter prediction can be performed on the trajectory of the dynamic obstacle. Similar to the Kalman filter in step S130, finally, the expected position of the dynamic obstacle at the current moment can be predicted, and this predicted point is used as the Kalman prediction point.

[0117] In some embodiments, the optimal estimate calculated from the last target point in the trajectory can be used as the tracking starting point. The target point of the current frame of the dynamic obstacle can be obtained according to the method of step S120, and this target point is used as the observation point.

[0118] After obtaining the observation point, the tracking starting point, and the Kalman prediction point, the first angle θ1 between the line connecting the Kalman prediction point (x p , y p ) and the tracking starting point (x, y) and the x-axis can be calculated. The expression (9) is as follows:

[0119]

[0120] Calculate the observation point (x o , y o)The second angle θ2 between the line connecting to the tracking starting point (x, y) and the x-axis, and the expression (10) is as follows:

[0121]

[0122] An angle difference threshold can be preset in advance. When the difference between θ1 and θ2 is greater than the angle difference threshold, it can be determined that this observation is unreliable; when the difference between θ1 and θ2 is less than or equal to the angle difference threshold, it can be determined that this observation is reliable.

[0123] Furthermore, in some embodiments, a distance threshold can also be preset in advance, and the distance between the Kalman prediction point (x p , y p ) and the observation point (x o , y o ) is calculated. When the distance is greater than the distance threshold, it can be determined that this observation is unreliable; when the distance is less than or equal to the distance threshold, it can be determined that this observation is reliable. As Figure 7 shown, the white area inside the circle is the area where the observation is reliable.

[0124] Among them, the angle difference threshold and the distance threshold can be obtained based on a large number of pre-conducted test experiments. For example, for the angle difference threshold, steps S310 to S340 can be used for testing. For a certain dynamic obstacle, when the angle difference between the first angle and the second angle is obtained, the reliability of this prediction can be judged by obtaining the actual position of the dynamic obstacle. When a large number of angle differences are obtained, determine the range within which the angle difference should be when the prediction is unreliable, so as to obtain the angle difference threshold.

[0125] In some embodiments, when it is determined that this observation is unreliable, the currently obtained Kalman prediction point and observation point can be discarded, the Kalman prediction point of the next frame can be predicted again, and the target point of the next frame can be obtained as the observation point, and the reliability verification of the Kalman observation point can be continued.

[0126] In some embodiments, when the observations of n consecutive times are all verified as unreliable, it can be regarded as a tracking failure. At the same time, a prompt message of tracking failure is sent to the user (or the control end of the autonomous mobile device, etc.), so as to prompt the user to repair the autonomous mobile device in time and warn of the risk of possible collision of the autonomous mobile device. Among them, n is a positive integer, and the specific size of n can be set by the developer himself.

[0127] In some embodiments, when the observation is verified as reliable, the observation point and the Kalman prediction point can be weighted and averaged to calculate the reliable position of the dynamic obstacle at the current moment. Add this reliable position to the trajectory corresponding to the dynamic obstacle, so as to realize the real-time tracking of the dynamic obstacle.

[0128] It should be noted that during the movement of the autonomous mobile device, new dynamic obstacles may appear at any time within the detection range of the sensor. Therefore, a period needs to be set, and steps S310 to S340 need to be executed once in each period to ensure the safety of the autonomous mobile device.

[0129] Based on steps S310 to S340, the dynamic trajectory points are predicted through Kalman filtering prediction, and combined with the reliability verification of the predicted points, the trajectory of the dynamic obstacle is continuously updated.

[0130] It should be understood that compared with steps S110 to S130, for each period, steps S310 to S340 only need to perform operations such as clustering on the perception data of the first few frames, and do not need to execute complex algorithms such as clustering for each frame, reducing resource waste and improving operation efficiency. However, the stability of the trajectory tracking of dynamic obstacles is lower. In practical applications, steps S110 to S130 or steps S310 to S340 can be selected according to actual needs to perform trajectory tracking of dynamic obstacles. For example, if the autonomous mobile device has sufficient computing power and computing resources and is more inclined to pursue the stability of dynamic obstacles to improve the tracking accuracy and accuracy of dynamic obstacles, steps S110 to S130 can be selected. On the contrary, if the computing power of the autonomous mobile device is weak and / or the computing resources are insufficient, steps S310 to S340 with less power consumption can be selected.

[0131] For ease of understanding, an exemplary embodiment is provided here to Figure 8 explain the trajectory tracking method provided by the embodiment shown.

[0132] See Figure 8 , Figure 8 is a flowchart of the trajectory tracking method for a certain frame during the movement of the autonomous mobile device. The perception information of the autonomous mobile device is obtained in advance through sensors, multiple frames of perception data are obtained, and obstacles are clustered from them. For each obstacle in each frame, the corresponding target point is obtained, and multiple target points are obtained. By clustering the multiple target points, the trajectories of multiple obstacles can be obtained. Among the trajectories of multiple obstacles, the trajectories of static obstacles are excluded, and only the trajectories of dynamic obstacles are retained.

[0133] At the current moment, for the trajectory of each dynamic obstacle, Kalman filtering prediction is performed to obtain the Kalman prediction points of the current frame; at the same time, the target point of the dynamic obstacle is obtained as the observation point according to the perception information collected in the current frame, and the last point of the trajectory of the dynamic obstacle is used as the tracking starting point.

[0134] Judge whether the current prediction is reliable according to a preset angular difference threshold and distance threshold. When the current prediction is reliable, update the reliable position of the dynamic obstacle at the current moment according to the Kalman prediction point and the observation point, and add it to the trajectory of the dynamic obstacle.

[0135] When the predictions are unreliable for multiple consecutive times, determine that the tracking fails and feedback a prompt message indicating the tracking failure to the user.

[0136] See Figure 9 , Figure 9 is a structural block diagram of a trajectory tracking device provided in an embodiment of the present application. The trajectory tracking device 100 can be applied to an autonomous mobile device. The trajectory tracking device 100 includes a data acquisition module 110, a target point determination module 120, and a trajectory determination module 130.

[0137] The data acquisition module 110 is used to acquire the perception data collected by the sensor. For the specific description of the data acquisition module 110, see step S110.

[0138] The target point determination module 120 is used to determine the target point of the obstacle according to the perception data, and the target point is the point on the obstacle contour that is closest to the autonomous mobile device. For the specific description of the target point determination module 120, see step S120 and steps S210 to S230.

[0139] The trajectory determination module 130 is used to determine the trajectory of the dynamic obstacle according to the target point of the obstacle. For the specific description of the trajectory determination module 130, see step S130.

[0140] Those skilled in the art can clearly understand that the trajectory tracking device 100 provided in the embodiment of the present application can implement the trajectory tracking method provided in the embodiment of the present application. For the specific working process of the above nodes, reference can be made to the corresponding process of the trajectory tracking method in the embodiment of the present application, which will not be elaborated here.

[0141] In the embodiments provided in the present application, the coupling, direct coupling or communication connection between the displayed or discussed modules can be indirect coupling or communication coupling through some interfaces, devices or modules, and can be in electrical, mechanical or other forms. The embodiments of the present application do not limit this.

[0142] In addition, in the embodiments of the present application, each functional module can be integrated in a processing module, or each module can exist physically alone, or two or more modules can be integrated in one module. The above integrated modules can be implemented in the form of hardware or in the form of software functional modules. The embodiments of the present application do not limit this.

[0143] See Figure 10 ,Figure 10 This is a structural block diagram of an autonomous mobile device provided by an embodiment of the present application. The autonomous mobile device 200 includes a memory 210 and one or more processors 220. Among them, one or more application programs are stored in the memory 210, and the one or more application programs are used to execute the methods in the above method embodiments when called by the one or more processors 220.

[0144] Among them, the processor 220 may include one or more processing cores. The processor 220 connects various parts within the entire autonomous mobile device 200 through various interfaces and lines. By running or executing instructions, programs, code sets or instruction sets stored in the memory 210, and by calling data stored in the memory 210, it executes various functions of the autonomous mobile device and processes data. The processor 220 may be implemented in at least one hardware form of digital signal processing (DSP), field programmable gate array (FPGA), or programmable logic array (PLA). The processor 220 may integrate one or several combinations of a central processing unit (CPU), a graphics processing unit (GPU), and a modem, etc. It can be understood that the above modem may not be integrated into the processor 220 and can be implemented separately through a communication chip.

[0145] The memory 210 may include a random access memory (RAM), and may also include a read only memory (ROM). The memory 210 may be used to store instructions, programs, codes, code sets or instruction sets. The memory 210 may include a program storage area and a data storage area. Among them, the program storage area may store instructions for implementing an operating system, instructions for implementing at least one function, instructions for implementing the above method embodiments, etc. The data storage area may store data created during the use of the autonomous mobile device 200.

[0146] [[ID=***]]The embodiment of the present application also provides a computer-readable storage medium, in which program code is stored, and the program code is used to execute the methods described in the above method embodiments when called by a processor. [[ID=***]]

[0147] A computer-readable storage medium may be an electronic memory such as a flash memory, an Electrically Erasable Programmable Read Only Memory (EEPROM), an Electrical Programmable Read Only Memory (EPROM), a hard disk, or a Read Only Memory (ROM). In some embodiments, the computer-readable storage medium may include a non-transitory computer-readable storage medium (NTRSM). The computer-readable storage medium has a storage space for program codes that execute any method steps in the above methods. These program codes may be read out from or written into one or more computer program products. The program codes may be compressed in a suitable form.

[0148] The above are only the preferred embodiments of the present application and do not impose any formal restrictions on the present application. Although the present application has been disclosed above with the preferred embodiments, it is not intended to limit the present application. Any person skilled in the art can make some changes or modifications to equivalent embodiments by using the above-disclosed technical content within the scope of the technical solution of the present application. However, as long as it does not depart from the content of the technical solution of the present application, any brief modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present application still fall within the scope of the technical solution of the present application.

Claims

1. A trajectory tracking method, characterized in that: The method comprises: Obtain the perception data collected by the sensor; Determining a target point of the obstacle based on the perception data, the target point being the point on the obstacle outline closest to the autonomous mobile device; Determine the trajectory of the dynamic obstacle based on the target point of the obstacle.

2. The method according to claim 1, characterized in that Determining the trajectory of the dynamic obstacle according to the target point of the obstacle includes: Cluster the target points of the obstacle to obtain at least one cluster, each cluster forming a trajectory of the obstacle; Determine the starting point and ending point of the obstacle's trajectory; The trajectory of the dynamic obstacle is determined from the trajectory of the obstacle according to the trajectory starting point and the trajectory end point.

3. The method according to claim 2, characterized in that The determining the trajectory of the dynamic obstacle from the trajectory of the obstacle according to the trajectory starting point and the trajectory end point includes: Determining an average speed of the obstacle based on the trajectory starting point and the trajectory ending point; If the average speed of the obstacle is greater than or equal to the speed threshold, the trajectory of the obstacle is determined to be a trajectory of a dynamic obstacle.

4. The method according to claim 2 or 3, characterized in that Determining the trajectory of the dynamic obstacle from the trajectory of the obstacle according to the trajectory starting point and the trajectory end point, further comprising: Determine a first angle based on the current position of the autonomous mobile device and the end position of the task; Determining a second angle according to the trajectory starting point and the trajectory end point; determining an absolute value of a difference between the first angle and the second angle; If the absolute value belongs to the specified angle interval, the trajectory corresponding to the absolute value is determined to be the trajectory of the dynamic obstacle.

5. The method according to claim 2, characterized in that Before determining the starting point and the end point of the obstacle's trajectory, the method further includes: If the number of points included in the obstacle trajectory is less than the number threshold, the obstacle trajectory is deleted.

6. The method according to claim 2, characterized in that Determining the starting point and the ending point of the obstacle's trajectory includes: The coordinates of the starting point of the obstacle trajectory are determined as the mean of the first N points in the obstacle trajectory, where N is a positive integer; The coordinates of the last N points in the obstacle's trajectory are averaged to determine the coordinates of the trajectory's end point.

7. The method according to claim 1, characterized in that Determining the target point of the obstacle according to the perception data includes: Clustering the sensing data to obtain at least one cluster, each cluster corresponding to an obstacle; Determining an obstacle outline and a center of the obstacle outline based on the points in the cluster; The target point of the obstacle is determined according to the points in the cluster, the center of the obstacle outline, the current position of the autonomous mobile device and the end position of this task.

8. The method according to claim 7, characterized in that The determining of the obstacle outline and the center of the obstacle outline according to the points in the clusters includes: Determine the obstacle outline according to the maximum horizontal coordinate, minimum horizontal coordinate, maximum vertical coordinate and minimum vertical coordinate in the cluster; The mean of the minimum and maximum abscissas in the cluster is used as the abscissa of the center of the obstacle outline; The mean of the minimum ordinate and the maximum ordinate in the cluster is used as the ordinate of the center of the obstacle outline.

9. The method according to claim 8, characterized in that The determining of the target point of the obstacle according to the points in the cluster, the center of the obstacle outline, the current position of the autonomous mobile device, and the end position of the current task includes: Determining a first angle according to the current position and the end position; A target point of the obstacle is determined according to the points in the cluster, the center of the obstacle outline, the current position, and the first angle.

10. The method according to claim 9, characterized in that The determining the target point of the obstacle according to the points in the cluster, the center of the obstacle outline, the current position, and the first angle includes: If the first angle is within the first angle interval and the ordinate of the center is greater than the ordinate of the current position, determining the minimum abscissa in the cluster as the abscissa of the target point, and determining the minimum ordinate in the cluster as the ordinate of the target point; If the first is in the first angle interval and the vertical coordinate of the center is less than or equal to the vertical coordinate of the current position, the minimum horizontal coordinate in the cluster is determined as the horizontal coordinate of the target point, and the maximum vertical coordinate in the cluster is determined as the vertical coordinate of the target point.

11. The method according to claim 9, characterized in that The determining the target point of the obstacle according to the points in the cluster, the center of the obstacle outline, the current position, and the first angle includes: If the first angle is within the second angle interval and the abscissa of the center is greater than the abscissa of the current position, determining the minimum abscissa in the cluster as the abscissa of the target point, and determining the minimum ordinate in the cluster as the ordinate of the target point; If the first angle is within the second angle interval and the abscissa of the center is less than or equal to the abscissa of the current position, the maximum abscissa in the cluster is determined as the abscissa of the target point, and the minimum ordinate in the cluster is determined as the ordinate of the target point.

12. The method according to claim 9, characterized in that The determining the target point of the obstacle according to the points in the cluster, the center of the obstacle outline, the current position, and the first angle includes: If the first angle is within the third angle interval and the abscissa of the center is greater than the abscissa of the current position, the minimum abscissa in the cluster is determined as the abscissa of the target point, and the maximum ordinate in the cluster is determined as the ordinate of the target point; If the first angle is in the third angle interval and the abscissa of the center is less than or equal to the abscissa of the current position, the maximum abscissa in the cluster is determined as the abscissa of the target point, and the maximum ordinate in the cluster is determined as the ordinate of the target point.

13. The method according to claim 9, characterized in that The determining the target point of the obstacle according to the points in the cluster, the center of the obstacle outline, the current position, and the first angle includes: If the first angle is within a fourth angle interval and the ordinate of the center is greater than the ordinate of the current position, determining the maximum abscissa in the cluster as the abscissa of the target point, and determining the minimum ordinate in the cluster as the ordinate of the target point; If the first is in the fourth angle interval and the vertical coordinate of the center is less than or equal to the vertical coordinate of the current position, the maximum horizontal coordinate in the cluster is determined as the horizontal coordinate of the target point, and the maximum vertical coordinate in the cluster is determined as the vertical coordinate of the target point.

14. The method according to claim 1, wherein Before determining the trajectory of the dynamic obstacle according to the target point of the obstacle, the method further includes: The target points of the obstacles are saved into a target point set, each of which has a collection time; Determine the difference between the acquisition time of the current frame perception data and the acquisition time of the target point; Delete target points in the target point set whose difference is greater than the difference threshold.

15. A trajectory tracking device, characterized in that: The device comprises: A data acquisition module is used to obtain the perception data collected by the sensor; a target point determination module, configured to determine a target point of the obstacle based on the sensing data, wherein the target point is the point on the obstacle outline closest to the autonomous mobile device; The trajectory determination module is used to determine the trajectory of the dynamic obstacle based on the target point of the obstacle.

16. An autonomous mobile device, characterized in that: include: Memory; one or more processors; One or more application programs are stored in the memory and are configured to execute the method according to any one of claims 1 to 14 when called by the one or more processors.

17. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores program code, and the program code is used to execute the method according to any one of claims 1 to 14 when called by a processor.

Images