Laser radar and camera timestamp soft synchronization method

Abstract

The invention discloses a laser radar and camera timestamp soft synchronization method, and relates to the technical field of automatic driving. According to the method, the camera and the laser radar are jointly fixed on the same holder which is horizontally placed; after the holder is rotated horizontally according to a preset angular velocity, the camera and the laser radar are started together, and each frame of image and each piece of point cloud corresponding to each timestamp of the camera and the laser radar are acquired; according to each frame of image and the preset angular velocity of the holder, calculating to obtain a first function relational expression corresponding to the rotation angle of the camera and the timestamp; according to each image and the preset angular velocity of the holder, calculating to obtain a second function relational expression corresponding to the rotation angle of the laser radar and the timestamp; calculating a starting time difference corresponding to the camera and the laser radar according to the first function relation expression and the second function relation expression; and delay compensation is performed on the starting time of the corresponding sensing equipment according to the starting time difference, so that high-efficiency and accurate synchronization of the timestamps of the laser radar and the camera can be realized.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method for soft synchronization of LiDAR and camera timestamps. Background Technology

[0002] Autonomous driving technology often requires the use of multiple sensors for data acquisition and fusion processing. Among these, data fusion between LiDAR and cameras is a commonly used technique in autonomous driving. Since LiDAR and cameras start collecting data at different times, the prerequisite for data fusion is that both LiDAR and cameras collect information simultaneously; that is, timestamp synchronization between the camera and LiDAR is necessary.

[0003] Timestamp synchronization methods for LiDAR and cameras are generally divided into two types: hard synchronization and soft synchronization. Hard synchronization, provided by related technologies, requires devices such as GPS to output corresponding pulses for synchronization. This method requires additional synchronization components and is relatively complex to implement. Soft synchronization, on the other hand, typically uses software or the camera's SDK to issue trigger signals. However, the time consumed when the software calls the API for triggering the operation leads to significant errors, resulting in lower accuracy for this type of timestamp synchronization method. Summary of the Invention

[0004] To address the aforementioned problems in existing technologies, this invention provides a soft synchronization method for lidar and camera timestamps, enabling highly efficient and accurate synchronization of lidar and camera timestamps. The technical solution provided by this invention is as follows:

[0005] According to one aspect of the present invention, a method for soft synchronization of timestamps between a lidar and a camera is provided, characterized in that the method includes:

[0006] S10: Fix the camera and lidar together on the same horizontally placed gimbal;

[0007] S20: After rotating the gimbal horizontally at a preset angular velocity, the camera and the lidar are activated together, and the camera acquires each frame image corresponding to each time stamp, and the lidar acquires each point cloud corresponding to each time stamp.

[0008] S30: Based on the frame images under each timestamp and the preset angular velocity of the gimbal, calculate the first functional relationship expression between the camera rotation angle and the timestamp;

[0009] S40: Based on each image under each timestamp and the preset angular velocity of the gimbal, calculate the second functional relationship expression between the rotation angle of the lidar and the timestamp;

[0010] S50: Calculate the start-up time difference between the camera and the lidar based on the first functional relationship expression and the second functional relationship expression;

[0011] S60: Before the camera and the lidar are started together again, delay compensation is performed on the start-up time of the corresponding sensing device based on the start-up time difference.

[0012] In a preferred embodiment, step S30 includes:

[0013] S31: For two frames of images under adjacent timestamps, extract the FAST key points corresponding to the two frames of images respectively, and calculate the initial pose of the camera corresponding to the two frames of images based on the preset angular velocity of the gimbal and the FAST key points corresponding to the two frames of images.

[0014] S32: Based on the initial pose, the optimal camera pose corresponding to the two frames of images before and after the camera is calculated using the homography matrix;

[0015] S33: Calculate the total first rotation time difference between two consecutive frames of the camera image based on adjacent timestamps;

[0016] S34: Obtain the total first execution time required to execute steps S31 to S33 based on the first built-in timestamp corresponding to the camera;

