A method for detecting double loops in factor graphs

Abstract

The present invention discloses a method for detecting double loops of factor graphs, which obtains measurement information collected by multi-source sensors such as laser radar, satellite, and IMU, obtains state information based on an adaptive factor graph multi-source information fusion navigation algorithm, performs coarse loop detection based on state constraints, and if it passes, uses the laser radar point cloud map to perform precise loop detection. If both double loop detections pass, the state information is extracted to perform factor graph loop correction; if it fails, the next time node is detected. The present invention can be applied to scenes such as urban positioning, vehicle speed measurement, and attitude alignment; the present invention has good robustness and low time complexity, and can shorten the factor graph loop detection time.

Description

Technical Field

[0001] The invention belongs to the field of factor graph multi-source information fusion navigation algorithm, and in particular relates to a factor graph double loop detection method. Background Art

[0002] Factor graph is a bidirectional probabilistic graph model, which contains two types of nodes: one is the variable node, which represents the variables in the global multivariate function; the other is the factor node, which represents the local function in the factorization. Each local function is only related to some variables of the global function. If and only if the variable is the independent variable of the local function, there is a connection edge between the factor node and the corresponding variable node. As a graphical tool for analyzing problems, factor graph has been applied in many fields, such as signal processing, artificial intelligence, integrated navigation, etc. In the field of integrated navigation, the factor graph multi-source information fusion navigation algorithm has become a hot research field due to its rich sensor information and plug-and-play characteristics. For example, combining vehicle sensors and urban environmental information to complete urban navigation positioning, or eliminating faulty sensors under strong airflow interference to achieve robust navigation of aircraft, etc.

[0003] In the process of combined navigation, the estimation of pose often involves a recursive process, that is, the pose of the current frame is calculated from the pose of the previous frame. The error divergence in this process will cause a large cumulative error. Loop detection can effectively correct such inference errors. Loop detection is used in the fields of LiDAR and visual SLAM. It enables the navigation system to accurately identify the locations that have been visited before, and solves the drift problem of pose inference by identifying past location information. For example, in the absence of reliable geometric positioning information in urban canyons or indoors, multi-source information fusion navigation is performed. The drift of inferred sensor errors over time is the primary factor affecting navigation accuracy. Traditional adaptive factor algorithms are difficult to meet actual application scenarios. Summary of the invention

[0004] In order to solve the above problems in the prior art, the present invention provides a factor graph double loop detection method introducing state constraints.

[0005] In order to achieve the above object, the present invention provides a method for detecting double loops in a factor graph, which specifically comprises the following steps:

[0006] Step 1: Obtain satellite, inertial navigation and lidar sensor measurement information;

[0007] Step 2: Define the system state variables as variable nodes of the factor graph, define the sensor measurement information as factor nodes, and then use the adaptive method to improve the error model to build an adaptive factor graph model. The current state variables are obtained through the factor graph optimization method, and the classification features of the state variables are extracted through statistical analysis. The classification features at each moment are compared for rough loop detection. If it passes, the key frame for precise loop detection is locked;

[0008] Step 3: Perform neural network training on the lidar measurement information that has undergone rough loop closure detection in step 2 to obtain a network structure suitable for the predetermined scenario, so as to use the network to extract point cloud features from the lidar measurement information of the two key frames locked in step 2, obtain classification features, and construct a similarity function between the two frames for precise loop closure detection. If it passes, it is confirmed that the loop closure detection is completed at two moments;

[0009] Step 4: Use the lidar measurement information from step 3 to obtain state change information and add loop factor correction.

[0010] Furthermore, the sensor measurement information in step 1 includes the latitude and longitude information calculated by the satellite, the specific force and angular velocity information output by the inertial navigation sensor, and the point cloud information output by the lidar.

[0011] Furthermore, in step 2, for the factor graph configuration and different selections of sensors, the current state variable can select various information such as longitude and latitude, horizontal speed, posture, etc. as constraints. The example of the present invention selects the location information of longitude and latitude as an example.

[0012] Furthermore, the step 2 is specifically as follows:

