PPP / INS / laser radar combined positioning method and system based on graph optimization

Abstract

The invention relates to the technical field of navigation and positioning, in particular to a PPP / INS / laser radar combined positioning method and system based on graph optimization. Constructing a to-be-optimized factor graph by taking the IMU pre-integral measurement value, the variation of the laser radar inertial odometer and the precise single-point positioning as factor nodes, and acquiring the initial pose of the motion carrier by using the IMU original observation data, the IMU odometer and the laser radar inertial odometer; and judging the signal quality of a global navigation satellite system (GNSS) by using a specified index, selecting a laser radar inertial odometer-based factor or combining precise point positioning and a laser radar inertial odometer according to the signal quality of the GNSS to carry out optimization solution on the factor graph, so as to obtain the pose of the motion carrier according to an optimization solution result. According to the method, accurate and robust estimation of global positioning can be realized, and continuous and accurate positioning of a carrier in each application scene can be realized.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and in particular to a PPP / INS / laser radar combined positioning method and system based on graph optimization. Background Art

[0002] With the continuous advancement and exploration of mobile robots and unmanned platforms in various application scenarios, the requirements for the positioning capabilities of mobile carriers are becoming higher and higher. Precision point positioning (PPP) is an important mode of the Global Navigation Satellite System (GNSS). It 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 rely on the environment and infrastructure. However, as time goes by, it is affected by the noise of the Inertial Measurement Unit (IMU), and the error will continue to accumulate with integration. Light Detection and Ranging (LiDAR) has the advantages of high ranging accuracy, good real-time performance, and strong anti-interference ability. It can accurately locate by building high-precision maps, and it is more reliable and stable for long-term simultaneous positioning and mapping (SLAM) systems. However, there are still some problems: distortion of motion state point clouds, performance degradation in unstructured scenes, and weak closed-loop detection capabilities. SLAM systems that use a single sensor (such as cameras, lidar) often cannot adapt to target requirements. The three navigation methods all have defects and shortcomings, but they can complement each other's strengths.

[0003] Domestic and foreign scholars have conducted a series of studies on the GNSS / INS / LiDAR combined positioning system. For example, Li et al. proposed a LiDAR odometer and mapping system based on PPP, used the singular value decomposition Jacobian matrix model to derive the positioning covariance of LiDAR SLAM, and used LiDAR SLAM to reduce the PPP error by loose combination based on factor graph, but the single-frequency PPP accuracy is low and it is difficult to meet the high-precision positioning requirements. Sun Zhiyuan et al. used a LiDAR odometer based on planar features to integrate with RTK through loose combination, effectively improving the position accuracy in GNSS occluded environments and improving the estimation performance of speed and attitude, but RTK requires a base station, while PPP only requires one receiver. The navigation and positioning application scenarios of autonomous driving and mobile robots may have poor signal quality and cannot meet the PPP positioning accuracy requirements. For this reason, a joint positioning solution is urgently needed to meet the continuous and precise positioning application of moving carriers in complex environments with severe signal interference. Summary of the invention

[0004] To this end, the present invention provides a PPP / INS / lidar combined positioning method and system based on graph optimization, which solves the problems that the long-term relative position calculation of the existing lidar SLAM will cause large drift and cannot achieve absolute positioning, and the GNSS cannot achieve continuous and reliable high-precision positioning in complex urban environments.

[0005] According to the design scheme provided by the present invention, on the one hand, a PPP / INS / lidar combined positioning method based on graph optimization is provided, comprising:

[0006] Acquire the data of each sensor in the combined positioning sensor system and pre-process the data of each sensor. The combined positioning sensor system is carried on a moving carrier. The data of each sensor includes: laser radar 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;

[0007] The state of the motion carrier at the timestamp of the lidar frame is used as the variable node, and the IMU pre-integrated measurement value, the change of the lidar inertial odometer and the precise single-point positioning are used as factor nodes to construct the factor graph to be optimized, and the initial position and posture of the motion carrier are obtained using the IMU original observation data, IMU odometer and lidar inertial odometer.

