Robot mapping and positioning method, device and equipment and storage medium

Abstract

The invention discloses a robot mapping and positioning method, device and equipment and a storage medium, and relates to the technical field of synchronous positioning and mapping, and the method comprises the steps: carrying out the adaptive filtering of a two-dimensional laser radar point cloud of a current environment, screening the filtered point cloud, and obtaining angular points and line points; fitting the line points to obtain fitted line segments, acquiring a local sub-map, and performing local correlation feature matching on the local sub-map and the filtered point cloud based on the angular points, the fitted line segments and the point cloud features of each region of the current environment to acquire a rough pose of the robot; robot poses are obtained through feature constraints determined based on the robot rough poses, and a grid map and a feature map are constructed according to the robot poses; historical robot poses at different moments are obtained and optimized, and the grid map and the feature map are adjusted according to the optimized robot poses. According to the invention, high-precision mapping and positioning are realized.

Description

Technical Field

[0001] The present invention relates to the technical field of simultaneous localization and mapping, and particularly relates to a method, device, equipment and storage medium for robot mapping and localization. Background Art

[0002] Simultaneous localization and mapping is a process in which a logistics robot determines its current pose using sensor observation information and historical map information in an unknown environment, and constructs a map through historical poses, which solves the problems of simultaneous localization and mapping. Although there are already many methods for mapping and localization, there are still many problems in actual applications, such as positioning loss or instability, mapping failure or unsatisfactory results, and poor positioning and mapping accuracy. Therefore, a stable and high-precision mapping and localization method is needed to meet the requirements of complex scenario applications. Summary of the Invention

[0003] In view of this, the purpose of the present invention is to provide a method, device, equipment and storage medium for robot mapping and localization, which can complete mapping and localization stably and with high precision, and meet the requirements of actual complex scenarios. The specific solutions are as follows:

[0004] In a first aspect, the present application discloses a method for robot mapping and localization, including:

[0005] Obtain the two-dimensional lidar point cloud of the current environment, perform adaptive filtering on the two-dimensional lidar point cloud to obtain the filtered point cloud, and screen the filtered point cloud to obtain corner points and line points;

[0006] Fit the line points to obtain a fitted line segment, obtain a local sub-map according to the historical map, and perform local correlation feature matching based on the corner points, the fitted line segment, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud to obtain the rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map;

[0007] Determine feature constraints based on the rough pose of the robot, obtain the pose of the robot through the feature constraints, and construct a grid map and a feature map according to the pose of the robot;

[0008] Obtain the historical robot poses at different times, optimize each historical robot pose, and adjust the grid map and the feature map according to the optimized robot poses.

[0009] Optionally, the performing adaptive filtering on the two-dimensional lidar point cloud to obtain the filtered point cloud includes:

[0010] Divide the two-dimensional lidar point cloud according to the grid based on the current first resolution to obtain the divided point cloud;

[0011] Calculate the center of the point cloud after all divisions in each grid, adjust the current first resolution to obtain a new current first resolution, and then jump back to the step of dividing the two-dimensional lidar point cloud according to the grid based on the current first resolution until, when the number of jumps meets the preset condition, determine the filtered point cloud according to the center of the point cloud after division.

[0012] Optionally, the filtering of the filtered point cloud to obtain corner points and line points includes:

[0013] Calculate the smoothness of the filtered point cloud, and filter the filtered point cloud according to the smoothness and a target threshold to obtain corner points and line points; wherein, if the number of the filtered point cloud is less than the target threshold, determine the filtered point cloud as the corner points.

[0014] Optionally, the fitting of the line points to obtain a fitted line segment includes:

[0015] Divide the line points into regions according to a second resolution to obtain a number of divided regions;

[0016] Cluster the line points in each of the divided regions to obtain a number of clustered line points;

[0017] Perform line segment fitting on each of the clustered line points in each of the divided regions to obtain a fitted line segment.

[0018] Optionally, the obtaining of the rough pose of the robot by performing local correlation feature matching based on the corner points, the fitted line segments, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud includes:

[0019] Determine a local search space, and calculate a first matching degree between the filtered point cloud in the local search space and the grid sub-map;

[0020] Calculate a second matching degree between the corner points, the fitted line segments and the point cloud features of each region of the current environment in the local search space and the feature sub-map;

[0021] Determine a target matching degree according to the first matching degree and the second matching degree, and obtain the rough pose of the robot corresponding to the target matching degree.

[0022] Optionally, the determining of a feature constraint based on the rough pose of the robot and obtaining the pose of the robot through the feature constraint includes:

[0023] Calculate the feature constraints based on the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, and the corner points, the fitted line segments, and the point cloud features of each region of the current environment;

[0024] Calculate the cost function according to the feature constraints, the rough pose of the robot, the filtered point cloud, the grid submap, the feature submap, and the corner points, the fitted line segments, and the point cloud features of each region of the current environment;

[0025] Obtain the robot pose based on the cost function and the rough pose of the robot.

[0026] Optionally, optimizing each of the historical robot poses and adjusting the grid map and the feature map according to the corresponding optimized robot poses includes:

[0027] Calculate the constraints between key nodes according to the robot pose;

[0028] Determine the constraints between key nodes and submaps according to the robot pose and the local submap;

[0029] Determine the constraints between submaps according to the local submap;

[0030] Determine the constraint residuals between nodes based on the constraints between key nodes and the robot pose;

[0031] Determine the constraint residuals between key nodes and submaps through the robot pose, the local submap, and the constraints between key nodes and submaps;

[0032] Determine the constraint residuals between submaps according to the local submap and the constraints between submaps;

[0033] Perform constraint residual optimization through the constraint residuals between nodes, the constraint residuals between key nodes and submaps, and the constraint residuals between submaps to adjust and optimize each of the historical robot poses;

