Vehicle laser positioning method and device

Abstract

The invention provides a vehicle laser positioning method and device, and the method comprises the steps: determining a rotation calibration value according to first RTK data in an initialization stage, and carrying out the first alignment operation of the initialization stage on a laser odometer based on the rotation calibration value; performing rigid body registration alignment on the RTK track and the laser track in an operation stage after initialization is completed, determining a rotation and translation calibration amount, and performing second alignment operation on the laser speedometer based on the rotation and translation calibration amount; after the second alignment operation, determining a twiddle factor according to positioning data output by the laser speedometer and third RTK data of the vehicle in the world coordinate system, and constructing a factor graph based on the twiddle factor; and optimizing the laser speedometer after the second alignment operation according to the factor graph to obtain an optimized laser speedometer, and carrying out vehicle laser positioning based on the optimized laser speedometer. Through the mode, high-precision and globally consistent positioning capability can be realized under a global coordinate system based on the laser speedometer without depending on a prior high-precision map, and the method has higher environmental adaptability and engineering application value.

Description

Technical Field

[0001] This application relates to the field of vehicle technology, and in particular to a vehicle laser positioning method and device. Background Technology

[0002] Existing vehicle localization methods mainly fall into two categories. One is the NDT (Normal Distributions Transform) localization scheme based on high-precision maps, which is a widely used method in the current field of autonomous driving. It estimates the vehicle's pose in the map by matching real-time acquired laser point clouds with a pre-built high-precision map, achieving high-precision localization in structurally stable environments such as urban roads and unobstructed outdoor environments. However, although high-precision map localization has high accuracy, it is highly dependent on high-precision maps, has high construction and maintenance costs, and poor adaptability, which limits its large-scale deployment and application in complex and variable environments.

[0003] Another type is the odometry method based on laser fusion, which can achieve high-precision real-time position estimation within a local coordinate system. However, as the system operates for longer periods, accumulated errors inevitably occur, causing positioning drift. Furthermore, since pose data in a local coordinate system lacks global consistency, it needs to be projected onto a global coordinate system through coordinate transformation to meet the practical requirements of vehicle positioning and navigation systems. However, ground vehicles lack significant attitude excitation during operation, making it difficult to accurately calibrate spatial extrinsic parameters (pose relationships), thus limiting alignment accuracy. More importantly, over time, the accumulation of nonlinear errors during system operation will continuously widen the deviation from the actual trajectory, affecting the global consistency and stability of the final positioning. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a vehicle laser positioning method and apparatus to solve the problems that existing vehicle positioning methods based on NDT are highly dependent on high-precision maps and have high maintenance costs, and that high-precision local positioning methods based on laser odometry are difficult to use directly for vehicle navigation.

[0005] This application provides a vehicle laser positioning method, the method comprising: During the initialization phase, the first RTK data in the world coordinate system is acquired when the vehicle is stationary. The rotation calibration amount is determined based on the first RTK data, and the first alignment operation of the laser odometer in the initialization phase is performed based on the rotation calibration amount. During the running phase after initialization, the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometry after the first alignment operation are acquired respectively, and the RTK trajectory and laser trajectory are formed. Rigid body registration and alignment are performed on the RTK trajectory and the laser trajectory to determine the rotation and translation calibration amount, and a second alignment operation is performed on the laser odometer based on the rotation and translation calibration amount; After the second alignment operation, the rotation factor is determined based on the positioning data output by the laser odometry and the third RTK data of the vehicle in the world coordinate system, and a factor map is constructed based on the rotation factor. The laser odometer after the second alignment operation is optimized based on the factor graph to obtain an optimized laser odometer for vehicle laser positioning.

[0006] This application embodiment also provides a vehicle laser positioning device, the device comprising: The acquisition module is used to acquire the first RTK data in the world coordinate system when the vehicle is stationary during the initialization phase. The first alignment module is used to determine the rotation calibration amount based on the first RTK data, and to perform the first alignment operation of the laser odometer in the initialization phase based on the rotation calibration amount. The acquisition module is also used to acquire the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometer after the first alignment operation during the running phase after initialization, and form the RTK trajectory and the laser trajectory. The second alignment module is used to perform rigid body registration and alignment of the RTK trajectory and the laser trajectory, determine the rotation and translation calibration amount, and perform a second alignment operation on the laser odometer based on the rotation and translation calibration amount. The determination module is used to determine the rotation factor based on the positioning data output by the laser odometer and the third RTK data of the vehicle in the world coordinate system after the second alignment operation, and to construct a factor map based on the rotation factor. An optimization module is used to optimize the laser odometer after the second alignment operation according to the factor graph to obtain an optimized laser odometer for vehicle laser positioning.

