PPP / INS / lidar integrated positioning method and system based on graph optimization

Abstract

The present application relates to navigation positioning technical field, especially relates to a kind of based on graph optimization's PPP / INS / laser radar combined positioning method and system, the state of motion carrier is as variable node in laser radar frame time stamp, IMU pre-integration measurement value, laser radar inertial odometry change and precise point positioning are as factor node to construct factor graph to be optimized, and initial pose of motion carrier is obtained using IMU original observation data, IMU odometry and laser radar inertial odometry;Global navigation satellite system GNSS signal quality is judged using specified index, and based on GNSS signal quality, based on laser radar inertial odometry factor or combined precise point positioning and laser radar inertial odometry is selected to factor graph is optimized and is solved, to obtain the pose of motion carrier according to optimization solution result.The present application can realize the accurate, robust estimation of global positioning, and can realize the continuous accurate positioning of each application scenario download body.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of navigation positioning, and particularly relates to a PPP / INS / LiDAR combined positioning method and system based on graph optimization. BACKGROUND

[0002] With the continuous advancement and exploration of mobile robots and unmanned platforms in various application scenarios, the requirements for the positioning capability of mobile carriers are also becoming higher and higher. Precise point positioning (PPP) is an important mode of the global satellite navigation system (GNSS), and can achieve good positioning results in open environments, but the continuity and reliability of positioning in complex urban environments are difficult to guarantee. The inertial navigation system (INS) is an internal sensing navigation system that does not depend on the environment and infrastructure, but the error will continuously accumulate with the integration of the inertial measurement unit (IMU) noise over time. The light detection and ranging (LiDAR) has the advantages of high ranging accuracy, good real-time performance, strong anti-interference ability, etc., and can be used for precise positioning by constructing a high-precision map, and is more reliable and stable for the simultaneous localization and mapping (SLAM) system running for a long time, but still has some problems: distortion of motion state point cloud, degradation of performance in unstructured scenes, weak loop closure detection capability, etc. The SLAM system using a single sensor (such as a camera or a LiDAR) often cannot meet the target requirements, and all the three navigation methods have defects and shortcomings, but can complement each other's advantages.

[0003] Scholars at home and abroad have carried out a series of researches on GNSS / INS / lidar integrated positioning system. For example, Li et al. proposed a laser radar odometry and mapping system based on PPP, used singular value decomposition Jacobian matrix model to derive the positioning covariance of laser radar SLAM, and used laser radar SLAM in a loose combination based on factor graph to reduce the PPP error, but the single-frequency PPP precision is low, which is difficult to meet the demand of high-precision positioning. Sun Zhiyuan et al. used a laser radar odometry based on planar features to fuse with RTK in a loose combination, effectively improved the position accuracy in GNSS shadow environment, and improved the estimation performance of velocity and attitude, but RTK needs a reference station, while PPP only needs a receiver. In the application scenarios of autonomous driving and mobile robot navigation and positioning, the signal quality is poor and the PPP positioning accuracy cannot be met. Therefore, it is urgent to develop a joint positioning scheme to meet the continuous and accurate positioning of moving carriers in complex environments with serious signal interference. SUMMARY

[0004] Therefore, the present application provides a PPP / INS / lidar integrated positioning method and system based on graph optimization, which solves the problems that long-time solution of relative position by existing laser radar SLAM will cause large drift and cannot realize absolute positioning, and GNSS cannot realize continuous and reliable high-precision positioning in complex urban environments.

[0005] According to the design scheme provided by the present application, on the one hand, a PPP / INS / lidar integrated positioning method based on graph optimization is provided, which comprises:

[0006] Obtain sensor data in the integrated positioning sensor system, and pre-process the sensor data, the integrated positioning sensor system is mounted on a moving carrier, and the sensor data includes laser radar point cloud data, IMU raw observation data in an inertial navigation system (INS), and precise point positioning (PPP) positioning data, the PPP positioning data is obtained by solving global navigation satellite system (GNSS) receiver raw satellite data;

[0007] The state of the moving carrier at the laser radar frame timestamp is taken as a variable node, the IMU pre-integral measurement value, the laser radar inertial odometry change and the precise point positioning are taken as factor nodes to construct a factor graph to be optimized, and the initial pose of the moving carrier is obtained by using the IMU raw observation data, the IMU odometry and the laser radar inertial odometry;

[0008] The global navigation satellite system (GNSS) signal quality is determined by using a specified index, if the signal quality is less than a preset threshold, the factor graph is optimized and solved based on the constraints between adjacent variable nodes and based on a laser radar inertial odometry factor, if the signal quality is not less than the preset threshold, the factor graph is jointly optimized and solved based on the constraints between adjacent variable nodes and by using precise point positioning and laser radar inertial odometry, and the motion carrier pose is obtained according to the optimization and solving result.

[0009] As the PPP / INS / laser radar integrated positioning method based on graph optimization of the application, further, the pre-processing of the sensor data includes:

[0010] The relative carrier motion pre-integration measurement value between the two adjacent time stamps is obtained based on the IMU original observation data and by using the IMU pre-integration method, and the pre-integration measurement value includes the motion carrier velocity change measurement value, the motion carrier position change measurement value and the motion carrier rotation change measurement value.

[0011] The multi-frequency predicted pose is obtained by using the relative carrier motion pre-integration measurement value, and the laser radar point cloud data is de-distorted and feature extracted based on the multi-frequency predicted pose.