[0013] Step 2.1: Use the measurement information from step 1 to build a factor graph model, and add an adaptive method to the factor graph to correct the error model for optimization;

[0014] Step 2.2: Use the current state variables to perform rough loop detection, and the search strategy adopted is KD tree search. Find the k points closest to the matching point, calculate the time interval between the k points and the matching point, if the time interval is greater than the preset value and the interval distance is less than the preset value, then the loop detection is completed and enter step 3; otherwise, the loop detection is terminated and wait for the next moment input to re-execute.

[0015] Furthermore, the adaptive factor graph model in step 2 is

[0016]

[0017] Where X * is the state variable to be optimized, is the variable value x when the objective function f(x) takes the minimum value, is the sum of the objective functions from the 1st to the i-th moment, is the Markov norm of the objective function f(x), is the weight function, h k (X i ) is the predicted value of the kth sensor state at time i, z i,k is the kth sensor measurement at time i, according to the residual r of each sensor i k =h k (X i-1 )-z i-1,k Adjust the sensor factor node weight. Specific weight function S(r i k )as follows

[0018]

[0019] Where exp(x) is the exponential function, r i k Represents the residual value of the sensor, r0 represents the residual threshold of the sensor, the higher the positioning accuracy of the sensor, the lower the r0 value, that is, the smaller the weight change. γ is a parameter that characterizes the positioning accuracy of each sensor, and γ determines how fast the weight changes.

[0020] Furthermore, the step 3 is specifically as follows:

[0021] Step 3.1: Use the lidar point cloud data obtained in step 1 to train PointNet to obtain a network suitable for the preset environment;

[0022] Step 3.2: Use the PointNet network to extract features from the two matching points obtained in step 2 to obtain m classification features corresponding to the point cloud image;

[0023] Step 3.3: Calculate the Hamming distance for the m classification features to make a similarity judgment. If the similarity reaches or exceeds the preset value, the precise loop detection is completed and the next step of loop correction is entered; otherwise, the loop detection is terminated and the next input is waited for to be executed again.

[0024] Furthermore, the PointNet method in step 3 is specifically a point cloud processing network, which can directly input unordered point clouds for processing and learn the global features of point cloud data. The specific steps are:

[0025] Step 3.1.1: Input transformation, using the spatial transformer network (STN) to align the input point cloud data;

[0026] Step 3.1.2: Point cloud processing: use multi-layer perceptron (MLP) to process point cloud data, upgrade the 3D data to 64 dimensions and extract point cloud features;

[0027] Step 3.1.3: Feature transformation, using STN to align input features;

[0028] Step 3.1.4: Feature processing, use MLP to process feature data and upgrade the 64-dimensional data to 1024 dimensions;

[0029] Step 3.1.5: Symmetric function processing, using maximum pooling to fuse features to generate 1*1024 dimensional global features;

[0030] Step 3.1.6: Feature classification: Use MLP to learn the input 1024-dimensional point cloud features and output m classification features.

[0031] Furthermore, the step 4 is specifically as follows:

[0032] Step 4.1: Use the lidar point cloud information to calculate the iterative closest point method (ICP) to obtain the pose transformation information and corresponding error function between two moments;

[0033] Step 4.2: Input the pose transformation information and the corresponding error function between two moments into the factor graph, establish a loop factor node, and then achieve global optimization.

[0034] The beneficial effects of the present invention compared to the prior art are as follows: the present invention has a better effect on conventional application scenarios, and can be applied to the fields of vehicle-mounted combined navigation under satellite denial and airborne multi-source sensor speed measurement, attitude correction, etc. The use process has high robustness and high reliability. In the scenario where reliable geometric positioning information is lacking, the present invention uses state constraints to reduce the time complexity of loop detection, and the present invention realizes the factor graph loop detection function that introduces state constraints. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is the overall algorithm flow chart of the present invention;

[0036] Figure 2 Schematic diagram of the factor graph structure for introducing state constraints; DETAILED DESCRIPTION

[0037] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0038] The present invention will be further described below in conjunction with the accompanying drawings:

