A dynamic target perception system and method integrating camera and laser radar

Abstract

The present invention provides a dynamic target perception system and method integrating a camera and a laser radar, and belongs to the field of dynamic data measurement technology. It solves the technical problems of single function and low precision of the existing dynamic target perception system. The dynamic target perception system includes a monocular camera, a laser radar and a controller. The monocular camera can transmit the captured image to the controller. The controller can use a two-dimensional target detection algorithm to obtain the two-dimensional outer rectangle of the dynamic target, and obtain the region of interest in the three-dimensional space according to the projection relationship, and then perform point cloud clustering and fit the three-dimensional outer rectangle in the 3D ROI; the controller also includes a decision planning module, an inter-frame matching module, a tracker management module and a convolutional neural network. The system and method of the present invention can accurately and reliably realize the precise detection and tracking of three-dimensional dynamic targets and predict the movement trend of the target according to the historical trajectory, thereby assisting the automatic driving function of the motor vehicle.

Description

Technical Field

[0001] The present invention belongs to the technical field of dynamic data measurement, and relates to a dynamic target perception system and method, and in particular to a dynamic target perception system and method integrating a camera and a laser radar. Background Art

[0002] The dynamic target perception system in complex environments is an important part of autonomous driving technology. The perception system needs to detect and track three-dimensional dynamic targets and predict the target's movement trend based on historical trajectories to provide sufficient environmental information for decision-making and planning systems. At present, the sensors used for three-dimensional target detection are mainly cameras and lidars. When a single camera is used for recognition, the camera is easily affected by ambient lighting conditions and it is difficult to obtain more accurate motion information; when a single lidar is used for recognition, the laser point cloud is sparse, and when the target is far away, the recognition effect will be significantly reduced.

[0003] my country's patent (announcement number: CN109102702A; announcement date: 2018-12-28) discloses a vehicle speed measurement method based on video vehicle detection and radar signal fusion, the steps are as follows: Step 1, vehicle identification and tracking: use the camera to collect vehicle video information, identify and track the vehicle; Step 2, speed measurement: use the echo signal obtained by the radar to determine the vehicle's speed, distance and direction information; Step 3, result fusion: according to the vehicle distance and direction information detected by the radar, determine the vehicle's center of mass coordinates and driving direction, match them with the vehicle's center of mass coordinates and driving direction obtained by video detection, and display the vehicle's speed information on the video.

[0004] This patent document combines video target detection with radar signals to complete vehicle speed measurement. It can only complete speed detection of dynamic targets. The overall function is relatively simple and there is no public method for accurate tracking and prediction. Summary of the invention

[0005] In view of the above-mentioned problems existing in the prior art, the present invention provides a dynamic target perception system integrating a camera and a lidar. The technical problem to be solved by the present invention is: how to accurately and reliably realize the precise detection and tracking of three-dimensional dynamic targets and predict the movement trend of the targets based on historical trajectories, so as to assist the automatic driving function of motor vehicles.

[0006] The purpose of the present invention can be achieved by the following technical solutions:

[0007] A dynamic target perception system integrating camera and laser radar comprises a monocular camera, a laser radar and a controller, wherein the monocular camera, the laser radar and the controller are electrically connected, and is characterized in that the monocular camera can transmit the captured image to the controller, and the controller can use a two-dimensional target detection algorithm to obtain a two-dimensional outer rectangle of the dynamic target, and obtain a region of interest (ROI) in three-dimensional space according to the projection relationship, and then perform point cloud clustering in the 3D ROI and fit the three-dimensional outer rectangle; the controller also comprises a decision planning module, an inter-frame matching module, a tracker management module and a convolutional neural network (CNN).

[0008] The principle is as follows: The technical solution of the present invention combines the respective advantages of cameras and laser radars, and proposes a three-dimensional target detection process that integrates monocular cameras and laser radars. First, a two-dimensional target detection algorithm is used on the image to obtain the two-dimensional outer rectangle of the target, and the region of interest (ROI) in the three-dimensional space is obtained according to the projection relationship. Then, point cloud clustering is performed in the 3D ROI and the three-dimensional outer rectangle is fitted. At the same time, in order to provide sufficient information for downstream modules such as decision-making and planning in autonomous driving, it is also necessary to track and predict the trajectory of the detected three-dimensional targets. The present invention regards the tracking problem as an inter-frame matching and tracker management problem; first, the appearance features extracted by convolutional neural networks (CNN) are used to match the two-frame targets between frames, and then the quaternary finite state machine model proposed in the solution is used to manage the matching results. In the trajectory prediction part, an innovative trajectory prediction model based on lane line constraints is proposed to model vehicle motion in the road coordinate system.

