3D visual guidance positioning method based on multi-sensor fusion

Abstract

The invention relates to the technical field of multi-sensor information fusion positioning, and discloses a multi-sensor fusion-based 3D visual guidance positioning method, which comprises the following steps of S1, acquiring data of an inertial measurement unit, a visual sensor and a laser radar, and performing time-space synchronization and preprocessing; s2, 3D / 2D feature points are extracted and matched, cross-modal feature matching is carried out, a local map is constructed, and the quality of the feature points and the motion state of a carrier are evaluated in real time. According to the method, the 3D / 2D feature points are extracted and matched, cross-modal feature matching is performed, the local map is constructed, and the local map is maintained by adopting a sliding window method, so that the map is updated in real time, and the calculation efficiency is kept. According to the efficient data fusion strategy, the real-time requirement can be met while the positioning precision is guaranteed, and the method is suitable for application scenes with high response speed requirements, such as real-time obstacle avoidance and path planning in automatic driving.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of multi-sensor information fusion positioning, in particular to a positioning method combining 3D vision, inertial navigation and vehicle dynamics model, which is suitable for automatic driving, robot navigation and augmented reality scenes. BACKGROUND

[0002] In the fields of robot technology, automatic driving, augmented reality (AR) and industrial automation, precise 3D vision guided positioning technology is the core foundation of realizing environment perception, path planning and autonomous decision-making. With the increasing complexity of application scenarios, a single sensor has been difficult to meet the positioning needs of high precision and strong robustness, and multi-sensor fusion technology has therefore become a research hotspot. However, the existing technology still faces significant challenges in data fusion efficiency, environmental adaptability and real-time performance, and needs to be innovated and broken through.

[0003] In the prior art, cameras, lidar (LiDAR) and inertial measurement units (IMU) have their own limitations. Cameras can capture two-dimensional image information and achieve high-precision object recognition and classification by combining deep learning algorithms. However, cameras are sensitive to light conditions and prone to positioning deviation in backlight, low light or texture missing scenarios. In addition, they lack direct three-dimensional spatial information and need to indirectly derive depth through multi-view geometry or structure from motion (SfM) techniques, which has high computational complexity and limited real-time performance. Lidar can directly obtain high-precision three-dimensional point cloud data by emitting laser pulses to measure distance, and is not sensitive to light changes. However, lidar is expensive and its performance decreases in bad weather such as rain, snow and fog. In addition, its data sparsity results in insufficient recognition ability for small objects or edge features. Inertial measurement units can achieve short-time high-precision motion estimation by measuring angular velocity and acceleration, and have high-frequency output characteristics. However, inertial measurement units have cumulative error problems and will cause positioning drift if used alone for a long time, so they need to be fused with other sensors to correct errors.

[0004] Multi-sensor fusion technology is faced with challenges such as data fusion level and algorithm complexity, time and space synchronization difficulties, insufficient environmental adaptability, and real-time and computing resource conflicts. Data fusion is usually divided into data layer, feature layer and decision layer fusion. Although existing algorithms such as Kalman filter, particle filter and deep learning model have been applied to multi-sensor data association and state estimation, the heterogeneity of different sensor data in time and space synchronization, dimensional difference and noise characteristics leads to high complexity of fusion algorithm and difficulty in universalization. The differences in sensor installation position, sampling frequency and data transmission delay easily lead to time and space inconsistency. Although traditional methods such as hardware trigger synchronization or software interpolation can partially alleviate the problem, there is still a loss of accuracy in dynamic scenes. Existing technologies are mostly optimized for specific scenarios, lack of cross-scene generalization ability, and high-precision multi-sensor fusion requires processing of massive data, which requires high computing resources. Traditional methods can improve efficiency, but they need to rely on high-performance hardware, increasing system cost and energy consumption.

[0005] To solve the above problems, the present application provides a 3D vision guided positioning method based on multi-sensor fusion, which aims to improve positioning accuracy, enhance environmental adaptability, and improve robustness through innovative data fusion strategies and adaptive mechanisms, and provide reliable technical support for accurate positioning in complex scenarios. SUMMARY

[0006] The present application aims to provide a 3D vision guided positioning method based on multi-sensor fusion to solve the problems raised in the background art.

[0007] To achieve the above-mentioned purpose, the present application provides the following technical scheme: a 3D vision guided positioning method based on multi-sensor fusion, comprising the following steps: Step S1: collecting inertial measurement unit , vision sensor and laser radar data, performing time and space synchronization and preprocessing; Step S2: extracting 3D / 2D feature points, performing cross-modal feature matching, constructing a local map, and evaluating feature point quality and carrier motion state in real time; Step S3: fusing vision re-projection error, pre-integration error and car dynamics model constraints through tight coupling visual odometry to output initial pose estimation; Step S4: dynamically adjusting the positioning mode according to the complexity of the environment, enabling 3D / 2D joint positioning in harsh environments, and using only 3D feature points in normal environments; Step S5: correcting the pose through sliding window optimization and global loop detection to eliminate cumulative errors.

