Urban tunnel high-precision positioning method based on multi-source information fusion

Abstract

The invention belongs to the technical field of positioning navigation, and discloses an urban tunnel high-precision positioning method based on multi-source information fusion, which comprises the following steps: firstly, acquiring a point cloud of an environment where a target is located according to a laser radar, and measuring angular velocity and acceleration information of the target by an inertial measurement unit; a Beidou satellite navigation system and a pseudo satellite obtain coordinate position information of a target, and then obtain edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points of an environment where the target is located; according to the obtained features, the maximum number of feature points needing to be optimized is detected, a scene evaluation and target optimization objective function is designed to optimize the maximum number of the feature points needing to be optimized, a positioning optimization objective function is designed, and an optimal positioning result is solved based on a factor graph incremental smoothing optimization method. The method is suitable for extracting various features and combining with multi-source information to realize high-precision positioning of the urban tunnel, and the accuracy and reliability of positioning in the tunnel environment are guaranteed.

Description

Technical Field

[0001] The invention belongs to the technical field of positioning and navigation, and relates to a high-precision positioning method for an urban tunnel based on multi-source information fusion. Background Art

[0002] In recent years, the scale of tunnel construction in my country has continued to expand, especially in urban tunnels. As an important transportation link, tunnels are of vital importance for their efficiency and safety. However, due to the unique environmental characteristics of tunnels, such as signal shielding and multipath interference, the performance of traditional navigation and positioning technologies has been greatly reduced, and vehicles and pedestrians often encounter positioning information loss or a sharp drop in accuracy.

[0003] The current navigation and positioning technology has high positioning accuracy and can maintain stable navigation capabilities for a long time in areas with wide vision and no obstructions. However, in areas where satellites are blocked for a long time, such as tunnels, Beidou navigation alone may cause signal loss, resulting in reduced positioning accuracy and temporary failure of navigation capabilities. In order to solve this problem, common indoor navigation technologies include infrared technology, Bluetooth technology, ZigBee technology, ultrasonic technology, RFID technology, etc. These technologies have good performance in indoor buildings such as shopping malls, parking lots, and warehouses, but the positioning effect is not ideal for tunnels with thick walls and long lengths. Therefore, a high-precision positioning method for urban tunnels based on multi-source information fusion is proposed to realize high-precision positioning feature extraction of Beidou signals in tunnel space, especially in ultra-long tunnel environments, and ensure continuous and accurate positioning services in the special environment of urban tunnels, which is of great significance. Summary of the invention

[0004] The purpose of the present invention is to provide a high-precision positioning method for urban tunnels based on multi-source information fusion. By integrating multiple feature extraction methods, it can effectively overcome the adverse effects of the tunnel environment on positioning and provide decision-making data support for realizing fast, high-precision, coherent and reliable positioning services for tunnels.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] A high-precision positioning method for urban tunnels based on multi-source information fusion, comprising the following steps:

[0007] S1. Obtain the point cloud of the tunnel environment where the target is located through the laser radar, measure the angular velocity and acceleration information of the target in three-dimensional space through the inertial measurement unit, and obtain the coordinate position information of the target through the Beidou satellite navigation system and pseudo-satellite;

[0008] S2. According to step S1, edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points of the tunnel environment where the target is located are obtained;

[0009] S3. Based on the acquired edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points, the maximum number of feature points that need to be optimized is detected based on the laser radar positioning performance degradation detection method and the point cloud feature optimization screening method. ;

[0010] S4. Design scene evaluation and target optimization objective functions based on the acquired edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points, and use the scene evaluation and target optimization objective functions to determine the maximum number of feature points that need to be optimized. Optimize

[0011] S5. Design a positioning optimization objective function according to the data obtained in steps S1 to S4, and obtain the optimal positioning result based on the factor graph incremental smoothing optimization method.

[0012] As a limitation, the specific process of step S2 is:

[0013] S21. Calculate the normal vector angle between the point in the point cloud and its neighboring points according to the feature point extraction method based on the normal vector angle , obtain the edge feature points and plane feature points of the tunnel environment where the target is located;

[0014] S22, converting the point cloud into a depth image, and extracting the rod-shaped edge features of the tunnel environment where the target is located by a clustering method;

