Laser global positioning method and system based on road topology

Through the laser global positioning method based on road topology structure, the road center line is extracted and nodes are matched in the environmental topology map, the problems of incorrect matching and low computing efficiency in the existing technology are solved, and more accurate and stable autonomous driving positioning is achieved.

CN118999523APending Publication Date: 2024-11-22BEIJING INST OF TECH
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202410948914.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-16
Publication Date
2024-11-22

AI Technical Summary

Technical Problem

Existing descriptor-based laser global positioning methods are difficult to avoid the problem of mismatch in complex or similar environments, and are inefficient in computing efficiency and insufficient robustness.

Method used

Using laser global positioning method based on road topology, the road center line is extracted, the intersection or corner is judged, and the nodes are matched in the pre-constructed environmental topology map to calculate the relative pose transformation to achieve accuracy. position.

Benefits of technology

Improves positioning accuracy, robustness and computing efficiency, avoids mismatch, and is suitable for autonomous driving applications in complex and similar environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118999523A_ABST
    Figure CN118999523A_ABST
Patent Text Reader

Abstract

The invention provides a laser global positioning method and system based on road topology, and belongs to the technical field of automatic driving. The method comprises the following steps: firstly, converting laser radar point cloud data into a barrier binary plane image, and extracting road topology at the current moment; when it can be judged that the unmanned autonomous mobile platform is in the road intersection or corner scene, matching is carried out in a pre-constructed environment topology map, and a matched node, namely the node where the unmanned autonomous mobile platform is currently located, is found; extracting road topology and poses corresponding to the matched nodes, wherein the poses are coarse positioning results; calculating relative pose transformation between the road topology at the current moment and the obtained road topology of the matched node; and the relative pose is converted and compensated to the coarse positioning result, the precise pose of the unmanned autonomous mobile platform is obtained, and global positioning is realized. The problem of mismatching caused by similar scenes is solved, the complexity of a global positioning algorithm is reduced, and an effective and reliable positioning scheme is provided for unmanned driving.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to a laser global positioning method and system based on road topology. Background Art

[0002] With the rapid development of social economy and information technology, the application scenarios of unmanned driving have gradually expanded to various fields such as logistics and transportation, agricultural production, and military, and the demand for navigation and positioning technology has also increased. Global positioning plays a vital role in the navigation and positioning of mobile robots and unmanned vehicles. Global positioning uses the received sensor data and prior map information to provide a relatively reliable global posture, and is used to optimize and eliminate its cumulative errors in the back-end of SLAM. At present, visual sensors are often used for global positioning due to their low cost. However, visual sensors are quite sensitive to environmental factors such as lighting and weather, and have low robustness. In contrast, lidar has a wide detection range and is more adaptable to environmental changes, so laser global positioning methods have also begun to receive more attention.

[0003] In the early laser global positioning technology, the commonly used direct point cloud registration method is feasible in theory, but it is limited in practical applications due to its low computational efficiency. To solve this problem, subsequent studies proposed descriptor-based laser global positioning technology. This technology significantly reduces processing time and computing resource consumption by simplifying point cloud data using efficient descriptors. Despite this, when encountering scenes with similar appearance, existing descriptor-based methods still find it difficult to avoid the problem of false matching. This is mainly because these methods rely on the similarity calculation of point cloud data, which often lacks sufficient discrimination in complex or similar environments.

[0004] In view of the various problems existing in the above methods, there is an urgent need for a computationally efficient, accurate and stable laser global positioning method. Summary of the invention

[0005] In view of this, the present invention proposes a laser global positioning method based on road topology structure, which can improve the accuracy, robustness and stability of positioning calculation.

[0006] A laser global positioning method based on road topology is used for positioning an unmanned autonomous mobile platform equipped with a multi-line laser radar. The method comprises:

[0007] Step 1: construct a binary plane image of the obstacle based on the point cloud data obtained by the multi-line laser radar at the current moment;

[0008] Step 2: extracting the road centerline from the obstacle binary plane image to obtain the road topology at the current moment;

[0009] Step 3: If the current road topology extracted in step 2 is an intersection or a corner, match it in the pre-constructed environment topology map to find the matching node, that is, the node where the unmanned autonomous mobile platform is currently located; extract the road topology and posture corresponding to the matching node, and the posture is the rough positioning result; wherein the pre-constructed environment topology map is an undirected connected graph, and the information stored in the nodes in the graph includes the posture of the unmanned autonomous mobile platform and the corresponding road topology;

[0010] Step 4: Calculate the relative posture transformation between the current road topology and the matching node road topology obtained in step 3; compensate the relative posture transformation to the rough positioning result to obtain the precise posture of the unmanned autonomous mobile platform and achieve global positioning.

[0011] Preferably, in the step 1, a height difference-based algorithm is used to construct the binary plane image of the obstacle.

[0012] Preferably, in step 1, the constructed obstacle binary plane image is further subjected to image filtering, and the specific steps are as follows:

[0013] Step S11, using breadth-first traversal to obtain all eight-neighborhood connected pixel blocks in the binary plane image of the obstacle;

[0014] Step S12, calculating the number of pixels in the eight-neighborhood connected pixel block obtained in step S11, if the number of pixels is less than a set first threshold, setting all pixel values ​​in the eight-neighborhood connected pixel block to zero to filter out noise.

