Positioning method and system for timing point cloud registration based on guardrail

Abstract

The invention discloses a positioning method and system for timing point cloud registration based on a guardrail, and relates to the technical field of automotive electronics, and the method comprises the steps: carrying out the ground detection of a laser radar point cloud, and obtaining a roll angle roll, a pitch angle pitch and a height z of a vehicle; in the timing interval, the vehicle position corresponding to the first frame point cloud is taken as an initial value; according to the initial value and the real-time vehicle speed and angular speed information of the vehicle, track plotting is carried out, and the vehicle position corresponding to each frame of point cloud in the timing interval is obtained; generating a guardrail point cloud; registering the laser radar point cloud and the point cloud template to obtain the abscissa x, the ordinate y and the course angle yaw of the vehicle; calculating a registration score; and establishing a combined status bit judgment mechanism, and outputting a status bit. According to the method, on the basis of point cloud registration based on the guardrail, timing calibration is added, and the phenomenon of longitudinal instability during registration caused by few longitudinal characteristics is avoided while the positioning transverse precision is effectively guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of automotive electronics technology, and in particular to a positioning method and system for performing timed point cloud registration based on a guardrail. Background Art

[0002] In port applications, accurate positioning is usually required to guide autonomous vehicles to load, unload and carry goods. GPS has some limitations in complex environments such as port yards: tall container stacks, cranes and other equipment in the yard may block or reflect GPS signals; and the port has dense electromagnetic activities, which may interfere with GPS signals and affect the reliability of positioning. As one of the important sensors on autonomous vehicles, lidar sensors can provide high-precision, high-resolution three-dimensional data, which can serve as an effective supplement to GPS measurements.

[0003] Currently, the following technical means are used to realize the positioning function of port scenes using laser point cloud data: 1) Use SLAM technology to realize real-time environmental map construction and positioning; 2) Match the point cloud data collected in real time with the known map or model to determine the precise position of the laser radar equipment or other target objects in the port area. However, since the environment of the port yard is highly dynamic, containers and cargo are constantly being moved, stacked and removed. SLAM technology may encounter difficulties in this dynamically changing environment. Figure 1 When registering the point cloud with a known map or model, a high-precision point cloud map of the yard needs to be collected in advance as a point cloud template. This solution can accurately locate in an environment with rich guardrail features, but it may be difficult to find enough matching points in an environment with few features. Once the port infrastructure changes, mismatching may occur, causing the vehicle to deviate. Summary of the invention

[0004] The present invention provides a positioning method and system for performing timed point cloud registration based on a guardrail, so as to overcome at least one technical problem existing in the prior art.

[0005] On the one hand, an embodiment of the present invention provides a positioning method for performing timed point cloud registration based on a guardrail, comprising:

[0006] Get the vehicle location;

[0007] Collect LiDAR point cloud;

[0008] Perform ground detection on the laser radar point cloud to obtain the vehicle's roll angle roll, pitch angle pitch and height z;

[0009] In the timing interval, the vehicle position corresponding to the first frame of point cloud is taken as the initial value;

[0010] Collect the real-time speed and angular velocity information of the vehicle;

[0011] According to the initial value, the real-time speed and angular velocity information of the vehicle, dead reckoning is performed to obtain the vehicle position corresponding to each frame of the point cloud within the timing interval;

[0012] Extracting the guardrail position, performing equal-interval differential guardrail point positions on the guardrail, generating a guardrail point cloud, and using the guardrail point cloud as a point cloud template;

[0013] Based on the vehicle position obtained by dead reckoning, the laser radar point cloud and the point cloud template are registered to obtain the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw; combined with the vehicle's roll angle roll, pitch angle pitch, height z, horizontal coordinate x, vertical coordinate y and heading angle yaw, the six-dimensional posture of the vehicle is obtained;

[0014] Calculate the registration score;

[0015] Establishing a combined state position judgment mechanism according to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction;

[0016] According to the judgment mechanism, a status bit is output.

[0017] Optionally, ground detection is performed on the laser radar point cloud to obtain the vehicle's roll angle roll, pitch angle pitch and height z, specifically:

[0018] Segmenting the laser radar point cloud based on a height threshold to obtain ground points;

[0019] Using the ground points to fit a ground model and a normal of the ground model;

[0020] According to the ground model and the normal of the ground model, the roll angle roll, pitch angle pitch and height z of the vehicle are calculated.

[0021] Optionally, the ground model and the normal of the ground model are obtained by fitting the ground points, specifically:

[0022] Based on the ground points, a ground model and a normal of the ground model are obtained by fitting using a RANSAC algorithm.

[0023] Optionally, the real-time vehicle speed and angular velocity information of the vehicle is collected, specifically:

[0024] Use the inertial measurement unit (IMU) to collect the real-time speed of the vehicle;

[0025] The wheel speed sensor is used to collect the real-time angular velocity information of the vehicle.

[0026] Optionally, the vehicle position corresponding to each frame of point cloud within the timing interval is Among them, x represents the horizontal coordinate of the vehicle at the corresponding time of the current frame point cloud, y represents the vertical coordinate of the vehicle at the corresponding time of the current frame point cloud, and θ t represents the vehicle heading angle at the corresponding moment of the current frame point cloud, x′ represents the vehicle horizontal coordinate at the corresponding moment of the previous frame point cloud, y′ represents the vehicle vertical coordinate at the corresponding moment of the previous frame point cloud, θ′ t It represents the vehicle heading angle at the corresponding moment of the previous frame point cloud, v represents the real-time vehicle speed, Δt represents the time difference between the current frame point cloud and the previous frame point cloud, and ω represents the real-time angular velocity.

