A vehicle automatic driving assisted positioning fusion method and its domain control system

Abstract

The present invention discloses a vehicle automatic driving assisted positioning fusion method and its domain control system, comprising: acquiring a variety of positioning related data of the vehicle; performing preprocessing operations on the various positioning related data to obtain GNSS factors, IMU pre-integration factors and preprocessing results; performing a first NDT point cloud registration and posture calculation on the preprocessing results in sequence to obtain a posture calculation result; performing NDT map registration using the posture calculation result to obtain an NDT map registration result and a laser odometer factor; constructing a sliding window map according to the NDT map registration result; performing a second NDT point cloud registration on the sliding window map to obtain a second NDT point cloud registration result; performing closed-loop detection on the second NDT point cloud registration result to generate a closed-loop detection factor; performing constraint factor fusion and factor graph optimization on four factors to obtain an optimization result; and generating a motion trajectory of the vehicle according to the optimization result and the posture calculation result.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a vehicle autonomous driving assisted positioning fusion method and a domain control system thereof. Background Art

[0002] With the promotion of autonomous driving technology and the complexity of vehicle environmental perception requirements, autonomous driving domain controllers are important carriers for realizing autonomous driving functions, carrying the computing power and performance requirements of modules such as environmental perception fusion, decision-making planning, and chassis control. In terms of functions, the current autonomous driving domain controllers mainly support various environmental perception sensors, and do not integrate high-precision inertial navigation, satellite navigation, and 4G RTK (real-time kinematic technology). The degree of integration is not high enough, and the sensors require additional power cables, and the system wiring harness is still complex. In terms of algorithms, there are more and more studies on simultaneous positioning and mapping (SLAM) algorithms. Some algorithms require the establishment of clear matching relationships between feature points, and these clear feature matches are most prone to errors, especially when the unmanned vehicle is moving. The collected scenes sometimes change, and the results of point-to-point matching or point-to-line matching are not ideal. Summary of the invention

[0003] The object of the present invention is to provide a vehicle automatic driving assisted positioning fusion method and its domain control system, so as to improve the real-time and accuracy of positioning data communication.

[0004] The technical solution of the present invention to solve the above technical problems is as follows:

[0005] The present invention provides a vehicle automatic driving assisted positioning fusion method, the vehicle automatic driving assisted positioning fusion method comprising:

[0006] S1: Acquire various positioning-related data of the vehicle;

[0007] S2: performing a preprocessing operation on the multiple positioning related data to obtain a GNSS factor, an IMU pre-integration factor and a preprocessing result;

[0008] S3: Performing a first NDT point cloud registration on the preprocessing result to obtain a first point cloud registration result;

[0009] S4: performing pose calculation on the first point cloud registration result to obtain a pose calculation result;

[0010] S5: Perform NDT map registration using the pose calculation result to obtain an NDT map registration result and a laser odometer factor;

[0011] S6: constructing a sliding window map according to the NDT map registration result;

[0012] S7: performing a second NDT point cloud registration on the sliding window map to obtain a second NDT point cloud registration result;

[0013] S8: performing a closed-loop detection on the second NDT point cloud registration result to generate a closed-loop detection factor;

[0014] S9: performing constraint factor fusion on the GNSS factor, the IMU pre-integration factor, the laser odometer factor, and the closed-loop detection factor to obtain a fusion result;

[0015] S10: performing factor graph optimization on the fusion result to obtain an optimization result;

[0016] S11: Generate a motion trajectory of the vehicle according to the optimization result and the posture calculation result.

[0017] Optionally, in step S1, the various positioning-related data of the vehicle include: absolute position, angular velocity, acceleration and laser point cloud.

[0018] Optionally, in step S2, the preprocessing operation includes coordinate transformation, pre-integration and dedistortion, and step S2 includes:

[0019] S201: performing coordinate transformation on the absolute posture to obtain an initial posture and a GNSS factor;

[0020] S202: Pre-integrate the angular velocity and the acceleration using an IMU pre-integration model to obtain a pre-integration result and an IMU factor;

[0021] S203: performing motion estimation on the pre-integration result to obtain a motion estimation result;

[0022] S204: Dedistorting the laser point cloud and the pre-integration result to obtain a dedistortion result;

[0023] S205: performing feature calculation on the dedistortion result to obtain a feature calculation result;

[0024] S206: Outputting the initial posture, the motion estimation result and the feature calculation result as the preprocessing result.

[0025] Optionally, in step S202, the IMU pre-integration model includes:

[0026]

[0027]

[0028]

