Fusion positioning method for urban street dynamic scene

Abstract

The invention discloses an urban street dynamic scene-oriented fusion positioning method. The method comprises the following steps of S1, acquiring data of a camera and an IMU (Inertial Measurement Unit); s2, detecting and tracking image feature points; s3, estimating pose transformation; s4, filtering dynamic feature points on the image frames; s5, recollecting static feature points; s6, performing tight coupling optimization on vision and IMU (Inertial Measurement Unit); the method improves the positioning precision and robustness in a scene with many dynamic objects such as urban streets, introduces the target detection neural network to filter out the feature points on the dynamic objects, prevents the visual estimation result from being greatly interfered by many exterior points, and improves the visual estimation accuracy under the condition that the feature points are too few. Part of filtered feature points are recollected, some possible static feature points on targets such as vehicles and people are found, and tracking loss caused by too few feature points is avoided. A reliable positioning solution is provided for unmanned aerial vehicles, logistics robots, autonomous vehicles and the like.

Description

Technical Field

[0001] The present invention relates to the fields of autonomous driving and computer vision, and in particular to a dynamic object shielding SLAM method for urban street scenes. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is one of the underlying core technologies of autonomous driving. It can be divided into visual SLAM and laser SLAM based on the main sensor classification. A single sensor is difficult to meet the requirements of high-precision and high-robust autonomous positioning. The comprehensive positioning effect of fusing multi-sensor information and even introducing technologies such as deep learning has been significantly better than traditional SLAM in some aspects. Visual positioning has rich information sources and high accuracy, but it has some inherent defects. However, it is still easy to lose tracking in some complex specific scenes, such as low-texture scenes with few feature points, scenes with large lighting changes, and scenes with a large number of dynamic objects. The advantage of the inertial navigation system (IMU) is that it can output more accurate acceleration and angular velocity when the main body moves quickly, and the short-term positioning effect is good. The main disadvantage is that the cumulative error of long-term positioning is large, which just complements the effect of visual positioning. Therefore, VIO (Visual Inertial Odometry) that fuses visual and inertial information occupies a core position in fusion SLAM. General VIO systems are still based on visual information, so they are still limited by the inherent defects of visual positioning and perform poorly in some specific scenarios. For important positioning scenarios such as urban street scenes, the visual positioning effect is poor due to the large number of dynamic objects. Deep learning technology can be introduced to improve the positioning accuracy and robustness in this scenario. Summary of the invention

[0003] The purpose of the present invention is to solve the problem that there are many dynamic objects in urban street scenes, and the positioning accuracy and robustness of the VIO fusion positioning system are low. When performing fusion positioning trajectory tracking, the present invention combines the target detection algorithm with the traditional VIO system, and removes the feature points on the dynamic objects while maintaining lightweight computing and high real-time performance, thereby enhancing the positioning accuracy and robustness in scenes with many dynamic objects such as urban streets.

[0004] The technical solution is as follows:

[0005] A fusion positioning method for dynamic scenes in urban streets uses the MobileNet target detection network, vision, and IMU complementary methods to improve positioning accuracy and robustness. The specific steps include:

[0006] S1, camera and IMU data collection

[0007] Continuously acquire image frames and IMU measurements using cameras and inertial navigation systems;

[0008] S2. Image feature point detection and tracking

[0009] The KLT sparse optical flow method and Harris corner method are used to detect and track the feature points of the existing frame, and RANSAC is performed to filter out obvious abnormal feature points.

[0010] S3. Estimated pose transformation

[0011] The IMU measurement values ​​between two image frames are pre-integrated, and the pose transformation between the two frames is estimated based on the IMU measurement data.

[0012] S4. Filter out dynamic feature points on the image frame

[0013] Train a lightweight object detection network MobileNet-SSD, use MobileNet-SSD to run object detection in parallel, and add a KCF tracker to reduce the computational pressure of re-detecting objects in each frame. Take out various common dynamic objects on the street, such as cars, people, cats, and dogs, make a merge mask, and filter the feature point pairs obtained by S2 to obtain the bth k Static feature point set under the frame and the filtered dynamic feature point set

[0014] The present invention improves the 3×3 size Conv dw layer and Conv layer of the MobileNet-SSD method, that is, the model related parameters are L 2 Regularization makes MobileNet-SSD run faster and improves detection accuracy by 10–20%, while maintaining lightweight computation and high real-time performance.

