Inertial measurement unit initialization method and device and storage medium

Abstract

The invention relates to an inertial measurement unit initialization method and device and a storage medium, and the method comprises the steps: S1, initializing a monocular or binocular SLAM, inserting a key frame, obtaining a map composed of a camera pose and points, and obtaining a track T on the map; s2, defining an MAP estimation problem based on inertial measurement between the track T and the key frame, considering Gaussian errors of IMU pre-integration and prior distribution, converting the estimation problem into an optimization problem, updating a rotation matrix Rwg to solve the optimization problem, obtaining the optimal estimation of an inertial variable and a scale factor, and then scaling a map based on the optimal scale factor to obtain an optimal estimation result; and rotating to align the z-axis with the estimated direction of gravity; and S3, carrying out joint optimization on the key frame based on the optimal estimation of the inertia variable and the scale factor. Compared with the prior art, the method has the advantages of accelerating the convergence speed of the inertial unit and the like.

Description

Technical Field

[0001] The present invention relates to the fields of computer vision and robot navigation, and in particular to an inertial measurement unit initialization method, device and storage medium. Background Art

[0002] In conventional augmented reality applications, usually only the camera image is used, and the SLAM point cloud is used for calculation to achieve the synchronization of augmented reality AR space and real space. Although SLAM simultaneous positioning and mapping technology has made significant progress today, there are still some challenges and problems in practical applications, even in AR applications of fine target objects such as underground cables, such as the initialization of the inertial measurement unit in the SLAM system. Initialization is an important step to provide a stable augmented reality display for the AR space. The current initialization method completely relies on the ORB-SLAM basic method, and its convergence speed for obtaining inertial parameters is slow. Summary of the invention

[0003] The purpose of the present invention is to provide an inertial measurement unit initialization method, device and storage medium in order to improve the convergence speed.

[0004] The purpose of the present invention can be achieved by the following technical solutions:

[0005] A method for initializing an inertial measurement unit, the method comprising:

[0006] S1. Initialize monocular or binocular SLAM, insert keyframes, obtain a map consisting of camera poses and points, and obtain trajectory T on the map;

[0007] S2, define the MAP estimation problem based on the inertial measurement between the trajectory T and the keyframes, consider the Gaussian error of the IMU pre-integration and the prior distribution, transform the estimation problem into an optimization problem, and update the rotation matrix R wg Solve the optimization problem to obtain the best estimate of the inertial variables and the scale factor, then scale the map based on the best scale factor and rotate it so that the z-axis is aligned with the estimated gravity direction, where the estimated gravity is obtained based on the optimized rotation matrix;

[0008] S3. Jointly optimize the key frames based on the best estimates of the inertial variables and the scale factors.

[0009] Furthermore, the set of inertial measurements between the trajectory T and the key frames is I 0:k = {I 0,1 ...I k-1,k}.

[0010] Furthermore, the posterior distribution of the MAP estimation problem is maximized as follows:

[0011]

[0012] Among them, p(I 0:k |y k ) represents the likelihood estimate, p(y k ) represents the prior value.

[0013] Furthermore, the MAP estimation problem is:

[0014]

[0015] Where s is the visual scale factor, R wg represents the rotation matrix, represents the body velocity, and b represents the bias of the accelerometer and gyroscope.

[0016] Furthermore, the conversion of the estimation problem into an optimization problem is specifically as follows:

[0017] Taking the negative logarithm of the estimation problem and assuming IMU pre-integration and Gaussian errors in the prior distribution, we obtain the optimization problem.

[0018] Furthermore, the optimization problem is specifically:

[0019]

[0020] ∑b represents the covariance matrix.

[0021] Further, update the rotation matrix R wg Specifically:

[0022]

[0023] in, represents the updated rotation matrix, represents the rotation matrix before updating, δα g and, δβ g Indicates the updated angle.

[0024] Furthermore, in the process of solving the optimization problem, the scale factor is greater than 0.

[0025] Another aspect of the present invention further provides an inertial measurement unit initialization device, comprising a memory, a processor, and a program stored in the memory, wherein the processor implements the above method when executing the program.

[0026] Another aspect of the present invention provides a computer-readable storage medium having a program stored thereon, wherein the program implements the above method when executed.

[0027] Compared with the prior art, the present invention has the following beneficial effects:

[0028] The present invention defines a MAP estimation problem and transforms the estimation problem into an optimization problem, thereby achieving a good estimation of inertial and visual parameters, thereby improving the convergence speed of inertial parameters, and accurately estimating the scale, IMU parameters, gravity direction, etc., thereby accelerating the initialization speed of the inertial measurement unit and enabling the inertial measurement unit to enter a working state faster. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 Schematic diagram of the overall architecture of the system of the present invention. DETAILED DESCRIPTION

[0030] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0031] Example 1

[0032] The present invention proposes an improved IMU (inertial measurement unit) initialization method, the system structure of the method is as follows Figure 1 As shown, the method is designed for a synchronous positioning and mapping system based on the ORB-SLAM3 framework to provide a stable augmented reality display. The method for initializing the IMU for ORB-SLAM3 provides a new IMU initialization method, which aims to solve the problems existing in the prior art and provide a fast and accurate initialization process for the SLAM system based on ORB-SLAM3. By combining visual data with IMU readings and adopting the method of maximum a posteriori probability (MAP) estimation, the present invention can complete high-quality IMU initialization in a short time, thereby improving the overall performance of the SLAM system.

[0033] Methods include:

[0034] S1. Initialize monocular or binocular SLAM, insert keyframes, obtain a map consisting of camera poses and points, and obtain trajectory T on the map;

[0035] S2, define the MAP estimation problem based on the inertial measurement between the trajectory T and the keyframes, consider the Gaussian error of the IMU pre-integration and the prior distribution, transform the estimation problem into an optimization problem, and update the rotation matrix R wg Solve the optimization problem to obtain the best estimate of the inertial variables and the scale factor, then scale the map based on the best scale factor and rotate it so that the z-axis is aligned with the estimated gravity direction, where the estimated gravity is obtained based on the optimized rotation matrix;

[0036] S3. Jointly optimize the key frames based on the best estimates of the inertial variables and the scale factors.

[0037] Vision-only MAP estimation in S1:

[0038] Use ORB-SLAM3 to obtain the initial visual frame and extract key points;

[0039] Run monocular or binocular SLAM in the shortest time to generate a small-scale map;

[0040] The map is optimized by visual bundle adjustment (BA) to obtain the optimized trajectory T 0:k =[R,p] 0:k .

[0041] The MAP estimate of inertia is obtained in S2:

[0042] Using T 0:k Inertial measurement between key frames I0:k = {I0, I1...Ik-1, Ik};

[0043] Define the MAP estimation problem to maximize the posterior distribution p(I0:k|Yk);

[0044] Considering the Gaussian error of IMU pre-integration and prior distribution, the problem is transformed into an optimization problem;

[0045] The possible value range of IMU bias is represented by the covariance matrix Σb;

[0046] Update the rotation matrix Rwg, parameterizing the update using two angles (δαg, δβg);

[0047] Make sure the scaling factor s remains greater than 0 during the optimization process.

[0048] S3 incorporates visual inertial navigation MAP estimation:

[0049] Joint visual-inertial optimization of all keyframes;

[0050] Includes the same bias prior information as in the inertia-only step;

[0051] Further refine the results and reduce linearization errors.

[0052] IMU initialization is an important step for ORB-SLAM3 to provide a stable augmented reality display for AR space. It requires obtaining good initial values ​​of inertial variables: body velocity, gravity direction, and IMU bias. However, relying entirely on the ORB-SLAM basic method, the convergence speed of obtaining inertial parameters is slow (up to 30 seconds).

[0053] At the same time, ignoring the uncertainty of the sensor during IMU initialization will produce huge unpredictable errors. Therefore, with proper consideration of sensor uncertainty, we summarize IMU initialization as a MAP estimation problem, which can be implemented in three steps:

[0054] First, a vision-only MAP estimation is performed: by initializing a monocular or stereo SLAM and starting the run in the shortest possible time, a map of a certain size consisting of k=10 camera poses and hundreds of points is obtained, which is optimized using vision-only BA. These poses are converted to the context of the map for reference, and the trajectory T is obtained. 0:k =[R, p] 0:k , where T and p mainly identify the variables of the next scale.

[0055] Second, perform MAP estimation with inertia captured: In this step, our goal is to use only T in the sense of MAP estimation. - The inertia measurements between 0:k and these keyframes are used to get the best estimate of the inertia variables. These inertia variables can be superimposed in the inertia-only state vector:

[0056]

