A laser odometer positioning optimization algorithm based on OSM building information

The laser odometer positioning optimization algorithm addresses cumulative drift and environmental inaccuracies by using OSM building information for precise vehicle positioning, enhancing accuracy and reducing memory requirements.

CN118730159BActive Publication Date: 2024-12-03HOHAI UNIV
View PDF 5 Cites 0 Cited by

Patent Information

Application Number
CN202411224595.9
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-09-03
Publication Date
2024-12-03
Estimated Expiration
2044-09-03

AI Technical Summary

Technical Problem

Traditional laser SLAM methods suffer from cumulative drift errors in long-distance driving and require loop detection, limiting vehicle trajectory, while GNSS-based positioning is inaccurate in obstructed environments.

Method used

A laser odometer positioning optimization algorithm using OSM building information, involving interpolation, semantic segmentation, rotation-invariant feature matching, particle swarm optimization, and KL divergence for accurate positioning.

Benefits of technology

Ensures consistent global accuracy and reduces cumulative errors, providing efficient and accurate positioning in GPS-denied environments with reduced memory and cost.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118730159B_ABST
    Figure CN118730159B_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of navigation self-positioning, and relates to a laser odometer positioning optimization algorithm based on OSM building information. The method utilizes OSM interpolation road points to generate OSM feature descriptors; utilizes frame point clouds to generate point cloud feature descriptors; utilizes a binary conversion method to convert OSM feature descriptors and point cloud feature descriptors into rotationally invariant OSM feature descriptors and point cloud feature descriptors; utilizes a two-stage screening method to obtain a matching candidate set of interpolated road points; utilizes a particle swarm to obtain a robot posture motion equation and determine a motion model; utilizes a motion model to predict the particle swarm motion posture, obtains edge points of a point cloud, utilizes distance errors to construct a similarity model, and utilizes the similarity model to construct a particle weight model; utilizes a probability distribution method to resample the particle swarm, and outputs a mean posture of a stable particle swarm as a final posture to obtain a positioning trajectory.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of navigation self-positioning, and in particular relates to a laser odometer positioning optimization algorithm based on OSM building information. Background Art

[0002] With the development of self-driving cars, autonomous positioning technology, as one of the cutting-edge development technologies, has become a hot research direction, especially the autonomous positioning algorithm in the global positioning system denial environment is the focus of research. The traditional method of positioning and mapping through GNSS relies on receiving satellite signals. In environments with many obstructions such as indoors, underground or high-rise areas, the positioning and mapping errors increase, and the mapping effect is often unsatisfactory. The emergence of laser SLAM filtering navigation and positioning algorithm has solved this problem well. However, the laser radar odometer uses the laser radar point cloud information as input and uses inter-frame matching to achieve vehicle positioning. Long-distance driving will produce a lot of cumulative drift. Usually, the laser SLAM method is to eliminate the cumulative error by constructing a global map and using loop detection, but this will greatly limit the vehicle's running trajectory. The OSM map stores a large amount of necessary map information, and the OSM map information can provide coordinate information with consistent global accuracy. To address this problem, the inventor proposes a laser odometer positioning optimization algorithm based on OSM building information. Only the building information in the OSM map is used to constrain the laser radar odometer, which overcomes the shortcomings of traditional methods such as low accuracy and high requirements for applicable environments. It effectively solves the problem of accumulated errors of the laser radar odometer and can ensure that the global accuracy of positioning is consistent with that of the generated map. Summary of the invention

[0003] The purpose of the present invention is to overcome the deficiencies in the prior art and provide a laser odometer positioning optimization algorithm based on OSM building information.

[0004] In order to achieve the purpose of the present invention, the present invention will be implemented by adopting the following technical solutions.

[0005] A laser odometer positioning optimization algorithm based on OSM building information includes the following steps:

[0006] S1. Using the interpolation method, interpolate the road points of the OSM map, and create an OSM map feature descriptor according to the features of the OSM map;

[0007] S2. Use the PointNet++ algorithm to perform semantic segmentation and extraction of building point clouds on the scanned frame point clouds, and create feature descriptors of laser point clouds based on the features of the building laser point clouds;