[0015] S5. Recollection of static feature points

[0016] Preliminary estimate of the visual pose transformation result and the essential matrix. If the ratio of the visual pose transformation result to the IMU pose transformation estimation result exceeds the threshold, or the number of matched feature point pairs is less than the threshold, start the thread of static feature point recollection; that is, match the filtered feature point set, map one set of feature points to another frame through the essential matrix, calculate the distance value to the pole line after mapping, if it is less than the threshold, it is judged as a static feature point, recollected, added to the static feature point set, and re-estimate the visual pose transformation result.

[0017] S6, tight coupling optimization of camera vision and IMU

[0018] Use filtering-based or optimization-based methods to perform back-end optimization of vision and IMU, and output the final positioning estimation trajectory in real time.

[0019] Beneficial Effects

[0020] The present invention improves the positioning accuracy and robustness in scenes with many dynamic objects such as urban streets. It introduces a target detection neural network to filter out feature points on dynamic objects to prevent more external points from causing significant interference to the visual estimation results. At the same time, when there are too few feature points, some of the filtered feature points are re-collected to find feature points on some potentially static targets such as cars and people to avoid losing tracking due to too few feature points. It provides a reliable positioning solution for drones, logistics robots, self-driving cars, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 It is the overall flow chart of the positioning system of the present invention;

[0022] Figure 2 This is the target detection result of MobileNet-SSD in an embodiment of the present invention;

[0023] Figure 3 It is a handheld hardware experimental platform of an embodiment of the present invention;

[0024] Figure 4 This is a comparison diagram of the estimated trajectory and the standard trajectory of the positioning system according to an embodiment of the present invention;

[0025] Figure 5 This is an error curve of the estimated trajectory of the positioning system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0026] The following is an explanation of the embodiments of the present invention by specific specific examples, but it is not intended to limit the exemplary embodiments according to the present application. In order to avoid confusion or blurring the key points of the present invention, some specific details will be omitted in the description. It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. In order to make the purpose, technical solutions and advantages of the present invention clearer, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0027] The present invention is suitable for positioning and navigation of self-driving cars, delivery robots, handheld devices, drones and other equipment in urban street scenes with many dynamic objects, and can also be used for positioning and navigation of the above-mentioned equipment in conventional scenes.

[0028] like Figure 1 , a fusion positioning method for dynamic scenes in urban streets, using the MobileNet target detection network, vision and IMU complementary methods to improve positioning accuracy and robustness, specifically including the following steps:

[0029] S1, camera and IMU data collection

[0030] Use cameras and inertial navigation systems to continuously acquire image frames and IMU measurements; e.g. Figure 3 The handheld hardware experimental platform used in this system includes IMU and camera modules.

[0031] S2, image feature point detection and tracking;

[0032] The KLT sparse optical flow method and Harris corner method are used to detect and track the feature points of the existing frame, and RANSAC (Random Sample Consensus) is performed to filter out obvious abnormal feature points to obtain the b k The original feature point set of the frame in For b k The nth feature point of the frame image.

[0033] S3, estimating the pose transformation according to the IMU measurement value within the interval between two image frame acquisitions;

[0034] Usually, the image frame frequency collected is about 30Hz, and the IMU frequency is about 100Hz. Since the IMU frequency is much higher than the camera, in order to facilitate real-time update calculations, it is necessary to pre-integrate and align the IMU data with the visual frame. According to the IMU error model, the main error terms of the IMU measurement value include the acceleration bias value b a , and the gravity effect g w The main errors of the gyroscope include bias b w , both of which are also affected by additive Gaussian noise. The relationship between the observed values ​​and the true values ​​of the accelerometer and gyroscope is expressed as follows:

[0035]

[0036] where n a , n w are all Gaussian white noise, a t is the true value of the accelerometer, w t is the true value of the gyroscope, It represents the rotation relationship from the world coordinate system to the current IMU coordinate system. The following representation is similar. For two consecutive frames b k and b k+1 , whose time interval is [t k , t k+1 ], based on the above known bias, the IMU is pre-integrated to obtain the displacement change within the time interval Speed ​​Change With IMU attitude changes

[0037]

[0038] in

[0039]

[0040] is the rotation matrix between the two frame principal coordinate systems, ω x is the angular velocity in the x-axis direction, and the rest are similar. If the estimated bias changes slightly, the first-order approximations of the three will be adjusted according to the estimated values:

[0041]

[0042] J is the corresponding Jacob matrix, is a quaternion operator. The IMU pose transformation estimation and update between two frames can be completed.

[0043] S4, MobileNet-SSD filters out dynamic feature points on the image frame;

[0044] The present invention uses the city landscape dataset Cityscapes to train a lightweight target detection network MobileNet-SSD, which contains a large number of real-life photos of city street driving.

[0045] MobileNet-SSD runs as a separate thread in parallel with the feature point detection thread in S2. It performs target detection after downsampling the original image frame and adds a KCF (Kernelized Correlation Filters) tracker to reduce the computational pressure of re-detecting targets in each frame. It extracts labels of various common dynamic targets on the street, such as cars, people, cats, and dogs, to generate masks, and merges the masks of multiple targets into one mask.

[0046] The corresponding image feature points in S2 are roughly filtered using the above mask to filter out the feature points on targets such as cars and people. The remaining feature points are a set of static feature points. Where m<n.

[0047] Furthermore, the present invention improves the 3×3 size Conv dw layer and Conv layer in MobileNet-SSD, that is, the weights, biases and other parameters of the convolutional layer are L 2 Regularization:

[0048]

[0049] Where θ is the parameter weight, λ is the regularization coefficient, is a regularization term used to reduce the parameter space. Regularizing the loss function yields:

[0050]

[0051] f(x,θ * ) is the model output after regularization. As a result, the number of model parameters is reduced, the amount of calculation of the 3×3 convolution layer parameters is increased, and the amount of calculation of the 1×1 convolution layer is reduced, making the distribution of calculations more even, shortening the overall network calculation time, and being suitable for highly real-time VIO systems.

[0052] S5, static feature point re-collection;

[0053] Use b k Frame and b k+1 The static feature point set of the frame is matched to obtain the number of matched feature point pairs m0. For a pair of feature points that are matched on two frames, their pixel coordinates are p1 and p2 respectively. Considering that the camera intrinsic parameter K is known, the rigid body motion rotation of the camera between the two frames is R, and the displacement is t. P is the spatial position of the real object point corresponding to the observed pixel point. s1 and s2 are the scale information of the two frames. The relationship between the two pixel points in the two camera coordinate systems can be obtained:

[0054] s1p1=KP,s2p2=K(RP+t)

[0055] Considering the equality in the sense of scale, take x1 = K -1 p1, we can get the epipolar geometry constraint:

[0056]

[0057] Get the basic matrix Essential matrix F = K -T EK -1 For multiple pairs of feature points in a static set, the above constraint value will not be strictly 0 due to the presence of noise. RANSAC is used to find the optimal average constraint that is closest to 0. And the corresponding R and The combined homogeneous camera pose transformation

[0058] Comparison of camera visual pose transformation With IMU attitude changes like

[0059]

[0060] That is, when the absolute value of the ratio of the camera visual static point estimated pose transformation to the IMU pose estimated pose transformation is greater than the threshold T0, or the number of static feature points is less than the threshold T1, the feature point re-collection work is performed. Usually, T0 = [1.05, 1.2], T1 = 70.

[0061] for and Difference Will it with The feature point set is matched to obtain several feature point pairs. For these matching feature point pairs, b k Point p1 on the frame passes Can be mapped to b k+1 On the polar line I2 of the frame, ideally they will completely coincide:

[0062]

[0063] Where (u1, v1) is the pixel coordinate of p1. If the feature point is mismatched, or the point in the real 3D world corresponding to the feature point is moving, it will deviate greatly from I2 after mapping. Its distance to I2 is:

[0064]

[0065] For a suitable threshold T2, recollect all feature point pairs with d < T2 and add In, re-estimate And the corresponding camera visual pose transformation results T2 is usually 3 pixels. The absolute distance value set in the actual system depends on the internal and external parameters of the camera.

[0066] S6, tight coupling optimization of camera vision and IMU

[0067] The selection and parameter setting of the tightly coupled optimization method depends on the specific application environment. The innovative method of the present invention does not include this part. However, for the completeness of the system description, a simple description of the commonly used classical framework used in this part is still given. For the measurement data within a certain time range, a sliding window of a certain length is maintained, where the time range is usually between 0.5 seconds and more than ten seconds, and the setting of the sliding window length varies from a few frames to dozens of frames. The state quantities include:

[0068]

[0069] where x i is the IMU state corresponding to the i-th frame image, including:

[0070]

[0071] That is, the speed, displacement, rotation, accelerometer bias, and gyroscope bias measured by the IMU. represents the coordinate transformation between the camera and IMU, λ jis the jth feature point, i.e., the final feature point obtained after step S5. The window contains a set of feature points of several frame images. In order to make the positioning system more accurate and robust, a tightly coupled optimization method is adopted. The tightly coupled optimization method is a prior art, which is performed by establishing a suitable measurement value estimation residual model and selecting a Kalman filter, factor graph optimization or other filtering or optimization-based method for tightly coupled optimization. The final positioning estimation trajectory, i.e., the pose transformation result within each time unit, can be output in real time. The composed pose graph and the corresponding generated trajectory.

[0072] like Figure 1 As shown in the figure, the image-IMU data collected in real time or recorded in the data set will be processed by the ROS system and published in the form of ROS Topic. The VIO system can receive the sensor data for processing by subscribing to the corresponding Topic. The image feature point detection and tracking, IMU pre-integration, and MobileNets target object detection are run on independent threads. The image feature point detection and tracking thread receives the results of MobileNets-SSD recognition to filter dynamic feature points and aligns with the IMU thread by timestamp. The static feature points remaining after filtering are matched to complete the preliminary estimation of the visual pose, and compared with the IMU pose estimation results at the corresponding time to determine whether the number of static feature points is less than the threshold. If the conditions are met, the static feature point re-collection thread is triggered, and then the feature point matching and visual pose estimation are performed again. The residual of the preliminary pose estimation model of vision and IMU will be added to the tightly coupled optimization to obtain the final global pose estimation trajectory.

[0073] Performance Testing

[0074] The present invention uses a handheld hardware experimental platform (such as Figure 3 ) Actual measurements and public data sets are used to verify the performance of the fusion positioning system.

[0075] Table 1 shows the test results of the street dynamic scene dataset VIODE in the present invention, which measures the root mean square error (RMSE) of the absolute trajectory error (ATE) between the estimated trajectory and the standard trajectory, in meters. It can be seen that the positioning accuracy of decimeter level can be achieved in the high-interference urban street scene, effectively reducing the interference of dynamic objects.

[0076] Table 1 RMSE between the estimated trajectory of this system and the standard trajectory under VIODE

[0077]

[0078] Figure 2 The MobileNet-SSD network target detection result for a street view image. The detected targets have category labels and confidence levels.

[0079] Figure 4 This is a comparison of the estimated trajectory and the standard trajectory on a two-dimensional plane when the system is running once. The dotted line is the standard trajectory line, the colored line is the estimated trajectory line, and the color of the trajectory line intuitively indicates the size of the error (red means large error, blue means small error). It can be seen that the absolute trajectory error in most areas is within 0.3 meters.

[0080] Figure 5 is a graph showing the absolute trajectory error of the system trajectory estimation over time, and Figure 4 Correspondingly, it can be seen that the trajectory error is large during initialization, and then tends to be stable. The root mean square error of the absolute trajectory error throughout the process is about 0.27m.

[0081] The above description is only a specific implementation of the present invention. It should be pointed out that for ordinary technicians in this technical field, it cannot be determined that the specific implementation of the present invention is limited to these descriptions. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention. Those skilled in the art can make various changes in form and details, including making several simple deductions or substitutions, without departing from the spirit and scope of the present invention.

Claims

1. A fusion positioning method for dynamic scenes in urban streets, characterized in that: The following steps are involved: S1, camera and IMU data collection Continuously acquire image frames and IMU measurements using cameras and inertial navigation systems; S2. Image feature point detection and tracking Use KLT sparse optical flow method and Harris corner method to detect and track the feature points of the existing frame, and perform RANSAC to filter out obvious abnormal feature points; S3. Estimated pose transformation Pre-integrate the IMU measurement values ​​between two image frames, and estimate the pose transformation between the two frames based on the IMU measurement data; S4. Filter out dynamic feature points on the image frame Train a lightweight target detection network MobileNet-SSD, use MobileNet-SSD to run target detection in parallel, add a KCF tracker to reduce the computational pressure of re-detecting targets in each frame; extract various common dynamic targets on the street, such as cars, people, cats, and dogs, make a merge mask, filter the feature point pairs obtained by S2, and get the first b Static feature point set under k frames and the filtered dynamic feature point set S5. Recollection of static feature points Preliminarily estimate the visual pose transformation result and the essential matrix. If the ratio of the visual pose transformation result to the IMU pose transformation estimation result exceeds the threshold, or the number of matched feature point pairs is less than the threshold, start the thread of static feature point recollection; that is, match the filtered feature point set, map one set of feature points to another frame through the essential matrix, calculate the distance value to the pole line after mapping, if it is less than the threshold, it is judged as a static feature point, recollected, added to the static feature point set, and re-estimate the visual pose transformation result; S6, tight coupling optimization of camera vision and IMU Use filtering-based or optimization-based methods to perform back-end optimization of vision and IMU, and output the final positioning estimation trajectory in real time.