[0009] This system can obtain accurate motion information of dynamic targets, ensure accurate and effective recognition effects, use the DeepSORT algorithm for multi-target tracking, and use a four-element state machine model to manage the tracker to improve the robustness of the tracking algorithm.

[0010] A dynamic target perception and prediction method integrating camera and laser radar, which uses the above-mentioned dynamic target perception system to detect, track and predict three-dimensional dynamic targets, includes the following steps:

[0011] a. Target detection: First, align the laser radar and camera to establish the spatial position relationship between the laser radar and the camera; use the YOLOv4 (You Only Look Once) target detection algorithm to obtain a 2D bounding rectangle on the image; then generate a 3D conical region of interest corresponding to the 2D bounding rectangle based on the projection relationship between the point cloud and the image; finally, cluster the point cloud within the 3D ROI and fit the 3D bounding rectangle;

[0012] b. Target tracking: Match the target between frames based on the DeepSORT algorithm; use a finite state machine to manage the tracking results;

[0013] c. Trajectory prediction: Use lane lines to constrain the movement of the vehicle, and use the Extended Kalman Filter (EKF) for trajectory prediction.

[0014] The purpose of using finite state machines to manage tracking results is to reduce the impact of missed detections and false detections by the detector.

[0015] In the above-mentioned dynamic target perception and prediction method of integrating camera and laser radar, in step a, when establishing the spatial position relationship between the laser radar and the camera, that is, the relationship between the radar coordinate system and the image pixel coordinate system, the mathematical model used is:

[0016]

[0017] in, is the pixel coordinate, is the radar coordinate; K is the camera intrinsic parameter matrix; and are the rotation matrix and translation matrix from the radar coordinate system to the camera coordinate system respectively; λ is the normalization coefficient.

[0018] A unified time and space reference is a necessary condition for multi-sensor fusion. This technical solution adopts the above mathematical model to establish the relationship between the radar coordinate system and the image pixel coordinate system, which is accurate and reliable.

[0019] In the above-mentioned method for sensing and predicting dynamic targets by integrating a camera and a laser radar, in step a, the point cloud is also preprocessed; the method includes the following steps in sequence:

[0020] Downsampling: First, the point cloud is downsampled using a voxel filtering algorithm;

[0021] Denoising: The point cloud is then denoised using statistical filtering;

[0022] Irrelevant point filtering: The irrelevant point filtering module is responsible for filtering out point clouds that are not of interest.

[0023] The point cloud is preprocessed by downsampling, denoising and irrelevant point filtering to improve the efficiency and effect of the algorithm. The present invention mainly filters the points outside the image frame and the ground points. The points outside the image frame are judged according to the above mathematical model calculation formula (1). The present invention assumes that the ground is a plane and uses the RANSAC algorithm for plane fitting. Since most of the point clouds in the laser point cloud in urban roads are generated by ground reflection, when the RANSAC algorithm is used for plane fitting, the final inner point is the ground point.

[0024] In the above-mentioned dynamic target perception and prediction method of fusion camera and laser radar, in step a, the 3D ROI generation process of the fusion image is as follows: according to the pinhole imaging principle, a cone in space will form a rectangle after being projected onto the image; any rectangle on the image corresponds to a conical area in space; the target detection algorithm YOLOv4 based on convolutional neural network (CNN) is used to detect the two-dimensional outer rectangle of the target on the image; then the three-dimensional conical area corresponding to each two-dimensional outer rectangle is calculated according to the above-mentioned mathematical model calculation formula (1); there is only one target in each three-dimensional conical area.

[0025] In the above-mentioned dynamic target perception and prediction method of integrating camera and laser radar, in step a, the point cloud clustering and three-dimensional outer rectangle fitting process is as follows: after obtaining the 3DROI, the point cloud is clustered in each three-dimensional conical area using the Euclidean Cluster method; after the clustering is successful, each three-dimensional rectangular area will obtain a number of point cloud clusters C = {C i |i=1,2,...,n},for each point cloud cluster C i , project its point cloud onto the image, calculate the distance between each point in the point cloud cluster and the center of the 2D bounding rectangle, and record the minimum distance as d(C i ), select the point cloud cluster with the smallest minimum distance as the target point cloud cluster C o ,Right now