[0029] Among them, v t+Δt represents the speed of the vehicle at time t+Δt, P t+Δt represents the position of the vehicle at time t+Δt, represents the rotation of the vehicle at time t+Δt, v t represents the speed of the vehicle at time t, g w represents the gravitational acceleration of the vehicle in the world coordinate system, Δt represents a period of time, Represents the rotation matrix from the inertial system to the world coordinate system, represents the original measured acceleration of the IMU at time t and represents the deviation of the acceleration that changes slowly over time, represents the Gaussian white noise of acceleration, Represents the original measured angular velocity of the IMU at time represents the deviation of angular velocity over time, Gaussian white noise representing the angular velocity.

[0030] Optionally, in step S6, the sliding window is a window of a fixed size set on the time axis that slides over time, and only the variables within the window are optimized each time, and the remaining variables are marginalized.

[0031] Optionally, the step S8 comprises:

[0032] S81: using the characteristic value attribute of each grid in the second NDT point cloud registration result, classifying the appearance of each grid to obtain a classification result;

[0033] S82: constructing a similarity function between two frames according to the classification result;

[0034] S83: Performing a coarse closed-loop detection using the similarity function to obtain a coarse closed-loop detection result;

[0035] S84: If the rough closed-loop detection result meets the preset threshold, proceed to step S85;

[0036] S85: Perform precise closed-loop detection using the sum of distances from the mean of each grid to the coordinate origin to obtain a precise closed-loop detection result, wherein the precise closed-loop detection result includes the closed-loop detection factor.

[0037] Optionally, in step S9, in the process of adding the laser odometer factor, only the current frame associated with the current state of the vehicle is added as the constraint factor in the graph, and the laser scanning frames between the two frames will not be optimized.

[0038] The present invention also provides a vehicle automatic driving domain control system using the above-mentioned vehicle automatic driving auxiliary positioning fusion method, and the vehicle automatic driving domain control system includes:

[0039] A positioning-related data acquisition module, wherein the positioning-related data acquisition module is used to acquire various positioning-related data of the vehicle;

[0040] An autonomous driving processor is used to perform a series of processing on various positioning-related data of the vehicle to generate a motion trajectory of the vehicle.

[0041] Optionally, the positioning-related data acquisition module includes a GNSS+RTK unit, an IMU unit and a sensor unit, wherein the GNSS+RTK unit is used to obtain the absolute position of the vehicle; the IMU unit is used to obtain the angular velocity and acceleration of the vehicle, and the sensor unit is used to obtain the laser point cloud of the vehicle.

[0042] The present invention has the following beneficial effects:

[0043] The present invention integrates IMU, GNSS and 4G RTK real-time differential systems inside the autonomous driving domain controller, reduces external line connections, improves the real-time performance of positioning data communication, and achieves the purpose of reducing technical costs. The data fusion algorithm based on factor graph optimization more intuitively shows the relationship between different nodes. When state quantities need to be added, the factor graph can directly add factors on the basis of the original graph. Similarly, if the measurement value is less reliable or the signal is lost, it is only necessary to simply reduce the factors on the basis of the original graph. No special programming or model modification is required, which greatly reduces the amount of calculation when dealing with SLAM problems. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 This is a flow chart of the vehicle automatic driving assisted positioning fusion method of the present invention;

[0045] Figure 2 This is a framework diagram of the vehicle automatic driving assisted positioning fusion of the present invention;

[0046] Figure 3 Schematic diagram of the factor graph optimization process of the present invention. DETAILED DESCRIPTION

[0047] The principles and features of the present invention are described below in conjunction with the accompanying drawings. The examples given are only used to explain the present invention and are not used to limit the scope of the present invention.

[0048] The present invention provides a vehicle automatic driving auxiliary positioning fusion method, referring to Figure 1 As shown, the vehicle automatic driving assisted positioning fusion method includes:

[0049] S1: Acquire various positioning-related data of the vehicle;

[0050] In the present invention, the various positioning-related data of the vehicle include at least absolute position, angular velocity, acceleration and laser point cloud.

[0051] Among them, the absolute pose is obtained through the GNSS+RTK unit, the angular velocity and acceleration are obtained through the IMU unit; the laser point cloud is obtained through the sensor unit.

[0052] The GNSS+RTK unit is compatible with various star systems and frequency bands, can search more satellites, has better stability, and supports RTK high-precision positioning with an accuracy of up to centimeter level.

