Vision and inertial odometer combined navigation method, device, equipment and medium

Abstract

The application provides a visual and inertial odometer combined navigation method, device, equipment and medium, comprising determining a state vector of visual / inertial odometer combined navigation; constructing a system model of visual / inertial odometer combined navigation; constructing an observation model of the visual / inertial odometer combined navigation system based on a pure pose constraint; completing Kalman filtering according to the system model and the observation model of the visual / inertial odometer combined navigation system, and outputting the filtered attitude, speed and position information of the carrier, wherein the observation model of the visual / inertial odometer combined navigation filtering is re-derived based on the pure pose constraint theory, the new observation model no longer contains three-dimensional feature information, so the zero space mapping and three-dimensional reconstruction process are no longer needed, thereby avoiding the inconsistent model problem of the multi-state constraint Kalman filtering and the influence of the error introduced by the inaccurate three-dimensional reconstruction, and improving the combined navigation precision.

Description

Technical Field

[0001] The present invention mainly relates to the field of visual-inertial integrated navigation technology, and in particular to a visual-inertial odometer integrated navigation method, device, equipment and medium. Background Art

[0002] The problem of integrated visual / inertial odometry navigation has always been a hot topic in the field of state estimation research, especially for the application of unmanned vehicles such as unmanned vehicles and drones in satellite-denied environments. Among all integrated visual / inertial odometry navigation solutions, the combination of an inertial measurement unit (IMU) and a monocular camera has attracted much attention due to its small size and low cost. However, monocular cameras have the problem of uncertain measurement scale. However, an inertial measurement unit (IMU) can provide an absolute scale for the visual odometry and can also be used to assist in the extraction and matching of visual features. After the integrated navigation system loses visual information, the inertial navigation system can still operate with high precision for a short period of time.

[0003] Integrated navigation methods using vision and inertial odometry can be categorized as loose or tight integration, depending on the fusion method. Loose integration processes inertial and visual information separately, using inertial measurement unit (IMU) measurements for state transfer and visual pose calculations for updates. This approach fails to leverage the IMU's complementary role in vision and cannot correct for drift introduced by visual pose. Tight integration utilizes raw inertial and visual information to optimize all IMU and camera measurements, then implements the optimization based on filtering or nonlinear optimization. Because IMU and camera measurements can influence each other, tight integration offers greater robustness and accuracy than loose integration. Tight integration methods based on nonlinear optimization are generally considered more accurate than tight integration methods based on filtering because they utilize more measurement information in each iteration. However, the iterative solution of nonlinear equations requires significant computational resources, making real-time computation difficult on resource-constrained platforms. In contrast, methods based on the extended Kalman filter offer improved computational efficiency and are easily implemented on small mobile devices.

[0004] The Multi-State Constrained Kalman Filter (MSCKF) is one of the most classic filter-based visual-inertial odometry methods. Research has shown that the MSCKF offers high computational efficiency and robustness, maintaining consistent accuracy across different hardware platforms. However, the MSCKF is susceptible to linearization errors, which accumulate over time and degrade performance.

[0005] A significant source of linearization error lies in the limitations of traditional multi-view geometry descriptions. The MSCKF observation model is related to the three-dimensional feature positions, but to maintain the simplicity of the state space, the MSCKF system state does not include the feature point positions. Therefore, when linearizing the reprojection error, the system error state and the feature point error must first be jointly expressed, and then the null space mapping method is applied to restore it to the standard EKF format, reducing the consistency of the model. Furthermore, the three-dimensional feature positions used by the MSCKF come from a simple nonlinear optimization process, which involves first recovering the three-dimensional feature positions using triangulation and then optimizing them using the Gauss-Newton method. During the optimization process, the relative pose of the cameras is considered known, making the optimization process less rigorous and accurate, and easily introducing linearization errors into the subsequent filtering estimation process.

[0006] Therefore, how to maintain the advantages of high efficiency and robustness of the multi-state constrained Kalman filter (MSCKF) while improving its disadvantage of being susceptible to linearization errors, so as to obtain a more efficient and high-precision visual-inertial odometry integrated navigation technology solution is a problem worth studying. Summary of the Invention

[0007] Aiming at the problems of model consistency degradation and susceptibility to interference from three-dimensional reconstruction errors in traditional visual-inertial odometry integrated navigation methods based on multi-state constrained Kalman filtering (MSCKF), the present invention proposes a visual-inertial odometry integrated navigation method, device, equipment and medium.

[0008] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0009] In one aspect, the present invention provides a visual and inertial odometry integrated navigation method, comprising:

[0010] The strapdown inertial navigation solution is performed based on the motion information received by the inertial measurement unit in the visual / inertial odometry integrated navigation system to obtain the attitude, velocity and position information of the carrier;

[0011] Extract feature points based on the image information synchronously collected by the camera in the visual / inertial odometry integrated navigation system and track the extracted feature points;

