Visual inertial navigation fusion pose estimation method and device based on composite manifold optimization

Abstract

The invention discloses a visual inertial navigation fusion pose estimation method and device based on composite manifold optimization. The method comprises the following steps: acquiring real-time image data through an image acquisition module of to-be-estimated equipment and acquiring inertial measurement data through a sensing module of the to-be-estimated equipment; calculating image observation data according to the real-time image data based on an observation projection model corresponding to a preset image acquisition module; calculating inertial navigation pre-integration data according to the inertial measurement data based on a pre-established inertial navigation measurement and dynamic model; the method is based on a pre-established composite full-decoupling manifold lt; sO (3), R3, R3gt; and the optimized pose estimation framework calculates pose data of the to-be-estimated equipment according to the image observation data and the inertial navigation pre-integration data. Therefore, by implementing the method, the pose of the to-be-estimated equipment can be subjected to composite manifold optimization, the calculation efficiency of a visual inertial navigation fusion optimization algorithm is improved, and the consistency of inertial navigation pre-integration is kept.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a method and device for visual inertial navigation fusion pose estimation based on composite manifold optimization. Background Art

[0002] Environmental perception and self-motion perception play a vital role in the navigation process. The robot's motion perception relies on navigation and positioning technologies. These technologies include inertial estimation methods (such as inertial sensors, pedometers, wheel encoders, magnetometers, and gyroscopes) and relative positioning methods (such as global navigation satellite system (GNSS), ultra-wideband (UWB), acoustic ranging, lidar, and cameras). In the field of robot navigation, visual inertial navigation can provide the robot's current state and local map, which is essential for path planning, obstacle avoidance, and real-time control. As a widely used navigation technology, visual inertial navigation has made significant contributions to guidance, navigation, and control systems (GNC). In particular, since visual inertial navigation relies on the use of onboard sensors (cameras and inertial navigation), this perception mode is particularly important in the absence of external positioning methods such as GPS, ultra-wideband (UWB), and visual motion capture systems. Therefore, visual inertial navigation is widely used in indoor positioning (AR and XR), underwater detection, urban reconstruction, and search and rescue missions.

[0003] The current method for obtaining the real-time posture state of robots mainly adopts <SO(3)×R 3 ,R 3 > The velocity-decoupled chained rotation-translation manifold can only achieve real-time state estimation with a short fixed lag time window and fewer feature points Summary of the invention

[0004] The present invention provides a visual inertial navigation fusion pose estimation method and device based on composite manifold optimization, which can achieve longer time window optimization under limited computing resources and reserve more sufficient processing time for the visual front end, thereby improving the computational efficiency of the visual inertial navigation fusion optimization algorithm and maintaining the consistency of inertial navigation pre-integration.

[0005] In order to solve the above technical problems, the first aspect of the present invention discloses a visual inertial navigation fusion pose estimation method based on composite manifold optimization, the method comprising:

[0006] Acquiring real-time image data through an image acquisition module of the device to be estimated and acquiring inertial measurement data through a sensor module of the device to be estimated;

[0007] Based on the preset observation projection model corresponding to the image acquisition module, the image observation data is calculated according to the real-time image data;

[0008] Based on the pre-established inertial navigation measurement and dynamic model, the inertial navigation pre-integrated data is calculated according to the inertial measurement data;

[0009] Based on pre-built composite full decoupling <SO(3),R 3 ,R 3 >A manifold-optimized pose estimation framework that calculates the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data.

[0010] As an optional implementation, the observation projection model is established by the following steps:

[0011] Representing the surrounding spatial environment information of the device to be estimated by a three-dimensional point cloud;

[0012] Extracting feature points from the real-time image data acquired by the image acquisition module;

[0013] Convert the three-dimensional coordinates of the feature points to the image acquisition module coordinate system, and project the image to the image pixel coordinates;

[0014] Combined with the Kannala-Brandt camera distortion parameters, the homogeneous coordinates of the image acquisition module coordinate system are substituted into the Kannala-Brandt camera distortion parameters to obtain the observation projection model.

[0015] As an optional implementation, the inertial measurement data includes angular velocity data and acceleration data.

[0016] As an optional implementation, the inertial navigation measurement and dynamic model are established by the following steps:

[0017] The inertial navigation measurement and dynamic model includes measuring angular velocity and measuring acceleration. Based on the inertial measurement data, the angular velocity in the real inertial navigation coordinate system is superimposed with the angular velocity offset and the angular velocity noise to obtain the measured angular velocity;