[0015] S23. Extract reflectivity greater than the threshold from the point cloud point, , as the reflectivity feature point.

[0016] As a further limitation, the specific process of step S21 is:

[0017] S211, according to the feature point extraction method based on the normal vector angle, calculate the normal vector angle between the point in the point cloud and its neighboring points , the normal vector angle The calculation formula is: (1)

[0018] in, For vector and vector The angle between is the current point scanned by the laser radar, , For LiDAR scans and Points on the same scan line;

[0019] S212, divide each scanning line scanned by the laser radar into six segments on average, and calculate the normal vector angle of each point in each segment. Sort by size, The two points with the largest angle are taken as edge points, and the remaining The first ten points with the largest angles are used as the extended edge points, and then the edge points and the extended edge points are stored in the variables In the example, the edge feature points of the tunnel environment where the target is located are obtained;

[0020] Will The four points with the smallest angle are taken as plane points, and the remaining points except edge feature points, plane points, and non-adjacent points and non-filtered points other than edge feature points and plane points are taken as extended plane points, and then the plane points and extended plane points are stored in variables In the above figure, the plane feature points of the tunnel environment where the target is located are obtained.

[0021] As a further limitation, the specific process of step S22 is:

[0022] S221, converting the point cloud of the tunnel environment where the target is located acquired by the laser radar into a depth image;

[0023] S222, dividing the pixels of the depth image into different regions by a clustering method based on depth information;

[0024] Traverse all the pixels in the depth image from top to bottom and from left to right, and remove the pixels from the bottom of the depth image upwards. The pixels of the row, , is the height of the depth image, and the remaining pixels in the depth image are stored in an open set middle;

[0025] For the open set Each valid pixel in , traverse the neighboring pixels in the four directions of front, back, left, and right, and compare the valid pixels The distance difference between the neighboring pixels in the four directions of front, back, left, and right is less than the threshold. , The range is 0 to 1 meter, then the current valid pixel point Add to Cluster subsets and the current valid pixel point And the neighboring pixels that meet the threshold condition are from the open set Repeat the above process until the set is opened. If it is empty or there is no neighboring pixel point that meets the above conditions, the final result contains Cluster subsets Collection cluster, Greater than 0 and less than the effective pixel Total number of

[0026] S223, rod-shaped feature candidate screening;

[0027] Calculating Collections Each cluster subset in the cluster If the aspect ratio is less than 1, the cluster subset is discarded. ;

[0028] Calculating Collections Each remaining subset of clusters of The difference, Cluster subset The height of the highest point in Cluster subset The height value of the lowest point in the cluster subset of , For rod-shaped feature candidates to meet the threshold, , then the cluster subset Save as rod feature candidate;

[0029] S224, rod edge feature extraction;

[0030] Each cluster subset saved as a rod-shaped feature candidate is then The points in the image are fitted into a circle, and then the center of the fitted circle is used to represent the edge point of the cylindrical object on the current scan line to complete the rod-shaped edge feature extraction of the tunnel environment where the target is located.

[0031] As the second limitation, the specific process of step S3 is:

[0032] S31. Definition For the All feature points contained in the frame point cloud include edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points. is the maximum number of feature points that need to be optimized;

[0033] S32. Definition As a function to measure the positioning performance of the laser radar, the degradation factor is obtained ; (2)

[0034] in, For the The frame point cloud contains the information matrix of all feature points;

[0035] S33, Selection The function is used as a measure of scene feature degradation, and we get: (3)

[0036] in, For After solving the determinant, take the logarithm;

[0037] S34, calculate the degradation factor every 1s to 2s , set the threshold ,when When , 20% of all feature points are selected as the maximum number of feature points that need to be optimized ,Right now ,otherwise .

[0038] As a further limitation, the specific process of step S4 is:

[0039] S41, designing scene evaluation and target optimization objective functions according to the acquired edge feature points, plane feature points, rod-shaped edge features, and reflectivity feature points;

[0040] definition is the edge feature point, plane feature point, rod edge feature, and reflectivity feature point subset to be optimized. for The matrix formed by the scenario evaluation and target optimization objective function is as follows: (4)

[0041] in, , is a random feature subset, For A collection of

[0042] S42. Determine a random feature subset The size is , is the attenuation factor;

