A pose estimation method, apparatus, device and computer readable storage medium

Abstract

The application discloses a pose estimation method and device, equipment and a computer readable storage medium, comprising: obtaining the odometer distance corresponding to the moving object in the structured scene; collecting the original point cloud data set of the structured scene; extracting each feature line segment from the original point cloud data set; wherein the original point cloud data set comprises a plurality of feature line segments composed of point subsets; calculating the heading angle of the moving object according to the positive direction of the acquisition terminal corresponding to each feature line segment and the original point cloud data set; and estimating the pose of the moving object in the structured scene according to the odometer distance and the heading angle. The application does not need to additionally modify the existing scene, avoids interference with the normal working environment on site, and can accurately calculate the pose of the robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of object space pose estimation, in particular to a pose estimation method, device, equipment and computer readable storage medium. BACKGROUND

[0002] In some structured scenes, such as long shelves, long shelves and some scenes with long legs supporting the bottom, regular placement and narrow channels in the middle, a mobile object (such as a robot) carries a point cloud scanning device to run in the scene. The point cloud data obtained by the point cloud scanning device is often too sparse, lacks obvious scene features, and the point cloud data collected at different positions has high repeatability, so that the traditional point cloud registration method (such as iterative closest point and normal distribution transformation) cannot accurately determine the position of the mobile object.

[0003] The current commonly used pose estimation method of the mobile object in the structured scene mainly includes two kinds. One is to increase some regularly distributed markers by artificial means for pose estimation, such as spraying paint at fixed intervals, placing fixed shape object combinations at fixed intervals, or laying magnetic strips on the ground. The other is to estimate the position of the mobile object by using the odometer alone. However, the above two methods have their own shortcomings. First, the method of setting markers needs to modify the environment on site, which not only increases the deployment cost, but also may interfere with the normal working environment on site. And these modification measures may be damaged over time, such as paint fading, fixed objects being moved, magnetic strips demagnetizing, etc., thereby interfering with the positioning of the mobile object, and even causing the mobile object to collide with the on-site equipment and accidents. Second, the method of using the odometer alone will produce a large error over time for the estimation of the lateral and directional positions, which is difficult to meet the positioning requirements.

[0004] In summary, how to effectively solve the problems of the current pose estimation method, such as high deployment cost, interference with the normal working environment on site, poor safety, and low pose estimation accuracy, is a problem that needs to be solved by the technical personnel in the field at present. SUMMARY

[0005] The purpose of the present application is to provide a pose estimation method which does not need to modify the existing scene, avoids interference with the normal working environment on site, and can accurately calculate the pose of the robot. Another purpose of the present application is to provide a pose estimation device, equipment and computer readable storage medium.

[0006] To solve the above technical problems, the present application provides the following technical solutions:

[0007] A pose estimation method, comprising:

[0008] obtaining a distance of a moving object in a structured scene corresponding to an odometer;

[0009] obtaining an original point cloud data set of the structured scene;

[0010] extracting each feature line segment from the original point cloud data set; wherein the original point cloud data set comprises a plurality of feature line segments each composed of a point subset;

[0011] calculating a heading angle of the moving object according to each feature line segment and a positive direction of a terminal corresponding to the original point cloud data set;

[0012] estimating a pose of the moving object in the structured scene according to the distance of the odometer and the heading angle.

[0013] In one embodiment of the present application, extracting each feature line segment from the original point cloud data set comprises:

[0014] filtering point cloud data of a preset region from the original point cloud data set to obtain a regional point cloud data set;

[0015] performing Euclidean clustering on the regional point cloud data set to obtain each clustered line segment;

[0016] adjusting point cloud data of each clustered line segment to obtain each feature line segment.

[0017] In one embodiment of the present application, filtering point cloud data of a preset region from the original point cloud data set comprises:

[0018] obtaining a target radius of a preset circular region with the moving object as the center;

[0019] estimating a first lateral distance and a first longitudinal distance of the moving object in a channel of the structured scene according to the distance of the odometer;

[0020] performing point cloud data filtering on the original point cloud data set according to the target radius, the first lateral distance and the first longitudinal distance.

[0021] In one embodiment of the present application, adjusting point cloud data of each clustered line segment comprises:

[0022] performing point cloud data filtering on each clustered line segment through bounding box calculation to obtain each filtered line segment;

[0023] performing straight line model estimation on each filtered line segment using a random sample consensus algorithm.

[0024] In one embodiment of the present application, performing point cloud data filtering on each clustered line segment through bounding box calculation comprises:

[0025] The bounding box calculation is performed on each cluster line segment by using the oriented bounding box to obtain each bounding box;

[0026] A preset bounding box short side threshold and a preset bounding box long side threshold are obtained;

[0027] The point cloud data in each bounding box is filtered according to the preset bounding box short side threshold and the preset bounding box long side threshold.

[0028] In one specific embodiment of the present application, the heading angle of the moving object is calculated according to each feature line segment and the positive direction of the acquisition terminal corresponding to the original point cloud data set, comprising:

[0029] The included angles between each feature line segment and the positive direction of the acquisition terminal are calculated respectively to obtain a heading angle set;

[0030] A preset angle difference threshold is obtained;

[0031] Each included angle in the heading angle set is clustered by using the Euclidean clustering algorithm according to the angle difference threshold to obtain each angle class;

[0032] A preset angle difference threshold is obtained;

[0033] Each included angle in the heading angle set is clustered by using the Euclidean clustering algorithm according to the angle difference threshold to obtain each angle class;

[0034] The mean value of each included angle in each angle class is calculated to obtain each angle mean value;

[0035] The odometer angle corresponding to the moving object is obtained;

[0036] The angle mean value with the smallest difference from the odometer angle is selected from each angle mean value;