[0039] like Figure 1 As shown, the present invention discloses a method for detecting double loops in a factor graph, comprising the following specific steps:

[0040] Step 1: Obtain satellite, inertial navigation and lidar sensor measurement information;

[0041] Specifically, the sensor measurement information includes the latitude and longitude information calculated by the satellite, the specific force and angular velocity information output by the inertial navigation sensor, and the point cloud information output by the lidar. Before building the factor graph model, the measurement information of different sensors needs to be preprocessed, including time matching of each factor node and IMU pre-integration. The factor node matching uses the laser radar acquisition frequency of 10Hz as the benchmark, the IMU data acquisition frequency is 100Hz, and the relative position between two adjacent nodes is calculated through pre-integration to achieve synchronization. The GPS data acquisition frequency is 1Hz, and the GPS data is matched using timestamps.

[0042] Step 2: Define the system state variables as variable nodes of the factor graph, define the sensor measurement information as factor nodes, and then use the adaptive method to improve the error model to build an adaptive factor graph model. Obtain the state variables at the current moment through the factor graph optimization method, extract the classification features of the state variables through statistical analysis, and compare the classification features at each moment to perform coarse loop detection. If it passes, lock the key frame of precise loop detection and continue to step 3; if it fails, execute step 1 at the next moment;

[0043] According to the factor graph configuration and different selection of sensors, the current state variable can select various information such as longitude and latitude, horizontal speed, posture, etc. as constraints. The example of the present invention selects the location information of longitude and latitude as an example.

[0044] The step 2 is specifically as follows:

[0045] Step 2.1: Use the measurement information from step 1 to build a factor graph model, and add an adaptive method to the factor graph to correct the error model for optimization;

[0046] Step 2.2: Use the current state variables to perform rough loop detection, and the search strategy adopted is KD tree search. Find the k points closest to the matching point, calculate the time interval between the k points and the matching point, if the time interval is greater than the preset value and the interval distance is less than the preset value, then the loop detection is completed and enter step 3; otherwise, the loop detection is terminated and wait for the next moment input to re-execute.

[0047] Specifically, in this embodiment, the adaptive factor graph model in step 2 is

[0048]

[0049] Where X * is the state variable to be optimized, is the variable value x when the objective function f(x) takes the minimum value, is the sum of the objective functions from the 1st to the i-th moment, is the Markov norm of the objective function f(x), S(r i k ) is the weight function, h k (X i ) is the predicted value of the kth sensor state at time i, z i,k is the kth sensor measurement at time i, according to the residual r of each sensor i k =h k (X i-1 )-z i-1,k Adjust the sensor factor node weight. Specific weight function S(r i k )as follows

[0050]

[0051] Where exp(x) is the exponential function, r i k Represents the residual value of the sensor, r0 represents the residual threshold of the sensor. The higher the positioning accuracy of the sensor, the lower the r0 value, that is, the smaller the weight change. γ is a parameter that characterizes the positioning accuracy of each sensor, and γ determines how fast the weight changes. In the invention example, the inertial navigation system used in the vehicle-mounted combined navigation experiment is the Xsens Mti-10 series sensor, and the gyroscope constant drift is 18° / h. The laser radar sensor uses HDL-32E, and the real-time point cloud matching error can be as low as 0.2cm, with an effective range of 80m. The GPS receiver uses the commonly used u-blox M8T model. Due to the multipath effect in urban areas, the satellite positioning accuracy is low, and the average positioning accuracy is 15 to 20m. When designing the adaptive weights, after adjustment with reference to the sensor performance, the adaptive weights of the laser radar, IMU, and satellite sensor are obtained as 2, 4, and 8.

[0052] In addition, the search strategy used in step 2 is KD tree search. Find the k points closest to the matching point, calculate the time interval between the k points and the matching point, and if the time interval is greater than the preset value and the interval distance is less than the preset value, the loop detection is completed and the next step of precise loop detection is entered; otherwise, the loop detection is terminated and the next input is waited for re-execution.

