A pose determination method and apparatus

Abstract

The application discloses a pose determination method and device, and relates to the technical field of mobile robots. The method comprises the following steps: dividing a reference point cloud into a plurality of grids; calculating the mean value and the covariance matrix of the reference point cloud in the normal distribution; performing eigenvalue decomposition on the covariance matrix; transforming a target point cloud based on a current pose to obtain a grid in which the target point cloud is located; determining an effective grid in the grid in which the target point cloud is located according to the eigenvalue of the grid and the eigenvector corresponding to the eigenvalue; the effective grid comprises a surface grid and / or a line grid; calculating the value of a target function according to the mean value and the target point cloud in the effective grid; when a preset termination condition is met, determining that the current pose is a target pose, otherwise, updating the current pose based on the value of the target function, and performing transformation on the target point cloud based on the current pose; wherein the target function comprises a line target function and a surface target function. The embodiment can improve the accuracy of the positioning result.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mobile robots, and in particular to a pose determination method and device. BACKGROUND

[0002] Precise positioning is a necessary condition for ensuring the safe driving of mobile robots such as autonomous vehicles, and laser radar positioning based on point cloud maps is widely used. This method matches the target point cloud obtained by real-time scanning of the laser radar with the reference point cloud (i.e. the point cloud map) established in advance to obtain the pose of the mobile robot. The currently used point cloud matching method includes NDT (Normal Distributions Transform).

[0003] However, for line features, this method not only considers the constraint perpendicular to the line, but also considers the constraint parallel to the line, and similarly, for surface features, this method not only considers the constraint perpendicular to the surface, but also considers the constraint parallel to the surface, and the constraint parallel to the line and the constraint parallel to the surface will cause the positioning result to deviate. SUMMARY

[0004] Therefore, the embodiments of the present application provide a pose determination method and device, which can improve the accuracy of the positioning result.

[0005] In a first aspect, the embodiments of the present application provide a pose determination method, comprising:

[0006] dividing the reference point cloud into a plurality of grids;

[0007] calculating the mean and covariance matrix of the reference point cloud in the grid under the normal distribution;

[0008] performing eigenvalue decomposition on the covariance matrix to obtain the eigenvalue of the grid;

[0009] transforming the target point cloud based on the current pose to obtain the grid in which the target point cloud is located;

[0010] determining an effective grid in the grid in which the target point cloud is located according to the eigenvalue of the grid; wherein the effective grid includes a surface grid with a surface distribution feature and / or a line grid with a line distribution feature;

[0011] calculating the value of the target function according to the mean and the target point cloud in the effective grid, determining the current pose as the target pose when a preset termination condition is met, otherwise, updating the current pose based on the value of the target function and performing the transformation of the target point cloud based on the current pose;

[0012] The target function includes a line target function and / or a surface target function, when the effective grid is a line grid, the value of the line target function is calculated by the eigenvector perpendicular to the straight line and the corresponding eigenvalue, the mean value, and the target point cloud in the effective grid; when the effective grid is a surface grid, the value of the surface target function is calculated by the eigenvector perpendicular to the surface and the corresponding eigenvalue, the mean value, and the target point cloud in the effective grid.

[0013] In a second aspect, an embodiment of the present application provides a pose determination apparatus, comprising:

[0014] a partition module configured to partition a reference point cloud into a plurality of grids, calculate a mean value of the reference point cloud in a normal distribution in the grid, and calculate a covariance matrix of the reference point cloud in the normal distribution in the grid;

[0015] a characteristic decomposition module configured to perform characteristic decomposition on the covariance matrix to obtain eigenvalues of the grid;

[0016] a transformation module configured to transform a target point cloud based on a current pose to obtain a grid where the target point cloud is located;

[0017] a determination module configured to determine an effective grid in the grid where the target point cloud is located according to the eigenvalues of the grid, wherein the effective grid includes a surface grid with a surface distribution characteristic and / or a line grid with a line distribution characteristic, calculate a value of a target function according to the mean value and the target point cloud in the effective grid, when a preset termination condition is met, determine that the current pose is a target pose, otherwise, update the current pose based on the value of the target function, and trigger the transformation module;

[0018] The target function includes a line target function and / or a surface target function, when the effective grid is a line grid, the value of the line target function is calculated by the eigenvector perpendicular to the straight line and the corresponding eigenvalue, the mean value, and the target point cloud in the effective grid; when the effective grid is a surface grid, the value of the surface target function is calculated by the eigenvector perpendicular to the surface and the corresponding eigenvalue, the mean value, and the target point cloud in the effective grid.

[0019] In a third aspect, an embodiment of the present application provides an electronic device, comprising:

[0020] one or more processors;

[0021] a storage device configured to store one or more programs,

[0022] When the one or more programs are executed by the one or more processors, the one or more processors implement the method in any of the above embodiments.

[0023] In a fourth aspect, an embodiment of the present application provides a computer readable medium having stored thereon a computer program which, when executed by a processor, implements the method according to any of the above embodiments.

[0024] An embodiment of the above application has the following advantages or beneficial effects: by eigen-decomposition of the covariance matrix, the line grid and the plane grid in the grid where the target point cloud is located are filtered out, for the line grid, only the eigenvector perpendicular to the straight line and the corresponding eigenvalue are used to calculate the value of the line target function, i.e. only the constraint perpendicular to the line is considered, and the constraint parallel to the line is not considered; for the plane grid, only the eigenvector perpendicular to the plane and the corresponding eigenvalue are used to calculate the value of the plane target function, i.e. only the constraint perpendicular to the plane is considered, and the constraint parallel to the plane is not considered. The embodiment of the present application can reduce the error caused by the constraint parallel to the line or the plane, and improve the accuracy of the pose.

