GNSS-SLAM initialization method, system, mobile robot and storage medium

Abstract

This solution involves a GNSS-SLAM initialization method, system, mobile robot and storage medium. The method includes: using the IMU coordinate system as the main body coordinate system, calibrating the GNSS, wheel odometer, and lidar, and obtaining the relative posture relationship after calibration, converting the wheel odometer data and lidar data to the IMU coordinate system; in the IMU coordinate system, calculating the local lidar odometer trajectory data; preprocessing the GNSS data, and constructing an objective function based on the preprocessed GNSS data and the local lidar odometer trajectory data; adjusting the parameters according to the objective function until the initialization is completed. By calibrating the sensor and converting the coordinate system, the local lidar odometer trajectory data can be fused and aligned with the preprocessed GNSS data, ensuring that the GNSS-SLAM is established in the same coordinate system, without the need to use hardware for auxiliary fusion positioning, reducing hardware costs, and improving the robustness and stability of the GNSS-SLAM system.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a GNSS-SLAM initialization method, system, mobile robot and storage medium. Background Art

[0002] In recent years, with the continuous improvement of science and technology, mobile robot technology has been widely used in various fields of daily life. In daily operation, mobile robots need to obtain reliable posture information. The commonly used positioning methods are GNSS positioning or SLAM positioning. Among them, GNSS positioning calculates the latitude and longitude of the current receiver by differential calculation through satellite communication; SLAM positioning technology is a commonly used robot positioning technology, which means that the robot locates its own posture information in an unknown environment through observed environmental features, and constructs a map according to its own environment, so as to achieve the purpose of simultaneous positioning and map construction. GNSS positioning has good positioning effect in open environments, and its positioning accuracy is easily affected by occlusion interference, while SLAM positioning has good positioning accuracy in scenes with rich features, but it is easy to degrade in open scenes, resulting in positioning failure.

[0003] The commonly used outdoor positioning method is the fusion of GNSS and SLAM, but the fusion of GNSS and SLAM has the problem of inconsistent coordinate systems. Therefore, the traditional fusion method is dual-antenna positioning or magnetometer-assisted orientation. However, the traditional fusion positioning method has the problem of high hardware cost. Summary of the invention

[0004] Based on this, in order to solve the above technical problems, a GNSS-SLAM initialization method, system, mobile robot and storage medium are provided, which can reduce the cost of GNSS-SLAM fusion positioning.

[0005] A GNSS-SLAM initialization method, the method comprising:

[0006] Using the IMU coordinate system as the body coordinate system, calibrate the GNSS, wheel odometer, and lidar, and obtain the relative position and posture relationship after calibration;

[0007] Acquire IMU data, GNSS data, wheel odometer data, and lidar data, and convert the wheel odometer data and the lidar data into the IMU coordinate system according to the relative posture relationship;

[0008] In the IMU coordinate system, local laser radar odometer trajectory data is obtained according to the IMU data, the wheel odometer data, and the laser radar data;

[0009] Preprocessing the GNSS data, and constructing an objective function based on the preprocessed GNSS data and the local laser radar odometer trajectory data;

[0010] The parameters are adjusted according to the objective function until initialization is completed.

[0011] In one embodiment, the IMU coordinate system is used as the body coordinate system to calibrate the GNSS, wheel odometer, and lidar, including:

[0012] Using the IMU coordinate system as the body coordinate system, calibrating the internal reference data of the wheel odometer and the IMU;

[0013] The IMU coordinate system is used as the body coordinate system to calibrate the external parameter data of the wheel odometer, the IMU, the GNSS, and the lidar.

[0014] In one of the embodiments, in the IMU coordinate system, obtaining local laser radar odometer trajectory data according to the IMU data, the wheel odometer data, and the laser radar data includes:

[0015] In the IMU coordinate system, the IMU data and the wheel odometer data are fused to obtain relative motion;

[0016] According to the relative motion, distortion correction is performed on the laser radar data to obtain corrected laser radar data;

[0017] Local lidar odometer trajectory data is obtained according to the IMU data, the wheel odometer data, and the corrected lidar data.

[0018] In one embodiment, obtaining local lidar odometer trajectory data according to the IMU data, the wheel odometer data, and the corrected lidar data includes:

[0019] Construct the inter-frame IMU pre-integration residual ∑||imu|| according to the IMU data 2 ;

[0020] According to the wheel odometer data, the inter-frame physical odometer domain integral residual ∑||odom|| 2 ;

[0021] Construct the feature point distance residual ∑||lidar|| based on the corrected lidar data 2 ;

[0022] According to the IMU pre-integration residual ∑||imu|| 2 , the physical odometer domain integral residual ∑||odom|| 2, the feature point distance residual ∑||lidar|| 2 Construct residual function min (x) {PP+∑||imu|| 2 +∑||odom|| 2 +∑||lidar|| 2}, where PP is the marginalized prior information.

[0023] In one embodiment, preprocessing the GNSS data includes:

[0024] removing noise data from the GNSS data using Doppler shift measurements;

[0025] Acquire latitude, longitude and height data according to the GNSS, and convert the latitude, longitude and height data into three-dimensional data in the ECEF coordinate system;

[0026] Constructing the ENU coordinate system of the GNSS, and calculating the transformation matrix for converting the three-dimensional data in the ECEF coordinate system to the ENU coordinate system;

[0027] The coordinates of the GNSS data after the noise data is removed in the ENU coordinate system are calculated.

[0028] In one embodiment, constructing an objective function based on the preprocessed GNSS data and the local laser radar odometer trajectory data includes:

[0029] According to the timestamp, searching for the correspondence between the local laser radar odometer trajectory data and the preprocessed GNSS data;

[0030] An objective function is constructed according to the corresponding relationship.

[0031] In one embodiment, adjusting parameters according to the objective function until initialization is completed includes:

[0032] Verifying the preprocessed GNSS data and the local lidar odometer trajectory data according to the objective function;

[0033] If the verification is successful, the initialization is successful; if the verification is not successful, some pre-processed GNSS data are removed and the verification is performed again until the initialization is completed.

[0034] A GNSS-SLAM initialization system, the system comprising:

[0035] The calibration module is used to calibrate the GNSS, wheel odometer, and lidar using the IMU coordinate system as the body coordinate system, and obtain the relative position and posture relationship after calibration;

[0036] A data conversion module, used to obtain IMU data, GNSS data, wheel odometer data, and laser radar data, and convert the wheel odometer data and the laser radar data into the IMU coordinate system according to the relative posture relationship;

[0037] A data calculation module, used for obtaining local laser radar odometer trajectory data according to the IMU data, the wheel odometer data and the laser radar data in the IMU coordinate system;

[0038] A function construction module, used for preprocessing the GNSS data and constructing an objective function according to the preprocessed GNSS data and the local laser radar odometer trajectory data;

[0039] The initialization module is used to adjust parameters according to the objective function until the initialization is completed.

[0040] A mobile robot comprises a memory and a processor, wherein the memory stores a computer program, and the processor implements the following steps when executing the computer program:

[0041] Using the IMU coordinate system as the body coordinate system, calibrate the GNSS, wheel odometer, and lidar, and obtain the relative position and posture relationship after calibration;

[0042] Acquire IMU data, GNSS data, wheel odometer data, and lidar data, and convert the wheel odometer data and the lidar data into the IMU coordinate system according to the relative posture relationship;

[0043] In the IMU coordinate system, local laser radar odometer trajectory data is obtained according to the IMU data, the wheel odometer data, and the laser radar data;

[0044] Preprocessing the GNSS data, and constructing an objective function based on the preprocessed GNSS data and the local laser radar odometer trajectory data;

[0045] The parameters are adjusted according to the objective function until initialization is completed.

[0046] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the following steps:

[0047] Using the IMU coordinate system as the body coordinate system, calibrate the GNSS, wheel odometer, and lidar, and obtain the relative position and posture relationship after calibration;

[0048] Acquire IMU data, GNSS data, wheel odometer data, and lidar data, and convert the wheel odometer data and the lidar data into the IMU coordinate system according to the relative posture relationship;

[0049] In the IMU coordinate system, local laser radar odometer trajectory data is obtained according to the IMU data, the wheel odometer data, and the laser radar data;

