A hierarchical fusion positioning method, system, electronic device, and storage medium

Abstract

This invention relates to a hierarchical fusion positioning method, comprising: S1, acquiring a first positioning feature and a second positioning feature through a first information acquisition unit and a second information acquisition unit respectively; a first information fusion unit fusing and calculating a first type of pose estimation value based on the first positioning feature and the pose increment value obtained from the second positioning feature; S2, a second information fusion unit fusing and calculating a second type of pose estimation value based on the first type of pose estimation value, the second positioning feature, and high-precision map features; and positioning the vehicle based on the second type of pose estimation value. This invention provides a hierarchical fusion positioning method that is fully compatible with existing combined positioning devices, reducing hardware design difficulty and manufacturing costs; it also provides two positioning results, improving system robustness.

Description

Technical Field

[0001] This invention relates to a hierarchical fusion positioning method, system, electronic device, and storage medium, belonging to the fields of autonomous driving and positioning. Background Technology

[0002] Traditional vehicles typically rely on a combination of GNSS (Global Navigation Satellite System) and IMU (Inertial Measurement Unit) positioning algorithms to provide information such as vehicle position, speed, and attitude. However, with the development of intelligent driving technology, the accuracy and reliability of GNSS / IMU combined positioning algorithms cannot meet the requirements of downstream perception and control modules, thus necessitating the integration of information from odometers, vision, LiDAR, maps, and other sources.

[0003] Chinese patent CN110221328A discloses a combined navigation method. This method acquires and determines the vehicle's first position and first heading angle based on satellite positioning information, angular velocity information, acceleration information, and visual information collected by the vehicle. It then matches the first position and first heading angle with a pre-set onboard map to determine the congestion information of each lane at the vehicle's location and generates navigation prompts. This method utilizes GNSS, IMU, and visual information to generate position and heading, but it does not use wheel speed and map information to correct the original position and heading, resulting in low positioning accuracy.

[0004] Chinese patent CN111596329A discloses a method for obtaining target positioning information by performing Kalman filtering calculations using GPS data, matched positioning data (LiDAR), and odometer data. This method does not utilize IMU data, resulting in low attitude accuracy. Furthermore, fusing GPS, LiDAR, and odometer data in the same filter places high demands on the computing power of the computing platform and the robustness of the positioning algorithm, and it cannot directly use existing GNSS / IMU combined positioning devices. Summary of the Invention

[0005] To address the shortcomings of the existing technologies, the present invention aims to provide a hierarchical fusion positioning method, system, and apparatus to solve the problems of insufficient utilization of information from multiple sensors such as GNSS, IMU, odometer, vision, and lidar, excessively high computational requirements for fusion positioning algorithms, and insufficient algorithm robustness in the existing technologies.

[0006] This invention provides a hierarchical fusion localization method, the method comprising the following steps:

[0007] S1, the first information acquisition unit and the second information acquisition unit respectively acquire the first positioning feature and the second positioning feature, and the first information fusion unit fuses and calculates the first type of pose estimation value based on the first positioning feature and the pose increment value obtained from the second positioning feature.

[0008] S2, the second information fusion unit calculates and outputs the second type of pose estimation value based on the first type of pose estimation value, the second positioning feature and the high-precision map feature;

[0009] The vehicle is located based on the second type of pose estimation value.

[0010] Furthermore, the first positioning feature includes GNSS position, GNSS heading, IMU acceleration, IMU angular velocity, and odometry velocity, and the second positioning feature includes point cloud data and image information.

[0011] Further, in step S1, the first information fusion unit fuses and calculates the first type of pose estimation value based on the first positioning feature and the pose increment value obtained from the second positioning feature, specifically including the following steps:

[0012] S101, the second positioning feature is used to calculate the pose increment value through the first matching algorithm;

[0013] S102, the first information fusion unit calculates and outputs a first type of pose estimation value by fusing the second positioning feature with the pose increment value.

