CN118707542A - Self-positioning method, system, electronic device and storage medium for mobile robot - Google Patents

Abstract

The present application discloses a self-positioning method, system, electronic device and storage medium for a mobile robot, and the technical field to which it belongs is robot automation technology. The self-positioning method for a mobile robot includes: initializing an inertial measurement unit and recording inertial measurement data; interpolating the inertial measurement data to obtain a first pose conversion matrix, and using the first pose conversion matrix to perform coordinate transformation on the point cloud data; performing inter-frame matching on the point cloud data to obtain a point cloud feature matching result; pre-integrating the inertial measurement data between adjacent key frames to obtain a pre-integration result; performing ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data; constructing a factor graph optimization model, and using the factor graph optimization model to globally optimize the posture information of the mobile robot, so as to obtain the positioning information of the mobile robot in the world coordinate system. The present application can improve the self-positioning accuracy of the mobile robot.

Description

Self-positioning method, system, electronic device and storage medium for mobile robot

Technical Field

The present application relates to the field of robot automation technology, and in particular to a self-positioning method, system, electronic device and storage medium for a mobile robot.

Background Art

Mobile robots need to locate themselves before performing tasks in unfamiliar environments, and the accuracy of positioning directly affects the accuracy of map construction and the execution of tasks. Sensors commonly used for positioning and map construction are cameras or lidars. The solution using camera positioning can build an odometer through image information, but the map it builds is difficult to intuitively reflect the structure of the environment. The solution using lidar positioning can achieve positioning through point cloud registration, and at the same time establish a dense point cloud map to truly reflect the surrounding environment. Using lidar alone, relying on point cloud matching between frames, a simple lidar odometer can be constructed, but because the lidar itself has errors, the errors accumulate over time, and the odometer often fails in situations where it is difficult to extract feature points, such as long corridors.

Therefore, how to improve the self-positioning accuracy of the mobile robot is a technical problem that technicians in this field currently need to solve.

Summary of the invention

The purpose of the present application is to provide a self-positioning method, system, electronic device and storage medium for a mobile robot, which can improve the self-positioning accuracy of the mobile robot.

In order to solve the above technical problems, the present application provides a self-positioning method for a mobile robot, wherein the mobile robot is a robot provided with a laser radar, an inertial measurement unit and a GPS positioning module, and the self-positioning method for the mobile robot comprises:

Initializing the inertial measurement unit and recording the measurement value of the inertial measurement unit to obtain inertial measurement data;

Receive the point cloud data collected by the laser radar, interpolate the inertial measurement data according to the timestamp of the point cloud data to obtain a first pose transformation matrix, and use the first pose transformation matrix to perform coordinate transformation on the point cloud data; wherein the first pose transformation matrix is used to describe the pose change of the point cloud data at the collection time relative to the starting time;

Determine feature points in the point cloud data, and perform frame matching on the point cloud data according to the feature points to obtain a point cloud feature matching result;

Selecting key frames from the inertial measurement data, and pre-integrating the inertial measurement data between adjacent key frames to obtain a pre-integration result; wherein the key frames are data frames collected simultaneously from the point cloud data and the inertial measurement data;

Performing ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data;

Constructing a factor graph optimization model according to the point cloud feature matching result, the pre-integration result and the ENU coordinate data;

If a motion closed loop is detected, the factor graph optimization model is used to globally optimize the posture information of the mobile robot so as to obtain the positioning information of the mobile robot in the world coordinate system.

Optionally, interpolating the inertial measurement data according to the timestamp of the point cloud data to obtain a first pose conversion matrix includes:

Select the nth frame of point cloud data;

Determine the timestamp of the nth frame of point cloud data;

The two frames of inertial measurement data closest to the timestamp of the nth frame of point cloud data are set as reference inertial measurement data;

Generate a second posture conversion matrix corresponding to the reference inertial measurement data; wherein the second posture conversion matrix is a posture conversion matrix of the collection time of the reference inertial measurement data relative to the starting time;

Performing interpolation processing on the second pose conversion matrix to obtain the first pose conversion matrix of the nth frame point cloud data;

Determine whether the point cloud data has been selected; if not, add 1 to the value of n and enter the step of selecting the nth frame of point cloud data.

Optionally, determining feature points in the point cloud data includes:

Calculating the curvature of each point in the point cloud data;

Edge points and plane points are selected from the point cloud data as the feature points according to the curvature; wherein the edge points are P1 points with the largest curvature in the point cloud data, and the plane points are P2 points with the smallest curvature in the point cloud data.

Optionally, performing inter-frame matching on the point cloud data according to the feature points to obtain a point cloud feature matching result includes:

Performing point-to-line feature matching according to the coordinates of the edge points to obtain a first distance; wherein the first distance is the distance from the edge point in the k+1th frame of point cloud data to the target straight line, the target straight line is a straight line passing through two edge points in the kth frame of point cloud data, and the first distance is greater than 0;

Performing point-to-plane feature matching according to the coordinates of the plane point to obtain a second distance; wherein the second distance is the distance from the plane point in the k+1th frame point cloud data to the target plane, the target plane is the plane where the three non-collinear plane points in the kth frame point cloud data are located, and the second distance is greater than 0;

Determine the coordinate transformation matrix of the point cloud of adjacent frames;

Generate an edge point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the first distance;

Generate a plane point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the second distance;

Taking the minimum value of the first distance as the optimization goal, the edge point constraint equation and the plane point constraint equation are solved to obtain a point cloud feature matching result.

Optionally, performing ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data includes:

Convert the measured value of the GPS positioning module to the ECEF coordinate system to obtain ECEF coordinate data;

The ECEF coordinate data is converted to the ENU coordinate system to obtain the ENU coordinate data.

Furthermore, it also includes:

Determine comparison information between a current frame and the key frame in the point cloud data collected by the laser radar;

Determining whether the comparison information meets a preset condition;

If so, it is determined that there is a motion closed loop;

If not, it is determined that there is no motion closed loop.

Further, determining the comparison information between the current frame and the key frame in the point cloud data collected by the laser radar includes:

Calculating the time difference between the current frame and the key frame in the point cloud data collected by the laser radar to obtain first comparison information;

and/or, calculating the length of a motion trajectory between a current frame and a key frame in the point cloud data collected by the laser radar to obtain second comparison information;

and/or, calculating a straight-line distance between a current frame collected by the laser radar and the key frame to obtain third comparison information;

And/or, calculate the point cloud registration scores of the current frame and the key frame in the point cloud data collected by the laser radar to obtain fourth comparison information.

The present application also provides a self-positioning system for a mobile robot, wherein the mobile robot is a robot provided with a laser radar, an inertial measurement unit and a GPS positioning module, and the self-positioning system for the mobile robot comprises:

A first data acquisition module, used for initializing the inertial measurement unit and recording the measurement value of the inertial measurement unit to obtain inertial measurement data;

A second data acquisition module is used to receive the point cloud data collected by the laser radar, interpolate the inertial measurement data according to the timestamp of the point cloud data to obtain a first pose conversion matrix, and use the first pose conversion matrix to perform coordinate transformation on the point cloud data; wherein the first pose conversion matrix is used to describe the pose change of the point cloud data at the collection time relative to the starting time;

An inter-frame matching module is used to determine feature points in the point cloud data, and perform inter-frame matching on the point cloud data according to the feature points to obtain a point cloud feature matching result;