[0015] Preferably, in step 2, the algorithm for extracting the road centerline is a watershed algorithm.

[0016] Preferably, in step 3, whether the extracted road topology at the current moment is an intersection or a corner is determined by:

[0017] Step S301, determining the number of branches n of the road topology at the current moment extracted in step 2;

[0018] Step S302, if the number of branches n is greater than 2, it is preliminarily determined that the road topology at the current moment is an intersection, and step S303 is executed; if the number of branches n is equal to 2, step S304 is executed to further determine whether the road topology at the current moment is a corner;

[0019] Step S303, extracting the topological feature points of the intersection, which topological feature points maximize the number of edge points in the surrounding 3x3 neighborhood; executing step 305;

[0020] Step S304, find the starting point and end point p of the current road topology by minimizing the cost function (I) s ,pe ;

[0021]

[0022] Where I(i,j) is the value of the pixel at (i,j) in the image;

[0023] Let R(i,j) be (i,j),p s ,p e The radius of the circumscribed circle determined by the three points is used to calculate the maximum curvature κ of the road topology at the current moment. max , if the maximum curvature κ max If it is greater than the set value C, it is preliminarily determined that the road topology at the current moment is a corner, and the point with the maximum curvature is taken as the topological feature point of the corner, and step S305 is executed;

[0024] Step S305, if the current road topology meets the road features of an intersection or a corner, the reception time of the point cloud data at the current moment is compared with the reception time of the point cloud data with the same road features last time. If the time interval ΔT is less than the set interval, it is finally determined that the current road topology is indeed an intersection or a corner.

[0025] Preferably, in step 3, the matching is performed in the pre-constructed environment topology map, the node where the unmanned autonomous mobile platform is currently located is found, and the road topology and posture corresponding to the current node are extracted as the specific steps of the rough positioning result include:

[0026] Step S311, giving the initial node from which the unmanned autonomous mobile platform departs in the environmental topology map, and recording the historical road topology type sequence passed;

[0027] Step S312, searching the environment topology map for the first path that meets the historical road topology type sequence through depth-first search;

[0028] Step S313, output the posture stored in the last node in the path obtained in S312 to obtain a rough positioning result.

[0029] Preferably, the step 4 specifically includes the following steps:

[0030] Step S41, using the topological feature points to align the topological feature points of the current road topology and the matching node road topology;

[0031] Step S42, taking the topological feature point as the center, according to the angular resolution ρ, the road topology at the current moment is rotated by different rotation angles θ=kρ, k∈[0,M-1] in sequence, where M is the angular resolution of the road topology rotation at the current moment;

[0032] Step S43: at each rotation angle θ, for each topological point p in the road topology at the current moment i , search for the topological point q with the smallest Euclidean distance in the road topology of the matching node j ; If the minimum distance d is less than the set second threshold, p i ,q j Recorded as a pair of matching topological points;

[0033] Step S44, at each rotation angle θ, by calculating in step S43, the number of matching topological points N between the current road topology and the matching node road topology is obtained. matched , N matched The proportion of the total number of topological points N in the road topology at the current moment is used as the road topology matching degree score corresponding to the rotation angle θ;

[0034] Step S45: Sum the highest road topology matching scores of all rotation angles θ max As the final matching score of the current road topology and the matching node road topology;

[0035] Step S46: If the final matching score in step S45 exceeds the third threshold, the global positioning is considered successful; at this time, the relative translation transformation of the topological feature points aligned in step S41 and the highest road topology matching degree score in step S45 are obtained. max The relative rotation transformation of the corresponding road topology is used to compensate the relative translation transformation and relative rotation transformation to the rough positioning result obtained in step 3, and the precise positioning result T is output. The calculation formula is:

[0036]

[0037] Where Δt and ΔR are the compensated relative translation and relative rotation, respectively, and T′ is the coarse positioning result.

[0038] The present invention also provides a laser global positioning system based on road topology, which is used for positioning an unmanned autonomous mobile platform equipped with a multi-line laser radar, and includes: an environmental topology map storage module, a road topology acquisition module, a topology type judgment module, a coarse positioning module and a fine positioning module;

[0039] The environmental topology map storage module is used to store a pre-constructed environmental topology map, which is an undirected connected graph. The information stored in the nodes in the graph includes the position and posture of the unmanned autonomous mobile platform and the corresponding road topology;

[0040] The road topology acquisition module is used to construct a binary plane image of the obstacle based on the point cloud data obtained by the multi-line laser radar at the current moment; extract the road centerline from the binary plane image of the obstacle to obtain the road topology at the current moment;

[0041] The topology type determination module is used to determine whether the current road topology obtained by the road topology acquisition module is an intersection or a corner, and if so, to start the coarse positioning module;

[0042] The coarse positioning module is used to match the environmental topology map stored in the environmental topology map storage module after being started, find the matching node, that is, the node where the unmanned autonomous mobile platform is currently located; extract the road topology and posture corresponding to the matching node, the posture is the coarse positioning result, and send it to the fine positioning module;

[0043] The precise positioning module is used to calculate the relative posture transformation between the road topology at the current moment and the matching node road topology obtained by the rough positioning module; the relative posture transformation is compensated to the rough positioning result to obtain the precise posture of the unmanned autonomous mobile platform and realize global positioning.

