AGV obstacle tracking method based on multi-modal data and interactive Kalman filtering

Abstract

The invention belongs to the field of outdoor AGV obstacle tracking, and particularly relates to an AGV obstacle tracking method based on multi-modal data and interactive Kalman filtering. Comprising the following steps: calibrating a monocular camera and a laser radar, and establishing point cloud data and image data with synchronous timestamps; performing preprocessing and target detection operation on the point cloud data and the target detection frame to obtain preprocessed point cloud data and a generated target detection frame so as to obtain obstacle category information; acquiring an obstacle observation position according to the preprocessed point cloud data and the target detection frame; meanwhile, an improved SORT multi-target tracking algorithm is adopted, and a historical track of the obstacle is established; through a plurality of Kalman filtering fusion methods, a trajectory prediction result is obtained through prediction; finally, the AGV communicates with a dispatching center, global information is obtained, and the track trend is corrected in a targeted mode in combination with obstacle category information; and the historical track of the obstacle and the corrected prediction track are sent to the AGV for obstacle avoidance path planning.

Description

Technical Field

[0001] The invention belongs to the field of outdoor AGV obstacle tracking, in particular to an AGV obstacle tracking method based on multimodal data and interactive Kalman filtering. Background Art

[0002] Conventional AGVs use sensors such as lidar, monocular cameras or binocular cameras to make obstacle avoidance decisions and path planning based on the detected real-time obstacle positions. On the one hand, in order to control costs, a single sensor type is used, and specific sensors are only suitable for specific environments. For example, cameras are not suitable for low-light conditions, and lidar has weak echo signals in rainy and snowy weather. Therefore, using a single sensor alone will affect the accuracy of obstacle positioning. On the other hand, the movement of obstacles in high-mobility situations is uncertain. Obstacle avoidance based only on real-time positions and ignoring the future movement trends of obstacles will reduce the safety of AGV operation. Summary of the invention

[0003] The purpose of the present invention is to provide an outdoor AGV obstacle tracking method, specifically an AGV obstacle tracking method based on multimodal data and interactive Kalman filtering, which ensures that the AGV can accurately locate surrounding obstacles and predict its future trajectory during low-speed automatic driving. The prediction information provided by this method can help subsequent obstacle avoidance decisions and path planning to ensure safe driving.

[0004] The technical solution adopted by the present invention to achieve the above-mentioned purpose is: an AGV obstacle tracking method based on multimodal data and interactive Kalman filtering, comprising the following steps:

[0005] 1) Calibrate the monocular camera and lidar. At the same time, establish a timestamp synchronization mechanism. Use linear interpolation to align the point cloud data time collected in real time with the camera data time to obtain point cloud data and image data with synchronized timestamps.

[0006] 2) Preprocessing and target detection operations are performed on the point cloud data and image data respectively to obtain preprocessed point cloud data and generated target detection frames, and then obtain obstacle category information;

[0007] 3) Obtain the obstacle observation position based on the preprocessed point cloud data and target detection frame;

[0008] 4) According to the acquired obstacle position, the improved SORT multi-target tracking algorithm is used to associate continuous multi-frame data and establish the historical trajectory of the obstacle;

[0009] 5) Through multiple Kalman filter fusion methods, the trajectory prediction results of each obstacle in the next 5 seconds are obtained;

[0010] 6) The AGV communicates with the dispatch center to obtain global information, and combines the obstacle category information in step 2) to make targeted corrections to the trajectory trend to obtain the corrected predicted trajectory;

[0011] 7) The historical trajectory of the obstacle and the corrected predicted trajectory are sent to the AGV for obstacle avoidance path planning.

[0012] The monocular camera and laser radar are calibrated as follows:

[0013] The checkerboard calibration method is used to accurately measure the relative positions of the monocular camera, lidar and the vehicle body, and obtain their respective rotation matrices R and translation matrices T.

[0014] Among them, the external parameters of the monocular camera are The external parameters of the laser radar are

[0015] The establishment of a timestamp synchronization mechanism is specifically as follows:

[0016] 1-1) Assume that the camera acquisition time is t, and the latest time of the laser radar acquisition before and after time t is t 1 and t 2 ;

[0017] 1-2) Match the corresponding points of the point clouds of the previous and next two frames through the iterative closest point algorithm;