[0008] Preferably, the multi-source sensor data collection and time and space synchronization preprocessing according to step S1 specifically comprises the following steps: Step S1.1: Synchronous Acquisition The raw data streams from visual sensors (cameras) and LiDAR are used to establish a unified time reference. Time stamp correction and alignment of visual sensor and LiDAR data: Fine-grained time alignment is performed using the least squares method to calculate the timestamp deviation of each sensor data packet. : ,in, and Representing the IMU and LiDAR at time 10:00 and 20:00 respectively eigenfunction values, Let be the time offset to be solved. This represents the total number of sampling points; The specific steps for calculating the timestamp deviation of each sensor data packet are as follows: S1.1.1 For each sampling time ( (e.g., 1, 2, ..., N), calculate Predicted eigenvalues and the characteristic values ​​actually measured by lidar ; S1.1.2 Then calculate the difference between the two and square it; S1.1.3 Sum the squared errors of all sampling points to form a total error function; The ultimate goal of S1.1.4 is to find a value. This minimizes the total error function. Step S1.2: Unify the data from multiple sensors to the same spatial coordinate system, such as the vehicle coordinate system, and perform spatial coordinate system calibration: The intrinsic parameter matrix of the vision sensor was obtained using the checkerboard calibration method. and external references (rotation matrix) Translation vector ), and determining the lidar using the hand-eye calibration method ( ) and inertial measurement unit ( The spatial transformation relationship between () involves the following formulas: Rotation matrix relationship: Translation vector relationship: , where subscript Indicates the coordinate system in which the calibration board (i.e., the "hand" in hand-eye calibration) is located; superscript and Representing lidar and Their respective coordinate systems; Calculated using the hand-eye sign method Compared to rotation matrix Translation vector Specific steps: S1.2.1 respectively in and Obtain multiple sets of position data for the calibration plate in a coordinate system; S1.2.2 Calibration data is calculated from the calibration plate to... Rotation matrix of coordinate system Translation vector ; S1.2.3 similarly calculates the distance from the calibration plate to... Rotation matrix of coordinate system Translation vector ; S1.2.4 Substituting the above intermediate results into the rotation matrix / translation vector relationship formula, the result can be obtained. Compared to rotation matrix Translation vector ; Step S1.3: In In data processing, a Kalman filter is used to remove high-frequency noise from the data: Equations of state: ,in, It is a state vector, which typically contains The measured angular velocity and acceleration It is the state transition matrix, which describes how the system state changes over time. This is process noise, representing all other influencing factors not considered in the model; Observation equation: ,in, It is the observation vector, i.e. The actual measured value, It is the observation matrix, which maps the state vector to the observation space. This is observation noise, representing random errors in the measurement process; Velocity and displacement are calculated from accelerometer and gyroscope measurements using kinematic integrals: ,in, It is the speed at the current moment. The speeds at the previous moment are respectively The previous moment and The accelerometer output at the current moment, It is the sampling interval; ,in, is the displacement at the current time, is the displacement at the previous time; Step S1.4: In the visual sensor data processing, in order to correct the geometric distortion caused by the lens, a distortion correction method is adopted, and these distortions mainly include radial distortion and tangential distortion: wherein, is the original pixel coordinate, and is the radial distortion coefficient, is the distance from the pixel to the center point of the image, and the calculation formula is ; wherein, and are the tangential distortion coefficients; In order to improve the visual effect of the image, so that the details in the image are clearer and the contrast is higher, histogram equalization is adopted to improve the contrast: , wherein, is the equalized gray level, is the number of pixels of the gray level in the original image, is the total number of pixels of the image, is the total number of gray levels; Step S1.5: In the laser radar data preprocessing, point cloud filtering and coordinate transformation are included, wherein the point cloud filtering includes statistical filtering to remove outliers and voxel grid filtering to reduce data volume, The specific method of the statistical filtering to remove outliers is as follows: By calculating the mean ( ) and standard deviation ( ) in the neighborhood of each point, it is judged whether the point is an outlier, and for each point, if the absolute value of the difference between the point and the neighborhood mean is greater than 3 times the standard deviation ( >3 ), it is considered that the point is an outlier and is removed; The specific method of the voxel grid filtering to reduce data volume is as follows: The three-dimensional space is divided into a series of voxels with a side length of , and the original points are replaced by the voxel centroid: wherein, is the coordinate of the voxel centroid, is the number of points in the voxel, is the coordinates of a point; The coordinate transformation realizes the conversion of the point cloud coordinate system through rotation and translation operations wherein, is the point coordinate converted to the vehicle body coordinate system, is the rotation matrix from the laser radar coordinate system to the vehicle body coordinate system, is the point coordinate in the original laser radar coordinate system, is the translation vector from the laser radar coordinate system to the vehicle body coordinate system; Step S1.6: data fusion preparation S1.6.1 Time interpolation is performed on the data of the radar (usually 100-1000 Hz), the vision (10-30 Hz), and the laser radar (10-20 Hz) to generate a unified time sequence. S1.6.2 Extraction of angular velocity / acceleration features of the radar, ORB feature points of the vision, and plane / edge features of the laser radar provides structured data for subsequent fusion. S1.6.2 Extraction of angular velocity / acceleration features of the radar, ORB feature points of the vision, and plane / edge features of the laser radar provides structured data for subsequent fusion. S1.6.2 Extraction of angular velocity / acceleration features of the radar, ORB feature points of the vision, and plane / edge features of the laser radar provides structured data for subsequent fusion.

[0009] Preferably, the extraction of the 3D / 2D feature points according to step S2 includes laser radar feature extraction and vision feature extraction, and the laser radar feature extraction includes: Plane feature extraction: principal component analysis (PCA) is used to detect local planes, and the eigenvalues of the covariance matrix are calculated: If / < and / < , it is determined as a plane feature. Edge feature extraction: point cloud curvature screening is performed, and the curvature calculation formula is: wherein, is the neighborhood point set, is the neighborhood centroid; The vision feature extraction includes ORB feature detection and BRIEF descriptor. The ORB feature detection is performed in the following manner: For each pixel in the image , a circle (usually with a radius of 3 pixels, and there are 16 pixel points on the circumference) is defined around it, the gray value difference between these pixel points and the center pixel is calculated, and the sum is taken to obtain the response value : wherein, represents the gray value of the pixel , and represents the gray value of the center pixel the gray value of the pixel, by comparing the response value with a preset threshold value, to determine whether the pixel is a corner point; non-maximum suppression is used to retain local maximum points and remove adjacent corner points, to ensure that the detected corner points are significant; wherein the BRIEF descriptor is generated as follows: randomly selecting for each pixel, a binary descriptor is generated: The positions of the pixel pairs are predefined, but in actual applications, random sampling can be used to increase the diversity of the descriptor. For each pair of pixels , the gray values thereof are compared, so that each feature point generates a binary vector of length n, i.e., its BRIEF descriptor.

[0010] Preferably, the cross-modal feature matching process according to step S2 comprises laser radar-vision matching and vision-vision matching. The specific steps of the laser radar-vision matching are as follows: S2.1.1 feature extraction: 3D plane or edge features are extracted from the laser radar data; corresponding 2D features are extracted from the vision image; S2.1.2 projection: 3D features are projected onto the image plane, and the projection formula is as follows: wherein, is a projection function, and is an extrinsic parameter matrix (rotation matrix and translation vector), is a 3D point, is an image coordinate; S2.1.3 error calculation: the projection error , i.e., the difference between the projected point and the actual image point, is calculated; S2.1.4 error elimination: RANSAC (random sample consensus) algorithm is used to eliminate the error matching, and the inlier threshold is set to 2 pixels; The specific steps of the vision-vision matching are as follows: S2.2.1 feature extraction: BRIEF (Binary Robust Independent Elementary Features) descriptors are extracted from two vision images; S2.2.2 matching: a FLANN (Fast Library for Approximate Nearest Neighbors) matcher is used for BRIEF descriptor matching; S2.2.3 distance calculation: the Hamming distance is calculated: wherein, are two BRIEF descriptors, denotes a bitwise XOR operation, is the length of the descriptor; S2.2.4 Inlier filtering: keep inliers with Hamming distance less than 50.

[0011] Preferably, according to step S2, in the process of local map construction, the following steps are specifically adopted: S2.3.1 Pose optimization: construct an error function by minimizing re-projection error: where, is the observation data, i.e., the pixel coordinates of the th map point observed in the th frame, is the projection function used to project the map point from the world coordinate system to the image coordinate system of the th frame, is the pose (including rotation and translation) of the th frame, is the position of the map point in the world coordinate system, is the Huber kernel function used to reduce the impact of outliers on optimization; S2.3.2 Optimization algorithm: use the Levenberg-Marquardt (LM) algorithm to solve the above nonlinear optimization problem; S2.3.3 Sliding window method: adopt a sliding window strategy to maintain the local map, and the window size is fixed, for example, keep the feature points and related information of the last N frames (such as N = 10); the advantage of this is that the map can be updated in real time and the calculation efficiency is maintained; S2.3.4 Prior information preservation when marginalizing old frames: when a new frame is added to the window and an old frame is removed, the data of the old frame needs to be properly handled to avoid information loss, and the formula is used, is the updated covariance matrix, is the Jacobian matrix of the new frame relative to the current window, is the information matrix of the new frame, is the prior covariance matrix of the old frame.

[0012] Preferably, according to step S2, the feature point quality evaluation includes visual feature quality and lidar feature quality; The specific steps of the visual feature quality evaluation are as follows: S2.4.1 Calculate the feature point response value Feature point response values ​​are usually calculated using a feature detection algorithm (such as SIFT, SURF, ORB, etc.). These values ​​reflect the salience of the feature points in the image. Removing feature points with response values ​​less than 10 can eliminate noise points that are unlikely to be true feature points.

[0013] S2.4.2 Evaluate scale invariance. Scale invariance refers to the stability of feature points at different scales. Calculate the maximum scale. With minimum scale If the ratio is greater than 3, it indicates that the feature point has good stability at different scales and should be retained. S2.4.3 Calculate the change in principal direction angle The principal direction is an important attribute of feature points, used to describe the direction of the image gradient around the feature point. The angle change of the principal direction between adjacent frames is calculated. Feature points with an angle change greater than 30° are removed because these points may have undergone significant shifts or rotations during the tracking process and are no longer stable. The specific steps for evaluating the feature quality of the lidar are as follows: S2.5.1 Consistency of Planar Feature Normal Vectors: In lidar data processing, planar features are often used to represent flat surfaces. The angle between the normal vectors of planar features between adjacent frames is calculated. ,if Greater than 0.95 (i.e.) If the angle is less than approximately 18.2°, then the two normal vectors are considered to be highly consistent, and the planar feature has good stability. S2.5.2 Collinearity of Edge Features: Edge features are typically used to represent the contours or boundaries of objects. The variance of the distances between edge feature points is calculated. Variance removal Discrete points greater than 0.01 are excluded because these points may not be on the same straight line or may have a large measurement error.

[0014] Preferably, the assessment of the carrier's motion state according to step S2 includes the following two methods: S2.6.1 Calculates motion increments (including rotational increments) through pre-integration. Speed ​​increment Location increment To predict the motion state of the carrier: Rotational Increment : ,in, It is the first angular velocity at time t, It is the zero bias of the gyroscope. It is the sampling interval; Speed ​​increment : ,in, It is the first acceleration at any moment It is the zero bias of the accelerometer. It is the rotation matrix from the previous moment; Position increment : ,in, It is the rotation matrix from the previous moment; S2.6.2 The motion state of the carrier is determined by calculating the translational velocity and rotational angular velocity: Formula for calculating translational velocity: Formula for calculating rotational angular velocity: Rules for determining motion state: Stationary: When the translational velocity is less than 0.1 m / s and the rotational angular velocity is less than 5° / s, it is determined to be in a stationary state; Low speed: When the translation speed is between 0.1 and 1 m / s or the rotational angular velocity is between 5° and 15° / s, it is judged as a low speed state; High speed: When the translational speed is greater than or equal to 1m / s or the rotational angular velocity is greater than or equal to 15° / s, it is judged as a high speed state.

[0015] Preferably, according to step S3, visual reprojection error needs to be fused. By applying pre-integration error and constraints from the vehicle dynamics model, high-precision initial pose estimation is achieved, which is specifically divided into the following steps: S3.1 optimizes camera pose and 3D spatial point coordinates through feature point matching constraints to model visual reprojection error, for each pair of consecutive frames. Feature points matched in Its reprojection error is defined as: It describes the actual observed pixel coordinates. The difference between the predicted pixel coordinates and those calculated using camera pose and 3D point coordinates, where, This represents the pose transformation matrix from the world coordinate system to the camera. For camera to The calibration extrinsic parameter matrix of the inertial measurement unit (IMU). For feature points Coordinates in normalized space This is a projection function that projects points in three-dimensional space onto the image plane. For feature points In frame The actual observed pixel coordinates; through algorithm optimization The adjusted variable is used to minimize the reprojection error by using a cost function , where is a Huber robust kernel function to reduce the influence of outliers on the optimization result, is the covariance matrix of pixel coordinates to represent the statistical properties of observation noise, represents the summation of all matched feature points . S3.2. Measure the motion increment between adjacent frames by using measurement constraints Pre-integration error constraints, which calculate the pre-integration of measurements for a given time interval : where , are the gyroscope and accelerometer biases; get the pre-integration error term: Constrain the motion estimation of adjacent frames by using a cost function to minimize the pre-integration error, where is a Huber robust kernel function to nonlinearly transform the error, is the pre-integration covariance matrix, represents the summation of all , elements; S3.3. Vehicle dynamics model constraints to optimize and constrain the vehicle's pose estimation by introducing vehicle kinematics / dynamics priors to improve pose plausibility; use a bicycle model to constrain vehicle motion: Non-holonomic constraints: where is the coordinate of the vehicle's rear axle center, is the vehicle's heading angle, , are the vehicle's velocity components in and directions, respectively; Steering geometry constraints: where is the front wheel steering angle, is the vehicle's wheelbase, is the vehicle's turning radius, is the vehicle's speed, is the rate of change of the heading angle;​ transforming the dynamic model into residual terms constructing a cost function using the residual terms optimizing the vehicle's pose estimation by minimizing the cost function, wherein, is a Huber robust kernel function, is a dynamic model covariance; by introducing the non-holonomic constraints of the bicycle model and steering geometry constraints and formulating these constraints as residual terms, the rationality of the vehicle pose estimation can be effectively improved; S3.4 constructing a joint cost function for fusing multi-source constraints to solve the optimal state quantity solving using a nonlinear optimization algorithm (such as Dog-Leg or Schur complement marginalization) and maintaining a fixed number of historical frames through a sliding window method, wherein, is a state vector of the system, 、 、 are error terms of the projective sensor, and the vehicle-mounted sensor, respectively, 、 、 are error covariance matrices corresponding to the sensors; S3.5 outputting the optimized state quantity: wherein, represents a transformation matrix from the world coordinate system to the camera coordinate system, represents a velocity vector of the camera in the world coordinate system, is a bias of the gravitational acceleration, is a bias of the gyroscope, refers to the covariance matrix of the position estimation at the time, is a vehicle control quantity.

[0016] Preferably, in step S4, an environment complexity evaluation system needs to be constructed first to quantify the dynamicity and geometric degeneration degree of the environment, providing criteria for mode switching, and the evaluation method is as follows: S4.1 calculating the spatial distribution uniformity of feature points in the image plane: wherein, is the number of image blocks, is the probability distribution of the number of feature points in the th grid, is the total number of feature points, is the number of feature points in the th grid, the smaller the value, the more concentrated the distribution is; when all feature points are concentrated in one grid, H=0; when the feature points are uniformly distributed in all grids, H= ; S4.2 By measure the carrier motion intensity: where, is the acceleration module (after bias deduction), is the angular velocity module, The greater the value, the more intense the motion; S4.3 Use the geometric degradation factor to detect the scene geometry singularity: , Complexity comprehensive index: where, , , is the weight coefficient; According to the environmental complexity in step S4, the positioning mode is dynamically switched, and the trigger conditions are as follows: Joint positioning mode (adverse environment): activated when or ; Pure 3D positioning mode (regular environment): activated when or ; In order to enable the system to adapt to the changing environmental conditions, the threshold will be dynamically updated, and the historical complexity distribution is calculated by using the sliding window to dynamically update the threshold: where, is the threshold value at the current time, is the update coefficient, which is used to control the weight distribution between new data and old data, is the average complexity of the latest frame .

[0017] Preferably, according to step S5, the sliding window optimization specifically includes window management strategy, marginalization prior construction, and joint optimization model, and the specific steps are as follows: S5.1.1 Window management strategy: Based on the disparity threshold and the time interval to filter key frames: or , according to the scene complexity to adaptively adjust the window size : where, is the reference window size, is the complexity sensitivity coefficient; S5.1.2 Marginalization prior construction: First, divide the variables within the window into variables to be optimized. variables to be marginalized Then, prior information is constructed through matrix partitioning: This will be used as a fixed constraint in subsequent optimizations: ; S5.1.3 Joint Optimization Model: By optimizing variables: Construct the cost function ; Step S5, global loop closure detection and constraint fusion, is specifically divided into the following steps: S5.2.1 Bag-of-Words Model Matching: Construct a visual vocabulary tree based on DBoW2 and calculate inter-frame similarity: ,in, For BoW vectors, The similarity threshold; Calculate the current frame With historical frames Similarity between If similarity If the value exceeds a threshold (typically 0.15), it is considered a loop closure candidate; the geometric correspondence is verified using the fundamental matrix F, and if it satisfies... This further confirms the candidate loopback; S5.2.2 Loop Containment Construction: Calculate the loop inter-frame transform using PnP or ICP: The constraint uncertainty is calculated based on the distribution of feature points: ,in, Pixel noise, Geometric noise, It is a Jacobian matrix; According to step S5, cumulative errors are eliminated through pose graph optimization. Optimization variables include pose graph correction propagation and map point updates. The pose graph correction propagation corrects the global pose through loop closure constraints, and the correction amount... Backpropagation is performed to the local window to update the pose of each keyframe: The map point update is based on the new pose. and the old position And corrections, updating map points. Location: .

[0018] This invention provides a 3D vision-guided localization method based on multi-sensor fusion. It has the following beneficial effects: (1) This invention integrates an inertial measurement unit (IMU) This invention utilizes data from visual sensors and LiDAR, and employs a tightly coupled visual odometry system to fuse visual reprojection error, pre-integration error, and vehicle dynamics model constraints to achieve high-precision initial pose estimation. This multi-source data fusion approach effectively overcomes the limitations of single sensors, such as camera sensitivity to lighting, high cost and performance degradation of LiDAR in adverse weather conditions, and cumulative errors in inertial measurement units. By comprehensively utilizing the advantages of each sensor, this invention significantly improves positioning accuracy, providing more reliable position information for applications such as robot navigation and autonomous driving.

[0019] (2) This invention can dynamically adjust the positioning mode according to the complexity of the environment. In harsh environments, such as rain, snow, fog, or insufficient light, 3D / 2D joint positioning is enabled to fully utilize the complementarity of lidar and visual sensors and improve the robustness of positioning. In normal environments, only 3D feature points are used for positioning, reducing computational complexity and improving real-time performance. This adaptive mechanism enables this invention to adapt to positioning needs under different environmental conditions, has strong environmental adaptability, and broadens application scenarios.

[0020] (3) By evaluating the quality of feature points and the motion state of the carrier in real time, and by using sliding window optimization and global loop closure detection to correct the pose, this invention effectively eliminates accumulated errors and improves the robustness of the system. Even in complex or dynamic environments, such as those with a large number of dynamic obstacles or simple scene geometry, this invention can maintain stable positioning performance and reduce the risk of positioning failure or drift.

[0021] (4) This invention performs spatiotemporal synchronization and preprocessing on multi-source sensor data, including timestamp correction and alignment, spatial coordinate system calibration, high-frequency noise removal, distortion correction, histogram equalization, point cloud filtering, and coordinate transformation. These preprocessing steps not only improve the quality of the data but also provide high-quality structured data for subsequent data fusion, thereby improving the efficiency and accuracy of data processing. For example, removing high-frequency noise through a Kalman filter can significantly improve the measurement accuracy of the sensor; distortion correction and histogram equalization can improve the quality of visual images and increase the accuracy of feature extraction.

[0022] (5) This invention extracts and matches 3D / 2D feature points, performs cross-modal feature matching, constructs a local map, and uses a sliding window method to maintain the local map. This invention achieves real-time map updates while maintaining computational efficiency. This efficient data fusion strategy enables this invention to meet real-time requirements while ensuring positioning accuracy, making it suitable for application scenarios with high response speed requirements, such as real-time obstacle avoidance and path planning in autonomous driving. Attached Figure Description

[0023] Figure 1The overall architecture diagram of the method of the present application; Figure 2 The flow chart of data collection and preprocessing of the present application; Figure 3 The schematic diagram of feature extraction and matching of the present application; Figure 4 The flow chart of dynamic switching of positioning mode of the present application; Figure 5 The comparison table of the pose correction and accumulated error elimination method of the present application. DETAILED DESCRIPTION

[0024] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0025] Examples of the embodiments are shown in the drawings, wherein the same or similar reference signs represent the same or similar elements or elements with the same or similar functions throughout. The embodiments described below by reference to the drawings are exemplary and are intended to explain the present application, and cannot be understood as a limitation of the present application.

