Sensor fusion positioning method and system based on visual factors

Abstract

The invention provides a visual factor-based sensor fusion positioning method, which is applied to an intelligent driving vehicle and comprises the following steps of: 1, extracting ground elements according to a high-precision map ground standard recognition algorithm; 2, environment semantic information is obtained from the image obtained by the visual sensor through a semantic recognition algorithm; step 3, matching the environment semantic information with the ground elements extracted from the high-precision map through a function # imgabs0 # to obtain a semantic matching factor; 4, tracking the features of the environmental semantic information by using an optical flow method to obtain a stable visual key frame and a semantic key frame factor; and step 5, optimizing the semantic matching factor, the semantic key frame factor and the related sensor measurement factor by adopting a nonlinear optimization library to obtain a global pose so as to realize positioning.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving technology, and in particular to a sensor fusion positioning method and system based on visual factors. Background Art

[0002] High-precision location information is the foundation and core of intelligent driving's perception of the environment and decision-making control. Facing the increasingly complex application scenarios of intelligent driving, continuous, reliable, and seamless high-precision location services have become an urgent need for industrial applications. At the same time, the realization of high-precision vehicle positioning relies on various sensor technologies. Common technologies include global satellite navigation system inertial navigation system, visual sensors, lidar, high-precision maps, and wheel speed meters. However, in complex and changing environments, positioning systems using a single or a combination of two sensors have gradually failed to meet the requirements of high reliability and high precision positioning. For example, GNSS (global navigation satellite system) signals are easily affected by signal attenuation, reflection or blockage. INS (inertial navigation positioning) accuracy is affected by error accumulation, and sensors such as vision and lidar also have defects such as being affected by repeated structures and textures. Multi-source fusion technology that makes full use of the complementary characteristics of different sensors has important practical significance and application value for improving the accuracy and continuity of vehicle navigation positioning in complex environments, and is regarded as an effective solution to solve the limitations and vulnerabilities of positioning systems. A complete multi-source fusion positioning solution requires the establishment of an overall data processing process, including multi-source sensor calibration, data preprocessing, fusion algorithm and quality control, to ensure the reliability of high-precision positioning services. Among them, the multi-source fusion algorithm directly affects the quality of the positioning results. The current mainstream multi-source fusion algorithms are represented by Kalman filter (KF) and factor graph optimization (FGO). In intelligent driving applications, making reasonable choices and improvements on the two solutions based on computing power support, accuracy requirements and the types of sensors can achieve robust and ubiquitous high-precision positioning. In existing unmanned vehicle positioning solutions, traditional fusion solutions are usually used. In sensor fusion, since the estimation of position, speed, attitude, zero bias and feature detection do not meet linear conditions, the most commonly used algorithm in multi-source fusion filtering is the nonlinear extended Kalman filter (EKF). The traditional EKF model is a state space model and a measurement model, and its filtering process consists of two parts: time update and measurement update. However, in the complex and changeable real environment, GNSS is often subject to false fixation due to the influence of urban multipath, ionosphere, etc., that is, GNSS positioning in the fixed solution state will drift slowly or jump. Laser matching positioning will have large matching errors due to the dynamic and changeable environment. The traditional EKF method cannot effectively isolate erroneous positioning information, and EKF is a single iteration and cannot fully utilize historical information for effective constraints, which leads to offset in fusion positioning, affecting positioning accuracy and reliability, and causing the unmanned vehicle to deviate from the route and take over, greatly affecting operational efficiency. Summary of the invention

[0003] In view of this, the present invention provides a sensor fusion positioning method and system based on visual factors to solve the technical problem that the fusion positioning offset phenomenon occurs in the prior art and affects the positioning accuracy and reliability.

[0004] The present invention provides a sensor fusion positioning method based on visual factors, which is applied to intelligent driving vehicles. The method comprises: step 1, extracting ground elements according to a high-precision map ground standard recognition algorithm; step 2, obtaining environmental semantic information from an image acquired by a visual sensor through a semantic recognition algorithm; step 3, combining the environmental semantic information with the ground elements extracted from the high-precision map through Perform feature matching to obtain the semantic matching factor, where xi and pi are point cloud coordinates, NX and NP represent the number of point clouds, R is the rotation matrix, and t is the translation vector; Step 4, use the optical flow method to track the features of the environmental semantic information to obtain a stable visual key frame and a semantic key frame factor; Step 5, use a nonlinear optimization library to optimize the semantic matching factor, the semantic key frame factor, and the related sensor measurement factor to obtain the global pose, thereby achieving positioning.

