A positioning correction method and system in a laser feature degradation environment and a storage medium

Abstract

The present application relates to a positioning correction method and system in a laser feature degradation environment and a storage medium, comprising U1. When the vehicle travels in a port yard area or a tunnel area, target area surface feature data information is obtained based on the vehicle head laser radar, vehicle pose data information is obtained based on the vehicle-mounted combined navigation, the vehicle pose data information is input into a high-precision map, and lane center line shape point data information where the vehicle is located is obtained; U2. The target area surface feature data information is traversed, and according to a surface feature validity function P, target area optimal surface feature data information is output; U3. A Frenet coordinate system is established based on the lane center line shape point data information where the vehicle is located, and the target area optimal surface feature data information is projected into the Frenet coordinate system. The present application not only solves the problem of collision risk caused by the deviation of the vehicle heading angle and position, but also corrects the positioning according to the optimal surface feature information, thereby improving the positioning accuracy.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of laser radar positioning technology, in particular to a positioning correction method and system in a laser feature degradation environment and a storage medium. BACKGROUND

[0002] The surface features recognized by the laser radar in the port yard and tunnel scene are obvious, and can be used for positioning correction and constraint. At present, the sensors used by vehicles include combined inertial navigation, laser radar, intelligent camera, etc., which are used to obtain the position information of the vehicle, and the feature matching positioning is performed combined with high-precision map information, and then the positioning results of each sensor are input into the multi-sensor fusion positioning algorithm to calculate more robust and smooth positioning information.

[0003] The containers in the port yard are placed according to specific positions, and the containers have strong surface feature information. When the vehicle passes through the container, the surface feature can be stably obtained by the laser radar. The boundary of the yard area and the lane line of the adjacent lane in the port scene are highly parallel, and there is a stable distance between them. The lane line, yard information and other data can be obtained through the high-precision map, and the lane line data and yard boundary information are extracted for correction.

[0004] When the vehicle runs in the yard, the combined navigation signal is poor due to tire hoist shielding, weather and other reasons, which will cause the deviation of the vehicle heading angle and position, and cause collision risk. How to solve the above problems has become a problem that we need to solve. SUMMARY

[0005] In view of the above problems, the present application provides a positioning correction method, system and storage medium in a laser feature degradation environment, which not only solves the problem of collision risk caused by the deviation of the vehicle heading angle and position, but also corrects the positioning according to the optimal surface feature information, and improves the accuracy of the positioning.

[0006] In order to achieve the above purpose and other related purposes, the technical scheme provided by the present application is as follows:

[0007] A positioning correction method in a laser feature degradation environment, comprising:

[0008] U1. When the vehicle travels in the port yard area or the tunnel area, the target area surface feature data information is obtained based on the vehicle head laser radar, the vehicle pose data information is obtained based on the vehicle-mounted combined navigation, the vehicle pose data information is input into the high-precision map, and the vehicle lane center line shape point data information is obtained;

[0009] U2. Traverse the target area surface feature data information, and according to the surface feature validity function P, n is the number of points on each surface feature, M is the environmental feature parameter, h mThe average angle of the face feature projection point is output as the optimal face feature data information of the target region.

[0010] U3. A Frenet coordinate system is established based on the lane center line point data information of the vehicle, the optimal face feature data information of the target region is projected into the Frenet coordinate system, and optimal face feature projection curve data information is output.

[0011] U4. The angle data information between the optimal face feature projection curve data information and the lane center line point data information of the vehicle is obtained, and the optimal deviation angle data information and the vehicle-mounted laser radar coordinate data information are output according to the optimal deviation angle algorithm.

[0012] U5. Based on the optimal deviation angle data information, the vehicle heading angle is corrected, the vehicle coordinate data information is obtained according to the vehicle-mounted laser radar coordinate data information, and the corrected pose data information of the vehicle is output.

[0013] Further, in step U2, the average angle h of the face feature projection point is m ,

[0014]

[0015] Wherein, x is the horizontal coordinate of the face feature projection point, y is the vertical coordinate of the face feature projection point, and n is the number of points on each face feature.

[0016] Further, in step U3, the optimal face feature projection curve data information is a function z=kt+b, where t is the horizontal coordinate of the projection point, z is the vertical coordinate of the projection point, k is the slope, and b is a constant parameter.

