CN105931275A - Monocular and IMU fused stable motion tracking method and device based on mobile terminal - Google Patents

Abstract

The invention discloses a monocular and IMU fused stable motion tracking method and device based on a mobile terminal, belonging to the technical field of AR/VR motion tracking. The method comprises the following steps of: judging whether the number of tracking feature points of a current frame of an image is greater than a pre-set threshold value or not, if so, performing feature point tracking by adopting an optical flow method so as to obtain the current pose of a camera, if not, obtaining feature points by adopting a FAST feature detection operator, and performing feature matching of the image by adopting a BRIEF algorithm calculation descriptor so as to obtain the current pose of the camera; performing Kalman filtering of the current pose of the camera so as to obtain a visual pose; obtaining acceleration and angular speed values generated by an IMU in a three-dimensional space, and performing integral operation of the acceleration and angular speed values so as to obtain the pose of the IMU; and performing Kalman fusion of the visual pose and the pose of the IMU, and performing motion tracking. Compared with the prior art, more stable and rapid motion tracking can be obtained on mobile terminal equipment.

Description

The stable motion tracking merged based on mobile terminal monocular and IMU and device

Technical field

The present invention relates to AR/VR motion tracking technology field, particularly relate to a kind of merge based on mobile terminal monocular and IMU Stable motion tracking and device.

Background technology

Motion tracking technology is intended to measure, follows the tracks of, records object movement locus in three dimensions, and it mainly passes through Sensor technology obtains the information of moving scene, and is calculated the tracked object attitude in space in real time.It is mainly applied In AR (Augmented Reality, augmented reality)/VR (Virtual Reality, virtual reality), wearable device, machine The fields such as people and automatic Pilot navigation.Currently, move the motion trackings such as AR/VR and mainly use handle to interact, handing over The gyroscope simply using mobile phone during Hu carries out rotating tracking.To Nister in 2004, visual odometry is proposed first Since concept, the method for view-based access control model speedometer has become the main flow of real-time Attitude estimation and motion tracking.It is by estimating phase Machine, in the incremental motion in space, determines the movement locus of camera at time and space.

At present the motion tracking method of main flow has a visual tracking method based on monocular or binocular, wherein single objective vision with Track method is relatively low due to its equipment cost, the current mobile platform being widely present in, and in mobile phone, flat board, therefore suffers from Increasing attention.But owing to cost is limited, the photographic head frame per second of the mobile phone terminal of current main-stream is the most relatively low and image passes The noise of sensor is relatively large so that it is poor to the adaptability of environment during motion tracking.Currently, based on mobile terminal Monocular movement tracking be primarily present on sensor and not enough of both in principle.Itself say from sensor, due to Limited by mobile terminal image quality and frame per second, when in ambient image, less being easily caused of characteristic point is followed the tracks of unsuccessfully；When equipment enters During row rapid movement, image can be made to produce motion blur, cause motion tracking failure；From the principle, motion based on monocular Tracking can only illustrate the relative motion trend of camera by increment list, and does not have absolute dimensional information.These two aspects is very the biggest Constrain the concrete actual application being currently based on mobile terminal.

Summary of the invention

The technical problem to be solved in the present invention is to provide one and can obtain more stable and quick on the equipment of mobile terminal The stable motion tracking merged based on mobile terminal monocular and IMU of motion tracking and device.

For solving above-mentioned technical problem, the present invention provides technical scheme as follows:

A kind of stable motion tracking merged based on mobile terminal monocular and IMU, including:

Obtain image；

Judge that whether the tracking characteristics of the present frame of image counts out more than predetermined threshold value, if it is, use optical flow method Carry out feature point tracking, obtain the current pose of camera, if it is not, then use FAST feature detection operator to obtain characteristic point, and Use BRIEF algorithm to calculate description and image is carried out characteristic matching, obtain the current pose of camera；

The current pose of camera is carried out Kalman filtering, obtains vision pose；

Obtain acceleration and magnitude of angular velocity that IMU produces at three dimensions, and acceleration and magnitude of angular velocity are integrated Computing, obtains IMU pose and predicts the outcome；

Vision pose and IMU pose are predicted the outcome and carries out Kalman's fusion, enter according to the posture information obtained after merging Row motion tracking.

Further, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

To the Corresponding matching feature point set on two two field pictures adjacent before and after image, it is calculated the basis between two two field pictures Matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

According to essential matrix, SVD is used to recover to obtain the relative pose of adjacent interframe；

Relative pose is multiplied with the absolute pose of the camera of the former frame obtained, obtains the current pose of camera.

Further, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

Following the tracks of the block taking a certain size around successful characteristic point respectively, using image correlation algorithm SSD, remove not Meet the characteristic point of threshold value.

Further, described employing FAST feature detection operator obtains characteristic point, and uses BRIEF algorithm to calculate description Image is carried out characteristic matching, and the current pose obtaining camera includes:

Use FAST feature detection operator to obtain characteristic point image present frame, use BRIEF algorithm to calculate and describe son also With initial frame characteristic matching, directly calculate the initial frame transformation matrix to present frame；

Transformation matrix is multiplied with the absolute pose of the camera of initial frame, obtains the current pose of camera.

