A dynamic obstacle target detection and tracking method for a patrol robot

Abstract

The application relates to a dynamic obstacle target detection and tracking method of an inspection robot, and belongs to the technical field of robots. An adaptive Euclidean clustering algorithm is used to cluster point cloud information obtained by a laser radar loaded on the inspection robot to obtain an obstacle target class; based on this, three-dimensional target frame and center point position information of the target frame of the obstacle target class are obtained; finally, Kalman filtering is used to model the center position information of the obstacle target to predict the motion trajectory of the target. Meanwhile, an equivalent measurement value data association algorithm is introduced to associate the position information of the targets of two adjacent frames, so that the target detection and continuous tracking of the dynamic obstacle are realized. Through the detection and continuous tracking of the dynamic obstacle, the motion trajectory of the obstacle target information can be better predicted, and effective information input for the avoidance of the dynamic obstacle by the inspection robot is provided.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology and relates to a method for dynamic obstacle target detection and tracking of an inspection robot. Background Technology

[0002] Currently, most factories still rely on manual inspections for daily checks, with inspectors typically conducting inspections every two hours. Methods include visual observation, listening, and handheld temperature checks, depending on the inspector's experience. Manual inspections pose significant safety hazards. Therefore, manual inspections are not only time-consuming and labor-intensive but also fail to promptly identify production problems, making them unsuitable for the demands of intelligent manufacturing in today's competitive market.

[0003] In research on inspection robots, obstacle detection and tracking technologies are core technologies and key to achieving intelligent mobility. Currently, common obstacle detection methods mainly rely on single-frame detection, failing to predict and track obstacle trajectories. This leads to inaccurate obstacle location information and increases the risk of accidents during inspections. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide a method for dynamic obstacle target detection and tracking of an inspection robot.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A method for dynamic obstacle target detection and tracking of an inspection robot, the method comprising the following steps:

[0007] An adaptive Euclidean clustering algorithm is used to cluster the point cloud information obtained by the lidar on the inspection robot to obtain obstacle target families;

[0008] Obtain the 3D bounding box and center point location information of the obstacle target family;

[0009] Kalman filtering is used to model the center position information of the obstacle target and predict the target's trajectory;

[0010] A data association algorithm based on equivalent measurements is introduced to associate the positional information of targets in two adjacent frames, thereby enabling target detection and continuous tracking of dynamic obstacles.

[0011] Optionally, the adaptive Euclidean clustering algorithm is as follows: the sensing area of ​​the LiDAR is divided into several concentric circles, and the clustering threshold of different concentric circles changes autonomously to adjust the clustering threshold and reduce the difficulty coefficient of determining the threshold.

[0012] Optionally, the adaptive Euclidean clustering algorithm is specifically as follows:

[0013] Let the point cloud data to be segmented be M = {p i |p i =(x i ,y i ,z i )∈R 3 ,i=1,...,I}, where I represents the number of points contained in a single laser beam; C i C j If C represents two families in M, then C i C j The following relationship must be satisfied:

[0014]

[0015]

[0016] In the formula, p i ∈C i p j ∈C j d thres The threshold for clustering is defined in the formula; the distance relationship between two clusters cannot overlap; the beam angle interval θ of the 16-line lidar is 2°, and as the distance increases, fewer and fewer laser beams are scanned from the target; the variable threshold d is designed based on the distance range between the clustered target and the lidar. * :

[0017]

[0018] In the formula, R represents the distance between the clustered target and the center of the lidar, and θ represents the spacing between adjacent lidar beams; the longer the laser beam scanning distance, the higher the threshold d. * The larger the value, the better. Divide the input point cloud into several concentric circles, set a corresponding clustering distance threshold for each concentric circle region, and then use kd-tree to perform nearest neighbor search to complete the point cloud segmentation based on the clustering distance threshold.

[0019] Optionally, the acquisition of the 3D target bounding box and the center point position information of the target bounding box is as follows: after obtaining a target class, the point cloud inside tends to cluster around a center. The boundary of the rectangular box is determined by finding the maximum and minimum points in the x, y, and z directions. The center position information of the 3D rectangular box is obtained through geometric calculation.

[0020] Optionally, the Kalman filter is used to model the center position information of the obstacle target as follows: the motion state of the obstacle target is described as X = [x, y, z, v]. x ,v y ,v zThe model's observations are Z = [x, y, z], where X represents the motion state quantity, Z represents the observation quantity, and x, y, and z represent the coordinates of the target's motion state in the lidar coordinate system, respectively. x V represents the velocity in the x-axis direction. y V represents the velocity in the y-axis direction. z This represents the velocity in the z-axis direction.