[0005] Furthermore, the method for tracking the features of environmental semantic information using the optical flow method includes: step 41, initializing the node, checking and reading the parameters; step 42, subscribing to the node according to the callback function to obtain image data; step 43, adding reverse optical flow tracking between the previous and next frame images, and performing reverse tracking through the tracking points of the previous frame.

[0006] Furthermore, the high-precision map ground standard recognition algorithm includes: step 11, selecting an area from the image acquired by the vehicle vision sensor, and selecting four points from the area; step 12, obtaining the coordinates of the high-precision map corresponding to the four points, and calculating the transformation matrix; step 13, remapping the image to the high-precision map according to the transformation matrix.

[0007] Furthermore, the method for optimization using a nonlinear optimization library includes: step 51, constructing a calculation model of the cost function; step 52, constructing an optimization problem to be solved; step 53, inputting the defined cost function structure into the constructed problem, completing problem solving, and achieving optimization.

[0008] Furthermore, the relevant sensor measurement factors include inertial measurement pre-integration factors, integrated navigation posture factors, speed factors, and kinematic model factors.

[0009] Furthermore, the step 5 also includes: fusing the optimized measurement factors through a sliding window to estimate the real-time position and posture of the current vehicle.

[0010] The present invention also provides a system for a sensor fusion positioning method based on visual factors, the system comprising: a sensor component, including a visual sensor, for acquiring sensor data for subsequent use; a calculation matching module, connected to the sensor component, for extracting ground elements according to a high-precision map ground standard recognition algorithm, obtaining environmental semantic information from an image acquired by the visual sensor through a semantic recognition algorithm, and combining the environmental semantic information with the ground elements extracted from the high-precision map. Perform feature matching to obtain the semantic matching factor, where xi and pi are point cloud coordinates, NX and NP represent the number of point clouds, R is the rotation matrix, and t is the translation vector. The optical flow method is used to track the features of the environmental semantic information to obtain a stable visual key frame and a semantic key frame factor. The optimization positioning module is connected to the calculation matching module and is used to use a nonlinear optimization library to optimize the semantic matching factor, the semantic key frame factor, and the related sensor measurement factor to obtain the global pose, thereby achieving positioning.

[0011] Furthermore, the optimization positioning module is also used to fuse the optimized measurement factors through a sliding window to estimate the real-time position and posture of the current vehicle.

[0012] Furthermore, the relevant sensor measurement factors include inertial measurement pre-integration factors, integrated navigation posture factors, speed factors, and kinematic model factors.

[0013] The present invention provides a sensor fusion positioning method and system based on visual factors. The technical solution converts IMU, wheel speed, integrated navigation, laser matching, visual matching information, and visual feature points into relevant measurement factors by optimizing the positioning module, and fuses them through a sliding window, thereby estimating the current real-time posture of the vehicle, obtaining the global posture, and realizing positioning. The present invention can solve the problems of large matching errors, offset phenomenon in fusion positioning, and low operating efficiency in the prior art. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 It is a flow chart of a sensor fusion positioning method based on visual factors provided by the present invention; Figure 2 This is a framework diagram of a sensor fusion positioning system based on visual factors provided by the present invention; Figure 3 It is a schematic diagram of the process flow of the high-precision map ground standard recognition algorithm provided by the present invention; Figure 4 It is a flow chart of a method for tracking features of environmental semantic information using the optical flow method provided by the present invention; Figure 5 It is a schematic flow chart of a method for optimization using a nonlinear optimization library provided by the present invention; Figure 6 It is a comparison diagram of the factor fusion effect provided by the present invention; Figure 7 It is a schematic diagram of the combined navigation fixed isolation abnormal drift data provided by the present invention; Figure 8 It is a schematic diagram of the isolation error effect provided by the present invention. DETAILED DESCRIPTION

[0015] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0016] Method Item Example: The present invention provides a sensor fusion positioning method based on visual factors, which is applied to intelligent driving vehicles, such as Figure 1 As shown, the method includes: Step 1: Extract ground features according to the high-precision map ground standard recognition algorithm; like Figure 3 As shown, the high-precision map ground standard recognition algorithm includes: Step 11, selecting an area from the image acquired by the vehicle vision sensor, and selecting four points from the area; Step 12, obtain the coordinates of the four points in the high-precision map and calculate the transformation matrix; Step 13: Remap the image to a high-precision map based on the transformation matrix.

[0017] This step uses the BEV (Bird's-Eye-View, a sensor data representation method for a bird's-eye view) high-precision map ground annotation recognition method, that is, the high-precision map ground identification algorithm, to extract ground elements, including lane lines, ground markings and other information.