[0008] The laser odometer positioning optimization algorithm also includes the following steps:

[0009] S3, using a binary conversion method, converting the OSM map feature descriptor and the building laser point cloud feature descriptor into a rotation-invariant feature descriptor of the OSM map and a rotation-invariant feature descriptor of the building laser point cloud;

[0010] S4, using a two-stage screening method, firstly perform a preliminary screening on the rotation invariant feature descriptors of the OSM map and the rotation invariant feature descriptors of the laser point cloud, and then perform a fine matching on the OSM map feature descriptors and the laser point cloud feature descriptors to obtain a matching candidate set of interpolated road points;

[0011] S5. Use particle swarm transformation to generate Gaussian distribution to initialize particles that conform to Gaussian distribution, obtain the motion equation that conforms to the robot's posture through the odometer of the laser radar used, and determine the motion model;

[0012] S6. Use the robot's motion model to predict the particle swarm's motion posture, identify the corner points in the scanned building point cloud by the smoothness of the points, calculate the distance error between the corner points and the OSM map interpolation road points by minimizing the Euclidean distance, build a similarity model between the laser point cloud and the OSM map, and use the similarity model to construct a particle weight model, and use the particle weight model to update and normalize the particles;

[0013] S7. The probability distribution method based on KL divergence is used to resample the particle swarm, and the mean pose of the stable particle swarm is output as the optimized pose to obtain the optimized positioning trajectory.

[0014] As a preferred solution of the present invention, the implementation process of step S1 includes the following steps:

[0015] S11, set the threshold value, n Interpolate road points at an interval of meters and generate a UTM coordinate system for each interpolated road point Generate an OSM map feature descriptor, representing the tuple of OSM map building edges as and the interpolated road points in the UTM coordinate system are expressed as ;

[0016] S12, divide 360° into 360 equal parts at 1° intervals, and search from each angle Find the shortest distance to the edge of a building :

[0017] ,

[0018] In the formula, Refers to the distance from the current position to the building edge t; , Corresponding to the minimum and maximum angles at the top of the building; the horizontal axis and the interpolated road point Constructing vertices with buildings Between the line segments, , represents the angle between the target angle direction and the building t; if the building t is not found in the target angle direction, then is considered as 0; when When , that is, when the target angle is between the two vertices of the building edge t, calculate ; and define the feature descriptor of the OSM map as .

[0019] As a preferred solution of the present invention, the implementation process of step S2 includes the following steps:

[0020] S21. Use a laser beam parallel to the ground in the laser radar scanning line to scan the building, use the frame point cloud scanned by this single-line laser as the data source of the laser point cloud feature descriptor, and convert the frame point cloud format into a format that can be recognized by the PointNet++ algorithm;

[0021] S22. Use the PointNet++ algorithm to perform semantic segmentation of buildings on each frame of point cloud scanned by the LiDAR and extract them separately;

[0022] S23, according to the building frame point cloud extracted in step S22, based on the point features of the laser radar scanning data, by calculating the building point The shortest distance To create a frame point cloud feature descriptor ,in Indicates the point to the building distance.

[0023] As a preferred solution of the present invention, the implementation process of step S3 includes the following steps:

[0024] S31, using the binary conversion method, by converting the distance value between the OSM map descriptor of each one-dimensional vector and the building frame point cloud feature descriptor Convert to binary format, that is, each column vector of the matrix is ​​a distance value in the feature descriptor The binary representation of the matrix is ;in, is an operator, which means rounding up; Indicates the maximum distance value between the OSM map feature descriptor and the building point cloud feature descriptor; ,

[0025] in, is the distance value The binary representation of

[0026] S32, sum the two-dimensional features of all angles in each row to construct the rotation invariant feature descriptor of the OSM map Rotation-invariant feature descriptors for laser point clouds of buildings And the rotation-invariant descriptor set for OSM maps .

[0027] As a preferred solution of the present invention, the implementation process of step S4 includes the following steps:

[0028] S41, set the threshold n, and convert the building point cloud feature descriptor to be found The corresponding rotation invariant descriptor A rotation-invariant descriptor set for OSM maps Compare, sort by absolute value and extract the top similarity Rotation-invariant descriptors for OSM maps ;