[0026] C O =argmin{d(C i )}, i=1, 2,,..,n#(2)

[0027] Then the target point cloud cluster C o Perform 3D outer rectangle fitting; in the vertical direction, directly use C o The maximum and minimum values ​​in the z direction (z min , z max) as the boundary of the target in the vertical direction; in the horizontal direction, the 3D rectangle degenerates into a 2D rectangle, and the optimization formula of the 2D enclosing rectangle is:

[0028]

[0029] Among them, (x i ,y i ) is the point coordinate, θ, a1, a2 are the parameters of two mutually perpendicular lines, and the point cloud cluster is divided into two parts, L1 and L2, according to the distance from the point to the boundary. The present invention uses the search algorithm L-Shape to solve this optimization problem.

[0030] In the vertical direction, since the roads in cities are relatively flat and the XOY plane of the lidar is often parallel to the ground in most applications, C can be used directly. o The maximum and minimum values ​​in the z direction (z min , z max ) as the vertical boundary of the target.

[0031] In the above-mentioned dynamic target perception and prediction method of fusion camera and lidar, in step b, the inter-frame matching of the target based on the DeepSORT algorithm is as follows: the DeepSORT algorithm uses CNN to extract the appearance feature vector of the target, uses the cosine distance of the feature vector to describe the similarity between the targets, and then uses the Hungarian algorithm for inter-frame matching.

[0032] In the above-mentioned dynamic target perception and prediction method of fusion camera and laser radar, in step b, the use of a finite state machine to manage the tracking results is as follows: the finite state machine consists of three parts: state, event and action; each tracker has four states: tracking state, vanished state, initializing state and buffer state; when the tracker is in the tracking state, it means that the tracking result is stable and reliable; the vanishing state indicates that the target has left the field of view, and the tracker in the vanishing state will be deleted from the memory and no longer participate in the operation; the initial state and the buffer state are between the tracking state and the vanishing state, which are used to counter the influence of false detection and missed detection; the initial state is the initial state of the tracker;

[0033] Since the detector may have false detection, when the target in the t-1 frame fails to match the target in the t-2 frame, the target may be a new target entering the field of view or a false target. Therefore, the tracker will enter the initial state when it is initialized. init The tracker will only change from the initial state to the tracking state if all frames are matched successfully; otherwise, the tracker will enter the disappearing state and no longer participate in the calculation.

[0034] Since the detector may miss detection, when the target in the t-1th frame fails to match the target in the tth frame, the target may fail to match because it leaves the field of view or because the detector misses detection. Therefore, when the tracker is in the tracking state, if the match fails in a certain frame, it changes from the tracking state to the buffering state; if in the next N buffer If one of the frames matches successfully, it will return to the tracking state; otherwise, it will enter the disappearing state.

[0035] The detector will inevitably have missed detection and false detection. In order to reduce the impact of missed detection and false detection on tracking, a tracking management strategy needs to be introduced. To this end, the present invention proposes a four-element finite state machine model to complete tracking management. The state transition event can be adjusted according to the actual scenario.

[0036] In the above-mentioned dynamic target perception and prediction method integrating camera and laser radar, in step c, the trajectory prediction step using lane line constraints is:

[0037] Assume that the target is moving along the lane line, and the longitudinal speed along the lane is v x , the lateral velocity is v y , and there is no acceleration in both directions; let the target in the road coordinate system be (x, y); then the motion equation of the target in the road coordinate system can be expressed as:

[0038]

[0039] After linearization, it can be expressed as:

[0040]

[0041] During target detection, the target coordinates obtained are the coordinates in the vehicle body coordinate system, denoted as To establish the observation equation, it is necessary to establish the relationship between the vehicle coordinate system and the road coordinate system. Assuming that the geometric shape of the lane line is a Clothoid curve, the curvature of any position of the lane line can be described as: c(x) = c0 + c1x, where c0 is the curvature of the initial position, c1 is the curvature change rate, and x is the road mileage. Based on this assumption, the road coordinates (x, y) and the vehicle coordinates The following approximate relationship can be established:

[0042]

[0043] Among them, y off is the distance between the vehicle and the center line of the road, is the angle between the vehicle and the road;

[0044] The above formula is the observation model of the target, and the observation vector After linearization, we get

[0045]

[0046] in,

[0047] Since the observation equation is a nonlinear equation, EKF can be used to estimate and predict the target state.