[0012] As the PPP / INS / laser radar integrated positioning method based on graph optimization of the application, further, the de-distortion and feature extraction of the laser radar point cloud data includes:

[0013] The edge features and plane features of the laser radar point cloud data are extracted by using the curvature of the points in the region, and the edge features and plane features are down-sampled to eliminate the repeated features in the edge features and plane features.

[0014] As the PPP / INS / laser radar integrated positioning method based on graph optimization of the application, further, the laser radar inertial odometry change calculation in the factor node includes:

[0015] The voxel map is obtained according to the edge features and plane features extracted in the current time laser radar scanning;

[0016] The next time laser radar frame is matched to the voxel map by scanning, and the corresponding relationship of the edges and / or planes of adjacent time is obtained in the voxel map;

[0017] The edge feature and edge distance equation and the plane feature and plane distance equation are constructed according to the corresponding relationship of the edges and planes of adjacent time in the voxel map, and the minimum value of the two distances is obtained by solving the equation by using the Gauss-Newton method;

[0018] The relative transformation between the state variables of adjacent time is obtained according to the minimum value, and the relative transformation is taken as the laser radar odometry change quantity connecting the poses of the two state variables.

[0019] As the PPP / INS / lidar integrated positioning method based on graph optimization of the present application, further, the precise point positioning calculation in the factor node also contains:

[0020] The rotation matrix from the world coordinate system to the navigation coordinate system is constructed according to the initial heading angle, and the rotation matrix from the navigation coordinate system to the earth-centered earth-fixed coordinate system is constructed according to the longitude and latitude of the position of the laser radar at the initial time of the moving carrier;

[0021] The two rotation matrices and the anchor point are used to represent the conversion between the global navigation satellite system GNSS receiver position in the earth-centered earth-fixed coordinate system and the world coordinate system; the vector of the global navigation satellite system GNSS receiver position in the carrier coordinate system relative to the IMU measurement center is used to correct the space of the global navigation satellite system GNSS measurement reference and the integrated positioning sensor system reference reference, the GNSS measurement reference is the receiver phase center, and the integrated positioning sensor system reference reference is the IMU measurement center.

[0022] As the PPP / INS / lidar integrated positioning method based on graph optimization of the present application, further, the initial pose of the moving carrier is obtained by using the IMU raw observation data, the IMU odometer and the laser radar inertial odometer, containing:

[0023] The inertial-laser initialization pose is obtained by using the 6-axis IMU combined with the laser radar inertial odometer, and the heading angle is aligned by using the inertial-laser initialization pose.

[0024] The horizontal rotation angle between the precise point positioning PPP and the laser radar inertial odometer is obtained based on the factor graph, the horizontal rotation angle is taken as the initial heading angle, and the initial heading angle initial value is set, and the anchor point and the initial heading angle are jointly initialized by combining the precise point positioning PPP and the laser radar inertial odometer.

[0025] As the PPP / INS / lidar integrated positioning method based on graph optimization of the present application, further, the anchor point and the initial heading angle are jointly initialized by combining the precise point positioning PPP and the laser radar inertial odometer, containing:

[0026] The anchor point initial value is set based on prior information, and in the case where there is no prior information, the origin in the local world system is set as the anchor point initial value, the origin of the local world system is the coordinate of the earth-centered earth-fixed coordinate system of the precise point positioning at the first frame time of the laser radar under the condition that the origins of the world coordinate system and the navigation coordinate system coincide;

[0027] The change threshold is set according to displacement change or angle change, the motion carrier displacement key frame is determined by using the change threshold, the precise point positioning is converted into the north-east sky coordinate system by using the anchor point initial value, the rotation angle between the world coordinate system and the north-east sky coordinate system is obtained through the factor graph, and the residual error of the receiver in the navigation coordinate system and the motion carrier in the local coordinate system is obtained based on the rotation angle.

[0028] In still another aspect, the application further provides a PPP / INS / lidar integrated positioning system based on graph optimization, comprising a data acquisition module, a factor graph construction module and a joint solution module, wherein,

[0029] The data acquisition module is used for acquiring sensor data in the integrated positioning sensor system and pre-processing the sensor data, the integrated positioning sensor system is carried on the motion carrier, and the sensor data comprises lidar point cloud data, IMU raw observation data in the inertial navigation system (INS) and precise point positioning (PPP) positioning data, the precise point positioning (PPP) positioning data is obtained by solving global navigation satellite system (GNSS) receiver raw satellite data;

[0030] The factor graph construction module is used for taking the state of the motion carrier at the lidar frame timestamp as a variable node, taking the IMU pre-integration measurement value, the lidar inertial odometer change and the precise point positioning as a factor node to construct a factor graph to be optimized, and obtaining the initial pose of the motion carrier by using the IMU raw observation data, the IMU odometer and the lidar inertial odometer;

[0031] The joint solution module is used for judging the global navigation satellite system (GNSS) signal quality by using a specified index, if the signal quality is less than a preset threshold, optimizing and solving the factor graph based on the constraint between adjacent variable nodes and based on the lidar inertial odometer factor, if the signal quality is not less than the preset threshold, jointly optimizing and solving the factor graph based on the constraint between adjacent variable nodes and by using the precise point positioning and the lidar inertial odometer, so as to obtain the pose of the motion carrier according to the optimization and solution result.

[0032] The application has the following beneficial effects:

[0033] The application uses multi-frequency PPP position information, performs coarse alignment of a laser radar inertial odometer LIO and precise point positioning PPP in an initialization stage, combines feature-based LIO and PPP positioning results and performs fine optimization on anchor points and initial heading angles through a factor graph, realizes accurate and robust estimation of global positioning based on the factor graph, fully utilizes the complementary characteristics among PPP / INS / laser radar sensors, and realizes continuous and accurate positioning of each application scenario. BRIEF DESCRIPTION OF DRAWINGS

[0034] Figure 1 A flowchart of the PPP / INS / laser radar combined positioning process based on graph optimization in the embodiment is shown.

[0035] Figure 2 A factor graph structure in the embodiment is shown.

[0036] Figure 3 A vehicle trajectory, initial heading angle error and anchor point error in experiment 1 in the embodiment is shown.

[0037] Figure 4 A vehicle trajectory, initial heading angle error and anchor point error in experiment 2 in the embodiment is shown.

[0038] Figure 5 Satellite number and PDOP value in experiment 1 in the embodiment are shown.

[0039] Figure 6 Satellite number and PDOP value in experiment 2 in the embodiment are shown.

[0040] Figure 7 A positioning accuracy comparison of each algorithm in experiment 1 in the embodiment is shown.

[0041] Figure 8 A positioning accuracy comparison of each algorithm in experiment 2 in the embodiment is shown.

[0042] Figure 9 A pose accuracy comparison of two algorithms in experiment 1 in the embodiment is shown.

[0043] Figure 10 A pose accuracy comparison of two algorithms in experiment 2 in the embodiment is shown.

[0044] Figure 11 LIO / PPP incremental and LIOP error curves in experiment 1 in the embodiment are shown.

[0045] Figure 12The LIP / PPP increment versus LIOP error curve is shown in Example 2. DETAILED DESCRIPTION

[0046] In order to make the objects, technical solutions and advantages of the present application clearer and more comprehensible, the present application will be further described in detail below with reference to the drawings and technical solutions.

[0047] Before introducing the scheme of the present application, first, the various coordinate systems used are described as follows:

[0048] The Earth-centered-Earth-fixed frame (e-frame) has its origin at the center of the Earth, and its coordinate axes are fixed to the Earth and rotate with the Earth. The Z-axis is parallel to the mean axis of rotation of the Earth, the X-axis is in the equatorial plane and points to the intersection of the equator and the prime meridian, and the Y-axis is perpendicular to the X-axis and the Z-axis to form a right-handed coordinate system.

[0049] The navigation coordinate system (n-frame) is also called the local horizontal coordinate system. Its origin is the same as that of the body coordinate system, the X-axis points east along the tangent direction of the prime vertical circle of the reference ellipsoid, the Y-axis points north along the tangent direction of the meridian of the current reference ellipsoid, and the Z-axis points upward perpendicular to the reference ellipsoid to form an east-north-up (ENU) coordinate system.

[0050] The body coordinate system (b-frame) has its origin at the measurement center of the inertial device. The X-axis of the body coordinate system points right along the lateral axis of the carrier, the Y-axis points forward, and the Z-axis points upward along the vertical axis of the carrier to form a right-front-up body coordinate system.

[0051] The world coordinate system (W-frame) is one of the most common coordinate systems in SLAM algorithms. It is a man-made reference coordinate system, and its coordinate axes are orthogonal to each other and comply with the right-hand rule. The origin and the direction of the coordinate axes can be arbitrarily specified according to different requirements. In the embodiment of the present application, the direction of the world coordinate axis is front-left-up.

[0052] State vector of a moving carrier For

[0053]

[0054] wherein R, p and v are respectively the attitude, position and velocity vectors of the carrier, and b represents the IMU bias vector.

[0055] Since the state estimation of a moving carrier such as a robot is usually a maximum a posteriori distribution problem and a Gaussian measurement error is usually assumed, the state estimation can be modeled by a factor graph and the conditional probability of each factor in the graph is assumed to follow a Gaussian distribution, and then the maximum a posteriori problem is equivalent to a nonlinear least squares problem.

[0056]