2. A fusion positioning method for urban street dynamic scenes as claimed in claim 1, characterized in that: In step S3, the pose transformation is estimated based on the IMU measurement values ​​within the interval between two image frame acquisitions, as follows: According to the IMU error model, the main error terms of the IMU measurement value include the acceleration bias value b a , and the gravity effect g w ; The main errors of the gyroscope include bias b w , both of which are also affected by additive Gaussian noise; the relationship between the observed values ​​and the true values ​​of the accelerometer and gyroscope is expressed as follows: where n a , n w are all Gaussian white noise, a t is the true value of the accelerometer, ω t is the true value of the gyroscope, It represents the rotation relationship from the world coordinate system to the IMU coordinate system at the current moment; for two consecutive frames b k and b k+1 , whose time interval is [t k , t k+1 ], based on the above known bias, the IMU is pre-integrated to obtain the displacement change within the time interval Speed ​​Change With IMU attitude changes in is the rotation matrix between the two frame principal coordinate systems, ω x is the angular velocity in the x-axis direction; if the estimated bias change is small, the first-order approximation of the three will be adjusted according to the estimated value: J is the corresponding Jacob matrix, is a quaternion operator; it can complete the IMU pose transformation estimation and update between two frames.

3. A fusion positioning method for urban street dynamic scenes as claimed in claim 1, characterized in that: In step S4, the 3×3 size Conv dw layer and Conv layer in MobileNet-SSD are improved, that is, the weights, biases and other parameters of the convolutional layer are L 2 Regularization: Where θ is the parameter weight, λ is the regularization coefficient, is a regularization term to reduce the parameter space; the loss function is regularized to obtain: f(x,θ * ) is the model output after regularization.

4. A fusion positioning method for urban street dynamic scenes as claimed in claim 1, characterized in that: In step S5, the specific process of re-collecting static feature points is as follows: Use b k Frame and b k+1 The static feature point set of the frame is matched to obtain the number of matched feature point pairs m0; for a pair of feature points that are matched on the two frames, their pixel coordinates are p1 and p2 respectively; considering that the camera intrinsic parameter K is known, the rigid body motion rotation of the camera between the two frames is R, and the displacement is t; P is the spatial position of the real object point corresponding to the observed pixel point, s1 and s2 are the scale information of the two frames, and the relationship between the two pixel points in the two camera coordinate systems can be obtained: s1p1=KP,s2p2=K(RP+t) Considering the equality in the sense of scale, take x1 = K -1 p1, we can get the epipolar geometry constraint: Get the basic matrix Essential matrix F = K -T EK -1 ; For multiple pairs of feature points in a static set, the above constraint values ​​will not be strictly 0 due to the presence of noise. RANSAC is used to find the optimal average constraint that is closest to 0. And the corresponding R and The combined homogeneous camera pose transformation Comparison of camera visual pose transformation With IMU attitude changes like That is, when the absolute value of the ratio of the camera visual static point estimated pose transformation to the IMU pose estimated transformation is greater than the threshold T0, or the number of static feature points is less than the threshold T1, the feature point re-collection work is performed; for and Difference Will it with The feature point set is matched to obtain several feature point pairs; for these matching feature point pairs, b k Point p1 on the frame passes Can be mapped to b k+1 On the polar line I2 of the frame, ideally they will completely coincide: Where (u1, v1) is the pixel coordinate of p1, and its distance to I2 is: Set the threshold T2, re-collect all feature point pairs with d < T2, and add In, re-estimate And the corresponding camera visual pose transformation results 5. A fusion positioning method for dynamic scenes in urban streets as claimed in claim 4, characterized in that: Take T0 = [1.05, 1.2], T1 = 70; T2 is 3 pixels.

Images