A method for determining the attitude and positioning of special vehicles based on ground-view odometry

Abstract

This invention provides a vehicle posture determination and positioning method, particularly a vehicle-mounted combined posture determination and positioning method for special vehicles based on a ground-view odometry. The method comprises: acquiring data captured by the ground-view odometry, including road surface images and IMU data; extracting feature points from the road surface images using a SIFT-based front-end algorithm for the vehicle-based ground-view highway visual odometry to form feature descriptors; optimizing and processing multi-frame feature descriptors using a BA-optimized back-end algorithm for the vehicle-based ground-view highway visual odometry to obtain inter-frame pose estimates; and fusing the inter-frame pose estimates with the IMU data using a loosely coupled approach to predict the vehicle's state and posture using an EKF algorithm. Compared to existing technologies, this invention offers advantages such as achieving real-time and accurate posture positioning for special vehicles.

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:

[0004] 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.

[0005] Cumulative error control and blinding tolerance: Due to the inherent limitations of image sensors, each frame of the solution is subject to the accumulation of small errors. Therefore, algorithms are needed to ensure that the error does not diverge over time within the vehicle's limited mileage.

[0006] Robust image processing algorithm for ground texture: Due to the special perspective of the ground-based odometry, it is necessary to develop an algorithm that can fully extract and process the edge feature information of the road texture, find the features that are truly ground-based, static, and time-invariant between frames, and ensure that they are not lost due to the movement, rotation, and shaking of the vehicle.

[0007] After searching, Chinese invention patent application publication number CN110207714A discloses a method, vehicle-mounted system, and vehicle for determining vehicle posture. The method comprises: obtaining first inertial navigation information and first odometer information from the vehicle, wherein the first inertial navigation information includes the vehicle's posture, speed, and position change information; determining a first current estimated posture of the vehicle based on the first inertial navigation information and the first odometer information; obtaining extrinsic parameters of N monocular cameras of the vehicle, where N is an integer greater than 1; fusing an inertial navigation odometry method with a visual SLAM algorithm for each monocular camera based on the N monocular camera extrinsics and the first current estimated posture to estimate the current posture of the N monocular cameras; performing coordinate transformation on the current postures of the N monocular cameras to obtain N transformed postures of the vehicle; and obtaining a fused posture of the vehicle based on the N transformed postures. This existing patent application suffers from insufficient posture accuracy and real-time performance.

[0008] How to achieve real-time and accurate posture positioning of special vehicles based on ground-view odometry has become a technical problem that needs to be solved. Summary of the Invention

[0009] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a special vehicle-mounted combination attitude determination and positioning method based on a ground perspective odometer.

[0010] The purpose of the present invention can be achieved by the following technical solutions:

[0011] According to one aspect of the present invention, a method for determining the attitude and positioning of a vehicle-mounted combination of a special vehicle based on a ground view odometer is provided, the method comprising the following steps:

[0012] Step 1: Obtain the captured data of the ground-view odometry, including road images and IMU data;

[0013] Step 2: A SIFT-based vehicle-based ground-view highway visual odometry front-end algorithm extracts feature points from the road surface image to form feature descriptors;

[0014] Step 3: Based on the BA-optimized vehicle-use ground-view visual odometry backend algorithm, the multi-frame feature descriptors output in step 2 are optimized and processed to obtain inter-frame pose estimation;

[0015] 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.

[0016] Preferably, based on the front-end algorithm, the process of forming a feature descriptor includes:

[0017] Construct DoG scale space based on road surface images;

[0018] Detect local extreme points in the three-dimensional space of DoG scale space as candidate feature points;

[0019] Eliminate low-contrast feature points and edge response points from candidate feature points and retain stable feature points;

[0020] Determine the main direction of the feature points;

[0021] With the feature point as the center, the coordinate system rotated to the main direction of the feature point is divided into 4×4 sub-regions. The gradient histograms in 8 directions are calculated for each sub-region to form a 128-dimensional feature descriptor.

[0022] More preferably, the input road surface image is subjected to Gaussian blur processing at different scales to form a multi-scale image pyramid, and the three-dimensional space of the DoG scale space is obtained by differential operation of Gaussian blurred images of adjacent scales. The differential Gaussian DoG method is used to approximately calculate and detect feature points that exist at different scales as candidate feature points.