[0008] The specified indicators are used to judge the signal quality of the global navigation satellite system GNSS. If the signal quality is less than the preset threshold, the factor graph is optimized based on the constraints between adjacent variable nodes and based on the lidar inertial odometer 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 using precise single point positioning and lidar inertial odometer to obtain the position and posture of the moving carrier according to the optimization solution results.

[0009] As the PPP / INS / LiDAR combined positioning method based on graph optimization of the present invention, further, the data of each sensor is preprocessed, including:

[0010] Based on the IMU original observation data and using the IMU pre-integration method, a relative carrier motion pre-integration measurement value between two adjacent timestamps is obtained, wherein the pre-integration measurement value includes a motion carrier velocity change measurement value, a motion carrier position change measurement value, and a motion carrier rotation change measurement value;

[0011] The multi-frequency predicted pose is obtained by using the pre-integrated measurement value of the relative carrier motion, and the dedistortion and feature extraction of the lidar point cloud data are assisted based on the multi-frequency predicted pose.

[0012] As the PPP / INS / LiDAR combined positioning method based on graph optimization of the present invention, further, dedistortion and feature extraction of LiDAR point cloud data include:

[0013] 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.

[0014] As the PPP / INS / lidar combined positioning method based on graph optimization of the present invention, further, the calculation of the change amount of the lidar inertial odometer in the factor node includes:

[0015] Obtain a voxel map based on edge features and plane features extracted from the laser radar scan at the current moment;

[0016] 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;

[0017] According to the corresponding relationship between the edge and the plane at adjacent moments in the voxel map, the edge feature and edge distance equation and the plane feature and plane distance equation are constructed, and the Gauss-Newton method is used to solve the equation to obtain the minimum value of the two distances;

[0018] 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 laser radar odometer change connecting the two state variable postures.

[0019] As the PPP / INS / lidar combined positioning method based on graph optimization of the present invention, further, the precise single point positioning calculation in the factor node also includes:

[0020] The rotation matrix from the world coordinate system to the navigation coordinate system is constructed based on the initial heading angle, and the rotation matrix from the navigation coordinate system to the Earth-centered Earth-fixed coordinate system is constructed based on the longitude and latitude of the motion carrier position at the initial moment of the laser radar;

[0021] 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 GNSS measurement reference and the combined positioning sensor system reference are spatially corrected using the vector of the GNSS receiver position in the carrier coordinate system relative to the IMU measurement center. The GNSS measurement reference is the receiver phase center, and the combined positioning sensor system reference is the IMU measurement center.

[0022] As the PPP / INS / LiDAR combined positioning method based on graph optimization of the present invention, further, the initial position and posture of the moving carrier are obtained by using the IMU original observation data, the IMU odometer and the LiDAR inertial odometer, including:

[0023] Use the 6-axis IMU in combination with the lidar inertial odometer to obtain the inertial-radar initialization pose, and use the inertial-radar initialization pose to align the heading angle;

[0024] The horizontal rotation angle between precise point positioning PPP and lidar inertial odometer is obtained based on the factor graph, and 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 odometer.

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

[0026] The initial value of the anchor point is set based on the prior information. In the absence of prior information, the origin in 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 first frame time of the laser radar when the origin of the world coordinate system and the navigation coordinate system coincide.

[0027] A change threshold is set according to the displacement change or angle change, and the displacement key frame of the motion carrier is determined by 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.

[0028] On the other hand, 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:

[0029] A data acquisition module is used to acquire data from each sensor in a combined positioning sensor system and pre-process the data from each sensor. The combined positioning sensor system is carried on a moving carrier. The data from each sensor include: 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 is obtained by solving the raw satellite data of a global navigation satellite system GNSS receiver.

[0030] 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 timestamp of the lidar frame as the variable node, the IMU pre-integrated measurement value, the change of the lidar inertial odometer and the precise single-point positioning as the factor node, and using the IMU original observation data, IMU odometer and lidar inertial odometer to obtain the initial position and posture of the motion carrier;

[0031] 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 based on the lidar inertial odometer 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 odometer to obtain the position and posture of the motion carrier based on the optimization solution results.

[0032] Beneficial effects of the present invention:

[0033] The present invention uses multi-frequency PPP position information, roughly aligns the laser radar inertial odometer LIO with the precise single-point positioning PPP in the initialization stage, combines the feature-based LIO and PPP positioning results through factor graphs, and finely optimizes the anchor point and initial heading angle, and realizes accurate and robust estimation of global positioning based on factor graphs, making full use of the complementary characteristics between PPP / INS / lidar sensors to achieve continuous and accurate positioning of the carrier in various application scenarios. Further vehicle-mounted experiments were carried out in open and complex urban environments. The experimental results show that the present solution can provide high-precision, continuous and reliable positioning services in different environments, and has good application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a schematic diagram of the PPP / INS / LiDAR combined positioning process based on graph optimization in the embodiment;

[0035] Figure 2 It is a schematic diagram of the factor graph structure in the embodiment;

[0036] Figure 3 The vehicle trajectory, initial heading angle error and anchor point error in Experiment 1 in the embodiment are shown;

[0037] Figure 4 The vehicle trajectory, initial heading angle error and anchor point error in Experiment 2 of the embodiment are shown;

[0038] Figure 5 This is a schematic diagram of calculating the number of satellites and PDOP values ​​in Experiment 1 in the embodiment;

[0039] Figure 6 This is a schematic diagram of calculating the number of satellites and PDOP values ​​in Experiment 2 in the embodiment;

[0040] Figure 7 This is a comparison of the positioning accuracy of each algorithm in Experiment 1 in the embodiment;

[0041] Figure 8 This is a comparison of the positioning accuracy of each algorithm in Experiment 2 in the embodiment;

[0042] Fig. 9 This is a comparison diagram of the posture accuracy of the two algorithms in Experiment 1 of the embodiment;

[0043] Fig.10 This is a comparison diagram of the posture accuracy of the two algorithms in Experiment 2 of the embodiment;

[0044] Fig.11 The LIO / PPP increment and LIOP error curve in Experiment 1 in the embodiment are shown;

[0045] Fig.12Schematic diagram of the LIP / PPP increment and LIOP error curve in Experiment 2 in the embodiment. DETAILED DESCRIPTION

[0046] In order to make the purpose, technical solutions and advantages of the present invention clearer and more understandable, the present invention is further described in detail below in conjunction with the accompanying drawings and technical solutions.

[0047] Before introducing the solution of this case, the coordinate systems used are explained as follows:

[0048] The origin of the Earth-centered-Earth-fixed frame (E-frame) is at the center of the Earth. The coordinate axes are fixed to the Earth and rotate with it. The Z-axis is parallel to the Earth's mean rotation axis. The X-axis is located in the equatorial plane and points to the intersection of the equator and the mean Greenwich meridian. 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 system) is also called the local horizontal coordinate system. Its origin is the same as the carrier coordinate system. The X axis is eastward along the tangent direction of the reference ellipsoid meridian, the Y axis is northward along the tangent direction of the current reference ellipsoid meridian, and the Z axis is perpendicular to the reference ellipsoid and upward, forming an east-north-up (ENU) coordinate system.

[0050] The origin of the body frame (B system) is selected at the measurement center of the inertial device. The X-axis of the body frame is to the right along the horizontal axis of the body, the Y-axis is along the forward direction of the body, and the Z-axis is upward along the vertical axis of the body, forming a right-front-upper body coordinate system.

[0051] The world coordinate system (W system) is one of the most common coordinate systems in SLAM algorithms. It is a manually specified reference coordinate system whose coordinate axes are orthogonal to each other and conform to the right-hand rule. The origin and the direction of the coordinate axis can be arbitrarily specified according to different requirements. In this embodiment, the direction of the world coordinate axis is the upper left front.

[0052] The state vector of the moving carrier for

[0053]

[0054] Among them, R, p, and v are the attitude, position, and velocity vectors of the carrier respectively; b represents the zero bias vector of the inertial measurement unit IMU.

[0055] Since the state estimation of moving carriers such as robots is usually a maximum a posteriori probability distribution problem and Gaussian measurement errors are often assumed, it can be modeled through a factor graph and the conditional probability of each factor in the graph is assumed to obey a Gaussian distribution. The maximum a posteriori problem is then equivalently converted into a nonlinear least squares problem.

