Pose estimation method, device and equipment and computer readable storage medium

Abstract

The invention discloses a pose estimation method, device and equipment and a computer readable storage medium. The pose estimation method comprises the steps that an original point cloud data set of a structured scene where a moving target is located is extracted; extracting each feature point from the original point cloud data set; determining course information of the moving target according to the positive direction of the acquisition equipment for acquiring the original point cloud data set and each feature point; acquiring a speedometer distance corresponding to the moving target; and estimating the pose of the moving target in the structured scene according to the course information and the distance of the speedometer. According to the method, the existing scene does not need to be additionally transformed, the interference on the normal working environment of the site is avoided, and the pose of the moving target can be accurately calculated.

Description

Technical Field

[0001] The present application relates to the technical field of object spatial pose estimation, and particularly to a pose estimation method, device, equipment and computer-readable storage medium. Background Technique

[0002] In some structured scenarios, such as long shelves, long storage racks, etc., which have long legs at the bottom for support, are regularly placed, and have narrow channels in the middle, a moving target (such as a robot) equipped with a point cloud scanning device operates in the scenario. 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 traditional point cloud registration methods (such as Iterative Closest Point and Normal Distribution Transform) cannot accurately determine the position of the moving target.

[0003] Currently, the commonly used pose estimation methods for moving targets in structured scenarios mainly include two types. One is to perform pose estimation by artificially adding some regularly distributed markers, such as spraying paint at fixed intervals, placing combinations of objects with fixed shapes at fixed intervals, or laying magnetic strips on the ground. The other is to estimate the position of the moving target solely through an odometer. However, both of the above methods have their respective disadvantages. First of all, the method of setting markers requires the transformation of the on-site environment, which not only increases the deployment cost but may also interfere with the normal working environment on-site. And these transformation measures may be damaged over time, such as the paint fading, the fixed objects being moved, the magnetic strips demagnetizing, etc., thus interfering with the positioning of the moving target and even possibly causing the moving target to collide with on-site equipment and resulting in on-site accidents. Secondly, the method of solely using an odometer will have a large error over time in the estimation of the horizontal and directional directions, and it is difficult to meet the positioning requirements.

[0004] In summary, how to effectively solve the problems of high deployment cost, interference with the normal working environment on-site, poor safety, low pose estimation accuracy, etc. of the current pose estimation methods is an urgent problem that those skilled in the art need to solve currently. Summary of the Invention

[0005] The purpose of the present application is to provide a pose estimation method, which does not require additional transformation of the existing scenario, avoids interference with the normal working environment on-site, and can accurately calculate the pose of the moving 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, including:

[0008] Extract the original point cloud data set of the structured scene where the moving target is located;

[0009] Extract each feature point from the original point cloud data set;

[0010] Determine the heading information of the moving target according to the positive direction of the acquisition device for acquiring the original point cloud data set and each feature point;

[0011] Obtain the odometer distance corresponding to the moving target;

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

[0013] In a specific embodiment of the present application, extracting each feature point from the original point cloud data set includes:

[0014] Perform voxel filtering on the original point cloud data set to obtain an initial voxel-filtered point set;

[0015] Find the nearest neighbor points corresponding to each point in the initial voxel-filtered point set from the original point cloud data set;

[0016] Use each nearest neighbor point to perform corresponding replacement on each point in the initial voxel-filtered point set to obtain a target voxel-filtered point set;

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

[0018] Perform effective point screening on the radius-filtered point set to obtain each feature point.

[0019] In a specific embodiment of the present application, performing voxel filtering on the original point cloud data set to obtain an initial voxel-filtered point set includes:

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

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

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

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

[0024] Perform a first radius filtering on the filtered point set of the target voxels to obtain a third point set; wherein, there is one point within the first radius corresponding to the first radius filtering.

[0025] Perform a second radius filtering on the third point set to obtain a fourth point set; wherein, there are at least five points within the second radius corresponding to the second radius filtering.

[0026] Determine the fourth point set as the filtered point set after radius filtering.

[0027] In a specific implementation manner of the present application, perform effective point screening on the filtered point set after radius filtering to obtain each feature point, including:

[0028] Calculate the first vectors formed by each filtered point after radius filtering in the filtered point set after radius filtering and the nearest neighbor points corresponding to the filtered points after radius filtering, respectively, to obtain each first vector group.

[0029] Calculate the angles between each pair of the first vectors in each first vector group.

[0030] Obtain each preset standard angle and the first angle difference threshold.

[0031] Calculate the differences between each angle and the corresponding standard angle.

[0032] Screen out the first vector groups in which the differences between the angles between each pair of the first vectors satisfy the first angle difference threshold from each first vector group.

[0033] Determine the filtered points after radius filtering corresponding to each screened first vector group as each feature point.

[0034] In a specific implementation manner of the present application, determine the heading information of the moving target according to the positive direction of the acquisition device for acquiring the original point cloud data set and each feature point, including:

[0035] Calculate the second vectors formed by each feature point in each feature point and the nearest neighbor points corresponding to the feature point, respectively, to obtain each second vector group.

[0036] Calculate the angles between each second vector in each second vector group and the positive direction of the acquisition device, respectively, to obtain each heading angle subset.

