阅读说明：本技术 一种单目视觉定位的imu辅助跟踪方法及装置 (Monocular vision positioning IMU (inertial measurement unit) auxiliary tracking method and device ) 是由 白立建 李骊 于 2019-10-16 设计创作，主要内容包括：本发明提供了一种单目视觉定位的IMU辅助跟踪方法及装置,在视觉系统初始化完成且IMU在数据稳定时初始化完成后,获取视觉定位结果和IMU定位结果；在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下,依据所述视觉定位结果对所述IMU定位结果进行校正；在视觉跟踪失败的情况下,利用所述IMU定位结果辅助视觉定位。视觉跟踪正常时,通过视觉定位结果对IMU定位结果进行校正,提高IMU定位定姿的精度,视觉跟踪失败时,通过利用IMU定位结果辅助视觉定位,提高动态跟踪定位精度。(The invention provides an IMU (inertial measurement unit) auxiliary tracking method and device for monocular vision positioning, wherein a vision positioning result and an IMU positioning result are obtained after initialization of a vision system is completed and the initialization is completed when data of the IMU is stable; under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time, correcting the IMU positioning result according to the visual positioning result; and in the case of visual tracking failure, assisting visual positioning by utilizing the IMU positioning result. When the visual tracking is normal, the IMU positioning result is corrected through the visual positioning result, the IMU positioning and attitude determining precision is improved, and when the visual tracking fails, the dynamic tracking and positioning precision is improved by assisting the visual positioning through the IMU positioning result.)

1. An IMU assisted tracking method for monocular visual positioning, comprising:

after the initialization of the vision system is completed and the initialization of the IMU is completed when the data is stable, obtaining a vision positioning result and an IMU positioning result;

under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time, correcting the IMU positioning result according to the visual positioning result;

and in the case of visual tracking failure, assisting visual positioning by utilizing the IMU positioning result.

2. The method of claim 1, further comprising:

and establishing a visual coordinate system to finish the initialization of the visual system.

3. The method of claim 2, wherein after completion of the vision system initialization, the method further comprises:

calculating an attitude transfer matrix from the IMU to a navigation coordinate system when the IMU data is stable;

calculating a posture transfer matrix from the IMU to the visual coordinate system;

and calculating the attitude transfer matrix from the visual coordinate system to the navigation coordinate system according to the attitude transfer matrix from the IMU to the navigation coordinate system and the attitude transfer matrix from the IMU to the visual coordinate system, and finishing the initialization of the IMU.

4. The method of claim 3, wherein after the computing the pose transfer matrix of the IMU to the visual coordinate system, the method further comprises:

and setting the Z-axis Euler angle from the IMU to the navigation coordinate system as the Z-axis Euler angle of the posture transfer matrix from the IMU to the visual coordinate system.

5. The method of claim 4, wherein after the computing the pose transfer matrix of the visual coordinate system to the navigational coordinate system, the method further comprises:

and when the IMU data is stable, calculating the posture transfer matrix from the visual coordinate system to the navigation coordinate system for multiple times, and averaging the posture transfer matrices from the visual coordinate system to the navigation coordinate system obtained through calculation to obtain an accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

6. The method of claim 5, wherein said using the IMU positioning results to assist visual positioning comprises:

acquiring acceleration and gyroscope data obtained by IMU measurement;

updating a pose transfer matrix of the IMU to the visual coordinate system based on the gyroscope data;

calculating a posture transfer matrix from the IMU to the navigation coordinate system based on the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system and the updated posture transfer matrix from the IMU to the visual coordinate system;

calculating a displacement vector in the navigation coordinate system based on the acceleration;

and calculating the displacement vector in the visual coordinate system according to the displacement vector in the navigation coordinate system and the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

7. An IMU assisted tracking apparatus for monocular visual positioning, comprising:

the positioning result acquisition unit is used for acquiring a visual positioning result and an IMU positioning result after the initialization of the visual system is completed and the initialization of the IMU is completed when the data is stable;

the positioning result correcting unit is used for correcting the IMU positioning result according to the visual positioning result under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time;

and the auxiliary visual positioning unit is used for utilizing the IMU positioning result to assist visual positioning under the condition of visual tracking failure.

8. The apparatus of claim 7, further comprising:

and the visual initialization unit is used for establishing a visual coordinate system and finishing the initialization of the visual system.

9. The apparatus of claim 8, further comprising:

the IMU initialization unit is used for calculating a posture transfer matrix from the IMU to a navigation coordinate system when the IMU data is stable; calculating a posture transfer matrix from the IMU to the visual coordinate system; and calculating the attitude transfer matrix from the visual coordinate system to the navigation coordinate system according to the attitude transfer matrix from the IMU to the navigation coordinate system and the attitude transfer matrix from the IMU to the visual coordinate system, and finishing the initialization of the IMU.

10. The apparatus of claim 9, further comprising:

and the Z-axis Euler angle setting unit is used for setting the Z-axis Euler angle from the IMU to the navigation coordinate system as the Z-axis Euler angle of the posture transfer matrix from the IMU to the visual coordinate system.

Technical Field

The invention relates to the technical field of visual positioning, in particular to an IMU (inertial measurement unit) auxiliary tracking method and device for monocular visual positioning.

Background

The traditional monocular visual navigation is to perform positioning and tracking according to one frame of image, is limited by the updating frequency of the image, and is easy to have the phenomena of frame loss and tracking failure during rapid movement, so that the positioning and pose determination can not be performed rapidly and accurately. The IMU (inertial Measurement Unit) can sensitively measure the angular velocity and the acceleration of a carrier in an inertial space, and position and fix the posture through integration, so that the IMU can be adopted for position tracking under the condition of quick movement and visual loss.

How to obtain an accurate positioning result by utilizing IMU assisted tracking becomes a technical problem to be solved urgently in the field.

Disclosure of Invention

In view of the above, the invention provides an IMU (inertial measurement unit) assisted tracking method for monocular vision positioning, which quickly fuses vision and IMU information and solves the problem that vision cannot position and position with high precision during quick movement.

In order to achieve the above purpose, the invention provides the following specific technical scheme:

an IMU assisted tracking method for monocular visual positioning, comprising:

after the initialization of the vision system is completed and the initialization of the IMU is completed when the data is stable, obtaining a vision positioning result and an IMU positioning result;

under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time, correcting the IMU positioning result according to the visual positioning result;

and in the case of visual tracking failure, assisting visual positioning by utilizing the IMU positioning result.

Optionally, the method further includes:

and establishing a visual coordinate system to finish the initialization of the visual system.

Optionally, after the initialization of the vision system is completed, the method further includes:

calculating an attitude transfer matrix from the IMU to a navigation coordinate system when the IMU data is stable;

calculating a posture transfer matrix from the IMU to the visual coordinate system;

and calculating the attitude transfer matrix from the visual coordinate system to the navigation coordinate system according to the attitude transfer matrix from the IMU to the navigation coordinate system and the attitude transfer matrix from the IMU to the visual coordinate system, and finishing the initialization of the IMU.

Optionally, after the computing the pose transfer matrix of the IMU to the visual coordinate system, the method further includes:

and setting the Z-axis Euler angle from the IMU to the navigation coordinate system as the Z-axis Euler angle of the posture transfer matrix from the IMU to the visual coordinate system.

Optionally, after the computing the pose transfer matrix of the visual coordinate system to the navigation coordinate system, the method further includes:

and when the IMU data is stable, calculating the posture transfer matrix from the visual coordinate system to the navigation coordinate system for multiple times, and averaging the posture transfer matrices from the visual coordinate system to the navigation coordinate system obtained through calculation to obtain an accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

Optionally, the assisting the visual positioning by using the positioning result of the IMU includes:

acquiring acceleration and gyroscope data obtained by IMU measurement;

updating a pose transfer matrix of the IMU to the visual coordinate system based on the gyroscope data;

calculating a posture transfer matrix from the IMU to the navigation coordinate system based on the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system and the updated posture transfer matrix from the IMU to the visual coordinate system;

calculating a displacement vector in the navigation coordinate system based on the acceleration;

and calculating the displacement vector in the visual coordinate system according to the displacement vector in the navigation coordinate system and the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

An IMU-assisted tracking apparatus for monocular visual positioning, comprising:

the positioning result acquisition unit is used for acquiring a visual positioning result and an IMU positioning result after the initialization of the visual system is completed and the initialization of the IMU is completed when the data is stable;

the positioning result correcting unit is used for correcting the IMU positioning result according to the visual positioning result under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time;

and the auxiliary visual positioning unit is used for utilizing the IMU positioning result to assist visual positioning under the condition of visual tracking failure.