[0053] Step 3: Perform neural network training on the lidar measurement information to obtain a network structure suitable for the predetermined scene, so as to use the network to extract point cloud features from the lidar measurement information of the two key frames locked in step 2, obtain classification features, and construct a similarity function between the two frames for accurate loop detection. If it passes, confirm that the loop detection is completed at the two moments and continue to step 4; if it fails, execute step 1 at the next moment;

[0054] Specifically, step 3 is divided into the following steps:

[0055] Step 3.1: Use the lidar point cloud data obtained in step 1 to train PointNet to obtain a network suitable for the preset environment;

[0056] Step 3.2: Use the PointNet network to extract features from the two matching points obtained in step 2 to obtain m classification features corresponding to the point cloud image;

[0057] Step 3.3: Calculate the Hamming distance for the m classification features to make a similarity judgment. If the similarity reaches or exceeds the preset value, the precise loop detection is completed and the next step of loop correction is entered; otherwise, the loop detection is terminated and the next input is waited for to be executed again.

[0058] In addition, in this embodiment, the PointNet method in step 3 is specifically a point cloud processing network, which can directly input unordered point clouds for processing and learn the global features of point cloud data. The specific steps are:

[0059] Step 3.1.1: Input transformation, use the spatial transformer network (STN) to align the input point cloud data. The network generates an affine transformation matrix to normalize the rotation, translation and other changes of the point cloud. The input is the original point cloud data and the output is a 3*3 rotation matrix.

[0060] Step 3.1.2: Point cloud processing: use multi-layer perceptron (MLP) to process point cloud data. Input n*3 point cloud, use 64*64 two-layer perceptron to process each point, learn and extract the information of each point, and upgrade the 3D data to 64D to extract point cloud features.

[0061] Step 3.1.3: Feature transformation: use the STN network to align the input 64-dimensional point cloud features;

[0062] Step 3.1.4: Feature processing, use MLP to process feature data and upgrade the 64-dimensional data to 1024 dimensions;

[0063] Step 3.1.5: Symmetric function processing, using maximum pooling to fuse n*1024 point cloud features to generate 1*1024 dimensional features for description;

[0064] Step 3.1.6: Feature classification: MLP is used to learn the input 1024-dimensional point cloud features, and the output is m classification features, representing the classification categories, and each category corresponds to the point cloud classification score.

[0065] Step 4: Use the lidar measurement information obtained in step 3 to obtain state change information and add loop factor correction.

[0066] Specifically, in this embodiment, step 4 is specifically as follows:

[0067] Step 4.1: Input the lidar point cloud information and use the iterative closest point method (ICP) to align the point cloud data between two time points. The optimal coordinate change, i.e., the rotation matrix R and the translation vector t, is calculated iteratively through the least squares method to minimize the error function, thereby obtaining the pose transformation information and the corresponding error function between the two time points.

[0068] Step 4.2: Input the pose transformation information and the corresponding error function between the two moments into the factor graph, establish a loop factor node between the two matching moments, and then achieve global optimization.

[0069] The factor graph structure in steps 2 to 4 is as follows Figure 2 As shown, the prior factor node is added at the initial moment Add the GPS unary factor node f connected to the variable node at time i i GPS , IMU binary factor node f connected to the variable nodes at two adjacent moments i INS And the Lidar binary factor node f connected to the variable nodes at two adjacent moments i Lidar The double loop detection method is as follows: first, a coarse loop detection is performed, that is, the GPS position status information is used to detect whether the GPS loop requirements are met. Then perform accurate loop closure detection, that is, use the lidar point cloud information to detect whether it meets the lidar loop closure requirements If the double loop closure detection is passed, a loop closure factor f is added back Make corrections; otherwise, the loop detection fails and waits for the next input to execute again.

[0070] In addition, an embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium may store a program, which, when executed, includes part or all of the steps of any factor graph double loop detection method recorded in the above method embodiment.