[0017] S35: Determine the first actual rotation duration corresponding to the camera based on the difference between the first total rotation time difference and the first total execution time;

[0018] S36: Repeat steps S31 to S35, and fit the first functional relationship expression between the camera rotation angle and the timestamp based on each optimal camera pose and the corresponding first actual rotation duration.

[0019] In a preferred embodiment, step S40 includes:

[0020] S41: For two point clouds under adjacent timestamps, the ICP algorithm is used to perform fine matching between the two point clouds to obtain the fine matching objective function;

[0021] S42: Based on the fine matching objective function, determine the optimal radar pose corresponding to the two point clouds before and after the lidar by iterative solution;

[0022] S43: Calculate the total second rotation time difference between the two point clouds before and after the lidar based on adjacent timestamps;

[0023] S44: Obtain the total second execution time required to execute steps S41 to S43 based on the second built-in timestamp corresponding to the lidar;

[0024] S45: Determine the second actual rotation duration corresponding to the lidar based on the difference between the total second rotation time difference and the total second execution time;

[0025] S46: Repeat steps S41 to S45, and fit the second functional relationship expression between the lidar rotation angle and the timestamp based on each optimal radar pose and the corresponding second actual rotation duration.

[0026] In a preferred embodiment, step S60 includes:

[0027] Before the next simultaneous activation of the camera and the lidar, if the camera's activation time is earlier than the lidar's, the camera's activation time will be delayed and compensated based on the time difference.

[0028] Before the next simultaneous activation of the camera and the lidar, if the camera's activation time is later than the lidar's, the activation time of the lidar will be compensated for the delay based on the time difference.

[0029] In a preferred embodiment, step S36 further includes:

[0030] Before fitting the first functional relationship expression between the camera rotation angle and the timestamp, the k-means clustering method is used to optimize the optimal camera pose and the corresponding first actual rotation duration data of each group.

[0031] In a preferred embodiment, step S46 further includes:

[0032] Before fitting the second functional relationship expression between the lidar rotation angle and the timestamp, the k-means clustering method is used to optimize the optimal lidar pose and the corresponding second actual rotation duration data for each group.

[0033] In a preferred embodiment, both the camera and the lidar are located at the center of the gimbal's pivot, and the camera and the lidar operate in the same horizontal direction.

[0034] Compared with existing technologies, the soft synchronization method for lidar and camera timestamps provided by this invention has the following advantages:

[0035] This invention provides a soft synchronization method for timestamps of a lidar and a camera. The method involves fixing the camera and lidar together on a horizontally placed gimbal; rotating the gimbal horizontally at a preset angular velocity, simultaneously activating the camera and lidar, and acquiring frame images and point clouds corresponding to each timestamp; calculating a first functional relationship between the camera rotation angle and the timestamp based on the frame images and the preset angular velocity of the gimbal; calculating a second functional relationship between the lidar rotation angle and the timestamp based on the image slices and the preset angular velocity of the gimbal; calculating the startup time difference between the camera and lidar based on the first and second functional relationships; and performing delay compensation on the startup time of the corresponding sensing devices based on the startup time difference, thereby achieving highly efficient and accurate synchronization of the lidar and camera timestamps. Attached Figure Description

[0036] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0037] Figure 1 This is a schematic diagram illustrating the implementation environment of a soft synchronization method for timestamps of a lidar and a camera, according to an exemplary embodiment.

[0038] Figure 2 This is a flowchart illustrating a method for soft synchronization of timestamps between a lidar and a camera according to an exemplary embodiment of the present invention.

[0039] Figure 3 This is a diagram illustrating the relationship between the rotation angle of a camera and a lidar and time, according to an exemplary embodiment.

[0040] Figure 4 This is a block diagram of a device for implementing a soft synchronization method for lidar and camera timestamps. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of this invention clearer, the invention is described in detail below with reference to specific embodiments (but not limited to the embodiments given) and accompanying drawings. Obviously, the described embodiments are merely some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0042] To facilitate the explanation of the technical content provided by this invention, the following definitions are provided:

[0043] Camera pose: The pose transformation of the camera from its original position to its current position, including translation and / or rotation transformations.

[0044] Radar pose: The attitude transformation of the lidar from its original position to its current position, including translation and / or rotation transformations.

[0045] ICP algorithm: Iterative Closest Point (ICP) algorithm, a point cloud matching algorithm used to register two point clouds.

[0046] This invention provides a schematic diagram of the implementation environment for a lidar and camera timestamp soft synchronization method, as shown in the embodiment of the invention. Figure 1 As shown, the implementation environment includes a gimbal O, a camera A positioned at the center of the gimbal O's pivot axis, and a lidar B positioned at the center of the gimbal's pivot axis, wherein:

[0047] The gimbal O rotates horizontally around its own axis, but does not translate.

[0048] Camera A rotates along with the gimbal O and continuously captures images as the timestamp increases;

[0049] LiDAR B also rotates with the gimbal O and continuously acquires point clouds as the timestamp increases.

[0050] Figure 2 This is a flowchart illustrating a method for soft synchronization of timestamps between a lidar and a camera according to an exemplary embodiment of the present invention. Figure 2 As shown, the soft synchronization method between the lidar and the camera timestamp includes:

[0051] S10: The camera and lidar are fixed together on the same horizontally placed gimbal.

[0052] Preferably, both the camera and the lidar are located at the center of the gimbal's rotation axis, and the camera and the lidar operate in the same horizontal direction.

[0053] S20: After rotating the gimbal horizontally at a preset angular velocity, the camera and the lidar are activated together, and the camera acquires each frame image corresponding to each time stamp, and the lidar acquires each point cloud corresponding to each time stamp.

[0054] S30: Based on the frame images under each timestamp and the preset angular velocity of the gimbal, calculate the first functional relationship expression between the camera rotation angle and the timestamp.

[0055] In a preferred embodiment, step S30 includes:

[0056] S31: For two frames of images with adjacent timestamps, extract the FAST key points corresponding to the two frames respectively, and calculate the initial pose of the camera corresponding to the two frames based on the preset angular velocity of the gimbal and the FAST key points corresponding to the two frames.

[0057] The steps for extracting key points from FAST may include:

[0058] (1) Select pixel p in the image, and assume that the brightness of pixel p is I. p ;

[0059] (2) Set a threshold T (for example, T is I) p 20% of;

[0060] (3) Select n pixels on a circle with radius r centered on pixel p;

[0061] (4) If there are N consecutive points on the selected circle with a brightness greater than I. p +T or less than I p If -T, then pixel p is determined as a feature point;

[0062] (5) Repeat the above four steps, performing the same operation on each pixel, until all key points in the image are determined.

[0063] S32: Based on the initial pose, the optimal camera pose corresponding to the two frames of images before and after the camera is calculated using the homography matrix.

[0064] Step S31 extracts the FAST keypoints of the image. Based on the preset angular velocity of the gimbal, the initial pose of the two frames can be calculated. Since the camera only rotates and does not translate, the pose can only be recovered using the homography matrix. There are four pose results obtained from the homography matrix. According to the camera imaging principle, two pose results can be eliminated. At this point, based on the prior information of the initial pose, the optimal camera pose, i.e., the camera rotation angle information, can be determined.

[0065] S33: Calculate the total first rotation time difference between two consecutive frames of the camera image based on adjacent timestamps.

[0066] The first rotation time difference is the total time required for the camera to complete the rotation behavior between the two frames captured.

[0067] S34: Obtain the first total execution time required to execute steps S31 to S33 based on the first built-in timestamp corresponding to the camera.

[0068] In one possible implementation, the camera program records a first total execution time value required to execute steps S31 to S33; in another possible implementation, the camera program may preset a first total execution time value required to execute steps S31 to S33.