[0027] Optionally, the laser radar point cloud and the guardrail point cloud are registered, specifically:

[0028] The normal distribution transform (NDT) registration algorithm is used to register the laser radar point cloud and the guardrail point cloud.

[0029] Optionally, calculate the registration score, specifically:

[0030] Calculate the probability density of each point in the laser radar point cloud in the corresponding probability distribution;

[0031] All probability densities are averaged to obtain the registration score.

[0032] Optionally, a combined state position judgment mechanism is established according to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction, specifically:

[0033] Determine whether the number of lidar point clouds is greater than the first threshold. If not, output status bit 1; if so, determine whether the alignment score is greater than the second threshold. If not, output status bit 2; if so, determine whether the horizontal and vertical second-order derivatives of the Hessian matrix are greater than the third threshold. If both the horizontal and vertical directions are greater than, it means that both the horizontal and vertical directions are available, and output status bit 3; if the horizontal direction is greater than and the vertical direction is less than, it means that the horizontal direction is available and the vertical direction is unavailable, and output status bit 4; if the vertical direction is greater than and the horizontal direction is less than, it means that the vertical direction is available and the horizontal direction is unavailable, and output status bit 5; if both the horizontal and vertical directions are less than, it means that both the horizontal and vertical directions are unavailable, and output status bit 6.

[0034] On the other hand, the present invention also provides a positioning system for performing timed point cloud registration based on a guardrail, comprising:

[0035] The acquisition module is used to obtain the vehicle position; to collect the laser radar point cloud; and to collect the vehicle's real-time speed and angular velocity information;

[0036] A preprocessing module is used to perform ground detection on the laser radar point cloud to obtain the vehicle's roll angle roll, pitch angle pitch and height z;

[0037] The module is set to take the vehicle position corresponding to the first frame of point cloud as the initial value within the timing interval;

[0038] A calculation module, used for performing dead reckoning according to the initial value, the real-time vehicle speed and angular velocity information of the vehicle, to obtain the vehicle position corresponding to each frame of the point cloud within the timing interval;

[0039] An extraction module is used to extract the guardrail position, and perform equal-interval differential guardrail point positions on the guardrail to generate a guardrail point cloud, and use the guardrail point cloud as a point cloud template;

[0040] A registration module is used to register the laser radar point cloud and the point cloud template based on the vehicle position obtained by dead reckoning to obtain the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw; and obtain the vehicle's six-dimensional posture by combining the vehicle's roll angle roll, pitch angle pitch, height z, horizontal coordinate x, vertical coordinate y and heading angle yaw;

[0041] A calculation module, used to calculate the registration score;

[0042] Establishing a module for establishing a combined state position judgment mechanism according to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction;

[0043] The output module is used to output the status bit according to the judgment mechanism.

[0044] The innovative features of the embodiments of the present invention include:

[0045] 1. In this embodiment, point cloud registration is performed based on the guardrail, which can effectively ensure the lateral accuracy of positioning. At the same time, in order to prevent longitudinal drift, a timed calibration method is adopted to introduce a frame of vehicle position information as the initial value at regular intervals. This can effectively avoid the longitudinal instability phenomenon caused by the lack of longitudinal features during registration, which is one of the innovations of the embodiment of the present invention.

[0046] 2. In this embodiment, a combined status bit judgment mechanism is established based on the number of point clouds, registration scores and Hessian matrices. The status bits are used to provide reliable registration and positioning results for downstream use, which is one of the innovations of the embodiments of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0048] Figure 1 A flow chart of a positioning method provided by an embodiment of the present invention;

[0049] Figure 2 A simplified schematic diagram of a port yard provided by an embodiment of the present invention;

[0050] Figure 3 A flow chart of ground detection provided by an embodiment of the present invention;

[0051] Figure 4 A flow chart for calculating a registration score provided by an embodiment of the present invention;

[0052] Figure 5 A flow chart of establishing a judgment mechanism provided by an embodiment of the present invention;

[0053] Figure 6 A schematic diagram of the structure of a positioning system provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0054] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. 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 are within the scope of protection of the present invention.

[0055] It should be noted that the terms "including" and "having" and any variations thereof in the embodiments of the present invention and the accompanying drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device including a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products or devices.

[0056] The embodiment of the present invention discloses a positioning method and system for performing timed point cloud registration based on a guardrail, which are described in detail below.

[0057] Figure 1 A flow chart of a positioning method provided by an embodiment of the present invention, Figure 2 For a simplified schematic diagram of a port yard provided in an embodiment of the present invention, please refer to Figure 1 and Figure 2 , a positioning method based on guardrail for timing point cloud registration is provided in an embodiment of the present invention, comprising:

[0058] Step 1: Get the vehicle location;

[0059] Step 2: Collect lidar point cloud;

[0060] Step 3: Perform ground detection on the LiDAR point cloud to obtain the vehicle's roll angle, pitch angle, and height z;

[0061] Step 4: Within the timing interval, take the vehicle position corresponding to the first frame point cloud as the initial value;

[0062] Step 5: Collect the real-time speed and angular velocity information of the vehicle;

