An optimization method for lidar odometry positioning based on OSM road network constraints

Abstract

The present invention discloses an optimization method for lidar odometry positioning under the constraint of OSM road network. The method includes Step 1: obtaining the vehicle pose using lidar odometry and taking this pose as the motion equation; Step 2: extracting inflection points using the heading angle change rate, the straight-curve ratio of the trajectory, and the turning angle change constraint; Step 3: simplifying the nodes of the OSM map and matching the extracted inflection points with the simplified OSM map to obtain a matching set; Step 4: predicting the particle motion using the particle motion equation and updating the particle weights at the inflection points using a weight model; Step 5: resampling the particles and selecting the mean of the particle set as the pose output after trajectory correction to obtain the optimized positioning. The present invention overcomes the disadvantages of high cost and high memory of traditional positioning methods, solves the problem of error drift of lidar odometry, and has good positioning accuracy and real-time performance.

Description

Technical Field

[0001] The present invention belongs to the field of vehicle navigation and self-positioning, and particularly relates to an optimization method for lidar odometry positioning based on OSM road network constraints. Background Art

[0002] With the gradual rise of autonomous driving, people's demand for positioning is getting higher and higher. Since lidar can extract depth information and is independent of GPS and lighting conditions, the method of using lidar for positioning has gradually emerged. By extracting feature points of the point cloud and performing inter-frame matching, lidar odometry can be obtained. However, the problem of cumulative drift makes this positioning method inapplicable to long-distance driving. To solve this problem, some domestic and foreign scholars use the SLAM method to construct maps and use loop closure to eliminate errors. However, this method has a high cost and the vehicle's motion trajectory is restricted. In view of this problem, this patent proposes an optimization method for lidar odometry positioning based on OSM road network constraints, which only uses the information of the nodes and edges of the road network map to perform error constraints on the inflection points of the lidar odometry, overcomes the disadvantages of high cost and large memory requirements of the traditional method, can effectively solve the error drift problem of lidar odometry, and can ensure the positioning accuracy and real-time performance of the system. Summary of the Invention

[0003] To solve the technical problems mentioned in the above background art, the present invention proposes an optimization method for lidar odometry positioning.

[0004] To achieve the above technical objectives, the technical solution of the present invention is as follows:

[0005] An optimization method for lidar odometry positioning based on OSM road network constraints, comprising the following steps:

[0006] 1) Obtain the pose of the vehicle using lidar odometry and obtain the motion equation of this pose;

[0007] 2) Extract inflection points using the change rate of the vehicle's heading angle, the straight-curve ratio of the vehicle's running trajectory, and the turning angle change constraint;

[0008] 3) Preprocess the OSM open-source map, simplify the nodes and edges using the Douglas-Peucker algorithm, store the node positions and the orientations of the edges, and match the inflection points extracted in 2) with the simplified OSM map to obtain a matching candidate set;

[0009] 4) Predict the particle motion trajectory using the particle motion equation, and update the particle weights at the inflection points of the predicted particle motion trajectory using the weight model;

[0010] 5) Resample the particles and select the mean of the particle set as the pose output after trajectory correction to obtain the optimized positioning.

[0011] Preferably, the implementation process of step 1) is as follows:

[0012] 1.1) Initialize N particles that conform to the Gaussian normal distribution. Each particle represents a possible initial pose of the vehicle, and the initial weight of each particle is 1 / N;

[0013] 1.2) Obtain the motion equation through the inter-frame matching of edge points and plane points in the lidar point cloud image Assume that the pose of the vehicle in the k-th frame is X k , where x, y, z represent the position of the vehicle in the global coordinate system, represents the attitude of the vehicle in the global coordinate system. Through the motion equation The pose change of the vehicle from the (k - 1)-th frame to the k-th frame is expressed as:

[0014]

[0015]

[0016] where, is obtained from the motion equation x k , y k , z k are the position coordinates of the vehicle in the k-th frame in the global coordinate system, x k-1 , y k-1 , z k-1 are the position coordinates of the vehicle in the (k - 1)-th frame in the global coordinate system, is the coordinate transformation from the lidar coordinate system in the (k - 1)-th frame to the global coordinate system, represents the position change of the vehicle in the k-th frame and the (k - 1)-th frame in the lidar coordinate system in the (k - 1)-th frame, represents the attitude change of the vehicle in the k-th frame and the (k - 1)-th frame in the global coordinate system. The transpose of the parameter is which is the prediction noise and conforms to the Gaussian distribution; γ k , θ k , are the attitudes of the vehicle in the k-th frame in the global coordinate system, γ k-1 , θ k-1 , are the attitudes of the vehicle in the (k - 1)-th frame in the global coordinate system.

[0017] Preferably, the implementation process of step 2) is as follows:

[0018] 2.1) Coarse screen the inflection points using the change rate of the vehicle's heading angle to obtain possible inflection points;

[0019] 2.2) Use the straight-curve ratio and turning angle change of the vehicle's running trajectory to finely screen the inflection points, and remove the pseudo-inflection points caused by overtaking, sudden braking, etc.

[0020] Preferably, the implementation process of step 3) is as follows:

[0021] 3.1) Obtain the OSM open-source map of the specified area and save the corresponding nodes and edges;

[0022] 3.2) Set a threshold, and use the Douglas-Peucker algorithm to simplify the nodes and edges. Simplify multiple nodes at the turning points into a single node for storage, and store the node positions and edge orientation information;

[0023] 3.3) Match the extracted inflection points with the stored road network nodes, and use the length and angle error thresholds between adjacent inflection points to obtain the matching candidate set P.

[0024] Preferably, the implementation process of step 4) is as follows:

[0025] 4.1) Calculate the heading angle error factor α and relative length error s of the trajectory segments formed by adjacent inflection points:

[0026]

[0027]

[0028] Construct the similarity model ω1 of the trajectory using the heading angle error factor α and length error s:

[0029] ω1 = e -(λ*α+(1-λ)*s)

[0030] where λ is the normalization weight factor;

[0031] 4.2) Use the distance between the particle and the road network nodes to construct the probability density function ω2:

[0032]

[0033] where σ d represents the degree of dispersion;

[0034] where the definition of the parameter d' is as follows:

[0035]

[0036] where d represents the distance between the particle and the road network node, and d th is the set distance threshold;

[0037] 4.3) Use the similarity model ω1 and the probability density function ω2 to jointly construct a particle weight model

[0038]

[0039] Among them, represents the weight of the i-th particle, n represents the total number of points in the candidate matching set P, the parameter j = (1, 2, 3,..., n), ω1(p i ,P j ) is the similarity between the running trajectory of the vehicle and the possible road candidate segment, and ω2(p i ,P j ) is the probability density function; p i is the position of the vehicle at the current inflection point P j is the position of the node to be matched

[0040] 4.4) Through the particle weight model update the weights of the N particles and perform weight normalization:

[0041]

[0042] Preferably, the implementation process of step 5) is as follows:

[0043] 5.1) Use the roulette wheel idea in the genetic algorithm to resample the particles according to the proportion of the weights to ensure that the particles generated by resampling move towards the poses of the high-weight particles, and obtain a new particle set

[0044] 5.2) Use the pose mean of the new particle set as the optimized pose output as the final positioning result:

[0045]

[0046] Beneficial effects brought by adopting the above technical solutions:

[0047] (1) The present invention uses a lightweight OSM road network map, and the system mainly stores the information of nodes and edges in the road network. Therefore, only extremely low costs and memory are required while ensuring the positioning accuracy.

[0048] (2) The particle weight model adopted by the present invention can avoid the increase in positioning error caused by the incorrect association between the inflection point and the node for the case where the candidate matching set is multiple nodes, and can ensure the ambiguity of calibration.

[0049] (3) The present invention eliminates the cumulative error caused by the lidar with the help of the lidar odometer and the OSM map, and has good positioning accuracy and real-time performance. This system can be used in GPS-denied environments, getting rid of the dependence on the lighting environment, and has broad market prospects and application value. Brief Description of the Drawings

[0050] Figure 1 is the basic flowchart of the present invention;

[0051] Figure 2 is the schematic diagram of precise inflection point screening;

[0052] Figure 3 is the schematic diagram of OSM node simplification Detailed Embodiment

[0053] The technical solution of the present invention will be described in detail below in conjunction with the drawings.

[0054] The present invention designs an optimization method for lidar odometer positioning based on OSM road network constraints, as Figure 1 shown, the steps are as follows:

[0055] Step 1: Use the lidar odometer to obtain the vehicle pose, and use this pose as the motion equation;

[0056] Step 2: Extract inflection points using the heading angle change rate, the straight-curve ratio of the trajectory, and the turning angle change constraint;

[0057] Step 3: Simplify the nodes of the OSM map, and match the extracted inflection points with the simplified OSM map to obtain a matching set;

[0058] Step 4: Predict the particle motion using the particle motion equation, and update the particle weights at the inflection points using the weight model;

[0059] Step 5: Resample the particles, and select the mean value of the particle set as the pose output after trajectory correction to obtain the optimized positioning. In this embodiment, Step 1 can be implemented using the following preferred solution:

[0060] (1) Initialize N particles that conform to the Gaussian normal distribution, representing the possible initial poses of the trolley, and the initial weight of each particle is 1 / N.

[0061] (2) Obtain the inter-frame transfer matrix T k k -1 through the inter-frame matching of the edge points and plane points in the lidar point cloud image, and use this as the motion model of the particles to update the particle states.

