A multi-sensor data fusion positioning system

Abstract

The application provides a multi-sensor data fusion positioning system. The multi-sensor data fusion positioning system comprises a combined positioning subunit, a two-dimensional state error Kalman filtering unit, a laser radar point cloud matching positioning unit and a three-dimensional state error Kalman filtering unit. The combined positioning subunit comprises a GNSS module, an RTK service module and an IMU module. The two-dimensional state error Kalman filtering unit comprises a high-precision map module, a visual lane line detection module, a lane line matching module, a wheel speed odometer calculation module and a first state error Kalman filtering module. The laser radar point cloud matching positioning unit comprises a point cloud module, a point cloud map module, a positioning initialization module and an NDT matching positioning module. The three-dimensional state error Kalman filtering unit comprises a Kalman filtering initialization module, an IMU pose state quantity calculation module, a judgment module, a second state error Kalman filtering module and a posteriori pose module. The multi-sensor data fusion positioning system can enhance the robustness of global positioning and has strong scene adaptability.

Description

Technical Field

[0001] This invention relates to the field of intelligent connected vehicle intelligent driving technology, and in particular to a multi-sensor data fusion positioning system. Background Technology

[0002] For advanced autonomous driving systems (such as Level 4), the localization module is one of the most crucial modules. Localization includes global localization and local localization. For autonomous driving, its accuracy needs to reach the centimeter level. In addition, the localization system must have strong robustness, meaning that it can still output accurate real-time localization results even when data from any sensor degrades. The localization module typically integrates measurement data from multiple sensors, such as GNSS, IMU, wheel speed odometer (output from vehicle chassis encoder trajectory extrapolation), visual inspection information, visual odometer, and LiDAR odometer. It uses filtering algorithms (ESKF, EKF, UKF, etc.) to obtain smooth, centimeter-level absolute localization. Among these, registration localization based on point cloud maps and LiDAR (Lidar Odometry) usually carries a large weight in the entire fused localization process due to its high accuracy, strong scene adaptability, and good robustness and reliability. It is a relatively reliable source of "absolute localization" data in the autonomous driving localization system.

[0003] Global positioning obtains a vehicle's pose in global coordinates on Earth using the Global Positioning System (GNSS). Global positioning inherently relies on GNSS; however, in complex scenarios where autonomous driving is deployed, such as urban canyons, overpasses, tunnels, and tree-lined avenues, situations inevitably arise where GNSS signals are absent or intermittent. Therefore, an autonomous driving positioning system that solely relies on a GNSS-dependent global positioning scheme lacks robust positioning capabilities in complex scenarios. This is the most critical problem for all-weather autonomous driving systems.

[0004] Vehicle-side local localization aims to obtain the vehicle's pose within a local scene (such as indoors or an industrial park) or a local map coordinate system. Traditional autonomous mobile robots (such as AGVs) often employ local localization techniques to perform tasks such as path planning in a given scene. For example, autonomous mobile robots frequently use Simultaneous Localization and Mapping (SLAM) for online localization. The advantage of this method is that it can achieve reasonably accurate local localization results using various low-cost sensors in the absence of GNSS signals, combined with advanced multi-sensor data fusion algorithms. However, this method suffers from problems such as increased cumulative odometer errors in large-scale scenes and positioning delays, making it difficult to directly apply to complex autonomous driving scenarios. Because autonomous vehicles typically travel at high speeds over long distances in highly complex environments, the maps constructed in real-time by the mapping algorithm are often significantly offset, leading to accumulated odometer errors in the localization algorithm and a decrease in positioning accuracy. Summary of the Invention

[0005] To address the aforementioned problems in existing technologies, this invention proposes a multi-sensor data fusion positioning system that enhances the robustness of global positioning and has strong scene adaptability.

[0006] Specifically, this invention proposes a multi-sensor data fusion positioning system, applicable to vehicles, comprising:

[0007] The combined positioning subunit includes a GNSS module, an RTK service module, and an IMU module. The GNSS module is used to acquire GNSS data, the RTK service module is used to provide real-time motion differential enhancement solution services, and the IMU module is used to acquire vehicle IMU data.