[0069] S35: Determine the first actual rotation duration corresponding to the camera based on the difference between the first total rotation time difference and the first total execution time.

[0070] The time required for the camera to complete the rotation behavior after capturing two frames of images includes the actual rotation time and the total first execution time required to execute steps S31 to S33. Subtracting the total first execution time from the total first rotation time difference gives the first actual rotation time required for the camera to complete the rotation behavior.

[0071] S36: Repeat steps S31 to S35, and fit the first functional relationship expression between the camera rotation angle and the timestamp based on each optimal camera pose and the corresponding first actual rotation duration.

[0072] Preferably, step S36 further includes:

[0073] Before fitting the first functional relationship expression between the camera rotation angle and the timestamp, the k-means clustering method is used to optimize the optimal camera pose and the corresponding first actual rotation duration data of each group.

[0074] The first functional relational expression can be shown in Formula 1.

[0075] ω c (t)=λ c Formula 1 for t+m

[0076] Where t = t c -t c1 t is the first actual rotation duration, t c The total time difference of the first rotation, t c1 ω represents the total execution time of the first execution. c (t) represents the camera rotation angle, λ c Both m and m are constants.

[0077] S40: Based on each image under each timestamp and the preset angular velocity of the gimbal, calculate the second functional relationship expression between the rotation angle of the lidar and the timestamp.

[0078] In a preferred embodiment, step S40 includes:

[0079] S41: For two point clouds under adjacent timestamps, the ICP algorithm is used to perform fine matching between the two point clouds to obtain the fine matching objective function.

[0080] Based on the frequency of the lidar, the point cloud data at the initial moment and the point cloud data at the end of a frame scan are registered. Since the angular velocity of the gimbal is known, this parameter is used as the initial extrinsic parameter. Then, fine registration is performed according to the ICP algorithm to finally obtain a more accurate lidar pose and establish the relationship between rotation angle and time.

[0081] Preferably, the closest point between two point clouds is selected as the corresponding point. The rotation and translation transformation matrix is ​​solved through all corresponding point pairs. The registration error between the two point clouds is reduced by iterative process until the threshold requirement or number of iterations is met, and the fine matching objective function is obtained as shown in Formula 2.

[0082]

[0083] in, and It is a pair of corresponding matching points. The error is obtained through the initial poses R and T, and then N is... p The summation of the error terms for corresponding points is the sum of the squares of the Euclidean distances between all corresponding points.

[0084] S42: Based on the fine matching objective function, the optimal radar pose corresponding to the two point clouds before and after the lidar is determined by iterative solution.

[0085] S43: Calculate the total second rotation time difference between the two point clouds before and after the lidar based on adjacent timestamps.

[0086] S44: Obtain the total second execution time required to execute steps S41 to S43 based on the second built-in timestamp corresponding to the lidar.

[0087] In one possible implementation, the lidar program records a second total execution time value required to execute steps S41 to S43; in another possible implementation, the lidar program may preset a second total execution time value required to execute steps S31 to S33.

[0088] S45: Determine the second actual rotation duration corresponding to the lidar based on the difference between the total second rotation time difference and the total second execution time.

[0089] The time required for the lidar to complete the rotation behavior of acquiring the two point clouds includes the actual rotation time and the total second execution time required to execute steps S41 to S43. Subtracting the total second execution time from the total second rotation time difference gives the second actual rotation time required for the lidar to complete the rotation behavior.

[0090] S46: Repeat steps S41 to S45, and fit the second functional relationship expression between the lidar rotation angle and the timestamp based on each optimal radar pose and the corresponding second actual rotation duration.

[0091] Preferably, step S46 further includes:

[0092] Before fitting the second functional relationship expression between the lidar rotation angle and the timestamp, the k-means clustering method is used to optimize the optimal lidar pose and the corresponding second actual rotation duration data for each group.

[0093] The second functional relationship expression can be shown in Formula 3.