[0063] Step 6: Perform dead reckoning based on the initial value, the real-time speed and angular velocity information of the vehicle to obtain the vehicle position corresponding to each frame of the point cloud within the timing interval;

[0064] Step 7: Extract the guardrail position, perform equal-interval differential guardrail point positions on the guardrail, generate the guardrail point cloud, and use the guardrail point cloud as the point cloud template;

[0065] Step 8: Based on the vehicle position obtained by dead reckoning, align the lidar point cloud and the point cloud template to obtain the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw; combine the vehicle's roll angle roll, pitch angle pitch, height z, horizontal coordinate x, vertical coordinate y and heading angle yaw to obtain the vehicle's six-dimensional posture;

[0066] Step 9: Calculate the registration score;

[0067] Step 10: Establish a combined state position judgment mechanism based on the number of point clouds, registration score and the second-order derivative of the Hessian matrix direction;

[0068] Step 11: Output the status bit according to the judgment mechanism.

[0069] Specifically, please refer to Figure 1 and Figure 2 In the port scene, guardrails are placed on both sides of the road for safe driving. Autonomous driving vehicles are equipped with laser radars. When the equipment is working, it emits lasers and receives reflected laser signals to obtain point cloud data of the surrounding environment.

[0070] The embodiment of the present invention provides a positioning method for performing timed point cloud registration based on a guardrail, wherein the vehicle position and the laser radar point cloud are collected respectively through step 1 and step 2, wherein the vehicle position comes from the vehicle body signal and can be obtained by solving sensors such as GPS. The laser radar point cloud data comes from the laser radar sensor, which is usually installed on both sides of the front of the vehicle to collect real-time point cloud data.

[0071] After obtaining the point cloud data, in step 3, the lidar point cloud is ground detected, and the ground model is fitted according to the point cloud data. From the fitted ground model, the normal vector of the ground model can be directly obtained. Then, the roll angle roll, pitch angle pitch and height z of the vehicle can be calculated respectively according to the ground model and the normal vector of the ground model.

