IMU-based multi-sensor fusion positioning method and device, and storage medium

Abstract

The invention provides an IMU (Inertial Measurement Unit)-based multi-sensor fusion positioning method and device and a storage medium. The positioning method comprises the following steps: obtaining an IMU pre-integration result based on a constructed IMU pre-integration model; adding the system positioning result at a moment and the IMU pre-integration result to obtain a system positioning predicted value at the current moment; obtaining original data of the plurality of sensors, and performing preprocessing and synchronization to obtain observation values of the plurality of sensors at the current moment; independently filtering the observation values of the plurality of sensors and the system positioning predicted value at the current moment by using the constructed filtering subsystem so as to obtain updated system positioning predicted values of the plurality of sensors at the current moment; according to the system positioning observation values of the plurality of sensors and the confidence coefficients of the plurality of sensors at the previous moment, updating the confidence coefficients of the plurality of sensors at the current moment; and according to the confidence of the plurality of sensors at the current moment, carrying out weighted summation on the updated system positioning predicted values of the plurality of sensors to obtain a system positioning result at the current moment.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and more specifically, to an IMU-based multi-sensor fusion positioning method, device, and storage medium. Background Art

[0002] With the advancement of technology, more and more sensors are used in positioning, including Inertial Measurement Unit (IMU), Global Navigation Satellite System (GNSS), LiDAR, camera, etc. Different sensors have different advantages. GNSS can achieve a position accuracy of less than 5cm based on Real-Time Kinematic (RTK) technology in open scenes, but it will basically fail in indoor or severely blocked scenes; LiDAR can stabilize at centimeter-level accuracy in well-structured scenes, but there will be meter-level errors in similar or open environments; IMU is very accurate in a very short time, but will have serious drift over a long period of time; Cameras have high positioning accuracy in static and well-lit environments, but are almost ineffective in dynamic and low-vision environments.

[0003] The essential purpose of multi-sensor fusion positioning is to make full use of the complementary advantages of different sensors, obtain more robust positioning results than using a single sensor, and solve the positioning loss problem caused by sensor degradation in challenging environments. Existing technologies usually simply fuse multi-sensor data, but this does not necessarily solve the above problems, especially when single or multiple sensor data are abnormal or fail, the system with imperfect design and consideration will still have the problem of navigation and positioning loss. For example, when a centralized filter is used, the measurement data of all sensors are transmitted to the central site for processing. When a sensor has a serious data failure, the entire centralized filter may become unstable and inaccurate. Therefore, how to improve the robustness of the multi-sensor fusion positioning system is an urgent problem to be solved.

[0004] The above information disclosed in the Background section is only for enhancement of understanding of the background of the present application and therefore it may contain information that does not constitute the prior art that is already known to a person of ordinary skill in the art. Summary of the invention

[0005] In order to solve at least one of the above problems, the present application proposes an IMU-based multi-sensor fusion positioning method, device and storage medium.

[0006] According to the first aspect of the present application, at least one embodiment of the present application provides an IMU-based multi-sensor fusion positioning method, the positioning method comprising:

[0007] An IMU pre-integration model is constructed, and an IMU pre-integration result is obtained based on the constructed IMU pre-integration model;

[0008] Accumulate the system positioning result at the previous moment and the IMU pre-integration result to obtain the system positioning prediction value at the current moment;

[0009] Acquire raw data of multiple sensors, and perform preprocessing and synchronization to obtain observation values ​​of the multiple sensors at the current moment;

[0010] Constructing a filtering subsystem, and using the constructed filtering subsystem to independently filter the observation values ​​of the multiple sensors and the system positioning prediction value at the current moment, so as to obtain the system positioning prediction values ​​updated by the multiple sensors at the current moment;

[0011] Update the confidences of the multiple sensors at a current moment according to the system positioning observations of the multiple sensors and the confidences of the multiple sensors at a previous moment;

[0012] According to the confidences of the multiple sensors at the current moment, the system positioning prediction values ​​updated by the multiple sensors are weighted and summed to obtain the system positioning result at the current moment.

[0013] According to some embodiments, the IMU pre-integration model is:

[0014]

[0015]

