CN110515055B

CN110515055B - Method for optimizing laser radar positioning by utilizing radius search

Info

Publication number: CN110515055B
Application number: CN201911015902.1A
Authority: CN
Inventors: 杨欢; 陈诚; 张旸
Original assignee: AutoCore Intelligence Technology Nanjing Co Ltd
Current assignee: AutoCore Intelligence Technology Nanjing Co Ltd
Priority date: 2019-10-24
Filing date: 2019-10-24
Publication date: 2020-12-04
Anticipated expiration: 2039-10-24
Also published as: CN110515055A

Abstract

The invention belongs to the technical field of road network navigation, and provides a method for optimizing laser radar positioning by using radius search. According to the invention, for the position of the vehicle to be positioned, local road distribution and vehicle reachable area information are inquired in the database through the GPS rough coordinate, and the unreachable poses of the vehicle are filtered when the predicted pose is generated, so that the number of the predicted poses can be greatly reduced, and the matching precision and speed are improved.

Description

Method for optimizing laser radar positioning by utilizing radius search

Technical Field

The invention relates to radio navigation in the field of automatic driving, in particular to a method for optimizing laser radar positioning by utilizing radius search.

Background

Vehicle localization is a very important link in autonomous driving systems for determining the orientation and position of a car in a high-precision map. There are many ways for vehicle positioning, among which the use of lidar point cloud for matching is a common way, NDT-Normal Distribution Transform (NDT), hereinafter referred to as NDT, is a common lidar point cloud matching algorithm, the principle of which is to convert target point cloud and high-precision point cloud map information generated by the lidar into Normal distribution of multidimensional variables, and by calculating distribution probability, if the matching degree of the two is higher, it is considered that the scanning point cloud and the map are matched, NDT has the advantage of high efficiency and off-line calculation, but due to the limitation of the algorithm, for the same lidar point cloud, there will be multiple possible matching position points in the high-precision map, in order to improve the positioning accuracy, it is necessary to transmit the position of the lidar point cloud roughly as input to NDT, and in the current common way, this position is given by GPS positioning, however, due to the problems of GPS accuracy and drift, the estimated attitude given each time is different, so that the required matching accuracy may not be achieved for a long time, and even the vehicle positioning is lost.

Disclosure of Invention

The purpose of the invention is as follows: the invention aims to provide an optimization method, which improves ndt accuracy of pose prediction, thereby ensuring ndt positioning accuracy and stability.

The technical scheme is as follows: in order to solve the technical problem, the method for optimizing the laser radar positioning by utilizing the radius search provided by the invention comprises the following steps of:

step 1, initializing and establishing a plurality of search threads.

Step 2, inquiring local road distribution and vehicle reachable area information in a reachable area and a road database through the rough coordinates acquired by the GPS;

step 3, filtering the inaccessible pose of the vehicle by combining the vehicle state, searching an optimal pose point generating function F in the database, generating a pose point P by the F during the process operation, and generating a predicted pose;

step 4, ndt matching is carried out on the laser radar point cloud and the existing point cloud map by using the P as an initial pose;

and 5, judging that the searching thread is suspended after the set precision S is reached, and returning to the step 3 to continue running if the set progress S is not reached.

Specifically, in step 2, possible road information and a possible travel area around the vehicle body are obtained by using a map provider, Baidu height, or the like or a self-built map service inquiry according to the basic position of the vehicle body acquired by the vehicle-mounted GPS. And after the road information is acquired, acquiring the orientation information of the vehicle body according to the vehicle-mounted IMU or other sensors, and screening out the road and the area where the vehicle is most likely to run/stop in the current state.

The pseudo code of the adaptive algorithm function F after the query result in the step 3 is generated is as follows:

{

…

results = QueryByGPS ()// query of the current possible road list from the vehicle GPS

foreach (lane in result)// cycle of the current road list

{

A = Euler (lane)// acquisition of road orientation Euler angle

B = euler (vehicle)// obtaining the euler angle of the vehicle

if (abs (A-B) < N)// if the deviation of the road orientation angle and the vehicle orientation angle is less than N

Add/Add the current road to an optional list

}

…

}。

Has the advantages that: the traditional radius search algorithm randomly generates estimated poses according to GPS coordinates, and the speed and precision required by ndt matching are difficult to meet.

In addition to the technical problems addressed by the present invention, the technical features constituting the technical solutions, and the advantages brought by the technical features of the technical solutions described above. In order to make the objects, technical solutions and advantages of the present invention clearer, other technical problems, other technical features included in the technical solutions and advantages brought by the technical features which can be solved by the present invention will be described more clearly and completely with reference to the accompanying drawings in the embodiments of the present invention.

Drawings

