Dynamic SLAM method with multi-target tracking capability in autonomous driving scenarios

Abstract

The present invention discloses a dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario. The method comprises the following steps: first, extracting semantic segmentation masks for each frame of an image sequence captured by a camera, and obtaining corresponding optical flow results by means of an optical flow method; then performing point cloud-image projection on the point cloud captured by a radar, and converting the points in the point cloud frame corresponding to the image into a pixel coordinate system by means of a coordinate transformation; then extracting static point clouds in each frame and point clouds corresponding to different vehicles; then performing an initial pose sequence of the vehicle where the onboard camera is located and an initial relative transformation matrix of different vehicles in each image frame; then utilizing the existing information to construct a graph optimization problem to further optimize the pose results of the vehicle itself and the vehicles in the image; and finally obtaining target pose information. The present invention adopts a laser radar and a camera as sensors, combines semantic segmentation information, can stably operate in a dynamic environment, and output the motion state of the vehicle in the image, and has high practical application value.

Description

Technical Field

[0001] The present invention belongs to the fields of artificial intelligence, robotics and computer vision, and relates to a SLAM method for autonomous driving scenarios, and specifically relates to a multi-target tracking SLAM positioning method based on semantic segmentation and multi-sensor data in autonomous driving scenarios. Background Art

[0002] Simultaneous localization and mapping (SLAM) is a method that uses sensor-equipped observations to estimate a person's motion and build a model of their surroundings in real time in an unknown environment. SLAM is widely used in autonomous driving, robotics, high-precision mapping, drones, and augmented reality.

[0003] SLAM can be mainly divided into radar SLAM and visual SLAM according to the different sensors used. Radar SLAM is currently mainly based on lidar and millimeter wave radar. It is relatively mature and can achieve stable positioning and mapping effects and provide relatively accurate depth information, but it cannot provide rich semantic information like vision. Visual SLAM is mainly based on monocular, binocular and RGBD cameras. It is low-cost, has good real-time performance and broad application prospects, but it will be affected by conditions such as lighting. Both methods have their own advantages, and combining the two can achieve better results.

[0004] Most current SLAM methods assume a static environment, meaning that sensor observations are static components. The algorithm then uses geometry and other methods to achieve positioning and mapping. However, real-world scenarios often do not meet this static environment assumption. When faced with scenes containing dynamic elements, existing methods are affected by these dynamic elements, resulting in mismatches and accumulating errors over time, causing the SLAM algorithm to fail and, therefore, fail to meet accuracy and robustness requirements in practical applications. Furthermore, because existing methods focus on building maps of static elements, they are unable to meet further requirements in scenarios such as autonomous driving. This is because the front-end SLAM algorithm needs to provide the back-end planning layer with richer information, such as the location and speed of surrounding vehicles, to enable better decision-making.

[0005] Existing SLAM algorithms for dynamic scenes use two types of information: geometric and semantic. The geometric method projects feature points from the previous frame onto the current frame and calculates the reprojection error. The magnitude of the reprojection error determines whether a feature point is dynamic, thereby eliminating the dynamic portion. This method can filter out both dynamic and static features, but because the reprojection error is also affected by dynamic elements, the improved accuracy is limited. The semantic method uses pre-trained semantic neural networks to process images, extracting semantic information from the image using the results of object segmentation or semantic segmentation, and then selectively using the image information based on this semantic information. This method can significantly reduce the impact of dynamic elements, but it also suffers from the inability to eliminate potentially moving objects and the lack of available information in the image. Eliminating dynamic elements is the main approach used by current SLAM methods to handle dynamics, resulting in inefficient use of dynamic semantic information. Summary of the Invention

[0006] The purpose of this invention is to provide a multi-target tracking SLAM positioning method based on semantic segmentation and multi-sensor data in autonomous driving scenarios. This method uses data collected by laser radar and cameras as input, combines target detection technology, and uses the depth information of the radar and the semantic information obtained by target detection to construct a 3D object. The pose transformation of the static components and 3D objects is calculated separately. The pose transformation is optimized by combining the calculated information with a graph optimization method to obtain a laser camera fusion SLAM system capable of tracking dynamic objects. It has been verified that this algorithm can effectively locate and track vehicles in autonomous driving scenarios containing dynamic elements, and has high practical application value.