[0037] Obtain the preset second angle difference threshold.

[0038] Cluster each angle in the heading angle set formed by each heading angle subset according to the second angle difference threshold using the Euclidean clustering algorithm to obtain each angle class.

[0039] Calculate the mean values of the angles included in each angle class to obtain each angle mean value.

[0040] Obtain the odometer angle corresponding to the moving target;

[0041] Select the angle mean with the smallest difference from the odometer angle from among the angle means;

[0042] Determine the selected angle mean as the heading information.

[0043] In a specific embodiment of the present application, 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 using the Euclidean clustering algorithm according to the second angle difference threshold, it further includes:

[0044] Add 90 degrees to each included angle in each subset of heading angles to obtain a first expanded angle;

[0045] Subtract 90 degrees from each included angle in each subset of heading angles to obtain a second expanded angle;

[0046] Add 180 degrees to each included angle in each subset of heading angles to obtain a third expanded angle;

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

[0048] In a specific embodiment of the present application, calculating the mean of each included angle in each angle class includes:

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

[0050] Select the first preset number of angle classes from the end with a larger number in the angle class sequence;

[0051] Calculate the mean of each included angle in each selected angle class;

[0052] Correspondingly, determining the selected angle mean as the heading information includes:

[0053] Obtain the relative angle between the positive direction of the moving target and the positive direction of the acquisition device;

[0054] Select the angle mean with the smallest difference from the odometer angle from among the angle means, and determine the heading information based on the selected angle mean and the relative angle.

[0055] In a specific 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 includes:

[0056] Rotate the point cloud formed by each feature point according to the average angle, so that the forward direction of the acquisition device is parallel to the channel direction of the moving target in the structured scene;

[0057] Use a statistical histogram to determine the lateral distance of the moving target in the channel of the structured scene;

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

[0059] Estimate the 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, comprising:

[0061] A memory for storing a computer program;

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

[0063] A computer-readable storage medium, on which a computer program is stored, and the computer program realizes the steps of the pose estimation method as described above when executed by a processor.

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

[0065] As can be seen from the above technical solutions, by extracting the original point cloud data set of the structured scene where the moving target is located, determining the heading information of the moving target according to the forward direction of the acquisition device for collecting the original point cloud data set and each feature point, and then fusing the odometer information of the moving target itself, the accurate pose of the moving target in the structured scene is realized. Since the extracted point features are under the original structured scene, the pose calculation is realized by extracting the inherent information in the scene, and there is no need to transform the existing scene additionally. Therefore, the problem of feature degradation over time will not occur, the interference to the normal working environment on site 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 itself, the pose of the moving target can be accurately calculated.

[0066] Correspondingly, the present application further provides a pose estimation device, a device and a computer-readable storage medium corresponding to the above pose estimation method, which have the above technical effects and will not be elaborated here. Description of the Drawings

[0067] In order to more clearly illustrate the technical solutions in the embodiments of the present application or related technologies, the following will briefly introduce the drawings required for use in the description of the embodiments or related technologies. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0068] Figure 1 It is a flowchart of an implementation of a pose estimation method in an embodiment of the present application;

[0069] Figure 2 It is a flowchart of an implementation of another pose estimation method in an embodiment of the present application;

[0070] Figure 3 It is a schematic diagram of an original point cloud data set in an embodiment of the present application;

[0071] Figure 4 It is a schematic diagram of a point set after radius filtering in an embodiment of the present application;

[0072] Figure 5 It is a schematic diagram of another point set after radius filtering in an embodiment of the present application;

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

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

[0075] Figure 8 It is a schematic diagram of the process of extracting the lateral distance in an embodiment of the present application;

[0076] Figure 9 It is a statistical histogram for lateral distance statistics in an embodiment of the present application;

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

[0078] Figure 11 It is a structural block diagram of a pose estimation device in an embodiment of the present application;

[0079] Figure 12 It is a specific structural schematic diagram of a pose estimation device provided in an embodiment of the present application. Specific Embodiments

[0080] To enable those skilled in the art to better understand the solution of this application, the following further detailed description of this application will be given in conjunction with the accompanying drawings and specific embodiments. Obviously, the described embodiments are only a part of the embodiments of this application, rather than all the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the scope of protection of this application.

[0081] See Figure 1 , Figure 1 which is the implementation flowchart of a pose estimation method in an embodiment of this application. The method may include the following steps:

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

[0083] When the moving target runs in the structured scene, extract the original point cloud data set of the structured scene where the moving target is located. For example, a single-line lidar or a multi-line lidar can be used to extract the original point cloud data set of the structured scene where the moving target is located. The lidar is generally set at a height of 20 cm - 30 cm below the ground. By setting this height, the structural support parts (such as legs, etc.) and some other fragmented information in the structured scene can be collected.

[0084] The moving target may include a robot, an autonomous vehicle, etc.

[0085] The structured scene may include shelves, storage racks, etc. supported by high legs, or some breeding scenes such as chicken coops, duck coops, 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, as well as a lot of other fragmented information or noise. After extracting the original point cloud data set of the structured scene where the moving target is located, 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 moving target according to the positive direction of the acquisition device for collecting the original point cloud data set and each feature point.

