Star map matching based on virtual features and semantic particle filter cooperative positioning method

Abstract

The application discloses a star map matching and semantic particle filtering cooperative positioning method based on virtual features, and particularly relates to the technical field of robot autonomous positioning and vehicle navigation, wherein a global map and multi-frame local observation data are constructed based on virtual features and real pole features; four-point combination is sampled based on the global map features and a rotation invariant feature descriptor is constructed, and a global star map index is constructed; the local observation data is repeatedly combined with the four-point combination and the rotation invariant feature descriptor is constructed, the global index is inquired through star map matching, and the initial pose of a positioning subject is estimated in combination with a Kabsch algorithm; the initial pose output by the star map matching is used to initialize semantic particle filtering, a particle set, uniform weight and model parameters are generated; the particle pose is updated based on a mileage counter, and the particle weight is calculated in combination with local observation and global map features; a trigger condition is set, the particle distribution is reset after successful matching; the estimated pose of each frame of particle filtering is stored, and an optimized trajectory is output after missing poses are filled.

Description

Technical Field

[0001] This invention relates to the field of robot autonomous localization and vehicle navigation technology, specifically to a collaborative localization method based on virtual feature-based star map matching and semantic particle filtering. Background Technology

[0002] With the development of autonomous driving and mobile robot technologies, autonomous positioning technology has become a core supporting capability, especially in GPS-denied environments where GPS signals are blocked or malfunctioning. Positioning accuracy directly determines the operational safety of equipment. Traditional positioning methods have significant drawbacks: GNSS positioning relies on satellite signals, which are prone to increased positioning errors or even failure in indoor, underground, or high-rise areas due to signal blockage; laser SLAM technology uses inter-frame point cloud matching for positioning, but long-distance travel can lead to cumulative drift. Although loop closure detection can eliminate errors, it requires building a global point cloud map, which consumes a lot of storage resources and limits the flexibility of the positioning subject's movement trajectory; existing particle filter positioning only utilizes geometric features such as distance and angle, without incorporating environmental semantic information. It is prone to positioning drift in similar geometric environments such as continuous walls and repetitive rod-shaped objects, and the initial pose depends on prior information, lacking a reliable reset mechanism when particles degenerate; star map matching methods are mostly used to solve the problem of accurate positioning of astronomical images, and are not optimized for structured "virtual features" (such as wall intersections) in autonomous driving scenarios and artificial environments, and lack semantic constraints, resulting in insufficient matching robustness.

[0003] In artificial environments, the features formed by wall intersections possess global coordinate stability and can serve as a natural global constraint benchmark. However, traditional technologies have not designed solutions to address the pain point of "lack of real rod-shaped objects in the scene." Furthermore, even if features related to wall intersections are extracted, errors can easily generate a large number of clustered "false features," leading to reduced feature effectiveness. In addition, existing technologies cannot achieve accurate matching and positioning of features with physical rod-shaped objects without prior information, making it difficult to form a complete positioning system. Summary of the Invention

[0004] To address the aforementioned progressively worsening problems, this invention provides a collaborative localization method based on virtual feature-based star map matching and semantic particle filtering. This method uses multiple technologies to collaboratively solve the problems of cumulative error and insufficient localization robustness of laser odometry.

[0005] To address the problems of large cumulative error, insufficient robustness, and reliance on prior information in existing positioning technologies under GPS-denied environments, this invention provides the following technical solution: a collaborative positioning method based on virtual feature star map matching and semantic particle filtering, comprising the following steps:

[0006] 1) Virtual feature extraction and map construction: Obtain the original 3D point cloud data of the environment, extract virtual features and real pole features after preprocessing, and construct a global map (including 2D location, semantic labels, and radius) and multi-frame local observation data based on the virtual features and real pole features;

[0007] 2) Star map matching localization: Based on the global map feature sampling, 4-point combination is sampled and rotation-invariant feature descriptor is constructed to build a global star map index; the 4-point combination and rotation-invariant feature descriptor are repeated for local observation data, and the global index is queried through star map matching. The initial pose of the localized subject is estimated by combining the Kabsch algorithm.

