A fusion positioning method for urban street dynamic scene

Abstract

The application discloses a kind of fusion positioning methods for urban street dynamic scene, comprising the following steps: S1, camera and IMU data acquisition;S2, image feature point detection and tracking;S3, estimate pose transformation;S4, filter out dynamic feature points on image frame;S5, static feature point re-collection;S6, tight coupling optimization of vision and IMU;The application improves the positioning accuracy and robustness in the scene of more dynamic objects such as urban streets, introduces a target detection neural network to filter out feature points on dynamic objects to prevent more outliers from causing significant interference to the visual estimation result, and in the case of too few feature points, re-collect some filtered feature points on vehicles, people and other targets that may be static to avoid losing tracking due to too few feature points. Provides a reliable positioning solution for drones, 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 core technologies underlying autonomous driving. Based on the primary sensor, it can be categorized into visual SLAM and laser SLAM. A single sensor alone cannot meet the high-precision and robustness requirements for autonomous positioning. Fusion of multi-sensor information, even incorporating deep learning techniques, has significantly outperformed traditional SLAM in some areas. Visual positioning, while rich in information sources, offers high accuracy, but has inherent limitations. It can still easily lose tracking in complex and specific scenarios, such as low-texture scenes with few feature points, scenes with significant lighting variations, and scenes with numerous dynamic objects. The advantage of an inertial navigation system (IMU) is that it can output relatively accurate acceleration and angular velocity during rapid motion, resulting in excellent short-term positioning performance. However, its main disadvantage is the large cumulative error in long-term positioning, which complements the effectiveness of visual positioning. Therefore, VIO (Visual Inertial Odometry), which fuses visual and inertial information, plays a central role in fusion SLAM. Conventional VIO systems still rely primarily on visual information and are therefore limited by the inherent flaws of visual positioning, resulting in poor performance in certain scenarios. However, in critical positioning scenarios such as urban street scenes, where visual positioning is less effective due to the prevalence of dynamic objects, deep learning techniques can be introduced to improve positioning accuracy and robustness in these scenarios. Summary of the Invention

[0003] The purpose of this invention is to address the low positioning accuracy and poor robustness of VIO fusion positioning systems in urban street scenes, where there are many dynamic objects. This invention combines target detection algorithms with traditional VIO systems during fusion positioning trajectory tracking. While maintaining lightweight computing and high real-time performance, it eliminates feature points on dynamic objects, thereby enhancing positioning accuracy and robustness in scenes with a high number of dynamic objects, such as urban streets.

[0004] The technical solution is as follows:

[0005] A fusion positioning method for dynamic urban street scenes uses the MobileNet object detection network, vision, and IMU to complement each other to improve positioning accuracy and robustness. The specific steps include:

[0006] S1, camera and IMU data acquisition

[0007] Consecutive image frames and IMU measurements are collected using a camera and an inertial navigation system;

[0008] S2, image feature point detection and tracking

[0009] Feature points of the existing frame are detected and tracked using KLT sparse optical flow method and Harris corner point method, and RANSAC is performed to exclude obvious abnormal feature points.

[0010] S3, estimate pose transformation

[0011] The IMU measurements between two image frames are pre-integrated, and the pose transformation between the two frames is estimated from the IMU measurement data.

[0012] S4, filter dynamic feature points on the image frame

[0013] A lightweight target detection network MobileNet-SSD is trained, MobileNet-SSD is used for parallel target detection, and a KCF tracker is added to reduce the calculation pressure of re-detecting targets in each frame. Common dynamic targets such as cars, people, cats, dogs and the like on the street are taken out, a merged mask is made, the feature point pairs obtained in S2 are filtered, and a static feature point set under the b k frame is obtained and the filtered dynamic feature point set

[0014] The present application improves the 3x3 size Conv dw layer and Conv layer of MobileNet-SSD method, that is, the L 2 regularization of the model related parameters, so that the running speed of MobileNet-SSD is faster, and the detection accuracy is improved by 10-20%, and the lightweight calculation and high real-time performance are maintained.

[0015] S5, static feature point re-collection

