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

Abstract

The application discloses a pose estimation method, device and equipment and a computer readable storage medium. The method comprises the following steps: extracting an original point cloud data set of a structured scene in which a mobile target is located; extracting feature points from the original point cloud data set; determining heading information of the mobile target according to a positive direction of a collecting device collecting the original point cloud data set and the feature points; obtaining a distance corresponding to the mobile target; and estimating a pose of the mobile target in the structured scene according to the heading information and the distance. The application does not need to additionally modify an existing scene, avoids interference with a normal working environment on site, and can accurately calculate the pose of the mobile target.

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 target (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 target.

[0003] The current commonly used pose estimation method of the mobile target in the structured scene mainly includes two kinds. One is to artificially increase some regularly distributed markers 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 target 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 target, and even causing the mobile target 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, and it 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 mobile target. 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] extracting a raw point cloud data set of a structured scene where a moving target is located;

[0009] extracting feature points from the raw point cloud data set;

[0010] determining heading information of the moving target according to a positive direction of a collecting device collecting the raw point cloud data set and the feature points;

[0011] obtaining an odometer distance corresponding to the moving target;

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

[0013] In one embodiment of the present application, extracting feature points from the raw point cloud data set comprises:

[0014] performing voxel filtering on the raw point cloud data set to obtain an initial voxel filtered point set;

[0015] finding, from the raw point cloud data set, a nearest neighbor point corresponding to each point in the initial voxel filtered point set;

[0016] replacing each point in the initial voxel filtered point set with the corresponding nearest neighbor point to obtain a target voxel filtered point set;

[0017] performing radius filtering on the target voxel filtered point set to obtain a radius filtered point set;

[0018] performing effective point screening on the radius filtered point set to obtain the feature points.

[0019] In one embodiment of the present application, performing voxel filtering on the raw point cloud data set to obtain an initial voxel filtered point set comprises:

[0020] performing first voxel filtering on the raw point cloud data set to obtain a first point set;

[0021] performing second voxel filtering on the first point set to obtain a second point set; wherein a second voxel size used in the second voxel filtering is greater than a first voxel size used in the first voxel filtering, and the second voxel size is a non-integer multiple of the first voxel size;

[0022] determining the second point set as the initial voxel filtered point set.

[0023] In one embodiment of the present application, performing radius filtering on the target voxel filtered point set to obtain a radius filtered point set comprises:

[0024] performing first radius filtering on the filtered point set of the target voxel to obtain a third point set, wherein the first radius filtering corresponds to a first radius range containing one point;

[0025] performing second radius filtering on the third point set to obtain a fourth point set, wherein the second radius filtering corresponds to a second radius range containing at least five points;

[0026] determining the fourth point set as the radius filtered point set.

[0027] In an embodiment of the present application, effective point screening is performed on the radius filtered point set to obtain feature points, including:

[0028] calculating a first vector formed by each radius filtered point in the radius filtered point set and a nearest neighbor point corresponding to the radius filtered point to obtain a first vector group;

[0029] calculating an included angle between each first vector in the first vector group;

[0030] obtaining a preset standard included angle and a first angle difference threshold value;

[0031] calculating a difference value between each included angle and a corresponding standard included angle;

[0032] screening, from the first vector group, a first vector group in which the difference values of the included angles between each first vector satisfy the first angle difference threshold value;

[0033] determining, as feature points, the radius filtered points corresponding to the screened first vector groups.

[0034] In an embodiment of the present application, the heading information of the moving target is determined according to a positive direction of a device for collecting the original point cloud data set and the feature points, including:

[0035] calculating a second vector formed by each feature point and a nearest neighbor point corresponding to the feature point to obtain a second vector group;

[0036] calculating an included angle between each second vector in the second vector group and the positive direction of the device to obtain a heading angle subset;

[0037] obtaining a preset second angle difference threshold value;

[0038] performing clustering on each included angle in a heading angle set formed by the heading angle subset according to the second angle difference threshold value by using a Euclidean clustering algorithm to obtain an angle class;

[0039] performing mean value calculation on each included angle contained in each angle class to obtain an angle mean value;

[0040] obtaining an odometer angle corresponding to the moving target;

[0041] filtering an angle mean value with a minimum difference value from the odometer angle from the angle means;

[0042] determining the filtered angle mean value as the heading information.

[0043] In an embodiment of the present application, after obtaining the subsets of heading angles, before clustering each included angle in the set of heading angles formed by the subsets of heading angles according to the second angle difference threshold using the Euclidean clustering algorithm, the method further comprises:

[0044] adding 90 degrees to each included angle in the subsets of heading angles to obtain first extended angles;

[0045] subtracting 90 degrees from each included angle in the subsets of heading angles to obtain second extended angles;

[0046] adding 180 degrees to each included angle in the subsets of heading angles to obtain third extended angles;

[0047] adding the first, second and third extended angles corresponding to each included angle in the subsets of heading angles to the corresponding subset of heading angles.

[0048] In an embodiment of the present application, the mean value calculation of each included angle in each angle class comprises:

[0049] sorting the number of included angles in each angle class in descending order to obtain an angle class sequence;

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

[0051] calculating the mean value of each included angle in each filtered angle class;

[0052] Correspondingly, determining the filtered angle mean value as the heading information comprises:

[0053] obtaining a relative angle between the positive direction of the moving target and the positive direction of the collection device;

[0054] filtering an angle mean value with a minimum difference value from the odometer angle from the angle means, and determining the heading information according to the filtered angle mean value and the relative angle.

