Laser radar tracking method, device, electronic device and storage medium

Abstract

The present application discloses a laser radar tracking method, device, electronic device and storage medium, the method comprising: acquiring multiple frames of original point cloud data; using a first preset algorithm to process the original point cloud data of two adjacent frames to determine the tracking target; using a second preset algorithm to determine the position and posture of the tracking target in two adjacent frames, and obtaining the initial speed value of the tracking target according to the position and posture of the tracking target in two adjacent frames; using a third preset algorithm according to the position and posture of the tracking target in two adjacent frames, the initial speed value of the tracking target and the original point cloud data of the tracking target in two adjacent frames, to obtain the trajectory estimation result of the tracking target. The method provided by the present invention uses the inter-frame target point cloud matching results to solve the inter-frame position change and heading angle change, thereby realizing the accurate estimation of the position and posture change and motion trajectory of the tracking target.

Description

Technical Field

[0001] The present invention relates to the field of laser radar tracking, and in particular to a laser radar tracking method, device, electronic device and computer-readable storage medium. Background Art

[0002] With the development of science and technology, intelligent transportation technology can effectively improve the traffic management department's ability to dispatch traffic on roads, improve transportation efficiency, enhance traffic safety, and thus reduce energy consumption and environmental pollution caused by existing traffic problems. LiDAR has a centimeter-level spatial resolution, which is suitable for local traffic environment perception. It is currently widely used in simultaneous localization and mapping (SLAM), mobile mapping, target recognition and tracking, etc.

[0003] Traditional multi-object tracking (MOT) mainly uses continuous sensor observation data to establish a corresponding relationship between targets, and continuously estimates and predicts the motion state of multiple targets based on the established matching cost function. However, due to the changes in the scanning angle of different laser radar frames, the shape of the scanned moving target point cloud is usually constantly changing. The existing target tracking method ignores the change in the centroid position of the target point cloud during tracking, and represents the position of the target with the same centroid position, resulting in a large error between the estimation of the tracked target trajectory and position change and the actual situation, making it difficult to accurately solve the target's true motion trajectory, and it is also impossible to accurately estimate the target's heading change.

[0004] Therefore, it is urgent to solve the problem that the estimation of target trajectory and position change is relatively different from the actual situation due to the existing technology that only the target's center of mass position is used to represent the target position, and the trajectory change of the tracked target cannot be accurately estimated. Summary of the invention

[0005] In view of this, it is necessary to provide a laser radar tracking method, device, electronic device and computer-readable storage medium to solve the problems of inaccurate state parameter estimation and mistracking of the tracking target in the prior art.

[0006] In order to solve the above problems, the present invention provides a laser radar tracking method, comprising:

[0007] Get multiple frames of original point cloud data;

[0008] Processing the original point cloud data of two adjacent frames using a first preset algorithm to determine a tracking target;

[0009] Determine the position and posture of the tracking target in two adjacent frames by using a second preset algorithm, and obtain an initial velocity value of the tracking target according to the position and posture of the tracking target in two adjacent frames;

[0010] A trajectory estimation result of the tracking target is obtained by using a third preset algorithm according to the position and posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target, and the original point cloud data of the tracking target in two adjacent frames.

[0011] Further, the original point cloud data of adjacent frames are processed using a first preset algorithm to obtain a tracking target, including:

[0012] Extracting a target to be tracked according to the original point cloud data of the adjacent frames;

[0013] Calculate the similarity of the target to be tracked in two adjacent frames using a nearest neighbor data association algorithm;

[0014] The tracking target is determined according to the similarity between the two adjacent frames.

[0015] Furthermore, the similarity between two adjacent frames MatSimi(i,j) includes the integer part posSimi i,j and decimal part geoSimi i,j ,

[0016] MatSimi(i,j)=posSimi i,j +geoSimi i,j ;in:

[0017]

[0018]

[0019] is the distance between the center of mass of the target to be tracked in two adjacent frames, d Thre is the preset plane distance threshold, and are the side lengths of the target to be tracked in the kth frame and the k+1th frame respectively, radius=2(length+width), and round means rounding the number to the unit digit.

[0020] Further, when the center-of-mass plane distance of the target to be tracked in two adjacent frames is greater than the plane distance threshold, it is determined that the similarity of the target to be tracked in the two adjacent frames is 0, and the target to be tracked is not a tracking target.

[0021] Furthermore, for the target to be tracked in the Kth frame of original point cloud data, the target to be tracked with the largest similarity and greater than 0 is selected in the K+1th frame of point cloud data as the tracking target.