[0007] This application also provides an electronic device, including: a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the electronic device is running, the processor communicates with the memory via the bus. When the machine-readable instructions are executed by the processor, the steps of the vehicle laser positioning method described above are performed.

[0008] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, performs the steps of the vehicle laser positioning method described above.

[0009] This application provides a vehicle laser positioning method and apparatus. In the initialization phase, a first alignment operation is performed, providing coarse alignment through single-point alignment to achieve coordinate angle consistency. In the operational phase, a second alignment operation is performed, providing fine alignment through trajectory alignment to achieve coordinate system consistency alignment, thus achieving multi-stage alignment from coarse to fine. Furthermore, a factor graph optimization framework is introduced in the operational phase, incorporating rotation factors to further reduce the cumulative error of the laser odometer. In this way, high-precision, globally consistent positioning capabilities can be achieved in a global coordinate system based on the laser odometer without relying on prior high-precision maps, exhibiting stronger environmental adaptability and engineering application value.

[0010] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0011] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0012] Figure 1 A flowchart of a vehicle laser positioning method provided in an embodiment of this application is shown; Figure 2 This illustration shows a schematic diagram of an input source for a GTSAM optimizer provided in an embodiment of this application; Figure 3 This paper shows a schematic diagram of the structure of a vehicle laser positioning device provided in an embodiment of this application; Figure 4 A schematic diagram of the structure of an electronic device provided in an embodiment of this application is shown. Detailed Implementation

[0013] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. Based on the embodiments of this application, every other embodiment obtained by those skilled in the art without inventive effort falls within the scope of protection of this application.

[0014] Research has found that existing vehicle positioning methods mainly fall into two categories. One is the NDT (Normal Distributions Transform) positioning scheme based on high-precision maps, which is a widely used method in the current field of autonomous driving. It estimates the vehicle's pose in the map by matching real-time collected laser point clouds with pre-built high-precision maps, and can achieve high-precision positioning in stable outdoor environments such as urban roads. However, although high-precision map positioning has high accuracy, it has several limitations: (1) High dependence on high-precision maps: Map construction requires the use of high-precision sensors (such as 3D laser scanners) to collect, map and process data in the target area. The process is complex, time-consuming and costly, and the accuracy depends on the technical capabilities of the third-party teams. (2) High maintenance costs: Maps need to be updated regularly with environmental changes (such as construction, road reconstruction, seasonal occlusion), otherwise the positioning effect will be directly affected, and the system maintenance cost will be high. (3) Poor versatility and scalability: For non-fixed routes, temporary work areas or indoor environments, it is impossible to build maps in advance, or the mapping work is difficult to implement, which limits the promotion of this scheme in scenarios such as industrial vehicles and security robots. (4) Long deployment cycle: Every time a new scene or project is added, high-precision map collection and calibration must be carried out again, which lacks flexibility.

[0015] Another type is the odometry method based on laser fusion, which can achieve high-precision real-time position estimation within a local coordinate system. However, as the system operates for longer periods, accumulated errors inevitably occur, causing positioning drift. Furthermore, since pose data in a local coordinate system lacks global consistency, it needs to be projected onto a global coordinate system through coordinate transformation to meet the practical requirements of vehicle positioning and navigation systems. However, ground vehicles lack significant roll and pitch attitude excitations during operation, making it difficult to accurately calibrate spatial extrinsic parameters (pose relationships) and limiting alignment accuracy. More importantly, over time, the accumulation of nonlinear errors during system operation will continuously widen the deviation from the actual trajectory, affecting the global consistency and stability of the final positioning.

[0016] Based on this, the present application provides a vehicle laser positioning method and apparatus to solve the problems that existing vehicle positioning methods based on NDT are highly dependent on high-precision maps and have high maintenance costs, and that high-precision local positioning methods based on laser odometry are difficult to use directly for vehicle navigation.