[0016] in, are the IMU angular velocity measurement value and acceleration measurement value, ω(t) and a(t) are the true value of IMU angular velocity and acceleration, respectively. g (t), b a (t) are the zero drift of the gyroscope and accelerometer, η g (t), η a (t) are the measurement noise of the gyroscope and accelerometer, R T is the rotation matrix converted to the world coordinate system at the current moment, and g is the gravitational acceleration.

[0017] According to some embodiments, constructing an IMU pre-integration model and obtaining an IMU pre-integration result based on the constructed IMU pre-integration model includes:

[0018] The IMU angular velocity measurement value Acceleration measurement Perform pre-integration to obtain the changes in system rotation, speed, and position:

[0019]

[0020]

[0021]

[0022] Among them, ΔR ij , Δν ij , Δp ij are the changes in rotation, speed, and position from the i-th frame to the j-th frame, respectively. are the IMU angular velocity and acceleration measurements at the kth frame, respectively, and b g,k 、b a,k are the zero drift of the gyroscope and accelerometer at the kth frame, η gd,k , η ad,k are the random walk noise of the gyroscope and accelerometer at the kth frame, is the rotation matrix converted to the world coordinate system at the i-th frame, R j is the rotation matrix from the jth frame to the ith frame, ΔR ik is the rotation change from frame i to frame k, ν i , ν j are the speeds at the i-th and j-th frames respectively, p i 、p j are the positions at the i-th and j-th frames respectively, Δt ij is the time from the i-th frame to the j-th frame.

[0023] According to some embodiments, acquiring raw data of multiple sensors, and performing preprocessing and synchronization to obtain observation values ​​of the multiple sensors at the current moment, includes:

[0024] When the sensor is a laser radar, a point cloud matching method is used to obtain a relative position change between the current frame point cloud data and the previous frame point cloud data;

[0025] When the sensor is a global navigation satellite system, the position and posture of the current frame is obtained based on the real-time dynamic carrier phase difference technology, and the relative position and posture change between the current frame and the previous frame is calculated.

[0026] According to some embodiments, the step of acquiring raw data of a plurality of sensors, performing preprocessing and synchronization, and obtaining observation values ​​of the plurality of sensors at the current moment, further includes:

[0027] When the relative position and posture changes of the multiple sensors are obtained, the relative position and posture changes of the multiple sensors are respectively synchronized in coordinate system and time to obtain the observation value of the sensor at the current moment, wherein:

[0028] The coordinate system synchronization includes: using a conversion matrix to unify the relative position and posture changes of each sensor into a world coordinate system;

[0029] The time synchronization includes: uniformly using system timestamps.

[0030] According to some embodiments, constructing a filtering subsystem and independently filtering the observation values ​​of the multiple sensors and the system positioning prediction value at the current moment using the constructed filtering subsystem to obtain the system positioning prediction values ​​updated by the multiple sensors at the current moment, includes:

[0031] The filter of the filtering subsystem adopts an extended Kalman filter model;

[0032] The Kalman filter model includes:

[0033]

[0034]

[0035]

[0036]

[0037]

[0038] Among them, X k-1 , P k-1 are the system state quantity and its covariance matrix at time k-1 respectively, are the system predicted state quantity and its covariance matrix of sensor i at time k, are the system state and covariance matrix of the i sensor after independent update at time k, φ k is the system state transfer matrix at time k, Q k is the system process noise covariance matrix at time k, is the Kalman gain of sensor i at time k, is the observation matrix of sensor i at time k, is the observation noise covariance matrix of sensor i at time k, is the observation value of sensor i at time k, and I is the unit matrix.

[0039] According to some embodiments, the system state quantity of the extended Kalman filter is set to:

[0040]

[0041] in, δν, δp are the changes of attitude, speed and position respectively, δb g ,δb a are the zero bias changes of the gyroscope and accelerometer respectively, X is the system state quantity and its dimension is 15.

[0042] According to some embodiments, the system process noise covariance matrix is ​​set to:

[0043]

[0044] Where Q is the system process noise covariance matrix, They are gyroscope measurement variance, accelerometer measurement variance, gyroscope zero bias variance, and accelerometer zero bias variance, respectively.

[0045] According to some embodiments, the observed value is set to:

[0046]

[0047] Where Z is the observed value, Δp are the observed angle and position changes respectively.