[0050] Preprocessing the GNSS data, and constructing an objective function based on the preprocessed GNSS data and the local laser radar odometer trajectory data;

[0051] The parameters are adjusted according to the objective function until initialization is completed.

[0052] The above-mentioned GNSS-SLAM initialization method, system, mobile robot and storage medium calibrate GNSS, wheel odometer and laser radar by taking IMU coordinate system as the main body coordinate system, and obtain the relative posture relationship after calibration; obtain IMU data, GNSS data, wheel odometer data and laser radar data, and convert the wheel odometer data and the laser radar data to the IMU coordinate system according to the relative posture relationship; in the IMU coordinate system, obtain local laser radar odometer trajectory data according to the IMU data, the wheel odometer data and the laser radar data; preprocess the GNSS data, and construct the objective function according to the preprocessed GNSS data and the local laser radar odometer trajectory data; adjust the parameters according to the objective function until the initialization is completed. By calibrating the sensor and converting the coordinate system, the local laser radar odometer trajectory data can be fused and aligned with the preprocessed GNSS data, ensuring that GNSS-SLAM is established in the same coordinate system, without the need to use hardware for auxiliary fusion positioning, reducing hardware costs, and improving the robustness and stability of the GNSS-SLAM system. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 An application environment diagram of a GNSS-SLAM initialization method in an embodiment;

[0054] Figure 2 A schematic diagram of a GNSS-SLAM initialization method according to an embodiment of the present invention;

[0055] Figure 3 A schematic diagram of the working principle of GNSS-SLAM initialization in one embodiment;

[0056] Figure 4 A structural block diagram of a GNSS-SLAM initialization system in one embodiment;

[0057] Figure 5 1 is a diagram showing the internal structure of a mobile robot in one embodiment. DETAILED DESCRIPTION

[0058] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0059] The GNSS-SLAM initialization method provided in the embodiment of the present application can be applied to Figure 1 In the application environment shown. Figure 1 As shown, the application environment includes a mobile robot 110. The mobile robot 110 can use the IMU coordinate system as the body coordinate system, calibrate the GNSS, wheel odometer, and laser radar, and obtain the relative posture relationship after calibration; the mobile robot 110 can obtain IMU data, GNSS data, wheel odometer data, and laser radar data, and convert the wheel odometer data and laser radar data to the IMU coordinate system according to the relative posture relationship; in the IMU coordinate system, the mobile robot 110 can obtain local laser radar odometer trajectory data according to the IMU data, wheel odometer data, and laser radar data; the mobile robot 110 can pre-process the GNSS data, and construct the objective function according to the pre-processed GNSS data and the local laser radar odometer trajectory data; the mobile robot 110 can adjust the parameters according to the objective function until the initialization is completed.

[0060] In one embodiment, Figure 2 As shown, a GNSS-SLAM initialization method is provided, comprising the following steps:

[0061] Step 202, using the IMU coordinate system as the main body coordinate system, calibrate the GNSS, wheel odometer, and lidar, and obtain the relative posture relationship after calibration.

[0062] The mobile robot can calibrate various sensors, wherein the sensors may include IMU, GNSS, wheel odometer, and laser radar. In this embodiment, the mobile robot can use the IMU coordinate system as the body coordinate system to calibrate other sensors and obtain the relative posture relationship after calibration.

[0063] Step 204, obtaining IMU data, GNSS data, wheel odometer data, and lidar data, and converting the wheel odometer data and lidar data into the IMU coordinate system according to the relative posture relationship.

[0064] The mobile robot can obtain the data collected by each sensor and convert the data collected by each sensor into the IMU coordinate system for the next step of data processing.

[0065] Step 206, in the IMU coordinate system, obtain local lidar odometer trajectory data based on the IMU data, wheel odometer data, and lidar data.

[0066] In the IMU coordinate system, the mobile robot can fuse the data collected by some sensors to obtain local lidar odometer trajectory data.

[0067] Step 208, preprocessing the GNSS data, and constructing an objective function based on the preprocessed GNSS data and the local lidar odometer trajectory data.

[0068] The mobile robot can pre-process the GNSS data, wherein the pre-processing may include two processes, one process is to clean up the noise data in the GNSS data, and the other process is to transform the matrix and calculate the coordinates of the GNSS data.