[0008] The two-dimensional state error Kalman filter unit includes a high-precision map module, a visual lane detection module, a lane matching module, a wheel speed odometer calculation module, and a first state error Kalman filter module. The high-precision map module obtains the vertical coordinates of the lane line where the vehicle is located based on the high-precision map and GNSS data. The visual lane detection module obtains the coordinates of the nearest and farthest points of the lane line where the vehicle is located through the vehicle's visual camera. The lane matching module obtains the lateral update amount of the vehicle based on the output results of the high-precision map module and the visual lane detection module. The wheel speed odometer calculation module is used to obtain the longitudinal update amount of the vehicle. The first state error Kalman filter module is used to fuse the lateral update amount, longitudinal update amount, GNSS data, and IMU data of the vehicle to obtain the two-dimensional positioning result of the vehicle.

[0009] The lidar point cloud matching and positioning unit includes a point cloud module, a point cloud map module, a positioning initialization module, and an NDT matching and positioning module. The point cloud module acquires the real-time point cloud data of the vehicle through the lidar of the vehicle. The point cloud map module is used to acquire an offline constructed point cloud map. The positioning initialization module is used to acquire the global coordinates of the vehicle. The NDT matching and positioning module acquires the matching and positioning pose of the vehicle based on the real-time point cloud data and the point cloud map.

[0010] The three-dimensional state error Kalman filter unit includes a Kalman filter initialization module, an IMU pose state quantity calculation module, a judgment module, a second state error Kalman filter module, and a posterior pose module. The Kalman filter initialization module obtains the initial value of the global pose state quantity based on global coordinates. The IMU pose state quantity calculation module calculates the estimated state quantity based on the initial value of the global pose state quantity and IMU data. The judgment module is used to determine whether the lidar point cloud matching and positioning unit outputs a valid matching and positioning pose. If yes, the second state error Kalman filter module updates the estimated state quantity based on the matching and positioning pose, and the posterior pose module calculates the three-dimensional positioning result of the vehicle based on the updated estimated state quantity. If no, the IMU pose state quantity calculation module directly outputs the three-dimensional positioning result based on the estimated state quantity.

[0011] If the GNSS module can receive GNSS data normally and the RTK service module can provide real-time motion differential enhancement solution service, then the multi-sensor data fusion positioning system outputs the positioning result fused from GNSS data and IMU data through the combined positioning subunit.

[0012] If the GNSS module can receive GNSS data normally and the RTK service module cannot provide real-time motion differential enhancement solution service, then the multi-sensor data fusion positioning system obtains the two-dimensional positioning result of the vehicle through the two-dimensional state error Kalman filter unit.

[0013] If the GNSS module cannot receive complete GNSS data, the multi-sensor data fusion positioning system obtains the three-dimensional positioning result of the vehicle through the lidar point cloud matching positioning unit and the three-dimensional state error Kalman filter unit.

[0014] According to one embodiment of the present invention, the GNSS data acquired by the first state error Kalman filter module includes the latitude and longitude data of the vehicle. The first state error Kalman filter module obtains the lateral state quantity and longitudinal state quantity of the vehicle based on the latitude and longitude data, and updates and corrects the lateral state quantity and longitudinal state quantity based on the lateral update quantity and longitudinal update quantity. The first state error Kalman filter module obtains the heading angle of the vehicle after fusing the GNSS data and IMU data.

[0015] According to one embodiment of the present invention, in the two-dimensional state error Kalman filter unit, the high-precision map module obtains the vertical coordinates of the left and right lane lines closest to the vehicle based on the high-precision map and GNSS data, the visual lane line detection module obtains the coordinates of the nearest and farthest points of the left and right lane lines closest to the vehicle through the vehicle's visual camera, and the lane line matching module obtains the lateral update amount of the vehicle based on the output results of the high-precision map module and the visual lane line detection module.

[0016] According to an embodiment of the present invention, the process by which the positioning initialization module obtains the global coordinates of the vehicle includes:

[0017] The global coordinates obtained after the transformation of the GNSS data are used to achieve positioning initialization, and the global coordinates are continuously updated based on the GNSS data;