[0029] ,

[0030] The difference between the point cloud feature vector and the OSM map feature vector is measured by L1 norm to achieve the sparsity of the feature vector and provide candidates for sorting;

[0031] S42, using the N-gram model The corresponding front OSM map feature descriptors The building point cloud feature descriptor to be queried Compare to complete the reordering of descriptors; set a threshold to filter out matching candidate sets of interpolation road points that meet the conditions;

[0032] ,

[0033] in Represents the feature descriptor for building point cloud With OSM map feature descriptors , the same distance value The frequency of occurrence in the N-gram model.

[0034] As a preferred solution of the present invention, the implementation process of step S5 includes the following steps:

[0035] S51. Use Box-Muller transformation to generate Gaussian distribution to initialize N particles that conform to Gaussian distribution, each particle represents the initial position and posture of the robot, and the initial weight of each particle is equal;

[0036] S52, obtain the motion equation through the robot odometer and IMU information and the frame matching algorithm in the SLAM algorithm ;

[0037] Assume that the position of the car at time t (frame k) is , expressed by six degrees of freedom , respectively represent the position (x, y, z) coordinates and attitude (roll, pitch, yaw) in the OSM map coordinate system;

[0038] According to the motion equation, the change of the car's posture from frame k-1 to frame k is expressed as:

[0039] ;

[0040] ;

[0041] ;

[0042] Represents the coordinate transformation from the k-1th frame laser radar coordinate system to the OSM map coordinate system;

[0043] ,

[0044] In the formula, , , is the position coordinate of the kth frame in the OSM map coordinate system, , , is the position coordinate of the k-1th frame in the OSM map coordinate system, , , represents the position change of the car between the kth frame and the k-1th frame in the k-1th frame laser radar coordinate system. Similarly, , , is the pose of the kth frame in the OSM map coordinate system, , , is the pose of the k-1th frame in the OSM map coordinate system, , , represents the change in the position of the car between the kth frame and the k-1th frame in the k-1th frame laser radar coordinate system. To predict noise, a Gaussian distribution is required.

[0045] As a preferred solution of the present invention, the implementation process of step S6 includes the following steps:

[0046] S61, setting a threshold, using the smoothness of the point to identify the edge points, i.e., the corner points, in the scanned building point cloud; calculating the plane smoothness according to the curvature of the point as an indicator for extracting the feature information of the building point cloud in the current frame; sorting the points in a scan according to the curvature value, selecting the point with the largest curvature as the edge feature point, and the curvature of the edge feature point needs to be greater than the threshold; points near the already selected feature points are no longer considered;

[0047] ,

[0048] In the formula, c is the plane smoothness of the point, S is the The surrounding neighborhood point set, the size of the set represents the number of these neighborhood points; For point The size or norm of ; For point With each neighboring point in the set S The sum of the Euclidean distances between ;

[0049] S62, calculating the distance error between the selected turning point and the OSM map node by minimizing the Euclidean distance;

[0050] ,

[0051] In the formula, For point With point The Euclidean distance between them; in the present invention, the dimension is 2, so ;

[0052] S63. Use the calculated distance error to build a similarity model between the point cloud and the OSM map ;

[0053] ,

[0054] The larger the distance, the greater the error and the smaller the weight; Similarity model The meaning of is to match the building within the threshold range of the current point. The farther the actual building is from the point cloud, the less credible the current pose is.

[0055] S64. Use similarity model to construct particle weight model :

[0056] ,

[0057] Representative The weight of a particle, Represents the similarity model of scanning a frame of building point cloud Number, parameters ;

[0058] S65, through the particle weight model Update the weights of N particles and normalize them:

[0059] .

[0060] As a preferred solution of the present invention, the implementation process of step S7 includes the following steps:

[0061] S71, using the probability distribution method based on KL divergence, the particles are distributed according to their weights. Resampling is performed to ensure that the particles generated by resampling move toward the positions of high-weight particles to obtain a new particle set. ;

[0062] S72. Output the pose mean of the new particle set as the optimized pose, as the final positioning result:

[0063] ,