[0069] Step 210, adjusting parameters according to the objective function until initialization is completed.

[0070] In this embodiment, the mobile robot uses the IMU coordinate system as the body coordinate system, calibrates the GNSS, wheel odometer, and laser radar, and obtains the relative posture relationship after calibration; obtains IMU data, GNSS data, wheel odometer data, and laser radar data, and converts the wheel odometer data and laser radar data to the IMU coordinate system according to the relative posture relationship; in the IMU coordinate system, obtains local laser radar odometer trajectory data according to the IMU data, wheel odometer data, and laser radar data; preprocesses the GNSS data, and constructs an objective function according to the preprocessed GNSS data and local laser radar odometer trajectory data; adjusts parameters according to the objective function until initialization is completed. By calibrating the sensor and converting the coordinate system, the local laser radar odometer trajectory data can be fused and aligned with the preprocessed GNSS data, ensuring that the GNSS-SLAM is established in the same coordinate system, without the need to use hardware for auxiliary fusion positioning, reducing hardware costs, and improving the robustness and stability of the GNSS-SLAM system.

[0071] In one embodiment, a GNSS-SLAM initialization method provided may also include a process for calibrating each sensor, and the specific process includes: using the IMU coordinate system as the body coordinate system to calibrate the internal parameter data of the wheel odometer and IMU; using the IMU coordinate system as the body coordinate system to calibrate the external parameter data of the wheel odometer, IMU, GNSS, and lidar, that is, the relative posture relationship.

[0072] In one embodiment, a GNSS-SLAM initialization method provided may also include a process of obtaining local lidar odometer trajectory data, and the specific process includes: fusing IMU data and wheel odometer data in the IMU coordinate system to obtain relative motion; correcting the lidar data for distortion based on the relative motion to obtain corrected lidar data; and obtaining local lidar odometer trajectory data based on the IMU data, wheel odometer data, and corrected lidar data.

[0073] In one embodiment, the specific process of calculating the local laser radar odometer trajectory data includes: constructing an inter-frame IMU pre-integration residual ∑||imu|| according to the IMU data 2 ; Construct the inter-frame physical odometer domain integral residual ∑||odom|| based on the wheel odometer data 2 ; Construct feature point distance residual ∑||lidar|| based on the corrected lidar data 2 ; According to the IMU pre-integration residual ∑||imu|| 2 , physical odometry domain integral residual ∑||odom|| 2 , feature point distance residual ∑||lidar|| 2 Construct residual function min (x) {PP+∑||imu|| 2 +∑||odom|| 2 +∑||lidar|| 2}, where PP is the marginalized prior information.

[0074] In one embodiment, a GNSS-SLAM initialization method provided may also include a process of preprocessing GNSS data, the specific process including: using Doppler frequency shift measurement to remove noise data in GNSS data; obtaining longitude and latitude data according to GNSS, and converting the longitude and latitude data into three-dimensional data in the ECEF coordinate system; constructing the ENU coordinate system of GNSS, and calculating the transformation matrix of converting the three-dimensional data in the ECEF coordinate system to the ENU coordinate system; calculating the coordinates of the GNSS data in the ENU coordinate system after removing the noise data.

[0075] In this embodiment, the transformation matrix is ​​calculated using the latitude and longitude of the first GNSS data and the three-dimensional coordinates in the ECEF coordinate system; the calculation formula is:

[0076] Among them, x 0 ,y 0 ,z 0The first valid GNSS latitude and longitude is converted to a 3D point under ECEF; longitude and latitude are the longitude of GNSS, and latitude rx(*) and rz(*) are axis-angle conversions, rotating around the x and z axes respectively.

[0077] In this embodiment, the calculation formula for calculating the three-dimensional coordinates in the ECEF coordinate system and the three-dimensional coordinates in the ENU coordinate system is: ENU =TP ECEF , where P ENU ,P ECEF is a three-dimensional point in two coordinate systems.

[0078] In one embodiment, a GNSS-SLAM initialization method provided may also include a process of constructing a magic table function, the specific process including: searching for the correspondence between local lidar odometer trajectory data and preprocessed GNSS data according to the timestamp; and constructing an objective function according to the correspondence.

