A Pose Initialization Method for GNSS / SLAM Integrated Navigation System

Abstract

A method for pose initialization of a GNSS / SLAM integrated navigation system, belonging to mobile robot navigation and positioning. In a relatively open field, start the GNSS and SLAM systems; operate the mobile robot to complete a non-linear motion of no less than 10 m, and collect the longitude, latitude and altitude data of GNSS and the pose data of SLAM during the movement of the robot; according to the time stamp of the collected SLAM data, perform linear interpolation on the longitude, latitude and altitude data of GNSS to obtain synchronized longitude, latitude and altitude data of GNSS; use the first data as a reference to convert the GNSS data into the northeast-up coordinate system to obtain the 3D trajectory data of the robot; based on the Lie group optimization theory, solve the optimization problem: align the trajectory of SLAM to the coordinate system according to the obtained pose matrix, start the GNSS / SLAM integrated navigation fusion algorithm, and complete the initialization. It effectively improves the initialization accuracy of the integrated navigation system.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot navigation and positioning, and in particular relates to a method for pose initialization of a GNSS / SLAM integrated navigation system based on Lie group optimization. Background Art

[0002] The Global Navigation Satellite System (GNSS) is a navigation and positioning method based on navigation satellites and serving the world. It can provide absolute positioning information that is all-weather, all-time-domain, and without cumulative errors, and is widely used in various outdoor mobile robot navigation tasks. The disadvantage is that it is vulnerable to building occlusion and cannot provide reliable positioning information for indoor mobile robots.

[0003] Simultaneous Localization and Mapping (SLAM) is a positioning system built based on external sensing sensors such as lidar and cameras. It estimates the position and pose (collectively referred to as pose) of the mobile robot while estimating the map of the surrounding environment, and can be widely used in indoor and outdoor navigation tasks. Its disadvantage is that there is cumulative error in positioning and it cannot be applied to large-scale navigation tasks.

[0004] There is a significant complementarity between GNSS and SLAM. Researchers have proposed a GNSS / SLAM integrated navigation system to meet the needs of mobile robots for large-scale and high-precision navigation and positioning. Spatial unity is the basis of the integrated navigation system. Therefore, before combined positioning, it is necessary to align the world coordinate system of SLAM with the north-east-down coordinate system of GNSS through an initialization process.

[0005] There are two common initialization methods: giving the initial pose through an external signal and obtaining the initial pose through singular value decomposition. However, these methods still have the following deficiencies: (1) The former heavily relies on external devices, such as a dual-antenna RTK positioning and orientation system, so the scope of use of this method is very limited. (2) The latter uses singular value decomposition (SVD) to analytically obtain the rotation matrix and translation vector between two trajectories, but this method ignores the lever-arm vector between the SLAM sensor coordinate system and the GNSS receiver coordinate system, which may deteriorate the accuracy of the initial pose. (3) In addition, the latter also assumes that the positioning values of GNSS and SLAM have the same uncertainty in the three dimensions of X, Y, and Z, which does not conform to the fact that there are different measurement accuracies in different dimensions. Summary of the Invention

[0006] The purpose of the present invention is to propose a pose initialization method for a GNSS / SLAM integrated navigation system based on Lie group optimization and considering the GNSS boom vector in order to improve problems such as the relatively low initial accuracy of the pose of the GNSS / SLAM integrated navigation system.

[0007] The present invention includes the following steps:

[0008] S1: Operate the robot to move to a relatively open area, and at the same time start the GNSS positioning system and the SLAM positioning system;

[0009] S2: Manipulate the mobile robot to complete a non-linear motion of no less than 10 m, and at the same time collect the longitude, latitude and altitude data of the GNSS and the pose data of the SLAM during the movement of the robot;

[0010] S3: According to the time stamp of the collected SLAM data, linearly interpolate the longitude, latitude and altitude data of the GNSS to obtain synchronized longitude, latitude and altitude data of the GNSS; Use the first data of the synchronized GNSS to initialize the northeast celestial coordinate system of the GNSS, and then convert the remaining synchronized longitude, latitude and altitude data of the GNSS into rectangular coordinate representation in the northeast celestial coordinate system to obtain the 3D trajectory data of the robot;

[0011] S4: Based on the Lie group optimization theory, solve the optimization problem:

[0012]

[0013] where i = 1,..., N represents the number of the data, E represents the northeast celestial coordinate system of the GNSS, W represents the world coordinate system of the SLAM, B represents the SLAM sensor coordinate system, and G represents the coordinate system of the GNSS receiver, represents the residual, represents the covariance matrix corresponding to the residual, represents the pose matrix of W relative to E (SE(3) represents the 6D matrix Lie group corresponding to the transformation matrix), represents the pose matrix of B i output by the SLAM relative to W, represents the position vector of G relative to B (which can be pre-calibrated and is assumed to be a known quantity), is the position vector of G i measured by the GNSS relative to E. The solution process of this problem is as follows:

[0014] S4.1 Initialize to a 4D identity matrix or a pose matrix obtained by the SVD method;

[0015] S4.2 Calculate the residual term: f i , i = 1,..., N

[0016] S4.3 Calculate the Jacobian matrix:

[0017]

[0018] Among them,

[0019]

[0020] S4.4 Solve the incremental equation:

[0021]

[0022] S4.5 Use the Lie algebra to Lie group exponential map to update the variables to be optimized:

[0023]

[0024] S4.6 Terminate the iteration if any of the following situations occur: (1) when ξ * is less than a certain threshold; (2) when the number of iterations exceeds a certain threshold; (3) when the cost function does not decrease but increases; otherwise, repeat S4.2 - S4.5 until the termination iteration condition is met, and output the current estimated value as the optimal solution of this Lie group optimization problem.

[0025] S5 According to the obtained pose matrix Align the trajectory of SLAM to the E system, start the GNSS / SLAM integrated navigation fusion algorithm, complete the initialization, and start the formal integrated navigation task.

[0026] The present invention simultaneously considers the lever arm vector of the GNSS receiver relative to the SLAM sensor and the covariance matrix of the measurement values, which can effectively improve the initialization accuracy of the integrated navigation system and improve the problem of low initial pose accuracy of the GNSS / SLAM integrated navigation system. Experiments prove that compared with the conventional SVD method, the method of the present invention effectively considers the lever arm error and measurement uncertainty, and significantly improves the accuracy of the initialization method. Brief Description of the Drawings

[0027] Figure 1 is the flow schematic diagram of the method of the present invention;

[0028] Figure 2 is the flow schematic diagram of the Lie group optimization step in the method of the present invention;

[0029] Figure 3 is the trajectory comparison diagram before and after the initialization of the GNSS / SLAM integrated navigation system. Detailed Embodiments

[0030] To better understand the above objects, features, and advantages of the present invention, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that, without conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. Many specific details are set forth in the following description to facilitate a thorough understanding of the present invention. However, the present invention can also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited by the specific embodiments disclosed below.

[0031] In the conventional SVD-based pose initialization method, the core approach is to analytically solve the following optimization problem using SVD:

[0032]

[0033] Obviously, the arm vector from the GNSS receiver to the SLAM sensor is ignored in this method and the covariance matrix is not set for the residual g according to the positioning accuracies of GNSS and SLAM i Set the covariance matrix.

[0034] As Figure 1 shown, the pose initialization of the GNSS / SLAM integrated navigation system using the present invention includes the following steps:

[0035] S0: Configure a GNSS positioning system, a SLAM positioning system, and their integrated navigation positioning algorithms on the mobile robot respectively; after power-on, the GNSS positioning system receives satellite information and calculates the positioning data of a single point, usually including data such as longitude, latitude, altitude, and corresponding uncertainties; the SLAM positioning system obtains corresponding perception data from the SLAM sensor and estimates the position, pose, and corresponding uncertainties of the current robot relative to the world coordinate system and other data.

[0036] S1: To ensure the normal operation of the GNSS positioning system in the initial stage and minimize the interference of building occlusion and multipath effects as much as possible, operate the robot to move to an open area and start the GNSS positioning system and the SLAM positioning system respectively.

[0037] S2: Manipulate the mobile robot to complete a non-linear movement of no less than 10 m, and collect the positioning data of GNSS and the positioning data of SLAM during the movement of the robot. The GNSS data usually includes timestamp, longitude, latitude, altitude, and their uncertainties, etc., and the SLAM data usually includes timestamp, pose, pose uncertainty, etc.

[0038] S3: According to the timestamps of the collected SLAM data, linearly interpolate the latitude, longitude, and altitude data of GNSS to obtain synchronized GNSS latitude, longitude, and altitude data. Then, use the first data of the synchronized GNSS to initialize the north-east-down coordinate system of GNSS, and convert the remaining synchronized GNSS latitude, longitude, and altitude data into the north-east-down coordinate system to obtain the 3D trajectory data of the robot.

[0039] S4: Based on the Lie group optimization theory, solve the optimization problem:

[0040]

[0041] where \(i = 1,\ldots,N\) represents the data number, \(E\) represents the north-east-down coordinate system of GNSS, \(W\) represents the world coordinate system of SLAM, \(B\) represents the SLAM sensor coordinate system, \(G\) represents the coordinate system of the GNSS receiver, represents the residual, represents the covariance matrix corresponding to the residual, represents the pose matrix of \(W\) relative to \(E\) (SE(3) represents the 6D matrix Lie group corresponding to the transformation matrix), represents the pose matrix of \(B\) i output by SLAM relative to \(W\), represents the position vector of \(G\) relative to \(B\) (which can be pre-calibrated and is assumed to be a known quantity), is the position vector of \(G\) i measured by GNSS relative to \(E\). As Figure 2 shown, the solution process of this Lie group optimization problem is as follows:

[0042] S4.1 Initialize to a 4D identity matrix or a pose matrix obtained by the SVD method;

[0043] S4.2 Calculate the residual term: \(f\) i , \(i = 1,\ldots,N\);

[0044]

[0045] S4.3 Calculate the Jacobian matrix:

[0046]

[0047] where:

[0048]

[0049] S4.4 Solve the increment equation:

[0050]

[0051] where the calculation formula for the covariance matrix is:

[0052]

[0053] Here, represents the positioning uncertainty of GNSS, represents the pose estimation uncertainty of SLAM, and matrix A i has the following calculation formula:

[0054]

[0055] S4.5 Update the variables to be optimized using the solution of the incremental equation:

[0056]

[0057] where the exponential map exp(ξ * ^) from Lie algebra to Lie group is defined as:

[0058]

[0059] S4.6 Terminate the iteration if any of the following situations occur: (1) when ξ * is less than a certain threshold, such as 10 -6 ; (2) when the number of iterations exceeds a certain threshold, such as 50; (3) when the cost function does not decrease but increases; otherwise, repeat S4.2, S4.3, S4.5 until the termination iteration condition is met, and output the current estimated value as the optimal solution of this Lie group optimization problem.

[0060] S5 Align the SLAM trajectory to the E system according to the obtained pose matrix Start the GNSS / SLAM integrated navigation algorithm, complete the initialization, and start the formal integrated navigation task.

[0061] To verify the above technical solutions of the present invention and verify the beneficial effects of the present invention, the above technical solutions will be described below through a specific embodiment. As Figure 3 shown, the solid line represents the motion trajectory of the robot obtained by GNSS, the dotted line represents the motion trajectory of the robot estimated by SLAM before initialization, the dashed line represents the motion trajectory given by SLAM after initialization using the conventional SVD method, and the dash-dotted line represents the motion trajectory given by SLAM after initialization using the method of the present invention. It can be seen from Figure 3 that the trajectory after initialization of the present invention is closer to the GNSS trajectory. The errors of each sampling point are statistically analyzed, and the results are shown in Table 1:

[0062] Table 1 Statistical table of absolute trajectory errors (unit: meter)

[0063]

[0064] As can be seen from the above, the method of the present invention effectively takes into account the boom error and measurement uncertainty, and significantly improves the accuracy of the conventional SVD initialization method.

Claims

1. A method for pose initialization of a GNSS / SLAM integrated navigation system, characterized in that Including the following steps: S1: Operate the robot to move to a relatively open area, and at the same time start the GNSS positioning system and the SLAM positioning system; S2: Manipulate the mobile robot to complete a non-linear motion of no less than 10 m, and at the same time collect the latitude, longitude and altitude data of GNSS and the pose data of SLAM during the movement of the robot; S3: According to the time stamp of the collected SLAM data, perform linear interpolation on the latitude, longitude and altitude data of GNSS to obtain synchronized GNSS latitude, longitude and altitude data; Use the first data of the synchronized GNSS to initialize the northeast celestial coordinate system of GNSS, and then convert the remaining synchronized GNSS latitude, longitude and altitude data to the northeast celestial coordinate system for rectangular coordinate representation to obtain the 3D trajectory data of the robot; S4: Based on the Lie group optimization theory, solve the optimization problem: Among them, i = 1, …, N represents the data number, E represents the northeast-down coordinate system of GNSS, W represents the world coordinate system of SLAM, B represents the SLAM sensor coordinate system, and G represents the coordinate system of the GNSS receiver. represents the residual. represents the covariance matrix corresponding to the residual. represents the pose matrix of W relative to E, and SE(3) represents the 6-dimensional matrix Lie group corresponding to the transformation matrix. represents the pose matrix of B i output by SLAM relative to W. represents the position vector of G relative to B. is the G measured by GNSS i relative to the position vector of E; S5: According to the obtained pose matrix Align the SLAM trajectory to the northeast celestial coordinate system, start the GNSS / SLAM integrated navigation fusion algorithm, complete the initialization, and start the formal integrated navigation task.

2. The method for pose initialization of a GNSS / SLAM integrated navigation system according to claim 1, characterized in that In step S4, the specific process of solving the optimization problem based on the Lie group optimization theory is as follows: S4.1 Initialize as a 4D identity matrix or the pose matrix obtained by the SVD method; S4.2 Calculate the residual term: f i , i = 1, …, N; S4.3 Calculate the Jacobian matrix: Where: S4.4 Solve the increment equation: S4.5 Use the Lie algebra to Lie group exponential mapping to update the variables to be optimized: S4.6 Terminate the iteration if any of the following conditions occur: (1) when ξ * is less than a certain threshold; (2) when the number of iterations exceeds a certain threshold; (3) when the cost function does not decrease but increases; otherwise, repeat S4.2 - S4.5 until the termination condition of the iteration is satisfied, and output the current estimated value as the optimal solution to this Lie group optimization problem.

Images