[0007] The above purpose is achieved through the following technical solutions:

[0008] Step 1: Input the image sequence captured by the on-board camera into the semantic segmentation network to obtain the semantic segmentation mask of each image frame in the image sequence. At the same time, the image sequence captured by the on-board camera is processed by the optical flow method to obtain the optical flow result of the current image sequence;

[0009] Step 2: Perform point cloud-image projection between the initial point cloud sequence collected by the onboard lidar and the image sequence collected by the onboard camera to obtain a 3D point cloud sequence;

[0010] Step 3: Based on the semantic segmentation mask of each image frame in the image sequence and the 2D information in the 3D point cloud sequence, perform point cloud semantic segmentation on each frame in the 3D point cloud sequence to obtain the vehicle point cloud and static point cloud of each frame. After clustering the vehicle point cloud of each frame, obtain the point cloud corresponding to different vehicles in each frame of the 3D point cloud;

[0011] Step 4: Determine the static feature points and dynamic feature points of the current two frames based on the first two adjacent image frames in the image sequence, the static point clouds of the 3D point clouds of the corresponding frames in the 3D point cloud sequence, and the point clouds corresponding to different vehicles. Then, determine the initial pose transformation of the vehicle where the on-board camera is located between the current adjacent frames and the initial relative transformation matrix of different vehicles between the current adjacent frames based on the optical flow results corresponding to the current two frames.

[0012] Step 5: Based on the static feature points and dynamic feature points of the current two image frames, the initial pose transformation of the vehicle where the onboard camera is located between the current adjacent frames, and the initial relative transformation matrix of different vehicles between the current adjacent frames, a graph optimization problem is constructed and solved to obtain the optimized pose transformation of the vehicle where the onboard camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles in the frame after the current two frames;

[0013] Step 6: The optimized pose transformation of the vehicle where the onboard camera is located is used as the final pose transformation of the onboard camera. Based on the optimized pose transformation of the vehicle where the onboard camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles between the current adjacent frames, the pose information of different vehicles between the current adjacent frames is calculated. Then, the speed of the corresponding vehicle is calculated based on the pose information of different vehicles between the current adjacent frames.

[0014] Step 7: Repeat steps 4 to 6, and perform pose calculation and optimization of adjacent frames in sequence based on the remaining image frames in the image sequence, the static point cloud of the corresponding frame 3D point cloud in the 3D point cloud sequence, the point cloud corresponding to different vehicles, and the optical flow results to obtain the pose transformation of the on-board camera in each frame, the pose information of different vehicles, and the corresponding speed.

[0015] In step 1, pixels belonging to vehicles in the semantic segmentation mask of each image frame are marked as 1, pixels containing dynamic elements other than vehicles are marked as -1, and the remaining pixels are marked as 0.

[0016] In step 2, the initial point cloud sequence in the radar coordinate system is converted to the camera coordinate system according to the external parameters of the vehicle-mounted laser radar to the vehicle-mounted camera, and then the initial point cloud sequence in the camera coordinate system is converted to the pixel coordinate system according to the intrinsic parameters of the camera, thereby obtaining a 3D point cloud sequence.

[0017] In step 3, the vehicle point cloud of each frame of 3D point cloud is first constructed into a corresponding KD tree, and then the KD tree of each frame is subjected to Euclidean clustering after setting the search radius to obtain the point cloud corresponding to different vehicles in each frame of 3D point cloud.

[0018] The step 4 is specifically as follows:

[0019] Step 4.1: The pixel positions of the static point clouds of the current two frames of 3D point clouds on the corresponding image frames are used as the static feature points of the current two frames of image frames, and the pixel positions of the point clouds corresponding to different vehicles in the current two frames of 3D point clouds on the corresponding image frames are used as the dynamic feature points of the current two frames of image frames;