[0056]

[0057] Where z represents the set of independent sensors, {r p , H p} represents the prior information of the system state, r(·) represents the residual function of each measurement, ‖·‖ p is the Markov 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 precision single-point positioning is seriously affected by signal interference, and continuous and accurate positioning cannot be achieved in complex environments. LiDAR / INS can use spatial structure information to achieve pose estimation, but it cannot solve the problem of cumulative error. To this end, an embodiment of the present invention provides a PPP / INS / LiDAR combined positioning method based on graph optimization, comprising:

[0059] S101, acquiring data of each sensor in a combined positioning sensor system and preprocessing the data of each sensor, wherein the combined positioning sensor system is carried on a moving carrier, and the data of each sensor includes: laser radar point cloud data, IMU raw observation data in an inertial navigation system INS, and precise point positioning PPP positioning data, wherein the precise point positioning PPP positioning data is obtained by solving raw satellite data of a global navigation satellite system GNSS receiver;

[0060] S102, taking the state of the motion carrier at the laser radar frame timestamp as a variable node, taking the IMU pre-integrated measurement value, the laser radar inertial odometer change and the precise single point positioning as factor nodes to construct a factor graph to be optimized, and using the IMU original observation data, the IMU odometer and the laser radar inertial odometer to obtain the initial position and posture of the motion carrier;

[0061] S103. Use specified indicators to determine the signal quality of the global navigation satellite system GNSS. If the signal quality is less than a preset threshold, optimize and solve the factor graph based on the constraints between adjacent variable nodes and based on the lidar inertial odometer factor. If the signal quality is not less than the preset threshold, jointly optimize and solve the factor graph based on the constraints between adjacent variable nodes and using precise single-point positioning and lidar inertial odometer to obtain the position and posture of the motion carrier based on the optimization solution result.

[0062] Using multi-frequency PPP position information, the lidar inertial odometer LIO and the precise point positioning PPP are coarsely aligned in the initialization stage. The feature-based LIO and PPP positioning results are combined through the factor graph, and the anchor point and initial heading angle are precisely optimized to achieve PPP / INS / lidar combined positioning.

[0063] In the implementation process of PPP / INS / LiDAR combination algorithm, Figure 1 As shown in Figure 1, it can be divided into three parts: data preprocessing, initialization, and factor graph optimization. In the data preprocessing stage, the system inputs the original observations of LiDAR, IMU, and PPP positioning information, and uses the IMU pre-integration to obtain high-frequency predicted pose to assist LiDAR in point cloud dedistortion and feature extraction.

[0064] In the initialization stage, the inertial INS / lidar is initialized first, and the initial position is obtained by using IMU observation, IMU odometer and lidar odometer. In this embodiment, a 6-axis IMU can be used. The initial attitude cannot be obtained by the magnetometer. It can be considered that the Z axis of the world system coincides with the celestial direction of the navigation coordinate system. To solve this problem, in this embodiment, the horizontal rotation angle between PPP and LIO is calculated by constructing a factor graph to obtain the initial value of the initial heading angle. Finally, due to the error between the anchor point and the true value, after obtaining the initial value of the heading angle, PPP and LIO are jointly initialized for the anchor point and the initial heading angle to achieve precise optimization of the initial heading angle.

[0065] After the initialization phase, the degradation of PPP is checked to ensure the robustness of the system. When PPP is available, LIO and PPP perform joint pose estimation; when PPP is unavailable, the system is downgraded to LIO, which can maintain high accuracy in a short period of time and continuously output positioning results, improving the robustness and stability of the system.

[0066] In the PPP / INS / LiDAR combined positioning algorithm based on factor graph, such as Figure 2 As shown in the figure, the factor graph is constructed by 6 different factors and variable nodes. Among them, the variable node represents the state of the carrier at the timestamp of the lidar frame, and the factor node represents the corresponding constraint relationship. When inserting a new position node, the incremental smoothing and mapping based on the Bayesian tree (iSAM2) is used for optimization.

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

[0068]

