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

Abstract

The invention provides a visual and inertial odometer integrated navigation method, device and equipment and a medium. The method comprises the following steps: determining a state vector of visual / inertial odometer integrated navigation; constructing a visual / inertial odometer integrated navigation system model; constructing an observation model of a vision / inertial odometer integrated navigation system based on pure pose constraint; kalman filtering is completed according to the system model and the observation model of the visual / inertial odometer integrated navigation system, attitude, speed and position information of a filtered carrier is output, and the visual / inertial odometer integrated navigation filtering observation model is deduced again through a pure pose constraint theory. The new observation model does not contain three-dimensional feature information any more, so that null-space mapping and three-dimensional reconstruction processes are not needed any more, the problem of model inconsistency existing in multi-state constraint Kalman filtering and the influence of errors introduced by inaccurate three-dimensional reconstruction are avoided, and the integrated navigation precision is improved.

Description

Technical Field

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

[0002] The problem of visual / inertial odometry combined 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 the visual / inertial odometry combined navigation schemes, the scheme using an inertial measurement unit (IMU) and a monocular camera has attracted much attention due to its small size and low cost. However, there is a problem of uncertain measurement scale of the monocular camera. 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 combined navigation system loses visual information, the inertial navigation system can still work with high precision for a short time.

[0003] For the integrated navigation method of vision and inertial odometer, it can be divided into loose combination and tight combination according to the fusion method. The loose combination method processes the inertial navigation information and visual information separately. The measurement information of the inertial measurement unit (IMU) is used for state transfer, and the pose calculated by the visual information is used for updating. This method does not utilize the auxiliary effect of the inertial measurement unit (IMU) on vision and cannot correct the drift introduced by the visual pose. The tight combination method uses the original information of inertia and vision to optimize all the measurement values ​​of IMU and camera, and then implements it based on filtering or nonlinear optimization. Since the measurement values ​​of IMU and camera will affect each other, the tight combination has better robustness and accuracy than the loose combination. It is generally believed that the tight combination method based on nonlinear optimization is more accurate than the tight combination method based on filtering because more measurement information is used in each iteration cycle. However, the iterative solution of nonlinear equations requires a lot of computing resources, so it is difficult to achieve real-time computing on resource-constrained platforms. In contrast, the method based on extended Kalman filter has better computational efficiency and is easy to implement on small mobile devices.

[0004] Multi-state constrained Kalman filter (MSCKF) is one of the most classic filter-based visual inertial odometry methods. Studies have shown that MSCKF has high computational efficiency and good robustness, and can maintain consistent accuracy on different hardware platforms. However, at the same time, MSCKF is susceptible to linearization errors, which accumulate over time and degrade performance.

[0005] An important source of linearization error lies in the limitations of traditional multi-view geometry description. The observation model of MSCKF is related to the three-dimensional feature position, but in order to keep the state space simple, the system state of MSCKF does not contain the feature point position. Therefore, when linearizing the reprojection error, the system error state and the feature point error can only be expressed together, and then the null space mapping method is applied to restore it to the standard EKF format, which reduces the consistency of the model. At the same time, the three-dimensional feature position used by MSCKF comes from a simple nonlinear optimization process. The specific method is to first restore the three-dimensional feature position by triangulation, and then optimize the position using the Gauss-Newton method. During the optimization process, the relative position of the camera is considered to be known, so the optimization process is not rigorous and accurate enough, and it is easy to introduce linearization errors to the subsequent filtering estimation process.

[0006] Therefore, how to maintain the advantages of high efficiency and robustness of 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 the traditional visual-inertial odometry integrated navigation method based on multi-state constrained Kalman filter (MSCKF), the present invention proposes a visual-inertial odometry integrated navigation method, device, equipment and medium.

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

[0009] In one aspect, the present invention provides a visual and inertial odometer 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 odometer integrated navigation system to obtain the attitude, speed and position information of the carrier;

[0011] Extract feature points based on the image information synchronously collected by the camera in the visual / inertial odometer 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 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;

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

[0015] Kalman filtering is completed according to the system model and observation model of the visual / inertial odometer 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 odometer 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 odometer 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 odometer 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 combined 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 construct a system model of visual / inertial odometer integrated navigation, including 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 posture error state based on a dual-state transformation of velocity error and position error;

[0021] The fifth module is used to construct an 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 according to the system model and observation model of the visual / inertial odometer combined 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 odometer 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 odometer combined navigation method.