[0048] The vehicle kinematic model can predict the state of the vehicle at the next moment based on the current state of the vehicle and the input control quantity. Therefore, the vehicle trajectory prediction problem can be regarded as the problem of constructing a vehicle kinematic model. Commonly used vehicle kinematic models include the uniformly accelerated linear model and the kinematic bicycle model. The uniformly accelerated linear model assumes that the vehicle performs uniformly accelerated linear motion, which is simple and effective, but the description of the vehicle motion is not accurate enough. The kinematic bicycle model can describe the vehicle motion more accurately, but it requires information such as the vehicle's center of gravity and wheel direction, which is difficult to obtain directly. Therefore, the present invention proposes a vehicle kinematic model based on lane line constraints, which uses lane lines and target historical trajectories for trajectory prediction.

[0049] The effect and performance of the detection algorithm are tested on the KITTI dataset. The computing platform CPU is Intel Core i5-9300H@2.40GHz, and the GPU is NVIDIA GeForce GTX 1660 Ti. All algorithms are implemented in C++, and the neural network is deployed and accelerated using TensorRT. After testing, the average algorithm takes about 25ms. The experimental results show that the algorithm performs well in most scenarios. The present invention reduces the retrieval range of the target point cloud through the image 2D target detection algorithm.

[0050] In order to verify the effectiveness of the trajectory prediction algorithm, the model of the present invention is experimentally compared with the uniform acceleration motion model using simulation data. When generating simulation data, it is assumed that the vehicle travels along the center line of the lane and the observation error conforms to the normal distribution. In urban roads, the shape of the lane can be divided into straight lines (c0 = 0, c1 = 0), arcs (c0 ≠ 0, c1 = 0) and transition curves (c1 ≠ 0). In this paper, c0 = 2 × 10 -3 , c1=4×10 -5 , three different types of lanes were tested respectively. The test results show that when the lane line is a straight line, c0=0, c1=0, the algorithm of the present invention will degenerate into a uniformly accelerated linear motion model, and the two trajectories will overlap; when the lane line is an arc or a transition curve, the algorithm of the present invention is more in line with the real trajectory. In summary, the trajectory prediction algorithm based on lane line constraints proposed in the present invention has better prediction effect than the traditional uniformly accelerated motion model.

[0051] Compared with the prior art, the present invention has the following advantages:

[0052] 1. The present invention integrates a monocular camera and a laser radar, first uses a convolutional neural network to perform two-dimensional target detection on the image, and generates a conical region of interest (ROI) according to the geometric projection relationship; clusters the point cloud within the ROI and fits a three-dimensional bounding rectangle; then performs inter-frame matching of the three-dimensional target based on appearance features and the Hungarian algorithm, and proposes a tracker management model based on a quaternion state machine to improve the robustness of the tracking algorithm.

[0053] 2. The present invention integrates a monocular camera and a laser radar, first uses a visual detection algorithm to narrow the search range of the target point cloud; uses the spatial information of the point cloud to perform Euclidean clustering to obtain the target point cloud, and fits the target's three-dimensional outer bounding rectangle in a search-based manner; and uses lane line information to predict the target trajectory, with better results than the uniform acceleration motion model. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a control flow diagram of the dynamic target perception and prediction method.

[0055] Figure 2 This is a model diagram of using a finite state machine to manage tracking results in this dynamic target perception and prediction method.

[0056] Figure 3 It is a coordinate system definition diagram of the prediction model in this dynamic target perception and prediction method. DETAILED DESCRIPTION

[0057] The following are specific embodiments of the present invention and the accompanying drawings to further describe the technical solution of the present invention, but the present invention is not limited to these embodiments.

[0058] The dynamic target perception system of the fusion camera and laser radar includes a monocular camera, a laser radar and a controller. The monocular camera, the laser radar and the controller are electrically connected. The monocular camera can transmit the captured image to the controller. The controller can use a two-dimensional target detection algorithm to obtain a two-dimensional outer rectangle of the dynamic target, and obtain a region of interest (ROI) in three-dimensional space according to a projection relationship, and then perform point cloud clustering in the 3D ROI and fit the three-dimensional outer rectangle; the controller also includes a decision planning module, an inter-frame matching module, a tracker management module and a convolutional neural network (CNN).