[0017] Further, the slope k=tan(h m ), h m is the average angle of the face feature projection point, and h m is not equal to 0.5π, b=p z -kp t , wherein

[0018]

[0019] Further, in step U4, the optimal deviation angle algorithm includes:

[0020] U41. According to the angle data information between the optimal face feature projection curve data information and the lane center line point data information of the vehicle, the cosine value cosθ of the angle between the unit vector in the lane line direction and the unit vector in the face feature projection direction is obtained,

[0021]

[0022] wherein H1 is a unit vector in the direction of the lane line, H p is a unit vector in the direction of the surface feature projection, H1 = (cos(h l ), sin(h l ), 0.0), H p = (cos(h mg ), sin(h mg ), 0.0), h l is the orientation of the lane line, h mg is the orientation of the surface feature projection;

[0023] U42. Based on the cosine value of the included angle cos θ, if |cos θ| is greater than 0.1, it is rejected, if |cos θ| is less than or equal to 0.1, it is retained, to obtain the included angle offset α = arc (cos θ) - 0.5π;

[0024] U43. Based on the included angle offset α, the orientation of the surface feature projection direction is corrected to obtain h f1 = h mg + α, h f2 = h mg - α, and further obtain the cosine value of the angle difference of the lane line

[0025] cos δ1 = cos (hl) * cos (h f1 ) + sin (hl) * sin (h f1 ),

[0026] cos δ2 = cos (hl) * cos (h f2 ) + sin (hl) * sin (h f2 );

[0027] U44. Based on the cosine value of the lane line angle cos δ1 and cos δ2, the optimal deviation angle is selected, if |cos δ1| is less than |cos δ2|, the optimal deviation angle β = α, otherwise β = -α.

[0028] Further, in step U44, the corrected vehicle heading angle h c ', h c ' = h c + β, wherein h c is the original vehicle heading angle.

[0029] Further, the vehicle pose data information includes vehicle heading angle data information and vehicle position coordinate data information.

[0030] Further, the vehicle-mounted laser radar coordinate data information updates the position of the vehicle-mounted laser radar according to the optimal deviation angle data information.

[0031] To achieve the above object and other related objects, the present application also provides a positioning correction system in a laser feature degradation environment, comprising a computer device programmed or configured to perform the steps of any one of the positioning correction methods in a laser feature degradation environment.

[0032] To achieve the above object and other related objects, the present application also provides a computer readable storage medium having stored thereon a computer program programmed or configured to perform any one of the positioning correction methods in a laser feature degradation environment.

[0033] The present application has the following positive effects:

[0034] 1. The present application corrects the pose of the vehicle by the optimal surface feature data information of the target area, which is not only simple in data processing process, but also high in accuracy.

[0035] 2. In the laser feature degradation scene of the port, tunnel, etc., the optimal surface feature is calculated and selected according to the laser radar surface feature information, and the relative heading angle and lateral position are calculated; in the degradation scene, the relative relationship between the surface feature and the elements in the vector map is converted into the relative relationship between the vehicle and the vector map according to the relative relationship between the surface feature and the elements in the vector map; the global positioning result is calculated from the local relative positioning result, which increases the diversity of the fusion positioning source and provides a positioning constraint in the laser feature degradation scene. BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1 The present application is a method flowchart. DETAILED DESCRIPTION

[0037] Exemplary embodiments of the present disclosure are described below with reference to the accompanying drawings, which include various details of the embodiments of the present disclosure to assist in understanding, which should be considered as merely exemplary. Therefore, those of ordinary skill in the art should recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of the present disclosure. Also, for the sake of clarity and conciseness, the description below omits the description of well-known functions and structures.

[0038] Embodiment 1: As shown in a positioning correction method in a laser feature degradation environment, comprising: Figure 1

[0039] U1. When the vehicle is driving in the port yard area or the tunnel area, the target area surface feature data information is obtained based on the vehicle head laser radar, the vehicle pose data information is obtained based on the vehicle-mounted integrated navigation, the vehicle pose data information is input into the high-precision map, and the lane center line point data information of the vehicle is obtained; ​

[0040] U2. Traverse the surface feature data of the target region, and based on the surface feature validity function P, n is the number of points on each surface feature, M is the environmental feature parameter, and h m The average angle between the projection points of the surface features is used to output the optimal surface feature data information of the target region.

