Vehicle-mounted combined attitude determination and positioning method for special vehicle based on ground visual angle odometer

Abstract

The invention designs a vehicle attitude determination and positioning method, and particularly relates to a special vehicle vehicle-mounted combined attitude determination and positioning method based on a ground view angle odometer, and the method comprises the following steps: obtaining the captured data of the ground view angle odometer, including a road surface image and IMU (Inertial Measurement Unit) data; the method comprises the following steps: extracting feature points from a pavement image based on an SIFT (Scale Invariant Feature Transform) vehicle ground view angle road visual odometer front-end algorithm to form feature descriptors; the method comprises the following steps: optimizing and processing a multi-frame feature descriptor based on a BA optimized vehicle ground view angle visual odometer back-end algorithm to obtain inter-frame pose estimation; inter-frame pose estimation and IMU data are fused based on a loose coupling mode, and the state and pose of the vehicle are predicted through an EKF algorithm. Compared with the prior art, the method has the advantages of realizing real-time and accurate pose positioning of the special vehicle and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle-mounted attitude determination and positioning, and in particular to a vehicle-mounted combined attitude determination and positioning method for a special vehicle based on a ground view angle odometer. Background Art

[0002] Ground-based odometry is a novel on-board attitude and positioning method for specialized vehicle configurations. It aims to address several inherent shortcomings of traditional odometry. For example, while traditional satellite positioning systems offer good positioning accuracy in open outdoor environments, they are often limited by signal strength and diffraction in scenarios such as urban canyons, tunnels, foggy scenes, and indoor environments, resulting in inaccurate or even inability to locate the vehicle. Traditional visual odometry solutions often use a head-up perspective to calculate pose information by extracting features from the surrounding scene. However, this perspective relies on environmental characteristics, is susceptible to occlusion by dynamic objects (such as pedestrians and vehicles), and suffers from viewing limitations in scenes with drastic lighting changes. Furthermore, it suffers from sensor degradation in strong sunlight and extreme weather conditions. This is the context in which the concept of ground-based odometry was proposed. Ground-based odometry uses a camera mounted on the underside of the vehicle to capture road surface texture information (such as asphalt grain, scratches, and zebra crossings) from a vertical downward perspective to determine the vehicle's pose. It leverages epipolar geometry and kinematic constraints to accurately calculate the vehicle's relative motion. The advantage of this technology lies in its excellent environmental robustness. For example, a dedicated light source provides stable lighting conditions, effectively avoiding interference from changes in external light. At the same time, the road texture is time-invariant and the distance from the camera is constant, which significantly reduces the impact of dynamic objects on the system. In addition, the vertical viewing angle design makes the system less susceptible to occlusion problems caused by surrounding vehicles or the environment, further improving reliability. In terms of data accuracy, due to the use of high-resolution imaging and close-range shooting, the system can extract more detailed road features, thereby obtaining more accurate positioning results. From the perspective of application scalability, the ground-based odometry is particularly suitable for environments where GPS signals are missing, such as tunnels and underground parking lots. It greatly expands the operational scenarios of autonomous vehicles and provides a reliable solution for stable positioning in complex environments.

[0003] However, to date, the application of ground-based odometry still faces many technical challenges that limit its practical application. These challenges mainly include: Real-time requirements: High frame rate images (e.g., 70fps and above) need to be processed to ensure that the fields of view between adjacent frames overlap when the vehicle is traveling at high speeds, which places high demands on the real-time performance of the algorithm.

[0004] Cumulative Error Control and Blind Tolerance: Due to the limitations of the image sensor itself, each frame of the resolved content will be affected by the accumulation of tiny errors. Therefore, algorithms are needed to ensure that the errors of the vehicle within a limited mileage do not diverge over time.

[0005] Robust Image Processing Algorithm for Ground Texture: Due to the special perspective of the ground view odometer, it is necessary to develop an algorithm that can fully extract and process the edge feature information of the road surface texture, find the features that truly belong to the ground, are static, and invariant between frames, and ensure that they are not lost due to the movement, rotation, and shaking of the vehicle.

[0006] After retrieval, Chinese Patent Application Publication No. CN110207714A discloses a method, an in-vehicle system, and a vehicle for determining the pose of a vehicle. The method includes: obtaining first inertial navigation information and first vehicle odometer information of the vehicle, where the first inertial navigation information includes the attitude of the vehicle, the speed of the vehicle, and the position change information of the vehicle; determining the first current estimated pose of the vehicle according to the first inertial navigation information and the first vehicle odometer information; obtaining the external parameters of N monocular cameras of the vehicle, where N is an integer greater than 1; fusing the inertial odometry algorithm with the visual SLAM algorithm of each monocular camera according to the external parameters of the N monocular cameras and the first current estimated pose to estimate the current poses of the N monocular cameras; respectively performing coordinate transformation on the current poses of the N monocular cameras to obtain N transformed poses of the vehicle; and obtaining the fused pose of the vehicle according to the N transformed poses. This existing patent application has problems of insufficient accuracy and real-time performance of the pose.

