Positioning Method, System, Device and Storage Medium Based on Multi-Sensor Fusion

Abstract

The present invention provides a positioning method, system, device and storage medium based on multi-sensor fusion. The method includes: obtaining lidar measurement data of a carrier from a lidar, and extracting lidar factors from the lidar measurement data; obtaining inertial measurement data from an inertial measurement unit, and extracting IMU pre-integration factors from the inertial measurement data; obtaining millimeter-wave radar measurement data from a millimeter-wave radar, and extracting millimeter-wave radar factors from the millimeter-wave radar measurement data; performing factor graph optimization based on the lidar factors, the IMU pre-integration factors and the millimeter-wave radar factors to obtain positioning data of the carrier. The present invention improves the accuracy of mapping and positioning in various scenarios.

Description

Background Art

[0002] In applications such as subway anti-collision, community patrol, and inspection of public road infrastructure, it is necessary to obtain the position and attitude information of the sensor solution body in the environment in real time, so as to further determine the global position of the target object in the environmental map in combination with the perception algorithm solution.

[0003] In the subway tunnel scenario, there are long roads with high similarity. The walls are cement walls, which are attached with cables and substation equipment. The cross-section is truncated circular or square. The lighting conditions are poor, and most objects appear grayish-black, lacking texture features, resulting in instability in feature point extraction by Visual-Inertial Odometry (VO). Also, due to being underground, it is impossible to receive GPS (Global Positioning System) signals, so there are many difficulties in mapping and positioning.

[0004] It should be noted that the information disclosed in the above background art section is only used to enhance the understanding of the background of the present invention, and thus may include information that does not constitute the prior art known to those of ordinary skill in the art. Summary of the Invention

[0005] Aiming at the problems in the prior art, the purpose of the present invention is to provide a positioning method, system, device and storage medium based on multi-sensor fusion to improve the accuracy of mapping and positioning in various scenarios.

[0006] An embodiment of the present invention provides a positioning method based on multi-sensor fusion, including the following steps:

[0007] Obtain the lidar measurement data of the carrier from the lidar, and extract the lidar factor from the lidar measurement data;

[0008] Obtain the inertial measurement data from the inertial measurement unit, and extract the IMU pre-integration factor from the inertial measurement data;

[0009] Obtain the millimeter-wave radar measurement data from the millimeter-wave radar, and extract the millimeter-wave radar factor from the millimeter-wave radar measurement data;

[0010] Perform factor graph optimization based on the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor to obtain the positioning data of the carrier.

[0011] In some embodiments, extracting the millimeter-wave radar factor from the millimeter-wave radar measurement data includes the following steps:

[0012] Calculate a histogram based on the millimeter-wave radar measurement data;

[0013] Find the speed interval with the largest count value from the histogram;

[0014] Take the speed mean of the speed interval with the largest count value as the millimeter-wave measurement speed, and take the millimeter-wave measurement speed as the millimeter-wave radar factor.

[0015] In some embodiments, after taking the speed mean of the speed interval with the largest count value as the millimeter-wave measurement speed, the following steps are further included:

[0016] Incorporate the millimeter-wave measurement speed into the x-axis position adjacent to the lidar frame [0] point.

[0017] In some embodiments, the performing factor graph optimization to obtain the positioning data of the carrier includes the following steps:

[0018] Input the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor into the residual block to obtain the positioning data of the carrier output by the residual block.

[0019] In some embodiments, after inputting the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor into the residual block, the following steps are further included:

[0020] Calculate the cost function based on the millimeter-wave measurement speed, the inertial measurement unit rotation matrix of the current frame, and the carrier state parameters.

[0021] In some embodiments, the calculating the cost function includes the following steps:

[0022] After left-multiplying the millimeter-wave measurement speed by the inertial measurement unit rotation matrix of the current frame, perform residual calculations with the speed values in the x-axis, y-axis, and z-axis directions in the carrier state parameters respectively to obtain the state speed quantity residuals, which serve as the calculated cost function.

[0023] In some embodiments, when inputting the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor into the residual block, it further includes obtaining the carrier state parameters based on the lidar measurement data and the inertial measurement data and inputting them into the residual block.