[0018] Step 2, obtaining environmental semantic information from the image acquired by the visual sensor through a semantic recognition algorithm; Environmental semantic information is obtained through semantic recognition methods such as BEV, including lane lines, arrows, stop lines, road boundaries, etc.

[0019] Step 3: Combine the environmental semantic information with the ground features extracted from the high-precision map. Perform feature matching to obtain the semantic matching factor, where x i and p i is the point cloud coordinate, N X and N PRepresents the number of point clouds, R is the rotation matrix, and t is the translation vector; The matching is performed when When the value of is the smallest, the match is successful.

[0020] Step 4, using the optical flow method to track the features of the environment semantic information, obtain a stable visual key frame, and obtain a semantic key frame factor; like Figure 4 As shown, the method for tracking the features of environmental semantic information using the optical flow method includes: Step 41, initialize the node, check and read parameters; Step 42, subscribing to the node according to the callback function to obtain image data; Step 43, adding reverse optical flow tracking between the previous and next frame images, and performing reverse tracking through the tracking points of the previous frame.

[0021] Optical flow refers to a way of expressing image motion, which is usually defined as the apparent motion of image brightness patterns in an image sequence, that is, the expression of the motion speed of points on the surface of spatial objects on the imaging plane of the visual sensor.

[0022] Step 5: Use the nonlinear optimization library to optimize the semantic matching factor, semantic key frame factor, and related sensor measurement factors, fuse the optimized measurement factors through a sliding window, estimate the real-time posture of the current vehicle, and obtain the global posture, thereby achieving positioning.

[0023] like Figure 5 As shown, the method for optimizing using a nonlinear optimization library includes: Step 51, constructing a calculation model of the cost function; Step 52, constructing an optimization problem to be solved; Step 53, input the defined cost function structure into the constructed problem, complete the problem solving, and achieve optimization.

[0024] This step usually uses the Ceres nonlinear optimization library to optimize all factors and obtain the global pose. Ceres is a widely used least squares problem solving library. In Ceres, you only need to define the optimization problem to be solved according to certain steps and then hand it over to the solver for calculation.

[0025] The present invention provides a sensor fusion positioning method and system based on visual factors. The technical solution converts IMU, wheel speed, integrated navigation, laser matching, visual matching information, and visual feature points into relevant measurement factors by optimizing the positioning module, and fuses them through a sliding window, thereby estimating the current real-time posture of the vehicle, obtaining the global posture, and realizing positioning. The present invention can solve the problems of large matching errors, offset phenomenon in fusion positioning, and low operating efficiency in the prior art.

[0026] Device Item Example: The present invention provides a system for a sensor fusion positioning method based on visual factors, such as Figure 2 As shown, the system includes a sensor component, a calculation matching module and an optimization positioning module.

[0027] Sensor components, including visual sensors, for acquiring sensor data for subsequent use; The sensor assembly includes IMU (inertial measurement unit), wheel speed, integrated navigation and other sensors, which provide sensor data for subsequent calculation of measurement factors.

[0028] The calculation matching module is connected to the sensor component and is used to extract ground features according to the ground standard recognition algorithm of the high-precision map, obtain environmental semantic information from the image obtained by the visual sensor through the semantic recognition algorithm, and compare the environmental semantic information with the ground features extracted from the high-precision map. Perform feature matching to obtain the semantic matching factor, where xi and pi are point cloud coordinates, N X and N P represents the number of point clouds, R is the rotation matrix, t is the translation vector, and the optical flow method is used to track the features of the semantic information of the environment to obtain stable visual key frames and semantic key frame factors; The optimization positioning module is connected to the calculation matching module and is used to optimize the semantic matching factor, the semantic key frame factor, and the related sensor measurement factor using a nonlinear optimization library to obtain the global pose, thereby achieving positioning. The optimization positioning module is also used to fuse the optimized measurement factors through a sliding window to estimate the real-time pose of the current vehicle, and to calculate the effect of the fusion of the measurement factors. Figure 6-Figure 8 The related sensor measurement factors include inertial measurement pre-integration factor, integrated navigation attitude factor, speed factor, kinematic model factor. Since these measurement factors are not required technical features of the present invention, they are not described in detail.

[0029] The present invention provides a sensor fusion positioning method and system based on visual factors. The technical solution converts IMU, wheel speed, integrated navigation, laser matching, visual matching information, and visual feature points into relevant measurement factors by optimizing the positioning module, and fuses them through a sliding window, thereby estimating the current real-time posture of the vehicle, obtaining the global posture, and realizing positioning. The present invention can solve the problems of large matching errors, offset phenomenon in fusion positioning, and low operating efficiency in the prior art.