[0007] How to achieve real-time and accurate pose positioning of special vehicles based on the ground view odometer has become a technical problem to be solved. Summary of the Invention

[0008] The purpose of the present invention is to overcome the defects of the above-mentioned existing technologies and provide a method for on-vehicle combined pose positioning of special vehicles based on the ground view odometer.

[0009] The purpose of the present invention can be achieved by the following technical solutions: According to one aspect of the present invention, there is provided a method for on-vehicle combined pose positioning of special vehicles based on the ground view odometer. The method includes the following steps: Step 1: Obtain the captured data of the ground view odometer, including road surface images and IMU data; Step 2: Based on the SIFT-based front-end algorithm of the vehicle ground view road visual odometer, extract feature points from the road surface image to form feature descriptors; Step 3: Based on the BA-optimized visual odometry backend algorithm for vehicle ground perspective, the multi-frame feature descriptors output in Step 2 are optimized and processed to obtain the inter-frame pose estimation. Step 4: Based on the loose coupling method, the inter-frame pose estimation is fused with the IMU data, and the vehicle state and pose are predicted through the EKF algorithm.

[0010] Preferably, based on the front-end algorithm, the process of forming the feature descriptor includes: Construct a DoG scale space based on the road surface image; Detect local extreme points in the three-dimensional space of the DoG scale space as candidate feature points; Eliminate low-contrast feature points and edge response points from the candidate feature points, and retain stable feature points; Determine the main direction of the feature points; Centered on the feature points, divide the 4×4 sub-regions in the coordinate system rotated to the main direction of the feature points, calculate the gradient histograms in 8 directions for each sub-region, and form a 128-dimensional feature descriptor.

[0011] More preferably, perform Gaussian blur processing on the input road surface image at different scales to form a multi-scale image pyramid, obtain the three-dimensional space of the DoG scale space through the differential operation of adjacent scale Gaussian blurred images, and use the differential Gaussian DoG method to approximately calculate and detect the feature points existing at different scales as candidate feature points.

[0012] More preferably, the specific method for retaining stable feature points is: The DoG function of the candidate feature point The main curvature of is proportional to the eigenvalues of the 2×2 Hessian matrix Let the maximum eigenvalue and the minimum eigenvalue of the matrix H be and respectively, then , , , where, and respectively represent the trace and determinant of the matrix ; Let , representing the ratio of the maximum eigenvalue and the minimum eigenvalue , then there is: , When the following formula is satisfied, eliminate the feature point; otherwise, retain it: , where, represents the time constant.

[0013] Preferably, in the front - end algorithm, texture memory is used to bind the input road surface image to optimize the Gaussian filtering calculation. The candidate feature points are detected by combining the parallel non - maximum suppression algorithm with atomic operations, and the feature descriptor is formed by using Warp - level parallel acceleration.

[0014] Preferably, the process of obtaining the inter - frame pose estimation based on the back - end algorithm includes: Constructing a sliding window optimization framework: Set the sliding window size to N, and store the camera pose and the observed road points within the current window; Project the road points onto the i image plane of the i - th frame. For the matched feature points, calculate the reprojection error between the feature points and the reprojected points; Constructing a non - linear optimization problem: The optimization objective is to minimize the sum of the reprojection errors of all observations within the sliding window. The optimization variables include the Lie algebra of the camera pose and the coordinates of the road points; Iteratively solve the non - linear optimization problem based on the L - M algorithm to obtain the optimized camera pose and road point coordinates within the sliding window.

[0015] More preferably, for the reprojection error, the Huber loss function is used to reduce the influence of mismatches: , where is the cost function, measuring the contribution of the error; is the error of the i -th camera pose at the j -th road point; represents the unit impulse function.

[0016] Preferably, the iterative solution of the non - linear optimization problem based on the L - M algorithm includes: Calculate the errors with respect to the pose and the road point respectively, to form the corresponding Jacobian matrices and : , All are combined into a matrix , all are combined into a matrix , and all the errors are combined into a matrix ; Construct the Hessian matrix and the gradient vector : , Solve the increment using the L-M algorithm : , Among them, is the identity matrix; Utilize the block sparse structure of the matrix, adopt Schur complement decomposition, first solve the pose increment, and then back-substitute to solve the landmark point increment.