[0025] Further effects of the above non-conventional optional mode will be described in the following in combination with the specific embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0026] The accompanying drawings are used to better understand the present application, and do not constitute an improper limitation on the present application. Among them:

[0027] Figure 1 is a schematic diagram of a spherical distribution point cloud provided by an embodiment of the present application;

[0028] Figure 2 is a schematic diagram of a planar distribution point cloud provided by an embodiment of the present application;

[0029] Figure 3 is a schematic diagram of a straight line distribution point cloud provided by an embodiment of the present application;

[0030] Figure 4 is a schematic diagram of the constraint of a straight line distribution point cloud in the optimization process provided by an embodiment of the present application;

[0031] Figure 5 is a flowchart of a pose determination method provided by an embodiment of the present application;

[0032] Figure 6 is a flowchart of a pose determination method provided by another embodiment of the present application;

[0033] Figure 7 is a schematic diagram of a pose determination device provided by an embodiment of the present application;

[0034] Figure 8 is a structural schematic diagram of a computer system of a terminal device or a server suitable for implementing the embodiments of the present application. DETAILED DESCRIPTION

[0035] The exemplary embodiments of this application are described herein making reference to the accompanying drawings, which illustrate various details of the application, and are to be taken in conjunction with the description below. Therefore, persons skilled in the art will recognize that modifications and changes can be made to the described embodiments without departing from the scope and spirit of the application. Also, descriptions of well-known functions and constructions are omitted for clarity and conciseness.

[0036] The NDT algorithm divides the reference point cloud into different grids, and based on the reference point cloud in the grid, calculates P vehicle that makes score(P vehicle ) maximum.

[0037]

[0038] wherein score(P vehicle ) is used to represent the score of the current pose, is used to represent the coordinate of the i-th point in the target point cloud of the k-th effective grid, μ k is used to represent the mean value of the reference point cloud in the k-th effective grid under the normal distribution, ∑ k is used to represent the covariance matrix of the reference point cloud in the k-th effective grid under the normal distribution, M k is used to represent the number of points in the target point cloud of the k-th effective grid.

[0039] The reference point cloud in the grid can exist in different distribution forms, such as spherical distribution, planar distribution and linear distribution, as shown in Figures 1-3 .

[0040] Taking a line feature as an example, in the optimization process, each point outside the line will be subjected to a constraint to the line, as shown in Figure 4 , which can be decomposed into a constraint perpendicular to the line and a constraint parallel to the line. The constraint perpendicular to the line is needed in the optimization process, which can make the straight line obtained by real-time scanning coincide with the straight line in the point cloud map, while the constraint parallel to the line is redundant, which causes deviation of the positioning result.

[0041] The plane feature is similar to the line feature, and the constraint perpendicular to the plane is needed in the optimization process, while the constraint parallel to the plane is redundant.

[0042] Therefore, for the line feature, the existing NDT algorithm not only considers the constraint perpendicular to the line, but also considers the constraint parallel to the line, and for the plane feature, the method not only considers the constraint perpendicular to the plane, but also considers the constraint parallel to the plane, and the constraint parallel to the line and the constraint parallel to the plane will cause deviation of the positioning result.

[0043] In view of this, as shown inFigure 5 As shown, the embodiment of the present application provides a pose determination method, comprising:

[0044] Step 501: dividing the reference point cloud into multiple grids.

[0045] The reference point cloud is a point cloud map established in advance. The smaller the size of the grid is, the higher the positioning accuracy is, but the larger the calculation amount is. The larger the size of the grid is, the smaller the calculation amount is, but the lower the positioning accuracy is. Therefore, in the actual application scenario, the size of the grid can be determined according to the business requirement.

[0046] Step 502: calculating the mean and covariance matrix of the reference point cloud in the grid under the normal distribution.

[0047] According to the reference point cloud in the grid, the normal distribution parameters of the grid are calculated, including the mean and the covariance matrix.

[0048] Step 503: performing eigenvalue decomposition on the covariance matrix to obtain the eigenvalue of the grid.

[0049] The number of eigenvalues and eigenvectors is related to the specific business scenario. For example, in a three-dimensional scenario, three eigenvalues and three eigenvectors can be obtained by decomposition, and in a two-dimensional scenario, two eigenvalues and two eigenvectors can be obtained by decomposition. Therefore, different ways of eigenvalue decomposition can be used according to the requirements of the scenario. For the convenience of description, the subsequent embodiments take a three-dimensional scenario as an example for description.

[0050] Step 504: transforming the target point cloud based on the current pose to obtain the grid where the target point cloud is located.

[0051] The first transformation uses the initial pose estimated in advance, and the subsequent iteration process will update the initial pose until the termination condition is met. It should be noted that the execution order of each step is not unique. For example, step 504 can be executed after step 503, or it can be executed before step 502.

[0052] Step 505: determining the effective grid in the grid where the target point cloud is located according to the eigenvalue of the grid.

[0053] The effective grid includes a face grid with a distribution feature of a face and / or a line grid with a distribution feature of a line. In the actual application scenario, the grid with a distribution feature of a point and a ball can also be regarded as an effective grid, and the composition of the effective grid is not limited to one case.

[0054] The eigenvalue can reflect the distribution feature of the point cloud in the direction of the eigenvector, and therefore, based on the eigenvalue and the corresponding eigenvector, the line grid and the face grid can be screened out.

