Inertial vision integrated navigation method and device based on Lie group state transformation

Abstract

The application discloses an inertial vision combined navigation method and device based on Lie group state transformation, which comprises the following steps: inputting navigation information of a carrier acquired by an inertial navigation system in an inertial vision combined navigation system on a carrier and vision navigation information synchronously collected by a camera in the inertial vision combined navigation system; constructing a system model, an observation model and an updating equation of the inertial vision combined navigation system suitable for a Lie group state transformation Kalman filter; performing state estimation by using the Lie group state transformation Kalman filter based on the constructed system model and observation model, obtaining a correction amount of the inertial / vision combined navigation system, outputting inertial / vision combined navigation information including an attitude, a speed and a position, and updating the navigation state according to the updating equation until the navigation is completed. The application aims at overcoming the inconsistency problem in the traditional inertial / vision combined navigation by using the Lie group state transformation method, and improving the navigation precision of the inertial / vision combined navigation system without increasing the calculation amount.

Description

Technical Field

[0001] This invention relates to the field of inertial / visual integrated navigation technology, and in particular to an inertial / visual integrated navigation method and device based on Lie group state transformation. Background Technology

[0002] Currently, unmanned systems are typically equipped with inertial / vision integrated navigation systems. Visual navigation systems extract features from visual images obtained from camera sensors to estimate the vehicle's motion state, but their error characteristics are highly dependent on environmental features. Inertial navigation systems, on the other hand, can provide high-frequency, continuous navigation parameter output, offering continuous, autonomous, and accurate navigation parameters even under conditions of sparse visual image features. Inertial / vision integrated navigation systems combine the advantages of both and are a key supporting technology for unmanned platform navigation systems.

[0003] One commonly used inertial / vision integrated navigation algorithm is the Multi-State Constraint Kalman Filter (MSCKF). MSCKF fuses inertial and visual information within the extended Kalman filter framework. Compared to purely visual odometry, MSCKF can adapt to more intense motion and texture loss over a certain period, exhibiting higher robustness. Compared to optimization-based inertial / vision integrated navigation algorithms, MSCKF offers comparable accuracy but faster speed, making it suitable for embedded platforms with limited computing resources.

[0004] The commonly used MSCKF algorithm suffers from inconsistent state estimations. Because it uses EKF for linearization, the linearized system may produce erroneous observability, causing the corresponding state variance to decrease with filtering, which is inconsistent with the theoretical value. This affects the consistency of estimation, thereby reducing navigation accuracy or even causing the filter to diverge. Summary of the Invention

[0005] To address the technical problems existing in the prior art, the present invention provides an inertial vision integrated navigation method and device based on Lie group state transformation.

[0006] To achieve the above objectives, the present invention is implemented through the following technical solution:

[0007] On the one hand, an inertial vision integrated navigation method based on Lie group state transformation is provided, including:

[0008] The input carrier is the navigation information acquired by the inertial navigation system in the inertial vision integrated navigation system and the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system.

[0009] Construct a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0010] Construct an observation model for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters;

[0011] Construct update equations for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0012] Based on the constructed system model and observation model, state estimation is performed using a Lie group state transformation Kalman filter to obtain the correction amount of the inertial / visual integrated navigation system. The navigation state is then updated according to the constructed update equation until navigation ends.

[0013] Compared with the prior art, the present invention has the following advantages:

[0014] 1) This invention provides an inertial / visual integrated navigation method, which is not limited to monocular vision, but can also be extended to binocular vision or other vision fields.

[0015] 1) The present invention provides an inertial / visual integrated navigation device, including a memory and a processor, wherein the memory stores a calculation program and the processor performs algorithm execution calculations.

[0016] On the other hand, an inertial vision-based integrated navigation device based on Lie group state transformation is provided, comprising:

[0017] The first module is used to input the navigation information of the carrier obtained by the inertial navigation system in the inertial vision integrated navigation system on the carrier, as well as the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system.

[0018] The second module is used to construct a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0019] The third module is used to construct an observation model for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters.

[0020] The fourth module is used to construct the update equations for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters.