[0044] Preferably, the topology type judgment module includes a first judgment submodule, an intersection topology feature point determination submodule, a corner topology feature point determination submodule, and a second judgment submodule;

[0045] The first judgment submodule is used to judge whether the number of branches n of the road topology at the current moment is greater than 2. If n is greater than 2, it is preliminarily determined that the road topology at the current moment is an intersection, and the intersection topology feature point determination submodule is started; if the number of branches n is equal to 2, the corner topology feature point determination submodule is started;

[0046] The intersection topology feature point determination submodule is used to extract the topology feature points of the intersection, which topology feature points maximize the number of edge points in the surrounding 3x3 neighborhood, and notify the second judgment submodule;

[0047] The corner topology feature point determination submodule is used to determine whether it is a corner according to the maximum curvature of the road topology at the current moment, and obtain the topology feature points of the corner to notify the second judgment submodule;

[0048] The second judgment submodule is used to compare the reception time of the point cloud data at the current moment with the reception time of the point cloud data of the last time the same road feature appeared when the road topology at the current moment meets the road features of an intersection or a corner. If the time interval ΔT is less than the set interval, it is finally determined that the road topology at the current moment is indeed an intersection or a corner.

[0049] Preferably, the precise positioning module includes an alignment submodule, a topological point matching submodule, a matching degree scoring submodule and a transformation submodule;

[0050] The alignment submodule is used to align the topological feature points of the road topology at the current moment and the matching node road topology using the topological feature points;

[0051] The topological point matching submodule is used to rotate the current road topology by different rotation angles θ according to the angle resolution ρ, with the topological feature point as the center; at each rotation angle θ, for each topological point p in the current road topology i , search for the topological point q with the smallest Euclidean distance in the road topology of the matching node j , if p i ,q j If the distance is less than the second threshold, p i ,q j Recorded as matching topological point;

[0052] The matching degree scoring submodule is used to perform topological point matching using the topological point matching submodule at each rotation angle θ to obtain the number of matching topological points N between the current road topology and the matching node road topology. matched ; N matched The ratio of the total number of topological points N in the current road topology is used as the road topology matching degree score corresponding to the rotation angle θ; the highest road topology matching degree score of all rotation angles θ is calculated as max as the final matching score of the road topology at the current moment and the road topology of the matching node; if the final matching score exceeds the third threshold, it is considered that the global positioning is successful, and the transformation submodule is started;

[0053] After the transformation submodule is activated, the relative translation transformation amount of the topological feature points of the alignment submodule is obtained, and the highest road topology matching degree score is obtained. max The relative rotation transformation of the corresponding road topology, the relative translation transformation and relative rotation transformation are compensated to the rough positioning result, and the precise positioning result T is output. The calculation formula is:

[0054]

[0055] Where Δt and ΔR are the compensated relative translation and relative rotation, respectively, and T′ is the coarse positioning result.

[0056] Beneficial effects:

[0057] (1) The present invention does not simplify point cloud data based on descriptors, nor does it rely on similarity calculation of point cloud data. Instead, it utilizes the rich road structure features contained in the environment and cooperates with the geometric information of the point cloud. This solution avoids the mismatching problem caused by similar scenes, and can show sufficient differentiation in complex or similar environments, effectively avoiding the occurrence of mismatching and ensuring the accuracy of positioning results. At the same time, through this multi-dimensional data fusion, the accuracy and robustness of positioning are effectively improved.

[0058] (2) The present invention uses the road topology represented by the road centerline, which is an abstract road feature. It removes useless features in the road and makes the road features obvious. It can not only ensure the accuracy of the positioning results, but also the topological operation greatly reduces the complexity of the operation and improves the calculation efficiency.

[0059] (3) The present invention does not rely on image information collected by visual sensors and is more robust to changes in ambient lighting conditions.

[0060] (4) This method can also achieve more stable global positioning in different environments, especially in urban environments with repeated or similar geographic features, effectively avoiding the occurrence of false matching, thereby providing more reliable support for autonomous driving and other navigation applications.

[0061] (5) In a specific implementation, when determining whether it is an intersection, the function values ​​of the surrounding 8 points are summed up by the formula, and the function value of the center point is subtracted, ensuring that the selected center point is the point with the most edge points around it. By finding the point with the most edge points in the 3x3 neighborhood, the feature points of the intersection can be accurately located. This extraction of topological feature points of the intersection has advantages in accuracy and robustness. In terms of accuracy, by considering the distribution of edge points in the neighborhood, the center position of the intersection can be determined more accurately; in terms of robustness, the calculation in the 3x3 neighborhood can smooth out some noise and errors, thereby improving the stability of feature point extraction.

[0062] (6) In a preferred embodiment, M different angles from 0 to 360 degrees are considered when performing compensation. By segmenting in this way, the road topology can be analyzed and calculated at different angles around the feature point. This method helps to analyze the changes in road topology in different directions, thereby more accurately determining the shape and characteristics of the road. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 A flowchart of a laser global positioning method based on road topology according to an embodiment of the present invention;

[0064] Figure 2 A visualization diagram of road topology extracted from a received laser point cloud according to an embodiment of the present invention;