[0024] In some embodiments, extracting the lidar factor from the lidar measurement data includes the following steps:

[0025] Extract the lidar point cloud features of the current frame from the lidar measurement data, where the lidar point cloud features include surface feature point features, line feature point features, and irregular point features;

[0026] Obtain the current point cloud map;

[0027] Perform point cloud feature matching based on the lidar point cloud features of the current frame and the point cloud map, and obtain the predicted lidar position as the lidar factor.

[0028] In some embodiments, the following steps are further included:

[0029] Determine whether the current mode is the mapping mode;

[0030] If it is the mapping mode, construct a point cloud map based on the lidar measurement data within a preset time window.

[0031] In some embodiments, when constructing a point cloud map based on the lidar measurement data within a preset time window, implement the construction, saving, and reading of the point cloud map based on a thread parallel to the lidar measurement data, and perform pre-rasterization processing on the saved point cloud map to obtain multiple grids of preset sizes;

[0032] When performing point cloud feature matching based on the lidar point cloud features and the point cloud map, use the point cloud data in the preset number of grids before and after the carrier position in the point cloud map to perform point cloud feature matching with the lidar point cloud features of the current frame.

[0033] An embodiment of the present invention further provides a positioning system based on multi-sensor fusion for implementing the positioning method based on multi-sensor fusion. The system includes:

[0034] A first data processing module for obtaining the lidar measurement data of the carrier from the lidar and extracting the lidar factor from the lidar measurement data;

[0035] A second data processing module for obtaining inertial measurement data from an inertial measurement unit and extracting the IMU pre-integration factor from the inertial measurement data;

[0036] A third data processing module for obtaining millimeter-wave radar measurement data from a millimeter-wave radar and extracting the millimeter-wave radar factor from the millimeter-wave radar measurement data;

[0037] A carrier positioning module for performing factor graph optimization based on the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor to obtain the positioning data of the carrier.

[0038] An embodiment of the present invention further provides a positioning device based on multi-sensor fusion, including:

[0039] A processor;

[0040] A memory storing executable instructions of the processor;

[0041] Wherein, the processor is configured to execute the steps of the multi-sensor fusion-based positioning method by executing the executable instructions.

[0042] An embodiment of the present invention further provides a computer-readable storage medium for storing a program, which when executed by a processor implements the steps of the multi-sensor fusion-based positioning method.

[0043] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, and cannot limit the present disclosure.

[0044] The multi-sensor fusion-based positioning method, system, device, and storage medium of the present invention have the following beneficial effects:

[0045] The present invention first separately obtains the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor, and then jointly optimizes the factor graph of the three factors to obtain the positioning data of the carrier. The millimeter-wave radar factor can provide a stable speed constraint, thereby improving the stability and accuracy of mapping and positioning. The present invention can be applied to subway tunnel scenarios, such as positioning the subway itself, and can be used as the basis for anti-collision control. The millimeter-wave radar provides the relative speed of static objects in the environment. The present invention can also be applied to other mapping and positioning scenarios, such as streets, communities, enclosed spaces, etc. Description of the Drawings

[0046] By reading the detailed description of the non-limiting embodiments with reference to the following drawings, other features, objectives, and advantages of the present invention will become more obvious.

[0047] Figure 1 is a flowchart of the multi-sensor fusion-based positioning method according to an embodiment of the present invention;

[0048] Figure 2 is a flowchart of extracting the lidar factor from the lidar measurement data according to an embodiment of the present invention;

[0049] Figure 3 is a flowchart of extracting the millimeter-wave radar factor from the millimeter-wave radar measurement data according to an embodiment of the present invention;

[0050] Figure 4 is a schematic structural diagram of the multi-sensor fusion-based positioning system according to an embodiment of the present invention;

[0051] Figure 5 is a schematic structural diagram of the multi-sensor fusion-based positioning device according to an embodiment of the present invention;

[0052] Figure 6 is a schematic structural diagram of the computer-readable storage medium according to an embodiment of the present invention. Detailed implementation manners