[0018] The measured acceleration is obtained by multiplying the rotation matrix from the world coordinate system to the inertial coordinate system, the real acceleration from the world coordinate system to the inertial coordinate system and the gravitational acceleration, and superimposing the acceleration offset and acceleration noise.

[0019] As an optional implementation, the composite full decoupling <SO(3),R 3 ,R 3 >The pose estimation framework of manifold optimization is established through the following steps:

[0020] Defining Composite Full Decoupling <SO(3),R 3 ,R 3 >The increasing and decreasing forms of manifolds;

[0021] Establish the camera feature point reprojection error based on the inverse depth stereo projection 3D coordinates and provide the corresponding composite full decoupling <SO(3),R 3 ,R 3 >Jacobian derivative matrix of the manifold;

[0022] Establish the uncertainty propagation model of the inertial navigation state and SE2(3) manifold based on inertial navigation pre-integration, determine the numerical calculation method of inertial navigation pre-integration, and provide the corresponding Jacobian matrix of SE2(3) manifold;

[0023] Construct uncertainty-weighted visual reprojection error terms;

[0024] Construct uncertainty-weighted inertial navigation pre-integrated error terms to provide corresponding composite full decoupling <SO(3),R 3 ,R 3 >Jacobian derivative matrix of the manifold.

[0025] As an optional implementation, the pre-established composite full decoupling <SO(3),R 3 ,R 3 >The manifold-optimized pose estimation framework, the step of calculating the pose data of the device to be estimated according to the image observation data and the inertial navigation pre-integration data, comprises:

[0026] Determine the minimized energy function;

[0027] Based on the minimized energy function, the composite full decoupling <SO(3),R 3 ,R 3 > A manifold-optimized pose estimation framework and an iterative optimization algorithm are used to optimize and calculate the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data under a fixed lag window.

[0028] As an optional implementation, the iterative optimization algorithm is a Levenberg-Marquardt iterative optimization method.

[0029] The second aspect of the present invention discloses a visual inertial navigation fusion pose estimation device based on composite manifold optimization, the device comprising:

[0030] An acquisition module, used to acquire real-time image data through an image acquisition module of the device to be estimated and to acquire inertial measurement data through a sensor module of the device to be estimated;

[0031] A first calculation module, configured to calculate image observation data according to the real-time image data based on a preset observation projection model corresponding to the image acquisition module;

[0032] A second calculation module is used to calculate the inertial navigation pre-integrated data according to the inertial navigation measurement data based on the pre-established inertial navigation measurement and dynamic model;

[0033] Estimation module for full decoupling of complex <SO(3),R 3 ,R 3 >A manifold-optimized pose estimation framework that calculates the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data.

[0034] The third aspect of the present invention discloses another visual inertial navigation fusion pose estimation device based on composite manifold optimization, the device comprising:

[0035] A memory storing executable program code;

[0036] a processor coupled to the memory;

[0037] The processor calls the executable program code stored in the memory to execute the visual inertial navigation fusion pose estimation method based on composite manifold optimization disclosed in the first aspect of the present invention.

[0038] The fourth aspect of the present invention discloses a computer storage medium, which stores computer instructions. When the computer instructions are called, they are used to execute the visual inertial navigation fusion pose estimation method based on composite manifold optimization disclosed in the first aspect of the present invention.

[0039] Compared with the prior art, the embodiments of the present invention have the following beneficial effects:

[0040] In the embodiment of the present invention, real-time image data is acquired through an image acquisition module of the device to be estimated, and inertial measurement data is acquired through a sensor module of the device to be estimated; image observation data is calculated based on the real-time image data based on the observation projection model corresponding to the preset image acquisition module; inertial navigation pre-integration data is calculated based on the inertial measurement data based on the pre-established inertial navigation measurement and dynamic model; based on the pre-established composite full decoupling <SO(3),R 3 ,R 3 >The pose estimation framework of manifold optimization calculates the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data. It can be seen that the implementation of the present invention can perform composite manifold optimization on the pose of the device to be estimated, achieve a longer time window optimization under limited computing resources, and reserve more sufficient processing time for the visual front end, thereby improving the computational efficiency of the visual inertial navigation fusion optimization algorithm and maintaining the consistency of the inertial navigation pre-integration. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1A schematic diagram of a flow chart of a visual inertial navigation fusion pose estimation method based on composite manifold optimization disclosed in an embodiment of the present invention;

[0042] Figure 2 A schematic diagram of the structure of a visual inertial navigation fusion pose estimation device based on composite manifold optimization disclosed in an embodiment of the present invention;

[0043] Figure 3 A schematic diagram of the structure of another visual-inertial navigation fusion pose estimation device based on composite manifold optimization disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0044] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. 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.