[0018] And / or global coordinates are obtained by numerical integration of the IMU data over short time intervals to achieve positioning initialization, and the global coordinates are continuously updated based on the IMU data.

[0019] According to one embodiment of the present invention, in the point cloud module of the lidar point cloud matching and positioning unit, the point cloud data of the vehicle obtained by the lidar of the vehicle is converted into real-time point cloud data after being downsampled and filtered by voxel grid; the point cloud map is generated offline by data obtained by multiple sensors on the vehicle through an optimized fusion mapping algorithm.

[0020] According to an embodiment of the present invention, the voxel grid downsampling filtering includes the steps of: using the VoxelGrid class of the open-source point cloud library to create a series of three-dimensional voxel grids based on the point cloud data acquired by the lidar; for each three-dimensional voxel grid, using the centroid of all points in the three-dimensional voxel grid to characterize other points in the three-dimensional voxel grid.

[0021] According to an embodiment of the present invention, the NDT matching and positioning module uses the NDT matching algorithm to match the real-time point cloud data and the point cloud map. In the NDT algorithm parameter configuration, the matching voxel leaf size is set to 3, the number of iteration steps is 30, the resolution is 1.0, the step size is 0.1, and the minimum tolerance is 0.01.

[0022] According to one embodiment of the present invention, if the NDT matching algorithm fails to match, the NDT matching positioning module is initialized based on the current global coordinates output by the positioning initialization module or the intermediate results obtained by the NDT matching algorithm.

[0023] According to one embodiment of the present invention, in the three-dimensional state error Kalman filter unit, the Kalman filter initialization module obtains the initial value of the global pose state quantity based on global coordinates through rolling iteration.

[0024] According to one embodiment of the present invention, the IMU pose state quantity calculation module selects a time interval and performs median numerical integration calculation on the IMU data based on the initial value of the global pose state quantity to obtain the estimated state quantity.

[0025] This invention provides a multi-sensor data fusion positioning system, which introduces a lidar point cloud matching positioning unit module to enhance the robustness and scene adaptability of global positioning. By using multi-sensor data fusion global positioning on the vehicle and vehicle-side local positioning technology, the real-time performance and robustness required by the overall positioning system are taken into account. This multi-sensor data fusion positioning system has strong scene adaptability.

[0026] It should be understood that the above general description and the following detailed description of the invention are exemplary and illustrative, and are intended to provide further explanation of the invention as described in the claims. Attached Figure Description

[0027] The accompanying drawings are included to provide a further understanding of the invention. They are incorporated in and constitute a part of this application. The drawings illustrate embodiments of the invention and, together with this specification, serve to explain the principles of the invention.

[0028] In the attached image:

[0029] Figure 1 A schematic diagram of a multi-sensor data fusion positioning system according to an embodiment of the present invention is shown.

[0030] The above figures include the following reference numerals:

[0031] Multi-sensor data fusion positioning system 100

[0032] Combined positioning subunit 101

[0033] Two-dimensional state error Kalman filter unit 102

[0034] LiDAR point cloud matching and positioning unit 103

[0035] 3D state error Kalman filter unit 104

[0036] GNSS Module 105

[0037] RTK service module 106

[0038] IMU module 107

[0039] High-precision map module 108

[0040] Visual lane detection module 109

[0041] Lane matching module 110

[0042] Wheel speed odometer calculation module 111

[0043] First state error Kalman filter module 112

[0044] Point cloud module 113

[0045] Point cloud map module 114

[0046] Positioning initialization module 115

[0047] NDT Matching and Positioning Module 116

[0048] Kalman filter initialization module 117

[0049] IMU pose and state quantity calculation module 118

[0050] Judgment Module 119

[0051] Second state error Kalman filter module 120

[0052] Posterior Pose Module 121 Detailed Implementation

[0053] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other.

[0054] 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 a part of the embodiments of this application, and not all of them. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit this application or its application or use. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0055] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this application. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0056] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of this application. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following drawings denote similar items; therefore, once an item is defined in one drawing, it need not be further discussed in subsequent drawings.

