Lightweight vehicle pose estimation method based on adaptive neighborhood clustering and information physics fusion

Abstract

The application belongs to the technical field of intelligent traffic and automatic driving perception, and discloses a lightweight vehicle pose estimation method based on adaptive neighborhood clustering and information physical fusion, which comprises the following steps: S1. adaptive preprocessing of sparse point cloud; S2. key feature enhancement and geometric modeling; S3. relative coordinate system construction and pose decoupling; S4. pose optimization solution based on distribution measurement; and S5. high-dimensional state smoothing estimation and output. The application solves the problems of feature extraction difficulty, high equipment cost and poor real-time performance of the existing perception scheme under the conditions of low computing power and sparse data, enhances the robustness and accuracy of the single sensor system in the complex dynamic environment for the six-degree-of-freedom high-dimensional pose perception of the vehicle, and provides a lighter and more reliable data foundation and theoretical support for the real-time decision planning and control of the automatic driving vehicle.

Description

Technical Field

[0001] This invention belongs to the field of intelligent transportation and autonomous driving perception technology, specifically involving a lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion. Background Technology

[0002] Intelligent Connected Vehicles (ICVs) are key platforms for achieving safe, efficient, and energy-saving transportation. Accurate and real-time environmental perception and target pose estimation are the decision-making basis for vehicle trajectory tracking and cooperative control. As autonomous driving technology extends to long-haul logistics and special transportation scenarios, perception systems need to achieve stable tracking of distant dynamic targets on low-computing-power edge devices, placing extremely high demands on the lightweight and robustness of algorithms.

[0003] However, in practical engineering applications, perception systems face severe challenges due to cost control and hardware power consumption limitations. On the one hand, while multi-sensor fusion solutions based on "LiDAR + vision" offer strong environmental understanding capabilities, their high equipment costs and the immense computational burden of multi-source data registration make real-time deployment on embedded platforms difficult. On the other hand, GNSS-based absolute positioning solutions are prone to failure in signal blind areas such as tunnels and canyons, leading to the loss of collaborative control benchmarks. More critically, when using low-cost single-radar solutions, point cloud density decreases sharply inversely with increasing detection distance, resulting in targets appearing as discrete, disordered, and feature-deficient sparse point sets in the information space. Existing fixed-threshold clustering algorithms struggle to accommodate multi-scale density differences, and traditional geometric fitting methods are prone to gradient vanishing under sparse conditions, leading to slow convergence and low accuracy in pose calculation.

[0004] Based on this, this invention constructs a lightweight perception architecture guided by physical priors. First, starting from the data distribution characteristics of physical space, it employs density-based adaptive radius neighborhood clustering and spatial attention mechanisms to solve the challenges of feature extraction and enhancement for long-distance sparse point clouds. Second, it establishes a relative coordinate system and rigid body motion decoupling model based on the rear vehicle's centroid in the information space, utilizing normalized Wasserstein distance (NWD) and extended Kalman filter (EKF) to handle observation noise and state estimation. This invention, by integrating adaptive feature extraction and relative pose decoupling algorithms, provides a high-precision, robust, and low-cost theoretical support and technical solution for vehicle pose perception in autonomous driving systems under low computing power and single-sensor conditions. Summary of the Invention

[0005] To address the challenges of existing single-radar sensing systems under long-range and low-computing-power conditions, such as difficulties in feature extraction due to the nonlinear decay of point cloud sparsity with distance, low accuracy of traditional geometric matching on discrete data, and high computational load from global coordinate system transformation, this invention provides a lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion. This method adaptively adjusts the clustering strategy according to the physical distribution characteristics of the point cloud, reduces the computational dimensionality through relative coordinate system decoupling, and optimizes pose calculation using distribution metrics, thereby achieving low-cost, highly robust real-time six-DOF vehicle pose estimation.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion includes the following steps:

[0008] S1. Sparse point cloud adaptive preprocessing;

[0009] The system acquires raw sparse point cloud data from a single radar, calculates the local density distribution of the sparse point cloud, constructs a density-based adaptive search radius function, and dynamically adjusts the neighborhood radius of the DBSCAN algorithm based on this function to perform neighborhood clustering and segmentation on the sparse point cloud to obtain the target point cloud cluster.

[0010] S2. Key Feature Enhancement and Geometric Modeling;

[0011] A local spatial attention mechanism is introduced to calculate the spatial attention weight of each point in the clustered point cloud. Key feature points are selected based on a weighted strategy, and a rectangular geometric model of the target vehicle is established by combining a random sampling consensus algorithm.

[0012] S3. Relative coordinate system construction and pose decoupling;

[0013] Abandoning the global coordinate system, a relative coordinate system with the center of gravity of the vehicle as the origin is constructed, and a rigid body transformation model is established from the coordinate system of the preceding vehicle to the relative coordinate system. Rigid body kinematic constraints are used to decouple the relative pose of the preceding vehicle.

[0014] S4. Pose optimization solution based on distribution metrics;

[0015] The observed point cloud distribution and the predicted rectangular model are regarded as two-dimensional Gaussian distributions. The normalized Wasserstein distance is introduced as a goodness-of-fit evaluation index. The initial pose parameters of the vehicle in the relative coordinate system are solved by nonlinear optimization.

[0016] S5. High-dimensional state smoothing estimation and output;

[0017] Based on the calculated initial pose parameters, the three-dimensional position and attitude quaternions are derived, the system state vector is constructed, and the correlation between observation noise and system state is processed by extended Kalman filtering to output the smoothed six-DOF high-dimensional pose of the front vehicle.

[0018] Furthermore, in step S1, constructing the density-based adaptive search radius function specifically includes: taking into account the physical characteristic that radar point cloud density decreases inversely with distance, defining an arbitrary point... Clustering search radius for:

[0019]

[0020] In the formula, Based on the search radius; This is the distance scaling factor; For point Local point density at; and These are the global maximum and minimum densities of the point cloud in the current frame, respectively; this function automatically expands the search radius in sparse regions and shrinks the search radius in dense regions.

[0021] Furthermore, step S2 includes the following sub-steps:

[0022] S2.1 Construct the attention weight function and assign weights;