[0055] In an embodiment of the present application, estimating the pose of the moving target in the structured scene according to the heading information and the odometer distance comprises:

[0056] rotating a point cloud composed of each feature point according to the angle mean, so that a positive direction of the acquisition device is parallel to a channel direction of the moving target in the structured scene;

[0057] determining a lateral distance of the moving target in the channel of the structured scene by using a statistical histogram;

[0058] estimating a longitudinal distance of the moving target in the channel of the structured scene according to the odometer distance and the lateral distance;

[0059] estimating a pose of the moving target in the structured scene according to the heading information, the lateral distance and the longitudinal distance.

[0060] A pose estimation device comprises:

[0061] a memory for storing a computer program;

[0062] a processor for implementing steps of the pose estimation method as described above when executing the computer program.

[0063] A computer readable storage medium, wherein the computer readable storage medium stores a computer program, and the computer program is executed by a processor to implement steps of the pose estimation method as described above.

[0064] The pose estimation method provided in the present application extracts an original point cloud data set of a structured scene where a moving target is located; extracts each feature point from the original point cloud data set; determines heading information of the moving target according to a positive direction of an acquisition device for acquiring the original point cloud data set and each feature point; acquires an odometer distance corresponding to the moving target; and estimates a pose of the moving target in the structured scene according to the heading information and the odometer distance.

[0065] According to the above technical solution, by extracting an original point cloud data set of a structured scene where a moving target is located, determining heading information of the moving target according to a positive direction of an acquisition device for acquiring the original point cloud data set and each feature point, and then fusing odometer information of the moving target, the accurate pose of the moving target in the structured scene is realized. Since the point features in the original structured scene are extracted, the calculation of the pose is realized by extracting inherent information in the scene, and the existing scene does not need to be modified, so the problem of feature degradation over time does not occur, the interference to the normal working environment is avoided, and the safety of the moving target running in the structured scene is improved. By analyzing the existing features and combining the odometer information of the moving target, the pose of the moving target can be accurately calculated.

[0066] Correspondingly, the application further provides a pose estimation device, equipment and computer readable storage medium corresponding to the pose estimation method, which have the above technical effects, and details are not repeated here. BRIEF DESCRIPTION OF DRAWINGS

[0067] In order to more clearly illustrate the technical solutions in the embodiments of the present application or 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 some embodiments of the present application, and for those skilled in the art, other drawings can be obtained without creative labor on the basis of these drawings.

[0068] Figure 1 An implementation flowchart of a pose estimation method in an embodiment of the present application is provided.

[0069] Figure 2 An implementation flowchart of another pose estimation method in an embodiment of the present application is provided.

[0070] Figure 3 A schematic diagram of a raw point cloud data set in an embodiment of the present application is provided.

[0071] Figure 4 A schematic diagram of a point set filtered by radius in an embodiment of the present application is provided.

[0072] Figure 5 A schematic diagram of another point set filtered by radius in an embodiment of the present application is provided.

[0073] Figure 6 A schematic diagram of a state of a moving target in a structured scene in an embodiment of the present application is provided.

[0074] Figure 7 A schematic diagram of a state of a point cloud before and after rotation in an embodiment of the present application is provided.

[0075] Figure 8 A schematic diagram of a transverse distance extraction process in an embodiment of the present application is provided.

[0076] Figure 9 A statistical histogram for transverse distance statistics in an embodiment of the present application is provided.

[0077] Figure 10 A structural block diagram of a pose estimation device in an embodiment of the present application is provided.

[0078] Figure 11 A structural block diagram of a pose estimation equipment in an embodiment of the present application is provided.

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

[0080] In order to make the person skilled in the art better understand the scheme of 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, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0081] Reference is made to Figure 1 , Figure 1 An implementation flowchart of a pose estimation method in an embodiment of the present application can include the following steps.

[0082] S101: Extract an original point cloud data set of a structured scene where a mobile target is located.

[0083] When the mobile target is running in the structured scene, an original point cloud data set of the structured scene where the mobile target is located is extracted, such as an original point cloud data set of the structured scene where the mobile target is located can be extracted by using a single-line laser radar or a multi-line laser radar. The laser radar is generally set at a height of 20-30 cm below the ground. By setting this height, the structural support part (such as a support leg, etc.) and some miscellaneous other information in the structured scene can be collected.

[0084] The mobile target can include a robot, an unmanned vehicle, etc.

[0085] The structured scene can include a shelf supported by high support legs, a storage rack, etc., or some breeding scenes such as a chicken coop, a duck coop, etc.

[0086] S102: Extract each feature point from the original point cloud data set.

[0087] The extracted original point cloud data set of the structured scene includes structured feature points, and also includes a lot of miscellaneous other information or noise. After the original point cloud data set of the structured scene where the mobile target is located is extracted, each feature point is extracted from the original point cloud data set. Each feature point can describe the structure of the structured scene to a certain extent.

[0088] S103: Determine the heading information of the mobile target according to the positive direction of the collection device for collecting the original point cloud data set and each feature point.

[0089] The positive direction of the acquisition device for collecting the original point cloud data set can be consistent with the positive direction of the moving target, or can have a fixed angle deviation from the positive direction of the moving target. After the feature points are extracted, the heading information of the moving target is determined according to the positive direction of the acquisition device for collecting the original point cloud data set and the feature points. For example, a vector formed by a line connecting the feature points can be obtained, and the heading information of the moving target is determined according to the included angle between the vector and the positive direction of the acquisition device for collecting the original point cloud data set.