[0018] 1-3) Interpolate the coordinates and intensity values ​​of each point pair, assuming t 1 Time point p 1 Match t 2 Time point p 2 , then the interpolation of the laser radar point cloud data at time t is:

[0019]

[0020] The step 2) is specifically:

[0021] a. For the image data collected by the monocular camera, the YOLO series visual detection algorithm is used to generate the target detection frame (u, v, a, h, cls, score);

[0022] Among them, (u,v) represents the center point of the detection frame, a is the aspect ratio, h is the detection frame height, cls is the detection frame category, and score is the detection frame confidence; the dynamic obstacle categories of vehicles and pedestrians are retained, and the detection frames with confidence score ≤ 0.2 are removed;

[0023] b. For the interpolated LiDAR point cloud data, a filtering method is used to remove point cloud noise, and then the unnecessary point clouds of ground and off-ground objects are removed by downsampling methods such as direct-pass filtering or voxel filtering.

[0024] The step 3) is specifically:

[0025] 3-1) According to the external parameter matrix of the monocular camera and the lidar, the processed point cloud is projected into the target detection frame generated by the monocular camera;

[0026] 3-2) Count the median of the point cloud depth data as the depth value of the 2D detection box;

[0027] 3-3) Use the external parameters of the monocular camera to transform the coordinates of the obstacle relative to the AGV in the current frame, and calculate the direction angle θ of the obstacle movement in combination with the previous frame; the default direction of the first frame is 0, and the obstacle observation position (x, y, z, θ) is obtained;

[0028] 3-4) For the detection box with sparse projected point cloud, use the monocular ranging method to roughly estimate the depth of the object:

[0029] f*H / y

[0030] Among them, the pixel difference between the bottom edge of the detection frame and the optical center is y, f is the focal length, and H is the height of the camera setting;

[0031] After obtaining the depth information, execute step 3-3) to obtain the observed position (x, y, z, θ) of the obstacle relative to the AGV.

[0032] The improved SORT algorithm is specifically:

[0033] The Kalman filter of the improved SORT algorithm is: the Kalman filter of the obstacle motion state constructs three motion models: uniform linear, uniformly accelerated linear and uniform turning;

[0034] Among them, the transfer matrices of the three Kalman filters are The confidence level of each model is initialized to 0.33.

[0035] In step 4), the improved SORT multi-target tracking algorithm is used to associate continuous multi-frame data and establish the historical trajectory of the obstacle, specifically:

[0036] 4-1) If the current frame is the starting frame, initialize all observed obstacle instances and the corresponding Kalman filters, exit the current round of SORT algorithm, and proceed to the next round of observation;

[0037] 4-2) If the current frame is not the starting frame, calculate the Euclidean distance matrix M between the latest position of the existing obstacle instance and the newly observed obstacle position dis , the M disAs the cost matrix, the Hungarian algorithm is used to calculate the one-to-one matching relationship between the instance position and the observation position, and the matching observation position is appended to the corresponding obstacle history trajectory sequence to obtain the obstacle history trajectory;

[0038] 4-3) If an observation does not match an existing obstacle, initialize a new obstacle instance and the corresponding Kalman filter for it;

[0039] 4-4) If an existing obstacle instance has no observation match for more than 3 times, the instance will be deleted.

[0040] In step 5), the trajectory prediction result of each obstacle in the next 5 seconds is obtained by using multiple Kalman filter fusion methods, specifically:

[0041] 5-1) The dimensions of the state spaces of different Kalman filters are raised or reduced to align the dimensions, and the input information of the state vector and the covariance matrix are fused through the full probability formula. The state vector of the i-th filter is:

[0042]

[0043] The covariance matrix is:

[0044]

[0045] in, is the state of the jth Kalman filter at the k-1th step, is the probability of transferring from the jth motion model to the ith motion model at the k-1th step;

[0046] 5-2) Each Kalman filter performs prediction and update calculations based on the fused information;

[0047] 5-3) Recalculate the confidence of each filter:

[0048]

[0049] in, is the likelihood value obtained based on the error between the predicted value and the observed value. The larger the error, the smaller the likelihood value.

[0050] 5-4) Based on the confidence, the state vector and covariance matrix are fused and output, and the state vector is: The covariance matrix is:

[0051]

[0052] 5-5) Based on the latest model confidence, the prediction and output fusion parts of the iterative interactive Kalman filter are used to obtain the trajectory prediction results of each obstacle in the next 5 seconds.