[0014] Furthermore, the specific steps for calculating the pose increment value in step S101 are as follows: the second positioning feature at the current moment is matched with the second positioning feature at the previous moment using the first matching algorithm to obtain the pose increment value.

[0015] Furthermore, step S2 specifically includes the following steps:

[0016] S201, the second matching algorithm is used to match the second positioning feature, the initial pose estimate, and the high-precision map feature at the current time, and the second type of pose observation value is calculated and output.

[0017] S202, the second information fusion unit is used to fuse and calculate the first positioning features and the second type of pose observations, and output the second type of pose estimation value.

[0018] Furthermore, the initial pose estimate is obtained by the second information fusion unit by fusing the first positioning feature and the first type of pose estimate.

[0019] Furthermore, the second information acquisition unit includes a lidar and a camera, while the first information acquisition unit includes a GNSS, an IMU, and an odometer.

[0020] Further, the first information fusion unit calculates the first type of pose estimation value through a first Kalman filter, wherein the state vector of the first Kalman filter is a 15-dimensional vector, and the 15-dimensional vector includes the IMU's attitude error, velocity error, position error, angular velocity bias, and acceleration bias; and

[0021] The second information fusion unit calculates the second type of pose estimation value through the second Kalman filter, wherein the state vector of the second type of Kalman filter is an 18-dimensional vector, and the 18-dimensional vector includes the attitude error, velocity error, position error, angular velocity bias, acceleration bias and map position offset of the IMU.

[0022] A hierarchical fusion localization system includes a first localization system and a second localization system. The first localization system outputs a first type of pose estimation value, which is provided to the second localization system. The second localization system provides pose increments to the first localization system and outputs a second type of pose estimation value.

[0023] The first positioning system includes a first information acquisition unit and a first information fusion unit. The first information acquisition unit is used to acquire a first positioning feature. The first information fusion unit is used to fuse the pose increment provided by the second positioning system and the first positioning feature, and output a first type of pose estimation value.

[0024] The second positioning system includes a second information acquisition unit, a third information acquisition unit, a first matching unit, a second matching unit, and a second information fusion unit. The second information acquisition unit is used to acquire a second positioning feature, and the third information acquisition unit is used to acquire high-precision map features. The first matching unit is used to match the second positioning feature at the current time with the second positioning feature at the previous time to output a pose increment. The second information fusion unit is used to fuse the first positioning feature and the pose increment to output an initial pose value. The second matching unit is used to match the second positioning feature, the initial pose, and the high-precision map features to output a second type of pose observation value. The second information fusion unit is also used to fuse the second type of pose observation value and the first positioning feature to output a second type of pose estimate value.

[0025] An autonomous vehicle, the autonomous vehicle including the hierarchical fusion positioning system as described above.

[0026] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of a hierarchical fusion positioning method as described above.

[0027] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0028] 1. This invention makes full use of multiple sensors such as GNSS, IMU, odometer, vision and lidar, and can simultaneously output the position in both GNSS coordinate system and map coordinate system, which is convenient for downstream use as needed; at the same time, it provides two redundant positioning results according to different computing performance, which improves the robustness of the positioning system.

[0029] 2. This invention provides two positioning systems, including a first positioning system and a second positioning system, which adopt a hierarchical fusion approach. The first positioning system can run independently in the combined positioning device, which facilitates compatibility with existing combined positioning devices and ensures the real-time performance of the algorithm. At the same time, it is designed with an input interface for pose increment, which can be used to further improve the accuracy of the first type of pose estimation value. The second positioning system can run on a computing platform with strong computing performance, providing the first positioning system with the estimated value of pose increment, and receiving the first type of pose estimation data. Combined with data from LiDAR, camera and high-precision map, it calculates and outputs the second type of pose estimation value, thereby improving the positioning accuracy. Attached Figure Description

[0030] Figure 1 This is a flowchart of a hierarchical fusion positioning method according to the present invention;

[0031] Figure 2 This is a flowchart of step S1 in a hierarchical fusion positioning method of the present invention;