[0020] Step 4.2: Use the optical flow results to match the static feature points of the current two image frames to obtain the 2D optical flow static matching results of the current two frames, and use the optical flow results to match the dynamic feature points of the current two image frames to obtain the 2D optical flow dynamic matching results of the current two frames;

[0021] Step 4.3: Record the two adjacent frames as time t-1 and time t, perform 2D coordinate matching from the static feature points at time t-1 to the 2D optical flow static matching results at time t, and obtain the matching relationship between the static feature points at time t-1 and the 2D coordinates at time t as the 3D-2D static matching result. Based on the 3D-2D static matching result, use the PnP and random sampling consensus algorithm to solve the pose transformation T of the vehicle where the vehicle camera is located between time t-1 and time t;

[0022] Step 4.4: Use the 2D optical flow dynamic matching results at time t to check whether the point clouds corresponding to different vehicles at time t-1 are matched with the point clouds corresponding to different vehicles at time t. If they are matched, perform 2D coordinate matching from the dynamic feature points at time t-1 to the 2D optical flow dynamic matching results at time t. The matching relationship from the dynamic feature points at time t-1 to the 2D coordinates at time t is obtained and used as the 3D-2D dynamic matching result. Based on the 3D-2D dynamic matching result, the initial relative transformation matrix H of different vehicles between time t-1 and time t is obtained by using the PnP and random sampling consensus algorithms.

[0023] The step 5 is specifically as follows:

[0024] Step 5.1: Construct a graph optimization problem based on the static feature points and dynamic feature points of the current two image frames, the initial pose transformation of the vehicle where the onboard camera is located between the current adjacent frames, and the initial relative transformation matrix of different vehicles between the current adjacent frames. The graph optimization problem is specifically:

[0025] The pose of the vehicle where the onboard camera is located at time t is determined based on the pose transformation T between time t-1 and time t and the pose at time t-1. A time window of length k is formed from time tk to time t. In the current time window, the pose of the vehicle where the onboard camera is located at each time, the static feature points of each image frame, the dynamic feature points of each image frame, and the initial relative transformation matrix H of different vehicles in each image frame are constructed as vertices of the graph optimization;

[0026] In the current time window, the projection error between the position of the vehicle where the onboard camera is located at each moment and the static feature points of the corresponding image frame is constructed into edges between corresponding vertices; the position transformation T between the position of the vehicle where the onboard camera is located at adjacent moments is constructed into edges between corresponding vertices; the projection error between the position of the vehicle where the onboard camera is located at each moment and the dynamic feature points at the corresponding moment is constructed into edges between corresponding vertices; the transformation error between the dynamic feature points of adjacent image frames and the initial relative transformation matrix H of the corresponding vehicle is constructed into edges between corresponding vertices;

[0027] Step 5.2: Solve the graph optimization problem constructed in the current time window to obtain the optimized pose transformation of the vehicle where the on-board camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles in the frame after the current two frames.

[0028] In step 6, the calculation formula for the posture information of each vehicle between the current adjacent frames is as follows:

[0029] T car =H -1 *T

[0030] Among them, H represents the optimized relative transformation matrix of each vehicle, T represents the optimized posture transformation of the vehicle where the onboard camera is located at time t, and T car Represents the current vehicle's position information from time t-1 to time t.

[0031] In step 6, the speed of each vehicle is calculated as follows:

[0032] v=t car -(IR car )c t-1

[0033] Among them, t car Represents the position information T of each vehicle car The translation part in , I represents the 3*3 unit matrix, R car Represents the current vehicle posture information T car The rotating part in c t-1 Represents the centroid coordinates of the 3D point cloud of the current vehicle at time t-1.

[0034] The beneficial effects of the present invention are:

[0035] In view of the fact that traditional SLAM cannot cope with dynamic scenes, the present invention proposes a dynamic SLAM method with multi-target tracking capability in autonomous driving scenarios, which effectively overcomes the shortcomings of traditional SLAM methods instability and low precision under dynamic target interference, and can accurately estimate the motion of dynamic vehicles in autonomous driving scenarios. At the same time, the optical flow method and the semantic segmentation method in deep learning are introduced, and the radar and image information carried by the autonomous driving vehicle are combined to complete robust information fusion. Different from the mainstream dynamic SLAM research that uses semantics to remove dynamic information, the present invention utilizes the dynamic object information in the scene and provides richer scene information such as target pose and target speed for the subsequent planning and control part of autonomous driving research. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 Flowchart of the method of the present invention.