[0048] According to some embodiments, the observation matrix is ​​set as:

[0049]

[0050] Wherein, H is the observation matrix.

[0051] According to some embodiments, the observation noise covariance matrix is ​​set to:

[0052]

[0053] Wherein, R is the observation noise covariance matrix, are the observation noise variances of attitude and position, respectively.

[0054] According to some embodiments, updating the confidences of the multiple sensors at the current moment according to the system positioning observations of the multiple sensors and the confidences of the multiple sensors at the previous moment includes:

[0055] Calculate the innovation and innovation covariance matrix of the multiple sensors:

[0056]

[0057]

[0058] in, is the new information of sensor i at time k, is the new information covariance matrix of sensor i at time k.

[0059] According to some embodiments, updating the confidences of the multiple sensors at the current moment according to the system positioning observations of the multiple sensors and the confidences of the multiple sensors at the previous moment further includes:

[0060] Calculate the maximum likelihood probability of the plurality of sensor observations according to the innovations of the plurality of sensors and the innovation covariance matrix;

[0061] Combine the maximum likelihood probability of the multiple sensor observations with the confidence of the multiple sensors at the previous moment, and update the confidence of the multiple sensors at the current moment:

[0062]

[0063]

[0064] In the formula, is the maximum likelihood probability of the state quantity of sensor i at time k, is the sensor confidence of sensor i at time k-1 and time k, and det() is the determinant of the matrix to be solved.

[0065] According to some embodiments, the weighted summing of the system positioning prediction values ​​updated by the multiple sensors according to the confidences of the multiple sensors at the current moment to obtain the system positioning result at the current moment includes:

[0066]

[0067]

[0068] Among them, X k , P k are the system state quantity and its covariance matrix at time k respectively.

[0069] According to a second aspect of the present application, at least one embodiment of the present application provides an electronic device, including:

[0070] one or more processors; and

[0071] A memory for storing one or more programs;

[0072] When the one or more programs are executed by the one or more processors, the one or more processors execute the method as described in any one of the first aspects.

[0073] According to a third aspect of the present application, at least one embodiment of the present application provides a computer-readable storage medium having a computer program stored thereon, wherein when the program is executed by a processor, the method described in any one of the first aspects is implemented.

[0074] The present application provides an IMU-based multi-sensor fusion positioning method, device, and storage medium, which have at least one of the following beneficial effects:

[0075] 1. High robustness and scalability. The multi-sensor fusion framework with IMU as the core can improve the system positioning accuracy and robustness in complex environments: by updating the sensor confidence in real time, high-quality sensor data can be used first and degraded sensor data can be eliminated in different working environments, thereby realizing the complementary advantages of sensors. In addition, since each sensor builds a filter and updates it independently, it is easy to add new sensors or remove old sensors, improving the scalability of the positioning system.

[0076] 2. Low resource usage and high real-time performance. The multi-sensor fusion framework with IMU as the core avoids the complex derivation of modeling multiple sensors at the same time. In addition, the multi-sensor fusion positioning method adopts the extended Kalman filter, so that the state quantity of the current frame is only related to the state quantity of the previous frame. Therefore, the multi-sensor fusion positioning method has a small amount of calculation, occupies less computing resources, and has higher real-time performance.

[0077] It should be understood that the foregoing general description and the following detailed description are exemplary only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] By describing in detail exemplary embodiments thereof with reference to the accompanying drawings, the above and other objects, features and advantages of the present application will become more apparent. The accompanying drawings described below are only some embodiments of the present application, and are not intended to limit the present application.

[0079] Figure 1 A flowchart of a multi-sensor fusion positioning method based on IMU is shown in an exemplary embodiment;

[0080] Figure 2 A flowchart of a multi-sensor fusion positioning method based on IMU is shown;

[0081] Figure 3 A schematic diagram of the filtering model of the present application is shown;

[0082] Figure 4 A structural diagram of an electronic device provided by the present application is shown. DETAILED DESCRIPTION

[0083] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in many forms and should not be construed as limited to the embodiments set forth herein; rather, these embodiments are provided so that this application will be comprehensive and complete and will fully convey the concepts of the example embodiments to those skilled in the art. The same reference numerals in the figures represent the same or similar parts, and thus their repeated description will be omitted.