[0032] Figure 3 This is a flowchart of step S2 in the hierarchical fusion positioning method of the present invention;

[0033] Figure 4 This is a structural diagram of a hierarchical fusion positioning system according to the present invention;

[0034] Figure 5 This is a structural diagram of the first positioning module in a hierarchical fusion positioning device of the present invention;

[0035] Figure 6 This is a structural diagram of the second positioning module in a hierarchical fusion positioning device of the present invention. Detailed Implementation

[0036] To enable those skilled in the art to better understand the technical solutions in this application, the technical solutions in 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 the embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0037] The terms "comprising," "including," or any other variations thereof in this application are intended to cover non-exclusive inclusion, meaning that a process, method, article, or system that includes a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element. In this application, "at least one" means one or more, "more" means two or more, "at least two" means two or three or more, and "and / or" describes the relationship between related objects, indicating that three relationships can exist. For example, "A and / or B" can indicate three cases: only A exists, only B exists, and both A and B exist simultaneously, where A and B can be singular or plural. The character " / " generally indicates that the preceding and following related objects are in an "or" relationship. "At least one of the following" or similar expressions refer to any combination of these items. For example, at least one of a, b, or c can be represented as: a, b, c, “a and b”, “a and c”, “b and c”, or “a and b and c”.

[0038] like Figure 1-3 As shown, the present invention provides a hierarchical fusion localization method, which specifically includes the following steps:

[0039] S1, the first information acquisition unit and the second information acquisition unit respectively acquire the first positioning feature and the second positioning feature, and the first information fusion unit fuses and calculates the first type of pose estimation value based on the first positioning feature and the pose increment value obtained from the second positioning feature.

[0040] Specifically, it includes the following steps:

[0041] S101, the second localization feature at the current moment is matched with the second localization feature at the previous moment through the first matching algorithm, and then the pose increment value is calculated.

[0042] Feature matching algorithms are complex and have numerous publications, including ICP (Iterative Nearest Neighbor) type, NDT (Normal Distribution Transform), LOAM type, brute-force matching, and fast approximate nearest neighbor (FLANN), etc. Among them, ICP type algorithms are also known as point cloud matching algorithms. They refer to taking two point clouds Ps and Pt as input and outputting a transformation T such that T(Ps) and T(Pt) have the highest possible degree of overlap. The transformation includes rotation and translation. Point cloud matching algorithms include two steps: coarse registration and fine registration. Coarse registration is a relatively rough registration when the transformation between the two point clouds is completely unknown. Its purpose is mainly to provide a better initial value for the transformation for fine registration. The fine registration criterion is to give an initial transformation and further optimize to obtain a more accurate transformation.

[0043] In this embodiment of the invention, the second positioning feature acquired by the second information acquisition unit is point cloud data and image information. Specifically, the second information acquisition unit includes a detection system and a camera system. The detection system can be a radar, such as a lidar, used to provide point cloud information, and the camera system is a camera used to provide image information. The positioning system acquires point cloud data at the current and previous moments through the lidar and image information at the current and previous moments through the camera. This image information is real-world environmental information of the vehicle's driving scene, including gas stations, buildings, lane markings, roads, trees, pedestrians, etc. Then, the point cloud data at the current moment is matched with the point cloud data at the previous moment using a first matching algorithm to obtain the pose increment of the point cloud data. Simultaneously, the image information at the current moment is matched with the image information at the previous moment to obtain the pose increment of the image information. It should be noted that the matching range is features within a specific distance near the vehicle.

[0044] Specifically, the features extracted by the camera are image information, and the feature extraction methods include Haris, FAST, SIFI, SURF, etc. The feature matching algorithms include brute-force matching and fast approximate nearest neighbor (FLANN) algorithm; the feature matching algorithms of LiDAR mainly include ICP-type algorithms, NDT algorithms, LOAM-type algorithms, etc.

[0045] S102, the first information fusion unit calculates and outputs the first type of pose estimation value by fusing the second positioning feature with the above pose increment value.

