A visual continuous positioning method based on short-term inertial measurement

Abstract

The present invention provides a visual continuous positioning method based on short-term inertial measurement, comprising the following steps: after the visual sensor is initialized at time t0, the carrier is positioned by visual tracking to obtain visual posture information; when the visual tracking is lost at time t1, the visual repositioning method integrating inertial measurement is used for repositioning, and if the repositioning is successful, the visual tracking positioning is continued; if the repositioning is unsuccessful, the visual sensor is initialized again, and the initialization is completed at time t2; the inertial sensor performs inertial measurement within the time interval between t1 and t2 to obtain the inertial posture change information of the carrier; using the inertial posture change information, the world coordinate system initialized at time t2 is aligned to the world coordinate system initialized at time t0, and a visually coherent and robust positioning result is obtained. The present invention proposes a weighted matching visual repositioning model to obtain more accurate visual posture information and improve the success rate and accuracy of visual repositioning.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle visual navigation, and in particular relates to a visual continuous positioning method based on short-term inertial measurement. Background Art

[0002] With the rapid development of visual sensor technology, computer technology and artificial intelligence technology, there have been a series of technological breakthroughs in the research directions of target recognition, visual tracking, visual mapping and visual positioning in the field of computer vision, and visual navigation has begun to occupy a place in the field of autonomous navigation. Visual sensors have the characteristics of concealment, lightness, low power consumption and low price, so using visual sensors for navigation has great advantages.

[0003] Visual navigation can often achieve good tracking results in texture-rich areas and achieve high positioning accuracy. However, vision is very easy to lose tracking in scenes with drastic changes in lighting, sparse features, and fast movement. The visual sensor will be reinitialized each time the tracking is lost. Since visual positioning is a local coordinate system (not a geographic system), each initialization of the visual coordinate system is random, and users will get segmented positioning results during use, which is difficult to meet the user's needs for consistent and effective positioning in actual use. Summary of the invention

[0004] The purpose of the present invention is to overcome the deficiencies in the prior art and provide a visual continuous positioning method based on short-term inertial measurement. The solution of the present invention can solve the problems existing in the above-mentioned prior art.

[0005] The technical solution of the present invention:

[0006] According to a first aspect, a visual continuous positioning method based on short-term inertial measurement is provided, comprising the following steps:

[0007] Step 1: After the visual sensor is initialized at time t0, the carrier is positioned through visual tracking to obtain visual posture information;

[0008] Step 2: If visual tracking is lost at time t1, repositioning is performed using the visual repositioning method that integrates inertial measurement. If repositioning is successful, proceed to step 1 for visual tracking and positioning. If repositioning is unsuccessful, reinitialize the visual sensor. Initialization is completed at time t2, and proceed to step 3.

[0009] Step 3: the inertial sensor performs inertial measurement in the time interval between t1 and t2 to obtain the inertial attitude change information of the carrier;

[0010] Step 4: Using the inertial posture change information, align the world coordinate system initialized at time t2 to the world coordinate system initialized at time t0 to obtain a visually coherent and robust positioning result.

[0011] Furthermore, the visual relocalization method integrating inertial measurement comprises the following steps:

[0012] Use inertial measurement to estimate the carrier motion, obtain the inertial pose of the next cycle, and send it to the visual sensor;

[0013] The visual sensor is transferred to the visual camera coordinate system according to the obtained inertial posture, and the visual feature matching area is delineated to achieve the initial positioning of the visual matching;

[0014] In the visual feature matching area, the current frame is matched with the previous video frame, the previous reference frame, and the previous key frame to obtain three different sets of feature matching pairs;

[0015] The pose change matrix T from the current frame to the previous video frame can be obtained by using the eight-point method of epipolar geometry constraints: c21 , including rotation and translation, the pose change matrix T from the current frame to the previous reference frame c31 , the pose change matrix T from the current frame to the previous key frame c41 , the first round obtains the visual pose information of the current frame through weighted fusion:

[0016]

[0017] The pose matrix Represents the conversion of the world coordinate system corresponding to the i-th frame (current frame) to the camera coordinate system. Respectively represent the conversion of the world coordinate system corresponding to the previous video frame, the previous reference frame and the previous key frame to the camera coordinate system; w1, w2 and w3 are the visual pose weights corresponding to the previous video frame, the previous reference frame and the previous key frame respectively. Considering their different influences, the following relationship should be dynamically satisfied:

[0018]

[0019] When n1<10, n1 takes the value of 0; when n2<10, n2 takes the value of 0;

[0020] When n3<10, n3 takes the value of 0;

[0021] Where n1, n2, and n3 represent the number of matching point pairs corresponding to the previous video frame, the previous reference frame, and the previous key frame, respectively. The more matching pairs there are, the higher the confidence level of the camera pose solution. When the number of matching pairs is less than 10, the camera pose result is unreliable and the weight is 0. This results in the estimated camera pose value for the first round of relocalization.

[0022] The second round of optimization model uses the 3D-2D feature matching between local map points and the current frame to construct the visual reprojection error and minimize the reprojection error r cam It can be expressed as the following formula:

[0023]

[0024] Where m represents the map point ID, X m is the coordinate of the 3D map point in the world coordinate system, p am Indicates X on the ath frame m The corresponding pixel observation point, the depth is expressed as π;

[0025] Use the camera pose estimate obtained in the first round As the initial value, it is brought into the above iterative solution process, and finally the accurate camera pose estimation value of visual relocalization is obtained

[0026] Furthermore, the inertial sensor performs inertial measurement in the time interval between t1 and t2, and the method for obtaining the inertial attitude change information of the carrier is:

[0027] Calculate the inertial pose change from time i to time j, including the rotation R wbj and pan p j :

[0028]