[0017] In one example, this application provides a vehicle laser positioning method based on Fast-LIO2 laser odometry. Fast-LIO2 is a high-efficiency, tightly coupled lidar-inertial navigation system that uses an Error State Extended Kalman Filter (ESEKF) as the core of state estimation and fuses laser point cloud and IMU data to achieve real-time odometry. This system constructs observations through inter-frame feature matching (such as corner points and planar points) and combines IMU pre-integration prediction to achieve high-frequency updates of position and attitude. It features low latency, high accuracy, and strong anti-drift capability, and is widely used in local positioning tasks in the fields of autonomous driving and robotics.

[0018] In practical implementation, the Fast-LIO2 laser odometry receives radar point cloud and raw IMU data and synchronizes them in time. It calculates residuals through IMU pre-integration and generates point cloud residuals by matching the point cloud with a local map. Using an error-state-based extended Kalman filter (ESEKF), the residuals are fused for state updates, allowing for real-time estimation of high-frequency local laser odometry results.

[0019] Please see Figure 1 , Figure 1 This is a flowchart illustrating a vehicle laser positioning method provided in an embodiment of this application. Figure 1 As shown in the illustration, an embodiment of this application provides a vehicle laser positioning method, comprising: S101. During the initialization phase, acquire the first RTK data in the world coordinate system when the vehicle is stationary.

[0020] When the algorithm initializes and starts, the vehicle is stationary, and 1-2 seconds of valid data are acquired. The local odometer data frequency is 10 Hz, and the RTK data frequency is 50 Hz. Typically, only 1-2 seconds are needed to average the data, ensuring the stability of the initial data alignment and completing the algorithm initialization.

[0021] S102. Determine the rotation calibration amount based on the first RTK data, and perform the first alignment operation of the initialization phase on the laser odometer based on the rotation calibration amount.

[0022] In one possible implementation, the method for determining the rotation calibration amount in step S102 may include: The RTK pose in the first RTK data is converted into a rotation matrix, and the average rotation matrix is ​​determined. The average rotation matrix is ​​then projected using SVD to obtain the mean azimuth angle as the rotation calibration value.

[0023] During the initialization phase, the initial data output by the local laser odometry is set to the default value. At this point, the RTK pose in the RTK data subscribed to in the world coordinate system (e.g., the MGRS coordinate system) is the quaternion. ,in The virtual part, (Represents the real part). According to the projection parameters, MGRS is a gridded representation based on the UTM (Universal Transverse Mercator) coordinate system. Its local coordinate origin is located at the lower left corner of the grid. The X-axis (Eastings) is along the due east direction, and the value increases from left to right; the Y-axis (Northings) is along the due north direction, and the value increases from bottom to top.

[0024] Although RTK can achieve centimeter-level positioning accuracy, it still requires the device to remain stationary during initialization to acquire its RTK pose over a period of time. First, each quaternion... Transform it into a 3x3 rotation matrix, sum all the rotation matrices and take the average. The average rotation matrix obtained at this time is not necessarily strictly orthogonal, so perform singular value decomposition (SVD).

[0025] The formula is expressed as follows: Assuming M is the average rotation matrix, , Orthogonal matrix Diagonal matrix, singular values Orthogonal matrix.

[0026] Construct the nearest effective rotation matrix:

[0027] Then, the yaw angle (which is the rotation about the Z-axis) is extracted from the nearest effective rotation matrix using Euler angle decomposition (ZYX) to obtain the mean azimuth angle.

[0028] In one possible implementation, the first alignment operation of the initialization phase on the laser odometer based on the rotation calibration amount in step S102 may include: Based on the rotation calibration, the coordinates of the local laser odometry are converted to be consistent with the world coordinate system where the RTK data is located.

[0029] In practice, local laser odometry and RTK data are acquired during the initialization phase. The initial yaw angle of the laser odometry is 0 by default, while the yaw angle of the RTK is determined by the orientation of the MGRS coordinate axis as described above. This difference in yaw angle is the projection angle for the first alignment operation. For example, if the initial yaw angle of the laser odometry is 0 degrees, and the yaw angle of the RTK data in the world coordinate system is 60 degrees, then (60-0) degrees is the rotation calibration amount for the first alignment operation of the laser odometry to the world coordinate system. Using this rotation calibration amount as the coordinate projection relationship for coarse alignment, the local odometry coordinates are converted to an orientation consistent with the world coordinate system of the RTK using a single-point alignment method, thus completing the rotation correction of the local odometry.