[0046] Specifically, the first information acquisition unit includes GNSS, IMU, and odometer. The first positioning features include GNSS position, GNSS heading, IMU acceleration, IMU angular velocity, and odometer velocity. GNSS can be a multi-mode satellite positioning module based on the Global Positioning System, Russia's GLONASS navigation system, the European Union's Galileo navigation system, China's BeiDou-2 navigation system, and satellite-based augmentation systems. The IMU is a device that measures the three-axis attitude angles (or angular rates) and acceleration of an object. Its main components are gyroscopes and accelerometers. The accuracy of the IMU directly affects the accuracy of the inertial system. The odometer is a device that measures the travel distance of a vehicle.

[0047] In this embodiment of the invention, satellite observation data can be received via GNSS and processed to obtain satellite positioning information for the vehicle, specifically the vehicle's position and heading information; the vehicle's acceleration and angular velocity information can be obtained via an IMU, wherein the IMU may include a gyroscope and an accelerometer, which are not specifically limited in this invention; the vehicle's speed information can also be collected via an odometer.

[0048] In this embodiment of the invention, after acquiring the vehicle's GNSS position information, GNSS heading information, IMU acceleration information, IMU angular velocity information, and odometer velocity information, Kalman filtering calculations can be performed using the above information and the pose increments of point cloud data and image information to estimate the vehicle's position and attitude information. This estimate is then provided to the outside world and the next step as the vehicle's first type of pose estimation value. Fusing the pose increments with the first positioning features can further improve the accuracy of the first type of pose estimation value. It should be noted that the first type of pose estimation value is based on pose estimation in the GNSS coordinate system.

[0049] S2, the second information fusion unit calculates and outputs the second type of pose estimation value based on the first type of pose estimation value, the second positioning feature and the high-precision map feature; and locates the vehicle based on the second type of pose estimation value.

[0050] Specifically, it includes the following steps:

[0051] S201, the second information fusion unit is used to fuse the first positioning feature and the first type of pose estimation value to output the initial pose estimation value. The second matching algorithm is used to match the second positioning feature, the initial pose estimation value and the high-precision map feature at the current time to calculate the second type of pose observation value.

[0052] In this embodiment of the invention, the initial pose estimate is output by performing Kalman filtering calculations on the vehicle's GNSS position information, GNSS heading information, IMU acceleration information, IMU angular velocity information, and odometer velocity information with the first type of pose estimate.

[0053] High-precision maps, specifically, are high-precision in two ways. First, they have higher absolute coordinate accuracy, which refers to the accuracy between a target on the map and the actual external world. Second, they contain richer and more detailed road and traffic information elements.

[0054] The initial pose estimate is input as the initial pose for point cloud or image matching. A high-precision map is then matched with the point cloud data to determine the initial pose; that is, the map location points in the high-precision map are matched and filtered based on the data points in the point cloud to determine the point with the highest matching degree, which is then used as the first target location point. Similarly, the high-precision map is matched with the image information to determine the initial pose; that is, the map location points in the high-precision map are matched and filtered based on the real-world information in the image information to determine the point with the highest matching degree, which is then used as the second target location point. The second type of pose observation can be either the first target location point or (and) the second target location point. Point cloud data and image information can supplement areas that sensors cannot detect, providing better feedback on real-time conditions and external information, and obtaining accurate traffic conditions for the current location. Matching these with the high-precision map allows for precise vehicle positioning; combined with the initial pose estimate, the accuracy of vehicle positioning in three-dimensional coordinates can be further improved.

[0055] S202, the second information fusion unit is used to fuse and calculate the first positioning feature and the second type of pose observation value, and output the second type of pose estimation value, and the vehicle is positioned according to the second type of pose estimation value.

[0056] In this embodiment of the invention, a second type of pose estimation value is output by performing Kalman filtering calculations on the vehicle's GNSS position information, GNSS heading information, IMU acceleration information, IMU angular velocity information, and odometer velocity information, along with the second type of pose observation values. It should be noted that the second type of pose estimation value is a pose estimation based on a map coordinate system.