[0043] S43. From the map Search for a random subset of features in For each feature point in , calculate the residual function. If the feature point has been calculated before, skip it;

[0044] S44, Update , until the iteration condition of the maximum set round is met, the residual function is minimized as the optimization goal, and the features that make the scene evaluation and target optimization objective functions optimal are obtained, so as to achieve the maximum number of feature points that need to be optimized Make optimizations.

[0045] As a further limitation, the specific process of step S5 is:

[0046] S51, design positioning optimization objective function;

[0047] Based on the factor graph incremental smoothing optimization of Beidou satellite navigation system, pseudo-satellite, lidar, and inertial measurement unit fusion, the system is assumed to be The state quantity is: (5)

[0048] in, is the three-dimensional position vector obtained based on the BeiDou satellite navigation system, pseudo-satellite and lidar. is the attitude quaternion vector obtained based on the inertial measurement unit, is the three-dimensional velocity vector obtained based on the inertial measurement unit;

[0049] S52, based on the factor graph incremental smoothing optimization method, solving to obtain the optimal positioning result;

[0050] Various residual constraint functions are constructed by marginalizing errors, inertial pre-integration factors, laser line and surface factors, laser reflectivity factors, and pseudo-satellite position factors to form a maximum likelihood estimation function and perform factor graph incremental optimization: (6)

[0051] in, is the marginalized residual of the Schur complement of the matrix, is the inertia pre-integration residual, It is the line and surface residual of the point cloud obtained by the lidar. is the reflectivity residual of the point cloud obtained by the lidar, is the pseudo-satellite position residual, is the number of inertial measurement unit data, The number of point clouds acquired for the LiDAR, is the number of position data obtained through pseudo-satellites, The system to be solved is at the current time The state quantity of

[0052] Constructing a point-to-line distance error function : (7)

[0053] in, For the Frame point cloud extraction edge feature points, For the Frame point cloud extraction Expanded edge feature points, For the Frame point cloud extraction Expanded edge feature points, and Must form a line and and and The sum of the distances is the smallest;

[0054] Construct the distance function between points and surfaces : (8)

[0055] in, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, , and must form a plane and , and and The sum of distances is minimal; (9)

[0056] By using nonlinear optimization to solve formula (6), the optimal positioning result can be obtained.

[0057] Due to the adoption of the above technical solution, the present invention has the following technical advances compared with the prior art:

[0058] (1) The present invention obtains edge feature points and plane feature points of the tunnel environment where the target is located through a feature point extraction method based on the normal vector angle. This method does not rely on the distance between adjacent points, but uses the normal vector angle to characterize the smoothness of the plane where the point is located, thereby alleviating the problem that the traditional method is heavily dependent on the quality of the provided data, resulting in low feature extraction accuracy;

[0059] (2) The present invention realizes the rapid extraction of rod-shaped edge features through depth images. Rod-shaped edge features will become an important factor in constraining the positioning direction. Extracting effective rod-shaped edge features will improve the robustness of the positioning algorithm in complex environments.

[0060] (3) Based on the extracted multiple features and other multi-source information, the present invention adopts the factor graph incremental smoothing optimization method to solve the positioning optimization objective function, thereby achieving accurate positioning in the tunnel, which has strong practical significance.

[0061] In summary, the present invention is suitable for extracting multiple features and combining multi-source information to achieve high-precision positioning of urban tunnels, effectively overcoming the adverse effects of the tunnel environment on positioning, and providing decision-making data support for realizing fast, high-precision, coherent and reliable positioning services for tunnels. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 It is a method flow chart of a high-precision positioning method for urban tunnels based on multi-source information fusion according to an embodiment of the present invention;

[0063] Figure 2 The figure shows a three-dimensional point cloud image of a section of a tunnel acquired by a laser radar in an embodiment of the present invention;

[0064] Figure 3 FIG. 1 is a schematic diagram of a feature point extraction method based on normal vector angle according to an embodiment of the present invention;

[0065] Figure 4 The figure shows the edge feature points and plane feature points extracted in the embodiment of the present invention;

[0066] Figure 5 The figure shows the edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points of the target in the tunnel environment extracted after the degradation scene evaluation and optimization according to the embodiment of the present invention;

[0067] Figure 6 The figure shows the final feature extraction effect diagram based on the point cloud image in an embodiment of the present invention. DETAILED DESCRIPTION