Optionally, the apparatus further comprises:

and the visual initialization unit is used for establishing a visual coordinate system and finishing the initialization of the visual system.

Optionally, the apparatus further comprises:

the IMU initialization unit is used for calculating a posture transfer matrix from the IMU to a navigation coordinate system when the IMU data is stable; calculating a posture transfer matrix from the IMU to the visual coordinate system; and calculating the attitude transfer matrix from the visual coordinate system to the navigation coordinate system according to the attitude transfer matrix from the IMU to the navigation coordinate system and the attitude transfer matrix from the IMU to the visual coordinate system, and finishing the initialization of the IMU.

Optionally, the apparatus further comprises:

and the Z-axis Euler angle setting unit is used for setting the Z-axis Euler angle from the IMU to the navigation coordinate system as the Z-axis Euler angle of the posture transfer matrix from the IMU to the visual coordinate system.

Optionally, the IMU initializing unit is further configured to calculate a posture transfer matrix from the visual coordinate system to the navigation coordinate system for multiple times when IMU data is stable, and average the calculated posture transfer matrices from the visual coordinate system to the navigation coordinate system to obtain an accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

Optionally, the auxiliary visual positioning unit is specifically configured to:

acquiring acceleration and gyroscope data obtained by IMU measurement;

updating a pose transfer matrix of the IMU to the visual coordinate system based on the gyroscope data;

calculating a posture transfer matrix from the IMU to the navigation coordinate system based on the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system and the updated posture transfer matrix from the IMU to the visual coordinate system;

calculating a displacement vector in the navigation coordinate system based on the acceleration;

and calculating the displacement vector in the visual coordinate system according to the displacement vector in the navigation coordinate system and the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

Compared with the prior art, the invention has the following beneficial effects:

the invention discloses an IMU (inertial measurement unit) auxiliary tracking method for monocular vision positioning, which corrects an IMU positioning result according to a vision positioning result under the condition that vision tracking is normal and the vision positioning time is consistent with the IMU positioning time, improves the IMU positioning and attitude determining precision, fuses the vision positioning result and the IMU positioning result according to the measurement characteristics of the IMU and a vision system under the condition that the vision tracking is failed, and utilizes the IMU positioning result to assist the vision positioning and improve the dynamic tracking and positioning precision.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.

FIG. 1 is a schematic flowchart of an IMU assisted tracking method for monocular visual positioning according to an embodiment of the present invention;

fig. 2 is a schematic structural diagram of an IMU auxiliary tracking apparatus for monocular visual positioning according to an embodiment of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

The embodiment discloses an auxiliary tracking method for an IMU (inertial measurement unit) based on monocular visual positioning, which can be applied to mobile terminals such as smart phones, tablet computers and the like, and please refer to fig. 1, wherein the method comprises the following steps:

s101: after the initialization of the vision system is completed and the initialization of the IMU is completed when the data is stable, obtaining a vision positioning result and an IMU positioning result;

after the mobile device is powered on, the visual system and the IMU are initialized first.

Wherein the initializing of the vision system comprises establishing a vision coordinate system.

When the IMU data is stable, specifically, whether the IMU data is stable may be determined by determining whether a difference between the IMU data and the IMU data in the stationary state is smaller than a threshold.

IMU initialization includes computing IMU to navigational coordinate system pose transfer matrices when IMU data is stable

Computing pose transfer matrices for IMUs to visual coordinate systems

And transferring the matrix according to the posture from the IMU to the navigation coordinate system

And IMU to visual coordinate system pose transfer matrix

Calculating a posture transfer matrix from a visual coordinate system to a navigation coordinate systemWherein the content of the first and second substances,

the calculation method of (c) is as follows:

wherein a pose transfer matrix of the IMU to the navigation coordinate system is calculated

The method comprises the following steps:

the initial environment of the conventional POS is a static or slightly-shaking environment, and the coarse alignment can be carried out by adopting a conventional analytic method. Most of the used inertial navigation systems adopt a reference coordinate system fixedly connected with the earth^[73]So two vectors that are not collinear in space are measured by means of the inertial instrument: gravity vector g and earth rotation angular rate vector omega_ieSelf-alignment is achieved. Commonly used coarse alignment algorithms include: utilizing g and omega_ieAnd g x ω of structure_ieCalculating an attitude matrix; ② utilizing g, g multiplied by omega of structure_ieAnd (g × ω) of structure_ie) Calculating an attitude matrix by Xg; and thirdly, a direct calculation method by using the output of the inertia device. In making the initial alignment, the local longitude λ and latitude L are known, the gravitational acceleration g and the earth's rotational angular velocity ω_ieThe components in the geographic coordinate system are determined and can be represented as the following two vectors:

strapdown matrix

The method comprises 9 unknown elements, and in order to solve all 9 elements, 9 equations need to be constructed, which means that 3 three-dimensional vectors are needed, so that any non-collinear vector in a carrier coordinate system is takenBy means of a strapdown matrix

The transformation into the navigation coordinate system is:

then there are:

since the strapdown matrix is an orthogonal matrix, it is possible to reduce the number of the orthogonal matrix

Can be written as:

the gravitational acceleration and the earth rotation acceleration are known quantities, and their cross product is also a known quantity.

Thus can make d₁＝g，d₂＝g×ω_ie，d₃＝(g×ω_ie) X g, or d₁＝g，d₂＝ω_ie，d₃＝g×ω_ieThen, there are:

or:

to improve

Can be obtained multiple times

Taking an average value, but because the Z-axis Euler angle in the attitude angle solved by initialization is inconsistent with the Z-axis Euler angle of the gyroscope in the IMU, calculation is requiredPreviously, the IMU was transferred to the pose transfer matrix of the visual coordinate systemThe Z-axis Euler angle of the IMU is set as the Z-axis Euler angle of the gyroscope of the IMU, and on the basis, the attitude transfer matrix from the visual coordinate system to the navigation coordinate system is calculated for multiple times when the IMU data is stable

And for a plurality of the calculatedTaking an average value to obtain accurate

The initialization of the IMU is completed.

It should be further noted that, the IMU data is collected in real time, and when the IMU is not moved, the IMU is initialized again to obtain the IMU dataIs a constant matrix.

S102: under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time, correcting the IMU positioning result according to the visual positioning result;

because the visual tracking updating frequency is slower, and the IMU updating frequency is faster, the visual positioning result in an ideal state can be ensured to be consistent with the IMU positioning result only under the condition that the visual positioning time is consistent with the IMU positioning time, and when the visual positioning time is consistent with the IMU positioning time, the IMU positioning result is corrected according to the visual positioning result to obtain accurate positioning data.

S103: and in the case of visual tracking failure, assisting visual positioning by utilizing the IMU positioning result.

If the visual tracking fails, if the visual system loses frames, the visual positioning result is inaccurate, and the visual positioning needs to be assisted by the IMU positioning result.

Obtaining acceleration measured by IMU

And gyroscope data.

First, a pose transfer matrix for the IMU to a visual coordinate system based on gyroscope data

And updating, namely setting the Z-axis Euler angle from the IMU to the navigation coordinate system as the Z-axis Euler angle of the attitude transfer matrix from the IMU to the visual coordinate system, so that the Z-axis Euler angle in the attitude angle is consistent with the Z-axis Euler angle of a gyroscope in the IMU.

Then based on the accurate attitude transfer matrix from the visual coordinate system to the navigation coordinate system and the updated attitude transfer matrix from the IMU to the visual coordinate system

Computing a pose transfer matrix from an IMU to a navigation coordinate system

A displacement vector in the navigational coordinate system is then calculated based on the velocity.

In particular, a pose transfer matrix from the IMU to the navigation coordinate system is utilizedAnd acceleration measured by the accelerometer

Calculating acceleration in a navigation coordinate system

Acceleration measured by an accelerometer

The absolute acceleration includes all accelerations of the IMU relative to the inertial system, including harmful accelerations such as gravitational acceleration, so the acceleration in the navigation coordinate system needs to be eliminated

Harmful acceleration in the vehicle.

The present embodiment provides two methods for determining the relationship between the gravitational acceleration and the visual coordinate system, wherein the method for determining the relationship between the gravitational acceleration and the visual coordinate system is to determine the relationship between the gravitational acceleration and the visual coordinate system, and the method for determining the relationship between the gravitational acceleration and the visual coordinate system includes:

the method comprises the following steps: solving an attitude transfer matrix from the IMU to the navigational coordinate system

Given the attitude transformation matrix from IMU to visual coordinate system at the same time

Can find out

The gravity acceleration coincides with the Z axis of the navigation coordinate system, so that the displacement change of the IMU under the visual coordinate system can be calculated.