[0021] The fifth module is used to perform state estimation using a Lie group state transformation Kalman filter based on the constructed system model and observation model, obtain the correction amount of the inertial / visual integrated navigation system, and update the navigation state according to the constructed update equation until the navigation ends.

[0022] On the other hand, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to perform the following steps:

[0023] The input carrier is the navigation information acquired by the inertial navigation system in the inertial vision integrated navigation system and the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system.

[0024] Construct a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0025] Construct an observation model for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters;

[0026] Construct update equations for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0027] Based on the constructed system model and observation model, state estimation is performed using a Lie group state transformation Kalman filter to obtain the correction amount of the inertial / visual integrated navigation system. The navigation state is then updated according to the constructed update equation until navigation ends.

[0028] On the other hand, a computer-readable storage medium is provided on which a computer program is stored, and when the computer program is executed by a processor, it performs the following steps:

[0029] The input carrier is the navigation information acquired by the inertial navigation system in the inertial vision integrated navigation system and the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system.

[0030] Construct a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0031] Construct an observation model for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters;

[0032] Construct update equations for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0033] Based on the constructed system model and observation model, state estimation is performed using a Lie group state transformation Kalman filter to obtain the correction amount of the inertial / visual integrated navigation system. The navigation state is then updated according to the constructed update equation until navigation ends.

[0034] Compared with the prior art, the present invention has the following advantages:

[0035] 1) This invention uses the Lie group nonlinear state error definition method, which enables a more accurate characterization of the nonlinear properties in the inertial / visual integrated navigation system, resulting in more accurate modeling and thus better consistency in variance estimation and accuracy in filtering estimation.

[0036] 2) The positioning error prediction model used in this invention is a Lie group state transformation Kalman filter, which replaces the specific force term in the navigation system model with a gravity-related term, overcoming the adverse effects of specific force changes on state estimation and making it more suitable for dynamic systems.

[0037] 3) Compared with optimization-based methods, this invention has the advantage of low computational cost while ensuring comparable accuracy, and the implementation process is consistent with that of the Kalman filter. Attached Figure Description

[0038] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0039] Figure 1 This is a flowchart of an embodiment of an inertial vision-integrated navigation method based on Lie group state transformation;

[0040] Figure 2 This is the trajectory of one experiment using the open-source KITTI dataset in one embodiment;

[0041] Figure 3 This is a graph showing the horizontal position error results of a single experiment using the open-source KITTI dataset in one embodiment.

[0042] Figure 4 This is a diagram showing the heading angle error results of a single experiment using the open-source KITTI dataset in one embodiment;

[0043] Figure 5 This is a comparison chart of statistical results from a single experiment using the open-source KITTI dataset in one embodiment. Detailed Implementation

[0044] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the spirit of the disclosed content will be clearly explained below with reference to the accompanying drawings and detailed description. Any person skilled in the art, after understanding the embodiments of the present invention, can make changes and modifications based on the techniques taught in the present invention without departing from the spirit and scope of the present invention. The illustrative embodiments and descriptions of the present invention are used to explain the present invention, but are not intended to limit the present invention.

[0045] This invention improves the existing MSCKF inertial / visual integrated navigation algorithm by utilizing a Lie group state transformation Kalman filter, and calls it LG-MSCKF. This invention can overcome the variance estimation inconsistency problem caused by the traditional extended Kalman filter by utilizing nonlinear error states, and at the same time utilizes the world coordinate system to reduce the calculation error caused by nonlinear error states under conditions of large initial alignment errors, thereby improving the accuracy and stability of inertial / visual integrated navigation.

[0046] Reference Figure 1 One embodiment provides an inertial vision integrated navigation method based on Lie group state transformation (referred to as LG-MSCKF), comprising:

[0047] The input carrier is the navigation information acquired by the inertial navigation system in the inertial vision integrated navigation system and the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system.

[0048] Construct a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0049] Construct an observation model for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters;

[0050] Construct update equations for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0051] Based on the constructed system model and observation model, state estimation is performed using a Lie group state transformation Kalman filter to obtain the correction amount of the inertial / visual integrated navigation system. The navigation state is then updated according to the constructed update equation until navigation ends.