[0012] Determine the state vector of the visual / inertial odometry integrated navigation, including the state vector associated with the inertial measurement unit and the state vector associated with the camera;

[0013] Construct a system model for visual / inertial odometry integrated navigation, including a system model related to the inertial navigation error state based on a dual-state transformation of velocity error and position error, and a state and covariance transfer model related to the camera pose error state based on a dual-state transformation of velocity error and position error;

[0014] The reprojection error of the feature points in the image, which is a linear representation of the system error state and the feature point position error, is used as the observation quantity. An observation model of the visual / inertial odometry integrated navigation system is constructed based on pure pose constraints.

[0015] Kalman filtering is completed based on the system model and observation model of the visual / inertial odometry integrated navigation system, and the attitude, speed, and position information of the filtered carrier are output.

[0016] In another aspect, the present invention provides a visual and inertial odometry combined navigation device, comprising:

[0017] The first module is used to perform strapdown inertial navigation solution based on the motion information received by the inertial measurement unit in the visual / inertial odometry integrated navigation system to obtain the attitude, speed and position information of the carrier;

[0018] The second module is used to extract feature points based on image information synchronously collected by the camera in the visual / inertial odometry integrated navigation system and track the extracted feature points;

[0019] The third module is used to determine the state vector of the visual / inertial odometry integrated navigation, including the state vector related to the inertial measurement unit and the state vector related to the camera;

[0020] The fourth module is used to build a system model for visual / inertial odometry integrated navigation, including building a system model related to the inertial navigation error state based on the dual-state transformation of velocity error and position error, and building a state and covariance transfer model related to the camera pose error state based on the dual-state transformation of velocity error and position error;

[0021] The fifth module is used to construct the observation model of the visual / inertial odometry integrated navigation system based on pure pose constraints, using the reprojection error of the feature points in the image represented by the linearization of the system error state and the feature point position error as the observation quantity;

[0022] The sixth module is used to complete Kalman filtering based on the system model and observation model of the visual / inertial odometry integrated navigation system, and output the attitude, speed, and position information of the filtered carrier.

[0023] On the other hand, the present invention provides a computer device including a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the above-mentioned visual and inertial odometer combined navigation method when executing the computer program.

[0024] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, which implements the steps of the above-mentioned visual and inertial odometry combined navigation method when the computer program is executed by a processor.

[0025] On the other hand, the present invention provides a computer program product, which is stored on a computer-readable storage medium and includes computer instructions, which, when executed by a processor, enable a computer device to implement the steps of the above-mentioned visual and inertial odometry combined navigation method.

[0026] The observation model in the filtering method involved in the present invention is not limited to being used in conjunction with a system model based on a dual-state transformation Kalman filter, and can be extended to be applied in other filtering-type integrated navigation systems that can use the observation model.

[0027] The filtering method involved in the present invention is not limited to the combined navigation system of monocular vision / inertial odometry, but can be extended to the combined navigation system of binocular vision / inertial odometry, and can also be further extended to the combined navigation system of multiple sensors including vision / inertial information.

[0028] Compared with the prior art, the technical effects of the present invention are:

[0029] (1) The speed error and position error of the visual and inertial odometry combined navigation method in the present invention provide more precise definitions with better consistency and observability.

[0030] (2) The visual / inertial integrated navigation system model in the present invention no longer contains force-related terms, but is replaced by gravity-related terms, so that the integrated navigation can be better applied to complex dynamic environments and the filtering has better consistency.

[0031] (3) The visual-inertial integrated navigation observation model in the present invention no longer contains three-dimensional feature information and can be directly represented by the system error state linearization. There is no need for null space mapping to restore the standard extended Kalman filter format, thus avoiding the risk of reduced model consistency.

[0032] (4) The visual-inertial integrated navigation system in the present invention no longer requires three-dimensional feature information, thus eliminating the three-dimensional feature reconstruction process and avoiding the risk of errors introduced by an inaccurate three-dimensional reconstruction process.

[0033] (5) The present invention is easy to implement, highly efficient, and requires little computation. The filtering process of the present invention is completely consistent with the standard extended Kalman filter process, is simple to implement, and is easier to understand.

[0034] The visual and inertial odometry integrated navigation method proposed in the present invention has strong engineering application flexibility and applicability. Since the computational complexity is relatively small, there is enough room for adding additional functions to the integrated navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed in the embodiments or prior art description. Obviously, the drawings in the following description only show some of the embodiments of the present application, and for those skilled in the art, other drawings can also be obtained from the structures shown in the drawings without creative labor.

[0036] Figure 1 is a flow chart of a visual and inertial odometer combined navigation method provided by an embodiment;

[0037] Figure 2 is a graph of experimental results of a first UAV flight experiment, Figure 2 is (a) a flight trajectory comparison graph, Figure 2 is (b) a horizontal position error result comparison graph;