[0053] The global information includes: obtaining static obstacles and road traffic rules information from the map, and obtaining the operating position information of other AGVs from the dispatch center.

[0054] The step 6) is specifically:

[0055] According to the obstacle category information obtained in step 2) combined with the global information, the trajectory trend is corrected in a targeted manner:

[0056] 6-1) For pedestrians, according to the positions of surrounding obstacles including static obstacles, other AGVs and dynamic obstacles identified by the current AGV, combined with the social force model, the corrected trajectory points are iteratively obtained, and then the corrected predicted trajectory is obtained;

[0057] a. The force exerted by a pedestrian towards the target is:

[0058]

[0059] Among them, m i For pedestrian quality, is the pedestrian’s expected speed towards the target direction, v i is the current speed of the pedestrian, τ i is the time constant for adjusting the speed;

[0060] b. The repulsive force between pedestrians and obstacles is:

[0061]

[0062] Among them, A and B are model parameters of force intensity and attenuation distance, r ij is the comfortable distance between pedestrian i and pedestrian j, d ij is the actual distance between pedestrian i and pedestrian j, n ij is the unit vector from pedestrian i to pedestrian j;

[0063] c. Pedestrian force summary, corrected speed and corrected position:

[0064]

[0065] x i (t+Δt)=x i (t)+Δt·v i (t)

[0066] 6-2) For vehicles, the probability discrimination model of logistic regression based on data training is used to determine whether the vehicle state is stationary or moving. The discrimination formula is:

[0067]

[0068] Where x is the vehicle feature, f(x) is the discrimination result, f(x)>0.5 means the vehicle is moving, otherwise the vehicle is stationary;

[0069] If the vehicle is stationary, the historical trajectory point information is aggregated to determine its world coordinate position, synchronized to the map cache, and the motion estimation of the stationary object is stopped. The corrected coordinate position of the stationary vehicle is the mean of the historical trajectory points, that is:

[0070] The present invention has the following beneficial effects and advantages:

[0071] 1. The method provided by the present invention enables the AGV to perceive the future movement trend of obstacles in advance, intervene in the AGV motion planning earlier, and improve the driving safety of the AGV.

[0072] 2. This invention provides an innovative AGV obstacle tracking method based on multimodal data and interactive Kalman filtering. The deviation of the position estimation of high-maneuverability obstacles by a single sensor and Kalman filter is analyzed. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 Method diagram of the outdoor AGV obstacle tracking method of the present invention. DETAILED DESCRIPTION

[0074] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0075] The obstacle positioning result is inaccurate under the condition of a single sensor. Therefore, this method combines two sensors, a monocular camera and a lidar, and performs front fusion in the data collection stage to achieve accurate positioning. First, the external parameter calibration is performed according to the relative posture (rotation and translation) between the sensor and the vehicle body. When the vehicle body is running, the sensor collects data at a fixed frequency. Since the timestamps and frequencies of different sensors are not aligned, the linear interpolation method is used for data alignment. For the image data collected by the camera, a 2D detection algorithm is used to generate a target detection frame, and the detection frame with low confidence is eliminated. For the point cloud data collected by the lidar, it is first processed by downsampling and denoising, and then the point cloud is projected to the target detection frame through the external parameter matrix. The depth data of the point cloud is aggregated to obtain the depth value d of the 2D detection frame. The position (x, y, z) of the obstacle relative to the AGV at the current moment is obtained through coordinate transformation, and the direction angle θ of the obstacle movement is calculated using the two-frame position.

[0076] A multi-target tracking algorithm is used to associate continuous multi-frame data to determine the historical trajectory of the obstacle. During the operation of outdoor AGV, the movement of obstacles is highly maneuverable. At the same time, the relative position change is nonlinear during the relative movement between the vehicle and the obstacle. Therefore, the tracking process is implemented based on a multi-model interactive Kalman filter to consider the conversion of different movement modes of the obstacle. After obtaining the historical trajectory of the obstacle and the Kalman filter that describes its movement, its future trajectory can be predicted in multiple steps. Finally, the trajectory is corrected by combining the obstacle category information and global information (such as maps, dispatch centers).

[0077] like Figure 1 As shown in FIG. 1 , it is a flow chart of the method of the present invention. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering of the present invention comprises the following steps:

[0078] 1) Calibrate the monocular camera and lidar. At the same time, establish a timestamp synchronization mechanism. Use linear interpolation to align the point cloud data time collected in real time with the camera data time to obtain point cloud data and image data with synchronized timestamps.

[0079] A. Calibrate the monocular camera and lidar, specifically:

[0080] The checkerboard calibration method is used to accurately measure the relative positions of the monocular camera, lidar and the vehicle body, and obtain their respective rotation matrices R and translation matrices T.

[0081] Among them, the external parameters of the monocular camera are The external parameters of the laser radar are

[0082] B. Establish a timestamp synchronization mechanism, specifically:

[0083] 1-1) Assume that the camera acquisition time is t, and the latest time of the laser radar acquisition before and after time t is t 1 and t 2 ;

[0084] 1-2) Match the corresponding points of the point clouds of the previous and next two frames through the iterative closest point algorithm;

[0085] 1-3) Interpolate the coordinates and intensity values ​​of each point pair, assuming t 1 Time point p 1 Match t 2 Time point p 2 , then the interpolation of the laser radar point cloud data at time t is:

[0086]

[0087] 2) Preprocessing and target detection operations are performed on the point cloud data and image data respectively to obtain preprocessed point cloud data and generated target detection frames, and then obtain obstacle category information;

[0088] a. For the image data collected by the monocular camera, the YOLO series visual detection algorithm is used to generate the target detection frame (u, v, a, h, cls, score);

[0089] Among them, (u,v) represents the center point of the detection frame, a is the aspect ratio, h is the detection frame height, cls is the detection frame category, and score is the detection frame confidence; the dynamic obstacle categories of vehicles and pedestrians are retained, and the detection frames with confidence score ≤ 0.2 are removed;

[0090] b. For the interpolated LiDAR point cloud data, a filtering method is used to remove point cloud noise, and then the unnecessary point clouds of ground and off-ground objects are removed by downsampling methods such as direct-pass filtering or voxel filtering.

[0091] 3) Obtain the obstacle observation position based on the preprocessed point cloud data and target detection frame;

[0092] 3-1) According to the external parameter matrix of the monocular camera and the lidar, the processed point cloud is projected into the target detection frame generated by the monocular camera;

[0093] 3-2) Count the median of the point cloud depth data as the depth value of the 2D detection box;

[0094] 3-3) Use the external parameters of the monocular camera to transform the coordinates of the obstacle relative to the AGV in the current frame, and calculate the direction angle θ of the obstacle movement in combination with the previous frame; the default direction of the first frame is 0, and the obstacle observation position (x, y, z, θ) is obtained;

[0095] 3-4) For the detection box with sparse projected point cloud, use the monocular ranging method to roughly estimate the depth of the object:

[0096] f*H / y

[0097] Among them, the pixel difference between the bottom edge of the detection frame and the optical center is y, f is the focal length, and H is the height of the camera setting;

[0098] After obtaining the depth information, execute step 3-3) to obtain the observed position (x, y, z, θ) of the obstacle relative to the AGV.

[0099] 4) According to the acquired obstacle position, the improved SORT multi-target tracking algorithm is used to associate continuous multi-frame data and establish the historical trajectory of the obstacle;

[0100] Among them, the improved SORT algorithm is specifically:

[0101] The Kalman filter of the improved SORT algorithm is: the Kalman filter of the obstacle motion state constructs three motion models: uniform linear, uniformly accelerated linear and uniform turning;

[0102] Among them, the transfer matrices of the three Kalman filters are The confidence level of each model is initialized to 0.33.

[0103] The improved SORT multi-target tracking algorithm is used to associate continuous multi-frame data and establish the historical trajectory of obstacles. Specifically:

[0104] 4-1) If the current frame is the starting frame, initialize all observed obstacle instances and the corresponding Kalman filters, exit the current round of SORT algorithm, and proceed to the next round of observation;

[0105] 4-2) If the current frame is not the starting frame, calculate the Euclidean distance matrix M between the latest position of the existing obstacle instance and the newly observed obstacle position dis , the M dis As the cost matrix, the Hungarian algorithm is used to calculate the one-to-one matching relationship between the instance position and the observation position, and the matching observation position is appended to the corresponding obstacle history trajectory sequence to obtain the obstacle history trajectory;

[0106] 4-3) If an observation does not match an existing obstacle, initialize a new obstacle instance and the corresponding Kalman filter for it;