[0055] Step 506: calculating the value of the target function according to the mean value and the target point cloud in the effective grid;

[0056] The target function includes a line target function and / or a surface target function, and when the effective grid is a line grid, the value of the line target function is calculated by the eigenvector perpendicular to the straight line and the corresponding eigenvalue, the mean value and the target point cloud in the effective grid; when the effective grid is a surface grid, the value of the surface target function is calculated by the eigenvector perpendicular to the surface and the corresponding eigenvalue, the mean value and the target point cloud in the effective grid.

[0057] Step 507: determining whether the preset termination condition is met, if yes, executing step 508, otherwise, updating the current pose based on the value of the target function and executing step 504.

[0058] The termination condition can be that the number of iterations reaches a certain threshold, or that the positioning accuracy reaches a set threshold, and the embodiment of the application does not limit this.

[0059] Step 508: determining that the current pose is the target pose.

[0060] The embodiment of the application filters out the line grid and the surface grid in the grid where the target point cloud is located through the eigenvalue decomposition of the covariance matrix, for the line grid, only the eigenvector perpendicular to the straight line and the corresponding eigenvalue are used to calculate the value of the line target function, that is, only the constraint perpendicular to the line is considered, and the constraint parallel to the line is not considered; for the surface grid, only the eigenvector perpendicular to the surface and the corresponding eigenvalue are used to calculate the value of the surface target function, that is, only the constraint perpendicular to the surface is considered, and the constraint parallel to the surface is not considered. The embodiment of the application can reduce the error caused by the constraint parallel to the line or the surface, and improve the accuracy of the pose.

[0061] In order to ensure the positioning accuracy, preferably, the number of points in the grid is generally not less than 6.

[0062] In an embodiment of the application, the effective grid is determined in the grid where the target point cloud is located according to the eigenvalues of the grid, comprising:

[0063] The line eigenvalues and the surface eigenvalues of each grid where the target point cloud is located are calculated according to the eigenvalues of the grid and the eigenvectors corresponding to the eigenvalues;

[0064] The line grid and the surface grid are determined in the multiple grids where the target point cloud is located according to the line eigenvalues and the surface eigenvalues;

[0065] The effective grid is determined in the line grid and the surface grid where the target point cloud is located.

[0066] Compared with point cloud distribution in the form of points and spheres, point cloud in the line grid and the surface grid has more obvious features, so that the accuracy of the pose can be improved. In addition, the point cloud in the form of points and spheres is filtered out, so that the calculation amount can be reduced and the pose determination efficiency can be improved.

[0067] In view of the fact that only ground points cannot have good constraints in the horizontal direction, in one embodiment of the present application,

[0068] The covariance matrix is subjected to eigenvalue decomposition to obtain eigenvalues of the grid, including:

[0069] The covariance matrix is subjected to eigenvalue decomposition to obtain eigenvalues of the grid and eigenvectors corresponding to the eigenvalues;

[0070] In the line grid and the surface grid where the target point cloud is located, an effective grid is determined, including:

[0071] According to the eigenvalues of the surface grid and the eigenvectors corresponding to the eigenvalues, the surface grid where the target point cloud is located is divided into a ground grid and a non-ground grid;

[0072] In the line grid, the ground grid and the non-ground grid where the target point cloud is located, an effective grid is determined.

[0073] The embodiment of the present application determines whether the grid belongs to the line grid or the surface grid through the eigenvalues, and further divides the surface grid into the ground grid and the non-ground grid based on the eigenvalues and the eigenvectors, so that the effective grid contains not only the line grid and the ground grid but also the non-ground grid, and the positioning accuracy can be improved. It should be noted that the above method of determining the effective grid is not only applicable to a three-dimensional scene, but also applicable to a two-dimensional scene and other scenes.

[0074] In an actual application scenario, the target point cloud can be transformed first, and then the type of the grid can be determined according to the eigenvalues of the grid, or the type of the grid can be determined according to the eigenvalues of the grid first, and then the target point cloud can be transformed.

[0075] In one embodiment of the present application, the covariance matrix is subjected to eigenvalue decomposition to obtain eigenvalues of the grid and eigenvectors corresponding to the eigenvalues, including:

[0076] The covariance matrix is subjected to eigenvalue decomposition based on the first formula;

[0077] The first formula includes:

[0078]

[0079] Wherein, Sigma is used to represent a covariance matrix, lambda1, lambda2 and lambda3 are used to represent eigenvalues respectively, q1, q2 and q3 are used to represent eigenvectors corresponding to the eigenvalues respectively, and q1, q2 and q3 are orthogonal to each other.

[0080] In order to ensure the accuracy of the line grid and the surface grid identification, the eigenvectors are orthogonal to each other. Through the embodiment of the application, the eigenvalues and the eigenvectors in a three-dimensional scene can be obtained, and similarly, the eigenvalues and the eigenvectors in other-dimensional scenes can also be obtained.

[0081] In an embodiment of the application, according to the eigenvalues of the grid, the line eigenvalues and the surface eigenvalues of each grid where the target point cloud is located are calculated, comprising:

[0082] According to the second formula, the line eigenvalues of each grid where the target point cloud is located are calculated;

[0083] According to the third formula, the surface eigenvalues of each grid where the target point cloud is located are calculated;

[0084] The second formula comprises:

[0085]

[0086] The third formula comprises:

[0087]

[0088] Wherein, c is used to represent a line eigenvalue, p is used to represent a surface eigenvalue, lambda1, lambda2 and lambda3 are used to represent eigenvalues respectively, and lambda1<=lambda2<=lambda3.