[0079] In this embodiment, if Figure 3 As shown in the figure, according to the timestamp, the correspondence between the local lidar odometer trajectory number and the preprocessed GNSS data is found, and the objective function is constructed based on the correspondence: Among them, N is the number of corresponding point pairs, i is the index value, P Gi ,P ENUi For the corresponding point pairs, is the transformation matrix of GNSS and IMU, is the transformation to be solved.

[0080] In one embodiment, a GNSS-SLAM initialization method provided may also include a process of determining whether to initialize, and the specific process includes: verifying the preprocessed GNSS data and local lidar odometer trajectory data according to the objective function; if the verification passes, the initialization is successful; if the verification fails, then part of the preprocessed GNSS data is removed and verification is performed again until the initialization is completed.

[0081] When the GNSS data involved in the optimization reaches a certain value, the optimization solution can be started. Solution After that, the preprocessed GNSS data and local LiDAR odometer trajectory data point pairs can be cached and verified. When the residual is less than the threshold, the initialization is successful; otherwise, some data participating in the optimization are eliminated and the initialization operation is continued until the initialization is successful.

[0082] It should be understood that, although the various steps in the above-mentioned flow chart are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least a part of the steps in the above-mentioned flow chart may include a plurality of sub-steps or a plurality of stages, and these sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these sub-steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the sub-steps or stages of other steps.

[0083] In one embodiment, Figure 4 As shown, a GNSS-SLAM initialization system is provided, including: a calibration module 410, a data conversion module 420, a data calculation module 430, a function construction module 440 and an initialization module 450, wherein:

[0084] The calibration module 410 is used to calibrate the GNSS, wheel odometer, and lidar using the IMU coordinate system as the body coordinate system, and obtain the relative position and posture relationship after calibration;

[0085] The data conversion module 420 is used to obtain IMU data, GNSS data, wheel odometer data, and laser radar data, and convert the wheel odometer data and laser radar data into the IMU coordinate system according to the relative posture relationship;

[0086] The data calculation module 430 is used to obtain local laser radar odometer trajectory data according to the IMU data, the wheel odometer data, and the laser radar data in the IMU coordinate system;

[0087] A function construction module 440 is used to preprocess the GNSS data and construct an objective function based on the preprocessed GNSS data and the local laser radar odometer trajectory data;

[0088] The initialization module 450 is used to adjust parameters according to the objective function until the initialization is completed.

[0089] In one embodiment, the calibration module 410 is also used to calibrate the internal parameter data of the wheel odometer and IMU using the IMU coordinate system as the body coordinate system; and to calibrate the external parameter data of the wheel odometer, IMU, GNSS, and lidar using the IMU coordinate system as the body coordinate system.

[0090] In one embodiment, the data calculation module 430 is also used to fuse the IMU data and the wheel odometer data in the IMU coordinate system to obtain relative motion; perform distortion correction on the lidar data based on the relative motion to obtain corrected lidar data; and obtain local lidar odometer trajectory data based on the IMU data, the wheel odometer data, and the corrected lidar data.

[0091] In one embodiment, the data calculation module 430 is further used to construct an inter-frame IMU pre-integration residual ∑||imu|| according to the IMU data. 2 ; Construct the inter-frame physical odometry domain integral residual Σ||odom|| based on the wheel odometry data 2 ; Construct feature point distance residual ∑||lidar|| based on the corrected lidar data 2 ; According to the IMU pre-integration residual ∑||imu|| 2 , physical odometry domain integral residual ∑||odom|| 2 , feature point distance residual ∑||lidar|| 2 Construct residual function min (x) {PP+∑||imu|| 2 +∑||odom|| 2 +∑||lidar|| 2}, where PP is the marginalized prior information.

[0092] In one embodiment, the function construction module 440 is also used to remove noise data in GNSS data using Doppler frequency shift measurement; obtain longitude and latitude data based on GNSS, and convert the longitude and latitude data into three-dimensional data in the ECEF coordinate system; construct the ENU coordinate system of GNSS, and calculate the transformation matrix for converting the three-dimensional data in the ECEF coordinate system to the ENU coordinate system; calculate the coordinates of the GNSS data in the ENU coordinate system after removing the noise data.