[0069] in and is the original measurement value of the IMU in the b system at time t, subject to the slowly changing zero bias b tand white noise n t impact. is the rotation matrix from W to b, and g is the constant gravity vector in W. The motion of the carrier can be inferred using the IMU measurements. 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 first performed to extract edge features and plane features by calculating the curvature of points in the local area. Points with larger curvature values ​​are classified as edge features, and 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 laser radar system.

[0076] The edge and plane features extracted from the LiDAR scan at the i-th moment 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 of the edge or plane 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 the points that form the corresponding edge line in the voxel map. Construct the corresponding plane in the voxel map. Then use the Gauss-Newton method to solve the optimal transformation and find the minimum value:

[0082]

[0083] Finally, we get the state node x i With x i+1 The relative transformation ΔT i,i+1 , which is the lidar odometry factor connecting these two poses:

[0084]

[0085] On the basis of the tight combination of LiDAR / INS, the PPP factor will be added to achieve PPP / INS / LiDAR combined positioning. 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, thus realizing the conversion from the ECEF system to the W system.

[0086]

[0087] in, 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 W system to n system, and They 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 λ and η represent the longitude and latitude of the system position at the initial moment of the lidar, respectively.

[0090] Since the measurement reference of GNSS is the phase center of the receiver, and the reference reference in this embodiment is the IMU measurement center, the two are not synchronized in space, so it is necessary to use the GNSS center in the b system. Relative to the IMU center The vector (also called the lever arm) Make corrections:

[0091]

[0092] State estimation is a nonlinear estimation, so its effect depends largely on the initial value. Through initialization, the estimated value of the initial state can be obtained. During the operation of the system, complex urban environments may be encountered, resulting in poor GNSS signal quality or even no signal. The system has made corresponding treatments for 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 of the present case, the threshold can be set to a displacement change of 0.05m or an angle change of 0.2rad. Using the initial value of the anchor point, the PPP position is transferred 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] In this way, the ENU system and the local world system can be unified, the initialization is completed, and the initial heading angle and anchor point can 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 solved satellites, position geometric precision factor (Position Dilution of Precision, PDOP) and variance. According to empirical values, PPPs with PDOP values ​​greater than 3.2, less 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] A data acquisition module is used to acquire data from each sensor in a combined positioning sensor system and pre-process the data from each sensor. The combined positioning sensor system is carried on a moving carrier. The data from each sensor include: 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 is obtained by solving the raw satellite data of a global navigation satellite system GNSS receiver.

[0100] 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 timestamp of the lidar frame as the variable node, the IMU pre-integrated measurement value, the change of the lidar inertial odometer and the precise single-point positioning as the factor node, and using the IMU original observation data, IMU odometer and lidar inertial odometer to obtain the initial position and posture of the motion carrier;

[0101] 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 based on the lidar inertial odometer 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 odometer to obtain the position and posture of the motion carrier based on the optimization solution results.

[0102] In order to verify the effectiveness of this solution, the following is a further explanation based on experimental data:

[0103] On-board experiments were conducted in Zhengzhou, Henan Province on February 18, 2023 and November 8, 2023 through a multi-sensor integration platform. The experimental data acquisition platform is equipped with sensors such as tactical-level inertial navigation, multi-mode multi-frequency GNSS and lidar. In terms of spatial alignment, the experimental platform is made of high-strength aluminum alloy material, and is precision-processed at the millimeter level by high-precision CNC machine tools. The IMU and the lidar are fixedly connected, and there is coordinate conversion. However, this coordinate conversion is only used when the lidar point cloud is dedistorted. In terms of time alignment, a high-precision time synchronization board containing a high-precision FPGA system and a precision crystal oscillator is used to achieve 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 during the solution to ensure time uniformity. The main sensor parameters of the on-board 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, lasting 900 seconds and with a total length of about 5853 meters; Experiment 2 is an urban complex environment experiment, lasting 1140 seconds and with a total length of about 6010 meters. Figure 3 , 4 These are the actual scenes of Experiments 1 and 2. The colors in the figure represent the number of satellites being solved. The darker the color, the more satellites there are. Figure 3 As shown in Figure 1, there are sparse trees on both sides of the road in Experiment 1. It can be clearly seen that the overall environment is relatively open and lacks rich features, which is not conducive to the pose solution of LIO, but is conducive to the reception of GNSS signals, and a higher-precision PPP solution value can be obtained. Figure 4 As shown in the figure, in Experiment 2, there are trees and buildings on both sides of the road, 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 will have different effects on the number of satellites and satellite signal quality, posing challenges to PPP. Even some sections of the road are interrupted by GNSS signals, and PPP cannot be solved. However, at the same time, complex urban environments also have relatively rich feature information, which is conducive to the feature matching of lidar odometers.