[0107] 4-4) If an existing obstacle instance has no observation match for more than 3 times, the instance will be deleted.

[0108] 5) Through multiple Kalman filter fusion methods, the trajectory prediction results of each obstacle in the next 5 seconds are obtained;

[0109] 5-1) The dimensions of the state spaces of different Kalman filters are raised or reduced to align the dimensions, and the input information of the state vector and the covariance matrix are fused through the full probability formula. The state vector of the i-th filter is:

[0110]

[0111] The covariance matrix is:

[0112]

[0113] in, is the state of the jth Kalman filter at the k-1th step, is the probability of transferring from the jth motion model to the ith motion model at the k-1th step;

[0114] 5-2) Each Kalman filter performs prediction and update calculations based on the fused information;

[0115] 5-3) Recalculate the confidence of each filter:

[0116]

[0117] in, is the likelihood value obtained based on the error between the predicted value and the observed value. The larger the error, the smaller the likelihood value.

[0118] 5-4) Based on the confidence, the state vector and covariance matrix are fused and output, and the state vector is: The covariance matrix is:

[0119]

[0120] 5-5) Based on the latest model confidence, the prediction and output fusion parts of the iterative interactive Kalman filter are used to obtain the trajectory prediction results of each obstacle in the next 5 seconds.

[0121] 6) The AGV communicates with the dispatch center to obtain global information, and combines the obstacle category information in step 2) to make targeted corrections to the trajectory trend to obtain the corrected predicted trajectory;

[0122] According to the obstacle category information obtained in step 2), combined with the global information including static obstacles and road traffic rules obtained from the map, and the operating position information of other AGVs obtained from the dispatch center, the trajectory trend is corrected in a targeted manner:

[0123] 6-1) For pedestrians, according to the positions of surrounding obstacles including static obstacles, other AGVs and dynamic obstacles identified by the current AGV, combined with the social force model, the corrected trajectory points are iteratively obtained, and then the corrected predicted trajectory is obtained;

[0124] a. The force exerted by a pedestrian towards the target is:

[0125]

[0126] Among them, m i For pedestrian quality, is the pedestrian’s expected speed towards the target direction, v i is the current speed of the pedestrian, τ i is the time constant for adjusting the speed;

[0127] b. The repulsive force between pedestrians and obstacles is:

[0128]

[0129] Among them, A and B are model parameters of force intensity and attenuation distance, r ij is the comfortable distance between pedestrian i and pedestrian j, d ij is the actual distance between pedestrian i and pedestrian j, n ij is the unit vector from pedestrian i to pedestrian j;

[0130] c. Pedestrian force summary, corrected speed and corrected position:

[0131]

[0132] x i (t+Δt)=x i (t)+Δt·v i (t)

[0133] 6-2) For vehicles, the probability discrimination model of logistic regression based on data training is used to determine whether the vehicle state is stationary or moving. The discrimination formula is:

[0134]

[0135] Where x is the vehicle feature, f(x) is the discrimination result, f(x)>0.5 means the vehicle is moving, otherwise the vehicle is stationary;

[0136] If the vehicle is stationary, the historical trajectory point information is aggregated to determine its world coordinate position, synchronized to the map cache, and the motion estimation of the stationary object is stopped. The corrected coordinate position of the stationary vehicle is the mean of the historical trajectory points, that is:

[0137] 7) The historical trajectory of the obstacle and the corrected predicted trajectory are sent to the AGV for obstacle avoidance path planning.

[0138] Embodiment 1:

[0139] The example illustrates how a 16-line laser radar and a monocular camera can track obstacles. It is divided into four stages.

[0140] Phase 1: Sensor calibration and timestamp alignment

[0141] 1. Use the checkerboard calibration method to accurately measure the relative positions of the monocular camera, lidar and the vehicle body, and obtain their respective rotation matrices R and translation matrices T. The extrinsic parameters of the monocular camera are The external parameters of the laser radar are

[0142] 2. During vehicle operation, the sensor regularly reports point cloud data and image data.

[0143] 3. Establish a timestamp synchronization mechanism and align the point cloud data to the camera data by linear interpolation. Assume that the camera acquisition time is t, and the closest time of the laser radar before and after t is t 1 and t 2 First, the ICP algorithm (iterative closest point algorithm) is used to match the corresponding points of the point clouds of the previous and next frames, and then the coordinates and intensity values ​​of each point pair are interpolated. Assume that t 1 Time point p 1 Match t 2 Time point p 2 , then the lidar data interpolation at time t is

