阅读说明：本技术 激光雷达匹配定位方法及装置 (Laser radar matching positioning method and device ) 是由 梁宝华 黄友 张国龙 张放 李晓飞 张德兆 王肖 霍舒豪 于 2019-09-19 设计创作，主要内容包括：本发明提供了一种激光雷达匹配定位方法,包括：获取当前激光雷达点云信息；根据位置坐标,将地面点和地面以上点映射在预设的栅格地图上；计算栅格地图中的每个栅格的地面点的分布和地面以上点的分布；根据每个栅格的地面点的分布和地面以上点的分布,计算每个栅格中各点的概率；以栅格地图中的第一栅格为中心,确定预设搜索范围内的栅格集；当车辆位于栅格集中的任一栅格时,根据与车辆高度对应的点的概率,计算不同偏航角对应的匹配概率；根据栅格集中的每个栅格中不同偏航角对应的匹配概率,确定概率矩阵；根据概率矩阵,对定位结果进行评价,生成评价结果；根据评价结果,确定车辆的定位结果。由此,排除了场景的差别,能很好适应多场景。(The invention provides a laser radar matching positioning method, which comprises the following steps: acquiring current laser radar point cloud information; according to the position coordinates, mapping the ground points and the points above the ground on a preset grid map; calculating the distribution of the ground points of each grid in the grid map and the distribution of the points above the ground; calculating the probability of each point in each grid according to the distribution of the ground point of each grid and the distribution of the points above the ground; determining a grid set in a preset search range by taking a first grid in a grid map as a center; when the vehicle is positioned in any grid in the grid set, calculating the matching probability corresponding to different yaw angles according to the probability of points corresponding to the height of the vehicle; determining a probability matrix according to the matching probability corresponding to different yaw angles in each grid in the grid set; evaluating the positioning result according to the probability matrix to generate an evaluation result; and determining the positioning result of the vehicle according to the evaluation result. Therefore, the method eliminates the difference of scenes and can be well adapted to multiple scenes.)

1. A laser radar matching positioning method is characterized by comprising the following steps:

acquiring current laser radar point cloud information; the laser radar point cloud information comprises ground points and points above the ground, and each point has a position coordinate;

mapping the ground point and the points above the ground on a preset grid map according to the position coordinates;

calculating the distribution of ground points and the distribution of points above the ground of each grid in the grid map;

calculating the probability of each point in each grid according to the distribution of the ground point of each grid and the distribution of the points above the ground;

determining a grid set in a preset search range by taking a first grid in a grid map as a center; the first grid is a grid where a positioning result predicted value is located;

when the vehicle is located in any grid in the grid set, calculating matching probabilities corresponding to different yaw angles according to the probability of points corresponding to the height of the vehicle;

determining a probability matrix according to the matching probability corresponding to different yaw angles in each grid in the grid set;

evaluating a positioning result according to the probability matrix to generate an evaluation result;

and determining the positioning result of the vehicle according to the evaluation result.

2. The method of claim 1, wherein prior to the obtaining current lidar point cloud information, the method further comprises:

acquiring original laser point cloud information;

and performing coordinate conversion on the original laser point cloud information to obtain the current laser point cloud information under a world coordinate system.

3. The method according to claim 1, wherein calculating the probability of each point in each grid according to the distribution of the ground point and the distribution of the points above the ground for each grid comprises:

according to the formulaCalculating the probability of each point in each grid;

wherein (mu)^g，σ^g) Distribution of ground points (μ)^o，σ^o) For the distribution of points above the ground, the position of each grid on the grid map is (i, j), the position of each grid on the world coordinate system is (x, y), and (i, j) ═ x/r, y/r, r is the resolution of the grid map, p is the resolution of the grid map_z|i，jIs the probability, p, of a point in the (i, j) grid at a height z_z|i，j＝p_(x，y，z)。

4. The method according to claim 1, wherein when the vehicle is located in any one of the grids, the calculating the matching probabilities corresponding to different yaw angles according to the probability of the point corresponding to the vehicle height to obtain the matching probability set of each grid specifically includes:

according to the formulaCalculating the matching probability corresponding to different yaw angles;