[0093] In one embodiment, the function construction module 440 is further used to find the correspondence between the local lidar odometer trajectory data and the preprocessed GNSS data according to the timestamp; and construct the objective function according to the correspondence.

[0094] In one embodiment, the initialization module 450 is also used to verify the preprocessed GNSS data and local lidar odometer trajectory data according to the objective function; if the verification passes, the initialization is successful; if the verification fails, part of the preprocessed GNSS data is removed and verification is performed again until the initialization is completed.

[0095] In one embodiment, a mobile robot is provided, whose internal structure diagram can be shown as follows: Figure 5As shown. The mobile robot includes a processor, a memory, a network interface, a display screen and an input device connected through a system bus. Among them, the processor of the mobile robot is used to provide computing and control capabilities. The memory of the mobile robot includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the mobile robot is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a GNSS-SLAM initialization method is implemented. The display screen of the mobile robot can be a liquid crystal display screen or an electronic ink display screen, and the input device of the mobile robot can be a touch layer covering the display screen, or a button, trackball or touchpad set on the shell of the mobile robot, or an external keyboard, touchpad or mouse, etc.

[0096] Those skilled in the art will understand that Figure 5 The structure shown in the figure is merely a block diagram of a partial structure related to the scheme of the present application, and does not constitute a limitation on the mobile robot to which the scheme of the present application is applied. The specific mobile robot may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0097] In one embodiment, a mobile robot is provided, including a memory and a processor, wherein a computer program is stored in the memory, and when the processor executes the computer program, the following steps are implemented:

[0098] Using the IMU coordinate system as the body coordinate system, calibrate the GNSS, wheel odometer, and lidar, and obtain the relative position and posture relationship after calibration;

[0099] Obtain IMU data, GNSS data, wheel odometer data, and lidar data, and convert the wheel odometer data and lidar data to the IMU coordinate system based on the relative posture relationship;

[0100] In the IMU coordinate system, the local lidar odometer trajectory data is obtained based on the IMU data, wheel odometer data, and lidar data;

[0101] Preprocess the GNSS data and construct the objective function based on the preprocessed GNSS data and local lidar odometer trajectory data;

[0102] Adjust the parameters according to the objective function until initialization is completed.

[0103] In one embodiment, when the processor executes the computer program, the following steps are also implemented: using the IMU coordinate system as the body coordinate system, calibrating the internal parameter data of the wheel odometer and IMU; using the IMU coordinate system as the body coordinate system, calibrating the external parameter data of the wheel odometer, IMU, GNSS, and lidar.

[0104] In one embodiment, when the processor executes the computer program, the following steps are also implemented: in the IMU coordinate system, the IMU data and the wheel odometer data are fused to obtain relative motion; based on the relative motion, the lidar data is distortion corrected to obtain corrected lidar data; based on the IMU data, the wheel odometer data, and the corrected lidar data, local lidar odometer trajectory data is obtained.

[0105] In one embodiment, when the processor executes the computer program, the following steps are further implemented: constructing an inter-frame IMU pre-integration residual ∑||imu|| according to the IMU data 2 ; Construct the inter-frame physical odometer domain integral residual ∑||odom|| based on the wheel odometer data 2 ; Construct feature point distance residual ∑||lidar|| based on the corrected lidar data 2 ; According to the IMU pre-integration residual ∑||imu|| 2 , physical odometry domain integral residual ∑||odom|| 2 , feature point distance residual ∑||lidar|| 2 Construct residual function min (x) {PP+∑||imu|| 2 +∑||odom|| 2 +∑||lidar|| 2}, where PP is the marginalized prior information.

[0106] In one embodiment, when the processor executes the computer program, the following steps are also implemented: using Doppler frequency shift measurement to remove noise data in GNSS data; obtaining longitude and latitude data based on GNSS, and converting the longitude and latitude data into three-dimensional data in the ECEF coordinate system; constructing the ENU coordinate system of GNSS, and calculating the transformation matrix for converting the three-dimensional data in the ECEF coordinate system to the ENU coordinate system; calculating the coordinates of the GNSS data in the ENU coordinate system after removing the noise data.