[0089] Wherein, 0<=c<1, the greater the value of c is, the more the distribution characteristics tend to be linear, 0<=p<1, the greater the value of p is, the more the distribution characteristics tend to be surface.

[0090] The application can accurately describe the distribution characteristics of the grid in the direction of the eigenvector through lambda1, lambda2 and lambda3, and further accurately divide the type of the grid, thereby improving the positioning accuracy.

[0091] In an actual application scenario, the line grid and the surface grid can be screened out by sorting the values of c and p, and the line grid and the surface grid can also be screened out by a preset threshold value, for example, the grid with a value of c greater than 0.8 is a line grid. In addition, the value of c of the grid can be calculated first, the line grid is screened out from multiple grids, then the value of p of the remaining grid is calculated, the surface grid is screened out, and vice versa. Of course, the value of c of the grid and the value of p of the grid can also be calculated, and the type of the grid is determined by comparing the values of c and p. The calculation method of the values of c and p is not limited to formula (3) and formula (4), for example, c=lambda3-lambda2,

[0092] The p value is similar, p = λ2- λ1,

[0093] In an embodiment of the present application, according to the line feature value and the surface feature value, the line grid and the surface grid are determined in the plurality of grids where the target point cloud is located, comprising:

[0094] When the line feature value is greater than a preset first threshold value, it is determined that the grid where the target point cloud is located is a line grid, and when the surface feature threshold value is greater than a preset second threshold value, it is determined that the grid where the target point cloud is located is a surface grid.

[0095] Considering that the line feature value of the line grid is larger and the surface feature value of the surface grid is larger, therefore, in the embodiment of the present application, the type of the grid is determined by the preset first threshold value and the second threshold value. If the line feature value of the grid is greater than the first threshold value and the surface feature value is greater than the second threshold value, the grid can be used as a line grid or a surface grid. In the actual application scenario, the line feature value can also be arranged in the first percentage of the grid in descending order, and the surface feature value can be arranged in the second percentage of the grid in descending order.

[0096] The embodiment of the present application can comprehensively consider the line feature and the surface feature of the grid, and improve the accuracy of grid division.

[0097] In an embodiment of the present application, according to the feature value of the surface grid and the feature vector corresponding to the feature value, the surface grid where the target point cloud is located is divided into a ground grid and a non-ground grid, comprising:

[0098] The angle between the feature vector corresponding to λ1 and the ground normal vector is calculated, and when the angle is less than a set angle threshold value, the surface grid is a ground grid, otherwise, the surface grid is a non-ground grid.

[0099] Since λ1≤ λ2≤ λ3, the feature vector corresponding to λ1 is the normal vector of the surface grid, and by calculating the angle between the feature vector and the ground normal vector, whether the surface grid is a ground grid can be determined. Specifically, the feature vector corresponding to λ1 can be dot multiplied with the vector (0, 0, 1), and then divided by the length of λ1, so as to determine whether the surface grid is a ground grid. In the actual application scenario, the plane equation of the surface grid can also be calculated, and based on the plane equation of the surface grid and the ground equation, whether the surface grid is a ground grid can be determined.

[0100] Through the embodiment of the present application, the surface grid can be further divided into a ground grid and a non-ground grid, so as to ensure that the effective grid contains a non-ground grid and avoid that the horizontal direction cannot be effectively constrained when there is only a ground point.

[0101] In an embodiment of the present application, in the line grid, the ground grid and the non-ground grid where the target point cloud is located, an effective grid is determined, comprising:

[0102] According to the order from large to small of the line feature values, a first number of line grids in the line grid in which the target point cloud is located are selected as effective grids;

[0103] According to the order from large to small of the surface feature values, a second number of ground grids in the ground grid in which the target point cloud is located are selected as effective grids;

[0104] According to the order from large to small of the surface feature values, a third number of non-ground grids in the non-ground grid in which the target point cloud is located are selected as effective grids.

[0105] It is considered that only the ground points cannot well constrain the horizontal direction, and the effective grids determined by the embodiment of the application include the line grids, the ground grids and the non-ground grids. It is considered that the larger the c value is, the closer to the line, and the larger the p value is, the closer to the surface, and therefore, the embodiment of the application selects the effective grids according to the order from large to small of the line feature values and the surface feature values. In the actual application scenario, all the line grids and the surface grids can also be selected as the effective grids.

[0106] The embodiment of the application further reduces the calculation amount and improves the positioning accuracy by the grid screening.

[0107] Preferably, the second number is the same as the third number, and the first number is equal to the sum of the second number and the third number.

[0108] In the actual application scenario, the first number, the second number and the third number can also have other proportional relationships, such as 1:1:1.

[0109] In an embodiment of the application, the line target function includes:

[0110]

[0111] wherein, score(P vehicle ) is used to represent the score of the current pose, is used to represent the coordinates of the i th point in the target point cloud of the k th effective grid, μ k is used to represent the average of the coordinates of the points in the reference point cloud of the k th effective grid, M k is used to represent the number of points in the target point cloud of the k th effective grid, and is used to represent the two feature vectors of the k th effective grid perpendicular to the straight line, and is used to represent the feature value corresponding to the feature vector of the k th effective grid perpendicular to the straight line.

[0112] In the embodiment of the present application, the line target function includes the eigenvector perpendicular to the straight line and the corresponding eigenvalue, the mean value of the reference point cloud in the effective grid under the normal distribution, and the coordinates of the points in the target point cloud in the effective grid. Compared with formula (1), the line target function removes the eigenvector parallel to the straight line and the corresponding eigenvalue, avoids the influence of the constraint parallel to the straight line on the positioning result, and improves the accuracy of determining the pose.