[0107] The error changes of the initial heading angle solution in Experiment 1 and Experiment 2 are as follows: Figure 3 (b) Figure 4 As shown in (b) in the figure, the initialization at the beginning will cause the error to fluctuate, but it will converge immediately and fluctuate around 1.1° and 0.5°. The main reason for the error is that the coordinates in the world system cannot be integrated with PPP during the initialization phase, and the coordinates are not accurate enough, resulting in errors in the initial value calculation of the initial heading angle, which affects subsequent calculations. The changes in the anchor point optimization errors in Experiment 1 and Experiment 2 are shown in Figure 1. Figure 3 (c) Figure 4 As shown in (c), since the initial value of the anchor point is obtained by PPP, there is a certain error. By optimizing the anchor point, the anchor point error is reduced and converges quickly. However, due to insufficient constraints on the anchor point, the improvement of anchor point optimization is relatively limited.

[0108] The changes in the number of GNSS satellites and the corresponding position precision factor in Experiment 1 and Experiment 2 are shown in the figure below. Figure 5 and 6As shown. It can be seen that in Experiment 1, the satellite signal is not blocked, and the GNSS signal quality and the number of satellites are greatly improved compared with the complex urban environment, but the open environment poses a challenge to the LIO solution. In Experiment 2, there are dense high-rise buildings and trees in the shadow area, and the satellite signal is severely blocked. The total blocking time is about 120s, which may cause the satellite to lose lock, resulting in discontinuous observation and affecting the settlement result of PPP. According to statistics, in Experiment 1, the maximum number of solved satellites is 13, the average is 11.7, and the PDOP value ranges from 0.9 to 1.5; in Experiment 2, the maximum number of solved satellites is 12, the average is 9.1, and the epochs with less than 6 satellites account for 10.3%, and the PDOP value ranges from 1.06 to 9.7. In Experiment 2, in the 3D direction, the RMSE is greater than 1m, accounting for 46.0%. Under the condition of satellite signal blocking, the number of satellites is greatly reduced. The reduction in the number of satellites and signal quality will seriously lead to poor navigation accuracy, which is challenging for continuous high-precision and reliable positioning.

[0109] To verify the effectiveness of the algorithm, the PPP / INS / LiDAR combined algorithm is used to solve the data offline, and the solution results are compared with PPP and LIO. The main aspects are: 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 an open environment, the accuracy of LIOP in the E, N, and U directions is close to that of PPP, and is at the same order of magnitude. The 3D RMSE of PPP and LIOP are 0.17 and 0.21 m, respectively, both at the decimeter level. However, the accuracy of LIOP is slightly lower than that of PPP, and the LIOP error curve fluctuates around the PPP error curve. This can be attributed to two reasons. On the one hand, the linear interpolation method is used in time alignment. When the speed is large and the speed is variable, the interpolation result is inaccurate, which is very easy to cause errors. On the other hand, multiple turns are required during the data collection process of Experiment 1, and in an open environment, the quality of the LiDAR point cloud decreases, and there are fewer feature points. When the posture changes greatly, it has a certain impact on the positioning result. It can be concluded from Experiment 1 that in an open environment, the combined result can achieve decimeter-level accuracy, which is close to PPP, and can achieve robust and stable positioning.