[0045] The terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish different objects rather than to describe a specific order. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, device, product or end including a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units that are not listed, or may optionally include other steps or units inherent to these processes, methods, products or ends.

[0046] Reference to "embodiment" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiment may be included in at least one embodiment of the present invention. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments.

[0047] It is understood explicitly and implicitly by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0048] The present invention discloses a method and device for visual inertial navigation fusion pose estimation based on composite manifold optimization, which can perform composite manifold optimization on the pose of the device to be estimated, achieve longer time window optimization under limited computing resources, and reserve more sufficient processing time for the visual front end, thereby improving the computational efficiency of the visual inertial navigation fusion optimization algorithm and maintaining the consistency of the inertial navigation pre-integration. The following are detailed descriptions.

[0049] Embodiment 1

[0050] See also Figure 1 , Figure 1 : is a flowchart of a method for visual inertial navigation fusion pose estimation based on composite manifold optimization disclosed in an embodiment of the present invention. Figure 1 The described visual inertial navigation fusion pose estimation method based on composite manifold optimization can be applied to a visual inertial navigation fusion pose estimation device based on composite manifold optimization, wherein the pose estimation based on composite manifold optimization can be integrated in a local server or a cloud server, which is not limited in the embodiments of the present invention. Figure 1 As shown, the visual inertial navigation fusion pose estimation method based on composite manifold optimization may include the following operations:

[0051] 101. Acquire real-time image data through an image acquisition module of the device to be estimated, and acquire inertial measurement data through a sensor module of the device to be estimated.

[0052] In the embodiment of the present invention, the image acquisition module includes an imaging device capable of detecting visible light, infrared light or ultraviolet light, and accordingly, the real-time image data includes visible light, infrared light or ultraviolet light imaging of the target object acquired by the image acquisition module. The sensor module acquires relevant measurement data of the acceleration and angular velocity of the target object.

[0053] For ease of understanding, the image acquisition module in this embodiment uses a terminal camera, and the image data is an image taken by the terminal camera.

[0054] 102. Based on the observation projection model corresponding to the preset image acquisition module, the image observation data is calculated according to the real-time image data.

[0055] The observation projection model is established through the following steps:

[0056] The surrounding spatial environment information of the device to be estimated is represented by a three-dimensional point cloud;

[0057] Extracting feature points from real-time image data acquired by the image acquisition module;

[0058] The three-dimensional coordinates of the feature points are converted to the image acquisition module coordinate system, and the images are projected to the image pixel coordinates;

[0059] Combined with the Kannala-Brandt camera distortion parameters, the homogeneous coordinates of the image acquisition module coordinate system are substituted into the Kannala-Brandt camera distortion parameters to obtain the observation projection model.

[0060] Among them, the feature points are extracted using the FAST corner detection algorithm. The full name of the FAST corner detection algorithm is Features From Accelerated Segment Test. Since it does not involve complex operations such as scale and gradient, the FAST corner detection algorithm has a very fast detection speed. It uses the grayscale value of the pixel in a certain neighborhood to compare with the center point to determine whether it is a corner point. If a pixel is significantly different from enough pixels in its surrounding neighborhood, the pixel may be a corner point.

[0061] The Kanade-Lucas-Tomasi algorithm is used to realize two-dimensional feature point tracking of images. The Kanade-Lucas-Tomasi algorithm, referred to as the KLT algorithm, is a target tracking algorithm based on feature points. It selects feature points in the video sequence and uses the optical flow method to calculate the motion trajectory of these feature points between consecutive frames, thereby realizing target tracking.

[0062] The feature points extracted by the FAST corner detection algorithm are rotated and translated in the three-dimensional point cloud coordinate system to the terminal camera coordinate system, and projected to the image pixel coordinates. The Kannala-Brandt camera distortion projection model is used to adapt to the wide-angle lens of the terminal camera. The fourth-order model of the projection model is as follows:

[0063]

[0064] Among them, [f x ,f y ,c x ,c y ] is the focal length and optical center coordinates of the camera, [k1, k2, k3, k4] is the fourth-order Kannala-Brandt camera distortion parameter, which can be obtained through offline calibration of the camera intrinsic parameters, π(·) is the Kannala-Brandt camera projection function, x=[m x ,m y ,m z ,ρ] T is the homogeneous coordinate of the three-dimensional point in the current camera coordinate system, where m x ,m y ,m z is the unit orientation vector of the three-dimensional point, and ρ is the inverse depth of the three-dimensional point.

[0065] 103. Based on the pre-established inertial navigation measurement and dynamic model, the inertial navigation pre-integration data is calculated according to the inertial measurement data.

