Intersection environment local mileage dead reckoning positioning method and system and storage medium

Abstract

The application relates to a kind of intersection environment local mileage reckoning positioning method, system and storage medium, the method comprises: L1. Vehicle travels on road, obtains road point cloud data information in real time based on vehicle-mounted laser radar, obtains the angular velocity and acceleration data information of vehicle in real time based on vehicle-mounted IMU;L2. Based on the angular velocity and acceleration data information of the vehicle, the acceleration and angular velocity of the vehicle are corrected using ESKF algorithm, and the corrected angular velocity and acceleration data information of the vehicle are output;L3. Based on the road point cloud data information, the pose of the vehicle is estimated using local point cloud map construction and matching algorithm, and the low-frequency relative pose data information of the vehicle is output.The application not only improves the system local odometer reckoning precision, but also makes the automatic driving vehicle simply pass through the relevant intersection scene through local mileage information, to ensure the reliability of automatic driving.

Description

Technical Field

[0001] This invention relates to the field of vehicle positioning technology, and in particular to a method, system, and storage medium for local mileage estimation and positioning at intersections. Background Technology

[0002] With the booming development of world trade and the unstoppable trend of globalization, maritime trade, as a crucial component of global trade, is a vital means for countries to participate in international trade. Port terminals serve as the connecting point between maritime and land transportation, and their importance in trade transportation is self-evident. Therefore, improving the efficiency of container transshipment, reducing labor costs and operational risks, and thereby enhancing port competitiveness, has always been a goal pursued by ports worldwide.

[0003] Whether using GNSS or INS positioning, errors are unavoidable in autonomous driving positioning systems, and the positioning results often deviate from the actual location. In autonomous driving systems, a global positioning system is used across all scenarios. Occasional positioning anomalies can lead to malfunctions in the autonomous driving system. Global positioning struggles to guarantee accuracy in all scenarios. In intersection environments, relevant visual features are distant and few in number, making it difficult to construct local semantic maps. Furthermore, if RTK (Real-Time Kerneling) navigation fails due to network or other inherent problems, it is difficult to guarantee that the autonomous vehicle can navigate intersections using global positioning. Summary of the Invention

[0004] In view of the above problems, the present invention provides a method, system and storage medium for local mileage estimation and positioning in intersection environment, which not only improves the accuracy of local mileage estimation in the system, but also enables autonomous vehicles to pass through relevant intersection scenarios simply by relying on local mileage information, thus ensuring the reliability of autonomous driving.

[0005] To achieve the above and other related objectives, the present invention provides the following technical solution: A method for local mileage estimation and positioning at intersections, the method comprising: L1. When the vehicle is driving on the road, it acquires road point cloud data information in real time based on the vehicle-mounted lidar and acquires vehicle angular velocity and acceleration data information in real time based on the vehicle-mounted IMU. L2. Based on the angular velocity and acceleration data of the vehicle, the ESKF algorithm is used to correct the acceleration and angular velocity of the vehicle, and the corrected angular velocity and acceleration data of the vehicle are output. L3. Based on the road point cloud data information, the local point cloud map construction and matching algorithm is used to estimate the vehicle's pose and output the vehicle's low-frequency relative pose data information. L4 fuses the low-frequency relative pose data of the vehicle with the corrected angular velocity and acceleration data, performs local odometer calculation, and outputs the relative pose and velocity data of the vehicle.

[0006] Furthermore, in step L2, the correction of the vehicle's acceleration and angular velocity using the ESKF algorithm includes: L21. Based on the vehicle's angular velocity and acceleration data, establish the vehicle's state function. , in, , , R is the rotation matrix, I is the transition matrix, and a m Let a be the acceleration at time m. b Let a be the acceleration at time b. τ Let W be the acceleration at time τ. τ Let a be the angular velocity at time τ. n Let W be the acceleration at time n. n Let a be the angular velocity at time n. w Let W be the acceleration at time w. w Let ω be the angular velocity at time w, and x be the vehicle's state matrix. L22. Based on the vehicle's state function Establish a recursive function for the vehicle state. , Where Δt is the time interval; L23. Recursive function based on the vehicle state It predicts the vehicle's state data for the next moment, corrects the vehicle's angular velocity and acceleration data, and outputs the corrected vehicle angular velocity and acceleration data.