[0037] The selected angle mean value is determined as the heading angle.

[0038] In one specific embodiment of the present application, after obtaining the heading angle set, before clustering each included angle in the heading angle set by using the Euclidean clustering algorithm according to the angle difference threshold, further comprising:

[0039] Each included angle in the heading angle set is increased by 90 degrees respectively to obtain each first expanded angle;

[0040] Each included angle in the heading angle set is reduced by 90 degrees respectively to obtain each second expanded angle;

[0041] Each included angle in the heading angle set is increased by 180 degrees respectively to obtain each third expanded angle;

[0042] adding each first expansion angle, each second expansion angle and each third expansion angle to the set of heading angles;

[0043] correspondingly, performing mean value calculation on each included angle in each angle class, including:

[0044] performing size sorting on the number of included angles in each angle class to obtain an angle class sequence;

[0045] selecting a preset number of angle classes from one end of the angle class sequence with a large number;

[0046] performing mean value calculation on each included angle in each selected angle class;

[0047] correspondingly, determining the heading angle as the mean value of the selected angle, including:

[0048] obtaining a relative angle between the positive direction of the mobile object and the positive direction of the collection terminal;

[0049] determining the heading angle according to the mean value of the selected angle and the relative angle.

[0050] In one specific embodiment of the present application, estimating the pose of the mobile object in the structured scene according to the odometer distance and the heading angle, including:

[0051] rotating the point cloud formed by each feature line segment according to the mean value of the angle, so that the positive direction of the collection terminal is parallel to the passage direction of the mobile object in the structured scene;

[0052] determining a second lateral distance of the mobile object in the passage of the structured scene by using a statistical histogram;

[0053] estimating a second longitudinal distance of the mobile object in the passage of the structured scene according to the odometer distance and the second lateral distance;

[0054] estimating the pose of the mobile object in the structured scene according to the heading angle, the second lateral distance and the second longitudinal distance.

[0055] A pose estimation device, comprising:

[0056] an odometer distance acquisition module configured to acquire an odometer distance corresponding to a mobile object in a structured scene;

[0057] an original point cloud data set acquisition module configured to acquire an original point cloud data set of the structured scene;

[0058] a feature line segment extraction module configured to extract feature line segments from the original point cloud data set, wherein the original point cloud data set includes a plurality of feature line segments each composed of a point subset;

[0059] a heading angle calculation module configured to calculate a heading angle of the moving object according to the feature line segments and a positive direction of a terminal corresponding to the original point cloud data set;

[0060] a pose estimation module configured to estimate a pose of the moving object in the structured scene according to the odometry distance and the heading angle.

[0061] A pose estimation device comprises:

[0062] a memory configured to store a computer program;

[0063] a processor configured to implement steps of the pose estimation method as described above when executing the computer program.

[0064] A computer readable storage medium having a computer program stored thereon, the computer program being configured to implement steps of the pose estimation method as described above when executed by a processor.

[0065] The pose estimation method provided in the present application obtains an odometry distance corresponding to a moving object in a structured scene, collects an original point cloud data set of the structured scene, extracts feature line segments from the original point cloud data set, wherein the original point cloud data set includes a plurality of feature line segments each composed of a point subset, calculates a heading angle of the moving object according to the feature line segments and a positive direction of a terminal corresponding to the original point cloud data set, and estimates a pose of the moving object in the structured scene according to the odometry distance and the heading angle.

[0066] As can be seen from the above technical solution, by collecting an original point cloud data set of a structured scene, extracting and screening line features in the structured scene, calculating a heading angle of a moving object by using the line features, and then estimating a pose of the moving object in the structured scene according to an odometry distance and the heading angle, the extracted feature line segments are information under the original structured scene, the calculation of the pose is realized by extracting inherent information in the scene, and no additional modification is needed for the existing scene, so there is no problem of feature degradation over time, the normal working environment of the scene is not disturbed, and the safety of the moving object in the structured scene is improved. By analyzing the existing features and combining the odometry information of the moving object, the pose of the robot can be accurately calculated.

[0067] Correspondingly, the present application also provides a pose estimation apparatus, device and computer readable storage medium corresponding to the above-mentioned pose estimation method, which have the above technical effects, and will not be described here again. BRIEF DESCRIPTION OF DRAWINGS

[0068] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the technical solutions in the related art, the drawings needed to be used in the embodiments or the related art description will be briefly introduced. Obviously, the drawings in the following description only constitute some embodiments of the present application, and for those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0069] Figure 1 An implementation flowchart of a pose estimation method in an embodiment of the present application;

[0070] Figure 2 An implementation flowchart of another pose estimation method in an embodiment of the present application;

[0071] Figure 3 A schematic diagram of a raw point cloud data set in an embodiment of the present application;

[0072] Figure 4 A schematic diagram of another raw point cloud data set in an embodiment of the present application;

[0073] Figure 5 A schematic diagram of a structured scene composed of horizontal and vertical feature line segments in an embodiment of the present application;

[0074] Figure 6 A schematic diagram of a directional bounding box and an axis-aligned bounding box in an embodiment of the present application;

[0075] Figure 7 A schematic diagram of a state of a moving object in a structured scene in an embodiment of the present application;

[0076] Figure 8 A schematic diagram of a lateral distance extraction process in an embodiment of the present application;

[0077] Figure 9 A statistical histogram for lateral distance statistics in an embodiment of the present application;

[0078] Figure 10 A structural block diagram of a pose estimation apparatus in an embodiment of the present application;

[0079] Figure 11 A structural block diagram of a pose estimation device in an embodiment of the present application;

[0080] Figure 12 A specific structural schematic diagram of a pose estimation device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0081] For the person skilled in the art to better understand the present application, the present application will be further described in detail below in combination with the drawings and specific embodiments. Obviously, the described embodiments are only part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor are within the scope of protection of the present application.