[0041] U3. Establish a Frenet coordinate system based on the centerline point data of the lane where the vehicle is located, project the optimal surface feature data of the target area onto the Frenet coordinate system, and output the optimal surface feature projection curve data.

[0042] U4. Obtain the angle data between the optimal surface feature projection curve data and the center line point data of the lane where the vehicle is located, and output the optimal deviation angle data and the vehicle-mounted lidar coordinate data according to the optimal deviation angle algorithm;

[0043] U5. Based on the optimal deviation angle data, the vehicle heading angle is corrected, and the vehicle coordinate data is obtained according to the vehicle-mounted lidar coordinate data, and the corrected vehicle pose data is output.

[0044] In this embodiment, in step U2, the average included angle h of the surface feature projection points m ,

[0045] Where x is the abscissa of the surface feature projection point, y is the ordinate of the surface feature projection point, and n is the number of points on each surface feature.

[0046] In this embodiment, in step U3, the optimal surface feature projection curve data information is the function z = kt + b, where t is the abscissa of the projection point, z is the ordinate of the projection point, k is the slope, and b is a constant parameter.

[0047] In this embodiment, the slope k = tan(h) m ), h m Let h be the average angle between the projection points of the surface feature, and h m Not equal to 0.5π, b = p z -kp t ,in

[0048]

[0049] Example 2: Based on the positioning correction method in the laser feature degradation environment of Example 1, the present invention will be further explained and described below.

[0050] In this embodiment, the optimal deviation angle algorithm includes:

[0051] U41. According to the included angle data information between the optimal surface feature projection curve data information and the lane center line shape point data information where the vehicle is located, the cosine value cosθ of the included angle between the unit vector in the lane line direction and the unit vector in the surface feature projection direction is obtained,

[0052]

[0053] where H1 is the unit vector in the lane line direction, H p is the unit vector in the surface feature projection direction, H1=(cos(h l ), sin(h l ), 0.0), H p =(cos(h mg ), sin(h mg ), 0.0), h l is the orientation of the lane line, and h mg is the orientation of the surface feature projection direction;

[0054] U42. Based on the cosine value cosθ, if |cosθ| is greater than 0.1, it is rejected, and if |cosθ| is less than or equal to 0.1, it is retained, to obtain the included angle offset α=arc(cosθ)-0.5π;

[0055] U43. Based on the included angle offset α, the orientation of the surface feature projection direction is corrected to obtain h f1 =h mg +α, h f2 =h mg -α, and further obtain the cosine value of the angle difference δ1 and δ2 between the lane line and the surface feature projection direction.

[0056] cosδ1=cos(hl)*cos(h f1 )+sin(hl)*sin(h f1 ),

[0057] cosδ2=cos(hl)*cos(h f2 )+sin(hl)*sin(h f2 );

[0058] U44. Based on the cosine values cosδ1 and cosδ2 of the angle difference between the lane line and the surface feature projection direction, the optimal deviation angle is selected. If |cosδ1| is less than |cosδ2|, the optimal deviation angle β=α, otherwise β=-α.

[0059] In this embodiment, in step U44, the corrected vehicle heading angle h c ′, h c ′=h c +β, where h cThe vehicle original heading angle.

[0060] In the embodiment, the vehicle pose data information comprises vehicle heading angle data information and vehicle position coordinate data information.

[0061] In the embodiment, the vehicle-mounted laser radar coordinate data information updates the position of the vehicle-mounted laser radar according to the optimal deviation angle data information.

[0062] In the embodiment, the present application provides a positioning correction system in a laser feature degradation environment, comprising a computer device programmed or configured to perform the steps of any one of the positioning correction methods in a laser feature degradation environment.

[0063] In the embodiment, the present application provides a computer readable storage medium having stored thereon a computer program programmed or configured to perform any one of the positioning correction methods in a laser feature degradation environment.

[0064] Any reference to memory, storage, database, or other medium used in the embodiments provided by the present application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration but not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0065] In summary, the present application not only solves the problem of collision risk caused by deviation of vehicle heading angle and position, but also corrects the positioning according to optimal surface feature information, thereby improving the accuracy of positioning.