A pre-integration module, used to select a key frame from the inertial measurement data, pre-integrate the inertial measurement data between adjacent key frames, and obtain a pre-integration result; wherein the key frame is a data frame collected simultaneously from the point cloud data and the inertial measurement data;

A third data acquisition module is used to perform ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data;

A model building module, used to build a factor graph optimization model according to the point cloud feature matching result, the pre-integration result and the ENU coordinate data;

The optimization module is used to use the factor graph optimization model to globally optimize the posture information of the mobile robot if a motion closed loop is detected, so as to obtain the positioning information of the mobile robot in the world coordinate system.

The present application also provides a storage medium on which a computer program is stored, and when the computer program is executed, the steps of the self-positioning method of the mobile robot are implemented.

The present application also provides an electronic device, including a memory and a processor, wherein the memory stores a computer program, and when the processor calls the computer program in the memory, the steps of the self-positioning method of the mobile robot are implemented.

The present application provides a self-positioning method for a mobile robot, which is applied to a mobile robot equipped with a laser radar, an inertial measurement unit and a GPS positioning module. The present application records the inertial measurement data collected by the inertial measurement unit after initializing the inertial measurement unit, and uses the inertial measurement data to perform difference processing and coordinate transformation on the point cloud data collected by the laser radar so as to correct the point cloud distortion caused by the laser radar during the movement of the mobile robot. The present application also performs inter-frame matching on the point cloud data to obtain point cloud feature matching results so as to accurately estimate the relative position change of the mobile robot. The present application determines the key frames collected simultaneously based on the point cloud data and the inertial measurement data, and then performs pre-integration calculation based on the key frames so as to reduce the amount of calculation of the inertial measurement data during the self-positioning process and maintain continuous tracking of the robot's motion state. The present application obtains ENU coordinate data by converting the measurement value of the GPS positioning module through ENU coordinates, so that the measurement value of the GPS positioning module can be matched with the local coordinate system of the robot, providing a global reference for positioning. This application constructs a factor graph optimization model based on point cloud feature matching results, pre-integration results and ENU coordinate data, integrates different sensor data, and performs global optimization when a motion closed loop is detected, so as to eliminate cumulative errors and improve positioning accuracy and stability. It can be seen that this application can improve the self-positioning accuracy of the mobile robot by fusing the data of the laser radar, inertial measurement unit and GPS positioning module for positioning. This application also provides a self-positioning system for a mobile robot, a storage medium and an electronic device, which have the above-mentioned beneficial effects and will not be repeated here.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the embodiments of the present application, the following is a brief introduction to the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

FIG1 is a flow chart of a self-positioning method for a mobile robot provided in an embodiment of the present application;

FIG2 is a flow chart of a self-positioning method based on multi-sensor information fusion provided in an embodiment of the present application;

FIG3 is a schematic diagram of a laser single-frame scanning provided in an embodiment of the present application;

FIG4 is a schematic diagram of solving a transformation matrix using an IMU interpolation method provided in an embodiment of the present application;

FIG5 is a factor graph of SLAM positioning provided by an embodiment of the present application;

FIG6 is a schematic diagram of a factor graph model provided in an embodiment of the present application;

FIG. 7 is a flow chart of a loop detection provided in an embodiment of the present application.

DETAILED DESCRIPTION

In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

Please refer to Figure 1 below, which is a flow chart of a self-positioning method for a mobile robot provided in an embodiment of the present application.

Specific steps may include:

S101: Initializing the inertial measurement unit and recording the measurement value of the inertial measurement unit to obtain inertial measurement data;

Among them, this embodiment can be used for a processor of a mobile robot, which is a robot equipped with a laser radar, an inertial measurement unit IMU and a GPS positioning module. This embodiment integrates the positioning information collected by the laser radar, the inertial measurement unit and the GPS positioning module to realize the self-positioning of the mobile robot. In the implementation process of this embodiment, the laser radar, the inertial measurement unit IMU and the GPS positioning module can be used to continuously collect corresponding positioning information.

In this embodiment, the inertial measurement unit may be initialized first, and then the initialized inertial measurement unit may be used to measure the motion state of the mobile machine to obtain inertial measurement data (including acceleration data and angular velocity data).

After the inertial measurement unit is initialized, the present embodiment may also use a cubic B-spline curve to fit the measurement value of the inertial measurement unit to obtain inertial measurement data, so as to fit the smoothly changing part in the inertial measurement data and suppress the influence of noise and abnormal values.

S102: receiving point cloud data collected by the laser radar, interpolating the inertial measurement data according to the timestamp of the point cloud data to obtain a first pose transformation matrix, and performing coordinate transformation on the point cloud data using the first pose transformation matrix;

Among them, the laser radar can periodically emit laser beams to detect the surrounding environment and obtain multi-frame point cloud data. Point cloud data is a collection of points in three-dimensional space, and each point contains its spatial coordinates (X, Y, Z).

Point cloud data has a timestamp, which indicates the time when the data was collected. The timestamp of the same frame of point cloud data is the same. Since the sampling frequency of the inertial measurement unit is higher than that of the lidar, there may be multiple frames of inertial measurement data between two consecutive frames of point cloud data.

According to the inertial measurement data, the posture conversion matrix of the current moment relative to the starting moment can be determined. The present application can query the inertial measurement data before and after the point cloud data collection time according to the timestamp of the point cloud data, and interpolate the above inertial measurement data to obtain the first posture conversion matrix. The first posture conversion matrix is used to describe the posture change of the mobile robot at the time of collecting the point cloud data relative to the starting moment. The above starting moment refers to: the moment when the mobile robot starts the laser radar and the inertial measurement unit to collect positioning information.

After obtaining the first pose conversion matrix, the first pose conversion matrix can be used to perform coordinate transformation on the point cloud data to eliminate point cloud distortion caused by the laser radar during the movement of the mobile robot. The point cloud data used in subsequent operations of this embodiment is the point cloud data after the coordinate transformation operation in S102.

S103: determining feature points in the point cloud data, and performing inter-frame matching on the point cloud data according to the feature points to obtain a point cloud feature matching result;

In this embodiment, points with obvious features in the point cloud data can be extracted as feature points, and the point cloud data can be matched between frames according to the feature points to obtain point cloud feature matching results. The point cloud feature matching results are used to describe the conversion relationship between two adjacent frames of point cloud data.

As a feasible implementation method, this embodiment can determine the feature points in the point cloud data based on the curvature of each point. Inter-frame matching refers to the process of aligning point cloud data at two different times. This embodiment can achieve inter-frame matching by comparing the positions of feature points in two adjacent frames of point cloud data.

S104: selecting key frames from the inertial measurement data, and pre-integrating the inertial measurement data between adjacent key frames to obtain a pre-integration result;

Among them, the laser radar and the inertial measurement unit can collect data according to their respective cycles. In this embodiment, the data frame collected simultaneously with the point cloud data and the inertial measurement data (that is, the point cloud data and the inertial measurement data with the same timestamp) is used as a key frame. In this embodiment, the key frame selected from the inertial measurement data specifically refers to: a frame of inertial measurement data collected simultaneously with any frame of point cloud data.

Inertial measurement data is continuously generated at a high frequency (e.g., 100 Hz to 1000 Hz), while point cloud data is usually generated at a lower frequency (e.g., 10 Hz). The purpose of pre-integration is to integrate the inertial measurement data between two keyframes to estimate the relative pose change between the two keyframes. The pre-integration result is the pose change information between two adjacent keyframes.