wherein points is the points of the original point cloud_rawAfter coordinate change, point cloud under a world coordinate system; in the world coordinate system, the number of search grids in the x and y directions is m, the search range in the yaw direction is da, the search range of grids in the x direction is (r-m, r + m), the search range of grids in the y direction is (c-m, c + m), the search range of yaw is (yaw-da, yaw + da), and yaw is a yaw angle.

5. The method according to claim 1, wherein determining a probability matrix according to the matching probabilities corresponding to different yaw angles in each grid in the grid set specifically includes:

determining the maximum matching probability in each grid according to the matching probability corresponding to different yaw angles in each grid in the grid set;

the maximum match probability in each grid constitutes a probability matrix.

6. The method according to claim 1, wherein the evaluating the positioning result according to the probability matrix to generate an evaluation result specifically includes:

carrying out normalization processing on the probability matrix;

respectively calculating Gaussian distribution parameters in the x direction and the y direction according to the probability matrix after normalization processing;

and comparing the Gaussian distribution parameters in the x direction and the y direction with preset threshold values, and if each distribution parameter is smaller than the parameters in the preset threshold values, evaluating the result as a positioning result.

7. The method of claim 6, wherein the method is based on a formulaCarrying out normalization processing on the probability matrix; wherein, P_ijAs a probability matrix, P0_ijSum (p) is the sum of all elements of the probability matrix;

according to the formulaCalculating Gaussian distribution parameters in the x direction and the y direction; wherein, mu_xMean value of x direction, σ_xIs the variance in the x direction, μ_yMean value of y direction, σ_yVariance in y-direction;

if it is notA positioning result is available where abs () is an absolute valued function, mu₀，σ₀Respectively, a mean threshold and a mean square error threshold.

8. The method according to claim 1, wherein determining a positioning result of the vehicle according to the evaluation result specifically comprises:

according to the formula x ═ μ_x*r，y＝σ_yObtaining a target first coordinate and a target second coordinate of the vehicle in a world coordinate system; wherein r is the resolution of the grid map;

according to the probability matrixObtaining a target yaw angle of the vehicle in a world coordinate system corresponding to yaw _ max;

calculating a target first coordinate and a target second coordinate of the vehicle in a world coordinate systemMean value mu of grid corresponding to coordinate and target second coordinate^gObtaining a target third coordinate of the vehicle in a world coordinate system; the first coordinate, the second coordinate and the third coordinate of the target form a target position, and the target position and the target yaw angle form a positioning result.

9. A lidar matched positioning apparatus, wherein the apparatus comprises:

the system comprises an acquisition unit, a processing unit and a processing unit, wherein the acquisition unit is used for acquiring current laser radar point cloud information; the laser radar point cloud information comprises ground points and points above the ground, and each point has a position coordinate;

the mapping unit is used for mapping the ground point and the points above the ground on a preset grid map according to the position coordinates;

a calculation unit for calculating a distribution of ground points and a distribution of points above the ground for each grid in the grid map;

the calculation unit is also used for calculating the probability of each point in each grid according to the distribution of the ground point of each grid and the distribution of the points above the ground;

the determining unit is used for determining a grid set in a preset search range by taking a first grid in a grid map as a center; the first grid is a grid where a positioning result predicted value is located;

the calculation unit is further configured to calculate matching probabilities corresponding to different yaw angles according to probabilities of points corresponding to the vehicle height when the vehicle is located in any one of the grids in the grid set;

the determining unit is further configured to determine a probability matrix according to matching probabilities corresponding to different yaw angles in each grid in the grid set;

the evaluation unit is used for evaluating the positioning result according to the probability matrix to generate an evaluation result;

the determining unit is further used for determining the positioning result of the vehicle according to the evaluation result.

10. An apparatus, comprising a memory for storing a program and a processor for performing the method of any of claims 1-8.

Technical Field

The invention relates to the field of automatic driving, in particular to a laser radar matching positioning method and device.

Background

In recent years, continuous breakthrough and development of key unmanned technologies greatly accelerate the landing and popularization of the unmanned technologies. The method mainly comprises the steps of high-precision differential GNSS (Global Navigation Satellite System) positioning, laser radar matching positioning, visual positioning and the like. The laser radar matching positioning is used as an important positioning means, and quick and stable positioning and accurate positioning effect evaluation are the most critical factors.

The laser radar matching positioning technology generally comprises two parts of calculating an optimal matching positioning result and evaluating the positioning result.