[0022] Further, the position and posture of the tracking target in two adjacent frames are determined by using a second preset algorithm, including: using Kalman filtering to calculate the position and posture of the tracking target in two adjacent frames.

[0023] Further, a trajectory estimation result of the tracking target is obtained according to the position and posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target, and the original point cloud data of the tracking target in two adjacent frames using a third preset algorithm, including:

[0024] Obtaining initial transformation parameters according to the position and posture of the tracking target in two adjacent frames;

[0025] According to the initial transformation parameters and the original point cloud data of the tracking target in two adjacent frames, the precise transformation parameters are solved by using the ICP algorithm;

[0026] A trajectory estimation result of the tracked target is obtained according to the precise transformation parameter and the initial value of the speed of the tracked target.

[0027] The present invention also provides a laser radar tracking device, comprising:

[0028] An acquisition module is used to acquire multiple frames of original point cloud data;

[0029] A tracking target determination module, used to process the original point cloud data of two adjacent frames using a first preset algorithm to determine the tracking target;

[0030] A calculation module, which uses a second preset algorithm to determine the position and posture of the tracking target in two adjacent frames, and obtains an initial value of the speed of the tracking target according to the position and posture of the tracking target in two adjacent frames;

[0031] The trajectory estimation module obtains a trajectory estimation result of the tracking target according to the posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target and the original point cloud data of the tracking target in two adjacent frames by using a third preset algorithm.

[0032] The present invention also provides an electronic device, including a processor and a memory, wherein a computer program is stored in the memory, and when the computer program is executed by the processor, the laser radar tracking method as described in any of the above technical solutions is implemented.

[0033] The present invention also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the laser radar tracking method as described in any of the above technical solutions is implemented.

[0034] The beneficial effect of adopting the above embodiment is: the laser radar tracking method provided by the present invention firstly processes the original point cloud data of two adjacent frames using the first preset algorithm to determine the tracking target; secondly, determines the position and posture of the tracking target in two adjacent frames using the second preset algorithm and obtains the initial value of the speed of the tracking target; finally, obtains the trajectory estimation result of the tracking target according to the position and posture of the tracking target in two adjacent frames, the initial value of the speed of the tracking target and the original point cloud data of the tracking target in two adjacent frames using the third preset algorithm. The method provided by the present invention uses the inter-frame target point cloud matching results to solve the inter-frame position change and heading angle change, thereby realizing the accurate estimation of the position and posture change and motion trajectory of the tracking target. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 A schematic diagram of a scene of an embodiment of an application system of the laser radar tracking method provided by the present invention;

[0036] Figure 2 A schematic diagram of a flow chart of an embodiment of a laser radar tracking method provided by the present invention

[0037] Figure 3 A schematic structural diagram of an embodiment of a laser radar tracking device provided by the present invention;

[0038] Figure 4 It is a structural block diagram of an embodiment of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0039] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0040] The present invention provides a laser radar tracking method, device, electronic device and computer-readable storage medium, which are described in detail below.

[0041] Figure 1 Schematic diagram of an application scenario of the laser radar tracking device provided by the present invention. The system may include a server 100, in which the laser radar tracking device is integrated. Figure 1 Servers in .

[0042] In the embodiment of the present invention, the server 100 is mainly used for:

[0043] Get multiple frames of original point cloud data;

[0044] Processing the original point cloud data of two adjacent frames using a first preset algorithm to determine a tracking target;

[0045] Determine the position and posture of the tracking target in two adjacent frames by using a second preset algorithm, and obtain an initial velocity value of the tracking target according to the position and posture of the tracking target in two adjacent frames;

[0046] A trajectory estimation result of the tracking target is obtained by using a third preset algorithm according to the position and posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target, and the original point cloud data of the tracking target in two adjacent frames.

[0047] In the embodiment of the present invention, the server 100 may be an independent server, or a server network or server cluster composed of servers. For example, the server 100 described in the embodiment of the present invention includes but is not limited to a computer, a network host, a single network server, a plurality of network server sets or a cloud server composed of a plurality of servers. The cloud server is composed of a large number of computers or network servers based on cloud computing.

[0048] It is to be understood that the terminal 200 used in the embodiment of the present invention may be a device including both receiving and transmitting hardware, that is, a device having receiving and transmitting hardware capable of performing two-way communication on a two-way communication link. Such a device may include: a cellular or other communication device having a single-line display or a multi-line display or a cellular or other communication device without a multi-line display. The specific terminal 200 may be a desktop computer, a portable computer, a network server, a PDA (Personal Digital Assistant), a mobile phone, a tablet computer, a wireless terminal device, a communication device, an embedded device, etc. The present embodiment does not limit the type of the terminal 200.