By pre-integrating the inertial measurement data between key frames, repeated integration calculations can be avoided during the optimization process. The pre-integration results are robust to noise and bias in the inertial measurement data to improve positioning accuracy.

S105: performing ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data;

The measurement value of the GPS positioning module can include the longitude, latitude and altitude information of the WGS84 coordinate system. The WGS84 coordinate system takes the center of the earth as the origin and uses longitude, latitude and geodetic height to describe the position of any point on the earth. The ENU coordinate system is a local coordinate system with the location of the mobile robot as the origin, the X axis pointing to the east, the Y axis pointing to the north, and the Z axis pointing to the zenith.

This embodiment can determine the coordinate system conversion matrix between the WGS84 coordinate system and the ENU coordinate system, and then use the coordinate system conversion matrix to perform ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data. The ENU coordinate system is the northeast celestial coordinate system.

S106: constructing a factor graph optimization model according to the point cloud feature matching result, the pre-integration result and the ENU coordinate data;

The factor graph is a probabilistic graph model used to represent and optimize complex functions containing multiple variables. The factor graph includes the point cloud feature matching factor (i.e., odometer factor) corresponding to the point cloud feature matching result, the pre-integration factor (i.e., loop detection factor) corresponding to the pre-integration result, and the GPS factor corresponding to the ENU coordinate data.

The point cloud feature matching result is used to describe the transformation relationship between two adjacent frames of point cloud data (i.e., the relative pose change between adjacent frames of the robot). The point cloud feature matching result can be used as an edge in the factor graph to connect the nodes representing the robot pose at different times. The pre-integration result is the pose change information between two adjacent key frames, which provides the robot pose estimation based on inertial measurement data. The pre-integration result can also be used as an edge in the factor graph. The ENU coordinate data provides the absolute position of the robot in the global coordinate system. The ENU coordinate data can be used as a constraint in the factor graph.

The factor graph optimization model is a model that uses a factor graph structure for state estimation and can integrate information from different sensors. The goal of the factor graph optimization model is to find a set of variable values that maximizes the joint probability of all factors (i.e., point cloud feature matching factors, pre-integration factors, and GPS factors) or minimizes the cost function. When a motion loop is detected, constraints can be added to the factor graph. These factors connect nodes that represent the same position at different time points and provide an opportunity to eliminate cumulative errors. This embodiment constructs a factor graph optimization model based on point cloud feature matching results, pre-integration results, and ENU coordinate data, which can fuse different sensor data into a unified probability framework.

S107: If a motion closed loop is detected, the factor graph optimization model is used to perform global optimization on the posture information of the mobile robot so as to obtain the positioning information of the mobile robot in the world coordinate system.

Among them, after the motion closed loop is detected, the closed-loop factor corresponding to the closed-loop constraint and the point cloud feature matching factor, pre-integration factor and GPS factor in the factor graph optimization model can be converted into an optimization problem, such as a nonlinear least squares problem. In this embodiment, an optimization algorithm can be used to minimize the total error of all factors, and the state of the node is adjusted until the optimal pose estimate that minimizes all observation errors is found. After the global optimization of the pose information of the mobile robot is completed, the estimated values of all pose nodes can be used, and the updated pose information is the positioning information of the robot in the world coordinate system. Through the above scheme, the cumulative errors of various sensors can be corrected, and the map consistency is enhanced at the same time.

This embodiment is applied to a mobile robot provided with a laser radar, an inertial measurement unit and a GPS positioning module. This embodiment records the inertial measurement data collected by the inertial measurement unit after initializing the inertial measurement unit, and uses the inertial measurement data to perform difference processing and coordinate transformation on the point cloud data collected by the laser radar so as to eliminate the point cloud distortion caused by the laser radar during the movement of the mobile robot. This embodiment also performs inter-frame matching on the point cloud data to obtain point cloud feature matching results so as to accurately estimate the relative position change of the mobile robot. This embodiment determines the key frames collected simultaneously based on the point cloud data and the inertial measurement data, and then performs pre-integration calculation based on the key frames so as to reduce the amount of calculation of the inertial measurement data during the self-positioning process and maintain continuous tracking of the robot's motion state. This embodiment obtains ENU coordinate data by converting the measurement value of the GPS positioning module through ENU coordinates, so that the measurement value of the GPS positioning module can be matched with the local coordinate system of the robot, providing a global reference for positioning. This embodiment constructs a factor graph optimization model based on the point cloud feature matching results, pre-integration results and ENU coordinate data, integrates different sensor data, and performs global optimization when a motion closed loop is detected to eliminate cumulative errors and improve positioning accuracy and stability. It can be seen that this embodiment can improve the self-positioning accuracy of the mobile robot by fusing the data of the laser radar, inertial measurement unit and GPS positioning module for positioning.

As a further introduction to the embodiment corresponding to FIG. 1 , this embodiment can determine the first pose transformation matrix of each frame of point cloud data, so as to utilize the first pose transformation matrix to perform coordinate transformation on the corresponding point cloud data to eliminate point cloud distortion.

Specifically, this embodiment can determine the first pose transformation matrix of each frame of point cloud data in the following manner:

Step A1: Select the nth frame of point cloud data.

When step A1 is executed for the first time in this embodiment, the value of n is 1.

Step A2: Determine the timestamp of the nth frame of point cloud data.

Step A3: setting the two frames of inertial measurement data closest to the timestamp of the n-th frame of point cloud data as reference inertial measurement data.

The timestamp of the nth frame of point cloud data is _tn , and the reference inertial measurement data selected in this step includes the inertial measurement data with a timestamp earlier than _tn and the inertial measurement data with a timestamp later than _{tn and} the inertial measurement data with a timestamp closest to _tn .

Step A4: generating a second posture conversion matrix corresponding to the reference inertial measurement data;

The second posture conversion matrix is a posture conversion matrix of the collection time of the reference inertial measurement data relative to the starting time.

Step A5: interpolate the second pose conversion matrix to obtain the first pose conversion matrix of the nth frame point cloud data.

Among them, two second posture transformation matrices can be obtained through step A4. In this embodiment, the second posture transformation matrix can be interpolated according to the timestamp of the reference inertial measurement data to obtain the posture transformation matrix at time _tn , that is, the first posture transformation matrix of the nth frame point cloud data.

Step A6: Determine whether the point cloud data has been selected; if not, add 1 to the value of n and go to step A1.

This embodiment can also determine the total number of frames of point cloud data n _total , and after obtaining the first position transformation matrix of the n _total point cloud data, the loop of step A1 to step A6 can be exited. The above embodiment uses inertial measurement data to compensate for the distortion of point cloud data caused by robot movement during the acquisition process, which can improve the accuracy and consistency of point cloud data.

As a further introduction to the embodiment corresponding to Figure 1, the feature points in the point cloud data can be determined in the following manner: calculate the curvature of each point in the point cloud data; select edge points and plane points from the point cloud data as the feature points based on the curvature; wherein the edge points are the P1 points with the largest curvature in the point cloud data, and the plane points are the P2 points with the smallest curvature in the point cloud data.

On the basis that the feature points include edge points and plane points, the point cloud feature matching results can be obtained by performing inter-frame matching in the following ways:

Step B1: performing point-to-line feature matching according to the coordinates of the edge points to obtain a first distance;

The first distance is the distance from the edge point in the k+1th frame of point cloud data to the target straight line, the target straight line is a straight line passing through two edge points in the kth frame of point cloud data, and the first distance is greater than 0;