[0030] S103. During the running phase after initialization, the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometer after the first alignment operation are acquired respectively, and the RTK trajectory and the laser trajectory are formed.

[0031] After initialization is complete, the vehicle starts running and enters the operation phase. In one possible implementation, step S103 may include: During the operation phase, after the first alignment is completed, the positioning data of the local laser odometry and the second RTK data after the first alignment are continuously stored in the queue until a preset running time or a preset driving distance is met; for example, the running time exceeds 20 seconds or the driving distance exceeds 20 meters. The intersection of the RTK trajectory points and laser trajectory points in the queue is calculated according to the timestamps to form the RTK trajectory and the laser trajectory.

[0032] Here, if the trajectory does not move significantly, alignment is not performed to prevent excessive alignment noise; the time interval at which the two sets of trajectories intersect is taken to ensure the reasonableness of the alignment timestamp.

[0033] Furthermore, step S103 may also include: If the length difference between the RTK trajectory and the laser trajectory is greater than a first preset threshold, then the longer trajectory is downsampled; if the length difference between the RTK trajectory and the laser trajectory is still greater than a second preset threshold after downsampling, then each RTK trajectory point in the RTK trajectory and each laser trajectory point in the laser trajectory are matched one by one according to the timestamp; the successfully matched RTK trajectory points and laser trajectory points are retained in the queue to form the RTK trajectory and the laser trajectory.

[0034] Here, if the length of one trajectory is significantly greater than that of another, the longer trajectory is downsampled to reduce redundancy and avoid matching errors; if the difference in trajectory length after sampling is large, such as still >10%, then time matching is performed on the two trajectory points to require a one-to-one correspondence. For each point, the point with the closest time within 0.1 seconds of the maximum difference is matched.

[0035] S104. Perform rigid body registration and alignment on the RTK trajectory and the laser trajectory, determine the rotation and translation calibration amount, and perform a second alignment operation on the laser odometer based on the rotation and translation calibration amount.

[0036] In this step, the RTK trajectory and the laser trajectory are paired and rigid body alignment is performed using the Umeyama algorithm. The yaw angle is extracted as the rotation and translation calibration amount based on the consistency alignment transformation matrix to complete the second alignment operation.

[0037] In practice, the trajectory points in the RTK trajectory and the laser trajectory, along with their respective timestamps, are stored in queues of the Umeyama module. For the trajectory points in the queues, sets of source points (odom) and target points (rtk) are constructed respectively. The two sets of paired trajectory points are rigidly aligned using the Umeyama algorithm. After convergence, a consistent alignment transformation matrix is ​​returned, and the yaw angle is extracted as a rotation calibration value to correct the heading deviation between the odometry and RTK, thus obtaining a global laser odometry.

[0038] Here, trajectory alignment is used for alignment to determine the yaw angle, and a second alignment operation is performed on the laser odometer after the first alignment operation based on the yaw angle. That is, trajectory alignment minimizes the overall error, improves robustness, and achieves global optimization.

[0039] Considering that laser odometry is essentially a local estimation system, while RTK is an asynchronous and independent absolute positioning source, the two still suffer from differences in frequency and attitude estimation methods. Therefore, slight time and attitude deviations will still occur between the global laser odometry trajectory and the RTK trajectory in actual operation, especially noticeable in environments such as turning, acceleration / deceleration, and slopes. Therefore, this application's embodiments further optimize the laser odometry.

[0040] S104. After the second alignment operation, determine the rotation factor based on the positioning data output by the laser odometry and the third RTK data of the vehicle in the world coordinate system, and construct a factor map based on the rotation factor.

[0041] In one possible implementation, GlobalFusion is used to align real-time third RTK data and positioning data output by the laser odometry after the second alignment operation in time, and to extract the local laser odometry pose of the third RTK data and the second alignment operation in attitude; rotation matrices are extracted for the attitude points after synchronous alignment, rotation error matrices are calculated, and rotation factors are obtained.