[0007] Furthermore, in step L21, the vehicle's state matrix x, , Where p is position, v is velocity, R is rotation matrix, a is acceleration, and w is velocity. b Let be the angular velocity at time b.

[0008] Furthermore, in step L3, the estimation of the vehicle's pose using a local point cloud map construction and matching algorithm includes: L31. Based on the road point cloud data information, downsample the road point cloud data information of the current frame and output the sampled point cloud data information; L32. Based on the sampled point cloud data, find the associated points in the Ivoxel Map, perform plane fitting, calculate the distance and normal vector between the associated points and the fitted plane, and output the distance data between the associated points and the fitted plane. L33. Based on the distance data between the associated points and the fitted plane, the associated points are filtered using the least squares algorithm with point-plane constraints, and the filtered associated point data is output. L34. Insert the filtered associated point data information into the Ivoxel Map, estimate the vehicle's pose, and output the vehicle's low-frequency relative pose data information.

[0009] Furthermore, in step L34, the vehicle pose is estimated and inserted into the Ivoxel Map based on the filtered associated point data information to construct a sliding window submap map, thereby obtaining local vehicle position data information and the angle data information between the local vehicle body and the submap map normal vector.

[0010] Furthermore, the vehicle's heading angle data is obtained based on the angle data between the local vehicle body and the submap map normal vector.

[0011] Furthermore, the vehicle's pose is the vehicle's position and heading angle data.

[0012] Furthermore, the Ivoxel Map can dynamically grow or delete points.

[0013] To achieve the above and other related objectives, the present invention also provides a local mileage estimation and positioning system for intersection environments, including a computer device programmed or configured to perform the steps of any of the local mileage estimation and positioning methods for intersection environments described herein.

[0014] To achieve the above and other related objectives, the present invention also provides a computer-readable storage medium storing a computer program programmed or configured to perform any of the intersection environment local mileage estimation and positioning methods described herein.

[0015] The present invention has the following positive effects: 1. This invention corrects the data information provided by the vehicle-mounted IMU through the ESKF algorithm, providing accurate data support for local estimation and positioning, which not only improves the output frequency of the system, but also increases the robustness of the system.

[0016] 2. This invention estimates the vehicle's pose by constructing and matching local point cloud maps, providing more accurate data for subsequent fusion positioning. This not only improves the accuracy of local mileage estimation but also reduces the reliance on global positioning such as integrated navigation in intersection scenarios. By guiding the vehicle through intersections through local mileage estimation, the robustness of autonomous vehicles is improved. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of the method flow of the present invention. Detailed Implementation

[0018] The exemplary embodiments of this disclosure are described below with reference to the accompanying drawings, including various details of the embodiments to aid understanding, and should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of this disclosure. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0019] Example 1: As Figure 1 As shown, a method for local mileage estimation and positioning at intersections is provided, the method comprising: L1. When the vehicle is driving on the road, it acquires road point cloud data information in real time based on the vehicle-mounted lidar and acquires vehicle angular velocity and acceleration data information in real time based on the vehicle-mounted IMU. L2. Based on the angular velocity and acceleration data of the vehicle, the ESKF algorithm is used to correct the acceleration and angular velocity of the vehicle, and the corrected angular velocity and acceleration data of the vehicle are output. L3. Based on the road point cloud data information, the local point cloud map construction and matching algorithm is used to estimate the vehicle's pose and output the vehicle's low-frequency relative pose data information. L4 fuses the low-frequency relative pose data of the vehicle with the corrected angular velocity and acceleration data, performs local odometer calculation, and outputs the relative pose and velocity data of the vehicle.