[0064] In the formula, is the particle pose after resampling; For the weight model The weight value after normalization; is the average pose, that is, the final output pose.

[0065] Beneficial effects: (1) The present invention utilizes lightweight OpenStreetMap, and the system mainly stores information on building vertices and edges in the OSM map, so it only requires lower cost and memory while ensuring positioning accuracy.

[0066] (2) The present invention adopts a road point interpolation method. By matching the feature descriptors between the OSM map and the building point cloud scanned at the interpolation point, it can avoid the problems of low positioning efficiency and low positioning accuracy caused by the uncertainty of the initial pose during the global positioning process, and ensure that the system can rely on highly reliable environmental features for efficient and accurate positioning.

[0067] (3) The present invention uses a particle filter method with the help of a laser radar odometer and OSM map to eliminate the cumulative error generated by the laser radar, and has good positioning accuracy and real-time performance. This system can be used in GPS-denied environments, free from dependence on point cloud maps, and has broad market prospects and application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 This is a flowchart of the laser odometer positioning optimization algorithm of the present invention;

[0069] Figure 2 Schematic diagram of search angles for OSM feature descriptors, where , is a tuple of building edges t in the vector map, is the interpolation point position in the UTM coordinate system, , They correspond to the two angles from the starting search position to the two ends of the building, and the search angles of the OSM feature descriptor;

[0070] Figure 3 This is a schematic diagram of the distance from the OSM feature descriptor interpolation point to the building edge, where , , is a tuple of building edges t in the vector map, is the interpolation point position in the UTM coordinate system, Refers to the distance from the current interpolation point to the two ends of the building edge t, d is the distance from the interpolation point to the building t along the search angle direction; , They represent the angle between the search angle direction and the direction from the interpolation point to the two end points of the building edge;

[0071] Figure 4 It is a schematic diagram of constructing a building point cloud feature descriptor in the present invention;

[0072] Figure 5 It is a schematic diagram of the curvature calculation of the edge points of a building in the present invention. DETAILED DESCRIPTION

[0073] The technical solution of the present invention will be clearly and completely described below in combination with the embodiments of the present invention and the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0074] As an embodiment of the present invention, Figures 1 to 5 As shown in FIG. 1 , a laser odometer positioning optimization algorithm based on OSM building information includes the following steps:

[0075] S1. Using the interpolation method, interpolate the road points of the OSM map, and create an OSM map feature descriptor according to the features of the OSM map;

[0076] S2. Use the PointNet++ algorithm to perform semantic segmentation and extraction of building point clouds on the scanned frame point clouds, and create laser point cloud feature descriptors based on the features of the building laser point clouds;

[0077] S3, using a binary conversion method, converting the OSM map feature descriptor and the building laser point cloud feature descriptor into a rotation-invariant feature descriptor of the OSM map and a rotation-invariant feature descriptor of the building laser point cloud;

[0078] S4, using a two-stage screening method, firstly perform a preliminary screening on the rotation invariant feature descriptors of the OSM map and the rotation invariant feature descriptors of the laser point cloud, and then perform a fine matching on the OSM map feature descriptors and the laser point cloud feature descriptors to obtain a matching candidate set of interpolated road points;

[0079] S5. Use particle swarm transformation to generate Gaussian distribution to initialize particles that conform to Gaussian distribution, obtain the motion equation that conforms to the robot's posture through the odometer of the laser radar used, and determine the motion model;

[0080] S6. Use the robot's motion model to predict the particle swarm's motion posture, identify the corner points in the scanned building point cloud by the smoothness of the points, calculate the distance error between the corner points and the OSM map interpolation road points by minimizing the Euclidean distance, build a similarity model between the laser point cloud and the OSM map, and use the similarity model to construct a particle weight model, and use the particle weight model to update and normalize the particles;

[0081] S7. The probability distribution method based on KL divergence is used to resample the particle swarm, and the mean pose of the stable particle swarm is output as the optimized pose to obtain the optimized positioning trajectory.

[0082] As an embodiment of the present invention, Figure 2 and Figure 3 As shown, the implementation process of step S1 includes the following steps:

[0083] S11, set the threshold value, n Interpolate road points at an interval of meters and generate a UTM coordinate system for each interpolated road point Generate an OSM map feature descriptor, representing the tuple of OSM map building edges as and the interpolated road points in the UTM coordinate system are expressed as ;

[0084] S12, divide 360° into 360 equal parts at 1° intervals, and search from each angle Find the shortest distance to the edge of a building :

[0085] ,

[0086] In the formula, Refers to the distance from the current position to the building edge t; , Corresponding to the minimum and maximum angles at the top of the building; the horizontal axis and the interpolated road point Constructing vertices with buildings Between the line segments, , represents the angle between the target angle direction and the building t; if the building t is not found in the target angle direction, then is considered as 0; when When , that is, when the target angle is between the two vertices of the building edge t, calculate ; and define the feature descriptor of the OSM map as .

[0087] As an embodiment of the present invention, Figure 4 As shown, the implementation process of step S2 includes the following steps:

[0088] S21. Use a laser beam parallel to the ground in the laser radar scanning line to scan the building, use the frame point cloud scanned by this single-line laser as the data source of the laser point cloud feature descriptor, and convert the frame point cloud format into a format that can be recognized by the PointNet++ algorithm;

[0089] S22. Use the PointNet++ algorithm to perform semantic segmentation of buildings on the point clouds scanned by the laser radar and extract them separately.

[0090] S23, according to the building frame point cloud extracted in step S22, based on the point features of the laser radar scanning data, by calculating the building point The shortest distance To create a frame point cloud feature descriptor ,in Indicates the point to the building distance.

[0091] As an embodiment of the present invention, the implementation process of step S3 includes the following steps:

[0092] S31, using the binary conversion method, by converting the distance value between the OSM map descriptor of each one-dimensional vector and the building frame point cloud feature descriptor Convert to binary format, that is, each column vector of the matrix is ​​a distance value in the feature descriptor The binary representation of the matrix is ;in, is an operator, which means rounding up; Indicates the maximum distance value between the OSM map feature descriptor and the building point cloud feature descriptor; ,

[0093] in, is the distance value The binary representation of

[0094] S32, sum the two-dimensional features of all angles in each row to construct the OSM map rotation invariant feature descriptor , OSM map rotation invariant descriptor set Rotation-invariant feature descriptors for building laser point clouds .

[0095] As an embodiment of the present invention, the implementation process of step S4 includes the following steps:

[0096] S41, set the threshold n, and convert the building point cloud feature descriptor to be found The corresponding rotation invariant descriptor A rotation-invariant descriptor set for OSM maps Compare, sort by absolute value and extract the top similarity Rotation-invariant descriptors for OSM maps ;

[0097] ,

[0098] The difference between the point cloud feature vector and the OSM map feature vector is measured by L1 norm to achieve the sparsity of the feature vector and provide candidates for sorting;

[0099] S42, using the N-gram model The corresponding front OSM map feature descriptors The building point cloud feature descriptor to be queried Compare to complete the reordering of descriptors; set a threshold to filter out matching candidate sets of interpolation road points that meet the conditions;

[0100] ,

[0101] in Represents the feature descriptor for building point cloud With OSM map feature descriptors , the same distance value The frequency of occurrence in the N-gram model.

[0102] As an embodiment of the present invention, the implementation process of step S5 includes the following steps:

[0103] S51. Use Box-Muller transformation to generate Gaussian distribution to initialize N particles that conform to Gaussian distribution, each particle represents the initial position and posture of the robot, and the initial weight of each particle is equal;

[0104] S52, obtain the motion equation through the robot odometer and IMU information and the frame matching algorithm in the SLAM algorithm ;

[0105] Assume that the position of the car at time t (frame k) is , expressed by six degrees of freedom , respectively represent the position (x, y, z) coordinates and attitude (roll, pitch, yaw) in the OSM map coordinate system;

[0106] According to the motion equation, the change of the car's posture from frame k-1 to frame k is expressed as:

[0107] ;

[0108] ;

[0109] ;

[0110] Represents the coordinate transformation from the k-1th frame laser radar coordinate system to the OSM map coordinate system;

[0111] ,