[0111] Table 2 Accuracy RMSE / m of the 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, the quality of GNSS is relatively good and has richer features. At this stage, the positioning accuracy of LIOP is better than that of PPP. In the case of occlusion, as shown in the shaded part, PPP will have large errors or even be unable to solve in the complex urban environment, and cannot achieve high-precision continuous positioning; the combination of PPP / INS / LiDAR can effectively improve the continuity of positioning, and can also maintain meter-level positioning accuracy in the complex urban environment. After re-integrating PPP, the system converges quickly and provides relatively stable positioning. In Experiment 1, the RMSE of LIOP in the E, N, and U directions is improved by 22.7%, 45.5%, and 39.4% respectively compared with PPP. However, since the field of view of the LiDAR is only 30°, there is a lack of excitation in the gravity direction, and GNSS is insensitive in the U direction, both PPP and the combination result still have a large gap in accuracy in the U direction and the horizontal direction.

[0114] The system jumps at the red marked part. The reason for the jump is that PPP does not meet the screening conditions at this time, and the system is downgraded to LIO. LIO will gradually diverge during operation. However, the overall positioning effect of LIOP is better than PPP. From Experiment 2, it can be concluded that in complex urban environments, the combined system can achieve relatively high-precision continuous robust positioning.

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

[0116]

[0117] In terms of posture estimation, the posture accuracy of Experiment 1 is as follows Fig. 9 As shown in Table 4, LIO is in a divergent state, and each turn will affect the posture. As time goes on, the error accumulates continuously. LIOP integrates PPP and restrains the divergence of LIO by constraining the posture, reducing the impact of the weak observability of yaw. Compared with LIO, LIOP improves RMSE by 95.4%, 87.6% and 99.7% in the three directions of roll, pitch and yaw, respectively.

[0118] Table 4 Experiment 1 Two algorithms attitude accuracy RMSE / °

[0119]

[0120] Experiment 2: Posture accuracy Fig.10As shown in Table 5, in the first half of the experiment, the attitude error of LIOP was around 0°. By adding constraints, the fusion result was more stable than the divergent state of LIO. However, in the shadow part, i.e., the complex urban environment, due to the poor quality of GNSS signals, the system was downgraded to LIO, resulting in divergence in the roll and pitch directions. After re-integration with PPP, it quickly converged to around 0°. In the yaw direction, since the system has been building corresponding constraints through factor graphs, there is no large fluctuation, and the error is always kept around 1°. The source of the error is the error in the initial heading angle estimation. Compared with LIO, the RMSE of LIOP in the roll, pitch, and yaw directions increased by 55.0%, 54.8%, and 96.0%, respectively, proving that LIOP can maintain good accuracy in attitude in complex urban environments.

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

[0122]

[0123] In addition to analyzing the position and attitude, the displacement increment is analyzed. Fig.11 It can be seen that after fusion, the obtained LIO increment is consistent with the PPP increment in trend on the curve, and fluctuates around the curve, indicating that the accuracy of LIO in a short time is similar to that of PPP, and can be used for short-term positioning in complex urban environments; compared with the LIOP error curve, it can be seen that the larger the increment, the larger the corresponding LIOP error. It can be considered that the vehicle affects the LIO solution and PPP interpolation during rapid movement, causing the error to increase.

[0124] Depend on Fig.12 As can be seen from (b) and (c) in Experiment 2, in the relatively open environment, some LIO and PPP increments will have large errors, 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 divergent, which does not conform to the actual situation of the carrier movement. Figure 6 and Figure 8 It can be seen that when there are fewer GNSS satellites, the PDOP value increases, the PPP error increases, and even the solution cannot be calculated. At this time, the system degenerates into a divergent state of LIO, the LIO increment fluctuates, and the system error increases. After reintegrating PPP, the system converges and tends to be stable, verifying the stability of the system.

[0125] The above experimental data show that the scheme in this case unifies the coordinate system by calculating the initial heading angle, realizes the fusion of LIO and PPP by constructing a factor graph, and builds a combined system to achieve complementary advantages between sensors. The combined algorithm can effectively reduce the drift of LiDAR SLAM. In an open environment, the combined algorithm can obtain stable decimeter-level positioning results; in a complex urban environment, it can achieve continuous and reliable positioning and achieve meter-level positioning accuracy, which can achieve the accuracy of advanced GNSS / INS / LiDAR combined algorithms and has good application prospects.

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