[0053] The IMU unit can output 3-axis acceleration and 3-axis angular velocity, accurately output the vehicle's motion state, and input its acceleration and angular velocity information into the autonomous driving processor for information fusion calculation for SLAM positioning. During driving, the vehicle can usually calculate its own posture by knowing the rotation angle and acceleration information. The angular velocity and acceleration measurement values ​​of the IMU have the highest accuracy in rotation angle and acceleration, and can provide higher frequency information output. The role of the IMU in the factor graph is to provide good posture constraints that complement the laser odometer factor.

[0054] Specifically, the GNSS+RTK unit uses the INS-YI100C differential GPS unit.

[0055] Specifically, the IMU unit uses the BW-127 inertial measurement unit from Beiwei Sensing.

[0056] S2: performing a preprocessing operation on the multiple positioning related data to obtain a GNSS factor, an IMU pre-integration factor and a preprocessing result;

[0057] refer to Figure 2 As shown, the preprocessing operation includes coordinate transformation, pre-integration and de-distortion, so the step S2 includes:

[0058] S201: performing coordinate transformation on the absolute posture to obtain an initial posture and a GNSS factor;

[0059] Since the drift of the LiDAR odometer and the inertial odometer grows very slowly, it is not necessary to add the GNSS factor in real time after the GNSS factor is generated. The present invention adds the GNSS factor at the initial position and loop detection, and in other operating conditions only adds the GNSS factor when the estimated position covariance is greater than the received GNSS position covariance.

[0060] S202: Pre-integrate the angular velocity and the acceleration using an IMU pre-integration model to obtain a pre-integration result and an IMU factor;

[0061] The IMU pre-integration model includes:

[0062]

[0063]

[0064]

[0065] Among them, v t+Δt represents the speed of the vehicle at time t+Δt, P t+Δt represents the position of the vehicle at time t+Δt, represents the rotation of the vehicle at time t+Δt, v t represents the speed of the vehicle at time t, g w represents the gravitational acceleration of the vehicle in the world coordinate system, Δt represents a period of time, Represents the rotation matrix from the inertial system to the world coordinate system, represents the original measured acceleration of the IMU at time t and represents the deviation of the acceleration that changes slowly over time, represents the Gaussian white noise of acceleration, Represents the original measured angular velocity of the IMU at time represents the deviation of angular velocity over time, Gaussian white noise representing the angular velocity.

[0066] S203: performing motion estimation on the pre-integration result to obtain a motion estimation result;

[0067] S204: Dedistorting the laser point cloud and the pre-integration result to obtain a dedistortion result;

[0068] S205: performing feature calculation on the dedistortion result to obtain a feature calculation result;

[0069] S206: Outputting the initial posture, the motion estimation result and the feature calculation result as the preprocessing result.

[0070] S3: Performing a first NDT point cloud registration on the preprocessing result to obtain a first point cloud registration result;

[0071] S4: performing pose calculation on the first point cloud registration result to obtain a pose calculation result;

[0072] S5: Perform NDT map registration using the pose calculation result to obtain an NDT map registration result and a laser odometer factor;

[0073] The laser odometry factor, similar to the IMU pre-integration factor, plays a vital role in motion estimation. Compared with the GNSS factor, the laser odometry factor has obvious advantages in estimating the accuracy of the pose, and it is not affected by the occlusion of obstacles in the environment. Regarding the addition of the laser odometry factor, in order to ensure the real-time performance of the algorithm, in the process of adding the laser odometry factor, the present invention only adds the current frame associated with the current state of the vehicle as a constraint factor in the graph, and the laser scanning frame between the two frames will not be optimized, which greatly improves the calculation efficiency. At the same time, it also helps to maintain a relatively sparse factor graph, which is suitable for real-time nonlinear optimization.

[0074] S6: constructing a sliding window map according to the NDT map registration result;

[0075] The sliding window is to set a fixed window that slides over time on the time axis. Each time, only the variables within the window are optimized and the remaining variables are marginalized. Since all variables will be re-linearized during the optimization iteration, the linearization cumulative error is small and the accuracy is guaranteed. At the same time, since the window size is fixed, the number of optimized variables remains basically unchanged, which can meet the real-time requirements.

[0076] S7: performing a second NDT point cloud registration on the sliding window map to obtain a second NDT point cloud registration result;

[0077] S8: performing a closed-loop detection on the second NDT point cloud registration result to generate a closed-loop detection factor;

[0078] After the closed-loop conditions are ripe, a closed-loop detection factor is added. In fact, the benefit of adding a closed-loop detection factor is the optimization of rotation and pitch angles. In the actual mapping process, the point cloud map with the loop closure factor added has good performance in scenarios with large map rotation and height changes.