[0057] In the description of this application, it should be understood that the orientation or positional relationship indicated by directional terms such as "front, back, up, down, left, right", "horizontal, vertical, horizontal" and "top, bottom" is usually based on the orientation or positional relationship shown in the accompanying drawings, and is only for the convenience of describing this application and simplifying the description. Unless otherwise stated, these directional terms do not indicate or imply that the device or element referred to must have a specific orientation or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on the scope of protection of this application; the directional terms "inner" and "outer" refer to the inner and outer contours relative to the outline of each component itself.

[0058] For ease of description, spatial relative terms such as "above," "on top of," "on the upper surface of," "above," etc., are used herein to describe the spatial positional relationship of a device or feature as shown in the figures to other devices or features. It should be understood that spatial relative terms are intended to encompass different orientations in use or operation beyond the orientation of the device as described in the figures. For example, if the device in the figures were inverted, a device described as "above" or "on top of" other devices or structures would subsequently be positioned as "below" or "under" other devices or structures. Thus, the exemplary term "above" can include both "above" and "below." The device may also be positioned in other different ways, rotated 90 degrees, or in other orientations, and the spatial relative descriptions used herein will be interpreted accordingly.

[0059] Furthermore, it should be noted that the use of terms such as "first" and "second" to define components is merely for the purpose of distinguishing the corresponding components. Unless otherwise stated, these terms have no special meaning and therefore should not be construed as limiting the scope of protection of this application. In addition, although the terminology used in this application is selected from commonly known and used terms, some terms mentioned in this application's specification may have been chosen by the applicant according to his or her judgment, and their detailed meanings are explained in the relevant sections of this description. Moreover, this application should be understood not only through the actual terms used, but also through the meaning implied by each term.

[0060] Figure 1 A schematic diagram of a multi-sensor data fusion positioning system according to an embodiment of the present invention is shown. As shown in the figure, the present invention provides a multi-sensor data fusion positioning system 100 suitable for vehicles. The multi-sensor data fusion positioning system 100 mainly includes a combined positioning subunit 101, a two-dimensional state error Kalman filter unit 102, a lidar point cloud matching positioning unit 103, and a three-dimensional state error Kalman filter unit 104.

[0061] The integrated positioning subunit 101 includes a GNSS module 105, an RTK service module 106, and an IMU module 107. The GNSS module 105 acquires GNSS data. GNSS (Global Navigation Satellite System) is a space-based radio navigation and positioning system that provides users with all-weather 3D coordinates, velocity, and time information at any location on the Earth's surface or in near-Earth space. It includes one or more satellite constellations and the augmentation systems required to support specific operations. The RTK (Real-time kinematic) service module provides real-time motion differential augmentation services. It transmits carrier phase data collected by the reference station to the user receiver for differential coordinate calculation, achieving centimeter-level positioning accuracy in real-time in the field. The IMU module 107 acquires IMU data from the vehicle. The IMU (Inertial Measurement Unit) measures the vehicle's three-axis attitude angles (or angular rates) and acceleration.

[0062] The two-dimensional state error Kalman filter unit 102 includes a high-precision map module 108, a visual lane detection module 109, a lane matching module 110, a wheel speed odometer calculation module 111, and a first state error Kalman filter module 112. The high-precision map module 108 combines high-precision map data and GNSS data provided by the supplier to obtain the vertical coordinates of the vehicle's lane line. The visual lane detection module 109 uses the vehicle's visual camera to obtain the coordinates of the nearest and farthest points of the vehicle's lane line. The lane matching module 110 obtains the vehicle's lateral update based on the outputs of the high-precision map module 108 and the visual lane detection module 109. The wheel speed odometer calculation module 111 obtains the vehicle's longitudinal update. The first state error Kalman filter module 112 fuses the vehicle's lateral update, longitudinal update, GNSS data, and IMU data through rolling iteration to obtain the vehicle's two-dimensional positioning result. This two-dimensional positioning result includes lateral positioning, longitudinal positioning, and heading angle.