[0089] Obtain the forward direction of the acquisition device for acquiring the original point cloud data set of the mobile target. The forward direction of the acquisition device can be the same as the forward direction of the mobile target or there can be a fixed angular deviation from the forward direction of the mobile target. After extracting each feature point, determine the heading information of the mobile target according to the forward direction of the acquisition device for acquiring the original point cloud data set and each feature point. For example, vectors formed by connecting lines between each feature point can be obtained, and the heading information of the mobile target can be determined according to the included angle between each formed vector and the forward direction of the acquisition device for acquiring the original point cloud data set.

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

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

[0092] The mobile target body is equipped with an odometer, and the running distance of the mobile target can be recorded through the odometer carried by the mobile target body. Obtain the odometer distance corresponding to the mobile target.

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

[0094] After determining the heading information of the mobile target and obtaining the odometer distance corresponding to the mobile target, estimate the pose of the mobile target in the structured scene according to the heading information and the odometer distance. The starting position of the mobile target in the structured scene can be recorded in advance. For example, the starting position of the mobile target in the structured scene is generally the exit or entrance of the passage in the structured scene. The pose of the mobile target in the structured scene can be estimated according to the starting position, heading information, and odometer distance of the mobile target in the structured scene.

[0095] As can be seen from the above technical solution, by extracting the original point cloud data set of the structured scene where the mobile target is located, determining the heading information of the mobile target according to the forward direction of the acquisition device for acquiring the original point cloud data set and each feature point, and then fusing the odometer information of the mobile target body, the accurate pose of the mobile target in the structured scene is realized. Since the extracted are the point features under the original structured scene, the pose calculation is realized by extracting the inherent information in the scene, without the need to additionally transform the existing scene. Therefore, there will be no problem of feature degradation over time, avoiding interference with the normal working environment on-site and improving the safety of the mobile target running in the structured scene. Through the analysis of the existing features and combining the odometer information of the mobile target body, the pose of the mobile target can be accurately calculated.

[0096] It should be noted that based on the above embodiments, the embodiments of the present application also provide corresponding improvement solutions. In the subsequent embodiments, the same steps or corresponding steps as those in the above embodiments can be referred to each other, and the corresponding beneficial effects can also be referred to each other, which will not be elaborated one by one in the following improved embodiments.

[0097] See Figure 2 , Figure 2 which is a flowchart of another pose estimation method in the embodiments of the present application. This method may include the following steps:

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

[0099] See Figure 3 , Figure 3 which is a schematic diagram of an original point cloud data set in the embodiments of the present application. The original point cloud data set collected by the acquisition device has structured feature points, and there is also a lot of fragmented other information or noise. The 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 the initial voxel-filtered point set.

[0101] After extracting the original point cloud data set of the structured scene, perform voxel filtering on the original point cloud data set to obtain the initial voxel-filtered point set. Through voxel filtering, the noise in the original point cloud data set can be effectively removed, and the features of the structured scene can be retained.

[0102] In a specific implementation manner of the present application, step S202 may include the following steps:

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

[0104] Step 2: Perform second voxel filtering on the first point set to obtain the second point set; wherein, the second voxel size used in the second voxel filtering is larger 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;

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

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

[0107] When performing voxel filtering on the original point cloud dataset, two voxel filtering processes can be included. It is preset that the second voxel size used in the second voxel filtering is larger 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 voxelSize1, and the second voxel size is set to voxelSize2. Generally, voxelSize2 is set to the minimum distance minDistance between two legs, and voxelSize1 is generally set to 0.3 times (voxelSize2 + 0.05). First, perform the first voxel filtering on the original point cloud dataset to obtain the first point set. Then, perform the second voxel filtering on the first point set to obtain the second point set. Furthermore, determine the second point set as the point set after initial voxel filtering.

[0108] By setting the voxel size of the large-size voxel filtering to minDistance, only one point can be retained in each leg area. Considering that there may be a situation where multiple points on the same leg point cloud are assigned to different voxel areas, a small voxel filtering is performed before the large voxel filtering. The voxel size of the small voxel filtering can be set to 0.3 * (voxelSize2 + 0.05) to prevent duplication with the voxel boundary of the large voxel filtering. The specific size of the voxel size of the small voxel filtering can be fine-tuned according to the actual scenario. The point set after voxel filtering can be named pcByVoxelFilter.

[0109] By first performing the initial voxel filtering using the voxel filter body with the voxel size of the small voxel filtering, and then performing the further voxel filtering using the voxel filter body with the voxel size of the large voxel filtering, and setting the voxel size of the large voxel filtering as a non-integer multiple of the voxel size of the small voxel filtering, both the effective removal of noise is achieved and the duplication between the voxels of the small voxel filtering and the voxel boundary of the large voxel filtering can be prevented.

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