[0008] 3) Star map-semantic particle filter fusion localization: Initialize the semantic particle filter with the initial pose output by star map matching, and generate a particle set, uniform weights and model parameters that conform to the preset distribution; update the particle pose based on odometry (motion update), and calculate the particle weights by combining the geometric, semantic and radius features of local observations and the global map (observation update); set the fixed interval of star map matching and the particle failure trigger condition, and reset the particle distribution after successful matching; store the particle filter estimated pose for each frame, fill in the missing poses and output the optimized trajectory.

[0009] Preferably, the virtual feature in step 1) is a virtual prism feature obtained through facade segmentation, wall detection, wall intersection calculation, and cluster optimization, as follows:

[0010] 1.1) Facade segmentation: Calculate the point cloud normal and the ground normal. The dot product is used to filter points whose absolute dot product value is less than a pre-set facade filtering threshold as facade point clouds.

[0011] 1.2) Wall detection: For the elevation point cloud, a planar detection algorithm is used to obtain the wall normal, center point and dimensions. The number of filtered points is less than the pre-set minimum number of planar points.

[0012] 1.3) Wall intersection line calculation: The absolute value of the dot product of the normal line is less than or equal to the preset wall parallelism judgment threshold. For non-parallel walls, the intersection line is solved by plane equation. The lower limit of the virtual prism height is preset to the upper limit of the virtual prism height. The midpoint of the endpoint of the intersection line within the preset height range is taken as the candidate center of the virtual prism.

[0013] 1.4) Clustering optimization: Cluster the candidate centers based on density peak clustering (DPC) to obtain a virtual prism feature set. The virtual prism feature set includes the center XY coordinates, semantic label 0, and radius taken as the pre-set virtual prism radius.

[0014] Preferably, when the wall detection in step 1.2) uses a planar detection algorithm, the neighborhood search parameter is set to K-nearest neighbor search, the number of nearest neighbor points is set to the pre-set number of nearest neighbor points for wall detection, the point cloud index within the bounding box is extracted for each planar candidate, the median of the point cloud normal corresponding to the index is calculated and normalized, and used as the representative normal of the wall.

[0015] Preferably, the implementation process of star map matching and positioning in step 2) is as follows:

[0016] 2.1) 4-point combination sampling: Group global features according to semantic labels, traverse each group of feature points, query the nearest neighbor points of a pre-set number of neighbor points, and randomly select 3 points to form a 4-point combination with the current point;

[0017] 2.2) Construction of rotation-invariant descriptors:

[0018] 2.2.1) Principal axis selection: Calculate the distance between all two points in the 4-point combination. , For composite interior point indexes, Select the edge corresponding to the maximum distance as the principal axis, denoted as edge AB, and calculate the principal axis vector. ;

[0019] 2.2.2) Construction of rotation matrix: Constructing the rotation matrix ,in The X and Y components are the principal axis quantities. The magnitude of the principal axis vector;

[0020] 2.2.3) Local coordinate transformation: Define the local coordinate transformation function. Transform the two remaining points C and D in the combination of four points (excluding A and B) to the local coordinate system to obtain the local coordinates. , ;

[0021] 2.2.4) 4D Descriptor Generation: Constructing rotation-invariant feature descriptors using the X and Y components of local coordinates. ;

[0022] 2.3) Matching and Pose Estimation: Local descriptors are queried through the global star map index, and matching pairs with semantic consistency and descriptor distance less than a pre-set descriptor distance threshold are selected; the initial pose is estimated by combining the Kabsch algorithm (decentralization, SVD decomposition, rotation matrix correction, translation vector calculation).

[0023] Preferably, the rotation matrix in step 2.2.2) Its function is to make the principal axis vector Mapped to the horizontal right direction ,satisfy To eliminate the influence of the positioning subject's pose rotation on the descriptor; in step 2.2.3), the local coordinate transformation is achieved by subtracting the coordinates of the principal axis starting point A. By eliminating translation interference, the translation-rotation double invariance of the descriptor is finally achieved.

[0024] Preferably, the implementation process of the Kabsch algorithm in step 2.3) is as follows: calculate the local matching point set. Matching point set with the global set The center point is then decentered, and the covariance matrix is ​​constructed. , For the number of matching points, Perform SVD decomposition and correct the rotation matrix to ensure that the determinant is greater than 0. Calculate the translation vector based on the center point and the rotation matrix to obtain the pose transformation relationship.

[0025] Preferably, the implementation process of star map-semantic particle filter fusion localization is as follows:

[0026] 3.1) Particle filter initialization: Based on the initial pose of the star map, generate position deviation conformance... Angular deviation conforms The set of particles, in which, To pre-set the standard deviation of the positional deviation, To pre-determine the standard deviation of the angle deviation, the initial weight is 0.002; set the model parameters, including geometric variance, semantic gain, and effective sample number threshold;

[0027] 3.2) Particle Update and Resampling: Particle pose is updated based on relative motion between frames obtained by odometry; geometric probability is calculated based on pre-set geometric distribution parameters combined with local observations and global map; similarity is calculated by class matching and radius similarity of semantic factors; particle weights are updated and normalized; when the number of effective samples is less than the pre-set threshold for the number of effective samples, system resampling is adopted.

[0028] 3.3) Star map matching coordination: Star map matching is triggered every pre-set star map matching interval frame or when a particle fails. When the matching score is greater than or equal to the pre-set matching score threshold, the particle distribution standard deviation is dynamically adjusted with a new pose and the particle set is reset. When the number of effective samples is less than the pre-set effective sample number threshold, the particle fails.

[0029] Preferably, the calculation process for the semantic factor in step 3.2) is as follows: category matching factor , This is a semantic label consistency indicator function. To observe semantic tags, For global semantic labels; feature similarity factor of real poles , Mapping radius cosine similarity to Interval results; combined semantic factor feature similarity weights feature similarity weights The semantic gain and feature similarity weight are both preset parameters.

[0030] Preferably, the system resampling process in step 3.2) is as follows: calculate particle weights. accumulation and , Generate uniformly distributed sampling locations for particle indexing. The sampling interval is 0.002; the sampling positions are traversed, and the particle whose weight accumulation first covers the position is selected as the new particle, and the weight of the new particle is set to 0.002.

[0031] Preferably, during particle reset in step 3.3), the positional standard deviation of the particle distribution is... Angular standard deviation ,based on and Generate a new set of particles and reset the weights to a uniform distribution.

[0032] This invention provides global constraints through virtual prisms, and combines star map matching and semantic particle filtering to effectively solve the problem of cumulative positioning error in GPS denied environments. It has high precision, high robustness and real-time performance, and can be widely used in autonomous positioning scenarios for robots and autonomous vehicles. Attached Figure Description

[0033] Figure 1 This is the basic flowchart provided by the present invention;

[0034] Figure 2 This invention provides Figure 1 Image showing the results of feature extraction from the virtual prism.

[0035] Figure 3 This invention provides Figure 1 Virtual Prism Feature Clustering Results

[0036] Figure 4 This invention provides Figure 1 Flowchart for global map construction. Detailed Implementation

[0037] The following specific embodiments illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0038] like Figures 1-4 As shown, this invention provides a co-localization method based on star map matching and semantic particle filtering using virtual prism features, comprising the following steps:

[0039] 1) Constructing a global map and local observations:

[0040] 1.1) Obtain the original 3D point cloud data of the localization scene and preprocess it by voxel downsampling (voxel size 0.1 unit) and normal estimation (search radius 1.0 unit, maximum number of neighborhood points 30);

[0041] 1.2) Extracting Virtual Prism Features: Calculate the dot product of the point cloud normal and the ground normal (direction vector [0,0,1]), and filter the facade point clouds with an absolute dot product value < 0.3; for the facade point clouds, use a planar detection algorithm (normal variance threshold 10°, coplanarity threshold 10°, minimum number of points 20) to detect the walls, solve for the intersection lines of non-parallel walls (absolute dot product value of normal ≤ 0.95), and take the midpoint of the endpoints of the intersection lines within the height range (lower limit -2 units to upper limit 3 units) as the candidate centers of the virtual prism; cluster the candidate centers based on density peak clustering (DPC) to obtain the virtual prism features (center XY coordinates, semantic label 0, radius 0.01 units).

[0042] 1.3) Extracting real rod features: Perform spherical projection on the original point cloud (vertical field of view -25° to 3°, projection resolution 64×500), extract real rods by clustering (minimum number of points 3, clustering distance threshold 0.2 units) and circle fitting (radius 0.02-0.4 units), set semantic label 1, and store its center XY coordinates and radius;