Further, the described current pose to camera carries out Kalman filtering, obtains vision pose and includes:

Step 1: for each frame of image, uses optical flow method accumulation acquired results and characteristic point directly to mate eligible result and enter Row Kalman filtering, obtains the current pose of camera more accurately, and carries out continuous iterated transform；

Step 2: use the renewal equation of Kalman Filtering for Discrete, is calculated current covariance and estimates P_k ^-, it is concrete, The renewal equation of Kalman Filtering for Discrete is:

${\hat{x}}_k=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}$

P_k ^-=AP_k-1A^T+Q

Wherein,Being optical flow method calculated camera attitude, A is state-transition matrix, and B is to control gain, P_k-1It is The covariance of former frame is estimated, Q is noise covariance matrix；

Step 3: use observational equation, calculates Kalman gain K_k, concrete, observational equation is:

z_k=Hx_k+v_k

K_k=P_k ^-H^T(HP_k ^-H^T+R)^-1

Wherein, z_kIt is that H is observing matrix, v by FAST Feature Points Matching algorithm calculated camera attitude_kRepresent and see Surveying noise, R is the covariance matrix of observation noise；

Step 4: according to Kalman Filtering for Discrete device state renewal equation, the system that updates is arranged:

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$

$P_k=(I-K_{k}H)P_k^{-}.$

Device is followed the tracks of in a kind of stable motion merged based on mobile terminal monocular and IMU, including:

Acquisition module: be used for obtaining image；

Visual tracking module: whether be more than predetermined threshold value for judging that the tracking characteristics of the present frame of image is counted out, as Fruit is, then use optical flow method to carry out feature point tracking, obtain the current pose of camera, if it is not, then use FAST feature detection Operator obtains characteristic point, and uses BRIEF algorithm calculating description that image is carried out characteristic matching, obtains the present bit of camera Appearance；

Filtration module: for the current pose of camera is carried out Kalman filtering, obtain vision pose；

IMU pose computing module: for obtaining acceleration and the magnitude of angular velocity that IMU produces at three dimensions, and to acceleration Degree and magnitude of angular velocity are integrated computing, obtain IMU pose and predict the outcome；

Fusion Module: carry out Kalman's fusion for predicting the outcome vision pose and IMU pose, obtains according to after merging Posture information carry out motion tracking.

Further, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

To the Corresponding matching feature point set on two two field pictures adjacent before and after image, it is calculated the basis between two two field pictures Matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

According to essential matrix, SVD is used to recover to obtain the relative pose of adjacent interframe；

Relative pose is multiplied with the absolute pose of the camera of the former frame obtained, obtains the current pose of camera.

Further, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

Following the tracks of the block taking a certain size around successful characteristic point respectively, using image correlation algorithm SSD, remove not Meet the characteristic point of threshold value.

Further, described employing FAST feature detection operator obtains characteristic point, and uses BRIEF algorithm to calculate description Image is carried out characteristic matching, and the current pose obtaining camera includes:

Use FAST feature detection operator to obtain characteristic point image present frame, use BRIEF algorithm to calculate and describe son also With initial frame characteristic matching, directly calculate the initial frame transformation matrix to present frame；

Transformation matrix is multiplied with the absolute pose of the camera of initial frame, obtains the current pose of camera.

Further, described filtration module, it is additionally operable to:

Kalman filtering module: use optical flow method accumulation acquired results and direct of characteristic point for each frame for image Join eligible result and carry out Kalman filtering, obtain the current pose of camera more accurately, and carry out continuous iterated transform；

First computing module: for using the renewal equation of Kalman Filtering for Discrete, be calculated current covariance and estimate Meter P_k ^-, concrete, the renewal equation of Kalman Filtering for Discrete is:

${\hat{x}}_k=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}$

P_k ^-=AP_k-1A^T+Q

Wherein,Being optical flow method calculated camera attitude, A is state-transition matrix, and B is to control gain, P_k-1It is The covariance of former frame is estimated, Q is noise covariance matrix；

Second computing module: be used for using observational equation, calculates Kalman gain K_k, concrete, observational equation is:

z_k=Hx_k+v_k

K_k=P_k ^-H^T(HP_k ^-H^T+R)^-1

Wherein, z_kIt is that H is observing matrix, v by FAST Feature Points Matching algorithm calculated camera attitude_kRepresent and see Surveying noise, R is the covariance matrix of observation noise；

Update system module: for according to Kalman Filtering for Discrete device state renewal equation, the system that updates is arranged:

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$

$P_k=(I-K_{k}H)P_k^{-}.$

The method have the advantages that