[0023] Construct a weight assignment function based on Sigmoid nonlinear activation:

[0024]

[0025]

[0026]

[0027] In the formula, Point The attention weights have values ​​ranging from (0,1); This represents the Sigmoid function. It is used to map the unbounded feature values ​​of the input to the (0,1) interval and output a probability weight, which has smooth gating properties; This is the geometric feature sensitivity coefficient, used to adjust the model's dependence on abrupt shape changes; This is the intensity characteristic sensitivity coefficient, used to adjust the model's dependence on reflective properties; Indicates the curvature response value; Represents point cloud clusters The k-th point in; These correspond to the variances of the three principal directions obtained from principal component analysis. ; Indicates normalized reflection intensity; This represents the original radar strength value;

[0028] The mechanism automatically assigns high weights to vehicle corner points and license plate points, medium weights to vehicle body plane points, and low weights to surrounding discrete noise points.

[0029] S2.2 Construct a rectangular rigid body based on weighted RANSAC;

[0030] I. Define the rigid body state vector;

[0031] The target vehicle is simplified into a two-dimensional rectangular rigid body model, and a state parameter vector is defined. :

[0032]

[0033] In the formula, Represents the coordinates of the geometric center of the rectangle; This represents the heading angle of the rectangle's vertical axis relative to the X-axis of the radar coordinate system; These represent the length and width of the rectangle, respectively.

[0034] II. Weighted Loss Function ;

[0035]

[0036] In the formula, Point To fit the rectangle The shortest Euclidean distance between the four sides;

[0037] III. Robust iterative solution;

[0038] A weighted RANSAC strategy is used to solve the problem, and the process is repeated iteratively until the model parameters with the minimum loss are selected as the optimal estimates. ;

[0039] S2.3 Geometric solution and joint derivation of key feature points;

[0040] I. Solving the coordinates of the axis center The solution It will serve as the origin reference for constructing the relative coordinate system in step S3;

[0041]

[0042] In the formula, Indicates the center coordinates of the vehicle's rear axle; The coordinates of the geometric center of the fitted rectangle for the vehicle are given; L represents the length of the vehicle. This indicates the vehicle's heading angle, which is the angle between the longitudinal axis and the x-axis. This is a unit vector along the vehicle's longitudinal axis, representing the vehicle's direction of travel.

[0043] II. Calculation of key corner point coordinates;

[0044] Calculate the coordinates of the left and right corners of the vehicle's rear for subsequent Wasserstein distance calculation and visualization;

[0045] Left rear corner point :

[0046]

[0047] Back right corner point :

[0048]

[0049] In the formula, W represents the width of the vehicle; This is the unit vector along the vehicle's horizontal axis.

[0050] Furthermore, step S3 includes the following sub-steps:

[0051] S3.1 Joint Derivation of Rigid Body Transformation;

[0052] The coordinates of the center point of the rear axle of the front vehicle calculated using step S2.3 Derivation of its relative coordinate system coordinates in Satisfies the rigid body transformation relationship:

[0053]

[0054] because The above formula simplifies to ; This represents a translation vector relative to the coordinate system. It is a two-dimensional rotation matrix about the Z-axis;

[0055]

[0056] In the formula, Represents the horizontal component in a relative coordinate system; Represents the longitudinal component in a relative coordinate system; Indicates the vertical component; Indicates the relative distance between this vehicle and the vehicle in front; Indicates the relative azimuth angle between this vehicle and the vehicle in front;

[0057] S3.2 Define the relative pose parameter vector to be solved and perform pose decoupling;

[0058] I. Relative pose parameter vector:

[0059] in, The relative heading angle between the vehicle in front and this vehicle;

[0060] II. Position and pose decoupling;

[0061] Using the vehicle in front extracted in step S2 in the radar system Key feature point coordinates below , And the vehicle's IMU / odometer data, convert it to obtained from the system ;Will Substituting into the inverse process of the rigid body transformation described above, the initial relative state value at the current moment, i.e., the relative pose parameter vector, is solved using the nonlinear least squares method. .

[0062] Furthermore, step S4 includes the following sub-steps:

[0063] S4.1 Two-dimensional Gaussian distribution modeling of discrete point clouds and rigid body models;

[0064] I. Modeling of Observation Distribution ;

[0065] The target point cloud cluster obtained after preprocessing in step S1 Calculate its mean vector Covariance Matrix :

[0066]

[0067]

[0068] This distribution In the information space, the "centroid position" and "spatial divergence" of the observed target are abstractly expressed;

[0069] II. Predictive Distribution Modeling ;

[0070] Based on the relative pose parameters obtained from decoupling in step S3 The rectangular rigid body model it defines is modeled as a two-dimensional Gaussian distribution. :

[0071]

[0072]

[0073] S4.2 Calculate the second type Wasserstein distance between two Gaussian distributions;

[0074] For two two-dimensional Gaussian distributions and Its second type Wasserstein distance It has a closed-form solution:

[0075]

[0076] S4.3 Construction of Normalized Metrics and Nonlinear Optimization Solution;

[0077] I. Normalization modeling;

[0078] A nonlinear mapping function is introduced to convert the Wasserstein distance into a normalized metric, NWD.

[0079]

[0080] In the formula, This is the normalization constant;

[0081] II. Implementation of nonlinear optimization;

[0082] Construct a cost function that aims to maximize NWD:

[0083]

[0084] The Levenberg-Marquardt algorithm is used for iterative solution, and the optimal solution is finally output. That is, the initial pose parameters of the vehicle in front in the relative coordinate system.

[0085] Furthermore, step S5 includes the following sub-steps:

[0086] S5.1 Derivation of attitude quaternions;

[0087] The relative heading angle obtained from the decoupling in steps S3 and S4 Derive the attitude quaternion of the preceding vehicle in the relative coordinate system;

[0088]

[0089] In the formula, The relative attitude is represented by a unit quaternion;

[0090] S5.2 State space construction;

[0091] Define the system state vector