[0034] Adjust the grid map and the feature map according to the transformation relationship between the optimized robot pose and the historical robot pose.

[0035] In a second aspect, the present application discloses a robot mapping and positioning device, including:

[0036] A corner point and line point acquisition module, configured to acquire the two-dimensional lidar point cloud of the current environment, perform adaptive filtering on the two-dimensional lidar point cloud to obtain the filtered point cloud, and screen the filtered point cloud to obtain corner points and line points;

[0037] A rough pose acquisition module, configured to fit the line points to obtain a fitted line segment, obtain a local sub-map according to a historical map, and perform local correlation feature matching on the corner points, the fitted line segment, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud to obtain a rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map;

[0038] A map construction module, configured to determine feature constraints based on the rough pose of the robot, obtain the robot pose through the feature constraints, and construct a grid map and a feature map according to the robot pose;

[0039] A map determination module, configured to obtain historical robot poses at different times, optimize each of the historical robot poses, and adjust the grid map and the feature map according to the optimized robot poses.

[0040] In a third aspect, the present application discloses an electronic device, including:

[0041] A memory, configured to store a computer program;

[0042] A processor, configured to execute the computer program to implement the robot mapping and positioning method as described above.

[0043] In a fourth aspect, the present application discloses a computer-readable storage medium, configured to store a computer program; wherein, when the computer program is executed by a processor, the robot mapping and positioning method as described above is implemented.

[0044] When this application performs mapping and positioning, it first obtains the 2D lidar point cloud of the current environment, adaptively filters the 2D lidar point cloud to obtain the filtered point cloud, and screens the filtered point cloud to obtain corner points and line points; then fits the line points to obtain the fitted line segments, obtains the local sub-map according to the historical map, and performs local correlation feature matching based on the corner points, the fitted line segments, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud to obtain the rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map; then determines the feature constraints based on the rough pose of the robot, obtains the robot pose through the feature constraints, constructs a grid map and a feature map according to the robot pose; finally obtains the historical robot poses at different times, optimizes each historical robot pose, and adjusts the grid map and the feature map according to the corresponding optimized robot poses. It can be seen that after adaptively filtering the lidar point cloud in this application, the point cloud is screened to obtain corner points and line points; using the point cloud features and the environmental map features, feature matching and constraint optimization fine matching are performed to calculate the robot pose, and a grid map and a feature map are established. Finally, constraints are calculated, and the pose is optimized according to the constraints and the grid map and the feature map are adjusted. In this way, by providing more accurate and reliable pose information, this application can improve the stability of the mapping algorithm, solve the problems of unstable and deviated robot mapping, ensure the quality of mapping, be more suitable for actual application scenarios, and at the same time provide a more accurate map and positioning to meet the requirements of the precise docking scenario. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only the embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained according to the provided drawings without creative efforts.

[0046] Figure 1 It is a flowchart of a robot mapping and positioning method disclosed in this application;

[0047] Figure 2 It is a schematic diagram of a 2D lidar point cloud disclosed in this application;

[0048] Figure 3 It is a schematic diagram of the filtered point cloud disclosed in this application;

[0049] Figure 4 It is a schematic diagram of a corner point disclosed in this application;

[0050] Figure 5 It is a schematic diagram of a line point disclosed in this application;

[0051] Figure 6 A schematic diagram of a fitting line segment disclosed in this application;

[0052] Figure 7 A schematic diagram of a grid map disclosed in this application;

[0053] Figure 8 A schematic diagram of a feature sub - map disclosed in this application;

[0054] Figure 9(a) is a schematic diagram of the slope of a line segment disclosed in this application; Figure 9(b) is a schematic diagram of the distance between adjacent line segments disclosed in this application; Figure 9(c) is a schematic diagram of the angle between adjacent line segments disclosed in this application;

[0055] Figure 10 A schematic diagram of a regional feature disclosed in this application;

[0056] Figure 11(a) is a schematic diagram of the line - line distance constraint disclosed in this application; Figure 11(b) is a schematic diagram of the point - line distance constraint disclosed in this application;

[0057] Figure 12 A flowchart of a specific robot mapping and positioning method disclosed in this application;

[0058] Figure 13 A schematic diagram of the constraint between key nodes disclosed in this application;

[0059] Figure 14 A schematic diagram of the constraint between a key node and a local sub - map disclosed in this application;

[0060] Figure 15 A schematic diagram of the constraint between sub - maps disclosed in this application;

[0061] Figure 16 A schematic diagram of the structure of a robot mapping and positioning device disclosed in this application;

[0062] Figure 17 A structural diagram of an electronic device disclosed in this application. Detailed implementation manners

[0063] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0064] Current mapping and positioning methods may have problems such as lost or unstable positioning, failed mapping or unsatisfactory results, and poor accuracy of positioning and mapping. To solve the above technical problems, the present application discloses a robot mapping and positioning method, device, equipment, and storage medium, which can complete mapping and positioning stably and with high precision, meeting the requirements of actual complex scenarios.

[0065] See Figure 1 As shown, an embodiment of the present invention discloses a robot mapping and positioning method, including:

[0066] Step S11: Obtain the two-dimensional lidar point cloud of the current environment, perform adaptive filtering on the two-dimensional lidar point cloud to obtain the filtered point cloud, and screen the filtered point cloud to obtain corner points and line points.

[0067] In this embodiment, the present application first obtains the two-dimensional lidar point cloud information of the current environment, which is expressed as: , as Figure 2 shown; where 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. Then, perform adaptive filtering on the two-dimensional lidar point cloud to obtain the filtered point cloud , as Figure 3 shown. Specifically, divide the two-dimensional lidar point cloud according to the grid based on the current first resolution to obtain the divided point cloud; calculate the center of all the divided point clouds in each grid, adjust the current first resolution to obtain a new current first resolution, and then jump back to the step of dividing the two-dimensional lidar point cloud according to the grid based on the current first resolution until the number of jumps meets the preset condition, and then determine the filtered point cloud according to the center of the divided point cloud. Specifically,