[0052] This invention is applicable to vehicle navigation equipped with a visual / inertial odometry integrated navigation system. The visual / inertial odometry integrated navigation system includes an inertial navigation system and a camera sensor. The type of vehicle is not limited. The inertial navigation system records the vehicle's motion information, while the camera collects environmental image information. By analyzing gyroscope and accelerometer data, a strapdown inertial navigation algorithm is used to infer the vehicle's attitude, velocity, and position; simultaneously, feature point extraction and tracking are performed on the images synchronously acquired by the camera.

[0053] We construct the system model, observation model, and update equations for an inertial vision integrated navigation system that is suitable for Lie group state transformation Kalman filters.

[0054] The motion information of the inertial system sensitive carrier involved in this invention includes: angle change or angular velocity data recorded by a three-axis gyroscope and force ratio or force integral increment data measured by a three-axis accelerometer; and two-dimensional images captured by a monocular camera and synchronously acquired with the inertial navigation system.

[0055] After startup, the inertial / visual integrated navigation system undergoes initialization. The inertial navigation system receives motion information from the vehicle, while the camera simultaneously acquires image information. The data received by the inertial navigation system includes angular increments or angular velocity information from the three-axis gyroscope and specific force or specific force integral increment information from the three-axis accelerometer. This data is used to perform inertial navigation calculations. Based on the vehicle motion information received by the inertial navigation system, the vehicle's attitude, velocity, and position information are calculated for inertial navigation. The data acquired by the camera includes two-dimensional visual image information synchronized with the inertial navigation system.

[0056] Inertial navigation system (INS) initialization requires recording information such as the initial position of the carrier, alignment time, and the installation relationship between the carrier and the INS for initial alignment. The initial alignment process determines the initial attitude, velocity, and position information of the carrier and the INS, preparing for subsequent INS calculations.

[0057] The first frame of the image is used to extract key feature points, which are then used as the initial feature points for tracking. An algorithm selects FAST (Features from Accelerated Segment Test) corner points as initial feature points, extracting FAST corner points from image information synchronously acquired by the camera, and storing the two-dimensional pixel coordinate information of these feature points. The KLT (Kanade Lucas Tomasi) optical flow tracking method is then used to track these extracted feature points. Before the formal combined visual / inertial navigation filtering, the visual image needs to complete the tasks of feature point tracking and matching, removing abnormal feature points, and extracting new feature points. Additionally, after tracking a feature point, new feature points are detected in the image region outside the tracking point to supplement the number of feature points obtained during the tracking process.

[0058] In one embodiment, a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters is constructed, including:

[0059] Constructing the state vector of an inertial vision integrated navigation system Where x LG-b Let x be the inertial-dependent state vector. LG-c This is the camera-related state vector;

[0060] Determine the inertial-dependent state vector x LG-b ,as follows:

[0061]

[0062] In the formula, It is a Hamiton unit quaternion representing a rotation from the inertial navigation coordinate system b to the tangent plane coordinate system w; It is the projection of the velocity of the inertial navigation system relative to the w-frame into the w-frame; It is the projection of the inertial navigation system's position relative to the w-frame into the w-frame, b g and b a These are zero bias for the gyroscope and zero bias for the accelerometer, respectively. This represents the rotation from the visual navigation coordinate system c to the inertial navigation coordinate system b; This represents the positional relationship between the visual navigation coordinate system c and the inertial navigation coordinate system b.

[0063] Hamiton unit quaternion Attitude rotation matrix from b-series to w-series The relationship between quaternions containing errors and true quaternions is as follows:

[0064]

[0065]

[0066] in This is quaternion multiplication. express The estimated value, express The estimated value, φ b w denoted by , representing the attitude error between the b-system and the w-system, and I representing the identity matrix.

[0067] The inertial-dependent error state vector is determined as follows:

[0068]

[0069] In the formula, each quantity represents the attitude error φ between the b-system and the w-system. b w Speed ​​error after transformation Transformed position error Gyroscope zero bias error δb g accelerometer zero bias error δb a Attitude error between C and B systems Position error calculated by the camera Compared to the traditional definition of additive error, this invention uses transformed velocity error and transformed position error. The transformed definitions of velocity and position errors are as follows:

[0070]

[0071]

[0072] in and Let be the velocity error vector and the position error vector, and × denote the cross product.