In the present invention, for being currently based on the problem of mobile tracking, the present invention uses quick FAST based on monocular to calculate Method and optical flow method are main, merge the existing IMU in mobile terminal (Inertial Measurement Unit Inertial Measurement Unit) simultaneously Hardware, on the premise of not by external equipment, it is achieved the stable motion tracking merged based on monocular and IMU.The present invention Using quick FAST algorithm and optical flow method, processing speed is fast, can realize real-time tracking；Feature Points Matching and optical flow method are merged Following the tracks of, the single method precision that visual tracking result ratio of precision is traditional is high；Melting of vision and IMU data is carried out under EKF framework Close, combine camera and the respective advantage of inertial sensor achieves pose quickly and accurately and estimates.Further, the present invention is led to Cross the stability by IMU data acquisition and high frame per second thereof, can effectively overcome and produce based on image characteristic point deficiency, motion blur etc. Raw tracking failure problem.While vision carries out tenacious tracking, by Kalman filtering realize the obtained attitude of vision with Track, to obtain mobile terminal spatial pose more steady, accurate.Meanwhile, the accurate camera pose obtained by Kalman filtering IMU data are corrected, reduce the IMU drift impact on precision itself.Finally, utilize Kalman filtering to IMU and monocular The pose that camera obtains merges, while obtaining stable motion tracking, it is achieved the size estimation rebuilding monocular.With existing Having technology to compare, the present invention can obtain more stable and quick motion tracking on the equipment of mobile terminal.

Accompanying drawing explanation

Fig. 1 is the schematic flow sheet of the stable motion tracking merged based on mobile terminal monocular and IMU of the present invention；

Fig. 2 is the vision pose calculating of the stable motion tracking merged based on mobile terminal monocular and IMU of the present invention With Kalman filter theory schematic diagram；

Fig. 3 is vision pose and the IMU of the stable motion tracking merged based on mobile terminal monocular and IMU of the present invention Pose Kalman merges principle schematic；

Fig. 4 is the coordinate system signal of the stable motion tracking merged based on mobile terminal monocular and IMU of the present invention Figure；

Fig. 5 is monocular vision and the IMU of the stable motion tracking merged based on mobile terminal monocular and IMU of the present invention System schematic；

Fig. 6 is that the technical scheme based on mobile terminal monocular and the stable motion tracking of IMU fusion of the present invention always flows Journey block diagram；

Fig. 7 is the structural representation of the stable motion tracking device merged based on mobile terminal monocular and IMU of the present invention.

Detailed description of the invention

For making the technical problem to be solved in the present invention, technical scheme and advantage clearer, below in conjunction with accompanying drawing and tool Body embodiment is described in detail.

On the one hand, the present invention provides a kind of stable motion tracking merged based on mobile terminal monocular and IMU, such as Fig. 1 Shown in, including:

Step S101: obtain image；

Step S102: judge that whether the tracking characteristics of the present frame of image counts out more than predetermined threshold value, if it is, adopt Carry out feature point tracking by optical flow method, obtain the current pose of camera, if it is not, then use FAST feature detection operator to obtain spy Levy a little, and use BRIEF algorithm calculating description that image is carried out characteristic matching, obtain the current pose of camera；

This step is the motion tracking process obtaining visual pattern based on monocular, carries out obtaining visual pattern based on monocular Before motion tracking, in terms of Image Feature Detection, it is contemplated that the computing capability that mobile terminal is more weak, use speed FAST feature detection operator, is tracked in conjunction with optical flow method.For picture frame sequence: I₀,...,I_k,I_k+1,...,I_k+n..., I₀,...,I_kTwo field picture uses FAST feature detection operator, and BRIEF algorithm calculates and describes son, and mates, until I_kFrame The match is successful, and some logarithm is more than threshold value, then initialize successfully.

In this step, the detailed process of motion tracking based on monocular acquisition visual pattern can be:

With position corresponding to the first frame as initial point, and the camera pose state position of the first two field picture [I | 0], I_kThe phase of two field picture The absolute pose of machine is [R_(0,k)|t_(0,k)].During the tracking of subsequent frame, the efficiency processed for raising, by using light stream Method carries out consecutive frame feature point tracking, until after n frame, when tracing into I_k+nWhen image characteristic point number is less than threshold value, for ensureing The robustness followed the tracks of, to I_k+nTwo field picture is reused FAST feature detection operator and is obtained the continuation tracking of more characteristic point, and Use BRIEF algorithm to calculate and describe son and I_kTwo field picture carries out characteristic matching and obtains pose more accurately.By above-mentioned tracking Method, has taken into account the efficiency during monocular vision is followed the tracks of and robustness to a certain extent.

Step S103: the current pose of camera is carried out Kalman filtering, obtains vision pose；

In this step, Kalman filtering (Kalman filtering) is a kind of state equation using linear system, logical Cross system input and output observation data, system mode is carried out the algorithm of optimal estimation.This step uses Kalman filtering energy Enough effective filter out the impact of the noise in system and interference, thus improve the stability that camera is followed the tracks of.

Step S104: obtain acceleration and magnitude of angular velocity that IMU produces at three dimensions, and to acceleration and magnitude of angular velocity It is integrated computing, obtains IMU pose and predict the outcome；

In this step, the IMU (Inertial measurement unit is called for short IMU) related to is Measuring Object three axle appearance State angular velocity (or angular speed) and the device of acceleration.General, IMU contains the accelerometer of three single shafts and three The gyro of single shaft, accelerometer founds the acceleration signal of three axles for detecting object in carrier coordinate system unification and independence, and gyro is used for examining Survey carrier relative to the angular velocity signal of navigational coordinate system, in this step, IMU can 3 vertical axis produce acceleration and Magnitude of angular velocity, is integrated predicting pose, and the monocular vision sensor in mobile device can provide the 3D not having yardstick Position and the measured value of pose.Obtaining IMU data front and back between consecutive frame and carry out pose prediction, a later frame vision pose is estimated It is counted as being updated for measured value.