[0017] Preferably, the process of predicting the vehicle state and pose through the EKF algorithm includes: EKF prediction stage driven by IMU raw data: Predict the future state of the vehicle through the inertial motion model; EKF update stage based on visual observation: Use the inter-frame pose as the observation update, and adopt quaternion logarithmic mapping to process the attitude residual.

[0018] More preferably, the inertial motion model is represented as follows: , Among them, is the corresponding rotation matrix,[[ID=3G4]] is the gravity vector, and are respectively k the position vector at the +1 moment and k the moment, and are respectively k the velocity vector at the +1 moment and k the moment, is the sampling interval, is the accelerometer measurement value, is the accelerometer bias, and are respectively k the quaternion at the +1 moment and k the moment, representing the pose of the vehicle, and are respectively the IMU measurement value and the IMU bias; Covariance propagation is represented as follows: , Among them, F is the state transition matrix of the system, describing the dynamic behavior of the vehicle; Q is the covariance matrix of the process noise; represents the covariance matrix of the prior estimate; is the motion model of the vehicle, X is the state vector, is the acceleration measurement variance, is the variance of angular velocity measurement, is the variance of accelerometer bias, is the variance of IMU bias.

[0019] Compared with the prior art, the present invention has the following beneficial effects: 1) By adopting SIFT feature extraction front-end optimization, the present invention ensures high-precision feature matching; the back-end combines sliding window BA non-linear optimization to suppress cumulative errors, which is superior to the traditional EKF filtering method. Finally, the IMU data is fused in a loose coupling manner to make up for the deficiency of pose estimation of pure visual odometer in fast movement or short-term occlusion; therefore, the accuracy of vehicle pose positioning is improved.

[0020] 2) The front-end algorithm of the present invention optimizes the SIFT feature extraction front-end through GPU acceleration, ensuring high-precision feature matching and real-time performance.

[0021] 3) In the front-end algorithm of the present invention, texture memory is used to bind the input road surface image to optimize the Gaussian filtering calculation. Candidate feature points are detected by combining the parallel non-maximum suppression algorithm with atomic operations, and a feature descriptor is formed by using Warp-level parallel acceleration. Through CUDA parallel computing, real-time processing is achieved, supporting the vehicle to run stably at a high speed of 130 km / h, and the resource occupancy meets the requirements of in-vehicle domain controllers, and the power consumption is reduced by 30% compared with similar solutions.

[0022] 4) The present invention adopts pure online positioning, without the need to pre-build a map, and realizes long-term stable positioning through real-time BA optimization and IMU correction, which is especially suitable for dynamic road environments and has wide applicability. Description of the Drawings

[0023] Figure 1 is a schematic diagram of the installation method of the ground perspective odometer; Figure 2 is a schematic flow chart of the pose positioning direction in the present invention; Figure 3 is the algorithm flow of the front-end of the feature point method; Figure 4 is a schematic diagram of the direction information of the seed point; Figure 5 is a schematic diagram of the relationship between the camera and the road markings in the back-end algorithm; Among them, 1: camera, 2: lighting device. Detailed Embodiment

[0024] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0025] Special vehicles often operate in harsh environments, and their satellite positioning signals are easily blocked or interfered with, resulting in inaccurate satellite positioning or even inability to position. Ordinary visual odometers are generally fixed on the vehicle body, with a relatively high ground clearance, and are easily affected by abnormal light, special weather, and obstacles, resulting in degradation of sensor performance, positioning errors, and blindness in abnormal scenarios. The above defects are all inherent system defects and are difficult to correct through later algorithm processing.

[0026] The ground perspective odometer includes: A vision sensor, namely a monocular / binocular camera, for capturing ground texture features (such as road edges, lane lines).

[0027] An inertial measurement unit (IMU), which provides high-frequency motion data (acceleration, angular velocity) to compensate for visual dynamic blur.

[0028] A lidar (LiDAR) (optional), which generates point cloud data for ground point segmentation and plane fitting.

[0029] The ground perspective odometer is installed under the vehicle bottom, such as Figure 1 , and can extract and process road surface information from the ground perspective through camera 1, and has the advantages of a controllable lighting device 2, a moderate and almost constant scene distance, an intuitive displacement feedback, and being independent of external occlusion. Therefore, the present invention proposes a vehicle-mounted combined pose and position method for special vehicles based on a ground perspective odometer for the configuration of special vehicles with limited satellite positioning and forward-looking odometers.

[0030] The following is an explanation of the English abbreviations involved in the present invention: Inertial Measurement Unit (IMU) Scale-Invariant Feature Transform (SIFT) Bundle Adjustment (BA) Difference of Gaussians (DoG) Warping Transformation (Warp) Levenberg-Marquardt Algorithm (L-M) Extended Kalman Filter (EKF).