[0043] 1.4) Constructing a global map and local observations: Merge features from multiple frames to form a global map (including the 2D position, semantic labels, and radius of the virtual prism / real pole); divide keyframes according to the driving distance of the positioning subject (every 1.5 units), and extract local observation data (virtual prism / real pole observations) from each frame.

[0044] 2) Star map matching index construction:

[0045] 2.1) Semantic grouping: Global map features are grouped according to semantic labels (virtual prism 0, real pole 1), and a neighborhood search index is constructed for each group;

[0046] 2.2) Quad sampling: Traverse each feature point in the group, query the 20 nearest neighbor points (excluding itself), skip if there are less than 3 neighbor points; randomly select 3 points from the neighbor points to form a 4-point combination with the current point, and sample no more than 50 combinations for each feature point;

[0047] 2.3) Construction of rotation-invariant descriptors: Calculate the distance between all two points in the 4-point combination, select the longest side as the principal axis (AB), and construct the rotation matrix. ( (Using the principal axis vector), map the principal axis to the (1,0) direction; define the local coordinate function. Transform the other two points (C and D) to the local coordinate system to generate a descriptor. ;

[0048] 2.4) Index building: Collect all descriptors, corresponding global indexes and semantic tags, and build a global star map index through the neighborhood search index for subsequent matching queries.

[0049] 3) Initial pose and failure reset positioning:

[0050] 3.1) Initial pose estimation: Take the first frame of local observation data and repeat steps 2.1)-2.3) to construct local descriptors; query local descriptors through the global star map index and filter matching pairs with semantic consistency and descriptor distance < 0.05;

[0051] 3.2) Kabsch algorithm for pose calculation: For the local point (src) and global point (dst) of the matching pair, calculate the center point. , (M is the number of matching points), resulting in a decentralized point set. , Calculate the covariance matrix SVD decomposition yields Construct rotation matrix (like Then adjust (Last column of symbols), translation vector To obtain the initial pose ;

[0052] 3.3) Failure Reset: In subsequent localization, if the number of valid samples from the semantic particle filter... If the star map matching is triggered and steps 3.1-3.2 are repeated, and the matching score (number of valid matching points) is ≥5, the particle set is reset with a new pose.

[0053] 4) Semantic particle filter initialization:

[0054] 4.1) Particle Generation: Set the number of particles to 500, and each particle is generated using a 4×4 homogeneous transformation matrix. Indicate candidate pose; Based on the baseline, generate positional deviation. ( (Gaussian distribution), angular deviation ,pass ( Generate a particle set (for the bias homogeneous matrix);

[0055] 4.2) Weight and Parameter Settings: Initial Weights Set the observation model parameters: geometric variance semantic gain Feature similarity weights Valid sample size threshold .

[0056] 5) Particle filter continuous tracking:

[0057] 5.1) Motion Update: Obtain inter-frame relative motion via odometry Calculate motion covariance Add Gaussian noise to each particle and update its pose. ;

[0058] 5.2) Observation Update: Transform local observation features to the global coordinate system and query the global index to obtain the nearest neighbor distance. Cut off Calculate geometric probability ;

[0059] 5.3) Semantic factor calculation: Category matching factor (When a=b, (Otherwise, it is 0); similarity factor of true rod features ( (Radius cosine similarity); combined semantic factors ;

[0060] 5.4) Weight Update: Normalization ;

[0061] 5.5) Resampling: If System resampling is employed: sampling positions are generated. Select weighted accumulation and coverage The particles, with new weights set to .

[0062] 6) Synergy between star map matching and particle filtering:

[0063] 6.1) Triggering condition: Star map matching is triggered every 10 frames of local observation (fixed interval). When the value is below a preset threshold, star map matching is triggered (particle failure).

[0064] 6.2) Particle Reset: When the matching score is ≥5, use the new pose. Based on the standard deviation of position, the position is dynamically adjusted. Angular standard deviation (S is the matching score), generate a new particle set and reset the weights.

[0065] 7) Output optimized trajectory:

[0066] 7.1) Pose estimation: Select the 10% of particles with the highest weight in each frame and calculate the weighted average pose (position). ,angle );

[0067] 7.2) Missing padding: For frames whose poses were not obtained, "forward padding + backward padding" is used to fill in the missing frames;

[0068] 7.3) Trajectory Output: Stores the estimated pose of all frames and outputs it in CSV or TUM format for localization, navigation, or result evaluation.