Step B2: performing point-to-plane feature matching according to the coordinates of the plane point to obtain a second distance;

The second distance is the distance from the plane point in the k+1th frame of point cloud data to the target plane, the target plane is the plane where the three non-collinear plane points in the kth frame of point cloud data are located, and the second distance is greater than 0;

Step B3: Determine the coordinate transformation matrix of the point cloud of adjacent frames;

Step B4: generating an edge point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the first distance;

Step B5: generating a plane point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the second distance;

Step B6: Taking the minimum value of the first distance as the optimization goal, solving the edge point constraint equation and the plane point constraint equation to obtain a point cloud feature matching result.

As a further introduction to the embodiment corresponding to FIG. 1 , the embodiment can perform the following operations on the measurement values of the GPS positioning module to obtain ENU coordinate data: convert the measurement values of the GPS positioning module to the ECEF coordinate system to obtain ECEF coordinate data; convert the ECEF coordinate data to the ENU coordinate system to obtain the ENU coordinate data. The ECEF coordinate system is an earth-centered earth-fixed coordinate system.

As a further introduction to the embodiment corresponding to Figure 1, a factor graph optimization model can be constructed in the following manner, including: generating a first error function corresponding to the point cloud feature matching result; generating a second error function corresponding to the pre-integration result; generating a third error function corresponding to the ENU coordinate data; and constructing a factor graph optimization model including the first error function, the second error function and the third error function.

The above process constructs a factor graph optimization model of three error functions so that the constraints of point cloud feature matching, pre-integration results and ENU coordinate data can be optimized in a unified framework. By solving this optimization problem, an optimal pose estimation that comprehensively considers information from multiple sensors can be obtained.

As a further introduction to the embodiment corresponding to Figure 1, whether a motion loop exists can be determined in the following manner: determine the comparison information between the current frame and the key frame in the point cloud data collected by the laser radar; determine whether the comparison information meets the preset conditions; if so, determine that a motion loop exists; if not, determine that no motion loop exists.

The above comparison information is the result of comparing the current frame in the point cloud data with the key frame in the point cloud data, where the current frame in the point cloud data is the most recently acquired frame of point cloud data, and the key frame in the point cloud data is the data frame in the point cloud data acquired with the inertial measurement data. In other words, the key frame in the point cloud data refers to: a frame of point cloud data acquired simultaneously with any frame of inertial measurement data.

The comparison information includes: any one or a combination of any one of the time difference between the current frame and the key frame, the length of the motion trajectory between the current frame and the key frame, the straight-line distance between the current frame and the key frame, and the point cloud registration score between the current frame and the key frame;

If the time difference between the current frame and the key frame is greater than the first threshold, the comparison information is determined to meet the preset conditions; if the length of the motion trajectory between the current frame and the key frame is greater than the second threshold, the comparison information is determined to meet the preset conditions; if the straight-line distance between the current frame and the key frame is less than the third threshold, the comparison information is determined to meet the preset conditions; if the point cloud alignment score between the current frame and the key frame is less than the fourth threshold, the comparison information is determined to meet the preset conditions.

As a feasible implementation method, this embodiment can calculate the time difference between the current frame and the key frame in the point cloud data collected by the laser radar to obtain the first comparison information; it can also calculate the length of the motion trajectory between the current frame and the key frame in the point cloud data collected by the laser radar to obtain the second comparison information; it can also calculate the straight-line distance between the current frame collected by the laser radar and the key frame to obtain the third comparison information; it can also calculate the point cloud alignment score of the current frame and the key frame in the point cloud data collected by the laser radar to obtain the fourth comparison information; any one or a combination of any several of the first comparison information, the second comparison information, the third comparison information and the fourth comparison information is used as the comparison information for detecting the motion closed loop.

The process described in the above embodiment is explained below through an embodiment in actual application.

In order to improve the accuracy of pose estimation and adaptability in multiple scenarios, multi-sensor fusion solutions are increasingly being adopted. In unknown environments, the task execution of mobile robots depends on accurate self-positioning. The positioning accuracy is directly related to the accuracy of map construction and the execution results of tasks. In order to achieve positioning and map construction, multiple sensors such as lidar, camera, and IMU are usually used. Cameras can generate odometers using image information, but the generated maps are often difficult to directly reflect the structure of the environment. In contrast, lidar can achieve positioning through point cloud registration and simultaneously create dense point cloud maps to truly reflect the characteristics of the surrounding environment. Using a single lidar for positioning usually relies on feature matching between point cloud maps, which can construct a lidar odometer. However, due to the errors of the sensor itself, these errors will accumulate over time, especially in long corridors and other situations where there are no obvious feature points, the lidar odometer often fails. Therefore, in order to ensure the adaptability of mobile robot self-positioning in a variety of different scenarios, more and more studies have adopted multi-sensor fusion methods. This multi-sensor fusion method can effectively overcome the limitations and errors of a single sensor, thereby improving the autonomous positioning and navigation capabilities of mobile robots.

This embodiment proposes a self-positioning method based on multi-sensor information fusion, using laser radar, GPS and IMU as the main sensors to achieve self-positioning and map construction of mobile robots with multi-sensor information fusion. When processing IMU data, a discrete integral model is introduced to derive a pre-integration model, thereby simplifying the integral calculation process of IMU data. By combining the laser radar odometer and the IMU pre-integration model, a factor graph optimization framework is constructed to improve the accuracy and reliability of positioning and map construction. In addition, GPS factors and loop detection factors are introduced to further enhance the accuracy and reliability of multi-sensor information fusion. This embodiment also introduces a loop detection link to optimize the accumulated error and reduce the closed-loop error; by optimizing the residual equation, a high-precision robot pose estimation is obtained. Please refer to Figure 2, which is a flow chart of a self-positioning method based on multi-sensor information fusion provided by an embodiment of the present application. The process is as follows: perform ENU coordinate conversion on the GPS measurement value (i.e., the measurement value of the GPS positioning module), perform IMU initialization and IMU pre-integration on the IMU measurement value (i.e., the measurement value of the inertial measurement unit), perform point cloud preprocessing, feature point extraction, feature matching, key frame detection, loop detection on the original point cloud (i.e., point cloud data), perform factor graph optimization based on the ENU coordinate conversion results of the GPS measurement value, the IMU pre-integration results, the loop detection results, and the feature matching results, and perform global pose optimization based on the factor graph optimization results. The above scheme can also construct a lidar odometer based on the feature matching results.

The self-positioning method based on multi-sensor information fusion provided in this embodiment includes the following steps:

Step C1: Initialize the IMU and store the IMU measurement values into a vector.

Among them, after the IMU is initialized, the angular velocity fed back by the IMU can be fitted by cubic B-spline to obtain the relationship between the change of the IMU angle and time.

Step C2: After receiving the point cloud data from the lidar, the pose transformation matrix is interpolated from the IMU information according to the timestamp of the point, and the point cloud distortion caused by the movement of the mobile robot is removed by the pose transformation matrix.

Please refer to Figure 3, which is a schematic diagram of a laser single-frame scanning provided by an embodiment of the present application, showing the scanning start time and the current scanning time, with x as the horizontal coordinate and y as the vertical coordinate. During each scanning process, the start time is recorded as _ts , and in the world coordinate system, the angle between the scanning start point and the y-axis is recorded as _θs ; at time _tn , the angle between the scanned point and the y-axis is recorded as _θn , and the end of the scan is recorded as _te . The relationship between time and angle can be calculated as follows:

Formula (1);

Please refer to Figure 4, which is a schematic diagram of solving the transformation matrix using an IMU interpolation method provided in an embodiment of the present application. t represents time. The IMU receives posture information at time t _k-1 , t _k , t _k+1 , and t _k+2 respectively. By conversion, the transformation matrix of the posture at each time relative to the posture at the starting time can be obtained. According to formula (1), after obtaining the current scanning time _tn (i.e., the timestamp of the point cloud data), the transformation matrix corresponding to the two most recent times ( _tk and _tk+1 ) of _tn in the IMU data is matched according to the timestamp (i.e., the second pose transformation matrix). and , calculate the transformation matrix at time _tn (i.e., the first pose transformation matrix) by interpolation method, recorded as :

Formula (2);

In Figure 4, _bf represents the time difference between _tn and _tk , and _bb represents the time difference between _tn and _tk+1 .

After obtaining the transformation matrix, the coordinate transformation of the point cloud at each sampling moment is performed according to formula (2) to eliminate the point cloud distortion caused by the laser radar during the movement of the mobile robot.

Step C3: Extract feature points in the point cloud according to the curvature, and obtain the laser radar odometer by inter-frame matching of the feature points.

Among them, this step can calculate the curvature of each point in the point cloud, extract feature points based on the curvature, and perform frame matching through the feature points in each frame of the point cloud image to obtain the laser odometer. Before this step, there can also be operations of denoising and downsampling the point cloud data. Laser odometer is essentially a point cloud registration process. The transformation matrix obtained after registration is the change in posture of the current frame relative to the previous frame.

When calculating the curvature, n points around the target point are selected to form a point set S. The curvature c of the target point is calculated as shown in formula (3). Considering the data processing time and accuracy, n=5 is usually selected.

Formula (3);

Among them, _pi is the coordinate value of the target point, and _pj is the coordinate value of the surrounding points.

If the calculated c value is large, the target point is recorded as an edge point; if the c value is small, it is recorded as a plane point. In each area, any number (such as 2) of edge points with the largest c value and any number (such as 4) of plane points with the smallest c value are selected as feature points.

Assume that the edge point set fitted in the k-th frame point cloud is ε _k and the plane point set is H _k , project the feature points in the k+1-th frame point cloud to the moment when the scan starts, and obtain the edge point set , a plane point set .

When processing edge points, the point-to-line strategy is used for matching: select a point i from the edge point set ε _k+1 of the k+1 frame point cloud, and use the ICP algorithm to find the nearest neighbor point j in ε _k and the nearest neighbor point l of point j on the adjacent scan line to ensure that the three points are not collinear. Then, the distance d (i.e., the first distance) from point i to the straight line (i.e., the line connecting point j and point l, also known as the target straight line) is calculated according to formula (4).

Formula (4);

In the formula, is the coordinate of point i in the k+1th frame mapped to the initial moment of the current frame, X _(k,j) is the coordinate of point j in the kth frame, and X _(k,l) is the coordinate of point l in the kth frame.

When processing planar points, the point-to-plane strategy is used for matching: find point i in the planar point set H _k+1 of the k+1th frame point cloud, select two points j and l adjacent to point i in H _k , and then match the adjacent point m of point j in the adjacent scan line. At this time, the plane formed by the three points j, l, and m (i.e., the target plane) is not coplanar with point i. The distance d _h from point i to the plane (i.e., the second distance) is calculated according to formula (5).

Formula (5);

X _(k,m) is the coordinate of point m in the kth frame.

The position information of the mobile robot in space includes the rotation matrix R and the translation vector , then the coordinate transformation of the corresponding feature points between the two frames of point clouds can be expressed as follows:

Formula (6);

In the above formula represents the coordinates of the feature points in the k+1th frame, represents the coordinates of the feature points in the kth frame, and equation (6) is the coordinate transformation matrix of the point cloud of adjacent frames.

Therefore, the position transformation relationship of point i is as shown in formula (7).

Formula (7);

Where T _(k+1,i) represents the pose matrix of point i at the beginning of the k+1th frame, _ti is the time corresponding to point i, _tk and _tk+1 represent the time when the point cloud scanning ends at the kth frame and the k+1th frame, and _Tk+1 represents the transformation matrix of the point cloud of the k+1th frame relative to the point cloud of the kth frame.

The conversion relationship from the midpoint i of the k+1th frame to the start time of the current frame is:

Formula (8);

represents the coordinates of point i in the k+1th frame to the start time of the current frame, represents the coordinates of point i in the k+1th frame.

In this embodiment, the coordinate transformation matrix of the point cloud of adjacent frames can be converted into equation (8), and the edge point constraint equation can be obtained according to equation (4) and equation (8): :

Formula (9).

In this embodiment, the coordinate transformation matrix of the point cloud of adjacent frames can be converted into equation (8), and the plane point constraint equation can be obtained according to equations (5) and (8): :

Formula (10).

The constraint equation is solved by using the Levenberg-Marquardt method. Taking the minimum distance d obtained in equation (4) as the optimization goal, the conversion relationship between point cloud frames (i.e., point cloud feature matching result) can be obtained:

Formula (11).

In the formula, , represents the Jacobian matrix; λ represents the coefficient factor. T represents the transposed matrix, diag is the function used to construct the diagonal matrix, represents the constraint equation.

Step C4: Pre-integrate the IMU data according to the time corresponding to the lidar key frame.

IMU uses the internal accelerometer and gyroscope to measure the acceleration and speed of the mobile robot, and integrates the posture transformation at each moment. Assuming that in the world coordinate system, the robot motion parameters measured by IMU are velocity v, angular velocity w, acceleration a; the rotation matrix R and translation vector p in the posture matrix, the motion equation is:

Formula (12).

In the above formula represents the first derivative of R; represents the p-first-order derivative; represents the first-order derivative of v; represents the antisymmetric matrix corresponding to the angular velocity w; after time ΔT, the posture transformation is obtained by integrating equation (12):

Formula (13).

In the above formula, t represents the time at a certain moment, and Exp represents the exponential function. The rotation matrix integral result can be obtained through the above formula (13): , speed integration results And the translation vector integral result . , , , , They represent the rotation matrix, angular velocity, velocity, acceleration and translation vector at the corresponding time t respectively, and Δt represents the time differential.

Since the data measured by IMU has certain random errors, assuming that the error obeys Gaussian distribution, the IMU measurement error model is expressed as:

Formula (14).

In the formula, is the angular velocity in the IMU measurement, is the acceleration in the IMU measurement, , represents the acceleration bias and Gaussian noise in the gyroscope, , is the zero bias and Gaussian noise in the accelerometer. is the actual angular velocity, is the actual acceleration, is the acceleration due to gravity.

The relationship between the state quantity and the measured value can be deduced from equations (13) and (14):

Formula (15);

In the formula, and Represents discrete random noise. represents the angular velocity measurement value, Represents the acceleration measurement.

Formula (16);

In the above formula, Cov represents the covariance function.

In actual use, the measurement frequency of IMU is much higher than that of LiDAR. Therefore, according to the measurement frequency of LiDAR, the point cloud data fed back at the same time as IMU is found as the key frame, and the IMU data at the corresponding time of the key frame is integrated uniformly:

Formula (17);