[0038] Figure 3 is a graph of experimental results of a second UAV flight experiment, Figure 3 is (a) a flight trajectory comparison graph, Figure 3 is (b) a horizontal position error result comparison graph. DETAILED DESCRIPTION

[0039] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0040] A visual / inertial odometer combined navigation system is installed on a carrier, the visual / inertial odometer combined navigation system comprising an inertial measurement unit (IMU) and a camera, the IMU receiving motion information of the carrier, the camera synchronously collecting image information, performing strapdown inertial navigation calculation according to the motion information received by the IMU to obtain attitude, speed and position information of the carrier; and extracting feature points from the image information synchronously collected by the camera and tracking the extracted feature points.

[0041] Specifically, inertial navigation calculation is performed according to the motion information received by the IMU to obtain attitude, speed and position information of the IMU; FAST (Features from Accelerated Segment Test) corners are extracted as feature points from the image information synchronously collected by the camera, and two-dimensional pixel coordinate information of the feature points is stored; and KLT (Kanade Lucas Tomasi) optical flow tracking method is used to track the extracted feature points.

[0042] The IMU receives motion information from the carrier, including angular increments or angular velocity information from the three-axis gyroscope and specific force or integrated specific force increment information from the three-axis accelerometer. This data is used to perform inertial navigation. The image information captured by the monocular camera is a two-dimensional image acquired synchronously with the IMU. Based on this two-dimensional image, FAST corner points are extracted and their pixel coordinates are stored. The extracted corner points are then tracked using the KLT optical flow tracking algorithm.

[0043] The first frame is used to extract key image feature points, which serve as the initial tracking feature points. FAST (Features from Accelerated Segment Test) corner points are widely used in visual navigation due to their advantages, such as low computational complexity and strong real-time performance. Therefore, this paper selects FAST corner points as the initial feature points.

[0044] Before formally implementing filtering for integrated visual and inertial odometry navigation, the visual image must track and match feature points, remove abnormal feature points, and extract new feature points. The KLT (KanadeLucasTomasi) tracking method was chosen. This method, based on optical flow tracking, is highly adaptable to feature deformation. Furthermore, after tracking a feature point, new feature points are detected in image areas outside the tracked point to replenish the number of feature points during the tracking process.

[0045] The coordinate systems involved in the present invention are clarified below: the world reference coordinate system w, the IMU coordinate system b, and the camera coordinate system c, which are defined as follows:

[0046] World Reference Coordinate System (W): This world reference coordinate system is the local tangent plane coordinate system, fixed to the Earth and coinciding with the local geographic coordinate system of the point at which the image is captured. To ensure seamless algorithm operation, the position and attitude of the IMU and camera, as well as the key points in the scene, are expressed in this world reference coordinate system.

[0047] Inertial navigation system coordinate system (also known as IMU coordinate system) b: This coordinate system is the navigation solution load system, with the center of the three accelerometers as the origin and the sensitive directions of the three sensitive axes as the X, Y, and Z axes. The three directions satisfy the right-hand rule and point to the lower right front. During installation, the coordinate axis directions should be as consistent as possible with the vehicle coordinate system to make the installation angle smaller.

[0048] Camera coordinate system c: This coordinate system is fixed to the camera. The origin of the coordinate system is at the optical center of the camera. The X-axis and Y-axis are parallel to the imaging plane, where the X-axis is horizontal, the Y-axis is vertical, and the Z-axis is parallel to the axis of the lens. The direction is determined by the X-axis and Y-axis according to the right-hand rule.

[0049] In traditional visual / inertial odometry, feature point locations are typically added to the state vector, and new observations are used to continuously optimize the feature point positions and the vehicle pose until the feature points disappear. However, unlike simultaneous localization and mapping (SLAM) systems, inertial / visual odometry does not require an environmental map, so adding feature points to the state vector for optimization complicates the calculation process. The Multi-State Constrained Kalman Filter (MSCKF) algorithm improves on this. The state vector no longer includes feature point locations, but instead uses the camera pose at past moments. The state vector is a sliding window that continuously adds new camera poses and discards old ones.

[0050] Reference Figure 1 In one embodiment, a visual and inertial odometry combined navigation method is provided, comprising:

[0051] The strapdown inertial navigation solution is performed based on the motion information received by the inertial measurement unit in the visual / inertial odometry integrated navigation system to obtain the attitude, velocity and position information of the carrier;

[0052] Extract feature points based on the image information synchronously collected by the camera in the visual / inertial odometry integrated navigation system and track the extracted feature points;

[0053] Determine the state vector of visual / inertial odometry integrated navigation;

[0054] Wherein: the state vector of the visual / inertial odometry integrated navigation includes a state vector related to the inertial measurement unit and a state vector related to the camera;

[0055] Build a system model for visual / inertial odometry integrated navigation;