[0107] In one embodiment, when the processor executes the computer program, the following steps are also implemented: according to the timestamp, searching for the correspondence between the local lidar odometer trajectory data and the preprocessed GNSS data; and constructing the objective function according to the correspondence.

[0108] In one embodiment, when the processor executes the computer program, the following steps are also implemented: the preprocessed GNSS data and the local lidar odometer trajectory data are verified according to the objective function; if the verification passes, the initialization is successful; if the verification fails, some of the preprocessed GNSS data are removed and the verification is performed again until the initialization is completed.

[0109] In one embodiment, a computer readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the following steps are implemented:

[0110] Using the IMU coordinate system as the body coordinate system, calibrate the GNSS, wheel odometer, and lidar, and obtain the relative position and posture relationship after calibration;

[0111] Obtain IMU data, GNSS data, wheel odometer data, and lidar data, and convert the wheel odometer data and lidar data to the IMU coordinate system based on the relative posture relationship;

[0112] In the IMU coordinate system, the local lidar odometer trajectory data is obtained based on the IMU data, wheel odometer data, and lidar data;

[0113] Preprocess the GNSS data and construct the objective function based on the preprocessed GNSS data and local lidar odometer trajectory data;

[0114] Adjust the parameters according to the objective function until initialization is completed.

[0115] In one embodiment, when the computer program is executed by the processor, the following steps are also implemented: using the IMU coordinate system as the body coordinate system, calibrating the internal parameter data of the wheel odometer and IMU; using the IMU coordinate system as the body coordinate system, calibrating the external parameter data of the wheel odometer, IMU, GNSS, and lidar.

[0116] In one embodiment, when the computer program is executed by the processor, the following steps are also implemented: in the IMU coordinate system, the IMU data and the wheel odometer data are fused to obtain relative motion; based on the relative motion, the lidar data is distortion corrected to obtain corrected lidar data; based on the IMU data, the wheel odometer data, and the corrected lidar data, local lidar odometer trajectory data is obtained.

[0117] In one embodiment, when the computer program is executed by the processor, the following steps are further implemented: constructing an inter-frame IMU pre-integration residual ∑||imu|| according to the IMU data 2 ; Construct the inter-frame physical odometer domain integral residual ∑||odom|| based on the wheel odometer data 2; Construct feature point distance residual ∑||lidar|| based on the corrected lidar data 2 ; According to the IMU pre-integration residual ∑||imu|| 2 , physical odometry domain integral residual ∑||odom|| 2 , feature point distance residual ∑||lidar|| 2 Construct residual function min (x) {PP+∑||imu|| 2 +∑||odom|| 2 +∑||lidar|| 2}, where PP is the marginalized prior information.

[0118] In one embodiment, when the computer program is executed by the processor, the following steps are also implemented: using Doppler frequency shift measurement to remove noise data in GNSS data; obtaining longitude and latitude data based on GNSS, and converting the longitude and latitude data into three-dimensional data in the ECEF coordinate system; constructing the ENU coordinate system of GNSS, and calculating the transformation matrix for converting the three-dimensional data in the ECEF coordinate system to the ENU coordinate system; calculating the coordinates of the GNSS data in the ENU coordinate system after removing the noise data.

[0119] In one embodiment, when the computer program is executed by the processor, the following steps are also implemented: according to the timestamp, searching for the correspondence between the local lidar odometer trajectory data and the preprocessed GNSS data; and constructing the objective function according to the correspondence.

[0120] In one embodiment, when the computer program is executed by the processor, the following steps are also implemented: the preprocessed GNSS data and local lidar odometer trajectory data are verified according to the objective function; if the verification passes, the initialization is successful; if the verification fails, part of the preprocessed GNSS data is removed and verification is performed again until the initialization is completed.