[0066] The inertial navigation measurement and dynamic model are established through the following steps:

[0067] The inertial navigation measurement and dynamic model includes measuring angular velocity and measuring acceleration. Based on the inertial measurement data, the angular velocity in the real inertial navigation coordinate system is superimposed with the angular velocity offset and angular velocity noise to obtain the measured angular velocity.

[0068] The measured acceleration is obtained by multiplying the rotation matrix from the world coordinate system to the inertial coordinate system, the real acceleration from the world coordinate system to the inertial coordinate system and the gravitational acceleration, and superimposing the acceleration offset and acceleration noise.

[0069] As can be seen from the above, the inertial measurement data includes angular velocity data and acceleration data. The inertial navigation model measurement includes the three-axis angular velocity measurement of the gyroscope and the three-axis linear acceleration measurement of the accelerometer. The inertial navigation measurement model is as follows:

[0070]

[0071] in To measure angular velocity and acceleration respectively, I ω WI is the angular velocity in the real inertial navigation coordinate system, W a WI , W g is the real acceleration and gravity acceleration in the world coordinate system, R IW is the rotation matrix from the world coordinate system to the inertial navigation coordinate system, b g , b a are the angular velocity and acceleration offset, n g , n a is the angular velocity and acceleration measurement noise. The noise-free discrete dynamic model for inertial navigation state propagation is as follows (3):

[0072]

[0073] in W t k+1 , W v k+1 , and W t k , W v k , t k+1 With t k Translation, velocity and rotation between two image frames, ΔT = t k+1 -t k is the time interval between two image frames, α kk+1 , β kk+1 , γ kk+1 Because k+1 to kThe inter-frame inertial navigation estimated “pseudo-measurement value” at time instant. In continuous time, it can be expressed as formula (4):

[0074]

[0075] 104. Full decoupling based on pre-established composite <SO(3),R 3 ,R 3 >The manifold-optimized pose estimation framework calculates the pose data of the device to be estimated based on image observation data and inertial navigation pre-integration data.

[0076] Composite full decoupling <SO(3),R 3 ,R 3 >The pose estimation framework of manifold optimization is established through the following steps:

[0077] Defining Composite Full Decoupling <SO(3),R 3 ,R 3 >The increasing and decreasing forms of manifolds;

[0078] Establish the camera feature point reprojection error based on the inverse depth stereo projection 3D coordinates and provide the corresponding composite full decoupling <SO(3),R 3 ,R 3 >Jacobian derivative matrix of the manifold;

[0079] Establish the uncertainty propagation model of the inertial navigation state and SE2(3) manifold based on inertial navigation pre-integration, determine the numerical calculation method of inertial navigation pre-integration, and provide the corresponding Jacobian matrix of SE2(3) manifold;

[0080] Construct uncertainty-weighted visual reprojection error terms;

[0081] Construct uncertainty-weighted inertial navigation pre-integrated error terms to provide corresponding composite full decoupling <SO(3),R 3 ,R 3 >Jacobian derivative matrix of the manifold.

[0082] in, <SO(3),R 3 ,R 3 >The increase and decrease of the manifold is different from the manifold representation of rotation, translation, and velocity estimated by the existing visual inertial odometer: <SO(3)×R 3 ,R 3 >Velocity-decoupled chained rotation-translation manifold, <SE(3),R 3 >Speed-decoupled synchronous rotation and translation manifold, SE2(3) synchronous rotation and translation and velocity manifold. SE2(3) as a synchronous rotation and translation and velocity manifold can be expressed as formula (5):

[0083]

[0084] Where R is the rotation matrix, v is the velocity, t is the translation, τ is the increment, θ, ν and ρ represent the increments of rotation, velocity and translation respectively.

[0085] definition <SO(3),R 3 ,R 3 >The increase and decrease operations on the manifold can be expressed as formula (6) on the SE2(3) manifold:

[0086]

[0087] Similarly, <SO(3)×R 3 ,R 3 >The increase and decrease operations on the manifold can be expressed as formula (7) on the SE2(3) manifold:

[0088]

[0089] <SE(3),R 3 >The increase and decrease operations on the manifold can be expressed as formula (8) on the SE2(3) manifold:

[0090]

[0091] The increase and decrease operation of SE2(3) manifold can be expressed as formula (9):

[0092]

[0093] It can be found <SO(3),R 3 ,R 3 > Manifold increase and decrease has fewer operations. In formulas (6), (7), (8), and (9), Exp(·) and Log(·) are the exponential and logarithmic mappings of the rotation matrix, respectively. The exponential mapping is formula (10):

