A Precise Point Positioning and LiDAR Combined Navigation and Positioning Method

Abstract

This invention belongs to the field of multi-source fusion navigation, specifically designing a combined navigation method of precise single-point positioning and lidar, further enhancing the stability and reliability of navigation and positioning. The method utilizes the high-precision positioning results of lidar to construct a cycle slip detection model, assisting in correcting cycle slips caused by precise single-point positioning observations after satellite signal interruption, thus achieving rapid convergence and improving the reliability of positioning in autonomous driving systems. Addressing the problem of map overlap and positioning drift caused by lidar tracking failures in similar scenarios such as tunnels and long corridors, the method employs topological road network information to establish backend constraints and a relocation method based on point cloud SC description to achieve map repair and positioning correction functions, respectively. For the error accumulation phenomenon of lidar SLAM in large outdoor scenes, the method uses PPP calculation results to provide an initial position for lidar scanning and matching to correct accumulated errors.

Description

Technical Field

[0001] This invention belongs to the field of multi-source fusion navigation, and specifically designs a combined navigation method of precise single-point positioning and lidar to further enhance the stability and reliability of navigation and positioning. Background Technology

[0002] Perception and localization are crucial prerequisites for autonomous driving systems, determining the quality of the system. Their functionality can be achieved through lidar and satellite navigation.

[0003] Precise Point Positioning (PPP) is an enhancement of point positioning technology. It uses precise orbit and clock bias files broadcast by IGS instead of the original broadcast ephemeris, thus achieving centimeter-level positioning accuracy. Compared to differential positioning, PPP does not require a reference station and has advantages such as lower cost and wider applicability. However, PPP has a slow convergence speed; when satellite signals are interrupted, it often takes 15–30 minutes to converge to dynamic dm-level positioning accuracy, significantly compromising real-time positioning performance.

[0004] LiDAR possesses extremely high perception capabilities. The Simultaneous Localization and Mapping (SLAM) problem based on LiDAR can be described as a situation where a user starts moving from an unknown location in an unknown environment, performs self-localization based on sensor information and a map during movement, and simultaneously constructs an incremental map based on this self-localization to achieve autonomous positioning and navigation. However, this process is highly dependent on environmental features and is prone to positioning and tracking failures in scenarios with indistinct environmental characteristics, such as tunnels, underground passages, and long indoor corridors. Summary of the Invention

[0005] In view of this, the present invention provides a precise single-point positioning and lidar combined navigation positioning method, namely a PPP / laser SLAM combined navigation method. This method consists of two parts: lidar navigation method and Beidou satellite navigation method. It adopts a loose combination to achieve fusion, realizes the complementary advantages of the two navigation methods, effectively solves the shortcomings of single navigation, enhances reliability, and expands the application scenarios of positioning.

[0006] The technical solution of this invention is:

[0007] A precise single-point positioning and lidar combined navigation and positioning method, the method includes: a first case, positioning in positioning scenarios where satellite signals are interrupted, such as tunnels and indoor corridors, and achieving rapid convergence of precise single-point positioning after satellite signals are restored; a second case, positioning in positioning scenarios where satellite signals are interrupted and environmental features are similar, such as tunnels and indoor corridors; and a third case, positioning in a large outdoor positioning scenario.

[0008] In the first scenario: a rapid convergence method for precise single-point positioning after satellite signal recovery in positioning scenarios such as tunnels and indoor corridors where satellite signals are interrupted, including the following steps:

[0009] Step 1: Environmental perception and positioning are achieved using lidar. After satellite signal recovery, the geometric distance between the receiver and the satellite is calculated using the lidar positioning results. This eliminates the range observations in carrier-to-ionospheric and wide-lane combinations, and constructs the observation equations.

[0010]

[0011]

[0012] In the formula, L IF-SLAM Represents the deionization observations after eliminating geometric distances; L WL-SLAM For the wide-lane combination observation after eliminating geometric distance; dt r Represents receiver clock bias and satellite clock bias; T is the tropospheric delay along the propagation path; I WL λ represents the ionospheric delay of the wide-lane combination; IF Indicates the deionization combination wavelength; λ WL Indicates the combined wavelength of the wide-lane network; N IF Indicates the deionization combined ambiguity; N WL Indicates the ambiguity of the wide lane combination; For observation noise. ε SLAM The error in estimating geometric distance for lidar; c is the speed of light;