[0073] It can be observed that, in determining the inertial-related error state vector in this invention, compared with the variables in existing state transformation methods, the transformation of velocity error and position error has been added. Therefore, the ability to handle nonlinearity of velocity error and position error has been improved, especially under the condition of large misalignment angle and large degree of nonlinearity. This enables a more accurate characterization of the nonlinearity in the inertial / visual integrated navigation system, and is more accurate in modeling. Therefore, it has better consistency in variance estimation and accuracy in filtering estimation.

[0074] The differential equation for the inertial-dependent error state vector is as follows:

[0075]

[0076] Where F LG-b Represents the system matrix, G LG-b Represents the noise driving matrix, w LG-b Represents the inertial-related noise vector;

[0077]

[0078] Where w g and w a The white noise for measurements of the gyroscope and accelerometer are respectively, w wg and w wa The white noise of the drive corresponding to the zero bias of the gyroscope and accelerometer are w, respectively. φ-bc and w δr-bc These are the relative attitude angle noise and relative displacement noise between the inertial navigation system and the camera, respectively.

[0079]

[0080] in and Let I3 represent the projections of the Earth's rotation vector and gravity vector into the w-frame, respectively. Let I3 be the identity matrix of dimension 3, and × denote the antisymmetric matrix composed of vector elements. express The estimated value, express The estimated value;

[0081]

[0082] in This represents the attitude rotation matrix from the b-frame to the w-frame.

[0083] Determine the camera-related state vector x LG-c , means as follows:

[0084]

[0085] in and Let x represent the rotation and displacement between the camera coordinate system and the world coordinate system in the pose of the i-th frame, respectively. LG-c It contains the camera pose of the current N frames, where N depends on the length of the currently tracked feature points that have been tracked and the set window width threshold, i = 1…N.

[0086] The camera-related error state vector is defined as follows:

[0087]

[0088] in and Let represent the camera's attitude angle error and position error in the world coordinate system during the pose of the i-th frame, respectively;

[0089] Each time a keyframe of the camera is obtained (i.e., a frame with sufficiently large translational or rotational motion), the camera pose state corresponding to the keyframe needs to be added to the camera-related state vector, and the state covariance matrix needs to be expanded. This represents the camera-related state vector after adding the camera pose state of the new keyframe, and the camera-related error state vector after adding the camera pose state of the new keyframe. Represented as:

[0090]

[0091] J LG The Jacobian matrix represents the relationship between the camera-related error state vector after adding the camera pose state of the new keyframes and the camera-related error state vector before amplification. These represent the camera's attitude angle error and position error in the world coordinate system during the pose of frame N+1, respectively.

[0092] J LG The format is as follows:

[0093]

[0094] Where I3 is an identity matrix of dimension 3. express The estimated value, express The estimated value.

[0095] Amplified state covariance matrix Represented as:

[0096]

[0097] Where P k Let I be the state covariance matrix before amplification. 6N+15 It is an identity matrix with dimensions 6N+15.

[0098] In inertial / visual integrated navigation, inertial navigation system information is used for state transfer, and visual information is used for observation. Various types of visual information can be used for observation, such as reprojection error, intensity error, epipolar constraint error, and trifocal tensor error. In this invention, the visual information used is the reprojection error of feature points. In one embodiment, an observation model for an inertial / visual integrated navigation system suitable for a Lie group state transformation Kalman filter is constructed, including:

[0099] Suppose that among a series of feature points extracted from the visual navigation information acquired by the camera, the j-th feature point f j The j-th feature point f contained in the i-th frame image j The three-dimensional coordinates in the camera coordinate system are: The j-th feature point f j The two-dimensional coordinates in the image are

[0100]

[0101] in Let X, Y, and Z be the X, Y, and Z coordinates of the j-th feature point in the i-th frame image in the camera coordinate system corresponding to the i-th frame image; This represents the coordinates of the j-th feature point in the world reference coordinate system; This represents the coordinates of the i-th camera position in the world reference coordinate system, where the i-th camera position corresponds to the i-th frame image; Represents the rotation matrix from the world reference coordinate system to the i-th camera coordinate system;

[0102]