[0021] Optionally, the data association algorithm is: a nearest neighbor algorithm based on Euclidean distance, which calculates the distance index between the predicted measurement value and the measurement value by setting an association threshold. The measurement value falling within the threshold is a valid measurement value. The reciprocal of each valid measurement value is taken and normalized to obtain the weight of each valid measurement value.

[0022] Optionally, the association threshold expression is:

[0023] [Z(k)-Z(k|k-1)] T S(k) -1 [Z(k)-Z(k|k-1)]≤γ (1-4)

[0024] In the formula, γ is the threshold value, Z(k) is the measured value, Z(k|k-1) is the predicted measured value, and S(k) is the covariance matrix of the vector difference between the measured value and the predicted measured value. When there are multiple measured values ​​within the threshold, the effective measured value with the smallest Euclidean distance from the predicted measured value is selected as the correct measured value and associated with the target.

[0025] For d jp Take the reciprocal and normalize to obtain the weight of each valid measurement;

[0026]

[0027] In the formula, j represents the nth valid measurement value, p represents the predicted value label at this time, and m(k) represents the number of valid measurements in the current target association gate;

[0028] Multiple valid measurements can be combined into a single equivalent measurement by using the weights of the valid measurements:

[0029]

[0030] Introducing equivalent measurements allows for the combination of all measured information within the gate, ensuring the accuracy of data association and continuous tracking of dynamic obstacle targets.

[0031] The beneficial effects of this invention are: by detecting and tracking dynamic obstacles, it helps inspection robots achieve more effective and reliable obstacle avoidance, realizing highly stable and high-precision autonomous inspection functions. This makes inspection robots more durable and reliable for factory inspections, meeting various inspection needs.

[0032] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0033] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0034] Figure 1 This is a flowchart of the dynamic obstacle detection and tracking process of the inspection robot of the present invention;

[0035] Figure 2 This is a flowchart of the adaptive Euclidean clustering algorithm of the present invention;

[0036] Figure 3 This is a schematic diagram of the Kalman filter model and data association of the present invention. Detailed Implementation

[0037] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0038] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0039] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0040] like Figure 1 As shown, the method for dynamic obstacle target detection and tracking of an inspection robot provided by this invention includes the following steps: First, an adaptive Euclidean clustering algorithm is used to cluster the point cloud information obtained by the lidar mounted on the inspection robot to obtain obstacle target families. Based on this, the 3D target bounding boxes and the center point position information of the target bounding boxes are obtained. Then, Kalman filtering is used to model the center position information of the obstacle targets and predict their motion trajectory. Simultaneously, a data association algorithm based on equivalent measurements is introduced to associate the position information of targets in adjacent frames, thereby achieving target detection and continuous tracking of dynamic obstacles.

[0041] Specifically, the flowchart of the adaptive Euclidean clustering algorithm is as follows: Figure 2 As shown,

[0042] Let the point cloud data to be segmented be M = {p i |p i =(x i ,y i ,z i )∈R 3 ,i=1,...,I}, where I represents the number of points contained in a single laser beam. C i C j Let C be two families in M. i C j The following relationship should be satisfied:

[0043]

[0044]

[0045] In the formula, p i ∈C i p j ∈C j d thresThe threshold for clustering is given by the formula. The formula describes the distance relationship between two clusters, which must not overlap. The beam angle interval θ = 2° for a 16-line lidar, so as the distance increases, fewer laser beams are scanned on the target, and the distance between two consecutive points can be large. Therefore, a variable threshold d is designed based on the distance range between the clustered target and the lidar. * :

[0046]

[0047] In the formula, R represents the distance between the clustered target and the center of the lidar, and θ represents the spacing between adjacent lidar beams. The longer the laser beam scanning distance, the higher the threshold d. * The larger the value, the better. In summary, the input point cloud is divided into several concentric layers, and a corresponding clustering distance threshold is set for each layer. Then, a kd-tree is used for nearest neighbor search, and the point cloud is segmented based on the clustering distance threshold.

[0048] Furthermore, after segmentation, the boundaries of the bounding box are determined by finding the maximum and minimum points in the x, y, and z directions of the family point cloud. Then, geometric calculations are used to obtain the center position information of the 3D bounding box.

[0049] Furthermore, such as Figure 3 The Kalman filter algorithm is used to model the motion state of the obstacle target as X = [x, y, z, v]. x ,v y ,v z The model's observations are Z = [x, y, z]. By setting an association threshold, a distance index is calculated between the predicted and actual measurements; only measurements falling within the threshold are considered valid. The threshold expression is:

[0050] [Z(k)-Z(k|k-1)] T S(k) -1 [Z(k)-Z(k|k-1)]≤γ (1-4)