[0094]

[0095] where θ=[θ1θ2θ3] T ∈R^3;

[0096]

[0097] The logarithmic mapping is formula (12):

[0098]

[0099] in

[0100]

[0101] In formula (6), (7), (8), (9), Jl (·)and is the left Jacobian matrix of SO(3) and its inverse matrix,

[0102]

[0103] Furthermore, in the step of establishing the camera feature point reprojection error based on the inverse depth stereo projection 3D coordinates, the reprojection error of a single 3D feature point can be expressed as the residual between the feature observation pixel coordinates and its projection on the target frame: The variable involved in the residual is the position and posture of the target frame inertial navigation The position and posture of the inertial navigation in the frame where the 3D feature point is located The target frame and the frame where the three-dimensional feature point is located (the image frame where the three-dimensional feature point is first obtained by triangulation) both observe the three-dimensional feature point at the same time, and the embodiment of the present invention uses stereographic projection to express the three-dimensional feature point in a parameterized form. h p s , this parameterized form can realize unconstrained optimization for three-dimensional feature points. The residual function is formula (15):

[0104]

[0105] where π t (·) is the projection function that projects the 3D feature points in the homogeneous coordinate system to the pixel coordinates on the target frame, see formula (1) for details, is the pixel position of the 2D feature point of the 3D feature point on the target frame, are the fixed extrinsic parameters of the inertial navigation and camera in the current frame and the target frame respectively. It is a stereo back-projection function, which transforms the three-dimensional feature points on the target frame into a stereo parameterized form h p s Mapped to its equivalent homogeneous parameterized form with inverse depth ρ The following formula (16):

[0106]

[0107] The specific conversion from the stereo parameterized form to the homogeneous parameterized form is as follows:

[0108]

[0109] According to the residual function (15) of the reprojection error of a single 3D feature point and the chain rule of derivation, the derivative Jacobian of the residual term with respect to the variable involved in the residual can be obtained as formula (18):

[0110]

[0111] in is the Jacobian matrix of the reprojected residual to the target frame and the inertial navigation pose of the frame, is the Jacobian matrix of the three-dimensional feature points of the stereo parameterization of the reprojection residual. The derivation of the intermediate term, in the embodiment of the present invention, due to the use of composite full decoupling <SO(3),R 3 ,R 3 >manifold, we can obtain the following theorem (19):

[0112]

[0113] Applying Theorem (19) to formula (18), we can obtain and Jacobian matrix.

[0114] Furthermore, in the step of establishing the inertial navigation state and its uncertainty propagation model based on inertial navigation pre-integration, the inertial navigation pre-integration measurement can be obtained by numerical calculation according to specific assumptions. The present invention assumes that in the inertial navigation measurement interval Δt=t j+1 -t j Angular velocity measurement value is a constant, and the acceleration in the world coordinate system W a WI is a constant, and the present invention uses semi-rotational Euler discretization to discretize the integral term. Define t j to j+1 The rotation (5a) and half rotation (5b) of the two inertial navigation measurement moments are:

[0115]

[0116] At the same time, assuming that t k to k+1 Angular velocity deviation between two image frames With acceleration The deviation remains unchanged, so we get k to k+1 The inertial navigation pre-integration discrete iterative formula (6), where t j ∈[t k ,t k+1 ] is the inertial navigation measurement time between the two image frames, where the inertial navigation measurement frequency (about 200 Hz) is much larger than the image measurement frequency (about 30 Hz), ΔT>Δt.

[0117]

[0118] where α kj ,β kj ,γ kj With α kj+1 ,βkj+1 ,γ kj+1 They are the inertial navigation pre-integration terms at t j With t j+1 The translation, velocity, and rotation terms at the moment of inertial navigation measurement. and Respectively, t j to j+1 The initial value of the inertial navigation pre-integration is zero, α kk+1 =0,β kk+1 =0,γ kk+1 =I.

[0119] In order to propagate the uncertainty of inertial navigation pre-integration, the incremental state of pre-integration is defined as:

[0120]

[0121] Using the SE2(3) manifold, the discrete-time linearized pre-integrated incremental state dynamics of “small signal propagation” is defined as:

[0122]

[0123] Then, the pre-integrated error covariance matrix can be iteratively expressed as

[0124]

[0125] where Σ kj+1 ,Σ kj t j+1 With t j The pre-integrated error covariance matrix at time , and its initial state is zero Σ kk =0 9×9 . The incremental state Jacobian matrix A can be expressed as:

[0126]

[0127] The present invention adopts the semi-rotation inertial navigation pre-integration, thereby defining the semi-rotation acceleration as