[0062] Assume that the pose of the trolley at the k-th frame is X k, represented by six degrees of freedom, is which respectively represent the position (xyz) coordinates and attitude (roll, pitch, yaw) in the global coordinate system. Through the motion equation, the pose change of the vehicle from the (k - 1)-th frame to the k-th frame can be expressed as:

[0063]

[0064]

[0065] where can be obtained from the transition matrix of the odometer and represents the coordinate transformation from the lidar coordinate system of the (k - 1)-th frame to the global coordinate system, is the prediction noise and conforms to the Gaussian distribution.

[0066] In this embodiment, step 2 can be implemented by the following preferred scheme:

[0067] (1) Coarsely screen the inflection points using the rate of change of the heading angle to obtain possible inflection points. Extract the trajectory segments where the heading angle rate of consecutive frames is greater than a certain threshold, and use the point with the maximum heading angle rate as the inflection point.

[0068] (2) Use the straightness-curvature ratio of the trajectory and the change of the turning angle to achieve fine screening of the inflection points. As Figure 2 shown, set the straightness-curvature ratio threshold S and the angle threshold β. If the straightness-curvature ratio is greater than S or the change of the turning angle is less than β, it is considered a pseudo inflection point and removed to achieve fine screening of the inflection points.

[0069] In this embodiment, step 3 can be implemented by the following preferred scheme:

[0070] (1) Obtain the OSM open-source map of the specified area and save the corresponding nodes and edges;

[0071] (2) Simplify the OSM map information. As Figure 3 shown, set the threshold and use the Douglas-Peucker algorithm to simplify the nodes and edges. Simplify multiple nodes at the turning points into a single node for storage, and store the node positions and the orientation information of the edges;

[0072] (3) Match the extracted inflection points with the stored road network nodes, and use the length and angle error thresholds between adjacent inflection points to obtain the matching candidate set P.

[0073] In this embodiment, step 4 can be implemented by the following preferred scheme:

[0074] (1) According to the extracted inflection points, calculate the orientation angle error factor α and the relative length error s of the trajectory segments formed by adjacent inflection points. The calculation methods are as follows:

[0075]

[0076]

[0077] And a similarity model ω1 of the trajectory is constructed by using the length error and the orientation angle error as follows:

[0078] ω1 = e -(λ*α+(1-λ)*s)

[0079] where λ is a normalization weight factor used to characterize the dependence of the similarity model on the angle similarity and the length similarity.

[0080] (2) A probability density function ω2 based on the measurement value is constructed by using the distance between the particle and the road network node as follows:

[0081]

[0082] where the definition of d' is as follows:

[0083]

[0084] where d represents the measurement value, that is, the distance between the particle and the road network node, and d th is the set distance threshold.

[0085] (3) A particle weight model is jointly constructed by using the similarity model ω1 and the probability density function ω2 based on the measurement value as follows:

[0086]

[0087] where represents the weight size of the i-th particle, n represents the total number of points in the obtained candidate matching set P, ω1(p i , P j ) is the similarity between the car trajectory and the possible road candidate segment, and ω2(p i , P j ) is the probability density function based on the distance between the particle and the road network node.

[0088] Let p i-1 refer to the position of the previous inflection point P i-1 refer to the position of the previous matching node p i refer to the position of the current inflection point P j refer to the position of the node to be matched

[0089] Then there is:

[0090]

[0091]

[0092] ω1 and ω2 can be calculated through these formulas

[0093] When the inflection point has a greater degree of association with the corresponding node in the matching set P, ω1 will be larger at this time. According to the particle weight model, the particles closer to this node will be assigned higher weights. The weights of N particles are updated through this weight model and weight normalization is performed. The results of the normalization are as follows:

[0094]

[0095] In this example, step 5 can be implemented by the following preferred solution:

[0096] (1) Using the idea of roulette in the genetic algorithm, resample the particles and copy the particles according to the proportion of the weights to ensure that the particles generated by resampling move towards the poses of high-weight particles, obtaining a new particle set.

[0097] (2) Take the pose mean of the obtained new particle set as the pose output after odometer optimization and as the final positioning result. That is, the representation of the true pose of the trolley is as follows:

[0098]

[0099] The above is only to illustrate the technical idea of the present invention in combination with specific preferred embodiments, and the protection scope of the present invention cannot be limited thereby. Any changes made on the basis of the technical solution according to the technical idea proposed by the present invention fall within the protection scope of the present invention.

Claims