[0071] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0072] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable memory. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a memory and includes several instructions for a computer device (which can be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned memory includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, magnetic disk or optical disk and other media that can store program codes.

[0073] A person skilled in the art can understand that all or part of the steps in the various methods of the above embodiments can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable memory, and the memory can include: a flash drive, a read-only memory (English: Read-Only Memory, abbreviated as: ROM), a random access memory (English: Random Access Memory, abbreviated as: RAM), a magnetic disk or an optical disk, etc.

[0074] The embodiments of the present invention are described in detail above with reference to the accompanying drawings, but the present invention is not limited to the above embodiments, and various changes can be made within the knowledge scope of ordinary technicians in this field without departing from the purpose of the present invention.

Claims

1. A method for detecting double loops in factor graphs, characterized in that: include: Step 1: Obtain satellite, inertial navigation and lidar measurement information; Step 2: Define the system state variables as variable nodes of the factor graph, define the laser radar measurement information as factor nodes, and then use the adaptive method to improve the error model to construct an adaptive factor graph model. The current state variables are obtained by the factor graph optimization method, and the classification features of the state variables are extracted by statistical analysis. The classification features at each moment are compared to perform coarse loop detection, thereby locking the key frame for accurate loop detection; wherein, the adaptive factor graph model is Where X * is the state variable to be optimized, is the variable value x when the objective function f(x) takes the minimum value, is the sum of the objective functions from the 1st to the i-th moment, is the Markov norm of the objective function f(x), is the weight function, h k (X i ) is the predicted value of the kth sensor state at time i, z i,k is the kth sensor measurement at time i, according to the residual r of each sensor i k =h k (X i-1 )-z i-1,k Adjust sensor factor node weights; Step 3: Perform neural network training on the laser radar measurement information that has undergone rough loop detection in step 2 to obtain a network structure suitable for the predetermined scene, thereby using the neural network to extract point cloud features from the laser radar measurement information of the key frame, obtain classification features, and construct a similarity function between the two frames for precise loop detection. If passed, it is confirmed that the loop detection is completed at two moments; Step 4: Use the lidar measurement information obtained through accurate loop closure detection in step 3 to obtain state change information and add loop closure factor correction.

2. The detection method according to claim 1, characterized in that: In the step 2, for the factor graph configuration and different selections of sensors, the constraints of the state variables at the current moment include one or more of longitude and latitude, horizontal speed, and attitude.

3. The detection method according to claim 1, characterized in that: The second step comprises: The measurement information in step 1 is used to construct a factor graph model, and an adaptive method is added to the factor graph to correct the error model for optimization; The current state variable is used for coarse loop detection, and the search strategy adopted is KD tree search; the k points closest to the matching point are found, and the time interval between the k points and the matching point is calculated. If the time interval is greater than the preset value and the interval distance is less than the preset value, the loop detection is completed.

4. The detection method according to claim 1, characterized in that: The weight function S(r i k )as follows Where exp(x) is the exponential function, r i k Represents the residual value of the sensor, r0 represents the residual threshold of the sensor, the higher the positioning accuracy of the sensor, the lower the r0 value, that is, the smaller the weight change, and γ is a parameter that characterizes the positioning accuracy of each sensor.

5. The detection method according to claim 1, characterized in that: The step three comprises: Use the laser radar point cloud data obtained in step 1 to perform PointNet training to obtain a network suitable for the preset environment; use the PointNet network to extract features from the two matching points obtained in step 3 to obtain m classification features corresponding to the point cloud image; The Hamming distance is calculated for the m classification features to make a similarity judgment. If the similarity reaches a preset value or above, the precise loop detection is completed and the next step of loop correction is entered; otherwise, the loop detection is terminated and the next input is waited for to be re-executed.

6. The detection method according to claim 1, characterized in that: The fourth step comprises: The iterative closest point method is calculated using the laser radar point cloud information to obtain the pose transformation information and corresponding error function between two moments; The pose transformation information between two moments and the corresponding error function are input into the factor graph, and loop factor nodes are established to achieve global optimization.

7. A computer-readable storage medium, characterized in that: include: The computer-readable storage medium stores a computer program, wherein when the computer program is executed by a processor, the steps of a factor graph double loop detection method as described in any one of claims 1 to 6 are implemented.

Images