[0057] Where s is the visual scale factor and R is a rotation matrix used to calculate the gravity vector g = R in the reference frame. wg gI, gI=(0,0,G) T , G is the magnitude of gravity; b = (b a ,b g )∈R6 are the biases of the accelerometer and gyroscope, which are considered constant during the initialization phase. 0:k ∈R3 is the velocity of the body from the first keyframe to the last keyframe, and the initial estimate comes from T 0:k At this point, we only consider the inertial measurement set I 0:k = {I 0,1 ...I k-1,k}. We can therefore state the MAP estimation problem where the posterior distribution to be maximized is:

[0058]

[0059] p(I 0:k |y k ) represents the likelihood estimate, p(y k ) represents the prior value. Considering the independence of the measurements, the inertial MAP estimation problem can be written as:

[0060]

[0061] Taking the negative logarithm and assuming IMU pre-integration and Gaussian errors of the prior distribution, this ultimately leads to the optimization problem:

[0062]

[0063] Because the latest trajectory estimated by visual SLAM is regarded as a constant and a priori residual is added to force the IMU bias to be close to zero. Therefore, the possible range of values ​​of the IMU bias is predetermined by the covariance matrix ∑b. When formally estimating, a contraction needs to be defined to update R during the estimation optimization process. wg Since rotation about the direction of gravity does not assume a change in gravity, this update uses two angles (δα g , δβ g ) to parameterize:

[0064]

[0065] At the same time, in order to ensure that the scale factor remains greater than 0 during the optimization process, we can define its update as:

[0066] s new =s old exp(δs)

[0067] After the inertial optimization is complete, the frame pose, velocity, and 3D map points are scaled by the estimated scale factor and rotated to align the z-axis with the estimated gravity direction. The bias is updated and the IMU pre-integration is repeated to reduce future linearization errors.

[0068] Finally, we perform MAP estimation with visual-inertial fusion: After having good estimates of inertial and visual parameters, we perform a joint visual-inertial optimization to further refine the result. This optimization has a common bias for all keyframes and includes the same prior information about the bias as in the inertial-only step.

[0069] This paper conducts detailed initialization experiments on actual production environment datasets and shows that this initialization method is very effective. Under a 2-second trajectory, a scale error of 5% is achieved. In order to further improve the initial estimate, visual inertial BA is performed a few seconds or tens of seconds after initialization, converging to a scale error of 1%. After these BAs, the overall AR spatial map can be confirmed that the scale, IMU parameters, gravity direction, etc. have been accurately estimated, and the AR space is reliable.

[0070] Example 2

[0071] The present invention also provides a schematic structural diagram of an inertial measurement unit initialization device. At the hardware level, the inertial measurement unit initialization device may include multiple processors, an internal bus, a network interface, a memory, and a non-volatile memory, and of course may also include hardware required for other services. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to achieve the above Figure 1 Of course, in addition to the software implementation, the present invention does not exclude other implementations, such as logic devices or a combination of software and hardware, etc., that is, the execution subject of the following processing flow is not limited to each logic unit, but can also be hardware or logic devices.

[0072] For the improvement of a technology, it can be clearly distinguished whether it is a hardware improvement (for example, improvement of the circuit structure of diodes, transistors, switches, etc.) or a software improvement (improvement of the method flow). However, with the development of technology, many improvements of the method flow today can be regarded as direct improvements of the hardware circuit structure. Designers almost always obtain the corresponding hardware circuit structure by programming the improved method flow into the hardware circuit. Therefore, it cannot be said that the improvement of a method flow cannot be implemented with a hardware entity module. For example, a programmable logic device (PLD) (such as a field programmable gate array (FPGA)) is such an integrated circuit whose logical function is determined by the user's programming of the device. Designers can "integrate" a digital system on a PLD by programming themselves, without having to ask chip manufacturers to design and make dedicated integrated circuit chips. Moreover, nowadays, instead of manually making integrated circuit chips, this kind of programming is mostly implemented by "logic compiler" software, which is similar to the software compiler used when developing and writing programs, and the original code before compilation must also be written in a specific programming language, which is called hardware description language (HDL). There is not only one HDL, but many kinds, such as ABEL (Advanced Boolean Expression Language), AHDL (Altera Hardware Description Language), Confluence, CUPL (Cornell University Programming Language), HDCal, JHDL (Java Hardware Description Language), Lava, Lola, MyHDL, PALASM, RHDL (Ruby Hardware Description Language), etc. The most commonly used ones are VHDL (Very-High-Speed ​​Integrated Circuit Hardware Description Language) and Verilog. Those skilled in the art should also know that it is only necessary to program the method flow slightly in the above-mentioned hardware description languages ​​and program it into the integrated circuit, and then it is easy to obtain the hardware circuit that implements the logic method flow.