[0056] The system model of the visual / inertial odometry integrated navigation system includes constructing a system model related to the inertial navigation error state based on a dual-state transformation of velocity error and position error, and constructing a state and covariance transfer model related to the camera pose error state based on a dual-state transformation of velocity error and position error;

[0057] The reprojection error of the feature points in the image, which is a linear representation of the system error state and the feature point position error, is used as the observation quantity. An observation model of the visual / inertial odometry integrated navigation system is constructed based on pure pose constraints.

[0058] Kalman filtering is completed based on the system model and observation model of the visual / inertial odometry integrated navigation system, and the attitude, speed, and position information of the filtered carrier are output.

[0059] The present invention aims to re-derive the observation model of the visual / inertial odometry integrated navigation filter based on the pure pose constraint theory. The new observation model no longer contains three-dimensional feature information, so the null space mapping and three-dimensional reconstruction process are no longer required, thereby avoiding the model inconsistency problem and the influence of the error introduced by inaccurate three-dimensional reconstruction in the multi-state constrained Kalman filter (MSCKF), and improving the accuracy of the integrated navigation. Specifically, the observation model of the visual / inertial odometry integrated navigation system is constructed based on the pure pose constraint, including: in the multi-view geometric description based on the pure pose constraint, in the series of extracted feature points, the lth feature point f is l A total of n frames are observed, then there exists 1≤i,j,k≤n, and the three-dimensional coordinates in the camera coordinate system corresponding to the i-th frame image are defined as Expressed as:

[0060]

[0061] in, Represents the relative displacement relationship between the j-th frame image and the k-th frame image, express The antisymmetric matrix of Represents the relative displacement relationship between the j-th frame image and the i-th frame image, Indicates the relative rotation relationship between the j-th frame image and the i-th frame image, Indicates the relative rotation relationship between the j-th frame image and the k-th frame image, and Represent the two-dimensional coordinates of the feature points in the j-th frame and the k-th frame image respectively;

[0062] The reprojection error of the lth feature point in the i-th frame image for:

[0063]

[0064] in is the estimated value of the two-dimensional coordinate of the lth feature point in the i-th frame image obtained by pure pose constraint, is the two-dimensional coordinate of the lth feature point in the i-th frame image obtained directly by feature extraction;

[0065] The reprojection error is linearized using the system error state, and the observation model is obtained as follows:

[0066]

[0067] in is the reprojection error Relative to the Jacobian matrix of the error state vector δx; δx represents the error state vector corresponding to the state vector x, the state vector x of the visual / inertial odometry integrated navigation is [x b x c ] T , x b is the state vector associated with the inertial measurement unit, x c is the state vector associated with the camera; is the corresponding observation noise.

[0068] In one embodiment, a system model related to the inertial navigation error state is constructed based on a dual-state transformation of velocity error and position error as follows:

[0069]

[0070] Among them F b Expressed as a system matrix, δx b Indicates that x b The corresponding error state vector, G b is the noise transfer matrix, w b is the system noise, x b is the state vector associated with the inertial measurement unit.

[0071] At time t, the error state vector associated with the inertial measurement unit is expressed as:

[0072]

[0073] in, is the Hamilton unit quaternion, which represents the rotation from the b system to the w system at time t. is the projection of the velocity of the inertial measurement unit relative to the w system in the w system, is the projection of the position of the inertial measurement unit relative to the w system in the w system, b g and b a They are gyroscope bias and accelerometer bias, represents the rotation from the c system to the b system, Indicates the positional relationship between system c and system b, and Together they represent the relative pose between the inertial navigation system and the camera. The w system is the world reference coordinate system, the b system is the IMU coordinate system, and the c system is the camera coordinate system.

[0074]

[0075] In the formula δb g , δb a 、 They represent attitude error, velocity error after state transformation, position error after state transformation, gyroscope bias error, accelerometer bias error, relative attitude error between inertial navigation and camera, and relative position error between inertial navigation and camera respectively;

[0076] The new speed error after state transformation is defined as:

[0077]

[0078] yes The estimated value of represent The antisymmetric matrix of Represents the speed error state before the state change.

[0079] In order to obtain better consistency and observability in the subsequent Kalman filtering process, the present invention defines the new position error after the state transformation as:

[0080]

[0081] Represents the rotation matrix from b system to w system, Represents the estimated value of the rotation matrix from the w system to the b system, yes The estimated value of represent The antisymmetric matrix of Represents the speed error state before the state change.

[0082] Furthermore, the present invention proposes a new system matrix F b The specific representation is:

[0083]

[0084] represents the projection of the Earth's rotational angular velocity in the w system, express The antisymmetric matrix of . represents the projection of the gravity vector in the w system, express The antisymmetric matrix, I3 represents the 3×3 identity matrix;

[0085] Furthermore, the present invention proposes a new noise transfer matrix G b The specific representation is:

[0086]

[0087] System noise w b for:

[0088] w b = [w g w a w wg w wa w φ-bc w δp-bc ] T