[0068] In order to better explain the present invention and facilitate understanding, the present invention is described in detail below through specific implementation modes in conjunction with the accompanying drawings.

[0069] like Figure 1 As shown, this embodiment is a high-precision positioning method for urban tunnels based on multi-source information fusion, comprising the following steps:

[0070] S1. Obtain the point cloud of the tunnel environment where the target is located through the laser radar, measure the angular velocity and acceleration information of the target in three-dimensional space through the inertial measurement unit, and obtain the coordinate position information of the target through the Beidou satellite navigation system and pseudo-satellite;

[0071] like Figure 2 The figure shows a 3D point cloud image of a tunnel obtained by the LiDAR. Different colors in the figure reflect the density of the point cloud in the area. The density is arranged in descending order of red, orange, yellow, green, and blue. The higher the density, the more accurate it is.

[0072] S2. According to step S1, edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points of the tunnel environment where the target is located are obtained;

[0073] The specific process of step S2 is:

[0074] S21. Calculate the normal vector angle between the point in the point cloud and its neighboring points according to the feature point extraction method based on the normal vector angle , obtain the edge feature points and plane feature points of the tunnel environment where the target is located;

[0075] In the LiDAR positioning algorithm, edge features and plane features have been proven to be effective representatives of environmental features. The traditional edge feature point and plane feature point extraction method is based on curvature feature point extraction, which calculates the point in its neighborhood. The curvature inside is used to characterize the smoothness of the point. If the curvature of the point is too large, it is an edge feature point. On the contrary, if the curvature is small, it is a plane feature point. The specific calculation formula of the curvature is: ①

[0076] in, For the The first frame in the point cloud Points, For the The first frame in the point cloud Points, For The neighborhood consisting of the five points before and after the center;

[0077] As can be seen from formula ①, the feature point extraction method based on curvature mainly characterizes the smoothness of the plane where the point is located by calculating the distance of the point neighborhood. This method can quickly select edge feature points and plane feature points, but it is heavily dependent on the quality of the provided data and has low accuracy. To overcome this problem, this step proposes a feature point extraction method based on the normal vector angle. This method does not rely on the distance between adjacent points, but uses the normal vector angle to characterize the smoothness of the plane where the point is located.

[0078] The specific process of this step is:

[0079] S211, according to the feature point extraction method based on the normal vector angle, calculate the normal vector angle between the point in the point cloud and its neighboring points ;

[0080] like Figure 3 The figure shows a schematic diagram of the feature point extraction method based on the normal vector angle. The dotted line in the figure represents the laser beam emitted by the laser radar, and the mark on the solid line represents the point scanned by the laser radar on the same scanning line. Suppose the current point scanned by the laser radar is , then As the center, we can get the points on the same scan line and ;Depend on Figure 3 It can be seen that The local plane where the point is located is more prominent relative to the center of the lidar, so Points should be extracted as edge feature points; and vector and The angle of is larger at this time; similarly, if the As the center, it can be seen that the vector and The angle between them is close to 0. The local plane where is located happens to be relatively flat; therefore, by calculating the angle between the two vectors, we can reflect The smoothness of the plane near the point can effectively extract edge feature points and plane feature points;

[0081] By point For example, the normal vector angle The calculation formula is: ②

[0082] in, For vector and vector The angle between is the current point scanned by the laser radar, , For LiDAR scans and Points on the same scan line;

[0083] S212, in order to ensure uniform extraction of edge feature points and plane feature points, each scanning line scanned by the laser radar is evenly divided into six segments, and the points contained in each segment are divided according to the normal vector angle Sort by size, The two points with the largest angle are taken as edge points, and the remaining The first ten points with the largest angles are used as the extended edge points, and then the edge points and the extended edge points are stored in the variables In the example, the edge feature points of the tunnel environment where the target is located are obtained;

[0084] Will The four points with the smallest angle are taken as plane points, and the remaining points except edge feature points, plane points, and non-adjacent points and non-filtered points other than edge feature points and plane points are taken as extended plane points, and then the plane points and extended plane points are stored in variables In the example, the plane feature points of the tunnel environment where the target is located are obtained; Figure 4 The extracted edge feature points and plane feature points are shown;