[0065] Figure 3 The figure is a schematic diagram of an environment topology map in one embodiment of the present invention; the dots represent nodes, and the lines connecting the dots are edges of the map.

[0066] Figure 4 This is a performance comparison chart of the accuracy-recall curve of an example of the present invention and other methods, where the horizontal axis is the recall rate and the vertical axis is the accuracy rate;

[0067] Figure 5 This is a result diagram of successful global positioning of an example of the present invention, the thinner curve is the moving trajectory, and the bold part is the position where the global positioning is successfully achieved;

[0068] Figure 6 A structural diagram of a laser global positioning system based on road topology according to an embodiment of the present invention;

[0069] Figure 7 for Figure 6 The structural diagram of the topology type judgment module in the figure;

[0070] Figure 8 for Figure 6 Structural diagram of the medium precision positioning module. DETAILED DESCRIPTION

[0071] The present invention provides a laser global positioning solution based on road topology. The basic idea is to first convert the laser radar point cloud data into a binary plane image of the obstacle and extract the road topology at the current moment; when it can be judged that it is at a road intersection or corner scene, matching is performed in a pre-constructed environmental topology map, and a rough positioning result is obtained based on the matching nodes; then the topological feature points are used to obtain the relative posture transformation between the road topology at the current moment and the road topology of the matching node, and the relative posture transformation is compensated to the rough positioning result to obtain the precise posture of the unmanned autonomous mobile platform.

[0072] It can be seen that the present invention does not simplify point cloud data based on descriptors, nor does it rely on the similarity calculation of point cloud data. Instead, it utilizes the rich topological information contained in the environment and cooperates with the geometric information of the point cloud. This solution can show sufficient discrimination in complex or similar environments and effectively avoid the occurrence of false matches.

[0073] Moreover, the road topology shown by the road centerline is an abstract road feature that removes useless features in the road. The road features are obvious, which not only ensures the accuracy of the positioning results, but also the topological operation greatly reduces the complexity of the operation and improves the calculation efficiency.

[0074] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0075] Figure 1 FIG. 2 shows a flow chart of a laser global positioning method based on road topology of the present invention. Figure 1 As shown, it includes the following steps:

[0076] Step 1: construct a binary plane image of the obstacle based on the point cloud data obtained by the multi-line laser radar at the current moment.

[0077] In one embodiment, this step uses a multi-line laser radar carried by an unmanned autonomous mobile platform to collect and acquire raw point cloud data in real time and record the acquisition time. Based on the point cloud height difference algorithm, a binary plane image of each frame of the obstacle is constructed and filtered preprocessed.

[0078] The filtering process of step 1 specifically includes the following sub-steps:

[0079] S11, using breadth-first traversal, obtain all eight-neighborhood connected pixel blocks in the binary plane image of the obstacle.

[0080] S12, calculating the number of pixels in the eight-neighborhood connected pixel block obtained in S11, if the number of pixels is less than a set first threshold, setting all pixel values ​​in the eight-neighborhood connected pixel block to zero, thereby filtering out noise.

[0081] Step 2: extract the road centerline from the obstacle binary plane image to obtain the road topology at the current moment.

[0082] In this step, the watershed algorithm is used to extract the road centerline from the obstacle plane image constructed in step 1, and the point cloud data is converted into a road network model with practical value to obtain the road topology at the current moment. The visualization result is as follows: Figure 2 shown. Figure 2 Figure (c) shows a cross-shaped road centerline.

[0083] This step 2 specifically includes the following sub-steps:

[0084] S21, using the obstacle binary plane image obtained in step 1 and pre-processed by filtering as the input of the watershed algorithm. In the image, the obstacle is determined by the height difference of the point cloud, forming a clear binary image feature.

[0085] S22, the watershed algorithm is used to process the image of S21. By simulating the natural flow of water droplets on the terrain, the dividing line in the image, that is, the center line of the road, is found and determined. This process will process the eight-neighborhood connected pixel blocks in S1, segment them, and identify the key features representing the center line of the road.

[0086] S23, extract accurate information of the road centerline from the algorithm segmentation result, and construct the road topology accordingly. The topology structure will describe the layout of the road at the current moment in detail, providing necessary spatial information reference for subsequent navigation and positioning tasks.

[0087] Step 3, road topology feature judgment: Determine whether the current road topology extracted in step 2 is an intersection or corner. If so, match it in the pre-constructed environment topology map to find the matching node, that is, the node where the unmanned autonomous mobile platform is currently located; extract the road topology and posture corresponding to the matching node, and the posture is the rough positioning result. The posture includes the lateral displacement x, the longitudinal displacement y and the yaw angle. If it is not an intersection or a corner, continue the processing of steps 1 and 2 and continue to judge.

[0088] In this step, the pre-constructed environment topology map is an undirected connected graph, such as Figure 3 As shown in the figure, the information stored in the nodes in the figure includes the position and posture of the unmanned autonomous mobile platform and the corresponding road topology. The circles in the figure represent the nodes of the global prior map, and the lines represent the edges of the map. The global prior topology map can be formed through steps one and two. By matching the current node through depth-first search in the environment topology map, a rough positioning result can be obtained.

[0089] In this step, the method for determining whether the current road topology is an intersection or a corner is as follows:

[0090] S301, calculating the number of branches n of the road topology extracted in step 2, and the calculation formula is:

[0091]

[0092] Where E is the set of edge points of the topological image, I(i,j) is the value of the pixel at (i,j) in the image, and the value of I(i,j) is 0 or 1.

[0093] S302, if the number of branches n calculated in S301 is greater than 2, it is preliminarily determined that the road topology at the current moment is an intersection, and step S303 is executed; if the number of branches n is equal to 2, step S304 is executed to further determine whether the road topology at the current moment is a corner.

[0094] S303, n>2, extract the topological feature points of the intersection as follows:

[0095]

[0096] The above formula is used to find a point (i, j) with the largest number of edge points in the 3x3 neighborhood around it. This point is called the topological feature point of the intersection. The formula sums the function values ​​of the surrounding 8 points and subtracts the function value of the center point. The formula ensures that the selected center point is the point with the most edge points around it. By finding the point with the most edge points in the 3x3 neighborhood, the feature points of the intersection can be accurately located. This extraction of topological feature points of the intersection has advantages in accuracy and robustness. In terms of accuracy, by considering the distribution of edge points in the neighborhood, the center position of the intersection can be determined more accurately; in terms of robustness, the calculation in the 3x3 neighborhood can smooth out some noise and errors, and improve the stability of feature point extraction. Then execute step S305.

[0097] S304, n = 2, then calculate the maximum curvature κ of the road topology at the current moment max :

[0098]

[0099] Where R(i,j) is (i,j),p s ,p e The radius of the circumscribed circle determined by the three points, where p s ,p e are the starting point and end point of the road topology at the current moment, respectively, and the calculation formula is:

[0100]

[0101] If the maximum curvature κ is obtained max If it is greater than the set value C, it is preliminarily determined that the road topology at the current moment is a corner, and the point with the maximum curvature is taken as the topological feature point of the corner. p is:

[0102] p=argmaxκ(i,j) (5)

[0103] This step finds the starting point and the end point by minimizing a cost function. This cost function is based on the sum of squares of the function values ​​in the 3x3 neighborhood, reflecting the distribution of points in the neighborhood. The starting point and the end point of the topology are used as topological feature points. At the same time, the point with the maximum curvature is also a feature point. Therefore, for a corner, the topological feature points include the starting point, the end point, and the point with the maximum curvature. Then execute step S305.

[0104] S305: If the current road topology preliminarily meets the requirements of an intersection or a corner, the time of receiving the corresponding point cloud at the current moment is recorded and compared with the time when the same road feature appeared last time. If the time interval ΔT < C step , C step If the interval is set, the current road topology is finally determined to be an intersection or a corner.

[0105] In step 3, when it is determined that the current scene is an intersection or a corner, the process of determining the rough positioning result includes:

[0106] S311, given the initial node of the platform in the environment topology map, and recording the historical road topology type sequence passed;

[0107] S312, in a given environment topology map, searching for a first path that conforms to a historical topology type sequence through a depth-first search;

[0108] S313, output the posture information stored in the last node of the path obtained in S312 to obtain a global coarse positioning result.

[0109] Step 4: Calculate the relative posture transformation between the current road topology and the matching node road topology obtained in step 3; compensate the relative posture transformation to the rough positioning result to obtain the precise posture of the unmanned autonomous mobile platform and achieve global positioning.

[0110] In this embodiment, this step includes the following sub-steps:

[0111] S41, align the topological feature points of the current road topology and the topological feature points of the matching node road topology. For a corner, there are three feature points, and the alignment can be completed by minimizing the Euclidean distance between the current topological feature points and the topological feature points of the matching node road topology using the least squares method. In S42, the point with the maximum curvature is used as the rotation center.

[0112] S42, taking the topological feature point as the center, the current road topology is rotated by different angles θ, and the calculation formula is:

[0113] θ=kρ,k∈[0,M-1] (6)

[0114] In the formula, the angular resolution is ρ = 2π / M, where M represents the angular resolution when the current road topology is rotated, that is, a complete 360-degree circle is divided into M equal parts. This means that when calculating or analyzing road topological features, M different angles from 0 to 360 degrees are considered. By segmenting in this way, the road topology can be analyzed and calculated at different angles around the feature point. This method helps to analyze the changes in road topology in different directions, so as to more accurately determine the shape and characteristics of the road.

[0115] S43, at each rotation angle θ, sequentially search for the point with the smallest Euclidean distance between each point in the current road topology and the matching node road topology, and the distance calculation formula is:

[0116]

[0117] In the formula, pi ,q j are the points in the current road topology and the matching node road topology respectively. i , find the point q with the smallest distance in the matching node road topology j . It is further necessary to determine whether the distance d satisfies d<C d , C d To set the threshold, if it is met, it is considered that p i The corresponding node road topology point q j Match successfully, p i ,q j Recorded as matching topological point.

[0118] S44, at each rotation angle θ, through the calculation in S43, obtain the number of matching topology points N between the current road topology and the matching node road topology matched , calculate the road topology matching degree score corresponding to the rotation angle θ, which is calculated as follows:

[0119]

[0120] S45, the highest road topology matching score for all rotation angles max , as the final matching score between the current road topology and the matching node road topology;