[0063] The LiDAR point cloud matching and positioning unit 103 includes a point cloud module 113, a point cloud map module 114, a positioning initialization module 115, and an NDT matching and positioning module 116. The point cloud module 113 acquires real-time point cloud data of the vehicle using the vehicle's LiDAR. The point cloud map module 114 acquires a high-quality point cloud map constructed offline. The positioning initialization module 115 acquires the vehicle's global coordinates. The NDT matching and positioning module 116 acquires the vehicle's matching and positioning pose based on the real-time point cloud data and the point cloud map.

[0064] The three-dimensional state error Kalman filter unit 104 includes a Kalman filter initialization module 117, an IMU pose state quantity calculation module 118, a judgment module 119, a second state error Kalman filter module 120, and a posterior pose module 121. The Kalman filter initialization module 117 obtains the initial value of the global pose state quantity based on global coordinates. The IMU pose state quantity calculation module 118 calculates the estimated state quantity based on the initial value of the global pose state quantity and IMU data. The judgment module 119 determines whether the lidar point cloud matching and positioning unit 103 outputs a valid matching and positioning pose. If yes, the second state error Kalman filter module 120 updates the estimated state quantity based on the matching and positioning pose, and the posterior pose module 121 calculates the vehicle's three-dimensional positioning result based on the updated estimated state quantity; otherwise, the IMU pose state quantity calculation module 118 directly outputs the three-dimensional positioning result based on the estimated state quantity. The three-dimensional positioning result contains three-dimensional six-degree-of-freedom data.

[0065] If the GNSS module 105 can receive GNSS data normally and the RTK service module 106 can provide real-time motion differential enhancement solution service, then the multi-sensor data fusion positioning system 100 outputs a high-precision positioning result fused from GNSS data and IMU data through the combined positioning subunit 101.

[0066] If the GNSS module 105 can receive GNSS data normally and the RTK service module 106 cannot provide real-time motion differential enhancement solution service, the multi-sensor data fusion positioning system 100 switches to the two-dimensional state error Kalman filter unit 102. The two-dimensional state error Kalman filter unit 102 fuses the matching results of the lane matching module 110 and the wheel speed odometer solution module 111, as well as the GNSS data and IMU data, to obtain a two-dimensional positioning result of the vehicle with relatively high accuracy.

[0067] If the GNSS module 105 cannot receive complete GNSS data, i.e., cannot obtain valid GNSS data, or when there is intermittent GNSS data, the multi-sensor data fusion positioning system 100 obtains a higher-precision three-dimensional positioning result of the vehicle through the LiDAR point cloud matching positioning unit 103 and the three-dimensional state error Kalman filter unit 104 by fusing heterogeneous sensor data.

[0068] This invention provides a multi-sensor data fusion positioning system 100, which fuses data from multiple sensors. Based on a state error Kalman filter fusion algorithm, it combines global positioning and vehicle-side local positioning technologies. Depending on the actual scene and whether sensor data degradation occurs, different modules output high-precision positioning results, balancing real-time performance and robustness. This multi-sensor data fusion positioning system 100 introduces a LiDAR point cloud matching positioning unit 103 based on a high-precision point cloud map and real-time LiDAR scanned point cloud to enhance the robustness and scene adaptability of global positioning. It has a low-cost advantage and is particularly suitable for complex autonomous driving scenarios, such as urban canyons, overpasses, tunnels, and tree-lined avenues.

