Visual inertial fused localization method and device
By fully stacking and matrix merging of multiple feature points and models in camera states in visual inertial fusion positioning, building and eliminating the problem of large calculations in traditional MSCKF algorithms is solved, and positioning efficiency and accuracy are improved.
Patent Information
- Application Number
- PCT/CN2024/089422
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Priority Date
- 2023-05-09
- Filing Date
- 2024-04-23
- Publication Date
- 2024-11-14
AI Technical Summary
The traditional Kalman Filter (MSCKF) algorithm under multi-state constraints has a large amount of calculation during the state update process, which affects the positioning efficiency.
By fully stacking and matrix merging a single model of multiple feature points in multiple camera states, the product terms of the first-order partial derivative matrix and the system state matrix are constructed, projected and eliminated, and the second-order partial derivative matrix is obtained. The residual model then performs state updates, avoiding left zero space calculation and QR decomposition dimensionality reduction calculation.
This method saves calculations, improves positioning efficiency, reduces CPU overhead, and improves calculation efficiency and positioning accuracy.
Smart Images

Figure CN2024089422_14112024_PF_FP_ABST
Abstract
Description
Visual-inertial fusion positioning method and equipment
[0001] This application claims priority to the Chinese invention patent application entitled “Visual-inertial fusion positioning method and device” and application number 2023105176004, filed on May 9, 2023. The entire contents of that application are incorporated by reference into this application. Technical Field
[0002] The present disclosure relates to the field of navigation and positioning technology, and more particularly to a visual-inertial fusion positioning method and device. Background Art
[0003] Visual Inertial Odometry (VIO) is an algorithm that fuses data from cameras and inertial measurement units (IMUs) to achieve simultaneous localization and mapping (SLAM). It can be broadly categorized into two types: filtering-based and optimization-based. The Multi-State Constraint Kalman Filter (MSCKF) is a filtering-based VIO algorithm.
[0004] However, the inventors have discovered that the traditional MSCKF algorithm has at least the following technical problems: the amount of calculation during the state update process is large, which affects the positioning efficiency.
[0005] Summary of the Invention
[0006] The embodiments of the present disclosure provide a visual-inertial fusion positioning method and device to overcome the problem that the MSCKF algorithm has a large amount of calculation during the state update process, which affects the positioning efficiency.
[0007] In a first aspect, an embodiment of the present disclosure provides a visual-inertial fusion positioning method, comprising:
[0008] Obtain target observation data corresponding to the target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state;
[0009] A first residual model is constructed based on the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector;
[0010] Projecting the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term;
[0011] Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0012] In a second aspect, an embodiment of the present disclosure provides a visual-inertial fusion positioning device, comprising:
[0013] an acquisition module, configured to acquire target observation data corresponding to a target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; the target state vector includes multiple camera states;
[0014] A construction module for constructing a first residual model based on the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector;
[0015] a projection module, configured to project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term;
[0016] An elimination module is used to eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0017] In a third aspect, an embodiment of the present disclosure provides an electronic device, including: a processor and a memory;
[0018] The memory stores computer-executable instructions;
[0019] The processor executes the computer-executable instructions stored in the memory, so that the at least one processor executes the visual-inertial fusion positioning method described in the first aspect and various possible designs of the first aspect.
[0020] In a fourth aspect, an embodiment of the present disclosure provides a computer-readable storage medium, which stores computer-executable instructions. When a processor executes the computer-executable instructions, the visual-inertial fusion positioning method described in the first aspect and various possible designs of the first aspect is implemented.
[0021] In a fifth aspect, an embodiment of the present disclosure provides a computer program product, including a computer program, which, when executed by a processor, implements the visual-inertial fusion positioning method described in the first aspect and various possible designs of the first aspect. BRIEF DESCRIPTION OF THE DRAWINGS
[0022] In order to more clearly illustrate the embodiments of the present disclosure or the technical solutions in the prior art, a brief introduction will be given below to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present disclosure. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.
[0023] FIG1 is a schematic diagram showing the principle of a visual-inertial fusion positioning method provided by an embodiment of the present disclosure;
[0024] FIG2 is a flowchart of a visual-inertial fusion positioning method according to an embodiment of the present disclosure;
[0025] FIG3 is a schematic diagram of the observation relationship between multiple feature points and multiple camera states in the current sliding window corresponding to the target state vector provided by an embodiment of the present disclosure;
[0026] FIG4 is a second flow chart of the visual-inertial fusion positioning method provided by an embodiment of the present disclosure;
[0027] FIG5 is a schematic diagram of the observation relationship between multiple feature points corresponding to the target state vector and multiple camera states in the current sliding window and multiple historical camera states in the common viewing window provided by an embodiment of the present disclosure;
[0028] FIG6 is a structural block diagram of a visual-inertial fusion positioning device provided in an embodiment of the present disclosure;
[0029] FIG7 is a schematic diagram of the hardware structure of the visual-inertial fusion positioning device provided in an embodiment of the present disclosure. DETAILED DESCRIPTION
[0030] The following describes embodiments of the present disclosure in more detail with reference to the accompanying drawings. Although certain embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be construed as limited to the embodiments described herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present disclosure. It should be understood that the drawings and embodiments of the present disclosure are for illustrative purposes only and are not intended to limit the scope of protection of the present disclosure.
[0031] The visual-inertial fusion positioning method and device provided in this embodiment first obtain target observation data corresponding to a target state vector, the target observation data including observation data of at least one feature point, the observation data of each feature point including observation data of the feature point under at least one camera state, and constructing a first residual model based on the target observation data. The first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term. The first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of at least one feature point. The system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point. The target state vector includes an inertial measurement unit (IMU) state vector and a camera state vector. The residual model is projected based on the first-order partial derivative matrix to obtain a second residual model. The second residual model includes a product term of a second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term. The three-dimensional coordinate error vector of the second residual model is eliminated to obtain a target residual model, and the target state vector is updated based on the target residual model. In this embodiment, a first residual model is obtained by fully stacking and matrix merging a single model of multiple feature points in multiple camera states, and then projecting it to obtain a second residual model including a second-order partial derivative matrix. The second residual model is then eliminated to obtain a target residual model for state update. This calculation process does not involve left null space calculation and projection, as well as QR decomposition dimensionality reduction calculation, which saves computational effort and improves positioning efficiency.
[0032] To make the objectives, technical solutions, and advantages of the embodiments of the present disclosure more clear, the technical solutions in the embodiments of the present disclosure will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present disclosure, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present disclosure without making any creative efforts shall fall within the scope of protection of the present disclosure.
[0033] Visual Inertial Odometry (VIO) is an algorithm that integrates data from cameras and inertial measurement units (IMUs) to achieve simultaneous localization and mapping (SLAM). It can be divided into two categories: filtering-based and optimization-based. Multi-State Constraint Kalman Filter (MSCKF) is a filtering-based VIO algorithm. However, the amount of computation required by MSCKF during the state update process is large, which affects positioning efficiency. Specifically, during the state update, it is necessary to perform left null space calculation and left null space projection on each feature point observation to be updated. Moreover, because the number of rows in the matrix of the model obtained after the left null space projection far exceeds the number of columns, it is also necessary to use the QR decomposition method for dimensionality reduction. This series of complex calculations will cause a lot of CPU overhead in the next step, affecting the computational efficiency and positioning efficiency.
[0034] To solve the above technical problems, the inventors of this application have discovered that a single model of multiple feature points in multiple camera states can be fully stacked and matrix-merged to obtain a first residual model, which can be projected to form a second residual model including a square matrix of second-order partial derivatives. This second residual model can then be eliminated to obtain a target residual model for state update. This calculation process does not involve left null space calculation and projection, nor does it involve QR decomposition dimensionality reduction calculation, which saves computational effort and improves positioning efficiency. Based on this, the embodiments of the present disclosure provide a visual-inertial fusion positioning method.
[0035] Referring to Figure 1, Figure 1 is a schematic diagram of the principle of a visual-inertial fusion positioning method provided by an embodiment of the present disclosure. As shown in Figure 1, the visual-inertial fusion system includes a positioning device 101, an IMU 102, and a camera 103. Among them, the positioning device 101 adopts an extended Kalman filter (EKF) framework to predict the EKF based on the IMU data (IMU readings) perceived by the IMU 102, and updates the EKF based on the image frames taken by the camera 103, that is, the observation data of the feature points. Specifically, based on the camera 103, the camera state at different times (for example, the camera 103 position P and the attitude quaternion Q) is added to the state vector, and the feature points can be seen by multiple cameras 103, thereby forming a geometric constraint (Constraint) between multiple camera states (Multi-State), and then the geometric constraint can be used to construct a residual model to update the EKF. The positioning device 101 can be applied to fields such as robots, drones, augmented reality (AR) and virtual reality (VR).
[0036] In the specific implementation process, the state vector and covariance are first initialized. Then, during the prediction phase, the system base state and system covariance are propagated from image time k to image time k+1 based on each IMU reading, typically processing multiple IMU readings. During the update phase, after the camera 103 acquires the image frame at time k+1, it calculates the current camera state based on the image frame and adds the current camera state to the state vector to obtain the target state vector, while also expanding the state covariance. A target residual model is constructed based on the observation data corresponding to the target state vector, and the EKF is then updated based on the target residual model.
[0037] Specifically, in constructing the target residual model, target observation data corresponding to the target state vector is obtained; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; a first residual model is constructed according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point. derivative matrix; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of a feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector; according to the first-order partial derivative matrix, the residual model is projected to obtain a second residual model; the second residual model includes a product term of a second-order partial derivative matrix and a system state matrix, and a covariance term corresponding to the first observation noise term; the three-dimensional coordinate error vector of the second residual model is eliminated to obtain a target residual model, and the target state vector is updated by EKF according to the target residual model.
[0038] The visual-inertial fusion positioning method provided by the embodiment of the present disclosure projects a first residual model obtained by fully stacking and matrix merging single models of multiple feature points in multiple camera states to obtain a second residual model including a square matrix of second-order partial derivatives. The second residual model is then eliminated to obtain a target residual model for state update. The calculation process does not involve left null space calculation and projection, and QR decomposition dimensionality reduction calculation, which saves computational effort and improves positioning efficiency.
[0039] Refer to Figure 2, which is a flowchart of a visual-inertial fusion positioning method provided by an embodiment of the present disclosure. The method of this embodiment can be applied to the positioning device shown in Figure 1. The visual-inertial fusion positioning method includes:
[0040] 201. Obtain target observation data corresponding to a target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state.
[0041] In the disclosed embodiments, the target state vector is the system state vector when the update timing determined by the update strategy is reached. The system state vector X1 includes the base state XA (IMU state) and the augmented state XB (camera state). The base state XA may include the IMU's attitude, displacement, velocity, accelerometer bias, gyroscope bias, and may also include rotational and / or translational extrinsics from the camera to the IMU. The augmented state XB may include the camera's rotation, displacement, and velocity, among other things.
[0042] In one embodiment of the present disclosure, to ensure that the system state vector does not increase indefinitely and thus affect computational efficiency, an upper limit on the number of augmentations in the sliding window (i.e., sliding window) of the system state vector can be set. For example, the upper limit is set to n, where XI = [XA, XB0, XB1, ..., XBs]. When s = n, before further augmentation, it is necessary to delete the existing augmented states in the sliding window to ensure that the number of augmented states in the sliding window is less than or equal to the upper limit n. The specific deletion strategy (number of deletions, deletion positions, etc.) can be determined based on actual conditions and is not limited in this embodiment of the present disclosure.
[0043] In the embodiment of the present disclosure, the observation data of the feature point in the camera state is the relevant data of the image frame containing the feature point captured by the camera in the camera state. The content of the target observation data is limited according to the update strategy adopted.
[0044] Exemplarily, in one implementable method, all observation data in the sliding window corresponding to the feature points that cannot be observed in the current sliding window can be determined as target observation data, and then a residual model is constructed based on the target observation data and an extended Kalman filter (EKF) is used for state update.
[0045] In another implementation, the maximum number of augmented states in the sliding window can be set to m. When s == m, all observations of the first two or more camera states, or the last two or more camera states, or one or more camera states before and after the first two or more states, are extracted as needed. These extracted observations are then used as target observations. A residual model is then constructed based on these target observations, and the state is updated using an EKF. The decision on which windows to extract for state update can be made based on whether the system is currently in motion or whether the amount of motion (e.g., distance) has reached a specific limit.
[0046] In another possible implementation, to improve calculation accuracy, based on any of the above methods, in addition to the target observation data determined in each of the above methods, historical camera state observation data outside the current sliding window, that is, observation data associated with at least one feature point in the sliding window prior to the earliest time in the sliding window, can also be determined as target observation data. For details, please refer to the description of the subsequent embodiments and will not be repeated here.
[0047] Referring to Figure 3, Figure 3 is a schematic diagram of the observation relationship between multiple feature points and multiple camera states in the current sliding window corresponding to the target state vector provided by an embodiment of the present disclosure. As shown in Figure 3, the five-pointed star represents the feature point, the box represents the augmented camera state sequence, and the dotted line represents the observation relationship between the feature point and the camera state. The current sliding window corresponding to the target state vector includes feature points f0 to f4, and the camera states include Ca0 to Ca4. Feature point f0 is observed by the camera in five camera states (Ca0 to Ca4), feature point f1 is observed by the camera in three camera states (Ca2 to Ca4), feature point f2 is observed by the camera in four camera states (Ca1 to Ca4), feature point f3 is observed by the camera in three camera states (Ca2 to Ca4), and feature point f4 is observed by the camera in three camera states (Ca2 to Ca4).
[0048] In some disclosed embodiments, before constructing the first residual model based on the target observation data, the process may also include: obtaining multiple IMU data after the previous moment and before the current moment; predicting the system state vector at the previous moment based on the multiple IMU data to obtain the system state vector to be augmented; obtaining the image frame at the current moment, calculating the camera state vector corresponding to the image frame based on the image frame, and augmenting the system state vector to be augmented based on the camera state vector to obtain the target state vector.
[0049] Specifically, during the prediction phase, multiple IMU data sets between image time k and time k+1 can be integrated to propagate the system base state and system covariance from image time k to image time k+1. After obtaining the image frame at time k+1, the camera state is calculated and augmented to obtain the target state vector.
[0050] 202. Construct a first residual model based on the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector.
[0051] Specifically, the feature points can be triangulated based on the camera state in the current sliding window (i.e., the three-dimensional coordinates of the feature points can be fitted), and then the observation data of multiple feature points under multiple camera states can be determined based on the preset state update strategy (refer to the example description of step 201 above), so as to construct a first residual model based on the three-dimensional coordinates of the multiple feature points and the corresponding observation data of the multiple camera states.
[0052] Among them, in the triangulation estimation process, the camera states of multiple different perspectives (different positions) and the observation data of the feature points corresponding to the camera states can be used to solve the three-dimensional coordinates of the feature points based on the camera state based on the nonlinear optimization algorithm. After the three-dimensional coordinates of the feature points in the camera state are obtained by triangulation estimation, the three-dimensional coordinates of the feature points in the world coordinate system can be calculated based on the three-dimensional coordinates and the position relationship of the camera in the world coordinate system. Where W represents the world coordinate system, P represents the position, and i represents the i-th feature point f.
[0053] In some disclosed embodiments, constructing a first residual model based on the target observation data may include: for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point, constructing a third residual model based on the observation data of the feature point under the camera state in the target observation data; the third residual model includes a product term of a first partial derivative matrix and a target state error vector, a product term of a second partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a second observation noise term; the first partial derivative matrix is a first-order partial derivative of the residual function with respect to the target state vector, and the second partial derivative matrix is a first-order partial derivative of the residual function with respect to the three-dimensional coordinate vector of the feature point; stacking the third residual models corresponding to multiple feature points to obtain a fourth residual model; and performing matrix merging on the product terms in the fourth residual model to obtain the first residual model.
[0054] In some disclosed embodiments, constructing a third residual model for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point according to the observation data of the feature point in the target observation data under the camera state may include: constructing a residual function for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point according to the observation data of the feature point in the target observation data under the camera state; the residual function is related to the target state vector and the three-dimensional coordinate vector of the feature point; and linearizing the residual function to obtain a third residual model.
[0055] In some disclosed embodiments, stacking the third residual models corresponding to the plurality of feature points to obtain a fourth residual model may include: stacking the third residual models corresponding to the feature points of the at least one camera state to obtain a fifth residual model; and stacking the fourth residual models corresponding to the at least one feature point to obtain a fourth residual model.
[0056] In the embodiment of the present disclosure, the first-order partial derivative matrix may be a Jacobian matrix.
[0057] Exemplarily, the constraint in vision refers to the reprojection error of the feature point to the camera, that is, the error between the two-dimensional coordinates of the feature point actually observed by the camera and the two-dimensional coordinates of the estimated three-dimensional coordinates of the feature point projected onto the image.
[0058] First, a residual function r = f(XI, Pf) is constructed based on the reprojection error, where r is the residual, XI is the target state vector, and Pf is the three-dimensional coordinate of the feature point.
[0059] Secondly, based on the observation of the i-th feature point in the j-th camera state, the third residual model is constructed:
[0060] Among them, r i,j represents the observation residual of the i-th feature point in the j-th camera state; J x(i,j) Represents the Jacobian matrix of the residual for the target state vector, that is, the first partial derivative matrix, Represents the system state error, that is, the target state error vector, J f(i,j) Represents the Jacobian matrix of the residual corresponding to the three-dimensional coordinates of the i-th feature point, that is, the second partial derivative matrix, Indicates the small amount of three-dimensional coordinate error of the i-th feature point in the world coordinate system, that is, the three-dimensional coordinate error vector, n i,j It represents the observation noise of the system for the i-th feature point in the j-th camera state, that is, the second observation noise.
[0061] Again, based on all observation data in the camera state sequence specified by the i-th feature point in the sliding window, a residual model is constructed, that is, Formula 1 is stacked to form a fourth residual model with the following matrix pattern:
[0062] Among them, r i represents the arrangement or stacking of the observation residuals of the i-th feature point in all specified camera states; J x(i) Indicates that the residual is the arrangement or stacking of the Jacobian matrix of the system state, indicating that the system state error is small. Indicates the arrangement or stacking of the Jacobian matrix corresponding to the three-dimensional coordinates of the i-th feature point. Indicates that the three-dimensional coordinate error of the i-th feature point in the world coordinate system is small, n i represents the stacked combination of the observation noise of the system for the i-th feature point under all specified camera states.
[0063] Again, based on all observations of the camera states related to the feature points within the sliding window that can be used for state update, a fifth residual model is constructed:
[0064] Among them, J x Indicates that the Jacobian matrix of the system state variables is obtained by the observation residuals of all the sliding windows of all the specified feature points, J f It represents the Jacobian matrix of the observation residuals in all sliding windows of all specified feature points for the three-dimensional coordinates of all feature points used to construct the residuals, and n represents the first observation noise.
[0065] Again, the fifth residual model can be matrix merged to obtain the first residual model:
[0066] It can be seen that the system Jacobian matrix in the above formula (4) is J = [J x J f ], the system status is Represents the error between the basic state and the augmented state of the system, Represents the error in the 3D coordinates of all specified feature points; however, the EKF system state variable itself does not contain the 3D coordinates of any feature points. Therefore, it needs to be eliminated later.
[0067] In order to understand the meaning of formula (4) more vividly, the following is an example of formula (4) with reference to Figure 3. s = 4, the camera pose sequence in the current sliding window can observe feature points 0, 1, 2, 3, and 4. There are 5 augmented camera states in the current sliding window, and the observation relationship between feature points and camera states is shown in Figure 3. Therefore, formula (4) can be expressed in the following form.
[0068] The corresponding covariance R is:
[0069] 203. Project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term.
[0070] Specifically, to obtain the second-order partial derivative matrix, the matrix to be projected can be determined based on the dimension of the first-order partial derivative matrix. For example, the transposed matrix of the first-order partial derivative matrix can be determined as the matrix to be projected.
[0071] For example, in order to perform fast elimination, the first residual model shown in formula (4) can be projected onto [J x J f ] T , and the following second residual model is obtained.
[0072] 204. Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0073] Specifically, after obtaining a second residual model including a square matrix by projecting the first residual model, Gaussian elimination method (specifically, for example, Schur elimination method) can be used to eliminate the three-dimensional coordinate error vector in the system state matrix in the second residual model.
[0074] For example, in formula (6), the n vector usually has the same member value, and each element of n is denoted as u. Formula (6) can be simplified as follows:
[0075] Right now:
[0076] In formula (8) is the variable that needs to be eliminated. Formula (8) can be projected into L space The following residual model is obtained:
[0077] After arranging and simplifying, we can get:
[0078] Since the EKF system state only contains the camera pose, but not the three-dimensional coordinates of the feature points, we can get:
[0079] From formula (11), we can see that the target residual model after elimination can be organized into the following standard form.
[0080] Therefore, the EKF can be updated based on formula (12) and combined with the following formula.
[0081] The gain calculation formula is: K=PJ T (JPJ T +R) -1 (13)
[0082] Where K represents the Kalman gain, P is the system covariance, and J is the Jacobian matrix.
[0083] The calculation formula of state correction is: △X=Kr (14)
[0084] The state update formula is: X=X+△X (15)
[0085] The covariance update formula is: P = (I-KJ)P(I-KJ) T +KRK T (16)
[0086] In some disclosed embodiments, after completing an EKF update based on the above formula, the observation data corresponding to the feature points used for the update will be reset so that the observation data is no longer used for subsequent EKF updates. After being reset, the update result, such as the updated target state vector, can be output.
[0087] The disclosed embodiments of the above formulas (1) to (16) project the residual model (including the noise model) with the non-existent state variables into the Jacobi transposed space, and after being projected into the Jacobi transposed space, project it into a specific space (e.g., a space related to the Schur elimination method), thereby obtaining a residual model that can mathematically eliminate the unnecessary partial system states (i.e., the derivation result contains a 0 matrix multiplied by the unnecessary partial system states), thereby obtaining a residual model after projection that does not contain the unnecessary partial system states. The noise in the residual model is projected through the above Schur elimination matrix to the observation noise model n corresponding to the target state variable, and then the EKF is updated. This achieves the goal of improving positioning efficiency while ensuring positioning accuracy.
[0088] From the above description, it can be seen that the first residual model obtained by fully stacking and matrix merging a single model of multiple feature points in multiple camera states is projected to obtain a second residual model including a second-order partial derivative matrix, and then the second residual model is eliminated to obtain a target residual model for state update. This calculation process does not involve left null space calculation and projection, as well as QR decomposition dimensionality reduction calculation, which saves computational effort and improves positioning efficiency.
[0089] Refer to Figure 4, which is a second flow chart of the visual-inertial fusion positioning method provided by an embodiment of the present disclosure. In order to improve the calculation accuracy, the present embodiment adds the application of observation data within the common window. The visual-inertial fusion positioning method includes:
[0090] 401. Obtain first observation data of at least one feature point in a current sliding window corresponding to a target state vector, obtain second observation data of at least one feature point under at least one historical camera state outside the current sliding window, and determine the first observation data and the second observation data as target observation data.
[0091] 402. For each feature point of the at least one feature point and each historical camera state of the at least one historical camera state corresponding to the feature point, construct a sixth residual model based on the observation data of the feature point under the historical camera state in the target observation data; the sixth residual model includes a product term of a third partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a third observation noise term; the third partial derivative matrix is a first-order partial derivative of a residual function constructed with the historical camera state as a constant with respect to the three-dimensional coordinate vector of the feature point.
[0092] 403 : Stack the third residual model and the sixth residual model corresponding to the plurality of feature points to obtain a fourth residual model.
[0093] 404. Perform matrix merging on the product terms in the fourth residual model to obtain a first residual model.
[0094] Specifically, the feature points in the current sliding window may have historical camera states with observation relationships outside the current sliding window. If these historical camera states are added to the constraints on the three-dimensional coordinates of the feature points, the amount of observation data can be increased, which helps to improve the accuracy of the first-order partial derivative of the three-dimensional coordinates, thereby improving the accuracy of the residual model and the accuracy of the EKF update.
[0095] Exemplarily, referring to FIG5 , FIG5 is a schematic diagram of the observation relationship between multiple feature points corresponding to the target state vector provided by an embodiment of the present disclosure and multiple camera states in the current sliding window and multiple historical camera states in the common view window. As shown in FIG5 , based on the observation relationship shown in FIG3 , the observation relationship between the feature point f0 in the common view window and the historical camera states Ca-2 to Ca-1 in the common view window is added. The common view window refers to a window outside the current sliding window in which there are historical camera states that have an observation relationship with at least one feature point in the sliding window. Furthermore, the constraints of the feature point f0 and the historical camera states Ca-2 to Ca-1 in the common view window can be added to the residual model to obtain the following residual model:
[0096] By comparing formula (17) with formula (5), it can be found that the observation data based on the common window adds constraints on the three-dimensional coordinate error vector of the feature point.
[0097] The disclosed embodiment adds the residuals formed by the landmarks observed within the current sliding window and the observations of the landmarks by the old window outside the sliding window to the current residual construction, and sets the position (position and attitude) of the old window as a constant, so that the residuals constrain the current EKF solution. This makes the EKF solution more accurate and the positioning precision higher.
[0098] 405. Project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term;
[0099] 406. Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform an EKF update on the target state vector according to the target residual model.
[0100] Steps 405 to 406 in the disclosed embodiment are similar to steps 203 to 204 in the disclosed embodiment above, and are not described again here.
[0101] From the above description, it can be seen that by adding the observation data of the historical camera state outside the current sliding window, that is, in the common window that has an observation relationship with at least one feature point in the current sliding window, into the construction of the residual model, the amount of observation data can be increased, which helps to improve the accuracy of the first-order partial derivative of the three-dimensional coordinates, thereby improving the accuracy of the residual model and the accuracy of the EKF update.
[0102] Corresponding to the visual-inertial fusion positioning method described in the previous embodiment, Figure 6 is a block diagram of the visual-inertial fusion positioning device provided in an embodiment of the present disclosure. For ease of illustration, only the portions relevant to the present embodiment are shown. Referring to Figure 6 , the device 60 includes an acquisition module 601, a construction module 602, a projection module 603, and an elimination module 604.
[0103] The acquisition module 601 is configured to acquire target observation data corresponding to a target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; and the target state vector includes multiple camera states.
[0104] A construction module 602 is configured to construct a first residual model based on the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit (IMU) state vector and a camera state vector;
[0105] A projection module 603 is configured to project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term;
[0106] The elimination module 604 is configured to eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0107] In one embodiment of the present disclosure, the construction module 602 is specifically used to: for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point, construct a third residual model according to the observation data of the feature point under the camera state in the target observation data; the third residual model includes a product term of a first partial derivative matrix and a target state error vector, a product term of a second partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a second observation noise term; the first partial derivative matrix is a first-order partial derivative of the residual function with respect to the target state vector, and the second partial derivative matrix is a first-order partial derivative of the residual function with respect to the three-dimensional coordinate vector of the feature point; stack the third residual models corresponding to multiple feature points to obtain a fourth residual model; and perform matrix merging on the product terms in the fourth residual model to obtain the first residual model.
[0108] The construction module 602 is specifically used to: for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point, construct a residual function according to the observation data of the feature point under the camera state in the target observation data; the residual function is related to the target state vector and the three-dimensional coordinate vector of the feature point; and linearize the residual function to obtain a third residual model.
[0109] The construction module 602 is specifically used to: stack the third residual models corresponding to the feature points of the at least one camera state to obtain a fifth residual model; and stack the fourth residual models corresponding to the at least one feature point to obtain a fourth residual model.
[0110] The acquisition module 601 is specifically used to: obtain first observation data of at least one feature point in the current sliding window corresponding to the target state vector; obtain second observation data of at least one of the feature points under at least one historical camera state outside the current sliding window; and determine the first observation data and the second observation data as target observation data.
[0111] The construction module 602 is also specifically used to: for each feature point of the at least one feature point and each historical camera state of the at least one historical camera state corresponding to the feature point, construct a sixth residual model according to the observation data of the feature point in the historical camera state in the target observation data; the sixth residual model includes the product term of the third partial derivative matrix and the three-dimensional coordinate error vector of the feature point, and the third observation noise term; the third partial derivative matrix is the first-order partial derivative of the residual function constructed with the historical camera state as a constant with respect to the three-dimensional coordinate vector of the feature point; the third residual model and the sixth residual model corresponding to multiple feature points are stacked to obtain a fourth residual model.
[0112] The device 60 also includes a prediction and augmentation module for: obtaining multiple IMU data after the previous moment and before the current moment; predicting the system state vector at the previous moment based on the multiple IMU data to obtain the system state vector to be augmented; obtaining an image frame at the current moment, calculating the camera state vector corresponding to the image frame based on the image frame, and augmenting the system state vector to be augmented based on the camera state vector to obtain a target state vector.
[0113] The elimination module 604 is specifically configured to: perform elimination on the three-dimensional coordinate error vector of the second residual model based on the Schur elimination method to obtain a target residual model.
[0114] The device provided in this embodiment can be used to execute the technical solution of the above method embodiment. Its implementation principle and technical effects are similar and will not be described in detail in this embodiment.
[0115] In order to implement the above embodiment, the embodiment of the present disclosure further provides an electronic device.
[0116] Referring to FIG7 , a schematic diagram of the structure of an electronic device 700 suitable for implementing an embodiment of the present disclosure is shown. The electronic device 700 may be a terminal device or a server. The terminal device may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, personal digital assistants (PDAs), tablet computers (Portable Android Devices, PADs), portable multimedia players (PMPs), vehicle-mounted terminals (e.g., vehicle-mounted navigation terminals), and fixed terminals such as digital TVs and desktop computers. The electronic device shown in FIG7 is merely an example and should not limit the functionality and scope of use of the embodiments of the present disclosure.
[0117] As shown in FIG7 , the electronic device 700 may include a processing device (e.g., a central processing unit, a graphics processing unit, etc.) 701, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 702 or a program loaded from a storage device 708 into a random access memory (RAM) 703. Various programs and data required for the operation of the electronic device 700 are also stored in the RAM 703. The processing device 701, the ROM 702, and the RAM 703 are connected to each other via a bus 704. An input / output (I / O) interface 705 is also connected to the bus 704.
[0118] Typically, the following devices may be connected to the I / O interface 705: an input device 706 including, for example, a touch screen, a touchpad, a keyboard, a mouse, a camera, a microphone, an accelerometer, a gyroscope, etc.; an output device 707 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; a storage device 708 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 709. The communication device 709 may allow the electronic device 700 to communicate with other devices wirelessly or by wire to exchange data. Although FIG. 7 shows an electronic device 700 having various devices, it should be understood that it is not required to implement or have all of the devices shown. More or fewer devices may be implemented or have alternatively.
[0119] In particular, according to an embodiment of the present disclosure, the process described above with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present disclosure includes a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from the network via the communication device 709, or installed from the storage device 708, or installed from the ROM 702. When the computer program is executed by the processing device 701, the above-mentioned functions defined in the method of the embodiment of the present disclosure are performed.
[0120] It should be noted that the computer-readable medium mentioned above in the present disclosure may be a computer-readable signal medium or a computer-readable storage medium, or any combination of the two. A computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or component, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, a computer-readable storage medium may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, device, or component. In the present disclosure, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, which carries computer-readable program code. Such a propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium may be transmitted using any suitable medium, including but not limited to wires, optical cables, RF (radio frequency), etc., or any suitable combination thereof.
[0121] The computer-readable medium may be included in the electronic device, or may exist independently without being incorporated into the electronic device.
[0122] The computer-readable medium carries one or more programs. When the one or more programs are executed by the electronic device, the electronic device executes the method shown in the above embodiment.
[0123] Computer program code for performing the operations of the present disclosure may be written in one or more programming languages, or a combination thereof, including object-oriented programming languages such as Java, Smalltalk, C++, and conventional procedural programming languages such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer through any type of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet Service Provider).
[0124] The flowcharts and block diagrams in the accompanying drawings illustrate the possible implementation architecture, functions and operations of the systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each box in the flowchart or block diagram can represent a module, program segment, or a part of code, and the module, program segment, or a part of code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order than that marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of the boxes in the block diagram and / or flowchart, can be implemented with a dedicated hardware-based system that performs the specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.
[0125] The units involved in the embodiments described in this disclosure may be implemented in software or hardware. In some cases, the name of a unit does not limit the unit itself. For example, the first acquisition unit may also be described as a "unit for acquiring at least two Internet Protocol addresses."
[0126] The functions described above herein may be performed, at least in part, by one or more hardware logic components. For example, and without limitation, exemplary types of hardware logic components that may be used include: field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chip (SOCs), complex programmable logic devices (CPLDs), and the like.
[0127] In the context of the present disclosure, a machine-readable medium can be a tangible medium that can contain or store a program for use by or in conjunction with an instruction execution system, device or equipment. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium can include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
Claims
1. A visual-inertial fusion positioning method, comprising: Obtain target observation data corresponding to the target state vector; The target observation data includes observation data of at least one feature point; The observation data of each feature point includes the observation data of the feature point under at least one camera state; A first residual model is constructed according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector; According to the first-order partial derivative matrix, the residual model is projected to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term; Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
2. The method according to claim 1, wherein: The step of constructing a first residual model according to the target observation data comprises: For each feature point in the at least one feature point and each camera state in the at least one camera state corresponding to the feature point, a third residual model is constructed according to the observation data of the feature point in the camera state in the target observation data; the third residual model includes a product term of a first partial derivative matrix and a target state error vector, a product term of a second partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a second observation noise term; the first partial derivative matrix is a first-order partial derivative of the residual function with respect to the target state vector, and the second partial derivative matrix is a first-order partial derivative of the residual function with respect to the three-dimensional coordinate vector of the feature point; Stacking the third residual models corresponding to the plurality of feature points to obtain a fourth residual model; The product terms in the fourth residual model are matrix-merged to obtain a first residual model.
3. The method according to claim 2, wherein: The constructing, for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point, a third residual model according to observation data of the feature point in the camera state in the target observation data, comprises: For each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point, construct a residual function according to the observation data of the feature point in the camera state in the target observation data; the residual function is related to the target state vector and the three-dimensional coordinate vector of the feature point; The residual function is linearized to obtain a third residual model.
4. The method according to claim 2, wherein: The step of stacking the third residual models respectively corresponding to the plurality of feature points to obtain a fourth residual model comprises: stacking the third residual models of the feature points corresponding to the at least one camera state to obtain a fifth residual model; The fourth residual models corresponding to the at least one feature point are stacked to obtain a fourth residual model.
5. The method according to claim 2, wherein: The obtaining of target observation data corresponding to the target state vector includes: Obtaining first observation data of at least one feature point in the current sliding window corresponding to the target state vector; Acquire second observation data of at least one of the feature points under at least one historical camera state outside the current sliding window; The first observation data and the second observation data are determined as target observation data.
6. The method according to claim 5, wherein: The constructing of the first residual model according to the target observation data further includes: For each feature point in the at least one feature point and the feature point corresponding to For each of at least one historical camera state, a sixth residual model is constructed according to the observation data of the feature point in the target observation data under the historical camera state; the sixth residual model includes a product term of a third partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a third observation noise term; the third partial derivative matrix is a first-order partial derivative of a residual function constructed with the historical camera state as a constant with respect to the three-dimensional coordinate vector of the feature point; The step of stacking the third residual models respectively corresponding to the plurality of feature points to obtain a fourth residual model comprises: The third residual model and the sixth residual model corresponding to the plurality of feature points are stacked to obtain a fourth residual model.
7. The method according to any one of claims 1 to 6, wherein: Before constructing the first residual model according to the target observation data, the method further includes: Get multiple IMU data after the previous moment and before the current moment; Predicting the system state vector at the previous moment according to the plurality of IMU data to obtain the system state vector to be augmented; An image frame at a current moment is acquired, a camera state vector corresponding to the image frame is calculated according to the image frame, and the system state vector to be augmented is augmented based on the camera state vector to obtain a target state vector.
8. The method according to any one of claims 1 to 6, wherein: Eliminating the three-dimensional coordinate error vector of the second residual model to obtain a target residual model includes: Based on the Schur elimination method, the three-dimensional coordinate error vector of the second residual model is eliminated to obtain a target residual model.
9. A visual inertial fusion positioning device, wherein: include: An acquisition module is used to acquire target observation data corresponding to a target state vector; The target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; the target state vector includes multiple camera states; A construction module is used to construct a first residual model according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first an observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector; A projection module, configured to project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of a second-order partial derivative matrix and a system state matrix, and a covariance term corresponding to the first observation noise term; An elimination module is used to eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
10. An electronic device, wherein: include: Processor and memory; The memory stores computer-executable instructions; The processor executes the computer-executable instructions stored in the memory, so that the processor executes the visual-inertial fusion positioning method according to any one of claims 1 to 8.
11. A computer-readable storage medium, wherein: The computer-readable storage medium stores computer-executable instructions. When the processor executes the computer-executable instructions, the visual-inertial fusion positioning method according to any one of claims 1 to 8 is implemented.
12. A computer program product comprising a computer program, wherein: When the computer program is executed by a processor, the method for visual-inertial fusion positioning according to any one of claims 1 to 8 is implemented.
Citation Information
Patent Citations
Robot positioning method with fusion of visual features and IMU information
CN110345944A
Miniature unmanned aerial vehicle high-speed sensing and positioning method
CN111707261A
Method and device for positioning based on vision and inertia mileage
CN111797906A
Binocular vision inertial odometer method with synchronous time error estimation
CN112985450A
Visual fusion positioning method and device, electronic equipment and readable storage medium
CN113532455A
Cited By
Target positioning method integrating unmanned aerial vehicle and area monitoring and storage medium
CN120219479A
Puncture correction method, device and equipment based on dynamic vision and storage medium
CN120953309A