[0037] Figure 2 An original picture that is input to the method of the present invention.

[0038] Figure 3 This is the optical flow processing result image of the method of the present invention.

[0039] Figure 4 This is a picture of the semantic processing result of the method of the present invention.

[0040] Figure 5 This is a picture of the point cloud clustering result of the method of the present invention.

[0041] Figure 6 This is the image of the dynamic / static feature point processing result of the method of the present invention.

[0042] Figure 7 This is a geometric relationship picture of the multi-target tracking dynamic SLAM method of the present invention.

[0043] Figure 8 This figure shows the result of the method of the present invention using the train 0003 sequence of the KITTI Tracking dataset to estimate the speed of the vehicle in the image.

[0044] Figure 9 This figure shows the tracking and positioning effect of the sensor-carrying vehicle and the vehicle in the picture using the train 0003 sequence of the KITTI Tracking dataset by the method of the present invention. DETAILED DESCRIPTION

[0045] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings provided herein. Obviously, the embodiments provided herein are only some embodiments of the present invention, not all embodiments. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0046] Compared with traditional SLAM, this algorithm combines the depth information of laser point cloud and the semantic information of the image, uses point cloud clustering method to model the target in the image, and incorporates it into the SLAM calculation process to achieve simultaneous positioning of the sensor-carrying vehicle and the target vehicle in the image.

[0047] like Figure 1 As shown, the present invention includes the following steps:

[0048] Step 1: Input the image sequence captured by the on-board camera into the semantic segmentation network to obtain the semantic segmentation mask of each image frame in the image sequence. At the same time, the image sequence captured by the on-board camera is processed by the optical flow method to obtain the optical flow result of the current image sequence. Specifically, after inputting the image frame at each moment and the image frame at the next moment into the optical flow network or the traditional optical flow algorithm, the optical flow vector of each pixel in the image frame at the current moment is obtained. The optical flow result of the current image sequence is composed of the optical flow vectors of each pixel in all image frames, such as Figure 3 shown.

[0049] In step 1, for Figure 2 An RBG picture is shown and input into the semantic segmentation network, such as Figure 4 As shown in FIG, in the semantic segmentation mask of each image frame, the pixels belonging to the vehicle are marked as 1, the pixels containing dynamic elements other than the vehicle are marked as −1, and the remaining pixels are marked as 0.

[0050] Step 2: Perform point cloud-image projection between the initial point cloud sequence collected by the onboard lidar and the image sequence collected by the onboard camera to obtain a 3D point cloud sequence;

[0051] In step 2, the initial point cloud sequence in the radar coordinate system is converted to the camera coordinate system based on the external parameters of the vehicle-mounted lidar to the vehicle-mounted camera, and then the initial point cloud sequence in the camera coordinate system is converted to the pixel coordinate system based on the intrinsic parameters of the camera, thereby obtaining a 3D point cloud sequence. The points in the 3D point cloud sequence contain depth information and pixel coordinates of the image (i.e., 2D information).

[0052] Specifically: Each scan of the laser radar will obtain n data points in the radar coordinate system, and the coordinates of each data point in the laser radar coordinate system are p l For p l =[x l ,yl ,z l ], x l ,y l ,z l They represent the values ​​of the data point on the x, y, and z axes in the radar coordinate system. The external parameter RT of the camera can be converted to the camera coordinate system through the lidar. The coordinate p of the current data point in the camera coordinate system c Expressed as:

[0053] p c =RT*p l

[0054] For the coordinate p of the current data point in the camera coordinate system c =[x c ,y c ,z c ], x c ,y c ,z c They represent the values ​​of the data points on the x, y, and z axes in the camera coordinate system, which can be converted to the pixel coordinate system through the camera's intrinsic parameters, expressed as:

[0055]