1. An optimization method for lidar odometry positioning under the constraint of OSM road network, characterized in that, It includes the following steps: 1) Use the lidar odometer to obtain the vehicle pose and obtain the motion equation of this pose; 2) Extract inflection points by using the change rate of the vehicle heading angle, the straight-curve ratio of the vehicle running trajectory, and the inflection angle change constraint; 3) Obtain the OSM open-source map, simplify the nodes and edges of the OSM open-source map by using the Douglas-Peucker algorithm, store the node positions and the orientations of the edges, and match the inflection points extracted in step 2) with the simplified OSM map to obtain a matching candidate set; 4) Use the particle motion equation to predict the particle motion trajectory, and update the particle weights at the inflection points of the predicted particle motion trajectory by using the weight model; 5) Resample the particles, and select the mean value of the particle set as the pose output after trajectory correction to obtain the optimized positioning; The implementation process of step 4) is as follows: 4.1) Calculate the heading angle error factor α and the relative length error s of the trajectory segment formed by adjacent inflection points: Construct the similarity model ω1 of the trajectory by using the heading angle error factor α and the length error s: ω1 = e -(λ*α+(1-λ)*s) where λ is the normalization weight factor; 4.2) Construct the probability density function ω2 by using the distance between the particle and the road network node: Among them, σ d represents the degree of dispersion; where the definition of the parameter d' is as follows: where d represents the distance between the particle and the road network node, and d th is the set distance threshold; 4.3) Construct a particle weight model jointly using the similarity model ω1 and the probability density function ω2 Among them, represents the weight of the i-th particle, n represents the total number of points in the candidate matching set P, the parameter j = (1, 2, 3,..., n), ω1(p i , P j ) is the similarity between the trolley running trajectory and the road candidate segment, ω2(p i , P j ) is the probability density function; p i is the position of the trolley at the current inflection point P j is the position of the node to be matched 4.4) Through the particle weight model Update the weights of N particles and perform weight normalization: ; The implementation process of step 5) is as follows: 5.1) Using the idea of roulette in the genetic algorithm, resample the particles according to the proportion of the weights to ensure that the particles generated by resampling move towards the poses of the high-weight particles, and obtain a new particle set 5.2) Take the pose mean value of the new particle set as the optimized pose output as the final positioning result:

2. The optimized method for lidar odometry positioning based on OSM road network constraints as claimed in claim 1, wherein, The implementation process of step 1) is as follows: 1.1) Initialize N particles that conform to the Gaussian normal distribution. Each particle represents the initial pose of the vehicle, and the initial weight of each particle is 1 / N; 1.2) Obtain the motion equation through the inter-frame matching of edge points and plane points in the lidar point cloud image Assume that the pose of the vehicle at the k-th frame is X k , where x, y, z represent the position of the vehicle in the global coordinate system, and γ, θ, represent the attitude of the vehicle in the global coordinate system. The pose change of the vehicle from the (k - 1)-th frame to the k-th frame is expressed as follows through the motion equation : Among them, is obtained from the motion equation where \(x\) k , \(y\) k , and \(z\) k are the position coordinates of the vehicle at the \(k\)-th frame in the global coordinate system, \(x\) k-1 , \(y\) k-1 , and \(z\) k-1 are the position coordinates of the vehicle at the \((k - 1)\)-th frame in the global coordinate system. is the coordinate transformation from the lidar coordinate system at the \((k - 1)\)-th frame to the global coordinate system. represents the position change of the vehicle at the \(k\)-th frame and the \((k - 1)\)-th frame in the lidar coordinate system at the \((k - 1)\)-th frame. represents the attitude change of the vehicle at the \(k\)-th frame and the \((k - 1)\)-th frame in the global coordinate system. The transpose of the parameter is which is the prediction noise and follows a Gaussian distribution; \(\gamma\) k , \(\theta\) k , are the attitudes of the vehicle at the \(k\)-th frame in the global coordinate system, \(\gamma\) k-1 , \(\theta\) k-1 , are the attitudes of the vehicle at the \((k - 1)\)-th frame in the global coordinate system.

3. The method for optimizing lidar odometry positioning under the constraint of OSM road network according to claim 2, characterized in that, The implementation process of step 2) is as follows: 2.1) Coarsely screen the inflection points by using the change rate of the vehicle heading angle to obtain the inflection points; 2.2) Fine-screen the inflection points by using the straight-curve ratio of the vehicle running trajectory and the inflection angle change to remove the pseudo inflection points caused by overtaking and sudden braking.

4. The optimized method for lidar odometry positioning based on OSM road network constraints as described in claim 3, wherein The implementation process of step 3) is as follows: 3.1) Obtain the OSM open-source map of the specified area and save the corresponding nodes and edges; 3.2) Set a threshold, simplify the nodes and edges by using the Douglas-Peucker algorithm, simplify multiple nodes at the turning point into a single node for storage, and store the node positions and the edge orientation information; 3.3) Match the extracted inflection points with the stored road network nodes, and obtain the matching candidate set P by using the length and angle error threshold constraints between adjacent inflection points.

Images