In the formula, Δt _ij represents the time interval between time i and time j, i and j represent different time periods, and k represents the time corresponding to the key frame. represents the rotation matrix at time j, represents the speed at time j, represents the translation vector at time j, represents the rotation matrix at time i, represents the speed at time i, represents the translation vector at time i, represents the speed at time k, represents the rotation matrix at time k, represents the random noise at time k, represents the zero bias of the accelerometer at time k, represents the acceleration bias in the gyroscope at time k, represents the speed measurement value at time k, represents the angular velocity measurement value at time k, and Δt represents the time derivative.

If the posture at time i is to be optimized later, all subsequent integral results need to be updated. To avoid this situation, the rotation increment ΔR _ij , the velocity increment Δv _{ij ,} and the translation increment Δp _ij are shown in equation (18). After the transformation, all calculations in equation (18) are independent of R _i . Therefore, after the posture is optimized, the integral value is not affected, and there is no need to update the subsequent integral results.

Formula (18);

In the above formula, Represents the change in velocity between time i and time k.

By removing the high-order terms from the zero bias Taylor expansion, it can be simplified to a linear function. When the zero bias changes, the pre-integrated value is corrected. Then equation (18) can be converted to:

Formula (19);

In the formula, , and represents the rotation matrix measurement, velocity measurement and translation vector measurement of the pre-integrated model, represents rotation noise, _δvij represents velocity noise, and _δpij represents displacement noise.

Step C4 improves the existing IMU integration. When processing IMU data, a discrete integration model is introduced to derive a pre-integration model, thereby simplifying the integration calculation process of the IMU data.

Step C5: Receive the GPS measurement value at this time, perform ENU coordinate conversion, and use these data to construct a factor graph optimization model to perform global optimization on the posture.

The location information obtained by GPS is expressed in latitude, longitude and altitude, with WGS-48 (The World Geodetic System 1984) as the reference coordinate system. To convert to the global coordinate system, it is necessary to first convert to the ENU (East North Up) coordinate system. According to formula (20), the point (La, Lo, H) in the WGS-48 coordinate system can be converted to the ECEF coordinate system, recorded as (X _E , Y _E , Z _E ).

Formula (20);

Where e represents eccentricity, N represents radius of curvature, La represents longitude, Lo represents latitude, and H represents altitude.

Then convert to the ENU coordinate system:

Formula (21);

Where (E, N, U) is the point coordinate in the ENU coordinate system, (X _N , Y _N , Z _N ) represents the zero offset of the coordinate system, and M represents the transformation matrix, which is expressed as:

Formula (22);

Where (La ₀ , Lo ₀ ) represents the longitude and latitude of the ENU origin.

From equations (20) and (21), the transformation relationship from WGS-84 to ENU can be deduced:

Formula (23);

After obtaining the ENU coordinate information, it is combined with the IMU pre-integrated data, point cloud feature matching results and key frames to construct a factor graph optimization model. The essence of the factor graph model is to find the maximum a posteriori probability corresponding to the state value based on the observed value, and simplify the optimization function to solve the least squares problem. If the state quantity _xi of the mobile robot and the landmark state variable _li are known, the observation quantity _zi can be inferred.

Formula (24);

Where P(z|x) is the conditional probability of the robot observing the variable in the current state, It represents the conditional probability of the i-th observed variable z _i under the given state quantity x _i .

The SLAM positioning problem is to solve the robot's state quantity x _i based on the observation quantity z _i measured by the sensor. It is a posterior probability problem:

Formula (25);

Where 𝑃(x) is the initial estimate of the robot’s position; P(z) is the prior probability distribution of the sensor measurement results.

Convert to maximum a posteriori probability solution:

Formula (26);

represents the optimal robot state quantity, and x represents the robot state.

Decompose the prior probability and likelihood probability in equation (26) and use them as factors.

Formula (27);

In the above formula represents a factor, ϕ( _xi ) represents the prior probability distribution of the robot state quantity _xi , and the optimization function can be transformed into:

Formula (28);

Indicates that when there is no observation data, the robot state initial estimate of .

The SLAM model is converted into a factor graph, with state variables recorded as nodes and observed variables as edges, as shown in Figure 5. Figure 5 is a factor graph of SLAM positioning provided by an embodiment of the present application, in which x ₁ , x ₂ , and x ₃ represent robot state nodes, l ₁ and l ₂ represent landmark state variables, f ₁ represents a priori factors, f ₂ represents landmark observation factors, and f ₃ represents odometer factors.

In reality, the observed values of sensors fluctuate to a certain extent and usually obey Gaussian distribution, so the factor is generally expressed as an exponential:

Formula (29);

Where _fi represents the error function and ∑ _i represents the covariance matrix.

Substituting equation (29) into equation (28) and taking the negative logarithm of the right side of the equation, the optimization function can be transformed into a least squares problem.

Formula (30);

The optimization function of the multi-sensor positioning system is finally transformed into:

Formula (31);

Among them, f ₃ (x ₁ ) represents the error function of the GPS factor, f ₂ (x ₁ ) represents the error function of the loop detection factor, and f ₁ (x ₁ ) represents the error function of the odometer factor, which can be uniformly expressed as:

Formula (32);

and Respectively represent the pose of the i-th and j-th frames in the world coordinate system; ΔT _ij is the transformation matrix of the two nodes. Please refer to Figure 6, which is a schematic diagram of a factor graph model provided by an embodiment of the present application, in which f ₂ represents the IMU pre-integration factor, f ₄ represents the loop detection factor, f ₁ represents the laser radar odometer factor, x ₁ , x ₂ , x ₃ , x ₄ , x ₅ represent robot state nodes, f ₃ represents the GPS factor, and GPS represents the GPS measurement node. The GPS factor and the loop detection factor are introduced to further enhance the accuracy and reliability of multi-sensor information fusion.

This step introduces GPS factors and loop detection factors to further enhance the accuracy and reliability of multi-sensor information fusion. The factor graph optimization model constructed in this step can model and process the uncertainty of measurement data, enabling the robot to estimate its own position more accurately.

Step C6: When the motion closed loop of the mobile robot is detected, a global optimization is performed on the posture of the entire motion process and the constructed map to eliminate the closed loop error and complete the loop detection.

When the robot is working in a confined space, there is a situation where it returns to the starting point, that is, there is a motion closed loop; based on this closed-loop motion information, special constraints are constructed to perform a global optimization of the position and posture information of the robot's movement process to reduce the closed-loop error.

Please refer to Figure 7, which is a flowchart of a loop detection provided by an embodiment of the present application. The specific process is as follows: construct key frames, store key frame poses, process the latest key frames, and determine whether the loop conditions are met. If the loop conditions are not met, enter the step of constructing key frames. If the key frame construction is met, build a local map, perform ICP (closest point search method) alignment, and determine whether the score meets the requirements. If the score does not meet the loop conditions, enter the step of constructing key frames. If the score meets the loop conditions, perform pose correction.

When constructing a graph, if each frame is checked for a closed motion loop, the amount of calculation is large and time-consuming, so the key frame detection method is adopted. The criteria for judging whether there is a closed motion loop are:

(1) The time difference between the current frame and the historical key frame exceeds the threshold;

(2) The trajectory length between the current frame and the historical key frame exceeds the threshold;

(3) The straight-line distance between the current frame and the historical key frame is lower than the threshold;

(4) The point cloud registration score between the current frame and the historical keyframe is lower than the threshold.