[0059] In this embodiment, a three-dimensional target detection process integrating a monocular camera and a laser radar is proposed by combining the respective advantages of the camera and the laser radar. First, a two-dimensional target detection algorithm is used on the image to obtain the two-dimensional outer rectangle of the target, and the region of interest (ROI) in the three-dimensional space is obtained according to the projection relationship, and then point cloud clustering is performed in the 3D ROI and the three-dimensional outer rectangle is fitted. At the same time, in order to provide sufficient information for downstream modules such as decision-making planning in autonomous driving, it is also necessary to track and predict the trajectory of the detected three-dimensional target. The present invention regards the tracking problem as an inter-frame matching and tracker management problem; first, the appearance features extracted by the convolutional neural network (CNN) are used to match the two-frame targets between frames, and then the quaternary finite state machine model proposed in the scheme is used to manage the matching results. In the trajectory prediction part, an innovative trajectory prediction model based on lane line constraints is proposed to model vehicle motion in the road coordinate system. The system can obtain accurate motion information of dynamic targets, ensure accurate and effective recognition effects, use the DeepSORT algorithm for multi-target tracking, and use a quaternary state machine model to manage the tracker to improve the robustness of the tracking algorithm.

[0060] Combination Figure 1 , Figure 2 and Figure 3 As shown, this embodiment specifically describes a dynamic target perception and prediction method for detecting, tracking and predicting three-dimensional dynamic targets using the above-mentioned dynamic target perception system, including the following steps:

[0061] a. Target detection: First, align the laser radar and camera to establish the spatial position relationship between the laser radar and the camera; use the YOLOv4 (You Only Look Once) target detection algorithm to obtain a 2D bounding rectangle on the image; then generate a 3D conical region of interest corresponding to the 2D bounding rectangle based on the projection relationship between the point cloud and the image; finally, cluster the point cloud within the 3D ROI and fit the 3D bounding rectangle;

[0062] b. Target tracking: Match the target between frames based on the DeepSORT algorithm; use a finite state machine to manage the tracking results;

[0063] c. Trajectory prediction: Use lane lines to constrain the movement of the vehicle, and use the Extended Kalman Filter (EKF) for trajectory prediction.

[0064] Furthermore, in step a, when establishing the spatial position relationship between the laser radar and the camera, that is, the relationship between the radar coordinate system and the image pixel coordinate system, the mathematical model used is:

[0065]

[0066] in, is the pixel coordinate, is the radar coordinate; K is the camera intrinsic parameter matrix; and are the rotation matrix and translation matrix from the radar coordinate system to the camera coordinate system respectively; λ is the normalization coefficient.

[0067] A unified time and space reference is a necessary condition for multi-sensor fusion. In this embodiment, the above mathematical model is used to establish the relationship between the radar coordinate system and the image pixel coordinate system, which is accurate and reliable.

[0068] Step a also includes preprocessing the point cloud; it includes the following steps in sequence:

[0069] Downsampling: First, the point cloud is downsampled using a voxel filtering algorithm;

[0070] Denoising: The point cloud is then denoised using statistical filtering;

[0071] Irrelevant point filtering: The irrelevant point filtering module is responsible for filtering out point clouds that are not of interest.

[0072] The point cloud is preprocessed by downsampling, denoising and irrelevant point filtering to improve the efficiency and effect of the algorithm. The present invention mainly filters the points outside the image frame and the ground points. The points outside the image frame are judged according to the above mathematical model calculation formula (1). The present invention assumes that the ground is a plane and uses the RANSAC algorithm for plane fitting. Since most of the point clouds in the laser point cloud in urban roads are generated by ground reflection, when the RANSAC algorithm is used for plane fitting, the final inner point is the ground point.

[0073] In step a, the 3D ROI generation process of the fused image is as follows: according to the pinhole imaging principle, a cone in space will form a rectangle after being projected onto the image; any rectangle on the image corresponds to a conical area in space; the target detection algorithm YOLOv4 based on the convolutional neural network (CNN) is used to detect the two-dimensional enclosing rectangle of the target on the image; then the three-dimensional conical area corresponding to each two-dimensional enclosing rectangle is calculated according to the above mathematical model calculation formula (1); there is only one target in each three-dimensional conical area.

[0074] In step a, the point cloud clustering and 3D bounding rectangle fitting process is as follows: after obtaining the 3DROI, the point cloud is clustered in each 3D conical region using the Euclidean Cluster method; after successful clustering, each 3D rectangular region will obtain a number of point cloud clusters C = {C i |i=1,2,...,n},for each point cloud cluster C i , project its point cloud onto the image, calculate the distance between each point in the point cloud cluster and the center of the 2D bounding rectangle, and record the minimum distance as d(C i ), select the point cloud cluster with the smallest minimum distance as the target point cloud cluster C o ,Right now