[0029] in, and They represent the deviations of the gyroscope and accelerometer at time k, respectively, obtained by rough calibration, and Represent the measured values ​​of the gyroscope and accelerometer at time k, R wbi represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time i, R wbj represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time j, R wbk represents the rotation from the inertial load system coordinate system to the world coordinate system w1 at time k, g represents the gravitational acceleration in the world coordinate system w1, and v i 、v k and v j They represent the speed of the world coordinate system w1 at time i, time k and time j respectively, and p i and p j They represent the positions in the world coordinate system w1 at time i and time j respectively, Δt is the integration time interval, the value of i is t1, and the value of j is t2.

[0030] Furthermore, the steps of aligning the world coordinate system initially constructed at time t2 to the world coordinate system initially constructed at time t0 are:

[0031] During the visual tracking process, the attitude transformation matrix from the camera system to the world coordinate system w1 at each moment is solved, and the attitude transformation matrix from the camera coordinate system c2 to the world coordinate system w1 at time t1 is recorded, which is expressed as T w1c2 ;

[0032] Using the external parameter matrix, the camera system is converted to the inertial system, as shown in the following formula:

[0033] T w1b1 =T w1c2 T cb

[0034] Where T cb is the external parameter matrix, which represents the change of posture from the carrier system to the camera system, T w1b1 is the attitude transformation matrix from the inertial carrier system b1 to the world coordinate system w1 at time t1, including rotation and translation;

[0035] The carrier motion from time t1 to time t2 is obtained through inertial navigation, and the attitude transformation matrix from the inertial carrier system b2 to the world coordinate system w1 at time t2 is obtained, which is expressed as T w1b2 ;

[0036] Using the external parameter matrix, the inertial load system is transformed into the camera system, as shown in the following formula:

[0037] T w1c3 =T w1b2 T cb -1

[0038] Where T w1c3 It is the attitude transformation matrix from the camera coordinate system c3 to the world coordinate system w1 at time t2, including rotation and translation;

[0039] The final coordinate system transformation is completed by using the conversion relationship between the camera system and the world coordinate system at time t2, as shown in the following formula:

[0040] T w1w2 =T w1c3 T w2c3 -1 =T w1c3 T c3w2

[0041] Where T w1w2 is the attitude transformation matrix from the world coordinate system w2 at time t2 to the world coordinate system w1 at time t0, including rotation and translation; T w2c3Represents the pose transformation matrix from the camera coordinate system c3 to the world coordinate system w2, including a 3*3 rotation matrix and a 3*1 translation vector. Since the coordinate systems coincide, the rotation matrix here is the unit matrix and the translation vector is 0.

[0042] At this point, the transformation of the two world coordinate systems is obtained. After the visual tracking is lost, the reconstructed pose trajectory can be transformed from the world coordinate system w2 to the world coordinate system w1, thereby obtaining a continuous and effective visual positioning result.

[0043] According to a second aspect, a medium is provided, wherein the medium stores a visual continuous positioning method based on short-term inertial measurement according to the present invention.

[0044] According to a third aspect, a navigation control system is provided, wherein the navigation control system uses a visual continuous positioning method based on short-term inertial measurement described in the present invention to perform navigation positioning control.

[0045] The beneficial effects of the present invention compared with the prior art are as follows:

[0046] (1) The present invention proposes a visual relocalization method integrating inertial measurement, which uses inertial measurement to assist visual matching positioning. A weighted matching visual relocalization model is simultaneously proposed to obtain more accurate visual pose information, further improving the success rate and accuracy of visual relocalization.

[0047] (2) The present invention aims to solve the problems of reinitialization and incoherent visual positioning after visual tracking is lost. It studies a visual initialization coordinate transformation method based on short-term inertial measurement under visual loss state, and uses inertial sensor positioning to construct the transformation relationship between two sets of visual local coordinate systems to achieve coherent and robust navigation positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] The included drawings are used to provide a further understanding of the embodiments of the present invention, which constitute a part of the specification, are used to illustrate the embodiments of the present invention, and together with the text description, explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0049] Figure 1 A schematic diagram of the steps of a visual continuous positioning method based on short-term inertial measurement provided according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0050] It should be noted that, in the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0051] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0052] Unless otherwise specifically stated, the relative arrangement, numerical expressions and numerical values ​​of the parts and steps set forth in these embodiments do not limit the scope of the present invention. Meanwhile, it should be understood that, for ease of description, the sizes of the various parts shown in the accompanying drawings are not drawn according to the actual proportional relationship. The technology, method and equipment known to those of ordinary skill in the relevant art may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be considered as a part of the specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, and therefore, once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0053] like Figure 1 As shown, according to a first aspect of the present invention, an embodiment provides a visual continuous positioning method based on short-term inertial measurement, comprising the following steps:

[0054] Step 1: After the visual sensor is initialized at time t0, the carrier is positioned through visual tracking to obtain visual posture information;

[0055] Step 2: If visual tracking is lost at time t1, repositioning is performed using the visual repositioning method that integrates inertial measurement. If repositioning is successful, proceed to step 1 for visual tracking and positioning. If repositioning is unsuccessful, reinitialize the visual sensor. Initialization is completed at time t2, and proceed to step 3.

[0056] Step 3: the inertial sensor performs inertial measurement in the time interval between t1 and t2 to obtain the inertial attitude change information of the carrier;

[0057] Step 4: Using the inertial posture change information, align the world coordinate system initialized at time t2 to the world coordinate system initialized at time t0 to obtain a visually coherent and robust positioning result.

[0058] Further in one embodiment, the visual relocalization method integrating inertial measurement comprises the following steps:

[0059] Use inertial measurement to estimate the carrier motion, obtain the inertial pose of the next cycle, and send it to the visual sensor;