[0049] Those skilled in the art will understand that Figure 1 The application environment shown in the figure is only one application scenario of the solution of the present invention and does not constitute a limitation on the application scenario of the solution of the present invention. Other application environments may also include Figure 1 More or fewer terminals as shown in Figure 1 Only two terminals are shown. It can be understood that the laser radar tracking device can also include one or more other terminals, which are not limited here.

[0050] In addition, refer to Figure 1 As shown, the laser radar tracking device may also include a memory 300 for storing data, such as point cloud data, initial value of tracking target speed and other data.

[0051] It should be noted that Figure 1The scene diagram of the laser radar tracking device shown is only an example. The laser radar tracking device and the scene described in the embodiment of the present invention are intended to more clearly illustrate the technical solution of the embodiment of the present invention, and do not constitute a limitation on the technical solution provided by the embodiment of the present invention. Ordinary technicians in this field can know that with the evolution of the laser radar tracking device and the emergence of new business scenarios, the technical solution provided by the embodiment of the present invention is also applicable to similar technical problems.

[0052] The embodiment of the present invention provides a laser radar tracking method, and its flow chart is as follows: Figure 2 As shown, the laser radar tracking method includes:

[0053] Step S201, obtaining multiple frames of original point cloud data;

[0054] Step S202: Processing the original point cloud data of two adjacent frames using a first preset algorithm to determine a tracking target;

[0055] Step S203, using a second preset algorithm to determine the position and posture of the tracking target in two adjacent frames, and obtaining an initial velocity value of the tracking target according to the position and posture of the tracking target in two adjacent frames;

[0056] Step S204: Obtain a trajectory estimation result of the tracking target according to the position and posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target, and the original point cloud data of the tracking target in two adjacent frames using a third preset algorithm.

[0057] In an embodiment of the present invention, first, the original point cloud data of two adjacent frames are processed using a first preset algorithm to determine the tracking target; secondly, the position and posture of the tracking target in two adjacent frames are determined using a second preset algorithm and the initial velocity value of the tracking target is obtained; finally, the trajectory estimation result of the tracking target is obtained based on the position and posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target and the original point cloud data of the tracking target in two adjacent frames using a third preset algorithm. The method provided by the present invention uses the inter-frame target point cloud matching results to solve the inter-frame position change and heading angle change, thereby achieving accurate estimation of the position and posture change and motion trajectory of the tracking target.

[0058] Preferably, in step S202, the original point cloud data of adjacent frames are processed using a first preset algorithm to obtain a tracking target, including:

[0059] Extracting a target to be tracked according to the original point cloud data of the adjacent frames;

[0060] Calculate the similarity of the target to be tracked in two adjacent frames using a nearest neighbor data association algorithm;

[0061] The tracking target is determined according to the similarity between the two adjacent frames.

[0062] Preferably, the similarity between two adjacent frames MatSimi(i,j) includes an integer part posSimi i,j and decimal part geoSimi i,j ,

[0063] MatSimi(i,j)=posSimi i,j +geoSimi i,j ;in:

[0064]

[0065]

[0066] is the distance between the centroid planes of the target to be tracked in two adjacent frames measured by the same laser radar, d Thre is the preset plane distance threshold, and are the side lengths of the target to be tracked in the kth frame and the k+1th frame respectively, radius=2(length+width), and round means rounding the number to the unit digit.

[0067] As a specific embodiment, for a target passing through an urban scene, the target's moving speed generally does not exceed 20 m / s, and the sampling frequency of the laser radar is 5 Hz. Therefore, the plane position change of the same tracked target in adjacent frames should be less than (20 m / s × 0.2 s) = 4 m. You can choose d Thre =8m.

[0068] Preferably, when the center-of-mass plane distance of the target to be tracked in two adjacent frames is greater than the plane distance threshold, it is determined that the similarity of the target to be tracked in the two adjacent frames is 0, and the target to be tracked is not a tracking target.

[0069] Preferably, for the target to be tracked in the Kth frame of original point cloud data, the target to be tracked with the largest similarity greater than 0 in the K+1th frame of point cloud data is selected as the tracking target.

[0070] As a specific embodiment, all the targets to be tracked in the kth frame are identified and summarized into a set Identify all the targets to be tracked in the k+1th frame and summarize them into a set Determining the tracking target means: finding the same target to be tracked in two consecutive frames; the same target to be tracked in the two frames is the tracking target. After determining the tracking target, G k and G k+1The same targets to be tracked are merged to form a new target set to be tracked in the k+1th frame, which is used for calculation with the target set to be tracked in the k+2th frame.