[0042] GlobalFusion, as a post-processing module after fine alignment, aligns the global odometry and RTK trajectory in time, extracts the differences between the two in terms of attitude, and transforms them into rotation factors for GTSAM graph optimization.

[0043] The two trajectories are synchronized using linear interpolation based on timestamps, ensuring a one-to-one correspondence between the two trajectories on the same time axis, matching a set of strictly synchronized trajectory points. Given in The Slerp formula for spherical linear interpolation is used between them. Interpolation is performed to ensure that the interpolated quaternion maintains the unit norm and follows the shortest path rotation, resulting in the synchronized trajectory.

[0044] Given a pair of synchronized attitudes For each pair of synchronized trajectory points, extract their respective rotation matrices and calculate the rotation error matrix. and error vector , where Log is the logarithmic mapping from SO3 to so3, representing the error direction and angle on the minimum rotation path. The yaw angle of the rotation error matrix is ​​used as a rotation factor to correct the azimuth angle of the laser odometry to the RTK coordinate system. It can also be combined with a noise model and added to the factor graph, enabling the system to optimize the odometry's attitude variables, gradually aligning them with the absolute direction provided by RTK, thus improving the stability and consistency of the orientation estimation.

[0045] Furthermore, the basic factors in the factor graph can be further enriched. Step S104, which involves constructing the factor graph based on the rotation factors, may further include: The inter-frame LIO factor is determined based on local laser odometry; the absolute pose GPS factor is determined based on the pose in real-time RTK data; the inter-frame IMU factor is determined based on the IMU through pre-integration; and a factor graph is constructed based on the rotation factor, the inter-frame LIO factor, the GPS factor, and the inter-frame IMU factor.

[0046] Among them, the rotation factor can be determined based on the first and second alignment operations, the inter-frame LIO factor can be provided by the local laser odometry, the GPS factor of the absolute pose can be provided by the real-time RTK pose, and the inter-frame IMU factor can be provided by the IMU through pre-integration. The above four basic factors together construct the factor map.

[0047] Furthermore, step S104, which involves constructing a factor graph based on the rotation factor, further includes: Select at least one scalability factor; construct a GTSAM factor map based on the rotation factor and the at least one scalability factor. The scalability factors include: a closed-loop factor constructed from pose recognition closed-loop data, a wheel speed factor constructed from wheel speed meter data, and an inter-frame factor constructed from visual odometry.

[0048] Please see Figure 2 , Figure 2 This is a schematic diagram of an input source for a GTSAM optimizer provided in an embodiment of this application. Figure 2 As shown, the GTSAM optimizer introduces a series of expansion factors and basic correction factors. These include: rotation (error) factor, inter-frame LIO factor, GPS factor and inter-frame IMU factor, closed-loop factor, wheel speed factor, and VIO / VO inter-frame factor.

[0049] Specifically, when a vehicle returns to a visited area while driving, radar closed-loop detection (such as Scan Context + ICP) is used to identify the closed-loop factor constructed by the pose closed loop; low-frequency, low-drift wheel speed meter data is fused to construct wheel speed factor to supplement inter-frame odom or short-term speed constraints; in scenarios where laser odometry is prone to degradation, VIO or VO is used to construct inter-frame factors as a redundancy supplement.

[0050] In this way, by introducing a factor graph optimization framework based on GTSAM, multi-source information such as rotation error, RTK, wheel speed measurement, and VIO / VO is integrated as redundant constraints to optimize the system state and further reduce the cumulative error of the laser odometry.

[0051] S105. Optimize the laser odometer after the second alignment operation according to the factor graph to obtain the optimized laser odometer, and perform vehicle laser positioning based on the optimized laser odometer.

[0052] Based on the previous steps, odometry, RTK, and other data have been unified into the MGRS coordinate system. Then, various factors from the factor graph are used to optimize the laser odometry, resulting in a global laser odometry and enabling global positioning. For example, a rotation factor constructed by GlobalFusion corrects the errors in Fast-LIO2 attitude estimation (especially yaw), ensuring consistency with the absolute orientation provided by RTK; an inter-frame factor is constructed using the relative pose changes between consecutive global odom frames to constrain the physical rationality of the estimation and the smoothness of the trajectory; and an absolute position in the MGRS coordinate system is introduced to construct a GPS factor, preventing drift and providing global anchoring.