[0057] Based on the above introduction, the vehicle positioning method of this application can be combined with autonomous driving technology to realize autonomous driving or assisted driving. Compared with traditional positioning methods, this invention makes full use of multiple sensors such as GNSS, IMU, odometer, camera, and lidar, and can simultaneously output the position in GNSS coordinate system and map coordinate system. This not only facilitates downstream use on demand, but also effectively improves the reliability of the positioning system and greatly reduces road condition error, sensor error, high-precision map error and algorithm error in traditional positioning methods.

[0058] Specifically, the first information fusion unit calculates the first type of pose estimate using a first Kalman filter, and the second information fusion unit calculates the second type of pose estimate using a second Kalman filter. Kalman filtering is an optimal estimation algorithm that estimates the position / velocity / attitude of the carrier using observations related to motion states such as position / velocity / attitude / angular velocity / acceleration output from raw sensors (GNSS, IMU, odometry) or matching algorithms. Kalman filtering can estimate the state of a dynamic system from a series of data containing measurement noise, provided the measurement variance is known. Kalman filtering iteratively estimates certain parameters optimally, and its fusion calculation formula includes the spatial state equation and the Kalman filtering algorithm equation.

[0059] For the space state equation:

[0060] X k =Φ k / k-1 X k-1 +w k ,

[0061] Z k =H k X k +v k ,

[0062] In the formula, X is the state vector, Z is the observation vector, and Φ k / k-1 H is the state transition matrix from time k-1 to time k. k Let w be the observation matrix at time k. k v is the state noise at time k. k Let be the observation noise at time k.

[0063] The Kalman filter algorithm equation is as follows:

[0064]

[0065]

[0066]

[0067]

[0068] P k =(IK k H k )P k / k-1 ,

[0069] In the formula, K is the Kalman gain matrix; P is the error covariance matrix, representing the uncertainty of the state vector X; Q is the state noise matrix, representing the noise of the state vector X; and R is the observation noise matrix, representing the noise of the observation Z.

[0070] Specifically, for the first Kalman filter in step S1, the state vector X is a 15-dimensional vector, whose elements are the IMU's attitude error, velocity error, position error, angular velocity bias, and acceleration bias, respectively. Bias is one of the acceleration or angular velocity calibration methods, i.e., the offset of the sensor's measured value relative to zero. In this invention, both acceleration bias and angular velocity bias are obtained through Kalman filtering estimation. Introducing acceleration bias and angular velocity bias can further calibrate the pose to improve pose accuracy. Observation Z represents the position error, heading error, velocity error, and pose increment, where the position error is the difference between the GNSS position and the IMU position, the heading error is the difference between the GNSS heading and the IMU heading, and the velocity error is the difference between the odometer velocity and the IMU velocity. The first Kalman filter estimates the attitude error, velocity error, position error, angular velocity bias, and acceleration bias of the IMU. Then, the estimated errors are subtracted from the attitude, velocity, and position of the IMU to obtain the estimated values ​​of the IMU pose. It is assumed that the IMU is precisely installed on the vehicle, and the pose of the IMU represents the pose of the vehicle.

[0071] Specifically, for the second Kalman filter in step S201, the elements of the state vector X are the attitude error, velocity error, position error, angular velocity bias, and acceleration bias of the IMU, respectively, and the observation Z is the position error, heading error, velocity error, and the first type of pose estimation value. The second Kalman filter outputs the initial pose estimation value.