[0090] The acquisition device can include a single-line laser radar or a multi-line laser radar.

[0091] S104: Obtain the odometer distance corresponding to the moving target.

[0092] The moving target body carries an odometer, and the running distance of the moving target can be recorded by the odometer carried by the moving target body. The odometer distance corresponding to the moving target is obtained.

[0093] S105: Estimate the pose of the moving target in the structured scene according to the heading information and the odometer distance.

[0094] After the heading information of the moving target is determined and the odometer distance corresponding to the moving target is obtained, the pose of the moving target in the structured scene is estimated according to the heading information and the odometer distance. The starting position of the moving target in the structured scene can be recorded in advance, for example, the starting position of the moving target in the structured scene is generally the outlet or inlet of the channel in the structured scene. The pose of the moving target in the structured scene can be estimated according to the starting position of the moving target in the structured scene, the heading information and the odometer distance.

[0095] As can be seen from the above technical solution, by extracting the original point cloud data set of the structured scene in which the moving target is located, determining the heading information of the moving target according to the positive direction of the acquisition device for collecting the original point cloud data set and the feature points, and then fusing the odometer information of the moving target body, the accurate pose of the moving target in the structured scene is realized. Since the point features in the original structured scene are extracted, 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 moving target running in the structured scene. Through analysis of the existing features and combination of the odometer information of the moving target body, the pose of the moving target can be accurately calculated.

[0096] It should be noted that based on the above embodiment, the application embodiment also provides a corresponding improvement scheme. In the subsequent embodiments, the steps involved in the above embodiments or between the corresponding steps can be mutually referred to, and the corresponding beneficial effects can also be mutually referred to. In the following improved embodiments, they will not be described one by one.

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

[0098] S201: Extract the original point cloud data set of the structured scene where the moving target is located.

[0099] Referring to Figure 3 , Figure 3 is a schematic diagram of an original point cloud data set in an embodiment of the application. The original point cloud data set collected by the collection device has structured feature points, and there are also a lot of other information or noise. The extracted original point cloud data set of the structured scene where the moving target is located can be named pcInitializaton.

[0100] S202: Perform voxel filtering on the original point cloud data set to obtain an initial voxel filtered point set.

[0101] After the original point cloud data set of the structured scene is extracted, voxel filtering is performed on the original point cloud data set to obtain an initial voxel filtered point set. Through voxel filtering, noise in the original point cloud data set can be effectively removed, and the characteristics of the structured scene are retained.

[0102] In a specific embodiment of the application, step S202 can include the following steps:

[0103] Step one: Perform first voxel filtering on the original point cloud data set to obtain a first point set;

[0104] Step two: Perform second voxel filtering on the first point set to obtain a second point set; wherein the second voxel size used by the second voxel filtering is greater than the first voxel size used by the first voxel filtering, and the second voxel size is a non-integer multiple of the first voxel size;

[0105] Step three: Determine the second point set as the initial voxel filtered point set.

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

[0107] In the voxel filtering of the original point cloud dataset, two voxel filtering processes can be included. The second voxel size used in the second voxel filtering is set to be greater than the first voxel size used in the first voxel filtering, and the second voxel size is a non-integer multiple of the first voxel size. For example, the first voxel size is set to be voxelSize1, and the second voxel size is set to be voxelSize2. The voxelSize2 is generally set to be the minimum distance minDistance between the two legs, and the voxelSize1 is generally set to be 0.3 times (voxelSize2+0.05). First, the original point cloud dataset is subjected to the first voxel filtering to obtain a first point set. Then, the first point set is subjected to the second voxel filtering to obtain a second point set. Further, the second point set is determined as the initial voxel filtering point set.

[0108] By setting the voxel size of the large-size voxel filtering to be minDistance, only one point in each leg region can be reserved. Considering that multiple points on the same leg point cloud may be assigned to different voxel regions, a small voxel filtering is performed before the large voxel filtering. The voxel size of the small voxel filtering can be set to be 0.3*(voxelSize2+0.05), so as to prevent the repetition of the voxel boundary with the large voxel filtering. The specific size of the small voxel filtering can be adjusted according to the actual scene. The initial voxel filtering point set after the voxel filtering can be named as pcByVoxelFilter.

[0109] By performing the initial voxel filtering using the voxel filtering with the small voxel size, and then performing the further voxel filtering using the voxel filtering with the large voxel size, and setting the large voxel size to be a non-integer multiple of the small voxel size, the effective removal of the noise can be achieved, and the repetition of the voxel boundary between the small voxel filtering and the large voxel filtering can be prevented.

[0110] S203: Find the nearest neighbor points corresponding to each point in the initial voxel filtering point set from the original point cloud dataset.

[0111] After obtaining the initial voxel filtered point set, the points retained in the initial voxel filtered point set are all gravity points in the voxel, which are not necessarily points in the original point cloud data set, and will lead to errors in subsequent calculations. Therefore, the nearest neighbor points of each point in the initial voxel filtered point set are searched from the original point cloud data set. A KD tree (K-dimensional tree) can be constructed for pcInitializaton, and the nearest neighbor point of each point in pcByVoxelFilter is searched from the KD tree, and the nearest neighbor point is retained as the voxel filtered point cloud realPCByVoxelFilter. The nearest neighbor point of each point in the initial voxel filtered point set in the original point cloud data set can be one or more. When the nearest neighbor point is one, the point is directly determined as the nearest neighbor point. When the nearest neighbor point is more than one, a point can be randomly selected from the multiple points as the nearest neighbor point, or a suitable point can be selected from the multiple points as the nearest neighbor point according to the positional relationship of other points around the point.

