Robot automatic repositioning method based on alignment of multi-element prior information
By using a multivariate prior information alignment method, combined with GPS, IMU, and LiDAR data, high-precision and automated robot repositioning was achieved, solving the problems of poor repositioning accuracy and insufficient automation in poor lighting conditions in existing technologies.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- KUNMING UNIV OF SCI & TECH
- Filing Date
- 2026-03-03
- Publication Date
- 2026-05-29
Smart Images
Figure CN121763333B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of high-precision robot positioning technology, specifically to an automatic robot relocation method based on multivariate prior information alignment. Background Technology
[0002] Relocalization technology is a prerequisite for mobile robots to achieve autonomous navigation. Mobile robots must first determine their own pose through relocalization before they can proceed with the subsequent autonomous navigation process.
[0003] To achieve mobile robot relocalization, people have made a lot of efforts from different perspectives. Currently, relocalization methods include: (1) relocalization based on visual perception of the surrounding environment. This method can achieve good relocalization in bright light environments, but the difficulty of relocalization increases at night and in exposed environments; this method requires training visual perception through deep learning, which increases time costs and computing resources; (2) using the method of calculating the matching degree between frames of 3D LiDAR, and then using 2D binary grid. Figure 2 The second screening relocation method requires the conversion of two-dimensional grid before relocation. In environments with unclear structural features, relocation is more difficult. (3) By manually assisting, the lidar point cloud is matched with the overall prior map. This method increases the manual input, and because the prior information required for relocation is singular, the positioning stability and accuracy are poor.
[0004] Despite these efforts, the improvement of robot relocation accuracy and the automation of relocation are still under development. Therefore, improving robot relocation accuracy and achieving relocation automation at the same time are urgent problems to be solved. Summary of the Invention
[0005] To address the shortcomings of existing technologies, this invention provides an automatic robot relocalization method based on multivariate prior information alignment, which has advantages such as improving the accuracy of robot relocalization and achieving automated relocalization, thus solving the aforementioned technical problems.
[0006] To achieve the above objectives, the present invention provides the following technical solution: an automatic robot relocalization method based on multivariate prior information alignment, comprising the following steps:
[0007] S1: Extract prior GPS location information;
[0008] S2: Extract prior pose information;
[0009] S3: Extract prior point cloud information;
[0010] S4: Obtain real-time GPS location information and align the real-time GPS location information with the prior GPS location information to obtain coarse location information;
[0011] S5: Time-align the coarse position information with the prior pose information and the prior point cloud information to obtain the coarse pose and the point cloud to be aligned;
[0012] S6: Project the real-time lidar point cloud onto the coarse pose to obtain the coarse pose point cloud;
[0013] S7: Finely align the coarse pose point cloud with the point cloud to be aligned, calculate the fine pose, and obtain the relocalization result.
[0014] As a preferred technical solution of the present invention, the extraction of prior GPS location information in S1 specifically includes the following steps:
[0015] S1.1: Convert GPS latitude and longitude information into GPS location in meters. The specific expression is as follows:
[0016]
[0017]
[0018] in, and These represent the GPS positions in meters in the x and y directions, respectively. , This is the origin of the GPS navigation coordinates; Represents the Earth's radius. and These represent the longitude and latitude of the GPS coordinates, respectively. It is the natural logarithm. It is a tangent trigonometric function. Pi;
[0019] S1.2: The angular velocity and acceleration data measured by the IMU are used to calculate the trajectory and obtain the calculated position. The specific expression is as follows:
[0020]
[0021]
[0022]
[0023] in, This indicates the calculated position based on the flight path, including the position in the x, y, and z directions. This represents the turning angle calculated from the flight path. This indicates the speed calculated from the flight path. Indicates an exponential mapping. This represents the turning angle calculated from the trajectory corresponding to the last sampling time. This represents the velocity calculated from the trajectory corresponding to the last sampling time. This indicates the position calculated from the trajectory corresponding to the last sampling time. Indicates angular velocity, Indicates acceleration. Indicates zero angular velocity bias. Indicates zero bias acceleration. Represents gravitational acceleration. Indicates the sampling time interval;
[0024] S1.3: Determine the Euclidean distance between the GPS position (in meters) and the horizontal position calculated from the flight track. Whether it is less than the threshold, the specific expression is as follows:
[0025]
[0026]
[0027] in, This represents the Euclidean distance between the GPS position (in meters) and the horizontal position calculated from the flight track. If the distance is less than a threshold, the GPS position (in meters) is considered the prior GPS position. Adding the timestamp of the current GPS data completes the prior GPS position information. and These represent the x and y positions calculated from the flight path, respectively. and These represent the GPS positions in meters in the x and y directions, respectively.
[0028] As a preferred embodiment of the present invention, the extraction of prior pose information in step S2 includes the following steps:
[0029] S2.1: Construct the residual for line and surface feature matching of LiDAR point cloud, the specific expression of which is as follows:
[0030]
[0031]
[0032]
[0033] in, and Let each represent the prior pose to be solved, including rotation and translation. The first laser radar scan One point, express The point obtained after coordinate transformation and These represent the line feature point matching residual and the surface feature point matching residual, respectively. and Indicates when When the feature point is a line, the distance in the point cloud map is... The two most recent line feature points, Indicates when When it is a feature point on a surface, its distance in the point cloud map The three nearest face feature points, Indicates modulo, and These represent the number of line feature points and area feature points scanned by the lidar, respectively.
[0034] S2.2: Constructing the residuals of the IMU pre-integration model includes the following steps:
[0035] S2.2.1: Constructing the IMU pre-integration model:
[0036]
[0037]
[0038]
[0039] in, , , These represent the IMU's first... From the sampling point to the... Pre-integration of rotation angle, pre-integration of velocity, and pre-integration of position at each sampling point. Indicates an exponential mapping. and These represent the angular velocity and acceleration measurements at the k-th sampling point of the IMU, respectively. Indicates zero angular velocity bias. Indicates zero bias acceleration. This represents the time interval between the (k+1)th sampling point and the kth sampling point. , These represent the pre-integral values of the turning angle and the pre-integral value of the velocity from the i-th sampling point to the k-th sampling point, respectively. , These are the multiplication symbol and the addition symbol, respectively.
[0040] S2.2.2: Construct the residuals of the IMU pre-integration model in S2.2.1, with the following specific expression:
[0041]
[0042]
[0043]
[0044] in, Represents a logarithmic mapping, with superscript. Indicates transpose. , This represents the prior pose to be solved under the IMU pre-integration model. , , These are the rotational residual, velocity residual, and position residual obtained from pre-integration, respectively. , , This represents the rotation angle, velocity, and position of the previous positioning point estimated by pre-integration. Indicates the current location speed. Indicates the time interval between two positioning points;
[0045] S2.3: Construct the residual of the prior GPS position, the specific expression of which is as follows:
[0046]
[0047] in, Indicates the prior GPS location. Indicates location information, and These represent the GPS positions in meters in the x and y directions, respectively. Represents the x and y positions in the prior pose to be solved;
[0048] S2.4: Constructing the probability density function The specific expression is as follows:
[0049]
[0050] in, , These represent the variances of the measurements from the lidar and IMU, respectively. This represents the variance of GPS measurements. , , Let represent the residuals of the lidar point cloud line and surface feature matching, the residuals of the IMU pre-integration model, and the residuals of the prior GPS position, respectively. When it is at its maximum, the obtained value is... As a priori pose , , Indicates exponentiation. Indicates the pose to be solved;
[0051] S2.5: Based on S2.4, a least squares problem is constructed, and the nonlinear optimization LM method is used to solve the least squares problem. The specific expression is as follows:
[0052]
[0053] in, Describes the minimum value function. , , These represent the weighted sum of squared residuals for the line and surface feature matching of the lidar point cloud, the sum of squared residuals for the IMU pre-integration model, and the sum of squared residuals for the prior GPS position, respectively.
[0054] S2.6: Prior pose , Adding the timestamp of the GPS data at this time constitutes the prior pose information.
[0055] As a preferred technical solution of the present invention, the extraction of prior point cloud information specifically involves converting the lidar point cloud to a prior pose, as shown in the following expression:
[0056]
[0057] in, This represents each point in the lidar point cloud. express Transform to a point on the prior pose. , Indicates the prior pose, Set up to the prior point cloud set In the process, the prior point cloud is obtained, and the GPS data timestamp at this time is added to form the prior point cloud information.
[0058] As a preferred technical solution of the present invention, in step S4, the real-time GPS location information is aligned with the prior GPS location information to obtain coarse location information. Specifically, by comparing the horizontal distance between each prior GPS location and the real-time GPS location, the prior GPS location information corresponding to the smallest distance is the coarse location information.
[0059] As a preferred technical solution of the present invention, in step S5, the coarse position information is time-aligned with the prior pose information and the prior point cloud information to obtain the coarse pose and the point cloud to be aligned. Specifically, the prior pose information whose timestamp is equal to the timestamp of the coarse position information is used as the coarse pose, and the prior point cloud information whose timestamp is equal to the timestamp of the coarse position information is used as the point cloud to be aligned.
[0060] As a preferred embodiment of the present invention, in step S6, the real-time lidar point cloud is projected onto the coarse pose to obtain a coarse pose point cloud, the specific expression of which is as follows:
[0061]
[0062] in, This represents each point in the real-time lidar point cloud. , The rotation and translation of the coarse pose are projected onto the points on the coarse pose, representing all of them. Aggregate to real-time point cloud aggregation In the process, a rough pose point cloud is obtained.
[0063] As a preferred embodiment of the present invention, in step S7, the coarse pose point cloud and the point cloud to be aligned are finely aligned to calculate the fine pose and obtain the relocalization result. The specific expression is as follows:
[0064]
[0065] in, The rotation angle and position parameters to be optimized are respectively. The first point cloud representing the coarse pose One point, Indicates the distance in the point cloud to be matched The nearest point, This indicates the number of points in the approximate pose. Describes the minimum value function. This represents the final relocalization result, i.e., the current angle and position of the robot. It represents the sum of squares.
[0066] Compared with existing technologies, this invention provides an automatic robot relocalization method based on multivariate prior information alignment, which has the following beneficial effects:
[0067] This invention first extracts prior GPS location information, prior pose information, and prior point cloud information. Then, it aligns the real-time GPS location information with the prior GPS location information to obtain coarse location information. Next, it aligns the coarse location information with the prior pose information and prior point cloud information in time to obtain coarse pose and point cloud to be aligned. Then, it projects the real-time LiDAR point cloud onto the coarse pose to obtain a coarse pose point cloud. Finally, it performs fine alignment between the coarse pose point cloud and the point cloud to be aligned to calculate the fine pose and obtain the relocalization result. This achieves high accuracy in the relocalization result and the relocalization process does not require manual assistance. Attached Figure Description
[0068] Figure 1 This is a flowchart of the robot automatic relocalization method based on multivariate prior information alignment according to the present invention;
[0069] Figure 2 This is a flowchart illustrating the process of extracting prior pose information in this invention.
[0070] Figure 3 The image shows the actual deployment and operation results of the robot automatic relocalization method based on multivariate prior information alignment according to the present invention. Detailed Implementation
[0071] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0072] Please see Figures 1-3 A robot automatic relocalization method based on multivariate prior information alignment includes the following steps:
[0073] S1: Extract prior GPS location information, convert latitude and longitude to meter-level coordinates and unify the measurement units, combine IMU track estimation and Euclidean distance threshold judgment to filter valid data, and obtain a reliable prior location basis without manual intervention;
[0074] S2: Extract prior pose information, fuse three types of residuals: lidar line and surface features, IMU pre-integration, and GPS position, and achieve optimal estimation through probability density function construction and LM nonlinear optimization;
[0075] S3: Extract prior point cloud information, form standardized prior point cloud data, and automatically complete coordinate calibration to ensure data consistency;
[0076] S4: Obtain real-time GPS location information and align the real-time GPS location information with the prior GPS location information to obtain coarse location information;
[0077] S5: Time-align the coarse position information with the prior pose information and the prior point cloud information to obtain the coarse pose and the point cloud to be aligned;
[0078] S6: Real-time LiDAR point cloud is projected onto coarse pose to obtain coarse pose point cloud, eliminating coordinate obstacles and reducing initial differences, thus reducing the difficulty of subsequent alignment.
[0079] S7: Finely align the coarse pose point cloud with the point cloud to be aligned, calculate the fine pose, and obtain the relocalization result.
[0080] The entire process replaces manual intervention with automated algorithms. At the same time, through the complementary use of multi-source data from GPS, IMU, and LiDAR, multi-round error screening (threshold judgment, distance comparison), and precise mathematical optimization (LM method, residual constraints), the positioning error is reduced layer by layer, ultimately achieving the core technical effect of improving the robot's repositioning accuracy and realizing automated repositioning.
[0081] Extracting prior GPS location information in S1 specifically includes the following steps:
[0082] S1.1: Convert GPS latitude and longitude information into GPS location in meters. The specific expression is as follows:
[0083]
[0084]
[0085] in, and These represent the GPS positions in meters in the x and y directions, respectively. , This is the origin of the GPS navigation coordinates; Represents the Earth's radius. and These represent the longitude and latitude of the GPS coordinates, respectively. It is the natural logarithm. It is a tangent trigonometric function. Pi;
[0086] S1.2: The angular velocity and acceleration data measured by the IMU are used to calculate the trajectory and obtain the calculated position. The specific expression is as follows:
[0087]
[0088]
[0089]
[0090] in, This indicates the calculated position based on the flight path, including the position in the x, y, and z directions. This represents the turning angle calculated from the flight path. This indicates the speed calculated from the flight path. Indicates an exponential mapping. This represents the turning angle calculated from the trajectory corresponding to the last sampling time. This represents the velocity calculated from the trajectory corresponding to the last sampling time. This indicates the position calculated from the trajectory corresponding to the last sampling time. Indicates angular velocity, Indicates acceleration. Indicates zero angular velocity bias. Indicates zero bias acceleration. Represents gravitational acceleration. Indicates the sampling time interval;
[0091] S1.3: Determine the Euclidean distance between the GPS position (in meters) and the horizontal position calculated from the flight track. Whether it is less than the threshold, the specific expression is as follows:
[0092]
[0093]
[0094] in, This represents the Euclidean distance between the GPS position (in meters) and the horizontal position calculated from the flight track. If the distance is less than a threshold, the GPS position (in meters) is considered the prior GPS position. Adding the timestamp of the current GPS data completes the prior GPS position information. and These represent the x and y positions calculated from the flight path, respectively. and These represent the GPS positions in meters in the x and y directions, respectively.
[0095] Extracting prior pose information in S2 includes the following steps:
[0096] S2.1: Construct the residual for line and surface feature matching of LiDAR point cloud, the specific expression of which is as follows:
[0097]
[0098]
[0099]
[0100] in, and Let each represent the prior pose to be solved, including rotation and translation. The first laser radar scan One point, express The point obtained after coordinate transformation and These represent the line feature point matching residual and the surface feature point matching residual, respectively. and Indicates when When the feature point is a line, the distance in the point cloud map is... The two most recent line feature points, Indicates when When it is a feature point on a surface, its distance in the point cloud map The three nearest face feature points, Indicates modulo, and These represent the number of line feature points and area feature points scanned by the lidar, respectively.
[0101] S2.2: Constructing the residuals of the IMU pre-integration model includes the following steps:
[0102] S2.2.1: Constructing the IMU pre-integration model:
[0103]
[0104]
[0105]
[0106] in, , , These represent the IMU's first... From the sampling point to the... Pre-integration of rotation angle, pre-integration of velocity, and pre-integration of position at each sampling point. and These represent the angular velocity and acceleration measurements at the k-th sampling point of the IMU, respectively. Indicates zero angular velocity bias. Indicates zero bias acceleration. This represents the time interval between the (k+1)th sampling point and the kth sampling point. , These represent the pre-integral values of the turning angle and the pre-integral value of the velocity from the i-th sampling point to the k-th sampling point, respectively. , These are the multiplication symbol and the addition symbol, respectively.
[0107] S2.2.2: Construct the residuals of the IMU pre-integration model in S2.2.1, with the following specific expression:
[0108]
[0109]
[0110]
[0111] in, Represents a logarithmic mapping, with superscript. Indicates transpose. , This represents the prior pose to be solved under the IMU pre-integration model. , , These are the rotational residual, velocity residual, and position residual obtained from pre-integration, respectively. , , This represents the rotation angle, velocity, and position of the previous positioning point estimated by pre-integration. Indicates the current location speed. Indicates the time interval between two positioning points;
[0112] S2.3: Construct the residual of the prior GPS position, the specific expression of which is as follows:
[0113]
[0114] in, Indicates the prior GPS location. , and These represent the GPS positions in meters in the x and y directions, respectively. Represents the x and y positions in the prior pose to be solved;
[0115] S2.4: Constructing the probability density function The specific expression is as follows:
[0116]
[0117] in, , These represent the variances of the measurements from the lidar and IMU, respectively. This represents the variance of GPS measurements. , , Let represent the residuals of the lidar point cloud line and surface feature matching, the residuals of the IMU pre-integration model, and the residuals of the prior GPS position, respectively. When it is at its maximum, the obtained value is... As a priori pose , ;
[0118] S2.5: Based on S2.4, a least squares problem is constructed, and the nonlinear optimization LM method is used to solve the least squares problem. The specific expression is as follows:
[0119]
[0120] in, Describes the minimum value function. , , These represent the weighted sum of squared residuals for the line and surface feature matching of the lidar point cloud, the sum of squared residuals for the IMU pre-integration model, and the sum of squared residuals for the prior GPS position, respectively.
[0121] S2.6: Prior pose , Adding the timestamp of the GPS data at this time constitutes the prior pose information.
[0122] Extracting prior point cloud information specifically involves converting the LiDAR point cloud onto a prior pose, as shown in the following expression:
[0123]
[0124] in, This represents each point in the lidar point cloud. express Transform to a point on the prior pose. , Indicates the prior pose, Set up to the prior point cloud set In the process, the prior point cloud is obtained, and the GPS data timestamp at this time is added to form the prior point cloud information.
[0125] In S4, the real-time GPS location information is aligned with the prior GPS location information to obtain coarse location information. Specifically, this is done by comparing the horizontal distance between each prior GPS location and the real-time GPS location, and the prior GPS location information corresponding to the smallest distance is the coarse location information.
[0126] In S5, the coarse position information is time-aligned with the prior pose information and prior point cloud information to obtain the coarse pose and the point cloud to be aligned. Specifically, the prior pose information whose timestamp is equal to the timestamp of the coarse position information is used as the coarse pose, and the prior point cloud information whose timestamp is equal to the timestamp of the coarse position information is used as the point cloud to be aligned.
[0127] In S6, the real-time LiDAR point cloud is projected onto the coarse pose to obtain the coarse pose point cloud, the specific expression of which is as follows:
[0128]
[0129] in, This represents each point in the real-time lidar point cloud. , Representing the rotation and translation of the coarse pose, the points projected onto the coarse pose, and all Aggregate to real-time point cloud aggregation In the process, a rough pose point cloud is obtained.
[0130] In S7, the coarse pose point cloud and the point cloud to be aligned are finely aligned, the fine pose is calculated, and the relocalization result is obtained. The specific expression is as follows:
[0131]
[0132] in, The rotation angle and position parameters to be optimized are respectively. The first point cloud representing the coarse pose One point, Indicates the distance in the point cloud to be matched The nearest point, This indicates the number of points in the approximate pose. Describes the minimum value function. This represents the final relocalization result, i.e., the current angle and position of the robot. Represents the sum of squares;
[0133] This invention's implementation uses the ROS melodic platform on an Ubuntu system based on the Linux kernel for sensor data acquisition and communication. The relocation algorithm is developed and deployed in a real vehicle using the PCL library and C++14. Experiments show that the relocation time of this invention is less than 15ms. The results of the real-vehicle deployment are as follows: Figure 3 As shown, the white point cloud represents the point cloud to be aligned, the green point cloud represents the coarse pose point cloud, and the red point cloud represents the real-time point cloud display after successful relocalization. That is, the more the red and white point clouds overlap, the more accurate the relocalization result. Figure 3 The results of four random relocations in the implementation examples of this invention show that the relocation results proposed by this invention have high accuracy and the relocation process does not require manual assistance.
[0134] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.
Claims
1. A robot automatic relocalization method based on multivariate prior information alignment, characterized in that: Includes the following steps: S1: Extract prior GPS location information; S2: Extract prior pose information; S3: Extract prior point cloud information; S4: Obtain real-time GPS location information and align the real-time GPS location information with the prior GPS location information to obtain coarse location information; S5: Time-align the coarse position information with the prior pose information and the prior point cloud information to obtain the coarse pose and the point cloud to be aligned; S6: Project the real-time lidar point cloud onto the coarse pose to obtain the coarse pose point cloud; S7: Fine-align the coarse pose point cloud with the point cloud to be aligned, calculate the fine pose, and obtain the relocalization result. The specific expression is as follows: ； in, The rotation angle and position parameters to be optimized are respectively. The first point cloud representing the coarse pose One point, Indicates the distance in the point cloud to be matched The nearest point, This indicates the number of points in the approximate pose. Describes the minimum value function. and This represents the final relocalization result, i.e., the current angle and position of the robot. To express summation, It represents the sum of squares.
2. The robot automatic relocalization method based on multivariate prior information alignment according to claim 1, characterized in that: The extraction of prior GPS location information in S1 specifically includes the following steps: S1.1: Convert GPS latitude and longitude information into GPS location in meters. The specific expression is as follows: ； ； in, and These represent the GPS positions in meters in the x and y directions, respectively. , This is the origin of the GPS navigation coordinates; Represents the Earth's radius. and These represent the longitude and latitude of the GPS coordinates, respectively. It is the natural logarithm. It is a tangent trigonometric function. Pi; S1.2: The angular velocity and acceleration data measured by the IMU are used to calculate the trajectory and obtain the calculated position. The specific expression is as follows: ； ； ； in, This indicates the position calculated from the flight path, including the position in the x, y, and z directions. This represents the turning angle calculated from the flight path. This indicates the speed calculated from the flight path. Indicates an exponential mapping. This represents the turning angle calculated from the trajectory corresponding to the last sampling time. This represents the velocity calculated from the trajectory corresponding to the last sampling time. This indicates the position calculated from the trajectory corresponding to the last sampling time. Indicates angular velocity, Indicates acceleration. Indicates zero angular velocity bias. Indicates zero bias acceleration. Represents gravitational acceleration. Indicates the sampling time interval; S1.3: Determine the Euclidean distance between the GPS position (in meters) and the horizontal position calculated from the flight track. Whether it is less than the threshold, the specific expression is as follows: ； ； in, This represents the Euclidean distance between the GPS position (in meters) and the horizontal position calculated from the flight track. If the distance is less than a threshold, the GPS position (in meters) is considered the prior GPS position. Adding the timestamp of the current GPS data completes the prior GPS position information. and These represent the x and y positions calculated from the flight path, respectively. and These represent the GPS positions in meters in the x and y directions, respectively.
3. The robot automatic relocalization method based on multivariate prior information alignment according to claim 2, characterized in that: The extraction of prior pose information in S2 includes the following steps: S2.1: Construct the residual for line and surface feature matching of LiDAR point cloud, the specific expression of which is as follows: ； ； ； in, and Let each represent the prior pose to be solved, including rotation and translation. The first laser radar scan One point, express The point obtained after coordinate transformation and These represent the line feature point matching residual and the surface feature point matching residual, respectively. and Indicates when When the feature point is a line, the distance in the point cloud map is... The two most recent line feature points, Indicates when When it is a feature point on a surface, its distance in the point cloud map The three nearest face feature points, Indicates modulo, and These represent the number of line feature points and area feature points scanned by the lidar, respectively. S2.2: Constructing the residuals of the IMU pre-integration model includes the following steps: S2.2.1: Constructing the IMU pre-integration model: ； ； ； in, , , These represent the IMU's first... From the sampling point to the... Pre-integration of rotation angle, pre-integration of velocity, and pre-integration of position at each sampling point. Indicates an exponential mapping. and These represent the angular velocity and acceleration measurements at the k-th sampling point of the IMU, respectively. Indicates zero angular velocity bias. Indicates zero bias acceleration. This represents the time interval between the (k+1)th sampling point and the kth sampling point. , These represent the pre-integral values of the turning angle and the pre-integral value of the velocity from the i-th sampling point to the k-th sampling point, respectively. , These are the multiplication symbol and the addition symbol, respectively. S2.2.2: Construct the residuals of the IMU pre-integration model in S2.2.1, with the following specific expression: ； ； ； in, Represents a logarithmic mapping, with superscript. Indicates transpose. , This represents the prior pose to be solved under the IMU pre-integration model. , , These are the rotational residual, velocity residual, and position residual obtained from pre-integration, respectively. , , This represents the rotation angle, velocity, and position of the previous positioning point estimated by pre-integration. Indicates the current location speed. Indicates the time interval between two positioning points; S2.3: Construct the residual of the prior GPS position, the specific expression of which is as follows: ； in, Indicates the prior GPS location. Indicates location information, and These represent the GPS positions in meters in the x and y directions, respectively. Represents the x and y positions in the prior pose to be solved; S2.4: Constructing the probability density function The specific expression is as follows: ； in, , These represent the variances of the measurements from the lidar and IMU, respectively. This represents the variance of GPS measurements. , , Let represent the residuals of the lidar point cloud line and surface feature matching, the residuals of the IMU pre-integration model, and the residuals of the prior GPS position, respectively. When it is at its maximum, the obtained value is... As a priori pose , , This indicates exponentiation. Indicates the pose to be solved; S2.5: Based on S2.4, a least squares problem is constructed, and the nonlinear optimization LM method is used to solve the least squares problem. The specific expression is as follows: ； in, Describes the minimum value function. , , These represent the weighted sum of squared residuals for the line and surface feature matching of the lidar point cloud, the sum of squared residuals for the IMU pre-integration model, and the sum of squared residuals for the prior GPS position, respectively. S2.6: Prior pose , Adding the timestamp of the GPS data at this time constitutes the prior pose information.
4. The robot automatic relocalization method based on multivariate prior information alignment according to claim 3, characterized in that: The extraction of prior point cloud information specifically involves converting the LiDAR point cloud into a prior pose, as shown in the following expression: ； in, This represents each point in the lidar point cloud. express Transform to a point on the prior pose. , Indicates the prior pose, Set up to the prior point cloud set In the process, the prior point cloud is obtained, and the GPS data timestamp at this time is added to form the prior point cloud information.
5. The robot automatic relocalization method based on multivariate prior information alignment according to claim 4, characterized in that: In step S4, the real-time GPS location information is aligned with the prior GPS location information to obtain coarse location information. Specifically, this is done by comparing the horizontal distance between each prior GPS location and the real-time GPS location, and the prior GPS location information corresponding to the smallest distance is the coarse location information.
6. The robot automatic relocalization method based on multivariate prior information alignment according to claim 5, characterized in that: In step S5, the coarse position information is time-aligned with the prior pose information and the prior point cloud information to obtain the coarse pose and the point cloud to be aligned. Specifically, the prior pose information whose timestamp is equal to the timestamp of the coarse position information is used as the coarse pose, and the prior point cloud information whose timestamp is equal to the timestamp of the coarse position information is used as the point cloud to be aligned.
7. The automatic robot relocalization method based on multivariate prior information alignment according to claim 6, characterized in that: In step S6, the real-time lidar point cloud is projected onto the coarse pose to obtain a coarse pose point cloud, the specific expression of which is as follows: ； in, This represents each point in the real-time lidar point cloud. , The rotation and translation of the coarse pose are projected onto the points on the coarse pose, representing all of them. Aggregate to real-time point cloud aggregation In the process, a rough pose point cloud is obtained.