[0103] in This represents the observed Gaussian white noise projected onto the camera plane by the j-th feature point in the i-th frame image;

[0104] The reprojection error of the j-th feature point in the i-th frame image is:

[0105]

[0106] in This is the estimated two-dimensional coordinate of the j-th feature point in the i-th frame of the image. It is expressed as follows:

[0107]

[0108] in Let X and Y be the estimated coordinates of the j-th feature point in the i-th frame image in the camera coordinate system corresponding to the i-th frame image, respectively.

[0109] Reprojection error The linearization formula is expressed as:

[0110]

[0111] in This represents the coordinates of the j-th feature point in the world coordinate system. This indicates reprojection position noise. and yes Jacobian matrices relative to the state vector and the feature point positions, respectively:

[0112]

[0113]

[0114] in for The estimated value;

[0115] Assume feature point f j M j The frame image contains f j The reprojection errors in all these images are stacked into a column, resulting in:

[0116]

[0117]

[0118] Assuming matrix A is a unitary matrix, the columns of A are... A basis of the left null space, multiplying the above expression on the left by A T We can obtain:

[0119]

[0120]

[0121] Stacked It is clearly full rank, therefore the dimension of the unitary matrix A is 2M. j ×(2M j -3), therefore The dimension is (2M) j -3)×1, using the transformed reprojection error Filtered observations and updates are then performed. In the above equation, the right side no longer includes the location of feature points, thus satisfying the observation equation pattern in Kalman filtering.

[0122] In one embodiment, the update equation for an inertial vision-integrated navigation system using a Lie group state transformation Kalman filter is constructed, including:

[0123] Suppose that the attitude error estimate of the inertial system is obtained by using a Kalman filter to perform state estimation based on the constructed system model and observation model. Nonlinear velocity and position error estimates and The filter velocity and position error states are updated according to the Lie group nonlinear velocity and position error definitions, and the update equations are as follows:

[0124]

[0125] Where exp(·) denotes the exponential mapping of the matrix.

[0126] It is understood that the state estimation method used in the above embodiments for the error state after the Lie group state transformation is the Kalman filter. In practical applications, other optimal estimation algorithms can also be used.

[0127] The navigation information output by the final navigation system of this invention includes, but is not limited to, navigation information such as the speed, position, and attitude information of the carrier.

[0128] To verify the effectiveness of the inertial-visual integrated navigation method based on Lie group state transformation (referred to as the LG-MSCKF algorithm) provided in this invention, inertial navigation and visual data from the open-source dataset KITTI are used as examples. The performance of the LG-MSCKF algorithm is compared with three traditional inertial / visual integrated navigation methods: multi-state constrained Kalman filtering (MSCKF), observability-constrained MSCKF (OC-MSCKF), and velocity-state transformation-based MSCKF (ST-MSCKF). The inertial navigation output frequency is 100Hz, the image frame rate is 10Hz, and the trajectory is as follows. Figure 2 As shown.

[0129] The horizontal position error results are as follows Figure 3 As shown, the heading angle error results are as follows: Figure 4 As shown, the statistical results are as follows: Figure 5 As shown. From Figures 3-5It can be seen that the three algorithms OC-MSCKF, ST-MSCKF, and LG-MSCKF all have higher position and attitude estimation accuracy than the MSCKF algorithm. The accuracy of OC-MSCKF and ST-MSCKF algorithms is comparable, but ST-MSCKF has slightly higher accuracy. The inertial vision integrated navigation method based on Lie group state transformation provided in this invention (referred to as the LG-MSCKF algorithm) has the highest accuracy. This is due to the more stringent definition of the filtered state error, which increases the transformation of velocity and position errors, thus improving the nonlinear processing capability of velocity and position errors and ensuring the long-term accuracy of filtering.

[0130] In one embodiment, an inertial vision-based integrated navigation device based on Lie group state transformation is provided, comprising:

[0131] The first module is used to input the navigation information of the carrier obtained by the inertial navigation system in the inertial vision integrated navigation system on the carrier, as well as the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system.

[0132] The second module is used to construct a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters;

[0133] The third module is used to construct an observation model for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters.

[0134] The fourth module is used to construct the update equations for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters.