[0023] More preferably, the retained stable feature points are specifically:

[0024] DoG function of candidate feature points The principal curvature and 2×2 Hessian matrix The eigenvalues ​​are proportional to , let the matrix H The maximum and minimum eigenvalues ​​of ,but

[0025] ,

[0026] ,

[0027] in, and Represents matrices respectively The trace and determinant of ;

[0028] set up , represents the maximum eigenvalue and the minimum eigenvalue The ratio of , then:

[0029] ,

[0030] When the following equation is satisfied, the feature point is removed; otherwise, it is retained:

[0031] ,

[0032] in, Represents the time constant.

[0033] Preferably, in the front-end algorithm, texture memory is used to bind the input road surface image to optimize Gaussian filtering calculation, candidate feature points are detected by a parallel non-maximum suppression algorithm combined with atomic operations, and Warp-level parallel acceleration is used to form feature descriptors.

[0034] Preferably, the process of obtaining inter-frame pose estimation based on the back-end algorithm includes:

[0035] Sliding window optimization framework construction: Set the sliding window size to N and store the camera pose and observed landmarks in the current window;

[0036] Project the landmark points to i In the frame image plane, for the matched feature points, the reprojection error between them and the reprojection points is calculated;

[0037] Nonlinear optimization problem construction: 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 landmark points.

[0038] The nonlinear optimization problem is solved iteratively based on the LM algorithm to obtain the optimized camera pose and landmark point coordinates in the sliding window.

[0039] More preferably, for the reprojection error, the Huber loss function is used to reduce the impact of mismatching:

[0040] ,

[0041] in, is the cost function, which measures the contribution of the error; For the i The camera pose is j The error of the landmark points; represents the unit impulse function.

[0042] Preferably, the iterative solution to the nonlinear optimization problem based on the LM algorithm includes:

[0043] Calculate the error separately Position and waypoints The derivative of , forming the corresponding Jacobian matrix and :

[0044] ,

[0045] All Combined into a matrix , all Combined into a matrix , all errors Combined into a matrix ;

[0046] Constructing the Hessian matrix and the gradient vector :

[0047] ,

[0048] Use LM algorithm to solve the increment :

[0049] ,

[0050] in, is the identity matrix;

[0051] use The block sparse structure of the matrix uses Schur complement decomposition to first solve the pose increment and then back-substitute to solve the landmark point increment.

[0052] Preferably, the process of predicting the vehicle state and posture by using the EKF algorithm includes:

[0053] EKF prediction phase driven by IMU raw data: predicting the future state of the vehicle through the inertial motion model;

[0054] EKF update stage based on visual observation: the inter-frame pose is used as the observation update, and the quaternion logarithmic mapping is used to process the pose residual.

[0055] More preferably, the inertial motion model is expressed as follows:

[0056] ,

[0057] in, for The corresponding rotation matrix, is the gravity vector, and They are k +1 moment and k The position vector at time, and They are k +1 moment and k The velocity vector at time t, is the sampling interval, is the accelerometer measurement value, is the accelerometer bias, and They are k +1 moment and k The quaternion at the moment represents the vehicle's position and posture. and are IMU measurement value and IMU bias respectively;

[0058] Covariance propagation is expressed as follows:

[0059] ,

[0060] in, 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 vehicle's motion model, X is the state vector, is the acceleration measurement variance, is the angular velocity measurement variance, is the variance of the accelerometer bias, is the variance of the IMU bias.

[0061] Compared with the prior art, the present invention has the following beneficial effects:

[0062] 1) This invention ensures high-precision feature matching by optimizing the SIFT feature extraction front-end. It also combines a sliding window BA nonlinear optimization back-end to suppress cumulative errors, outperforming the traditional EKF filtering method. Finally, it fuses IMU data in a loosely coupled manner to compensate for the insufficient pose estimation of pure visual odometry during rapid motion or brief occlusion. This improves the accuracy of vehicle positioning.

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