[0111] After obtaining the point set after initial voxel filtering, the points retained in the point set after initial voxel filtering are all the centroid points within the voxel. This point is not necessarily a point in the original point cloud dataset, which will lead to incorrect guidance for subsequent calculations. Therefore, the nearest neighbor points corresponding to each point in the point set after initial voxel filtering are searched for in the original point cloud dataset. For example, a KD tree (K-dimensional tree) can be constructed for pcInitializaton, and the nearest neighbor points of each point in pcByVoxelFilter are searched from it. The retained nearest neighbor points are the point cloud realPCByVoxelFilter after voxel filtering. The nearest neighbor points of each point in the point set after initial voxel filtering in the original point cloud dataset may be one or multiple. When the nearest neighbor point is one, this point is directly determined as the nearest neighbor point; when the nearest neighbor points are multiple, a point can be randomly selected from multiple points as the nearest neighbor point, or a suitable point can be selected from multiple points according to the positional relationship of other points around the already determined point as the nearest neighbor point.

[0112] S204: Use each nearest neighbor point to perform corresponding replacement on each point in the point set after initial voxel filtering to obtain the point set after target voxel filtering.

[0113] After finding the nearest neighbor points corresponding to each point in the point set after initial voxel filtering from the original point cloud dataset, use each nearest neighbor point to perform corresponding replacement on each point in the point set after initial voxel filtering to obtain the point set after target voxel filtering. By using the nearest neighbor points in the original point cloud dataset to perform corresponding replacement on each point in the point set after initial voxel filtering, incorrect guidance for subsequent calculations is avoided.

[0114] S205: Perform radius filtering on the point set after target voxel filtering to obtain the point set after radius filtering.

[0115] After using each nearest neighbor point to perform corresponding replacement on each point in the point set after initial voxel filtering to obtain the point set after target voxel filtering, there will still be some noise points. Perform radius filtering on the point set after target voxel filtering to obtain the point set after radius filtering. By continuing to perform radius filtering on the point set after target voxel filtering after using each nearest neighbor point to perform corresponding replacement on each point in the point set after initial voxel filtering, further denoising of the original point cloud dataset is achieved.

[0116] In a specific implementation manner of the present application, step S205 may include the following steps:

[0117] Step 1: Perform first radius filtering on the point set after target voxel filtering to obtain the third point set; wherein, within the first radius range corresponding to the first radius filtering, there is one point;

[0118] Step 2: Perform second radius filtering on the third point set to obtain a fourth point set; wherein, at least five points are included within the second radius corresponding to the second radius filtering.

[0119] Step 3: Determine the fourth point set as the point set after radius filtering.

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

[0121] When performing radius filtering on the point set after filtering the target voxel, first perform first radius filtering on the point set after filtering the target voxel to obtain a third point set, then perform second radius filtering on the third point set to obtain a fourth point set, and determine the fourth point set as the point set after radius filtering. Set that one point is included within the first radius corresponding to the first radius filtering, and five points are included within the second radius corresponding to the second radius filtering.

[0122] Continuing with the above example, it can be set that during the first radius filtering, the points with only one point within the radius with (minDistance - 0.1) as the radius are retained. Perform radius filtering on the point cloud after the first radius filtering again, set the maximum distance between the two legs as maxDistance, set the filtering radius as (maxDistance + 0.1), retain the points with at least 5 points within the radius, and also retain the surrounding points. Name the point cloud after two radius filterings as pcByRadiusFilter.

[0123] By performing two radius filterings with different scales on the point set after filtering the target voxel, further denoising of the original point cloud data set is achieved, and the overall structure of the point cloud in the structured scene is maintained.

[0124] S206: Perform effective point screening on the point set after radius filtering to obtain each feature point.

[0125] See Figure 4 , Figure 4 This is a schematic diagram of a point set after radius filtering in an embodiment of the present application. After obtaining the point set after radius filtering, the point set after radius filtering may contain invalid points that only meet the radius range but have angular deviations. Perform effective point screening on the point set after radius filtering to obtain each feature point. By performing effective point screening on the point set after radius filtering, the accuracy of the description of each feature point for the structured scene is ensured.

[0126] In a specific implementation manner of the present application, step S206 may include the following steps:

[0127] Step 1: Calculate the first vectors formed by each point after radius filtering in the point set after radius filtering and the nearest neighbor points corresponding to the points after radius filtering respectively, to obtain each group of first vectors;

[0128] Step 2: Calculate the angles between every two first vectors in each first vector group;

[0129] Step 3: Obtain each preset standard angle and the first angle difference threshold;

[0130] Step 4: Calculate the differences between each angle and the corresponding standard angle;

[0131] Step 5: Screen out the first vector groups in which the differences between the angles between every two first vectors in each first vector group all meet the first angle difference threshold;

[0132] Step 6: Determine the radius-filtered points corresponding to each screened first vector group as each feature point.

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

[0134] When performing effective point screening on the radius-filtered point set, calculate the first vectors formed by each radius-filtered point in the radius-filtered point set and the nearest neighbor points corresponding to the radius-filtered points respectively to obtain each first vector group, and calculate the angles between every two first vectors in each first vector group. Preset the standard angles between every two first vectors, and preset the first angle difference threshold, obtain each preset standard angle and the first angle difference threshold, calculate the differences between each angle and the corresponding standard angle, screen out the first vector groups in which the differences between the angles between every two first vectors in each first vector group all meet the first angle difference threshold, and determine the radius-filtered points corresponding to each screened first vector group as each feature point. By performing effective point screening on the radius-filtered point set according to the preset first angle difference threshold, it is ensured that each screened feature point conforms to the angle distribution law of each point in the structured scene.