[0113] In actual application scenarios, the non-linear optimization method such as Newton method and gradient descent method can be used to optimize score(P vehicle ) to find P vehicle . vehicle .

[0114] In an embodiment of the present application, the surface target function includes:

[0115]

[0116] , wherein score(P vehicle ) is used to represent the score of the current pose, is used to represent the coordinates of the i th point in the target point cloud of the k th effective grid, μ k is used to represent the mean value of the coordinates of the points in the reference point cloud of the k th effective grid, M k is used to represent the number of points in the target point cloud of the k th effective grid, is used to represent the eigenvector perpendicular to the surface of the k th effective grid, is used to represent the eigenvalue corresponding to the eigenvector perpendicular to the surface of the k th effective grid.

[0117] In the embodiment of the present application, the surface target function includes the eigenvector perpendicular to the surface and the corresponding eigenvalue, the mean value of the reference point cloud in the effective grid under the normal distribution, and the coordinates of the points in the target point cloud in the effective grid. Compared with formula (1), the surface target function removes the eigenvector parallel to the surface and the corresponding eigenvalue, avoids the influence of the constraint parallel to the surface on the positioning result, and improves the accuracy of determining the pose.

[0118] In actual application scenarios, the line target function can also be a variation of formula (5), such as formula (7), and the surface target function can also be a variation of formula (6), such as formula (8), wherein the line target function removes the eigenvector parallel to the line and the eigenvalue, and the surface target function removes the eigenvector parallel to the surface and the eigenvalue.

[0119]

[0120]

[0121] AsFigure 6 As shown, the embodiment of the present application takes an autonomous vehicle as an example to illustrate the pose determination method in a three-dimensional scene, which comprises the following steps:

[0122] Step 601: Divide the reference point cloud into multiple grids.

[0123] The reference point cloud is obtained by scanning the area to be positioned by the vehicle-mounted laser radar of the autonomous vehicle.

[0124] Step 602: Calculate the mean and covariance matrix of the reference point cloud in the grid under normal distribution.

[0125] For the grid with point number not less than 6, the coordinate mean of the reference point cloud in each grid under normal distribution is calculated, and the covariance matrix is calculated based on the coordinate mean.

[0126] Step 603: Perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalue of the grid and the eigenvector corresponding to the eigenvalue.

[0127] Eigenvalue decomposition is performed on the covariance matrix based on formula (2) to obtain the eigenvalues λ1, λ2 and λ3 of the grid, and the eigenvectors q1, q2 and q3 corresponding to the eigenvalues, q1, q2 and q3 are orthogonal to each other.

[0128] Step 604: Transform the target point cloud based on the current pose to obtain the grid where the target point cloud is located.

[0129] When the target point cloud is scanned, the target point cloud is transformed based on the initial vehicle pose estimated in advance or the current pose determined in the last iteration to obtain the grid where the target point cloud is located.

[0130] Step 605: Calculate the line eigenvalue and surface eigenvalue of each grid where the target point cloud is located according to the eigenvalue of the grid.

[0131] Based on formula (3) and formula (4), the c value and p value of the grid are calculated.

[0132] Step 606: When the line eigenvalue is greater than a preset first threshold, the grid where the target point cloud is located is determined as a line grid, and when the surface eigenvalue is greater than a preset second threshold, the grid where the target point cloud is located is determined as a surface grid.

[0133] Step 607: Calculate the angle between the eigenvector corresponding to λ1 and the ground normal vector, when the angle is less than a set angle threshold, the surface grid is a ground grid, otherwise, the surface grid is a non-ground grid.

[0134] Step 608: Selecting a first number of line grids in the line grid in which the target point cloud is located as effective grids in the order of line eigenvalues from large to small.

[0135] Selecting h line grids as effective grids in the order of c values from large to small.

[0136] Step 609: Selecting a second number of ground grids in the ground grid in which the target point cloud is located as effective grids in the order of surface eigenvalues from large to small.

[0137] Selecting u1 ground grids as effective grids in the order of p values from large to small.

[0138] Step 610: Selecting a third number of non-ground grids in the non-ground grid in which the target point cloud is located as effective grids in the order of surface eigenvalues from large to small.

[0139] Selecting u2 non-ground grids as effective grids in the order of p values from large to small.

[0140] u1 = u2, h = u1 + u2.

[0141] Step 611: When the effective grid is a line grid, calculating the value of the line target function according to the eigenvalue and the corresponding eigenvalue perpendicular to the straight line, the mean value, and the target point cloud in the effective grid; when the effective grid is a surface grid, calculating the value of the surface target function according to the eigenvalue and the corresponding eigenvalue perpendicular to the surface, the mean value, and the target point cloud in the effective grid.

[0142] Step 612: Determining whether a termination condition is met, if yes, executing step 613, otherwise, updating the current pose based on the value of the target function, and executing step 604.

[0143] Step 613: Determining that the current pose is a target pose.

[0144] Selecting the selected effective grids as the final matching grids, and performing optimization iteration based on the target functions of formulas (5) and (6) to obtain a final vehicle pose.

[0145] For line features, the present embodiment only considers the eigenvalue and the eigenvalue perpendicular to the line, and for surface features, the present embodiment only considers the eigenvalue and the eigenvalue perpendicular to the surface. Therefore, the present embodiment can avoid the influence of constraints parallel to the line and parallel to the surface, and improve the positioning accuracy.