[0053] By using factor graph constraints, the laser odometer after the second alignment operation is further optimized, reducing the cumulative error of the laser odometer during long-term operation. Based on the optimized laser odometer, vehicle laser positioning is performed to obtain high-precision positioning results.

[0054] This application provides a vehicle laser positioning method that performs a first alignment operation during the initialization phase, providing coarse alignment through single-point alignment to achieve coordinate angle consistency. During the runtime phase, a second alignment operation is performed, providing fine alignment through trajectory alignment to achieve coordinate system consistency alignment. This achieves multi-stage alignment from coarse to fine. The single-point alignment in the first phase prevents getting trapped in local minima, while the trajectory alignment in the second phase provides a globally optimal solution, avoiding the impact of single-point errors. Furthermore, a factor graph optimization framework is introduced during the runtime phase, incorporating rotation factors to further reduce the cumulative error of the laser odometry. In this way, high-precision, globally consistent positioning capabilities can be achieved in a global coordinate system based on laser odometry without relying on prior high-precision maps, exhibiting stronger environmental adaptability and engineering application value.

[0055] Please see Figure 3 , Figure 3 This is a schematic diagram of the structure of a vehicle laser positioning device provided in an embodiment of this application. Figure 3 As shown, the vehicle laser positioning device 300 includes: The acquisition module 310 is used to acquire the first RTK data in the world coordinate system when the vehicle is stationary during the initialization phase. The first alignment module 320 is used to determine the rotation calibration amount based on the first RTK data, and to perform the first alignment operation of the laser odometer in the initialization phase based on the rotation calibration amount. The acquisition module 310 is also used to acquire the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometer after the first alignment operation during the running phase after initialization, and form the RTK trajectory and the laser trajectory. The second alignment module 330 is used to perform rigid body registration and alignment of the RTK trajectory and the laser trajectory, determine the rotation and translation calibration amount, and perform a second alignment operation on the laser odometer based on the rotation and translation calibration amount. The determination module 340 is used to determine the rotation factor based on the positioning data output by the laser odometer and the third RTK data of the vehicle in the world coordinate system after the second alignment operation, and to construct a factor map based on the rotation factor. The optimization module 350 is used to optimize the laser odometer after the second alignment operation according to the factor map to obtain the optimized laser odometer for vehicle laser positioning.

[0056] Furthermore, when the first alignment module 320 is used to determine the rotation calibration amount based on the first RTK data, the first alignment module 320 is used to: The RTK pose in the first RTK data is converted into a rotation matrix, and the average rotation matrix is ​​determined. The mean rotation matrix is ​​projected using SVD to obtain the mean azimuth angle, which is then used as the rotation calibration value.

[0057] Furthermore, when the first alignment module 320 performs the first alignment operation of the laser odometer during the initialization phase based on the rotation calibration amount, the first alignment module 320 is used to: Based on the rotation calibration, the coordinates of the local laser odometry are converted to be consistent with the world coordinate system of the RTK data.

[0058] Furthermore, when the acquisition module 310 is used to acquire the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometry after the first alignment operation, and to form the RTK trajectory and the laser trajectory, the acquisition module is used to: During the operation phase, after the first alignment is completed, the positioning data of the local laser odometry after the first alignment and the second RTK data are continuously stored in the queue until the preset running time or preset driving distance is met. The intersection of RTK trajectory points and laser trajectory points in the queue is obtained by taking the timestamps to form the RTK trajectory and the laser trajectory.

[0059] Furthermore, when the acquisition module 310 is used to acquire the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometry after the first alignment operation, and to form the RTK trajectory and the laser trajectory, the acquisition module is also used to: If the length difference between the RTK trajectory and the laser trajectory is greater than a first preset threshold, then the longer trajectory is downsampled. If the length difference between the RTK trajectory and the laser trajectory after downsampling is still greater than the second preset threshold, then each RTK trajectory point in the RTK trajectory and each laser trajectory point in the laser trajectory will be matched one by one according to the timestamp. The successfully matched RTK trajectory points and laser trajectory points are retained in the queue to form the RTK trajectory and the laser trajectory.