[0112] In the formula, , , is the position coordinate of the kth frame in the OSM map coordinate system, , , is the position coordinate of the k-1th frame in the OSM map coordinate system, , , represents the position change of the car between the kth frame and the k-1th frame in the k-1th frame laser radar coordinate system. Similarly, , , is the pose of the kth frame in the OSM map coordinate system, , , is the pose of the k-1th frame in the OSM map coordinate system, , , represents the change in the position of the car between the kth frame and the k-1th frame in the k-1th frame laser radar coordinate system. To predict noise, a Gaussian distribution is required.

[0113] As an embodiment of the present invention, Figure 4 As shown, the implementation process of step S6 includes the following steps:

[0114] S61, setting a threshold, using the smoothness of the point to identify the edge points, i.e., the corner points, in the scanned building point cloud; calculating the plane smoothness according to the curvature of the point as an indicator for extracting the feature information of the building point cloud in the current frame; sorting the points in a scan according to the curvature value, selecting the point with the largest curvature as the edge feature point, and the curvature of the edge feature point needs to be greater than the threshold; points near the already selected feature points are no longer considered;

[0115] ,

[0116] In the formula, c is the plane smoothness of the point, S is the The surrounding neighborhood point set, the size of the set represents the number of these neighborhood points; For point the size (or norm) of ; For point With each neighboring point in the set S The sum of the Euclidean distances between ;

[0117] S62, calculating the distance error between the selected turning point and the OSM map node by minimizing the Euclidean distance;

[0118] ,

[0119] In the formula, For point With point The Euclidean distance between them; in the present invention, the dimension is 2, so ;

[0120] S63. Use the calculated distance error to build a similarity model between the point cloud and the OSM map ;

[0121] ,

[0122] The larger the distance, the greater the error and the smaller the weight. Similarity model The meaning of is to match the building within the threshold range of the current point. The farther the actual building is from the point cloud, the less credible the current pose is.

[0123] S64. Use similarity model to construct particle weight model :

[0124] ,

[0125] Representative The weight of a particle, Represents the similarity model of scanning a frame of building point cloud Number, parameters ;

[0126] S65, through the particle weight model Update the weights of N particles and normalize them:

[0127] .

[0128] As an embodiment of the present invention, the implementation process of step S7 includes the following steps:

[0129] S71, using the probability distribution method based on KL divergence, the particles are distributed according to their weights. Resampling is performed to ensure that the particles generated by resampling move toward the positions of high-weight particles to obtain a new particle set. ;

[0130] S72. Output the pose mean of the new particle set as the optimized pose, as the final positioning result:

[0131] ,

[0132] In the formula, is the particle pose after resampling; For the weight model The weight value after normalization; is the average pose, that is, the final output pose.

[0133] The technical solution of the present invention is described in detail above in conjunction with the embodiments / drawings, but the present invention is not limited to the above technical solution. For ordinary technicians in this technical field, after knowing the contents recorded in the present invention, they can make several equivalent changes and substitutions without departing from the principle of the present invention. These equivalent changes and substitutions should also be regarded as belonging to the protection scope of the present invention.

Claims