[0060] The visual sensor transfers the obtained inertial posture to the visual camera coordinate system, delineates the visual feature matching area, and realizes the initial positioning of visual matching;

[0061] In the visual feature matching area, the current frame is matched with the previous video frame, the previous reference frame, and the previous key frame to obtain three different sets of feature matching pairs;

[0062] The pose change matrix from the current frame to the previous video frame can be obtained by using the eight-point method of epipolar geometry constraints. Including rotation and translation, the pose change matrix from the current frame to the previous reference frame The pose change matrix from the current frame to the previous key frame The first round obtains the visual pose information of the current frame through weighted fusion:

[0063]

[0064] The pose matrix Represents the conversion of the world coordinate system corresponding to the i-th frame (current frame) to the camera coordinate system. Respectively represent the conversion of the world coordinate system corresponding to the previous video frame, the previous reference frame and the previous key frame to the camera coordinate system; w1, w2 and w3 are the visual pose weights corresponding to the previous video frame, the previous reference frame and the previous key frame respectively. Considering their different influences, the following relationship should be dynamically satisfied:

[0065]

[0066] When n1<10, n1 takes the value of 0; when n2<10, n2 takes the value of 0;

[0067] When n3<10, n3 takes the value of 0;

[0068] Where n1, n2, and n3 represent the number of matching point pairs corresponding to the previous video frame, the previous reference frame, and the previous key frame, respectively. The more matching pairs there are, the higher the confidence level of the camera pose solution. When the number of matching pairs is less than 10, the camera pose result is unreliable and the weight is 0. This results in the estimated camera pose value for the first round of relocalization.

[0069] The second round of optimization model uses the 3D-2D feature matching between local map points and the current frame to construct the visual reprojection error and minimize the reprojection error. It can be expressed as the following formula:

[0070]

[0071] Where m represents the map point ID, X m is the coordinate of the 3D map point in the world coordinate system, p am Indicates X on the ath frame m The corresponding pixel observation point, the depth is expressed as π;

[0072] Use the camera pose estimate obtained in the first round As the initial value, it is brought into the above iterative solution process, and finally the accurate camera pose estimation value of visual relocalization is obtained

[0073] Further in one embodiment, the inertial sensor performs inertial measurement in the time interval between t1 and t2, and the method for obtaining the inertial attitude change information of the carrier is:

[0074] Calculate the inertial pose change from time i to time j, including the rotation R wbj and pan p j :

[0075]

[0076] in, and They represent the deviations of the gyroscope and accelerometer at time k, respectively, obtained by rough calibration, and Represent the measured values ​​of the gyroscope and accelerometer at time k, R wbi represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time i, R wbj represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time j, R wbk represents the rotation from the inertial load system coordinate system to the world coordinate system w1 at time k, g represents the gravitational acceleration in the world coordinate system w1, and v i 、v k and v jThey represent the speed of the world coordinate system w1 at time i, time k and time j respectively, and p i and p j They represent the positions in the world coordinate system w1 at time i and time j respectively, Δt is the integration time interval, the value of i is t1, and the value of j is t2.

[0077] Further, in one embodiment, the steps of aligning the world coordinate system initially constructed at time t2 to the world coordinate system initially constructed at time t0 are:

[0078] During the visual tracking process, the attitude transformation matrix from the camera system to the world coordinate system w1 at each moment is solved, and the attitude transformation matrix from the camera coordinate system c2 to the world coordinate system w1 at time t1 is recorded, which is expressed as T w1c2 ;

[0079] Using the external parameter matrix, the camera system is converted to the inertial system, as shown in the following formula:

[0080] T w1b1 =T w1c2 T cb

[0081] Where T cb is the external parameter matrix, which represents the change of posture from the carrier system to the camera system, T w1b1 is the attitude transformation matrix from the inertial carrier system b1 to the world coordinate system w1 at time t1, including rotation and translation;

[0082] The carrier motion from time t1 to time t2 is obtained through inertial navigation, and the attitude transformation matrix from the inertial carrier system b2 to the world coordinate system w1 at time t2 is obtained, which is expressed as T w1b2 ;

[0083] Using the external parameter matrix, the inertial load system is transformed into the camera system, as shown in the following formula:

[0084] T w1c3 =T w1b2 T cb -1

[0085] Where T w1c3 It is the attitude transformation matrix from the camera coordinate system c3 to the world coordinate system w1 at time t2, including rotation and translation;

[0086] The final coordinate system transformation is completed by using the conversion relationship between the camera system and the world coordinate system at time t2, as shown in the following formula:

[0087] T w1w2 =T w1c3 T w2c3 -1 =T w1c3 Tc3w2

[0088] Where T w1w2 is the attitude transformation matrix from the world coordinate system w2 at time t2 to the world coordinate system w1 at time t0, including rotation and translation; T w2c3 Represents the pose transformation matrix from the camera coordinate system c3 to the world coordinate system w2, including a 3*3 rotation matrix and a 3*1 translation vector. Since the coordinate systems coincide, the rotation matrix here is the unit matrix and the translation vector is 0.

[0089] At this point, the transformation of the two world coordinate systems is obtained. After the visual tracking is lost, the reconstructed pose trajectory can be transformed from the world coordinate system w2 to the world coordinate system w1, thereby obtaining a continuous and effective visual positioning result.

[0090] Furthermore, in one embodiment, the external parameter matrix T cb The extrinsic parameters include translation and rotation, and the calculation method includes the following steps:

[0091] Obtain visual frame pose information and inertial pose information;

[0092] The rotation constraint error equation in the same space and time is established based on the visual frame pose information and the inertial pose information;

[0093] The rotation constraint solving method based on dual optimization is adopted to obtain the rotation external parameters and gyro bias of the two sensors.