[0072] Specifically, for the second Kalman filter in step S202, the state vector X is an 18-dimensional state vector, whose elements are the IMU's attitude error, velocity error, position error, angular velocity bias, acceleration bias, and map position offset, respectively. The attitude error, velocity error, position error, and map position offset are sub-states in the east, north, and sky directions, while the angular velocity bias and acceleration bias are sub-states in the X, Y, and Z directions. The observation Z consists of the first position error, the second position error, the attitude error, and the velocity error. The first position error includes one or more of the following: the difference between the position matched by the LiDAR and the map and the IMU position; the second position error includes the difference between the GNSS position and the IMU position; the attitude error includes one or more of the following: the difference between the attitude matched by the LiDAR and the map and the IMU attitude; the velocity error is the difference between the odometry velocity and the IMU velocity. Other parameters are the same as those of the first Kalman filter. It should be noted that the map position offset is estimated by the second Kalman filter algorithm. Introducing the map position offset can minimize the impact of map errors on vehicle positioning and improve the consistency between the vehicle's position on the map and its actual location in the environment.

[0073] This invention proposes a hierarchical fusion positioning method. By acquiring and utilizing point cloud data and image information collected by the vehicle, a matching algorithm is used to obtain the corresponding pose increment value. Then, the vehicle's GNSS position and heading information, IMU acceleration and angular velocity information, and odometer velocity information are acquired. These information and the pose increment are fused using a Kalman filter algorithm to output a first type of pose estimate. The first type of pose estimate is input into the Kalman filter algorithm to obtain an initial pose. Then, a matching algorithm is used to match point cloud data features, image information features, the initial pose estimate, and a high-precision map to obtain a second type of pose observation value. Finally, the second type of pose observation value is fused with the first positioning feature to output a second type of pose estimate value. This hierarchical fusion positioning method can simultaneously output the vehicle position in both the GNSS coordinate system and the map coordinate system, facilitating downstream use as needed. Furthermore, depending on the hardware computing performance, two sets of positioning algorithms can be run simultaneously, providing two positioning results, thus improving the robustness of the positioning system.

[0074] It should be noted that, for the sake of simplicity, the method embodiments are all described as a series of actions. However, those skilled in the art should understand that the embodiments of the present invention are not limited to the described order of actions, because according to the embodiments of the present invention, some steps can be performed in other orders or simultaneously. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are preferred embodiments, and the actions involved are not necessarily essential to the embodiments of the present invention.

[0075] like Figure 4 The diagram shows a structural diagram of an embodiment of a hierarchical fusion positioning system according to the present invention. The system may specifically include a first positioning system and a second positioning system. The first positioning system is used to output a first type of pose estimation value. The first type of pose estimation is provided to the outside world and the second positioning system respectively. The second positioning system is used to provide pose increments to the first positioning system and output a second type of pose estimation value to the outside world.

[0076] This positioning system can be divided into two parts: a first positioning system and a second positioning system. The first positioning system can run independently within a combined positioning device, facilitating compatibility with existing devices. This ensures the real-time performance of the algorithm while reducing hardware design complexity and manufacturing costs. An input interface for pose increments is designed, which can further improve the accuracy of the first type of pose estimation output by the first positioning system. The second positioning system runs on a high-performance computing platform, providing the first positioning system with pose increment estimates. Simultaneously, it receives the first type of pose estimation data and, combined with data from LiDAR, camera, and high-precision maps, calculates the second type of pose estimation. This system simultaneously outputs both the first and second type of pose estimations for positioning. By providing two different positioning results, the robustness of the positioning system can be improved.

[0077] Furthermore, the first positioning system includes a first information acquisition unit and a first information fusion unit. The first information acquisition unit includes GNSS, IMU, and odometry, and is used to acquire first positioning features. The first information fusion unit is used to fuse the pose increment provided by the second positioning system and the second positioning features, and output a first type of pose estimate.

[0078] Specifically, in the first positioning system, the GNSS outputs observations of the vehicle's position and heading, the IMU outputs data on the vehicle's angular velocity and acceleration, and the odometer outputs the vehicle's forward speed. The first Kalman filter receives pose increment data from the GNSS / IMU / odometer and externally provided data, estimates the vehicle's position, velocity, and attitude, and provides this as a first-type pose estimate (in the GNSS coordinate system) to the second positioning system.