[0064] 3) The front-end algorithm of the present invention uses texture memory to bind the input road image to optimize Gaussian filtering calculations, detects candidate feature points through a parallel non-maximum suppression algorithm combined with atomic operations, and uses Warp-level parallel acceleration to form feature descriptors. Through CUDA parallel computing, real-time processing is achieved, supporting stable vehicle operation at a high speed of 130 km / h. The resource usage meets the requirements of the on-board domain controller, and the power consumption is reduced by 30% compared with similar solutions.

[0065] 4) This invention adopts pure online positioning, without the need for pre-built maps, and achieves long-term stable positioning through real-time BA optimization and IMU correction. It is particularly suitable for dynamic road environments and has wide applicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 This is a schematic diagram of the installation method of the ground-view odometer;

[0067] Figure 2 It is a schematic diagram of the process of determining the posture and positioning in the present invention;

[0068] Figure 3 It is the algorithm flow of the front end of the feature point method;

[0069] Figure 4 It is a schematic diagram of the direction information of the seed point;

[0070] Figure 5 This is a schematic diagram of the relationship between the camera and landmark points in the backend algorithm;

[0071] Among them, 1: camera, 2: lighting device. DETAILED DESCRIPTION

[0072] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0073] Specialized vehicles often operate in harsh environments, where their satellite positioning signals are easily blocked or interfered with, resulting in inaccurate or even nonexistent positioning. Conventional visual odometry sensors are typically mounted high above the vehicle, making them susceptible to abnormal lighting, weather conditions, and obstructions. This can degrade sensor performance, lead to positioning errors, and even blindness in unusual scenarios. These inherent systemic flaws are difficult to correct through post-processing algorithms.

[0074] Ground-level odometry includes:

[0075] Visual sensors, i.e. monocular / binocular cameras, are used to capture ground texture features (such as road edges and lane lines).

[0076] Inertial Measurement Unit (IMU), which provides high-frequency motion data (acceleration, angular velocity) to compensate for visual motion blur.

[0077] Laser Radar (LiDAR) (optional) generates point cloud data for ground point segmentation and plane fitting.

[0078] The ground view odometer is installed on the bottom of the vehicle. Figure 1 , capable of extracting and processing road surface information from a ground-level perspective using a camera 1, and possessing advantages such as a controllable lighting device 2, a moderate and nearly constant scene distance, intuitive displacement feedback, and independence from external occlusions. Therefore, the present invention proposes an on-board combined attitude determination and positioning method for special vehicles with limited satellite positioning and heads-up odometers, based on a ground-level perspective odometer.

[0079] The English abbreviations involved in the present invention are explained below:

[0080] Inertial Measurement Unit (IMU)

[0081] Scale-Invariant Feature Transform (SIFT)

[0082] Bundle Adjustment (BA)

[0083] Difference of Gaussians (DoG)

[0084] Image Warping Transformation (Warp)

[0085] Levenberg-Marquardt algorithm (LM)

[0086] Extended Kalman Filter (EKF).

[0087] Example 1

[0088] This embodiment relates to a method for determining the attitude and positioning of a special vehicle based on a ground-view odometer. Figure 2 , including the following steps:

[0089] Step 1: Obtain the captured data of the ground view odometer, including road image data and sensor data;

[0090] Step 2: The front-end algorithm for the vehicle-based ground-view highway visual odometry (hereinafter referred to as the front-end algorithm) based on the scale-invariant feature transform (SIFT) extracts feature points from the road surface image obtained by the ground-view odometry to form a multi-dimensional feature description vector. This algorithm includes the following sub-steps:

[0091] 201. Constructing a Difference of Gaussian (DoG) scale space: Gaussian blurring of input road surface image data at different scales is performed to form a multi-scale image pyramid. The DoG scale space is then obtained by performing a differential operation on Gaussian blurred images at adjacent scales.

[0092] 202. Detecting extreme feature points: Detecting local extreme points as candidate feature points in the three-dimensional space of the DoG scale space, where the three-dimensional space includes the two-dimensional image plane and the scale space dimension;

[0093] 203. Screening stable feature points: By calculating the Hessian matrix of candidate feature points, low-contrast feature points and edge response points are eliminated, and stable feature points are retained;

[0094] 204. Determine the main direction of the feature point: Calculate the pixel gradient magnitude and direction within the feature point neighborhood, construct a gradient direction histogram, and determine the main direction of the feature point;