Step S105: predict the outcome vision pose and IMU pose and carry out Kalman's fusion, according to the position obtained after merging Appearance information carries out motion tracking；

In this step, for obtaining stable tracking pose, make full use of the information that the sensor of vision and IMU obtains, this Invent by using Kalman's fusion method, the pose prediction knot that the vision pose obtained by visual pattern and IMU integration obtain Fruit is merged, to realize message complementary sense and the Target state estimator of two Dissimilar sensors, thus the most accurate after obtaining fusion, Pose reliably.And then, carry out motion tracking according to the posture information after merging.

In the present invention, for being currently based on the problem of mobile tracking, the present invention uses quick FAST based on monocular to calculate Method and optical flow method are main, merge the existing IMU in mobile terminal (Inertial Measurement Unit Inertial Measurement Unit) simultaneously Hardware, on the premise of not by external equipment, it is achieved the stable motion tracking merged based on monocular and IMU.The present invention Using quick FAST algorithm and optical flow method, processing speed is fast, can realize real-time tracking；Feature Points Matching and optical flow method are merged Following the tracks of, the single method precision that visual tracking result ratio of precision is traditional is high；Melting of vision and IMU data is carried out under EKF framework Close, combine camera and the respective advantage of inertial sensor achieves pose quickly and accurately and estimates.Further, the present invention is led to Cross the stability by IMU data acquisition and high frame per second thereof, can effectively overcome and produce based on image characteristic point deficiency, motion blur etc. Raw tracking failure problem.While vision carries out tenacious tracking, by Kalman filtering realize the obtained attitude of vision with Track, to obtain mobile terminal spatial pose more steady, accurate.Meanwhile, the accurate camera pose obtained by Kalman filtering IMU data are corrected, reduce the IMU drift impact on precision itself.Finally, utilize Kalman filtering to IMU and monocular The pose that camera obtains merges, while obtaining stable motion tracking, it is achieved the size estimation rebuilding monocular.With existing Having technology to compare, the present invention can obtain more stable and quick motion tracking on the equipment of mobile terminal.

As a modification of the present invention, the employing optical flow method in step S102 carries out feature point tracking, obtains camera Current pose includes:

To the Corresponding matching feature point set on two two field pictures adjacent before and after image, it is calculated the basis between two two field pictures Matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

According to essential matrix, SVD is used to recover to obtain the relative pose of adjacent interframe；

Relative pose is multiplied with the absolute pose of the camera of the former frame obtained, obtains the current pose of camera.

Carry out the improvement of feature point tracking method for above-mentioned employing optical flow method, the present invention provides the most concrete a kind of reality Under executing such as:

By the Corresponding matching feature point set (X on two two field pictures before and after during following the tracks of_L,X_R), by computer vision Basic skills, can obtain its corresponding relation X_L ^TFX_R=0, thus can be calculated the basis matrix F between two width images further.

By mutual relation between E between basis matrix F and essential matrix: E=K_L ^TFK_R, wherein (K_L,K_R) camera is interior respectively Parameter, in the monocular system of this mobile terminal, this intrinsic parameter can be demarcated and K in advance_L=K_R。

According to the essential matrix E obtained, utilize SVD can recover to obtain the relative pose [R of adjacent interframe_(k,k+1)| t_(k,k+1)]。

With position corresponding to the first frame as initial point, with this relative pose [R_(k,k+1)|t_(k,k+1)] and the absolute position of former frame camera Appearance [R_(0,k)|t_(0,k)] be multiplied, i.e. can get the absolute pose [R of Current camera_(0,k+1)|t_(0,k+1)]。

The present invention in motor process, can obtain a series of relative poses corresponding to every frame picture at camera successively, And then obtain absolute pose.

As the another kind of improvement of the present invention, the employing optical flow method in step S102 carries out feature point tracking, obtains camera Current pose include:

Following the tracks of the block taking a certain size around successful characteristic point respectively, using image correlation algorithm SSD, remove not Meet the characteristic point of threshold value.

In the present invention, during using optical flow method to be tracked, if I_kTwo field picture calculates characteristic point and description After son, I_k+1Frame figure uses optical flow method tracking characteristics point, for ensureing the correctness of optical flow tracking characteristic point, is following the tracks of successfully Characteristic point takes the block of 8*8 size around it respectively, uses image correlation algorithm SSD, removes the characteristic point being unsatisfactory for threshold value, with Improve the accuracy of optical flow tracking.

As a further improvement on the present invention, the employing FAST feature detection operator in step S102 obtains characteristic point, and Using BRIEF algorithm to calculate description and image is carried out characteristic matching, the current pose obtaining camera includes:

Use FAST feature detection operator to obtain characteristic point image present frame, use BRIEF algorithm to calculate and describe son also With initial frame characteristic matching, directly calculate the initial frame transformation matrix to present frame；

Transformation matrix is multiplied with the absolute pose of the camera of initial frame, obtains the current pose of camera.