[0094] The obtained gyro bias is used to calibrate the inertial sensor and complete the online dynamic calibration of the inertial sensor.

[0095] According to the visual frame pose information, inertial pose information and rotation extrinsic parameters, the translation extrinsic parameters of the two sensors are obtained based on the decoupled translation extrinsic parameter estimation method.

[0096] Furthermore, the rotation constraint error equation is:

[0097]

[0098] Among them, e q (Z q ,x q ) represents the rotation constraint error, χ q is the rotation constraint parameter, is the gyro constant bias in the sliding window, J i is the gyro bias error of the pre-integrated rotation error at time i The first-order Jacobian matrix of The initial value of is set to zero, The inertial rotation amount calculated by the inertial sensor between time i and time j, is the visual rotation amount solved by the visual sensor between time i and time j, is the rotational external parameter between the visual sensor and the inertial sensor, for The inverse matrix of .

[0099] Furthermore, the dual optimization rotation constraint solving method comprises the following steps:

[0100] According to the visual frame pose information of the visual sensor obtained by the epipolar geometry and the inertial pose information of the inertial sensor, the rough rotational extrinsic parameters and gyro zero bias are obtained by using the rotational constraint error equation;

[0101] The obtained rough gyro zero bias is used to roughly correct the inertial posture information to obtain a roughly corrected inertial posture;

[0102] The roughly corrected inertial pose and the visual frame pose information obtained after BA optimization are used to obtain the precise rotational extrinsic parameters and gyro bias using the rotational constraint error equation.

[0103] Furthermore, the decoupling-based translation extrinsic parameter estimation method comprises the following steps:

[0104] The position constraint equation is obtained through the known relative speed information in the visual frame pose information and inertial pose information between adjacent key frames:

[0105] in represents the position error of the visual sensor at time t+1 and time t, and are the estimated values ​​of the visual sensor relative to the c0 system at time t+1 and time t, respectively. are the estimated values ​​of the inertial sensor relative to the c0 system at time t and t+1 respectively, for The inverse matrix of To solve the speed information corresponding to the t-th sliding window, is the gravity vector in the reference vision sensor coordinate system, is the position error term, s is the scale information, δt is the time interval, is the translation external parameter.

[0106] In the data of three consecutive frames, the relationship between adjacent frames is obtained, and the x p The parameters are estimated:

[0107]

[0108] Among them, χ p Estimated parameters for translation without absolute velocity;

[0109] The cost function is calculated by taking the positions of all paired visual sensors and inertial sensors at the initialization stage;

[0110] Obtain χ by solving the cost function p result;

[0111] The residual equation is established through the inertial sensor position information and velocity information between adjacent key frames in the sliding window:

[0112]

[0113] in, and represents the position and velocity error terms;

[0114] Will get χ p The result is used as a priori information and brought into the residual equation of the previous step to optimize and solve the velocity information, scale factor and translational parameters in the sliding window;

[0115] According to the obtained velocity information, the translation parameters including absolute velocity are estimated Solve; where s is the scale factor between the displacement measured by the inertial sensor and the displacement restored by the visual sensor, where, is the speed information corresponding to the nth sliding window, is the gravity vector in the reference vision sensor coordinate system.

[0116] According to a second aspect of the embodiment, a medium is provided, wherein the medium stores a visual continuous positioning method based on short-term inertial measurement described in the present invention.

[0117] According to a third aspect of the embodiment, a navigation control system is provided, wherein the navigation control system uses a visual continuous positioning method based on short-term inertial measurement described in the present invention to perform navigation positioning control.

[0118] In order to have a further understanding of the visual continuous positioning method based on short-term inertial measurement provided by the present invention, it is described in detail below with reference to specific examples and drawings.

[0119] Taking a certain model of inertial / visual navigation as an example, the inertial sensor used in the inertial navigation is MEMS micro inertial navigation, and the visual sensor used in the visual navigation is a visible light camera.

[0120] A visual continuous positioning method based on short-term inertial measurement comprises the following steps:

[0121] Step 1: After visual initialization at time t0, visual tracking and positioning are performed through feature extraction and feature matching until visual tracking is lost at time t1.

[0122] The classic ORB feature extraction method is used to extract the improved Fast corner points as feature points. The visual sensor extracts feature points from the input image. When the number of feature points extracted from a single frame is greater than 100 and the number of feature tracking in the previous and next frames is greater than 20, the vision starts to initialize.

[0123] For visual navigation, there are three main coordinate systems involved: image coordinate system, camera coordinate system and world coordinate system. This article defines the camera coordinate system as c system and the world coordinate system as w system. In the initialization stage, the vision uses the camera coordinate system at the current time t0 as the world coordinate system, which is counted as w1 system, and then starts the visual tracking thread.

[0124] Visual tracking is implemented using the general feature extraction and feature matching of ORB-SLAM2 until the number of matches between the previous and next visual frames falls below a threshold, and visual tracking fails. The timestamp t1 of the failed tracking image is recorded.

[0125] Step 2: After tracking is lost, a visual relocalization method integrating inertial measurement is proposed for relocalization. If the relocalization is successful, continue with step 1 for visual tracking and positioning; if the relocalization fails, the vision is reinitialized and the initialization is completed at visual time t2.

[0126] After visual determination of tracking loss, the relocalization phase begins. In the relocalization phase, the current frame is generally re-matched with the reference frame to see if image tracking and positioning can be achieved. This paper proposes a visual relocalization method that integrates inertial measurement. On the one hand, it comprehensively utilizes inertial / visual information for matching, uses inertial measurement to achieve motion estimation, and further improves the success rate of visual feature matching with a good initialization pose. On the other hand, a weighted matching visual relocalization model is proposed to match the current frame with the previous video frame, the previous reference frame, and the previous key frame, and synchronously matches the current frame with the local map, thereby improving the pose accuracy of the current frame visual relocalization and facilitating visually robust tracking.