[0053] Example embodiments will now be described more fully with reference to the accompanying drawings. However, the example embodiments can be implemented in various forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of the example embodiments to those skilled in the art. The features, structures, or characteristics described may be combined in any suitable manner in one or more embodiments.

[0054] In addition, the accompanying drawings are only schematic illustrations of the present disclosure and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus repeated descriptions thereof will be omitted. Some of the block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities may be implemented in software form, or in one or more hardware modules or integrated circuits, or in different networks and / or processor devices and / or microcontroller devices.

[0055] The flowcharts shown in the accompanying drawings are merely illustrative and do not necessarily include all the steps. For example, some steps may be decomposed, while some steps may be combined or partially combined. Therefore, the actual execution order may be changed according to the actual situation.

[0056] As Figure 1 shown, an embodiment of the present invention provides a positioning method based on multi-sensor fusion, including the following steps:

[0057] S100: Obtain the lidar measurement data of the vehicle from the lidar, and extract lidar factors from the lidar measurement data;

[0058] S200: Obtain inertial measurement data from an inertial measurement unit, and extract IMU pre-integration factors from the inertial measurement data; IMU is the Inertial Measurement Unit, the inertial measurement unit;

[0059] The IMU includes a three-axis accelerometer and a three-axis gyroscope, which respectively measure the acceleration and angular velocity information of an object and are not affected by external factors such as the surrounding environmental structure and light; the IMU can obtain the angular velocity and acceleration values at the current moment, and the system state (position, velocity, attitude) can be estimated through these values;

[0060] The general use of an IMU is to integrate the measured acceleration and angular velocity to calculate the pose of the carrier. However, due to the integration relationship, the pose drift obtained by integrating the IMU will increase with the increase of the integration time. On the other hand, when the optimization method optimizes the pose at a historical moment, the IMU integration value at a subsequent moment needs to be re-integrated, resulting in a large amount of calculation. Therefore, IMU pre-integration is proposed to solve the above problems. Briefly speaking, IMU pre-integration describes the observable quantity of IMU data between lidar frames (with a relatively short time interval, such as the lidar frame interval is usually 100 ms). It is only related to the IMU state quantity at the previous frame moment. Therefore, during the optimization process, the amount of calculation is small. As an observable quantity, IMU pre-integration is naturally used as a constraint for SLAM pose optimization;

[0061] S300: Obtain millimeter-wave radar measurement data from the millimeter-wave radar, and extract millimeter-wave radar factors from the millimeter-wave radar measurement data;

[0062] S400: Perform factor graph optimization based on the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor to obtain the positioning data of the carrier.

[0063] We observe that there are almost no other dynamic objects in the subway tunnel scenario. The millimeter-wave radar provides the relative speed of static objects in the environment. After processing with a speed histogram, the data of the millimeter-wave radar can provide a stable speed constraint for the inertial odometer in the forward direction in an environment where geometric structures such as tunnels degenerate. Incorporating it as an independent factor into the optimization process of the lidar-inertial odometer can improve the stability of mapping and positioning.

[0064] Specifically, in the positioning method of the present invention, first, the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor are obtained through steps S100 to S300 respectively. Then, through step S400, the three factors are jointly optimized by a factor graph to obtain the positioning data of the carrier. The millimeter-wave radar factor can provide a stable speed constraint in the x direction, thereby improving the stability and accuracy of mapping and positioning. When applying this method to the positioning of a subway train in a subway tunnel, the carrier is the subway train, and the obtained positioning data of the carrier includes at least the position of the train itself (including the coordinate values of the x-axis, y-axis, and z-axis in three directions) and the speed values in three directions (x-axis, y-axis, and z-axis directions), etc. This method can also be applied to other scenarios, and the carrier corresponds to vehicles or other objects in other scenarios, such as the carrier being a community patrol vehicle, etc.

[0065] In the positioning method based on multi-sensor fusion of this embodiment, the serial numbers of each step are only used to distinguish each step, rather than limiting the specific execution order of each step. The execution order among the above steps can be adjusted and changed as needed. For example, steps S100, S200, and S300 can be carried out simultaneously or sequentially. When carried out sequentially, the execution order can also be adjusted as needed.