[0089] where w g and w a are the measurement white noise of the gyroscope and accelerometer, w wg and w wa are the white noise driven processes of the gyroscope and accelerometer bias, w φ-bc and w δp-bc are the relative attitude angle noise and relative displacement noise between the IMU and the camera.

[0090] In an embodiment, the state and covariance propagation model related to the camera pose error state based on the velocity error and position error dual state transformation is constructed, including:

[0091] At time t, the state vector x c related to the camera is expressed as:

[0092]

[0093] where the quaternion q represents the rotation of the camera in the w system corresponding to the 1st frame to the Nth frame in which the feature point is tracked, represents the position coordinates of the camera in the w system corresponding to the 1st frame to the Nth frame in which the feature point is tracked, and the subscripts 1 to N represent the frame number in which the feature point is tracked;

[0094] The error state vector δx c related to the camera is expressed as:

[0095]

[0096] where represents the attitude angle error of the camera in the w system corresponding to the 1st frame to the Nth frame in which the feature point is tracked, represents the position coordinate error of the camera in the w system corresponding to the 1st frame to the Nth frame in which the feature point is tracked, and the subscripts 1 to N represent the frame number in which the feature point is tracked;

[0097] After obtaining the key frame of the camera with translational motion or with translational and rotational motion each time, the camera pose state corresponding to the new key frame needs to be added to the state vector related to the camera, and the state covariance matrix is expanded. Since the new camera pose is related to the current IMU state, the new camera error state is expressed as:

[0098]

[0099] in is the camera attitude angle error vector corresponding to the newly added N+1th key frame, is the vector of the camera position error corresponding to the newly added N+1th key frame in the world reference coordinate system; J represents the Jacobian matrix between the new camera error state and the error state vector before amplification.

[0100] The present invention proposes a new representation of the Jacobian matrix J, which is specifically as follows:

[0101]

[0102] The expanded state covariance matrix Expressed as:

[0103]

[0104] Among them, P k is the covariance matrix before amplification, I 6N+15 It is the identity matrix of 6N plus 15 dimensions;

[0105]

[0106] in is the covariance of the newly added camera-related state error and all state errors before the increase, is the variance of the newly added camera-related state error.

[0107] In the observation equation, the reprojection error Jacobian matrix relative to the error state vector δx It is expressed as follows:

[0108]

[0109] and They represent the camera attitude angle error and position error in the ii-th frame respectively. Assuming that the feature point l is tracked for a total of n frames, then ii satisfies 1≤ii≤n.

[0110] in, and

[0111] Expressed as:

[0112]

[0113] A, B, C, and D are as follows:

[0114]

[0115]

[0116] j, k represent the left and right reference frames, ii satisfies 1≤ii≤n. ii=others:0 represents when ii≠k, A1=0

[0117]

[0118] i=j,ii=any:0 means that when i=j, no matter what ii is, B is equal to 0.

[0119]

[0120]

[0121] It is expressed as follows:

[0122]

[0123]

[0124]

[0125]

[0126] The present invention introduces an observation model based on pure pose constraints, which decouples the observation model of the system from the three-dimensional feature points, thus avoiding the influence of inaccurate three-dimensional reconstruction on navigation accuracy. b , noise transfer matrix G b , the Jacobian matrix J, which enables the present invention to achieve better consistency and observability in the filtering process. Furthermore, the present invention adopts a dual-state transform Kalman filter (DST-EKF), which achieves more precise definitions of velocity error and position error compared to existing Kalman filtering methods (including but not limited to extended Kalman filter EKF), with better consistency and observability.

[0127] The reprojection error of the traditional MSCKF is related to the 3D feature position, but the state space does not contain the feature point position. Therefore, in the observation model, the reprojection error is first linearized using the system error state and the feature point position error. Then, the feature point position error is eliminated using null space mapping to restore the standard EKF format. In the observation model based on pure pose constraints proposed in this paper, the reprojection error can be directly linearized using the system error state, thus eliminating the 3D reconstruction process and null space mapping.

[0128] When visual information is used as an observation, there is no fixed filtering cycle. Instead, a certain strategy is adopted for filtering and updating. The filtering update is triggered in two situations. The first situation is when the tracking of a feature point ends. When a feature point disappears from the field of view, the tracking process of the feature point ends, and the camera pose containing the feature point is filtered and updated. The second situation is when the filtering state in the state vector reaches the upper limit. Whenever a new keyframe is generated, a new camera pose will be added to the state vector. If the life cycle of some feature points is very long, the dimension of the state vector will be very large. In order to improve computational efficiency and avoid excessive error accumulation caused by long-term non-updates, a maximum threshold is set for the camera state dimension in the state vector. When this threshold is reached, some camera states are deleted. At the same time, these states are filtered and updated using the existing observation information.