[0095] 205. Generate feature descriptors: Divide the feature point into 4×4 subregions in a coordinate system rotated to the main direction. Calculate the gradient histogram of each subregion in 8 directions to form a 128-dimensional feature description vector.

[0096] Algorithm acceleration and implementation based on CUDA acceleration:

[0097] (1) For the key link of feature extraction, we innovatively use texture memory to bind the input image to optimize Gaussian filter calculations. We achieve efficient extreme feature point detection through a parallel non-maximum suppression algorithm combined with atomic operations. We also use Warp-level parallelism to accelerate the gradient histogram calculation of the 128-dimensional SIFT descriptor, significantly improving the speed of feature extraction. In the feature matching stage, we use parallel KD-Tree construction and heap sorting to optimize the nearest neighbor search. We reduce global memory access through register optimization, and innovatively implement the RANSAC algorithm for multi-hypothesis parallel verification. We combine Eigen-SVD with GPU acceleration to solve the homography matrix, which greatly improves the matching efficiency.

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

[0099] Step 3: Based on the beam alignment (BA) optimized vehicle ground view visual odometry backend algorithm, the multi-frame feature descriptors output in step 2 are further optimized and processed to obtain the inter-frame pose estimation, which includes the following steps:

[0100] 301. Build a sliding window optimization framework: Maintain a sliding window containing N consecutive frames of camera poses and corresponding landmarks. The sliding window size is dynamically adjusted based on computing resources.

[0101] 302. Establish a reprojection error model: For each observed landmark point within the window, calculate its reprojection error in the corresponding frame. The reprojection error is defined as the Euclidean distance between the observed point position and the projected point position.

[0102] 303. Constructing an overall optimization problem: Taking the sum of squared reprojection errors of all frames within the sliding window as the optimization objective function, simultaneously optimizing the camera pose and landmark spatial coordinates;

[0103] 304. Solving nonlinear optimization problems: Using the Levenberg-Marquardt (LM) algorithm to iteratively solve the optimization problem, relinearizing the objective function at each iteration;

[0104] 305. Marginalization: When a new frame exceeds the window size, the constraints imposed by the removed frame on the system are retained through marginalization, and the inter-frame pose estimation is finally output.

[0105] Step 4: Multi-sensor data fusion based on VIO loosely coupled positioning, including the following steps:

[0106] 401. Multi-source data fusion: The inter-frame pose estimate output from step 3 is fused with the raw IMU data acquired on the vehicle bus through the EKF algorithm framework to construct a multi-dimensional state vector (including position, velocity, attitude, and sensor bias).

[0107] 402. Vehicle state estimation and uncertainty management based on a bidirectional complementary mechanism: High-frequency IMU raw data drives the EKF prediction phase, which infers the state through the inertial motion model to predict the vehicle's future state. Inter-frame pose is used as observation update, and quaternion logarithmic mapping is used to process pose residuals.

[0108] 403. Fault Tolerance Design: Based on Mahalanobis Distance ( χ ² Inspection) realizes anomaly detection and supports pure vision / pure IMU degradation mode to ensure continuous positioning when a single sensor fails;

[0109] 404. Optimized implementation: Through sparse matrix operations and hardware acceleration (FPGA), the fusion calculation time is controlled within 2ms, meeting the 100Hz real-time requirement.

[0110] Example 2

[0111] This embodiment also relates to a method for determining the attitude and positioning of a vehicle-mounted combination of a special vehicle based on a ground-view odometer, comprising the following steps:

[0112] Step 1: Obtain the captured data of the ground view odometer, including road image data and sensor data;

[0113] Step 2: A method for constructing a front-end algorithm for a vehicle-based ground-view highway visual odometry based on scale-invariant feature transform (SIFT) includes the following sub-steps:

[0114] 2001, DoG multi-scale spatial extreme value detection. Detect extreme response points that exist under different Gaussian blurs and use these points as candidate feature points.

[0115] Taking into account the multi-scale characteristics of the object, Gaussian blur kernel functions with different kernel sizes are used. Convolution operation with image is implemented. Two-dimensional image Scale space Usually expressed as:

[0116] ,

[0117] The differential Gaussian DoG method is used to approximate and detect feature points that exist at different scales.

[0118] ,

[0119] in, is the Gaussian scale space of the image.