[0112] S204: Corresponding replacement is performed on each point in the initial voxel filtered point set by using each nearest neighbor point to obtain a target voxel filtered point set.

[0113] After searching for the nearest neighbor points corresponding to each point in the initial voxel filtered point set from the original point cloud data set, corresponding replacement is performed on each point in the initial voxel filtered point set by using each nearest neighbor point to obtain a target voxel filtered point set. By using the nearest neighbor points in the original point cloud data set to replace each point in the initial voxel filtered point set, errors in subsequent calculations are avoided.

[0114] S205: Radius filtering is performed on the target voxel filtered point set to obtain a radius filtered point set.

[0115] After corresponding replacement is performed on each point in the initial voxel filtered point set by using each nearest neighbor point to obtain a target voxel filtered point set, some noise points still exist. Radius filtering is performed on the target voxel filtered point set to obtain a radius filtered point set. By continuing to perform radius filtering on the target voxel filtered point set after corresponding replacement is performed on each point in the initial voxel filtered point set by using each nearest neighbor point, further denoising of the original point cloud data set is achieved.

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

[0117] Step one: first radius filtering is performed on the target voxel filtered point set to obtain a third point set; wherein the first radius filtering corresponds to a first radius range containing one point;

[0118] Step two: performing second radius filtering on the third point set to obtain a fourth point set; wherein the second radius filtering corresponds to a second radius range containing at least five points;

[0119] Step three: determining the fourth point set as the radius-filtered point set.

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

[0121] In the radius filtering on the target voxel filtered point set, first, first radius filtering is performed on the target voxel filtered point set to obtain a third point set, then second radius filtering is performed on the third point set to obtain a fourth point set, and the fourth point set is determined as the radius-filtered point set. The first radius filtering corresponds to a first radius range containing one point, and the second radius filtering corresponds to a second radius range containing five points.

[0122] Based on the above example, in the first radius filtering, a point with a radius of (minDistance-0.1) and only one point in the radius range can be reserved. The point cloud after the first radius filtering is subjected to radius filtering again, the maximum distance between the two legs is set as maxDistance, the filtering radius is set as (maxDistance+0.1), the points with at least 5 points in the radius range are reserved, and the points around them are also reserved. The point cloud after the two times of radius filtering is named as pcByRadiusFilter.

[0123] By performing the radius filtering twice on the target voxel filtered point set with different scales, further denoising of the original point cloud data set is realized, and the overall structure of the point cloud of the structured scene is maintained.

[0124] S206: performing effective point screening on the radius-filtered point set to obtain each feature point.

[0125] Referring to Figure 4 , Figure 4 FIG. 1 is a schematic diagram of a radius-filtered point set in an embodiment of the present application. After obtaining the radius-filtered point set, the radius-filtered point set can contain invalid points that only meet the radius range but have angle deviation. The effective point screening is performed on the radius-filtered point set to obtain each feature point. By performing the effective point screening on the radius-filtered point set, the accuracy of the description of the structured scene by each feature point is ensured.

[0126] In a specific embodiment of the present application, step S206 can include the following steps:

[0127] Step one: calculating a first vector group composed of each first vector between each radius-filtered point in the radius-filtered point set and the corresponding nearest neighbor point of the radius-filtered point;

[0128] Step two: calculate the included angle between each first vector in each first vector group;

[0129] Step three: obtain the preset standard included angle and the first angle difference threshold value;

[0130] Step four: calculate the difference between each included angle and the corresponding standard included angle;

[0131] Step five: select the first vector group from each first vector group, wherein the difference between the included angle between each first vector in the first vector group meets the first angle difference threshold value;

[0132] Step six: determine each radius filtered point corresponding to each selected first vector group as each feature point.

[0133] For convenience of description, the above six steps can be combined for description.

[0134] In the effective point screening of the radius filtered point set, the first vector group is obtained by calculating the first vector formed by each radius filtered point in the radius filtered point set and the nearest neighbor point corresponding to the radius filtered point, and the included angle between each first vector in each first vector group is calculated. The standard included angle between each first vector is set in advance, and the first angle difference threshold value is set in advance. The preset standard included angle and the first angle difference threshold value are obtained, the difference between each included angle and the corresponding standard included angle is calculated, the first vector group is selected from each first vector group, wherein the difference between the included angle between each first vector in the first vector group meets the first angle difference threshold value, and each radius filtered point corresponding to each selected first vector group is determined as each feature point. By screening the radius filtered point set according to the first angle difference threshold value set in advance, it is ensured that each feature point obtained by screening meets the angle distribution rule of each point in the structured scene.

[0135] Referring to Figure 5 , Figure 5 is a schematic view of another radius filtered point set in the embodiment of the present application. Assuming that the legs in the structured scene are perpendicular in the horizontal and vertical directions, for each radius filtered point, the first vector group [v1, v2, v3, v4] formed by the four surrounding points and the radius filtered point is calculated, and the feature of the current structured scene point is any two vectors selected from the first vector group, which are in perpendicular relationship or parallel relationship. The included angle delta between all two-vector pairs in the first vector group is traversed, delta should be 0 degrees, 180 degrees, 90 degrees or -90 degrees, a tolerance error toleranceBiaAngle is set, and if the calculated delta is within the tolerance range, it can be considered that the point meets the requirements. The point cloud meeting the requirements is as shown in the square marked point in Figure 5 .