[0092] in, This represents the augmented state vector of the system at time k, with dimension . This includes the vehicle's position, attitude, speed, and angular velocity; Represents the vehicle's three-dimensional position coordinates; is the real part of the quaternion; The imaginary part of the quaternion describes the vehicle's rotational attitude in three-dimensional space. Indicates the longitudinal speed of the vehicle; Indicates the lateral speed of the vehicle; ω represents the angular velocity of the vehicle around the Z-axis, and ω represents the steering angular velocity of the vehicle.

[0093] S5.3 filter update;

[0094] By introducing quaternion normalization constraints and utilizing the prediction and update steps of the extended Kalman filter, the optimal solution obtained in step S4 is fused together. Output the smoothed optimal state estimate .

[0095] Beneficial effects:

[0096] 1. Enhance the perception robustness of sparse data: By using adaptive radius clustering and spatial attention mechanism, the problem of "oversegmentation at close range and missed detection at distant range" caused by the decrease in radar point cloud density with distance is overcome, and the geometric features of distant targets are effectively preserved.

[0097] 2. Achieving lightweight computing architecture: The innovative "relative centroid decoupling" model avoids complex global coordinate system matrix transformations and high-precision map dependencies, significantly reducing the time complexity of the algorithm and the computing power requirements of embedded hardware.

[0098] 3. Improved smoothness and accuracy of dynamic tracking: The introduction of normalized Wasserstein distance solves the limitations of traditional IoU in sparse point cloud matching. Combined with quaternion EKF filtering, sensor noise and gimbal lock-up risks are eliminated, ensuring the continuity and stability of the vehicle's six-degree-of-freedom pose output, and providing a reliable basis for autonomous driving control.

[0099] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0100] Figure 1 This is a flowchart of a lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion according to the present invention.

[0101] Figure 2 This is a schematic diagram of the near-field and far-field distribution of a lidar point cloud based on local density.

[0102] Figure 3 A schematic diagram of the functional relationship between the adaptive search radius and the local density;

[0103] Figure 4 This is a schematic diagram of the pose decoupling model based on the relative centroid coordinate system in this invention;

[0104] Figure 5 This diagram illustrates the limitations of the traditional IoU method in non-overlapping bounding box detection.

[0105] Figure 6 This is a schematic diagram of a pose optimization method based on normalized Wasserstein distance (NWD).

[0106] Figure 7 This is a graph showing the vehicle pose estimation error at different distances according to an embodiment of the present invention. Detailed Implementation

[0107] To make the technical solutions, advantages, and objectives of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the described embodiments of the present invention without creative effort are within the protection scope of this application.

[0108] like Figures 1-7 As shown, this invention provides a lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion, mainly applied to autonomous vehicles equipped with single-line or low-beam LiDAR, and specifically includes the following steps:

[0109] S1. Preprocessing of sparse point cloud data based on adaptive density field: acquire raw sparse point cloud data collected by a single radar, calculate the local density distribution of the point cloud; construct an adaptive search radius function based on density, use this function to dynamically adjust the neighborhood radius of the DBSCAN algorithm, perform clustering and inter-frame association on the sparse point cloud, and obtain the target point cloud cluster.

[0110] The core of this step lies in addressing the "near-dense, far-sparse" non-uniform distribution problem caused by the physical imaging mechanism of single-line / low-beam lidar during long-range perception. Existing DBSCAN equal-density clustering algorithms rely on a globally fixed neighborhood radius (Epsilon), which has inherent flaws when processing this type of data: if the radius is set too small, distant sparse targets will be incorrectly segmented into discrete noise (missed detection); if the radius is set too large, nearby adjacent targets (such as vehicles side-by-side) will be incorrectly merged (oversegmentation).

[0111] The existing DBSCAN algorithm uses a fixed radius. It cannot adapt to the fact that radar point cloud density decreases inversely with distance ( The physical properties of the clustering are considered. This step constructs a density-based adaptive search radius function to achieve robust clustering across the entire depth range.

[0112] S1.1 Sparsity Mechanism Analysis and Local Density Field Estimation;

[0113] LiDAR imaging follows the beam divergence principle in spherical coordinates. Assume the radar angular resolution is... At a distance of $r$, the arc length distance between adjacent laser points in physical space is approximately... Therefore, the spatial density of point clouds It exhibits a non-linear inverse relationship with the distance $r$ (on a two-dimensional projection plane). In three-dimensional space This physical constraint makes the sparsity of point clouds at long distances inevitable and regular.

[0114] To transform the aforementioned physical laws into computable mathematical indices, this embodiment introduces the k-Nearest Neighbors (k-NN) statistic. This involves acquiring the original point cloud set of the current frame from a single radar acquisition. A kd-dimensional tree data structure is used to spatially index the point cloud, thereby reducing the search time complexity to [missing information]. For any point in the set Search for its k nearest neighbors and calculate the average Euclidean distance. Define the local density index of point $p_i$. for:

[0115]

[0116] In the formula:

[0117] ;

[0118] This is a numerical stability constant to prevent the denominator from becoming zero due to overlapping points. To prevent small constants with a denominator of zero (take...) ), where k is the number of nearest neighbors.

[0119] Mechanism of action: This indicator It can directly reflect the density of point clouds in a local area. The larger the value, the denser the point cloud in that area (usually corresponding to close-range targets or areas with high reflectivity). The smaller the value, the sparser the data (corresponding to distant targets or noise).

[0120] S1.2 Adaptive search radius modeling based on hyperbolic tangent function;

[0121] 1. Model building principles:

[0122] To dynamically adapt point cloud density during clustering, it is necessary to construct a local density... To cluster search radius mapping function This function must satisfy the following physical constraints:

[0123] Monotonically decreasing property: density The smaller (sparser) the value, the larger the search radius r should be, to ensure that sparse points can be connected into clusters.

[0124] Boundary convergence: In extremely high-density areas (near distance), the radius should converge to the minimum feature scale of the vehicle to prevent excessive clustering; in extremely low-density areas (far distance), the radius should be limited to the maximum physical size of the vehicle to prevent misclustering of background noise.

[0125] 2. Nonlinear mapping model:

[0126] Constructing a density-based adaptive search radius function specifically includes: taking advantage of the physical characteristic that radar point cloud density decreases inversely with distance, defining an arbitrary point... Clustering search radius for:

[0127]