[0079] S9: performing constraint factor fusion on the GNSS factor, the IMU pre-integration factor, the laser odometer factor, and the closed-loop detection factor to obtain a fusion result;

[0080] S10: performing factor graph optimization on the fusion result to obtain an optimization result;

[0081] The method combining factor graph optimization with sliding window is widely used in various fusion positioning and mapping systems due to its good real-time and robustness. Therefore, the present invention takes the Normal Distributions Transform point cloud matching algorithm characterized by sliding window matching as the core, and uses factor graph optimization as the multi-sensor fusion means to establish a factor graph optimization SLAM framework based on multi-sensor fusion.

[0082] Figure 3 The figure is a schematic diagram of the factor graph optimization system. In the factor graph optimization, the accurate GNSS positioning information, IMU pre-integration information, loop detection information and laser odometer factor are fused as correction factors. In the process of building maps in complex large scenes, the accumulated error can be greatly eliminated, and high-precision SLAM map construction can be achieved. Among them, GNSS provides absolute posture information including the initial posture of the SLAM positioning system, which has the advantage of improving the repositioning capability of the unmanned vehicle.

[0083] S11: Generate a motion trajectory of the vehicle according to the optimization result and the posture calculation result.

[0084] Optionally, the step S8 comprises:

[0085] S81: using the characteristic value attribute of each grid in the second NDT point cloud registration result, classifying the appearance of each grid to obtain a classification result;

[0086] S82: constructing a similarity function between two frames according to the classification result;

[0087] S83: Performing a coarse closed-loop detection using the similarity function to obtain a coarse closed-loop detection result;

[0088] S84: If the rough closed-loop detection result meets the preset threshold, proceed to step S85;

[0089] S85: Perform precise closed-loop detection using the sum of distances from the mean of each grid to the coordinate origin to obtain a precise closed-loop detection result, wherein the precise closed-loop detection result includes the closed-loop detection factor.

[0090] Optionally, in step S9, in the process of adding the laser odometer factor, only the current frame associated with the current state of the vehicle is added as the constraint factor in the graph, and the laser scanning frames between the two frames will not be optimized.

[0091] The present invention also provides a vehicle automatic driving domain control system using the above-mentioned vehicle automatic driving auxiliary positioning fusion method, and the vehicle automatic driving domain control system includes:

[0092] A positioning-related data acquisition module, wherein the positioning-related data acquisition module is used to acquire various positioning-related data of the vehicle;

[0093] An autonomous driving processor is used to perform a series of processing on various positioning-related data of the vehicle to generate a motion trajectory of the vehicle.

[0094] Optionally, the positioning-related data acquisition module includes a GNSS+RTK unit, an IMU unit and a sensor unit, wherein the GNSS+RTK unit is used to obtain the absolute position of the vehicle; the IMU unit is used to obtain the angular velocity and acceleration of the vehicle, and the sensor unit is used to obtain the laser point cloud of the vehicle.

[0095] Specifically, in actual applications, the autonomous driving domain controller may include an autonomous driving processor, a microprocessor, a GNSS+RTK positioning module, an IMU module, as well as autonomous driving sensors, a wire-controlled chassis, and other external devices.

[0096] As a specific embodiment, the autonomous driving processor of the present invention adopts the Xavier chip of the embedded intelligent system including the autonomous driving system developed by NVIDIA, and the chip performance includes: eight-core CPU based on ARMv8 ISA, deep learning accelerator (DLA): 5TOPS (FP16) | 10TOPS (INT8), Volta GPU: 512CUDA cores | 20TOPS (INT8) | 1.3TFLOPS (FP32), vision processor: 1.6TOPS, stereo and optical flow engine (SOFE): 6TOPS, image signal processor (ISP): 1.5Giga Pixels / s, video encoder: 1.2GPix / s, video decoder: 1.8GPix / s.

[0097] The microcontroller uses Infineon TC297 series chips, including a three-core TriCore architecture with a 300MHz operating frequency, 728KB+8MB capacity, and RAM with ECC (error correction coding) protection. It is designed based on the ISO26262 standard and supports the highest safety level requirements of ASIL-D. In conjunction with the basic chip, the hardware core safety architecture design is realized.