[0026] A preferred embodiment of a 3D vision-guided positioning method based on multi-sensor fusion provided by the present application is shown in the drawings. Figures 1-4 The 3D vision-guided positioning method based on multi-sensor fusion comprises the following steps: Step S1: collecting inertial measurement unit , vision sensor and lidar data, performing space-time synchronization and preprocessing; Step S2: extracting 3D / 2D feature points, performing cross-modal feature matching, constructing a local map, and evaluating feature point quality and carrier motion state in real time; Step S3: fusing vision re-projection error, pre-integration error and vehicle dynamics model constraints through tight coupling visual odometry to output initial pose estimation; Step S4: dynamically adjusting the positioning mode according to the complexity of the environment, enabling 3D / 2D joint positioning in harsh environments, and using only 3D feature points in normal environments; Step S5: correcting the pose through sliding window optimization and global loop detection to eliminate accumulated error.

[0027] According to step S1, the multi-source sensor data collection and space-time synchronization preprocessing specifically comprises the following steps: Step S1.1: synchronously collecting The raw data streams from visual sensors (cameras) and LiDAR are used to establish a unified time reference. Time stamp correction and alignment of visual sensor and LiDAR data: Coarse synchronization is achieved through hardware trigger signals or PPS (pulse second signal), with deviations typically <1μs; Fine-grained time alignment is performed using the least squares method to calculate the timestamp deviation of each sensor data packet. : ,in, and Representing the IMU and LiDAR at time [time] respectively eigenfunction values, The time offset to be solved is... This represents the total number of sampling points; The specific steps for calculating the timestamp deviation of each sensor data packet are as follows: S1.1.1 For each sampling time ( (e.g., 1, 2, ..., N), calculate Predicted eigenvalues and the characteristic values ​​actually measured by lidar ; S1.1.2 Then calculate the difference between the two and square it; S1.1.3 Sum the squared errors of all sampling points to form a total error function; The ultimate goal of S1.1.4 is to find a value. This minimizes the total error function. Step S1.2: Unify the data from multiple sensors to the same spatial coordinate system, such as the vehicle coordinate system, and perform spatial coordinate system calibration: The intrinsic parameter matrix of the vision sensor was obtained using the checkerboard calibration method. and external references (rotation matrix) Translation vector ), and determining the lidar using the hand-eye calibration method ( ) and inertial measurement unit ( The spatial transformation relationship between () involves the following formulas: Rotation matrix relationship: Translation vector relationship: , where subscript Indicates the coordinate system in which the calibration board (i.e., the "hand" in hand-eye calibration) is located; superscript and Representing lidar and Their respective coordinate systems; According to the calculation by hand-eye calibration method The rotation matrix and translation vector of Specific steps: S1.2.1 Obtain multiple sets of position data of the calibration plate in and coordinate systems respectively; S1.2.2 Calculate the rotation matrix and translation vector from the calibration plate to coordinate system from the calibration data; S1.2.3 Similarly, calculate the rotation matrix and translation vector from the calibration plate to coordinate system; S1.2.4 Substitute the above intermediate results into the rotation matrix / translation vector relationship formula, and the rotation matrix and translation vector of relative to can be obtained; Step S1.3: In data processing, Kalman filter is used to remove high-frequency noise in the data: State equation: where, is the state vector, usually containing measured angular velocity and acceleration, is the state transition matrix, which describes how the system state changes over time, is the process noise, representing all other influencing factors not considered in the model; Observation equation: where, is the observation vector, i.e. actual measurement value, is the observation matrix, which maps the state vector to the observation space, is the observation noise, representing random errors in the measurement process; Through the iterative process of Kalman filter, the state vector closer to the true state can be estimated, thus effectively removing high-frequency noise; Use kinematic integration to calculate velocity and displacement from accelerometer and gyroscope measurement data: where, is the current velocity, is the previous velocity, respectively previous time and accelerometer output at the current time, is the sampling interval; wherein, is the displacement at the current time, is the displacement at the previous time; Step S1.4: In the visual sensor data processing, in order to correct the geometric distortion caused by the lens, a distortion correction method is adopted, and these distortions mainly include radial distortion and tangential distortion: wherein, is the original pixel coordinate, and is the radial distortion coefficient, is the distance from the pixel to the center point of the image, and the calculation formula is ; wherein, and are the tangential distortion coefficients; In order to improve the visual effect of the image, so that the details in the image are clearer and the contrast is higher, histogram equalization is adopted to improve the contrast: , wherein, is the equalized gray level, is the number of pixels of the gray level in the original image, is the total number of pixels of the image, is the total number of gray levels; Step S1.5: In the laser radar data preprocessing, point cloud filtering and coordinate transformation are included, wherein the point cloud filtering includes statistical filtering to remove outliers and voxel grid filtering to reduce data volume, The specific method of the statistical filtering to remove outliers is as follows: By calculating the mean ( ) and standard deviation ( ) in the neighborhood of each point, it is judged whether the point is an outlier, and for each point, if the absolute value of the difference between the point and the neighborhood mean is greater than 3 times the standard deviation ( >3 ), the point is considered to be an outlier and is removed; The specific method of the voxel grid filtering to reduce data volume is as follows: The three-dimensional space is divided into a series of voxels with a side length of , and the original points are replaced by the centroids of the voxels: wherein, It is a coordinate of physical and mental qualities. It is the number of points within a voxel. It is the first voxel. The coordinates of the points; The coordinate transformation is achieved by rotating and translating operations to convert the point cloud coordinate system. ,in, These are the point coordinates transformed into the vehicle coordinate system. It is the rotation matrix from the lidar coordinate system to the vehicle coordinate system. These are the point coordinates in the original lidar coordinate system. Translation vector from the lidar coordinate system to the vehicle coordinate system; Statistical filtering and voxel raster filtering are mainly used for data cleaning and dimensionality reduction, while coordinate transformation ensures the accuracy and consistency of data in a unified coordinate system. Step S1.6: Data Fusion Preparation S1.6.1 Time interpolation is performed on data from (typically 100-1000Hz), vision (10-30Hz), and lidar (10-20Hz) to generate a unified time series; S1.6.2 Extraction The angular velocity / acceleration features, visual ORB feature points, and lidar planar / edge features provide structured data for subsequent fusion. Step S2 extracts 3D / 2D feature points, including LiDAR feature extraction and visual feature extraction. The LiDAR feature extraction is further divided into: Planar Feature Extraction: Principal Component Analysis (PCA) is used to detect local planes, and the eigenvalues ​​of the covariance matrix are calculated. ,like / < and / < If so, it is determined to be a planar feature; Edge feature extraction: Filtered by point cloud curvature; curvature calculation formula: ,in, For the neighborhood point set, The neighborhood centroid; The visual feature extraction includes ORB feature detection and BRIEF descriptors; The ORB feature detection method is as follows: For each pixel in the image Define a circle around it (usually with a radius of 3 pixels and 16 pixels on the circumference), and calculate the distance between these pixels and the center pixel. the difference of the gray value of the pixel : wherein, the gray value of the pixel , the gray value of the center pixel , by comparing the response value with a preset threshold value, determining whether the pixel is a corner point; using non-maximum suppression to retain local maximum points, removing adjacent corner points, ensuring that the detected corner points are significant; wherein, the BRIEF descriptor is generated as follows: In each feature point, a pair of pixels is randomly selected to generate a binary descriptor: The positions of the pixel pairs are predefined, but in actual application, random sampling can be used to increase the diversity of the descriptor. For each pair of pixels , the gray values of the two pixels are compared, so that each feature point generates a binary vector with a length of n, i.e. its BRIEF descriptor; According to step S2, the cross-modal feature matching process includes laser radar-vision matching and vision-vision matching. The specific steps of the laser radar-vision matching are as follows: S2.1.1 Feature extraction: extracting 3D plane or edge features from laser radar data; extracting corresponding 2D features from visual images; S2.1.2 Projection: projecting 3D features onto the image plane, and the projection formula is as follows: wherein, is a projection function, and are extrinsic parameter matrices (rotation matrix and translation vector), is a 3D point, is an image coordinate; S2.1.3 Error calculation: calculating the projection error , i.e. the difference between the projected point and the actual image point; S2.1.4 Mis-matching elimination: using the RANSAC (Random Sample Consensus) algorithm to eliminate mis-matching, and the inlier threshold is set to 2 pixels; The specific steps of the vision-vision matching are as follows: S2.2.1 Feature extraction: extracting BRIEF (Binary Robust Independent Elementary Features) descriptors from two visual images; ​S2.2.2 Matching: BRIEF descriptor matching using FLANN (Fast Library for Approximate Nearest Neighbors) matcher; S2.2.3 Distance computation: Hamming distance computation : where, are two BRIEF descriptors, denotes the bitwise XOR operation, is the length of the descriptor; S2.2.4 Inlier filtering: Inliers with Hamming distance less than 50 are kept; According to step S2 in the process of local map construction, the following steps are specifically adopted: S2.3.1 Pose optimization: Construct an error function by minimizing the re-projection error: where, is the observation data, i.e., the pixel coordinates of the th map point observed in the th frame, is the projection function used to project the map point from the world coordinate system to the image coordinate system of the th frame, is the pose (including rotation and translation) of the th frame, is the position of the map point in the world coordinate system, is the Huber kernel function used to reduce the impact of outliers on optimization; S2.3.2 Optimization algorithm: Use the Levenberg-Marquardt (LM) algorithm to solve the above nonlinear optimization problem; The LM algorithm combines the advantages of gradient descent and Gauss-Newton method, and can improve the convergence speed when approaching the optimal solution; S2.3.3 Sliding window method: Adopt a sliding window strategy to maintain the local map, the window size is fixed, for example, keep the feature points and related information of the last N frames (such as N=10); The advantage of this is that the map can be updated in real time and the calculation efficiency is maintained; S2.3.4 Prior information preservation when marginalizing old frames: When a new frame is added to the window and an old frame is removed, the data of the old frame needs to be properly handled to avoid information loss, the formula is used, is the updated covariance matrix, is the Jacobian matrix of the new frame relative to the current window, is the information matrix of the new frame, is the prior covariance matrix of the old frame; The addition of a new frame changes the uncertainty distribution of the whole system, and by adding the prior covariance of the old frame , the impact of this change can be offset to some extent, making the map more stable and reliable; The evaluation of feature point quality according to step S2 includes visual feature quality and lidar feature quality; The specific steps of visual feature quality evaluation are as follows: S2.4.1 Calculate feature point response value The feature point response value is usually calculated by a certain feature detection algorithm (such as SIFT, SURF, ORB, etc.), which reflects the prominence of the feature point in the image; feature points with response value less than 10 can be removed, which can remove noise points that are unlikely to be real feature points.

[0028] S2.4.2 Evaluate scale invariance, which refers to the stability of feature points at different scales, calculate the ratio of maximum scale To the minimum scale If the ratio is greater than 3, it means that the feature point has good stability at different scales and is retained; S2.4.3 Calculate the change of the main direction angle The main direction is an important attribute of the feature point, which is used to describe the gradient direction of the image around the feature point. Calculate the angle change of the main direction between adjacent frames, remove feature points with angle change greater than 30°, because these points may have large deviation or rotation in the tracking process and are no longer stable; The specific steps of lidar feature quality evaluation are as follows: S2.5.1 Plane feature normal vector consistency: In lidar data processing, plane features are often used to represent flat surfaces. Calculate the included angle of the normal vectors of the plane features between adjacent frames If is greater than 0.95 (i.e. is less than about 18.2°), it is considered that the two normal vectors are highly consistent, and the plane feature has good stability; S2.5.2 Edge feature collinearity: Edge features are often used to represent the contours or boundaries of objects. Calculate the distance variance of edge feature points, remove discrete points with variance greater than 0.01, because these points may not be on the same straight line or there may be large measurement errors; S2.6.1 Calculate motion increments (including rotation increments , velocity increments Location increment To predict the motion state of the carrier: Rotation Increment : ,in, It is the first angular velocity at time t, It is the zero bias of the gyroscope. It is the sampling interval; Speed ​​increment : ,in, It is the first acceleration at any moment It is the zero bias of the accelerometer. It is the rotation matrix from the previous moment; Position increment : ,in, It is the rotation matrix from the previous moment; S2.6.2 The motion state of the carrier is determined by calculating the translational velocity and rotational angular velocity: Formula for calculating translational velocity: Formula for calculating rotational angular velocity: Rules for determining motion state: Stationary: When the translational velocity is less than 0.1 m / s and the rotational angular velocity is less than 5° / s, it is determined to be in a stationary state; Low speed: When the translation speed is between 0.1 and 1 m / s or the rotational angular velocity is between 5° and 15° / s, it is judged as a low speed state; High speed: When the translational speed is greater than or equal to 1 m / s or the rotational angular velocity is greater than or equal to 15° / s, it is judged as a high speed state; pass Motion prediction and state classification can effectively assess the motion state of a vehicle, providing an important basis for subsequent control, navigation and other tasks. According to step S3, visual reprojection error needs to be fused. By applying pre-integration error and constraints from the vehicle dynamics model, high-precision initial pose estimation is achieved, which is specifically divided into the following steps: S3.1 optimizes camera pose and 3D spatial point coordinates through feature point matching constraints to model visual reprojection error, for each pair of consecutive frames. Feature points matched in Its reprojection error is defined as: It describes the actual observed pixel coordinates. The difference between the predicted pixel coordinates and those calculated using camera pose and 3D point coordinates, where, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, Rwcis the pose transformation matrix from the world coordinate system to the camera, , are the velocity components of the vehicle in and directions, respectively; steering geometry constraints: where, is the front wheel steering angle, is the wheelbase of the vehicle, is the turning radius of the vehicle, is the velocity of the vehicle, is the rate of change of the heading angle; transform the dynamics model into residual terms use the residual terms to construct a cost function optimize the vehicle pose estimate by minimizing the cost function, where, is the Huber robust kernel function, is the dynamics model covariance; by introducing the nonholonomic constraints of the bicycle model and the steering geometry constraints and formulating these constraints as residual terms, the rationality of the vehicle pose estimate can be effectively improved; S3.4 to fuse multi-source constraints to solve the optimal state quantity, construct a joint cost function use a nonlinear optimization algorithm (such as Dog-Leg or Schur complement marginalization) to solve, and maintain a fixed number of historical frames by the sliding window method, where, is the state vector of the system, , , are the error terms of the projective sensor, and the on-board sensor, respectively, , , are the error covariance matrices of the corresponding sensors; S3.5 output the optimized state quantity: where, denotes the transformation matrix from the world coordinate system to the camera coordinate system, denotes the velocity vector of the camera in the world coordinate system, is the bias of the gravitational acceleration, is the bias of the gyroscope, is the covariance matrix of the position estimate at the time, is the vehicle control quantity; In step S4, an environment complexity evaluation system needs to be constructed to quantify the dynamicity and geometric degeneration of the environment, providing criteria for mode switching. The evaluation method is as follows: S4.1 calculate the spatial distribution uniformity of feature points in the image plane: ,in, Number of image blocks It is the first The probability distribution of the number of feature points within a grid cell. It is the total number of feature points. For the first The number of feature points within a grid cell The smaller the value, the more concentrated the distribution. When all feature points are concentrated in one grid, H=0; when feature points are evenly distributed across all grids, H= ; S4.2 passed Measurements assess the intensity of the carrier's motion: ,in, The acceleration modulus (after deducting the bias). For angular velocity modulus, The larger the value, the more intense the exercise. S4.3 uses a geometric degradation factor to detect the geometric uniformity of a scene: , Complexity metrics: ,in, , , These are the weighting coefficients; The positioning mode is dynamically switched based on the environmental complexity in step S4, and the triggering conditions are as follows: Joint positioning mode (adverse environment): when or Activated at time; Pure 3D positioning mode (normal environment): When or Activated at time; In order for the system to adapt to constantly changing environmental conditions, the threshold It will be dynamically updated, using a sliding window to statistically analyze the historical complexity distribution and dynamically update the threshold: ,in, It is the threshold at the current moment. These are update coefficients, used to control the weight distribution between new and old data. For the most recent frame Average complexity; Step S5, which involves sliding window optimization, specifically includes window management strategies, edge prior construction, and joint optimization models. The specific steps are as follows: S5.1.1 Window Management Strategy: Based on disparity threshold and time interval Screening key frames: or , according to the complexity of the scene Adaptive window size adjustment : , wherein, is the reference window size, is the complexity sensitivity coefficient; S5.1.2 Marginal Prior Construction: First, the variables in the window are divided into optimization variables and marginal variables , then the prior information is constructed by matrix block: , as a fixed term constraint in subsequent optimization: ; S5.1.3 Joint Optimization Model: Through optimization variables: , construct the cost function ; According to step S5, global loop detection and constraint fusion is specifically divided into the following steps: S5.2.1 Bag-of-Words Model Matching: Based on DBoW2, a visual vocabulary tree is constructed, and the inter-frame similarity is calculated: , wherein, is the BoW vector, is the similarity threshold; The similarity between the current frame and the historical frame is calculated If the similarity exceeds the threshold (typical value 0.15), it is considered as a loop candidate; the fundamental matrix F is used to verify the geometric correspondence, if , the loop candidate is further confirmed; S5.2.2 Loop Constraint Construction: Calculate the inter-loop frame transformation through PnP or ICP: , calculate the constraint uncertainty according to the feature point distribution: , wherein, is the pixel noise, is the geometric noise, is the Jacobian matrix; According to step S5, the accumulated error is eliminated through pose graph optimization, and the optimization variables include pose graph correction propagation, map point update, the pose graph correction propagation corrects the global pose through the loop constraint, and the correction amount is propagated to the local window to update the pose of each key frame: ; the map point update is updated according to the new pose and the old pose and the correction amount, update the map point the position of the map point: .

[0029] In summary, the present application proposes a 3D vision guided positioning method based on multi-sensor fusion. Through innovative data fusion strategy and adaptive mechanism, the key problems of precision, robustness and real-time of existing technology are effectively solved. This method not only improves the positioning accuracy and environmental adaptability, but also provides reliable technical support for precise positioning in complex scenes through efficient data processing and robustness enhancement measures.

[0030] It should be noted that in this paper, relationship terms such as first and second are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between such entities or operations. Moreover, the terms "include", "contain" or any other variants thereof are intended to cover non-exclusive inclusion, so that the process, method, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, article or device.

[0031] Finally, it should be noted that: the above only for the preferred embodiments of the present application, and does not limit the present application, although the foregoing detailed description of the present application is made with reference to the foregoing embodiments, for those skilled in the art, it still can be modified to the technical solutions recorded in the foregoing embodiments, or to replace some of the technical features. Any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A 3D vision-guided positioning method based on multi-sensor fusion, characterized in that, Comprise the following steps: Step S1: Collecting inertial measurement unit , visual sensor and lidar data, spatio-temporal synchronization and preprocessing; Step S2: extract 3D / 2D feature points, cross-modal feature matching, local map construction, real-time evaluation of feature point quality and carrier motion state; Step S3: Fusion of visual reprojection error via tightly coupled visual odometry Based on the pre-integration error and vehicle dynamics model constraints, the initial pose estimate is output. Step S4: dynamically adjust the positioning mode according to the environment complexity, enable 3D / 2D joint positioning in harsh environment, and only use 3D feature points in normal environment; Step S5: optimize and correct the pose through sliding window and global loop detection to eliminate cumulative error. 2.The 3D vision guided positioning method based on multi-sensor fusion of claim 1, wherein: According to the step S1, the multi-source sensor data acquisition and space-time synchronization preprocessing specifically comprises the following steps: Step S1.1: Synchronize acquisition , raw data streams of the vision sensor and the lidar, establish a unified time reference, align , the vision sensor and the lidar data with timestamp correction The least square method is used for fine-grained time alignment to calculate the timestamp deviation of each sensor data packet : , , respectively represent the feature function values of the IMU and the lidar at time , is the time offset to be solved, is the total number of sampling points; The specific steps of calculating the timestamp deviation of each sensor data packet are as follows: S1.1.1 For each sampling time instant =1,2,...,N), compute predicted eigenvalues and eigenvalues measured by the LIDAR ;​ S1.1.2 Then calculate the difference between the two and square it; S1.1.3 Accumulate the error squares of all sampling points to form a total error function; S1.1.4 The final goal is to find a value such that this total error function is minimized; Step S1.2: unify the multi-sensor data to the same spatial coordinate system, such as the vehicle coordinate system, and perform spatial coordinate system calibration: An internal parameter matrix of a vision sensor is acquired by a chessboard calibration method and an external parameter , a rotation matrix and a translation vector , and a spatial transformation relationship between a laser radar and an inertial measurement unit is determined by a hand-eye calibration method, involving the following formulas: Rotation matrix relationship: Translation vector relationship: wherein the subscript represents the calibration board, i.e., the coordinate system in which the "hand" in the hand-eye calibration is located; the superscripts and respectively represent the coordinate systems of the lidar and the camera, respectively; According to the calculation by the hand-eye calibration method the rotation matrix and the translation vector of the camera with respect to the robot S1.2.1 respectively in and under the coordinate system to obtain a plurality of sets of position data of the calibration plate; S1.2.2 The calibration data calculates a rotation matrix and a translation vector from the calibration board to the coordinate system ; S1.2.3 The rotation matrix and the translation vector from the calibration board to the coordinate system of the camera are calculated as well and the translation vector ; S1.2.4 Substitute the above calculation results into the rotation matrix / translation vector relationship formula, i.e. the rotation matrix and translation vector of the coordinate system of the second camera relative to ; Step S1.3: In In data processing, Kalman filter is used to remove high frequency noise in data: State equation: where, is the state vector, usually containing measured angular velocity and acceleration, is the state transition matrix, describing how the system state changes over time, is the process noise, representing all other influencing factors not considered in the model; Observation equation: where, is the observation vector, i.e. is the actual measurement of, is the observation matrix that maps the state vector to the observation space, is the observation noise that represents the random error in the measurement process; The velocity and displacement are calculated from the measurement data of the accelerometer and gyroscope by kinematic integration: wherein, is the velocity at the current time instant, is the velocity at the previous time instant, respectively the previous time instant and the accelerometer output at the current time instant, is the sampling interval; wherein, is the displacement at the current time instant, is the displacement at the previous time instant; In the visual sensor data processing, in order to correct the geometric distortion caused by the lens, a distortion correction method is adopted, and the distortion mainly includes radial distortion and tangential distortion: wherein, is the original pixel coordinate, and is the radial distortion coefficient, is the distance of the pixel to the image center point, calculated as ; wherein and is the tangential distortion coefficient; In order to improve the visual effect of the image and make the details in the image clearer and the contrast higher, histogram equalization is adopted to improve the contrast: , where, is the equalized gray level, is the number of pixels in the original image having gray level , is the total number of pixels in the image, is the total number of gray levels; Step S1.5: laser radar data preprocessing includes point cloud filtering and coordinate transformation, wherein the point cloud filtering includes statistical filtering to remove outliers and voxel grid filtering to reduce data volume, The specific way of statistical filtering to remove outliers is as follows: By calculating the mean value and standard deviation of the neighborhood of each point and judging whether the point is an outlier or not , for each point, if the absolute value of the difference between the point and the mean value of the neighborhood is greater than 3 times the standard deviation, the point is considered as an outlier and is removed. The specific way of voxel grid filtering to reduce data volume is as follows: The three-dimensional space is divided into a series of voxels with edge length The original points are replaced by the centroid of the point cloud within the voxel: wherein, is the coordinate of the centroid of the voxel, is the number of points within the voxel, is the coordinate of the th point within the voxel; The coordinate transformation realizes the conversion of the point cloud coordinate system through rotation and translation operations wherein, is the point coordinate converted to the vehicle body coordinate system, is a rotation matrix from the laser radar coordinate system to the vehicle body coordinate system, is the point coordinate in the original laser radar coordinate system, is a translation vector from the laser radar coordinate system to the vehicle body coordinate system; Step S1.6: data fusion preparation S1.6.1 pair time interpolating the visual, lidar data to generate a unified time series; S1.6.2 extraction The angular velocity / acceleration features, visual ORB feature points, and laser plane / edge features of the lidar provide structured data for subsequent fusion. 3.The 3D vision guided positioning method based on multi-sensor fusion of claim 1, wherein: According to step S2, 3D / 2D feature points are extracted and matched, including laser radar feature extraction and visual feature extraction, the laser radar feature extraction is divided into: Plane feature extraction: principal component analysis is used to detect local plane, and the eigenvalue of covariance matrix is calculated: , if / and / , plane feature is determined.​​ Edge feature extraction: curvature calculation formula through point cloud curvature screening: wherein, is a set of neighborhood points, is a neighborhood centroid; The visual feature extraction includes ORB feature detection and BRIEF descriptor; The specific way of ORB feature detection is as follows: For each pixel in the image Define a circle around it, and calculate the distance between these pixels and the center pixel. The difference in grayscale values ​​is calculated, and the sum is used to obtain the response value. : ,in, Represents pixels grayscale value, Represents the center pixel The grayscale value is compared with the response value. The system uses a preset threshold to determine whether a pixel is a corner point; non-maximum suppression is used to preserve local maxima and remove neighboring corner points to ensure that the detected corner points are significant. The specific way of BRIEF descriptor generation is as follows: Randomly select around each feature point Generate binary descriptor for each pixel point: The position of the pixel pairs is predefined, but in practical applications, the diversity of the descriptor can be increased by random sampling, for each pair of pixels , comparing their gray values, so that each feature point will generate a binary vector of length n, that is, its BRIEF descriptor.

4. The 3D vision guided positioning method based on multi-sensor fusion according to claim 1, characterized in that: According to step S2, the cross-modal feature matching process includes laser radar-vision matching and vision-vision matching; The specific steps of laser radar-vision matching are as follows: S2.1.1 Feature extraction: extract 3D plane or edge features from laser radar data; extract corresponding 2D features from visual images; S2.1.2 Projection: Project the 3D feature onto the image plane, the projection formula is as follows: wherein, is a projection function, and is an extrinsic matrix is a 3D point, is an image coordinate; S2.1.3 Error calculation: Calculate the projection error i.e. the difference between the projected point and the actual image point; S2.1.4 Mis-matching elimination: use RANSAC algorithm to eliminate mis-matching, and the inlier threshold is set to 2 pixels; The specific steps of vision-vision matching are as follows: S2.2.1 Feature extraction: extract BRIEF descriptors from two visual images; S2.2.2 Matching: use FLANN matcher to match BRIEF descriptors; S2.2.3 Distance calculation: Hamming distance is calculated : wherein, are two BRIEF descriptors, denotes a bitwise XOR operation, is the length of the descriptors; S2.2.4 Inlier screening: retain inliers with Hamming distance less than 50.

5. The 3D vision guided positioning method based on multi-sensor fusion according to claim 1, characterized in that: According to step S2, in the process of local map construction, the following steps are adopted: S2.3.1 Pose optimization: construct error function by minimizing re-projection error: wherein, is the observation data, i.e. the pixel coordinates of the thmap point observed in the thframe, is the projection function used to project a map point from the world coordinate system to the image coordinate system of the thframe, is the pose of the thframe, including rotation and translation, is the position of the map point in the world coordinate system, is the Huber kernel function used to reduce the influence of outliers on the optimization; S2.3.2 Optimization algorithm: Levenberg-Marquardt algorithm is used to solve the nonlinear optimization problem; S2.3.3 Sliding window method: a sliding window strategy is adopted to maintain the local map, and the window size is fixed, for example, the last N frames of feature points and their related information are retained; the advantage of this is that the map can be updated in real time and the calculation efficiency is maintained; S2.3.4 Prior information preservation when marginalizing out old frames: When a new frame is added to the window and an old frame is removed, the data of the old frame needs to be properly handled to avoid information loss. The formula where, is the updated covariance matrix, is the Jacobian matrix of the new frame with respect to the current window, is the information matrix of the new frame, is the prior covariance matrix of the old frame. 6.The 3D vision guided positioning method based on multi-sensor fusion of claim 1, wherein: According to step S2, the feature point quality is evaluated, including visual feature quality and lidar feature quality; The specific steps of visual feature quality evaluation are as follows: S2.4.1 Calculate feature point response value The feature point response value is usually calculated by a certain feature detection algorithm, which reflects the prominence of the feature point in the image; the feature points with response value less than 10 are removed, which can remove the noise points that are less likely to be real feature points; S2.4.2 Evaluate scale invariance, which refers to the stability of the feature point at different scales, calculate the ratio of the maximum scale to the minimum scale If the ratio is greater than 3, it means that the feature point has good stability at different scales, and is retained. S2.4.3 Calculate principal direction angle change The principal direction is an important attribute of a feature point, which describes the image gradient direction around the feature point. The angle change of the principal direction between adjacent frames is calculated The feature points with angle change greater than 30° are removed, because these points may have a large shift or rotation in the tracking process and are no longer stable. The specific steps of lidar feature quality evaluation are as follows: S2.5.1 Plane Feature Normal Vector Consistency: In the processing of lidar data, plane features are often used to represent flat surfaces. The angle between the normal vectors of the plane features between adjacent frames is calculated If greater than 0.95, i.e. less than about 18.2°, it is considered that the two normal vectors are highly consistent, and the plane feature has good stability; S2.5.2 Edge feature collinearity: Edge features are usually used to represent the contour or boundary of an object, and the distance variance between edge feature points is calculated , and the discrete points with variance greater than 0.01 are removed, because these points may not be on the same straight line or there is a large measurement error.

7. The 3D vision guided positioning method based on multi-sensor fusion according to claim 1, characterized in that: According to step S2, the carrier motion state is evaluated, including the following two ways: S2.6.1 Calculate motion increments, including rotation increments, by pre-integration , velocity increments , position increments to predict the motion state of the carrier: rotational increment : wherein, is the angular velocity at the time instant, is the bias of the gyroscope, is the sampling interval; velocity increment : wherein, is the acceleration at the time instant, is the zero offset of the accelerometer, is the rotation matrix at the previous time instant; Position increment : wherein, is the rotation matrix of the previous time instant; S2.6.2 Determine the motion state of the carrier by calculating the translational velocity and the angular velocity of rotation: Translation speed calculation formula: Rotational angular velocity calculation formula: Motion state determination rule: Stationary: when the translational velocity is less than 0.1 m / s and the angular velocity of rotation is less than 5° / s, it is determined as a stationary state; Low speed: when the translational velocity is between 0.1~1 m / s or the angular velocity of rotation is between 5°~15° / s, it is determined as a low speed state; High speed: when the translational velocity is greater than or equal to 1 m / s or the angular velocity of rotation is greater than or equal to 15° / s, it is determined as a high speed state. 8.The 3D vision guided positioning method based on multi-sensor fusion of claim 1, wherein: According to step S3, the visual re-projection error, and the pre-integration error and the car dynamics model constraints, a high-precision initial pose estimation is achieved, which is specifically divided into the following steps: S3.1 The camera pose and three-dimensional space point coordinates are optimized by feature point matching constraint to model the visual re-projection error. For each pair of consecutive frames matching feature points The re-projection error is defined as: It describes the actual observed pixel coordinates. The difference between the predicted pixel coordinates and those calculated using camera pose and 3D point coordinates, where, This represents the pose transformation matrix from the world coordinate system to the camera. For camera to The calibration extrinsic parameter matrix, For feature points Coordinates in normalized space This is a projection function that projects points in three-dimensional space onto the image plane. For feature points In frame The actual observed pixel coordinates; through algorithm optimization The adjusted variables aim to minimize the reprojection error, utilizing the cost function. To solve the above optimization problem, where, It is the Huber robust kernel function, used to reduce the impact of outliers on optimization results. It is the covariance matrix of pixel coordinates, used to represent the statistical characteristics of observation noise. This indicates that for all matching feature points Perform summation; S3.2 Utilization of measurement constraints on motion increments between adjacent frames pre-integration error constraints for a given time interval within measurements, calculation of pre-integration of measurements: wherein, , are gyroscope and accelerometer biases; resulting in a pre-integration error term: , constraining the motion estimation of adjacent frames, using a cost function , to minimize the pre-integration error, where, is a Huber robust kernel function, performing a non-linear transformation of the error, is a pre-integration covariance matrix, denotes the sum over all , elements of the matrix S3.3 By introducing vehicle kinematics / dynamics prior, the vehicle dynamics model constraint is improved to enhance the pose rationality, and the vehicle pose estimation is optimized and constrained; the bicycle model constraint is adopted to constrain the vehicle motion: Non-holonomic constraints: where, is the coordinate of the center of the rear axle of the vehicle, is the heading angle of the vehicle, , are the velocity components of the vehicle in and directions, respectively; Turning geometry constraints: wherein, is the front wheel steering angle, is the wheelbase of the vehicle, is the turning radius of the vehicle, is the speed of the vehicle, is the rate of change of the heading angle; transforming the dynamic model into residual terms using the residual terms to construct a cost function optimizing the vehicle pose estimate by minimizing the cost function, wherein is a Huber robust kernel function, is a dynamic model covariance; by introducing the nonholonomic constraints of the bicycle model and the steering geometry constraints and formulating these constraints as residual terms, the reasonability of the vehicle pose estimate can be effectively improved; S3.4 To solve the optimal state variables for the fusion of multi-source constraints, a joint cost function is constructed , which is solved using a nonlinear optimization algorithm and maintained with a fixed number of historical frames using a sliding window approach, where, is the state vector of the system, , , are the error terms of the projective sensor, and the on-board sensor, respectively, , , are the error covariance matrices corresponding to the sensors; S3.5 outputs the optimized state quantities: wherein denotes the transformation matrix from the world coordinate system to the camera coordinate system, denotes the velocity vector of the camera in the world coordinate system, is the bias of the gravitational acceleration, is the bias of the gyroscope, is the position estimate at the time instant, is the vehicle control quantity. 9.The 3D vision guided positioning method based on multi-sensor fusion of claim 1, wherein: In step S4, an environment complexity evaluation system needs to be constructed to quantify the environment dynamics and the geometric degradation degree, and to provide criteria for mode switching, and the evaluation method is as follows: S4.1 Calculate the spatial distribution uniformity of feature points in the image plane: wherein, is the number of image patches, is the probability distribution of the number of feature points in the th grid, is the total number of feature points, is the number of feature points in the th grid, The smaller the value, the more concentrated the distribution is. When all feature points are concentrated in one grid, H = 0; when the feature points are evenly distributed in all grids, H = log2N. ; S4.2 by Measurement of the degree of motion of the carrier: wherein, is the acceleration module, is the angular velocity module, the greater the more intense the movement; S4.3 Use the geometric degradation factor to detect the geometric simplicity of the scene: ， Complexity comprehensive index: wherein, , , is a weight coefficient; According to step S4, the positioning mode is dynamically switched according to the environment complexity, and the triggering conditions are as follows: Joint positioning mode: when or is activated; Pure 3D positioning mode: activated when or ​ In order to enable the system to adapt to changing environmental conditions, the threshold will be dynamically updated, using a sliding window to statistically track the complexity distribution, dynamically updating the threshold: wherein, is a threshold value at the current time, is an update coefficient for controlling the weight distribution between new data and old data, is the most recent frame complexity average.

10. The 3D vision guided positioning method based on multi-sensor fusion according to claim 1, characterized in that: According to step S5, the sliding window optimization specifically includes window management strategy, marginalization prior construction, and joint optimization model, and the specific steps are as follows: S5.1.1 Window management strategy: Based on a parallax threshold and a time interval Screening key frames: or , according to scene complexity self-adaptively adjust window size : wherein, is a reference window size, is a complexity-sensitive coefficient; S5.1.2 Marginalization prior construction: First, the variables in the window are divided into variables to be optimized and variables to be marginalized Then the prior information is constructed by matrix block: as a fixed term constraint in subsequent optimization: ; S5.1.3 Joint optimization model: By optimizing the variables: , a cost function is constructed ; According to step S5, the global loop detection and constraint fusion are specifically divided into the following steps: S5.2.1 Bag-of-words model matching: based on DBoW2, a visual vocabulary tree is constructed, and the inter-frame similarity is calculated: wherein, is a BoW vector, is a similarity threshold; Calculate the current frame With historical frames similarity between If similarity If the threshold is exceeded, it is considered a loop closure candidate; the geometric correspondence is verified using the fundamental matrix F. If it satisfies... This further confirms the candidate loopback; S5.2.2 Loop constraint construction: the inter-loop frame transformation is calculated by PnP or ICP: , the constraint uncertainty is calculated from the feature point distribution: where is the pixel noise, is the geometric noise, is the Jacobian matrix; According to step S5, accumulated errors are eliminated by pose graph optimization, optimization variables include pose graph correction propagation, map point update, the pose graph correction propagation corrects global poses by loop closure constraints, and the correction amount is back-propagated to local windows to update the poses of each key frame: ; the map point update updates the positions of map points according to new poses and old poses and the correction amount .

Images