[0094] ω l (t)=λ l Formula 3 (t+g)

[0095] Where t = t l -t l1 t is the second actual rotation duration, t l The total time difference of the first rotation, t l1 ω represents the total execution time of the first execution. l (t) represents the rotation angle of the lidar, λ l Both g and g are constants.

[0096] S50: Calculate the start-up time difference between the camera and the lidar based on the first functional relationship expression and the second functional relationship expression.

[0097] The initial extrinsic parameters of both the camera and the LiDAR are based on the rotation angle of the gimbal. Therefore, the relationship between the rotation angles of the LiDAR and the camera and time is also based on the rotation angle of the gimbal. Thus, the first and second functional relationship expressions obtained in steps S30 and S40 can be directly compared and analyzed on the same coordinate system according to the time relationship, resulting in the following graph showing the relationship between the rotation angles of the camera and the LiDAR and time: Figure 3 As shown.

[0098] according to Figure 3 It can be seen that the startup time difference t between the camera and the lidar is... b The calculation method can be:

[0099] Let ω in Formula 1 and Formula 3 c (t) and ω l The values ​​of (t) are all 0, and the corresponding horizontal axis times are calculated as t. lb and t cb At this time, the startup time difference t between the camera and the lidar is... b The expression is shown in Formula 4.

[0100] t b =t lb -t cb Formula 4

[0101] S60: Before the camera and the lidar are started together again, delay compensation is performed on the start-up time of the corresponding sensing device based on the start-up time difference.

[0102] In a preferred embodiment, step S60 includes:

[0103] Before the next simultaneous activation of the camera and the lidar, if the camera's activation time is earlier than the lidar's, the camera's activation time will be delayed and compensated based on the time difference.

[0104] Alternatively, before the camera and the lidar are started together again, if the camera starts later than the lidar, the start time of the lidar is compensated for by delay based on the start time difference.

[0105] Compared with existing technologies, the soft synchronization method for lidar and camera timestamps provided by this invention has the following advantages:

[0106] This invention provides a soft synchronization method for timestamps of a lidar and a camera. The method involves fixing the camera and lidar together on a horizontally placed gimbal; rotating the gimbal horizontally at a preset angular velocity, simultaneously activating the camera and lidar, and acquiring frame images and point clouds corresponding to each timestamp; calculating a first functional relationship between the camera rotation angle and the timestamp based on the frame images and the preset angular velocity of the gimbal; calculating a second functional relationship between the lidar rotation angle and the timestamp based on the image slices and the preset angular velocity of the gimbal; calculating the startup time difference between the camera and lidar based on the first and second functional relationships; and performing delay compensation on the startup time of the corresponding sensing devices based on the startup time difference, thereby achieving highly efficient and accurate synchronization of the lidar and camera timestamps.

[0107] In one possible implementation, Figure 4This is a block diagram of an apparatus for implementing a soft synchronization method for timestamps of a lidar and a camera, according to an exemplary embodiment. For example, apparatus 400 may be provided as a server. (Refer to...) Figure 4 The device 400 includes a processing component 422, which further includes one or more processors, and memory resources represented by memory 432 for storing instructions executable by the processing component 422, such as application programs. The application programs stored in memory 432 may include one or more modules, each corresponding to a set of instructions. Furthermore, the processing component 422 is configured to execute instructions to perform the aforementioned LiDAR and camera timestamp soft synchronization method.

[0108] Device 400 may also include a power supply component 426 configured to perform power management of device 400, a wired or wireless network interface 450 configured to connect device 400 to a network, and an input / output (I / O) interface 458. Device 400 may operate on an operating system stored in memory 432, such as Windows Server™, Mac OS X™, Unix™, Linux™, FreeBSD™, or similar.

[0109] Although the present invention has been described in detail above with general descriptions, specific embodiments, and experiments, modifications or improvements can be made to it, which will be obvious to those skilled in the art. Therefore, all such modifications or improvements made without departing from the spirit of the present invention fall within the scope of protection claimed by the present invention.