[0144] The second stage: object detection and depth information completion

[0145] 1. For the image data collected in the first stage, the YOLO series visual detection algorithm is used to generate the target detection frame (u, v, a, h, cls, score), where (u, v) represents the center point of the detection frame, a is the aspect ratio, h is the height of the detection frame, cls is the detection frame category, and score is the detection frame confidence. Dynamic obstacle categories such as vehicles and pedestrians are retained, and detection frames with a confidence score ≤ 0.2 are removed.

[0146] 2. For the interpolated point cloud data in the first stage, the filtering method is first used to remove the point cloud noise, and then the unnecessary point clouds such as the ground and off-ground objects are removed through downsampling methods such as straight-through filtering and voxel filtering.

[0147] 3. According to the external parameter matrix calculated in the first stage, the processed point cloud is projected into the target detection frame generated by the camera, and the median of the point cloud depth data is counted as the depth value of the 2D detection frame. Then, the coordinates are transformed through the camera external parameters to obtain the coordinates of the obstacle relative to the AGV at the current moment. Combined with the previous frame, the direction angle θ of the obstacle movement is calculated (the direction of the first frame is 0), and the obstacle observation position (x, y, z, θ) is obtained.

[0148] 4. For detection frames with sparse projected point clouds, use the monocular ranging method to roughly estimate the depth of the object. The pixel difference between the bottom edge of the detection frame and the optical center is y, f is the focal length, H is the height of the camera setting, and the depth is f*H / y. After obtaining the depth information, the observation position (x, y, z, θ) of the obstacle relative to the AGV is obtained in the same way as step 3 of this stage.

[0149] Phase 3: Obstacle trajectory tracking and prediction

[0150] 1. For the obstacle positions obtained in the second stage, the SORT multi-target tracking algorithm is used to associate continuous multi-frame data and establish the historical trajectory of the obstacle.

[0151] 2. Improve the Kalman filter of the SORT algorithm in step 1. Considering that high-maneuverability dynamic obstacles will have different motion mode conversions, we construct Kalman filters for the motion state of the obstacles with three motion models: uniform linear, uniformly accelerated linear, and uniform turning. The transfer matrices of the three Kalman filters are The confidence level of each model is initialized to 0.33.

[0152] 3. The interactive Kalman filter aims to dynamically adjust the filter confidence according to the error in the iterative process, and provides multiple Kalman filter fusion calculation methods, which are divided into four steps: input fusion, prediction and update, model confidence update and output fusion. First, the dimensions of the state spaces of different Kalman filters are raised or reduced to align the dimensions, and the input information of the state vector and the covariance matrix are fused through the full probability formula. The state vector of the i-th filter is Covariance matrix in, is the state of the j-th Kalman filter at the k-1th step (the same applies to the covariance matrix), is the probability of transferring from the jth motion model to the ith motion model at the k-1th step. Then each Kalman filter performs prediction and update calculations based on the fused information. Next, recalculate the confidence of each filter in is the likelihood value obtained based on the error between the predicted value and the observed value in this step. The larger the error, the smaller the likelihood value. Finally, based on the confidence level, the state vector and covariance matrix are fused and output. The state vector Covariance matrix

[0153] 4. Based on the latest model confidence, the prediction and output fusion parts of the iterative interactive Kalman filter are used to obtain the trajectory prediction results of each obstacle in the next 5 seconds.

[0154] Stage 4: Correcting the trajectory based on global information

[0155] 1. AGV communicates with the dispatch center to obtain global information, such as static obstacles (buildings, etc.) and road traffic rules from the map, and the operating location information of other AGVs from the dispatch center.

[0156] 2. According to the obstacle category information given by the second-stage detection model and the global information in step 1, the trajectory trend is corrected in a targeted manner. For pedestrians, the trajectory points are iteratively corrected according to the positions of surrounding obstacles (static obstacles, other AGVs, and dynamic obstacles identified by the current AGV) and the social force model. For vehicles, a simple probability discrimination model is used to determine whether the vehicle state is stationary or moving. If the vehicle is stationary, the historical trajectory point information is aggregated to determine its world coordinate position, synchronized to the map cache, and the motion estimation of stationary objects is stopped.