[0069] Preferably, the GNSS data acquired by the first state error Kalman filter module 112 includes the vehicle's latitude and longitude data. Based on this latitude and longitude data, the first state error Kalman filter module 112 obtains the vehicle's lateral and longitudinal state variables, and updates and corrects these variables based on the lateral and longitudinal update values, correspondingly obtaining the lateral and longitudinal positioning results in the two-dimensional positioning results. The first state error Kalman filter module 112 obtains the vehicle's initial heading angle based on the fusion of GNSS and IMU data, and iteratively updates the heading angle based on the integral increment of the median value of the yaw rate in the IMU data. More preferably, during the iterative update of the heading angle, the yaw angle update value after differential motion calculation of the angular velocity data from the four wheel encoders of the vehicle can also be incorporated to obtain a more accurate result. Specifically, when the RTK service module 106 cannot provide real-time motion differential enhancement calculation service, i.e., it does not receive the RTK differential enhancement calculation service, at the code level, the combined positioning subunit 101 will switch to the positioning data fused from GNSS and IMU provided by the output supplier, such as latitude, longitude, altitude, and attitude angles, and re-transform and redefine it to adapt it to the coordinate system definition and attitude angle definition convention of the vehicle hardware platform. In addition, the combined positioning subunit 101 will also process the GNSS data and IMU data, expressing the latitude, longitude, and altitude data in the local coordinate system through Mercator projection; and transforming the linear acceleration and angular velocity data of the IMU data from the navigation coordinate system to the vehicle coordinate system through three-dimensional spatial transformation, so as to provide initial data for the positioning initialization module 115 of the lidar point cloud matching positioning unit 103 and the IMU pose state quantity calculation module 118 of the three-dimensional state error Kalman filter unit 104.

[0070] Preferably, in the two-dimensional state error Kalman filter unit 102, the high-precision map module 108 performs a combined retrieval based on the high-precision map and GNSS data to obtain the vertical coordinates of the left and right lane lines closest to the vehicle. The visual lane line detection module 109 obtains the coordinates of the nearest and farthest points of the left and right lane lines closest to the front end of the visual camera through the vehicle's visual camera. The lane line matching module 110 obtains the vehicle's lateral update amount based on the output results of the high-precision map module 108 and the visual lane line detection module 109.

[0071] Preferably, the process by which the positioning initialization module 115 obtains the global coordinates of the vehicle includes:

[0072] The global coordinates obtained after GNSS data transformation are used to initialize the positioning, and the global coordinates are continuously updated based on the GNSS data.

[0073] And / or global coordinates are obtained by numerical integration of pose based on IMU data over short time intervals to achieve localization initialization, and global coordinates are continuously updated based on IMU data.

[0074] Preferably, in the point cloud module 113 of the LiDAR point cloud matching and positioning unit 103, the point cloud data of the vehicle acquired by the vehicle's LiDAR is filtered by voxel grid downsampling to form real-time point cloud data; the point cloud map is generated offline by data acquired by multiple sensors on the vehicle using an optimized fusion mapping algorithm. Based on various sensors with pre-calibrated intrinsic and extrinsic parameters, a high-quality, distortion-free point cloud map with low storage capacity is constructed offline using the fusion mapping algorithm. More preferably, the voxel grid downsampling filtering includes the following steps: using the VoxelGrid class of the open-source point cloud library to create a series of three-dimensional voxel grids based on the point cloud data acquired by the LiDAR; for each three-dimensional voxel grid, the centroid of all points within the three-dimensional voxel grid is used to characterize other points in the three-dimensional voxel grid. The density and number of point clouds after voxel grid downsampling will be significantly reduced, and the surface and other shape features of the original point cloud can be well preserved, thereby improving the matching speed and accuracy of the NDT matching and positioning module 116. The lidar point cloud matching and positioning unit 103 is based on lidar point cloud-assisted positioning technology, which has the advantage of low cost and high robustness and all-weather scene adaptability.

[0075] Preferably, the NDT matching and localization module 116 uses the NDT matching algorithm to match real-time point cloud data and point cloud maps. The NDT matching algorithm process is an iterative optimization and convergence process. In the NDT algorithm parameter configuration, the matching voxel leaf size is set to 3, the number of iterations is 30, the resolution is 1.0, the step size is 0.1, and the minimum tolerance is 0.01. After the optimized iterative matching meets the convergence condition, it will output a real-time, high-precision matching and localization pose. The obtained matching and localization pose is a relatively accurate homogeneous transformation matrix, which, after transformation and conversion, will be used to update the estimated state variables obtained by the IMU pose state variable calculation module 118 in the three-dimensional state error Kalman filter unit 104.

[0076] Preferably, if the NDT matching algorithm fails to match, the NDT matching positioning module 110 is initialized based on the current global coordinates output by the positioning initialization module 115 or the intermediate results obtained by the NDT matching algorithm. As mentioned above, the current global coordinates can be obtained through continuous iterative updates based on GNSS data or IMU data. The NDT matching positioning module 110 is initialized to ensure the real-time and effective operation of the NDT matching algorithm.