FIG. 1 is a flow chart of a method of an embodiment of the present invention;

FIG. 2 is an example of a conventional radius search algorithm;

fig. 3 is the result of the optimization using the algorithm of the present invention.

Detailed Description

Examples

As shown in fig. 1, in the method for optimizing lidar positioning by using radius search according to the embodiment, a multithread programming scheme is used, in another thread, with an input GPS coordinate as a center, within a range of a radius R and a height H, N pose parameters are generated by using a method F, and matching is continuously tried until a set matching accuracy S is met. If the pose parameters are only generated randomly, although the correct result can be screened out in a certain time theoretically, a large amount of waste in time can be caused due to the existence of randomness, so the core of the invention is to inquire local road distribution and vehicle reachable area information in a database through a GPS rough coordinate aiming at the position of the vehicle needing to be positioned, filter the inaccessible pose of the vehicle when generating the predicted pose, and greatly reduce the number of the predicted pose, thereby improving the matching precision and speed.

Fig. 2 is an example of a conventional radius search algorithm, in which black arrows are estimated poses generated by the algorithm, and the estimated poses are visible randomly distributed in radii and the like, and a large number of poses exist in unreachable areas, that is, invalid areas of the vehicle, but are still transmitted to ndt for matching operation, which wastes operation time and may also cause wrong positioning results.

FIG. 3 shows the result of the algorithm optimization of the present invention, which can limit the estimated pose in the current driving road of the vehicle according to the development road and the reachable area database, thus greatly improving the matching speed and precision.

The execution mode is as follows: establishing a plurality of search threads, searching an optimal pose point generating function F in a database according to the parameters R and H and the current GPS equipment operation environment, generating a pose point P by the F when the threads are operated, matching the point cloud by ndt by using the P as an initial pose, pausing the search threads after the set precision S is reached, and otherwise, continuing to operate.

Aiming at the conditions of insufficient precision and positioning drift of the existing vehicle-mounted GPS equipment, the method can greatly shorten the positioning initialization time of the laser radar by utilizing the existing road information database and the adaptive algorithm, can effectively improve the positioning precision of the laser radar, avoids generating wrong positioning information, and can achieve automatic execution without manual participation.

The average positioning accuracy error of the traditional radius search algorithm is 0.54 m, the average positioning success time is 24.6 seconds, and the average error positioning time is only 2.44 hours. By adopting the algorithm of the embodiment, the average positioning accuracy error reaches 0.36 m, the average positioning success time is shortened to 0.65 s, and the average error positioning time reaches 17.331 h.

The invention has essential difference from the scheme of using GPS as a positioning source in the general automatic driving scheme, in the invention, GPS signals are only used as optimization, so the requirements on the precision and the stability of the GPS signals are very low, and the prices of GPS receiving devices with different precisions in the market have difference of hundreds of times.

Claims

1. A method for optimizing laser radar positioning by using radius search is used for vehicle positioning of an automatic driving system, rough position information acquired by a low-cost low-precision GPS receiving device is used, and accurate positions of vehicles are obtained by inquiring an accessible area in a certain range and assisting an NDT algorithm through laser point cloud matching;

the optimization is realized by utilizing the radius search, a multithreading programming scheme is utilized, a plurality of pose parameters are generated by utilizing a method F within the range of the radius R and the height H by taking an input GPS coordinate as a center, and the search and the matching are finished until the set matching precision is met;

the method specifically comprises the following steps:

step 1, initializing and establishing a search thread;

step 2, inquiring local road distribution and vehicle reachable area information in a reachable area and a road database through the rough coordinates acquired by the GPS; the method specifically comprises the steps of inquiring a map provider to obtain a possible road list and a possible driving area around a vehicle body according to a basic position of the vehicle body obtained by a vehicle-mounted GPS;

step 3, acquiring vehicle body orientation information according to the vehicle-mounted IMU, and screening out roads and areas where the vehicle is most likely to run/stop in the current state from the possible road list and the possible running areas;

filtering inaccessible poses of vehicles by combining vehicle body orientation information, searching optimal pose points in a database, adopting a self-adaptive algorithm function F, generating pose points P by the F during process operation, and generating predicted poses;

the self-adaptive algorithm F function is to circulate the current road list according to the current possible road list inquired by the vehicle GPS, obtain the Euler angle of the road orientation and the Euler angle of the vehicle orientation, and add the current road into the optional list if the deviation between the road orientation angle and the vehicle orientation angle is less than N;

step 4, NDT matching is carried out on the laser radar point cloud and an existing point cloud map by using the P as an initial pose;

and 5, finishing the searching thread after judging that the required positioning precision is achieved, and returning to the step 3 to continue running if the required positioning precision is not achieved.