[0069] This embodiment will combine Figures 1-4 Taking the positioning scenario of an autonomous vehicle in an underground parking garage (GPS denied environment) as an example, the technical solution of the present invention will be described in detail. The hardware used includes Velodyne VLP-16 LiDAR (10Hz scanning frequency), wheel odometer (100Hz sampling rate), and industrial control computer (Intel i7-0K, 32GB memory). The software environment is Ubuntu 20.04 LTS, ROS Noetic, and PCL 1.12.1.

[0070] Step 1: Global Map Construction:

[0071] 1.1) Preprocessing of raw point cloud: The point cloud (PCD format, approximately 100,000 points per frame) of the LiDAR was collected along the preset trajectory (3 laps, total distance 1.2km) of the underground parking garage. Voxel downsampling (voxel size 0.1m) was performed to reduce the point cloud to 30,000 to 40,000 points per frame. The point cloud normal was estimated using KDTreeSearchParamHybrid (radius=1.0m, max_nn=30), with a normal error of <5°.

[0072] 1.2) Facade Point Cloud Filtering: Calculate the dot product of the point cloud normal and the ground normal ([0,0,1]) and filter. The facade point cloud (accounting for 25%-35% of the original data) was cleaned to remove ground interference;

[0073] 1.3) Wall Surface Detection: For the facade point cloud, call the PCL's segment_plane function (normal variance threshold 10°, coplanarity threshold 10°, minimum number of points 200) to detect the walls and obtain the wall normals. Center point ,size For planes with less than 200 filter points, retain 3-5 valid wall surfaces per frame;

[0074] 1.4) Wall intersection calculation: Traverse each pair of walls, if If the planes are parallel, skip this step; otherwise, solve the simultaneous plane equations. Solve for the intersection line and cut off the line segment with a height of 0.5m to 2.0m, and take the midpoint as the candidate center of the virtual prism;

[0075] 1.5) DPC Clustering Optimization: The DBSCAN algorithm is used to cluster candidate centers (neighborhood radius 0.3m, minimum number of points 5), and the coordinates of the cluster centers are calculated. With a radius of 0.05m and a semantic label of 0, a virtual prism feature set is formed;

[0076] 1.6) Real rod extraction: Perform cylindrical segmentation on the original point cloud (minimum radius 0.05m, maximum radius 0.5m), fit the cylindrical surface to obtain the center and radius, retain cylinders with height > 0.5m, and set semantic label 1;

[0077] 1.7) Global map generation: Merge the features of 100 frames, cluster the virtual prisms at 0.5m intervals and the real poles at radii ±0.05m intervals to construct a global map (containing 20 virtual prisms and 15 real poles), and store it in JSON format (fields: ID, x, y, s, r).

[0078] Step 2: Constructing the star map matching index:

[0079] 2.1) Semantic Grouping: The global features are divided into two groups according to semantic labels (virtual prism group s=0, real pole group s=1), and a KDTree index is constructed for each group;

[0080] 2.2) 4-point combination sampling: Traverse each feature point in each group, query the 20 nearest neighbors, and skip if the number of neighbors is less than 3 after removing the feature point itself; 30 4-point combinations are randomly generated for each feature point, and a total of about 12,000 combinations are generated globally;

[0081] 2.3) Descriptor Construction: For each combination, calculate the distances between 6 pairs of points, select the longest side as the principal axis AB, and calculate the vector. Construct rotation matrix ,verify Transform points C and D to the local coordinate system and generate a descriptor. ;

[0082] 2.4) Index building: Collect all descriptors, build a FLANN KDTree index, and store it as a .bin file. The time for a single query is <0.01s.

[0083] Step 3: Initial pose estimation:

[0084] 3.1) Local feature extraction: Take the first frame of laser point cloud and repeat steps 1.1)-1.6) to obtain local features of 5-8 virtual prisms and 1-2 real rods;

[0085] 3.2) Descriptor matching: Generate local descriptors, query the 5 most recent global descriptors through the global star map index, filter matching pairs with "semantic consistency + descriptor distance < 0.05", and retain results with ≥ 8 matching pairs;

[0086] 3.3) Kabsch pose calculation: For the matched pair, calculate the local center point. Global center point Decentralization , ;calculate SVD decomposition yields , , build ( ), Initial pose The verification matching score is 8, and the position error is <0.5m.