When implementing autonomous task execution of mobile robots, the premise is to ensure that the robot can accurately determine its own position and establish an environmental map for navigation and decision-making. However, in unstructured environments, robots often face a series of challenges, one of which is the weakening or interruption of positioning signals due to occlusion, which may seriously affect the positioning accuracy of the robot. Therefore, it is necessary to use other sensors to self-position the robot. In addition, in unstructured environments, the presence of dynamic objects will also affect the subsequent path planning and decision-making of the robot. For this reason, this embodiment proposes a self-positioning method based on multi-sensor information fusion, aiming to solve the problem that the cumulative error of a single sensor cannot be completely eliminated. First, a laser radar odometer is constructed using point cloud feature extraction and registration technology. The back-end optimization model is constructed by combining the laser radar odometer factor, the inertial sensor pre-integration factor and the loop detection factor. The GPS factor is introduced to obtain global pose information, and the pose estimation result with high accuracy and robustness is obtained by optimizing the residual equation. This comprehensive multi-sensor fusion algorithm provides key support for the autonomous task execution of mobile robots in unstructured environments, enabling them to locate and navigate more reliably.

The present application provides a self-positioning system for a mobile robot, wherein the mobile robot is a robot provided with a laser radar, an inertial measurement unit, and a GPS positioning module, and the self-positioning system for the mobile robot comprises:

A first data acquisition module, used for initializing the inertial measurement unit and recording the measurement value of the inertial measurement unit to obtain inertial measurement data;

A second data acquisition module is used to receive the point cloud data collected by the laser radar, interpolate the inertial measurement data according to the timestamp of the point cloud data to obtain a first pose conversion matrix, and use the first pose conversion matrix to perform coordinate transformation on the point cloud data; wherein the first pose conversion matrix is used to describe the pose change of the point cloud data at the collection time relative to the starting time;

An inter-frame matching module is used to determine feature points in the point cloud data, and perform inter-frame matching on the point cloud data according to the feature points to obtain a point cloud feature matching result;

A pre-integration module, used to select a key frame from the inertial measurement data, pre-integrate the inertial measurement data between adjacent key frames, and obtain a pre-integration result; wherein the key frame is a data frame collected simultaneously from the point cloud data and the inertial measurement data;

A third data acquisition module is used to perform ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data;

A model building module, used to build a factor graph optimization model according to the point cloud feature matching result, the pre-integration result and the ENU coordinate data;

The optimization module is used to use the factor graph optimization model to globally optimize the posture information of the mobile robot if a motion closed loop is detected, so as to obtain the positioning information of the mobile robot in the world coordinate system.

This embodiment is applied to a mobile robot provided with a laser radar, an inertial measurement unit and a GPS positioning module. This embodiment records the inertial measurement data collected by the inertial measurement unit after initializing the inertial measurement unit, and uses the inertial measurement data to perform difference processing and coordinate transformation on the point cloud data collected by the laser radar so as to eliminate the point cloud distortion caused by the laser radar during the movement of the mobile robot. This embodiment also performs inter-frame matching on the point cloud data to obtain point cloud feature matching results so as to accurately estimate the relative position change of the mobile robot. This embodiment determines the key frames collected simultaneously based on the point cloud data and the inertial measurement data, and then performs pre-integration calculation based on the key frames so as to reduce the amount of calculation of the inertial measurement data during the self-positioning process and maintain continuous tracking of the robot's motion state. This embodiment obtains ENU coordinate data by converting the measurement value of the GPS positioning module through ENU coordinates, so that the measurement value of the GPS positioning module can be matched with the local coordinate system of the robot, providing a global reference for positioning. This embodiment constructs a factor graph optimization model based on the point cloud feature matching results, pre-integration results and ENU coordinate data, integrates different sensor data, and performs global optimization when a motion closed loop is detected to eliminate cumulative errors and improve positioning accuracy and stability. It can be seen that this embodiment can improve the self-positioning accuracy of the mobile robot by fusing the data of the laser radar, inertial measurement unit and GPS positioning module for positioning.

Furthermore, the process in which the second data acquisition module interpolates the inertial measurement data according to the timestamp of the point cloud data to obtain the first pose conversion matrix includes: selecting the nth frame of point cloud data; determining the timestamp of the nth frame of point cloud data; setting the 2 frames of inertial measurement data closest to the timestamp of the nth frame of point cloud data as reference inertial measurement data; generating a second pose conversion matrix corresponding to the reference inertial measurement data; wherein the second pose conversion matrix is the pose conversion matrix of the collection time of the reference inertial measurement data relative to the starting time; interpolating the second pose conversion matrix to obtain the first pose conversion matrix of the nth frame of point cloud data; judging whether the point cloud data has been selected; if not, adding 1 to the value of n and entering the step of selecting the nth frame of point cloud data.

Furthermore, the process of determining the feature points in the point cloud data by the inter-frame matching module includes: calculating the curvature of each point in the point cloud data; selecting edge points and plane points from the point cloud data as the feature points according to the curvature; wherein the edge points are the P1 points with the largest curvature in the point cloud data, and the plane points are the P2 points with the smallest curvature in the point cloud data.

Furthermore, the inter-frame matching module performs inter-frame matching on the point cloud data according to the feature points, and the process of obtaining the point cloud feature matching result includes: performing point-to-line feature matching according to the coordinates of the edge points to obtain a first distance; wherein the first distance is the distance from the edge point in the k+1th frame point cloud data to the target straight line, the target straight line is a straight line passing through two edge points in the kth frame point cloud data, and the first distance is greater than 0; performing point-to-plane feature matching according to the coordinates of the plane points to obtain a second distance; wherein the second distance is the distance from the plane point in the k+1th frame point cloud data to the target plane, the target plane is the plane where three non-collinear plane points in the kth frame point cloud data are located, and the second distance is greater than 0; determining the adjacent frame point cloud coordinate transformation matrix; generating an edge point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the first distance; generating a plane point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the second distance; taking the minimum value of the first distance as the optimization goal, solving the edge point constraint equation and the plane point constraint equation to obtain the point cloud feature matching result.

Furthermore, the third data acquisition module performs ENU coordinate conversion on the measurement value of the GPS positioning module, and the process of obtaining ENU coordinate data includes: converting the measurement value of the GPS positioning module to the ECEF coordinate system to obtain ECEF coordinate data; converting the ECEF coordinate data to the ENU coordinate system to obtain the ENU coordinate data.

Furthermore, it also includes:

The closed-loop detection module is used to determine the comparison information between the current frame and the key frame in the point cloud data collected by the laser radar; it is also used to determine whether the comparison information meets the preset conditions; if so, it is determined that a motion closed loop exists; if not, it is determined that there is no motion closed loop.

Furthermore, the process by which the closed-loop detection module determines the comparison information between the current frame and the key frame in the point cloud data collected by the laser radar includes: calculating the time difference between the current frame and the key frame in the point cloud data collected by the laser radar to obtain first comparison information; and/or calculating the length of the motion trajectory between the current frame and the key frame in the point cloud data collected by the laser radar to obtain second comparison information; and/or calculating the straight-line distance between the current frame and the key frame collected by the laser radar to obtain third comparison information; and/or calculating the point cloud alignment score between the current frame and the key frame in the point cloud data collected by the laser radar to obtain fourth comparison information.

Since the embodiments of the system part correspond to the embodiments of the method part, please refer to the description of the embodiments of the method part for the embodiments of the system part, which will not be repeated here.

The present application also provides a storage medium on which a computer program is stored, and when the computer program is executed, the steps provided in the above embodiment can be implemented. The storage medium may include: a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and other media that can store program codes.