[0157] 3. Send the real-time location and predicted trajectory information of the obstacle to the AGV's obstacle avoidance decision and path planning system.

[0158] In summary, an innovative AGV obstacle tracking method based on multimodal data and interactive Kalman filtering is provided. The deviation between the position estimation of highly maneuverable obstacles by a single sensor and the Kalman filter is analyzed, and a method for sensing the obstacle position and future trajectory for path planning is proposed.

[0159] Those skilled in the art will appreciate that the above are only preferred embodiments of the present invention, and the various embodiments of the present disclosure and / or the features described in the claims may be combined or combined in various ways, even if such combinations or combinations are not explicitly described in the present disclosure. It is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art may still modify the technical solutions described in the aforementioned embodiments, or perform equivalent substitutions on some of the technical features therein. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

[0160] Although preferred embodiments of the present invention have been described, additional changes and modifications may be made to these embodiments by those skilled in the art once the basic inventive concepts are known. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention. Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering is characterized by: The following steps are involved: 1) Calibrate the monocular camera and lidar. At the same time, establish a timestamp synchronization mechanism. Use linear interpolation to align the point cloud data time collected in real time with the camera data time to obtain point cloud data and image data with synchronized timestamps. 2) Preprocessing and target detection operations are performed on the point cloud data and image data respectively to obtain preprocessed point cloud data and generated target detection frames, and then obtain obstacle category information; 3) Obtain the obstacle observation position based on the preprocessed point cloud data and target detection frame; 4) According to the acquired obstacle position, the improved SORT multi-target tracking algorithm is used to associate continuous multi-frame data and establish the historical trajectory of the obstacle; 5) Through multiple Kalman filter fusion methods, the trajectory prediction results of each obstacle in the next 5 seconds are predicted; 6) The AGV communicates with the dispatch center to obtain global information, and combines the obstacle category information in step 2) to make targeted corrections to the trajectory trend to obtain the corrected predicted trajectory; 7) The historical trajectory of the obstacle and the corrected predicted trajectory are sent to the AGV for obstacle avoidance path planning.

2. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1 is characterized in that: The monocular camera and laser radar are calibrated as follows: The checkerboard calibration method is used to accurately measure the relative positions of the monocular camera, lidar and the vehicle body, and obtain their respective rotation matrices R and translation matrices T. Among them, the external parameters of the monocular camera are The external parameters of the laser radar are 3. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1 is characterized in that: The establishment of a timestamp synchronization mechanism is specifically as follows: 1-1) Assume that the camera acquisition time is t, and the latest time of the laser radar acquisition before and after time t is t1 and t2 respectively; 1-2) Match the corresponding points of the point clouds of the previous and next two frames through the iterative closest point algorithm; 1-3) Interpolate the coordinates and intensity values ​​of each point pair. Assuming that the point p1 at time t1 matches the point p2 at time t2, the interpolation of the lidar point cloud data at time t is:

4. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1, characterized in that: The step 2) is specifically: a. For the image data collected by the monocular camera, the YOLO series visual detection algorithm is used to generate the target detection frame (u, v, a, h, cls, score); Among them, (u,v) represents the center point of the detection frame, a is the aspect ratio, h is the detection frame height, cls is the detection frame category, and score is the detection frame confidence; the dynamic obstacle categories of vehicles and pedestrians are retained, and the detection frames with confidence score ≤ 0.2 are removed; b. For the interpolated LiDAR point cloud data, a filtering method is used to remove point cloud noise, and then the unnecessary point clouds of ground and off-ground objects are removed by downsampling methods such as direct-pass filtering or voxel filtering.

5. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1, characterized in that: The step 3) is specifically: 3-1) According to the external parameter matrix of the monocular camera and the lidar, the processed point cloud is projected into the target detection frame generated by the monocular camera; 3-2) Count the median of the point cloud depth data as the depth value of the 2D detection box; 3-3) Use the external parameters of the monocular camera to transform the coordinates of the obstacle relative to the AGV in the current frame, and calculate the direction angle θ of the obstacle movement in combination with the previous frame; the default direction of the first frame is 0, and the obstacle observation position (x, y, z, θ) is obtained; 3-4) For the detection box with sparse projected point cloud, use the monocular ranging method to roughly estimate the depth of the object: f*H / y Among them, the pixel difference between the bottom edge of the detection frame and the optical center is y, f is the focal length, and H is the height of the camera setting; after obtaining the depth information, execute step 3-3) to obtain the observed position (x, y, z, θ) of the obstacle relative to the AGV.

6. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1, characterized in that: The improved SORT algorithm is specifically: The Kalman filter of the improved SORT algorithm is: the Kalman filter of the obstacle motion state constructs three motion models: uniform linear, uniformly accelerated linear and uniform turning; Among them, the transfer matrices of the three Kalman filters are The confidence level of each model is initialized to 0.

33.

7. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1, characterized in that: In step 4), the improved SORT multi-target tracking algorithm is used to associate continuous multi-frame data and establish the historical trajectory of the obstacle, specifically: 4-1) If the current frame is the starting frame, initialize all observed obstacle instances and the corresponding Kalman filters, exit the current round of SORT algorithm, and proceed to the next round of observation; 4-2) If the current frame is not the starting frame, calculate the Euclidean distance matrix M between the latest position of the existing obstacle instance and the newly observed obstacle position dis , the M dis As the cost matrix, the Hungarian algorithm is used to calculate the one-to-one matching relationship between the instance position and the observation position, and the matching observation position is appended to the corresponding obstacle history trajectory sequence to obtain the obstacle history trajectory; 4-3) If an observation does not match an existing obstacle, initialize a new obstacle instance and the corresponding Kalman filter for it; 4-4) If an existing obstacle instance has no observation match for more than 3 times, the instance will be deleted.

8. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1, characterized in that: In step 5), the trajectory prediction result of each obstacle in the next 5 seconds is obtained by using multiple Kalman filter fusion methods, specifically: 5-1) The dimensions of the state spaces of different Kalman filters are raised or reduced to align the dimensions, and the input information of the state vector and the covariance matrix are fused through the full probability formula. The state vector of the i-th filter is: The covariance matrix is: in, is the state of the jth Kalman filter at the k-1th step, is the probability of transferring from the jth motion model to the ith motion model at the k-1th step; 5-2) Each Kalman filter performs prediction and update calculations based on the fused information; 5-3) Recalculate the confidence of each filter: in, is the likelihood value obtained based on the error between the predicted value and the observed value. The larger the error, the smaller the likelihood value. 5-4) Based on the confidence, the state vector and covariance matrix are fused and output, and the state vector is: The covariance matrix is: 5-5) Based on the latest model confidence, the prediction and output fusion parts of the iterative interactive Kalman filter are used to obtain the trajectory prediction results of each obstacle in the next 5 seconds.

9. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1, characterized in that: The global information includes: obtaining static obstacles and road traffic rules information from the map, and obtaining the operating position information of other AGVs from the dispatch center.

10. The AGV obstacle tracking method based on multimodal data and interactive Kalman filtering according to claim 1, characterized in that: The step 6) is specifically: According to the obstacle category information obtained in step 2) combined with the global information, the trajectory trend is corrected in a targeted manner: 6-1) For pedestrians, according to the positions of surrounding obstacles including static obstacles, other AGVs and dynamic obstacles identified by the current AGV, combined with the social force model, the corrected trajectory points are iteratively obtained, and then the corrected predicted trajectory is obtained; a. The force exerted by a pedestrian towards the target is: Among them, m i For pedestrian quality, is the pedestrian’s expected speed towards the target direction, v i is the current speed of the pedestrian, τ i is the time constant for adjusting the speed; b. The repulsive force between pedestrians and obstacles is: Among them, A and B are model parameters of force intensity and attenuation distance, r ij is the comfortable distance between pedestrian i and pedestrian j, d ij is the actual distance between pedestrian i and pedestrian j, n ij is the unit vector from pedestrian i to pedestrian j; c. Pedestrian force summary, corrected speed and corrected position: x i (t+Δt)=x i (t)+Δt·v i (t) 6-2) For vehicles, the probability discrimination model of logistic regression based on data training is used to determine whether the vehicle state is stationary or moving. The discrimination formula is: Where x is the vehicle feature, f(x) is the discrimination result, f(x)>0.5 means the vehicle is moving, otherwise the vehicle is stationary; If the vehicle is stationary, the historical trajectory point information is aggregated to determine its world coordinate position, synchronized to the map cache, and the motion estimation of the stationary object is stopped. The corrected coordinate position of the stationary vehicle is the mean of the historical trajectory points, that is:

Images