[0013] Step two involves performing interepochal single-difference on the observation equations constructed in step one to obtain the cycle slip combined detection model. This model includes an ionosphere-depleted combined detection model and a wide-lane combined cycle slip detection model. The ionosphere-depleted combined model is expressed as follows:

[0014]

[0015] The wide-lane combined cycle slip detection model is represented as follows:

[0016]

[0017] In the formula, δ represents the single difference between epochs; δL IF-SLAM This represents the observed value of the lidar-assisted ionospheric desiccation combined cycle slip detection model; δN IF To eliminate ionospheric cycle slip; Indicates the noise of the single-difference deionization combined observation between epochs; δε SLAM The estimation error of the laser radar station distance after single-difference estimation; δL WL-SLAM For lidar-assisted carrier phase WL combination cycle slip detection; λ WL The wavelength for wide-lane combined observation; δN WL For wide-lane combined cycle jumps; c is the speed of light;

[0018] Step 3: Combine the de-ionization combined detection model and the wide-lane combined cycle slip detection model obtained in Step 2 to calculate cycle slips at different carrier frequencies, as shown in the following formula:

[0019]

[0020]

[0021] In the formula, f1 and f2 are carrier frequencies; λ1 and λ2 are carrier wavelengths; δN1 and δN2 are cycle slips of the two frequencies, respectively.

[0022] Step four: Use the cycle slips on different carrier frequencies calculated in step three for ambiguity repair. The successfully repaired ambiguity is used as a constraint term for the positioning information calculation to achieve rapid PPP convergence. This enables rapid convergence of positioning in scenarios where satellite signals are interrupted, such as tunnels and indoor corridors, as well as precise single-point positioning after satellite signals are restored.

[0023] In the second scenario, where satellite signals are interrupted in locations such as tunnels and indoor corridors, and the environmental characteristics are similar, the positioning method includes the following steps:

[0024] Step 1: Construct a topology map using the construction drawings of the positioning scene. Save important feature points in the environment, such as intersections, corners, and T-junctions, into the nodes of the topology map, and save their connection relationships into the edges of the topology map.

[0025] Step 2: Construct a global map using LiDAR. During the map construction process, the 3D point cloud data acquired by LiDAR is projected onto a 2D plane, and the centerline of the road is extracted using the watershed algorithm in image processing.

[0026] Step 3: By searching for the number of intersections between a circular curve with a specified radius centered on the LiDAR and the road centerline extracted in Step 2, and by determining the curvature of the road centerline within the circular curve, the current road features are identified. Constraints are then established between the poses of the current feature points obtained from the topology map in Step 1 and the poses inferred from the LiDAR, as shown in the following formula:

[0027]

[0028] E 2 (ξ i ,ξ j ;Σ ij ,ξ ij )=e(ξ i ,ξ j ξ ij ) T Σ ij -1 e(ξ i ,ξ j ξ ij )

[0029]

[0030] In the formula, i represents a road network node, j represents the current scan, θ represents the attitude angle, and ξ represents the attitude angle. i ξ represents the pose of a road network node relative to the world coordinate system; j Indicates the pose of the current scan relative to the world coordinate system; ξ ij This indicates the relative pose between the road network node and the current scan. ξ represents the relative position of the current node to other nodes in the road network in the world coordinate system; i；θ and ξ j；θ These represent the attitude angles of the current scan and the road network nodes, respectively. and This describes the coordinates of the scan and the network node in the world coordinate system when the scan is at the previous network node.

[0031] Step 4: Use the Levenberg-Marquardt algorithm to perform nonlinear least squares solution on the constraints established in Step 3, and update the solution results to the global map constructed by the lidar in Step 2 to obtain a high-precision global map.

[0032] Step 5: Using the Scan Context method, the laser point cloud is encoded into a matrix. After the global map constructed by the LiDAR in Step 4 is updated, each frame of laser point cloud used in the updated global map is encoded into a corresponding set of SC descriptors. And record the pose set of each frame of the laser point cloud. Assume the SC matching symbol corresponding to the currently acquired laser point cloud data is I. cBy using SC matching, the historical point cloud frame with the smallest cosine distance to the current laser point cloud is found in P and numbered k. If the cosine distance between the current point cloud and the historical point cloud is less than a threshold, the match is considered successful, meaning that the two point clouds are considered very similar. Their positions are roughly the same, and their attitudes differ only in the heading angle. The pose expression of the current laser point cloud can be written as:

[0033]

[0034] Where R is the rotation matrix and t is the translation matrix, their expressions are as follows:

[0035]

[0036] t =

[000] T

[0037] Where θ is the heading angle offset obtained in SC matching.

[0038] Step six: Use the pose of the current frame laser point cloud obtained in step five as the initial pose for the LiDAR front-end scanning and matching to achieve repositioning after LiDAR positioning drift, and complete positioning in scenarios where satellite signals are interrupted and environmental features are similar, such as tunnels and indoor corridors.

[0039] In the third scenario, in a large outdoor positioning environment, the positioning method is to use the precise single-point positioning solution as the initial value for LiDAR scanning and matching to correct the error accumulation phenomenon of the LiDAR.

[0040] Beneficial effects

[0041] (1) The method of the present invention utilizes the high-precision positioning results of lidar to construct a cycle detection model, which helps to correct the cycle slip generated by the precise single-point positioning observation after the satellite signal is interrupted, so as to achieve its rapid convergence and improve the reliability of positioning in the autonomous driving system.

[0042] (2) The method of the present invention addresses the problem of map overlap and positioning drift caused by the failure of lidar tracking in similar scenarios such as tunnels and long corridors. It achieves map repair and positioning correction functions by using topological road network information to establish back-end constraints and a relocation method based on point cloud SC description.

[0043] (3) The method of the present invention corrects the cumulative error by using the PPP solution results to provide the initial position for laser radar scanning and matching in large outdoor scenes to address the error accumulation phenomenon of laser SLAM. Attached Figure Description

[0044] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0045] Figure 2 This is a block diagram of the PPP performance improvement system in this invention;

[0046] Figure 3 This is a schematic diagram of the SLAM positioning correction method in this invention;

[0047] Figure 4 Here is a flowchart of the topology information matching process for the three types of data.

[0048] Figure 5 This is a schematic diagram of the SC matching process in step 3. Detailed Implementation

[0049] The invention will now be further described with reference to the accompanying drawings.

[0050] Reference Figure 1 The PPP / LiDAR SLAM integrated navigation system consists of two parts: a LiDAR navigation system and a BeiDou satellite receiver, fused using a loosely coupled approach. Precise point positioning (PPS) provides all-weather absolute positioning information and can be used as a "landmark" to correct LiDAR error accumulation in environments without closed loops. However, it cannot be applied in environments with interrupted satellite signals, such as tunnels. In these situations, the high-frequency, high-precision relative positioning information calculated by LiDAR is reliable. Furthermore, LiDAR's error accumulation is less significant compared to inertial navigation, allowing it to operate independently even under prolonged satellite signal interruptions. Therefore, it can be used to assist PPP in rapid convergence during satellite signal recovery. Simultaneously, to meet real-time positioning requirements, PPP calculations employ a time-delay-free precision correction product, correcting satellite ephemeris and clock biases in real time, eliminating the most significant error source in observations. To address the mapping overlap and positioning drift issues caused by LiDAR tracking loss in feature-scarce environments, topological road network information is used as feature enhancement to assist in incremental map construction. Furthermore, Scan Context encoding and matching of the point cloud provide a good initial pose for scan-match-based positioning calculations.

[0051] Reference Figure 2 The PPP / laser SLAM integrated navigation system targeted in this embodiment of the invention includes a satellite receiver, network data stream, laser SLAM, and PPP solution module. This method uses satellite receiver data and network satellite precision data to calculate the satellite position, calculates the geometric distance between the receiver and the satellite through the laser radar positioning results, eliminates the distance observation in the carrier deionization combination and wide lane combination as shown in Equation (1), obtains the cycle slip combination detection model as shown in Equation (2) by performing epoch-to-epoch single difference on Equation (1), obtains the cycle slip value as shown in Equation (3) by solving Equation (2), and then performs ambiguity repair to achieve fast PPP convergence.

[0052] Reference Figure 3The PPP / laser SLAM integrated navigation system targeted in this embodiment of the invention consists of modules such as external enhancement information and PPP positioning calculation. The external enhancement information mainly solves the problems of overlapping LiDAR mapping and positioning drift in scenarios where satellite signals are interrupted. The PPP positioning calculation results are used to assist in correcting the error accumulation phenomenon of LiDAR in large outdoor positioning scenarios.