[0127] When matching the previous and next visual frames, it is necessary to estimate the motion state information of the current frame and use the initial motion value to define the visual feature matching area, thereby achieving visual matching positioning. The more accurate the initial value, the higher the success rate of visual tracking. Existing algorithms usually use a uniform motion model for estimation, which assumes that the carrier motion remains unchanged in a short period of time and moves forward at a uniform speed.

[0128] This uniform motion model has a large change in velocity model when turning or when the carrier is accelerating, which often easily causes visual tracking to be lost. Therefore, this paper uses inertial measurement information to provide a better estimate for visual matching. The short time period of the visual front and back frames can make full use of the high short-term accuracy of the inertial sensor. The inertial pre-integration method proposed by Forster et al. is used to solve the motion state T of the inertial carrier system in the current frame. bi , transferred to the visual camera coordinates through external parameter calibration, expressed as T ci .

[0129] T ci =T cb T bi

[0130] Where T cb is the external parameter matrix, which represents the change in posture from the carrier system to the camera system. bi is the position of the inertial load system at the i-th moment, including rotation and translation. ci is the position of the camera system at the i-th moment, which also includes rotation and translation.

[0131] After obtaining the initial motion value of the current frame, this paper proposes a weighted matching visual relocalization model. The error model includes two optimization solution stages:

[0132] In the first stage, the candidate matching area is delineated using the initial motion value of the current frame, and the current frame is matched with the previous video frame, the previous reference frame, and the previous key frame to obtain three different sets of feature matching pairs.

[0133] The pose change matrix from the current frame to the previous video frame can be obtained by using the eight-point method of epipolar geometry constraints.

[0134] Including rotation and translation, the pose change matrix from the current frame to the previous reference frame The pose change matrix from the current frame to the previous key frame The first round obtains the visual pose information of the current frame through weighted fusion:

[0135]

[0136] The pose matrix Represents the conversion of the world coordinate system corresponding to the i-th frame (current frame) to the camera coordinate system. Respectively represent the conversion of the world coordinate system corresponding to the previous video frame, the previous reference frame and the previous key frame to the camera coordinate system.

[0137] w1, w1, and w1 are the visual pose weights corresponding to the previous video frame, the previous reference frame, and the previous key frame, respectively. Considering their different influences, the following relationship should be dynamically satisfied:

[0138]

[0139] n1<10,n1=0; n2<10,n2=0; n3<10,n3=0

[0140] Where n1, n2, and n3 represent the number of matching point pairs corresponding to the previous video frame, the previous reference frame, and the previous key frame, respectively. The more matching pairs there are, the higher the confidence of the camera pose solution. When the number of matching pairs is less than 10, the camera pose result is unreliable and the weight is 0. The camera pose estimate of the first round of relocalization is obtained.

[0141] The second round of optimization model uses the 3D-2D feature matching between local map points and the current frame to construct the visual reprojection error and minimize the reprojection error. It can be expressed as the following formula:

[0142]

[0143] Where m represents the map point ID, X m is the coordinate of the 3D map point in the world coordinate system, p am Indicates X on the ath frame m The corresponding pixel observation point, the depth is represented by π.

[0144] In the iterative optimization process, the camera pose estimate obtained in the first round is used As the initial value, it is brought into the above iterative solution process, and finally the accurate camera pose estimation value of visual relocalization is obtained

[0145] The entire relocalization time is set to 3s. If all image frames fail to be tracked within 3s, the vision is reinitialized and the timestamp t2 corresponding to the successfully initialized frame is recorded.

[0146] Step 3: The inertial sensor performs inertial measurement during the time interval between t1 and t2 to calculate the translation and attitude change of the carrier.

[0147] At timestamp t1 and timestamp t2, the accelerometer information and gyroscope information of the inertial sensor are saved. MEMS inertial navigation has poor positioning accuracy, but high short-term positioning accuracy, and can solve the posture changes during the loss of visual tracking.

[0148] However, due to the large device error of MEMS inertial navigation, in the initial stage of vision, the inertial / vision initialization method in VINS-MONO is used to roughly calibrate the deviations of the accelerometer and gyroscope.

[0149] Then, using the pre-integration model with the introduced deviation, as shown in the following formula, the inertial posture change from time i to time j can be calculated, including the rotation R wbj and pan p j .

[0150]

[0151] in, and They represent the deviations of the gyroscope and accelerometer at time k, respectively, which are obtained by coarse calibration. and Represent the measurement values ​​of the gyroscope and accelerometer at time k. wbi represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time i, R wbj represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time j, R wbk represents the rotation from the inertial load system coordinate system to the world coordinate system w1 at time k. g represents the gravitational acceleration in the world coordinate system w1, v i 、v k and v j They represent the speed of the world coordinate system w1 at time i, time k and time j respectively, and p i and p j Represent the positions in the world coordinate system w1 at time i and time j respectively.

[0152] Step 4: Using pure inertial navigation, align the world coordinate system initialized at time t2 to the world coordinate system initialized at time t0 to obtain a visually coherent and robust positioning result.

[0153] In order to make visual positioning coherent and robust, it is necessary to use pure inertial navigation to align the world coordinate system initialized at time t2 to the world coordinate system initialized at time t0. This involves three moments: visual initialization moment t0, visual tracking loss moment t1, and visual reinitialization moment t2.

[0154] It is assumed that at time t0, the camera coordinate system is c1, and the visual initialization establishes the world coordinate system w1; at time t1, the camera coordinate system is c2, the world coordinate system is still w1, and the inertial load system is b1; at time t2, the camera coordinate system is c3, the visual sensor is initialized again, and the world coordinate system w2 is constructed, and the inertial load system is b2.

