Robot positioning and repositioning method based on point cloud feature matching

Abstract

The invention relates to the technical field of logistics robot positioning, in particular to a robot positioning and repositioning method based on point cloud feature matching. The method comprises the specific steps that S1, two-dimensional laser radar point cloud information of a current environment, the initial position and posture of a logistics robot in the current environment and a current environment map are obtained, and the current environment map comprises a two-dimensional grid map and a feature map; s2, matching the position and attitude of the logistics robot by using the two-dimensional laser radar point cloud features, and performing CFSM rough matching and constraint optimization fine matching by using the two-dimensional laser radar point cloud features and environmental map features; s3, outputting real-time pose information of the robot; according to the method, the pose information of the robot can be quickly obtained, and the real-time requirement of a navigation scene is met; according to the invention, the problems of unreliable environment positioning and large error of the robot can be solved, and more accurate and reliable pose information can be provided; the problem of repositioning after the pose of the robot is lost can be solved, and the stability of a positioning algorithm is improved.

Description

Technical Field

[0001] The present invention belongs to the technical fields of computer application technology and positioning algorithms for logistics robots, and particularly relates to a method for robot positioning and relocalization based on point cloud feature matching. Background Art

[0002] With the development of intelligent robot technology, various robots such as industrial handling robots can automatically complete various handling tasks. Among them, high-precision positioning technology is an important technology for determining the position and posture of logistics robots in the environment, and is a prerequisite for logistics robots to achieve path planning and autonomous navigation. Currently, most positioning methods rely on pre-established two-dimensional grid maps, and different grid resolutions, environmental changes, dynamic obstacles, etc. all affect the positioning accuracy. The deviation of positioning is difficult to meet the needs of precise docking scenarios. At the same time, if the positioning result deviation is too large, the robot will lose its position and be unable to complete basic autonomous navigation tasks. Currently, most solutions are to push the robot to a designated position for relocalization, such as charging piles, positions marked on the map, etc., lacking flexibility. Summary of the Invention

[0003] Aiming at the deficiencies of the prior art, the purpose of the present invention is to provide a method for robot positioning and relocalization based on point cloud feature matching, which can not only provide accurate and reliable position and posture of the robot, but also can flexibly relocalize after the position and posture of the robot are lost, meeting the requirements of actual application scenarios.

[0004] To achieve the above purpose, the present invention adopts the following technical solutions:

[0005] A method for robot positioning and relocalization based on point cloud feature matching, the specific steps are as follows:

[0006] S1: Obtain the two-dimensional lidar point cloud information of the current environment, the initial position and posture of the logistics robot in the current environment, and the current environment map, where the current environment map includes a two-dimensional grid map and a feature map;

[0007] S2: Preprocess the obtained two-dimensional lidar point cloud information to obtain lidar point cloud features;

[0008] According to the obtained initial position and posture of the logistics robot, screen the local sub-map;

[0009] If the initial position and posture of the logistics robot in the current environment cannot be obtained in step S1, screen the local sub-map based on the two-dimensional lidar point cloud information and the lidar point cloud features;

[0010] Perform rough matching of relevant features using the two-dimensional lidar point cloud information, lidar point cloud features, and local sub-map;

[0011] Obtain feature constraints according to the rough matching results, optimize the feature constraints, and perform fine matching;

[0012] S3: Obtain the position and pose of the robot according to the matching results between the laser point cloud features and the local sub-map, including the real-time position and pose information of local positioning and the position and pose information of global re-localization.

[0013] Furthermore, the two-dimensional lidar point cloud information is the point cloud in the coordinate system of the logistics robot, which is obtained by converting the point cloud in the lidar coordinate system;

[0014] If the initial position and pose of the logistics robot in the current environment cannot be obtained in step S1, the initial position and pose of the logistics robot are the position and pose of the starting point of the environment map.

[0015] Furthermore, in step S2, the specific steps of screening the local sub-map are as follows:

[0016] Obtain the distance between the position of the logistics robot and the origin of the sub-map in the environment map;

[0017] Screen out the local sub-map according to the distance between the position of the logistics robot and the origin of the sub-map in the environment map.

[0018] Furthermore, in step S2, the specific steps of preprocessing the obtained two-dimensional lidar point cloud information to obtain laser point cloud features are as follows:

[0019] Sample the two-dimensional lidar point cloud information to obtain a sample of the two-dimensional lidar point cloud information;

[0020] For the sample of the two-dimensional lidar point cloud information, calculate and filter the laser point cloud smoothness of the two-dimensional lidar point cloud information sample;