[0072] Please refer to Figure 2 , the long straight guardrails in the yard can only provide lateral (perpendicular to the vehicle's forward direction) constraints. In order to avoid excessive longitudinal offset, the present invention adopts a timed calibration mode, that is, the vehicle position is used as the initial value every n seconds. The timing interval is set by step 4, and the vehicle position corresponding to the first frame point cloud in each timing interval is used as the initial value. In this way, the problem of excessive longitudinal offset caused by the guardrail only providing lateral constraints can be avoided.

[0073] In order to improve the accuracy and robustness of positioning, in step 5, the present invention collects the real-time speed and angular velocity information of the vehicle through other sensors, for example, using an inertial measurement unit IMU (Inertial Measurement Unit) to collect the real-time speed of the vehicle; using a wheel speed sensor to collect the real-time angular velocity information of the vehicle. And in step 6, based on the initial value, the real-time speed and angular velocity information of the vehicle, the track is calculated to obtain the vehicle position corresponding to each frame of the point cloud within the timing interval. Here, the vehicle position obtained by the track calculation is the updated vehicle position. By combining the laser point cloud data with the data of other sensors such as GPS, IMU (Inertial Measurement Unit) and wheel speed sensor, the accuracy and robustness of positioning are improved.

[0074] When performing dead reckoning, since the distance the vehicle moves between two adjacent point clouds is short, the motion trajectory between two adjacent points can be regarded as a straight line, that is, it moves v×Δt along the vehicle coordinate system. Projecting this distance on the x-axis and y-axis of the world coordinate system, the displacement of the vehicle in the world coordinate system during this period can be obtained as Δx=v×Δt*cosθ t , Δy=v×Δt*sinθ t , where v represents the real-time vehicle speed, and Δt represents the time difference between the current frame point cloud and the previous frame point cloud. In addition, within one frame time, the angle change is ω*Δt, where ω represents the real-time angular velocity.

[0075] After obtaining the displacement of the vehicle within one frame, the vehicle position corresponding to the next frame of point cloud can be calculated based on the initial value of the vehicle position. In this way, the vehicle position corresponding to each frame of point cloud within the timing interval can be calculated by incremental accumulation and summation: Among them, x represents the horizontal coordinate of the vehicle at the corresponding time of the current frame point cloud, y represents the vertical coordinate of the vehicle at the corresponding time of the current frame point cloud, and θ t represents the vehicle heading angle at the corresponding moment of the current frame point cloud, x′ represents the vehicle horizontal coordinate at the corresponding moment of the previous frame point cloud, y′ represents the vehicle vertical coordinate at the corresponding moment of the previous frame point cloud, θ′ t Indicates the vehicle heading angle at the corresponding moment of the previous frame of point cloud.

[0076] Normally, the guardrails in the yard are widely and evenly distributed and generally will not be adjusted. Therefore, the present invention uses the guardrail point cloud as a point cloud template and aligns and locates it with the real-time collected laser radar point cloud data. Based on this, it is necessary to obtain a high-precision guardrail point cloud map in advance. Therefore, the present invention collects the guardrail point cloud through step 7. When collecting the guardrail point cloud, it is first necessary to extract the guardrail position. After obtaining the guardrail position, the guardrail point cloud can be generated by performing equal-interval differential guardrail point positions on the guardrail, and the guardrail point cloud is used as a point cloud template.

[0077] It should be noted that the guardrail point cloud can be obtained through laser scanning, but the cost of high-precision laser scanners is relatively high. Therefore, in practical applications, the guardrail position can be extracted based on the high-precision map information obtained by RTK (Real Time Kinematic, real-time dynamic measurement), and then the guardrail point cloud can be collected based on the guardrail position.

[0078] After obtaining the updated vehicle position and template point cloud, in step 8, the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw are obtained by aligning the lidar point cloud and the point cloud template.

[0079] The NDT (Normal Distribution Transform) registration algorithm converts the reference point cloud into a normal distribution of multidimensional variables for registration. In reality, the point cloud obtained by the LiDAR scan may be slightly different from the reference point cloud (template point cloud). This deviation may come from measurement errors or changes in the "scene". NDT registration can ignore subtle changes, so it can be used to solve these subtle deviation problems. At the same time, the NDT registration algorithm is time-consuming and stable, and has little to do with the initial value. When the initial value error is large, it can also be corrected well. And because it does not use the feature calculation and matching of corresponding points in the registration, it does not need to consume a lot of cost to calculate the nearest points, so the time is faster. Therefore, when registering the LiDAR point cloud and the guardrail point cloud, the normal distribution transform NDT registration algorithm can be used.

[0080] In the NDT registration algorithm, if the transformation parameters can make the two laser data match well, then the probability density corresponding to the transformed points in the reference point cloud will be large. Therefore, to make the two laser point cloud data match optimally, it is only necessary to use the optimization method to iteratively calculate the transformation matrix that maximizes the sum of the probability densities. In the iterative registration process, in order to optimize the transformation parameters, it is necessary to minimize a cost function, usually the negative log-likelihood function from the source point cloud to the target point cloud. This optimization problem can be solved by gradient descent or Newton's method. In the Newton method, the new transformation parameters are obtained by the following formula: in is the change in transformation parameters, H is the Hessian matrix, is the gradient vector. The NDT registration algorithm can refer to the existing NDT algorithm, which will not be described here.

[0081] After obtaining the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw, combine them with the vehicle's roll angle roll, pitch angle pitch, and height z obtained in step 3 to obtain the vehicle's six-dimensional posture.

[0082] In the registration process, in order to ensure that the registration is effective, it is necessary to send a status bit description file to the downstream segment to tell the downstream segment the current registration status. The registration score is usually used to quantify the degree of matching between two point clouds. Therefore, the present invention calculates the registration score through step 9. The higher the score, the better the matching degree of the point cloud.

[0083] After obtaining the registration score, in step 10, a combined state bit judgment mechanism is established. When establishing the combined state bit judgment mechanism, in addition to the registration score, the number of point clouds and the Hessian matrix information are also used. In step 11, according to the judgment mechanism, for example, the number of point clouds, the registration score, and the second-order derivatives of the Hessian matrix in the horizontal and vertical directions are judged to meet the requirements, and the corresponding state bits in various different situations are output, so that the downstream section can judge the current registration status according to the received state bits.

[0084] The positioning method for performing timed point cloud registration based on guardrails provided by the present invention can effectively ensure the lateral accuracy of positioning by performing point cloud registration based on guardrails. At the same time, in order to prevent longitudinal drift, a timed calibration method is adopted to introduce a frame of vehicle position information as an initial value at regular intervals, which can effectively avoid the longitudinal instability phenomenon caused by the lack of longitudinal features during registration. In addition, a combined state bit judgment mechanism is established based on the number of point clouds, registration score and Hessian matrix, and a reliable registration and positioning result is provided for downstream use through the state bit.

[0085] Optionally, Figure 3 For a flow chart of ground detection provided by an embodiment of the present invention, please refer to Figure 1and Figure 3 In step 3, ground detection is performed on the lidar point cloud to obtain the vehicle's roll angle roll, pitch angle pitch and height z, specifically: step 31, segment the lidar point cloud based on the height threshold to obtain ground points; step 32, use the ground points to fit the ground model and the normal of the ground model; step 33, according to the ground model and the normal of the ground model, calculate the vehicle's roll angle roll, pitch angle pitch and height z.

[0086] Specifically, please refer to Figure 3 In this embodiment, first in step 31, the laser radar point cloud is segmented into ground points and non-ground points by threshold segmentation. The threshold here is the height threshold. For example, assuming that the height threshold is 0.2, points below the threshold are considered to be ground points. In step 32, the ground model is obtained by fitting the ground points. When fitting the ground model, the RANSAC algorithm can be used. It is assumed that the ground is simulated as a plane equation Ax+By+Cz+D=0. The model is fitted by iteratively selecting a random subset in the point cloud, calculating the model parameters (A, B, C, D), and determining which points are the inliers of the model. Inliers refer to points whose errors with the model are less than a certain threshold. Through iterative calculation, the model with the largest number of inliers is found as the ground model.

[0087] After obtaining the ground model, the normal vector n = (A, B, C) of the ground model can be directly obtained from the fitted ground model. In step 33, the pitch angle of the vehicle can be obtained by calculating the angle between the normal line of the ground model and the projection of the z axis on the xz plane; the roll angle of the vehicle can be obtained by calculating the angle between the normal line of the ground model and the projection of the z axis on the yz plane. The height z of the vehicle can be directly calculated by solving the ground model Ax + By + Cz + D = 0 given the (x, y) coordinates.

[0088] Optionally, Figure 4 For a flowchart of calculating the registration score provided in an embodiment of the present invention, please refer to Figure 1 and Figure 4 In step 9, the registration score is calculated, specifically: step 91, calculate the probability density of each point in the lidar point cloud in the corresponding probability distribution; step 92, take the average of all probability densities to obtain the registration score.

[0089] Specifically, please refer to Figure 4 Since NDT registration uses a probability distribution model to represent point clouds, the registration score is usually related to the probability density of each point in the point cloud. Therefore, in this embodiment, the probability density of each point in the laser radar point cloud in the corresponding probability distribution is first calculated in step 91, and then the registration score is obtained by averaging all the probability densities in step 92.

[0090] It should be noted that the use of the average value of the probability density as the registration score in step 92 is only an implementation method in this embodiment and is not intended to limit the present invention. In other embodiments, the sum of the probability density may also be used as the registration score.

[0091] Optionally, Figure 5 For a flow chart of establishing a judgment mechanism provided by an embodiment of the present invention, please refer to Figure 1 and Figure 5 In step 10, a combined status bit judgment mechanism is established according to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction, specifically: determine whether the number of laser radar point clouds is greater than the first threshold, if not, output status bit 1; if so, determine whether the registration score is greater than the second threshold, if not, output status bit 2, if so, determine whether the horizontal and vertical second-order derivatives of the Hessian matrix are greater than the third threshold, if both the horizontal and vertical directions are greater, it means that both the horizontal and vertical directions are available, and output status bit 3; if the horizontal direction is greater than and the vertical direction is less than, it means that the horizontal direction is available and the vertical direction is unavailable, and output status bit 4; if the vertical direction is greater than and the horizontal direction is less than, it means that the vertical direction is available and the horizontal direction is unavailable, and output status bit 5; if both the horizontal and vertical directions are less than, it means that both the horizontal and vertical directions are unavailable, and output status bit 6.

[0092] Specifically, generally, the more point clouds there are, the higher the matching accuracy is, while too few point clouds will increase the possibility of mismatching. Therefore, when establishing a combined state position judgment mechanism, the present invention uses the number of point clouds in addition to the intuitive registration score.

[0093] In addition, the elements on the diagonal of the Hessian matrix represent the second-order partial derivatives of the objective function with respect to a single variable. If the absolute value of a certain diagonal element is large, this indicates that the curvature of the objective function is large in the direction of that particular transformation parameter, which means that a small change in that direction may cause the error to increase rapidly. Therefore, it can be said that the transformation parameter is subject to strong constraints. The off-diagonal elements of the Hessian matrix represent the mixed second-order partial derivatives of the objective function with respect to two different variables. These elements provide information about the interaction and coupling between different transformation parameters. Therefore, the second-order derivative value of the Hessian matrix can be used to determine which direction has stronger constraints in NDT alignment. Therefore, when the present invention establishes a combined state bit judgment mechanism, the second-order derivative of the Hessian matrix is ​​also used.

[0094] Please refer to Figure 5,When establishing the judgment mechanism, first judge whether the number of laser radar point clouds is greater than the ,first threshold. When the number of laser point clouds is not greater than the ,first threshold, it means that the number of laser point clouds is small and ,there may be a large matching error. Therefore, the output status bit 1 is ,informed to the downstream segment that the current positioning ,result is unavailable.

[0095] When the number of point clouds is greater than the first threshold, it is determined whether the registration score is greater than the second threshold. The registration score can intuitively display the registration situation. The larger the registration score, the better the matching degree and the higher the positioning accuracy. Therefore, when the registration score is not greater than the second threshold, it means that the current registration situation is not good, and the status bit 2 is output to tell the downstream segment that the current positioning result is unavailable.

[0096] When the registration score is greater than the second threshold, it is determined whether the second-order derivatives of the Hessian matrix in the horizontal and vertical directions are greater than the third threshold. Specifically, in the port scene, the guardrail in the straight area provides horizontal constraints, and the registration is only available in the horizontal direction, while in the turning area, both the horizontal and vertical directions are available. If both the horizontal and vertical directions are greater than the third threshold, it means that both the horizontal and vertical directions are available, and the status bit 3 is output, thereby telling the downstream section that both the horizontal and vertical information in the current positioning information are available. If the horizontal direction is greater than the third threshold and the vertical direction is less than the third threshold, it means that the horizontal direction is available and the vertical direction is unavailable, and the status bit 4 is output, thereby telling the downstream section that the horizontal information in the current positioning information is available and the vertical information is unavailable. If the vertical direction is greater than the third threshold and the horizontal direction is less than the third threshold, it means that the vertical direction is available and the horizontal direction is unavailable, and the status bit 5 is output, thereby telling the downstream section that the vertical information in the current positioning information is available and the horizontal information is unavailable. If both the horizontal and vertical directions are less than the third threshold, it means that both the horizontal and vertical directions are unavailable, and the status bit 6 is output, thereby telling the downstream section that both the horizontal and vertical information in the current positioning information are unavailable.

[0097] When constructing the lateral and longitudinal second-order directional derivatives to evaluate the effectiveness of the lateral and longitudinal constraints in NDT registration, assuming that there is a direction d = (cosθ, sinθ), then the lateral second-order directional derivative Hessian_horizontal and the longitudinal second-order directional derivative Hessian_vertical of Hessian(x,y) along d are: Hessian_horizontal = Hessian xx (x,y)cosθcosθHessian yx (x,y)sinθcosθ+Hessian xy (x,y)cosθsinθ+Hessian yy (x,y)sinθsinθ,Hessian_vertical=Hessian yy (x,y)cosθcosθ+Hessian yx (x,y)sinθcosθ+Hessian xy(x,y)cosθsinθ+Hessian xx (x,y)sinθsinθ.

[0098] Based on the same inventive concept, the present invention also provides a positioning system based on guardrails for timing point cloud registration. Figure 6 For a schematic diagram of a positioning system provided by an embodiment of the present invention, please refer to Figure 6 , the positioning system 100 for performing timed point cloud registration based on guardrails provided in an embodiment of the present invention includes:

[0099] The acquisition module 101 is used to obtain the vehicle position; to collect the laser radar point cloud; and to collect the real-time vehicle speed and angular velocity information of the vehicle;

[0100] The preprocessing module 102 is used to perform ground detection on the laser radar point cloud to obtain the vehicle's roll angle roll, pitch angle pitch and height z;

[0101] Setting module 103, within a timing interval, taking the vehicle position corresponding to the first frame point cloud as an initial value;

[0102] The dead reckoning module 104 is used to perform dead reckoning based on the initial value, the real-time speed and angular velocity information of the vehicle, and obtain the vehicle position corresponding to each frame of the point cloud within the timing interval;

[0103] The extraction module 105 is used to extract the guardrail position, and perform equal-interval differential guardrail point positions on the guardrail to generate a guardrail point cloud, and use the guardrail point cloud as a point cloud template;

[0104] The registration module 106 is used to register the laser radar point cloud and the point cloud template based on the vehicle position obtained by dead reckoning to obtain the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw; and obtain the vehicle's six-dimensional posture by combining the vehicle's roll angle roll, pitch angle pitch, height z, horizontal coordinate x, vertical coordinate y and heading angle yaw;

[0105] A calculation module 107, used to calculate a registration score;

[0106] Establishing module 108, for establishing a combined state position judgment mechanism according to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction;

[0107] The output module 109 is used to output the status bit according to the judgment mechanism.

[0108] Specifically, please refer to Figure 6The vehicle position and the laser radar point cloud are collected by the collection module 101, wherein the vehicle position comes from the vehicle body signal and can be obtained by solving the GPS and other sensors. The laser radar point cloud data comes from the laser radar sensor, which is usually installed on both sides of the front of the vehicle to collect real-time point cloud data.

[0109] After obtaining the point cloud data, the preprocessing module 102 performs ground detection on the laser radar point cloud and obtains a ground model based on the point cloud data. From the fitted ground model, the normal vector of the ground model can be directly obtained. Then, the roll angle roll, pitch angle pitch and height z of the vehicle can be calculated respectively based on the ground model and the normal vector of the ground model.

[0110] Please refer to Figure 2 , the long straight guardrails in the yard can only provide lateral (perpendicular to the vehicle's forward direction) constraints. In order to avoid excessive longitudinal offset, the present invention adopts a timing calibration mode, that is, the vehicle position is used as the initial value every n seconds. The timing interval is set by setting module 103, and the vehicle position corresponding to the first frame point cloud in each timing interval is used as the initial value. In this way, the problem of excessive longitudinal offset caused by the guardrail only providing lateral constraints can be avoided.

[0111] In order to improve the accuracy and robustness of positioning, the present invention also collects the real-time speed and angular velocity information of the vehicle through the acquisition module 101, for example, the real-time speed of the vehicle is collected by using the inertial measurement unit IMU (Inertial Measurement Unit); the real-time angular velocity information of the vehicle is collected by using the wheel speed sensor. And the calculation module 104 performs track calculation based on the initial value, the real-time speed and angular velocity information of the vehicle, and the vehicle position corresponding to each frame of the point cloud within the timing interval can be obtained. Here, the vehicle position obtained by the track calculation is the updated vehicle position. By combining the laser point cloud data with the data of other sensors such as GPS, IMU (Inertial Measurement Unit) and wheel speed sensor, the accuracy and robustness of positioning are improved.

[0112] When performing dead reckoning, since the distance the vehicle moves between two adjacent point clouds is short, the motion trajectory between two adjacent points can be regarded as a straight line, that is, it moves v×Δt along the vehicle coordinate system. Projecting this distance on the x-axis and y-axis of the world coordinate system, the displacement of the vehicle in the world coordinate system during this period can be obtained as Δx=v×Δt*cosθ t , Δy=v×Δt*sinθ t , where v represents the real-time vehicle speed, and Δt represents the time difference between the current frame point cloud and the previous frame point cloud. In addition, within one frame time, the angle change is ω*Δt, where ω represents the real-time angular velocity.

[0113] After obtaining the displacement of the vehicle within one frame, the vehicle position corresponding to the next frame of point cloud can be calculated based on the initial value of the vehicle position. In this way, the vehicle position corresponding to each frame of point cloud within the timing interval can be calculated by incremental accumulation and summation: Among them, x represents the horizontal coordinate of the vehicle at the corresponding time of the current frame point cloud, y represents the vertical coordinate of the vehicle at the corresponding time of the current frame point cloud, and θ t represents the vehicle heading angle at the corresponding moment of the current frame point cloud, x′ represents the vehicle horizontal coordinate at the corresponding moment of the previous frame point cloud, y′ represents the vehicle vertical coordinate at the corresponding moment of the previous frame point cloud, θ′ t Indicates the vehicle heading angle at the corresponding moment of the previous frame of point cloud.

[0114] Normally, the guardrails in the yard are widely and evenly distributed and generally will not be adjusted. Therefore, the present invention uses the guardrail point cloud as a point cloud template to align and locate with the real-time collected laser radar point cloud data. Based on this, it is necessary to obtain a high-precision guardrail point cloud map in advance. Therefore, the present invention collects the guardrail point cloud through the extraction module 105. When collecting the guardrail point cloud, it is first necessary to extract the guardrail position. After obtaining the guardrail position, the guardrail point cloud can be generated by performing equal-interval differential guardrail point positions on the guardrail, and the guardrail point cloud is used as a point cloud template.

[0115] It should be noted that the guardrail point cloud can be obtained through laser scanning, but the cost of high-precision laser scanners is relatively high. Therefore, in practical applications, the guardrail position can be extracted based on the high-precision map information obtained by RTK (Real Time Kinematic, real-time dynamic measurement), and then the guardrail point cloud can be collected based on the guardrail position.

[0116] After obtaining the updated vehicle position and template point cloud, the laser radar point cloud and the point cloud template are registered by the registration module 106 to obtain the horizontal coordinate x, the vertical coordinate y and the heading angle yaw of the vehicle.

[0117] The NDT registration algorithm converts the reference point cloud into a normal distribution of multidimensional variables for registration. In reality, the point cloud obtained by the lidar scan may be slightly different from the reference point cloud (template point cloud). This deviation may come from measurement errors or changes in the "scene". NDT registration can ignore subtle changes, so it can be used to solve these subtle deviation problems. At the same time, the NDT registration algorithm is time-consuming and stable, and has little to do with the initial value. When the initial value error is large, it can also be corrected well. And because it does not use the feature calculation and matching of corresponding points in the registration, it does not need to consume a lot of cost to calculate the nearest points, so the time is faster. Therefore, when registering the lidar point cloud and the guardrail point cloud, the normal distribution transformation NDT registration algorithm can be used.

[0118] In the NDT registration algorithm, if the transformation parameters can make the two laser data match well, then the probability density corresponding to the transformed points in the reference point cloud will be large. Therefore, to make the two laser point cloud data match optimally, it is only necessary to use the optimization method to iteratively calculate the transformation matrix that maximizes the sum of the probability densities. In the iterative registration process, in order to optimize the transformation parameters, it is necessary to minimize a cost function, usually the negative log-likelihood function from the source point cloud to the target point cloud. This optimization problem can be solved by gradient descent or Newton's method. In the Newton method, the new transformation parameters are obtained by the following formula: in is the change in transformation parameters, H is the Hessian matrix, is the gradient vector. The NDT registration algorithm can refer to the existing NDT algorithm, which will not be described here.

[0119] After obtaining the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw, they are combined with the vehicle's roll angle roll, pitch angle pitch, and height z obtained above to obtain the vehicle's six-dimensional posture.

[0120] In the registration process, in order to ensure that the registration is effective, a status bit description file needs to be sent to the downstream segment to tell the downstream segment the current registration status. The registration score is usually used to quantify the degree of matching between two point clouds. Therefore, the present invention calculates the registration score through the calculation module 107. The higher the score, the better the matching degree of the point cloud.

[0121] After obtaining the registration score, the establishment module 108 is used to establish a combined state bit judgment mechanism. When establishing the combined state bit judgment mechanism, in addition to the registration score, the number of point clouds and the Hessian matrix information are also used. The output module 109 outputs the corresponding state bits under various different conditions according to the judgment mechanism, for example, respectively judging whether the number of point clouds, the registration score, and the second-order derivatives of the Hessian matrix in the horizontal and vertical directions meet the requirements, so that the downstream section can judge the current registration status according to the received state bits.

[0122] The positioning system provided by the present invention performs timed point cloud registration based on guardrails, and performs point cloud registration based on guardrails, which can effectively ensure the lateral accuracy of positioning. At the same time, in order to prevent longitudinal drift, a timed calibration method is adopted to introduce a frame of vehicle position information as the initial value at regular intervals, which can effectively avoid the longitudinal instability phenomenon caused by the lack of longitudinal features during registration. In addition, a combined state bit judgment mechanism is established based on the number of point clouds, registration scores and Hessian matrices, and a reliable registration and positioning result is provided for downstream use through the state bit.

[0123] Those skilled in the art can understand that the accompanying drawings are only schematic diagrams of an embodiment, and the modules or processes in the accompanying drawings are not necessarily required to implement the present invention.

[0124] Those skilled in the art can understand that the modules in the device in the embodiment can be distributed in the device in the embodiment according to the description of the embodiment, or can be changed accordingly and located in one or more devices different from the embodiment. The modules in the above embodiment can be combined into one module, or can be further divided into multiple sub-modules.

[0125] 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 make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning method for timed point cloud registration based on guardrails, characterized in that: include: Get the vehicle location; Collect LiDAR point cloud; Perform ground detection on the laser radar point cloud to obtain the vehicle's roll angle roll, pitch angle pitch and height z; In the timing interval, the vehicle position corresponding to the first frame of point cloud is taken as the initial value; Collect the real-time speed and angular velocity information of the vehicle; According to the initial value, the real-time speed and angular velocity information of the vehicle, dead reckoning is performed to obtain the vehicle position corresponding to each frame of the point cloud within the timing interval; Extracting the guardrail position, performing equal-interval differential guardrail point positions on the guardrail, generating a guardrail point cloud, and using the guardrail point cloud as a point cloud template; Based on the vehicle position obtained by dead reckoning, the laser radar point cloud and the point cloud template are registered to obtain the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw; combined with the vehicle's roll angle roll, pitch angle pitch, height z, horizontal coordinate x, vertical coordinate y and heading angle yaw, the six-dimensional posture of the vehicle is obtained; Calculate the registration score; Establishing a combined state position judgment mechanism according to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction; According to the judgment mechanism, a status bit is output.

2. The positioning method based on guardrail for timing point cloud registration according to claim 1, characterized in that: The laser radar point cloud is ground detected to obtain the vehicle's roll angle roll, pitch angle pitch and height z, specifically: Segmenting the laser radar point cloud based on a height threshold to obtain ground points; Using the ground points to fit a ground model and a normal of the ground model; According to the ground model and the normal of the ground model, the roll angle roll, pitch angle pitch and height z of the vehicle are calculated.

3. The positioning method based on guardrail for timing point cloud registration according to claim 2, characterized in that: The ground model and the normal of the ground model are obtained by fitting the ground points, specifically: Based on the ground points, a ground model and a normal of the ground model are obtained by fitting using a RANSAC algorithm.

4. The positioning method based on guardrail for timing point cloud registration according to claim 1, characterized in that: Collect the real-time speed and angular velocity information of the vehicle, specifically: Use the inertial measurement unit (IMU) to collect the real-time speed of the vehicle; The wheel speed sensor is used to collect the real-time angular velocity information of the vehicle.

5. The positioning method based on guardrail for timing point cloud registration according to claim 1, characterized in that: The vehicle position corresponding to each frame of point cloud within the timing interval is Among them, x represents the horizontal coordinate of the vehicle at the corresponding time of the current frame point cloud, y represents the vertical coordinate of the vehicle at the corresponding time of the current frame point cloud, and θ t represents the vehicle heading angle at the corresponding moment of the current frame point cloud, x′ represents the vehicle horizontal coordinate at the corresponding moment of the previous frame point cloud, y′ represents the vehicle vertical coordinate at the corresponding moment of the previous frame point cloud, θ′ t It represents the vehicle heading angle at the corresponding moment of the previous frame point cloud, v represents the real-time vehicle speed, Δt represents the time difference between the current frame point cloud and the previous frame point cloud, and ω represents the real-time angular velocity.

6. The positioning method based on guardrail for timing point cloud registration according to claim 1, characterized in that: The laser radar point cloud and the guardrail point cloud are registered, specifically: The normal distribution transform (NDT) registration algorithm is used to register the laser radar point cloud and the guardrail point cloud.

7. The positioning method based on guardrail for timing point cloud registration according to claim 1, characterized in that: Calculate the registration score, specifically: Calculate the probability density of each point in the laser radar point cloud in the corresponding probability distribution; All probability densities are averaged to obtain the registration score.

8. The positioning method based on guardrail for timing point cloud registration according to claim 1, characterized in that: According to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction, a combined state position judgment mechanism is established, specifically: Determine whether the number of lidar point clouds is greater than the first threshold. If not, output status bit 1; if so, determine whether the alignment score is greater than the second threshold. If not, output status bit 2; if so, determine whether the horizontal and vertical second-order derivatives of the Hessian matrix are greater than the third threshold. If both the horizontal and vertical directions are greater than, it means that both the horizontal and vertical directions are available, and output status bit 3; if the horizontal direction is greater than and the vertical direction is less than, it means that the horizontal direction is available and the vertical direction is unavailable, and output status bit 4; if the vertical direction is greater than and the horizontal direction is less than, it means that the vertical direction is available and the horizontal direction is unavailable, and output status bit 5; if both the horizontal and vertical directions are less than, it means that both the horizontal and vertical directions are unavailable, and output status bit 6.

9. A positioning system for timed point cloud registration based on guardrails, characterized in that: include: The acquisition module is used to obtain the vehicle position; to collect the laser radar point cloud; and to collect the vehicle's real-time speed and angular velocity information; A preprocessing module is used to perform ground detection on the laser radar point cloud to obtain the vehicle's roll angle roll, pitch angle pitch and height z; The module is set to take the vehicle position corresponding to the first frame of point cloud as the initial value within the timing interval; A dead reckoning module, configured to perform dead reckoning based on the initial value, the real-time speed and angular velocity information of the vehicle, and obtain the vehicle position corresponding to each frame of the point cloud within the timing interval; An extraction module is used to extract the guardrail position, and perform equal-interval differential guardrail point positions on the guardrail to generate a guardrail point cloud, and use the guardrail point cloud as a point cloud template; A registration module is used to register the laser radar point cloud and the point cloud template based on the vehicle position obtained by dead reckoning to obtain the vehicle's horizontal coordinate x, vertical coordinate y and heading angle yaw; and obtain the vehicle's six-dimensional posture by combining the vehicle's roll angle roll, pitch angle pitch, height z, horizontal coordinate x, vertical coordinate y and heading angle yaw; A calculation module, used to calculate the registration score; Establishing a module for establishing a combined state position judgment mechanism according to the number of point clouds, the registration score and the second-order derivative of the Hessian matrix direction; The output module is used to output the status bit according to the judgment mechanism.

Images