In the present invention, for ensureing accuracy and the flatness of above-mentioned tracking gained camera real-time pose, it is simple to motion tracking In the application in AR/VR field, the feature successfully tracked when optical flow method after n frame is counted less than threshold value, I_k+nTwo field picture weight New use FAST feature detection operator and BRIEF calculate describe son and with I_kFrame characteristic matching, directly calculates I_kFrame is to I_k+nTransformation matrix [the R of frame_(k,k+n)|t_(k,k+n)], be multiplied I_kThe camera absolute position pose [R of frame_(0,k)|t_(0,k)] ', and then Obtain accurate I_k+nFrame camera absolute pose [R_(0,k+n)|t_(0,k+n)]'。

As the further improvement of the present invention, schematic diagram is with reference to shown in Fig. 2, and step S103 includes:

Step 1: for each frame of image, uses optical flow method accumulation acquired results and characteristic point directly to mate eligible result and enter Row Kalman filtering, obtains the current pose of camera more accurately, and carries out continuous iterated transform；

Step 2: use the renewal equation of Kalman Filtering for Discrete, is calculated current covariance and estimates P_k ^-, it is concrete, The renewal equation of Kalman Filtering for Discrete is:

${\hat{x}}_k=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}$

P_k ^-=AP_k-1A^T+Q

Wherein,Being optical flow method calculated camera attitude, A is state-transition matrix, and B is to control gain, P_k-1It is The covariance of former frame is estimated, Q is noise covariance matrix；

Step 3: use observational equation, calculates Kalman gain K_k, concrete, observational equation is:

z_k=Hx_k+v_k

K_k=P_k ^-H^T(HP_k ^-H^T+R)^-1

Wherein, z_kIt is that H is observing matrix, v by FAST Feature Points Matching algorithm calculated camera attitude_kRepresent and see Surveying noise, R is the covariance matrix of observation noise；

Step 4: according to Kalman Filtering for Discrete device state renewal equation, the system that updates is arranged:

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$

$P_k=(I-K_{k}H)P_k^{-}.$

In the present invention, by the Kalman filtering to monocular camera attitude, while improving camera tracking stability, also Fusion for follow-up IMU and camera provides more accurately attitude measurement value [R smoothly_(0,k)|t_(0,k)]”。

In the present invention, vision pose and IMU pose carry out Kalman's fusion process and those skilled in the art can be used public The accomplished in many ways known, it is preferred that the present invention is referred to below embodiment and carries out:

Vision and IMU merge schematic flow sheet, as shown in Figure 3.Describe for convenience, define subscript w, i, v, c table respectively Show world coordinate system, IMU coordinate, visual coordinate system and camera coordinates system.Coordinate system defines, as shown in Figure 4；

1) assume that inertia measurement includes specific deviation b and white Gaussian noise n, then actual angular velocity omega and reality Acceleration a is as follows:

ω=ω_m-b_ω-n_ω, a=a_m-b_a-n_a

Wherein subscript m represents measured value, and dynamic deviation b can be represented as a stochastic process:

${\stackrel{\·}{b}}_{\mathrm{\ω}}=n_{b\mathrm{\ω}},{\stackrel{\·}{b}}_a=n_{ba}$

The quantity of state of wave filter includes IMU position in world coordinate systemAnd world coordinate system is relative to IMU coordinate The speed of systemWith attitude four elementMeanwhile, also gyroscope and deviation b of accelerometer_ω, b_aAnd the Ocular measure factor λ.And demarcate the rotation relationship between the IMU of gained and cameraTranslation relationIt is hereby achieved that one comprises 24 units Element state vector X:

$X=\left\{\begin{array}{cccccccc}{p_w^i}^T& {v_w^i}^T& {q_w^i}^T& {b_{\mathrm{\ω}}}^T& {b_a}^T& \mathrm{\λ}& p_i^c& q_i^c\end{array}\right\}$

2) in above-mentioned state is expressed and described, we use four elements to be described attitude.In this case, I Use four element errors to represent error and its covariance, so can increase numerical stability and express in minimum.So, The error condition that we define 22 elements is vectorial:

$\tilde{x}=\left\{\begin{array}{cccccccc}{{\mathrm{\Δp}}_w^i}^T& {{\mathrm{\Δv}}_w^i}^T& {{\mathrm{\δ\θ}}_w^i}^T& {{\mathrm{\Δb}}_{\mathrm{\ω}}}^T& {{\mathrm{\Δb}}_a}^T& \mathrm{\Δ}\mathrm{\λ}& {{\mathrm{\Δp}}_i^c}^T& {{\mathrm{\δ\θ}}_i^c}^T\end{array}\right\}$

In view of estimated valueWith its true value x, such asIn addition to four element errors, all states are become by we Amount uses the method, and wherein, four element errors are defined as:

${\mathrm{\δq}}_w^i=q_w^i\⊗{{\hat{q}}_w^i}^{-1}\≈{\left[\begin{array}{cc}\frac{1}{2}{{\mathrm{\δ\θ}}_w^i}^T& 1\end{array}\right]}^T,{\mathrm{\δq}}_i^c=q_i^c\⊗{{\hat{q}}_i^c}^{-1}\≈{\left[\begin{array}{cc}\frac{1}{2}{{\mathrm{\δ\θ}}_i^c}^T& 1\end{array}\right]}^T$