[0031] Example 1 This example relates to a vehicle-mounted combined attitude and position determination method for special vehicles based on ground-view odometry, as Figure 2 , including the following steps: Step 1: Obtain the capture data of the ground-view odometer, including road surface image data and sensor data; Step 2: Based on the vehicle ground-view road vision odometer front-end algorithm of Scale-Invariant Feature Transform (SIFT) (hereinafter referred to as the front-end algorithm), extract feature points from the road surface images obtained by the ground-view odometer to form a multi-dimensional feature description vector, including the following sub-steps: 201. Construct a Difference of Gaussian (DoG) scale space: Perform Gaussian blur processing on the input road surface image data at different scales to form a multi-scale image pyramid, and obtain the DoG scale space through the difference operation of adjacent scale Gaussian blur images; 202. Detect extreme feature points: Detect local extreme points in the three-dimensional space of the DoG scale space as candidate feature points, where the three-dimensional space includes a two-dimensional image plane and a scale space dimension; 203. Screen stable feature points: Eliminate low-contrast feature points and edge response points by calculating the Hessian matrix of the candidate feature points, and retain stable feature points; 204. Determine the main direction of the feature point: Calculate the pixel gradient magnitude and direction within the neighborhood of the feature point, construct a gradient direction histogram, and determine the main direction of the feature point; 205. Generate a feature descriptor: Centered on the feature point, divide a 4×4 sub-region in the coordinate system rotated to the main direction, calculate the gradient histogram in 8 directions for each sub-region, and form a 128-dimensional feature description vector.

[0032] Algorithm acceleration and implementation based on CUDA acceleration: (1)Regarding the key link of feature extraction, innovatively adopt texture memory to bind the input image to optimize the Gaussian filtering calculation. Implement efficient extreme feature point detection through the parallel non-maximum suppression algorithm combined with atomic operations, and use Warp-level parallel acceleration to calculate the gradient histogram of the 128-dimensional SIFT descriptor, significantly improving the feature extraction speed. In the feature matching stage, use parallel KD-Tree construction and heap sorting to optimize the nearest neighbor search, reduce global memory access through register optimization, and at the same time innovatively implement the RANSAC algorithm for multi-hypothesis parallel verification, combined with the GPU acceleration of Eigen-SVD to solve the homography matrix, greatly improving the matching efficiency.

[0033] (2)In terms of memory management, comprehensively use zero-copy memory to achieve direct data transfer between CPU and GPU, optimize global memory aligned access, and multi-stream asynchronous processing technology, effectively breaking through the data transmission bottleneck and ensuring the efficient operation of the system. The system also introduces a dynamic load balancing mechanism to adaptively adjust computing resources according to texture complexity, and has functions of anomaly detection and IMU degradation mode switching, combined with GPU dynamic frequency adjustment to achieve energy efficiency optimization and ensure the stability and reliability of the system.

[0034] Step 3: The vehicle ground view visual odometry backend algorithm optimized based on Bundle Adjustment (BA) further optimizes and processes the multi-frame feature descriptors output in Step 2 to obtain the inter-frame pose estimation, including the following steps: 301. Build a sliding window optimization framework: Maintain a sliding window containing the poses of consecutive N frames of the camera and the corresponding road points, and the size of the sliding window is dynamically adjusted according to computing resources; 302. Establish a reprojection error model: For each observed road point in the window, calculate its reprojection error in the corresponding frame, and the reprojection error is defined as the Euclidean distance between the observed point position and the projected point position; 303. Build an overall optimization problem: Take the sum of the squares of the reprojection errors of all frames in the sliding window as the optimization objective function, and optimize the camera pose and the spatial coordinates of the road points at the same time; 304. Solve the non-linear optimization problem: Use the Levenberg-Marquardt (L-M) algorithm to iteratively solve the optimization problem, and re-linearize the objective function in each iteration; 305. Marginalization processing: When a new frame is added and exceeds the window size, retain the constraint information of the removed frame on the system through the marginalization method, and finally output the inter-frame pose estimation.

[0035] Step 4: Multi-sensor data fusion based on VIO loose coupling positioning, including the following steps: 401. Multi-source data fusion: Fuse the inter-frame pose estimation output in Step 3 and the raw IMU data obtained from the vehicle bus through the EKF algorithm framework to construct a multi-dimensional state vector (including position, velocity, attitude, and sensor biases). 402. Vehicle state estimation and uncertainty management based on a two-way complementary mechanism: The high-frequency raw IMU data drives the EKF prediction stage, and the state is recursively estimated through the inertial motion model to predict the future state of the vehicle; the inter-frame pose is used as the observation update, and the attitude residuals are processed using quaternion logarithmic mapping. 403. Fault tolerance design: Based on the Mahalanobis distance ( χ ² test) to achieve anomaly detection, support the degraded mode of pure vision / pure IMU, and ensure continuous positioning when a single sensor fails. 404. Optimization implementation: Control the fusion calculation time-consuming within 2 ms through sparse matrix operations and hardware acceleration (FPGA) to meet the real-time requirement of 100 Hz.