[0082] Referring to Figure 1 , Figure 1 An implementation flowchart of the pose estimation method in the embodiments of the present application can include the following steps:

[0083] S101: Obtain the odometer distance corresponding to the mobile object in the structured scene.

[0084] The mobile object itself carries an odometer, which can record the running distance of the mobile object through the odometer carried by the mobile object itself. The odometer distance corresponding to the mobile object in the structured scene is obtained.

[0085] The mobile object can include a robot, an unmanned vehicle, etc.

[0086] The structured scene can include a shelf with a cross beam or a side baffle, a storage rack, or a scene with a wall on one side and a shelf on the other side.

[0087] S102: Collect the original point cloud data set of the structured scene.

[0088] When the mobile object runs in the structured scene, the original point cloud data set of the structured scene is collected, such as using a single-line laser radar to collect the original point cloud data set of the structured scene. The single-line laser radar is generally set at a height that can identify the bottom cross beam or side baffle and some other miscellaneous information.

[0089] S103: Extract each feature line segment from the original point cloud data set.

[0090] The original point cloud data set includes a plurality of feature line segments composed of point subsets.

[0091] The collected original point cloud data set of the structured scene includes structured feature line segments and other information or noise. After the original point cloud data set of the structured scene is collected, each feature line segment is extracted from the original point cloud data set. Each feature line segment can describe the structure of the structured scene to a certain extent.

[0092] S104: Calculate the heading angle of the mobile object according to the positive direction of the collection terminal corresponding to each feature line segment and the original point cloud data set.

[0093] The positive direction of the acquisition terminal for collecting the original point cloud data set is obtained, which can be consistent with the positive direction of the mobile object or have a fixed angle deviation with the positive direction of the mobile object. After the feature line segments are extracted, the heading angle of the mobile object is determined according to the positive direction of the acquisition terminal for collecting the original point cloud data set and the feature line segments, for example, the heading angle of the mobile object can be determined according to the included angle between the feature line segments and the positive direction of the acquisition terminal for collecting the original point cloud data set.

[0094] S105: Estimate the pose of the mobile object in the structured scene according to the odometer distance and the heading angle.

[0095] After the heading angle of the mobile object is calculated and the odometer distance corresponding to the mobile object is obtained, the pose of the mobile object in the structured scene is estimated according to the odometer distance and the heading angle. The starting position of the mobile object in the structured scene can be recorded in advance, for example, the starting position of the mobile object in the structured scene is generally the outlet or inlet of the channel in the structured scene, and the pose of the mobile object in the structured scene can be estimated according to the starting position of the mobile object in the structured scene, the heading angle and the odometer distance.

[0096] As can be seen from the above technical solutions, the original point cloud data set of the structured scene is collected, the line features in the structured scene are extracted and screened, the heading angle of the mobile object is calculated using the line features, and then the pose of the mobile object in the structured scene is estimated according to the odometer distance and the heading angle. Since the extracted feature line segments are information in the original structured scene, the calculation of the pose is realized by extracting the inherent information in the scene, and the existing scene does not need to be modified, so there is no problem of feature degradation over time, and the normal working environment of the scene is not disturbed, thereby improving the safety of the mobile object running in the structured scene. Through analysis of the existing features and combination of the odometer information of the mobile object, the pose of the robot can be accurately calculated.

[0097] It should be noted that based on the above embodiments, the present application also provides corresponding improvement schemes. In subsequent embodiments, the steps involved in the above embodiments or the corresponding steps can be mutually referenced, and the corresponding beneficial effects can also be mutually referenced. In the following improved embodiments, they will not be described one by one.

[0098] Referring to Figure 2 , Figure 2 is an implementation flowchart of another pose estimation method in the embodiments of the present application. The method can include the following steps:

[0099] S201: Obtain the odometer distance corresponding to the mobile object in the structured scene.

[0100] S202: collect an original point cloud data set of a structured scene.

[0101] Referring to Figure 3 , Figure 3 is a schematic diagram of an original point cloud data set in an embodiment of the present application. Referring to Figure 4 , Figure 4 is a schematic diagram of another original point cloud data set in an embodiment of the present application. The original point cloud data set collected by the collection terminal has a structured characteristic line segment, and there are also a lot of other information or noise. The original point cloud data set of the structured scene collected can be named pcInitializaton.

[0102] S203: screen point cloud data of a preset region from the original point cloud data set to obtain a regional point cloud data set.

[0103] The moving object moving in the structured scene will collect point cloud information outside the structured scene near the starting point or the ending point of the scene due to the maximum detection radius of the single-line laser radar carried by the moving object being generally about 10-20 m, and these information cannot be guaranteed to be consistent with the structured, which will interfere with the pose calculation of the moving object. At the same time, considering the characteristics of the laser radar, the accuracy of the point cloud at a long distance is poor, so only the point cloud within a certain size (such as filterSize) around the moving object is retained. Therefore, after collecting the original point cloud data set of the structured scene, the point cloud data of a preset region is screened from the original point cloud data set to obtain a regional point cloud data set. By screening the point cloud data of the preset region from the original point cloud data set, the interference of the point cloud information outside the structured scene on the pose calculation of the moving object is avoided.

[0104] In a specific embodiment of the present application, screening the point cloud data of a preset region from the original point cloud data set can include the following steps:

[0105] Step one: obtain a target radius of a circular region with the moving object as the center;

[0106] Step two: estimate a first lateral distance and a first longitudinal distance of the moving object in the channel of the structured scene according to the odometer distance;

[0107] Step three: screen the point cloud data of the original point cloud data set according to the target radius, the first lateral distance and the first longitudinal distance.