Thus, it is possible to obtain the lienarized equation of continuous error condition:

$\stackrel{\·}{\tilde{x}}=F_c\tilde{x}+G_{c}n$

Wherein,It it is noise vector.In current solution, we are outstanding to the speed of algorithm It is concerned about, to this end, within the time of integration of two adjacent states, it will be assumed that F_cAnd G_cIt it is steady state value.In order to it is carried out discretization Represent:

$F_d=\exp \left(F_c\mathrm{\Δ}t\right)=I_d+F_c\mathrm{\Δ}t+\frac{1}{2}{F}_c^2{\mathrm{\Δt}}^2+\mathrm{...}$

Meanwhile, the covariance matrix Q of discrete time can be obtained by integration_d:

$Q_d={\∫}_{\mathrm{\Δ}t}{F}_d\left(\mathrm{\τ}\right)G_c{Q}_c{G}_C^T{F}_d{\left(\mathrm{\τ}\right)}^{T}d\mathrm{\τ}$

By calculating gained F_dAnd Q_d, according to Kalman filtering, it is calculated state covariance matrix:

P_k+1|k=F_dP_k|kF_d ^T+Q_d

3) for the position measurement of cameraThe absolute pose that we obtain according to the visual tracking of one camera, and obtain The measurement position of its correspondence.Thus obtain following measurement model:

$z_p=p_v^c=C_{\left(q_v^w\right)}^T(p_w^i+C_{\left(q_w^i\right)}^T{p}_i^c)\mathrm{\λ}+n_p$

Wherein,It is the spin matrix of IMU attitude under world coordinate system,It is that visual coordinate system is relative to the world The spin matrix of coordinate system.

4) definition errors in position measurement model:

${\tilde{z}}_p=z_p-{\hat{z}}_p=C_{\left(q_v^w\right)}^T(p_w^i+C_{\left(q_w^i\right)}^T{p}_i^c)\mathrm{\λ}+n_p-C_{\left(q_v^w\right)}^T({\hat{p}}_w^i+C_{\left({\hat{q}}_w^i\right)}^T{\hat{p}}_i^c)\widehat{\mathrm{\λ}}$

Definition wheel measuring error model:

${\tilde{z}}_q=z_q-{\hat{z}}_q=H_q^{wi}{\mathrm{\δq}}_w^i=H_q^{ic}{\mathrm{\δq}}_i^c$

Wherein,WithIt is to be error state amount respectivelyWithWrong calculation matrix.Finally, calculation matrix can To be accumulated as:

$\left[\begin{array}{c}{\tilde{z}}_p\\ {\tilde{z}}_q\end{array}\right]=\left[\begin{array}{c}{H}_p\\ \begin{array}{cccc}{0}_{3\×6}& {\tilde{H}}_q^{wi}& 0_{3\×10}& {\tilde{H}}_q^{ic}\end{array}\end{array}\right]\tilde{x}$

5) when we get calculation matrix H, we can be updated according to the step of Kalman filter.

As it is shown in figure 5, be monocular vision and IMU fusion schematic diagram, by above-mentioned monocular vision based on Kalman filtering With IMU data fusion, obtain the attitude output that mobile terminal is stable, and then realize stable motion tracking, the technical side of the present invention Case overall block flow diagram, as shown in Figure 6.

The vision pose of above-described embodiment only present invention and IMU pose carry out a citing of Kalman's fusion, remove Beyond this embodiment, it is also possible to use and well known to a person skilled in the art other method, it is also possible to realize the technology effect of the present invention Really.

On the other hand, a kind of stable motion merged based on mobile terminal monocular and IMU of the present invention follows the tracks of device, such as Fig. 7 institute Show, including:

Acquisition module 11: be used for obtaining image；

Visual tracking module 12: whether be more than predetermined threshold value for judging that the tracking characteristics of each frame of image is counted out, as Fruit is, then use optical flow method to carry out feature point tracking, obtain the current pose of camera, if it is not, then use FAST special image Levy detective operators and obtain characteristic point, and use BRIEF algorithm calculating description that image is carried out characteristic matching, obtain working as of camera Front pose；

Filtration module 13: for the current pose of camera is carried out Kalman filtering, obtain vision pose；

IMU pose computing module 14: for obtaining acceleration and the magnitude of angular velocity that IMU produces at three dimensions, and to adding Speed and magnitude of angular velocity are integrated computing, obtain IMU pose and predict the outcome；

Fusion Module 15: carry out Kalman's fusion for predicting the outcome vision pose and IMU pose, obtains according to after merging To posture information carry out motion tracking.

With said method accordingly, compared with prior art, present invention is equally capable to obtain energy on the equipment of mobile terminal Add stable and quick motion tracking.

As a modification of the present invention, the employing optical flow method that visual tracking module 12 is carried out carries out feature point tracking, Current pose to camera includes:

To the Corresponding matching feature point set on two two field pictures adjacent before and after image, it is calculated the basis between two two field pictures Matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

According to essential matrix, SVD is used to recover to obtain the relative pose of adjacent interframe；

Relative pose is multiplied with the absolute pose of the camera of the former frame obtained, obtains the current pose of camera.