[0085] S22, converting the point cloud into a depth image, and extracting the rod-shaped edge features of the tunnel environment where the target is located by a clustering method;

[0086] In tunnels or open scenes with sparse features, the rod-shaped edge features will become an important factor constraining the positioning direction; therefore, extracting effective rod-shaped edge features in this step will improve the robustness of the positioning algorithm in complex environments; in order to achieve rapid extraction of rod-shaped edge features, the specific process of this step is as follows:

[0087] S221, converting the point cloud of the tunnel environment where the target is located acquired by the laser radar into a depth image;

[0088] S222, dividing the pixels of the depth image into different regions by a clustering method based on depth information;

[0089] Traverse all the pixels in the depth image from top to bottom and from left to right, and remove the pixels from the bottom of the depth image upwards. The pixels of the row, , is the height of the depth image, and the remaining pixels in the depth image are stored in an open set middle;

[0090] In this step, since the upper and lower positions of the depth image correspond to the scanning order of the laser radar in the environment, the ground point is likely to appear in the lower half of the depth image, so the bottom of the depth image is moved upward. Pixels in the row are removed to avoid interference from ground points;

[0091] For the open set Each valid pixel in , traverse the neighboring pixels in the four directions of front, back, left, and right, and compare the valid pixels The distance difference between the neighboring pixels in the four directions of front, back, left, and right is less than the threshold. , The range is 0 to 1 meter, then the current valid pixel point Add to Cluster subsets and the current valid pixel point And the neighboring pixels that meet the threshold condition are from the open set Repeat the above process until the set is opened. Empty or no neighboring pixel meets the above conditions. No neighboring pixel meets the above conditions refers to a valid pixel. The distance difference between the corresponding neighboring pixels is greater than the threshold, and finally the Cluster subsets Collection cluster, Greater than 0 and less than the effective pixel Total number of

[0092] S223, rod-shaped feature candidate screening;

[0093] Calculating Collections Each cluster subset in the cluster If the aspect ratio is less than 1, the cluster subset is discarded. ;

[0094] Calculating Collections Each remaining subset of clusters of The difference, Cluster subset The height of the highest point in Cluster subset The height value of the lowest point in the cluster subset of , For rod-shaped feature candidates to meet the threshold, , then the cluster subset Save as rod feature candidate;

[0095] S224, rod edge feature extraction;

[0096] Each cluster subset saved as a rod-shaped feature candidate is then The points in are fitted into a circle, and then the center of the fitted circle is used to represent the edge point of the cylindrical object on the current scan line to complete the rod-shaped edge feature extraction of the tunnel environment where the target is located;

[0097] S23. Extract reflectivity greater than the threshold from the point cloud point, , as the reflectivity feature point;

[0098] S3. Based on the acquired edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points, the maximum number of feature points that need to be optimized is detected based on the laser radar positioning performance degradation detection method and the point cloud feature optimization screening method. ;

[0099] Traditional LiDAR and inertial fusion positioning methods face performance degradation problems in complex environments, especially in tunnel environments. The reasons for this problem include two aspects: (a) the lack of effective LiDAR positioning performance degradation detection methods; (b) the inability to optimize the extracted features in real time according to the degree of environmental degradation. To address the above problems, this step has made targeted designs from two aspects: LiDAR positioning performance degradation detection and point cloud feature optimization screening, in order to improve the robustness of LiDAR positioning in complex and changing scenarios.

[0100] The specific process of this step is:

[0101] S31. Definition For the All feature points contained in the frame point cloud include edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points. is the maximum number of feature points that need to be optimized;

[0102] S32. To evaluate whether the LiDAR positioning is degraded, define As a function to measure the positioning performance of the laser radar, the degradation factor is obtained ; ③

[0103] in, For the The frame point cloud contains the information matrix of all feature points;

[0104] S33, Selection The function is used as a measure of scene feature degradation, and we get: ④

[0105] in, For After solving the determinant, take the logarithm;

[0106] S34, calculate the degradation factor every 1s to 2s , set the threshold ,when When , 20% of all feature points are selected as the maximum number of feature points that need to be optimized ,Right now ,otherwise ;

[0107] S4. Design scene evaluation and target optimization objective functions based on the acquired edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points, and use the scene evaluation and target optimization objective functions to determine the maximum number of feature points that need to be optimized. Optimize