[0068] According to the resolution divide the two-dimensional lidar point cloud by the grid to obtain the divided point cloud G: ;

[0069] Then, calculate the center of all the point clouds in each grid : ;

[0070] Adjust the resolution , and loop through the above two steps to obtain the filtered point cloud : ;

[0071] Among them, represents the grid division function (a function provided by the program library for dividing grids, such as the meshgrid function), represents the grid point cloud computing function (including but not limited to the mean function), n represents the number of grids, represents the loop function (including but not limited to the while loop function), and the loop ends when the condition is satisfied; represents the point cloud after the i-th division. And can be understood as a parameter. If it is 2, it means looping twice; if it is 3, it means looping three times.

[0072] After obtaining the filtered point cloud, the filtered point cloud is screened to obtain corner points and line points. When selecting fewer filtered point clouds, the smoothness of the filtered point cloud is calculated, and the filtered point cloud is screened according to the smoothness and the target threshold to obtain corner points and line points; among them, if the number of the filtered point cloud is less than the target threshold, the filtered point cloud is determined as a corner point. Specifically, calculate the point cloud smoothness S1: ; Screen corner points according to the smoothness threshold th1 ; Line points :

[0073] ;

[0074] Among them, represents the smoothness function (a function provided by the program library to calculate smoothness, including but not limited to the smooth function, calculateSmoothness() function). When the number of the filtered point cloud is less than the preset point cloud number threshold th2, it is defaulted to be processed as a corner point, represents the screening function (a function that realizes screening through numerical comparison. For example, if the number of the filtered point cloud is less than the preset point cloud number threshold th2, then the point cloud is defaulted to be a corner point, and the comparison of the point cloud smoothness is no longer carried out; if the number of the filtered point cloud is greater than or equal to the preset point cloud number threshold th2, then the comparison of the point cloud smoothness is carried out: the point cloud smoothness S1 of a certain filtered point cloud is 3. If the smoothness threshold th1 is 1, then the point cloud smoothness S1 is greater than the smoothness threshold th1, and this filtered point cloud is a line point; if the smoothness threshold th1 is 4, then the point cloud smoothness S1 is less than the smoothness threshold th1, and this filtered point cloud is a corner point). The filtered point cloud with a small smoothness belongs to the corner point. As shown, the line points are as Figure 4 shown, and the corner points are as Figure 5 shown.

[0075] Step S12: Fit the line points to obtain a fitted line segment, obtain a local sub-map according to the historical map, and perform local correlation feature matching based on the corner points, the fitted line segment, the point cloud features of each region in the current environment, and the local sub-map and the filtered point cloud to obtain the rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map.

[0076] In this embodiment, after obtaining the line points and corner points, fit the line points, divide the line points into regions according to the second resolution, and obtain a number of divided regions; cluster the line points in each divided region to obtain a number of clustered line points; perform line segment fitting on each clustered line point in each divided region to obtain a fitted line segment. Specifically, according to the resolution Divide the laser point cloud by region to obtain a number of divided regions R: ; then cluster the line points in each divided region to obtain a number of clustered line points : ; then perform line segment fitting on each clustered line point in each divided region to obtain a fitted line segment : ; where represents the region division function (a function provided by the program library for dividing regions, such as the cut function in the Pandas library), represents the point cloud clustering function, including but not limited to the Euclidean clustering function, the Kmeans clustering function, etc., represents the line segment fitting function, including but not limited to the least squares fitting function, the RANSAC (Random Sample Consensus) fitting function, etc., and the result is as Figure 6 shown, where n1 represents the number of laser regions.

[0077] Then obtain the local sub-map from the historical map according to the robot pose and the sub-map origin pose , in this application, the local sub-map is as Figure 7 and Figure 8 shown, including a grid sub-map and a feature sub-map , which is composed of the laser point cloud of local key nodes. Specifically, when obtaining the grid sub-map and the feature sub-map, define the sub-map origin pose and the robot current pose, and then perform relative transformation calculation of the pose on the sub-map origin pose and the robot current pose to convert the robot pose to the local coordinate system of the sub-map. Then obtain the sub-map origin adjacent to the robot pose, and obtain the grid sub-map from the global grid map according to the first sub-map index. Similarly, obtain the sub-map origin adjacent to the robot pose, and obtain the grid sub-map from the global feature map according to the second sub-map index. At the same time, the feature , including corner features, line segment features, and region features; calculating the corner feature CF from the corners, including but not limited to the angle and distance of the corners. Calculating the line segment feature LF from the fitted line segments, as shown in Fig. 9(a) the slope of the line segment ( in the figure is the inclination angle of the line segment, and the slope of the line segment can be determined according to the inclination angle), the distance between line segments in Fig. 9(b) ( in the figure is the included angle between the extension lines of the two line segments, and L is the distance between the two line segments) and the angle between line segments in Fig. 9(c) ( in the figure represents the angle between the two line segments), including but not limited to length, slope, endpoints, the angle between line segments, the distance between line segments, etc. Calculating the region feature RF from the point cloud within the region, as Figure 10 shown, including but not limited to the mean of the point cloud within the region, normal distribution, etc. In the figure, the abscissa represents the distance from the point cloud within the region to the center, and the ordinate represents the probability density.

[0078] Then, based on the corner points, fitted line segments, and the point cloud features, local sub - map, and local correlation features of the filtered point cloud in each region of the current environment, a rough match is made to obtain the rough pose of the robot. In this process, first, determine the local search space, and calculate the first matching degree between the filtered point cloud and the grid sub - map in the local search space; calculate the second matching degree between the corner points, fitted line segments, and the point cloud features in each region of the current environment and the feature sub - map; determine the target matching degree according to the first matching degree and the second matching degree, and obtain the rough pose of the robot corresponding to the target matching degree. Specifically:

[0079] First, generate the local search space S according to the interval range: ;

[0080] Calculate the first matching degree between the laser point cloud and the grid sub - map in the search space : ; ;

[0081] Calculate the second matching degree between the point cloud features and the feature sub - map in the search space : ;

[0082] Calculate the final matching degree FS: ;

[0083] Obtain the rough pose of the robot according to the matching degree : ;

[0084] Among them: represents the search space generation function (a function provided by the program library for generating the search space. The specific implementation process is: determine the pose according to the coordinates and orientation angle of the robot , with a resolution of 1 (the minimum adjustment step of the position coordinates), (the minimum unit of angle adjustment) to generate a search space. Then, each degree of freedom is adjusted by adding or subtracting the resolution step based on its current value to form candidate poses. The generated search space is , , , , , and other combinations), represents calculating the matching degree function (such as CSM (Correlative Scan Matching, scan matching algorithm), ICP (Iterative Closest Point, point cloud registration algorithm provided by the PCL (Point Cloud Library, an open-source, cross-platform point cloud processing library)), etc.), represents the number of poses in the search space, represents the weighted average function, represents the pose screening function (i.e., the maximum value function. The specific implementation process is as follows: If the matching degree of the search space is 0.9 and the matching degree of the search space is 0.8, then the search space corresponding to the maximum matching degree is selected, and the screened pose is ).

[0085] Step S13: Determine the feature constraints based on the rough pose of the robot, obtain the pose of the robot through the feature constraints, and construct a grid map and a feature map according to the pose of the robot.

[0086] In this embodiment, the feature constraints are calculated based on the rough pose of the robot, the filtered point cloud, the grid sub-map, the feature sub-map, and the point cloud features of the corner points, fitted line segments, and each area of the current environment; the cost function is calculated according to the feature constraints, the rough pose of the robot, the filtered point cloud, the grid sub-map, the feature sub-map, and the point cloud features of the corner points, fitted line segments, and each area of the current environment; the pose of the robot is obtained based on the cost function and the rough pose of the robot. Specifically, calculate the feature constraint C: ;

[0087] Calculate the residual cost CF:

[0088] ;

[0089] Optimize the cost function to obtain the accurate pose of the robot :

[0090] ;

[0091] Among them, B|D means calculating according to B with D as the premise. For example, means with as the premise to calculate the precise pose of the CF robot. means the constraint calculation function (a function for calculating constraints based on matrix operations, such as the MaybeAddConstraint function in ConstraintBuilder2D, the ComputeConstraint function in ConstraintBuilder2D, etc., at the robot pose coordinate system, the features in the feature and sub-map ( , ) are consistent, then calculate the distance difference of the feature ( the feature in) in the sub-map coordinate system. Multiply one matrix (feature pose) by the inverse of another matrix (sub-map origin pose) to obtain the relative motion constraint in the coordinate system), as shown in the point-line distance constraint in Figure 11(a) and the line-line distance constraint in Figure 11(b), including but not limited to point-point distance constraint, point-line distance constraint, line-line distance constraint, point cloud grid map constraint, means calculating the cost function (a function for calculating costs based on matrix operations, such as the CreateOccupiedSpaceCostFunction2D function, the TranslationDeltaCostFunctor2D function, the RotationDeltaCostFunctor2D function, etc. One matrix (a certain feature in or point cloud, at the pose) minus another matrix (feature constraint in the sub-map), calculate the Euclidean distance, Manhattan distance, etc., and determine the cost of the difference between the two matrices by the value obtained through matrix subtraction combined with a specific distance metric), means the optimization function (provided by the non-linear optimizer, continuously iterating through numerical optimization methods (Gauss-Newton method, gradient descent method) to reduce the CF residual (cost), such as the AutoDiffCostFunction function in ceres).

[0092] After obtaining the robot pose, the positioning is completed. After obtaining the robot pose, a grid map and a feature map are constructed according to the robot pose. Specifically, according to the pose of the robot in the map, the latest frame of point cloud is moved to the robot pose, and a grid map can be constructed. The features of the latest frame of point cloud are moved to the robot pose, and a feature map can be constructed.

[0093] Step S14: Obtain the historical robot poses at different times, optimize each of the historical robot poses, and adjust the grid map and the feature map according to the corresponding optimized robot poses.

[0094] In this embodiment, after the present application obtains the robot pose and constructs the grid map and the feature map, the grid map and the feature map can be further adjusted by optimizing the robot pose. Specifically, calculate the constraints between key nodes according to the robot pose; determine the constraints between the key nodes and the sub-map according to the robot pose and the local sub-map; determine the constraints between sub-maps according to the local sub-map; determine the constraint residuals between nodes based on the constraints between key nodes and the robot pose; determine the constraint residuals between the key nodes and the sub-map through the robot pose, the local sub-map, and the constraints between the key nodes and the sub-map; determine the constraint residuals between sub-maps according to the local sub-map and the constraints between sub-maps; optimize the constraint residuals through the constraint residuals between nodes, the constraint residuals between the key nodes and the sub-map, and the constraint residuals between sub-maps to adjust and optimize each historical robot pose; adjust the grid map and the feature map according to the transformation relationship between the optimized robot pose and the historical robot pose. Among them, the historical robot pose is the robot pose at different times, and there is a transformation relationship between different poses, such as rotation and translation. When adjusting the pose by optimizing the residuals, it is to solve by the most optimized iteration. Simply put, it is to continuously reduce the residuals between the constraints. For example, the robot sees feature A at a certain moment and obtains a constraint C1. After a period of time, the robot comes to the vicinity of feature A again and will also obtain a constraint C2. (Robot positioning is an estimation process, and the error of the positioning pose will become larger over time). There will be an error between constraint C1 and C2, but feature A remains unchanged. Therefore, by optimizing the residuals between the constraints, the error of the robot positioning pose will be reduced, that is, the robot pose is adjusted.