At present, most laser radar matching methods use a nonlinear optimization technology, and calculate an optimal matching result by taking the maximized laser matching probability as a target. When evaluating whether the laser matching result is available, the availability of the result is often evaluated based on comparing the matching probability of the optimal result with a fixed threshold, and if the matching probability is greater than the threshold, the matching is considered to be available, and if the matching probability is less than the threshold, the matching is considered to be failed.

The following problems exist in the calculation of the positioning result based on the optimized matching probability: the method only provides the probability value corresponding to the optimal solution, cannot provide probability distribution in a search space, loses a large amount of information, and can carry out more effective positioning result judgment by utilizing the information, even can carry out scene identification through the information.

The following problems exist when the matching probability is used as an evaluation index of matching: in different scenarios, the probability threshold for evaluating whether the matching result is valid is often different. For example, some scenes are mainly tree vegetation, some scenes are mainly buildings, and different scenes often have different matching probabilities; in the same scene, the matching probability changes along with the change of seasons. For example, the matching probability of a scene with multiple vegetations, fallen leaves in winter, a luxuriant summer and different seasons changes correspondingly. Therefore, the matching probability cannot be used as a stable and uniform means as a criterion for judging whether positioning is available.

Disclosure of Invention

The embodiment of the invention aims to provide a laser radar matching positioning method and device, and aims to solve the problems that in the prior art, the information loss amount is large and the positioning result cannot be effectively judged by taking the matching probability as an available standard for judging and positioning.

In order to solve the above problem, in a first aspect, the present invention provides a laser radar matching positioning method, where the method includes:

acquiring current laser radar point cloud information; the laser radar point cloud information comprises ground points and points above the ground, and each point has a position coordinate;

mapping the ground point and the points above the ground on a preset grid map according to the position coordinates;

calculating the distribution of ground points and the distribution of points above the ground of each grid in the grid map;

calculating the probability of each point in each grid according to the distribution of the ground point of each grid and the distribution of the points above the ground;

determining a grid set in a preset search range by taking a first grid in a grid map as a center; the first grid is a grid where a positioning result predicted value is located;

when the vehicle is located in any grid in the grid set, calculating matching probabilities corresponding to different yaw angles according to the probability of points corresponding to the height of the vehicle;

determining a probability matrix according to the matching probability corresponding to different yaw angles in each grid in the grid set;

evaluating a positioning result according to the probability matrix to generate an evaluation result;

and determining the positioning result of the vehicle according to the evaluation result.

In one possible implementation, before the obtaining the current lidar point cloud information, the method further includes:

acquiring original laser point cloud information;

and performing coordinate conversion on the original laser point cloud information to obtain the current laser point cloud information under a world coordinate system.

In a possible implementation manner, the calculating the probability of each point in each grid according to the distribution of the ground point and the distribution of the points above the ground for each grid specifically includes:

according to the formulaCalculating the probability of each point in each grid;

wherein (mu)^g，σ^g) Distribution of ground points (μ)^o，σ^o) For the distribution of points above the ground, the position of each grid on the grid map is (i, j), the position of each grid on the world coordinate system is (x, y), and (i, j) ═ x/r, y/r, r is the resolution of the grid map, p is the resolution of the grid map_z|i，jIs the probability, p, of a point in the (i, j) grid at a height z_z|i，j＝p_(x，y，z)。

In a possible implementation manner, when the vehicle is located in any one of the grids in the grid set, the calculating, according to the probability of the point corresponding to the vehicle height, the matching probabilities corresponding to different yaw angles to obtain a matching probability set of each grid specifically includes:

according to the formulaDifference of calculationMatching probability corresponding to the yaw angle;

wherein points is the points of the original point cloud_rawAfter coordinate change, point cloud under a world coordinate system; in the world coordinate system, the number of search grids in the x and y directions is m, the search range in the yaw direction is da, the search range of grids in the x direction is (r-m, r + m), the search range of grids in the y direction is (c-m, c + m), the search range of yaw is (yaw-da, yaw + da), and yaw is a yaw angle.

In a possible implementation manner, the determining a probability matrix according to matching probabilities corresponding to different yaw angles in each grid in the grid set specifically includes:

determining the maximum matching probability in each grid according to the matching probability corresponding to different yaw angles in each grid in the grid set;

the maximum match probability in each grid constitutes a probability matrix.