[0108] The specific process of this step is:

[0109] S41, designing scene evaluation and target optimization objective functions according to the acquired edge feature points, plane feature points, rod-shaped edge features, and reflectivity feature points;

[0110] definition is the edge feature point, plane feature point, rod edge feature, and reflectivity feature point subset to be optimized. for The matrix formed by the scenario evaluation and target optimization objective function is as follows: ⑤

[0111] in, , is a random feature subset, For A collection of

[0112] S42. Determine a random feature subset The size is , is the attenuation factor;

[0113] S43. From the map Search for a random subset of features in For each feature point in , calculate the residual function. If the feature point has been calculated before, skip it;

[0114] S44, Update , until the iteration condition of the maximum set round is met, the residual function is minimized as the optimization goal, and the features that make the scene evaluation and target optimization objective functions optimal are obtained, so as to achieve the maximum number of feature points that need to be optimized Optimize

[0115] like Figure 5 The figure shows the edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points of the target in the tunnel environment extracted after the degradation scene evaluation and optimization; Figure 6 The figure shows the final feature extraction effect based on the point cloud image. Figure 6 In the figure, the green dot marks are the reflectivity feature points of the extracted tunnel environment, the green rectangle marks are the edge feature points and plane feature points of the extracted tunnel environment, and the red dot and rectangle marks are the rod edge features of the extracted tunnel environment;

[0116] S5. Design a positioning optimization objective function according to the data obtained in steps S1 to S4, and obtain the optimal positioning result based on the factor graph incremental smoothing optimization method;

[0117] The specific process of this step is:

[0118] S51, design positioning optimization objective function;

[0119] Based on the factor graph incremental smoothing optimization of the limited fusion of Beidou satellite navigation system, pseudo-satellite, lidar, and inertial measurement unit, the system is assumed to be The state quantity is: ⑥

[0120] in, is the three-dimensional position vector obtained based on the BeiDou satellite navigation system, pseudo-satellite and lidar. is the attitude quaternion vector obtained based on the inertial measurement unit, is the three-dimensional velocity vector obtained based on the inertial measurement unit;

[0121] S52, based on the factor graph incremental smoothing optimization method, solving to obtain the optimal positioning result;

[0122] Various residual constraint functions are constructed by marginalizing errors, inertial pre-integration factors, laser line and surface factors, laser reflectivity factors, and pseudo-satellite position factors to form a maximum likelihood estimation function and perform factor graph incremental optimization: ⑦

[0123] in, is the marginalized residual of the Schur complement of the matrix, is the inertia pre-integration residual, It is the line and surface residual of the point cloud obtained by the lidar. is the reflectivity residual of the point cloud obtained by the lidar, is the pseudo-satellite position residual, is the number of inertial measurement unit data, The number of point clouds acquired for the LiDAR, is the number of position data obtained through pseudo-satellites, The system to be solved is at the current time The state quantity of

[0124] Constructing a point-to-line distance error function :

[0125] ⑧

[0126] in, For the Frame point cloud extraction edge feature points, For the Frame point cloud extraction Expanded edge feature points, For the Frame point cloud extraction Expanded edge feature points, and Must form a line and and and The sum of the distances is the smallest;

[0127] Construct the distance function between points and surfaces : ⑨

[0128] in, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, , and must form a plane and , and and The sum of distances is minimal; ⑩

[0129] By adopting nonlinear optimization method and solving formula ⑦, the optimal positioning result can be obtained.

[0130] This embodiment adopts a positioning method that considers edge feature points, plane feature points, rod-shaped edge features and reflectivity feature extraction, and performs feature optimization and factor graph incremental smoothing optimization. In order to verify this embodiment, this embodiment is compared with the traditional satellite positioning method (denoted as A1), the positioning method that does not consider edge feature points, plane feature points, rod-shaped edge features and reflectivity feature extraction, and does not consider feature optimization and factor graph incremental smoothing optimization (denoted as A2), and the positioning method that considers edge feature points, plane feature points, rod-shaped edge features and reflectivity feature extraction, and does not consider feature optimization-factor graph incremental smoothing optimization (denoted as A3) for evaluation index comparison. The final evaluation index results of each positioning method are shown in Table 1.

