Robot automatic repositioning method based on multivariate prior information alignment

Abstract

The invention relates to the technical field of robot high-precision positioning, and discloses a robot automatic repositioning method based on multivariate prior information alignment. The method comprises the steps of firstly extracting prior GPS position information, prior pose information and prior point cloud information, then carrying out position alignment on real-time GPS position information and the prior GPS position information to obtain rough position information, then carrying out time alignment on the rough position information, the prior pose information and the prior point cloud information to obtain a rough pose and a to-be-aligned point cloud, and finally, carrying out alignment on the to-be-aligned point cloud and the to-be-aligned point cloud. The real-time laser radar point cloud is firstly projected to the to-be-aligned point cloud, then the real-time laser radar point cloud is projected to the rough pose to obtain a rough pose point cloud, finally the rough pose point cloud and the to-be-aligned point cloud are finely aligned, a fine pose is calculated, and a repositioning result is obtained, so that the effects that the repositioning result is high in precision and the repositioning process does not need manual assistance are achieved.

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: 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: Finely align the coarse pose point cloud with the point cloud to be aligned, calculate the fine pose, and obtain the relocalization result.

[0007] As a preferred technical solution of the present invention, 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.

[0008] As a preferred embodiment of the present invention, the extraction of prior pose information in step 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.

[0009] 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: 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.

[0010] 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.

[0011] 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.

[0012] 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: 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 ensemble In the process, a rough pose point cloud is obtained.

[0013] 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: 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.

[0014] Compared with existing technologies, this invention provides an automatic robot relocalization method based on multivariate prior information alignment, which has the following beneficial effects: 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 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

[0015] Figure 1 This is a flowchart of the robot automatic relocalization method based on multivariate prior information alignment according to the present invention; Figure 2 This is a flowchart illustrating the process of extracting prior pose information in this invention. 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

[0016] 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.

[0017] Please see Figures 1-3 A robot automatic relocalization method based on multivariate prior information alignment includes the following steps: 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; 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; S3: Extract prior point cloud information, form standardized prior point cloud data, and automatically complete coordinate calibration to ensure data consistency; 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: 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. S7: Finely align the coarse pose point cloud with the point cloud to be aligned, calculate the fine pose, and obtain the relocalization result.

[0018] 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.

[0019] Extracting 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. Extracting 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. 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. , 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 , ; 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.

[0020] Extracting prior point cloud information specifically involves converting the LiDAR point cloud onto 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.

[0021] 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.

[0022] 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.

[0023] 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: 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 ensemble In the process, a rough pose point cloud is obtained.

[0024] 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: 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; 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.

[0025] 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 repositioning method based on multi-element prior information alignment, characterized in that: The method comprises the following steps: S1: extracting prior GPS position information; S2: extracting prior pose information; S3: extracting prior point cloud information; S4: acquiring real-time GPS position information, and performing position alignment on the real-time GPS position information and the prior GPS position information to obtain coarse position information; S5: performing time alignment on the coarse position information, the prior pose information and the prior point cloud information to obtain coarse pose and point cloud to be aligned; S6: projecting real-time laser radar point cloud onto the coarse pose to obtain coarse pose point cloud; S7: performing fine alignment on the coarse pose point cloud and the point cloud to be aligned, calculating a fine pose, and obtaining a repositioning result.

2. The robot automatic repositioning method based on multi-element prior information alignment according to claim 1, characterized in that: The step S1 of extracting prior GPS position information comprises the following steps: S1.1: converting longitude and latitude information of the GPS into GPS position in meters, and the specific expression is as follows: wherein, and denote the GPS position in the x- and y-direction in meters, respectively, , is the GPS navigation coordinate origin; denotes the earth radius, and denote the longitude and latitude of the GPS, respectively, is the natural logarithm, is the tangent trigonometric function, is the circle constant; S1.2: performing track calculation on angular velocity data and acceleration data measured by the IMU to obtain track calculated position, and the specific expression is as follows: wherein, represents a position of the trajectory estimation, comprising a position in x, y, z direction, represents a turn angle of the trajectory estimation, represents a velocity of the trajectory estimation, represents an exponential mapping, represents a turn angle of the trajectory estimation corresponding to a last sampling time, represents a velocity of the trajectory estimation corresponding to a last sampling time, represents a position of the trajectory estimation corresponding to a last sampling time, represents an angular velocity, represents an acceleration, represents an angular velocity bias, represents an acceleration bias, represents a gravitational acceleration, represents a sampling time interval; S1.3: Determine the Euclidean distance between the GPS position in meters and the dead reckoning horizontal position whether it is less than a threshold value, expressed as follows: wherein denotes the Euclidean distance between the GPS position in meters and the position of the dead reckoning in the horizontal plane, and if the distance is less than a threshold value, the GPS position in meters is the a priori GPS position, and the time stamp of the GPS data at this time is added to form the a priori GPS position information, and denote the position of the dead reckoning in the x and y direction, respectively, and denote the GPS position in the x and y direction, respectively, in meters.