In a possible implementation manner, the evaluating the positioning result according to the probability matrix to generate an evaluation result specifically includes:

carrying out normalization processing on the probability matrix;

respectively calculating Gaussian distribution parameters in the x direction and the y direction according to the probability matrix after normalization processing;

and comparing the Gaussian distribution parameters in the x direction and the y direction with preset threshold values, and if each distribution parameter is smaller than the parameters in the preset threshold values, evaluating the result as a positioning result.

In one possible implementation, the formula is based onCarrying out normalization processing on the probability matrix; wherein, P_ijAs a probability matrix, P0_ijSum (p) is the sum of all elements of the probability matrix;

according to the formulaComputingGaussian distribution parameters in the x-direction and y-direction; wherein, mu_xMean value of x direction, σ_xIs the variance in the x direction, μ_yMean value of y direction, σ_yVariance in y-direction;

if it is notA positioning result is available where abs () is an absolute valued function, mu₀，σ₀Respectively, a mean threshold and a mean square error threshold.

In a possible implementation manner, the determining a positioning result of the vehicle according to the evaluation result specifically includes:

according to the formula x ═ μ_x*r，y＝σ_yObtaining a target first coordinate and a target second coordinate of the vehicle in a world coordinate system; wherein r is the resolution of the grid map;

according to the probability matrixObtaining a target yaw angle of the vehicle in a world coordinate system corresponding to yaw _ max;

calculating the mean value mu of grids corresponding to the target first coordinate and the target second coordinate according to the target first coordinate and the target second coordinate of the vehicle in the world coordinate system^gObtaining a target third coordinate of the vehicle in a world coordinate system; the first coordinate, the second coordinate and the third coordinate of the target form a target position, and the target position and the target yaw angle form a positioning result.

In a second aspect, the present invention provides a lidar matching positioning apparatus, comprising:

the system comprises an acquisition unit, a processing unit and a processing unit, wherein the acquisition unit is used for acquiring current laser radar point cloud information; the laser radar point cloud information comprises ground points and points above the ground, and each point has a position coordinate;

the mapping unit is used for mapping the ground point and the points above the ground on a preset grid map according to the position coordinates;

a calculation unit for calculating a distribution of ground points and a distribution of points above the ground for each grid in the grid map;

the calculation unit is also used for calculating the probability of each point in each grid according to the distribution of the ground point of each grid and the distribution of the points above the ground;

the determining unit is used for determining a grid set in a preset search range by taking a first grid in a grid map as a center; the first grid is a grid where a positioning result predicted value is located;

the calculation unit is further configured to calculate matching probabilities corresponding to different yaw angles according to probabilities of points corresponding to the vehicle height when the vehicle is located in any one of the grids in the grid set;

the determining unit is further configured to determine a probability matrix according to matching probabilities corresponding to different yaw angles in each grid in the grid set;

the evaluation unit is used for evaluating the positioning result according to the probability matrix to generate an evaluation result;

the determining unit is further used for determining the positioning result of the vehicle according to the evaluation result.

In a third aspect, the invention provides an apparatus comprising a memory for storing a program and a processor for performing the method of any of the first aspects.

In a fourth aspect, the present invention provides a computer program product comprising instructions which, when run on a computer, cause the computer to perform the method according to any one of the first aspect.

In a fifth aspect, the invention provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, performs the method of any of the first aspects.

According to the laser radar matching positioning method and device provided by the embodiment of the invention, the real-time point cloud is divided into the ground part and the part above the ground, so that the decoupling of six degrees of freedom of the vehicle position and attitude is realized, the searching speed is accelerated, and the obtained probability matrix can provide more referenceable information for the positioning result evaluation. And fitting the normalized probability matrix into Gaussian distribution of probability, judging the usability of the positioning result by judging the mean value and the variance of the Gaussian distribution, wherein the mean value represents the deviation of the matching result and the predicted value, and the index is irrelevant to the scene and only related to the precision of the predicted value. Because the probability matrix is normalized, the difference of scenes is eliminated, and therefore, the method can be well suitable for multiple scenes.

Drawings

Fig. 1 is a schematic flow chart of a laser radar matching positioning method according to an embodiment of the present invention;

FIG. 2 is a schematic diagram of a probability matrix according to a first embodiment of the present invention;