[0066] The above specific embodiments do not constitute a limitation on the protection scope of the present disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modification, equivalent replacement, and improvement within the spirit and principles of the present disclosure should be included in the protection scope of the present disclosure.

Claims

1. A method of position correction in a laser feature degradation environment, the method comprising: Comprising: U1. When the vehicle is driving in a port yard area or a tunnel area, obtaining target area surface feature data information based on a vehicle head laser radar, obtaining vehicle pose data information based on a vehicle-mounted combined navigation, inputting the vehicle pose data information into a high-definition map, and obtaining lane center line shape point data information where the vehicle is located; U2. Traversing the target area surface feature data information, and according to a surface feature validity function P, ， n is the number of points on each face feature, M is the environmental feature parameter, h m is the average angle of the face feature projection point, and the optimal face feature data information of the output target area is obtained. U3. Establishing a Frenet coordinate system with the lane center line shape point data information where the vehicle is located, projecting the optimal surface feature data information of the target area to the Frenet coordinate system, and outputting optimal surface feature projection curve data information; U4. Obtaining an included angle data information between the optimal surface feature projection curve data information and the lane center line shape point data information where the vehicle is located, outputting optimal deviation angle data information and vehicle-mounted laser radar coordinate data information according to an optimal deviation angle algorithm; U5. Correcting a vehicle heading angle based on the optimal deviation angle data information, obtaining vehicle coordinate data information according to the vehicle-mounted laser radar coordinate data information, and outputting corrected vehicle pose data information; In step U4, the optimal deviation angle algorithm comprises: U41. Obtaining a cosine value cosθ of an included angle between a unit vector in a lane line direction and a unit vector in a surface feature projection direction according to the included angle data information between the optimal surface feature projection curve data information and the lane center line shape point data information where the vehicle is located, ， where H1is a unit vector in the direction of the lane line, H p is a unit vector in the direction of the face feature projection, H1= (cos(h l ), sin(h l ), 0.0), H p = (cos(h mg ), sin(h mg ), 0.0), h l is the heading of the lane line, h mg is the heading of the face feature projection; U42. Based on the cosine value cosθ, if |cosθ| is greater than 0.1, it is eliminated, and if |cosθ| is less than or equal to 0.1, it is retained, to obtain an included angle offset α = arc(cosθ) - 0.5π; U43. Based on the included angle offset α, the orientation of the face feature projection direction is corrected to obtain h f1 = h mg + α, h f2 = h mg - α, and further obtain the cosine value of the lane line angle difference cosδ1= cos(h l )*cos(h f1 )+sin(h l )*sin(h f1 ), cos δ2= cos (h l )* cos (h f2 )+ sin (h l )* sin (h f2 ); U44. Based on the cosine values cosδ1 and cosδ2 of the lane line angle, the optimal deviation angle is selected, if |cosδ1| is less than |cosδ2|, the optimal deviation angle is β = α, otherwise β = -α.

2. The method of claim 1, wherein, In step U2, the average included angle h of the face feature projection points m , ， Wherein, x is the horizontal coordinate of the surface feature projection point, y is the vertical coordinate of the surface feature projection point, and n is the number of points on each surface feature.

3. The method of claim 1, wherein, In step U3, the optimal surface feature projection curve data information is a function z = kt + b, where t is the horizontal coordinate of the projection point, z is the vertical coordinate of the projection point, k is the slope, and b is the constant parameter.

4. The method of claim 3, wherein: The slope k = tan(h m ), h m is the average angle of the face feature projection point, and h m is not equal to 0.5π, b = p z -kp t , wherein, ， 。 5. The method of claim 1, wherein, In step U44, the corrected vehicle heading angle h c ′, h c ′=h c +β, where h c is the vehicle original heading angle.

6. The method of claim 1, wherein: The vehicle pose data information comprises vehicle heading angle data information and vehicle position coordinate data information.

7. The method of claim 1, wherein: The vehicle-mounted laser radar coordinate data information updates the position of the vehicle-mounted laser radar according to the optimal deviation angle data information.

8. A positioning correction system in a laser feature degradation environment, comprising a computer device, characterized in that, The computer device is programmed or configured to perform the steps of the positioning correction method in the laser feature degradation environment according to any one of claims 1-7.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program programmed or configured to perform the positioning correction method in the laser feature degradation environment according to any one of claims 1-7.

Images