[0095] There is a transformation relationship between the pose before optimization and the pose after optimization. Simply put, the pose before optimization is translated and rotated to obtain the pose after optimization. Therefore, the grid map and the feature map can be adjusted according to the transformation relationship between the optimized robot pose and the historical robot pose.

[0096] In summary, when the present application performs mapping and positioning, it first obtains the two-dimensional lidar point cloud of the current environment, adaptively filters the two-dimensional lidar point cloud to obtain the filtered point cloud, and screens the filtered point cloud to obtain corner points and line points; then fits the line points to obtain a fitted line segment, obtains a local sub-map according to the historical map, and performs local correlation feature matching based on the corner points, the fitted line segment, the point cloud features of each region of the current environment, the local sub-map, and the filtered point cloud to obtain the rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map; then determines feature constraints based on the rough pose of the robot, obtains the robot pose through the feature constraints, constructs a grid map and a feature map according to the robot pose; finally, obtains the historical robot poses at different times, optimizes each of the historical robot poses, and adjusts the grid map and the feature map according to the corresponding optimized robot poses. It can be seen that after the present application adaptively filters the lidar point cloud, it screens the point cloud to obtain corner points and line points; uses the point cloud features and the environmental map features to perform feature matching and constraint optimization for precise matching, calculates the robot pose, and establishes a grid map and a feature map, and finally calculates the constraints, optimizes the pose according to the constraints, and adjusts the grid map and the feature map. In this way, by providing more accurate and reliable pose information, the present application can improve the stability of the mapping algorithm, solve the problems of unstable and deviated robot mapping, ensure the quality of mapping, be more suitable for actual application scenarios, and at the same time provide a more accurate map and positioning, which can meet the requirements of precise docking scenarios.

[0097] Based on the previous embodiment, it can be known that the present application obtains the historical robot poses at different times, optimizes each of the historical robot poses, and adjusts the grid map and the feature map according to the corresponding optimized robot poses. Next, the determination process of the grid map and the feature map will be described in detail.

[0098] See Figure 12 As shown, the embodiment of the present invention discloses a specific robot mapping and positioning method, including:

[0099] Step S21: Calculate the constraints between key nodes according to the robot pose, determine the constraints between the key nodes and the sub-map according to the robot pose and the local sub-map, and determine the constraints between the sub-maps according to the local sub-map.

[0100] The present application calculates loop constraints ; where, as Figure 13 shown, there are three key nodes, each node has a pose, and these poses can be mutually converted by a rotation and translation matrix, that is, the constraints between the key nodes ; as Figure 14As shown in the figure, the squares represent a grid map or a feature map, the hexagons represent certain key nodes, the dots connected to them by solid lines represent laser point clouds, and the points connected to them by dashed lines represent the origins of sub-maps. The transformation between this key node and the origin of the sub-map can be obtained by a rotation and translation matrix, that is, the constraint between the key node and the local sub-map: ; As Figure 15 shown in the figure, there are two dashed boxes, and each dashed box represents a sub-map. The dots represent the origins of the sub-maps, and the overlapping parts indicate that there are identical parts between the two maps. The transformation between the origins of these two maps can be obtained by a rotation and translation matrix, that is, the constraint between sub-maps: .

[0101] Among them, represents the function for calculating the constraint between nodes (a function for obtaining constraints through matrix calculations. By multiplying the pose matrix of one node by the inverse of the pose matrix of another node, the corresponding constraint between nodes is obtained), n2 represents the number of key nodes, represents the function for calculating the constraint between a node and a sub-map (a function for obtaining constraints through matrix calculations. By multiplying the pose matrix of a node by the inverse of the pose matrix of the origin of the sub-map, the corresponding constraint between the node and the sub-map is obtained), n3 represents the number of sub-maps, represents the function for calculating the constraint between sub-maps (a function for obtaining constraints through matrix calculations. By multiplying the pose matrix of the origin of one sub-map by the inverse of the pose matrix of the origin of another sub-map, the corresponding constraint between sub-maps is obtained); the above constraint calculation functions are all functions for obtaining constraints through matrix calculations. is the pose of the i-th robot; is the j-th local sub-map.

[0102] Step S22: Determine the constraint residuals between nodes based on the constraints between key nodes and the robot pose, determine the constraint residuals between a key node and a sub-map through the robot pose, local sub-map, and the constraint between the key node and the sub-map, and determine the constraint residuals between sub-maps according to the local sub-map and the constraint between sub-maps.

[0103] In this embodiment, the present application determines the constraint residuals between nodes based on the constraints between key nodes and the robot pose :

[0104] ;

[0105] Determine the constraint residuals between a key node and a sub-map through the robot pose, local sub-map, and the constraint between the key node and the sub-map :

[0106] ;

[0107] Determine the constraint residuals between sub - maps based on local sub - maps and the constraints between sub - maps :

[0108] ;

[0109] Among them: represents the function for calculating the constraint residuals between nodes (in this function, the pose matrix of one node is multiplied by the inverse of the pose matrix of another node, and then this result is multiplied by the inverse of the constraint matrix to obtain the residuals), n4 represents the number of constraints between nodes, represents the function for calculating the constraint residuals between a node and a sub - map (in this function, the pose matrix of a node is multiplied by the inverse of the original pose matrix of the sub - map, and then this result is multiplied by the inverse of the constraint matrix to obtain the residuals), n5 represents the number of constraints between a node and a sub - map, represents the function for calculating the constraint residuals between sub - maps (in this function, the original pose matrix of one sub - map is multiplied by the inverse of the original pose matrix of another sub - map, and then this result is multiplied by the inverse of the constraint matrix to obtain the residuals), n6 represents the number of constraints between sub - maps. The above residual functions are all functions that obtain residuals by calculating the Euclidean distance, Manhattan distance, etc.