[0128]

[0129] From this we can get,

[0130]

[0131] Substituting (27) into (25), we can get the Jacobian matrix A. In addition, the accelerometer B a and gyroscope B g The Jacobian matrix of the measurement is:

[0132]

[0133] Among them J r (θ) = J l (-θ).

[0134] The bias is set to a constant during the inertial navigation pre-integration process to avoid recalculating the IMU pre-integration pseudo-measurement α during the iterative optimization process. kk+1 ,β kk+1 ,γ kk+1 Small changes in the bias can be expressed as increments in the linearization point:

[0135]

[0136] These small bias changes act on the k to k+1 The inertial navigation pre-integration can approximate these deviation increments as the first-order expansion of the pre-integrated pseudo-measurement:

[0137]

[0138] The iterative calculation of the constant deviation Jacobian matrix during the inertial navigation pre-integration period can be expressed as:

[0139]

[0140] in Exists in After iterative operation of (31):

[0141]

[0142] Furthermore, in the step of constructing the uncertainty-weighted visual reprojection error term, the weighted sum of the residuals (15) is calculated by calculating the inverse of the observation covariance to obtain the reprojection error term:

[0143]

[0144] where Σ VIS ∈R 2×2 is the feature observation covariance in pixels. In the embodiment of the present invention, it is assumed that the observation noise in the horizontal and vertical directions in the image plane is independent and has the same standard deviation σ v ,but In order to improve the robustness against dynamic feature points, the present invention also adopts the Cauchy robust function to reduce the influence of abnormal feature points, such as dynamic feature points, on the optimization.

[0145] Furthermore, in the step of constructing the uncertainty-weighted INS pre-integration error term, for an INS pre-integration residual, the optimization mainly involves k and tk+1 Two states of the moment:

[0146] x IMU ={x k ,x k+1}#(34)

[0147] Each state consists of rotation, translation, velocity, and bias:

[0148]

[0149] The present invention defines the inertial navigation pre-integration residual of the fixed inertial navigation bias as:

[0150]

[0151] where α kk+1 ,β kk+1 ,γ kk+1 is the first-order expansion term (30).

[0152] The Jacobian matrix of the inertial navigation pre-integration residual is defined as:

[0153]

[0154] in,

[0155]

[0156] The present invention adopts composite full decoupling <SO(3),R 3 ,R 3 > manifold, the specific Jacobian matrix is ​​as follows:

[0157]

[0158] And we can get:

[0159]

[0160] The next moment t k+1 The overall Jacobian matrix of the state is expressed as:

[0161]

[0162] The inertial navigation pre-integration error is the weighted sum of the residuals (36), where the weighting matrix is ​​the inverse of the propagation covariance:

[0163]

[0164] where Σ IMU is the covariance matrix of the inertial navigation pre-integration propagation:

[0165]

[0166] where Σ kk+1 is the covariance matrix of rotation, velocity and translation obtained by (24), is the covariance matrix of the bias assuming Brownian motion. Given the random walk standard deviation of the continuous-time inertial navigation bias obtained through inertial navigation calibration, and The discrete-time deviation covariance is and

[0167] Further, based on the pre-established composite full decoupling <SO(3),R 3 ,R 3 >The manifold optimized pose estimation framework calculates the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data, including:

[0168] Determine the minimized energy function;

[0169] Based on minimizing energy function, composite full decoupling <SO(3),R 3 ,R 3 >The manifold-optimized pose estimation framework and iterative optimization algorithm optimize the pose data of the device to be estimated based on image observation data and inertial navigation pre-integration data under a fixed lag window.

[0170] Preferably, the iterative optimization algorithm in this embodiment is the Levenberg-Marquardt iterative optimization method.

[0171] Manifold optimization in visual inertial odometry is a nonlinear least squares problem, which can be expressed as minimizing the energy function E(x):

[0172]

[0173] Where x is the state involved in the optimization of the visual inertial odometer, including rotation, speed, translation, and the position of the three-dimensional feature points in the sliding window. Represents the measurement value in the visual inertial odometry, which can be the visual feature pixel position on the image plane or the inertial pre-integrated pseudo-measurement value between two image frames. i represents the covariance matrix, whose inverse W i The weight matrix representing the weighted least squares (LS) problem. Given the linearization point The nonlinear model h(·) can be linearized and approximated as:

[0174]

[0175] in is the function h i (x) is the Jacobian matrix at the linearization point. δx is the increment between the current state and the linearization point. Now we approximate the energy function linearly as,

[0176]

[0177] where the residual and Jacobian matrices are weighted in the form of square roots:

[0178]

[0179] Adding a damping term (Levenberg-Marquardt) can be done by minimizing the damped To obtain the optimal step size of LM:

[0180]

[0181] Embodiment 2

[0182] See also Figure 2 , Figure 2 Schematic diagram of the structure of a visual inertial navigation fusion pose estimation device based on composite manifold optimization disclosed in an embodiment of the present invention. Figure 2 As shown, the visual inertial navigation fusion pose estimation device based on composite manifold optimization may include:

[0183] An acquisition module 201 is used to acquire real-time image data through an image acquisition module of the device to be estimated and to acquire inertial measurement data through a sensor module of the device to be estimated;

[0184] A first calculation module 202 is used to calculate image observation data according to real-time image data based on an observation projection model corresponding to a preset image acquisition module;

[0185] The second calculation module 203 is used to calculate the inertial navigation pre-integrated data according to the inertial navigation measurement data based on the pre-established inertial navigation measurement and dynamic model;

[0186] Estimation module 204, for use based on pre-established composite full decoupling <SO(3),R 3 ,R 3 >The manifold-optimized pose estimation framework calculates the pose data of the device to be estimated based on image observation data and inertial navigation pre-integration data.

[0187] It can be seen that implementation Figure 2The described device can obtain real-time image data and inertial measurement data through the sensor module of the device to be estimated, calculate image observation data based on the real-time image data, calculate inertial navigation pre-integration data based on the inertial measurement data, calculate the position and posture data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data, and perform composite manifold optimization on the position and posture of the device to be estimated, so as to achieve longer time window optimization under limited computing resources, reserve more sufficient processing time for the visual front end, improve the computational efficiency of the visual inertial navigation fusion optimization algorithm and maintain the consistency of the inertial navigation pre-integration.

[0188] Specifically, the details of the module function steps in the above device and the technical advantages can refer to the corresponding description in the first embodiment, and this embodiment will not be repeated here.

[0189] Embodiment 3

[0190] See also Figure 3 , Figure 3 FIG. 1 is a schematic diagram of the structure of another visual inertial navigation fusion pose estimation device based on composite manifold optimization disclosed in an embodiment of the present invention. Figure 3 As shown, the visual inertial navigation fusion pose estimation device based on composite manifold optimization may include:

[0191] A memory 301 storing executable program codes;

[0192] a processor 302 coupled to the memory;

[0193] The processor 302 calls the executable program code stored in the memory 301 to execute the steps in the visual inertial navigation fusion pose estimation method based on composite manifold optimization described in the first embodiment of the present invention.

[0194] Embodiment 4

[0195] An embodiment of the present invention discloses a computer storage medium, which stores computer instructions. When the computer instructions are called, they are used to execute the steps in the visual inertial navigation fusion pose estimation method based on composite manifold optimization described in Embodiment 1 of the present invention.

[0196] Embodiment 5

[0197] An embodiment of the present invention discloses a computer program product, which includes a non-transitory computer-readable storage medium storing a computer program, and the computer program is operable to enable a computer to execute the steps in the visual-inertial navigation fusion pose estimation method based on composite manifold optimization described in Example 1.

[0198] The device embodiments described above are only illustrative, wherein the modules described as separate components may or may not be physically separated, and the components displayed as modules may or may not be physical modules, i.e., they may be located in one place, or they may be distributed on multiple network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Those of ordinary skill in the art may understand and implement it without creative work.

[0199] Through the specific description of the above embodiments, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on such an understanding, the above technical solution can be essentially or partly contributed to the prior art in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, and the storage medium includes a read-only memory (ROM), a random access memory (RAM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), a one-time programmable read-only memory (OTPROM), an electronically erasable rewritable read-only memory (EEPROM), a compact disc (CD-ROM) or other optical disc storage, magnetic disk storage, magnetic tape storage, or any other computer-readable medium that can be used to carry or store data.

[0200] Finally, it should be noted that the visual inertial navigation fusion pose estimation method and device based on composite manifold optimization disclosed in the embodiments of the present invention only discloses the preferred embodiments of the present invention, which are only used to illustrate the technical scheme of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical schemes described in the aforementioned embodiments can still be modified, or some of the technical features therein can be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical schemes from the spirit and scope of the technical schemes of the embodiments of the present invention.

Claims