[0060] Furthermore, when the second alignment module 330 performs rigid body registration and alignment of the RTK trajectory and the laser trajectory, determines the rotation and translation calibration amount, and performs a second alignment operation on the laser odometer based on the rotation and translation calibration amount, the second alignment module 330 is used to: The RTK trajectory and the laser trajectory are paired and rigid body alignment is performed using the Umeyama algorithm. The yaw angle is extracted as the rotation and translation calibration amount based on the consistency alignment transformation matrix to complete the second alignment operation.

[0061] Furthermore, when the determining module 340 determines the rotation factor based on the positioning data output by the laser odometry and the third RTK data of the vehicle in the world coordinate system after the second alignment operation, the determining module 340 is used to: GlobalFusion is used to align real-time third RTK data and positioning data output by laser odometry after the second alignment operation in time, and the pose of the third RTK data and the local laser odometry after the second alignment is extracted in attitude. For each attitude point after synchronization and alignment, the rotation matrix is ​​extracted, the rotation error matrix is ​​calculated, and the rotation factor is obtained.

[0062] Furthermore, when constructing a factor graph based on the rotation factor, the determining module 340 is used to: The inter-frame LIO factor is determined based on local laser odometry. The GPS factor for absolute pose is determined based on the pose in real-time RTK data. The inter-frame IMU factor is determined by pre-integration based on the IMU; The factor graph is constructed based on the rotation factor, the inter-frame LIO factor, the GPS factor, and the inter-frame IMU factor.

[0063] Furthermore, when constructing a factor graph based on the rotation factor, the determining module 340 is also used to: Select at least one scalability factor; the scalability factor includes: a closed-loop factor constructed from pose recognition closed-loop, a wheel speed factor constructed from wheel speed meter data, and an inter-frame factor constructed from visual odometry. Construct a GTSAM factor graph based on the rotation factor and at least one scalability factor.

[0064] Please see Figure 4 , Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Figure 4 As shown, the electronic device 400 includes a processor 410, a memory 420, and a bus 430.

[0065] The memory 420 stores machine-readable instructions that can be executed by the processor 410. When the electronic device 400 is running, the processor 410 and the memory 420 communicate via the bus 430. When the machine-readable instructions are executed by the processor 410, the steps of the vehicle laser positioning method as described in the above method embodiment can be performed. For specific implementation details, please refer to the method embodiment, which will not be repeated here.

[0066] This application also provides a computer-readable storage medium storing a computer program. When the computer program is run by a processor, it can execute the steps of the vehicle laser positioning method as described in the above method embodiments. For specific implementation details, please refer to the method embodiments, which will not be repeated here.

[0067] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0068] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Additionally, the shown or discussed mutual couplings, direct couplings, or communication connections may be through some communication interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.

[0069] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0070] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0071] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a processor-executable, non-volatile, computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0072] Finally, it should be noted that the above-described embodiments are merely specific implementations of this application, used to illustrate the technical solutions of this application, and not to limit them. The scope of protection of this application is not limited thereto. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments, or make equivalent substitutions for some of the technical features, within the scope of the technology disclosed in this application. Such modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be covered within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A vehicle laser positioning method, characterized in that, The method includes: During the initialization phase, the first RTK data in the world coordinate system is acquired when the vehicle is stationary. The rotation calibration amount is determined based on the first RTK data, and the first alignment operation of the laser odometer in the initialization phase is performed based on the rotation calibration amount. During the running phase after initialization, the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometry after the first alignment operation are acquired respectively, and the RTK trajectory and laser trajectory are formed. Rigid body registration and alignment are performed on the RTK trajectory and the laser trajectory to determine the rotation and translation calibration amount, and a second alignment operation is performed on the laser odometer based on the rotation and translation calibration amount; After the second alignment operation, the rotation factor is determined based on the positioning data output by the laser odometry and the third RTK data of the vehicle in the world coordinate system, and a factor map is constructed based on the rotation factor. The laser odometer after the second alignment operation is optimized based on the factor graph to obtain an optimized laser odometer for vehicle laser positioning.

2. The method according to claim 1, characterized in that, Determining the rotation calibration amount based on the first RTK data includes: The RTK pose in the first RTK data is converted into a rotation matrix, and the average rotation matrix is ​​determined. The mean rotation matrix is ​​projected using SVD to obtain the mean azimuth angle, which is then used as the rotation calibration value.