[0121] S46: If the final matching score in S45 exceeds the third threshold, the global positioning is considered successful. At this time, the relative translation transformation of the topological feature points in S41 and the highest road topology matching score in S45 can be obtained. max The relative rotation transformation of the corresponding road topology is used to compensate the relative translation transformation and relative rotation transformation to the rough positioning result obtained in step 3, and the precise positioning result is output. The calculation formula is:

[0122]

[0123] Where Δt and ΔR are the compensated relative translation and relative rotation, respectively, and T′ is the coarse positioning result.

[0124] At this point, all steps of the method of the present invention are completed.

[0125] Figure 4The curves show the accuracy-recall performance comparison of three different laser global positioning methods. Analysis shows that the accuracy of the method of the present invention (*-shaped node line) is relatively high in the entire recall range, close to 1, which means that it can correctly retrieve most of the positioning points. The performance of the Scan Context descriptor method (Scan Context) method (○-shaped node line) is relatively high when the recall rate is low, but with the increase of the recall rate, the accuracy decreases more significantly. The performance of the multi-dimensional projection descriptor method (M2DP) method (□-shaped node line) is relatively low in the entire recall range, and with the increase of the recall rate, the accuracy decreases. After comparison, it can be seen that the method of the present invention is superior to the traditional method in both accuracy and recall, which not only reduces the possibility of positioning errors, but also reduces the computational burden of subsequent processing steps, thereby improving the efficiency and reliability of the overall system.

[0126] Figure 5 This is a result diagram of successfully achieving global positioning using the method of the present invention. The thinner curve is the travel trajectory, and the bold part is the position where global positioning is successfully achieved, indicating that the method of the present invention has practical application value.

[0127] Based on the above method, the present invention also provides a laser global positioning system based on road topology, such as Figure 6 As shown, the system includes: an environment topology map storage module, a road topology acquisition module, a topology type judgment module, a coarse positioning module and a fine positioning module;

[0128] The environmental topology map storage module is used to store a pre-constructed environmental topology map, which is an undirected connected graph. The information stored in the nodes in the graph includes the position and posture of the unmanned autonomous mobile platform and the corresponding road topology;

[0129] The road topology acquisition module is used to construct a binary plane image of the obstacle based on the point cloud data obtained by the multi-line laser radar at the current moment; extract the road centerline from the binary plane image of the obstacle to obtain the road topology at the current moment;

[0130] The topology type determination module is used to determine whether the current road topology obtained by the road topology acquisition module is an intersection or a corner, and if so, to start the coarse positioning module;

[0131] The coarse positioning module is used to match the environmental topology map stored in the environmental topology map storage module after being started, find the matching node, that is, the node where the unmanned autonomous mobile platform is currently located; extract the road topology and posture corresponding to the matching node, the posture is the coarse positioning result, and send it to the fine positioning module;

[0132] The precise positioning module is used to calculate the relative posture transformation between the road topology at the current moment and the matching node road topology obtained by the rough positioning module; the relative posture transformation is compensated to the rough positioning result to obtain the precise posture of the unmanned autonomous mobile platform and realize global positioning.

[0133] In a preferred embodiment, if Figure 7 As shown, the topology type judgment module includes a first judgment submodule, an intersection topology feature point determination submodule, a corner topology feature point determination submodule, and a second judgment submodule;

[0134] The first judgment submodule is used to judge whether the number of branches n of the road topology at the current moment is greater than 2. If n is greater than 2, it is preliminarily determined that the road topology at the current moment is an intersection, and the intersection topology feature point determination submodule is started; if the number of branches n is equal to 2, the corner topology feature point determination submodule is started;

[0135] A submodule for determining topological feature points of an intersection, which is used to extract the topological feature points of the intersection, and the topological feature points make the number of edge points in the surrounding 3x3 neighborhood the largest, and notify the second judgment submodule;

[0136] A corner topology feature point determination submodule is used to determine whether it is a corner according to the maximum curvature of the road topology at the current moment, and obtain the topology feature points of the corner to notify the second judgment submodule;

[0137] The second judgment submodule is used to compare the reception time of the point cloud data at the current moment with the reception time of the point cloud data of the last time the same road feature appeared when the road topology at the current moment meets the road features of an intersection or a corner. If the time interval ΔT is less than the set interval, it is finally determined that the road topology at the current moment is indeed an intersection or a corner.

[0138] In a preferred embodiment, if Figure 8 As shown, the precise positioning module includes an alignment submodule, a topological point matching submodule, a matching degree scoring submodule and a transformation submodule.

[0139] An alignment submodule is used to align the topological feature points of the current road topology with the topological feature points of the matching node road topology;

[0140] The topological point matching submodule is used to rotate the current road topology by different rotation angles θ according to the angular resolution ρ, with the aligned topological feature points as the center; at each rotation angle θ, for each topological point p in the current road topology i , search for the topological point q with the smallest Euclidean distance in the road topology of the matching node j , if p i ,q j If the distance is less than the second threshold, p i,q j Recorded as matching topological point;

[0141] The matching degree scoring submodule is used to perform topological point matching using the topological point matching submodule at each rotation angle θ to obtain the number of matching topological points N between the current road topology and the matching node road topology. matched ; N matched The ratio of the total number of topological points N in the current road topology is used as the road topology matching degree score corresponding to the rotation angle θ; the highest road topology matching degree score of all rotation angles θ is calculated as max as the final matching score of the road topology at the current moment and the road topology of the matching node; if the final matching score exceeds the third threshold, it is considered that the global positioning is successful, and the transformation submodule is started;