The present invention in motor process, can obtain a series of relative poses corresponding to every frame picture at camera successively, And then obtain absolute pose.

As the another kind of improvement of the present invention, the employing optical flow method that visual tracking module 12 is carried out carries out feature point tracking, The current pose obtaining camera includes:

Following the tracks of the block taking a certain size around successful characteristic point respectively, using image correlation algorithm SSD, remove not Meet the characteristic point of threshold value.

In the present invention, remove the characteristic point being unsatisfactory for threshold value, it is possible to increase the accuracy of optical flow tracking.

As a further improvement on the present invention, what visual tracking module 12 was carried out uses FAST feature detection operator to image Obtain characteristic point, and use BRIEF algorithm calculating description that image is carried out characteristic matching, obtain the current pose bag of camera Include:

Use FAST feature detection operator to obtain characteristic point image present frame, use BRIEF algorithm to calculate and describe son also With initial frame characteristic matching, directly calculate the initial frame transformation matrix to present frame；

Transformation matrix is multiplied with the absolute pose of the camera of initial frame, obtains the current pose of camera.

In the present invention, it is possible to ensure accuracy and the flatness of above-mentioned tracking gained camera real-time pose, it is simple to move with Track is in the application in AR/VR field.

As the further improvement of the present invention, filtration module 13, it is additionally operable to:

Kalman filtering module: use optical flow method accumulation acquired results and direct of characteristic point for each frame for image Join eligible result and carry out Kalman filtering, obtain the current pose of camera more accurately, and carry out continuous iterated transform；

First computing module: for using the renewal equation of Kalman Filtering for Discrete, be calculated current covariance and estimate Meter P_k ^-, concrete, the renewal equation of Kalman Filtering for Discrete is:

${\hat{x}}_k=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}$

P_k ^-=AP_k-1A^T+Q

Wherein,Being optical flow method calculated camera attitude, A is state-transition matrix, and B is to control gain, P_k-1It is The covariance of former frame is estimated, Q is noise covariance matrix；

Second computing module: be used for using observational equation, calculates Kalman gain K_k, concrete, observational equation is:

z_k=Hx_k+v_k

K_k=P_k ^-H^T(HP_k ^-H^T+R)^-1

Wherein, z_kIt is that H is observing matrix, v by FAST Feature Points Matching algorithm calculated camera attitude_kRepresent and see Surveying noise, R is the covariance matrix of observation noise；

Update system module: for according to Kalman Filtering for Discrete device state renewal equation, the system that updates is arranged:

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$

$P_k=(I-K_{k}H)P_k^{-}.$

In the present invention, by the Kalman filtering to monocular camera attitude, while improving camera tracking stability, also Fusion for follow-up IMU and camera provides more accurately attitude measurement value [R smoothly_(0,k)|t_(0,k)]”。

The above is the preferred embodiment of the present invention, it is noted that for those skilled in the art For, on the premise of without departing from principle of the present invention, it is also possible to make some improvements and modifications, these improvements and modifications are also Should be regarded as protection scope of the present invention.

Claims (10)

1. the stable motion tracking merged based on mobile terminal monocular and IMU, it is characterised in that including:

Obtain image；

Judge that whether the tracking characteristics of the present frame of image counts out more than predetermined threshold value, if it is, use optical flow method to carry out Feature point tracking, obtains the current pose of camera, if it is not, then use FAST feature detection operator to obtain characteristic point, and uses BRIEF algorithm calculates description and image is carried out characteristic matching, obtains the current pose of camera；

The current pose of camera is carried out Kalman filtering, obtains vision pose；

Obtain acceleration and magnitude of angular velocity that IMU produces at three dimensions, and acceleration and magnitude of angular velocity be integrated computing, Obtain IMU pose to predict the outcome；

Vision pose and IMU pose are predicted the outcome and carries out Kalman's fusion, transport according to the posture information obtained after merging Motion tracking.

The stable motion tracking merged based on mobile terminal monocular and IMU the most according to claim 1, its feature exists In, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

To the Corresponding matching feature point set on two two field pictures adjacent before and after image, the basic square being calculated between two two field pictures Battle array；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

According to essential matrix, SVD is used to recover to obtain the relative pose of adjacent interframe；

Relative pose is multiplied with the absolute pose of the camera of the former frame obtained, obtains the current pose of camera.

The stable motion tracking merged based on mobile terminal monocular and IMU the most according to claim 1, its feature exists In, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

Following the tracks of the block taking a certain size around successful characteristic point respectively, using image correlation algorithm SSD, removal is unsatisfactory for The characteristic point of threshold value.

4. according to the described stable motion tracking merged based on mobile terminal monocular and IMU arbitrary in claim 1-3, its Being characterised by, described employing FAST feature detection operator obtains characteristic point, and uses BRIEF algorithm calculating description to enter image Row characteristic matching, the current pose obtaining camera includes:

Use FAST feature detection operator to obtain characteristic point image present frame, use BRIEF algorithm to calculate and describe son and with just Beginning frame characteristic matching, directly calculates the initial frame transformation matrix to present frame；

Transformation matrix is multiplied with the absolute pose of the camera of initial frame, obtains the current pose of camera.