[0136] S207: Determine the heading information of the moving target according to the positive direction of the acquisition device collecting the original point cloud data set and each feature point.

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

[0138] Step one: Calculate a second vector formed by each feature point in each feature point and the nearest neighbor point corresponding to the feature point, to obtain each second vector group;

[0139] Step two: Calculate the included angle between each second vector in each second vector group and the positive direction of the acquisition device, to obtain each heading angle subset;

[0140] Step three: Obtain a preset second angle difference threshold value;

[0141] Step four: Cluster each included angle in the heading angle set formed by each heading angle subset according to the second angle difference threshold value using the Euclidean clustering algorithm, to obtain each angle class;

[0142] Step five: Calculate the mean value of each included angle in each angle class, to obtain each angle mean value;

[0143] Step six: Obtain the odometer angle corresponding to the moving target;

[0144] Step seven: Select the angle mean value with the smallest odometer angle difference from each angle mean value;

[0145] Step eight: Determine the selected angle mean value as the heading information.

[0146] For convenience of description, the above eight steps can be combined for description.

[0147] When determining the heading information of the moving target according to the positive direction of the acquisition device collecting the original point cloud data set and each feature point, a second vector formed by each feature point in each feature point and the nearest neighbor point corresponding to the feature point is calculated, to obtain each second vector group. The included angle between each second vector in each second vector group and the positive direction of the acquisition device is calculated, to obtain each heading angle subset. A preset second angle difference threshold value is obtained, and each included angle in the heading angle set formed by each heading angle subset is clustered according to the second angle difference threshold value using the Euclidean clustering algorithm, such as the K-means clustering algorithm, to obtain each angle class. The mean value of each included angle in each angle class is calculated, to obtain each angle mean value. The odometer angle corresponding to the moving target is obtained, and the angle mean value with the smallest odometer angle difference is selected from each angle mean value. The selected angle mean value is determined as the heading information. The heading information is determined by combining the positive direction of the acquisition device and the odometer angle, which improves the accuracy of the determined heading information.

[0148] In one specific embodiment of the present application, after obtaining each heading angle subset, before clustering each included angle in the heading angle set formed by each heading angle subset according to the second angle difference threshold value using the Euclidean clustering algorithm, the method can further include the following steps:

[0149] Step one: add 90 degrees to each included angle in each heading angle subset to obtain a first extended angle;

[0150] Step two: subtract 90 degrees from each included angle in each heading angle subset to obtain a second extended angle;

[0151] Step three: add 180 degrees to each included angle in each heading angle subset to obtain a third extended angle;

[0152] Step four: add the first extended angle, the second extended angle and the third extended angle corresponding to each included angle in each heading angle subset to the corresponding heading angle subset.

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

[0154] Referring to Figure 6 , Figure 6 is a schematic diagram of a state of a moving target in a structured scene in an embodiment of the present application. After obtaining each heading angle subset, 90 degrees are added to each included angle in each heading angle subset to obtain a first extended angle, 90 degrees are subtracted from each included angle in each heading angle subset to obtain a second extended angle, and 180 degrees are added to each included angle in each heading angle subset to obtain a third extended angle. The first extended angle, the second extended angle and the third extended angle corresponding to each included angle in each heading angle subset are added to the corresponding heading angle subset. By angle extension of each heading angle subset, four scenarios of the state of the moving target in the structured scene can be extended. Figure 6

[0155] In one specific embodiment of the present application, mean value calculation of each included angle in each angle class can include the following steps:

[0156] Step one: sort the number of included angles in each angle class in descending order to obtain an angle class sequence;

[0157] Step two: select the first preset number of angle classes from the end of the angle class sequence with the largest number;

[0158] Step three: perform mean value calculation on each included angle in each selected angle class;

[0159] Correspondingly, the mean value of the selected angle is determined as the heading information, including: ​

[0160] Step one: obtaining the relative angle between the positive direction of the moving target and the positive direction of the collection device;

[0161] Step two: selecting the angle mean value with the smallest difference from the odometer angle from the angle mean values, and determining the heading information according to the selected angle mean value and the relative angle.

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

[0163] After clustering the included angles in each angle class by using the Euclidean clustering algorithm according to the second angle difference threshold, the number of included angles in each angle class is sorted in size to obtain an angle class sequence, and the first preset number of angle classes are selected from the large end of the angle class sequence, the mean value of each included angle in each selected angle class is calculated to obtain the angle mean value. Assuming that three angle classes are selected, the calculated angle mean values of the three angle classes are yaw1, yaw2, and yaw3 respectively. The relative angle between the positive direction of the moving target and the positive direction of the collection device is obtained, the angle mean value with the smallest difference from the odometer angle is selected from the angle mean values, and the heading information position_yaw is determined according to the selected angle mean value and the relative angle. By selecting the first preset number of angle classes from the large end of the angle class sequence for angle mean value calculation, and selecting the angle mean value with the smallest difference from the odometer angle for heading information calculation, the accuracy of the heading information calculation result is greatly improved.

[0164] S208: obtaining the odometer distance corresponding to the moving target.

[0165] S209: estimating the pose of the moving target in the structured scene according to the heading information and the odometer distance.

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

[0167] Step one: rotating the point cloud formed by the feature points according to the angle mean value, so that the positive direction of the collection device is parallel to the channel direction of the moving target in the structured scene;

[0168] Step two: determining the lateral distance of the moving target in the channel of the structured scene by using a statistical histogram;