[0075] C o =argmin{d(C i )}, i = 1, 2, ..., n # (2)

[0076] Then the target point cloud cluster C o Perform 3D outer rectangle fitting; in the vertical direction, directly use C o The maximum and minimum values ​​in the z direction (z min , z max ) as the boundary of the target in the vertical direction; in the horizontal direction, the 3D rectangle degenerates into a 2D rectangle, and the optimization formula of the 2D enclosing rectangle is:

[0077]

[0078] Among them, (x i ,y i ) is the point coordinate, θ, a1, a2 are the parameters of two mutually perpendicular lines, and the point cloud cluster is divided into two parts, L1 and L2, according to the distance from the point to the boundary. The present invention uses the search algorithm L-Shape to solve this optimization problem.

[0079] In the vertical direction, since the roads in cities are relatively flat and the XOY plane of the lidar is often parallel to the ground in most applications, C can be used directly. o The maximum and minimum values ​​in the z direction (z min , z max ) as the vertical boundary of the target.

[0080] Further, in step b, the target is matched between frames based on the DeepSORT algorithm: the DeepSORT algorithm uses CNN to extract the appearance feature vector of the target, uses the cosine distance of the feature vector to describe the similarity between the targets, and then uses the Hungarian algorithm to match between frames. In this embodiment, the use of a finite state machine to manage the tracking results is to reduce the impact of missed detection and false detection by the detector; the use of a finite state machine to manage the tracking results is as follows: the finite state machine consists of three parts: state, event, and action; each tracker has four states: tracking, vanishing, initializing, and buffering; when the tracker is in the tracking state, it indicates that the tracking result is stable and reliable; the vanishing state indicates that the target has left the field of view, and the tracker in the vanishing state will be deleted from the memory and will no longer participate in the operation; the initial state and buffer state are between the tracking state and the vanishing state, which are used to combat the impact of false detection and missed detection; the initial state is the initial state of the tracker;

[0081] Since the detector may have false detection, when the target in the t-1 frame fails to match the target in the t-2 frame, the target may be a new target entering the field of view or a false target. Therefore, the tracker will enter the initial state when it is initialized. init The tracker will only change from the initial state to the tracking state if all frames are matched successfully; otherwise, the tracker will enter the disappearing state and no longer participate in the calculation.

[0082] Since the detector may miss detection, when the target in the t-1th frame fails to match the target in the tth frame, the target may fail to match because it leaves the field of view or because the detector misses detection. Therefore, when the tracker is in the tracking state, if the match fails in a certain frame, it changes from the tracking state to the buffering state; if in the next N buffer If one of the frames matches successfully, it will return to the tracking state; otherwise, it will enter the disappearing state.

[0083] The detector will inevitably have missed detection and false detection. In order to reduce the impact of missed detection and false detection on tracking, a tracking management strategy needs to be introduced. To this end, the present invention proposes a four-element finite state machine model to complete tracking management. The state transition event can be adjusted according to the actual scenario.

[0084] Furthermore, in step c, the trajectory prediction steps using lane line constraints are:

[0085] Assume that the target is moving along the lane line, and the longitudinal speed along the lane is v x , the lateral velocity is v y, and there is no acceleration in both directions; let the target in the road coordinate system be (x, y); then the motion equation of the target in the road coordinate system can be expressed as:

[0086]

[0087] After linearization, it can be expressed as:

[0088]

[0089] During target detection, the target coordinates obtained are the coordinates in the vehicle body coordinate system, denoted as To establish the observation equation, it is necessary to establish the relationship between the vehicle coordinate system and the road coordinate system. Assuming that the geometric shape of the lane line is a Clothoid curve, the curvature of any position of the lane line can be described as: c(x) = c0 + c1x, where c0 is the curvature of the initial position, c1 is the curvature change rate, and x is the road mileage. Based on this assumption, the road coordinates (x, y) and the vehicle coordinates The following approximate relationship can be established:

[0090]

[0091] Among them, y off is the distance between the vehicle and the center line of the road, is the angle between the vehicle and the road;

[0092] The above formula is the observation model of the target, and the observation vector After linearization, we get

[0093]

[0094] in,

[0095] Since the observation equation is a nonlinear equation, EKF can be used to estimate and predict the target state.