[0142] After the transformation submodule is started, the relative translation transformation of the topological feature points aligned by the alignment submodule is obtained, and the highest road topology matching degree score is obtained. max The relative rotation transformation of the corresponding road topology, the relative translation transformation and relative rotation transformation are compensated to the rough positioning result, and the precise positioning result T is output. The calculation formula is:

[0143]

[0144] Where Δt and ΔR are the compensated relative translation and relative rotation, respectively, and T′ is the coarse positioning result.

[0145] The above is only an embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included within the scope of the claims of the present application.

Claims

1. A laser global positioning method based on road topology, used for positioning an unmanned autonomous mobile platform equipped with a multi-line laser radar, characterized in that: The method includes: Step 1: construct a binary plane image of the obstacle based on the point cloud data obtained by the multi-line laser radar at the current moment; Step 2: extracting the road centerline from the obstacle binary plane image to obtain the road topology at the current moment; Step 3: If the current road topology extracted in step 2 is an intersection or a corner, match it in the pre-constructed environment topology map to find the matching node, that is, the node where the unmanned autonomous mobile platform is currently located; extract the road topology and posture corresponding to the matching node, and the posture is the rough positioning result; wherein the pre-constructed environment topology map is an undirected connected graph, and the information stored in the nodes in the graph includes the posture of the unmanned autonomous mobile platform and the corresponding road topology; Step 4: Calculate the relative posture transformation between the current road topology and the matching node road topology obtained in step 3; compensate the relative posture transformation to the rough positioning result to obtain the precise posture of the unmanned autonomous mobile platform and achieve global positioning.

2. A laser global positioning method based on road topology according to claim 1, characterized in that: In the step 1, a height difference-based algorithm is used to construct the binary plane image of the obstacle.

3. The laser global positioning method based on road topology according to claim 1, characterized in that: In the step 1, the constructed obstacle binary plane image is further subjected to image filtering, and the specific steps are as follows: Step S11, using breadth-first traversal to obtain all eight-neighborhood connected pixel blocks in the binary plane image of the obstacle; Step S12, calculating the number of pixels in the eight-neighborhood connected pixel block obtained in step S11, if the number of pixels is less than a set first threshold, setting all pixel values ​​in the eight-neighborhood connected pixel block to zero to filter out noise.

4. The laser global positioning method based on road topology according to claim 1, characterized in that: In the step 2, the algorithm for extracting the road centerline is a watershed algorithm.

5. The laser global positioning method based on road topology according to claim 1, characterized in that: In step 3, whether the extracted road topology at the current moment is an intersection or a corner is determined by: Step S301, determining the number of branches n of the road topology at the current moment extracted in step 2; Step S302, if the number of branches n is greater than 2, it is preliminarily determined that the road topology at the current moment is an intersection, and step S303 is executed; if the number of branches n is equal to 2, step S304 is executed to further determine whether the road topology at the current moment is a corner; Step S303, extracting the topological feature points of the intersection, which topological feature points maximize the number of edge points in the surrounding 3x3 neighborhood; executing step 305; Step S304, find the starting point and end point p of the current road topology by minimizing the cost function (I) s ,p e ; Where I(i,j) is the value of the pixel at (i,j) in the image; Let R(i,j) be (i,j),p s ,p e The radius of the circumscribed circle determined by the three points is used to calculate the maximum curvature κ of the road topology at the current moment. max , if the maximum curvature κ max If it is greater than the set value C, it is preliminarily determined that the road topology at the current moment is a corner, and the point with the maximum curvature is taken as the topological feature point of the corner, and step S305 is executed; Step S305, if the current road topology meets the road features of an intersection or a corner, the reception time of the point cloud data at the current moment is compared with the reception time of the point cloud data with the same road features last time. If the time interval ΔT is less than the set interval, it is finally determined that the current road topology is indeed an intersection or a corner.

6. The laser global positioning method based on road topology according to claim 1, characterized in that: In step 3, the matching is performed in the pre-constructed environment topology map to find the node where the unmanned autonomous mobile platform is currently located, and the road topology and posture corresponding to the current node are extracted as the specific steps of the rough positioning result include: Step S311, giving the initial node from which the unmanned autonomous mobile platform departs in the environmental topology map, and recording the historical road topology type sequence passed; Step S312, searching the environment topology map for the first path that meets the historical road topology type sequence through depth-first search; Step S313, output the posture stored in the last node in the path obtained in S312 to obtain a rough positioning result.