[0066] As Figure 2 shown, in this embodiment, step S100: Extract lidar factors from the lidar measurement data, including the following steps:

[0067] S110: Extract the lidar point cloud features of the current frame from the lidar measurement data. The lidar point cloud features include surface feature point features, line feature point features, and irregular point features;

[0068] Among them, the surface feature point features refer to the feature point features corresponding to ultra-small curvature, the line feature point features refer to the feature point features corresponding to large curvature, and the irregular point features refer to the feature point features corresponding to intermediate curvature;

[0069] S120: Obtain the current point cloud map; this point cloud map is the point cloud map constructed based on the lidar measurement data before;

[0070] S130: Perform point cloud feature matching based on the lidar point cloud features of the current frame and the point cloud map, and obtain the lidar predicted position as the lidar factor.

[0071] In this embodiment, the positioning method further includes the following steps:

[0072] Judge whether it is in the mapping mode currently;

[0073] If it is in the mapping mode, construct a point cloud map based on the lidar measurement data within a preset time window. The present invention establishes a high-precision point cloud map through the mapping mode. When applied to the subway scenario, the mapping error for 4 consecutive subway stations is within 10 m.

[0074] In this embodiment, when constructing a point cloud map based on the lidar measurement data within a preset time window, implement the construction, saving, and reading of the point cloud map based on a thread parallel to the lidar measurement data, and perform pre-rasterization processing on the saved point cloud map to obtain multiple grids of preset sizes. Here, the preset size can be selected as needed. For example, save the point cloud map as a local map with grids of 50 m * 50 m * 50 m.

[0075] When performing point cloud feature matching based on the lidar point cloud features and the point cloud map, the point cloud data in a preset number of grids before and after the carrier position in the point cloud map is used to perform point cloud feature matching with the lidar point cloud features of the current frame, which can avoid performing point cloud feature matching with the entire point cloud map, thereby greatly improving the speed of point cloud feature matching and also improving the efficiency of carrier positioning. The preset number here can be set as needed. For example, when it is set to 3, only the point cloud features in the 3 grids before and after the current position point are used to match the lidar frame in the current window during positioning matching, reducing the search and matching space, improving the utilization efficiency of features, and effectively saving computing resources.

[0076] As Figure 3 shown, in this embodiment, in the step S300, extracting the millimeter-wave radar factor from the millimeter-wave radar measurement data includes the following steps:

[0077] S310: Calculate a histogram based on the millimeter-wave radar measurement data;

[0078] Specifically, when calculating the histogram, it includes the following steps:

[0079] First, select the speed value interval. For example, select the speed value interval to be 0.1 km / h;

[0080] Based on the millimeter-wave radar measurement data, determine the maximum speed and the minimum speed, determine the speed value range, and divide the speed value range into multiple speed intervals;

[0081] Based on the millimeter-wave radar measurement data, obtain multiple speed measurement values, and respectively assign the multiple speed measurement values to the corresponding speed ranges to generate a histogram. The horizontal axis of this histogram represents the speed value, and the vertical axis represents the number of speed measurement values falling into each speed range;

[0082] S320: Find the speed interval with the largest count value in the histogram; that is, find the speed interval with the largest number of speed measurement values;

[0083] S330: Take the speed mean value of the speed interval with the largest count value as the millimeter-wave measurement speed, and take the millimeter-wave measurement speed as the millimeter-wave radar factor. This speed mean value is the average of the minimum speed and the maximum speed of this speed interval.

[0084] As Figure 3 shown, in this embodiment, after the step S330: taking the speed mean value of the speed interval with the largest count value as the millimeter-wave measurement speed, it further includes the following steps:

[0085] S340: Incorporate the millimeter-wave measured speed into the x-axis position adjacent to the origin of the lidar frame to facilitate subsequent access to the millimeter-wave radar factor.

[0086] In this embodiment, the Ceres library is used to optimize various factors, and finally the positioning data of the carrier is published. Specifically, in the step S400, factor graph optimization is performed to obtain the positioning data of the carrier, including the following steps:

[0087] Input the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor into the residual block addresidualblock to obtain the positioning data of the carrier output by the residual block. In the present invention, by adding the millimeter-wave radar factor, the millimeter-wave measured speed is involved in the construction of the residual of the state quantity Vx in the local coordinate system.

[0088] Ceres is a non-linear optimization library, which is widely used in solving problems such as the BA problem in SLAM. SLAM (Simultaneous Localization and Mapping) is short for Simultaneous Localization and Mapping or Concurrent Mapping and Localization. Its main function is to enable the carrier to complete positioning (Localization), mapping (Mapping), and path planning (Navigation) in an unknown environment. Using Ceres to solve non-linear optimization problems generally consists of three parts: 1. The first part: construct the cost function, that is, the objective function for optimization. 2. The second part: construct the optimization problem to be solved through the cost function. 3. The third part: configure the solver parameters and solve the problem. This step is to set how the equation is solved and whether the solution process is output, etc.

[0089] The parameters of AddResidualBlock are as follows:

[0090] ResidualBlockId Problem::AddResidualBlock(CostFunction*,LossFunction*,const vector<double*>parameter_blocks);

[0091] Among them, AddResidualBlock() is a method under Problem, which is to add residual blocks to the object of Problem, and several can be added as needed. There are three parameters in the residual block. cost_function is the cost function; loss_function is generally nullptr, with a default value of 1; the third is the variable to be optimized, and the VBias state parameter (specifically introduced below) is passed in this embodiment.

[0092] In this embodiment, after inputting the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor into the residual block, the following steps are further included:

[0093] Calculate the cost function based on the millimeter-wave measurement velocity, the rotation matrix of the inertial measurement unit of the current frame, and the vehicle state parameters.

[0094] In this embodiment, when constructing the cost function, that is, the objective formula for optimization. This part needs to use the technique of functor. The method is to define a structure of the cost function, overload the () operator within the structure, and construct the least squares result of the difference between the obtained state velocity and the millimeter-wave radar velocity.

[0095] Specifically, in this embodiment, the calculation of the cost function includes the following steps:

[0096] After left-multiplying the millimeter-wave measurement velocity by the rotation matrix of the inertial measurement unit of the current frame, perform residual calculations on the left-multiplication result and the velocity values in the x-axis, y-axis, and z-axis directions of the vehicle state parameters respectively to obtain the state velocity quantity residuals, which are used as the cost function costfunction in the calculation.

[0097] In this embodiment, when inputting the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor into the residual block, it also includes obtaining the vehicle state parameters based on the lidar measurement data and the inertial measurement data and inputting them into the residual block. Here, the vehicle state parameters are the VBias state parameters, and the first three of them correspond to the velocity values in the x-axis, y-axis, and z-axis directions. VBias is the array where the state quantity variables are stored, with a dimension of [window value][9]. The odometer in this embodiment contains 5 state parameters, including: position P, velocity V, angle R, IMU acceleration bias Ba, and IMU angular velocity bias Bg, and each state has 3 dimensions. The VBias state parameters include velocity V, IMU acceleration bias Ba, and IMU angular velocity bias Bg. These 3 states have a total of 9 dimensions. The VBias state parameters can also be used as a factor and passed into the residual block AddResidualBlock for optimization.

[0098] In this embodiment, in step S400, the factors input to the residual block may further include other types of factors, such as marginalization factors. Marginalization refers to removing the oldest frame or the second newest frame in the sliding window, that is, removing some older or unsatisfactory visual frames in the sliding window. Therefore, marginalization is also described as the process of decomposing the joint probability distribution into the marginal probability distribution and the conditional probability distribution.

[0099] The marginalization objective is to no longer calculate the pose of this frame, and even may not calculate some landmarks related to this frame, but it is hoped to retain the constraint relationship of this frame to other frames in the sliding window, so that the constraint information will not be reduced, which is beneficial to optimizing the state variables. When removing the pose or feature, it is necessary to transform the associated constraints into a constraint term as a prior and put it into the optimization problem. This is what marginalization does. Otherwise, when removing the constraints from the window and losing all the constraints associated with them, the solution accuracy will decrease, and when the vehicle undergoes degenerate motion (such as: uniform motion), it is impossible to solve without historical information as a constraint.