[0120] 2002, Deleting unstable extreme points.

[0121] Unstable extreme points are response points with low contrast and located at the edge of the candidate feature points. The purpose of deleting them is to prevent feature point misidentification and improve robustness. At the feature points on the edge of the image, the principal curvature value is larger in the direction perpendicular to the edge gradient, while the principal curvature value is smaller along the edge direction. DoG function of candidate feature points The principal curvature and 2×2 Hessian matrix is proportional to the eigenvalue of .

[0122] ,

[0123] in, satisfy

[0124] ,

[0125] ,

[0126] in, and Represents matrices respectively The trace and determinant of .

[0127] set up , represents the matrix H The maximum eigenvalue of and the minimum eigenvalue The ratio of , then:

[0128] ,

[0129] When the following equation is satisfied, the feature point is removed; otherwise, it is retained.

[0130] ,

[0131] in, Represents the time constant.

[0132] 2003, the main direction of the feature point is calculated with the feature point as the center, The argument and magnitude of the image of the area with radius The modulus of the gradient and direction It can be obtained by the following formula

[0133] ,

[0134] ,

[0135] After obtaining the gradient direction, we need to use a histogram to calculate the gradient direction and magnitude of the pixels in the neighborhood of the feature point. The horizontal axis of the gradient direction histogram is the angle of the gradient direction, and the vertical axis is the accumulation of the gradient magnitudes corresponding to the gradient direction. The peak of the histogram is the main direction of the feature point.

[0136] 2004, Generative Feature Description

[0137] In order to ensure the rotation invariance of the feature vector, the coordinate axis is rotated by θ (the main direction of the feature point) in the neighborhood around the feature point. That is, the coordinate axis is rotated to the main direction of the feature point. The new coordinates of the pixels in the neighborhood after rotation are:

[0138] ,

[0139] After rotation, we take an 8×8 window centered on the main direction. The center of the square represents the location of the feature point. Each small grid contains a pixel. The pixel gradient is calculated for each small grid, so each pixel corresponds to a vector whose length is the magnitude of the gradient and whose direction represents the direction of the gradient. Then, a weighted sum operation is performed on these vectors using a Gaussian window. Finally, with the key point as the origin, the gradient histograms in eight directions are generated for the 4×4 blocks in the four quadrants to form four seed points, as shown in the following example. Figure 4 As shown on the right, each seed point carries information vectors in eight directions. These 32 vectors form the final 32-dimensional SIFT feature vector. The feature vector lengths are normalized to further eliminate the effects of lighting. This completes the construction of the front-end algorithm for vehicle-based ground-view highway visual odometry based on the scale-invariant feature transform (SIFT).

[0140] Step 3: Build the backend algorithm based on BA (Bundle Adjustment), such as Figure 5 ,include:

[0141] 3001, Sliding Window Optimization Framework Construction

[0142] 3001-1. Window initialization: Set the sliding window size (N) (e.g. 5~10 frames), store the camera pose in the current window ( ) and observed landmarks ( ).

[0143] 3001-2. Dynamic Adjustment Strategy:

[0144] When a new frame is added, if the window is full, the oldest frame is removed and marginalization is performed;

[0145] If there are too few feature points in the window, the window should be expanded appropriately to improve optimization stability.

[0146] 3001-3. State variable definition: The sliding window optimization variables are:

[0147] ,

[0148] in For the i camera poses, i =1,2,…… M ; For the j The three-dimensional coordinates of landmark points.

[0149] 3002, Reprojection Error Modeling

[0150] 3002-1. Projection function definition: Using the pinhole camera model, the landmark points Projection to i Frame image plane:

[0151] ,

[0152] in, is the camera intrinsic parameter matrix; is the normalized plane projection (removing homogeneous coordinates); is the reprojection point; is the projection function.

[0153] 3002-2. Error calculation: for matched feature points , calculate its difference with the reprojection point Error :

[0154] ,

[0155] 3002-3. Robust kernel function: Huber loss function is used to reduce the impact of mismatching:

[0156] ,

[0157] in, is the cost function, which measures the contribution of the error; is the error; represents the unit impulse function.

[0158] 3003, Nonlinear Optimization Problem Construction

[0159] Objective function: Minimize the sum of the reprojection errors of all observations within the sliding window:

[0160] ,

[0161] in: is the valid observation set; is the covariance matrix (usually the unit matrix or adaptively adjusted based on the feature point scale).

[0162] Optimization variables: including camera pose Lie algebra and landmark coordinates .

[0163] 3004, iterative solution based on LM algorithm

[0164] 3004-1. Jacobian matrix calculation:

[0165] Calculation error Position and waypoints The derivative of :

[0166] ,

[0167] The chain rule is used in combination with the projection model to obtain the derivative.

[0168] All Combined into a matrix , all Combined into a matrix , all errors Combined into a matrix .

[0169] 3004-2. Solving the incremental equation:

[0170] Constructing the Hessian matrix and the gradient vector :

[0171] ,

[0172] Use LM algorithm to solve the increment :

[0173] ,

[0174] 3004-3. Sparsity Acceleration:

[0175] use The block sparse structure of the matrix uses Schur complement decomposition to first solve the pose increment and then back-substitute to solve the landmark point increment.

[0176] 3005, Marginalization Strategy

[0177] 3005-1. Prior Information Construction: When removing the oldest frame, calculate the corresponding Mahalanobis prior constraint; construct the information matrix and residuals

[0178] 3005-2. Prior information retention: and Add subsequent optimization problems to avoid discarding historical constraints.

[0179] 3005-3. Numerical stability processing:

[0180] Use QR decomposition or SVD decomposition to prevent matrix singularity; if the prior information is too strong, appropriately reduce its weight to avoid over-constraint.

[0181] 3006, algorithm output

[0182] Optimized camera pose within the sliding window and landmark coordinates , used for subsequent odometer pose accumulation and IMU fusion.

[0183] Step 4: Multi-sensor fusion for VIO loosely coupled positioning, including the following:

[0184] 4001, System Architecture Design

[0185] Visual sensor: Ground-view monocular camera (200-1000Hz frame rate)

[0186] Inertial sensor: six-axis IMU (above 200Hz)

[0187] Time synchronization: Hardware trigger synchronization or software timestamp alignment (accuracy <1ms).

[0188] 4002, EKF prediction stage (driven by IMU raw data), predicts the future state of the vehicle through the inertial motion model, including the Extended Kalman Filter (EKF).

[0189] The inertial motion model is expressed as follows:

[0190] ,

[0191] in, for The corresponding rotation matrix, is the gravity vector, and They are k +1 moment and k The position vector at time, and They are k +1 moment and k The velocity vector at time t, is the sampling interval, is the accelerometer measurement value, is the accelerometer bias, and They are k +1 moment and k The quaternion at the moment represents the vehicle's position and posture. and are the IMU measurements and IMU bias respectively.

[0192] Covariance propagation is expressed as follows:

[0193] ,

[0194] in, F It is the state transfer matrix of the system, describing the dynamic behavior of the vehicle. Its specific definition depends on the specific model used for the vehicle. Q is the covariance matrix of the process noise; represents the covariance matrix of the prior estimate; is the vehicle's motion model, X is the state vector, is the acceleration measurement variance, is the angular velocity measurement variance, is the variance of the accelerometer bias, is the variance of the IMU bias.

[0195] The EKF's primary functions are state estimation and uncertainty management. Through two steps, prediction and update, it estimates the vehicle's pose and the state of environmental features. It combines sensor observations (such as lasers and cameras) with motion models to provide an optimal estimate.

[0196] 4003, EKF update phase based on visual observation

[0197] 4003-1. Observation Model

[0198] ,

[0199] in, Z vis is the observed value, h (X) is the observation function for state prediction, y is the residual between the observed and predicted values.

[0200] 4003-2. Special Processing of Quaternions

[0201] ,

[0202] in, q Represents the corresponding quaternion, is the quaternion of the error, is the quaternion of the vis system.

[0203] 4003-3. Kalman Update

[0204] ,

[0205] Among them, H, R are Jacobian matrices, P is the covariance obtained after the update, and is the noise covariance matrix, K is the Kalman gain, K y is the variance of the observations, and X is the state vector.

[0206] In addition, the method is based on the acceleration algorithm of the CUDA platform to meet the real-time requirements, including:

[0207] 5001, Parallel Computing Architecture Design