[0131] Table 1 method Positioning time Average positioning error / m A1 Invalidation Invalidation A2 1.25s ±32.41cm A3 1.51s ±20.32cm This embodiment 1.68s ±13.91cm

[0132] By observing the data in Table 1, it can be found that when the positioning method of this embodiment is adopted, although the positioning time is increased, the average positioning error is greatly reduced and the positioning accuracy is significantly improved.

[0133] It should be noted that the above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention is described in detail with reference to the above embodiments, those skilled in the art can still modify the technical solutions described in the above embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A high-precision positioning method for urban tunnels based on multi-source information fusion, characterized in that: The following steps are involved: S1. Obtain the point cloud of the tunnel environment where the target is located through the laser radar, measure the angular velocity and acceleration information of the target in three-dimensional space through the inertial measurement unit, and obtain the coordinate position information of the target through the Beidou satellite navigation system and pseudo-satellite; S2. According to step S1, edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points of the tunnel environment where the target is located are obtained; S3. Based on the acquired edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points, the maximum number of feature points that need to be optimized is detected based on the laser radar positioning performance degradation detection method and the point cloud feature optimization screening method. ; S4. Design scene evaluation and target optimization objective functions based on the acquired edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points, and use the scene evaluation and target optimization objective functions to determine the maximum number of feature points that need to be optimized. Optimize S5. Design a positioning optimization objective function according to the data obtained in steps S1 to S4, and obtain the optimal positioning result based on the factor graph incremental smoothing optimization method.

2. The high-precision positioning method for urban tunnels based on multi-source information fusion according to claim 1 is characterized in that: The specific process of step S2 is: S21. Calculate the normal vector angle between the point in the point cloud and its neighboring points according to the feature point extraction method based on the normal vector angle , obtain the edge feature points and plane feature points of the tunnel environment where the target is located; S22, converting the point cloud into a depth image, and extracting the rod-shaped edge features of the tunnel environment where the target is located by a clustering method; S23. Extract reflectivity greater than the threshold from the point cloud point, , as the reflectivity feature point.

3. The high-precision positioning method for urban tunnels based on multi-source information fusion according to claim 2 is characterized in that: The specific process of step S21 is: S211, according to the feature point extraction method based on the normal vector angle, calculate the normal vector angle between the point in the point cloud and its neighboring points , the normal vector angle The calculation formula is: （1） in, For vector and vector The angle between is the current point scanned by the laser radar, , For LiDAR scans and Points on the same scan line; S212, divide each scanning line scanned by the laser radar into six segments on average, and calculate the normal vector angle of each point in each segment. Sort by size, The two points with the largest angle are taken as edge points, and the remaining The first ten points with the largest angles are used as the extended edge points, and then the edge points and the extended edge points are stored in the variables In the example, the edge feature points of the tunnel environment where the target is located are obtained; Will The four points with the smallest angle are taken as plane points, and the remaining points except edge feature points, plane points, and non-adjacent points and non-filtered points other than edge feature points and plane points are taken as extended plane points, and then the plane points and extended plane points are stored in variables In the above figure, the plane feature points of the tunnel environment where the target is located are obtained.

4. The high-precision positioning method for urban tunnels based on multi-source information fusion according to claim 2 is characterized in that: The specific process of step S22 is: S221, converting the point cloud of the tunnel environment where the target is located acquired by the laser radar into a depth image; S222, dividing the pixels of the depth image into different regions by a clustering method based on depth information; Traverse all the pixels in the depth image from top to bottom and from left to right, and remove the pixels from the bottom of the depth image upwards. The pixels of the row, , is the height of the depth image, and the remaining pixels in the depth image are stored in an open set middle; For the open set Each valid pixel in , traverse the neighboring pixels in the four directions of front, back, left, and right, and compare the valid pixels The distance difference between the neighboring pixels in the four directions of front, back, left, and right is less than the threshold. , The range is 0 to 1 meter, then the current valid pixel point Add to Cluster subsets and the current valid pixel point And the neighboring pixels that meet the threshold condition are from the open set Repeat the above process until the set is opened. If it is empty or there is no neighboring pixel point that meets the above conditions, the final result contains Cluster subsets Collection cluster, Greater than 0 and less than the effective pixel Total number of S223, rod-shaped feature candidate screening; Calculating Collections Each cluster subset in the cluster If the aspect ratio is less than 1, the cluster subset is discarded. ; Calculating Collections Each remaining subset of clusters of The difference, Cluster subset The height of the highest point in Cluster subset The height value of the lowest point in the cluster subset of , For rod-shaped feature candidates to meet the threshold, , then the cluster subset Save as rod feature candidate; S224, rod edge feature extraction; Each cluster subset saved as a rod-shaped feature candidate is then The points in the image are fitted into a circle, and then the center of the fitted circle is used to represent the edge point of the cylindrical object on the current scan line to complete the rod-shaped edge feature extraction of the tunnel environment where the target is located.