[0077] Preferably, in the three-dimensional state error Kalman filter unit 104, the Kalman filter initialization module 117 obtains the initial values ​​of the global pose state variables based on global coordinates through rolling iteration. As mentioned above, the global coordinates can be obtained based on GNSS data or IMU data after transformation.

[0078] Preferably, the IMU pose state quantity calculation module 118 selects a time interval and performs median numerical integration on the IMU data based on the initial value of the global pose state quantity to obtain the estimated state quantity. It is easy to understand that the state quantities for IMU data generally refer to: 3-DOF position, 3-DOF attitude angle (represented by quaternions), 3-DOF linear velocity, 3-DOF IMU angular velocity deviation, and 3-DOF IMU linear acceleration deviation. More preferably, the posterior pose module 121 calculates the vehicle's 3D positioning result based on the updated estimated state quantities and clears the state error quantity to zero.

[0079] It should be noted that, for the three-dimensional state error Kalman filter unit 104, the GNSS clock of the preferred combination positioning subunit 101 is used as a reference, and an existing time alignment algorithm is used to perform time alignment interpolation on the timestamps of the lidar, IMU and vision camera, so that the data output by each sensor has a reasonably aligned timestamp before entering the three-dimensional state error Kalman filter unit 104.

[0080] It will be apparent to those skilled in the art that various modifications and variations can be made to the exemplary embodiments described above without departing from the spirit and scope of the invention. Therefore, it is intended that this invention cover modifications and variations falling within the scope of the appended claims and their equivalents.

Claims

1. A multi-sensor data fusion positioning system, suitable for vehicles, comprising: The combined positioning subunit includes a GNSS module, an RTK service module, and an IMU module. The GNSS module is used to acquire GNSS data, the RTK service module is used to provide real-time motion differential enhancement solution services, and the IMU module is used to acquire vehicle IMU data. The two-dimensional state error Kalman filter unit includes a high-precision map module, a visual lane detection module, a lane matching module, a wheel speed odometer calculation module, and a first state error Kalman filter module. The high-precision map module obtains the vertical coordinates of the lane line where the vehicle is located based on the high-precision map and GNSS data. The visual lane detection module obtains the coordinates of the nearest and farthest points of the lane line where the vehicle is located through the vehicle's visual camera. The lane matching module obtains the lateral update amount of the vehicle based on the output results of the high-precision map module and the visual lane detection module. The wheel speed odometer calculation module is used to obtain the longitudinal update amount of the vehicle. The first state error Kalman filter module is used to fuse the lateral update amount, longitudinal update amount, GNSS data, and IMU data of the vehicle to obtain the two-dimensional positioning result of the vehicle. The lidar point cloud matching and positioning unit includes a point cloud module, a point cloud map module, a positioning initialization module, and an NDT matching and positioning module. The point cloud module acquires the real-time point cloud data of the vehicle through the lidar of the vehicle. The point cloud map module is used to acquire an offline constructed point cloud map. The positioning initialization module is used to acquire the global coordinates of the vehicle. The NDT matching and positioning module acquires the matching and positioning pose of the vehicle based on the real-time point cloud data and the point cloud map. The three-dimensional state error Kalman filter unit includes a Kalman filter initialization module, an IMU pose state quantity calculation module, a judgment module, a second state error Kalman filter module, and a posterior pose module. The Kalman filter initialization module obtains the initial value of the global pose state quantity based on global coordinates. The IMU pose state quantity calculation module calculates the estimated state quantity based on the initial value of the global pose state quantity and IMU data. The judgment module is used to determine whether the lidar point cloud matching and positioning unit outputs a valid matching and positioning pose. If yes, the second state error Kalman filter module updates the estimated state quantity based on the matching and positioning pose, and the posterior pose module calculates the three-dimensional positioning result of the vehicle based on the updated estimated state quantity. If no, the IMU pose state quantity calculation module directly outputs the three-dimensional positioning result based on the estimated state quantity. If the GNSS module can receive GNSS data normally and the RTK service module can provide real-time motion differential enhancement solution service, then the multi-sensor data fusion positioning system outputs the positioning result fused from GNSS data and IMU data through the combined positioning subunit. If the GNSS module can receive GNSS data normally and the RTK service module cannot provide real-time motion differential enhancement solution service, then the multi-sensor data fusion positioning system obtains the two-dimensional positioning result of the vehicle through the two-dimensional state error Kalman filter unit. If the GNSS module cannot receive complete GNSS data, the multi-sensor data fusion positioning system obtains the three-dimensional positioning result of the vehicle through the lidar point cloud matching positioning unit and the three-dimensional state error Kalman filter unit.