[0084] The described features, structures or characteristics may be combined in one or more embodiments in any suitable manner. In the following description, many specific details are provided to provide a full understanding of the embodiments of the present disclosure. However, those skilled in the art will appreciate that the technical solutions of the present disclosure may be practiced without one or more of these specific details, or other modes, components, materials, devices, etc. may be adopted. In these cases, known structures, methods, devices, implementations, materials or operations will not be shown or described in detail.

[0085] The flowcharts shown in the accompanying drawings are only exemplary and do not necessarily include all the contents and operations or steps, nor do they have to be executed in the order described. For example, some operations or steps can be decomposed, and some operations or steps can be combined or partially combined, so the actual execution order may change according to actual conditions.

[0086] The terms "first", "second", etc. in the specification and claims of this application and the above-mentioned drawings are used to distinguish different objects, rather than to describe a specific order. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally includes steps or units that are not listed, or optionally includes other steps or units inherent to these processes, methods, products or devices.

[0087] Those skilled in the art will appreciate that the drawings are merely schematic diagrams of example embodiments, and the modules or processes in the drawings are not necessarily necessary for implementing the present application, and therefore cannot be used to limit the scope of protection of the present application.

[0088] Figure 1 A flow chart of an IMU-based multi-sensor fusion positioning method according to an exemplary embodiment is shown.

[0089] like Figure 2 As shown, the IMU-based multi-sensor fusion positioning method includes four parts: IMU pre-integration state propagation, sensor independent filtering, sensor confidence update and fusion output. The specific implementation includes steps S1 to S6.

[0090] In step S1, an IMU pre-integration model is constructed, and an IMU pre-integration result is obtained based on the constructed IMU pre-integration model.

[0091] The pre-integration model of IMU is:

[0092]

[0093]

[0094] in, are the IMU angular velocity measurement value and acceleration measurement value, ω(t) and a(t) are the true value of IMU angular velocity and acceleration, respectively. g (t), b a (t) are the zero drift of the gyroscope and accelerometer, η g (t), η a (t) are the measurement noise of the gyroscope and accelerometer, R T is the rotation matrix converted to the world coordinate system at the current moment, and g is the gravitational acceleration.

[0095] The IMU angular velocity measurement value Acceleration measurement Perform pre-integration to obtain the changes in system rotation, speed, and position:

[0096]

[0097]

[0098]

[0099] Among them, ΔR ij , Δν ij , Δp ij are the changes in rotation, speed, and position from the i-th frame to the j-th frame, respectively. are the IMU angular velocity and acceleration measurements at the kth frame, respectively, and b g,k 、b a,k are the zero drift of the gyroscope and accelerometer at the kth frame, η gd,k , η ad,k are the random walk noise of the gyroscope and accelerometer at the kth frame, is the rotation matrix converted to the world coordinate system at the i-th frame, R j is the rotation matrix from the jth frame to the ith frame, ΔR ik is the rotation change from frame i to frame k, ν i , ν j are the speeds at the i-th and j-th frames respectively, pi 、p j are the positions at the i-th and j-th frames respectively, Δt ij is the time from the i-th frame to the j-th frame.

[0100] In step S2, the system positioning result at the previous moment and the IMU pre-integration result are accumulated to obtain the system positioning prediction value at the current moment.

[0101] According to an exemplary embodiment, the system positioning result includes a system state quantity X and its covariance matrix P. Based on the system positioning result X at the last moment k-1 and P k-1 , real-time accumulation of IMU pre-integration results, that is, the change in system rotation, speed, and position ΔR ij , Δν ij , Δp ij , output the system positioning prediction value at the current moment and Among them, the subscript "k-1" represents the previous moment, "k" represents the current moment, "k / k-1" represents the current moment based on the previous moment, and the symbol "∧" represents the predicted value.

[0102] In step S3, raw data of multiple sensors are acquired, and preprocessed and synchronized to obtain observation values ​​of multiple sensors at the current moment.

[0103] According to an example embodiment, after obtaining the raw data of other available sensors except the IMU, the raw data is first preprocessed, and the preprocessing methods of different sensors are different:

[0104] When the sensor is a laser radar, a point cloud matching method is used to obtain the relative pose change between the current frame point cloud data and the previous frame point cloud data.