[0108] For convenience of description, the above three steps can be combined for description.

[0109] In the process of screening the point cloud data of the preset area from the original point cloud data set, the target radius of the preset circular area with the mobile object as the center is obtained, the first lateral distance and the first longitudinal distance of the mobile object in the channel of the structured scene are estimated according to the odometer distance, and the point cloud data of the original point cloud data set is screened according to the target radius, the first lateral distance and the first longitudinal distance.

[0110] For example, in the case of knowing the structured scene information in advance, especially the length and width of the structured scene, assuming that the length is scene_x and the width is scene_y, the extraction range [xMin, yMin], [xMax, yMax] of the point cloud of the laser radar is set in advance according to the first lateral distance position_y and the first longitudinal distance position_y estimated by the odometer, wherein xMin = position_x-filterSize / 2, xMax = position_x+filterSize / 2, yMin = position_y-filterSize / 2, yMax = position_y+filterSize / 2, and must satisfy xMin >= 0 (the starting point of the scene), xMax <= scene_x, yMax <= scene_y, yMin >= 0. The point cloud data inside the given cube is reserved by using the cube filter, and the filtered point cloud is named pcByCropBoxFilter.

[0111] By screening the point cloud data according to the specified target radius and the estimated first lateral distance and first longitudinal distance of the mobile object in the channel of the structured scene, the accuracy of the screened point cloud data is improved, and the effectiveness of the extracted point cloud data is ensured.

[0112] S204: Euclidean clustering is performed on the regional point cloud data set to obtain each clustering line segment.

[0113] Referring to Figure 5 , Figure 5 is a schematic diagram of a structured scene composed of horizontal and vertical feature line segments in an embodiment of the present application. After the regional point cloud data set is screened from the original point cloud data set, the regional point cloud data set is subjected to Euclidean clustering to obtain each clustering line segment.

[0114] In the above examples, when performing Euclidean clustering on the pcByCropBoxFilter, the maximum distance of points in the same class can be set to 1-3 times the resolution of the laser radar when the terminal is a single-line laser radar, so that the point cloud of the same beam or side can be clustered into a class. According to the understanding of the structured scene, such as the length of the beam in the scene is 1.0m, the resolution of the laser radar is 0.03m, and the actual number of point clouds should be about 33. Because the feature line segment collected by the laser radar in the actual use scene may be smaller than the length in the actual scene, the minimum number of clusters can be set to 0.6-0.9 times the actual number. The number of maximum points in the cluster can be set to be larger, so that the scene with one side as a wall can be extracted.

[0115] By performing Euclidean clustering on the regional point cloud data set, each cluster line segment can be efficiently screened from the regional point cloud data set.

[0116] S205: Adjusting the point cloud data of each cluster line segment to obtain each feature line segment.

[0117] Among them, the original point cloud data set includes a plurality of feature line segments composed of point subsets.

[0118] After performing Euclidean clustering on the regional point cloud data set to obtain each cluster line segment, there may still be noise in each cluster line segment, such as some noise at both ends of the cluster line segment. Therefore, the point cloud data of each cluster line segment is adjusted to obtain each feature line segment. By adjusting the point cloud data of each cluster line segment, the noise of each cluster line segment is removed, and the smoothness of each feature line segment obtained is improved.

[0119] In one specific embodiment of the present application, adjusting the point cloud data of each cluster line segment can include the following steps:

[0120] Step one: filtering the point cloud data of each cluster line segment by bounding box calculation to obtain each filtered line segment;

[0121] Step two: using the random sample consensus algorithm to estimate the straight line model of each filtered line segment.

[0122] For convenience of description, the above two steps can be combined for description.

[0123] In the point cloud data adjustment of each cluster line segment, first, the point cloud data of each cluster line segment is filtered by the bounding box calculation to obtain each filtered line segment. By filtering the point cloud data of each cluster line segment using the bounding box, only the point cloud data within the bounding box is retained, and the remaining point cloud data is deleted, thereby effectively removing the noise. After obtaining each filtered line segment, a straight line model is estimated for each filtered line segment using the random sample consensus (RANSAC) algorithm. The random sample consensus algorithm estimates the model parameters by randomly selecting part of the samples in the data set, and evaluates the fitting degree of these parameters to the remaining samples. The number of iterations of the random sample consensus algorithm can be set to about 1000, and the minimum error can be set to about 0.02. The number of inliers and outliers after calculation can be retained, and the straight line model is estimated by counting the percentage of inliers. By estimating the straight line model of each filtered line segment using the random sample consensus algorithm, the outlying points are effectively removed in a visual manner, and the smoothness of each feature line segment estimated is improved.

[0124] In one specific embodiment of the present application, filtering the point cloud data of each cluster line segment by bounding box calculation can include the following steps:

[0125] Step 1: Calculate the bounding box of each cluster line segment using an oriented bounding box (OBB) to obtain each bounding box.

[0126] Step 2: Obtain a preset bounding box short side threshold and a preset bounding box long side threshold.

[0127] Step 3: Filter the point cloud data within each bounding box according to the preset bounding box short side threshold and the preset bounding box long side threshold.

[0128] For convenience of description, the above three steps can be combined for description.

[0129] Considering that the edges of the oriented bounding box (OBB) are as close as possible to the real distribution of the point cloud and are aligned with the local coordinate system of the object, and the axis-aligned bounding box (AABB) is aligned with the x, y, and z axes of the world coordinate system, when filtering the point cloud data of each cluster line segment by bounding box calculation, the oriented bounding box is used to calculate the bounding box of each cluster line segment to obtain each bounding box.