2. The multi-sensor data fusion positioning system as described in claim 1, characterized in that, The GNSS data acquired by the first state error Kalman filter module includes the latitude and longitude data of the vehicle. The first state error Kalman filter module obtains the lateral state quantity and longitudinal state quantity of the vehicle based on the latitude and longitude data, and updates and corrects the lateral state quantity and longitudinal state quantity based on the lateral update quantity and longitudinal update quantity. The first state error Kalman filter module obtains the heading angle of the vehicle after fusing the GNSS data and IMU data.

3. The multi-sensor data fusion positioning system as described in claim 1, characterized in that, In the two-dimensional state error Kalman filter unit, the high-precision map module obtains the vertical coordinates of the left and right lane lines closest to the vehicle based on the high-precision map and GNSS data. The visual lane line detection module obtains the coordinates of the nearest and farthest points of the left and right lane lines closest to the front end of the visual camera through the vehicle's visual camera. The lane line matching module obtains the lateral update amount of the vehicle based on the output results of the high-precision map module and the visual lane line detection module.

4. The multi-sensor data fusion positioning system as described in claim 1, characterized in that, The process by which the positioning initialization module obtains the global coordinates of the vehicle includes: The global coordinates obtained after the transformation of the GNSS data are used to achieve positioning initialization, and the global coordinates are continuously updated based on the GNSS data; And / or global coordinates are obtained by numerical integration of the IMU data over short time intervals to achieve positioning initialization, and the global coordinates are continuously updated based on the IMU data.

5. The multi-sensor data fusion positioning system as described in claim 4, characterized in that, In the point cloud module of the lidar point cloud matching and positioning unit, the point cloud data of the vehicle is obtained by the lidar of the vehicle and then filtered by voxel grid downsampling to form real-time point cloud data; the point cloud map is generated offline by data obtained by multiple sensors on the vehicle through an optimized fusion mapping algorithm.

6. The multi-sensor data fusion positioning system as described in claim 5, characterized in that, The voxel grid downsampling filtering includes the following steps: using the VoxelGrid class of the open-source point cloud library to create a series of three-dimensional voxel grids based on the point cloud data acquired by the LiDAR; for each three-dimensional voxel grid, the centroid of all points in the three-dimensional voxel grid is used to characterize other points in the three-dimensional voxel grid.

7. The multi-sensor data fusion positioning system as described in claim 4, characterized in that, The NDT matching and positioning module uses the NDT matching algorithm to match the real-time point cloud data and the point cloud map. In the NDT algorithm parameter configuration, the matching voxel leaf size is set to 3, the number of iteration steps is 30, the resolution is 1.0, the step size is 0.1, and the minimum tolerance is 0.

01.

8. The multi-sensor data fusion positioning system as described in claim 7, characterized in that, If the NDT matching algorithm fails to match, the NDT matching and positioning module is initialized based on the current global coordinates output by the positioning initialization module or the intermediate results obtained by the NDT matching algorithm.

9. The multi-sensor data fusion positioning system as described in claim 1, characterized in that, In the three-dimensional state error Kalman filter unit, the Kalman filter initialization module obtains the initial value of the global pose state quantity based on global coordinates through rolling iteration.

10. The multi-sensor data fusion positioning system as described in claim 9, characterized in that, The IMU pose state quantity calculation module selects a time interval and performs median numerical integration on the IMU data based on the initial value of the global pose state quantity to obtain the estimated state quantity.

Images