[0155] Then T w1c1represents the pose transformation matrix from the camera coordinate system c1 to the world coordinate system w1, T w2c3 Represents the pose transformation matrix from the camera coordinate system c3 to the world coordinate system w2, including a 3*3 rotation matrix and a 3*1 translation vector. Since the coordinate systems coincide, the rotation matrices here are all unit matrices and the translation vectors are 0.

[0156] The overall conversion process is as follows:

[0157] (1) Step 1 During the visual tracking process, the attitude transformation matrix from the camera system to the world coordinate system w1 is solved at each moment, and the attitude transformation matrix from the camera coordinate system c2 to the world coordinate system w1 at time t1 is recorded, expressed as T w1c2 ;

[0158] (2) Use the external parameter matrix to transform the camera system into the inertial load system, as shown in the following formula:

[0159] T w1b1 =T w1c2 T cb

[0160] Where T cb is an external parameter, which represents the change in posture from the carrier system to the camera system. w1b1 It is the attitude transformation matrix from the inertial carrier system b1 to the world coordinate system w1 at time t1, including rotation and translation.

[0161] (3) Through step 3, the carrier motion from time t1 to time t2 can be obtained, and the attitude transformation matrix from the inertial carrier system b2 to the world coordinate system w1 at time t2 can be obtained, which is expressed as T w1b2 .

[0162] (4) Using the external parameter matrix, the inertial carrier system is transformed into the camera system as shown below: w1c3 =T w1b2 T cb -1

[0163] Where T w1c3 It is the attitude transformation matrix from the camera coordinate system c3 to the world coordinate system w1 at time t2, including rotation and translation.

[0164] (5) Using the conversion relationship between the camera system and the world coordinate system at time t2, the final coordinate system transformation is completed as shown in the following formula:

[0165] T w1w2 =T w1c3 T w2c3 -1 =T w1c3 T c3w2

[0166] Where Tw1w2 The attitude transformation matrix from the world coordinate system w2 at time t2 to the world coordinate system w1 at time t0, including rotation and translation.

[0167] At this point, the transformation of the two world coordinate systems is obtained. After the visual tracking is lost, the reconstructed pose trajectory can be transformed from the world coordinate system w2 to the world coordinate system w1, thereby obtaining a continuous and effective visual positioning result.

[0168] In one embodiment, the rotation extrinsic parameters and translation extrinsic parameters included in the extrinsic parameter matrix are solved as follows:

[0169] Step 1: Complete visual feature tracking and calculate inertial sensor pre-integration.

[0170] The visual sensor uses the optical flow method adopted in VINS-MONO for visual tracking, solves the current visual frame pose using the epipolar geometry constraint, and then uses the bundle adjustment (BA) technology to perform local and global pose optimization.

[0171] The inertial sensor adopts the inertial pre-integration method proposed by Forster et al. to integrate and solve the position and posture information of the inertial sensor (IMU).

[0172] Step 2: Propose a rotation constraint solving method based on dual optimization to obtain the rotational extrinsic parameters of the two sensors.

[0173] This step solves the rotation extrinsic parameters. Since the initial value of the model parameter affects the result of model convergence, the existing methods mostly use large-scale actions to stimulate model convergence. This paper proposes a rotation constraint solution method based on dual optimization, which not only reduces the motion state constraints, but also improves the calibration accuracy of the rotation extrinsic parameters. The first-level optimizer uses low-precision visual pose data and inertial pre-integration results, that is, inertial pose information, to obtain the initial calibration results. The second-level optimizer uses the results of the previous optimizer as the initial value to optimize again to obtain accurate calibration results.

[0174] Step 1 obtains the IMU pose information and the corresponding camera pose information. Unlike displacement information, since rotation does not have unknown scale information, the following constraints can be established for the camera and IMU poses in the same space and time:

[0175]

[0176] in is the inertial rotation obtained by IMU pre-integration between time i and time j, is the visual rotation amount solved by the camera between time i and time j, is the rotation extrinsic parameter between the camera and the IMU.

[0177] The difference is that in the first round of optimization, the camera posture information obtained by the epipolar geometry is used to construct the constraint equation of IMU to perform rotation constraints to obtain rough external parameter calibration values. Since the essential information of epipolar geometry is the solution of relative information between 2D-2D images, its robustness is poor, and there will inevitably be some erroneous information in the visual part, but these erroneous states are temporarily ignored in the rough calibration stage. In the second round of optimization, the visual posture information obtained after BA optimization (bundle adjustment) is used with IMU pre-integrated rotation constraints to obtain accurate calibration results.

[0178] The optimization part of the rotation constraint is shown below. It should be noted that the parameters optimized here include not only the rotation extrinsic parameters Also included is the gyro constant bias within the sliding window In order to provide a good initial estimate for precise calibration, a rough estimate of the gyro bias is made during the coarse calibration phase. The optimization parameters for this part are as follows:

[0179] χ q Rotation Constraint Parameters

[0180] True pre-integration rotation It can be expressed as:

[0181]

[0182] Among them J i is the pre-integrated rotation error about the gyro bias at time i The first-order Jacobian matrix of The initial value of is set to zero.

[0183] Based on the above formula, the error equation of the rotation constraint is specified as follows:

[0184]

[0185] e q (Z q ,x q ) represents the rotation constraint error

[0186] In the above formula, Z q Part of it includes and J i , which is obtained from the measurement information of IMU. It is the visual rotation information of the camera. The coarse calibration part is obtained by the epipolar geometry constraint, and the precise calibration part is obtained by BA optimization.

[0187] Through iterative solution, the rotation external factor of visual inertia is obtained to obtain the gyro bias. After that, the pre-integration information of IMU is updated. Because of the compensation of gyro bias, the quadratic divergence of attitude and position errors in the pre-integration results is reduced, and the inertial / visual navigation accuracy is improved.

