Method and apparatus for positioning vehicle

• US 10,613,227 B2
• Filed: 01/29/2018
• Issued: 04/07/2020
• Est. Priority Date: 04/20/2017
• Status: Active Grant

• Current Assignee: Baidu Online Network Technology (Beijing) Co., Ltd (Baidu Incorporated)
• Original Assignee: Baidu Online Network Technology (Beijing) Co., Ltd (Baidu Incorporated)
• Inventors: Wan, Guowei, Wang, Hao, Song, Shiyu, Xu, Baoqiang
• Primary Examiner(s): Dunn, Alex C
• Application Number:
  US15/882,131
• Publication Number:
  US 20180306922A1
• Time in Patent Office: 799 Days
• CPC Class Codes: G01S 17/42 Simultaneous measurement of distance and other co-ordinates indirect measurement G01S17/46
• CPC Class Codes: G01S 17/86 Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
• CPC Class Codes: G01S 17/89 for mapping or imaging
• CPC Class Codes: G01S 19/42 Determining position
• CPC Class Codes: G01S 7/4026 Antenna boresight
• CPC Class Codes: G01S 7/4808 Evaluating distance, position or velocity data
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30

Abstract

The present application discloses a method and apparatus for positioning a vehicle. An implementation of the method comprises: obtaining laser point cloud data of a laser point cloud collected by a laser radar on a vehicle, and obtaining an initial pose of a center point of the laser radar; calculating a matching probability between projection data corresponding to each sampling pose and map data of a reflected value map respectively; and calculating an optimal pose based on the matching probability between the projection data corresponding to the each sampling pose and the map data of the reflected value map, and determining a position of the vehicle based on the optimal pose.

Claims

1. A method for positioning a vehicle, the method comprising:

obtaining laser point cloud data of a laser point cloud collected by a laser radar on a vehicle, and obtaining an initial pose of a center point of the laser radar, the laser point cloud data comprising coordinates of laser points under a coordinate of the laser radar, and the initial pose comprising an initial plane position and an initial yaw angle of the center point of the laser radar in a world coordinate system;

calculating a matching probability between projection data corresponding to each sampling pose and map data of a reflected value map respectively, the sampling pose comprising a sampling plane position in a preset range of the initial plane position, and a sampling yaw angle in a preset range of the initial yaw angle, the projection data comprising a mean value and a variance of reflected values or heights of laser points in the collected laser point cloud based on projecting the sampling pose into a cell of the reflected value map and the map data comprising a preset mean value and a preset variance of the reflected values or heights corresponding to the cell; and

calculating an optimal pose based on the matching probability between the projection data corresponding to the each sampling pose and the map data of the reflected value map, and determining a position of the vehicle based on the optimal pose, the optimal pose comprising an optimal plane position and an optimal yaw angle of the center point of the laser radar in the world coordinate system,

wherein the method is performed by one or more processors.

2. The method according to claim 1, wherein the calculating a matching probability between projection data corresponding to each sampling pose and map data of a reflected value map respectively comprises:

calculating the matching probability P(z|x, y, yaw) between the projection data corresponding to the each sampling pose and the map data of the reflected value map respectively by adopting the following formula;

3. The method according to claim 2, wherein the calculating an optimal pose based on the matching probability between the projection data corresponding to the each sampling pose and the map data of the reflected value map comprises:

regarding a corresponding sampling pose with the maximum matching probability as the optimal pose.

4. The method according to claim 2, wherein the calculating an optimal pose based on the matching probability between the projection data corresponding to the each sampling pose and the map data of the reflected value map comprises:

calculating an a posteriori matching probability p(x, y, yaw) corresponding to the each sampling pose respectively by adopting the following formula;

P(x,y,yaw)=μ

·

P(z|x,y,yaw)·

p(x,y,yaw)

wherein, P(z|x, y, yaw) represents the matching probability between the projection data of a sampling pose and the map data of the reflected value map, p(x, y, yaw) represents a predicted value of the a posteriori matching probability obtained based on a calculation result of the matching probability of a historical laser point cloud, and μ

is a normalization coefficient; and

calculating the optimal pose based on the a posteriori matching probability corresponding to the each sampling pose.

5. The method according to claim 4, wherein the calculating the optimal pose based on the a posteriori matching probability corresponding to the each sampling pose comprises:

calculating a multiplication product of the a posteriori matching probability corresponding to the each sampling pose and a value of a sampling plane position on an x axis in the sampling pose respectively to obtain a plurality of first weighted terms;

dividing a sum of the plurality of first weighted terms by a sum of the a posteriori matching probabilities corresponding to all sampling poses to obtain a value of an optimal plane position on the x axis;