The present application also provides an electronic device, which may include a memory and a processor, wherein a computer program is stored in the memory, and when the processor calls the computer program in the memory, the steps provided in the above embodiment may be implemented. Of course, the electronic device may also include various network interfaces, power supplies and other components.

The various embodiments in the specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments, and the same and similar parts between the various 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 description. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of this application, several improvements and modifications can be made to the present application, and these improvements and modifications also fall within the scope of protection of this application.

It should also be noted that, in this specification, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprises", "comprising" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the statement "comprising a ..." does not exclude the presence of other identical elements in the process, method, article or device including the element.

Claims (10)

1. A self-positioning method for a mobile robot, characterized in that the mobile robot is a robot provided with a laser radar, an inertial measurement unit and a GPS positioning module, and the self-positioning method for the mobile robot comprises: Initializing the inertial measurement unit and recording the measurement value of the inertial measurement unit to obtain inertial measurement data; Receive the point cloud data collected by the laser radar, interpolate the inertial measurement data according to the timestamp of the point cloud data to obtain a first pose transformation matrix, and use the first pose transformation matrix to perform coordinate transformation on the point cloud data; wherein the first pose transformation matrix is used to describe the pose change of the point cloud data at the collection time relative to the starting time; Determine feature points in the point cloud data, and perform frame matching on the point cloud data according to the feature points to obtain a point cloud feature matching result; Selecting key frames from the inertial measurement data, and pre-integrating the inertial measurement data between adjacent key frames to obtain a pre-integration result; wherein the key frames are data frames collected simultaneously from the point cloud data and the inertial measurement data; Performing ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data; Constructing a factor graph optimization model according to the point cloud feature matching result, the pre-integration result and the ENU coordinate data; If a motion closed loop is detected, the factor graph optimization model is used to globally optimize the posture information of the mobile robot so as to obtain the positioning information of the mobile robot in the world coordinate system. 2. The self-positioning method of a mobile robot according to claim 1, characterized in that the inertial measurement data is interpolated according to the timestamp of the point cloud data to obtain a first pose conversion matrix, comprising: Select the nth frame of point cloud data; Determine the timestamp of the nth frame of point cloud data; The two frames of inertial measurement data closest to the timestamp of the nth frame of point cloud data are set as reference inertial measurement data; Generate a second posture conversion matrix corresponding to the reference inertial measurement data; wherein the second posture conversion matrix is a posture conversion matrix of the collection time of the reference inertial measurement data relative to the starting time; Performing interpolation processing on the second pose conversion matrix to obtain the first pose conversion matrix of the nth frame point cloud data; Determine whether the point cloud data has been selected; if not, add 1 to the value of n and enter the step of selecting the nth frame of point cloud data. 3. The self-positioning method of a mobile robot according to claim 1, wherein determining the feature points in the point cloud data comprises: Calculating the curvature of each point in the point cloud data; Edge points and plane points are selected from the point cloud data as the feature points according to the curvature; wherein the edge points are P1 points with the largest curvature in the point cloud data, and the plane points are P2 points with the smallest curvature in the point cloud data. 4. The self-positioning method of a mobile robot according to claim 3, characterized in that the point cloud data is matched between frames according to the feature points to obtain a point cloud feature matching result, comprising: Performing point-to-line feature matching according to the coordinates of the edge point to obtain a first distance; wherein the first distance is the distance from the edge point in the k+1th frame of point cloud data to the target straight line, the target straight line is a straight line passing through two edge points in the kth frame of point cloud data, and the first distance is greater than 0; Performing point-to-plane feature matching according to the coordinates of the plane point to obtain a second distance; wherein the second distance is the distance from the plane point in the k+1th frame point cloud data to the target plane, the target plane is the plane where the three non-collinear plane points in the kth frame point cloud data are located, and the second distance is greater than 0; Determine the coordinate transformation matrix of the point cloud of adjacent frames; Generate an edge point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the first distance; Generate a plane point constraint equation according to the adjacent frame point cloud coordinate transformation matrix and the second distance; Taking the minimum value of the first distance as the optimization goal, the edge point constraint equation and the plane point constraint equation are solved to obtain a point cloud feature matching result. 5. The self-positioning method of a mobile robot according to claim 1, characterized in that the measurement value of the GPS positioning module is converted into ENU coordinates to obtain ENU coordinate data, comprising: Convert the measured value of the GPS positioning module to the ECEF coordinate system to obtain ECEF coordinate data; The ECEF coordinate data is converted to the ENU coordinate system to obtain the ENU coordinate data. 6. The self-positioning method of a mobile robot according to claim 1, further comprising: Determine comparison information between a current frame and the key frame in the point cloud data collected by the laser radar; Determining whether the comparison information meets a preset condition; If so, it is determined that there is a motion closed loop; If not, it is determined that there is no motion closed loop. 7. The self-positioning method of a mobile robot according to claim 6, characterized in that determining the comparison information between the current frame and the key frame in the point cloud data collected by the laser radar comprises: Calculating the time difference between the current frame and the key frame in the point cloud data collected by the laser radar to obtain first comparison information; and/or, calculating the length of a motion trajectory between a current frame and a key frame in the point cloud data collected by the laser radar to obtain second comparison information; and/or, calculating a straight-line distance between a current frame collected by the laser radar and the key frame to obtain third comparison information; And/or, calculate the point cloud registration scores of the current frame and the key frame in the point cloud data collected by the laser radar to obtain fourth comparison information. 8. A self-positioning system for a mobile robot, characterized in that the mobile robot is a robot equipped with a laser radar, an inertial measurement unit and a GPS positioning module, and the self-positioning system for the mobile robot comprises: A first data acquisition module, used for initializing the inertial measurement unit and recording the measurement value of the inertial measurement unit to obtain inertial measurement data; A second data acquisition module is used to receive the point cloud data collected by the laser radar, interpolate the inertial measurement data according to the timestamp of the point cloud data to obtain a first pose conversion matrix, and use the first pose conversion matrix to perform coordinate transformation on the point cloud data; wherein the first pose conversion matrix is used to describe the pose change of the point cloud data at the collection time relative to the starting time; An inter-frame matching module is used to determine feature points in the point cloud data, and perform inter-frame matching on the point cloud data according to the feature points to obtain a point cloud feature matching result; A pre-integration module, used to select a key frame from the inertial measurement data, pre-integrate the inertial measurement data between adjacent key frames, and obtain a pre-integration result; wherein the key frame is a data frame collected simultaneously from the point cloud data and the inertial measurement data; A third data acquisition module is used to perform ENU coordinate conversion on the measurement value of the GPS positioning module to obtain ENU coordinate data; A model building module, used to build a factor graph optimization model according to the point cloud feature matching result, the pre-integration result and the ENU coordinate data; The optimization module is used to use the factor graph optimization model to globally optimize the posture information of the mobile robot if a motion closed loop is detected, so as to obtain the positioning information of the mobile robot in the world coordinate system. 9. An electronic device, characterized in that it comprises a memory and a processor, wherein a computer program is stored in the memory, and when the processor calls the computer program in the memory, the steps of the self-positioning method of the mobile robot as claimed in any one of claims 1 to 7 are implemented. 10. A storage medium, characterized in that computer executable instructions are stored in the storage medium, and when the computer executable instructions are loaded and executed by a processor, the steps of the self-positioning method of the mobile robot as described in any one of claims 1 to 7 are implemented.