The stable motion tracking merged based on mobile terminal monocular and IMU the most according to claim 4, its feature exists In, the described current pose to camera carries out Kalman filtering, obtains vision pose and includes:

Step 1: for each frame of image, uses optical flow method accumulation acquired results and characteristic point directly to mate eligible result and block Kalman Filtering, obtains the current pose of camera more accurately, and carries out continuous iterated transform；

Step 2: use the renewal equation of Kalman Filtering for Discrete, is calculated current covariance and estimates P_k ^-, concrete, discrete The renewal equation of Kalman filtering is:

${\hat{x}}_k=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}$

P_k ^-=AP_k-1A^T+Q

Wherein,Being optical flow method calculated camera attitude, A is state-transition matrix, and B is to control gain, P_k-1It is previous The covariance of frame is estimated, Q is noise covariance matrix；

Step 3: use observational equation, calculates Kalman gain K_k, concrete, observational equation is:

z_k=Hx_k+v_k

K_k=P_k ^-H^T(HP_k ^-H^T+R)^-1

Wherein, z_kIt is that H is observing matrix, v by FAST Feature Points Matching algorithm calculated camera attitude_kRepresent that observation is made an uproar Sound, R is the covariance matrix of observation noise；

Step 4: according to Kalman Filtering for Discrete device state renewal equation, the system that updates is arranged:

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$

$P_k=(I-K_{k}H)P_k^{-}.$

6. device is followed the tracks of in the stable motion merged based on mobile terminal monocular and IMU, it is characterised in that including:

Acquisition module: be used for obtaining image；

Visual tracking module: for judging that whether the tracking characteristics of the present frame of image counts out more than predetermined threshold value, if it is, Then use optical flow method to carry out feature point tracking, obtain the current pose of camera, if it is not, then use FAST feature detection operator to obtain Take characteristic point, and use BRIEF algorithm calculating description that image is carried out characteristic matching, obtain the current pose of camera；

Filtration module: for the current pose of camera is carried out Kalman filtering, obtain vision pose；

IMU pose computing module: for obtaining acceleration and the magnitude of angular velocity that IMU produces at three dimensions, and to acceleration with Magnitude of angular velocity is integrated computing, obtains IMU pose and predicts the outcome；

Fusion Module: carry out Kalman's fusion for predicting the outcome vision pose and IMU pose, according to the position obtained after merging Appearance information carries out motion tracking.

Device is followed the tracks of in the stable motion merged based on mobile terminal monocular and IMU the most according to claim 6, and its feature exists In, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

To the Corresponding matching feature point set on two two field pictures adjacent before and after image, the basic square being calculated between two two field pictures Battle array；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

According to essential matrix, SVD is used to recover to obtain the relative pose of adjacent interframe；

Relative pose is multiplied with the absolute pose of the camera of the former frame obtained, obtains the current pose of camera.

Device is followed the tracks of in the stable motion merged based on mobile terminal monocular and IMU the most according to claim 6, and its feature exists In, described employing optical flow method carries out feature point tracking, and the current pose obtaining camera includes:

Following the tracks of the block taking a certain size around successful characteristic point respectively, using image correlation algorithm SSD, removal is unsatisfactory for The characteristic point of threshold value.

9. according to the described stable motion tracking device merged based on mobile terminal monocular and IMU arbitrary in claim 6-8, its Being characterised by, described employing FAST feature detection operator obtains characteristic point, and uses BRIEF algorithm calculating description to enter image Row characteristic matching, the current pose obtaining camera includes:

Use FAST feature detection operator to obtain characteristic point image present frame, use BRIEF algorithm to calculate and describe son and with just Beginning frame characteristic matching, directly calculates the initial frame transformation matrix to present frame；

Transformation matrix is multiplied with the absolute pose of the camera of initial frame, obtains the current pose of camera.

Device is followed the tracks of in the stable motion merged based on mobile terminal monocular and IMU the most according to claim 9, and its feature exists In, described filtration module, it is additionally operable to:

Kalman filtering module: for using optical flow method accumulation acquired results and characteristic point directly to mate institute for each frame of image Obtain result and carry out Kalman filtering, obtain the current pose of camera more accurately, and carry out continuous iterated transform；

First computing module: for using the renewal equation of Kalman Filtering for Discrete, is calculated current covariance and estimates P_k ^-, Concrete, the renewal equation of Kalman Filtering for Discrete is:

${\hat{x}}_k=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}$

P_k ^-=AP_k-1A^T+Q

Wherein,Being optical flow method calculated camera attitude, A is state-transition matrix, and B is to control gain, P_k-1It is previous The covariance of frame is estimated, Q is noise covariance matrix；

Second computing module: be used for using observational equation, calculates Kalman gain K_k, concrete, observational equation is:

z_k=Hx_k+v_k

K_k=P_k ^-H^T(HP_k ^-H^T+R)^-1

Wherein, z_kIt is that H is observing matrix, v by FAST Feature Points Matching algorithm calculated camera attitude_kRepresent that observation is made an uproar Sound, R is the covariance matrix of observation noise；

Update system module: for according to Kalman Filtering for Discrete device state renewal equation, the system that updates is arranged:

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$

$P_k=(I-K_{k}H)P_k^{-}.$