[0087] Step 4: Semantic particle filter initialization:

[0088] 4.1) Particle Generation: Generates 500 particles with positional deviation. , , angular deviation ,pass Generate particle sets;

[0089] 4.2) Parameter Settings: Initial Weights Geometric variance semantic gain Feature similarity weight .

[0090] Step 5: Particle Filter Continuous Tracking:

[0091] 5.1) Motion Update: Odometry acquires relative motion , , covariance Add noise to the particles and update their pose. ;

[0092] 5.2) Observation Update: Local Features (s=0, r=0.05m) Transform to the global coordinate system and query the nearest neighbor distance. , Category matching (s=0 matches), No real rod, Weight After normalization ;

[0093] 5.3) Resampling: Calculation No resampling is required.

[0094] Step 6: Collaborative Optimization

[0095] 6.1) Star map triggering: Particle filter failure triggers star map matching, obtaining a new pose. ;

[0096] 6.2) Particle Reset: , Generate a new set of particles with a weight of 0.002;

[0097] Step 7: Trajectory Output: Estimate the pose for each frame, fill in the missing frames, and output the trajectory. The position error RMSE for a 1km driving distance is 0.42m, the angle error RMSE is 2.1°, and the frame rate is 12Hz, which meets the requirements of autonomous driving.

[0098] This embodiment demonstrates that the present invention provides global constraints through virtual prisms, and the combination of star map matching and semantic particle filtering effectively solves the problem of cumulative positioning error in GPS denied environments. It has high precision, high robustness and real-time performance, and can be widely applied to autonomous positioning scenarios of robots and autonomous vehicles.

[0099] Although the present invention has been described in detail above with general descriptions and specific embodiments, modifications or improvements can be made to it, which will be obvious to those skilled in the art. Therefore, all such modifications or improvements made without departing from the spirit of the present invention fall within the scope of protection claimed by the present invention.

Claims

1. A co-localization method based on virtual feature-based star map matching and semantic particle filtering, characterized in that: Includes the following steps: 1) Virtual feature extraction and map construction: Obtain the original 3D point cloud data of the environment, extract virtual features and real pole features after preprocessing, and construct a global map and multi-frame local observation data based on the virtual features and real pole features; 2) Star map matching localization: Based on global map features, four-point combinations are sampled and rotation-invariant feature descriptors are constructed to build a global star map index; the four-point combinations and rotation-invariant feature descriptors are repeated for local observation data, and the global index is queried through star map matching. The initial pose of the localized subject is estimated by combining the Kabsch algorithm; the sampling method of the four-point combination is as follows: global features are grouped according to semantic labels, each group of feature points is traversed, the nearest neighbor points of a pre-set number of neighboring points are queried, and three points are randomly selected to form a four-point combination with the current point; 3) Star map-semantic particle filter fusion localization: Initialize the semantic particle filter with the initial pose output by star map matching to generate a particle set, uniform weights and model parameters that conform to the preset distribution; update the particle pose based on odometry, and calculate the particle weight by combining the geometric, semantic and radius features of local observation and global map; set the fixed interval of star map matching and particle failure triggering conditions, and reset the particle distribution after successful matching; store the particle filter estimated pose for each frame, fill in the missing poses and output the optimized trajectory.

2. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 1, characterized in that: In step 1), the virtual features are virtual prism features obtained through facade segmentation, wall detection, wall intersection calculation, and cluster optimization. The process is as follows: (1.1) Facade segmentation: Calculate the point cloud normal and the ground normal. The dot product is used to filter points whose absolute dot product value is less than a pre-set facade filtering threshold as facade point clouds. (1.2) Wall detection: For the elevation point cloud, a planar detection algorithm is used to obtain the wall normal, center point and size. The number of filtered points is less than the pre-set minimum number of planar points. (1.3) Calculation of wall intersection line: The absolute value of the dot product of the normal line is less than or equal to the preset wall parallel judgment threshold. For non-parallel walls, the intersection line is solved by plane equation. The lower limit of the virtual prism height is preset to the upper limit of the virtual prism height. The midpoint of the endpoint of the intersection line within the preset height range is taken as the candidate center of the virtual prism. (1.4) Clustering optimization: Based on density peak clustering (DPC), candidate centers are clustered to obtain a virtual prism feature set. The virtual prism feature set includes the center XY coordinates, semantic label 0, and radius taken as the pre-set virtual prism radius.

3. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 2, characterized in that: In step 1.2), when using a planar detection algorithm for wall detection, the neighborhood search parameter is set to K-nearest neighbor search, and the number of nearest neighbor points is set to the pre-set number of nearest neighbor points for wall detection. For each planar candidate, the point cloud index within the bounding box is extracted, the median of the point cloud normal corresponding to the index is calculated and normalized, and used as the representative normal of the wall.

4. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 1, characterized in that: The implementation process of star map matching and positioning in step 2) is as follows: (2.1) 4-point combination sampling; (2.2) Construction of rotation-invariant descriptors: (2.2.1) Principal axis selection: Calculate the distance between all two points in the 4-point combination. , For composite interior point indexes, Select the edge corresponding to the maximum distance as the principal axis, denoted as edge AB, and calculate the principal axis vector. , Represent the coordinates of points A and B; (2.2.2) Construction of rotation matrix: Constructing the rotation matrix ,in The X and Y components are the principal axis quantities. The magnitude of the principal axis vector; (2.2.3) Local coordinate transformation: Define the local coordinate transformation function , The coordinates of the starting point A of the main axis. Let A and B represent the coordinates of the points to be transformed into the local coordinate system. Then, transform the remaining two points C and D (excluding A and B) in the four-point combination to the local coordinate system to obtain the local coordinates. , ; (2.2.4) 4D descriptor generation: Construct rotation-invariant feature descriptors using the X and Y components of local coordinates. ; (2.3) Matching and pose estimation: Local descriptors are queried through the global star map index, and matching pairs with semantic consistency and descriptor distance less than a pre-set descriptor distance threshold are selected; the initial pose is estimated by combining the Kabsch algorithm.

5. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 4, characterized in that: The implementation process of the Kabsch algorithm in step 2.3) is as follows: calculate the local matching point set. Matching point set with the global set The center point is then decentered, and the covariance matrix is ​​constructed. , For the number of matching points, Perform SVD decomposition and correct the rotation matrix to ensure that the determinant is greater than 0. Calculate the translation vector based on the center point and the rotation matrix to obtain the pose transformation relationship.

6. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 1, characterized in that: The implementation process of star map-semantic particle filter fusion localization is as follows: (3.1) Particle filter initialization: Based on the initial pose of the star map, generate position deviations that conform to... Angular deviation conforms The set of particles, in which, To pre-set the standard deviation of the positional deviation, The standard deviation of the angle deviation is preset; the initial weight is 0.002; the model parameters are set, including geometric variance, semantic gain, and effective sample number threshold. (3.2) Particle update and resampling: Particle pose is updated based on the relative motion between frames obtained by odometry; geometric probability is calculated based on pre-set geometric distribution parameters combined with local observation and global map; similarity is calculated by class matching and radius similarity of semantic factors; particle weights are updated and normalized; when the number of effective samples is less than the pre-set threshold for the number of effective samples, system resampling is adopted. (3.3) Star map matching coordination: Star map matching is triggered every pre-set star map matching interval frame or when a particle fails. When the matching score is greater than or equal to the pre-set matching score threshold, the particle distribution standard deviation is dynamically adjusted with a new pose and the particle set is reset. When the number of effective samples is less than the pre-set effective sample number threshold, the particle fails.

7. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 6, characterized in that: The calculation process for semantic factors in step 3.2) is as follows: Category matching factor , This is a semantic label consistency indicator function. To observe semantic tags, For global semantic labels; feature similarity factor of real poles , Mapping radius cosine similarity to Interval results; similarity weights based on comprehensive semantic factor features The semantic gain and feature similarity weight are both preset parameters.

8. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 6, characterized in that: The system resampling process in step 3.2) is as follows: calculate the particle weights. accumulation and , Generate uniformly distributed sampling locations for particle indexing. The sampling interval is 0.002; the sampling positions are traversed, and the particle whose weight accumulation first covers the position is selected as the new particle, and the weight of the new particle is set to 0.

002.

9. The star map matching and semantic particle filtering co-localization method based on virtual features according to claim 6, characterized in that: In step 3.3), during particle reset, the positional standard deviation of the particle distribution. Angular standard deviation ,based on and Generate a new set of particles and reset the weights to a uniform distribution.

Images