[0053] Reference Figure 4 In the PPP / laser SLAM integrated navigation system targeted by this invention, the mapping overlap phenomenon in scenarios with similar environmental features such as tunnels and indoor corridors is corrected by topological road network information. The centerline of the road is extracted using the watershed algorithm based on the distribution characteristics of the laser point cloud. The current road features are determined by searching the number of intersections between the circular curve formed by the laser radar with a specified radius and the road centerline, as well as the curvature of the road centerline within the circular curve. If the number of intersections is greater than 2, it is judged as an intersection; if it is less than 2, it is judged by the curvature as a corridor or corner. The current road features are compared with the topological map to obtain the pose of the current feature points, and then the constraint equation shown in equation (4) is established. The incremental map is optimized by nonlinear least squares solution.

[0054] refer to Figure 5 In the PPP / laser SLAM integrated navigation system addressed in this embodiment of the invention, positioning drift in scenarios with similar environmental features such as tunnels and indoor corridors is corrected using point cloud Scan Context description. First, the current frame's point cloud is SC encoded, and the frame with the smallest distance function is found in the historical frame SC descriptions. If the distance function is less than a threshold, a successful match is determined, and the laser radar's position information is obtained. Second, the laser radar's attitude is corrected using the heading angle offset obtained during the matching process, acquiring complete pose information. This complete pose information is then used as the initial pose for scanning and matching to achieve laser radar relocalization.

[0055] The PPP / laser SLAM integrated navigation system targeted in this embodiment of the invention provides an initial pose for the front-end scanning and matching by using PPP settlement results under high-quality observation conditions in open outdoor positioning environments, thereby suppressing the error accumulation phenomenon caused by the lack of loop closure detection of the laser radar.

[0056] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A precise single-point positioning and lidar combined navigation and positioning method, characterized in that... The method includes: The first scenario involves positioning in a tunnel or indoor corridor where satellite signals are interrupted, and achieving rapid convergence of precise single-point positioning after satellite signals are restored. The second scenario involves positioning in a tunnel or indoor corridor where satellite signals are interrupted but environmental features are similar. The third scenario involves positioning in a larger outdoor positioning scenario. The third scenario, in a large outdoor positioning environment, involves using the precise single-point positioning solution as the initial value for lidar scanning and matching to correct for lidar error accumulation. The first scenario, in a positioning environment where satellite signals are interrupted in tunnels or indoor corridors, involves a rapid convergence method for precise single-point positioning after satellite signal recovery. The steps include: Step 1: Use lidar to achieve environmental perception and positioning functions. After the satellite signal is recovered, use the lidar positioning results to calculate the geometric distance between the receiver and the satellite, eliminate the distance observation in the carrier deionization combination and wide-lane combination, and construct the observation equation. Step 2: Perform interepochal single difference on the observation equations constructed in Step 1 to obtain the cycle slip combined detection model. The cycle slip combined detection model includes the ionosphere de-ionization combined detection model and the wide-lane combined cycle slip detection model. Step 3: Combine the deionization combined detection model and the wide-lane combined cycle slip detection model obtained in Step 2 to solve the cycle slip at different carrier frequencies; Step four: Use the cycle slips on different carrier frequencies calculated in step three for ambiguity repair. The successfully repaired ambiguity is used as a constraint term for the positioning information calculation to achieve rapid PPP convergence. This enables rapid convergence of positioning in scenarios where satellite signals are interrupted in tunnels or indoor corridors, as well as precise single-point positioning after satellite signals are restored.

2. The precise single-point positioning and lidar combined navigation and positioning method according to claim 1, characterized in that: In step one, the constructed observation equation is: In the formula, This represents the deionized observations after eliminating geometric distances; For wide-lane combination observations after eliminating geometric distances; Indicates receiver clock bias and satellite clock bias; For tropospheric delay along the propagation path; Indicates the ionospheric delay of the wide-lane combination; Indicates the combined wavelength for deionization; Indicates the combined wavelength of the wide-lane design; Indicates the deionization combined ambiguity; Indicates the ambiguity of the wide lane combination; , To observe the noise, Errors in estimating geometric distance for lidar; It is the speed of light.