[0121] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0122] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0123] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A GNSS-SLAM initialization method, characterized in that: The method comprises: Using the IMU coordinate system as the body coordinate system, calibrate the GNSS, wheel odometer, and lidar, and obtain the relative position and posture relationship after calibration; Acquire IMU data, GNSS data, wheel odometer data, and lidar data, and convert the wheel odometer data and the lidar data into the IMU coordinate system according to the relative posture relationship; In the IMU coordinate system, local laser radar odometer trajectory data is obtained according to the IMU data, the wheel odometer data, and the laser radar data, including: in the IMU coordinate system, the IMU data and the wheel odometer data are fused to obtain relative motion; according to the relative motion, the laser radar data is subjected to distortion correction to obtain corrected laser radar data; local laser radar odometer trajectory data is obtained according to the IMU data, the wheel odometer data, and the corrected laser radar data; Preprocessing the GNSS data, and constructing an objective function based on the preprocessed GNSS data and the local laser radar odometer trajectory data; The parameters are adjusted according to the objective function until initialization is completed.

2. The GNSS-SLAM initialization method according to claim 1, characterized in that: Using the IMU coordinate system as the main coordinate system, calibrate the GNSS, wheel odometer, and lidar, including: Using the IMU coordinate system as the body coordinate system, calibrating the internal reference data of the wheel odometer and the IMU; The IMU coordinate system is used as the body coordinate system to calibrate the external parameter data of the wheel odometer, the IMU, the GNSS, and the lidar.

3. The GNSS-SLAM initialization method according to claim 1, characterized in that: Obtaining local laser radar odometer trajectory data according to the IMU data, the wheel odometer data, and the corrected laser radar data, including: Construct the inter-frame IMU pre-integration residual ∑||imu|| according to the IMU data 2 ; According to the wheel odometer data, the inter-frame physical odometer domain integral residual ∑||odom|| 2 ; Construct the feature point distance residual ∑||lidar|| based on the corrected lidar data 2 ; According to the IMU pre-integration residual ∑||imu|| 2 , the physical odometer domain integral residual ∑||odom|| 2 , the feature point distance residual ∑||lidar|| 2 Construct residual function min (x) {PP+∑||imu|| 2 +∑||odom|| 2 +∑||lidar|| 2 }, where PP is the marginalized prior information.

4. The GNSS-SLAM initialization method according to claim 1, characterized in that: Preprocessing the GNSS data includes: removing noise data from the GNSS data using Doppler shift measurements; Acquire latitude and longitude data according to the GNSS, and convert the latitude and longitude data into three-dimensional data in the ECEF coordinate system; Constructing the ENU coordinate system of the GNSS, and calculating the transformation matrix for converting the three-dimensional data in the ECEF coordinate system to the ENU coordinate system; The coordinates of the GNSS data after the noise data is removed in the ENU coordinate system are calculated.

5. The GNSS-SLAM initialization method according to claim 1, characterized in that: Constructing an objective function based on the preprocessed GNSS data and the local laser radar odometer trajectory data, including: According to the timestamp, searching for the correspondence between the local laser radar odometer trajectory data and the preprocessed GNSS data; An objective function is constructed according to the corresponding relationship.

6. The GNSS-SLAM initialization method according to claim 1, characterized in that: Adjust parameters according to the objective function until initialization is completed, including: Verifying the preprocessed GNSS data and the local laser radar odometer trajectory data according to the objective function; If the verification is successful, the initialization is successful; if the verification is not successful, some pre-processed GNSS data are removed and the verification is performed again until the initialization is completed.

7. A GNSS-SLAM initialization system, characterized in that: The system comprises: The calibration module is used to calibrate the GNSS, wheel odometer, and lidar using the IMU coordinate system as the body coordinate system, and obtain the relative position and posture relationship after calibration; A data conversion module, used to obtain IMU data, GNSS data, wheel odometer data, and laser radar data, and convert the wheel odometer data and the laser radar data into the IMU coordinate system according to the relative posture relationship; A data calculation module is used to obtain local laser radar odometer trajectory data according to the IMU data, the wheel odometer data, and the laser radar data in the IMU coordinate system, including: fusing the IMU data and the wheel odometer data in the IMU coordinate system to obtain relative motion; performing distortion correction on the laser radar data according to the relative motion to obtain corrected laser radar data; and obtaining local laser radar odometer trajectory data according to the IMU data, the wheel odometer data, and the corrected laser radar data; A function construction module, used for preprocessing the GNSS data and constructing an objective function according to the preprocessed GNSS data and the local laser radar odometer trajectory data; The initialization module is used to adjust parameters according to the objective function until the initialization is completed.

8. A mobile robot comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images