[0030] In summary, the embodiment of the present invention provides a sensor fusion positioning method and system based on visual factors. By optimizing the positioning module, IMU (inertial measurement unit), wheel speed, integrated navigation, laser matching, visual matching information, and visual feature points are converted into relevant measurement factors, which are fused through a sliding window to estimate the current real-time position of the vehicle, thereby obtaining the global position and completing the positioning. This technical solution can achieve stable operation of unmanned vehicles, and is applied to ports and open road vehicles, generating greater economic and social value.

[0031] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A sensor fusion positioning method based on visual factors, applied to intelligent driving vehicles, characterized in that: The method comprises: Step 1: Extract ground features according to the high-precision map ground standard recognition algorithm; Step 2, obtaining environmental semantic information from the image acquired by the visual sensor through a semantic recognition algorithm; Step 3: Combine the environmental semantic information with the ground features extracted from the high-precision map. Perform feature matching to obtain the semantic matching factor, where x i and p i is the point cloud coordinate, N X and N P Represents the number of point clouds, R is the rotation matrix, and t is the translation vector; Step 4, using the optical flow method to track the features of the environment semantic information, obtain a stable visual key frame, and obtain a semantic key frame factor; Step 5: Use the nonlinear optimization library to optimize the semantic matching factor, semantic key frame factor, and related sensor measurement factors to obtain the global pose, thereby achieving positioning.

2. According to the sensor fusion positioning method based on visual factors as described in claim 1, it is characterized in that: The method for tracking the features of environmental semantic information using the optical flow method comprises: Step 41, initialize the node, check and read parameters; Step 42, subscribing to the node according to the callback function to obtain image data; Step 43, adding reverse optical flow tracking between the previous and next frame images, and performing reverse tracking through the tracking points of the previous frame.

3. According to the sensor fusion positioning method based on visual factors as described in claim 1, it is characterized in that: The high-precision map ground standard recognition algorithm includes: Step 11, selecting an area from the image acquired by the vehicle vision sensor, and selecting four points from the area; Step 12, obtain the coordinates of the four points in the high-precision map and calculate the transformation matrix; Step 13: Remap the image to a high-precision map based on the transformation matrix.

4. According to the sensor fusion positioning method based on visual factors as described in claim 1, it is characterized in that: The method for optimizing using a nonlinear optimization library comprises: Step 51, constructing a calculation model of the cost function; Step 52, constructing an optimization problem to be solved; Step 53, input the defined cost function structure into the constructed problem, complete the problem solving, and achieve optimization.

5. According to the sensor fusion positioning method based on visual factors as described in claim 1, it is characterized in that: The related sensor measurement factors include inertial measurement pre-integration factors, integrated navigation attitude factors, speed factors, and kinematic model factors.

6. The sensor fusion positioning method based on visual factors according to claim 1 is characterized in that: The step 5 also includes: fusing the optimized measurement factors through a sliding window to estimate the real-time position and posture of the current vehicle.

7. A system for implementing the sensor fusion positioning method based on visual factors according to claims 1-6, characterized in that: The system comprises: Sensor components, including visual sensors, for acquiring sensor data for subsequent use; The calculation matching module is connected to the sensor component and is used to extract ground features according to the ground standard recognition algorithm of the high-precision map, obtain environmental semantic information from the image obtained by the visual sensor through the semantic recognition algorithm, and compare the environmental semantic information with the ground features extracted from the high-precision map. Perform feature matching to obtain the semantic matching factor, where x i and p i is the point cloud coordinate, N X and N P represents the number of point clouds, R is the rotation matrix, t is the translation vector, and the optical flow method is used to track the features of the semantic information of the environment to obtain stable visual key frames and semantic key frame factors; The optimization positioning module is connected to the calculation matching module and is used to optimize the semantic matching factor, the semantic key frame factor, and the related sensor measurement factor using the nonlinear optimization library to obtain the global pose, thereby achieving positioning.

8. The sensor fusion positioning system based on visual factors according to claim 7, characterized in that: The optimization positioning module is also used to fuse the optimized measurement factors through a sliding window to estimate the real-time position and posture of the current vehicle.

9. The sensor fusion positioning system based on visual factors according to claim 7, characterized in that: The related sensor measurement factors include inertial measurement pre-integration factors, integrated navigation attitude factors, speed factors, and kinematic model factors.

Images