3. The method according to claim 1 or 2, characterized in that, The laser odometer is subjected to the first alignment operation during the initialization phase based on the rotation calibration value, including: Based on the rotation calibration, the coordinates of the local laser odometry are converted to be consistent with the world coordinate system of the RTK data.

4. The method according to claim 1, characterized in that, The system acquires the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometry after the first alignment operation, and forms the RTK trajectory and the laser trajectory, including: During the operation phase, after the first alignment is completed, the positioning data of the local laser odometry after the first alignment and the second RTK data are continuously stored in the queue until the preset running time or preset driving distance is met. The intersection of RTK trajectory points and laser trajectory points in the queue is obtained by taking the timestamps to form the RTK trajectory and the laser trajectory.

5. The method according to claim 4, characterized in that, The system acquires the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometry after the first alignment operation, and forms the RTK trajectory and the laser trajectory, and also includes: If the length difference between the RTK trajectory and the laser trajectory is greater than a first preset threshold, then the longer trajectory is downsampled. If the length difference between the RTK trajectory and the laser trajectory after downsampling is still greater than the second preset threshold, then each RTK trajectory point in the RTK trajectory and each laser trajectory point in the laser trajectory will be matched one by one according to the timestamp. The successfully matched RTK trajectory points and laser trajectory points are retained in the queue to form the RTK trajectory and the laser trajectory.

6. The method according to claim 1, characterized in that, Rigid body registration and alignment are performed on the RTK trajectory and the laser trajectory to determine the rotation and translation calibration amount. A second alignment operation is then performed on the laser odometer based on the rotation and translation calibration amount, including: The RTK trajectory and the laser trajectory are paired and rigid body alignment is performed using the Umeyama algorithm. The yaw angle is extracted as the rotation and translation calibration amount based on the consistency alignment transformation matrix to complete the second alignment operation.

7. The method according to claim 1, characterized in that, After the second alignment operation, the rotation factor is determined based on the positioning data output by the laser odometry and the third RTK data of the vehicle in the world coordinate system, including: GlobalFusion is used to align real-time third RTK data and positioning data output by laser odometry after the second alignment operation in time, and the pose of the third RTK data and the local laser odometry after the second alignment is extracted in attitude. For each attitude point after synchronization and alignment, the rotation matrix is ​​extracted, the rotation error matrix is ​​calculated, and the rotation factor is obtained.

8. The method according to claim 7, characterized in that, Constructing a factor graph based on the rotation factor includes: The inter-frame LIO factor is determined based on local laser odometry. The GPS factor for absolute pose is determined based on the pose in real-time RTK data. The inter-frame IMU factor is determined by pre-integration based on the IMU; The factor graph is constructed based on the rotation factor, the inter-frame LIO factor, the GPS factor, and the inter-frame IMU factor.

9. The method according to claim 7 or 8, characterized in that, Constructing a factor graph based on the rotation factor also includes: Select at least one scalability factor; the scalability factor includes: a closed-loop factor constructed from pose recognition closed loop, a wheel speed factor constructed from wheel speed meter data, and an inter-frame factor constructed from visual odometry. Construct a GTSAM factor graph based on the rotation factor and at least one scalability factor.

10. A vehicle laser positioning device, characterized in that, The device includes: The acquisition module is used to acquire the first RTK data in the world coordinate system when the vehicle is stationary during the initialization phase. The first alignment module is used to determine the rotation calibration amount based on the first RTK data, and to perform the first alignment operation of the laser odometer in the initialization phase based on the rotation calibration amount. The acquisition module is also used to acquire the second RTK data of the vehicle in the world coordinate system and the positioning data output by the laser odometer after the first alignment operation during the running phase after initialization, and form the RTK trajectory and the laser trajectory. The second alignment module is used to perform rigid body registration and alignment of the RTK trajectory and the laser trajectory, determine the rotation and translation calibration amount, and perform a second alignment operation on the laser odometer based on the rotation and translation calibration amount. The determination module is used to determine the rotation factor based on the positioning data output by the laser odometer and the third RTK data of the vehicle in the world coordinate system after the second alignment operation, and to construct a factor map based on the rotation factor. An optimization module is used to optimize the laser odometer after the second alignment operation according to the factor graph to obtain an optimized laser odometer for vehicle laser positioning.

Images