[0105] When the sensor is a global navigation satellite system GNSS, the position and posture of the current frame can be obtained based on the real-time kinematic carrier phase difference technology (RTK), and then the relative position and posture change between the previous frame and the previous frame can be calculated.

[0106] For other sensors, the corresponding preprocessing method is adopted to obtain the relative posture change between the current frame data and the previous frame data.

[0107] After obtaining the relative pose changes observed by different sensors, the relative pose changes of multiple sensors are synchronized in coordinate system and time. Coordinate system synchronization can use the conversion matrix to unify the relative pose changes of each sensor to the world coordinate system, and time synchronization can use the system timestamp. Finally, the kth sensor observation value at the current time is obtained.

[0108] In step S4, a filtering subsystem is constructed, and the constructed filtering subsystem is used to independently filter the observation values ​​of multiple sensors and the system positioning prediction value at the current moment to obtain the system positioning prediction values ​​updated by multiple sensors at the current moment.

[0109] For each sensor in step S3, a filtering subsystem is constructed to transform the observation value of the kth sensor at the current moment into The system positioning prediction value at the current moment Perform filtering to obtain the system positioning prediction value updated by the i-th sensor The subscript "k" represents the current moment, "k / k-1" represents the current moment based on the previous moment, "k / k" represents the current moment based on the current moment, the superscript "i" represents the i-th one, and the symbol "∧" represents the predicted value.

[0110] The filter of the filtering subsystem adopts the extended Kalman filter model, and its filtering model is as follows: Figure 3 As shown, the details are as follows:

[0111]

[0112]

[0113]

[0114]

[0115]

[0116] Among them, X k-1 , P k-1 are the system state quantity and its covariance matrix at time k-1 respectively, are the system predicted state quantity and its covariance matrix of sensor i at time k, are the system state and covariance matrix of the i sensor after independent update at time k, φ k is the system state transfer matrix at time k, Q k is the system process noise covariance matrix at time k, is the Kalman gain of sensor i at time k, is the observation matrix of sensor i at time k, is the observation noise covariance matrix of sensor i at time k, is the observation value of sensor i at time k, and I is the unit matrix.

[0117] Among them, the system state quantity X of the extended Kalman filter is set as:

[0118]

[0119] in, δν, δp are the changes of attitude, speed and position respectively, δb g ,δb a They are the zero bias changes of the gyroscope and accelerometer respectively, and the dimension of X is 15.

[0120] For different sensors, Both can be directly obtained through IMU pre-integration.

[0121] The system process noise covariance matrix Q is set as follows:

[0122]

[0123] in, They are gyroscope measurement variance, accelerometer measurement variance, gyroscope zero bias variance, and accelerometer zero bias variance, respectively.

[0124] Different sensors have different observation values. For most sensors, the observation value Z is the position and posture, as follows:

[0125]

[0126] in, Δp are the observed angle and position changes respectively.

[0127] The corresponding observation matrix H is set as follows:

[0128]

[0129] The observation noise covariance matrix R is set as follows:

[0130]

[0131] in, are the observation noise variances of attitude and position, respectively.

[0132] In addition, the initial covariance matrix is ​​set to:

[0133]

[0134] in, are the initial variances of attitude, velocity and position respectively, are the initial zero bias variances of the gyroscope and accelerometer, respectively.

[0135] In step S5, the confidences of the multiple sensors at the current moment are updated according to the system positioning observation values ​​of the multiple sensors and the confidences of the multiple sensors at the previous moment.

[0136] Calculate the new information of each sensor based on the system positioning observation value of each sensor and the confidence of each sensor at the previous moment And the innovation covariance matrix According to the new information of each sensor And the innovation covariance matrix Calculate the maximum likelihood probability of the sensor observation value at the current moment, and combine it with the confidence of each sensor at the previous moment Update the confidence of each sensor at the current moment The subscript “k” represents the current moment, the superscript “i” represents the i-th one, and the symbol “~” represents new information.

[0137] News And the innovation covariance matrix The calculation formula is as follows:

[0138]

[0139]

[0140] in, is the new information of sensor i at time k, is the new information covariance matrix of sensor i at time k.

[0141] Calculate the maximum likelihood probability of the observation value of each sensor at time k, combine the confidence of each sensor at time k-1, and update the confidence of each sensor at the current time:

[0142]

[0143]

[0144] In the formula, is the maximum likelihood probability of the state quantity of sensor i at time k, is the sensor confidence of sensor i at time k-1 and time k, and det() is the determinant of the matrix to be solved.

[0145] The sensor confidence is normalized, that is,

[0146] According to some embodiments, the initialization of sensor confidence depends on prior information, and different initial confidences can be assigned to sensors in different environments. For example, in indoor environments or environments with severe occlusion, RTK basically fails, and a higher confidence can be assigned to the lidar. In similar environments or open scenes, the lidar effect is not good, and a higher confidence can be assigned to the RTK.

[0147] In step S6, based on the confidence of the multiple sensors at the current moment, the system positioning prediction values ​​updated by the multiple sensors are weighted and summed to obtain the system positioning result at the current moment.

[0148] Weighted sum of the system positioning prediction values ​​updated by multiple sensors at the current moment and And the confidence of each sensor at the current moment Fusion output current system positioning result X k and P k The subscript “k” represents the current moment, “k / k” represents the current moment based on the current moment, and the superscript “i” represents the i-th sensor.

[0149] The calculation formula is as follows:

[0150]

[0151]

[0152] Among them, X k , P k are the system state quantity and its covariance matrix at time k, that is, the system positioning result at the current moment.

[0153] The present application provides a multi-sensor fusion positioning method based on IMU. The multi-sensor fusion framework with IMU as the core can improve the system positioning accuracy and robustness in complex environments: by updating the sensor confidence in real time, high-quality sensor data can be used preferentially in different working environments and degraded sensor data can be eliminated, thereby realizing the complementary advantages of sensors. In addition, since each sensor can construct a filter and update it independently, it is easy to add new sensors or remove old sensors, thereby improving the scalability of the positioning system. And the multi-sensor fusion framework with IMU as the core avoids the complex derivation of modeling multiple sensors at the same time. In addition, the multi-sensor fusion positioning method adopts an extended Kalman filter, so that the state quantity of the current frame is only related to the state quantity of the previous frame. Therefore, the multi-sensor fusion positioning method has a small amount of calculation, occupies less computing resources, and has higher real-time performance.

[0154] Figure 4 A structural diagram of an electronic device provided by the present application is shown.

[0155] See also Figure 4 , Figure 4 An electronic device is provided, comprising a processor and a memory. The memory stores computer instructions. When the computer instructions are executed by the processor, the processor executes the computer instructions to achieve the following Figure 1 The method and refinement scheme shown.

[0156] It should be understood that the above-mentioned device embodiments are only illustrative, and the device disclosed in the present application can also be implemented in other ways. For example, the division of units or modules in the above-mentioned embodiments is only a logical function division, and there may be other division methods in actual implementation. For example, multiple units, modules or components can be combined, or can be integrated into another system, or some features can be ignored or not executed.

[0157] In addition, unless otherwise specified, each functional unit or module in each embodiment of the present application may be integrated into one unit or module, or each unit or module may exist physically separately, or two or more units or modules may be integrated together. The above-mentioned integrated unit or module may be implemented in the form of hardware or in the form of a software program module.

[0158] If the integrated unit or module is implemented in the form of hardware, the hardware may be a digital circuit, an analog circuit, etc. The physical implementation of the hardware structure includes but is not limited to transistors, memristors, etc. Unless otherwise specified, the processor or chip may be any appropriate hardware processor, such as CPU, GPU, FPGA, DSP, and ASIC, etc. Unless otherwise specified, the on-chip cache, off-chip memory, and storage may be any appropriate magnetic storage medium or magneto-optical storage medium, such as Resistive Random Access Memory (RRAM), Dynamic Random Access Memory (DRAM), Static Random Access Memory (SRAM), Enhanced Dynamic Random Access Memory (EDRAM), High-Bandwidth Memory (HBM), Hybrid Memory Cube (HMC), etc.

[0159] If the integrated unit or module is implemented in the form of a software program module and sold or used as an independent product, it can be stored in a computer-readable memory. Based on this understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a memory, including a number of instructions to enable a computer device (which can be a personal computer, server or network device, etc.) to perform all or part of the steps of the various embodiments of the present disclosure. The aforementioned memory includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, disk or optical disk and other media that can store program codes.