[0208] Decompose computationally intensive tasks such as SIFT feature extraction, feature matching, and RANSAC filtering into GPU-parallelizable thread blocks, allocating a 32×32 thread grid to each thread block to optimize thread scheduling and resource allocation.

[0209] Dynamic parallel technology is used to nest sub-kernels in GPU kernels to reduce CPU-GPU communication overhead.

[0210] 5002, Layered Acceleration Strategy

[0211] Feature extraction layer: Restructures the Gaussian pyramid construction process, adopts separable convolution optimization (horizontal and vertical convolution in parallel), and uses shared memory to cache local image data to reduce global memory access latency.

[0212] Feature matching layer: Implements CUDA-based KD-Tree nearest neighbor search and improves matching efficiency through register optimization and coalesced memory access.

[0213] Inter-frame estimation layer: Parallelizes the RANSAC algorithm, verifies multiple pose hypothesis models simultaneously, and uses atomic operations to count the number of inliers.

[0214] 5003, memory access optimization

[0215] Zero-Copy Memory is used to achieve direct data flow between CPU and GPU, avoiding redundant copies of video memory.

[0216] Texture Memory caches image data and uses hardware interpolation to accelerate Gaussian filter calculations;

[0217] The feature descriptor matrix is ​​aligned to 128 bytes in memory to match the Single Instruction Multiple Data (SIMD) instruction set (such as NVIDIA Tensor Core).

[0218] 5004, real-time guarantee mechanism, including:

[0219] Through stream processing (Stream Pipeline), data transmission and calculation are overlapped to achieve CPU-GPU asynchronous parallelism;

[0220] Dynamically adjust GPU frequency and thread block size to balance power consumption and real-time requirements (target latency ≤ 5ms);

[0221] The redundant calculation detection module automatically skips invalid feature extraction in low-texture areas, saving 30% of computing power.

[0222] The method for onboard combined attitude determination and positioning for special vehicle configurations, provided by this invention, involves a positioning algorithm based on a ground-view odometry. This method addresses the challenges of special vehicles operating under conditions where satellite signals are limited and conventional visual odometry is affected by abnormal lighting, weather conditions, and obstructions. This method offers enhanced environmental adaptability and reduces the risk of blinding. By combining a ground-view with a highly uniform LED light source, the method relies solely on road surface texture (such as asphalt grain and cracks), avoiding external environmental interference and significantly improving robustness in complex scenarios such as strong and weak light conditions and dynamic occlusion.

[0223] Existing ground-view systems are limited by CPU computing, with a frame rate of only 10-50fps, making them difficult to support high-speed vehicles. However, this invention uses CUDA parallel computing to achieve real-time processing of >70fps, supporting stable vehicle operation at speeds of 130km / h. Its resource usage meets the requirements of the on-board domain controller, and its power consumption is 30% lower than similar solutions. It offers excellent real-time performance and meets the needs of high-speed driving.

[0224] Existing commercial solutions rely on high-power flashlights or pre-built maps, which are costly and complex to maintain. However, this invention effectively reduces power consumption and avoids heat dissipation issues through low-power light source design and lightweight hardware that adapts to the installation space of the vehicle chassis.

[0225] The existing technology requires the pre-collection of road books, but the road surface is easily damaged by repairs and wear, which can lead to map failure. The present invention adopts pure online positioning, without the need for pre-built maps, and achieves long-term stable positioning through real-time BA optimization and IMU correction. It is particularly suitable for dynamic road environments and has wide applicability.

[0226] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and such modifications or substitutions are intended to be within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be subject to the scope of protection of the claims.

Claims