1. A laser odometer positioning optimization algorithm based on OSM building information, comprising the following steps: S1. Using the interpolation method, interpolate the road points of the OSM map, and create an OSM map feature descriptor according to the features of the OSM map; S2. Use the PointNet++ algorithm to perform semantic segmentation and extraction of building point clouds on the scanned frame point clouds, and create laser point cloud feature descriptors based on the features of the building laser point clouds; It is characterized in that The laser odometer positioning optimization algorithm also includes the following steps: S3, using a binary conversion method, converting the OSM map feature descriptor and the building laser point cloud feature descriptor into a rotation-invariant feature descriptor of the OSM map and a rotation-invariant feature descriptor of the building laser point cloud; the specific implementation process includes the following steps: S31, using the binary conversion method, by converting the distance value d in the OSM map descriptor of each one-dimensional vector and the building frame point cloud feature descriptor n Converted to binary format, that is, each column vector of the matrix is ​​a distance value d in the feature descriptor n The binary representation of the matrix is in, Is an operator, which means rounding up; d max Indicates the maximum distance value between the OSM map feature descriptor and the building point cloud feature descriptor; Among them, binary(d n ) is the distance value d n The binary representation of S32, sum the two-dimensional features of all angles in each row to construct the rotation invariant feature descriptor of the OSM map Rotation-invariant feature descriptors for laser point clouds of buildings And the rotation-invariant descriptor set for OSM maps S4. Using a two-stage screening method, the rotation-invariant feature descriptors of the OSM map and the rotation-invariant feature descriptors of the laser point cloud are first preliminarily screened, and then the OSM map feature descriptors and the laser point cloud feature descriptors are finely matched to obtain a matching candidate set of interpolated road points. The specific implementation process includes the following steps: S41, set the threshold n, and convert the rotation invariant descriptor corresponding to the building point cloud feature descriptor l to be found A rotation-invariant descriptor set for OSM maps Compare, sort by absolute value sum and extract the rotation invariant descriptors of the top n OSM maps with the highest similarity The difference between the point cloud feature vector and the OSM map feature vector is measured by L1 norm to achieve the sparsity of the feature vector and provide candidates for sorting; S42, using the N-gram model The corresponding first n OSM map feature descriptors m n Compare with the building point cloud feature descriptor l to be queried to complete the reordering of the descriptors; set a threshold to filter out the matching candidate set of interpolation road points that meet the conditions; f(d n )=count(d n ,ngrams(l n / m n )), where count(d n ,ngrams(l n / m n )) represents the feature descriptor l for the building point cloud n With OSM map feature descriptor m n , the same distance value d n Frequency of occurrence in the N-gram model; S5. Use particle swarm transformation to generate Gaussian distribution to initialize particles that conform to Gaussian distribution, obtain motion equations that conform to the robot's posture through the odometer of the laser radar used, and determine the motion model; the specific implementation process includes the following steps: S51. Use Box-Muller transformation to generate Gaussian distribution to initialize N particles that conform to Gaussian distribution, each particle represents the initial position and posture of the robot, and the initial weight of each particle is equal; S52, obtain the motion equation through the robot odometer and IMU information and the frame matching algorithm in the SLAM algorithm Assume that the k-th frame position of the car at time t is X k,t , expressed by six degrees of freedom They represent the position (x, y, z) coordinates and attitude (rol, pitch, yaw) in the OSM map coordinate system respectively; According to the motion equation, the change of the car's posture from frame k-1 to frame k is expressed as: T k k-1 Represents the coordinate transformation from the k-1th frame laser radar coordinate system to the OSM map coordinate system; In the formula, x k ,y k , z k is the position coordinate of the kth frame in the OSM map coordinate system, x k-1 ,y k-1 , z k-1 is the position coordinate of the k-1th frame in the OSM map coordinate system, represents the position change of the car between the kth frame and the kth frame in the kth-1th frame laser radar coordinate system. Similarly, γ k ,θ k , is the pose of the kth frame in the OSM map coordinate system, γ k-1 ,θ k-1 , is the pose of the k-1th frame in the OSM map coordinate system, represents the change in the position of the car between the kth frame and the k-1th frame in the k-1th frame laser radar coordinate system. To predict noise, it satisfies Gaussian distribution; S6. Use the robot's motion model to predict the particle swarm motion posture, identify the corner points in the scanned building point cloud by the smoothness of the points, calculate the distance error between the corner points and the OSM map interpolation road points by minimizing the Euclidean distance, build a similarity model between the laser point cloud and the OSM map, and use the similarity model to construct a particle weight model, and update and normalize the particles through the particle weight model; the specific implementation process includes the following steps: S61, setting a threshold, using the smoothness of the point to identify the edge points, i.e., the corner points, in the scanned building point cloud; calculating the plane smoothness according to the curvature of the point as an indicator for extracting the feature information of the building point cloud in the current frame; sorting the points in a scan according to the curvature value, selecting the point with the largest curvature as the edge feature point, and the curvature of the edge feature point needs to be greater than the threshold; points near the already selected feature points are no longer considered; In the formula, c is the plane smoothness of the point, S is the The set of surrounding neighborhood points, the size of the set |S| represents the number of these neighborhood points; For point The size or norm of ; For point With each neighboring point in the set S The sum of the Euclidean distances between ; S62, calculating the distance error between the selected turning point and the OSM map node by minimizing the Euclidean distance; Where d(x,y) is the point x(x1,x2,......,x n ) and point y(y1,y2,......,y n ) between them; n = 2 means the dimension is 2; S63. Use the calculated distance error to construct a similarity model between the point cloud and the OSM map ω i ; The larger the distance, the greater the error and the smaller the weight; similarity model ω i The meaning of is to match the building within the threshold range of the current point. The farther the actual building is from the point cloud, the less credible the current pose is. S64. Use the similarity model to construct the particle weight model ω j : ω j represents the weight of the jth particle, and n represents the similarity model ω in scanning a frame of building point cloud i number, parameter i=(1,2,3,......,n); S65, through the particle weight model ω j Update the weights of N particles and normalize them: S7. The probability distribution method based on KL divergence is used to resample the particle swarm, and the mean pose of the stable particle swarm is output as the optimized pose to obtain the optimized positioning trajectory.