[0020] In this embodiment, step L2, correcting the vehicle's acceleration and angular velocity using the ESKF algorithm, includes: L21. Based on the vehicle's angular velocity and acceleration data, establish the vehicle's state function. , in, , , R is the rotation matrix, I is the transition matrix, and a mLet a be the acceleration at time m. b Let a be the acceleration at time b. τ Let W be the acceleration at time τ. τ Let a be the angular velocity at time τ. n Let W be the acceleration at time n. n Let a be the angular velocity at time n. w Let W be the acceleration at time w. w Let ω be the angular velocity at time w, and x be the vehicle's state matrix. L22. Based on the vehicle's state function Establish a recursive function for the vehicle state. , Where Δt is the time interval; L23. Recursive function based on the vehicle state It predicts the vehicle's state data for the next moment, corrects the vehicle's angular velocity and acceleration data, and outputs the corrected vehicle angular velocity and acceleration data.

[0021] In this embodiment, in step L21, the vehicle's state matrix x, , Where p is position, v is velocity, R is rotation matrix, a is acceleration, and w is velocity. b Let be the angular velocity at time b.

[0022] Example 2: Based on the local mileage estimation and positioning method for intersection environment in Example 1, the present invention will be further explained and described below.

[0023] like Figure 1 As shown, a method for local mileage estimation and positioning at intersections is provided, the method comprising: L1. When the vehicle is driving on the road, it acquires road point cloud data information in real time based on the vehicle-mounted lidar and acquires vehicle angular velocity and acceleration data information in real time based on the vehicle-mounted IMU. L2. Based on the angular velocity and acceleration data of the vehicle, the ESKF algorithm is used to correct the acceleration and angular velocity of the vehicle, and the corrected angular velocity and acceleration data of the vehicle are output. L3. Based on the road point cloud data information, the local point cloud map construction and matching algorithm is used to estimate the vehicle's pose and output the vehicle's low-frequency relative pose data information. L4 fuses the low-frequency relative pose data of the vehicle with the corrected angular velocity and acceleration data, performs local odometer calculation, and outputs the relative pose and velocity data of the vehicle.

[0024] In this embodiment, step L3, which involves estimating the vehicle's pose using a local point cloud map construction and matching algorithm, includes: L31. Based on the road point cloud data information, downsample the road point cloud data information of the current frame and output the sampled point cloud data information; L32. Based on the sampled point cloud data, find the associated points in the Ivoxel Map, perform plane fitting, calculate the distance and normal vector between the associated points and the fitted plane, and output the distance data between the associated points and the fitted plane. L33. Based on the distance data between the associated points and the fitted plane, the associated points are filtered using the least squares algorithm with point-plane constraints, and the filtered associated point data is output. L34. Insert the filtered associated point data information into the Ivoxel Map, estimate the vehicle's pose, and output the vehicle's low-frequency relative pose data information.

[0025] In this embodiment, in step L34, the vehicle pose is estimated by inserting an Ivoxel Map based on the filtered associated point data information, constructing a sliding window submap map, and obtaining local vehicle position data information and the angle data information between the local vehicle body and the submap map normal vector.

[0026] In this embodiment, the vehicle's heading angle data is obtained based on the angle data between the local vehicle body and the submap map normal vector.

[0027] In this embodiment, the vehicle's pose refers to the vehicle's position and heading angle data.

[0028] In this embodiment, the Ivoxel Map can dynamically add or delete points.

[0029] The present invention provides a local mileage estimation and positioning system for intersection environment, including a computer device, which is programmed or configured to perform the steps of any of the local mileage estimation and positioning methods for intersection environment as described above.

[0030] To achieve the above and other related objectives, the present invention also provides a computer-readable storage medium storing a computer program programmed or configured to perform any of the intersection environment local mileage estimation and positioning methods described herein.

[0031] Any references to memory, storage, database, or other media used in the embodiments provided in this application may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0032] In summary, this invention not only improves the accuracy of local odometer calculation in the system, but also enables autonomous vehicles to pass through relevant intersection scenarios solely based on local mileage information, thus ensuring the reliability of autonomous driving.