[0160] The present application also provides a non-transitory computer storage medium storing a computer program. When the computer program is executed by multiple processors, the processors execute the following Figure 1 The method and refinement scheme shown.

[0161] It should be clearly understood that the present application describes how to form and use specific examples, but the present application is not limited to any details of these examples. On the contrary, based on the teaching of the content disclosed in the present application, these principles can be applied to many other embodiments.

[0162] In addition, it should be noted that the above figures are only schematic illustrations of the processes included in the method according to the exemplary embodiments of the present application, and are not intended to be limiting. It is easy to understand that the processes shown in the above figures do not indicate or limit the time sequence of these processes. In addition, it is also easy to understand that these processes can be performed synchronously or asynchronously, for example, in multiple modules.

[0163] The exemplary embodiments of the present application are specifically shown and described above. It should be understood that the present application is not limited to the detailed structures, configurations or implementations described herein; on the contrary, the present application is intended to cover various modifications and equivalent configurations included in the spirit and scope of the appended claims.

Claims

1. A multi-sensor fusion positioning method based on IMU, characterized in that: The positioning method comprises: An IMU pre-integration model is constructed, and an IMU pre-integration result is obtained based on the constructed IMU pre-integration model; Accumulate the system positioning result at the previous moment and the IMU pre-integration result to obtain the system positioning prediction value at the current moment; Acquire raw data of multiple sensors, and perform preprocessing and synchronization to obtain observation values ​​of the multiple sensors at the current moment; Constructing a filtering subsystem, and using the constructed filtering subsystem to independently filter the observation values ​​of the multiple sensors and the system positioning prediction value at the current moment, so as to obtain the system positioning prediction values ​​updated by the multiple sensors at the current moment; Update the confidences of the multiple sensors at a current moment according to the system positioning observations of the multiple sensors and the confidences of the multiple sensors at a previous moment; According to the confidences of the multiple sensors at the current moment, the system positioning prediction values ​​updated by the multiple sensors are weighted and summed to obtain the system positioning result at the current moment.

2. The multi-sensor fusion positioning method according to claim 1, characterized in that: The IMU pre-integration model is: in, are the IMU angular velocity measurement value and acceleration measurement value, ω(t) and a(t) are the true value of IMU angular velocity and acceleration, respectively. g (t), b a (t) are the zero drift of the gyroscope and accelerometer, η g (t), η a (t) are the measurement noise of the gyroscope and accelerometer, R T is the rotation matrix converted to the world coordinate system at the current moment, and g is the gravitational acceleration.

3. The multi-sensor fusion positioning method according to claim 2, characterized in that: The IMU pre-integration model is constructed, and an IMU pre-integration result is obtained based on the constructed IMU pre-integration model, including: The IMU angular velocity measurement value Acceleration measurement Perform pre-integration to obtain the changes in system rotation, speed, and position: Among them, ΔR ij , Δν ij , Δp ij are the changes in rotation, speed, and position from the i-th frame to the j-th frame, respectively. are the IMU angular velocity and acceleration measurements at the kth frame, respectively, and b g,k 、b a,k are the zero drift of the gyroscope and accelerometer at the kth frame, η gd,k , η ad,k are the random walk noise of the gyroscope and accelerometer at the kth frame, is the rotation matrix converted to the world coordinate system at the i-th frame, R j is the rotation matrix from the jth frame to the ith frame, ΔR ik is the rotation change from frame i to frame k, ν i , ν j are the speeds at the i-th and j-th frames respectively, p i 、p j are the positions at the i-th and j-th frames respectively, Δt ij is the time from the i-th frame to the j-th frame.

4. The multi-sensor fusion positioning method according to claim 1, characterized in that: The acquiring of raw data of multiple sensors, and preprocessing and synchronizing to obtain observation values ​​of the multiple sensors at the current moment, includes: When the sensor is a laser radar, a point cloud matching method is used to obtain a relative position change between the current frame point cloud data and the previous frame point cloud data; When the sensor is a global navigation satellite system, the position and posture of the current frame is obtained based on the real-time dynamic carrier phase difference technology, and the relative position and posture change between the current frame and the previous frame is calculated.