[0036] Example 2 This example also relates to a vehicle-mounted combined pose and position determination method for special vehicles based on a ground perspective odometer, including the following steps: Step 1: Obtain the capture data of the ground perspective odometer, including road surface image data and sensor data. Step 2: A method for constructing the front-end algorithm of a vehicle ground perspective road visual odometer based on the Scale-Invariant Feature Transform (SIFT), including the following sub-steps: 2001, Detection of extreme values in the DoG multi-scale space. Detect the extreme response points that exist under different Gaussian blurs, and take these points as candidate feature points.

[0037] Considering the multi-scale features of objects, use Gaussian blur kernel functions with different kernel sizes to implement the convolution operation with the image. The two-dimensional image scale space is usually expressed as: , Adopt the Difference of Gaussians (DoG) method to approximately calculate and detect the feature points that exist at different scales. There is , where is the Gaussian scale space of the image.

[0038] 2002, Delete unstable extreme points.

[0039] Unstable extreme points are candidate feature points with low contrast and response points located on the edge. The purpose of removing them is to prevent misidentification of feature points and improve robustness. For feature points on the image edge, the principal curvature value is relatively large in the direction of the vertical edge gradient, while it is relatively small along the edge direction. The DoG function of the candidate feature point The principal curvature of is proportional to the eigenvalues of the 2×2 Hessian matrix .

[0040] , where satisfies , , where and represent the trace and determinant of the matrix respectively.

[0041] Let , representing the maximum eigenvalue of the matrix H and the ratio of the minimum eigenvalue and of, then there is: , When the following formula is satisfied, this feature point is removed; otherwise, it is retained.

[0042] , where represents the time constant.

[0043] In 2003, to determine the main direction of the feature point, calculate the amplitude and phase angle of the image in the region centered on the feature point with a radius of . The modulus of the gradient of each point and the direction can be obtained through the following formula , , After obtaining the gradient direction, it is necessary to use a histogram to statistically calculate the gradient direction and amplitude corresponding to the pixels in the neighborhood of the feature point. The horizontal axis of the histogram of the gradient direction is the angle of the gradient direction, and the vertical axis is the accumulation of the gradient amplitudes corresponding to the gradient direction. The peak value in the histogram is the main direction of the feature point.

[0044] In 2004, generate feature description To ensure the rotational invariance of the feature vector, the coordinate axes need to be rotated by an angle θ (the principal direction of the feature point) around the feature point within the nearby neighborhood, that is, the coordinate axes are rotated to the principal direction of the feature point. The new coordinates of the pixels within the neighborhood after rotation are: , After rotation, we take an 8×8 window centered on the principal direction. The center of the square represents the position of the feature point, and each small grid encloses a pixel. The pixel gradient is calculated for each small grid. Therefore, each pixel corresponds to a vector, the length of which is the magnitude of the gradient, and the direction represents the direction of the gradient. Subsequently, using a Gaussian window, a weighted summation operation is performed on these vectors. Finally, with the key point as the origin, an 8-direction gradient histogram is made for the 4×4 small blocks in the four quadrants, forming 4 seed points, as Figure 4 shown on the right. Each seed point carries an information vector in 8 directions, and these 32 vectors are the final result of the 32-dimensional SIFT feature vector. The length of the feature vector is normalized to further remove the influence of illumination. Thus, the construction of the front-end algorithm for the vehicle-mounted ground-view road visual odometer based on Scale-Invariant Feature Transform (SIFT) is completed.

[0045] Step 3. Construction of the back-end algorithm based on BA (Bundle Adjustment) beam adjustment, as Figure 5 , including: 3001. Construction of the sliding window optimization framework 3001-1. Window initialization: Set the size (N) of the sliding window (such as 5 - 10 frames), and store the camera poses ( ) and the observed road points ( ) within the current window.

[0046] 3001-2. Dynamic adjustment strategy: When a new frame is added, if the window is full, remove the oldest frame and perform marginalization processing; If there are too few feature points in the window, appropriately expand the window to improve the optimization stability.

[0047] 3001-3. Definition of state variables: The variables optimized by the sliding window are: , where is the i th camera pose, i = 1, 2,... M ; is the three-dimensional coordinate of the j th road point.