7. The laser global positioning method based on road topology according to claim 1, characterized in that: The step 4 specifically includes the following steps Step S41, using the topological feature points to align the topological feature points of the current road topology and the matching node road topology; Step S42, taking the topological feature point as the center, according to the angular resolution ρ, the road topology at the current moment is rotated by different rotation angles θ=kρ, k∈[0,M-1] in sequence, where M is the angular resolution of the road topology rotation at the current moment; Step S43: at each rotation angle θ, for each topological point p in the road topology at the current moment i , search for the topological point q with the smallest Euclidean distance in the road topology of the matching node j ; If the minimum distance d is less than the set second threshold, p i ,q j Recorded as a pair of matching topological points; Step S44, at each rotation angle θ, by calculating in step S43, the number of matching topological points N between the current road topology and the matching node road topology is obtained. matched , N matched The proportion of the total number of topological points N in the road topology at the current moment is used as the road topology matching degree score corresponding to the rotation angle θ; Step S45: Sum the highest road topology matching scores of all rotation angles θ max As the final matching score of the current road topology and the matching node road topology; Step S46: If the final matching score in step S45 exceeds the third threshold, the global positioning is considered successful; at this time, the relative translation transformation of the topological feature points aligned in step S41 and the highest road topology matching degree score in step S45 are obtained. max The relative rotation transformation of the corresponding road topology is used to compensate the relative translation transformation and relative rotation transformation to the rough positioning result obtained in step 3, and the precise positioning result T is output. The calculation formula is: Where Δt and ΔR are the compensated relative translation and relative rotation, respectively, and T′ is the coarse positioning result.

8. A laser global positioning system based on road topology, used for positioning an unmanned autonomous mobile platform equipped with a multi-line laser radar, characterized in that: include: Environmental topology map storage module, road topology acquisition module, topology type judgment module, coarse positioning module and fine positioning module; The environmental topology map storage module is used to store a pre-constructed environmental topology map, which is an undirected connected graph. The information stored in the nodes in the graph includes the position and posture of the unmanned autonomous mobile platform and the corresponding road topology; The road topology acquisition module is used to construct a binary plane image of the obstacle based on the point cloud data obtained by the multi-line laser radar at the current moment; extract the road centerline from the binary plane image of the obstacle to obtain the road topology at the current moment; The topology type determination module is used to determine whether the current road topology obtained by the road topology acquisition module is an intersection or a corner, and if so, to start the coarse positioning module; The coarse positioning module is used to match the environmental topology map stored in the environmental topology map storage module after being started, find the matching node, that is, the node where the unmanned autonomous mobile platform is currently located; extract the road topology and posture corresponding to the matching node, the posture is the coarse positioning result, and send it to the fine positioning module; The precise positioning module is used to calculate the relative posture transformation between the road topology at the current moment and the matching node road topology obtained by the rough positioning module; the relative posture transformation is compensated to the rough positioning result to obtain the precise posture of the unmanned autonomous mobile platform and realize global positioning.

9. The laser global positioning system based on road topology according to claim 8, characterized in that: The topology type judgment module includes a first judgment submodule, an intersection topology feature point determination submodule, a corner topology feature point determination submodule, and a second judgment submodule; The first judgment submodule is used to judge whether the number of branches n of the road topology at the current moment is greater than 2. If n is greater than 2, it is preliminarily determined that the road topology at the current moment is an intersection, and the intersection topology feature point determination submodule is started; If the number of branches n is equal to 2, the corner topology feature point determination submodule is started; The intersection topology feature point determination submodule is used to extract the topology feature points of the intersection, which topology feature points maximize the number of edge points in the surrounding 3x3 neighborhood, and notify the second judgment submodule; The corner topology feature point determination submodule is used to determine whether it is a corner according to the maximum curvature of the road topology at the current moment, and obtain the topology feature points of the corner to notify the second judgment submodule; The second judgment submodule is used to compare the reception time of the point cloud data at the current moment with the reception time of the point cloud data of the last time the same road feature appeared when the road topology at the current moment meets the road features of an intersection or a corner. If the time interval ΔT is less than the set interval, it is finally determined that the road topology at the current moment is indeed an intersection or a corner.

10. The laser global positioning system based on road topology according to claim 8, characterized in that: The precise positioning module includes an alignment submodule, a topological point matching submodule, a matching degree scoring submodule and a transformation submodule; The alignment submodule is used to align the topological feature points of the road topology at the current moment and the matching node road topology using the topological feature points; The topological point matching submodule is used to rotate the current road topology by different rotation angles θ according to the angle resolution ρ, with the topological feature point as the center; at each rotation angle θ, for each topological point p in the current road topology i , search for the topological point q with the smallest Euclidean distance in the road topology of the matching node j , if p i ,q j If the distance is less than the second threshold, p i ,q j Recorded as matching topological point; The matching degree scoring submodule is used to perform topological point matching using the topological point matching submodule at each rotation angle θ to obtain the number of matching topological points N between the current road topology and the matching node road topology. matched ; N matched The ratio of the total number of topological points N in the current road topology is used as the road topology matching degree score corresponding to the rotation angle θ; the highest road topology matching degree score of all rotation angles θ is calculated as max as the final matching score of the road topology at the current moment and the road topology of the matching node; if the final matching score exceeds the third threshold, it is considered that the global positioning is successful, and the transformation submodule is started; After the transformation submodule is activated, the relative translation transformation amount of the topological feature points of the alignment submodule is obtained, and the highest road topology matching degree score is obtained. max The relative rotation transformation of the corresponding road topology, the relative translation transformation and relative rotation transformation are compensated to the rough positioning result, and the precise positioning result T is output. The calculation formula is: Where Δt and ΔR are the compensated relative translation and relative rotation, respectively, and T′ is the coarse positioning result.

Citation Information

Cited By

  • Unmanned environment sensing accurate positioning system and method

    CN119986743A