fig. 3 is a schematic structural diagram of a lidar matching positioning apparatus according to a second embodiment of the present invention.

Detailed Description

The present application will be described in further detail with reference to the following drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the relevant invention and not restrictive of the invention. It should be further noted that, for the convenience of description, only the portions related to the related invention are shown in the drawings.

It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict. The present application will be described in detail below with reference to the embodiments with reference to the attached drawings.

Fig. 1 is a schematic flow chart of a lidar matching and positioning method according to an embodiment of the present invention, where the method is applied to a terminal equipped with a lidar, such as an unmanned vehicle or a robot equipped with a lidar, and an execution subject of the method is a terminal, a server, or a processor with a computing function. The present application will be described by taking an example of applying the method to an unmanned Vehicle, and when the method is applied to an unmanned Vehicle, an execution subject of the method is an Automated Vehicle Control Unit (AVCU), that is, a central processing Unit of the unmanned Vehicle corresponds to a "brain" of the unmanned Vehicle. As shown in fig. 1, the present application includes the steps of:

step 101, acquiring current laser radar point cloud information; the lidar point cloud information includes ground points and points above the ground, each point having a location coordinate.

Specifically, the laser radar is provided with laser probes vertically arranged, the laser probes measure distance information by adopting a time of flight (ToF) technology, the laser radar rotates around a vertical shaft at a high speed in the operation process, and the laser probes measure environment information at a high frequency. The laser radar can acquire surrounding environment data of one circle by rotating one circle, and the surrounding environment information is provided in the form of discrete sparse three-dimensional space coordinates and is called as original laser point cloud information.

The surrounding environment information includes, but is not limited to, a scene mainly including tree vegetation and a scene mainly including buildings, and the present application is not limited thereto, and may be applied to various scenes.

In this step, the original laser point cloud information may be based on a laser coordinate system, or the point cloud information in the vehicle coordinate system may be obtained according to the installation positions of the vehicle and the laser radar, and after coordinate conversion, the current laser point cloud information in the world coordinate system, which is referred to as laser point cloud information for short, is obtained.

And 102, mapping the ground point and the point above the ground on a preset grid map according to the position coordinates.

Specifically, before the unmanned vehicle is not on the road formally, the test can be performed, and in the test stage, the laser point cloud map can be constructed, and the specific construction method can be that a 3D laser matching algorithm is adopted to perform point cloud interframe matching, and a laser point cloud map is constructed by a Simultaneous Localization and Mapping (SLAM) method based on a laser sensor, which is not limited in the present application.

And dividing the constructed laser point cloud map to obtain a grid map. The grid map is an XY grid plane which divides the surrounding environment information into a series of grids and can be regarded as discretized with a certain resolution r. Each grid represents its position in the grid map by an integer pair (i, j), where the position refers to the position in the upper left corner of the grid. Then the actual position, i.e. the grid map position (i, j) corresponding to the position (x, y) in the world coordinate system, is (x/r, y/r).

Step 103, calculating the distribution of the ground points and the distribution of the points above the ground of each grid in the grid map.

Specifically, the laser point cloud information is mapped to a grid map, and a plurality of points including ground points and points above the ground exist in one grid. Calculating the Gaussian distribution (mu) of the ground points according to the position coordinates of the ground points in the grid^g，σ^g) Wherein, mu^gIs the mean, σ, of the Gaussian distribution of the ground points^gIs the variance of the gaussian distribution of the ground points.

Calculating the Gaussian distribution (mu) of the ground points according to the position coordinates of the ground points in the grid^o，σ^o) Wherein, mu^oIs the mean, σ, of the Gaussian distribution of the ground points^oIs the variance of the gaussian distribution of the ground points.

And 104, calculating the probability of each point in each grid according to the distribution of the ground point of each grid and the distribution of the points above the ground.

Specifically, for a point falling on the (i, j) grid, the probability that the point corresponds to is the probability p that the z-coordinate of the point corresponds to_z|i，j。

According to the formulaCalculating the probability of each point in each grid;

wherein (mu)^g，σ^g) Distribution of ground points (μ)^o，σ^o) For the distribution of points above the ground, the position of each grid on the grid map is (i, j), the position of each grid on the world coordinate system is (x, y), and (i, j) ═ x/r, y/r, r is the resolution of the grid map, p is the resolution of the grid map_z|i，jIs the probability of a point in the (i, j) grid at a height z.