[0100] As Figure 4 shown, an embodiment of the present invention further provides a positioning system based on multi-sensor fusion for implementing the positioning method based on multi-sensor fusion. The system includes:

[0101] The first data processing module M100 is used to obtain the lidar measurement data of the vehicle from the lidar and extract the lidar factors from the lidar measurement data;

[0102] The second data processing module M200 is used to obtain the inertial measurement data from the inertial measurement unit and extract the IMU pre-integration factors from the inertial measurement data;

[0103] The third data processing module M300 is used to obtain the millimeter-wave radar measurement data from the millimeter-wave radar and extract the millimeter-wave radar factors from the millimeter-wave radar measurement data;

[0104] The vehicle positioning module M400 is used to perform factor graph optimization based on the lidar factors, the IMU pre-integration factors, and the millimeter-wave radar factors to obtain the positioning data of the vehicle.

[0105] In the positioning system of the present invention, first, the lidar factor, the IMU pre-integration factor, and the millimeter-wave radar factor are respectively obtained through the first data processing module M100, the second data processing module M200, and the third data processing module M300. Then, the carrier positioning module M400 jointly optimizes the three factors through a factor graph to obtain the positioning data of the carrier. The millimeter-wave radar factor can provide a stable speed constraint, thereby improving the stability and accuracy of mapping and positioning. When applying this system to the positioning of a subway train in a subway tunnel, the carrier is the subway train, and the obtained positioning data of the carrier includes at least the position of the train itself (including the coordinate values of the three directions of the x-axis, y-axis, and z-axis) and the speed values in the three directions (x-axis, y-axis, and z-axis directions), etc. This system can also be applied to other scenarios, where the carrier corresponds to vehicles or other objects in other scenarios, such as the carrier being a community patrol vehicle, etc.

[0106] In this embodiment, the functions of the various modules of the positioning system can be implemented by the specific implementation manners of the corresponding steps in the above positioning method, which will not be elaborated here.

[0107] In this embodiment, the positioning system further includes a mapping module, which is used to construct a point cloud map based on the lidar measurement data within a preset time window in the mapping mode.

[0108] An embodiment of the present invention also provides a positioning device based on multi-sensor fusion, including a processor; a memory in which executable instructions of the processor are stored; wherein, the processor is configured to execute the steps of the positioning method based on multi-sensor fusion by executing the executable instructions.

[0109] Those skilled in the art can understand that various aspects of the present invention can be implemented as a system, a method, or a program product. Therefore, various aspects of the present invention can be specifically implemented in the following forms, namely: a complete hardware implementation manner, a complete software implementation manner (including firmware, microcode, etc.), or an implementation manner combining hardware and software aspects, which can be collectively referred to as "circuit", "module", or "platform" here.

[0110] Next, refer to Figure 5 to describe the electronic device 600 according to this embodiment of the present invention. Figure 5 The shown electronic device 600 is only an example and should not bring any limitations to the functions and usage scope of the embodiments of the present invention.

[0111] As Figure 5As shown, the electronic device 600 is presented in the form of a general-purpose computing device. The components of the electronic device 600 may include, but are not limited to: at least one processing unit 610, at least one storage unit 620, a bus 630 connecting different system components (including the storage unit 620 and the processing unit 610), a display unit 640, etc.

[0112] Among them, the storage unit stores program code, and the program code can be executed by the processing unit 610, so that the processing unit 610 executes the steps according to various exemplary embodiments of the present invention described in the above-mentioned part of the positioning method based on multi-sensor fusion in this specification. For example, the processing unit 610 can execute as Figure 1 the steps shown in.

[0113] The storage unit 620 may include a readable medium in the form of a volatile storage unit, such as a random access storage unit (RAM) 6201 and / or a cache storage unit 6202, and may further include a read-only storage unit (ROM) 6203.

[0114] The storage unit 620 may also include a program / utility 6204 having a set (at least one) of program modules 6205. Such program modules 6205 include, but are not limited to: an operating system, one or more application programs, other program modules, and program data. Each or some combination of these examples may include the implementation of a network environment.