[0146] In addition, the present embodiment can screen out some point clouds that affect positioning accuracy, such as tree crowns, and select light poles, tree trunks, and wall surfaces as line and surface point clouds, which not only improves the calculation efficiency, but also improves the positioning accuracy.

[0147] AsFigure 7 As shown, the embodiment of the present application provides a pose determination device, comprising:

[0148] The partition module 701 is configured to partition the reference point cloud into a plurality of grids; calculate the mean value of the reference point cloud in the normal distribution in the grid; and calculate the covariance matrix according to the mean value and the covariance matrix of the reference point cloud in the normal distribution;

[0149] The eigenvalue decomposition module 702 is configured to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalue of the grid.

[0150] The transformation module 703 is configured to transform the target point cloud based on the current pose to obtain the grid where the target point cloud is located.

[0151] The determination module 704 is configured to determine the effective grid in the grid where the target point cloud is located according to the eigenvalue of the grid; wherein the effective grid includes a face grid with a distribution feature of a face and / or a line grid with a distribution feature of a line; calculate the value of the target function according to the mean value, the target point cloud in the effective grid, and when the preset termination condition is met, determine the current pose as the target pose, otherwise, update the current pose based on the value of the target function and trigger the transformation module 703.

[0152] The target function includes a line target function and / or a face target function, the value of the line target function is calculated by the characteristic vector perpendicular to the straight line and the corresponding eigenvalue, the mean value, and the target point cloud in the effective grid when the effective grid is a line grid; and the value of the face target function is calculated by the characteristic vector perpendicular to the face and the corresponding eigenvalue, and the target point cloud in the effective grid when the effective grid is a face grid.

[0153] In an embodiment of the present application, the eigenvalue decomposition module 702 is configured to perform eigenvalue decomposition on the covariance matrix based on a first formula to obtain the eigenvalue of the grid and the characteristic vector corresponding to the eigenvalue.

[0154] The first formula includes:

[0155]

[0156] Wherein, Σ is used to represent the covariance matrix, λ1, λ2 and λ3 are used to represent the eigenvalue respectively, q1, q2 and q3 are used to represent the characteristic vector corresponding to the eigenvalue respectively, and q1, q2 and q3 are orthogonal to each other.

[0157] In an embodiment of the present application, the determination module 704 is configured to calculate the line eigenvalue and the face eigenvalue of each grid where the target point cloud is located according to the eigenvalue of the grid; determine the line grid and the face grid in the plurality of grids where the target point cloud is located according to the line eigenvalue and the face eigenvalue; and determine the effective grid in the line grid and the face grid where the target point cloud is located.

[0158] In one embodiment of the present application, the eigenvalue decomposition module 702 is configured to perform eigenvalue decomposition on the covariance matrix to obtain eigenvalues of the grid and eigenvectors corresponding to the eigenvalues; the determination module 704 is configured to divide the surface grid where the target point cloud is located into a ground grid and a non-ground grid according to the eigenvalues of the surface grid and the eigenvectors corresponding to the eigenvalues; and determine the effective grid in the line grid, the ground grid and the non-ground grid where the target point cloud is located.

[0159] In one embodiment of the present application, the determination module 704 is configured to calculate the line eigenvalues of each grid where the target point cloud is located according to a second formula; and calculate the surface eigenvalues of each grid where the target point cloud is located according to a third formula.

[0160] The second formula includes:

[0161]

[0162] The third formula includes:

[0163]

[0164] Wherein, c is used to represent the line eigenvalue, p is used to represent the surface eigenvalue, λ1, λ2 and λ3 are respectively used to represent the eigenvalues, λ1≤λ2≤λ3.

[0165] In one embodiment of the present application, the determination module 704 is configured to determine the grid where the target point cloud is located as a line grid when the line eigenvalue is greater than a preset first threshold value, and determine the grid where the target point cloud is located as a surface grid when the surface eigenvalue is greater than a preset second threshold value.

[0166] In one embodiment of the present application, the determination module 704 is configured to calculate the angle between the eigenvector corresponding to λ1 and the ground normal vector, and determine the surface grid as a ground grid when the angle is less than a set angle threshold value, otherwise, the surface grid is a non-ground grid.

[0167] In one embodiment of the present application, the determination module 704 is configured to select a first number of line grids as effective grids in the line grid where the target point cloud is located in the order of the line eigenvalues from large to small; select a second number of ground grids as effective grids in the ground grid where the target point cloud is located in the order of the surface eigenvalues from large to small; and select a third number of non-ground grids as effective grids in the non-ground grid where the target point cloud is located in the order of the surface eigenvalues from large to small.

[0168] In one embodiment of the present application, the line objective function includes:

[0169]

[0170] Among them, score(P) vehicle The score used to characterize the current pose. μ is used to characterize the coordinates of the i-th point in the target point cloud of the k-th effective grid. k M is used to characterize the average coordinates of all points in the reference point cloud of the k-th effective grid. k Used to characterize the number of points in the target point cloud of the k-th effective grid. and Two eigenvectors perpendicular to the line are used to characterize the k-th effective grid. and The eigenvalues ​​corresponding to the eigenvectors perpendicular to the lines that characterize the k-th effective grid.

[0171] In one embodiment of the present invention, the surface objective function includes:

[0172]