Thus, the position of one point (x,y, z) can be expressed as

And 105, determining a grid set in a preset search range by taking a first grid in the grid map as a center.

The laser radar matching positioning needs to determine the position coordinates (x, y, z) and the attitude (roll, pitch, yaw) of the vehicle in space, and the three degrees of freedom (x, y, yaw) are the most important degrees of freedom of the vehicle and are also the degrees of freedom which need to be calculated in an important manner. The roll angle roll and the pitch angle pitch may be calculated by using an Inertial Measurement Unit (IMU) mounted on the vehicle. When the vehicle (x, y) coordinates of the vehicle are obtained through calculation, the vehicle z is the ground Gaussian parameter mu of the grid corresponding to the (x, y)^g. Therefore, the following description will focus on the calculation of the three degrees of freedom (x, y, yaw).

The first grid is the grid where the predicted value is located. The preset search range may be m in the number of search grids in the x and y directions, da in the search range in the yaw direction, (r-m, r + m) in the x direction, (c-m, c + m) in the y direction, and (yaw-da, yaw + da) in the yaw direction. The grids within the above range and the first grid together constitute a grid set.

And 106, when the vehicle is positioned in any grid in the grid set, calculating the matching probability corresponding to different yaw angles according to the probability of the point corresponding to the height of the vehicle.

Specifically, when the vehicle is located in the grid (i, j) ═ xi/r, yi/r and yaw ═ yawi, it is possible to obtain the ground gauss parameter μ in the grid having the vehicle height zi of (i, j)^gThen the vehicle match probability P_i，j，yawiCan be expressed as:

wherein points is the points of the original point cloud_rawAfter coordinate change, in world coordinate systemThe lower point cloud.

And step 107, determining a probability matrix according to the matching probability corresponding to different yaw angles in each grid in the grid set.

Specifically, when the vehicle is located in the grid (i, j), the matching probabilities corresponding to different yaw angles are calculated, and the maximum probability of the matching probabilities corresponding to different yaw angles is taken as the matching probability P corresponding to the grid (i, j)_ijWherein yawi corresponding to the maximum probability is denoted as yaw _ max, i.e.

All P's within a preset search range_ijThe probability matrix for that match is formed. Referring to fig. 2, fig. 2 is a grid set, each grid in the grid set corresponds to a cell, and the maximum matching probability of each grid in the grid set is obtained within a preset search range, so as to form a probability matrix.

And 108, evaluating the positioning result according to the probability matrix to generate an evaluation result.

Wherein step 108 comprises the following:

firstly, carrying out normalization processing on a probability matrix; then, respectively calculating Gaussian distribution parameters in the x direction and the y direction according to the probability matrix after normalization processing; and finally, comparing the Gaussian distribution parameters in the x and y directions with a preset threshold, and if each distribution parameter is smaller than the parameter in the preset threshold, evaluating the result as a positioning result.

In particular, according to the formulaCarrying out normalization processing on the probability matrix; wherein, P_ijAs a probability matrix, P0_ijSum (p) is the sum of all elements of the probability matrix; the purpose of the normalization process is to eliminate the influence of differences in the environmental scene on the positioning result.

According to the formulaCalculating Gaussian distribution parameters in the x direction and the y direction; wherein, mu_xMean value of x direction, σ_xIs the variance in the x direction, μ_yMean value of y direction, σ_yThe variance in the y direction is shown, and m is the number of all elements;

if it is notA positioning result is available where abs () is an absolute valued function, mu₀，σ₀Respectively, a mean threshold and a mean square error threshold.

And step 109, determining the positioning result of the vehicle according to the evaluation result.

Specifically, the formula x ═ μ can be determined according to_x*r，y＝σ_yAnd obtaining a target first coordinate and a target second coordinate of the vehicle in a world coordinate system.

According to the probability matrixAnd obtaining the target yaw angle of the vehicle in the world coordinate system corresponding to yaw _ max.

Calculating the mean value mu of the grids corresponding to the target first coordinate and the target second coordinate according to the target first coordinate and the target second coordinate of the vehicle in the world coordinate system^gObtaining a target third coordinate of the vehicle in a world coordinate system; the first coordinate, the second coordinate and the third coordinate of the target form a target position, and the target position and the target yaw angle form a positioning result.