1. A visual inertial navigation fusion pose estimation method based on composite manifold optimization, characterized in that: The method comprises: Acquiring real-time image data through an image acquisition module of the device to be estimated and acquiring inertial measurement data through a sensor module of the device to be estimated; Based on the preset observation projection model corresponding to the image acquisition module, the image observation data is calculated according to the real-time image data; Based on the pre-established inertial navigation measurement and dynamic model, the inertial navigation pre-integrated data is calculated according to the inertial measurement data; Based on pre-built composite full decoupling <SO(3),R 3 ,R 3 >A manifold-optimized pose estimation framework that calculates the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data.

2. The method for posture estimation according to claim 1, characterized in that: The observation projection model is established by the following steps: Representing the surrounding spatial environment information of the device to be estimated by a three-dimensional point cloud; Extracting feature points from the real-time image data acquired by the image acquisition module; Convert the three-dimensional coordinates of the feature points to the image acquisition module coordinate system, and project the image to the image pixel coordinates; Combined with the Kannala-Brandt camera distortion parameters, the homogeneous coordinates of the image acquisition module coordinate system are substituted into the Kannala-Brandt camera distortion parameters to obtain the observation projection model.

3. The method for posture estimation according to claim 1, characterized in that: The inertial measurement data includes angular velocity data and acceleration data.

4. The method for posture estimation according to claim 1, characterized in that: The inertial navigation measurement and dynamic model are established by the following steps: The inertial navigation measurement and dynamic model includes measuring angular velocity and measuring acceleration. Based on the inertial measurement data, the angular velocity in the real inertial navigation coordinate system is superimposed with the angular velocity offset and the angular velocity noise to obtain the measured angular velocity; The measured acceleration is obtained by multiplying the rotation matrix from the world coordinate system to the inertial coordinate system, the real acceleration from the world coordinate system to the inertial coordinate system and the gravitational acceleration, and superimposing the acceleration offset and acceleration noise.

5. The method for posture estimation according to claim 1, characterized in that: The composite full decoupling <SO(3),R 3 ,R 3 >The pose estimation framework of manifold optimization is established through the following steps: Defining Composite Full Decoupling <SO(3),R 3 ,R 3 >The increasing and decreasing forms of manifolds; Establish the camera feature point reprojection error based on the inverse depth stereo projection 3D coordinates and provide the corresponding composite full decoupling <SO(3),R 3 ,R 3 >Jacobian derivative matrix of manifold; Establish the uncertainty propagation model of the inertial navigation state and SE2(3) manifold based on inertial navigation pre-integration, determine the numerical calculation method of inertial navigation pre-integration, and provide the corresponding Jacobian matrix of SE2(3) manifold; Construct uncertainty-weighted visual reprojection error terms; Construct uncertainty-weighted inertial navigation pre-integrated error terms to provide corresponding composite full decoupling <SO(3),R 3 ,R 3 >Jacobian derivative matrix of the manifold.

6. The method for posture estimation according to claim 5, characterized in that: The pre-built composite full decoupling <SO(3),R 3 ,R 3 >The manifold-optimized pose estimation framework, the step of calculating the pose data of the device to be estimated according to the image observation data and the inertial navigation pre-integration data, comprises: Determine the minimized energy function; Based on the minimized energy function, the composite full decoupling <SO(3),R 3 ,R 3 > A manifold-optimized pose estimation framework and an iterative optimization algorithm are used to optimize and calculate the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data under a fixed lag window.

7. The method for posture estimation according to claim 6, characterized in that: The iterative optimization algorithm is the Levenberg-Marquardt iterative optimization method.

8. A visual inertial navigation fusion pose estimation device based on composite manifold optimization, characterized in that: The device comprises: An acquisition module, used to acquire real-time image data through an image acquisition module of the device to be estimated and to acquire inertial measurement data through a sensor module of the device to be estimated; A first calculation module, configured to calculate image observation data according to the real-time image data based on a preset observation projection model corresponding to the image acquisition module; A second calculation module is used to calculate the inertial navigation pre-integrated data according to the inertial navigation measurement data based on the pre-established inertial navigation measurement and dynamic model; Estimation module for full decoupling of complex <SO(3),R 3 ,R 3 >A manifold-optimized pose estimation framework that calculates the pose data of the device to be estimated based on the image observation data and the inertial navigation pre-integration data.

9. A visual inertial navigation fusion pose estimation device based on composite manifold optimization, characterized in that: The device comprises: A memory storing executable program code; a processor coupled to the memory; The processor calls the executable program code stored in the memory to execute the visual inertial navigation fusion pose estimation method based on composite manifold optimization as described in any one of claims 1 to 7.

10. A computer storage medium, characterized in that: The computer storage medium stores computer instructions, which, when called, are used to execute the visual inertial navigation fusion pose estimation method based on composite manifold optimization as described in any one of claims 1 to 7.

Images