[0173] Among them, score(P) vehicle The score used to characterize the current pose. μ is used to characterize the coordinates of the i-th point in the target point cloud of the k-th effective grid. k M is used to characterize the average coordinates of all points in the reference point cloud of the k-th effective grid. k Used to characterize the number of points in the target point cloud of the k-th effective grid. The eigenvector perpendicular to the surface used to characterize the kth effective grid. The eigenvalues ​​are used to characterize the eigenvectors perpendicular to the surface of the k-th effective grid.

[0174] This invention provides an electronic device, comprising:

[0175] One or more processors;

[0176] Storage device for storing one or more programs.

[0177] When one or more programs are executed by one or more processors, the one or more processors implement the methods as described in any of the above embodiments.

[0178] This invention provides a computer-readable medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any of the above embodiments.

[0179] The following is for reference. Figure 8 It shows a schematic diagram of the structure of a computer system 800 suitable for implementing a terminal device of the present invention. Figure 8 The terminal device shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of the present invention.

[0180] like Figure 8 As shown, the computer system 800 includes a central processing unit (CPU) 801, which can perform various appropriate actions and processes based on programs stored in read-only memory (ROM) 802 or programs loaded from storage section 808 into random access memory (RAM) 803. The RAM 803 also stores various programs and data required for the operation of the system 800. The CPU 801, ROM 802, and RAM 803 are interconnected via a bus 804. An input / output (I / O) interface 805 is also connected to the bus 804.

[0181] The following components are connected to I / O interface 805: an input section 806 including a keyboard, mouse, etc.; an output section 807 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and speakers, etc.; a storage section 808 including a hard disk, etc.; and a communication section 809 including a network interface card such as a LAN card, modem, etc. The communication section 809 performs communication processing via a network such as the Internet. A drive 810 is also connected to I / O interface 805 as needed. A removable medium 811, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed on drive 810 as needed so that computer programs read from it can be installed into storage section 808 as needed.

[0182] In particular, according to the embodiments disclosed in this invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments disclosed in this invention include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 809, and / or installed from removable medium 811. When the computer program is executed by central processing unit (CPU) 801, it performs the functions defined above in the system of this invention.

[0183] It should be noted that the computer-readable medium shown in the present application can be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium may, for example, but is not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or component, or any combination of the above. More specific examples of computer-readable storage media can include, but are not limited to, an electrical connection having one or more wires, a portable computer diskette, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present application, the computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, device or component. In the present application, the computer-readable signal medium can include a data signal carried in a baseband or as a part of a carrier wave, which carries computer-readable program code. Such a propagated data signal can take various forms, including but not limited to an electromagnetic signal, an optical signal or any suitable combination of the above. The computer-readable signal medium can also be any computer-readable medium other than the computer-readable storage medium, which can send, propagate or transmit a program for use by or in conjunction with an instruction execution system, device or component. The program code contained on the computer-readable medium can be transmitted by any suitable medium, including but not limited to wireless, wire, optical cable, RF, etc., or any suitable combination of the above.

[0184] The flowcharts and block diagrams in the drawings illustrate the possible implementation architectures, functions and operations of the systems, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowcharts or block diagrams can represent a module, a program segment or a part of code containing one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions noted in the blocks can occur in different order than that shown in the drawings. For example, two blocks that are shown in succession can actually be executed substantially in parallel, and sometimes in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams or flowcharts, and the combination of blocks in the block diagrams or flowcharts, can be implemented by a dedicated hardware-based system that performs the specified functions or operations, or can be implemented by a combination of dedicated hardware and computer instructions.

[0185] The modules described in the embodiments of the present application can be implemented in the form of software or in the form of hardware. The described modules can also be arranged in a processor, for example, a processor can be described as including a sending module, an obtaining module, a determining module and a first processing module. In some cases, the names of these modules do not constitute a limitation on the modules themselves, for example, the sending module can also be described as "a module that sends a picture obtaining request to a connected server".

[0186] The specific embodiments described above do not constitute a limitation on the protection scope of the present application. Those skilled in the art should understand that various modifications, combinations, sub-combinations and substitutions can occur depending on design requirements and other factors. Any modification, equivalent replacement and improvement made within the spirit and principles of the present application shall fall within the scope of the protection of the present application.

Claims

1. A pose determination method, characterized in that, include: Divide the reference point cloud into multiple grids; Calculate the mean and covariance matrix of the reference point cloud in the grid under a normal distribution; The covariance matrix is ​​subjected to eigenvalue decomposition to obtain the eigenvalues ​​of the grid. The target point cloud is transformed based on the current pose to obtain the grid in which the target point cloud is located; Based on the feature values ​​of the grid, a valid grid is determined in the grid where the target point cloud is located; wherein, the valid grid includes: a surface grid with a distribution feature of surface and / or a line grid with a distribution feature of line; Based on the mean and the target point cloud in the effective grid, the value of the objective function is calculated. When the preset termination condition is met, the current pose is determined to be the target pose. Otherwise, the current pose is updated based on the value of the objective function, and the transformation of the target point cloud based on the current pose is performed. The objective function includes a line objective function and / or a surface objective function. When the effective grid is a line grid, the value of the line objective function is obtained by computing the feature vector perpendicular to the line and its corresponding feature value, the mean, and the target points in the effective grid. When the effective grid is a surface grid, the value of the surface objective function is obtained by computing the feature vector perpendicular to the surface and its corresponding feature value, the mean, and the target points in the effective grid.

2. The method as described in claim 1, characterized in that, Based on the feature values ​​of the grid, valid grids are determined within the grid containing the target point cloud, including: Based on the feature values ​​of the grid, calculate the line feature values ​​and surface feature values ​​of each grid in which the target point cloud is located; Based on the line feature value and the surface feature value, determine the line grid and the surface grid in the multiple grids where the target point cloud is located; The effective grid is determined within the line grid and surface grid where the target point cloud is located.