3. The robot automatic repositioning method based on multi-element prior information alignment according to claim 2, characterized in that: The step S2 of extracting prior pose information comprises the following steps: S2.1: constructing a residual error of laser radar point cloud line-surface feature matching, and the specific expression 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 a residual error of the IMU pre-integration model, comprising the following steps: S2.2.1: constructing the IMU pre-integration model: wherein, , , respectively represent the rotation angle pre-integral, the velocity pre-integral, the position pre-integral from the i-th sample point to the k-th sample point of the IMU, , respectively represent the rotation angle pre-integral, the velocity pre-integral, the position pre-integral from the i-th sample point to the k-th sample point of the IMU, represents an exponential mapping, and respectively represent the angular velocity measurement value and the acceleration measurement value of the k-th sample point of the IMU, represents the angular velocity bias, represents the acceleration bias, represents the time interval between the k+1-th sample point and the k-th sample point, , respectively represent the rotation angle pre-integral, the velocity pre-integral from the i-th sample point to the k-th sample point, , respectively are a cumulative multiplication symbol and a cumulative addition symbol; S2.2.2: constructing a residual error of the IMU pre-integration model in S2.2.1, and the specific expression is as follows: wherein, denotes a logarithmic map, the superscript denotes a transpose, , denotes the prior pose to be solved under the IMU pre-integrated model, , , are the pre-integrated rotation residual, velocity residual, and position residual, respectively, , , denotes the rotation, velocity, and position of the last position point estimated by pre-integration, denotes the velocity of the current position, denotes the time interval between two position points; S2.3: constructing a residual error of the prior GPS position, and the specific expression is as follows: wherein, represents a prior GPS position, represents position information, and respectively represent the GPS position in the x direction and the y direction in meters, represents the x, y direction position in the prior pose to be solved; S2.4: Constructing the probability density function The specific expression is as follows: wherein, , are variances of the measurements of the laser radar, IMU, respectively, denotes the variance of the measurements of the GPS, , , denote the residual with respect to the laser radar point cloud line plane feature matching, the residual with respect to the IMU pre-integration model, the residual of the prior GPS position, respectively, when is maximum, the obtained is the prior pose , , denotes the exponential operation, denotes the pose to be solved; S2.5: constructing a least square problem based on S2.4, and solving the least square problem by using a nonlinear optimization LM method, and the specific expression is as follows: wherein, represents a minimum value function, , , respectively represent the weighted residual sum of squares about the laser radar point cloud line-surface feature matching, the residual sum of squares about the IMU pre-integral model, and the residual sum of squares of the prior GPS position. S2.6: Prior pose , The time stamp of the GPS data at this time is added to form the prior pose information.

4. The robot automatic repositioning method based on multi-element prior information alignment according to claim 3, characterized in that: The step of extracting prior point cloud information is specifically converting the laser radar point cloud to the prior pose, and the specific expression is as follows: wherein, represents each point of the laser radar point cloud, represents the point converted to the prior pose, , represents the prior pose, and the set of prior point clouds is added to the set of prior point clouds to obtain a prior point cloud, and the GPS data timestamp at this time is appended to constitute prior point cloud information.

5. The robot automatic repositioning method based on multi-element prior information alignment according to claim 4, characterized in that: In the step S4, the real-time GPS position information is aligned with the prior GPS position information to obtain coarse position information, which is specifically that the prior GPS position information corresponding to the minimum distance in the horizontal direction between each prior GPS position and the real-time GPS position is the coarse position information.

6. The robot automatic repositioning method based on multi-element prior information alignment according to claim 5, characterized in that: In the step S5, the coarse position information is time-aligned with the prior pose information and the prior point cloud information to obtain coarse pose and point cloud to be aligned, which is specifically that the prior pose of the prior pose information with the same timestamp as the timestamp of the coarse position information is taken as the coarse pose, and the prior point cloud of the prior point cloud information with the same timestamp as the timestamp of the coarse position information is taken as the point cloud to be aligned.

7. The robot automatic repositioning method based on multi-element prior information alignment according to claim 6, characterized in that: In the step S6, the real-time laser radar point cloud is projected onto the coarse pose to obtain coarse pose point cloud, and the specific expression is as follows: wherein, representing each point of the real-time lidar point cloud, , representing the rotation and translation of the coarse pose, projecting the points onto the coarse pose, collecting all sets of points to the set of real-time point clouds to obtain the coarse pose point cloud.

8. The robot automatic repositioning method based on multi-element prior information alignment according to claim 7, characterized in that: In the step S7, the coarse pose point cloud is fine-aligned with the point cloud to be aligned, a fine pose is calculated, and a repositioning result is obtained, and 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.

Images