[0048] 3002. Reprojection error modeling 3002-1. Projection function definition: Using the pinhole camera model, project the road landmark points onto the i frame image plane: , where is the camera intrinsic matrix; is the normalized plane projection (dehomogeneous coordinates); is the reprojection point; is the projection function.

[0049] 3002-2. Error calculation: For the matched feature points , calculate the error between it and the reprojection point : , 3002-3. Robust kernel function: Use the Huber loss function to reduce the impact of mismatches: , where is the cost function, measuring the contribution of the error; is the error; represents the unit impulse function.

[0050] 3003, Construction of the non-linear optimization problem Objective function: Minimize the sum of the reprojection errors of all observations within the sliding window: , where: is the set of valid observations; is the covariance matrix (usually take the identity matrix or adaptively adjust based on the scale of the feature points).

[0051] Optimization variables: Include the Lie algebra of the camera pose and the coordinates of the road landmark points .

[0052] 3004, Iterative solution based on the L-M algorithm 3004-1. Jacobian matrix calculation: Calculate the derivative of the error with respect to the pose and the road landmark point : , Use the chain rule and combine it with the projection model for differentiation.

[0053] All are combined into the matrix All Combine into a matrix All errors Combine into a matrix .

[0054] 3004 - 2. Solve the incremental equation: Construct the Hessian matrix and the gradient vector : , Use the L - M algorithm to solve the increment : , 3004 - 3. Sparsity acceleration: Utilize the block - sparse structure of the matrix, adopt Schur complement decomposition, first solve the pose increment, and then substitute back to solve the landmark increment.

[0055] 3005, Marginalization strategy 3005 - 1. Prior information construction: When removing the oldest frame, calculate its corresponding Mahalanobis prior constraint; construct the information matrix and the residual

[0056] 3005 - 2. Prior information retention: Incorporate and into the subsequent optimization problem to avoid discarding historical constraints.

[0057] 3005 - 3. Numerical stability processing: Adopt QR decomposition or SVD decomposition to prevent matrix singularity; if the prior information is too strong, appropriately reduce its weight to avoid over - constraint.

[0058] 3006, Algorithm output The optimized camera poses within the sliding window and the landmark coordinates , which are used for subsequent odometry pose accumulation and IMU fusion.

[0059] Step Four, Multi - sensor fusion for VIO loose - coupling positioning, including the following:[[]] 4001, System architecture design Vision sensor: Monocular camera from the ground perspective (frame rate 200 - 1000Hz) Inertial sensor: Six - axis IMU (above 200Hz) Time synchronization: Hardware - triggered synchronization or software timestamp alignment (accuracy < 1ms).

[0060] 4002, EKF Prediction Phase (driven by raw IMU data), predicts the future state of the vehicle through an inertial motion model, where the Extended Kalman Filter (EKF) is used.

[0061] The inertial motion model is represented as follows: , where is the corresponding rotation matrix, is the gravity vector, and are k the position vectors at time k +1 and and are k the velocity vectors at time k +1 and is the sampling interval, is the accelerometer measurement, is the accelerometer bias, and are k the quaternions at time k +1 and and represent the pose of the vehicle,

[0062] The covariance propagation is represented as follows: , where F is the state transition matrix of the system, which describes the dynamic behavior of the vehicle, and its specific definition depends on the specific model adopted by the vehicle; Q is the covariance matrix of the process noise; represents the covariance matrix of the prior estimate; is the motion model of the vehicle, X is the state vector, is the variance of the acceleration measurement, is the variance of the angular velocity measurement, is the variance of the accelerometer bias, is the variance of the IMU bias.

[0063] The main functions of the EKF are state estimation and uncertainty management. Through two steps of prediction and update, it estimates the pose of the vehicle and the state of environmental features. It combines sensor observation data (such as lasers, cameras, etc.) with the motion model to provide an optimal estimate.

[0064] 4003, EKF Update Phase Based on Visual Observation 4003-1. Observation Model , wherein, Z vis is the observed value, h (X) is the observation function of state prediction, y is the residual between the observed value and the predicted value.

[0065] 4003-2. Special Processing of Quaternion , wherein, q represents the corresponding quaternion, is the quaternion of the error, is the quaternion of the vis system.

[0066] 4003-3. Kalman Update , wherein, H and R are Jacobian matrices, P is the covariance obtained after update, and are noise covariance matrices, K is the Kalman gain, K y is the discrepancy of the observed value, and X is the state vector.