[0016] The visual pose transformation result and the essential matrix are preliminarily estimated, if the ratio of the visual pose transformation result and the pose transformation estimation result of the IMU exceeds the threshold value, or the number of matched feature point pairs is less than the threshold value, the thread of static feature point re-collection is started; that is, the filtered feature point set is matched, one group of feature points is mapped to another frame through the essential matrix, the distance value to the epipolar line after mapping is calculated, if it is less than the threshold value, it is judged as a static feature point, and re-collection is performed, and the static feature point set is added, and the visual pose transformation result is re-estimated.

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

[0018] A filtering-based or optimization-based method is used to perform visual and IMU back-end optimization, and the final positioning estimation trajectory is output in real time.

[0019] Beneficial effects

[0020] This invention improves positioning accuracy and robustness in scenarios with a high number of dynamic objects, such as urban streets. It introduces a target detection neural network to filter out feature points on dynamic objects, preventing significant interference from outliers on the visual estimation results. Furthermore, if feature points are too few, some of the filtered feature points are recollected to identify feature points on potentially static objects, such as cars and people, to avoid loss of tracking due to a lack of feature points. This provides a reliable positioning solution for drones, logistics robots, self-driving cars, and other applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 This 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 for the 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 the error curve of the estimated trajectory of the positioning system according to the embodiment of the present invention. DETAILED DESCRIPTION

[0026] The following specific embodiments of the present invention are described, but are not intended to limit the exemplary embodiments of 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 objects, 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 a large number of 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 urban street scenes uses the MobileNet target detection network, vision, and IMU to complement each other to improve positioning accuracy and robustness. The specific steps include:

[0029] S1, camera and IMU data acquisition

[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] Use KLT sparse optical flow method and Harris corner method to detect and track the feature points of the existing frame, and perform RANSAC (Random Sample Consensus) 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 based on the IMU measurement values ​​during the interval between two image frame acquisitions;

[0034] The image frame frequency is usually 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 gravitational effect g w The main errors of gyroscope include bias b w , both 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 ​​changes 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 little, the first-order approximations of the three will be adjusted according to the estimated value:

[0041]

[0042] J is the corresponding Jacob matrix, is a quaternion operator. This completes the IMU pose transformation estimation and update between two frames.