[0135] See Figure 5 , Figure 5 which is a schematic diagram of another radius-filtered point set in the embodiment of the present application. Assume that the legs in the structured scene are perpendicular in the horizontal and vertical directions. Then, for each radius-filtered point, calculate the first vector group [v1, v2, v3, v4] formed by its surrounding 4 points and the radius-filtered point respectively. The current structured scene point feature is that for any two vectors selected from the first vector group, they are in a perpendicular relationship or a parallel relationship. Traverse the angles delta between all pairs of vectors in the first vector group. Delta should be 0 degrees, 180 degrees, 90 degrees or -90 degrees. Set a tolerance error toleranceBiaAngle. If the calculated delta is within the tolerance range, it can be considered that the point meets the requirements. The point cloud that meets the requirements is shown as the square marked points in Figure 5 .

[0136] S207: Determine the heading information of the moving target based on the forward direction of the acquisition device that acquires the original point cloud dataset and each feature point.

[0137] In a specific implementation manner of the present application, step S207 may include the following steps:

[0138] Step 1: Calculate the second vector formed by each feature point and its corresponding nearest neighbor point among each feature point, and obtain each second vector group;

[0139] Step 2: Calculate the angle between each second vector in each second vector group and the forward direction of the acquisition device respectively, and obtain each heading angle subset;

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

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

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

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

[0144] Step 7: Screen out the angle mean value with the smallest difference from the odometer angle from each angle mean value;

[0145] Step 8: Determine the screened angle mean value as the heading information.

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

[0147] When determining the heading information of the moving target based on the forward direction of the acquisition device that acquires the original point cloud dataset and each feature point, calculate the second vector formed by each feature point and its corresponding nearest neighbor point among each feature point, and obtain each second vector group. Calculate the angle between each second vector in each second vector group and the forward direction of the acquisition device respectively, and obtain each heading angle subset. Obtain a preset second angle difference threshold, and cluster each angle in the heading angle set formed by each heading angle subset according to the second angle difference threshold using the Euclidean clustering algorithm, such as using the K-means clustering algorithm to perform angle clustering, and obtain each angle class. Calculate the mean value of each angle included in each angle class, and obtain each angle mean value. Obtain the odometer angle odomYaw corresponding to the moving target, screen out the angle mean value with the smallest difference from the odometer angle from each angle mean value, and determine the screened angle mean value as the heading information. By combining the forward direction of the acquisition device and the odometer angle to determine the heading information, the accuracy of the determined heading information is improved.

[0148] In a specific embodiment of the present application, 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 using the Euclidean clustering algorithm according to the second angle difference threshold, the method may further include the following steps:

[0149] Step 1: Add 90 degrees to each included angle in each subset of heading angles to obtain a first expanded angle;

[0150] Step 2: Subtract 90 degrees from each included angle in each subset of heading angles to obtain a second expanded angle;

[0151] Step 3: Add 180 degrees to each included angle in each subset of heading angles to obtain a third expanded angle;

[0152] Step 4: Add the first expanded angle, the second expanded angle, and the third expanded angle corresponding to each included angle in each subset of heading angles to the corresponding subset of heading angles.

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

[0154] See Figure 6 , Figure 6 which is a schematic diagram of the state of a moving target in a structured scenario in an embodiment of the present application. After obtaining each subset of heading angles, add 90 degrees to each included angle in each subset of heading angles to obtain a first expanded angle, subtract 90 degrees from each included angle in each subset of heading angles to obtain a second expanded angle, add 180 degrees to each included angle in each subset of heading angles to obtain a third expanded angle, and add the first expanded angle, the second expanded angle, and the third expanded angle corresponding to each included angle in each subset of heading angles to the corresponding subset of heading angles. By expanding the angles of each subset of heading angles, four scenarios of the possible states of the moving target in the structured scenario can be expanded, realizing the expansion of the states of the moving target in the structured scenario. Figure 6 in which the moving target may exist in the structured scenario, achieving the expansion of the existing states of the moving target in the structured scenario.

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

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

[0157] Step 2: Select the first preset number of angle classes from the end with a larger number in the angle class sequence;

[0158] Step 3: Calculate the mean value of each included angle in each of the selected angle classes;

[0159] Correspondingly, determining the mean value of the selected angles as the heading information includes:

[0160] Step 1: Obtain the relative angle between the positive direction of the moving target and the positive direction of the acquisition device;

[0161] Step 2: Screen out the angle mean with the smallest difference from the odometer angle among the angle means, and determine the heading information based on the screened angle mean and the relative angle.

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

[0163] After clustering the included angles in the heading angle set formed by each heading angle subset using the Euclidean clustering algorithm according to the second angle difference threshold to obtain each angle class, sort the number of included angles in each angle class in descending order to obtain an angle class sequence. Screen the first preset number of angle classes from the end with a larger number in the angle class sequence, calculate the mean value of each included angle in the screened angle class to obtain each angle mean. Assume that 3 angle classes are screened, and the corresponding angle means calculated for the 3 angle classes are yaw1, yaw2, and yaw3. Obtain the relative angle between the positive direction of the moving target and the positive direction of the acquisition device, screen out the angle mean with the smallest difference from the odometer angle among the angle means, and determine the heading information position_yaw based on the screened angle mean and the relative angle. By screening the first preset number of angle classes from the end with a larger number in the angle class sequence for angle mean calculation and screening the angle mean 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: Obtain the odometer distance corresponding to the moving target.