[0071] As a specific embodiment: for the target to be tracked in two adjacent frames, calculate its similarity in the kth frame and the k+1th frame, and select the target to be tracked with the maximum similarity in the two adjacent frames and posSimi greater than 0 as the tracking target; when a new target appears in the ranging range of the laser radar, add the new target to be tracked to the list of targets to be tracked.

[0072] As a specific embodiment, when the same target to be tracked in consecutive frames is blocked or leaves the ranging range of the laser radar (the target to be tracked is not tracked in 5 consecutive frames), it is determined that the tracking of the target to be tracked is lost, and the target to be tracked is deleted from the list of targets to be tracked, and the tracking of the target to be tracked is ended.

[0073] Preferably, in step S203, the position and posture of the tracking target in two adjacent frames are determined using a second preset algorithm, including: using Kalman filtering to calculate the position and posture of the tracking target in two adjacent frames.

[0074] As a specific embodiment, in step S203, due to the small ground undulation in the urban road environment, when the tracking target moves within the scanning range of the same fixed laser radar, it can be considered that its elevation changes very little and only moves in a plane. The centroid positions of the tracking target in the kth frame and the k+1th frame are calculated by using Kalman filtering. and Set the tracking target to move in a uniform linear motion, according to the formula v k =(x k+1 -x k ) / t;v k =(y k+1 -y k ) / t; t is the laser radar sampling period, and the initial value of the speed of the tracking target is obtained.

[0075] Preferably, in step S204, a trajectory estimation result of the tracking target is obtained according to the posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target and the original point cloud data of the tracking target in two adjacent frames using a third preset algorithm, including:

[0076] Obtaining initial transformation parameters according to the position and posture of the tracking target in two adjacent frames;

[0077] According to the initial transformation parameters and the original point cloud data of the tracking target in two adjacent frames, the precise transformation parameters are solved by using the ICP algorithm;

[0078] A trajectory estimation result of the tracked target is obtained according to the precise transformation parameter and the initial value of the speed of the tracked target.

[0079] As a specific embodiment, since the position change of the dynamic target on the road in the adjacent frames of the laser radar (the sampling interval is 0.05-0.2Hz) is usually small, the difference in the centroid distance change of the tracked target in two adjacent frames calculated by Kalman filtering is used as the initial displacement value in the initial transformation parameter; the distance threshold D in the ICP function parameter is outliers Set to 0.1m; use the ICP function provided by the PCL library to align the point clouds of the tracked target in adjacent frames to obtain accurate transformation parameters; and then estimate the trajectory of the tracked target in combination with the initial velocity value of the tracked target.

[0080] As a specific embodiment, for a tracking target that is successfully tracked in multiple consecutive frames, point cloud results obtained by multi-view scanning of the tracking target by a laser radar are fused to restore the three-dimensional shape of the tracking target.

[0081] The embodiment of the present invention provides a laser radar tracking device, and its structural block diagram is as follows: Figure 3 As shown, the laser radar tracking device 3000 includes:

[0082] An acquisition module 3001 is used to acquire multiple frames of original point cloud data;

[0083] A tracking target determination module 3002 is used to process the original point cloud data of two adjacent frames using a first preset algorithm to determine a tracking target;

[0084] The calculation module 3003 determines the position and posture of the tracking target in two adjacent frames by using a second preset algorithm, and obtains an initial value of the speed of the tracking target according to the position and posture of the tracking target in two adjacent frames;

[0085] The trajectory estimation module 3004 obtains a trajectory estimation result of the tracking target according to the position and posture of the tracking target in two adjacent frames, the initial velocity value of the tracking target and the original point cloud data of the tracking target in two adjacent frames using a third preset algorithm.

[0086] like Figure 4 As shown, the laser radar tracking method described above, the present invention also provides an electronic device, which can be a computing device such as a mobile terminal, a desktop computer, a notebook, a palm computer, and a server. The electronic device includes a processor 10, a memory 20, and a display 30.

[0087] In some embodiments, the memory 20 may be an internal storage unit of a computer device, such as a hard disk or memory of the computer device. In other embodiments, the memory 20 may also be an external storage device of the computer device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. equipped on the computer device. Furthermore, the memory 20 may also include both an internal storage unit of the computer device and an external storage device. The memory 20 is used to store application software and various types of data installed on the computer device, such as program codes for installing the computer device. The memory 20 may also be used to temporarily store data that has been output or is to be output. In one embodiment, a laser radar tracking program 40 is stored on the memory 20, and the laser radar tracking program 40 can be executed by the processor 10, thereby realizing the laser radar tracking method of each embodiment of the present invention.