[0051] In the formula, γ is the threshold value, Z(k) is the measured value, Z(k|k-1) is the predicted measured value, and S(k) is the covariance matrix of the vector difference between the measured value and the predicted measured value. When there are multiple measured values ​​within the threshold, the effective measured value with the smallest Euclidean distance from the predicted measured value is selected as the correct measured value and associated with the target.

[0052] Measurements within the threshold are considered valid measurements. Since valid measurements farther from the predicted measurement should have lower weights, therefore, for d... jp Take the reciprocal and normalize it to obtain the weight of each valid measurement.

[0053]

[0054] Multiple valid measurements can be combined into a single equivalent measurement by using the weights of the valid measurements:

[0055]

[0056] Introducing equivalent measurements allows for the combination of all measured information within the gate, ensuring the accuracy of data association and continuous tracking of dynamic obstacle targets.

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

Claims

1. A method for dynamic obstacle target detection and tracking of an inspection robot, characterized in that: The method includes the following steps: An adaptive Euclidean clustering algorithm is used to cluster the point cloud information obtained by the lidar on the inspection robot to obtain obstacle target families; Obtain the 3D bounding box and center point location information of the obstacle target family; Kalman filtering is used to model the center position information of the obstacle target and predict the target's trajectory; A data association algorithm based on equivalent measurements is introduced to associate the positional information of targets in two adjacent frames, thereby enabling target detection and continuous tracking of dynamic obstacles; The adaptive Euclidean clustering algorithm is as follows: the sensing area of ​​the LiDAR is divided into several concentric circles, and the clustering threshold of different concentric circles changes autonomously. Adjusting the clustering threshold reduces the difficulty coefficient of determining the threshold. The adaptive Euclidean clustering algorithm is specifically as follows: Let the point cloud data to be segmented be... In the formula I Indicates the number of points contained in a single laser beam; If they are two families in M, then The following relationship must be satisfied: (1-1) for (1-2) In the formula, , , The threshold value is used for clustering; the distance relationship between two clusters cannot overlap; as the distance increases, fewer laser beams are scanned from the target; the variable threshold is designed based on the distance range between the clustered target and the lidar. : (1-3) In the formula, R This represents the distance between the clustered target and the center of the lidar. This indicates the spacing between adjacent laser beams in a lidar system; the longer the laser beam scanning distance, the higher the threshold. The larger the value, the better. Divide the input point cloud into several concentric circles, set a corresponding clustering distance threshold for each concentric circle region, and then use kd-tree to perform nearest neighbor search to complete the point cloud segmentation based on the clustering distance threshold. The method of modeling the center position information of the obstacle target using Kalman filtering is as follows: describing the motion state of the obstacle target as... The model's observations are In the formula, X represents the motion state quantity. Z Representing the observed quantity, x , y and z These represent the coordinates of the target's motion state in the lidar coordinate system. Indicates in x Velocity in the axial direction, Indicates in y Velocity in the axial direction, Indicates in z Velocity in the axial direction; The data association algorithm is: a nearest neighbor algorithm based on Euclidean distance. By setting an association threshold, the distance index between the predicted measurement value and the measurement value is calculated. The measurement value falling within the threshold is a valid measurement value. The reciprocal of each valid measurement value is taken and normalized to obtain the weight of each valid measurement value. The association threshold expression is: (1-4) In the formula, γ is the threshold value. For measured values, To predict the measured value, This is the covariance matrix of the vector difference between the measured value and the predicted measured value. When there are multiple measured values ​​within the gate, the effective measured value with the smallest Euclidean distance from the predicted measured value is selected as the correct measured value and associated with the target. right Take the reciprocal and normalize to obtain the weight of each valid measurement; (1-5) In the formula j Indicates the nth valid measurement value. p This indicates the label of the predicted value at this point. m ( k () indicates the number of valid measurements within the current target associated gate; Multiple valid measurements can be combined into a single equivalent measurement by using the weights of the valid measurements: (1-6) Introducing equivalent measurements allows for the combination of all valid measurements within the gate, ensuring the accuracy of data correlation and continuous tracking of dynamic obstacle targets.

2. The method for dynamic obstacle target detection and tracking of an inspection robot according to claim 1, characterized in that: The process of obtaining the 3D target bounding box and the center point position information of the obstacle target family is as follows: After obtaining a target family, the point cloud within it tends to cluster around a center. This is achieved by finding the center point within the target family. x , y , z The boundaries of the rectangle are determined by the maximum and minimum points in three directions, and the center position information of the three-dimensional rectangle is obtained through geometric calculations.

Images