[0110] Other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention herein. This invention is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein. It should be understood that the invention is not limited to the precise structures described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A method for soft synchronization of timestamps between a lidar and a camera, characterized in that, The method includes: S10: Fix the camera and lidar together on the same horizontally placed gimbal; S20: After rotating the gimbal horizontally at a preset angular velocity, the camera and the lidar are activated together, and the camera acquires each frame image corresponding to each time stamp, and the lidar acquires each point cloud corresponding to each time stamp. S30: Based on the frame images under each timestamp and the preset angular velocity of the gimbal, calculate the first functional relationship expression between the camera rotation angle and the timestamp; S40: Based on each image under each timestamp and the preset angular velocity of the gimbal, calculate the second functional relationship expression between the rotation angle of the lidar and the timestamp; S50: Calculate the start-up time difference between the camera and the lidar based on the first functional relationship expression and the second functional relationship expression; S60: Before the camera and the lidar are started together again, delay compensation is performed on the start-up time of the corresponding sensing device based on the start-up time difference.

2. The method according to claim 1, characterized in that, Step S30 includes: S31: For two frames of images under adjacent timestamps, extract the FAST key points corresponding to the two frames of images respectively, and calculate the initial pose of the camera corresponding to the two frames of images based on the preset angular velocity of the gimbal and the FAST key points corresponding to the two frames of images. S32: Based on the initial pose, the optimal camera pose corresponding to the two frames of images before and after the camera is calculated using the homography matrix; S33: Calculate the total first rotation time difference between two consecutive frames of the camera image based on adjacent timestamps; S34: Obtain the total first execution time required to execute steps S31 to S33 based on the first built-in timestamp corresponding to the camera; S35: Determine the first actual rotation duration corresponding to the camera based on the difference between the first total rotation time difference and the first total execution time; S36: Repeat steps S31 to S35, and fit the first functional relationship expression between the camera rotation angle and the timestamp based on each optimal camera pose and the corresponding first actual rotation duration.

3. The method according to claim 1, characterized in that, Step S40 includes: S41: For two point clouds under adjacent timestamps, the ICP algorithm is used to perform fine matching between the two point clouds to obtain the fine matching objective function; S42: Based on the fine matching objective function, determine the optimal radar pose corresponding to the two point clouds before and after the lidar by iterative solution; S43: Calculate the total second rotation time difference between the two point clouds before and after the lidar based on adjacent timestamps; S44: Obtain the total second execution time required to execute steps S41 to S43 based on the second built-in timestamp corresponding to the lidar; S45: Determine the second actual rotation duration corresponding to the lidar based on the difference between the total second rotation time difference and the total second execution time; S46: Repeat steps S41 to S45, and fit the second functional relationship expression between the lidar rotation angle and the timestamp based on each optimal radar pose and the corresponding second actual rotation duration.

4. The method according to claim 1, characterized in that, Step S60 includes: Before the next simultaneous activation of the camera and the lidar, if the camera's activation time is earlier than the lidar's, the camera's activation time will be delayed and compensated based on the time difference. Before the next simultaneous activation of the camera and the lidar, if the camera's activation time is later than the lidar's, the activation time of the lidar will be compensated for the delay based on the time difference.

5. The method according to claim 2, characterized in that, Step S36 also includes: Before fitting the first functional relationship expression between the camera rotation angle and the timestamp, the k-means clustering method is used to optimize the optimal camera pose and the corresponding first actual rotation duration data of each group.

6. The method according to claim 3, characterized in that, Step S46 also includes: Before fitting the second functional relationship expression between the lidar rotation angle and the timestamp, the k-means clustering method is used to optimize the optimal lidar pose and the corresponding second actual rotation duration data for each group.

7. The method according to claim 1, characterized in that, Both the camera and the lidar are located at the center of the gimbal's rotation axis, and the camera and the lidar operate in the same horizontal direction.

Images