[0188] Step 3: Construct a decoupling-based translation extrinsic parameter estimation model to obtain the translation extrinsic parameters of the two sensors

[0189] The translation extrinsic parameters are tightly coupled with other initialization state quantities of inertial / visual sensor calibration. This paper constructs a decoupled translation extrinsic parameter estimator without absolute velocity to better obtain the translation extrinsic parameters. The translation vector is estimated and solved using a sliding window.

[0190] For visual navigation, there are three main coordinate systems involved: image coordinate system, camera coordinate system and world coordinate system. This article defines the camera coordinate system as c system and the world coordinate system as w system. Before initialization, the camera coordinate system of the first key frame in the sliding window is defined as a temporary reference coordinate system and defined as C0. After completing the gravity measurement of the frame, The estimate is then aligned to the world coordinate system.

[0191] Step 2 obtains the rotational external parameters of the inertial / visual sensor into the gyro bias and uses the gyro bias to compensate for the existing pre-integrated information.

[0192] Because the rotation extrinsic The camera posture at time t in the sliding window with C0 as the reference can be obtained Converted to IMU attitude at time t

[0193]

[0194] is the rotation matrix from the inertial navigation to the camera, which can be obtained by Transformed.

[0195] The position conversion is related to the scale. The scale information s is unknown for pure visual solution. When the two sensors are aligned, it is necessary to synchronously estimate the scale information s and the translation amount in the sliding window. and the speed information between frames in the sliding window In order to fuse visual and inertial data, the following initial states need to be estimated:

[0196]

[0197] where χ I is the translation estimation parameter containing absolute velocity, s is the proportional factor between IMU measured displacement and visual restored displacement, is the speed information corresponding to the nth sliding window, is the gravity vector in the reference camera coordinate system.

[0198] The residual equation can be established by pre-integrating the IMU position information and velocity information between adjacent key frames in the sliding window:

[0199]

[0200] and represents the position and velocity error terms, and are the estimated values ​​of the camera relative to the c0 system at time t+1 and time t respectively. δt is the time interval, for The inverse matrix of They are the estimated values ​​of the inertial navigation relative to the c0 system at time t and t+1 respectively.

[0201] During the initialization process, it is assumed that the number of sliding windows is n+1, χ I The state quantity that needs to be estimated is 3n+7 dimensions, the number of constraint equations constructed is 6n dimensions, and a large number of operations are used to estimate the speed information; and in the case of less prior information, the initialization process is often set to zero. Therefore, this paper improves the model estimation strategy and constructs a constraint equation that does not require estimated velocity information. Before initialization, the visual inertial translation extrinsic parameter Estimation and corresponding compensation are performed to improve the quality of initialization.

[0202] This constraint equation replaces the unknown absolute velocity information in the sliding window by pre-integrating the known relative velocity information between adjacent key frames. The position constraint equation in the above formula can be converted into the following form:

[0203]

[0204] In the data of three consecutive frames, the relationship between adjacent frames can be obtained. So far, we can avoid the absolute speed information and calculate χ p The parameters are estimated:

[0205]

[0206] χ p Estimated parameters for translation without absolute velocity.

[0207] The observations in the model include the IMU-measured translation, the scale-free displacement information obtained by the camera's visual reconstruction, the IMU pre-integrated velocity measurement information, and the camera's visual rotation information. The cost function is calculated by taking all paired camera and IMU positions at the initialization stage.

[0208] Obtain χ by solving the cost function p As a result, χ p As a priori information, Formula 1 is used to optimize and solve the velocity information, scale factor and translation external parameters in the sliding window.

[0209] In summary, the visual continuous positioning method based on short-term inertial measurement provided by the present invention has at least the following advantages over the prior art:

[0210] (1) The present invention proposes a visual relocalization method integrating inertial measurement, which uses inertial measurement to assist visual matching positioning, and simultaneously proposes a weighted matching visual relocalization model to obtain more accurate visual pose information, further improving the success rate and accuracy of visual relocalization;

[0211] (2) The present invention aims to solve the problem of re-initialization and incoherent visual positioning after visual tracking is lost. It studies a visual initialization coordinate conversion method based on short-term inertial measurement under visual loss state, and uses inertial sensor positioning to construct the conversion relationship between two sets of visual local coordinate systems to achieve coherent and robust navigation positioning.

[0212] (3) After the rotation extrinsic parameters and the translation extrinsic parameters are decoupled, the present invention accelerates the convergence of the translation extrinsic parameters by solving the rotation extrinsic parameters, thereby accelerating the iterative solution time of the entire sensor initialization;

[0213] (4) In order to solve the problem of low calibration accuracy of rotational extrinsic parameters, the present invention proposes a rotational constraint solving method based on double optimization. Through two rounds of sensor iterative optimization, the accuracy of extrinsic parameter estimation is improved, and the positioning accuracy of inertial / visual combined navigation is further improved.