[0127] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, 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 conjunction with the embodiments disclosed herein can be implemented by electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in the above description according to function. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. A person of ordinary skill in the art may use different methods to implement the described functions for each specific application, but such implementation is not considered to be beyond the scope of the present invention.

[0129] Those skilled in the art will appreciate that all or part of the steps in the above method can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium, such as a read-only memory, a disk or an optical disk. Optionally, all or part of the steps in the above embodiment can also be implemented using one or more integrated circuits, and accordingly, each module / unit in the above embodiment can be implemented in the form of hardware or in the form of software function modules. The present invention 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 only specific implementations of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The protection scope of the present invention is not limited thereto. Although the present invention is described in detail with reference to the above-described embodiments, ordinary technicians in the field should understand that any technician familiar with the technical field can still modify the technical solutions recorded in the above-described embodiments within the technical scope disclosed by the present invention, or can easily think of changes, or make equivalent replacements for some of the technical features therein; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall 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 the data of each sensor in the combined positioning sensor system and pre-process the data of each sensor. The combined positioning sensor system is carried on a moving carrier. The data of each sensor includes: laser radar 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 state of the moving carrier at the timestamp of the lidar frame is used as the variable node, and the IMU pre-integrated measurement value, the change of the lidar inertial odometer and the precise single-point positioning are used as factor nodes to construct a factor graph, and the initial position and posture of the moving carrier are obtained using the IMU original observation data, IMU odometer and lidar inertial odometer. The specified indicators are used to judge the signal quality of the global navigation satellite system GNSS. If the signal quality is less than the preset threshold, the factor graph is optimized based on the constraints between adjacent variable nodes and based on the lidar inertial odometer 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 using precise single point positioning and lidar inertial odometer 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 data of each sensor, including: Based on the IMU original observation data and using the IMU pre-integration method, a relative carrier motion pre-integration measurement value between two adjacent timestamps is obtained, wherein the pre-integration measurement value includes a motion carrier velocity change measurement value, a motion carrier position change measurement value, and a 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 dedistortion and feature extraction of the lidar point cloud data are assisted based on the multi-frequency predicted pose.

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 change in the LiDAR inertial odometer in the factor node includes: Obtain a voxel map based on edge features and plane features extracted from the laser radar scan at the current moment; 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; According to the corresponding relationship between the edge and the plane at adjacent moments in the voxel map, the edge feature and edge distance equation and the plane feature and plane distance equation are constructed, and the Gauss-Newton method is used to solve the equation 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 laser radar odometer change connecting the two state variable postures.

5. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 1, characterized in that: The precise point positioning calculation in the factor node also includes: The rotation matrix from the world coordinate system to the navigation coordinate system is constructed based on the initial heading angle, and the rotation matrix from the navigation coordinate system to the Earth-centered Earth-fixed coordinate system is constructed based on the longitude and latitude of the motion carrier position at the initial moment of the laser radar; 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 GNSS measurement reference and the combined positioning sensor system reference are spatially corrected using the vector of the GNSS receiver position in the carrier coordinate system relative to the IMU measurement center. 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, characterized in that: The initial position and posture of the motion carrier are obtained using the IMU original observation data, IMU odometer and lidar inertial odometer, including: Use the 6-axis IMU in combination with the 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 odometer is obtained based on the factor graph, and 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 odometer.

7. The PPP / INS / LiDAR combined positioning method based on graph optimization according to claim 6 is characterized in that: Combine precise point positioning (PPP) and lidar inertial odometer 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 in 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 first frame time of the laser radar 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 by 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 data from each sensor in a combined positioning sensor system and pre-process the data from each sensor. The combined positioning sensor system is carried on a moving carrier. The data from each sensor include: 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 is obtained by solving the raw satellite data of a 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 timestamp of the lidar frame as the variable node, the IMU pre-integrated measurement value, the change of the lidar inertial odometer and the precise single-point positioning as the factor node, and using the IMU original observation data, IMU odometer and lidar inertial odometer to obtain the initial position and posture of the motion carrier; 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 based on the lidar inertial odometer 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 odometer to obtain the position and posture of the motion 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