A method for vehicle global positioning based on laser radar
By constructing the registration calculation of reference point cloud map and normal distribution transformation algorithm, the error problem of initial position determination when the GPS signal is weak is solved, and the accuracy and efficiency improvement of the vehicle's global positioning is achieved.
Patent Information
- Application Number
- CN202411027292.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-30
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2044-07-30
AI Technical Summary
In scenarios with weak GPS signals, when lidar positioning assists global positioning, the initial positioning needs to be manually determined. The accuracy depends on the driver's familiarity, resulting in initial positioning error and experience dependence, which may lead to vehicle positioning failure.
A reference point cloud map is constructed to record the vehicle's optimal walking route and route points, and a normal distribution transformation algorithm is used to perform registration calculations to determine the initial position, and the vehicle's initial position posture is determined through route points and yaw angle enumeration.
It provides a method to accurately and quickly determine the initial position required for vehicle positioning, avoid artificial errors and experience dependence, improve the accuracy and efficiency of vehicle positioning, and ensure that the NDT error correction algorithm can work normally.
Smart Images

Figure CN118549939B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of vehicle positioning, and in particular to a method for performing global vehicle positioning based on laser radar. Background Art
[0002] LiDAR is widely used in vehicle autonomous driving technology. LiDAR has two main uses in autonomous vehicles: one is to sense the surrounding environment and identify surrounding objects that affect driving; the other is for positioning.
[0003] The main positioning system of an autonomous vehicle is usually a GPS (Global Positioning System) device with RTK (Real-Time Kinematic). RTK positioning technology is a real-time dynamic differential positioning technology based on high-precision carrier phase observations. The advantage is that the positioning accuracy can reach the centimeter level; the disadvantage of GPS devices with RTK is that satellite coverage is required, and satellite coverage is less in places such as indoors, bridges and tunnels. At this time, we need LiDAR positioning as an auxiliary positioning device to solve the shortcomings of GPS positioning.
[0004] The common algorithms for LiDAR positioning are usually called SLAM (Simultaneous Localization and Mapping) algorithms. Common SLAM algorithms mainly include NDT (Normal Distributions Transform) algorithm and ICP (Iterative Closest Point) algorithm, among which NDT algorithm is widely used. Usually, the process of NDT algorithm is to let the vehicle drive once, collect the point cloud data of LiDAR, use NDT algorithm to make point cloud map, and then perform global positioning, which can be used to let the vehicle obtain positioning and run the autonomous driving algorithm.
[0005] Global positioning refers to the process of giving the initial position and posture of the autonomous driving vehicle through multi-sensor fusion (such as GPS), manual specification, algorithm determination, etc. under the condition that the initial posture of the autonomous driving vehicle is uncertain. The NDT positioning algorithm is an iterative solution process. If the initial posture is not accurate, the NDT iteration will diverge and cause positioning failure. Therefore, global positioning is required at the beginning of the positioning algorithm to provide an initial posture for the positioning algorithm.
[0006] When using lidar positioning to assist in global positioning in scenarios with weak GPS signals, it is necessary to manually determine the initial pose used in the NDT iterative calculation. The accuracy of the initial pose depends on the driver's familiarity with the local environment. If the initial pose accuracy is too low, the NDT error correction algorithm cannot correct it, resulting in vehicle positioning failure, and thus failure to automatically plan the path and fail to reach the destination.
[0007] Therefore, there is an urgent need for a method for global vehicle positioning based on lidar, which can accurately provide the initial pose used in NDT iterative calculation and improve the accuracy of vehicle positioning. Summary of the invention
[0008] In order to solve the above technical problems, the present invention provides a method for global positioning of a vehicle based on a laser radar, comprising the following steps:
[0009] Constructing a reference point cloud map, using a laser radar to collect environmental information along the optimal walking route of the vehicle in the environment, constructing the reference point cloud map using a normal distribution transformation algorithm, and recording the optimal walking route of the vehicle and the route points included in the optimal walking route in the reference point cloud map, wherein the route points include the position and posture of the vehicle;
[0010] The point cloud collected by the laser radar on the vehicle is used as the source point cloud, and the source point cloud is a point cloud point sequence, and the point cloud point includes a position and a reflection intensity;
[0011] Taking the route points as the initial pose candidate points, performing registration calculation using the normal distribution transformation algorithm according to the source point cloud and the reference point cloud map, and determining the initial pose required for vehicle positioning calculation;
[0012] The vehicle is globally positioned according to the initial posture required for the vehicle positioning calculation.
[0013] Preferably, the method of taking the route points as the initial pose candidate points, performing registration calculation based on the source point cloud and the reference point cloud map using a normal distribution transformation algorithm, and determining the initial pose required for vehicle positioning calculation comprises:
[0014] Enumerate each route point in the optimal walking route;
[0015] Starting from the first route point, taking the first route point as the initial pose candidate point, performing registration calculation based on the source point cloud and the reference point cloud map using the normal distribution transformation algorithm, and obtaining the registration probability of the source point cloud; the registration probability is the probability that the source point cloud falls into the grid of the reference point cloud map;
[0016] Determine whether the registration probability of the source point cloud reaches the preset probability threshold:
[0017] If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated and the first route point is used as the initial pose required for vehicle positioning calculation;
[0018] If the registration probability of the source point cloud does not reach the preset probability threshold, the registration probability of the source point cloud is calculated one by one when each route point is used as a candidate point for the initial pose; when the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point corresponding to the time when the registration probability of the source point cloud reaches the preset probability threshold is the initial pose required for vehicle positioning calculation.
[0019] Preferably, the posture includes a yaw angle.
[0020] Preferably, if all route points are used as initial pose candidate points and the registration probability of the source point cloud does not reach the preset probability threshold:
[0021] The yaw angle of each route point is enumerated based on the principle of dividing the circumference into N equal parts to obtain the yaw angle enumeration route points, where N is a positive integer greater than 1;
[0022] Starting from the first yaw angle enumeration route point in the counterclockwise direction of the first route point, the first yaw angle enumeration route point in the counterclockwise direction of the first route point is used as the initial pose candidate point, and the normal distribution transformation algorithm is used to perform registration calculation according to the source point cloud and the reference point cloud map to obtain the registration probability of the source point cloud, and determine whether the registration probability of the source point cloud reaches a preset probability threshold:
[0023] If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated with the first yaw angle in the counterclockwise direction of the first route point as the initial pose required for vehicle positioning calculation;
[0024] If the registration probability of the source point cloud does not reach the preset probability threshold, first traverse the yaw angle enumeration route points of the first route point, then traverse the yaw angle enumeration route points of the second route point, and so on, and calculate the registration probability of the source point cloud when each yaw angle enumeration route point of each route point is used as the initial pose candidate point one by one;
[0025] When the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated according to the yaw angle of the route point corresponding to the registration probability of the source point cloud reaching the preset probability threshold as the initial pose required for vehicle positioning calculation.
[0026] Preferably, if the registration probability of the source point cloud does not reach a preset probability threshold when each yaw angle enumeration route point of each route point is used as an initial pose candidate point:
[0027] On the left and right sides of the optimal walking route, enumerate the route points by enumerating the yaw angles of the first adjacent route points whose distance to each route point is A;
[0028] Starting from the first yaw angle enumeration route point in the counterclockwise direction of the first first adjacent route point, the first yaw angle enumeration route point in the counterclockwise direction of the first first adjacent route point is used as the initial pose candidate point, and the normal distribution transformation algorithm is used to perform registration calculation according to the source point cloud and the reference point cloud map to obtain the registration probability of the source point cloud, and determine whether the registration probability of the source point cloud reaches a preset probability threshold:
[0029] If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated with the first yaw angle in the counterclockwise direction of the first adjacent route point as the initial pose required for vehicle positioning calculation;
[0030] If the registration probability of the source point cloud does not reach the preset probability threshold, first traverse the yaw angle enumeration route points of the first first adjacent route point, then traverse the yaw angle enumeration route points of the first first adjacent route point on the opposite side, and then traverse the yaw angle enumeration route points of the second first adjacent route point on the same side, and so on, and calculate the registration probability of the source point cloud when the yaw angle enumeration route points of the first adjacent route points on both sides of the optimal walking route are used as the initial pose candidate points one by one;
[0031] When the registration probability of the source point cloud reaches a preset probability threshold, the calculation is terminated, and the route point is enumerated as the initial posture required for vehicle positioning calculation based on the yaw angle of the first adjacent route point corresponding to the registration probability of the source point cloud reaching the preset probability threshold;
[0032] If the registration probability of the source point cloud does not reach the preset probability threshold when all the yaw angle enumeration route points of all the first adjacent route points are used as the initial pose candidate points, then on the side of the first adjacent route point far from the optimal walking route, the yaw angle enumeration route points of the second adjacent route points with a distance A from each first adjacent route point are enumerated;
[0033] The registration probability of the source point cloud is calculated one by one when the yaw angle of the second adjacent route point is enumerated as the initial pose candidate point;
[0034] When the registration probability of the source point cloud reaches a preset probability threshold, the calculation is terminated, and the yaw angle enumeration route point of the second adjacent route point corresponding to the registration probability of the source point cloud reaches the preset probability threshold is used as the initial pose required for vehicle positioning calculation; if the yaw angle enumeration route point of the second adjacent route point is used as the initial pose candidate point, the registration probability of the source point cloud does not reach the preset probability threshold, the yaw angle enumeration route point of the third adjacent route point with a distance A from each second adjacent route point is enumerated, and so on. When the yaw angle enumeration route points of all adjacent route points are traversed as initial pose candidate points, the normal distribution transformation algorithm is used to calculate the registration probability of the source point cloud to determine the initial pose required for vehicle positioning calculation.
[0035] Preferably, the method of taking the first route point as the initial pose candidate point, performing registration calculation according to the source point cloud and the reference point cloud map using a normal distribution transformation algorithm, and obtaining the registration probability of the source point cloud further includes: performing sparse processing on each route point, and the sparse processing is:
[0036] Taking the first route point as the starting point, along the optimal walking route, a route point is selected every distance B as the sparse route point.
[0037] Preferably, the process of taking route points as initial pose candidate points and using a normal distribution transformation algorithm to perform registration calculation based on the source point cloud and the reference point cloud map is as follows: taking the sparse route points as initial pose candidate points and using a normal distribution transformation algorithm to perform registration calculation based on the source point cloud.
[0038] Preferably, the N equal parts are divided into 20 equal parts.
[0039] Preferably, B is 3 meters.
[0040] The embodiments of the present invention have the following technical effects:
[0041] 1. The present invention provides a method for global positioning of a vehicle based on laser radar: construct a reference point cloud map, and record the optimal travel route of the vehicle and the route points included in the optimal travel route in the reference point cloud map; obtain the position and posture of the vehicle to be positioned as the source point cloud through the laser radar; use the route point as the initial posture, and use the normal distribution transformation algorithm to perform registration calculations based on the source point cloud and the reference point cloud map to determine the initial posture required for the vehicle positioning calculation, and then locate the vehicle. Determining the initial posture from the route points included in the optimal travel route not only provides a method for accurately and quickly determining the initial posture required for vehicle positioning, but also overcomes the errors and experience dependence when manually determining the initial posture, and can avoid the problem of being unable to complete vehicle positioning due to exceeding the capabilities of the NDT error correction algorithm.
[0042] 2. When determining the initial posture required for vehicle positioning from the route points, from the perspective of probability priority of the vehicle's walking route, the initial posture is determined from the route points included in the optimal walking route first, and then the scope is expanded to enumerate the route points by the yaw angles of the route points. Finally, the scope is expanded to enumerate the route points by the yaw angles of all adjacent route points on the left and right sides of the optimal walking route. Under the premise of ensuring the accuracy of the initial posture, the efficiency of determining the initial posture can be improved. BRIEF DESCRIPTION OF THE DRAWINGS
[0043] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0044] Figure 1 is a flow chart of a method for global positioning of a vehicle based on a laser radar provided by an embodiment of the present invention;
[0045] Figure 2 is a schematic diagram of enumeration of route points in an embodiment of the present invention;
[0046] Figure 3 is a schematic diagram of enumerating route points by yaw angles according to an embodiment of the present invention;
[0047] Figure 4 Schematic diagram of enumerating route points by yaw angles of adjacent route points in an embodiment of the present invention. DETAILED DESCRIPTION
[0048] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be described clearly and completely below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention.
[0049] The initial position and posture of the NDT positioning algorithm is crucial. In view of the large error of manually determining the initial position and the strong reliance on manual experience, the present invention provides a method for global positioning of a vehicle based on a laser radar, including the following steps, see Figure 1 :
[0050] Step S1: construct a reference point cloud map, use a laser radar to collect environmental information along the optimal walking route of the vehicle in the environment, use a normal distribution transformation algorithm to construct a reference point cloud map, and record the optimal walking route of the vehicle and the route points included in the optimal walking route in the reference point cloud map, and the route points include the position and posture of the vehicle;
[0051] The implementation steps of the NDT positioning algorithm to build a map are to collect lidar point cloud data, use the NDT algorithm to solve it, use the first frame of point cloud as the reference point cloud map, and start from the second frame of point cloud. Each frame of point cloud and the reference map use the NDT algorithm to perform point cloud registration, calculate the relative position and posture, and if the error with the last recorded position is greater than a certain threshold, add the current point cloud to the reference point cloud map, and use the current position as the last recorded position, and so on, until all point clouds are processed.
[0052] Exemplarily, the process of constructing a reference point cloud map in the embodiment of the present application is:
[0053] 1) Use the first frame point cloud as the reference map and set the current map pose to (0,0,0,0,0,0) (the first three numbers are the position (x, y, z), and the last three numbers are the attitude (yaw, pitch, roll). The attitude is the angle of rotation of the vehicle along the three coordinate axes. Yaw is the yaw angle, which indicates the rotation angle around the z-axis; pitch is the pitch angle, which indicates the rotation angle around the y-axis; roll is the roll angle, which indicates the rotation angle around the x-axis).
[0054] 2) Input a frame of point cloud as input point cloud.
[0055] 3) Use the NDT algorithm to calculate the relative position and posture between the input point cloud and the reference map.
[0056] 4) Compare the distance difference between the relative pose obtained in the previous step and the current map pose. If the distance is greater than 1 meter, translate and rotate the current point cloud using the relative pose obtained in the previous step, add it to the reference map, and modify the current map pose to the relative pose obtained in the previous step.
[0057] 5) If there is still point cloud data, return to 2); otherwise, end the construction.
[0058] The optimal route of the vehicle and the route points included in the optimal route are recorded in the reference point cloud map. The route points include the position and posture of the vehicle.
[0059] This application improves the construction process of the reference point cloud map. During the construction process of the reference point cloud map, the optimal route of the vehicle and the route points included in the optimal route are recorded. The optimal route is the route most frequently taken by users during use, such as the marked route for internal transportation in the factory. The optimal route has been planned in advance according to environmental requirements, route requirements, etc. This application only needs to obtain the planned optimal route, and does not limit the planning method of the optimal route. The reason for this is that the optimal route is the point set with the highest probability of appearing during the actual operation of the vehicle; therefore, estimating the point set with the highest probability and its surroundings, that is, determining the initial posture in the route point, can increase the probability of hitting and reduce calculation time.
[0060] Step S2: The point cloud collected by the laser radar on the vehicle is used as the source point cloud, and the source point cloud is a point cloud point sequence, and the point cloud point includes a position and a reflection intensity.
[0061] Step S3: Taking the route points as the initial pose candidate points, the normal distribution transformation algorithm is used to perform registration calculation according to the source point cloud and the reference point cloud map to determine the initial pose required for the vehicle positioning calculation.
[0062] Determining the initial posture from the route points included in the optimal walking route not only realizes a method to accurately and quickly determine the initial posture required for vehicle positioning, but also overcomes the errors and experience dependence when manually determining the initial posture, and can avoid the problem of being unable to complete vehicle positioning due to exceeding the capabilities of the NDT error correction algorithm.
[0063] The core idea of the NDT algorithm is to divide the target point cloud into several small cubes, solve the multidimensional normal distribution of each grid and calculate its probability distribution model according to the set parameters. When the source point cloud points at the same coordinates enter, the probability of each conversion point in the corresponding grid is calculated according to the normal distribution parameters, and the probability of all grids is accumulated. When this cumulative probability reaches the maximum, the optimal matching relationship is found. Another key point is to maximize the probability, which requires the back-end optimization method. The core is to calculate the descent gradient value of the objective function or the Jacobian matrix or Hessian matrix according to the selected method to achieve the optimal. The implementation steps are as follows:
[0064] 1) First, the source point cloud is segmented into voxels, and the normal distribution parameters (mean and variance) of the point cloud in each voxel after segmentation are calculated.
[0065] 2) Each point in the registration point cloud is transformed into the voxel of the source point cloud according to the initial matrix.
[0066] 3) Based on the mean and variance in the voxel and the transformed new coordinates, calculate the probability density of the point.
[0067] 4) Accumulate all converted probability densities.
[0068] 5) Iterate the optimization matrix until the optimal transformation matrix is found.
[0069] Preferably, taking the route points as the initial pose candidate points, performing registration calculation using the normal distribution transformation algorithm according to the source point cloud points and the reference point cloud map, and determining the initial pose required for the vehicle positioning calculation includes:
[0070] Step S31: Enumerate each route point in the optimal walking route, such as Figure 2 As shown, the figure shows the direction of the yaw angle in the pose of each waypoint, which has been recorded in the reference point cloud map.
[0071] Preferably, the first route point is used as the initial pose candidate point, and the normal distribution transformation algorithm is used to perform registration calculation according to the source point cloud and the reference point cloud map. Before obtaining the registration probability of the source point cloud, the method further includes: performing sparse processing on each route point, and the sparse processing is as follows:
[0072] Taking the first route point as the starting point, along the optimal walking route, a route point is selected every distance B as the sparse route point.
[0073] According to the test efficiency requirement, the distance B is set, and some route points are selected from all route points as the sparse route points for subsequent calculations.
[0074] Preferably, B is 3 meters. Under the premise of ensuring the calculation accuracy, the calculation cost is significantly reduced, the processing speed is accelerated, and the real-time performance of the positioning is enhanced.
[0075] Preferably, the route points are used as initial pose candidate points, and the normal distribution transformation algorithm is used to perform registration calculation based on the source point cloud and the reference point cloud map: the sparse route points are used as initial pose candidate points, and the normal distribution transformation algorithm is used to perform registration calculation based on the source point cloud.
[0076] Step S32: Starting from the first route point, take the first route point as the initial pose candidate point, and perform registration calculation based on the source point cloud and the reference point cloud map using the normal distribution transformation algorithm to obtain the registration probability of the source point cloud; the registration probability is the probability that the source point cloud point falls into the grid of the reference point cloud map.
[0077] Regardless of the source point cloud point or each point cloud point in the reference point cloud map, each point includes:
[0078] Step S33: Determine whether the registration probability of the source point cloud reaches a preset probability threshold:
[0079] If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated and the first route point is used as the initial pose required for vehicle positioning calculation.
[0080] Exemplarily, if the registration probability of the source point cloud is higher than 3.0, the first route point is considered to be the initial pose required for vehicle positioning calculation and is used for vehicle positioning calculation.
[0081] If the registration probability of the source point cloud does not reach the preset probability threshold, the registration probability of the source point cloud is calculated one by one when each route point is used as a candidate point for the initial pose; when the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point corresponding to the time when the registration probability of the source point cloud reaches the preset probability threshold is the initial pose required for vehicle positioning calculation.
[0082] Preferably, the attitude includes the yaw angle. Because the vehicle is basically running on a plane, even if there is a slope, it generally does not exceed 15 degrees. Based on these conditions, this application does not enumerate the pitch and roll angles, but only enumerates the yaw angles of the route points.
[0083] Preferably, if all route points are used as initial pose candidate points and the registration probability of the source point cloud does not reach a preset probability threshold, the method further includes:
[0084] Step S34: enumerate the yaw angle of each route point according to the principle of dividing the circumference into N equal parts to obtain the yaw angle enumeration route points, where N is a positive integer greater than 1; Figure 3 As shown. Based on the optimal walking route recorded in the reference point cloud map, the circumference of each route point is divided into N equal parts, and the yaw angle of each route point is obtained to enumerate the route points.
[0085] Preferably, N is divided into 20 equal parts.
[0086] Starting from the first yaw angle enumeration route point in the counterclockwise direction of the first route point, the first yaw angle enumeration route point in the counterclockwise direction of the first route point is used as the initial pose candidate point. According to the source point cloud and the reference point cloud map, the normal distribution transformation algorithm is used to perform registration calculation to obtain the registration probability of the source point cloud, and determine whether the registration probability of the source point cloud reaches the preset probability threshold:
[0087] If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated with the first yaw angle in the counterclockwise direction of the first route point as the initial pose required for vehicle positioning calculation;
[0088] If the registration probability of the source point cloud does not reach the preset probability threshold, first traverse the yaw angle enumeration route points of the first route point, then traverse the yaw angle enumeration route points of the second route point, and so on, and calculate the registration probability of the source point cloud when each yaw angle enumeration route point of each route point is used as the initial pose candidate point one by one;
[0089] When the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the yaw angle of the corresponding route point when the registration probability of the source point cloud reaches the preset probability threshold is used to enumerate the route point as the initial pose required for vehicle positioning calculation.
[0090] Preferably, if the registration probability of the source point cloud does not reach a preset probability threshold when each yaw angle enumeration route point of each route point is used as an initial pose candidate point, the method further includes:
[0091] Step S35: On the left and right sides of the optimal walking route, enumerate the yaw angle enumeration route points of the first adjacent route points whose distance to each route point is A. That is, the yaw angle enumeration route points of each route point are translated to the left and right sides of the optimal walking route to obtain the yaw angle enumeration route points of the first adjacent route points.
[0092] Starting from the first yaw angle enumeration route point in the counterclockwise direction of the first adjacent route point, the first yaw angle enumeration route point in the counterclockwise direction of the first adjacent route point is used as the initial pose candidate point. According to the source point cloud and the reference point cloud map, the normal distribution transformation algorithm is used to perform registration calculation to obtain the registration probability of the source point cloud, and determine whether the registration probability of the source point cloud reaches the preset probability threshold:
[0093] If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated with the first yaw angle in the counterclockwise direction of the first adjacent route point as the initial pose required for vehicle positioning calculation;
[0094] If the registration probability of the source point cloud does not reach the preset probability threshold, first traverse the yaw angle enumeration route points of the first first adjacent route point, then traverse the yaw angle enumeration route points of the first first adjacent route point on the opposite side, and then traverse the yaw angle enumeration route points of the second first adjacent route point on the same side, and so on, and calculate the registration probability of the source point cloud when the yaw angle enumeration route points of the first adjacent route points on both sides of the optimal walking route are used as the initial pose candidate points one by one;
[0095] When the registration probability of the source point cloud reaches a preset probability threshold, the calculation is terminated, and the route point is enumerated as the initial posture required for vehicle positioning calculation based on the yaw angle of the first adjacent route point corresponding to the registration probability of the source point cloud reaching the preset probability threshold;
[0096] If the registration probability of the source point cloud does not reach the preset probability threshold when all the yaw angle enumeration route points of all the first adjacent route points are used as the initial pose candidate points, then on the side of the first adjacent route point far from the optimal walking route, the yaw angle enumeration route points of the second adjacent route points with a distance A from each first adjacent route point are enumerated;
[0097] The registration probability of the source point cloud is calculated one by one when the yaw angle of the second adjacent route point is enumerated as the initial pose candidate point;
[0098] When the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the yaw angle enumeration route point of the second adjacent route point corresponding to the registration probability of the source point cloud reaching the preset probability threshold is used as the initial pose required for vehicle positioning calculation; if the yaw angle enumeration route point of the second adjacent route point is used as the initial pose candidate point, the registration probability of the source point cloud does not reach the preset probability threshold, and the yaw angle enumeration route point of the third adjacent route point with a distance A from each second adjacent route point is enumerated, and so on. When traversing the yaw angle enumeration route points of all adjacent route points as the initial pose candidate points, the normal distribution transformation algorithm is used to calculate the registration probability of the source point cloud to determine the initial pose required for vehicle positioning calculation. The yaw angle enumeration route points of all adjacent route points are as follows: Figure 4 shown.
[0099] When determining the initial posture required for vehicle positioning from route points, from the perspective of probability priority of the vehicle's walking route, the initial posture is determined from the route points included in the optimal walking route first, and then the scope is expanded to enumerate the route points by the yaw angles of the route points. Finally, the scope is expanded to enumerate the route points by the yaw angles of all adjacent route points on the left and right sides of the optimal walking route. Under the premise of ensuring the accuracy of the initial posture, the efficiency of determining the initial posture can be improved.
[0100] Step S4: Perform global positioning of the vehicle based on the initial posture required for vehicle positioning calculation.
[0101] For example, this application establishes a 50-meter reference point cloud map for vehicle global positioning, uses a global positioning algorithm, and uses an 8-thread thread pool to run the NDT solution algorithm. In three cases, the time statistics of global positioning are shown in Table 1:
[0102] Table 1 Running time of global positioning algorithm
[0103]
[0104] Usually, the vehicle will be on the optimal walking route, so generally, the initial position required for vehicle positioning can be determined by executing step S34 at most. On a 50-meter route, the global positioning of the vehicle can be completed in about 10 seconds.
[0105] It should be noted that the terms used in the present invention are only for describing specific embodiments, rather than limiting the scope of the present application. As shown in the present specification, unless the context clearly indicates an exception, the words "one", "a", "a kind of" and / or "the" do not specifically refer to the singular, but may also include the plural. The terms "comprise", "include" or any other variant thereof are intended to cover non-exclusive inclusion, so that the process, method or device including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method or device. In the absence of more restrictions, the elements defined by the sentence "comprise one..." do not exclude the presence of other identical elements in the process, method or device including the elements.
[0106] It should also be noted that the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside", "outside", etc., indicating the orientation or positional relationship, are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention. Unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", etc. should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be a connection between the two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.
[0107] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the technical solutions of the embodiments of the present invention.
Claims
1. A method for global positioning of a vehicle based on laser radar, It is characterized in that The steps include: Constructing a reference point cloud map, using a laser radar to collect environmental information along the optimal walking route of the vehicle in the environment, constructing the reference point cloud map using a normal distribution transformation algorithm, and recording the optimal walking route of the vehicle and the route points included in the optimal walking route in the reference point cloud map, wherein the route points include the position and posture of the vehicle; and the posture includes the yaw angle; The point cloud collected by the laser radar on the vehicle is used as the source point cloud, and the source point cloud is a point cloud point sequence, and the point cloud point includes a position and a reflection intensity; Taking the route points as the initial pose candidate points, performing registration calculation using the normal distribution transformation algorithm according to the source point cloud and the reference point cloud map, and determining the initial pose required for vehicle positioning calculation; Specifically include: Enumerate each route point in the optimal walking route; Starting from the first route point, taking the first route point as the initial pose candidate point, performing registration calculation based on the source point cloud and the reference point cloud map using the normal distribution transformation algorithm, and obtaining the registration probability of the source point cloud; the registration probability is the probability that the source point cloud point falls into the grid of the reference point cloud map; Determine whether the registration probability of the source point cloud reaches the preset probability threshold: If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated and the first route point is used as the initial pose required for vehicle positioning calculation; If the registration probability of the source point cloud does not reach the preset probability threshold, the registration probability of the source point cloud is calculated one by one when each route point is used as a candidate point for the initial pose; when the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point corresponding to the time when the registration probability of the source point cloud reaches the preset probability threshold is the initial pose required for the vehicle positioning calculation; If all route points are used as initial pose candidate points, and the registration probability of the source point cloud does not reach the preset probability threshold: The yaw angle of each route point is enumerated based on the principle of dividing the circumference into N equal parts to obtain the yaw angle enumeration route points, where N is a positive integer greater than 1; Starting from the first yaw angle enumeration route point in the counterclockwise direction of the first route point, the first yaw angle enumeration route point in the counterclockwise direction of the first route point is used as the initial pose candidate point, and the normal distribution transformation algorithm is used to perform registration calculation according to the source point cloud and the reference point cloud map to obtain the registration probability of the source point cloud, and determine whether the registration probability of the source point cloud reaches a preset probability threshold: If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated with the first yaw angle in the counterclockwise direction of the first route point as the initial pose required for vehicle positioning calculation; If the registration probability of the source point cloud does not reach the preset probability threshold, first traverse the yaw angle enumeration route points of the first route point, then traverse the yaw angle enumeration route points of the second route point, and so on, and calculate the registration probability of the source point cloud when each yaw angle enumeration route point of each route point is used as the initial pose candidate point one by one; When the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated as the initial pose required for vehicle positioning calculation based on the yaw angle of the route point corresponding to the registration probability of the source point cloud reaching the preset probability threshold; If the registration probability of the source point cloud does not reach the preset probability threshold when the yaw angles of each route point are enumerated as initial pose candidates, then: On the left and right sides of the optimal walking route, enumerate the route points by enumerating the yaw angles of the first adjacent route points whose distance to each route point is A; Starting from the first yaw angle enumeration route point in the counterclockwise direction of the first first adjacent route point, the first yaw angle enumeration route point in the counterclockwise direction of the first first adjacent route point is used as the initial pose candidate point, and the normal distribution transformation algorithm is used to perform registration calculation according to the source point cloud and the reference point cloud map to obtain the registration probability of the source point cloud, and determine whether the registration probability of the source point cloud reaches a preset probability threshold: If the registration probability of the source point cloud reaches the preset probability threshold, the calculation is terminated, and the route point is enumerated with the first yaw angle in the counterclockwise direction of the first adjacent route point as the initial pose required for vehicle positioning calculation; If the registration probability of the source point cloud does not reach the preset probability threshold, first traverse the yaw angle enumeration route points of the first first adjacent route point, then traverse the yaw angle enumeration route points of the first first adjacent route point on the opposite side, and then traverse the yaw angle enumeration route points of the second first adjacent route point on the same side, and so on, and calculate the registration probability of the source point cloud when the yaw angle enumeration route points of the first adjacent route points on both sides of the optimal walking route are used as the initial pose candidate points one by one; When the registration probability of the source point cloud reaches a preset probability threshold, the calculation is terminated, and the route point is enumerated as the initial posture required for vehicle positioning calculation based on the yaw angle of the first adjacent route point corresponding to the registration probability of the source point cloud reaching the preset probability threshold; If the registration probability of the source point cloud does not reach the preset probability threshold when all the yaw angle enumeration route points of all the first adjacent route points are used as the initial pose candidate points, then on the side of the first adjacent route point far from the optimal walking route, the yaw angle enumeration route points of the second adjacent route points with a distance A from each first adjacent route point are enumerated; The registration probability of the source point cloud is calculated one by one when the yaw angle of the second adjacent route point is enumerated as the initial pose candidate point; When the registration probability of the source point cloud reaches a preset probability threshold, the calculation is terminated, and the yaw angle enumeration route point of the second adjacent route point corresponding to the registration probability of the source point cloud reaching the preset probability threshold is used as the initial pose required for vehicle positioning calculation; if the yaw angle enumeration route point of the second adjacent route point is used as the initial pose candidate point, the registration probability of the source point cloud does not reach the preset probability threshold, the yaw angle enumeration route point of the third adjacent route point whose distance to each second adjacent route point is A is enumerated, and so on, when the yaw angle enumeration route points of all adjacent route points are traversed as the initial pose candidate points, the normal distribution transformation algorithm is used to calculate the registration probability of the source point cloud to determine the initial pose required for vehicle positioning calculation; The vehicle is globally positioned according to the initial posture required for the vehicle positioning calculation.
2. A method for vehicle global positioning based on laser radar according to claim 1, It is characterized in that The method of taking the first route point as the initial pose candidate point and performing registration calculation using the normal distribution transformation algorithm according to the source point cloud and the reference point cloud map to obtain the registration probability of the source point cloud further includes: performing sparse processing on each route point, wherein the sparse processing is: Taking the first route point as the starting point, along the optimal walking route, a route point is selected every distance B as the sparse route point.
3. A method for vehicle global positioning based on laser radar according to claim 2, It is characterized in that The method of using route points as initial pose candidate points and using a normal distribution transformation algorithm to perform registration calculation based on the source point cloud and the reference point cloud map is as follows: using the sparse route points as initial pose candidate points and using a normal distribution transformation algorithm to perform registration calculation based on the source point cloud.
4. The method for global positioning of a vehicle based on laser radar according to claim 1, It is characterized in that The N equal parts are divided into 20 equal parts.
5. The method for global positioning of a vehicle based on laser radar according to claim 2, It is characterized in that The B is 3 meters.