[0056] Among them, u represents the horizontal value of the data point in the pixel coordinate system, v represents the vertical value of the data point in the pixel coordinate system, and f x Indicates the horizontal scaling amount in the camera intrinsic parameter, c x Indicates the lateral translation in the camera intrinsic parameter, f y Indicates the vertical scaling of the camera intrinsic parameters, c y Indicates the longitudinal translation of the camera intrinsic parameters.

[0057] Step 3: Based on the semantic segmentation mask of each image frame in the image sequence and the 2D information in the 3D point cloud sequence, perform semantic segmentation on the 3D point cloud of each frame in the 3D point cloud sequence to obtain the vehicle point cloud and static point cloud of each frame of the 3D point cloud. Specifically, first, after semantic segmentation of each corresponding image frame according to the semantic segmentation mask of each image frame, the semantic information corresponding to the 2D coordinates of each point in the 3D point cloud sequence on the image is obtained. Then, the vehicle point cloud and static point cloud of the 3D point cloud sequence are determined based on the semantic information corresponding to the 2D coordinates. After clustering the vehicle point clouds of each frame of the 3D point cloud, the point clouds corresponding to different vehicles in each frame of the 3D point cloud are obtained.

[0058] In step 3, if Figure 5As shown in Figure 1, the vehicle point cloud of each frame 3D point cloud is first constructed into a corresponding KD tree, and then the KD tree of each frame is segmented by Euclidean clustering after setting the search radius to obtain the point cloud corresponding to different vehicles in each frame 3D point cloud. Among them, the point cloud corresponding to each vehicle in each frame can be expressed as P i ={p i1 ,…,p iN}(i>0), p i1 represents the first data point of vehicle i in the current frame, N represents the number of data points of vehicle i in the current frame; the static point cloud of each frame 3D point cloud can be expressed as P0={p 01 ,…,p 0M}, p 01 Represents the first data point of the static point cloud, and M represents the number of data points of the static point cloud.

[0059] Step 4: Determine the static feature points and dynamic feature points of the current two frames based on the first two adjacent image frames in the image sequence, the static point clouds of the 3D point clouds of the corresponding frames in the 3D point cloud sequence, and the point clouds corresponding to different vehicles. Then, determine the initial pose transformation of the vehicle where the onboard camera (i.e., onboard lidar) is located between the current adjacent frames and the initial relative transformation matrix of different vehicles between the current adjacent frames based on the optical flow results corresponding to the current two frames.

[0060] Step 4 is as follows:

[0061] Step 4.1: If Figure 6 As shown, the pixel positions of the static point clouds of the current two frames of 3D point clouds on the corresponding image frames are used as the static feature points of the current two frames of image frames, and the pixel positions of the point clouds corresponding to different vehicles in the current two frames of 3D point clouds on the corresponding image frames are used as the dynamic feature points of the current two frames of image frames;

[0062] Step 4.2: Use the optical flow results to match the static feature points of the current two image frames to obtain the 2D optical flow static matching results of the current two frames, and use the optical flow results to match the dynamic feature points of the current two image frames to obtain the 2D optical flow dynamic matching results of the current two frames;

[0063] Step 4.3: If Figure 7As shown, the two adjacent frames are recorded as time t-1 and time t (i.e., t-1 frame and t frame), and 2D coordinate matching is performed from the static feature points at time t-1 to the 2D optical flow static matching results at time t. The matching relationship between the static feature points at time t-1 and the 2D coordinates at time t is obtained and used as the 3D-2D static matching result. Based on the 3D-2D static matching result, the PnP (pespective-n-point) and random sampling consensus algorithm (Ransac, RANdom SAmple Consensus) are used to solve the pose transformation T of the vehicle where the vehicle-mounted camera is located between time t-1 and time t.