[0130] The bounding box short side threshold and the bounding box long side threshold are preset, the preset bounding box short side threshold and the preset bounding box long side threshold are obtained, and the point cloud data within each bounding box is filtered according to the preset bounding box short side threshold and the preset bounding box long side threshold.

[0131] Referring toFigure 6 , Figure 6 Figure 1 is a schematic diagram of a directional bounding box and an axis-aligned bounding box in an embodiment of the present application. For the same point cloud feature, Figure 6 The OBB and the case of the AABB calculation are shown, and the dotted line is the OBB result and the solid line is the AABB result. The long side and the short side of the bounding box obtained by OBB calculation are obtained. Since the line feature is extracted, the short side should be the size of the laser radar resolution in the ideal case, but considering the accuracy problem, the short side can be set to 2 or 3 times the laser radar resolution, and the long side is set to 0.6-0.9 times the actual line feature length in the scene. By adding this constraint condition, only clusters that meet the constraint condition are retained, and the rest of the point cloud is deleted, thereby realizing the filtering of point cloud data in each clustered line segment using the directional bounding box, which can reduce the volume or area of the bounding box, thereby more accurately adapting to the shape and direction of the clustered line segment, and further improving the smoothness of the clustered line segment.

[0132] S206: Calculate the included angle between each feature line segment and the positive direction of the collection terminal to obtain a heading angle set.

[0133] After obtaining each feature line segment, the included angle theta between each feature line segment and the positive direction of the collection terminal is calculated to obtain a heading angle set.

[0134] S207: Add 90 degrees to each included angle in the heading angle set to obtain each first expanded angle.

[0135] After calculating the heading angle set, 90 degrees are added to each included angle in the heading angle set to obtain each first expanded angle.

[0136] S208: Subtract 90 degrees from each included angle in the heading angle set to obtain each second expanded angle.

[0137] After calculating the heading angle set, 90 degrees are subtracted from each included angle in the heading angle set to obtain each second expanded angle.

[0138] S209: Add 180 degrees to each included angle in the heading angle set to obtain each third expanded angle.

[0139] After calculating the heading angle set, 180 degrees are added to each included angle in the heading angle set to obtain each third expanded angle.

[0140] S210: Add each first expanded angle, each second expanded angle, and each third expanded angle to the heading angle set.

[0141] After obtaining each first expanded angle, each second expanded angle, and each third expanded angle, each first expanded angle, each second expanded angle, and each third expanded angle are added to the heading angle set.

[0142] Referring to Figure 7 , Figure 7 is a schematic diagram of a state of a mobile object in a structured scene in an embodiment of the present application. By angle expansion on the heading angle set, the calculated n angles are all expanded into an angle array, n*4 angle values are obtained, four possible states of the mobile object in the structured scene are obtained by expansion, and expansion of the states of the mobile object in the structured scene is achieved.

[0143] S211: Obtain a preset angle difference threshold value.

[0144] The angle difference threshold value is preset, and the preset angle difference threshold value is obtained.

[0145] S212: Cluster each included angle in the heading angle set according to the angle difference threshold value by using the Euclidean clustering algorithm to obtain each angle class.

[0146] After the heading angle set is expanded and the preset angle difference threshold value is obtained, each included angle in the heading angle set is clustered according to the angle difference threshold value by using the Euclidean clustering algorithm, for example, the K-means clustering algorithm can be used to cluster each included angle in the heading angle set to obtain each angle class.

[0147] S213: Calculate the mean value of each included angle in each angle class to obtain each angle mean value.

[0148] After each angle class is clustered, the mean value of each included angle in each angle class is calculated to obtain each angle mean value.

[0149] In a specific embodiment of the present application, calculating the mean value of each included angle in each angle class can include the following steps:

[0150] Step 1: Sort the number of included angles in each angle class by size to obtain an angle class sequence.

[0151] Step 2: Select the first preset number of angle classes from the end of the angle class sequence with the largest number.

[0152] Step 3: Calculate the mean value of each included angle in each selected angle class.

[0153] For convenience of description, the above three steps can be combined for description.

[0154] After clustering each included angle in the heading angle set to obtain each angle class, the number of included angles in each angle class is sorted in size to obtain an angle class sequence. When sorting the number of included angles in each angle class in size, the number of included angles in each angle class can be arranged in descending order or in ascending order. By sorting the number of included angles in each angle class in size, the size relationship of the number of included angles in each angle class can be intuitively determined from the angle class sequence. After sorting the number of included angles in each angle class in size to obtain the angle class sequence, the first preset number of angle classes are selected from the end of the angle class sequence with the largest number of included angles.

[0155] It should be noted that the preset number can be set and adjusted according to actual conditions, and the embodiments of the present application do not limit this, for example, it can be set to 3.

[0156] After each angle class is selected, the mean value of each included angle in each selected angle class is calculated to obtain each angle mean value. Assuming that 3 angle classes are selected, the angle mean values corresponding to the 3 angle classes calculated are yaw1, yaw2, and yaw3. By selecting the first preset number of angle classes from the end of the angle class sequence with the largest number of included angles for angle mean value calculation, the accuracy of the heading information calculation result is greatly improved.

[0157] S214: Obtain the odometer angle corresponding to the mobile object.

[0158] The mobile object is recorded in angle by the odometer carried by the mobile object in advance, and the odometer angle odomYaw corresponding to the mobile object is obtained.

[0159] S215: Select the angle mean value with the smallest difference from the odometer angle from the angle mean values.

[0160] After each angle mean value is calculated and the odometer angle is obtained, the angle mean value with the smallest difference from the odometer angle is selected from the angle mean values.

[0161] S216: Determine the selected angle mean value as the heading angle.

[0162] After the angle mean value with the smallest difference from the odometer angle is selected from the angle mean values, the selected angle mean value is determined as the heading angle. By selecting the angle mean value with the smallest difference from the odometer angle as the heading angle, the accuracy of the heading angle calculation result is greatly improved.