[0079] Furthermore, the second positioning system includes a second information acquisition unit, a third information acquisition unit, a first matching unit, a second matching unit, and a second information fusion unit. The second information acquisition unit includes a lidar and a camera, used to acquire second positioning features. The third information acquisition unit is used to acquire high-precision map features. The first matching unit is used to match the first positioning features at the current time with the second positioning features at the previous time to output a pose increment value. The second information fusion unit is used to fuse the first positioning features and a first type of pose estimation to output an initial pose estimation value. The second matching unit is used to match the second positioning features, the initial pose estimation value, and the high-precision map features to output a second type of pose observation value. The second information fusion unit is also used to fuse the second type of pose observation value and the first positioning features to output a second type of pose estimation value.

[0080] Specifically, in the second positioning system, based on the pose estimated by the Kalman filter, the data from the LiDAR / camera, and the high-precision map data, this module uses the second matching algorithm to calculate the vehicle's pose in map coordinates, which is then used as the second type of pose observation. The second Kalman filter uses GNSS, IMU, odometer, and the second type of pose observation to calculate the IMU's pose estimate, adds the map offset to it, and outputs the pose result of the second positioning system (in map coordinates).

[0081] As the system implementation is basically similar to the method implementation, it is described in a relatively simple way. For relevant details, please refer to the description of the method implementation.

[0082] like Figure 5-6 The illustration shows a hierarchical fusion positioning device, which includes a first positioning module and a second positioning module, and the first positioning module and the second positioning module are communicatively connected.

[0083] The first positioning module includes a GNSS board, an IMU module, a microcontroller unit, a first storage unit, a power conversion unit, a bus communication module, a radar data interface, and a camera data interface. The GNSS board provides observation information on vehicle position, speed, and heading. The IMU module provides information such as acceleration, angular velocity, and temperature. The microcontroller unit's main function is to perform Kalman filtering fusion processing on the incremental pose data matched by GNSS, IMU, odometer, radar, and camera, and output reliable pose information to the outside world. The bus communication module includes an antenna interface and a bus interface. The antenna interface connects to an external GNSS antenna to provide raw electrical signals to the GNSS board. The bus interface receives odometer and GNSS differential correction data, and outputs pose data estimated by the positioning algorithm. The storage unit stores configuration files for the positioning algorithm, raw data input from external sources, and other contents.

[0084] The second positioning module is an on-board computing platform, which includes a computing unit and a second storage unit. The second positioning module transmits data with the outside world through a network switch.

[0085] This invention provides an autonomous vehicle, which includes the hierarchical fusion positioning system described above.

[0086] A computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of a hierarchical fusion positioning method.

[0087] The computer-readable medium described in this disclosure can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this disclosure, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in connection with an instruction execution system, apparatus, or device. In this disclosure, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals can take various forms, including, but not limited to, electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium can be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wires, optical fibers, RF (radio frequency), etc., or any suitable combination thereof.

[0088] The aforementioned computer-readable medium may be included in the aforementioned electronic device; or it may exist independently and not assembled into the electronic device.

[0089] Computer program code for performing the operations of this disclosure can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, and C++, and conventional procedural programming languages ​​such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (AN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0090] The foregoing description of specific exemplary embodiments of the invention is for illustrative and explanatory purposes. These descriptions are not intended to limit the invention to the precise forms disclosed, and it will be apparent that many changes and variations can be made in accordance with the foregoing teachings. The exemplary embodiments were chosen and described in order to explain the specific principles of the invention and its practical application, thereby enabling those skilled in the art to implement and utilize various different exemplary embodiments of the invention, as well as various different choices and variations. The scope of the invention is intended to be defined by the claims and their equivalents.

Claims