[0067] In addition, this method is based on an acceleration algorithm on the CUDA platform to meet the real-time requirements, including: 5001, Parallel Computing Architecture Design Decompose computationally intensive tasks such as SIFT feature extraction, feature matching, and RANSAC filtering into thread blocks that can be executed in parallel by the GPU. Each thread block is assigned a 32×32 thread grid to optimize thread scheduling and resource allocation; Adopt dynamic parallel technology to nest and call sub-kernels in the GPU kernel to reduce the CPU-GPU communication overhead.

[0068] 5002, Hierarchical Acceleration Strategy Feature Extraction Layer: Reconstruct the Gaussian pyramid construction process, adopt separable convolution optimization (horizontal / vertical convolution in parallel), and use shared memory to cache local image data to reduce global memory access latency; Feature Matching Layer: Implement KD-Tree nearest neighbor search based on CUDA, and improve the matching efficiency through register optimization and coalesced memory access; Inter-frame Estimation Layer: Parallelize the RANSAC algorithm, verify multiple pose hypothesis models simultaneously, and use atomic operations to count the number of inliers.

[0069] 5003, Memory Access Optimization Implement CPU-GPU data direct connection using zero-copy memory to avoid redundant video memory copying; Use texture memory to cache image data and utilize hardware interpolation to accelerate Gaussian filtering calculation; Align the feature descriptor matrix to 128 bytes of memory to match the single instruction multiple data stream (SIMD) instruction set (such as NVIDIA Tensor Core).

[0070] 5004, Real-time Guarantee Mechanism, including: Overlap data transmission and calculation through stream pipeline to achieve CPU-GPU asynchronous parallelism; Dynamically adjust GPU frequency and thread block size to balance power consumption and real-time requirements (target latency ≤ 5ms); The redundant calculation detection module automatically skips invalid feature extraction in low-texture areas, saving 30% of computing power.

[0071] The on-vehicle combined attitude and positioning method under the special vehicle configuration provided by the present invention involves a positioning algorithm based on a ground perspective odometer, and solves the problem of the on-vehicle combined attitude and positioning method for special vehicles under the conditions of satellite signal limitation, abnormal light, special weather, and obstacles affecting ordinary visual odometers for special vehicles. It has stronger environmental adaptability and reduces the risk of blindness. The present invention combines the ground perspective with a high-uniformity LED light source, relying only on road surface textures (such as asphalt patterns, cracks, etc.), avoiding external environmental interference, and significantly improving the robustness in complex scenarios such as strong light, weak light, and dynamic occlusion.

[0072] Existing ground perspective systems are limited by CPU computing, with a frame rate of only 10 - 50fps, making it difficult to support high-speed vehicles; while the present invention achieves real-time processing of > 70fps through CUDA parallel computing, supports the stable operation of vehicles at a high speed of 130km / h, and the resource occupancy meets the requirements of in-vehicle domain controllers, with a power consumption reduced by 30% compared to similar solutions, and excellent real-time performance, meeting the requirements of high-speed driving: Existing commercial solutions rely on high-power flashlights or pre-built maps, with high costs and complex maintenance, while the present invention effectively reduces power consumption through a low-power light source design and lightweight hardware to adapt to the installation space of the vehicle chassis, avoiding heat dissipation problems.

[0073] Existing technologies need to pre-collect road books, but the road surface is prone to map invalidation due to repair and wear; while the present invention adopts pure online positioning, without the need for a pre-built map, and achieves long-term stable positioning through real-time BA optimization and IMU correction, especially suitable for dynamic road environments, with wide applicability.

[0074] As described above, it is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of various equivalent modifications or substitutions, and these modifications or substitutions should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A vehicle-mounted combined attitude and positioning method for special vehicles based on ground perspective odometer, characterized in that The method includes the following steps: Step 1: Obtain the captured data of the ground perspective odometer, including road surface images and IMU data; Step 2: Based on the SIFT-based front-end algorithm of the vehicle ground perspective road visual odometer, extract feature points from the road surface images to form feature descriptors; Step 3: Based on the back-end algorithm of the vehicle ground perspective visual odometer optimized by BA, optimize and process the multi-frame feature descriptors output in Step 2 to obtain the inter-frame pose estimation; Step 4: Fuse the inter-frame pose estimation and IMU data in a loosely coupled manner, and predict the vehicle state and pose through the EKF algorithm.

2. The on-vehicle integrated attitude and position determination method for special vehicles based on ground perspective odometer according to claim 1, wherein Based on the front-end algorithm, the process of forming feature descriptors includes: Construct a DoG scale space based on the road surface images; Detect local extreme points in the three-dimensional space of the DoG scale space as candidate feature points; Eliminate low-contrast feature points and edge response points from the candidate feature points, and retain stable feature points; Determine the main direction of the feature points; Taking the feature points as the center, divide 4×4 sub-regions in the coordinate system rotated to the main direction of the feature points, calculate the gradient histograms in 8 directions for each sub-region, and form 128-dimensional feature descriptors.