[0128] In the formula, Based on the search radius; This is the distance scaling factor; For point Local point density at; and These are the global maximum and minimum densities of the point cloud in the current frame, respectively; this function automatically expands the search radius in sparse regions and shrinks the search radius in dense regions.

[0129] S1.3 Dynamic Neighborhood Clustering and Association

[0130] 1. Implementation of dynamic clustering:

[0131] The set of dynamic radii for each point obtained using the above calculations Replaces the global fixed parameters in traditional DBSCAN .

[0132] ① Traverse the point cloud set, for any unvisited point , and its corresponding Perform a neighborhood search with radius.

[0133] ② If the number of points in the neighborhood is greater than the minimum number of points threshold (In this embodiment, we take 4, which corresponds to the lowest feature of the LiDAR scanning beam.) Then, we create a new cluster and continue to expand using the points in this neighborhood as seeds.

[0134] ③ During the expansion process, for neighboring points within the neighborhood... Using its own adaptive radius Further searches are conducted. This asymmetric neighborhood expansion strategy is better suited to situations where the target surface density is uneven (e.g., dense at the front and sparse at the rear).

[0135] ④ Output the set of candidate target point cloud clusters .

[0136] 2. Spatiotemporal correlation matching:

[0137] To maintain the consistency of target IDs over time, the Hungarian Algorithm is used for inter-frame association.

[0138] ① State prediction: Predict the target in the previous frame using Kalman filtering. At the prior position in the current frame.

[0139] ② Cost matrix construction: Calculate the clusters of the current frame The cost matrix is ​​constructed by using the Euclidean distance between the centroid of the current frame and the predicted centroid of the previous frame. .

[0140] ③ Optimal matching: The minimum weight matching of the bipartite graph is solved by the Hungarian algorithm to complete the target ID assignment and trajectory initialization.

[0141] S2. Key Feature Enhancement and Modeling Based on Local Spatial Attention Mechanism: A local spatial attention mechanism is introduced to calculate the spatial attention weight of each point in the clustered point cloud and assign high weights to feature points located at the contour edges and corners; based on the weighted random sampling consensus (RANSAC) algorithm, the weighted sum of squared distances is minimized to establish and fit the rectangular geometric model of the target vehicle and extract the features of the rear axle center and corners.

[0142] In a single-radar sparse point cloud environment, geometric fitting based solely on the spatial distribution of points is easily affected by environmental noise (such as ground clutter and rain / fog noise). Furthermore, due to the discreteness of radar scan lines, the vehicle's facade often only captures discontinuous scan points. To accurately lock the vehicle's attitude without performing high-computational 3D reconstruction, this step introduces a physically based spatial attention mechanism and combines it with a weighted RANSAC algorithm for robust modeling.

[0143] S2.1 Spatial Feature Descriptor Extraction and Attention Mechanism Principles;

[0144] 1. Mechanism Analysis and Modeling:

[0145] As a man-made rigid body, a vehicle exhibits two significant physical characteristics in radar point clouds:

[0146] Geometric abrupt changes: The vehicle's outline is composed of regular straight lines (sides and rear) and right angles (corners). Compared to smooth sides, corners contain the richest information for determining the vehicle's heading and boundaries. In point cloud data, the local curvature at corners is significantly higher than that at planar points.

[0147] Non-uniformity of reflection intensity: The license plate, taillights and other components at the rear of the vehicle are usually equipped with reflective materials, and their laser reflection intensity is significantly higher than that of the vehicle body paint or the surrounding environment.

[0148] Therefore, this embodiment constructs an attention model that integrates "geometric curvature" and "physical strength", aiming to make the algorithm "focus" on the points that are most valuable for pose estimation.

[0149] 2. Parameter definition and calculation process:

[0150] For any candidate point cloud cluster For each of these points Calculate the following features:

[0151] ① Local curvature response :

[0152] Calculate points using Principal Component Analysis (PCA) and Covariance matrix of nearest neighbors .right Perform eigenvalue decomposition to obtain eigenvalues. The curvature response is defined as the proportion of the smallest eigenvalue in the sum of all eigenvalues.

[0153]

[0154] The larger the value, the more drastic the spatial changes in the area of ​​that point (i.e., the greater the likelihood of it being a corner or edge).

[0155] ② Normalized reflection intensity :

[0156] Original radar strength value Normalization to Interval.

[0157] 3. Construction of attention weight function:

[0158] Construct a weight assignment function based on Sigmoid nonlinear activation:

[0159]

[0160] In the formula:

[0161] The sigmoid function is used to map unbounded feature responses to probability weights of $(0,1)$, and has smooth gating properties.

[0162] : These are the geometric feature sensitivity coefficient and the intensity feature sensitivity coefficient, respectively, used to adjust the model's dependence on abrupt shape changes and reflective properties.

[0163] Specific function: This mechanism automatically assigns extremely high weights (close to 1) to vehicle corner points (high curvature) and license plate points (high intensity), while assigning medium weights to vehicle body plane points and low weights (close to 0) to surrounding discrete noise points.

[0164] S2.2 Rectangular rigid body modeling based on weighted RANSAC;

[0165] 1. Definition of rigid body state vector:

[0166] The target vehicle is simplified into a two-dimensional rectangular rigid body model, and a state parameter vector is defined. :

[0167]

[0168] In the formula:

[0169] : Coordinates of the geometric center of the rectangle;

[0170] Heading Angle: The vertical axis of the rectangle relative to the X-axis of the radar coordinate system.

[0171] : The length and width of the rectangle.

[0172] 2. Construction of weighted loss function:

[0173] The traditional RANSAC algorithm treats all inliers as equally weighted, making it susceptible to bias by dense lateral points (resulting in a heading angle parallel to the lateral points but with positional deviation). This embodiment constructs a weighted geometric distance loss function. :

[0174]

[0175] In the formula, This represents the shortest Euclidean distance from a point to the four sides of a rectangle.

[0176] Specific function: Due to corner points The loss function is very large, and it forces the rectangular model to prioritize "sucking" and "aligning" with the corner points of the vehicle, so that even when the sparse point cloud causes incomplete side line segments, it can still accurately recover the true size and orientation of the vehicle.

[0177] 3. Robust iterative solution:

[0178] The solution is obtained using a weighted RANSAC strategy:

[0179] ① Sampling: by probability Two points are randomly selected to generate a hypothetical rectangle.

[0180] ②Verification: Calculate the weighted loss of all points to the hypothetical rectangle. .

[0181] ③ Optimization: Repeat the above process Next, the model parameters with the minimum loss are selected as the optimal estimates. .

[0182] S2.3 Geometric solution and joint derivation of key feature points;

[0183] After obtaining the optimal geometric parameters $S_{box}^*$, they need to be transformed into key feature points required for the kinematic model. Since vehicle motion is usually described based on the rear axle center, while the rectangular fitting yields the geometric center, a geometric transformation derivation is required.

[0184] 1. Theoretical Derivation:

[0185] Assume the ratio of the vehicle's rear overhang to its total length is a fixed prior ratio. Alternatively, one can simply assume that the geometric center is the center of motion. This embodiment employs a precise derivation based on the geometric center:

[0186] Define rotation matrix :

[0187]

[0188] ① Rear axle center coordinates Solution:

[0189] The rear axle center is typically located at the geometric center offset along the negative direction of the vehicle's longitudinal axis. (Approximate location):

[0190]

[0191] ② Calculation of key corner coordinates:

[0192] To subsequently calculate the Wasserstein distance and for visualization, the left and right corner points of the vehicle's rear are solved:

[0193] Left rear corner point :

[0194]

[0195] Back right corner point :

[0196]

[0197] Function description:

[0198] This step transforms the purely geometric "rectangle" into a "kinematic anchor point" with physical meaning. The calculated... It will serve as the origin reference for constructing the relative coordinate system in step S3, and will also be the core state variable for subsequent Kalman filter tracking.

[0199] S3. Decoupling of pose dynamics based on relative centroid coordinate system: Abandoning the traditional global coordinate system transformation, a relative coordinate system with the vehicle's centroid as the origin is constructed; based on the principle of rigid body kinematics, a feature point transformation model is established from the front vehicle's body coordinate system to the relative coordinate system, decoupling the pose estimation problem into the solution of parameters such as relative distance, azimuth angle and relative heading angle.

[0200] This step aims to address the problems of non-inertial error correction and cumbersome matrix transformation computation in traditional pose estimation methods based on global coordinate systems (such as geodetic coordinate systems or inertial navigation systems). This invention innovatively constructs a lightweight information space decoupling architecture that "de-globalizes" the model.

[0201] S3.1 Analysis of Coordinate System Selection Mechanism and Definition of Relative Coordinate System;

[0202] 1. Technical Defect Analysis (Computational Redundancy and Error Mechanisms):

[0203] Existing methods mostly operate in a global coordinate system. Planning and control are carried out at each moment. Observation point It requires complex homogeneous transformation matrices Switch to global system: .Should The matrix not only contains the vehicle's global position information, but also needs to correct for non-inertial errors such as Coriolis force and Euler force generated by the vehicle's acceleration, deceleration, and steering in real time. This high-frequency, high-dimensional matrix operation is the root cause of the excessive computing load and insufficient real-time performance of embedded platforms.

[0204] 2. Decoupling principle and coordinate system definition:

[0205] This invention employs a physical chain prior concept to strongly couple the perception system with the vehicle's dynamics baseline. A relative coordinate system is constructed based on the center of gravity of the vehicle (and the following vehicle). .

[0206] Relative coordinate system :origin Located at the center of the rear axle of this vehicle; The axle points towards the front of the vehicle along its longitudinal axis; The axis is perpendicular to the ground and pointing upwards.

[0207] Forward vehicle body coordinate system :origin Located at the center of the rear axle of the front vehicle; The axis runs along the longitudinal axis of the front vehicle.

[0208] Specific function: The origin and axis of the system are always bound to the rigid body of the vehicle, maintaining relative stability during motion. This naturally eliminates non-inertial errors in observation caused by the vehicle's motion and avoids complex... Matrix correction steps.

[0209] S3.2 Relative Modeling and Derivation of State Decoupling in Rigid Body Kinematics

[0210] 1. Definition of the relative state vector to be estimated:

[0211] This step transforms the pose estimation problem into solving the core relative observation state vector. :

[0212]

[0213] In the formula:

[0214] The relative distance (relative range) between the centers of the rear axles of the two vehicles.

[0215] : The azimuth angle of the front vehicle's center of gravity relative to the longitudinal axis $X_R$ of this vehicle.

[0216] The relative heading angle of the vehicle in front relative to this vehicle.

[0217] 2. Joint derivation of rigid body transformation:

[0218] The center point of the rear axle of the front vehicle calculated using step S2.3 (At the origin of the preceding vehicle's system), derive its coordinate system relative to the origin. coordinates in Satisfy rigid body transformation relations:

[0219]

[0220] because The above formula simplifies to .

[0221] According to the definition of relative state, the translation vector The expansion in the two-dimensional plane is:

[0222]

[0223] (Note: The assumption of a flat road under a following vehicle scenario aligns with the principles of lightweight models. If road surface slope needs to be considered, then... ,in It is the pitch angle, but it remains unchanged here. (The lightest possible weight assumption.)

[0224] Rotation matrix Here is the two-dimensional rotation matrix about the Z-axis:

[0225]

[0226] 3. Functions and Dimensional Reduction Summary:

[0227] By constructing a relative coordinate system Successfully integrated six degrees of freedom The absolute pose estimation problem is decoupled and reduced in dimensionality to three core relative state variables only on the horizontal plane. The solution achieves an extreme simplification of the observation state space.

[0228] S3.3 Relative Dynamic Constraints and Cyber-Physical Integration

[0229] In order to establish the connection between the perception results (information space) and the vehicle motion model (physical space), this step establishes the differential relationship of the relative states, providing physical priors for the filtering in step S5.

[0230] 1. Relative velocity constraint:

[0231] relative distance rate of change It equals the difference in projections of the velocity vectors of the two vehicles onto the line connecting them:

[0232]

[0233] In the formula, and These are the absolute velocity vectors of the vehicle in front and the vehicle itself, respectively. It is the unit direction vector from this vehicle to the vehicle in front.

[0234] Joint Derivation: This differential relationship forms the core of the extended Kalman filter (EKF) state transition equation in step S5, namely... In The function allows the filter's prediction step to be constrained by physical priors, thereby significantly enhancing the robustness and smoothness of tracking.

[0235] 2. Solving the decoupling equations:

[0236] Using the vehicle in front extracted in step S2 in the radar system Key feature point coordinates below , And the vehicle's IMU / odometer data, convert it to obtained from the system .Will Substituting the inverse process of the rigid body transformation described above, the initial relative state value at the current moment is solved using the nonlinear least squares method. This initial value Subsequently, as the starting point for optimization in step S4, the physical geometric constraints are transformed into information states.

[0237] S4: Distribution optimization based on normalized Wasserstein distance: The observed sparse point cloud distribution and the predicted rectangular model are modeled as two-dimensional Gaussian distributions respectively. Normalized Wasserstein distance (NWD) is introduced to replace the traditional intersection-over-union ratio (IoU) as the goodness-of-fit evaluation index. The initial pose parameters of the vehicle in the relative coordinate system are solved by nonlinear optimization.

[0238] In long-range perception scenarios using a single radar, the observed point cloud is often incomplete due to limitations in scanning resolution (e.g., only a portion of the rear of the vehicle is scanned). This causes traditional evaluation metrics based on geometric overlap (such as IoU and Chamfer Distance) to exhibit gradient vanishing or local minima when the predicted bounding box does not overlap with the real target, thus failing to guide the optimization algorithm to converge.

[0239] This invention introduces the Wasserstein distance from optimal transport theory, transforming the rigid body geometric matching problem into a metric problem between probability distributions. The specific implementation is as follows:

[0240] S4.1 Two-Dimensional Gaussian Distribution Modeling Mechanism for Discrete Point Clouds and Rigid Body Models

[0241] 1. Analysis of Modeling Principles:

[0242] To "complete" the target features using mathematical methods under sparse conditions, this embodiment employs a two-dimensional Gaussian distribution to probabilistically represent the observed data and the prediction model. The Gaussian distribution has the property of an infinite support set, meaning that its probability density is non-zero at any location in space, which fundamentally guarantees the differentiability of the metric function everywhere.

[0243] 2. Modeling of Observation Distribution :

[0244] The target point cloud cluster obtained after preprocessing in step S1 Calculate its mean vector $\mu_a$ and covariance matrix. :

[0245]

[0246] This distribution In the information space, the "centroid position" and "spatial divergence" of the observed target are abstractly expressed.

[0247] 3. Predictive distribution modeling (Joint derivation):

[0248] For the relative pose parameters obtained by decoupling in step S3 The determined rectangular rigid body model (length) ,Width It is approximated as a two-dimensional Gaussian distribution. .

[0249] According to the principle of probability-geometric approximation, the center of the rectangle is the mean. The rotation matrix and size of the rectangle determine the covariance matrix:

[0250]

[0251]

[0252] In the formula, It is a two-dimensional rotation matrix.

[0253] Specific function: Through this modeling, discrete geometric parameters... Mapped to continuous probability distribution parameters This allows subsequent calculations to capture the overall structural features of the target rather than discrete points and their boundaries.

[0254] S4.2 Derivation of the Wasserstein Distance of the Second Kind and Analysis of Gradient Flow Characteristics

[0255] 1. Definition and derivation of distance:

[0256] Wasserstein distance measures the minimum physical cost required to "transport" and transform one probability distribution into another. For two two-dimensional Gaussian distributions... and Its second type Wasserstein distance It has a closed-form solution:

[0257]

[0258] 2. Physical meaning and mechanism analysis:

[0259] ① Position difference item: directly measures the relative distance between the two vehicles. and azimuth The deviation provides a globally convergent translation gradient.

[0260] ② Morphology and attitude difference terms: The covariance matrix is ​​coupled using trace operation, which can simultaneously measure the target size. Matching degree and relative heading angle The deviation.

[0261] ③ Gradient flow advantage: Unlike IoU, even if the distribution is predicted... With observed distribution They do not overlap at all in space (IoU=0). It remains a positive value, and regarding the pose parameters gradient It always exists and points in the direction of the optimal solution. This solves the problem that sparse point clouds cannot be optimized when the initialization deviation is large.

[0262] S4.3 Construction of Normalized Metrics and Solution of Nonlinear Optimization

[0263] 1. Normalization modeling:

[0264] because The range of values ​​is Directly using this as a loss function is sensitive to hyperparameters and makes it difficult to measure matching quality. This invention introduces a nonlinear mapping to construct a normalized Wasserstein distance:

[0265]

[0266] In the formula, This is the normalization constant.

[0267] Parameter function: NWD maps distance to The intervals, similar in nature to IoU, are defined as follows: 1 represents complete overlap, and 0 represents infinity. This normalization allows the optimizer to use a common convergence threshold.

[0268] 2. Implementation of nonlinear optimization:

[0269] Construct a cost function that aims to maximize NWD:

[0270]

[0271] Because this function relates to state variables Highly nonlinear, the Levenberg-Marquardt algorithm is used for iterative solution:

[0272] Initialization: Using the coarse relative pose calculated in step S3 Used as initial values.

[0273] Jacobian matrix calculation: Derivation using the chain rule about Jacobian matrix .

[0274] Iterative updates:

[0275]

[0276] Convergence criterion: When Less than the threshold The optimal solution is output when the maximum number of iterations is reached. .

[0277] S5: High-dimensional state smoothing estimation based on quaternion EKF: Based on the solved initial pose parameters, the three-dimensional spatial position and attitude quaternions are derived to construct the system state vector containing position and attitude; the extended Kalman filter (EKF) is used to introduce quaternion normalization constraints to handle the correlation between observation noise and system state, and output the smoothed six-DOF high-dimensional pose of the front vehicle.

[0278] Step S4 has obtained high-precision instantaneous values ​​of relative pose, but single-frame calculation is still affected by sensor sampling noise and cannot provide dynamic information such as velocity and angular velocity. In addition, traditional Euler angles suffer from gimbal lock and unsmooth interpolation when describing large-angle rotations.

[0279] This step constructs an Extended Kalman Filter (EKF) system based on quaternion kinematics to achieve smooth tracking and state prediction of the six-degree-of-freedom pose of the vehicle in front.

[0280] S5.1 Quaternion Space Mapping and Mechanism Analysis of Relative Attitudes

[0281] 1. Mapping principle:

[0282] Quaternion It is a hypercomplex number whose subsets of magnitude 1 form a three-dimensional rotation group. Double coverage. Compared to rotation matrices (9 parameters) and Euler angles (3 parameters), unit quaternions (4 parameters) have the mathematical properties of no singularity, high computational efficiency, and smooth interpolation.

[0283] 2. Joint derivation process:

[0284] The relative heading angle obtained from the decoupling in steps S3 and S4 By combining the planar assumption of vehicle motion (i.e., rotation around the Z-axis), a mapping function from Euler angle space to quaternion space is constructed. .

[0285] According to the rotation axis-angle conversion formula, the rotation axis is... Axial unit vector The rotation angle is Then the quaternion is represented as:

[0286]

[0287] Specific function: It maps a one-dimensional angle variable to a four-dimensional hyperspherical vector, allowing subsequent filtering processes to be performed in manifold space, avoiding periodic jumps in angle (such as...). arrive The impact of sudden changes on the filter.

[0288] S5.2 Construction of High-Dimensional State Space and Derivation of Dynamic Equations

[0289] 1. Definition of system state vector:

[0290] To describe the complete motion state of the vehicle, an augmented state vector is defined. :

[0291]

[0292] In the formula, For three-dimensional position, For attitude quaternions, Linear velocity, ω is the angular velocity about the Z-axis.

[0293] 2. Derivation of the state transition equation (prediction step):

[0294] Constructing a discrete-time state transition function based on a constant turning rate and speed (CTRV) model .

[0295] Position and velocity prediction:

[0296]

[0297] Derivation of Quaternion Differential Kinematics:

[0298] Based on the quaternion differential equation ,in The angular velocity quaternion, This is quaternion multiplication.

[0299] Discretize it (preserving zero order) to obtain the attitude update part of the state transition matrix:

[0300]

[0301] in It is an antisymmetric matrix:

[0302]

[0303] Jacobian matrix calculation:

[0304] To handle the linearization of EKF, the state transition matrix is ​​calculated. Used for covariance matrix The transmission: ,in Let be the process noise covariance matrix.

[0305] 3. Modeling the observation equation (update step):

[0306] Establish measurement model , where the observation vector The output (position + quaternion) comes from step S4.

[0307] Because the observed value (the optimal solution calculated in step S4) The observation matrix directly corresponds to the first 7 dimensions of the state vector. For a simple projection matrix:

[0308]

[0309] Using Kalman gain Update status, among which To observe the noise covariance matrix, the residuals are dynamically adjusted by Wasserstein distance optimization.

[0310] S5.3 Normalized manifold constraints and smoothed output;

[0311] 1. Constraint Mechanism Analysis:

[0312] The standard EKF update step is based on linear weighting in Euclidean space ( For the quaternion part, this addition operation violates its unit modulus constraint (i.e., This causes the attitude solution to no longer represent pure rotation, and may even cause the filter to diverge. Therefore, manifold constraints must be introduced.

[0313] 2. Constraint Filtering Update Implementation:

[0314] After completing the standard measurement update of EKF, the quaternion components in the state vector are... Perform forced normalization:

[0315]

[0316] Physical meaning: Geometrically, this operation is equivalent to reprojecting the updated state points from the tangent space back onto the unit hyperspherical manifold, ensuring the geometric validity of the attitude calculation.

[0317] 3. Function Description:

[0318] After the above processing, the system's final output is It not only includes the denoised six-DOF vehicle pose, but also implicitly estimates the target velocity and angular velocity. This provides downstream autonomous driving path planning modules with smoother and more predictive input data than simple geometric measurements, effectively solving the state transition problem when sensors lose frames or are occluded.

[0319] It is hereby declared that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion, characterized in that... This includes the following steps: S1. Sparse point cloud adaptive preprocessing; The system acquires raw sparse point cloud data from a single radar, calculates the local density distribution of the sparse point cloud, constructs a density-based adaptive search radius function, and performs neighborhood clustering and segmentation of the sparse point cloud based on this function to obtain the target point cloud cluster. Calculate the local density distribution of a sparse point cloud, i.e., calculate the local density index of the sparse point cloud. : ； In the formula, is the numerical stability constant; k is the number of nearest neighbors; Let be any point in the sparse point cloud set; Indicates the average Euclidean distance; In step S1, constructing the density-based adaptive search radius function specifically includes: taking into account the physical characteristic that radar point cloud density decreases inversely with distance, defining an arbitrary point... Clustering search radius for: In the formula, Based on the search radius; This is the distance scaling factor; For point Local point density at; and These are the global maximum and minimum densities of the point cloud in the current frame, respectively; this function automatically expands the search radius in sparse regions and shrinks the search radius in dense regions. S2. Key Feature Enhancement and Geometric Modeling; A local spatial attention mechanism is introduced to calculate the spatial attention weight of each point in the clustered point cloud. Key feature points are selected based on a weighted strategy, and a rectangular geometric model of the target vehicle is established by combining a random sampling consensus algorithm. S3. Relative coordinate system construction and pose decoupling; Abandoning the global coordinate system, a relative coordinate system with the center of gravity of the vehicle as the origin is constructed, and a rigid body transformation model is established from the coordinate system of the preceding vehicle to the relative coordinate system. Rigid body kinematic constraints are used to decouple the relative pose of the preceding vehicle. S4. Pose optimization solution based on distribution metrics; The observed point cloud distribution and the predicted rectangular model are regarded as two-dimensional Gaussian distributions. The normalized Wasserstein distance is introduced as a goodness-of-fit evaluation index. The initial pose parameters of the vehicle in the relative coordinate system are solved by nonlinear optimization. S5. High-dimensional state smoothing estimation and output; Based on the calculated initial pose parameters, the three-dimensional position and attitude quaternions are derived, the system state vector is constructed, and the correlation between observation noise and system state is processed by extended Kalman filtering to output the smoothed six-DOF high-dimensional pose of the front vehicle.

2. The lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion as described in claim 1, characterized in that, Step S2 includes the following sub-steps: S2.1 Construct the attention weight function and assign weights; Construct a weight assignment function based on Sigmoid nonlinear activation: In the formula, Point The attention weights have values ​​ranging from (0,1); This represents the Sigmoid function. It is used to map the unbounded feature values ​​of the input to the (0,1) interval and output a probability weight, which has smooth gating properties; This is the geometric feature sensitivity coefficient, used to adjust the model's dependence on abrupt shape changes; This is the intensity characteristic sensitivity coefficient, used to adjust the model's dependence on reflective properties; Indicates the curvature response value; Represents point cloud clusters The k-th point in; These correspond to the variances of the three principal directions obtained from principal component analysis. ; Indicates normalized reflection intensity; This represents the original radar strength value; The mechanism automatically assigns high weights to vehicle corner points and license plate points, medium weights to vehicle body plane points, and low weights to surrounding discrete noise points. S2.2 Construct a rectangular rigid body based on weighted RANSAC; I. Define the rigid body state vector; The target vehicle is simplified into a two-dimensional rectangular rigid body model, and a state parameter vector is defined. : In the formula, Represents the coordinates of the geometric center of the rectangle; This represents the heading angle of the rectangle's vertical axis relative to the X-axis of the radar coordinate system; These represent the length and width of the rectangle, respectively. II. Weighted Loss Function ; In the formula, Point To fit the rectangle The shortest Euclidean distance between the four sides; III. Robust iterative solution; A weighted RANSAC strategy is used to solve the problem, and the process is repeated iteratively until the model parameters with the minimum loss are selected as the optimal estimates. ; S2.3 Geometric solution and joint derivation of key feature points; I. Solving the coordinates of the axis center The solution It will serve as the origin reference for constructing the relative coordinate system in step S3; In the formula, Indicates the center coordinates of the vehicle's rear axle; The coordinates of the geometric center of the fitted rectangle for the vehicle are given; L represents the length of the vehicle. This indicates the vehicle's heading angle, which is the angle between the longitudinal axis and the x-axis. This is a unit vector along the vehicle's longitudinal axis, representing the vehicle's direction of travel. II. Calculation of key corner point coordinates; Calculate the coordinates of the left and right corners of the vehicle's rear for subsequent Wasserstein distance calculation and visualization; Left rear corner point : Right posterior angle point : In the formula, W represents the width of the vehicle; This is the unit vector along the vehicle's horizontal axis.

3. The lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion according to claim 2, characterized in that, Step S3 includes the following sub-steps: S3.1 Joint Derivation of Rigid Body Transformation; The coordinates of the center point of the rear axle of the front vehicle calculated using step S2.3 Derivation of its relative coordinate system coordinates in Satisfies the rigid body transformation relationship: because The above formula simplifies to ; This represents a translation vector relative to the coordinate system. It is a two-dimensional rotation matrix about the Z-axis; In the formula, Represents the horizontal component in a relative coordinate system; Represents the longitudinal component in a relative coordinate system; Indicates the vertical component; Indicates the relative distance between this vehicle and the vehicle in front; Indicates the relative azimuth angle between this vehicle and the vehicle in front; S3.2 Define the relative pose parameter vector to be solved and perform pose decoupling; I. Relative pose parameter vector: in, The relative heading angle between the vehicle in front and this vehicle; II. Position and pose decoupling; Using the vehicle in front extracted in step S2 in the radar system Key feature point coordinates below , And the vehicle's IMU / odometer data, convert it to obtained from the system ;Will Substituting into the inverse process of the rigid body transformation described above, the initial relative state value at the current moment, i.e., the relative pose parameter vector, is solved using the nonlinear least squares method. .

4. The lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion according to claim 3, characterized in that, Step S4 includes the following sub-steps: S4.1 Two-dimensional Gaussian distribution modeling of discrete point clouds and rigid body models; I. Modeling of Observation Distribution ; The target point cloud cluster obtained after preprocessing in step S1 Calculate its mean vector Covariance Matrix : This distribution In the information space, the "centroid position" and "spatial divergence" of the observed target are abstractly expressed; II. Predictive Distribution Modeling ; Based on the relative pose parameters obtained from decoupling in step S3 The rectangular rigid body model it defines is modeled as a two-dimensional Gaussian distribution. : S4.2 Calculate the second type Wasserstein distance between two Gaussian distributions; For two two-dimensional Gaussian distributions and Its second type Wasserstein distance It has a closed-form solution: S4.3 Construction of Normalized Metrics and Nonlinear Optimization Solution; I. Normalization modeling; A nonlinear mapping function is introduced to convert the Wasserstein distance into a normalized metric, NWD. In the formula, This is the normalization constant; II. Implementation of nonlinear optimization; Construct a cost function that aims to maximize NWD: The Levenberg-Marquardt algorithm is used for iterative solution, and the optimal solution is finally output. That is, the initial pose parameters of the vehicle in front in the relative coordinate system.

5. The lightweight vehicle pose estimation method based on adaptive neighborhood clustering and cyber-physical fusion according to claim 4, characterized in that, Step S5 includes the following sub-steps: S5.1 Derivation of attitude quaternions; The relative heading angle obtained from the decoupling in steps S3 and S4 Derive the attitude quaternion of the preceding vehicle in the relative coordinate system; In the formula, The relative attitude is represented by a unit quaternion; S5.2 State space construction; Define the system state vector in, This represents the augmented state vector of the system at time k, with dimension . This includes the vehicle's position, attitude, speed, and angular velocity; Represents the vehicle's three-dimensional position coordinates; is the real part of the quaternion; The imaginary part of the quaternion describes the vehicle's rotational attitude in three-dimensional space. Indicates the longitudinal speed of the vehicle; Indicates the lateral speed of the vehicle; ω represents the angular velocity of the vehicle around the Z-axis, and ω represents the steering angular velocity of the vehicle. S5.3 filter update; By introducing quaternion normalization constraints and utilizing the prediction and update steps of the extended Kalman filter, the optimal solution obtained in step S4 is fused together. Output the smoothed optimal state estimate .

Images