1. A hierarchical fusion positioning method, characterized in that, The method includes the following steps: S1, the first information acquisition unit and the second information acquisition unit respectively acquire the first positioning feature and the second positioning feature, and the first information fusion unit fuses and calculates the first type of pose estimation value based on the first positioning feature and the pose increment value obtained from the second positioning feature. S2, the second information fusion unit calculates and outputs a second type of pose estimation value based on the first type of pose estimation value, the second positioning feature, and the high-precision map feature. The second information fusion unit calculates the second type of pose estimation value using a second Kalman filter, where the state vector of the second Kalman filter includes map position offset. Step S2 specifically includes the following steps: S201, matching the current second positioning feature, the initial pose estimation value, and the high-precision map feature using a second matching algorithm to calculate and output a second type of pose observation value; S202, using the second information fusion unit to perform fusion calculation on the first positioning feature and the second type of pose observation value to output a second type of pose estimation value; the initial pose estimation value is obtained by the second information fusion unit through fusion calculation of the first positioning feature and the first type of pose estimation value. Simultaneously output the first type of pose estimation value and the second type of pose estimation value to locate the vehicle.

2. The hierarchical fusion positioning method according to claim 1, characterized in that, The first positioning feature includes GNSS position, GNSS heading, IMU acceleration, IMU angular velocity, and odometry velocity; the second positioning feature includes point cloud data and image information.

3. The hierarchical fusion positioning method according to claim 1, characterized in that, The first information fusion unit in step S1 calculates and outputs a first type of pose estimation value based on the first positioning feature and the pose increment value obtained from the second positioning feature, specifically including the following steps: S101, the second positioning feature is used to calculate the pose increment value through the first matching algorithm; S102, the first information fusion unit calculates and outputs a first type of pose estimation value by fusing the second positioning feature with the pose increment value.

4. The hierarchical fusion positioning method according to claim 3, characterized in that, The specific steps for calculating the pose increment value in step S101 are as follows: the second positioning feature at the current moment is matched with the second positioning feature at the previous moment using the first matching algorithm to obtain the pose increment value.

5. The hierarchical fusion positioning method according to claim 1, characterized in that, The second information acquisition unit includes a lidar and a camera, while the first information acquisition unit includes a GNSS, an IMU, and an odometer.

6. The hierarchical fusion positioning method according to claim 1, characterized in that, The first information fusion unit calculates the first type of pose estimation value through a first Kalman filter, wherein the state vector of the first Kalman filter is a 15-dimensional vector, and the 15-dimensional vector includes the IMU's attitude error, velocity error, position error, angular velocity bias, and acceleration bias; and The state vector of the second Kalman filter is an 18-dimensional vector, which also includes the attitude error, velocity error, position error, angular velocity bias, and acceleration bias of the IMU.

7. A hierarchical fusion positioning system, characterized in that, The system includes a first positioning system and a second positioning system. The first positioning system is used to output a first type of pose estimation value, which is provided to the second positioning system. The second positioning system is used to provide pose increment values ​​to the first positioning system and output a second type of pose estimation value. The first positioning system includes a first information acquisition unit and a first information fusion unit. The first information acquisition unit is used to acquire a first positioning feature. The first information fusion unit is used to fuse the pose increment value provided by the second positioning system and the first positioning feature, and output a first type of pose estimation value. The second positioning system includes a second information acquisition unit, a third information acquisition unit, a first matching unit, a second matching unit, and a second information fusion unit. The second information acquisition unit is used to acquire a second positioning feature, and the third information acquisition unit is used to acquire high-precision map features. The first matching unit is used to match the second positioning feature at the current time with the second positioning feature at the previous time to output a pose increment value. The second information fusion unit is used to fuse the first positioning feature and a first type of pose estimation value to output an initial pose estimation value. The second matching unit is used to match the second positioning feature, the initial pose estimation value, and the high-precision map features using a second matching algorithm to output a second type of pose observation value. The second information fusion unit is also used to fuse the second type of pose observation value and the first positioning feature to output a second type of pose estimation value. The second information fusion unit calculates the second type of pose estimation value using a second Kalman filter, and the state vector of the second Kalman filter includes map position offset.

8. An autonomous vehicle, characterized in that, The autonomous vehicle includes the hierarchical fusion positioning system as described in claim 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of a hierarchical fusion positioning method as described in any one of claims 1-6.

Images