[0026] The observation model in the filtering method involved in the present invention is not limited to being used in combination with a system model based on a dual-state transformation Kalman filter, but 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 odometer, but can be extended to the combined navigation system of binocular vision / inertial odometer, 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 odometer 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 specific 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. It no longer requires zero 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, thereby 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 has a small amount of calculation. The filtering process of the present invention is completely consistent with the standard extended Kalman filtering process, is simple to implement, and is easier to understand.

[0034] The visual and inertial odometer integrated navigation method proposed in the present invention has strong engineering application flexibility and applicability. Since the amount of calculation is relatively small, sufficient space can be left for adding additional functions to the integrated navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying creative work.

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

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

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

[0039] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0040] A visual / inertial odometer integrated navigation system is installed on the carrier. The visual / inertial odometer integrated navigation system includes an inertial measurement unit (IMU) and a camera. The IMU receives motion information of the carrier, and the camera synchronously collects image information. Strapdown inertial navigation is performed based on the motion information received by the IMU to obtain the attitude, speed and position information of the carrier; feature points are extracted from the image information synchronously collected by the camera and the extracted feature points are tracked.

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

[0042] The motion information of the carrier received by the IMU in the present invention includes: the angular increment or angular velocity information of the three-axis gyroscope and the specific force or specific force integral increment information of the three-axis accelerometer, and these data are used to complete the inference of inertial navigation. The image information collected by the monocular camera is a two-dimensional image collected synchronously with the IMU. According to the two-dimensional image, the extraction of FAST corner points and the storage of corner point pixel coordinates are completed, and the extracted corner points are tracked using the KLT optical flow tracking algorithm.

[0043] The first frame image is used to extract the key feature points of the image as the initial feature points for tracking. FAST (Features from Accelerated Segment Test) corner points have been widely used in the field of visual navigation due to their advantages such as small calculation amount and strong real-time performance. Therefore, the present invention selects FAST corner points as the initial feature points.

[0044] Before the visual and inertial odometer combined navigation filtering is officially carried out, the visual image needs to complete the task of tracking and matching feature points, removing abnormal feature points, and extracting new feature points. The KLT (KanadeLucasTomasi) tracking method is selected. This method is based on optical flow tracking and has good adaptability to feature deformation. In addition, after tracking the feature points, new feature points are detected in the image area outside the tracking points to make up for the number of feature points in the tracking process.

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

[0046] World reference coordinate system W: The world reference coordinate system is the local tangent plane coordinate system, which is fixed to the earth and coincides with the local geographic coordinate system of the point. In order to ensure the seamless operation of the algorithm, the position and posture of the IMU and camera, as well as the feature points in the scene, are expressed in the 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. When installing, the direction of the coordinate axis should be consistent with the vehicle coordinate system as much as possible to make the installation angle smaller.

[0048] Camera coordinate system c: This coordinate system is fixed to the camera, with the origin of the coordinate system 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 the Y-axis according to the right-hand rule.

[0049] In traditional visual / inertial odometry, the position of feature points is usually added to the state vector, and the position of feature points and the carrier pose are continuously optimized through new observations until the feature points disappear. However, unlike the simultaneous localization and mapping (SLAM) system, the inertial / visual odometry does not need to obtain the environment map, so adding feature points to the state vector for optimization will complicate the calculation process. The multi-state constrained Kalman filter (MSCKF) algorithm has made improvements in this regard. The state vector no longer contains the position of feature points, but is replaced by the camera pose at the past moment. The state vector is in the form of a sliding window, and new camera poses are continuously added and old camera poses are continuously discarded.

[0050] Reference Figure 1 In one embodiment, a visual and inertial odometer 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 odometer integrated navigation system to obtain the attitude, speed and position information of the carrier;

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

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

[0054] Wherein: the state vector of the visual / inertial odometer integrated navigation includes a state vector associated with the inertial measurement unit and a state vector associated with the camera;

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

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

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

[0058] Kalman filtering is completed according to the system model and observation model of the visual / inertial odometer 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 visual / inertial odometry integrated navigation filter observation model through pure pose constraint theory. The new observation model no longer contains three-dimensional feature information, so it no longer needs null space mapping and three-dimensional reconstruction process, thereby avoiding the model inconsistency problem and the influence of inaccurate three-dimensional reconstruction introduced by the multi-state constrained Kalman filter (MSCKF) and improving the accuracy of integrated navigation. Specifically, the observation model of the visual / inertial odometry integrated navigation system is constructed based on pure pose constraint, including: in the multi-view geometric description based on pure pose constraint, in a 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 It is 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, represents the relative rotation relationship between the j-th frame image and the i-th frame image, represents the relative rotation relationship between the j-th frame image and the k-th frame image, and Respectively represent the two-dimensional coordinates of the feature points in the j-th frame and the k-th frame image;