[0021] Obtain the laser point cloud clustering of the two-dimensional lidar point cloud information sample according to the smoothness of the laser point cloud;

[0022] Obtain the laser point cloud region division and line segment fitting according to the obtained laser point cloud clustering;

[0023] Obtain the laser point cloud features according to the obtained laser point cloud region division and line segment fitting.

[0024] Furthermore, in step S2, perform rough matching of relevant features using the two-dimensional lidar point cloud information, laser point cloud features, and local sub-map. The specific steps are as follows:

[0025] Generate a local search space according to a predetermined interval;

[0026] Calculate the matching score between the laser point cloud and the grid sub-map in the search space;

[0027] Calculate the matching score between the laser point cloud features and the feature sub-map in the search space;

[0028] Obtain the final matching score; obtain the rough position and pose of the logistics robot according to the result of the final matching score.

[0029] Further, in the step S2, the specific steps of optimizing the feature constraint and performing fine matching are as follows:

[0030] Obtain the feature constraint according to the rough position and pose of the logistics robot;

[0031] Obtain the cost function according to the feature constraint;

[0032] Optimize the cost function to obtain the accurate position and pose of the robot.

[0033] Further, the real-time position and pose calibration of the logistics robot is also included in the specific steps of optimizing the feature constraint and performing fine matching.

[0034] The beneficial effects of the present invention are as follows:

[0035] (1) Since the logistics robot has a complex scenario in actual applications, such as environmental changes and dynamic obstacles, the positioning method relying only on the grid map will deviate and cannot meet the usage scenario of precise docking, and seriously, the robot pose will be lost. The method of the present invention can provide an accurate and reliable robot pose, which is more suitable for the actual application scenario.

[0036] (2) The loss of robot positioning will lead to the inability to complete basic autonomous navigation tasks. Most current solutions are to push the robot to a designated position for repositioning, such as charging piles, positions marked on the map, etc., which lack flexibility. The method of the present invention can perform flexible repositioning without relying on a designated position, which is more flexible. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is the overall flowchart of the embodiment of the present invention;

[0038] Figure 2 It is the lidar point cloud of the embodiment of the present invention;

[0039] Figure 3 It is the two-dimensional grid map of the embodiment of the present invention;

[0040] Figure 4 It is the point cloud feature map of the embodiment of the present invention;

[0041] Figure 5 It is the filtered point cloud of the embodiment of the present invention;

[0042] Figure 6 The partial fitting line segment described in the embodiment of the present invention;

[0043] Figure 7 The line segment feature described in the embodiment of the present invention;

[0044] Figure 8 shows the feature between line segments described in the embodiment of the present invention;

[0045] Figure 9 The point cloud grid map constraint described in the embodiment of the present invention;

[0046] Figure 10 The line-line constraint described in the embodiment of the present invention;

[0047] Figure 11 The point-line constraint described in the embodiment of the present invention. Specific implementation manner

[0048] Next, in combination with the accompanying drawings and specific implementation manners, the present invention will be further described:

[0049] Embodiment 1

[0050] This embodiment provides as Figure 1 shown, the object of the present invention is to provide a robot positioning and relocalization method based on point cloud feature matching, including the following steps: S1 obtaining known information, S2 point cloud feature matching, and S3 outputting the robot pose.

[0051] Step S1, obtaining known information, including two-dimensional lidar point cloud information in the current environment, the initial position and orientation of the logistics robot in the current environment, and the current environment map.

[0052] In this embodiment, the two-dimensional lidar point cloud information of the current environment is represented as P laser =(x, y), as Figure 2 shown.

[0053] In this embodiment, the initial position and orientation of the logistics robot in the current environment are represented as P robot =(x, y, β).

[0054] In this embodiment, the current environment map is represented as M=(M G , M F ), including a two-dimensional grid map M G , as Figure 3 shown, and a point cloud feature map M F , as Figure 4 shown.

[0055] Step S2: Use the two-dimensional lidar point cloud features and environmental map features to perform CSM rough matching and constraint optimization fine matching; including local positioning with a given initial pose and global relocalization with an unknown initial pose.

[0056] In this embodiment, local positioning is applicable to the case where the robot pose is known and includes the following process:

[0057] According to the known position and pose of the logistics robot, filter the local sub-map M sub ∈M;

[0058] Preprocess the two-dimensional lidar point cloud information P laser to obtain the lidar point cloud feature L F ;

[0059] According to P laser , L F and M sub perform local correlation feature rough matching CFSM;

[0060] According to the rough matching result, calculate the feature constraints, and then optimize the constraints for fine matching;