[0169] Step three: estimating the longitudinal distance of the moving target in the channel of the structured scene according to the odometer distance and the lateral distance;

[0170] Step four: estimating the pose of the moving target in the structured scene according to the heading information, the lateral distance, and the longitudinal distance.

[0171] For convenience of description, the above four steps can be combined for description.

[0172] Referring to Figure 7 , Figure 7 is a schematic diagram of a point cloud before and after rotation in an embodiment of the present application. When estimating the pose of a moving target in a structured scene, the point cloud formed by each feature point is rotated according to the average angle, so that the positive direction of the acquisition device is parallel to the channel direction of the moving target in the structured scene.

[0173] Referring to Figure 8 , Figure 8 is a schematic diagram of a lateral distance extraction process in an embodiment of the present application. The lateral distance (position_y) of the moving target in the channel of the structured scene is determined by using a statistical histogram. The lateral distance determination process can include intercepting the point cloud in a certain area near the y-axis direction leg of the point cloud after rotation, and extracting the left and right areas respectively.

[0174] Referring to Figure 9 , Figure 9 is a statistical histogram for lateral distance statistics in an embodiment of the present application. After the left and right areas are extracted, a statistical histogram distribution is made according to the distance y. The rectangular bar (bin) corresponding to the point with the most frequency in the statistical histogram is selected as the lateral distance of the moving target.

[0175] The longitudinal distance (position_x) of the moving target in the channel of the structured scene is estimated according to the odometer distance and the lateral distance. For example, the longitudinal distance can be estimated by subtracting the lateral distance from the odometer distance. The starting point of the moving target entering the structured scene can also be obtained. The starting point of the moving target entering the structured scene can be set as the starting point of the odometer. The lateral movement distance is calculated according to the starting point and the lateral distance. The longitudinal distance is estimated by subtracting the lateral movement distance from the odometer distance. After the heading information, the lateral distance and the longitudinal distance are estimated, the pose of the moving target in the structured scene is estimated according to the heading information, the lateral distance and the longitudinal distance. By using the statistical histogram to estimate the lateral distance of the moving target in the structured scene, the complexity of determining the lateral distance in the structured scene is reduced, and the intuitiveness of determining the lateral distance in the structured scene is improved.

[0176] It should be noted that the lateral distance and the longitudinal distance of the moving target in the channel of the structured scene are both the positions of the moving target relative to the coordinate origin of the structured scene. The coordinate origin of the structured scene can be the same as the starting point of the moving target entering the structured scene, or there can be a fixed deviation. The present embodiment does not limit this.