2. According to claim 1, a laser odometer positioning optimization algorithm based on OSM building information is characterized in that: The implementation process of step S1 includes the following steps: S11, setting a threshold, interpolating road points at an interval of n meters, and generating an OSM map feature descriptor for each interpolated road point (x, y) in the UTM coordinate system, representing the tuple of the OSM map building edge as t = (X1, X2) = ((x1, y1), (x2, y2)) and the interpolated road point in the UTM coordinate system as (x, y); S12, divide 360° into 360 equal parts at intervals of 1°, and search for each angle θ n Find the shortest distance d to the edge of the building n : Where a and b refer to the distance from the current position to the building edge t; θ1 and θ2 correspond to the minimum and maximum angles to the top of the building respectively; θ is the distance between the horizontal axis and the line segment between the interpolated road point (x, y) and the building construction vertex (x1, y1) and (x2, y2). a =θ2-θ n ,θ b =θ n -θ1 represents the angle between the target angle direction and the building t; if the building t is not found in the target angle direction, then d n is considered as 0; when θ1≤θ n ≤θ2, that is, when the target angle is between the two vertices of the building edge t, calculate d n ; and define the feature descriptor of the OSM map as m n ={d1,d2,......,d 360 }.

3. The laser odometer positioning optimization algorithm based on OSM building information according to claim 1 is characterized in that: The implementation process of step S2 includes the following steps: S21. Use a laser beam parallel to the ground in the laser radar scanning line to scan the building, use the frame point cloud scanned by this single-line laser as the data source of the laser point cloud feature descriptor, and convert the frame point cloud format into a format that can be recognized by the PointNet++ algorithm; S22. Use the PointNet++ algorithm to perform semantic segmentation of buildings on each frame of point cloud scanned by the LiDAR and extract them separately; S23, according to the building frame point cloud extracted in step S22, based on the point features of the laser radar scanning data, the building points P = {X1, X2, ..., X N The shortest distance d n To create a frame point cloud feature descriptor l = {d1, d2, ..., d 360 }, where d n Indicates the point X to the building n distance.

4. The laser odometer positioning optimization algorithm based on OSM building information according to claim 1 is characterized in that: The implementation process of step S7 includes the following steps: S71, using the probability distribution method based on KL divergence, the particles are distributed according to the weight ω j Resampling is performed to ensure that the particles generated by resampling move toward the positions of high-weight particles to obtain a new particle set. S72. Output the pose mean of the new particle set as the optimized pose, as the final positioning result: In the formula, is the particle pose after resampling; For the weight model ω j The weight value after normalization; is the average pose, that is, the final output pose.

Citation Information

Patent Citations

  • Laser slam method based on subgraph merging and pose optimization

    AU2020101833A4

  • Systems, methods and devices for geo-localization

    CN109154502A

  • Positioning method based on road matching, chip subsystem and electronic equipment

    CN111044056A

  • Laser radar odometer positioning optimization method based on OSM road network constraint

    CN115560780A

  • Road network map auxiliary vehicle positioning method based on pose map optimization

    CN116839602A