[0088] In some embodiments, the processor 10 may be a central processing unit (CPU), a microprocessor or other data processing chip, used to run program codes or process data stored in the memory 20, such as executing a lidar tracking program.

[0089] In some embodiments, the display 30 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, an OLED (Organic Light-Emitting Diode) touch device, etc. The display 30 is used to display information on the computer device and to display a visual user interface. The components 10-30 of the computer device communicate with each other through a system bus.

[0090] This embodiment also provides a computer-readable storage medium on which a laser radar tracking program is stored. When the computer program is executed by a processor, the laser radar tracking method described in any of the above technical solutions is implemented.

[0091] The present invention discloses a laser radar tracking method, device, electronic device and computer-readable storage medium. First, the original point cloud data of two adjacent frames are processed by a first preset algorithm to determine the tracking target; secondly, the position and posture of the tracking target in two adjacent frames are determined by a second preset algorithm and the initial speed value of the tracking target is obtained; finally, a trajectory estimation result of the tracking target is obtained according to the position and posture of the tracking target in two adjacent frames, the initial speed value of the tracking target and the original point cloud data of the tracking target in two adjacent frames by a third preset algorithm; the position change and heading angle change between frames are solved by using the target point cloud matching result between frames, so as to realize the accurate estimation of the position and posture change and motion trajectory of the tracking target.

[0092] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0093] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A laser radar tracking method, characterized in that: include: Get multiple frames of original point cloud data; Extract the target to be tracked based on the original point cloud data of two adjacent frames; Calculate the similarity of the target to be tracked in two adjacent frames using a nearest neighbor data association algorithm; Determining a tracking target according to the similarity between the two adjacent frames; The Kalman filter is used to calculate the position and posture of the tracking target in two adjacent frames, and the initial value of the speed of the tracking target is obtained according to the position and posture of the tracking target in two adjacent frames; According to the position and posture of the tracking target in two adjacent frames, initial transformation parameters are obtained; according to the initial transformation parameters and the original point cloud data of the tracking target in two adjacent frames, precise transformation parameters are solved by using the ICP algorithm; according to the precise transformation parameters and the initial value of the speed of the tracking target, a trajectory estimation result of the tracking target is obtained; The similarity between the two adjacent frames Including integer part and the decimal part , ;in: is the centroid plane distance of the target to be tracked in two adjacent frames, is the preset plane distance threshold, and Respectively Frame and The side length of the target to be tracked in the frame, , Indicates rounding the number to the nearest digit.

2. The laser radar tracking method according to claim 1, characterized in that: When the centroid plane distance of the target to be tracked in two adjacent frames is greater than the plane distance threshold, it is determined that the similarity of the target to be tracked in the two adjacent frames is 0, and the target to be tracked is not a tracking target.

3. The laser radar tracking method according to claim 1, characterized in that: For the target to be tracked in the original point cloud data of the Kth frame, the target to be tracked with the largest similarity greater than 0 is selected in the point cloud data of the K+1th frame as the tracking target.

4. A laser radar tracking device, characterized in that: include: An acquisition module is used to acquire multiple frames of original point cloud data; A tracking target determination module is used to extract the target to be tracked based on the original point cloud data of two adjacent frames; Calculating the similarity of the target to be tracked in two adjacent frames using a nearest neighbor data association algorithm; determining the target to be tracked according to the similarity of the two adjacent frames; A calculation module, which uses Kalman filtering to calculate the position and posture of the tracking target in two adjacent frames, and obtains an initial value of the speed of the tracking target according to the position and posture of the tracking target in two adjacent frames; The trajectory estimation module obtains initial transformation parameters according to the position and posture of the tracking target in two adjacent frames; uses the ICP algorithm to solve the precise transformation parameters according to the initial transformation parameters and the original point cloud data of the tracking target in two adjacent frames; obtains the trajectory estimation result of the tracking target according to the precise transformation parameters and the initial value of the speed of the tracking target; The similarity between the two adjacent frames Including integer part and the decimal part , ;in: is the centroid plane distance of the target to be tracked in two adjacent frames, is the preset plane distance threshold, and Respectively Frame and The side length of the target to be tracked in the frame, , Indicates rounding the number to the nearest digit.

5. An electronic device, characterized in that: It includes a processor and a memory, wherein a computer program is stored in the memory, and when the computer program is executed by the processor, the laser radar tracking method as described in any one of claims 1 to 3 is implemented.

6. A computer-readable storage medium, characterized in that: The readable storage medium stores computer program instructions, which, when executed by a computer, enable the computer to execute the laser radar tracking method according to any one of claims 1 to 3.

Images