1. A method for determining the attitude and positioning of a special vehicle based on a ground-view odometer, characterized in that: The method comprises the following steps: Step 1: Obtain the captured data of the ground-view odometry, including road images and IMU data; Step 2: A SIFT-based vehicle-based ground-view highway visual odometry front-end algorithm extracts feature points from the road surface image to form feature descriptors; Step 3: Based on the BA-optimized vehicle-use ground-view visual odometry backend algorithm, the multi-frame feature descriptors output in step 2 are optimized and processed to obtain 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 using the EKF algorithm; Based on the front-end algorithm, the process of forming a feature descriptor includes: Construct DoG scale space based on road surface images; Detect local extreme points in the three-dimensional space of DoG scale space as candidate feature points; Eliminate low-contrast feature points and edge response points from candidate feature points and retain stable feature points; Determine the main direction of the feature points; With the feature point as the center, the coordinate system is rotated to the main direction of the feature point and divided into 4×4 sub-regions. The gradient histograms in 8 directions are calculated for each sub-region to form a 128-dimensional feature descriptor. In the front-end algorithm, texture memory is used to bind the input road image to optimize Gaussian filtering calculations, a parallel non-maximum suppression algorithm is combined with atomic operations to detect candidate feature points, and warp-level parallel acceleration is used to form feature descriptors. The process of obtaining inter-frame pose estimation based on the backend algorithm includes: Sliding window optimization framework construction: Set the sliding window size to N and store the camera pose and observed landmarks in the current window; Project the landmark points to i In the frame image plane, for the matched feature points, the reprojection error between them and the reprojection points is calculated; Nonlinear optimization problem construction: 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 landmark points. The Huber loss function is used to reduce the impact of mismatching on the reprojection error. Based on the iterative solution of the nonlinear optimization problem of LM algorithm, the camera pose and landmark coordinates in the optimized sliding window are obtained; including the use of Hessian matrix The block sparse structure uses Schur complement decomposition to first solve the pose increment and then back-substitute to solve the landmark point increment. The process of predicting the vehicle state and posture by the EKF algorithm includes: EKF prediction phase driven by IMU raw data: predicting the future state of the vehicle through the inertial motion model; EKF update stage based on visual observation: the inter-frame pose is used as the observation update, and the quaternion logarithmic mapping is used to process the pose residual.

2. The method for determining the attitude and positioning of a special vehicle based on a ground-view odometer according to claim 1, characterized in that: The input road image is processed with Gaussian blur at different scales to form a multi-scale image pyramid. The three-dimensional space of the DoG scale space is obtained by differential operation of Gaussian blurred images of adjacent scales. The differential Gaussian DoG method is used to approximately calculate and detect feature points that exist at different scales as candidate feature points.

3. The method for determining the attitude and positioning of a special vehicle based on a ground-view odometer according to claim 1, characterized in that: The retained stable feature points are specifically: DoG function of candidate feature points The principal curvature and 2×2 Hessian matrix The eigenvalues ​​are proportional to , let the matrix H The maximum and minimum eigenvalues ​​of ,but ， ， in, and Represents matrices respectively The trace and determinant of ; set up , represents the maximum eigenvalue and the minimum eigenvalue The ratio of , then: ， When the following equation is satisfied, the feature point is removed; otherwise, it is retained: ， in, Represents the time constant.

4. The method for determining the attitude and positioning of a special vehicle based on a ground-view odometer according to claim 1, characterized in that: For the reprojection error, the Huber loss function is used to reduce the impact of mismatching: ， in, is the cost function, which measures the contribution of the error; For the i The camera pose is j The error of the landmark points; represents the unit impulse function.

5. The method for determining the attitude and positioning of a special vehicle based on a ground-view odometer according to claim 1, characterized in that: The iterative solution to the nonlinear optimization problem based on the LM algorithm includes: Calculate the error separately Position and waypoints The derivative of , forming the corresponding Jacobian matrix and : ， All Combined into a matrix , all Combined into a matrix , all errors Combined into a matrix ; Constructing the Hessian matrix and the gradient vector : ， Use LM algorithm to solve the increment : ， in, is the identity matrix.

6. The method for determining the attitude and positioning of a special vehicle based on a ground-view odometer according to claim 1, characterized in that: The inertial motion model is expressed as follows: ， in, for The corresponding rotation matrix, is the gravity vector, and They are k +1 moment and k The position vector at time, and They are k +1 moment and k The velocity vector at time t, is the sampling interval, is the accelerometer measurement value, is the accelerometer bias, and They are k +1 moment and k The quaternion at the moment represents the vehicle's position and posture. and are IMU measurement value and IMU bias respectively; Covariance propagation is expressed as follows: ， in, 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 vehicle's motion model, X is the state vector, is the acceleration measurement variance, is the angular velocity measurement variance, is the variance of the accelerometer bias, is the variance of the IMU bias.

Images