[0064] Step 4.4: Use the 2D optical flow dynamic matching results at time t to check whether the point clouds corresponding to different vehicles at time t-1 match the point clouds corresponding to different vehicles at time t. If they do match, perform 2D coordinate matching from the dynamic feature points at time t-1 to the 2D optical flow dynamic matching results at time t. The matching relationship between the dynamic feature points at time t-1 and the 2D coordinates at time t is obtained and used as the 3D-2D dynamic matching result. Otherwise, no processing is performed. Based on the 3D-2D dynamic matching results, the initial relative transformation matrix H of different vehicles between time t-1 and time t is solved using the PnP and random sampling consensus algorithms. Based on the initial pose transformation of the vehicle where the on-board camera is located and the initial relative transformation matrices of different vehicles, the initial pose information of each vehicle can be calculated using the formula in step 7.

[0065] Step 5: Based on the static feature points and dynamic feature points of the current two image frames, the initial pose transformation of the vehicle where the onboard camera is located between the current adjacent frames, and the initial relative transformation matrix of different vehicles between the current adjacent frames, a graph optimization problem is constructed and solved to obtain the optimized pose transformation of the vehicle where the onboard camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles in the frame after the current two frames;

[0066] Step 5 is as follows:

[0067] Step 5.1: Construct a graph optimization problem based on the static feature points and dynamic feature points of the current two image frames, the initial pose transformation of the vehicle where the onboard camera is located between the current adjacent frames, and the initial relative transformation matrix of different vehicles between the current adjacent frames. The graph optimization problem is specifically:

[0068] The pose of the vehicle where the onboard camera is located at time t is determined based on the pose transformation T between time t-1 and time t and the pose at time t-1. A time window of length k is formed from time tk to time t. In the current time window, the pose of the vehicle where the onboard camera is located at each time, the static feature points of each image frame, the dynamic feature points of each image frame, and the initial relative transformation matrix H of different vehicles in each image frame are constructed as vertices of the graph optimization;

[0069] In the current time window, the projection error between the position of the vehicle where the onboard camera is located at each moment and the static feature points of the corresponding image frame is constructed into edges between corresponding vertices; the position transformation T between the position of the vehicle where the onboard camera is located at adjacent moments is constructed into edges between corresponding vertices; the projection error between the position of the vehicle where the onboard camera is located at each moment and the dynamic feature points at the corresponding moment is constructed into edges between corresponding vertices; the transformation error between the dynamic feature points of adjacent image frames and the initial relative transformation matrix H of the corresponding vehicle is constructed into edges between corresponding vertices;

[0070] Step 5.2: Solve the graph optimization problem constructed in the current time window to obtain the optimized pose transformation of the vehicle where the on-board camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles in the frame after the current two frames.

[0071] Step 6: Take the optimized pose transformation of the vehicle where the onboard camera is located as the final pose transformation of the onboard camera. According to the optimized pose transformation of the vehicle where the onboard camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles between the current adjacent frames, calculate the pose information of different vehicles between the current adjacent frames, such as Figure 9 As shown, the trajectory line on the left is the trajectory of the vehicle where the onboard camera is located, and the trajectory line on the right is the trajectory of all vehicles in the image.

[0072] In step 6, the calculation formula for the posture information of each vehicle in the image frame at time t is as follows:

[0073] T car =H -1 *T

[0074] Among them, H represents the optimized relative transformation matrix of each vehicle, T represents the optimized posture transformation of the vehicle where the onboard camera is located at time t, and T car Represents the current vehicle's position information from time t-1 to time t.

[0075] Then, the speed of the corresponding vehicle is calculated based on the posture information of different vehicles between the current adjacent frames. The calculation formula for the speed of each vehicle is as follows:

[0076] v=t car-(IR car )c t-1

[0077] Among them, t car Represents the position information T of each vehicle car The translation part in , I represents the 3*3 unit matrix, R car Represents the current vehicle posture information T car The rotating part in c t-1 Represents the centroid coordinates of the 3D point cloud of the current vehicle at time t-1.

[0078] Specifically, the vehicle velocity is estimated based on the calculated vehicle pose transformation, such as Figure 8 As shown, the numbers "57.11" and "52.50" in the figure represent the speeds of the corresponding vehicles, in km / h.