5. The high-precision positioning method for urban tunnels based on multi-source information fusion according to claim 1 is characterized in that: The specific process of step S3 is: S31. Definition For the All feature points contained in the frame point cloud include edge feature points, plane feature points, rod-shaped edge features and reflectivity feature points. is the maximum number of feature points that need to be optimized; S32. Definition As a function to measure the positioning performance of the laser radar, the degradation factor is obtained ; （2） in, For the The frame point cloud contains the information matrix of all feature points; S33, Selection The function is used as a measure of scene feature degradation, and we get: （3） in, For After solving the determinant, take the logarithm; S34, calculate the degradation factor every 1s to 2s , set the threshold ,when When , 20% of all feature points are selected as the maximum number of feature points that need to be optimized ,Right now ,otherwise .

6. The high-precision positioning method for urban tunnels based on multi-source information fusion according to claim 1 is characterized in that: The specific process of step S4 is: S41, designing scene evaluation and target optimization objective functions according to the acquired edge feature points, plane feature points, rod-shaped edge features, and reflectivity feature points; definition is the edge feature point, plane feature point, rod edge feature, and reflectivity feature point subset to be optimized. for The matrix formed by the scenario evaluation and target optimization objective function is as follows: （4） in, , is a random feature subset, For A collection of S42. Determine a random feature subset The size is , is the attenuation factor; S43. From the map Search for a random subset of features in For each feature point in , calculate the residual function. If the feature point has been calculated before, skip it; S44, Update , until the iteration condition of the maximum set round is met, the residual function is minimized as the optimization goal, and the features that make the scene evaluation and target optimization objective functions optimal are obtained, so as to achieve the maximum number of feature points that need to be optimized Make optimizations.

7. The high-precision positioning method for urban tunnels based on multi-source information fusion according to claim 1 is characterized in that: The specific process of step S5 is: S51, design positioning optimization objective function; Based on the factor graph incremental smoothing optimization of Beidou satellite navigation system, pseudo-satellite, lidar, and inertial measurement unit fusion, the system is assumed to be The state quantity is: （5） in, is the three-dimensional position vector obtained based on the Beidou satellite navigation system, pseudo-satellite and lidar. is the attitude quaternion vector obtained based on the inertial measurement unit, is the three-dimensional velocity vector obtained based on the inertial measurement unit; S52, based on the factor graph incremental smoothing optimization method, solving to obtain the optimal positioning result; Various residual constraint functions are constructed by marginalizing errors, inertial pre-integration factors, laser line and surface factors, laser reflectivity factors, and pseudo-satellite position factors to form a maximum likelihood estimation function and perform factor graph incremental optimization: （6） in, is the marginalized residual of the Schur complement of the matrix, is the inertia pre-integration residual, It is the line and surface residual of the point cloud obtained by the lidar. is the reflectivity residual of the point cloud obtained by the lidar, is the pseudo-satellite position residual, is the number of inertial measurement unit data, The number of point clouds acquired for the LiDAR, is the number of position data obtained through pseudo-satellites, The system to be solved is at the current time The state quantity of Constructing a point-to-line distance error function : （7） in, For the Frame point cloud extraction edge feature points, For the Frame point cloud extraction Expanded edge feature points, For the Frame point cloud extraction Expanded edge feature points, and Must form a line and and and The sum of the distances is the smallest; Construct the distance function between points and surfaces : （8） in, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, For the Frame point cloud extraction Plane feature points, , and must form a plane and , and and The sum of distances is minimal; （9） By using nonlinear optimization to solve formula (6), the optimal positioning result can be obtained.

Images