[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 the pose estimation method described above.

[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] An original point cloud data set extraction module 11 is configured to extract an original point cloud data set of a structured scene in which a mobile target is located.

[0180] A feature point extraction module 12 is configured to extract feature points from the original point cloud data set.

[0181] A heading information determination module 13 is configured to determine heading information of the mobile target according to a positive direction of a collection device that collects the original point cloud data set and the feature points.

[0182] An odometer distance acquisition module 14 is configured to acquire an odometer distance corresponding to the mobile target.

[0183] A pose estimation module 15 is configured to estimate a pose of the mobile target in the structured scene according to the heading information and the odometer distance.

[0184] According to the above technical solution, by extracting an original point cloud data set of a structured scene in which a mobile target is located, determining heading information of the mobile target according to a positive direction of a collection device that collects the original point cloud data set and the feature points, and then fusing odometer information of the mobile target, the accurate pose of the mobile target in the structured scene is realized. Since the point features in the original structured scene are extracted, the calculation of the pose is realized by extracting inherent information in the scene, and the existing scene does not need to be modified, so the problem of feature degradation over time does not occur, the normal working environment of the scene is not disturbed, and the safety of the mobile target running in the structured scene is improved. By analyzing the existing features and combining the odometer information of the mobile target, the pose of the mobile target can be accurately calculated.

[0185] In a specific embodiment of the present application, the feature point extraction module 12 includes:

[0186] An initial voxel filtered point set obtaining sub-module is configured to perform voxel filtering on the original point cloud data set to obtain an initial voxel filtered point set.

[0187] A nearest neighbor point searching sub-module is configured to search for the nearest neighbor points corresponding to each point in the initial voxel filtered point set from the original point cloud data set.

[0188] The target voxel filtered point set obtaining submodule is configured to replace each point in the initial voxel filtered point set with a corresponding nearest neighbor point to obtain a target voxel filtered point set.

[0189] The radius filtered point set obtaining submodule is configured to perform radius filtering on the target voxel filtered point set to obtain a radius filtered point set.

[0190] The feature point obtaining submodule is configured to perform effective point screening on the radius filtered point set to obtain each feature point.

[0191] In an embodiment of the present application, the initial voxel filtered point set obtaining submodule comprises:

[0192] The first point set obtaining unit is configured to perform first voxel filtering on the original point cloud data set to obtain a first point set.

[0193] The second point set obtaining unit is configured to perform second voxel filtering on the first point set to obtain a second point set; wherein the second voxel size used in the second voxel filtering is greater than the first voxel size used in the first voxel filtering, and the second voxel size is a non-integer multiple of the first voxel size.

[0194] The initial voxel filtered point set determining unit is configured to determine the second point set as the initial voxel filtered point set.

[0195] In an embodiment of the present application, the radius filtered point set obtaining submodule comprises:

[0196] The third point set obtaining unit is configured to perform first radius filtering on the target voxel filtered point set to obtain a third point set; wherein the first radius filtering corresponds to a first radius range containing one point.

[0197] The fourth point set obtaining unit is configured to perform second radius filtering on the third point set to obtain a fourth point set; wherein the second radius filtering corresponds to a second radius range containing at least five points.

[0198] The radius filtered point set determining unit is configured to determine the fourth point set as the radius filtered point set.

[0199] In an embodiment of the present application, the feature point obtaining submodule comprises:

[0200] The first vector group obtaining unit is configured to calculate a first vector between each radius filtered point in the radius filtered point set and a corresponding nearest neighbor point of the radius filtered point to obtain each first vector group.

[0201] The angle calculating unit is configured to calculate an angle between each first vector in each first vector group.

[0202] a standard included angle and a first angle difference threshold value acquisition unit configured to acquire a preset standard included angle and a first angle difference threshold value;

[0203] a difference calculation unit configured to calculate a difference between each included angle and a corresponding standard included angle;

[0204] a first vector group screening unit configured to screen, from each first vector group, a first vector group in which a difference between included angles between each pair of first vectors satisfies the first angle difference threshold value;

[0205] a feature point determination unit configured to determine each radius filtered point corresponding to each screened first vector group as a feature point.

[0206] In an embodiment of the present application, the heading information determination module 13 comprises:

[0207] a second vector group obtaining submodule configured to calculate a second vector formed by each feature point and a nearest neighbor point corresponding to the feature point, and obtain each second vector group;

[0208] a heading angle subset obtaining submodule configured to calculate an included angle between each second vector in each second vector group and a positive direction of the collection device, and obtain each heading angle subset;

[0209] a second angle difference threshold value acquisition submodule configured to acquire a preset second angle difference threshold value;

[0210] an angle class obtaining submodule configured to cluster each included angle in a heading angle set formed by each heading angle subset using a Euclidean clustering algorithm according to the second angle difference threshold value, and obtain each angle class;

[0211] an angle mean value obtaining submodule configured to calculate a mean value of each included angle in each angle class, and obtain each angle mean value;

[0212] an odometer angle acquisition submodule configured to acquire an odometer angle corresponding to the moving target;

[0213] a minimum angle mean value screening submodule configured to screen, from each angle mean value, an angle mean value having a smallest difference with the odometer angle;

[0214] a heading information determination submodule configured to determine the screened angle mean value as the heading information.

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

[0216] The first expansion angle obtaining module is configured to add 90 degrees to each included angle in each heading angle subset to obtain a first expansion angle before clustering each included angle in a heading angle set formed by the heading angle subsets according to a second angle difference threshold by using a Euclidean clustering algorithm.

[0217] The second expansion angle obtaining module is configured to subtract 90 degrees from each included angle in each heading angle subset to obtain a second expansion angle.

[0218] The third expansion angle obtaining module is configured to add 180 degrees to each included angle in each heading angle subset to obtain a third expansion angle.

[0219] The angle adding module is configured to add the first expansion angle, the second expansion angle and the third expansion angle corresponding to each included angle in each heading angle subset to the corresponding heading angle subset.

[0220] In an embodiment of the present application, the angle mean obtaining sub-module comprises:

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

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

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

[0224] The heading information determining sub-module comprises:

[0225] The relative angle obtaining unit is configured to obtain a relative angle between the positive direction of the mobile target and the positive direction of the collection device.

[0226] The heading information determining unit is configured to screen an angle mean with the smallest odometer angle difference from the angle means, and determine the heading information according to the screened angle mean and the relative angle.

[0227] In an embodiment of the present application, the pose estimation module 15 comprises:

[0228] The point cloud rotating sub-module is configured to rotate the point cloud formed by the feature points according to the angle mean, so that the positive direction of the collection device is parallel to the channel direction of the mobile target in the structured scene.

[0229] The lateral distance determining sub-module is configured to determine the lateral distance of the mobile target in the channel of the structured scene by using a statistical histogram.

[0230] a longitudinal distance estimation sub-module configured to estimate a longitudinal distance of the moving target in the lane of the structured scene according to the odometry distance and the lateral distance;

[0231] a pose estimation sub-module configured to estimate a pose of the moving target in the structured scene according to the heading information, the lateral distance and the longitudinal distance.

[0232] Corresponding to the above method embodiments, refer to Figure 11 , Figure 11 A schematic diagram of a pose estimation device provided in the present application, which can include:

[0233] a memory 332 configured to store a computer program;

[0234] a processor 322 configured to execute the computer program to implement the steps of the pose estimation method in the above method embodiments.

[0235] Specifically, refer to Figure 12 , Figure 12 A specific structural schematic diagram of a pose estimation device provided in the present embodiment, which can have great difference 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 program stored in the memory 332 can include one or more modules (not shown in the figure), each of which 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 to execute a series of instruction operations in the memory 332 on the pose estimation device 301.

[0236] The pose estimation device 301 can also 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.

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

[0238] Corresponding to the above method embodiments, the present application also provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the following steps:

[0239] Extract a raw point cloud data set of a structured scene where the moving target is located; extract feature points from the raw point cloud data set; determine heading information of the moving target according to a positive direction of a collecting device collecting the raw point cloud data set and the feature points; obtain a distance of a wheel revolution of the moving target; and estimate a pose of the moving target in the structured scene according to the heading information and the distance of the wheel revolution.

[0240] The computer readable storage medium can include a U disk, a mobile hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and various media capable of storing program codes.

[0241] For the computer readable storage medium provided in the present application, refer to the above method embodiments, which will not be repeated here.

[0242] 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 of each embodiment can be referred to each other. For the apparatus, device 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 relevant part can be referred to the method part.

[0243] The principles and implementation manners 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: extracting a raw point cloud data set of a structured scene where a moving target is located; extracting feature points from the raw point cloud data set; determining heading information of the moving target according to a positive direction of a collecting device collecting the raw point cloud data set and the feature points; obtaining a distance of a wheel revolution of the moving target; estimating a pose of the moving target in the structured scene according to the heading information and the distance of the wheel revolution.

2. The pose estimation method of claim 1, wherein, The extracting of the feature points from the raw point cloud data set comprises the following steps: performing voxel filtering on the raw point cloud data set to obtain an initial voxel filtered point set; finding, from the raw point cloud data set, a nearest neighbor point corresponding to each point in the initial voxel filtered point set; replacing each point in the initial voxel filtered point set with the corresponding nearest neighbor point to obtain a target voxel filtered point set; performing radius filtering on the target voxel filtered point set to obtain a radius filtered point set; performing effective point screening on the radius filtered point set to obtain the feature points.

3. The pose estimation method of claim 2, wherein, The performing of the voxel filtering on the raw point cloud data set to obtain the initial voxel filtered point set comprises the following steps: performing first voxel filtering on the raw point cloud data set to obtain a first point set; performing second voxel filtering on the first point set to obtain a second point set; wherein a second voxel size used in the second voxel filtering is greater than a first voxel size used in the first voxel filtering, and the second voxel size is a non-integer multiple of the first voxel size; determining the second point set as the initial voxel filtered point set.

4. The pose estimation method of claim 2, wherein, The performing of the radius filtering on the target voxel filtered point set to obtain the radius filtered point set comprises the following steps: performing first radius filtering on the target voxel filtered point set to obtain a third point set; wherein one point is contained in a first radius range corresponding to the first radius filtering; performing second radius filtering on the third point set to obtain a fourth point set; wherein at least five points are contained in a second radius range corresponding to the second radius filtering; determining the fourth point set as the radius filtered point set.

5. The pose estimation method of claim 4, wherein, The performing of the effective point screening on the radius filtered point set to obtain the feature points comprises the following steps: calculating a first vector formed by each radius filtered point in the radius filtered point set and a nearest neighbor point corresponding to the radius filtered point to obtain each first vector group; calculating an included angle between each first vector in each first vector group; obtaining each standard included angle and a first angle difference threshold value; calculating a difference value between each included angle and a corresponding standard included angle; screening, from each first vector group, a first vector group in which the difference values of the included angles between each first vector satisfy the first angle difference threshold value; determining each radius filtered point corresponding to each screened first vector group as a feature point.

6. The pose estimation method of claim 5, wherein, The determining of the heading information of the moving target according to the positive direction of the collecting device collecting the raw point cloud data set and the feature points comprises the following steps: calculating a second vector formed by each feature point in the feature points and a nearest neighbor point corresponding to the feature point to obtain each second vector group; calculating an included angle between each second vector in each second vector group and the positive direction of the collecting device to obtain each heading angle subset; obtain a preset second angle difference threshold value; cluster each included angle in the set of heading angles according to the second angle difference threshold value using a Euclidean clustering algorithm to obtain each angle class; perform mean value calculation on each included angle in each angle class to obtain each angle mean value; obtain an odometer angle corresponding to the moving target; select the angle mean value with the smallest difference from the odometer angle from the angle mean values; determine the selected angle mean value as the heading information.

7. The pose estimation method of claim 6, wherein, After obtaining each subset of heading angles, before clustering each included angle in the set of heading angles formed by each subset of heading angles according to the second angle difference threshold value using a Euclidean clustering algorithm, the method further includes: add 90 degrees to each included angle in each subset of heading angles to obtain a first extended angle; subtract 90 degrees from each included angle in each subset of heading angles to obtain a second extended angle; add 180 degrees to each included angle in each subset of heading angles to obtain a third extended angle; add the first, second, and third extended angles corresponding to each included angle in each subset of heading angles to the corresponding subset of heading angles.

8. The pose estimation method of claim 6, wherein, The mean value calculation on each included angle in each angle class includes: perform size sorting on the number of included angles in each angle class to obtain an angle class sequence; select a preset number of angle classes from one end with a large number in the angle class sequence; perform mean value calculation on each included angle in each selected angle class; Correspondingly, the selection of the angle mean value as the heading information includes: obtain a relative angle between the positive direction of the moving target and the positive direction of the collection device; select the angle mean value with the smallest difference from the odometer angle from the angle mean values, and determine the heading information according to the selected angle mean value and the relative angle.

9. The pose estimation method of claim 8, wherein, The estimation of the pose of the moving target in the structured scene according to the heading information and the odometer distance includes: rotate the point cloud formed by each feature point according to the angle mean value to make the positive direction of the collection device parallel to the passage direction of the moving target in the structured scene; determine the lateral distance of the moving target in the passage of the structured scene using a statistical histogram; estimate the longitudinal distance of the moving target in the passage of the structured scene according to the odometer distance and the lateral distance; estimate the pose of the moving target in the structured scene according to the heading information, the lateral distance, and the longitudinal distance.

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

11. A computer readable storage medium, characterized in that, The computer program is stored on the computer readable storage medium and is executed by the processor to implement the steps of the pose estimation method according to any one of claims 1 to 9.

Images