[0110] Step S23: Optimize the constraint residuals through the constraint residuals between nodes, the constraint residuals between key nodes and sub - maps, and the constraint residuals between sub - maps, so as to adjust and optimize the historical poses of the robot.

[0111] In this embodiment, the present application adjusts the node poses by optimizing the residuals. Specifically, the constraint residuals are optimized through the constraint residuals between nodes, the constraint residuals between key nodes and sub - maps, and the constraint residuals between sub - maps, so as to adjust and optimize the historical poses of the robot.

[0112] ;

[0113] ;

[0114] Among them, represents the residual optimization function (provided by the non - linear optimization library, continuously iterating to reduce the residuals (costs) through numerical optimization methods (Gauss - Newton method, gradient descent method), such as the AutoDiffCostFunction function in ceres), represents the pose adjustment function (a function for adjusting the pose through matrix calculation, such as multiplying the current pose matrix by the inverse of the pose matrix to be adjusted to obtain the adjusted pose matrix, provided by the Eigen matrix library, and realizing pose adjustment through rotation and translation matrices); RE is the optimized residual; is the optimized pose of the robot.

[0115] When optimizing the pose by adjusting the residuals, it is based on the optimal iterative solution. Simply put, it is to continuously reduce the residuals between the constraints. For example, at a certain moment, the robot sees feature A and obtains a constraint C1. After a period of time, the robot comes near feature A again and will obtain a constraint C2. (Robot positioning is an estimation process, and the positioning pose error will increase over time.) There will be an error between constraint C1 and C2, but feature A remains unchanged. Therefore, by optimizing the residuals between the constraints, the positioning pose error of the robot will be reduced, thus adjusting the robot's pose.

[0116] Step S24: Adjust the grid map and the feature map according to the transformation relationship between the optimized robot pose and the historical robot pose.

[0117] In this embodiment, there will be a transformation relationship between the pose before optimization and the pose after optimization. Simply put, the pose before optimization is transformed into the pose after optimization through translation and rotation. Therefore, the grid map and the feature map can be adjusted according to the transformation relationship between the optimized robot pose and the historical robot pose.

[0118] In this way, the present application optimizes the pose through constraints, further adjusts the grid map and the feature map, improves the stability of mapping and positioning, and also ensures the quality and accuracy of mapping and positioning. It can solve the problems of mapping failure or unstable deviation, and better meet the requirements of complex scene applications.

[0119] See Figure 16 As shown, the embodiment of the present invention discloses a robot mapping and positioning device, including:

[0120] Corner and line point acquisition module 11, configured to acquire the two-dimensional lidar point cloud of the current environment, perform adaptive filtering on the two-dimensional lidar point cloud, acquire the filtered point cloud, and screen the filtered point cloud to acquire corner points and line points;

[0121] Rough pose acquisition module 12, configured to fit the line points to obtain a fitted line segment, acquire a local sub-map according to the historical map, and perform local correlation feature matching based on the corner points, the fitted line segment, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud to acquire the rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map;

[0122] Map construction module 13, configured to determine feature constraints based on the rough pose of the robot, acquire the robot pose through the feature constraints, and construct a grid map and a feature map according to the robot pose;

[0123] A map determination module 14, configured to obtain historical robot poses at different times, optimize each of the historical robot poses, and adjust the grid map and the feature map according to the optimized robot poses.

[0124] In summary, when the present application performs mapping and positioning, it first obtains the two-dimensional lidar point cloud of the current environment, adaptively filters the two-dimensional lidar point cloud to obtain the filtered point cloud, and screens the filtered point cloud to obtain corner points and line points; then fits the line points to obtain a fitted line segment, obtains a local sub-map according to the historical map, and performs local correlation feature matching based on the corner points, the fitted line segment, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud to obtain a rough robot pose; the local sub-map includes a grid sub-map and a feature sub-map; then determines a feature constraint based on the rough robot pose, obtains the robot pose through the feature constraint, constructs a grid map and a feature map according to the robot pose; finally obtains the historical robot poses at different times, optimizes each of the historical robot poses, and adjusts the grid map and the feature map according to the corresponding optimized robot poses. It can be seen that after the present application adaptively filters the lidar point cloud, it screens the point cloud to obtain corner points and line points; uses the point cloud features and the environmental map features to perform feature matching and constraint optimization for precise matching, calculates the robot pose, and constructs a grid map and a feature map, and finally calculates the constraint, optimizes the pose according to the constraint and adjusts the grid map and the feature map. In this way, by providing more accurate and reliable pose information, the present application can improve the stability of the mapping algorithm, solve the problems of unstable and deviated robot mapping, ensure the quality of mapping, be more suitable for actual application scenarios, and at the same time provide a more accurate map and positioning, which can meet the requirements of precise docking scenarios.

[0125] In some specific embodiments, the corner point and line point acquisition module 11 may specifically include:

[0126] A point cloud division unit, configured to divide the two-dimensional lidar point cloud based on a current first resolution according to a grid to obtain the divided point cloud;

[0127] A jump unit, configured to calculate the center of all the divided point clouds in each grid, adjust the current first resolution to obtain a new current first resolution, and re-jump to the step of dividing the two-dimensional lidar point cloud based on the current first resolution according to the grid until the number of jumps meets a preset condition, and then determine the filtered point cloud according to the center of the divided point cloud.

[0128] In some specific embodiments, the corner point and line point acquisition module 11 may specifically include:

[0129] A corner point and line point acquisition unit, configured to calculate the smoothness of the filtered point cloud, and screen the filtered point cloud according to the smoothness and a target threshold to obtain corner points and line points; wherein, if the number of the filtered point cloud is less than the target threshold, the filtered point cloud is determined as the corner points.