[0165] S209: Estimate the pose of the moving target in the structured scene based on the heading information and the odometer distance.

[0166] In a specific implementation manner of the present application, step S209 may include the following steps:

[0167] Step 1: Rotate the point cloud formed by each feature point according to the angle mean to make the positive direction of the acquisition device parallel to the channel direction of the moving target in the structured scene;

[0168] Step 2: Use the statistical histogram to determine the lateral distance of the moving target in the channel of the structured scene;

[0169] Step 3: Estimate the longitudinal distance of the moving target in the channel of the structured scene based on the odometer distance and the lateral distance;

[0170] Step 4: Estimate the pose of the moving target in the structured scene based on the heading information, the lateral distance, and the longitudinal distance.

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

[0172] Refer to Figure 7 , Figure 7 , which is a schematic diagram of the states 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] Refer to Figure 8 , Figure 8 , which is a schematic diagram of a process for extracting the lateral distance in an embodiment of the present application. The statistical histogram is used to determine the lateral distance (position_y) of the moving target in the channel of the structured scene. The process of determining the lateral distance may include intercepting the point cloud within a certain area near the outrigger in the y-axis direction of the rotated point cloud, and respectively extracting the left and right areas.

[0174] Refer to Figure 9 , Figure 9 , which 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, and the rectangle bar (bin) corresponding to the point with the most occurrences in the statistical histogram is selected, which is the lateral distance of the moving target.

[0175] Estimate the longitudinal distance (position_x) of the moving target in the channel of the structured scene 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. It is also possible to obtain the starting point where the moving target enters the structured scene, set the starting point where the moving target enters the structured scene as the starting point of the odometer, calculate the lateral movement distance according to the starting point and the lateral distance, and subtract the lateral movement distance from the odometer distance to estimate the longitudinal distance. After estimating the heading information, the lateral distance, and the longitudinal distance, estimate the pose of the moving target in the structured scene 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 both the lateral distance and the longitudinal distance of the moving target in the channel of the structured scene are the positions of the moving target relative to the coordinate origin of the structured scene. The coordinate origin of the structured scene may be the same as the starting point where the moving target enters the structured scene, or there may be a fixed deviation. The embodiments of the present application do not make any limitations in this regard.

[0177] Corresponding to the above method embodiments, the present application also provides a pose estimation device. The pose estimation device described below can be correspondingly referred to the pose estimation method described above.

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

[0179] An original point cloud data set extraction module 11, configured to extract an original point cloud data set of a structured scene where a moving target is located;

[0180] A feature point extraction module 12, configured to extract respective feature points from the original point cloud data set;

[0181] A heading information determination module 13, configured to determine the heading information of the moving target according to the forward direction of the acquisition device for acquiring the original point cloud data set and each feature point;

[0182] An odometer distance acquisition module 14, configured to acquire the odometer distance corresponding to the moving target;

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

[0184] As can be seen from the above technical solutions, by extracting the original point cloud data set of the structured scene where the moving target is located, determining the heading information of the moving target according to the forward direction of the acquisition device for acquiring the original point cloud data set and each feature point, and then fusing the odometer information of the moving target itself, the accurate pose of the moving target in the structured scene is realized. Since the extracted point features are under the original structured scene, the pose calculation is realized by extracting the inherent information in the scene, without the need to additionally transform the existing scene, so there will be no problem of feature degradation over time, avoiding interference with the normal working environment on site, and improving the safety of the moving target running in the structured scene. By analyzing the existing features and combining the odometer information of the moving target itself, the pose of the moving 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, 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, configured to search for the respective 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 sub-module is used to perform corresponding replacement on each point in the initially voxel filtered point set by using each nearest neighbor point to obtain the target voxel filtered point set;

[0189] The radius filtered point set obtaining sub-module is used to perform radius filtering on the target voxel filtered point set to obtain the radius filtered point set;

[0190] The feature point obtaining sub-module is used to perform effective point screening on the radius filtered point set to obtain each feature point.

[0191] In a specific implementation manner of the present application, the initially voxel filtered point set obtaining sub-module includes:

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

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

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

[0195] In a specific implementation manner of the present application, the radius filtered point set obtaining sub-module includes:

[0196] The third point set obtaining unit is used to perform first radius filtering on the target voxel filtered point set to obtain the third point set; wherein, within the first radius corresponding to the first radius filtering, there is one point;

[0197] The fourth point set obtaining unit is used to perform second radius filtering on the third point set to obtain the fourth point set; wherein, within the second radius corresponding to the second radius filtering, there are at least five points;

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

[0199] In a specific implementation manner of the present application, the feature point obtaining sub-module includes:

[0200] The first vector group obtaining unit is used to calculate 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 respectively to obtain each first vector group;

[0201] The included angle calculating unit is used to calculate the included angle between each pair of first vectors in each first vector group;

[0202] A standard included angle and first angle difference threshold obtaining unit, configured to obtain each preset standard included angle and first angle difference threshold;

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