[0129] To verify the effectiveness of the proposed method, we compared the performance of the proposed integrated visual and inertial odometry navigation method (POMSCKF) with the traditional inertial / visual tight integration navigation method using a multi-state constrained Kalman filter (MSCKF) using inertial navigation and visual data from two UAV flight experiments. The IMU output frequency was 200Hz, and the image frame rate was 10Hz.

[0130] Figure 2 This is the experimental result of the first UAV flight experiment. Figure 2 (a) is a flight trajectory comparison diagram, Figure 2 (b) is a comparison chart of horizontal position error results; Figure 3 This is the experimental result diagram of the second UAV flight experiment. Figure 3 (a) is a flight trajectory comparison diagram, Figure 3 (b) is a comparison chart of horizontal position error results. Figure 2 and Figure 3 As shown in the figure, it can be seen that the visual and inertial odometry integrated navigation method (POMSCKF) proposed in the present invention has higher position accuracy than the traditional multi-state constrained Kalman filter (MSCKF) inertial / visual tight integrated navigation method.

[0131] One embodiment provides a visual and inertial odometry combined navigation device, comprising:

[0132] The first module is used to perform strapdown inertial navigation solution based on the motion information received by the inertial measurement unit in the visual / inertial odometry integrated navigation system to obtain the attitude, speed and position information of the carrier;

[0133] The second module is used to extract feature points based on image information synchronously collected by the camera in the visual / inertial odometry integrated navigation system and track the extracted feature points;

[0134] a third module configured to determine a state vector of the visual / inertial odometry integrated navigation, including a state vector related to the inertial measurement unit and a state vector related to the camera;

[0135] a fourth module configured to construct a system model of the visual / inertial odometry integrated navigation, including constructing a system model related to an inertial navigation error state based on a double state transformation of a velocity error and a position error and constructing a state and covariance propagation model related to a camera pose error state based on a double state transformation of a velocity error and a position error;

[0136] a fifth module configured to construct an observation model of the visual / inertial odometry integrated navigation system based on a pure pose constraint, taking a re-projection error of a feature point in an image represented by a co-linearization of a system error state and a feature point position error as an observation, including: in a multi-view geometry description based on the pure pose constraint, assuming that a series of extracted feature points are observed in n frames of images, the lth feature point f l exists 1≤i,j,k≤n, defining a three-dimensional coordinate in a camera coordinate system corresponding to the ith frame of image as is represented as:

[0137]

[0138] wherein, represents a relative displacement relationship between the jth frame of image and the kth frame of image, represents an anti-symmetric matrix of represents a relative displacement relationship between the jth frame of image and the ith frame of image, represents a relative rotation relationship between the jth frame of image and the ith frame of image, represents a relative rotation relationship between the jth frame of image and the kth frame of image, and respectively represent two-dimensional coordinates of the feature point in the jth frame of image and the kth frame of image;

[0139] a re-projection error of the lth feature point in the ith frame of image is:

[0140]

[0141] wherein is an estimated value of a two-dimensional coordinate of the lth feature point in the ith frame of image obtained from the pure pose constraint, is a two-dimensional coordinate of the lth feature point in the ith frame of image obtained directly from feature extraction;

[0142] linearizing the re-projection error with the system error state, an observation model is obtained as:

[0143]

[0144] wherein is the re-projection error with respect to the Jacobian matrix of the error state vector δx; δx represents an error state vector corresponding to the state vector x, the state vector x of the visual / inertial odometry combined navigation x = [x b x c ] T , x b is the state vector related to the IMU, x c is the state vector related to the camera; is the corresponding observation noise;

[0145] The sixth module is configured to complete Kalman filtering according to the system model and the observation model of the visual / inertial odometry combined navigation system, and output the filtered attitude, velocity and position information of the carrier.

[0146] The implementation method of each module and the construction of the model can adopt the method described in any of the foregoing embodiments, and thus will not be described herein.

[0147] In another aspect, the present application provides a computer device, comprising a memory and a processor, the memory stores a computer program, and the processor implements the steps of the visual / inertial odometry combined navigation method provided in any of the foregoing embodiments when executing the computer program. The computer device can be a server. The computer device comprises a processor, a memory, a network interface and a database connected through a system bus. The processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device comprises a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operating system and the computer program in the non-volatile storage medium. The database of the computer device is configured to store sample data. The network interface of the computer device is configured to communicate with an external terminal through a network connection.

[0148] In another aspect, the present application provides a computer readable storage medium, which stores a computer program, and the computer program implements the steps of the visual / inertial odometry combined navigation method provided in any of the foregoing embodiments when executed by a processor.

[0149] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0150] Matters not covered by the present invention are known technologies.

[0151] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0152] The above-described embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art could make various modifications and improvements without departing from the spirit of the present application, all of which fall within the scope of protection of the present application. Therefore, the scope of protection of the present patent application shall be determined by the appended claims.