3. The precise single-point positioning and lidar combined navigation and positioning method according to claim 2, characterized in that: In step two, the ionosphere depletion combined detection model is represented as follows: The wide-lane combined cycle slip detection model is represented as follows: In the formula, Indicates a single difference between epochs; This represents the observation values ​​of the lidar-assisted ionospheric desiccation combined cycle slip detection model; To eliminate ionospheric cycle slip; This indicates the noise from the single-difference ionospheric combination observations between epochs; This represents the estimation error of the laser radar station's star distance after single-difference estimation; For lidar-assisted carrier phase WL combination cycle slip detection measurement; For wide-lane combined observation wavelengths; For wide-lane combination cycle jumps; It is the speed of light.

4. The precise single-point positioning and lidar combined navigation and positioning method according to claim 2, characterized in that: In step three, the formula for calculating cycle slips at different carrier frequencies is as follows: In the formula, For carrier frequency; The carrier wavelength; These are cycle slips at two different frequencies.

5. The precise single-point positioning and lidar combined navigation and positioning method according to claim 1, characterized in that: The second scenario, a positioning method for situations where satellite signals are interrupted in tunnels or indoor corridors and environmental features are similar, includes the following steps: Step 1: Construct a topology map using the construction drawings of the positioning scene, save the feature points and information in the environment to the nodes of the topology map, and save their connection relationships to the edges of the topology map. Step 2: Construct a global map using LiDAR. During the map construction process, the 3D point cloud data acquired by LiDAR is projected onto a 2D plane, and the centerline of the road is extracted using the watershed algorithm in image processing. Step 3: By searching for the number of intersections between the circular curve formed by the LiDAR with a specified radius and the road centerline extracted in Step 2, as well as the curvature of the road centerline within the circular curve, the current road features are determined. Constraints are then established between the pose of the current feature points obtained from the topology map in Step 1 and the pose inferred by the LiDAR. Step 4: Use the Levenberg-Marquardt algorithm to perform nonlinear least squares solution on the constraints established in Step 3, and update the solution results on the global map constructed by the lidar in Step 2 to obtain a high-precision global map. Step 5: Encode the laser point cloud into a matrix. After the global map constructed by the LiDAR in Step 4 is updated, encode each frame of laser point cloud used in the updated global map into a corresponding set of SC descriptors. And record the pose set of each frame of the laser point cloud. Assuming the SC matcher corresponding to the currently acquired laser point cloud data is By using SC matching, the historical point cloud frame with the smallest cosine distance to the current laser point cloud is found in P, and it is numbered k. If the cosine distance between the current point cloud and the historical point cloud is less than a threshold, the match is considered successful, meaning that the two point clouds are considered very similar, with roughly the same position and only differing in attitude in the heading angle. The pose expression of the laser point cloud in the current frame is written as: Where R is the rotation matrix and t is the translation matrix, their expressions are as follows: in This refers to the heading angle offset obtained during SC matching; Step six: Use the pose of the current frame laser point cloud obtained in step five as the initial pose for the LiDAR front-end scanning and matching to achieve repositioning after LiDAR positioning drift, and complete positioning in scenarios where satellite signals are interrupted in tunnels or indoor corridors and environmental features are similar.

6. The precise single-point positioning and lidar combined navigation and positioning method according to claim 5, characterized in that: In step one, the feature points in the environment are intersections, corners, and T-junctions.

7. A precise single-point positioning and lidar combined navigation and positioning method according to claim 5 or 6, characterized in that: In step three, the constraints for establishing the pose of the current feature points and the pose inferred by the lidar are as follows: In the formula, Represents road network nodes. Indicates the current scan. Indicates attitude angle, This represents the pose of a road network node relative to the world coordinate system. Indicates the pose of the current scan relative to the world coordinate system; This indicates the relative pose between the road network node and the current scan. This represents the relative position of the current node to other nodes in the road network in the world coordinate system. and These represent the attitude angles of the current scan and the road network nodes, respectively. and This describes the coordinates of the scan and the network node in the world coordinate system when the scan is at the previous network node.

8. The precise single-point positioning and lidar combined navigation and positioning method according to claim 7, characterized in that: Step 5: Use the Scan Context method to encode the laser point cloud into a matrix.

Images