[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 the lightweight object detection network MobileNet-SSD. The dataset 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 downsamples the original image frames before performing object detection and adds a KCF (Kernelized Correlation Filters) tracker to reduce the computational burden of re-detecting objects in each frame. It extracts labels for common dynamic street objects, such as cars, people, cats, and dogs, to generate masks. Masks from multiple objects are then merged into a single mask.

[0046] The above mask is used to perform coarse filtering on the corresponding image feature points in S2 to filter out the feature points on targets such as cars and people. The remaining feature points are static feature point sets. 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 convolution 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. This reduces the number of model parameters, increases the computational complexity of the 3×3 convolutional layer parameters, and reduces the computational complexity of the 1×1 convolutional layer. This makes the computational complexity more evenly distributed and shortens the overall network computation time, making it 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 matched feature points on the two frames, their pixel coordinates are p1 and p2 respectively. Considering that the camera intrinsic parameter K is known, the rigid body 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. s1 and s2 are the scale information of the two frames. The relationship between the two pixels in the two camera coordinate systems can be obtained as:

[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 values ​​will not be strictly 0 due to the presence of noise. RANSAC is used to find the optimal solution that makes the average constraint closest to 0. And the corresponding R and Combined homogeneous camera pose transformation

[0058] Comparing camera visual pose transformations IMU attitude changes like

[0059]

[0060] That is, when the absolute value of the ratio of the camera's visual static point estimated pose transformation to the IMU's pose estimation transformation is greater than the threshold T0, or the number of static feature points is less than the threshold T1, feature point recollection is performed. Usually, T0 = [1.05, 1.2] and T1 = 70 are taken.

[0061] for and The difference of Put 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 Mappable 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 significantly 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 set to 3 pixels. The absolute distance value set in the actual system depends on the internal and external parameters of the camera.

[0066] Tightly coupled optimization of S6, camera vision, and IMU

[0067] The selection and parameter setting of the tightly coupled optimization method depends on the specific application environment. This section is not included in the innovative method of this invention. However, for the sake of system completeness, a brief description of the commonly used classical framework is still provided. For the measurement data within a certain time range, a sliding window of a certain length is maintained. The time range is usually between 0.5 seconds and more than ten seconds, and the sliding window length can be set from a few frames to dozens of frames. The state variables 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 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 frames of 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 an existing technology, which is optimized by establishing a suitable measurement value estimation residual model and selecting a filtering or optimization method such as Kalman filtering and factor graph optimization. The final positioning estimation trajectory can be output in real time, i.e., the pose transformation result within each time unit. The pose graph composed of the two components and the corresponding generated trajectory.

[0072] like Figure 1 As shown in the figure, image-IMU data collected in real time or recorded in a dataset is processed by the ROS system and published as a ROS topic. The VIO system can subscribe to the corresponding topic to receive and process the sensor data. Image feature point detection and tracking, IMU pre-integration, and MobileNets object detection run on separate threads. The image feature point detection and tracking thread receives the results of MobileNets-SSD recognition, filters dynamic feature points, and aligns them with the IMU thread by timestamp. The remaining static feature points are matched to complete a preliminary visual pose estimate. This is then compared with the IMU pose estimate at the corresponding time and the number of static feature points is determined to be less than a threshold. If this condition is met, the static feature point recollection thread is triggered, and feature point matching and visual pose estimation are then repeated. The residuals of the preliminary visual and IMU pose estimation models are incorporated into a 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 ) The performance of the fusion positioning system is verified by actual measurements and public data sets.

[0075] Table 1 shows the test results of the present invention on the VIODE street dynamic scene dataset. The measured value is the root mean square error (RMSE) of the absolute trajectory error (ATE) between the estimated trajectory and the standard trajectory, with the unit being meters. This shows that decimeter-level positioning accuracy can be achieved in highly noisy urban street scenes, 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 figure compares the estimated trajectory and the standard trajectory on a two-dimensional plane during a single run of the system. The dotted line represents the standard trajectory, and the colored line represents the estimated trajectory. The color of the trajectory directly indicates the magnitude of the error (red indicates greater error, blue indicates less error). It can be seen that the absolute trajectory error is within 0.3 meters in most areas.

[0080] Figure 5 is a graph showing the absolute trajectory error of the system trajectory estimation changing with 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 entire process is about 0.27m.

[0081] The foregoing description is merely a specific embodiment of the present invention. It should be noted that those skilled in the art should not assume that the specific implementation of the present invention is limited to these descriptions. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention are intended to be included within the scope of protection of the present invention. Those skilled in the art may make various changes in form and detail, including 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 by: The following steps are involved: S1, camera and IMU data acquisition Continuously acquire image frames and IMU measurements using a camera and inertial navigation system; 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 object detection network, MobileNet-SSD, and run object detection in parallel with MobileNet-SSD. Add a KCF tracker to reduce the computational burden of re-detecting objects in each frame. Take out the street dynamic targets, which include cars, people, cats, and dogs, make a merge mask, filter the feature point pairs obtained by S2, and get the first Static feature point set under the frame and the filtered dynamic feature point set ; S5. Recollection of static feature points 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 recollecting static feature points; match the filtered feature point set, take one set of feature points and map it to another frame through the essential matrix, calculate the distance value to the mapped epipolar line, 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-estimated the visual pose transformation result; Tightly coupled optimization of S6, 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 dynamic scenes in urban streets as claimed in claim 1, characterized in that: In step S3, the pose transformation is estimated based on the IMU measurements during the interval between the two image frames, as follows: According to the IMU error model, the error term of the IMU measurement value includes the acceleration bias value , and the influence of gravity ; Gyroscope error includes bias , both 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: in are all Gaussian white noise, is the true value of the accelerometer, is the true value of the gyroscope, It represents the rotation relationship between the world coordinate system and the IMU coordinate system at the current moment; for two consecutive frames , the time interval is , pre-integrate the IMU to obtain the displacement change within the time interval , speed change IMU attitude changes : in is the rotation matrix between the two frame principal coordinate systems, 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, It is a quaternion operator; it completes the IMU pose transformation estimation and update between two frames.

3. The fusion positioning method for dynamic scenes in urban streets 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, and the weights and bias parameters of the convolution layer are adjusted. Regularization: in is the parameter weight, is the regularization coefficient, is a regularization term used to reduce the parameter space; the loss function is regularized to obtain: is the model output after regularization.

Images