[0115] The bus 630 may represent one or more of several types of bus structures, including a storage unit bus or a storage unit controller, a peripheral bus, a graphics acceleration port, a processing unit, or a local bus using any bus structure in a variety of bus structures.

[0116] The electronic device 600 can also communicate with one or more external devices 700 (such as a keyboard, a pointing device, a Bluetooth device, etc.), and can also communicate with one or more devices that enable a user to interact with the electronic device 600, and / or communicate with any device (such as a router, a modem, etc.) that enables the electronic device 600 to communicate with one or more other computing devices. Such communication can be carried out through the input / output (I / O) interface 650. Moreover, the electronic device 600 can also communicate with one or more networks (such as a local area network (LAN), a wide area network (WAN), and / or a public network, such as the Internet) through the network adapter 660. The network adapter 660 can communicate with other modules of the electronic device 600 through the bus 630. It should be understood that, although not shown in the figure, other hardware and / or software modules can be used in combination with the electronic device 600, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems, etc.

[0117] In the positioning device based on multi-sensor fusion, when the program in the memory is executed by the processor, the steps of the positioning method based on multi-sensor fusion are implemented. Therefore, the device can also obtain the technical effects of the above-mentioned positioning method based on multi-sensor fusion.

[0118] The embodiment of the present invention also provides a computer-readable storage medium for storing a program, and when the program is executed by the processor, the steps of the positioning method based on multi-sensor fusion are implemented. In some possible implementation manners, various aspects of the present invention can also be implemented in the form of a program product, which includes program code, and when the program product is executed on a terminal device, the program code is used to cause the terminal device to execute the steps according to various exemplary embodiments of the present invention described in the above-mentioned positioning method based on multi-sensor fusion part of this specification.

[0119] Reference Figure 6 As shown, a program product 800 for implementing the above method according to an embodiment of the present invention is described. It can be a portable compact disc read-only memory (CD-ROM) and includes program code, and can be executed on a terminal device, such as a personal computer. However, the program product of the present invention is not limited thereto. In this document, the readable storage medium can be any tangible medium that contains or stores a program, and this program can be used by or in combination with an instruction execution system, apparatus, or device.

[0120] The program product may employ any combination of one or more readable media. The readable media can be a readable signal medium or a readable storage medium. The 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 of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium include: an electrical connection having one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0121] The computer-readable storage medium may include a data signal propagated in a baseband or as part of a carrier wave, in which the readable program code is carried. Such a propagated data signal may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the foregoing. The readable storage medium may also be any readable medium other than the 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 readable storage medium may be transmitted using any appropriate medium, including but not limited to wireless, wired, optical fiber cable, RF, etc., or any suitable combination of the foregoing.

[0122] The program code for performing the operations of the present invention may be written in any combination of one or more programming languages, including object-oriented programming languages such as Java, C++, etc., and also including conventional procedural programming languages such as the "C" language or similar programming languages. The program code may be executed entirely on the user's computing device, partially on the user's device, executed as a stand-alone software package, partially on the user's computing device and partially on a remote computing device, or entirely on the remote computing device or server. In the case of a remote computing device, the remote computing device may be connected to the user's computing device through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computing device (e.g., through the Internet using an Internet service provider).

[0123] When the program in the computer storage medium is executed by a processor, the steps of the positioning method based on multi-sensor fusion are implemented. Therefore, the computer storage medium can also achieve the technical effects of the above-mentioned positioning method based on multi-sensor fusion.

[0124] The above content is a further detailed description of the present invention in combination with specific preferred embodiments, and it cannot be determined that the specific implementation of the present invention is only limited to these descriptions. For those of ordinary skill in the technical field to which the present invention pertains, without departing from the concept of the present invention, several simple deductions or substitutions can still be made, and all should be regarded as belonging to the protection scope of the present invention.

Claims