[0096] The vehicle kinematic model can predict the state of the vehicle at the next moment based on the current state of the vehicle and the input control quantity. Therefore, the vehicle trajectory prediction problem can be regarded as the problem of constructing a vehicle kinematic model. Commonly used vehicle kinematic models include the uniformly accelerated linear model and the kinematic bicycle model. The uniformly accelerated linear model assumes that the vehicle performs uniformly accelerated linear motion, which is simple and effective, but the description of the vehicle motion is not accurate enough. The kinematic bicycle model can describe the vehicle motion more accurately, but it requires information such as the vehicle's center of gravity and wheel direction, which is difficult to obtain directly. Therefore, the present invention proposes a vehicle kinematic model based on lane line constraints, which uses lane lines and target historical trajectories for trajectory prediction.

[0097] The effect and performance of the detection algorithm are tested on the KITTI dataset. The computing platform CPU is Intel Core i5-9300H@2.40GHz, and the GPU is NVIDIA GeForce GTX 1660 Ti. All algorithms are implemented in C++, and the neural network is deployed and accelerated using TensorRT. After testing, the average algorithm takes about 25ms. The experimental results show that the algorithm performs well in most scenarios. The present invention reduces the retrieval range of the target point cloud through the image 2D target detection algorithm.

[0098] In order to verify the effectiveness of the trajectory prediction algorithm, the model of the present invention is experimentally compared with the uniform acceleration motion model using simulation data. When generating simulation data, it is assumed that the vehicle travels along the center line of the lane and the observation error conforms to the normal distribution. In urban roads, the shape of the lane can be divided into straight lines (c0 = 0, c1 = 0), arcs (c0 ≠ 0, c1 = 0) and transition curves (c1 ≠ 0). In this paper, c0 = 2 × 10 -3 , c1=4×10 -5 , three different types of lanes were tested respectively. The test results show that when the lane line is a straight line, c0=0, c1=0, the algorithm of the present invention will degenerate into a uniformly accelerated linear motion model, and the two trajectories will overlap; when the lane line is an arc or a transition curve, the algorithm of the present invention is more in line with the real trajectory. In summary, the trajectory prediction algorithm based on lane line constraints proposed in the present invention has better prediction effect than the traditional uniformly accelerated motion model.

[0099] The specific embodiments described herein are merely examples of the spirit of the present invention. Those skilled in the art may make various modifications or additions to the specific embodiments described or replace them in similar ways, but they will not deviate from the spirit of the present invention or exceed the scope defined by the appended claims.

Claims

1. A dynamic target perception and prediction method integrating camera and laser radar, which uses a dynamic target perception system integrating camera and laser radar to detect, track and predict three-dimensional dynamic targets; The dynamic target perception system includes a monocular camera, a laser radar and a controller, wherein the monocular camera, the laser radar and the controller are electrically connected, the monocular camera can transmit the captured image to the controller, and the controller can use a two-dimensional target detection algorithm to obtain a two-dimensional outer rectangle of the dynamic target, and obtain a region of interest in a three-dimensional space according to a projection relationship, and then perform point cloud clustering in a 3D ROI and fit the three-dimensional outer rectangle; the controller also includes a decision planning module, an inter-frame matching module, a tracker management module and a convolutional neural network; It is characterized in that The dynamic target perception and prediction method comprises the following steps: a. Target detection: First, align the lidar and camera to establish the spatial position relationship between the lidar and the camera; use the YOLOv4 target detection algorithm to obtain a 2D bounding rectangle on the image; then generate a 3D conical region of interest corresponding to the 2D bounding rectangle based on the projection relationship between the point cloud and the image; finally, cluster the point cloud within the 3D ROI and fit the 3D bounding rectangle; b. Target tracking: Match the target between frames based on the DeepSORT algorithm; use a finite state machine to manage the tracking results; c. Trajectory prediction: Use lane lines to constrain the movement of the vehicle and use extended Kalman filtering for trajectory prediction; In step a, when establishing the spatial position relationship between the laser radar and the camera, that is, the relationship between the radar coordinate system and the image pixel coordinate system, the mathematical model used is: in, is the pixel coordinate, is the radar coordinate; K is the camera intrinsic parameter matrix; and are the rotation matrix and translation matrix from the radar coordinate system to the camera coordinate system respectively; λ is the normalization coefficient; In step a, the point cloud clustering and 3D bounding rectangle fitting process is as follows: after obtaining the 3DROI, the point cloud is clustered in each 3D conical region using the Euclidean clustering method; after successful clustering, each 3D rectangular region will obtain several point cloud clusters C = {C i |i=1,2,…,n}, for each point cloud cluster C i , project its point cloud onto the image, calculate the distance between each point in the point cloud cluster and the center of the 2D bounding rectangle, and record the minimum distance as d(C i ), select the point cloud cluster with the smallest minimum distance as the target point cloud cluster C O ,Right now C O =argmin{d(C i )},i=1,2,…,n (2) Then the target point cloud cluster C O Perform 3D outer rectangle fitting; in the vertical direction, directly use C o The maximum and minimum values ​​in the z direction (z min ,z max ) as the boundary of the target in the vertical direction; in the horizontal direction, the 3D rectangle degenerates into a 2D rectangle, and the optimization formula of the 2D enclosing rectangle is: Among them, (x i ,y i ) is the point coordinate, θ, a1, a2 are the parameters of two perpendicular lines, and the point cloud cluster is divided into two parts, L1 and L2, according to the distance from the point to the boundary.