[0079] Step 7: Repeat steps 4 to 6, and perform pose calculation and optimization of adjacent frames in sequence based on the remaining image frames in the image sequence, the static point cloud of the corresponding frame 3D point cloud in the 3D point cloud sequence, the point cloud corresponding to different vehicles, and the optical flow results to obtain the pose transformation of the on-board camera in each frame, the pose information of different vehicles, and the corresponding speed.

[0080] It should also be noted that the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Therefore, features specified as "first" or "second" may explicitly or implicitly include at least one such feature. Throughout the description of the present invention, "plurality" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0081] In the description of this specification, the reference terms "one embodiment", "some embodiments", "example", "specific example" or "some examples" mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0082] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not intended to limit the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. A dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario, characterized in that: The following steps are involved: Step 1: Input the image sequence captured by the on-board camera into the semantic segmentation network to obtain the semantic segmentation mask of each image frame in the image sequence. At the same time, the image sequence captured by the on-board camera is processed by the optical flow method to obtain the optical flow result of the current image sequence; Step 2: Perform point cloud-image projection between the initial point cloud sequence collected by the onboard lidar and the image sequence collected by the onboard camera to obtain a 3D point cloud sequence; Step 3: Based on the semantic segmentation mask of each image frame in the image sequence and the 2D information in the 3D point cloud sequence, perform point cloud semantic segmentation on each frame in the 3D point cloud sequence to obtain the vehicle point cloud and static point cloud of each frame. After clustering the vehicle point cloud of each frame, obtain the point cloud corresponding to different vehicles in each frame of the 3D point cloud; Step 4: Determine the static feature points and dynamic feature points of the current two frames based on the first two adjacent image frames in the image sequence, the static point clouds of the 3D point clouds of the corresponding frames in the 3D point cloud sequence, and the point clouds corresponding to different vehicles. Then, determine the initial pose transformation of the vehicle where the on-board camera is located between the current adjacent frames and the initial relative transformation matrix of different vehicles between the current adjacent frames based on the optical flow results corresponding to the current two frames. Step 5: Based on the static feature points and dynamic feature points of the current two image frames, the initial pose transformation of the vehicle where the onboard camera is located between the current adjacent frames, and the initial relative transformation matrix of different vehicles between the current adjacent frames, a graph optimization problem is constructed and solved to obtain the optimized pose transformation of the vehicle where the onboard camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles in the frame after the current two frames; Step 6: The optimized pose transformation of the vehicle where the onboard camera is located is used as the final pose transformation of the onboard camera. Based on the optimized pose transformation of the vehicle where the onboard camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles between the current adjacent frames, the pose information of different vehicles between the current adjacent frames is calculated. Then, the speed of the corresponding vehicle is calculated based on the pose information of different vehicles between the current adjacent frames. Step 7: Repeat steps 4 to 6, and perform pose calculation and optimization for adjacent frames based on the remaining image frames in the image sequence, the static point cloud of the corresponding frame 3D point cloud in the 3D point cloud sequence, the point cloud corresponding to different vehicles, and the optical flow results to obtain the pose transformation of the vehicle-mounted camera in each frame, the pose information of different vehicles, and the corresponding speed; The step 4 is specifically as follows: Step 4.1: The pixel positions of the static point clouds of the current two frames of 3D point clouds on the corresponding image frames are used as the static feature points of the current two frames of image frames, and the pixel positions of the point clouds corresponding to different vehicles in the current two frames of 3D point clouds on the corresponding image frames are used as the dynamic feature points of the current two frames of image frames; Step 4.2: Use the optical flow results to match the static feature points of the current two image frames to obtain the 2D optical flow static matching results of the current two frames, and use the optical flow results to match the dynamic feature points of the current two image frames to obtain the 2D optical flow dynamic matching results of the current two frames; Step 4.3: Record the two adjacent frames as time t-1 and time t, perform 2D coordinate matching from the static feature points at time t-1 to the 2D optical flow static matching results at time t, and obtain the matching relationship between the static feature points at time t-1 and the 2D coordinates at time t as the 3D-2D static matching result. Based on the 3D-2D static matching result, use the PnP and random sampling consensus algorithm to solve the pose transformation T of the vehicle where the vehicle camera is located between time t-1 and time t; Step 4.4: Use the 2D optical flow dynamic matching results at time t to check whether the point clouds corresponding to different vehicles at time t-1 are matched with the point clouds corresponding to different vehicles at time t. If they are matched, perform 2D coordinate matching from the dynamic feature points at time t-1 to the 2D optical flow dynamic matching results at time t. The matching relationship from the dynamic feature points at time t-1 to the 2D coordinates at time t is obtained and used as the 3D-2D dynamic matching result. Based on the 3D-2D dynamic matching result, the initial relative transformation matrix H of different vehicles between time t-1 and time t is obtained by using the PnP and random sampling consensus algorithms.