[0073] The controller can be implemented in any appropriate manner, for example, the controller can take the form of a microprocessor or processor and a computer-readable medium storing a computer-readable program code (such as software or firmware) that can be executed by the (micro)processor, a logic gate, a switch, an application-specific integrated circuit (ASIC), a programmable logic controller, and an embedded microcontroller. Examples of controllers include, but are not limited to, the following microcontrollers: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20, and Silicone Labs C8051F320. The memory controller can also be implemented as part of the control logic of the memory. Those skilled in the art also know that in addition to implementing the controller in a purely computer-readable program code manner, the controller can be implemented in the form of a logic gate, a switch, an application-specific integrated circuit, a programmable logic controller, and an embedded microcontroller by logically programming the method steps. Therefore, this controller can be considered as a hardware component, and the devices included therein for implementing various functions can also be regarded as structures within the hardware component. Or even, the devices for implementing various functions can be regarded as both software modules for implementing the method and structures within the hardware component.

[0074] The systems, devices, modules or units described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0075] For the convenience of description, the above device is described as being divided into various units according to their functions. Of course, when implementing the present invention, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0076] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0077] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0078] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0079] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0080] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0081] The memory may include non-permanent storage in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0082] Example 3

[0083] The present invention also provides a computer-readable storage medium on which a program is stored. When the program is executed, the method in the embodiment is implemented.

[0084] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0085] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0086] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0087] The present invention may be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. The present invention may also be practiced in distributed computing environments where tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules may be located in local and remote computer storage media, including storage devices.

[0088] Each embodiment of the present invention is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0089] The preferred specific embodiments of the present invention have been described. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

[0090] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

[0091] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A method for initializing an inertial measurement unit, characterized in that: The method comprises: S1. Initialize monocular or binocular SLAM, insert keyframes, obtain a map consisting of camera poses and points, and obtain trajectory T on the map; S2, define the MAP estimation problem based on the inertial measurement between the trajectory T and the keyframes, consider the Gaussian error of the IMU pre-integration and the prior distribution, transform the estimation problem into an optimization problem, and update the rotation matrix R wg Solve the optimization problem to obtain the best estimate of the inertial variables and the scale factor, then scale the map based on the best scale factor and rotate it to align the z-axis with the estimated gravity direction, where the estimated gravity is obtained based on the optimized rotation matrix; S3. Jointly optimize the key frames based on the best estimates of the inertial variables and the scale factors.

2. The inertial measurement unit initialization method according to claim 1, characterized in that: The set of inertial measurements between the trajectory T and the keyframes is I 0:k = {I 0,1 ...I k-1,k }.

3. The inertial measurement unit initialization method according to claim 2, characterized in that: The posterior distribution that maximizes the MAP estimation problem is: Among them, p(I 0:k |y k ) represents the likelihood estimate, p(y k ) represents the prior value.

4. The inertial measurement unit initialization method according to claim 3, characterized in that: The MAP estimation problem is: Where s is the visual scale factor, R wg represents the rotation matrix, represents the body velocity, and b represents the bias of the accelerometer and gyroscope.

5. The inertial measurement unit initialization method according to claim 1, characterized in that: The transformation of the estimation problem into an optimization problem is specifically as follows: Taking the negative logarithm of the estimation problem and assuming IMU pre-integration and Gaussian errors in the prior distribution, we obtain the optimization problem.

6. The inertial measurement unit initialization method according to claim 5, characterized in that: The optimization problem is specifically: ∑b represents the covariance matrix.

7. The inertial measurement unit initialization method according to claim 5, characterized in that: Update the rotation matrix R wg Specifically: in, represents the updated rotation matrix, represents the rotation matrix before updating, δα g and, δβ g Indicates the updated angle.

8. The inertial measurement unit initialization method according to claim 7, characterized in that: In the process of solving the optimization problem, the scale factor is greater than 0.

9. An inertial measurement unit initialization device, comprising a memory, a processor, and a program stored in the memory, characterized in that: When the processor executes the program, the method according to any one of claims 1 to 8 is implemented.

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

Images