3. A vehicle-mounted combined attitude and position determination method for special vehicles based on a ground perspective odometer according to claim 2, characterized in that Perform Gaussian blur processing on the input road surface images at different scales to form a multi-scale image pyramid, obtain the three-dimensional space of the DoG scale space through the differential operation of adjacent scale Gaussian blurred images, and use the differential Gaussian DoG method to approximately calculate and detect feature points existing at different scales as candidate feature points.

4. A vehicle-mounted combined attitude and positioning method for special vehicles based on a ground perspective odometer according to claim 2, characterized in that, The specific retention of stable feature points is: DoG function of candidate feature points The principal curvature of is proportional to the eigenvalues of the 2×2 Hessian matrix H Let the maximum eigenvalue and the minimum eigenvalue of the matrix be ， ， Among them, and represent the trace and determinant of the matrix respectively; Let , represent the ratio of the maximum eigenvalue and the minimum eigenvalue . Then we have: ， When the following formula is satisfied, eliminate the feature point; otherwise, retain it: ， Among them, represents the time constant.

5. A vehicle-mounted combined attitude and positioning method for special vehicles based on a ground perspective odometer according to claim 1, characterized in that In the front-end algorithm, texture memory is bound to the input road surface images to optimize the Gaussian filtering calculation, detect candidate feature points through the parallel non-maximum suppression algorithm combined with atomic operations, and use Warp-level parallel acceleration to form feature descriptors.

6. A vehicle-mounted combined attitude and positioning method for special vehicles based on a ground perspective odometer according to claim 1, characterized in that, Based on the back-end algorithm, the process of obtaining the inter-frame pose estimation includes: Construction of a sliding window optimization framework: Set the sliding window size to N, and store the camera pose and observed road points within the current window; Project the road punctuation to the i frame image plane, and for the matched feature points, calculate the reprojection error between them and the reprojected points; Construction of a non-linear optimization problem: The optimization objective is to minimize the sum of the reprojection errors of all observations within the sliding window, and the optimization variables include the Lie algebra of the camera pose and the coordinates of the road points; Iteratively solve the non-linear optimization problem based on the L-M algorithm to obtain the optimized camera pose and road point coordinates within the sliding window.

7. A vehicle-mounted combined attitude and position determination method for special vehicles based on a ground perspective odometer according to claim 6, characterized in that, For the reprojection error, use the Huber loss function to reduce the influence of mismatches: ， Among them, is the cost function, which measures the contribution of the error; is the error of the i -th camera pose at the j -th landmark point; represents the unit impulse function.

8. A vehicle-mounted combined attitude and positioning method for special vehicles based on a ground perspective odometer according to claim 6, characterized in that, The iterative solution of the non-linear optimization problem based on the L-M algorithm includes: Calculate the errors separately for the pose and the landmark derivatives to form the corresponding Jacobian matrices and : ， All are combined into a matrix ; all are combined into a matrix ; all errors are combined into a matrix ; Construct the Hessian matrix and the gradient vector : ， Solve the increment using the L-M algorithm : ， Among them, is the identity matrix; Utilize With the block sparse structure of the matrix, Schur complement decomposition is adopted to first solve for the pose increment and then substitute back to solve for the landmark increment.

9. A vehicle-mounted combined attitude and position determination method for special vehicles based on a ground perspective odometer according to claim 1, characterized in that, The process of predicting the vehicle state and pose through the EKF algorithm includes: The EKF prediction stage driven by the IMU raw data: Predict the future state of the vehicle through the inertial motion model; The EKF update stage based on visual observations: The inter-frame pose is used as an observation update, and the attitude residual is processed using the quaternion logarithmic mapping.

10. A vehicle-mounted combined attitude and positioning method for special vehicles based on a ground perspective odometer according to claim 9, characterized in that, The representation of the inertial motion model is as follows: ， Among them, is the corresponding rotation matrix, is the gravity vector, and are respectively k the position vectors at time k +1 and and are respectively k the velocity vectors at time k +1 and is the sampling interval, is the accelerometer measurement value, is the accelerometer bias, and are respectively k the quaternions at time k +1 and and are respectively the IMU measurement value and the IMU bias; The covariance propagation is represented as follows: ， Among them, F is the state transition matrix of the system, describing the dynamic behavior of the vehicle; Q is the covariance matrix of the process noise; represents the covariance matrix of the prior estimate; is the motion model of the vehicle, X is the state vector, is the variance of the acceleration measurement, is the variance of the angular velocity measurement, is the variance of the accelerometer bias, is the variance of the IMU bias.

Images