[0204] A first vector group screening unit, configured to screen from each first vector group the first vector groups in which the differences between the included angles of every two first vectors satisfy the first angle difference threshold;

[0205] A feature point determination unit, configured to determine the radius-filtered points corresponding to each of the screened first vector groups as each feature point.

[0206] In a specific embodiment of the present application, the heading information determination module 13 includes:

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

[0208] A heading angle subset obtaining sub-module, configured to calculate the included angle between each second vector in each second vector group and the positive direction of the acquisition device respectively, to obtain each heading angle subset;

[0209] A second angle difference threshold obtaining sub-module, configured to obtain a preset second angle difference threshold;

[0210] An angle class obtaining sub-module, configured to perform clustering on each included angle in the heading angle set formed by each heading angle subset by using the Euclidean clustering algorithm according to the second angle difference threshold, to obtain each angle class;

[0211] An angle mean obtaining sub-module, configured to calculate the mean value of each included angle included in each angle class, to obtain each angle mean;

[0212] An odometer angle obtaining sub-module, configured to obtain the odometer angle corresponding to the moving target;

[0213] A minimum angle mean screening sub-module, configured to screen from each angle mean the angle mean with the smallest difference from the odometer angle;

[0214] A heading information determination sub-module, configured to determine the screened angle mean as the heading information.

[0215] In a specific embodiment of the present application, the device may further include:

[0216] The first extended angle obtaining module is used to add 90 degrees to each included angle in each subset of heading angles before clustering each included angle in the set of heading angles formed by each subset of heading angles using the Euclidean clustering algorithm according to the second angle difference threshold after obtaining each subset of heading angles, so as to obtain the first extended angle;

[0217] The second extended angle obtaining module is used to subtract 90 degrees from each included angle in each subset of heading angles to obtain the second extended angle;

[0218] The third extended angle obtaining module is used to add 180 degrees to each included angle in each subset of heading angles to obtain the third extended angle;

[0219] The angle adding module is used to add the first extended angle, the second extended angle and the third extended angle corresponding to each included angle in each subset of heading angles to the corresponding subset of heading angles.

[0220] In a specific embodiment of the present application, the angle mean obtaining sub-module includes:

[0221] The angle class sequence obtaining unit is used to sort the number of included angles in each angle class in descending order to obtain the angle class sequence;

[0222] The angle class screening unit is used to screen the first preset number of angle classes from the end with a larger number in the angle class sequence;

[0223] The mean value calculating unit is used to calculate the mean value of each included angle in each screened angle class;

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

[0225] The relative angle obtaining unit is used to obtain the relative angle between the positive direction of the moving target and the positive direction of the acquisition device;

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

[0227] In a specific embodiment of the present application, the pose estimation module 15 includes:

[0228] The point cloud rotation sub-module is used to rotate the point cloud formed by each feature point according to the angle mean, so that the positive direction of the acquisition device is parallel to the channel direction of the moving target in the structured scene;

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

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

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

[0232] Corresponding to the above method embodiment, refer to Figure 11 , Figure 11 , which is a schematic diagram of a pose estimation device provided by the present application. The device may include:

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

[0234] A processor 322, configured to implement the steps of the pose estimation method in the above method embodiment when executing the computer program.

[0235] Specifically, please refer to Figure 12 , Figure 12 , which is a specific structural schematic diagram of a pose estimation device provided in this embodiment. The pose estimation device may vary greatly due to configuration or performance differences, and may include a processor (central processing units, CPU) 322 (for example, one or more processors) and a memory 332. The memory 332 stores one or more computer programs 342 or data 344. Among them, the memory 332 may be short-term storage or persistent storage. The program stored in the memory 332 may include one or more modules (not shown in the figure), and each module may include a series of instruction operations on the data processing device. Further, the processor 322 may be set to communicate with the memory 332 and execute a series of instruction operations in the memory 332 on the pose estimation device 301.

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

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

[0238] Corresponding to the above method embodiment, the present application further provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the following steps may be implemented:

[0239] Extract the original point cloud dataset of the structured scenario where the moving target is located; extract each feature point from the original point cloud dataset; determine the heading information of the moving target according to the positive direction of the acquisition device for collecting the original point cloud dataset and each feature point; obtain the odometer distance corresponding to the moving target; estimate the pose of the moving target in the structured scenario according to the heading information and the odometer distance.

[0240] The computer-readable storage medium may include: various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memories (ROM), random access memories (RAM), magnetic disks, or optical discs.

[0241] For the introduction of the computer-readable storage medium provided in this application, please refer to the above method embodiments, and this application will not be elaborated here.

[0242] In this specification, the various embodiments are described in a progressive manner. The key point of each embodiment is to illustrate the differences from other embodiments. The same or similar parts among the various embodiments can be referred to each other. For the devices, equipment, and computer-readable storage media disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the description of the method part.

[0243] Specific examples are used in this article to elaborate on the principle and implementation manner of this application. The description of the above embodiments is only used to help understand the technical solution and its core idea of this application. It should be pointed out that for those of ordinary skill in the art in this technical field, without departing from the principle of this application, several improvements and modifications can be made to this application, and these improvements and modifications also fall within the protection scope of this application.