[0098] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A vehicle automatic driving assisted positioning fusion method, characterized in that: The vehicle automatic driving assisted positioning fusion method comprises the following steps: S1: Acquire various positioning-related data of the vehicle; S2: performing a preprocessing operation on the multiple positioning related data to obtain a GNSS factor, an IMU pre-integration factor and a preprocessing result; S3: Performing a first NDT point cloud registration on the preprocessing result to obtain a first point cloud registration result; S4: performing pose calculation on the first point cloud registration result to obtain a pose calculation result; S5: Perform NDT map registration using the pose calculation result to obtain an NDT map registration result and a laser odometer factor; S6: constructing a sliding window map according to the NDT map registration result; S7: performing a second NDT point cloud registration on the sliding window map to obtain a second NDT point cloud registration result; S8: performing a closed-loop detection on the second NDT point cloud registration result to generate a closed-loop detection factor; S9: performing constraint factor fusion on the GNSS factor, the IMU pre-integration factor, the laser odometer factor, and the closed-loop detection factor to obtain a fusion result; S10: performing factor graph optimization on the fusion result to obtain an optimization result; S11: Generate a motion trajectory of the vehicle according to the optimization result and the posture calculation result.

2. The vehicle automatic driving assisted positioning fusion method according to claim 1, characterized in that: In step S1, the various positioning-related data of the vehicle include: absolute position, angular velocity, acceleration and laser point cloud.

3. The vehicle automatic driving assisted positioning fusion method according to claim 2, characterized in that: In the step S2, the preprocessing operation includes coordinate transformation, pre-integration and dedistortion, and the step S2 includes: S201: performing coordinate transformation on the absolute posture to obtain an initial posture and a GNSS factor; S202: Pre-integrate the angular velocity and the acceleration using an IMU pre-integration model to obtain a pre-integration result and an IMU factor; S203: performing motion estimation on the pre-integration result to obtain a motion estimation result; S204: Dedistorting the laser point cloud and the pre-integration result to obtain a dedistortion result; S205: performing feature calculation on the dedistortion result to obtain a feature calculation result; S206: Outputting the initial posture, the motion estimation result and the feature calculation result as the preprocessing result.

4. The vehicle automatic driving assisted positioning fusion method according to claim 3, characterized in that: In step S202, the IMU pre-integration model includes: Among them, v t+Δt represents the speed of the vehicle at time t+Δt, P t+Δt represents the position of the vehicle at time t+Δt, represents the rotation of the vehicle at time t+Δt, v t represents the speed of the vehicle at time t, g w represents the gravitational acceleration of the vehicle in the world coordinate system, Δt represents a period of time, Represents the rotation matrix from the inertial system to the world coordinate system, represents the original measured acceleration of the IMU at time t and represents the deviation of the acceleration that changes slowly over time, represents the Gaussian white noise of acceleration, Represents the original measured angular velocity of the IMU at time represents the deviation of angular velocity over time, Gaussian white noise representing the angular velocity.

5. The vehicle automatic driving assisted positioning fusion method according to claim 1, characterized in that: In step S6, the sliding window is a window of a fixed size set on the time axis that slides over time, and only the variables within the window are optimized each time, and the remaining variables are marginalized.

6. The vehicle automatic driving assisted positioning fusion method according to claim 1, characterized in that: The step S8 comprises: S81: using the characteristic value attribute of each grid in the second NDT point cloud registration result, classifying the appearance of each grid to obtain a classification result; S82: constructing a similarity function between two frames according to the classification result; S83: Performing a coarse closed-loop detection using the similarity function to obtain a coarse closed-loop detection result; S84: If the rough closed-loop detection result meets the preset threshold, proceed to step S85; S85: Perform precise closed-loop detection using the sum of distances from the mean of each grid to the coordinate origin to obtain a precise closed-loop detection result, wherein the precise closed-loop detection result includes the closed-loop detection factor.

7. The vehicle automatic driving assisted positioning fusion method according to any one of claims 1 to 6, characterized in that: In step S9, in the process of adding the laser odometer factor, only the current frame associated with the current state of the vehicle is added as the constraint factor in the figure, and the laser scanning frames between the two frames will not be optimized.

8. A vehicle automatic driving domain control system using the vehicle automatic driving assisted positioning fusion method according to any one of claims 1 to 7, characterized in that: The vehicle automatic driving domain control system comprises: A positioning-related data acquisition module, wherein the positioning-related data acquisition module is used to acquire various positioning-related data of the vehicle; An autonomous driving processor is used to perform a series of processing on various positioning-related data of the vehicle to generate a motion trajectory of the vehicle.

9. The vehicle automatic driving domain control system according to claim 8, characterized in that: The positioning-related data acquisition module includes a GNSS+RTK unit, an IMU unit and a sensor unit. The GNSS+RTK unit is used to obtain the absolute position and posture of the vehicle; the IMU unit is used to obtain the angular velocity and acceleration of the vehicle, and the sensor unit is used to obtain the laser point cloud of the vehicle.

Images