[0130] In some specific embodiments, the rough pose acquisition module 12 may specifically include:

[0131] A region division unit, configured to divide the line points according to a second resolution to obtain a plurality of divided regions;

[0132] A line point clustering unit, configured to cluster the line points in each of the divided regions to obtain a plurality of clustered line points;

[0133] A line segment fitting unit, configured to perform line segment fitting on each of the clustered line points in each of the divided regions to obtain fitted line segments.

[0134] In some specific embodiments, the rough pose acquisition module 12 may specifically include:

[0135] A first matching degree calculation unit, configured to determine a local search space and calculate a first matching degree between the filtered point cloud in the local search space and the grid sub-map;

[0136] A second matching degree calculation unit, configured to calculate a second matching degree between the corner points, the fitted line segments and the point cloud features of each region of the current environment in the local search space and the feature sub-map;

[0137] A rough pose acquisition unit, configured to determine a target matching degree according to the first matching degree and the second matching degree, and obtain the rough pose of the robot corresponding to the target matching degree.

[0138] In some specific embodiments, the map construction module 13 may specifically include:

[0139] A feature constraint calculation unit, configured to calculate the feature constraints based on the rough pose of the robot, the filtered point cloud, the grid sub-map, the feature sub-map, and the corner points, the fitted line segments and the point cloud features of each region of the current environment;

[0140] A cost function calculation unit, configured to calculate a cost function according to the feature constraints, the rough pose of the robot, the filtered point cloud, the grid sub-map, the feature sub-map, and the corner points, the fitted line segments and the point cloud features of each region of the current environment;

[0141] A robot pose acquisition unit, configured to acquire the robot pose based on the cost function and the rough robot pose.

[0142] In some specific embodiments, the map determination module 14 may specifically include:

[0143] A first constraint calculation unit, configured to calculate the constraints between key nodes according to the robot pose;

[0144] A second constraint determination unit, configured to determine the constraints between the key nodes and the sub-map according to the robot pose and the local sub-map;

[0145] A third constraint determination unit, configured to determine the constraints between sub-maps according to the local sub-map;

[0146] A first residual determination unit, configured to determine the constraint residual between nodes based on the constraints between key nodes and the robot pose;

[0147] A second residual determination unit, configured to determine the constraint residual between the key node and the sub-map through the robot pose, the local sub-map, and the constraints between the key node and the sub-map;

[0148] A third residual determination unit, configured to determine the constraint residual between sub-maps according to the local sub-map and the constraints between sub-maps;

[0149] A pose adjustment unit, configured to optimize the constraint residuals through the constraint residuals between nodes, the constraint residuals between the key node and the sub-map, and the constraint residuals between sub-maps, so as to adjust and optimize each of the historical robot poses;

[0150] A map determination unit, configured to adjust the grid map and the feature map according to the conversion relationship between the optimized robot pose and the historical robot pose.

[0151] Furthermore, the embodiments of the present application also disclose an electronic device, Figure 17 It is a structural diagram of the electronic device 20 shown according to an exemplary embodiment, and the content in the figure should not be considered as any limitation to the scope of use of the present application.

[0152] Figure 17Schematic diagram of the structure of an electronic device 20 provided by an embodiment of the present application. The electronic device 20 may specifically include: at least one processor 21, at least one memory 22, a power supply 23, a communication interface 24, an input / output interface 25, and a communication bus 26. Among them, the memory 22 is used to store a computer program, and the computer program is loaded and executed by the processor 21 to implement the relevant steps in the robot mapping and positioning method disclosed in any of the foregoing embodiments. In addition, the electronic device 20 in this embodiment may specifically be an electronic computer.

[0153] In this embodiment, the power supply 23 is used to provide operating voltage for each hardware device on the electronic device 20; the communication interface 24 can create a data transmission channel between the electronic device 20 and external devices, and the communication protocol it follows is any communication protocol applicable to the technical solution of the present application, and specific limitations are not imposed on it here; the input / output interface 25 is used to obtain external input data or output data to the outside, and its specific interface type can be selected according to specific application needs, and specific limitations are not imposed here.

[0154] In addition, as a carrier for resource storage, the memory 22 may be a read-only memory, a random access memory, a magnetic disk, or an optical disc, etc., and the resources stored thereon may include an operating system 221, a computer program 222, etc., and the storage method may be temporary storage or permanent storage.

[0155] Among them, the operating system 221 is used to manage and control each hardware device and the computer program 222 on the electronic device 20, and it may be Windows Server, Netware, Unix, Linux, etc. In addition to the computer program that can be used to complete the robot mapping and positioning method executed by the electronic device 20 disclosed in any of the foregoing embodiments, the computer program 222 may further include computer programs that can be used to complete other specific tasks.

[0156] Furthermore, the present application also discloses a computer-readable storage medium for storing a computer program; wherein, when the computer program is executed by a processor, the robot mapping and positioning method disclosed above is implemented. For the specific steps of this method, reference may be made to the corresponding content disclosed in the foregoing embodiments, and details are not described herein again.

[0157] In the present specification, the various embodiments are described in a progressive manner. Each embodiment focuses on the differences from other embodiments, and the same or similar parts between the various embodiments may be referred to each other. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts may be referred to the description of the method part.

[0158] Those skilled in the art may further realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be implemented by electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described according to functions in the above description. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered as exceeding the scope of this application.

[0159] The steps of the methods or algorithms described in combination with the embodiments disclosed herein can be directly implemented by hardware, software modules executed by a processor, or a combination of both. The software modules can be placed in a random access memory (RAM), internal memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the technical field.

[0160] Finally, it should also be noted that in this document, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, article or device comprising a series of elements not only includes those elements, but also includes other elements not expressly listed, or elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "comprising an..." does not exclude the presence of additional identical elements in the process, method, article or device comprising the element.