Claims

1. A pose estimation method, characterized in that: include: Extract the original point cloud dataset of the structured scene where the moving target is located; Extracting each feature point from the original point cloud data set; Determining the heading information of the moving target according to the positive direction of the acquisition device that acquires the original point cloud data set and each feature point; Obtaining the odometer distance corresponding to the moving target; The position and posture of the mobile target in the structured scene is estimated according to the heading information and the odometer distance.

2. The method for posture estimation according to claim 1, characterized in that: Extracting feature points from the original point cloud data set includes: Performing voxel filtering on the original point cloud data set to obtain an initial voxel filtered point set; Find the nearest neighbor point corresponding to each point in the initial voxel filtered point set from the original point cloud data set; Using each nearest neighbor point to replace each point in the initial voxel filtered point set, 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; Effective points are screened on the radius filtered point set to obtain characteristic points.

3. The method for posture estimation according to claim 2, characterized in that: The original point cloud data set is subjected to voxel filtering to obtain an initial voxel filtered point set, including: Performing a first voxel filtering on the original point cloud data set to obtain a first point set; Performing a 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 larger 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; The second point set is determined as the initial voxel filtered point set.

4. The method for posture estimation according to claim 2, characterized in that: Performing radius filtering on the target voxel filtered point set to obtain the radius filtered point set includes: Performing a first radius filter on the target voxel filtered point set to obtain a third point set; wherein a point is contained within a first radius range corresponding to the first radius filter; Performing a second radius filter on the third point set to obtain a fourth point set; wherein the second radius range corresponding to the second radius filter contains at least five points; The fourth point set is determined as the radius-filtered point set.

5. The method for posture estimation according to claim 4, characterized in that: The effective points of the radius filtered point set are screened to obtain the characteristic points, including: Calculate first vectors formed by each radius filtered point in the radius filtered point set and the nearest neighboring point corresponding to the radius filtered point to obtain first vector groups; Calculate the angles between each pair of first vectors in each first vector group; Obtaining preset standard angles and a first angle difference threshold; Calculate the difference between each angle and the corresponding standard angle; Selecting from each first vector group a first vector group in which the differences between the angles between each pair of first vectors all satisfy the first angle difference threshold; The radius filtered points corresponding to the screened first vector groups are determined as feature points.

6. The method for posture estimation according to claim 5, characterized in that: Determining the heading information of the moving target according to the positive direction of the acquisition device for acquiring the original point cloud data set and each feature point, including: Calculate the second vectors formed by each feature point and the nearest neighbor point corresponding to the feature point to obtain each second vector group; Calculating the angles between each second vector in each second vector group and the positive direction of the acquisition device to obtain each heading angle subset; Obtaining a preset second angle difference threshold; Clustering each angle in the heading angle set composed of each heading angle subset using the Euclidean clustering algorithm according to the second angle difference threshold to obtain each angle class; Calculate the mean of each angle contained in each angle class to obtain the mean of each angle; Obtaining the odometer angle corresponding to the moving target; Selecting the angle mean with the smallest difference from the odometer angle from the angle means; The screened angle mean is determined as the heading information.

7. The method for posture estimation according to claim 6, characterized in that: After obtaining each heading angle subset, before clustering each angle in the heading angle set formed by each heading angle subset using the Euclidean clustering algorithm according to the second angle difference threshold, the method further includes: Add 90 degrees to each included angle in each heading angle subset to obtain a first expanded angle; Subtract 90 degrees from each included angle in each heading angle subset to obtain a second expanded angle; In each heading angle subset, each included angle is increased by 180 degrees to obtain a third expanded angle; The first expanded angle, the second expanded angle and the third expanded angle corresponding to each included angle in each heading angle subset are added to the corresponding heading angle subset.

8. The method for posture estimation according to claim 6, characterized in that: The mean of each angle contained in each angle class is calculated, including: Sort the number of angles contained in each angle class to obtain an angle class sequence; Filtering a preset number of angle classes from the end with the largest number in the angle class sequence; Calculate the mean of each angle contained in each angle class obtained by screening; Accordingly, the angle mean obtained by screening is determined as the heading information, including: Acquire the relative angle between the positive direction of the moving target and the positive direction of the acquisition device; The angle mean having the smallest difference with the odometer angle is selected from each angle mean, and the heading information is determined according to the selected angle mean and the relative angle.

9. The method for posture estimation according to claim 8, characterized in that: Estimating the position and posture of the mobile target in the structured scene according to the heading information and the odometer distance, including: Rotating the point cloud formed by each feature point according to the angle mean so that the positive direction of the acquisition device is parallel to the channel direction of the moving target in the structured scene; Determining the lateral distance of the moving target in the channel of the structured scene using a statistical histogram; estimating a longitudinal distance of the moving target in a channel of the structured scene according to the odometer distance and the lateral distance; The position and posture of the mobile target in the structured scene is estimated according to the heading information, the lateral distance, and the longitudinal distance.

10. A posture estimation device, characterized in that: include: Memory for storing computer programs; A processor, configured to implement the steps of the pose estimation method as claimed in any one of claims 1 to 9 when executing the computer program.

11. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the pose estimation method according to any one of claims 1 to 9.

Images