[0153] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that the present invention is susceptible to various modifications and variations. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A visual and inertial odometry combined navigation method, characterized in that: include: The strapdown inertial navigation solution is performed based on the motion information received by the inertial measurement unit in the visual / inertial odometry integrated navigation system to obtain the attitude, velocity and position information of the carrier; Extract feature points based on the image information synchronously collected by the camera in the visual / inertial odometry integrated navigation system and track the extracted feature points; Determine the state vector of the visual / inertial odometry integrated navigation, including the state vector associated with the inertial measurement unit and the state vector associated with the camera; Construct a system model for visual / inertial odometry integrated navigation, including a system model related to the inertial navigation error state based on a dual-state transformation of velocity error and position error, and a state and covariance transfer model related to the camera pose error state based on a dual-state transformation of velocity error and position error; The reprojection error of the feature points in the image linearized by the system error state and the feature point position error is used as the observation quantity. The observation model of the visual / inertial odometry integrated navigation system is constructed based on pure pose constraints, including: in the multi-view geometric description based on pure pose constraints, suppose that among the series of extracted feature points, the lth feature point f l A total of n frames are observed, then there exists 1≤i,j,k≤n, and the three-dimensional coordinates in the camera coordinate system corresponding to the i-th frame image are defined as Expressed as: in, Represents the relative displacement relationship between the j-th frame image and the k-th frame image, express The antisymmetric matrix of Represents the relative displacement relationship between the j-th frame image and the i-th frame image, Indicates the relative rotation relationship between the j-th frame image and the i-th frame image, Indicates the relative rotation relationship between the j-th frame image and the k-th frame image, and Represent the two-dimensional coordinates of the feature points in the j-th frame and the k-th frame image respectively; The reprojection error of the lth feature point in the i-th frame image for: in is the estimated value of the two-dimensional coordinate of the lth feature point in the i-th frame image obtained by pure pose constraint, is the two-dimensional coordinate of the lth feature point in the i-th frame image obtained directly by feature extraction; The reprojection error is linearized using the system error state, and the observation model is obtained as follows: in is the reprojection error Relative to the Jacobian matrix of the error state vector δx; δx represents the error state vector corresponding to the state vector x, the state vector x of the visual / inertial odometry integrated navigation is [x b x c ] T , x b is the state vector associated with the inertial measurement unit, x c is the state vector associated with the camera, is the corresponding observation noise; Kalman filtering is completed based on the system model and observation model of the visual / inertial odometry integrated navigation system, and the attitude, speed, and position information of the filtered carrier are output.

2. The visual and inertial odometry integrated navigation method according to claim 1, characterized in that: The motion information received by the inertial measurement unit includes angular increment or angular velocity information of the three-axis gyroscope and specific force or specific force integral increment information of the three-axis accelerometer.

3. The visual and inertial odometry integrated navigation method according to claim 1, characterized in that: The FAST corner points are extracted from the image information collected synchronously by the camera as feature points, and the KLT optical flow tracking method is used to track the extracted feature points.

4. The visual and inertial odometry integrated navigation method according to claim 1, 2 or 3, wherein: A system model related to the inertial guidance error state is constructed based on the dual-state transformation of velocity error and position error as follows: Among them F b Expressed as a system matrix, δx b Indicates that x b The corresponding error state vector, G b is the noise transfer matrix, w b is the system noise, x b is the state vector associated with the inertial measurement unit; The inertial navigation system coordinate system is the b system, the world coordinate system is the w system, and the camera coordinate system is the c system; At time t, the error state vector associated with the inertial measurement unit is expressed as: in, is the Hamilton unit quaternion, which represents the rotation from the b system to the w system at time t. is the projection of the velocity of the inertial measurement unit relative to the w system in the w system, is the projection of the position of the inertial measurement unit relative to the w system in the w system, b g and b a They are gyroscope bias and accelerometer bias, represents the rotation from the c system to the b system, Indicates the positional relationship between system c and system b, and Together they represent the relative pose between the inertial navigation system and the camera; Where φ b w 、 δb g , δb a 、 They represent attitude error, velocity error after state transformation, position error after state transformation, gyroscope bias error, accelerometer bias error, relative attitude error between inertial navigation and camera, and relative position error between inertial navigation and camera.

5. The visual and inertial odometry integrated navigation method according to claim 4, characterized in that: The speed error after state transformation is: in yes The estimated value of represent The antisymmetric matrix of Represents the speed error state before state change, Represents the rotation matrix from b system to w system, Represents the estimated value of the rotation matrix from the w system to the b system; The position error after state transformation is: in yes The estimated value of represent The antisymmetric matrix of Represents the speed error state before the state change.