[0161] The technical solutions provided in this application have been introduced in detail above. Specific examples have been used herein to elaborate on the principles and implementation manners of this application. The description of the above embodiments is only used to help understand the method and its core idea of this application; at the same time, for those of ordinary skill in the art, according to the idea of this application, there will be changes in the specific implementation manners and application scopes. In summary, the content of this specification should not be construed as a limitation to this application.

Claims

1. A method for robot mapping and positioning, characterized in that, Including: Obtain the two-dimensional lidar point cloud of the current environment, perform adaptive filtering on the two-dimensional lidar point cloud to obtain the filtered point cloud, and screen the filtered point cloud to obtain corner points and line points; Fit the line points to obtain a fitted line segment, obtain a local sub-map according to the historical map, and perform local correlation feature matching based on the corner points, the fitted line segment, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud to obtain the rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map; Determine feature constraints based on the rough pose of the robot, obtain the robot pose through the feature constraints, and construct a grid map and a feature map according to the robot pose; Obtain the historical robot poses at different times, optimize each historical robot pose, and adjust the grid map and the feature map according to the optimized robot poses.

2. The method for robot mapping and positioning according to claim 1, wherein, The step of performing adaptive filtering on the two-dimensional lidar point cloud to obtain the filtered point cloud includes: Divide the two-dimensional lidar point cloud according to the grid based on the current first resolution to obtain the divided point cloud; Calculate the centers of all the divided point clouds in each grid, adjust the current first resolution to obtain a new current first resolution, and then jump back to the step of dividing the two-dimensional lidar point cloud according to the grid based on the current first resolution until the number of jumps meets the preset condition, and then determine the filtered point cloud according to the centers of the divided point clouds.

3. The method for robot mapping and positioning according to claim 1, characterized in that, The step of screening the filtered point cloud to obtain corner points and line points includes: Calculate the smoothness of the filtered point cloud, and screen the filtered point cloud according to the smoothness and the target threshold to obtain corner points and line points; wherein, if the number of the filtered point clouds is less than the target threshold, the filtered point cloud is determined as the corner point.

4. The robot mapping and positioning method according to claim 1, wherein The step of fitting the line points to obtain a fitted line segment includes: Divide the line points according to the second resolution to obtain several divided regions; Cluster the line points in each divided region to obtain several clustered line points; Perform line segment fitting on each of the clustered line points in each divided region to obtain a fitted line segment.

5. The method for robot mapping and positioning according to claim 1, characterized in that, The step of performing local correlation feature matching based on the corner points, the fitted line segment, the point cloud features of each region of the current environment, the local sub-map and the filtered point cloud to obtain the rough pose of the robot includes: Determine the local search space, and calculate the first matching degree between the filtered point cloud and the grid sub-map in the local search space; Calculate the second matching degree between the corner points, the fitted line segment, the point cloud features of each region of the current environment and the feature sub-map in the local search space; Determine the target matching degree according to the first matching degree and the second matching degree, and obtain the rough pose of the robot corresponding to the target matching degree.

6. The method for robot mapping and positioning according to claim 1, characterized in that The step of determining feature constraints based on the rough pose of the robot and obtaining the robot pose through the feature constraints includes: Calculate the feature constraints based on the rough pose of the robot, the filtered point cloud, the grid sub-map, the feature sub-map, and the corner points, the fitted line segments, and the point cloud features of each region of the current environment; Calculate the cost function according to the feature constraints, the rough pose of the robot, the filtered point cloud, the grid sub-map, the feature sub-map, and the corner points, the fitted line segments, and the point cloud features of each region of the current environment; Obtain the robot pose based on the cost function and the rough pose of the robot.

7. The method for robot mapping and positioning according to any one of claims 1 to 6, characterized in that, Optimizing each of the historical robot poses, and adjusting the grid map and the feature map according to the optimized robot poses, including: Calculating the constraints between key nodes according to the robot pose; Determining the constraints between the key nodes and the sub-map according to the robot pose and the local sub-map; Determining the constraints between sub-maps according to the local sub-map; Determining the constraint residuals between nodes based on the constraints between key nodes and the robot pose; Determining the constraint residuals between the key nodes and the sub-map through the robot pose, the local sub-map, and the constraints between the key nodes and the sub-map; Determining the constraint residuals between sub-maps according to the local sub-map and the constraints between sub-maps; Performing constraint residual optimization through the constraint residuals between nodes, the constraint residuals between the key nodes and the sub-map, and the constraint residuals between sub-maps to adjust and optimize each of the historical robot poses; Adjust the grid map and the feature map according to the transformation relationship between the optimized robot pose and the historical robot pose.

8. A robot mapping and positioning device, characterized in that, Including: A corner point and line point acquisition module, configured to acquire the 2D lidar point cloud of the current environment, perform adaptive filtering on the 2D lidar point cloud to obtain the filtered point cloud, and screen the filtered point cloud to obtain corner points and line points; A rough pose acquisition module, configured to fit the line points to obtain fitted line segments, obtain a local sub-map according to the historical map, and perform local correlation feature matching based on the corner points, the fitted line segments, the point cloud features of each region of the current environment, the local sub-map, and the filtered point cloud to obtain the rough pose of the robot; the local sub-map includes a grid sub-map and a feature sub-map; A map construction module, configured to determine feature constraints based on the rough pose of the robot, obtain the robot pose through the feature constraints, and construct a grid map and a feature map according to the robot pose; A map determination module, configured to obtain the historical robot poses at different times, optimize each of the historical robot poses, and adjust the grid map and the feature map according to the corresponding optimized robot poses.

9. An electronic device, characterized in that, Including: A memory, configured to store a computer program; A processor, configured to execute the computer program to implement the robot mapping and positioning method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, For storing a computer program; wherein, when the computer program is executed by the processor, the robot mapping and positioning method according to any one of claims 1 to 7 is implemented.

Images