[0214] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A visual continuous positioning method based on short-term inertial measurement, characterized in that: The following steps are involved: Step 1: After the visual sensor is initialized at time t0, the carrier is positioned through visual tracking to obtain visual posture information; Step 2: If visual tracking is lost at time t1, repositioning is performed using the visual repositioning method that integrates inertial measurement. If repositioning is successful, proceed to step 1 for visual tracking and positioning. If repositioning is unsuccessful, reinitialize the visual sensor. Initialization is completed at time t2, and proceed to step 3. Step 3: the inertial sensor performs inertial measurement in the time interval between t1 and t2 to obtain the inertial attitude change information of the carrier; Step 4: Using the inertial posture change information, align the world coordinate system initialized at time t2 to the world coordinate system initialized at time t0 to obtain a visually coherent and robust positioning result. The visual relocation method integrating inertial measurement comprises the following steps: Use inertial measurement to estimate the carrier motion, obtain the inertial pose of the next cycle, and send it to the visual sensor; The visual sensor is transferred to the visual camera coordinate system according to the obtained inertial posture, and the visual feature matching area is delineated to achieve the initial positioning of the visual matching; In the visual feature matching area, the current frame is matched with the previous video frame, the previous reference frame, and the previous key frame to obtain three different sets of feature matching pairs; The pose change matrix T from the current frame to the previous video frame can be obtained by using the eight-point method of epipolar geometry constraints: c21 , including rotation and translation, the pose change matrix T from the current frame to the previous reference frame c31 , the pose change matrix T from the current frame to the previous key frame c41 , the first round obtains the visual pose information of the current frame through weighted fusion: The pose matrix Represents the conversion of the world coordinate system corresponding to the i-th frame, i.e. the current frame, to the camera coordinate system. Respectively represent the conversion of the world coordinate system corresponding to the previous video frame, the previous reference frame and the previous key frame to the camera coordinate system; w1, w2 and w3 are the visual pose weights corresponding to the previous video frame, the previous reference frame and the previous key frame respectively. Considering their different influences, the following relationship should be dynamically satisfied: When n1<10, n1 takes the value of 0; when n2<10, n2 takes the value of 0; When n3<10, n3 takes the value of 0; Where n1, n2, and n3 represent the number of matching point pairs corresponding to the previous video frame, the previous reference frame, and the previous key frame, respectively. The more matching pairs there are, the higher the confidence level of the camera pose solution. When the number of matching pairs is less than 10, the camera pose result is unreliable and the weight is 0. This results in the estimated camera pose value for the first round of relocalization. The second round of optimization model uses the 3D-2D feature matching between local map points and the current frame to construct the visual reprojection error and minimize the reprojection error r cam It can be expressed as the following formula: Where m represents the map point ID, X m is the coordinate of the 3D map point in the world coordinate system, p am Indicates X on the ath frame m The corresponding pixel observation point, the depth is expressed as π; Use the camera pose estimate obtained in the first round As the initial value, it is brought into the above iterative solution process, and finally the accurate camera pose estimation value of visual relocalization is obtained 2. The visual continuous positioning method based on short-term inertial measurement according to claim 1 is characterized in that: The inertial sensor performs inertial measurement in the time interval between t1 and t2, and the method for obtaining the inertial attitude change information of the carrier is: Calculate the inertial pose change from time i to time j, including the rotation R wbj and pan p j : in, and They represent the deviations of the gyroscope and accelerometer at time k, respectively, obtained by rough calibration, and Represent the measured values ​​of the gyroscope and accelerometer at time k, R wbi represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time i, R wbj represents the rotation from the inertial carrier coordinate system to the world coordinate system w1 at time j, R wbk represents the rotation from the inertial load system coordinate system to the world coordinate system w1 at time k, g represents the gravitational acceleration in the world coordinate system w1, and v i 、v k and v j They represent the speed of the world coordinate system w1 at time i, time k and time j respectively, and p i and p j They represent the positions in the world coordinate system w1 at time i and time j respectively, Δt is the integration time interval, the value of i is t1, and the value of j is t2.

3. The visual continuous positioning method based on short-term inertial measurement according to claim 2 is characterized in that: The steps of aligning the world coordinate system initialized at time t2 to the world coordinate system initialized at time t0 are as follows: During the visual tracking process, the attitude transformation matrix from the camera system to the world coordinate system w1 at each moment is solved, and the attitude transformation matrix from the camera coordinate system c2 to the world coordinate system w1 at time t1 is recorded, which is expressed as T w1c2 ; Using the external parameter matrix, the camera system is converted to the inertial system, as shown in the following formula: T w1b1 =T w1c2 T cb Where T cb is the external parameter matrix, which represents the change of posture from the carrier system to the camera system, T w1b1 is the attitude transformation matrix from the inertial carrier system b1 to the world coordinate system w1 at time t1, including rotation and translation; The carrier motion from time t1 to time t2 is obtained through inertial navigation, and the attitude transformation matrix from the inertial carrier system b2 to the world coordinate system w1 at time t2 is obtained, which is expressed as T w1b2 ; Using the external parameter matrix, the inertial load system is transformed into the camera system, as shown in the following formula: T w1c3 =T w1b2 T cb -1 Where T w1c3 It is the attitude transformation matrix from the camera coordinate system c3 to the world coordinate system w1 at time t2, including rotation and translation; The final coordinate system transformation is completed by using the conversion relationship between the camera system and the world coordinate system at time t2, as shown in the following formula: T w1w2 =T w1c3 T w2c3 -1 =T w1c3 T c3w2 Where T w1w2 The attitude transformation matrix from the world coordinate system w2 at time t2 to the world coordinate system w1 at time t0, including rotation and translation; T w2c3 Represents the pose transformation matrix from the camera coordinate system c3 to the world coordinate system w2, including a 3*3 rotation matrix and a 3*1 translation vector. Since the coordinate systems coincide, the rotation matrix here is the unit matrix and the translation vector is 0; At this point, the transformation of the two world coordinate systems is obtained. After the visual tracking is lost, the reconstructed pose trajectory can be transformed from the world coordinate system w2 to the world coordinate system w1, thereby obtaining a continuous and effective visual positioning result.

4. A medium, characterized in that The medium stores a visual continuous positioning method based on short-term inertial measurement as described in any one of claims 1-3.

5. A navigation control system, characterized in that: The navigation control system uses a visual continuous positioning method based on short-term inertial measurement as described in any one of claims 1-3 to perform navigation positioning control.

Images