3. The method as described in claim 2, characterized in that, The covariance matrix is ​​subjected to eigenvalue decomposition to obtain the eigenvalues ​​of the grid, including: Eigenvalue decomposition is performed on the covariance matrix to obtain the eigenvalues ​​of the grid and the eigenvectors corresponding to the eigenvalues; Determining the effective grid within the line grid and surface grid containing the target point cloud includes: Based on the feature values ​​of the surface mesh and the feature vectors corresponding to the feature values, the surface mesh containing the target point cloud is divided into ground mesh and non-ground mesh; The effective grid is determined from the line grid, ground grid, and non-ground grid in which the target point cloud is located.

4. The method as described in claim 3, characterized in that, Eigenvalue decomposition is performed on the covariance matrix to obtain the eigenvalues ​​of the grid and the corresponding eigenvectors, including: Based on the first formula, the covariance matrix is ​​subjected to eigenvalue decomposition. The first formula includes: Wherein, Σ is used to characterize the covariance matrix, λ1, λ2 and λ3 are used to characterize the eigenvalues, q1, q2 and q3 are used to characterize the eigenvectors corresponding to the eigenvalues, and q1, q2 and q3 are pairwise orthogonal; Based on the feature values ​​of the grid, calculate the line feature values ​​and surface feature values ​​of each grid in which the target point cloud is located, including: According to the second formula, calculate the line feature values ​​of each grid in which the target point cloud is located; According to the third formula, calculate the surface feature values ​​of each grid in which the target point cloud is located; The second formula includes: The third formula includes: Where c is used to characterize the line feature value, p is used to characterize the surface feature value, λ1, λ2 and λ3 are used to characterize the feature value respectively, and λ1≤λ2≤λ3.

5. The method as described in claim 4, characterized in that, Based on the line feature value and the surface feature value, the line grid and the surface grid are determined in the multiple grids containing the target point cloud, including: When the line feature value is greater than the surface feature value, the grid containing the target point cloud is the line grid; otherwise, the grid containing the target point cloud is the surface grid.

6. The method as described in claim 4, characterized in that, Based on the feature values ​​of the surface mesh and the corresponding feature vectors, the surface mesh containing the target point cloud is divided into ground meshes and non-ground meshes, including: Calculate the angle between the eigenvector corresponding to λ1 and the ground normal vector. When the angle is less than a set angle threshold, the surface mesh is a ground mesh; otherwise, the surface mesh is a non-ground mesh.

7. The method as described in claim 4, characterized in that, Determining the effective grid among the line grid, ground grid, and non-ground grid where the target point cloud is located includes: According to the order of the line feature values ​​from largest to smallest, a first number of line grids are selected as the effective grids in the line grid where the target point cloud is located; According to the order of the surface feature values ​​from largest to smallest, a second number of ground grids are selected as the effective grids in the ground grid where the target point cloud is located; According to the order of the surface feature values ​​from largest to smallest, the third number of non-ground grids in the non-ground grid where the target point cloud is located are selected as the effective grids.

8. The method according to any one of claims 4-7, characterized in that, The linear objective function includes: Among them, score(P) vehicle The score is used to characterize the current pose. μ is used to characterize the coordinates of the i-th point in the target point cloud of the k-th effective grid. k M is used to characterize the average coordinates of all points in the reference point cloud of the k-th effective grid. k Used to characterize the number of points in the target point cloud of the k-th effective grid. and Two eigenvectors perpendicular to the line are used to characterize the k-th effective grid. and The eigenvalues ​​corresponding to the eigenvectors perpendicular to the lines that characterize the k-th effective grid.

9. The method according to any one of claims 4-7, characterized in that, The surface objective function includes: Among them, score(P) vehicle The score is used to characterize the current pose. μ is used to characterize the coordinates of the i-th point in the target point cloud of the k-th effective grid. k M is used to characterize the average coordinates of all points in the reference point cloud of the k-th effective grid. k Used to characterize the number of points in the target point cloud of the k-th effective grid. The eigenvector perpendicular to the surface used to characterize the kth effective grid. The eigenvalues ​​are used to characterize the eigenvectors perpendicular to the surface of the k-th effective grid.

10. A pose determination device, characterized in that, include: The partitioning module is configured to divide the reference point cloud into multiple grids; Calculate the mean of the reference point cloud in the grid under a normal distribution; Based on the mean and covariance matrix of the reference point cloud under a normal distribution; The eigenvalue decomposition module is configured to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues ​​of the grid. The transformation module is configured to transform the target point cloud based on the current pose to obtain the mesh in which the target point cloud is located; The determination module is configured to determine a valid grid in the grid where the target point cloud is located based on the feature values ​​of the grid; wherein, the valid grid includes: a surface grid with a distribution feature of surfaces and / or a line grid with a distribution feature of lines; calculate the value of an objective function based on the mean and the target point cloud in the valid grid; when a preset termination condition is met, determine the current pose as the target pose; otherwise, update the current pose based on the value of the objective function and trigger the transformation module; The objective function includes a line objective function and / or a surface objective function. When the effective grid is a line grid, the value of the line objective function is obtained by computing the feature vector perpendicular to the line and its corresponding feature value, the mean, and the target points in the effective grid. When the effective grid is a surface grid, the value of the surface objective function is obtained by computing the feature vector perpendicular to the surface and its corresponding feature value, and the target points in the effective grid.

Images