5. The multi-sensor fusion positioning method according to claim 4, characterized in that: The method of acquiring raw data of multiple sensors, performing preprocessing and synchronization, and obtaining observation values ​​of the multiple sensors at the current moment, further includes: When the relative position and posture changes of the multiple sensors are obtained, the relative position and posture changes of the multiple sensors are respectively synchronized in coordinate system and time to obtain the observation value of the sensor at the current moment, wherein: The coordinate system synchronization includes: using a conversion matrix to unify the relative position and posture changes of each sensor into a world coordinate system; The time synchronization includes: uniformly using system timestamps.

6. The multi-sensor fusion positioning method according to claim 1, characterized in that: The constructing of the filtering subsystem and independently filtering the observation values ​​of the multiple sensors and the system positioning prediction value at the current moment by using the constructed filtering subsystem to obtain the system positioning prediction values ​​updated by the multiple sensors at the current moment include: The filter of the filtering subsystem adopts an extended Kalman filter model; The Kalman filter model includes: Among them, X k-1 , P k-1 are the system state quantity and its covariance matrix at time k-1 respectively, are the system predicted state quantity and its covariance matrix of sensor i at time k, are the system state and covariance matrix of the i sensor after independent update at time k, φ k is the system state transfer matrix at time k, Q k is the system process noise covariance matrix at time k, is the Kalman gain of sensor i at time k, is the observation matrix of sensor i at time k, is the observation noise covariance matrix of sensor i at time k, is the observation value of sensor i at time k, and I is the unit matrix.

7. The multi-sensor fusion positioning method according to claim 6, characterized in that: The system state quantity of the extended Kalman filter is set to: in, δν, δp are the changes of attitude, speed and position respectively, δb g ,δb a are the zero bias changes of the gyroscope and accelerometer respectively, X is the system state quantity and its dimension is 15.

8. The multi-sensor fusion positioning method according to claim 6, characterized in that: The system process noise covariance matrix is ​​set as: Where Q is the system process noise covariance matrix, They are gyroscope measurement variance, accelerometer measurement variance, gyroscope zero bias variance, and accelerometer zero bias variance, respectively.

9. The multi-sensor fusion positioning method according to claim 6, characterized in that: The observations are set to: Where Z is the observed value, Δp are the observed angle and position changes respectively.

10. The multi-sensor fusion positioning method according to claim 6, characterized in that: The observation matrix is ​​set as: Wherein, H is the observation matrix.

11. The multi-sensor fusion positioning method according to claim 6, characterized in that: The observation noise covariance matrix is ​​set to: Wherein, R is the observation noise covariance matrix, are the observation noise variances of attitude and position, respectively.

12. The multi-sensor fusion positioning method according to claim 1, characterized in that: The updating of the confidences of the multiple sensors at the current moment according to the system positioning observation values ​​of the multiple sensors and the confidences of the multiple sensors at the previous moment comprises: Calculate the innovation and innovation covariance matrix of the multiple sensors: in, is the new information of sensor i at time k, is the new information covariance matrix of sensor i at time k.

13. The multi-sensor fusion positioning method according to claim 12, characterized in that: The updating of the confidences of the multiple sensors at the current moment according to the system positioning observation values ​​of the multiple sensors and the confidences of the multiple sensors at the previous moment also includes: Calculate the maximum likelihood probability of the plurality of sensor observations according to the innovations of the plurality of sensors and the innovation covariance matrix; Combine the maximum likelihood probability of the multiple sensor observations with the confidence of the multiple sensors at the previous moment, and update the confidence of the multiple sensors at the current moment: In the formula, is the maximum likelihood probability of the state quantity of sensor i at time k, is the sensor confidence of sensor i at time k-1 and time k, and det() is the determinant of the matrix to be solved.

14. The multi-sensor fusion positioning method according to claim 1, characterized in that: The step of weighting and summing the system positioning prediction values ​​updated by the multiple sensors according to the confidences of the multiple sensors at the current moment to obtain the system positioning result at the current moment includes: Among them, X k , P k are the system state quantity and its covariance matrix at time k respectively.

15. An electronic device, characterized in that: include: one or more processors; as well as A memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors execute the method according to any one of claims 1 to 14.

16. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 14 is implemented.

Images