[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 odometer 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 based on the dual-state transformation of the velocity error and the position error is constructed as follows:

[0069]

[0070] Among them, F b Expressed as a system matrix, δx b Represents 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 the gyroscope bias and the accelerometer bias, represents the rotation from the c system to the b system, Indicates the positional relationship between the c system and the b system, and They jointly represent the relative position and posture 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 to w, Represents the estimated value of the rotation matrix from w to b. 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 rotation 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 corresponding to the gyroscope and accelerometer, respectively, wg and w wa are the white noise driving processes corresponding to the zero bias of the gyroscope and accelerometer, respectively, φ-bc and w δp-bc They are the relative attitude angle noise and relative displacement noise between IMU and camera respectively.

[0090] In one embodiment, 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 is constructed, including:

[0091] At time t, the state vector x associated with the camera c , expressed as:

[0092]

[0093] 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.

[0094] The error state vector δx associated with the camera c , expressed as:

[0095]

[0096] in Represents the angle error of the camera in the w system corresponding to the 1st frame...Nth frame where the feature point is tracked, Represents the position coordinate error 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 represent the number of frames where the feature point is tracked.

[0097] After obtaining a key frame with translation or translation and translation motion of the camera each time, it is necessary to add the camera pose state corresponding to the new key frame to the state vector related to the camera, and expand the state covariance matrix. 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 form of the Jacobian matrix J, the specific form of which is as follows:

[0101]

[0102] The augmented state covariance matrix It is expressed as:

[0103]

[0104] Where P k is the covariance matrix before augmentation, 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 addition, is the variance of the newly added camera-related state error.

[0107] In the observation equation, the reprojection error The Jacobian matrix with respect 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] It is expressed as:

[0112]

[0113] A, B, C, and D are represented 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 posture constraints, so that the observation model of the system is decoupled from the three-dimensional feature points, avoiding the influence of inaccurate three-dimensional reconstruction on navigation accuracy. b , the noise transfer matrix G b , Jacobian matrix J, so that the present invention obtains better consistency and observability in the filtering process. Furthermore, the present invention adopts a dual state transformation Kalman filter (DST-EKF), which obtains more accurate definitions of velocity error and position error compared to existing Kalman filtering methods (including but not limited to extended Kalman filter EKF), and has better consistency and observability.

[0127] The reprojection error of the traditional MSCKF is related to the three-dimensional feature position, but the state space does not contain the feature point position. Therefore, in the observation model, the reprojection error is first linearized by the system error state and the feature point position error, and then the feature point position error is eliminated by the null space mapping to restore the standard EKF format. In the observation model based on pure pose constraints proposed in the present invention, the reprojection error can be directly linearized by the system error state, thereby eliminating the three-dimensional 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 will be triggered in two cases. The first case is that the tracking of a certain 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 case is that the filtering state in the state vector reaches the upper limit. Whenever a new key frame 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 the computational efficiency and avoid excessive error accumulation caused by long-term non-update, 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] In order to verify the effectiveness of the method provided by the present invention, the inertial navigation and visual data of two UAV flight experiments are used as examples to compare the performance of the visual and inertial odometer integrated navigation method (POMSCKF) provided by the present invention with the traditional inertial / visual tight integrated navigation method of multi-state constrained Kalman filter (MSCKF). The IMU output frequency is 200Hz and the image frame rate is 10Hz.

[0130] Figure 2 This is the experimental result of the first UAV flight experiment. Figure 2 (a) is a flight trajectory comparison chart. 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 chart. Figure 3 (b) is a comparison chart of horizontal position error results. Figure 2 and Figure 3 As shown, it can be seen that the vision and inertial odometer 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 odometer 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 odometer 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 odometer integrated navigation system and track the extracted feature points;

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

[0135] The fourth module is used to construct a system model of visual / inertial odometer integrated navigation, including 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 posture error state based on a dual-state transformation of velocity error and position error;

[0136] The fifth module is used to construct an observation model of the visual / inertial odometer integrated navigation system based on pure pose constraints, using the reprojection error of the feature points in the image linearly represented by the system error state and the feature point position error as the observation quantity, including: in the multi-view geometric description based on pure pose constraints, suppose that among the series of feature points extracted, 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 It is expressed as:

[0137]

[0138] 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, represents the relative rotation relationship between the j-th frame image and the i-th frame image, represents the relative rotation relationship between the j-th frame image and the k-th frame image, and Respectively represent the two-dimensional coordinates of the feature points in the j-th frame and the k-th frame image;

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

[0140]

[0141] 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;

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

[0143]

[0144] 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 odometer 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;

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

[0146] The implementation methods of the above modules and the construction of the model can adopt the methods described in any of the above embodiments, which will not be repeated here.

[0147] 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 when the processor executes the computer program, the steps of the visual and inertial odometer combined navigation method provided in any of the above embodiments are implemented. The computer device may be a server. The computer device includes a processor, a memory, a network interface, and a database connected via a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes 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 operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store sample data. The network interface of the computer device is used to communicate with an external terminal via a network connection.

[0148] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the visual and inertial odometer combined navigation method provided in any of the above embodiments.

[0149] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and 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 the present 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. As an illustration and not limitation, RAM is available in many 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 may 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-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

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

Claims

1. A visual and inertial odometer 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 odometer integrated navigation system to obtain the attitude, speed and position information of the carrier; Extract feature points based on the image information synchronously collected by the camera in the visual / inertial odometer 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 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; The reprojection error of the feature points in the image linearly represented by the system error state and the feature point position error is used as the observation quantity, and the observation model of the visual / inertial odometer 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 It is 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, represents the relative rotation relationship between the j-th frame image and the i-th frame image, represents the relative rotation relationship between the j-th frame image and the k-th frame image, and Respectively represent the two-dimensional coordinates of the feature points in the j-th frame and the k-th frame image; 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 odometer 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 according to the system model and observation model of the visual / inertial odometer integrated navigation system, and the attitude, speed and position information of the filtered carrier are output.

2. The visual and inertial odometer 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 a three-axis gyroscope and specific force or specific force integrated increment information of a three-axis accelerometer.

3. The visual and inertial odometer integrated navigation method according to claim 1, characterized in that: The FAST corner points are extracted from the image information synchronously collected 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 odometer integrated navigation method according to claim 1, 2 or 3, characterized in that: The 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: where F b Expressed as a system matrix, δx b Represents 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 the gyroscope bias and the accelerometer bias, represents the rotation from the c system to the b system, Indicates the positional relationship between the c system and the b system, and Together they represent the relative position and posture between the inertial navigation system and the camera; Where φ b w , δb g ,δb a , They respectively 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 odometer integrated navigation method according to claim 4, characterized in that: The speed error after state change is: in yes The estimated value of represent The antisymmetric matrix of Represents the speed error state before the state change, Represents the rotation matrix from b to w, 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 odometer integrated navigation method according to claim 4, characterized in that: System Matrix F b It is expressed as: in represents the projection of the Earth's rotation 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, respectively, 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 They are the relative attitude angle noise and relative displacement noise between the inertial measurement unit and the camera, respectively.

7. The visual and inertial odometer 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 angle error of the camera in the w system corresponding to the 1st frame...Nth frame where the feature point is tracked, Represents the position coordinate error 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 represent the number of frames where the feature point is tracked. After obtaining a key frame with translation or translation and translation motion of the camera each time, it is necessary to add the camera pose state corresponding to the new key frame to the state vector related to the camera, and expand the state covariance matrix. 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 augmented state covariance matrix It is expressed as: Where P k is the covariance matrix before augmentation, 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 addition, is the variance of the newly added camera-related state error.

8. A visual and inertial odometer combined navigation device, 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 odometer 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 odometer integrated navigation system and track the extracted feature points; The third module is used to determine the state vector of the visual / inertial odometer combined navigation, including the state vector related to the IMU and the state vector related to the camera; The fourth module is used to construct a system model of visual / inertial odometer integrated navigation, including 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 posture error state based on a 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 linearly represented 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 odometer integrated navigation system based on pure posture constraints, including: in the multi-view geometry description based on pure posture 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 of image are defined as It is 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, represents the relative rotation relationship between the j-th frame image and the i-th frame image, represents the relative rotation relationship between the j-th frame image and the k-th frame image, and Respectively represent the two-dimensional coordinates of the feature points in the j-th frame and the k-th frame image; 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 odometer 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 according to the system model and observation model of the visual / inertial odometer combined 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 odometer 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 odometer integrated navigation method as claimed in claim 1 are implemented.

Images