[0163] In one specific embodiment of the present application, step S216 can include the following steps:

[0164] Step one: obtain the relative angle between the positive direction of the mobile object and the positive direction of the collection terminal;

[0165] Step two: determine the heading angle according to the angle mean obtained through screening and the relative angle.

[0166] For convenience of description, the above two steps can be combined for description.

[0167] There can be a relative angle between the positive direction of the mobile object and the positive direction of the collection terminal. The relative angle between the positive direction of the mobile object and the positive direction of the collection terminal is obtained. The heading angle position_yaw is determined according to the angle mean obtained through screening and the relative angle. By determining the heading angle according to the relative angle between the positive direction of the mobile object and the positive direction of the collection terminal, the accuracy of the heading angle determination is greatly improved.

[0168] S217: Rotate the point cloud formed by each feature line segment according to the angle mean, so that the positive direction of the collection terminal is parallel to the passage direction of the mobile object in the structured scene.

[0169] After calculating each angle mean, the point cloud formed by each feature line segment is rotated according to the angle mean, so that the positive direction of the collection terminal is parallel to the passage direction of the mobile object in the structured scene.

[0170] S218: Determine the second lateral distance of the mobile object in the passage of the structured scene by using a statistical histogram.

[0171] Referring to Figure 8 , Figure 8 is a schematic diagram of a lateral distance extraction process in an embodiment of the present application. After rotating the passage direction of the mobile object in the structured scene to be parallel to the positive direction of the collection terminal, the second lateral distance position_y of the mobile object in the passage of the structured scene is determined by using a statistical histogram. The distance corresponding to the point with the highest frequency in the statistical histogram is selected as the second lateral distance position_x of the mobile object in the passage of the structured scene.

[0172] Referring to Figure 9 , Figure 9 is a statistical histogram for lateral distance statistics in an embodiment of the present application. If the feature line segments are all in the horizontal direction, a minimum point threshold is set, and the first distance greater than the threshold in the statistical histogram is selected as the second lateral distance of the mobile object in the passage of the structured scene.

[0173] S219: Estimate the second longitudinal distance of the mobile object in the passage of the structured scene according to the odometer distance and the second lateral distance.

[0174] After the odometer distance and the second lateral distance are obtained, the second longitudinal distance of the moving object in the passage of the structured scene is estimated according to the odometer distance and the second lateral distance. The second longitudinal distance can be estimated by subtracting the second lateral distance from the odometer distance, and the starting point of the moving object entering the structured scene can be obtained, which can be set as the starting point of the odometer. The second longitudinal distance can be estimated by subtracting the lateral movement distance from the odometer distance, wherein the lateral movement distance is calculated according to the starting point and the second lateral distance.

[0175] S220: estimating the pose of the moving object in the structured scene according to the heading angle, the second lateral distance and the second longitudinal distance.

[0176] After the heading angle, the second lateral distance and the second longitudinal distance are estimated, the pose of the moving object in the structured scene is estimated according to the heading angle, the second lateral distance and the second longitudinal distance. By estimating the second lateral distance of the moving object in the structured scene by using the statistical histogram, the complexity of determining the second lateral distance in the structured scene is reduced, and the intuitiveness of determining the second lateral distance in the structured scene is improved.

[0177] Corresponding to the above method embodiments, the present application also provides a pose estimation device. The pose estimation device described below can be mutually corresponding with reference to the above-described pose estimation method.

[0178] Referring to Figure 10 , Figure 10 is a structural block diagram of a pose estimation device in an embodiment of the present application. The device can include:

[0179] The odometer distance acquisition module 11 is configured to acquire an odometer distance corresponding to the moving object in the structured scene.

[0180] The original point cloud data set acquisition module 12 is configured to acquire an original point cloud data set of the structured scene.

[0181] The feature line segment extraction module 13 is configured to extract each feature line segment from the original point cloud data set. The original point cloud data set includes a plurality of feature line segments composed of point subsets.

[0182] The heading angle calculation module 14 is configured to calculate the heading angle of the moving object according to each feature line segment and the positive direction of the acquisition terminal corresponding to the original point cloud data set.

[0183] The pose estimation module 15 is configured to estimate the pose of the moving object in the structured scene according to the odometer distance and the heading angle.

[0184] According to the technical solution, the original point cloud data set of the structured scene is collected, the line features in the structured scene are extracted and selected, the heading angle of the moving object is calculated by using the line features, and then the pose of the moving object in the structured scene is estimated according to the odometer distance and the heading angle. Since the extracted feature line segment is information in the original structured scene, the pose is calculated by extracting the inherent information in the scene, without the need to modify the existing scene, so there is no problem of feature degradation over time, avoiding interference with the normal working environment and improving the safety of the moving object in the structured scene. Through analysis of the existing features and in combination with the odometer information of the moving object, the pose of the robot can be accurately calculated.

[0185] In an embodiment of the present application, the feature line segment extraction module can include:

[0186] The region point cloud data set obtaining submodule is configured to select the point cloud data of a preset region from the original point cloud data set to obtain a region point cloud data set.

[0187] The clustered line segment obtaining submodule is configured to perform Euclidean clustering on the region point cloud data set to obtain each clustered line segment.

[0188] The feature line segment obtaining submodule is configured to adjust the point cloud data of each clustered line segment to obtain each feature line segment.

[0189] In an embodiment of the present application, the region point cloud data set obtaining submodule includes:

[0190] The target radius acquisition unit is configured to acquire a target radius of a circular region with the moving object as the center.

[0191] The first lateral distance and first longitudinal distance estimation unit is configured to estimate a first lateral distance and a first longitudinal distance of the moving object in the channel of the structured scene according to the odometer distance.