The method 2 comprises the following steps: and solving the component of gravity in the visual coordinate system based on the initialization step of the VINS system, thereby removing the harmful acceleration and solving the displacement change of the IMU in the visual coordinate system.

Integrating the accelerated speed after eliminating the harmful accelerated speed and calculating the position to obtain a displacement vector in the navigation coordinate system, and specifically, integrating the accelerated speed after eliminating the harmful accelerated speed to obtain a velocity vector V in the navigation coordinate systemⁿFor velocity vector VⁿPosition calculation is carried out to obtain the position in the navigation coordinate systemMoving vector Tⁿ。

According to the displacement vector T in the navigation coordinate systemⁿAnd an accurate pose transfer matrix from the visual coordinate system to the navigational coordinate system

Calculating a displacement vector T in a visual coordinate system^cam。

In the method for assisting tracking of the IMU in monocular visual positioning disclosed by the embodiment, under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time, the IMU positioning result is corrected according to the visual positioning result, so that the IMU positioning and attitude determining precision is improved, under the condition that the visual tracking is failed, the visual positioning result and the IMU positioning result are fused according to the measurement characteristics of the IMU and a visual system, the IMU positioning result is used for assisting the visual positioning, and the dynamic tracking and positioning precision is improved.

Based on the above embodiment, the embodiment discloses an IMU auxiliary tracking method for monocular vision positioning, and this embodiment correspondingly discloses an IMU auxiliary tracking apparatus for monocular vision positioning, please refer to fig. 2, and the apparatus includes:

a positioning result obtaining unit 201, configured to obtain a visual positioning result and an IMU positioning result after the initialization of the visual system is completed and the initialization of the IMU is completed when the data is stable;

a positioning result correcting unit 202, configured to correct, when the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time, the IMU positioning result according to the visual positioning result;

an auxiliary visual positioning unit 203, configured to assist visual positioning using the IMU positioning result in case of a visual tracking failure.

Optionally, the apparatus further comprises:

and the visual initialization unit is used for establishing a visual coordinate system and finishing the initialization of the visual system.

Optionally, the apparatus further comprises:

the IMU initialization unit is used for calculating a posture transfer matrix from the IMU to a navigation coordinate system when the IMU data is stable; calculating a posture transfer matrix from the IMU to the visual coordinate system; and calculating the attitude transfer matrix from the visual coordinate system to the navigation coordinate system according to the attitude transfer matrix from the IMU to the navigation coordinate system and the attitude transfer matrix from the IMU to the visual coordinate system, and finishing the initialization of the IMU.

Optionally, the apparatus further comprises:

and the Z-axis Euler angle setting unit is used for setting the Z-axis Euler angle from the IMU to the navigation coordinate system as the Z-axis Euler angle of the posture transfer matrix from the IMU to the visual coordinate system.

Optionally, the IMU initializing unit is further configured to calculate a posture transfer matrix from the visual coordinate system to the navigation coordinate system for multiple times when IMU data is stable, and average the calculated posture transfer matrices from the visual coordinate system to the navigation coordinate system to obtain an accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

Optionally, the auxiliary visual positioning unit is specifically configured to:

acquiring acceleration and gyroscope data obtained by IMU measurement;

updating a pose transfer matrix of the IMU to the visual coordinate system based on the gyroscope data;

calculating a posture transfer matrix from the IMU to the navigation coordinate system based on the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system and the updated posture transfer matrix from the IMU to the visual coordinate system;

calculating a displacement vector in the navigation coordinate system based on the acceleration;

and calculating the displacement vector in the visual coordinate system according to the displacement vector in the navigation coordinate system and the accurate posture transfer matrix from the visual coordinate system to the navigation coordinate system.

The auxiliary tracking device for the IMU of monocular vision positioning disclosed by the embodiment corrects the positioning result of the IMU according to the vision positioning result under the condition that the vision tracking is normal and the vision positioning time is consistent with the positioning time of the IMU, improves the positioning and attitude-fixing precision of the IMU, fuses the vision positioning result and the IMU positioning result according to the measurement characteristics of the IMU and a vision system under the condition that the vision tracking is failed, and utilizes the IMU positioning result to assist the vision positioning, so that the dynamic tracking positioning precision is improved.

The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. The device disclosed by the embodiment corresponds to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description.

It is further noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.

The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. A software module may reside in Random Access Memory (RAM), memory, Read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.

The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.