[0135] The fifth module is used to perform state estimation using a Lie group state transformation Kalman filter based on the constructed system model and observation model, obtain the correction amount of the inertial / visual integrated navigation system, and update the navigation state according to the constructed update equation until the navigation ends.

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

[0137] On the other hand, the present invention provides a computer device including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of the inertial vision integrated navigation method based on Lie group state transformation provided in any of the above embodiments. 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. The processor of the computer device provides computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and 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 computer program in the non-volatile storage medium. The database of the computer device stores sample data. The network interface of the computer device is used for communication with external terminals via a network connection.

[0138] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, it implements the steps of the inertial vision integrated navigation method based on Lie group state transformation provided in any of the above embodiments.

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

[0140] Matters not covered in this invention are common knowledge.

[0141] The technical features of the above embodiments can be combined in any way. For the sake of brevity, 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.

[0142] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

[0143] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An inertial vision-integrated navigation method based on Lie group state transformation, characterized in that, include: The input carrier is the navigation information acquired by the inertial navigation system in the inertial vision integrated navigation system and the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system. A system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters is constructed, including: Constructing the state vector of an inertial vision-integrated navigation system and error state vector ,in The state vector is inertial-dependent. This is the camera-related state vector; Determine the inertial-dependent state vector ,as follows: In the formula, It is a Hamiton unit quaternion, representing the coordinate system from the inertial navigation coordinate system. b Set to the tangent plane coordinate system w The rotation of the system; It is the inertial navigation system relative to w The speed of the system is w Projection in the system; It is the inertial navigation system relative to w The location of the system is w Projection in the system, and These are zero bias of the gyroscope and zero bias of the accelerometer, respectively. Represents the visual navigation coordinate system c Connect to the inertial navigation coordinate system b The rotation of the system; Represents the visual navigation coordinate system c Inertial navigation coordinate system b The positional relationship between systems; The inertial-dependent error state vector is determined as follows: Each quantity in the formula represents b Tie w attitude error between systems Speed ​​error after transformation Position error after transformation Gyroscope zero bias error Accelerometer zero bias error Attitude error between C and B systems Position error calculated by the camera ; speed error after transformation Position error after transformation for: in and These represent the velocity error vector and the position error vector, respectively. Indicates cross product; The differential equation for the inertial-dependent error state vector is as follows: in Represents the system matrix. Represents the noise driving matrix. Represents the inertial-related noise vector; in and These are the measurement white noise values ​​for the gyroscope and accelerometer, respectively. and These are the driving white noise corresponding to the zero bias of the gyroscope and accelerometer, respectively. and These are the relative attitude angle noise and relative displacement noise between the inertial navigation system and the camera, respectively. in and Represented as the Earth's rotation vector and gravity vector respectively. w The projection of the system, It is an identity matrix with dimension 3. This represents an antisymmetric matrix composed of vector elements. express The estimated value, express The estimated value; in Indicates from b Tie w The attitude rotation matrix of the system; Determine the camera-related state vector , means as follows: in and They represent the first i Rotation and displacement between the camera coordinate system and the world coordinate system in frame pose Includes current N The camera pose of the frame. N Depending on the length of the currently tracked feature points that have already been tracked and the set window width threshold, i =1… N ; The camera-related error state vector is defined as follows: in and They represent the first i The camera's attitude angle error and position error in the world coordinate system during frame pose; Each time a keyframe of the camera is obtained (i.e., a frame with sufficiently large translational or rotational motion), the camera pose state corresponding to the keyframe needs to be added to the camera-related state vector, and the state covariance matrix needs to be expanded. This represents the camera-related state vector after adding the camera pose state of the new keyframe, and the camera-related error state vector after adding the camera pose state of the new keyframe. Represented as: in The Jacobian matrix represents the relationship between the camera-related error state vector after adding the camera pose state of the new keyframes and the camera-related error state vector before amplification. , They represent the first N +1 frame pose: camera attitude angle error and position error in world coordinate system; The observation model for an inertial vision-integrated navigation system that utilizes a Lie group state transformation Kalman filter includes: Suppose that among a series of feature points extracted from the visual navigation information acquired by the camera, the i-th... Feature points Included in the In the frame image, the first Feature points The three-dimensional coordinates in the camera coordinate system are: , No. Feature points The two-dimensional coordinates in the image are : in , , The first The first frame of the image The feature point at the th ... In the camera coordinate system corresponding to the frame image X , Y , Z coordinate; Indicates the first The coordinates of each feature point in the world reference coordinate system; Indicates the first The coordinates of the position of the i-th camera in the world reference coordinate system, where the i-th camera position is... The position of the camera and the first Frame image correspondence; Represents the world reference coordinate system to the 1st Rotation matrix of each camera coordinate system; in Indicates the first The first frame of the image Gaussian white noise projected onto the camera plane from each feature point; No. The first frame of the image The reprojection error of each feature point is: in For the first The first frame of the image Two-dimensional coordinate estimates of each feature point It is expressed as follows: in , The first The first frame of the image The feature point at the th ... In the camera coordinate system corresponding to the frame image X , Y Estimated coordinates; Reprojection error The linearization formula is expressed as: in Indicates the first The coordinates of each feature point in the world coordinate system This indicates reprojection position noise. and yes Relative to the first The state vector of the frame image and the Jacobian matrix of the feature point locations: in for The estimated value; Assuming feature points Together The frame image contains, The reprojection errors in all these images are stacked into a single column, resulting in: Assumption A The matrix is ​​a unitary matrix. A The column composition A basis of the left null space, multiplied by the above expression on the left. A T We can obtain: Stacked It is clearly a full-rank column matrix, therefore a unitary matrix. A The dimension is ,thus The dimension is Using the transformed reprojection error Perform filtered observations and updates; The update equations for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters are constructed, including: Suppose that the attitude error estimate of the inertial system is obtained by using a Kalman filter to perform state estimation based on the constructed system model and observation model. Nonlinear velocity and position error estimates and The filter velocity and position error states are updated according to the Lie group nonlinear velocity and position error definitions, and the update equations are as follows: in Represents the exponential mapping of a matrix; Based on the constructed system model and observation model, state estimation is performed using a Lie group state transformation Kalman filter to obtain the correction amount of the inertial / visual integrated navigation system. The navigation state is then updated according to the constructed update equation until navigation ends.