[0061] In this embodiment, the two-dimensional lidar point cloud information is the point cloud in the robot coordinate system, which is obtained by converting the point cloud in the lidar coordinate system. The process is as follows: where P robot _coord =(x, y, β), P laser_coord =(x laser_coord , y laser_coord , β laser_coord ), is the rotation and translation matrix.

[0062] In this embodiment, when the initial position and pose of the logistics robot are not given, the position and pose of the starting point of the environmental map are used by default.

[0063] In this embodiment, the case where the position and pose of the logistics robot are known includes two cases: given precise position and pose and given rough position and pose. The deviation between the given rough position and pose and the actual position and pose is not large.

[0064] In this embodiment, the process of filtering the local sub-map is as follows:

[0065] Determine the distance between the position of the logistics robot and the origin of the sub-map in the environmental map M:

[0066] Dist i =f dist (P robot , M i ), i<M num

[0067] Determine the optimal sub-map M based on the distance between the position of the logistics robot and the origin of the sub-map in the environmental map M sub :

[0068] M sub = f select (Dist, M)

[0069] Where: f dist Represents the calculation distance function, which can be Euclidean distance, Manhattan distance, etc., and f select Represents the sub-map screening function, i represents the i-th sub-map, and M num Represents the number of sub-maps in the map M.

[0070] In this embodiment, the preprocessing process of the two-dimensional lidar point cloud information is as follows:

[0071] Downsampling of two-dimensional lidar point cloud information:

[0072] P′ laser = f downsampling (P laser )

[0073] Calculate the smoothness of the two-dimensional lidar point cloud according to the selected samples and perform filtering, as Figure 5 shown:

[0074] P″ laser = f filter (f smooth (P' laser ))

[0075] Obtain the two-dimensional lidar point cloud clustering from the smoothness of the two-dimensional lidar point cloud:

[0076] P″′ laser = f clustering (P″ laser )

[0077] Perform two-dimensional lidar point cloud region division and line segment fitting according to the two-dimensional lidar point cloud clustering, as Figure 6 shown:

[0078] Lines = f fitting (f seg (P″′ laser ))

[0079] Obtain the two-dimensional lidar point cloud features based on the two-dimensional lidar point cloud clustering and the two-dimensional lidar point cloud region division and line segment fitting:

[0080] L F = f features (P″′ laser , Lines)

[0081] where: f downsampling represents the downsampling function, f smooth represents the function for calculating smoothness, f filter represents the point cloud filtering function, f clustering represents the clustering function, f seg represents the region division function, f fitting represents the line fitting function, f features represents the feature calculation function, the laser point cloud feature L F contains several regional point cloud features L f , each L f contains the following features:

[0082] The two-dimensional lidar point cloud feature Point features , including but not limited to the normal distribution of the point cloud, the angle and distance of the point cloud center;

[0083] The two-dimensional lidar point cloud fitting line segment feature Line features , as Figure 7 shown, including but not limited to the line segment angle, distance, slope, center point, and end point;

[0084] The feature between adjacent fitting line segments of the two-dimensional lidar point cloud Line' features , as shown in Figures 8(a) and 8(b), including but not limited to the angle and distance between adjacent line segments.

[0085] In this embodiment, the local correlation feature matching CFSM process is as follows:

[0086] Generate a local search space according to the interval range:

[0087] S = f search (range)

[0088] Calculate the matching score between the two-dimensional lidar point cloud and the grid map in the search space:

[0089]

[0090] Calculate the matching score between the two-dimensional lidar point cloud feature and the feature sub-map in the search space:

[0091]

[0092] Calculate the final matching score:

[0093] FS = f Weighted_average ((FS′ j , FS″ j|S))

[0094] Obtain the rough position and pose of the logistics robot according to the matching result:

[0095] P′ robot = f select (FS, S)

[0096] Where: f search represents the search space generation function, f fit_scorce represents the calculation of the matching degree function, S num represents the number of poses in the search space, f Weighted_average represents the weighted average function, f select represents the pose screening function.

[0097] The process of constraint optimization and fine matching in this embodiment is as follows:

[0098] Calculate the feature constraints according to the rough pose of the logistics robot:

[0099]

[0100] Calculate the cost function according to the feature constraints:

[0101]

[0102] Optimize the cost function to obtain the accurate position and pose of the logistics robot:

[0103] P robot = f optimize (CF|P′ robot )

[0104] Where: f constraint represents the constraint calculation function, including but not limited to point-line distance constraints, as Figure 9 shown, line-line distance constraints, as Figure 10 shown, point cloud grid map constraints, as Figure 11 shown. f cost represents the calculation of the cost function, f optimize represents the optimization function.

[0105] The global relocalization in this embodiment is applicable to the case where the robot pose is unknown and includes the following process:

[0106] Preprocess the two-dimensional lidar point cloud information P laser , and obtain the two-dimensional lidar point cloud feature L F ;

[0107] According to P laser , L F , screen the local sub-map M sub ∈M;

[0108] According to P laser , L F and M sub perform a rough matching of correlation features CFSM;

[0109] According to the rough matching result, calculate the feature constraints, and then optimize the constraints for fine matching;

[0110] In this embodiment, the situation where the position and attitude of the logistics robot are unknown includes two cases: the initial position and attitude are not given or the position and attitude of the logistics robot are lost.

[0111] In this embodiment, the preprocessing process of the two-dimensional lidar point cloud information is as follows:

[0112] Downsampling of the two-dimensional lidar point cloud information:

[0113] P′ laser = f downsampling (P laser )

[0114] Calculate the smoothness of the two-dimensional lidar point cloud based on the selected samples and perform filtering:

[0115] P″ laser = f filter (f smooth (P′ laser ))

[0116] Obtain the two-dimensional lidar point cloud clustering based on the smoothness of the two-dimensional lidar point cloud:

[0117] P″′ laser = f clustering (P″ laser )

[0118] Perform two-dimensional lidar point cloud region division and line segment fitting based on the two-dimensional lidar point cloud clustering:

[0119] Lines = f fitting (f seg (P″′ laser ))

[0120] Obtain the two-dimensional lidar point cloud features based on the two-dimensional lidar point cloud clustering and two-dimensional lidar point cloud region division and line segment fitting:

[0121] L F = f features (P″′ laser , Lines)

[0122] Where: f downsampling represents the downsampling function, f smoothRepresents the calculation smoothness function, f filter Represents the point cloud filtering function, f chustering Represents the clustering function, f seg Represents the region division function, f fiting Represents the line segment fitting function, f features Represents the feature calculation function, the laser point cloud feature L F Contains several regional point cloud features L f , each L f Contains the following features:

[0123] Two-dimensional lidar point cloud feature Point features , including but not limited to the normal distribution of the point cloud, the angle and distance of the point cloud center;

[0124] Two-dimensional lidar point cloud fitting line segment feature Line features , including but not limited to the line segment angle, distance, slope, center point, end point;

[0125] Feature between adjacent fitting line segments of two-dimensional lidar point cloud Line' features , including but not limited to the angle and distance between adjacent line segments.

[0126] In this embodiment, the process of screening the local sub-map is as follows:

[0127] Calculate the two-dimensional lidar point cloud feature L F And the sub-map in the environmental map M The matching score of:

[0128]

[0129] Obtain the best sub-map M sub :

[0130] M sub = f select (Score, M)

[0131] Where: f fit Represents the calculation matching degree function, f select Represents the sub-map screening function, i represents the i-th sub-map, M num Then represents the number of sub-maps in the map M.

[0132] In this embodiment, the global correlation feature matching CFSM process is as follows:

[0133] Generate a local search space according to the interval range':

[0134] S G = f search (range')

[0135] Calculate the matching score between the 2D lidar point cloud and the grid sub-map in the search space :

[0136]

[0137] Calculate the matching score between the 2D lidar point cloud features and the feature sub-map in the search space :

[0138]

[0139] Calculate the final matching score:

[0140] GFS = f Weighted_average (GFS′ j , GFS″ j |S G )

[0141] Obtain the rough position and pose of the logistics robot according to the matching result:

[0142] GP′ robot = f select (GFS, S G )

[0143] Where: f search represents the search space generation function, f fit_scorce represents the function for calculating the matching degree, S num represents the number of poses in the search space, f Weighted_average represents the weighted average function, f select represents the pose screening function.

[0144] The process of the constrained optimization and fine matching in this embodiment is as follows:

[0145] Calculate the feature constraint according to the rough pose of the logistics robot:

[0146]

[0147] Calculate the cost function according to the feature constraint:

[0148]

[0149] Optimize the cost function to obtain the accurate position and pose of the logistics robot:

[0150] GP robot = f optimize (GCF|GP′ robot )

[0151] Real-time position and pose calibration of the logistics robot:

[0152] P robot = f calibration (P′ robot , GP′ robot )

[0153] where: f constraint represents a constraint calculation function, including but not limited to point-line distance constraint, line-line distance constraint, point cloud raster map constraint, f cost represents a calculation cost function, f optimize represents an optimization function, f calibration represents a pose calibration function.

[0154] Step S3: Obtain the position and pose of the logistics robot according to the matching result between the 2D lidar point cloud and the sub-map.

[0155] An implementation method of robot positioning and repositioning based on point cloud feature matching in the present invention obtains known information, including 2D lidar point cloud, the initial position and pose of the logistics robot, and the environmental map, and performs constraint optimization matching on the 2D lidar point cloud features and the environmental map. The output of the position and pose of the logistics robot is accurate and reliable, the repositioning is more flexible, and it can better meet the requirements of practical applications.

[0156] Although the present invention has been described in detail by referring to the accompanying drawings and in combination with the preferred embodiments, the present invention is not limited thereto. Without departing from the spirit and essence of the present invention, those of ordinary skill in the art can make various equivalent modifications or substitutions to the embodiments of the present invention, and these modifications or substitutions should all be within the scope of the present invention. / Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, which should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

[0157] For those skilled in the art, various corresponding changes and deformations can be made according to the technical solutions and concepts described above, and all these changes and deformations should belong to the protection scope of the claims of the present invention.

Claims

1. A robot positioning and relocalization method based on point cloud feature matching, characterized in that The specific steps are as follows: S1: Obtain the two-dimensional lidar point cloud information of the current environment, the initial position and attitude of the logistics robot in the current environment, and the current environment map, where the current environment map includes a two-dimensional grid map and a feature map; S2: Preprocess the obtained two-dimensional lidar point cloud information to obtain lidar point cloud features; According to the obtained initial position and attitude of the logistics robot, filter the local sub-map; If the initial position and attitude of the logistics robot in the current environment cannot be obtained in step S1, filter the local sub-map based on the two-dimensional lidar point cloud information and the lidar point cloud features; Use the two-dimensional lidar point cloud information, lidar point cloud features, and local sub-map to perform rough matching of relevant features; According to the rough matching result, obtain feature constraints, optimize the feature constraints, and perform fine matching; S3: Obtain the position and attitude of the robot according to the matching result between the lidar point cloud features and the local sub-map, including the real-time position and attitude information of local positioning and the position and attitude information of global repositioning.

2. The robot positioning and relocalization method based on point cloud feature matching according to claim 1, wherein The two-dimensional lidar point cloud information is the point cloud in the coordinate system of the logistics robot, which is converted from the point cloud in the lidar coordinate system; If the initial position and attitude of the logistics robot in the current environment cannot be obtained in step S1, the initial position and attitude of the logistics robot are the position and attitude of the starting point of the environment map.

3. The robot positioning and relocalization method based on point cloud feature matching according to claim 1, characterized in that, In step S2, the steps of filtering the local sub-map are specifically as follows: Obtain the distance between the position of the logistics robot and the origin of the sub-map in the environment map; Filter the local sub-map according to the distance between the position of the logistics robot and the origin of the sub-map in the environment map.

4. A robot positioning and relocalization method based on point cloud feature matching according to claim 1, characterized in that In step S2, preprocess the obtained two-dimensional lidar point cloud information to obtain lidar point cloud features. The specific steps are as follows: Sample the two-dimensional lidar point cloud information to obtain a sample of the two-dimensional lidar point cloud information; For the sample of the two-dimensional lidar point cloud information, calculate and filter the lidar point cloud smoothness of the two-dimensional lidar point cloud information sample; Obtain the lidar point cloud clustering of the two-dimensional lidar point cloud information sample according to the smoothness of the lidar point cloud; Obtain the lidar point cloud region division and line segment fitting according to the obtained lidar point cloud clustering; Obtain the lidar point cloud features according to the obtained lidar point cloud region division and line segment fitting.

5. A robot positioning and relocalization method based on point cloud feature matching according to claim 1, characterized in that, In step S2, use the two-dimensional lidar point cloud information, lidar point cloud features, and local sub-map to perform rough matching of relevant features. The specific steps are as follows: Generate a local search space according to a predetermined interval; Calculate the matching score between the lidar point cloud and the grid sub-map in the search space; Calculate the matching score between the lidar point cloud features and the feature sub-map in the search space; Obtain the final matching score; obtain the rough position and attitude of the logistics robot according to the final matching score result.

6. The robot positioning and relocalization method based on point cloud feature matching according to claim 1, characterized in that In step S2, the steps of optimizing the feature constraints and performing fine matching are specifically as follows: Obtain feature constraints according to the rough position and attitude of the logistics robot; Obtain a cost function according to the feature constraints; Optimize the cost function to obtain the accurate position and attitude of the robot.

7. The robot positioning and relocalization method based on point cloud feature matching according to claim 6, characterized in that, The steps of optimizing the feature constraints and performing fine matching specifically also include the real-time position and attitude calibration of the logistics robot.

Images