2. A dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario according to claim 1, characterized in that: In step 1, pixels belonging to vehicles in the semantic segmentation mask of each image frame are marked as 1, pixels containing dynamic elements other than vehicles are marked as -1, and the remaining pixels are marked as 0.

3. The dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario according to claim 1, characterized in that: In step 2, the initial point cloud sequence in the radar coordinate system is converted to the camera coordinate system according to the external parameters of the vehicle-mounted laser radar to the vehicle-mounted camera, and then the initial point cloud sequence in the camera coordinate system is converted to the pixel coordinate system according to the intrinsic parameters of the camera, thereby obtaining a 3D point cloud sequence.

4. The dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario according to claim 1, characterized in that: In step 3, the vehicle point cloud of each frame of 3D point cloud is first constructed into a corresponding KD tree, and then the KD tree of each frame is subjected to Euclidean clustering after setting the search radius to obtain the point cloud corresponding to different vehicles in each frame of 3D point cloud.

5. The dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario according to claim 1, characterized in that: The step 5 is specifically as follows: Step 5.1: Construct a graph optimization problem based on the static feature points and dynamic feature points of the current two image frames, the initial pose transformation of the vehicle where the onboard camera is located between the current adjacent frames, and the initial relative transformation matrix of different vehicles between the current adjacent frames. The graph optimization problem is specifically: The pose of the vehicle where the onboard camera is located at time t is determined based on the pose transformation T between time t-1 and time t and the pose at time t-1. A time window of length k is formed from time tk to time t. In the current time window, the pose of the vehicle where the onboard camera is located at each time, the static feature points of each image frame, the dynamic feature points of each image frame, and the initial relative transformation matrix H of different vehicles in each image frame are constructed as vertices of the graph optimization; In the current time window, the projection error between the position of the vehicle where the onboard camera is located at each moment and the static feature points of the corresponding image frame is constructed into edges between corresponding vertices; the position transformation T between the position of the vehicle where the onboard camera is located at adjacent moments is constructed into edges between corresponding vertices; the projection error between the position of the vehicle where the onboard camera is located at each moment and the dynamic feature points at the corresponding moment is constructed into edges between corresponding vertices; the transformation error between the dynamic feature points of adjacent image frames and the initial relative transformation matrix H of the corresponding vehicle is constructed into edges between corresponding vertices; Step 5.2: Solve the graph optimization problem constructed in the current time window to obtain the optimized pose transformation of the vehicle where the on-board camera is located between the current adjacent frames and the optimized relative transformation matrix of different vehicles in the frame after the current two frames.

6. The dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario according to claim 1, characterized in that: In step 6, the calculation formula for the posture information of each vehicle between the current adjacent frames is as follows: T car =H -1 *T Among them, H represents the optimized relative transformation matrix of each vehicle, T represents the optimized posture transformation of the vehicle where the onboard camera is located at time t, and T car Represents the current vehicle's position information from time t-1 to time t.

7. The dynamic SLAM method with multi-target tracking capability in an autonomous driving scenario according to claim 1, characterized in that: In step 6, the speed of each vehicle is calculated as follows: v=t car -(I-R car )c t-1 Among them, t car Represents the position information T of each vehicle car The translation part in , I represents the 3*3 unit matrix, R car Represents the current vehicle posture information T car The rotating part in c t-1 Represents the centroid coordinates of the 3D point cloud of the current vehicle at time t-1.

Images