[0192] The point cloud data selection unit is configured to select the point cloud data of the original point cloud data set according to the target radius, the first lateral distance and the first longitudinal distance.

[0193] In an embodiment of the present application, the feature line segment obtaining submodule includes:

[0194] The filtered line segment obtaining unit is configured to filter the point cloud data of each clustered line segment by bounding box calculation to obtain each filtered line segment.

[0195] The straight line model estimation unit is configured to estimate a straight line model of each filtered line segment by using a random sample consensus algorithm.

[0196] In an embodiment of the present application, the filtering line segment obtaining unit comprises:

[0197] The bounding box obtaining subunit is configured to perform bounding box calculation on each cluster line segment by using a directional bounding box to obtain each bounding box.

[0198] The bounding box side length threshold obtaining subunit is configured to obtain a preset bounding box short side threshold and a preset bounding box long side threshold.

[0199] The point cloud data filtering subunit is configured to filter point cloud data in each bounding box according to the preset bounding box short side threshold and the preset bounding box long side threshold.

[0200] In an embodiment of the present application, the heading angle calculation module comprises:

[0201] The heading angle set obtaining sub-module is configured to calculate an included angle between each feature line segment and a positive direction of the collection terminal respectively to obtain a heading angle set.

[0202] The angle difference threshold obtaining sub-module is configured to obtain a preset angle difference threshold.

[0203] The angle class obtaining sub-module is configured to cluster each included angle in the heading angle set by using a Euclidean clustering algorithm according to the angle difference threshold to obtain each angle class.

[0204] The angle mean value obtaining sub-module is configured to perform mean value calculation on each included angle contained in each angle class to obtain each angle mean value.

[0205] The odometer angle obtaining sub-module is configured to obtain an odometer angle corresponding to the moving object.

[0206] The heading angle determining sub-module is configured to select an angle mean value with the smallest difference from the odometer angle from the angle mean values.

[0207] The heading angle determining sub-module is configured to determine the selected angle mean value as the heading angle.

[0208] In an embodiment of the present application, the device can further comprise:

[0209] The first expanded angle obtaining sub-module is configured to increase each included angle in the heading angle set by 90 degrees before clustering the included angles in the heading angle set by using the Euclidean clustering algorithm according to the angle difference threshold after obtaining the heading angle set to obtain each first expanded angle.

[0210] The second expanded angle obtaining sub-module is configured to subtract 90 degrees from each included angle in the heading angle set to obtain each second expanded angle.

[0211] The third expansion angle obtaining submodule is configured to respectively add 180 degrees to each included angle in the heading angle set to obtain each third expansion angle.

[0212] The expansion angle adding submodule is configured to add each first expansion angle, each second expansion angle and each third expansion angle to the heading angle set.

[0213] The angle mean obtaining submodule comprises:

[0214] The angle class sequence obtaining unit is configured to sort the number of included angles in each angle class in descending order to obtain an angle class sequence.

[0215] The angle class screening unit is configured to screen the first preset number of angle classes from one end of the angle class sequence with a large number.

[0216] The mean calculating unit is configured to calculate the mean value of each included angle in each angle class screened.

[0217] The heading angle determining submodule comprises:

[0218] The relative angle obtaining unit is configured to obtain the relative angle between the positive direction of the mobile object and the positive direction of the collection terminal.

[0219] The heading angle determining unit is configured to determine the heading angle according to the screened angle mean value and the relative angle.

[0220] In one specific embodiment of the present application, the pose estimation module comprises:

[0221] The point cloud rotating submodule is configured to rotate the point cloud formed by each feature line segment according to the angle mean value, so that the positive direction of the collection terminal is parallel to the passage direction of the mobile object in the structured scene.

[0222] The second lateral distance determining submodule is configured to determine the second lateral distance of the mobile object in the passage of the structured scene by using the statistical histogram.

[0223] The second longitudinal distance estimating submodule is configured to estimate the second longitudinal distance of the mobile object in the passage of the structured scene according to the odometer distance and the second lateral distance.

[0224] The pose estimation submodule is configured to estimate the pose of the mobile object in the structured scene according to the heading angle, the second lateral distance and the second longitudinal distance.

[0225] Corresponding to the above method embodiments, refer to Figure 11 , Figure 11 The pose estimation device provided by the present application comprises:

[0226] The memory 332 is configured to store a computer program.

[0227] The processor 322 is configured to implement the steps of the pose estimation method according to any one of the above method embodiments when the processor 322 executes the computer program.

[0228] Specifically, refer to Figure 12 , Figure 12 A specific structural diagram of a pose estimation device according to the embodiment is shown in FIG. 3. The pose estimation device can have great differences due to different configurations or performances, and can include a processor (central processing units, CPU) 322 (for example, one or more processors) and a memory 332 storing one or more computer programs 342 or data 344. The memory 332 can be temporary storage or persistent storage. The programs stored in the memory 332 can include one or more modules (not shown in the figure), and each module can include a series of instruction operations in the data processing device. Further, the processor 322 can be configured to communicate with the memory 332 and execute the series of instruction operations in the memory 332 on the pose estimation device 301.

[0229] The pose estimation device 301 can further include one or more power supplies 326, one or more wired or wireless network interfaces 350, one or more input / output interfaces 358, and / or one or more operating systems 341.

[0230] The steps in the pose estimation method described above can be implemented by the structure of the pose estimation device.

[0231] According to the above method embodiments, the present application further provides a computer readable storage medium, and the computer readable storage medium stores a computer program. When the computer program is executed by a processor, the following steps can be implemented:

[0232] Obtaining a mileage distance corresponding to a moving object in a structured scene; collecting an original point cloud data set of the structured scene; extracting each feature line segment from the original point cloud data set; wherein the original point cloud data set includes a plurality of feature line segments composed of point subsets; calculating a heading angle of the moving object according to the direction of the positive direction of the acquisition terminal corresponding to each feature line segment and the original point cloud data set; and estimating the pose of the moving object in the structured scene according to the mileage distance and the heading angle.

[0233] The computer readable storage medium can include a U disk, a mobile hard disk, a read-only memory (Read-Only Memory, ROM), a random access memory (Random Access Memory, RAM), a magnetic disk or an optical disk, and various media that can store program codes.

[0234] The computer readable storage medium provided in the present application is described in the above method embodiments, and the present application will not be described here.

[0235] The embodiments in the specification are described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device, equipment and computer readable storage medium disclosed by the embodiments, since they correspond to the method disclosed by the embodiments, the description is relatively simple, and the related parts can be referred to the method part.

[0236] The principles and implementation modes of the present application are described by using specific examples. The above embodiment description is only used to help understand the technical solutions and core ideas of the present application. It should be pointed out that, for ordinary skilled in the art, without departing from the principles of the present application, the present application can be improved and modified in several ways, and these improvements and modifications also fall within the protection scope of the present application.

Claims

1. A pose estimation method, characterized by, The method comprises the following steps: acquiring a distance of a moving object in a structured scene corresponding to an odometer; collecting an original point cloud data set of the structured scene; extracting each feature line segment from the original point cloud data set; wherein the original point cloud data set comprises a plurality of feature line segments composed of point subsets; calculating a heading angle of the moving object according to each feature line segment and a positive direction of a terminal corresponding to the original point cloud data set; estimating a pose of the moving object in the structured scene according to the distance of the odometer and the heading angle.

2. The pose estimation method of claim 1, wherein, extracting each feature line segment from the original point cloud data set comprises: filtering point cloud data of a preset region from the original point cloud data set to obtain a regional point cloud data set; performing Euclidean clustering on the regional point cloud data set to obtain each clustered line segment; adjusting point cloud data of each clustered line segment to obtain each feature line segment.

3. The pose estimation method of claim 2, wherein, filtering point cloud data of a preset region from the original point cloud data set comprises: acquiring a target radius of a circular region with the moving object as the center; estimating a first lateral distance and a first longitudinal distance of the moving object in a channel of the structured scene according to the distance of the odometer; performing point cloud data filtering on the original point cloud data set according to the target radius, the first lateral distance and the first longitudinal distance.

4. The pose estimation method of claim 2, wherein, adjusting point cloud data of each clustered line segment comprises: performing point cloud data filtering on each clustered line segment through bounding box calculation to obtain each filtered line segment; performing straight line model estimation on each filtered line segment using a random sample consensus algorithm.

5. The pose estimation method of claim 4, wherein, performing point cloud data filtering on each clustered line segment through bounding box calculation comprises: performing bounding box calculation on each clustered line segment using a directional bounding box to obtain each bounding box; acquiring a preset bounding box short side threshold and a preset bounding box long side threshold; filtering point cloud data in each bounding box according to the preset bounding box short side threshold and the preset bounding box long side threshold.

6. The pose estimation method of any one of claims 1 to 5, characterized in that, calculating the heading angle of the moving object according to each feature line segment and the positive direction of the terminal corresponding to the original point cloud data set comprises: calculating the included angle between each feature line segment and the positive direction of the terminal to obtain a heading angle set; acquiring a preset angle difference threshold; clustering each included angle in the heading angle set using a Euclidean clustering algorithm according to the angle difference threshold to obtain each angle class; performing mean value calculation on each included angle contained in each angle class to obtain each angle mean value; acquiring an odometer angle corresponding to the moving object; filtering the angle mean value with the smallest difference from the odometer angle from each angle mean value; determining the filtered angle mean value as the heading angle.

7. The pose estimation method of claim 6, wherein, After obtaining the heading angle set, before clustering each included angle in the heading angle set using a Euclidean clustering algorithm according to the angle difference threshold, the method further comprises: increasing each included angle in the heading angle set by 90 degrees to obtain each first expanded angle; subtracting each included angle in the heading angle set by 90 degrees to obtain each second expanded angle; increasing each included angle in the heading angle set by 180 degrees to obtain each third expanded angle; adding each first expansion angle, each second expansion angle and each third expansion angle to the set of heading angles; correspondingly, performing mean value calculation on each included angle in each angle class, including: performing size sorting on the number of included angles in each angle class to obtain an angle class sequence; selecting a preset number of angle classes from one end of the angle class sequence with a large number; performing mean value calculation on each included angle in each selected angle class; correspondingly, determining the heading angle as the mean value of the selected angle, including: obtaining a relative angle between the positive direction of the mobile object and the positive direction of the collection terminal; determining the heading angle according to the mean value of the selected angle and the relative angle.

8. The pose estimation method of claim 6, wherein, estimating the pose of the mobile object in the structured scene according to the odometer distance and the heading angle, including: rotating a point cloud formed by each feature line segment according to the mean value of the angle, so that the positive direction of the collection terminal is parallel to the passage direction of the mobile object in the structured scene; determining a second lateral distance of the mobile object in the passage of the structured scene by using a statistical histogram; estimating a second longitudinal distance of the mobile object in the passage of the structured scene according to the odometer distance and the second lateral distance; estimating the pose of the mobile object in the structured scene according to the heading angle, the second lateral distance and the second longitudinal distance.

9. A pose estimation device, characterized by, including: a memory for storing a computer program; a processor for executing the computer program to realize the steps of the pose estimation method according to any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and the computer program is executed by the processor to realize the steps of the pose estimation method according to any one of claims 1 to 8.

Images