[0057] where z represents a set of independent sensors, {r p , H p represents the prior information of the system state, r(·) represents the residual function of each measurement, and ‖·‖ p is a Mahalanobis norm.

[0058] Navigation services and high-precision positioning play an important role in emerging fields such as autonomous driving and mobile robots. The performance of GNSS precise point positioning is severely affected by signal interference, and in complex environments, continuous and accurate positioning cannot be achieved. Laser radar / INS can utilize spatial structure information to achieve pose estimation, but cannot solve the problem of cumulative error. Therefore, the embodiment of the application provides a PPP / INS / laser radar combined positioning method based on graph optimization, comprising:

[0059] S101, acquiring sensor data in a combined positioning sensor system, and preprocessing the sensor data, the combined positioning sensor system being mounted on a moving carrier, the sensor data comprising: laser radar point cloud data, IMU raw observation data in an inertial navigation system (INS), and precise point positioning (PPP) positioning data, the precise point positioning (PPP) positioning data being obtained by solving global navigation satellite system (GNSS) receiver raw satellite data;

[0060] S102, taking the state of the moving carrier at the laser radar frame timestamp as a variable node, taking the IMU pre-integration measurement value, the laser radar inertial odometry change, and the precise point positioning as factor nodes to construct a factor graph to be optimized, and using the IMU raw observation data, the IMU odometry, and the laser radar inertial odometry to obtain an initial pose of the moving carrier;

[0061] S103, judging the global navigation satellite system (GNSS) signal quality by using a specified index, if the signal quality is less than a preset threshold, optimizing and solving the factor graph based on the constraints between adjacent variable nodes and based on the laser radar inertial odometry factor, if the signal quality is not less than the preset threshold, jointly optimizing and solving the factor graph based on the constraints between adjacent variable nodes and using the precise point positioning and the laser radar inertial odometry, and obtaining the pose of the moving carrier according to the optimization and solving result.

[0062] Using multi-frequency PPP position information, a laser radar inertial odometer LIO is coarsely aligned with precise point positioning PPP in an initialization stage, feature-based LIO and PPP positioning results are combined and anchor points and initial heading angles are finely optimized through a factor graph to realize PPP / INS / laser radar combined positioning.

[0063] As shown in a process of implementing the PPP / INS / laser radar combined algorithm, Figure 1 it can be divided into three parts of data preprocessing, initialization and factor graph optimization. In the data preprocessing stage, laser radar, IMU original observation and PPP positioning information are input into the system, high-frequency predicted poses are obtained by using IMU pre-integration to assist laser radar in point cloud distortion removal and feature extraction.

[0064] In the initialization stage, first, inertial INS / laser radar initialization is performed, initial positions are obtained by using IMU observation, IMU odometer and laser radar odometer, in the embodiment, a 6-axis IMU can be used, initial attitudes cannot be obtained by a magnetometer, it can be considered that the world system Z-axis is coincided with the direction of the sky of the navigation coordinate system, to solve this problem, in the embodiment, a horizontal rotation angle between PPP and LIO is calculated through a constructed factor graph to obtain an initial value of the heading angle. Finally, since there is an error between anchor points and true values, after the initial value of the heading angle is obtained, PPP and LIO are jointly initialized for anchor points and initial heading angles to realize fine optimization of the initial heading angle.

[0065] After the initialization stage ends, a PPP degeneration condition is checked to ensure system robustness. When PPP is available, LIO and PPP jointly estimate poses; when PPP is unavailable, the system is degraded to LIO, LIO can maintain high accuracy for a short time, continuously output positioning results, and improve system robustness and stability.

[0066] In the factor graph-based PPP / INS / laser radar combined positioning algorithm, as shown in Figure 2 a factor graph is constructed through six different factors and variable nodes. Among them, the variable node represents the state of the carrier at the laser radar frame timestamp, and the factor node represents the corresponding constraint relationship. When a new position node is inserted, an incremental smoothing and mapping (iSAM2) based on a Bayesian tree is used for optimization.

[0067] The measured values of IMU angular velocity and acceleration can be defined as follows:

[0068]

[0069] Among them, and are the original measured values of the IMU in the b system at time t, affected by the slowly changing zero bias b tand white noise n t impact. is the rotation matrix from the W frame to the b frame, and g is the constant gravity vector in the W frame. The IMU measurements can be used to infer the motion of the carrier. The velocity, position, and rotation of the robot at time t+Δt can be calculated as:

[0070]

[0071] in Assume that the angular velocity and acceleration of b remain constant during the above integration process. Then use the IMU pre-integration method to obtain the relative carrier motion between two adjacent timestamps. Between time i and j, the pre-integrated measurement value Δv ij , Δp ij , and ΔR ij It can be calculated by the following method:

[0072]

[0073] For LiDAR observations, the LiDAR data is first processed. When LiDAR data is received, feature extraction is performed first. Edge features and plane features are extracted by calculating the curvature of points within the local area. Points with larger curvature values ​​are classified as edge features, while points with smaller curvature values ​​are classified as plane features. The curvature c can be defined as follows:

[0074]

[0075] Where i is a point on scan line k, is the set of points around point i, j is One point in Represents the coordinates of point i in the lidar system.

[0076] The edge and plane features extracted from the lidar scan at time i are expressed as and First, and Downsampling is performed to eliminate duplicate features. Secondly, a feature-based matching method is used to scan and match the lidar frame at time i+1 to the voxel map, and the corresponding relationship between edges or planes is found in the corresponding voxel map.

[0077] The distance between an edge feature and its corresponding edge can be calculated using the following formula:

[0078]

[0079] The distance between a planar feature and its corresponding plane can be calculated using the following formula:

[0080]

[0081] For edge features and are points that form the corresponding edge line in the voxel map. For planar features constitute the corresponding plane in the voxel map. Then the Gauss-Newton method is used to solve the optimal transformation, and the minimum value is obtained:

[0082]

[0083] Finally, the state node x i and the relative transformation ΔT i+1 between x i,i+1 , that is, the laser radar odometry factor connecting the two poses:

[0084]

[0085] PPP factors will be added on the basis of tight integration of laser radar / INS, and PPP / INS / laser radar integrated positioning will be realized. The position in the ECEF system can be converted to the ENU system through the anchor point, and the ENU system can be converted to the local world system through the initial heading angle, so as to realize the conversion from the ECEF system to the W system.

[0086]

[0087] wherein, is the position of the GNSS receiver in the ECEF system, is the position of the GNSS receiver in the W system, anchor is the anchor point, is the rotation matrix from the n system to the ECEF system, is the rotation matrix from the W system to the n system, and are as follows,

[0088]

[0089] In the formula, is the horizontal angle between the W system and the n system, that is, the initial heading angle, λ and η represent the longitude and latitude of the system position at the initial time of the laser radar respectively.

[0090] Since the measurement reference of GNSS is the phase center of the receiver, and the reference reference in the embodiment of the application is the IMU measurement center, the two are not spatially synchronized, and the vector (also known as the lever arm) of the GNSS center relative to the IMU center needs to be corrected:

[0091] ​

[0092] State estimation is nonlinear, so its effectiveness depends heavily on the initial values. Initialization provides an estimate of the initial state. During system operation, complex urban environments may lead to poor or even no GNSS signal quality. The system addresses GNSS degradation.

[0093] First, in the absence of other prior information, the anchor point is set to the origin of the local world system, that is, the world system coincides with the origin of the navigation coordinate system, and the coordinates of PPP in the ECEF system at the moment of the first frame of the lidar are set as the initial value of the anchor point. Secondly, in order to ensure successful and rapid initialization and avoid initialization in a static state, during the initialization stage, the key frame is determined by setting a threshold so that the carrier is displaced. In this embodiment, the threshold can be set to a displacement change of 0.05m or an angle change of 0.2rad. The initial value of the anchor point is used to transfer the PPP position to the corresponding ENU system, and the rotation angle between the world system and the ENU system is calculated through the factor graph. The residual r can be constructed as follows:

[0094]

[0095] in, is the coordinate of the receiver in the n system, is the coordinate of the carrier in the local world system.

[0096] This unifies the ENU system with the local world system, completes initialization, and allows the initial heading angle and anchor point to be continuously optimized in subsequent optimizations.

[0097] In open areas, the signal quality of GNSS and the solution accuracy of PPP are the best, but in complex urban environments, GNSS signals are blocked, resulting in poor GNSS signal quality, signal loss, reduced PPP accuracy or even inability to solve. In this embodiment, three indicators can be used to judge the quality of GNSS signals, namely the number of satellites to be solved, the position geometric precision factor (Position Dilution of Precision, PDOP) and the variance. According to empirical values, PPPs with PDOP values ​​greater than 3.2, fewer than 9 satellites and variances greater than 0.25 are eliminated to avoid affecting system accuracy. In the case of poor GNSS signal quality, the system degenerates to LIO for posture solution.

[0098] Furthermore, based on the above method, an embodiment of the present invention also provides a PPP / INS / lidar combined positioning system based on graph optimization, comprising: a data acquisition module, a factor graph construction module and a joint solution module, wherein:

[0099] The data acquisition module is configured to acquire sensor data in a combined positioning sensor system and pre-process the sensor data, the combined positioning sensor system is mounted on a moving carrier, and the sensor data includes laser radar point cloud data, IMU raw observation data in an inertial navigation system (INS), and precise point positioning (PPP) positioning data, the PPP positioning data is obtained by solving global navigation satellite system (GNSS) receiver raw satellite data;

[0100] The factor graph construction module is configured to construct a to-be-optimized factor graph by taking a state of the moving carrier at a laser radar frame timestamp as a variable node, taking IMU pre-integration measurement values, laser radar inertial odometry changes, and precise point positioning as factor nodes, and obtaining an initial pose of the moving carrier by using the IMU raw observation data, the IMU odometry, and the laser radar inertial odometry.

[0101] The joint solving module is configured to judge GNSS signal quality by using a specified index, if the signal quality is less than a preset threshold, to optimize and solve the factor graph based on constraints between adjacent variable nodes and based on laser radar inertial odometry factors, if the signal quality is not less than the preset threshold, to jointly optimize and solve the factor graph based on constraints between adjacent variable nodes and by using precise point positioning and laser radar inertial odometry, and to obtain a pose of the moving carrier according to an optimization and solving result.

[0102] To verify the effectiveness of the scheme, the following experimental data are further explained:

[0103] The vehicle-mounted experiment was carried out in Zhengzhou, Henan Province on February 18, 2023 and November 08, 2023 by a multi-sensor integration platform. The experimental data acquisition platform is mounted with tactical-grade inertial navigation, multi-mode and multi-frequency GNSS, laser radar and other sensors. In terms of spatial alignment, the experimental platform adopts high-strength aluminum alloy material, which is precisely machined by a high-precision numerical control machine to millimeter level. The IMU and the laser radar are fixedly connected, and there is a coordinate conversion. However, this coordinate conversion is only used when the laser radar point cloud is deformed. In terms of time alignment, a high-precision time synchronization board card containing a high-precision FPGA system and a precise crystal oscillator is used to realize millimeter-level spatial synchronization and microsecond-level time synchronization between sensors. However, since the sampling frequency of the GNSS receiver is different from that of other sensors, a linear interpolation method is used to ensure the unity of time during solving. The main sensor parameters of the vehicle-mounted data acquisition platform are shown in Table 1, and the reference trajectory is the centimeter-level GNSS / IMU loose combination positioning result provided by the commercial software IE.

[0104] Table 1 Equipment model and information

[0105]

[0106] Experiment 1 is an open environment experiment, with a duration of 900s and a total length of about 5853m; Experiment 2 is a complex urban environment experiment, with a duration of 1140s and a total length of about 6010m. Figure 3 , 4 are the actual scenes of Experiment 1 and 2 respectively, and the color in the figure represents the number of satellites, and the darker the color, the more the number of satellites. As shown in Figure 3 , there are sparse trees on both sides of the road in Experiment 1, and the overall environment is relatively open, lacking rich features, which is not conducive to the pose calculation of LIO, but is conducive to the reception of GNSS signals, and can obtain higher precision PPP calculation values. As shown in Figure 4 , there are trees and buildings on both sides of the road in Experiment 2, and some roads are relatively narrow, which is a common scene in complex urban environments. In complex urban environments, road width, tree height, and building height have different effects on the number of satellites and satellite signal quality, which pose challenges to PPP, and even GNSS signals are interrupted on some road segments, and PPP cannot be calculated, but at the same time, complex urban environments also have rich feature information, which is conducive to feature matching of laser radar odometry.

[0107] The initial heading angle calculation error changes in Experiment 1 and Experiment 2 are shown in Figure 3 (b) and Figure 4 (b), respectively. The error fluctuation caused by short initialization at the beginning immediately converges, and fluctuates around 1.1° and 0.5°, respectively. The main reason for the error is that the coordinates in the world system cannot be fused with PPP during the initialization stage, resulting in inaccurate coordinates and causing errors in the calculation of the initial value of the initial heading angle, which affects subsequent calculations. The anchor point optimization error changes in Experiment 1 and Experiment 2 are shown in Figure 3 (c) and Figure 4 (c), respectively. Since the initial value of the anchor point is obtained from PPP, there is a certain error, and by optimizing the anchor point, the anchor point error is reduced and quickly converges, but due to insufficient constraints on the anchor point, the improvement of the anchor point optimization is limited.

[0108] The changes in the number of GNSS calculation satellites and the corresponding position dilution of precision in Experiment 1 and Experiment 2 are shown in Figure 5 and 6As shown in the figure, in Experiment 1, satellite signals were unobstructed, significantly improving GNSS signal quality and the number of satellites compared to complex urban environments. However, the open environment posed challenges for LIO resolution. In Experiment 2, however, satellite signals were significantly obstructed in the shadowed area by densely populated buildings and trees, resulting in more severe obstruction. The total obstruction lasted approximately 120 seconds, potentially causing satellite lock loss, resulting in discontinuous observations and impacting PPP settlement results. Statistics show that in Experiment 1, the maximum number of resolved satellites was 13, with an average of 11.7 and a PDOP range of 0.9 to 1.5. In Experiment 2, the maximum number of resolved satellites was 12, with an average of 9.1. Epochs with fewer than 6 satellites accounted for 10.3%, with PDOP values ​​ranging from 1.06 to 9.7. In Experiment 2, 46.0% of epochs had RMSE greater than 1 meter in 3D. Under conditions of satellite signal obstruction, the number of satellites was significantly reduced. This reduction in satellite count and signal quality can severely impair navigation accuracy, posing challenges to continuous, high-precision, and reliable positioning.

[0109] To verify the effectiveness of the algorithm, the PPP / INS / lidar combination algorithm was used to perform offline data analysis, and the solution results were compared with PPP and LIO. The analysis was mainly divided into three aspects: first, analysis of position, second, analysis of posture, and third, analysis of position increment.

[0110] In terms of position estimation, the results of Experiment 1 are as follows Figure 7 As shown in Table 2, in open environments, LIOP's accuracy in the E, N, and U directions is close to that of PPP, reaching the same order of magnitude. The 3D RMSEs of PPP and LIOP are 0.17 and 0.21 m, respectively, both at the decimeter level. However, LIOP's accuracy is slightly lower than that of PPP, and the LIOP error curve fluctuates around the PPP error curve. This can be attributed to two factors: first, the use of linear interpolation for time alignment leads to inaccurate interpolation results at high and variable speeds, which can easily lead to errors. Second, the data collection process in Experiment 1 required multiple turns, and in open environments, the quality of the lidar point cloud degraded, resulting in fewer feature points. This impacted the positioning results when the pose varied significantly. Experiment 1 shows that in open environments, the combined results can achieve decimeter-level accuracy, close to that of PPP, and achieve robust and stable positioning.

[0111] Table 2 Accuracy RMSE / m of three algorithms in Experiment 1

[0112]

[0113] The results of Experiment 2 are as follows Figure 8As shown in Table 3, in the first half of the experiment, GNSS quality was relatively good and had a rich set of features. During this period, LIOP's positioning accuracy was superior to PPP's. However, in occluded environments, as indicated by the shaded area, PPP experienced large errors or even failed to resolve in complex urban environments, preventing high-precision continuous positioning. Combining PPP, INS, and LiDAR effectively improved positioning continuity, maintaining meter-level positioning accuracy even in complex urban environments. After reintegrating PPP, the system converged quickly, providing relatively stable positioning. In Experiment 1, LIOP achieved RMSE improvements of 22.7%, 45.5%, and 39.4% compared to PPP in the E, N, and U directions, respectively. However, due to the limited 30° field of view of the LiDAR, the lack of excitation in the gravity direction, and the insensitivity of GNSS in the U direction, both PPP and the combined results still had significant lags in U accuracy compared to the horizontal direction.

[0114] The system jitters at the red-marked location. This is because PPP doesn't meet the selection criteria, forcing the system to downgrade to LIO. LIO gradually diverges during operation. However, overall, LIOP outperforms PPP in positioning. Experiment 2 shows that the combined system can achieve relatively high-precision, continuous, robust positioning in complex urban environments.

[0115] Table 3 Accuracy RMSE / m of three algorithms in Experiment 2

[0116]

[0117] In terms of pose estimation, the pose accuracy of Experiment 1 is as follows: Figure 9 As shown in Table 4, LIO is in a divergent state. Each turn affects the posture, and the error accumulates over time. LIOP, on the other hand, integrates PPP and, by constraining the posture, suppresses the divergence of LIO and reduces the impact of the weak observability of yaw. Compared with LIO, LIOP improves the RMSE in roll, pitch, and yaw by 95.4%, 87.6%, and 99.7%, respectively.

[0118] Table 4. Attitude accuracy RMSE / ° of the two algorithms in Experiment 1

[0119]

[0120] Experiment 2 Posture accuracy Figure 10and Table 5, in the first half of the experiment, the attitude error of LIOP is around 0°, by increasing the constraints, the fusion result is more stable than the divergence state of LIO, but in the shadow part, that is, the complex urban environment, due to the poor GNSS signal quality, the system is degraded to LIO, which leads to divergence in the roll and pitch directions, and quickly converges to around 0° after re-fusing PPP; while in the yaw direction, due to the system always building the corresponding constraints through the factor graph, there is no large fluctuation, and the error is always around 1°, the error source is the error in the initial heading angle estimation, compared with LIO, the RMSE of LIOP in roll, pitch and yaw directions is increased by 55.0%, 54.8% and 96.0% respectively, which proves that LIOP can maintain good accuracy in attitude in complex urban environment.

[0121] Table 5 Experiment 2 Attitude accuracy of two algorithms RMSE / °

[0122]

[0123] In addition to analyzing the position and attitude, the displacement increment is analyzed, by Figure 11 It can be seen that after fusion, the LIO increment obtained is consistent with the trend of the PPP increment on the curve, floating up and down around the curve, indicating that the accuracy of LIO in a short time is similar to that of PPP, and it can be used for short-time positioning in complex urban environment; compared with the error curve of LIOP, it can be seen that the larger the increment, the larger the error of LIOP, which can be considered as the influence of the vehicle moving at high speed on LIO solution and PPP interpolation, causing the error to increase.

[0124] As can be seen from (b) (c) in Figure 12 , in the relatively open environment in Experiment 2, part of the LIO and PPP increment will have a large error, but LIOP eliminates this part of the error through factor graph optimization, without causing too much impact; in the complex urban environment (600-850s), the PPP increment is large and shows a divergent state, which does not conform to the actual situation of the carrier motion, combined with Figure 6 and Figure 8 , it can be seen that the number of satellites for GNSS solution is small, the PDOP value increases, the PPP error increases, and even cannot be solved, at this time the system is degraded to LIO and shows a divergent state, the LIO increment appears fluctuation, and the system error increases. After re-fusing PPP, the system converges and tends to be stable, verifying the stability of the system.

[0125] Through the above experimental data, it is shown that the scheme unifies the coordinate system by calculating the initial heading angle, realizes the fusion of LIO and PPP through the construction of a factor graph, and realizes the complementary advantages between sensors through the construction of a combined system. The combined algorithm can effectively reduce the drift of the laser radar SLAM. In an open environment, the combined algorithm can obtain a stable decimeter-level positioning result. In a complex urban environment, continuous and reliable positioning can be realized, and a meter-level positioning accuracy can be achieved. The accuracy can reach that of the advanced GNSS / INS / laser radar combined algorithm, and has good application prospects.

[0126] Unless specifically stated otherwise, the relative steps, numerical expressions, and numerical values of the components and steps set forth in these embodiments do not limit the scope of the present application.

[0127] The various embodiments in the specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other. For the system disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method part.

[0128] The units and method steps of each example described in combination with the embodiments disclosed herein can be realized in electronic hardware, computer software or a combination of both. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example are generally described in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation does not exceed the scope of the present application.

[0129] Those skilled in the art can understand that all or part of the steps in the above method can be instructed by a program to complete the relevant hardware, and the program can be stored in a computer readable storage medium, such as a read-only memory, a magnetic disk or an optical disk. Alternatively, all or part of the steps of the above embodiments can also be implemented using one or more integrated circuits, and accordingly, each module / unit in the above embodiments can be implemented in the form of hardware or in the form of a software function module. The present application is not limited to any specific form of combination of hardware and software.

[0130] Finally, it should be noted that the above-described embodiments are merely specific embodiments of the present application, which are used to illustrate the technical solutions of the present application, but not to limit the present application, and the protection scope of the present application is not limited thereto. Although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still make modifications or easily think of changes to the technical solutions recorded in the foregoing embodiments, or make equivalent replacements to some technical features therein, within the technical range disclosed by the present application, and these modifications, changes or replacements do not make the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and all should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A PPP / INS / LiDAR combined positioning method based on graph optimization, characterized in that: Include: Acquire and pre-process the sensor data from the combined positioning sensor system, which is carried on a moving carrier. The sensor data includes: lidar point cloud data, raw observation data from the inertial navigation system (INS) and precise point positioning (PPP) data. The precise point positioning (PPP) data is obtained by solving raw satellite data from a global navigation satellite system (GNSS) receiver. The state of the motion carrier at the lidar frame timestamp is used as the variable node, and the IMU pre-integrated measurement value, lidar inertial odometer change and precise single point positioning are used as factor nodes to construct a factor graph. The initial position of the motion carrier is obtained using the IMU original observation data, IMU odometer and lidar inertial odometer. The GNSS signal quality of the global navigation satellite system is judged using specified indicators. If the signal quality is less than the preset threshold, the factor graph is optimized based on the constraints between adjacent variable nodes and the lidar inertial odometry factor. If the signal quality is not less than the preset threshold, the factor graph is jointly optimized based on the constraints between adjacent variable nodes and the use of precise single point positioning and lidar inertial odometry to obtain the position and posture of the moving carrier according to the optimization solution results.

2. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 1 is characterized in that: Preprocess the sensor data, including: Based on the IMU original observation data and using the IMU pre-integration method, the relative carrier motion pre-integration measurement value between two adjacent timestamps is obtained, and the pre-integration measurement value includes the motion carrier velocity change measurement value, the motion carrier position change measurement value and the motion carrier rotation change measurement value; The multi-frequency predicted pose is obtained by using the pre-integrated measurement value of the relative carrier motion, and the multi-frequency predicted pose is used to assist the dedistortion and feature extraction of the lidar point cloud data.

3. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 2 is characterized in that: Dedistortion and feature extraction of LiDAR point cloud data, including: The edge features and plane features of the lidar point cloud data are extracted using the curvature of the points in the area, and the edge features and plane features are downsampled to eliminate duplicate features in the edge features and plane features.

4. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 3 is characterized in that: The calculation of the LiDAR inertial odometry change in the factor node includes: Obtain a voxel map based on the edge features and plane features extracted from the current LiDAR scan; Scan and match the next moment's LiDAR frame to the voxel map, and obtain the corresponding relationship between edges and / or planes at adjacent moments in the voxel map; Based on the correspondence between edges and planes at adjacent moments in the voxel map, the edge feature-edge distance equation and the plane feature-plane distance equation are constructed, and the Gauss-Newton method is used to solve the equations to obtain the minimum value of the two distances. The relative transformation between the state variables at adjacent moments is obtained based on the minimum value, and the relative transformation is used as the change in the lidar odometer connecting the two state variable poses.

5. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 1 is characterized in that: The precise point positioning calculation in the factor node also includes: Construct the rotation matrix from the world coordinate system to the navigation coordinate system based on the initial heading angle, and construct the rotation matrix from the navigation coordinate system to the Earth-centered Earth-fixed coordinate system based on the longitude and latitude of the motion carrier position at the initial moment of the lidar; Two rotation matrices and anchor points are used to represent the transformation of the GNSS receiver position between the Earth-centered Earth-fixed coordinate system and the world coordinate system. The vector of the GNSS receiver position in the carrier coordinate system relative to the IMU measurement center is used to spatially correct both the GNSS measurement reference and the combined positioning sensor system reference. The GNSS measurement reference is the receiver phase center, and the combined positioning sensor system reference is the IMU measurement center.

6. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 1 is characterized in that: The initial position of the motion carrier is obtained using the IMU raw observation data, IMU odometer and lidar inertial odometer, including: Use a 6-axis IMU in combination with a lidar inertial odometer to obtain the inertial-radar initialization pose, and use the inertial-radar initialization pose to align the heading angle; The horizontal rotation angle between precise point positioning (PPP) and lidar inertial odometry is obtained based on the factor graph. The horizontal rotation angle is used as the initial heading angle and the initial value of the initial heading angle is set. The anchor point and the initial heading angle are jointly initialized by precise point positioning (PPP) and lidar inertial odometry.

7. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 6 is characterized in that: Combined precise point positioning (PPP) and lidar inertial odometry to initialize the anchor point and initial heading angle, including: The initial value of the anchor point is set based on the prior information. In the absence of prior information, the origin of the local world system is set as the initial value of the anchor point. The origin of the local world system is the coordinate of the precise single point positioning in the Earth-centered Earth-fixed coordinate system at the time of the first frame of the lidar when the origin of the world coordinate system and the navigation coordinate system coincide; A change threshold is set according to the displacement change or angle change, and the displacement key frame of the motion carrier is determined using the change threshold. The precise single-point positioning is converted to the northeast celestial coordinate system using the initial value of the anchor point. The rotation angle between the world coordinate system and the northeast celestial coordinate system is obtained through the factor graph, and the residual of the coordinates of the receiver in the navigation coordinate system and the coordinates of the motion carrier in the local coordinate system is obtained based on the rotation angle.

8. A PPP / INS / LiDAR combined positioning system based on graph optimization, characterized in that: It includes: data acquisition module, factor graph construction module and joint solution module, among which, A data acquisition module is used to acquire and pre-process the data of each sensor in the combined positioning sensor system, which is carried on a moving carrier. The sensor data includes: lidar point cloud data, IMU raw observation data in the inertial navigation system (INS), and precise point positioning (PPP) positioning data. The precise point positioning (PPP) positioning data is obtained by solving the raw satellite data of the global navigation satellite system (GNSS) receiver. The factor graph construction module is used to construct the factor graph to be optimized by taking the state of the motion carrier at the lidar frame timestamp as the variable node, and taking the IMU pre-integrated measurement value, the lidar inertial odometry change and the precise single point positioning as the factor nodes. The initial position and posture of the motion carrier are obtained by using the IMU original observation data, IMU odometry and lidar inertial odometry. The joint solution module is used to judge the GNSS signal quality of the global navigation satellite system using specified indicators. If the signal quality is less than a preset threshold, the factor graph is optimized and solved based on the constraints between adjacent variable nodes and the lidar inertial odometry factor. If the signal quality is not less than the preset threshold, the factor graph is jointly optimized and solved based on the constraints between adjacent variable nodes and using precise single point positioning and lidar inertial odometry to obtain the position and posture of the moving carrier based on the optimization solution results.

9. An electronic device, characterized in that: include: at least one processor, and a memory coupled to the at least one processor; The memory stores a computer program, and the computer program can be executed by the at least one processor to implement the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed, the method according to any one of claims 1 to 7 can be implemented.

Images