2. The inertial vision integrated navigation method based on Lie group state transformation according to claim 1, characterized in that, Hamiton unit quaternion ,from b Tie w The attitude rotation matrix of the system The relationship between quaternions containing errors and true quaternions is as follows: in This is quaternion multiplication. express The estimated value, express The estimated value, represent b Tie w Attitude error between systems Represents the identity matrix.

3. The inertial vision integrated navigation method based on Lie group state transformation according to claim 1, characterized in that, The format is as follows: in It is an identity matrix with dimension 3. express The estimated value, express The estimated value.

4. The inertial vision integrated navigation method based on Lie group state transformation according to claim 1, characterized in that, Amplified state covariance matrix Represented as: in The state covariance matrix before amplification. For a dimension of 6 N An identity matrix of +15.

5. An inertial vision integrated navigation device based on Lie group state transformation, used to implement the inertial vision integrated navigation method based on Lie group state transformation as described in any one of claims 1 to 4, characterized in that, include: The first module is used to input the navigation information of the carrier obtained by the inertial navigation system in the inertial vision integrated navigation system on the carrier, as well as the visual navigation information synchronously acquired by the camera in the inertial vision integrated navigation system. The second module is used to construct a system model for an inertial vision-integrated navigation system suitable for Lie group state transformation Kalman filters; The third module is used to construct an observation model for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters. The fourth module is used to construct the update equations for an inertial vision-integrated navigation system that is suitable for Lie group state transformation Kalman filters. The fifth module is used to perform state estimation using a Lie group state transformation Kalman filter based on the constructed system model and observation model, obtain the correction amount of the inertial / visual integrated navigation system, and update the navigation state according to the constructed update equation until the navigation ends.

6. A computer device comprising a memory and a processor, the memory storing a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the inertial vision integrated navigation method based on Lie group state transformation as described in any one of claims 1 to 4.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the inertial vision integrated navigation method based on Lie group state transformation as described in any one of claims 1 to 4.

Images