6. The visual and inertial odometry integrated navigation method according to claim 4, characterized in that: System matrix F b Expressed as: in represents the projection of the Earth's rotational angular velocity in the w system, express The antisymmetric matrix of represents the projection of the gravity vector in the w system, express The antisymmetric matrix, I3 represents the 3×3 identity matrix; Noise transfer matrix G b , expressed as: System noise w b for: In b =[in g In a In wg In wa In φ-bc In δp-bc ] T where w g and w a are the measurement white noise corresponding to the gyroscope and accelerometer, w wg and w wa are the white noise driving processes corresponding to the zero bias of the gyroscope and accelerometer, respectively, w φ-bc and w δp-bc are the relative attitude angle noise and relative displacement noise between the inertial measurement unit and the camera, respectively.

7. The visual and inertial odometry integrated navigation method according to claim 4, characterized in that: Construct a state and covariance transfer model related to the camera pose error state based on the dual-state transformation of velocity error and position error, including: At time t, the state vector x associated with the camera c , expressed as: Among them, quaternions Indicates the rotation from the c-system to the w-system corresponding to the 1st frame...Nth frame where the feature point is tracked. Indicates the position coordinates of the camera in the w system corresponding to the 1st frame...Nth frame where the feature point is tracked. The subscripts 1 to N indicate the number of frames where the feature point is tracked. The error state vector δx associated with the camera c , expressed as: in Represents the camera's attitude angle error in the w frame corresponding to the 1st frame...Nth frame where the feature point is tracked, Represents the position coordinate error of the camera in the w frame corresponding to the 1st frame...Nth frame where the feature point is tracked. The subscripts 1 to N represent the number of frames where the feature point is tracked. Each time a key frame with translation or translation and translation motion is obtained, the camera pose state corresponding to the new key frame needs to be added to the state vector related to the camera, and the state covariance matrix needs to be expanded. Since the new camera pose is related to the current IMU state, the new camera error state is expressed as: in is the camera attitude angle error vector corresponding to the newly added N+1th key frame, is the vector of the camera position error corresponding to the newly added N+1th key frame in the world reference coordinate system; J represents the Jacobian matrix between the new camera error state and the error state vector before amplification, in the following form: The expanded state covariance matrix Expressed as: Among them, P k is the covariance matrix before amplification, I 6N+15 It is the identity matrix of 6N plus 15 dimensions; in is the covariance of the newly added camera-related state error and all state errors before the increase, is the variance of the newly added camera-related state error.

8. A combined navigation device based on vision and inertial odometer, characterized in that: include: The first module is used to perform strapdown inertial navigation solution based on the motion information received by the IMU in the visual / inertial odometry integrated navigation system to obtain the attitude, speed and position information of the carrier; The second module is used to extract feature points based on image information synchronously collected by the camera in the visual / inertial odometry integrated navigation system and track the extracted feature points; The third module is used to determine the state vector of the visual / inertial odometry integrated navigation, including the state vector related to the IMU and the state vector related to the camera; The fourth module is used to build a system model for visual / inertial odometry integrated navigation, including building a system model related to the inertial navigation error state based on the dual-state transformation of velocity error and position error, and building a state and covariance transfer model related to the camera pose error state based on the dual-state transformation of velocity error and position error; The fifth module is used to use the reprojection error of the feature points in the image linearized by the system error state and the feature point position error as the observation quantity, and to construct the observation model of the visual / inertial odometry integrated navigation system based on pure pose constraints, including: in the multi-view geometry description based on pure pose constraints, suppose that among the series of feature points extracted, the lth feature point is observed in a total of n frames of images, then there exists 1≤i,j,k≤n, and the three-dimensional coordinates in the camera coordinate system corresponding to the i-th frame image are defined as Expressed as: in, Represents the relative displacement relationship between the j-th frame image and the k-th frame image, express The antisymmetric matrix of Represents the relative displacement relationship between the j-th frame image and the i-th frame image, Indicates the relative rotation relationship between the j-th frame image and the i-th frame image, Indicates the relative rotation relationship between the j-th frame image and the k-th frame image, and Represent the two-dimensional coordinates of the feature points in the j-th frame and the k-th frame image respectively; The reprojection error of the lth feature point in the i-th frame image for: in is the estimated value of the two-dimensional coordinate of the lth feature point in the i-th frame image obtained by pure pose constraint, is the two-dimensional coordinate of the lth feature point in the i-th frame image obtained directly by feature extraction; The reprojection error is linearized using the system error state, and the observation model is obtained as follows: in is the reprojection error Relative to the Jacobian matrix of the error state vector δx; δx represents the error state vector corresponding to the state vector x, the state vector x of the visual / inertial odometry integrated navigation is [x b x c ] T , x b is the state vector associated with the IMU, x c is the state vector associated with the camera, is the corresponding observation noise; The sixth module is used to complete Kalman filtering based on the system model and observation model of the visual / inertial odometry integrated navigation system, and output the attitude, speed, and position information of the filtered carrier.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the visual and inertial odometry integrated navigation method as claimed in claim 1 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the visual and inertial odometry integrated navigation method as claimed in claim 1 are implemented.

Images