2. The dynamic target perception and prediction method of the fusion camera and laser radar according to claim 1 is characterized in that: In step a, it also includes preprocessing the point cloud; In turn The following steps are involved: Downsampling: First, the point cloud is downsampled using a voxel filtering algorithm; Denoising: The point cloud is then denoised using statistical filtering; Irrelevant point filtering: The irrelevant point filtering module is responsible for filtering out point clouds that are not of interest.

3. The dynamic target perception and prediction method of the fusion camera and laser radar according to claim 1 is characterized in that: In step a, the 3D ROI generation process of the fused image is as follows: according to the pinhole imaging principle, a cone in space will form a rectangle after being projected onto the image; any rectangle on the image corresponds to a conical area in space; the target detection algorithm YOLOv4 based on convolutional neural network is used to detect the two-dimensional outer rectangle of the target on the image; then the three-dimensional conical area corresponding to each two-dimensional outer rectangle is calculated according to the above mathematical model calculation formula (1); there is only one target in each three-dimensional conical area.

4. The dynamic target perception and prediction method of the fusion camera and laser radar according to claim 1 is characterized in that: In step b, the inter-frame matching of the target based on the DeepSORT algorithm is as follows: the DeepSORT algorithm uses CNN to extract the appearance feature vector of the target, uses the cosine distance of the feature vector to describe the similarity between the targets, and then uses the Hungarian algorithm to perform inter-frame matching.

5. The dynamic target perception and prediction method of the fusion camera and laser radar according to claim 1 is characterized in that: In step b, the tracking result is managed by using a finite state machine as follows: the finite state machine consists of three parts: state, event and action; each tracker has four states: tracking state, disappearing state, initial state and buffer state; when the tracker is in the tracking state, it means that the tracking result is stable and reliable; the disappearing state indicates that the target has left the field of view, and the tracker in the disappearing state will be deleted from the memory and no longer participate in the operation; the initial state and the buffer state are between the tracking state and the disappearing state, and are used to counter the influence of false detection and missed detection; the initial state is the initial state of the tracker; The tracker will enter the initial state when it is initialized. init The tracker will only change from the initial state to the tracking state if all frames are matched successfully; otherwise, the tracker will enter the disappearing state and no longer participate in the calculation; When the tracker is in the tracking state, if the matching fails in a certain frame, it will change from the tracking state to the buffering state; if in the next N buffer If one of the frames matches successfully, it will return to the tracking state; otherwise, it will enter the disappearing state.

6. The dynamic target perception and prediction method of the fusion camera and laser radar according to claim 1 is characterized in that: In step c, the trajectory prediction steps using lane line constraints are: Assume that the target is moving along the lane line, and the longitudinal speed along the lane is v x , the lateral velocity is v y , and there is no acceleration in both directions; let the target in the road coordinate system be (x, y); then the motion equation of the target in the road coordinate system can be expressed as: After linearization, it can be expressed as: During target detection, the target coordinates obtained are the coordinates in the vehicle body coordinate system, denoted as To establish the observation equation, it is necessary to establish the relationship between the vehicle coordinate system and the road coordinate system. Assuming that the geometric shape of the lane line is a Clothoid curve, the curvature of any position of the lane line can be described as: c(x) = c0 + c1x, where c0 is the curvature of the initial position and c1 is the curvature change rate. Based on this assumption, the road coordinates (x, y) and the vehicle coordinates The following approximate relationship can be established: Among them, y off is the distance between the vehicle and the center line of the road, is the angle between the vehicle and the road; The above formula is the observation model of the target, and the observation vector After linearization, we get in,

Images