According to the laser radar matching positioning method provided by the embodiment of the invention, the real-time point cloud is divided into the ground part and the part above the ground, so that the decoupling of six degrees of freedom of the vehicle position and attitude is realized, the searching speed is accelerated, and the obtained probability matrix can provide more referenceable information for the positioning result evaluation. And fitting the normalized probability matrix into Gaussian distribution of probability, judging the usability of the positioning result by judging the mean value and the variance of the Gaussian distribution, wherein the mean value represents the deviation of the matching result and the predicted value, and the index is irrelevant to the scene and only related to the precision of the predicted value. Because the probability matrix is normalized, the difference of scenes is eliminated, and therefore, the method can be well suitable for multiple scenes.

Fig. 3 is a schematic structural view of a lidar matching positioning apparatus according to a second embodiment of the present invention, where the lidar matching positioning apparatus is applied to the lidar matching positioning method according to the first embodiment of the present invention, and as shown in fig. 3, the lidar matching positioning apparatus includes: an acquisition unit 301, a mapping unit 302, a calculation unit 303, a determination unit 304, and an evaluation unit 305.

The obtaining unit 301 is configured to obtain current lidar point cloud information; the laser radar point cloud information comprises ground points and points above the ground, and each point has a position coordinate.

The mapping unit 302 is configured to map the ground point and the point above the ground on a preset grid map according to the position coordinates.

The calculation unit 303 is configured to calculate a distribution of ground points and a distribution of points above the ground for each grid in the grid map.

The calculating unit 303 is further configured to calculate the probability of each point in each grid according to the distribution of the ground point and the distribution of the points above the ground of each grid.

The determining unit 304 is configured to determine a grid set within a preset search range by taking a first grid in the grid map as a center; and the first grid is the grid where the positioning result predicted value is located.

The calculation unit 303 is further configured to calculate matching probabilities corresponding to different yaw angles based on the probability of the point corresponding to the vehicle height when the vehicle is located in any one of the grid sets.

The determining unit 304 is further configured to determine a probability matrix according to the matching probability corresponding to different yaw angles in each grid in the grid set.

The evaluation unit 305 is configured to evaluate the positioning result according to the probability matrix, and generate an evaluation result.

The determining unit 304 is further configured to determine a positioning result of the vehicle according to the evaluation result.

The specific function of each unit corresponds to the method in the first embodiment, and is not described herein again.

By the laser radar matching positioning device provided by the embodiment of the invention, the real-time point cloud is divided into the ground part and the part above the ground, the decoupling of six degrees of freedom of the vehicle position and attitude is realized, the searching speed is accelerated, and the obtained probability matrix can provide more referenceable information for the positioning result evaluation. And fitting the normalized probability matrix into Gaussian distribution of probability, judging the usability of the positioning result by judging the mean value and the variance of the Gaussian distribution, wherein the mean value represents the deviation of the matching result and the predicted value, and the index is irrelevant to the scene and only related to the precision of the predicted value. Because the probability matrix is normalized, the difference of scenes is eliminated, and therefore, the method can be well suitable for multiple scenes.

The third embodiment of the invention provides equipment, which comprises a memory and a processor, wherein the memory is used for storing programs, and the memory can be connected with the processor through a bus. The memory may be a non-volatile memory such as a hard disk drive and a flash memory, in which a software program and a device driver are stored. The software program is capable of performing various functions of the above-described methods provided by embodiments of the present invention; the device drivers may be network and interface drivers. The processor is used for executing a software program, and the software program can realize the method provided by the first embodiment of the invention when being executed.

A fourth embodiment of the present invention provides a computer program product including instructions, which, when the computer program product runs on a computer, causes the computer to execute the method provided in the first embodiment of the present invention.

The fifth embodiment of the present invention provides a computer-readable storage medium, where a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the method provided in the first embodiment of the present invention is implemented.

Those of skill would further appreciate that the various illustrative components and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative components and steps have been described above generally in terms of their functionality in order to clearly illustrate this interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.

The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied in hardware, a software module executed by a processor, or a combination of the two. A software module may reside in Random Access Memory (RAM), memory, Read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.

The above embodiments are provided to further explain the objects, technical solutions and advantages of the present invention in detail, it should be understood that the above embodiments are merely exemplary embodiments of the present invention and are not intended to limit the scope of the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.