[0033] The specific embodiments described above do not constitute a limitation on the scope of protection of this 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 modifications, equivalent substitutions, and improvements made within the spirit and principles of this disclosure should be included within the scope of protection of this disclosure.

Claims

1. A method for local mileage estimation and positioning at intersections, characterized in that, The method includes: L1. When the vehicle is driving on the road, it acquires road point cloud data information in real time based on the vehicle-mounted lidar and acquires vehicle angular velocity and acceleration data information in real time based on the vehicle-mounted IMU. L2. Based on the angular velocity and acceleration data of the vehicle, the ESKF algorithm is used to correct the acceleration and angular velocity of the vehicle, and the corrected angular velocity and acceleration data of the vehicle are output. L3. Based on the road point cloud data information, the local point cloud map construction and matching algorithm is used to estimate the vehicle's pose and output the vehicle's low-frequency relative pose data information. L4. The low-frequency relative pose data of the vehicle and the corrected angular velocity and acceleration data of the vehicle are fused together to perform local odometer calculation and output the relative pose and velocity data of the vehicle. In step L2, the correction of the vehicle's acceleration and angular velocity using the ESKF algorithm includes: L21. Based on the vehicle's angular velocity and acceleration data, establish the vehicle's state function. ， in, , ， ， R is the rotation matrix, I is the transition matrix, and a m Let a be the acceleration at time m. b Let a be the acceleration at time b. τ Let W be the acceleration at time τ. τ Let a be the angular velocity at time τ. n Let W be the acceleration at time n. n Let a be the angular velocity at time n. w Let W be the acceleration at time w. w Let ω be the angular velocity at time w, and x be the vehicle's state matrix. L22. Based on the vehicle's state function Establish a recursive function for the vehicle state. ， Where Δt is the time interval; L23. Recursive function based on the vehicle state It predicts the vehicle state data for the next moment, corrects the vehicle's angular velocity and acceleration data, and outputs the corrected vehicle angular velocity and acceleration data. In step L21, the state matrix x of the vehicle, ， Where p is position, v is velocity, R is rotation matrix, a is acceleration, and w is velocity. b Let be the angular velocity at time b.

2. The method for local mileage estimation and positioning at intersections according to claim 1, characterized in that, In step L3, the estimation of the vehicle's pose using a local point cloud map construction and matching algorithm includes: L31. Based on the road point cloud data information, downsample the road point cloud data information of the current frame and output the sampled point cloud data information; L32. Based on the sampled point cloud data, find the associated points in the Ivoxel Map, perform plane fitting, calculate the distance and normal vector between the associated points and the fitted plane, and output the distance data between the associated points and the fitted plane. L33. Based on the distance data between the associated points and the fitted plane, the associated points are filtered using the least squares algorithm with point-plane constraints, and the filtered associated point data is output. L34. Insert the filtered associated point data information into the Ivoxel Map, estimate the vehicle's pose, and output the vehicle's low-frequency relative pose data information.

3. The method for local mileage estimation and positioning at intersections according to claim 2, characterized in that, In step L34, the vehicle pose is estimated and Ivoxel Map is inserted based on the filtered associated point data information to construct a sliding window submap map, thereby obtaining local vehicle position data information and the angle data information between the local vehicle body and the submap map normal vector.

4. The method for local mileage estimation and positioning at intersections according to claim 3, characterized in that: The heading angle data of the vehicle is obtained based on the angle between the local vehicle body and the normal vector of the submap.

5. The method for local mileage estimation and positioning at intersections according to claim 2, characterized in that: The vehicle's pose refers to its position and heading angle data.

6. The method for local mileage estimation and positioning at intersections according to claim 2, characterized in that: The IvoxelMap can dynamically grow or delete points.

7. A local mileage estimation and positioning system for intersection environments, comprising computer equipment, characterized in that, The computer device is programmed or configured to perform the steps of the local mileage estimation and positioning method for intersection environment as described in any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that is programmed or configured to perform the local mileage estimation and positioning method for intersection environments as described in any one of claims 1 to 6.

Images