calculating a multiplication product of the a posteriori matching probability corresponding to the each sampling pose and a value of the sampling plane position on a y axis in the sampling pose respectively to obtain a plurality of second weighted terms;

dividing the sum of the plurality of second weighted terms by the sum of the a posteriori matching probabilities corresponding to all sampling poses to obtain a value of the optimal plane position on the y axis;

calculating a multiplication product of the a posteriori matching probability corresponding to the each sampling pose and a sampling yaw angle in the sampling pose respectively to obtain a plurality of third weighted terms; and

dividing the sum of the plurality of third weighted terms by the sum of the a posteriori matching probabilities corresponding to all sampling poses to obtain an optimal yaw angle.

6. An apparatus for positioning a vehicle, the apparatus comprising:

at least one processor; and

a memory storing instructions, which when executed by the at least one processor, cause the at least one processor to perform operations, the operations comprising;

obtaining laser point cloud data of a laser point cloud collected by a laser radar on a vehicle, and obtaining an initial pose of a center point of the laser radar, the laser point cloud data comprising coordinates of laser points under a coordinate of the laser radar, and the initial pose comprising an initial plane position and an initial yaw angle of the center point of the laser radar in a world coordinate system;

calculating a matching probability between projection data corresponding to each sampling pose and map data of a reflected value map respectively, the sampling pose comprising a sampling plane position in a preset range of the initial plane position, and a sampling yaw angle in a preset range of the initial yaw angle, the projection data comprising a mean value and a variance of reflected values or heights of laser points in the collected laser point cloud based on projecting the sampling pose into a cell of the reflected value map and the map data comprising a preset mean value and a preset variance of the reflected values or heights corresponding to the cell; and

calculating an optimal pose based on the matching probability between the projection data corresponding to the each sampling pose and the map data of the reflected value map, and determining a position of the vehicle based on the optimal pose.

7. The apparatus according to claim 6, wherein the calculating a matching probability between projection data corresponding to each sampling pose and map data of a reflected value map respectively comprises:

calculating the matching probability P(z|x, y, yaw) between the projection data corresponding to the each sampling pose and the map data of the reflected value map respectively by adopting the following formula;

8. The apparatus according to claim 7, wherein the operations further comprises:

regarding a corresponding sampling pose with the maximum matching probability as an optimal pose;

calculating a multiplication product of the a posteriori matching probability corresponding to the each sampling pose and a value of a sampling plane position on an x axis in the sampling pose respectively to obtain a plurality of first weighted terms;

dividing a sum of the plurality of first weighted terms by a sum of the a posteriori matching probabilities corresponding to all sampling poses to obtain a value of an optimal plane position on the x axis;

calculating a multiplication product of the a posteriori matching probability corresponding to the each sampling pose and a value of the sampling plane position on a y axis in the sampling pose respectively to obtain a plurality of second weighted terms;

dividing the sum of the plurality of second weighted terms by the sum of the a posteriori matching probabilities corresponding to all sampling poses to obtain a value of the optimal plane position on the y axis;

calculating a multiplication product of the a posteriori matching probability corresponding to the each sampling pose and a sampling yaw angle in the sampling pose respectively to obtain a plurality of third weighted terms; and

dividing the sum of the plurality of third weighted terms by the sum of the a posteriori matching probabilities corresponding to all sampling poses to obtain an optimal yaw angle.

9. A non-transitory computer storage medium storing a computer program, which when executed by one or more processors, causes the one or more processors to perform operations, the operations comprising:

obtaining laser point cloud data of a laser point cloud collected by a laser radar on a vehicle, and obtaining an initial pose of a center point of the laser radar, the laser point cloud data comprising coordinates of laser points under a coordinate of the laser radar, and the initial pose comprising an initial plane position and an initial yaw angle of the center point of the laser radar in a world coordinate system;

calculating a matching probability between projection data corresponding to each sampling pose and map data of a reflected value map respectively, the sampling pose comprising a sampling plane position in a preset range of the initial plane position, and a sampling yaw angle in a preset range of the initial yaw angle, the projection data comprising a mean value and a variance of reflected values or heights of laser points in the collected laser point cloud based on projecting the sampling pose into a cell of the reflected value map and the map data comprising a preset mean value and a preset variance of the reflected values or heights corresponding to the cell; and

calculating an optimal pose based on the matching probability between the projection data corresponding to the each sampling pose and the map data of the reflected value map, and determining a position of the vehicle based on the optimal pose, the optimal pose comprising an optimal plane position and an optimal yaw angle of the center point of the laser radar in the world coordinate system.