1. A positioning method based on multi-sensor fusion, characterized in that, Including the following steps: Obtain the lidar measurement data of the vehicle from the lidar, and extract lidar factors from the lidar measurement data; Obtain inertial measurement data from the inertial measurement unit, and extract IMU pre-integration factors from the inertial measurement data; Obtain millimeter-wave radar measurement data from the millimeter-wave radar, and extract millimeter-wave radar factors from the millimeter-wave radar measurement data, including: calculating a histogram based on the millimeter-wave radar measurement data; finding the speed interval with the largest count value from the histogram; taking the speed mean of the speed interval with the largest count value as the millimeter-wave measurement speed, and taking the millimeter-wave measurement speed as the millimeter-wave radar factor; Perform factor graph optimization based on the lidar factors, the IMU pre-integration factors, and the millimeter-wave radar factors to obtain the positioning data of the vehicle, including: inputting the lidar factors, the IMU pre-integration factors, and the millimeter-wave radar factors into a residual block to obtain the positioning data of the vehicle output by the residual block; after left-multiplying the millimeter-wave measurement speed by the rotation matrix of the inertial measurement unit of the current frame, performing residual calculations with the speed values in the x-axis, y-axis, and z-axis directions in the vehicle state parameters respectively to obtain the state speed quantity residual as the cost function.

2. The positioning method based on multi-sensor fusion according to claim 1, characterized in that, After taking the speed mean of the speed interval with the largest count value as the millimeter-wave measurement speed, the following steps are further included: Incorporate the millimeter-wave measurement speed into the x-axis position of the point adjacent to the lidar frame [0].

3. The positioning method based on multi-sensor fusion according to claim 1, characterized in that, When inputting the lidar factors, the IMU pre-integration factors, and the millimeter-wave radar factors into the residual block, it further includes obtaining vehicle state parameters based on the lidar measurement data and the inertial measurement data and inputting them into the residual block.

4. The positioning method based on multi-sensor fusion according to claim 1, characterized in that, Extracting lidar factors from the lidar measurement data includes the following steps: Extract the lidar point cloud features of the current frame from the lidar measurement data, where the lidar point cloud features include plane feature point features, line feature point features, and irregular point features; Obtain the current point cloud map; Perform point cloud feature matching based on the lidar point cloud features of the current frame and the point cloud map to obtain the lidar predicted position as the lidar factor.

5. The positioning method based on multi-sensor fusion according to claim 4, characterized in that, The following steps are further included: Judge whether the current is the mapping mode; If it is the mapping mode, construct a point cloud map based on the lidar measurement data within a preset time window.

6. The positioning method based on multi-sensor fusion according to claim 5, characterized in that, When constructing a point cloud map based on the lidar measurement data within a preset time window, implement point cloud map construction, saving, and reading based on a thread parallel to the lidar measurement data, and perform pre-rasterization processing on the saved point cloud map to obtain multiple grids of preset sizes; When performing point cloud feature matching based on the lidar point cloud features and the point cloud map, use the point cloud data in the preset number of grids before and after the vehicle position in the point cloud map to perform point cloud feature matching with the lidar point cloud features of the current frame.

7. A positioning system based on multi-sensor fusion, characterized in that, For implementing the multi-sensor fusion-based positioning method according to any one of claims 1 to 6, the system includes: The first data processing module is configured to obtain the lidar measurement data of the vehicle from a lidar, and extract lidar factors from the lidar measurement data; The second data processing module is configured to obtain inertial measurement data from an inertial measurement unit, and extract IMU pre-integration factors from the inertial measurement data; The third data processing module is configured to obtain millimeter-wave radar measurement data from a millimeter-wave radar, and extract millimeter-wave radar factors from the millimeter-wave radar measurement data; The vehicle positioning module is configured to perform factor graph optimization based on the lidar factors, the IMU pre-integration factors, and the millimeter-wave radar factors to obtain the positioning data of the vehicle.

8. A positioning device based on multi-sensor fusion, characterized in that, Comprising: A processor; A memory, which stores executable instructions of the processor; Wherein, the processor is configured to execute the steps of the multi-sensor fusion-based positioning method according to any one of claims 1 to 6 by executing the executable instructions.

9. A computer-readable storage medium for storing a program, characterized in that, When the program is executed by the processor, the steps of the multi-sensor fusion-based positioning method according to any one of claims 1 to 6 are implemented.

Images