Multi-source information mixed type integrated navigation method under visual aid

Abstract

The application discloses a kind of multi-source information mixed combination navigation method under visual assistance, gyro angular velocity and accelerometer linear acceleration information are provided by inertial navigation system;Speed measuring equipment obtains the real-time speed information of mobile object;Global navigation satellite system provides position information and second pulse signal;Visual sensor provides the image information required for navigation;Strapdown inertial navigation algorithm update module carries out navigation solution;Kalman filtering navigation algorithm module estimates the state variable of system in real time;Fault detection and elimination module detects whether the system fails;Image feature detection and tracking module realizes the extraction tracking of image key frame;Sliding window graph optimization algorithm module updates covariance matrix;Loop detection module gives similarity score through image information to judge whether there is loop;Navigation result output module outputs correct navigation information.The problem that existing navigation mode cannot position and orient for a long time is solved.

Description

Technical Field

[0001] This invention belongs to the field of medium- and high-precision integrated navigation technology, and relates to a multi-source information hybrid integrated navigation method with visual assistance. Background Technology

[0002] With the explosive growth of drones, unmanned vehicles, and mobile robots, vision-assisted inertial navigation technology has gradually become a research hotspot for autonomous localization and orientation. Veth proposed a vision-assisted low-precision inertial navigation algorithm using multi-dimensional random feature point tracking, but its biggest drawback is that the number of tracked feature points must remain constant. In the same year, Mourikis proposed a multi-state constrained Kalman filter algorithm, which suffers from inconsistent filtering estimates. Bloesch proposed a robust visual-inertial odometry method, utilizing an extended Kalman filter architecture to tightly couple visual and inertial information, reducing computational complexity while maintaining accuracy. Mur-Artel, the designer of ORB-SLAM, incorporated inertial information into the algorithm framework using pre-integration theory, designing a vision / inertial integrated navigation algorithm with relocalization and loop closure detection functions. Regarding pre-integration theory, methods for merging integral increments and phase covariance matrices are currently lacking. Based on this theory, Shen Shaojie proposed the Visuel-inertiel Navication System (VINS) algorithm, which has seen initial applications in the field of low-precision autonomous navigation. Currently, in terms of autonomous navigation information fusion, many domestic scholars have optimized and improved existing multi-sensor combined navigation schemes, but the application scenarios are concentrated in the field of medium and low precision inertial navigation. There is almost no research in the field of vehicle-mounted long-endurance and high-precision autonomous navigation, which is still difficult to meet the application requirements of actual engineering. Summary of the Invention

[0003] The purpose of this invention is to provide a multi-source information hybrid navigation method with visual assistance, which solves the problem in existing navigation methods where the autonomous navigation error slowly diverges, resulting in the inability to locate and orient for a long time.

[0004] The technical solution adopted in this invention is a multi-source information hybrid navigation method with visual assistance, which specifically includes the following steps:

[0005] Step 1: The gyro inertial navigation system outputs the angular velocity in the carrier coordinate system. and acceleration A linear error model is used to calibrate and compensate the output pulse, and the output angular velocity is... and acceleration

[0006] Step 2, convert the angular velocity output in Step 1 to... and acceleration The speed is calculated in the strapdown inertial navigation algorithm update module. and location The updated output;

[0007] Step 3: Measure the vehicle speed using a speed measuring device. Let the pulse output by the speed measuring device per unit time be... according to Solve for the velocity vectors of the load system.

[0008] Step 4: Based on the angular rate output by the inertial navigation system The vehicle speed output by speed measuring device 2 Perform dead reckoning and output the velocity of the velocity measurement device in the navigation coordinate system.

[0009] Step 5: Provide real-time location information to the vehicle via the Global Navigation Satellite System. And the second pulse signal, based on the time t when the inertial navigation receives the position information data frame sent by the Global Navigation Satellite System. GNSS The second pulse time t corresponding to this frame of position data PPS Calculate the time delay δt between the output of the global navigation satellite system and the inertial navigation system.

[0010] Step 6, in the Kalman filter algorithm module, inertial navigation speed Speed ​​measurement equipment The velocity measurement δv is obtained after subtraction. n Inertial navigation position Location provided by Global Navigation Satellite System 3 The position measurement δp is obtained after subtraction. n The Kalman filter algorithm module is based on the velocity measurement δv n and position measurement δp n The optimal Kalman filter technique is used to estimate the state variables of the inertial navigation system in real time.

[0011] Step 7: Acquire image information through a vision sensor, remove distortion from the acquired image, obtain the correct coordinates of the three-dimensional spatial points projected onto the original image, and read all the correct coordinates to form the image after distortion removal;

[0012] Step 8: Extract feature points from the image after distortion removal in Step 7;

[0013] Step 9: Track the image feature points extracted in Step 8 using optical flow. Analyze the pixel changes based on the changes in pixel intensity and estimate the relationship between the corresponding feature points.

[0014] Step 10: Loop closure detection based on bag-of-words model. The similarity score given by the image information is used to determine whether the current scene is a previously visited location, to determine whether loops exist, and to retain keyframes with loops.

[0015] Step 11: Combine the optical flow error equation with the reprojection error equation of the keyframe into a minimum objective function, and solve for the solution that minimizes the error function.

[0016] Step 12: Use sliding window graph optimization to perform global optimization of the camera trajectory, construct graph optimization and perform maximum a posteriori estimation, estimate the system state variables, and update the covariance matrix in real time;

[0017] Step 13: After comparing the state variables of the system estimated in real time by Kalman filtering with the state variables output by the sliding window graph optimization algorithm, the results are output to the integrated navigation result output module.

[0018] The invention is further characterized by:

[0019] The specific process of step 6 is as follows:

[0020] Step 6.1, set the inertial navigation speed Speed ​​measured by speed measuring equipment The velocity measurement δv is obtained after subtraction. n , inertial navigation position Location provided by Global Navigation Satellite System 3 The position measurement δp is obtained after subtraction. n The specific formula is as follows:

[0021]

[0022] Step 6.2: Select the error state of the inertial navigation system as the velocity error δv. n Attitude error vector φ, position error δp n gyroscope zero bias ε b accelerometer zero point Simultaneously considering the odometer's scaling factor error δK OD , Heading installation angle error α ψ And pitch installation angle error α θ Then the error vector X of the inertial navigation system is shown in the following formula (2):

[0023]

[0024] In the formula, φ E φ U φ N The attitude error angles are, in order, eastward, upward, and northward.

[0025] The state equation of the inertial navigation system is shown in equation (3):

[0026]

[0027] Among them, W G This is gyroscope noise; W A Accelerometer noise;

[0028] The state transition matrix of the time update equation is shown in formula (4) below:

[0029]

[0030] Among them, F SINS This is the transition matrix corresponding to the error equation of the inertial navigation system.

[0031] The specific process of step 7 is as follows:

[0032] Step 7.1: Project the 3D spatial point onto the normalized image plane. Let the normalized coordinates of the 3D spatial point be [x, y]. T The radial and tangential distortions of points on the normalized plane are calculated as shown in the following formula (5):

[0033]

[0034] In the formula, [x corrected ,y corrected ] T The normalized coordinates of the point after distortion are represented by k1, k2, and k3, which are radial distortion parameters, and p1 and p2 are tangential distortion parameters.

[0035] Step 7.2: Project the distorted point obtained in Step 7.1 onto the pixel plane through the intrinsic parameter matrix to obtain the correct position of the point on the image. Read all the correct coordinates to form the distorted image. The coordinates are shown in the following formula (6):

[0036]

[0037] In the formula, (x, y) represents the correct position coordinates, [c x c y ] T This indicates a translation of c from the pixel origin. x c y pixel, f x f y This represents the camera's intrinsic parameters.

[0038] The specific process of step 8 is as follows:

[0039] Step 8.1: Distribute n feature points evenly across an image. Place n non-overlapping circles of radius r across the entire image, with each feature point located at the center of one circle. At this point, the pixel distance between each feature point and its nearest neighbor is d = 2r. The specific calculation method is as follows:

[0040]

[0041] In the formula, H represents the image pixel height; W represents the image pixel width;

[0042] Step 8.2, calculate the uniformity coefficient C of the feature point distribution. The specific calculation method is shown in the following formula (8):

[0043]

[0044] In the formula, C is the uniformity evaluation coefficient, and x i This represents the pixel distance between the i-th feature point and its nearest neighbor feature point;

[0045] Step 8.3: Select the corner point with the smallest uniformity evaluation coefficient within each image block as the feature point.

[0046] The specific process of step 9 is as follows:

[0047] Step 9.1: Calculate the pixel velocity in the camera coordinate system using the optical flow method, as shown in the following formula:

[0048]

[0049] in:

[0050]

[0051] In the formula, I x I y , These represent the grayscale values ​​of the pixel in the image along the x and y directions, respectively, and t. i The partial derivative at time i, where i takes the values ​​1, 2, ..., k; u and ν are the motion velocities of pixels in the image along the x and y axes, respectively.

[0052] Discretize the time t and solve for the position X of the target pixel in a series of image frames. c The calculation formula is shown in (10):

[0053]

[0054] Step 9.2, consider a three-dimensional spatial point P and its projection point p. The coordinates of spatial point P are [X,Y,Z,1]. T The pixel coordinates of the projection point are x = [x, y]. TThe corresponding pixel coordinates of point x′ in the second frame image are x′=[x′,y′] T Assuming the motion between the first and second frames is a rotation R and a translation vector t, then the two frames have the following homogeneous transformation relationship:

[0055] x′=Rx+t (11);

[0056] By accumulating the poses of all ordinary frames between two keyframes, the relative pose change between keyframes i and j under optical flow constraints is derived as follows:

[0057] x j =Tx i (12);

[0058] In the formula, T represents containing The transformation matrix.

[0059] The specific process of step 10 is as follows:

[0060] Take a prior similarity s(v) t ,v t-Δt The similarity score between a keyframe at a given moment and a keyframe at the previous moment is represented by the following formula:

[0061]

[0062] In the formula, s(v t ,v t-Δt ) represents prior similarity. Indicates the similarity between the current frame and a previous keyframe;

[0063] if If a loop is found, the keyframe is considered to have a possible loop; otherwise, it is considered not to have a loop and is discarded.

[0064] The specific process of step 11 is as follows:

[0065] Step 11.1: Based on the initial pose given in step 9.2, calculate the difference between the accumulated initial pose between keyframes and the pose after feature matching to obtain the optical flow error equation:

[0066] Based on camera model relationships:

[0067] sx′=Kexp(ξ ^ )P (14);

[0068] In the formula, K is the intrinsic parameter of the camera, P is the 3D coordinate of the pixel obtained by projection, and s is the depth of the feature point; Thus, the error equation between keyframes k and k+1, solved by the optical flow tracing method, can be obtained:

[0069]

[0070] Step 11.2: Let p2 be the theoretical projection point of spatial point P in the right figure, and p2′ be the actual projection point of spatial point P onto the right figure based on the pose estimated by feature matching. There is a distance between the two projection points p2 and p2′. By adjusting the camera pose, the distance between the projection points p2 and p2′ is minimized. i The coordinates are [X i ,Y i Z i ,1] T The pixel coordinates of the projection point are u i =[u i ,v i ] T From the camera coordinate system relationship in the camera model, we can see that:

[0071] su i =Kexp(ξ ^ )P i (16);

[0072] Reprojection error equation for keyframes:

[0073]

[0074] Step 11.3 combines the constraints of the optical flow error equation and the reprojection error equation of the keyframe into a minimized objective function:

[0075]

[0076] In the formula, M is the total number of optical flow tracking points, K is the total number of keyframes after loop closure detection, and R represents the total number of feature points.

[0077] We solve equation (19) nonlinearly to find the optimal camera pose ξ that minimizes the error function:

[0078]

[0079] The specific process of step 12 is as follows:

[0080] Step 12.1: Represent the relationship between constraints and camera poses by constructing a pose graph. Assume the camera poses at two time points are ξ. i and ξ j The relative motion between the two moments is Δξ ij The following can be written in the form of a Lie group:

[0081] T ij =T i -1 Tj (20);

[0082] Step 12.2: The camera's motion process has been constructed into a graphical model. The optimal system state variables are obtained by minimizing the objective function composed of the error terms, and the covariance is updated in real time. Specifically:

[0083] Let ε represent the set of all edges in the sliding window graph optimization model, then the minimum objective function is:

[0084]

[0085] In the formula, e ij Let Σ represent the position error from time i to time j, and let Σ be the covariance matrix.

[0086] The beneficial effects of this invention are that, addressing the slow divergence of traditional vehicle positioning errors, it explores and designs a vision-assisted inertial-based multi-source information hybrid integrated navigation method. This method rationally introduces visual information into the traditional high-precision filtered integrated navigation algorithm architecture. Compared with the traditional filtered integrated navigation algorithm architecture, it further improves the integrated navigation accuracy while ensuring high output frequency, real-time navigation information, and robustness. The real-time performance, accuracy, and complexity of the designed integrated navigation architecture are comprehensively evaluated through actual closed-loop sports car tests. Attached Figure Description

[0087] Figure 1 This is a schematic diagram of the navigation system used in the vision-assisted multi-source information hybrid integrated navigation method of the present invention;

[0088] Figure 2 This is a diagram showing the relationship between spatial point P and two frames of images in the vision-assisted multi-source information hybrid navigation method of this invention.

[0089] Figure 3 It is the pose graph constructed in the vision-assisted multi-source information hybrid integrated navigation method of the present invention;

[0090] Figure 4 This is a curve comparing the actual trajectory and the combined navigation trajectory of the multi-source information hybrid navigation method under visual assistance of the present invention.

[0091] In the diagram, 1. Inertial Navigation System, 2. Velocity Measurement Equipment, 3. Global Navigation Satellite System, 4. Visual Sensor, 5. Strapdown Inertial Navigation Algorithm Update Module, 6. Kalman Filter Algorithm Module, 7. Fault Detection and Removal Module, 8. Feature Detection and Tracking Module, 9. Sliding Window Optimization Algorithm Module, 10. Loop Closure Detection Module, and 11. Navigation Result Output Module. Detailed Implementation

[0092] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0093] The present invention provides a vision-assisted multi-source information hybrid navigation method, employing a navigation system such as... Figure 1 As shown, it comprises 11 modules. The inertial navigation system 1 provides gyro angular velocity and accelerometer linear acceleration information; the velocity measurement device 2 (traditional sensor VMS) acquires real-time velocity information of moving objects; the global navigation satellite system 3 provides high-precision position information and second pulse signals; the visual sensor 4 (including a depth camera and a monocular camera) provides image information required for navigation; the strapdown inertial navigation algorithm update module 5 performs navigation calculations based on the input angular velocity and acceleration; the Kalman filter navigation algorithm module 6 estimates the system's state variables in real time and updates the covariance matrix; the fault detection and elimination module 7 detects system faults by comparing the velocity measurement device 2, the global navigation satellite system 3, and the strapdown inertial navigation algorithm update module 5; the image feature detection and tracking module 8 extracts keyframes from images and tracks these keyframes during motion; the sliding window graph optimization algorithm module 9 uses image information to estimate the system's state variables and update the covariance matrix; the loop closure detection module 10 uses image information to provide a similarity score to determine if the current scene is a previously visited location, achieving pose correction; and the navigation result output module 11 outputs correct navigation information.

[0094] The navigation method of the aforementioned vision-assisted multi-source information hybrid integrated navigation system specifically includes the following steps:

[0095] Step 1: The gyro inertial navigation system 1 outputs the angular velocity in the carrier coordinate system. and acceleration A linear error model is used to calibrate and compensate the output pulse, i.e.

[0096]

[0097] in, These are the temperature-compensated output pulses from the gyroscope and accelerometer, respectively; K G K A These are the calibration and installation matrices for the gyroscope and accelerometer, respectively; ε b , These are the zero points of the gyroscope and accelerometer, respectively;

[0098] Step 2, the strapdown inertial navigation algorithm update module 5 updates the module based on the input angular velocity. acceleration Perform navigation calculations to achieve speed. and location Updated output:

[0099]

[0100] in:

[0101]

[0102]

[0103]

[0104] Where L, λ, and h represent the latitude, longitude, and altitude of the carrier, respectively; the subscripts E, N, and U indicate the east, north, and celestial directions along the local coordinate system; R M R N These represent the radii of the local meridian and ecliptic where the carrier is located; and the initial values ​​of velocity and position. Provided by Global Navigation Satellite System 3 (GNSS); Attitude matrix Alignment is completed via a strapdown inertial navigation system. The gravity value is calculated using the standard gravity model. The calculation formula generally adopts the WGS-84 model, as shown in formula (3) below:

[0105]

[0106] in, The unit of calculation is m / s 2 .

[0107] Step 3: Speed ​​measuring device 2 measures the vehicle speed. Let the pulse output by the speed measuring device per unit time be... The velocity vector under the load system is:

[0108]

[0109] Where, k VMS This refers to the scale coefficient of the speed measuring device;

[0110] Step 4, based on the angular rate output by inertial navigation system 1 The vehicle speed output by speed measuring device 2 Perform dead reckoning and output the velocity of the velocity measurement device in the navigation coordinate system. The calculation formula is as follows:

[0111]

[0112] Among them, the attitude matrix The update process can be obtained directly in the Strapless Navigation Algorithm Update Module 5.

[0113] Step 5: Global Navigation Satellite System 3 can provide high-precision location information to the vehicle in real time. The second pulse signal is used to synchronize the time of inertial navigation system 1 with that of global navigation satellite system 3; the delay time δt output by global navigation satellite system 3 relative to inertial navigation is shown in the following formula (6):

[0114] δt=t GNSS -t PPS (6);

[0115] In the formula, t GNSS The time at which the inertial navigation system receives a position information data frame transmitted by the Global Navigation Satellite System; t PPS When the second pulse corresponds to this frame of position data, δt is obviously ≥ 0.

[0116] Step 6, in Kalman filter algorithm module 6, inertial navigation speed With speed measuring device 2 speed The velocity measurement δv is obtained after subtraction. n Inertial navigation position Location provided by Global Navigation Satellite System 3 The position measurement δp is obtained after subtraction. n The Kalman filter algorithm module uses velocity measurement δv as a basis. n and position measurement δp n The optimal Kalman filter technique is used to estimate the state variables of the inertial navigation system in real time.

[0117] Step 6.1, Inertial Navigation Speed With speed measuring device 2 speed The velocity measurement δv is obtained after subtraction. n Inertial navigation position Location provided by Global Navigation Satellite System 3 The position measurement δp is obtained after subtraction. n The specific formula is as follows:

[0118]

[0119] Step 6.2, select the error state of inertial navigation system 1 as velocity error δv n Attitude error vector φ, position error δp n gyroscope zero bias ε b accelerometer zero point Simultaneously considering the odometer's scaling factor error δK OD , Heading installation angle error α ψ And pitch installation angle error α θ The error vector X of the inertial navigation system is shown in formula (8) below:

[0120]

[0121] In the formula, φ E φ U φ N The attitude error angles are, in order, eastward, upward, and northward.

[0122] The state equation of the inertial navigation system is shown in equation (9):

[0123]

[0124] Among them, W G This is gyroscope noise; W A This is accelerometer noise.

[0125] The state transition matrix of the time update equation is shown in formula (10) below:

[0126]

[0127] Among them, F SINS Let be the transition matrix corresponding to the error equation of the inertial navigation system, where the non-zero elements are:

[0128]

[0129]

[0130]

[0131] F 1，10 =-C 1,1 ,F 1，11 =-C 1,2 ,F 1，12 =-C 1,3

[0132] F 2，10 =-C 2,1 ,F 2，11 =-C 2,2 ,F 2，12 =-C 2,3

[0133] F 3，10 =-C 3,1 ,F 3，11 =-C 3,2 ,F 3，12 =-C 3,3

[0134] F 4,2 =-f sf,z ,F 4,3 =f sf,y ,

[0135]

[0136] F 5，1 =f sf,z ,F 5，3 =-f sf,x ,

[0137]

[0138] F 6，1 =f sf,y ,F 6，2 =-f sf,x ,

[0139]

[0140] F 4，13 =C 1,1 ,F 4，14 =C 1,2 ,F 4，15 =C 1,3

[0141] F 5，13 =C 2,1 ,F 5，14 =C 2,2 ,F 5，15 =C 2,3

[0142] F 6，13 =C 3,1 ,F 6，14 =C 3,2 ,F 6，15 =C 3,3

[0143] F 7，4 =1,F 8,5 =1,F 9,6 =1

[0144]

[0145] Among them, F i,j Represents matrix F SINS The element in the i-th row and j-th column, parameter ω represents the components of the angular velocity of the n-system relative to the i-system along the x, y, and z axes. ie,x ω ie,y ω ie,z ν represents the components of the Earth's rotational angular rate along the x, y, and z axes. E ν N ν UR represents the velocity vector in the "East-North-Sky" direction. N R represents the length of a geographic perpendicular. M Where h is the principal curvature radius of the Earth's meridian, L represents the geographic latitude, and f is the altitude. sf For the accelerometer specific force, f sf,x f sf,y f sf,z C represents the specific force of the accelerometer along the x, y, and z axes. i,j This represents the element in the i-th row and j-th column of the attitude matrix.

[0146] Step 7: Acquire image information through the vision sensor 4. The original images acquired from the lens all contain radial and tangential distortion. The acquired images are then distorted to obtain the correct coordinates of the three-dimensional points projected onto the original image. All correct coordinates are then read to form the distorted image. The specific process is as follows:

[0147] Step 7.1: Project the 3D spatial point onto the normalized image plane. Let the normalized coordinates of the 3D spatial point be [x, y]. T The radial and tangential distortions of points on the normalized plane are calculated as shown in the following formula (11):

[0148]

[0149] In the formula, [x corrected ,y corrected ] T This represents the normalized coordinates of the distorted point, where k1, k2, and k3 are radial distortion parameters, and p1 and p2 are tangential distortion parameters. In practical applications, these coefficients can be flexibly retained.

[0150] Step 7.2: Project the distorted point obtained in Step 7.1 onto the pixel plane through the intrinsic parameter matrix to obtain the correct position of the point on the image. Read all the correct coordinates to form the distorted image. The coordinates are shown in the following formula (12):

[0151]

[0152] In the formula, (x, y) represents the correct position coordinates, and pc x c y ] T This indicates a translation of c from the pixel origin. x c y pixel, f x f y This represents the camera's intrinsic parameters.

[0153] Step 8: Extract feature points from the distortion-removed image in Step 7. A fast, uniform feature point extraction method is used. The image is divided into several sub-image blocks of equal size based on the target number of feature points. For sub-image blocks without feature points, the nearest neighbor blocks are used for compensatory division. Finally, the corner point with the largest response value within each image block is selected as the feature point, achieving a uniform distribution of feature points. The specific process is as follows:

[0154] Step 8.1: Distribute n feature points evenly across an image. Place n non-overlapping circles of radius r across the entire image, with each feature point located at the center of one circle. At this point, the pixel distance between each feature point and its nearest neighbor is d = 2r. The specific calculation method is as follows:

[0155]

[0156] In the formula, H represents the image pixel height; W represents the image pixel width.

[0157] Step 8.2: Calculate the uniformity coefficient of feature point distribution. The specific calculation method is shown in the following formula:

[0158]

[0159] In the formula, C is the uniformity evaluation coefficient, and x i This represents the pixel distance between the i-th feature point and its nearest neighbor. A smaller value for C indicates better uniformity, while a larger value indicates worse uniformity.

[0160] Step 8.3: Select the corner point with the smallest uniformity evaluation coefficient within each image block as the feature point.

[0161] Step 9: Track the image feature points extracted in Step 8 using optical flow. Optical flow is the velocity of a pixel in the camera coordinate system. Analyze the changes in pixel intensity to estimate the relationship between corresponding feature points.

[0162] Step 9.1: Calculate the pixel's motion velocity in the camera coordinate system using the optical flow method, as shown in the following formula:

[0163]

[0164] in:

[0165]

[0166] In the formula, I x I y , These represent the grayscale values ​​of the pixel in the image along the x and y directions, respectively, and t. i The partial derivative at time i, where i takes the value of u and ν are the movement velocities of pixels in the image along the x and y axes, respectively;

[0167] Discretize the time t and solve for the position X of the target pixel in a series of image frames. c The calculation formula is shown in (17). In practice, if the solution cannot be obtained in one iteration, multiple iterations of the above equation are required.

[0168]

[0169] Step 9.2: Estimate the relationship between corresponding feature points. Consider a 3D spatial point P and its projection point p. The coordinates of spatial point P are [X,Y,Z,1]. T The pixel coordinates of the projection point are x = [x, y]. T The corresponding pixel coordinates of point x′ in the second frame image are x′=[x′,y′] T Assuming the motion between the first and second frames is represented by rotation R and translation vector t, then the following homogeneous transformation relationship exists between them:

[0170] x′=Rx+t (18);

[0171] By accumulating the poses of all ordinary frames between two keyframes, the relative pose change between keyframes i and j under optical flow constraints can be derived as follows:

[0172] x j =Tx i (19);

[0173] In the formula, T represents containing The transformation matrix.

[0174] Step 10, loop closure detection based on the Bag of Words (BoW) model, is essentially an appearance-based loop closure detection algorithm. It uses image information to provide a similarity score to determine if the current scene is a previously visited location, thus determining if a loop exists and retaining keyframes containing loops. Specifically:

[0175] Step 10.1, take a prior similarity s(v) t ,v t-Δt This represents the similarity between a keyframe image at a certain moment and a keyframe at the previous moment. The similarity score between the current frame and other keyframes is calculated using the following formula:

[0176]

[0177] In the formula, s(v t ,v t-Δt ) represents prior similarity. This indicates the similarity between the current frame and a previous keyframe.

[0178] Step 10.2, if If a loop is found, the keyframe is considered to have a possible loop; otherwise, it is considered not to have a loop and is discarded.

[0179] Step 11: Combine the constraints such as the optical flow error equation and the reprojection error equation of the keyframe into a minimum objective function, and solve for the solution that minimizes the error function.

[0180] Step 11.1: Based on the initial pose given in step 9.2, the difference between the accumulated initial pose between keyframes and the pose after feature matching is calculated to obtain the optical flow error equation.

[0181] Based on camera model relationships:

[0182] sx′=Kexp(ξ^)P (21);

[0183] In the formula, K is the intrinsic parameter of the camera, P is the 3D coordinate of the pixel obtained by projection, and s is the depth of the feature point. Thus, the error equation between keyframes k and k+1, solved by the optical flow tracing method, can be obtained:

[0184]

[0185] Step 11.2, the reprojection error is obtained by projecting the two-dimensional coordinates of the projection plane (the position obtained in step 7) and the position of the spatial point onto the two-dimensional plane, thus deriving the reprojection error equation of the keyframe.

[0186] The relationship between spatial point P and two frames of images is as follows: Figure 2 As shown in the diagram, p2 is the theoretical projection point of spatial point P in the right figure, and p2′ is the actual projection point of spatial point P onto the right figure based on the pose estimated by feature matching. The two projection points are some distance apart; this distance is minimized by adjusting the camera pose. Considering spatial point P and projection point p, we use Lie algebra ξ to describe the camera pose. i The coordinates are [X i ,Y i Z i ,1] T The pixel coordinates of the projection point are u i =[u i ,v i ] T From the camera coordinate system relationship in the camera model, we can see that:

[0187] su i =Kexp(ξ^)P i (twenty three);

[0188] Reprojection error equation for keyframes:

[0189]

[0190] Step 11.3 combines the constraints of the optical flow error equation and the reprojection error equation of the keyframe into a minimized objective function:

[0191]

[0192] In the formula, M is the total number of optical flow tracking points, K is the total number of keyframes after loop closure detection, and R represents the total number of feature points.

[0193] We solve equation (26) nonlinearly to find the optimal camera pose ξ that minimizes the error function:

[0194]

[0195] Step 12: Use a sliding window graph optimization method to globally optimize the camera trajectory. Construct a graph optimization and perform maximum a posteriori estimation to estimate the system's state variables and update the covariance matrix in real time. The specific steps are as follows:

[0196] Step 12.1, representing the relationships between constraints and camera poses by constructing a pose graph, such as... Figure 3 As shown. The idea is to control the motion between camera frames. 12 ,Τ 23 ,Τ 3- ,…Τ ij ,Τ j- ... serve as constraints between nodes, and then the camera poses ξ1, ξ2, ξ3, ... ξ of the nodes are obtained based on these constraints. i ,ξ j ,…,ξ n The pose graph optimization model consists of n nodes and edges. Nodes represent the pose at each time step, which are the optimization variables. The edges connecting the nodes represent the transformation constraints between poses, i.e., the changes in camera pose within that time step, typically representing the error term. Assume the camera pose at two time steps is ξ. i and ξ j The relative motion between the two moments is Δξ ij The following can be written in the form of a Lie group:

[0197] T ij =T i -1 T j (27);

[0198] In the formula, T ij Let T be the Lie group form of the relative motion between two moments i and j; iLet T be the Lie group form of the camera pose at time i; j Let j be the Lie group form of the camera pose at time j.

[0199] The above formula represents an ideal situation and contains a certain error, which requires further discussion of the error e. ij The derivative of ξ with respect to ξ can be used to construct the least squares error e. ij for:

[0200]

[0201] In the formula, Where R represents rotation and t is the translation vector.

[0202] e can be derived ij Regarding camera pose ξ i and ξ j Jacobian matrix:

[0203]

[0204] It has no practical significance and is generally about the following:

[0205]

[0206] In the formula, and Let represent the Lie algebraic forms of the camera's rotation matrix and translation vector, respectively.

[0207] Step 12.2: The camera's motion process has been constructed into a graphical model. The optimal system state variables are obtained when the objective function composed of the error terms is minimized, and the covariance is updated in real time.

[0208] Let ε represent the set of all edges in the sliding window graph optimization model, then the minimum objective function is:

[0209]

[0210] In the formula, e ij Let Σ represent the position error from time i to time j, and let Σ be the covariance matrix.

[0211] The camera trajectory optimization problem is to find the set of optimization variables that minimizes all objective functions.

[0212] Step 13: After comparing the state variables of the system estimated in real time by Kalman filtering technology with the state variables output by the sliding window graph optimization algorithm, the results are output to the integrated navigation result output module 11.

[0213]

[0214] In the formula, Let ξ represent the state variable in the Kalman filter. * ΔX represents the state variables optimized by the sliding window diagram, and ΔX represents the difference matrix of the state variables.

[0215] Compare each element in ΔX with the state variable constraint value δ. If it exceeds this value, then it is considered... It has diverged, using ξ * As the navigation output result; if it is less than this value, it is considered... Reliable, easy to use As a navigation output.

[0216] The visual-assisted inertial-based multi-source information hybrid integrated navigation architecture designed in this invention is as follows: Figure 1 As shown, the invention's key feature is the integrated navigation framework that incorporates visual information. This framework retains the multi-source sensor filtering integrated navigation architecture, eliminating the need for pre-integration processing of inertial navigation data to achieve high-frequency output of navigation information. Simultaneously, this framework acquires real-time environmental information through visual sensors. When the navigation environment is favorable and interference-free, visual-assisted integrated navigation positioning can be achieved. Combined with sliding window optimization and loop closure detection techniques, the navigation accuracy of the traditional filtering framework is further improved. If visual information is missing, the system can be re-initialized and switch to the filtering integrated navigation module. The open-loop architecture ensures continuous and stable output from the system.

[0217] This invention presents a vision-assisted inertial-based multi-source information hybrid integrated navigation architecture, primarily based on Kalman filtering and sliding window optimization techniques. Kalman filtering has been widely applied in the integrated navigation of medium- and high-precision inertial navigation systems with traditional auxiliary sensors (such as GNSS and VMS). The filtering-based architecture ensures high-frequency output of navigation information and theoretically achieves optimal accuracy. However, visual information lacks Markov property, being related not only to the current navigation state but also to historical information; filtering-based navigation frameworks cannot perfectly address this issue. In the low-precision navigation field, sliding window optimization has demonstrated good engineering performance in fusing visual information.

[0218] The vision-assisted inertial-based multi-source information hybrid integrated navigation architecture is based on the traditional filter navigation architecture. It introduces sliding window graph optimization and loop closure detection functions in an open-loop manner, requiring no modification to the original filter-based integrated architecture's software or hardware in engineering implementation. The outputs of the filter-based integrated architecture and the vision sensor serve as inputs to the sliding window graph optimization architecture. The optimized navigation output information is then used in an open-loop manner to correct the high-frequency output of the filter-based integrated architecture. Compared to conventional graph optimization-based integrated navigation frameworks, the designed integrated navigation framework improves the robustness of integrated navigation while maintaining accuracy. The filter-based integrated framework eliminates the pre-integration process of inertial information, ensuring that the output of integrated navigation information is real-time and high-frequency.

[0219] Figure 4 This is a curve comparison between the actual trajectory and the combined navigation trajectory of the visual-assisted multi-source information hybrid navigation method of this invention. The black solid line "Ⅲ" represents the actual vehicle trajectory, while the positioning trajectory of the inertial navigation / velocity measurement device combination is shown as the black dashed line "Ⅰ," exhibiting a slow divergence trend as navigation time increases. The introduction of visual information allows the carrier's current navigation information to correlate with historical data, as shown by the black dotted line "Ⅱ" in the figure. Combined with graph optimization and loop closure detection techniques, this effectively eliminates the accumulation of positioning errors. Compared to the actual vehicle trajectory, the visual-inertial integrated navigation positioning achieves better positioning results and has smaller positioning errors than the inertial navigation / velocity measurement device combination.

[0220] Example

[0221] 1) K in step 1 G K A These are the calibration and installation matrices for the gyroscope and accelerometer, respectively; ε b , These are the zero points of the gyroscope and accelerometer, respectively, with specific parameters as follows:

[0222]

[0223]

[0224]

[0225]

[0226] 2) There are no requirements for the vehicle mode in the navigation algorithm in step 2. The latitude of the vehicle is 34.257470°, the longitude is 108.987470°, and the altitude is 465.165985 meters.

[0227] 3) The values ​​of radial distortion parameters k1, k2, k3 and tangential distortion parameters p1, p2 in step 7 are as follows. Camera intrinsic parameter f x ,f y The values ​​are 458.654 and 457.296, respectively.

[0228] k1 = -0.28340811

[0229] k2 = 0.07395907

[0230] k3 = 0

[0231] p1 = 0.00019359

[0232] p2 = 1.76187114e-05

[0233] 4) The intrinsic parameter matrix K of the camera in step 11 is

[0234]

[0235] 5) The constraint value for the actual application in step 13 is δ = 0.5.

Claims

1. A multi-source information hybrid navigation method with visual assistance, characterized in that: Specifically, the steps include the following: Step 1: The gyro inertial navigation system outputs the angular velocity in the carrier coordinate system. and acceleration Specifically, a linear error model is used to calibrate and compensate the output pulse, and the output angular velocity is... and acceleration ; Step 2, convert the angular velocity output in Step 1 to... and acceleration The speed is calculated in the strapdown inertial navigation algorithm update module. and location The updated output; Step 3: Measure the vehicle speed using a speed measuring device. Let the pulse output by the speed measuring device per unit time be... ,according to , Solve for the velocity vectors of the load system. Step 4: Based on the angular rate output by the inertial navigation system Vehicle speed output by speed measurement equipment Perform dead reckoning and output the velocity of the velocity measurement device in the navigation coordinate system. ; Step 5: Provide real-time location information to the vehicle via the Global Navigation Satellite System. The second pulse signal, based on the time when the inertial navigation system receives the position information data frame transmitted by the Global Navigation Satellite System. The second pulse time corresponding to this frame of position data Calculate the time delay of the global navigation satellite system output relative to inertial navigation. ; Step 6, in the Kalman filter algorithm module, inertial navigation speed Speed ​​measurement equipment The speed measurement is obtained after subtraction. Inertial navigation position Location provided by Global Navigation Satellite System Position measurement is obtained after subtraction. The Kalman filter algorithm module is based on velocity measurement. and position measurement The optimal Kalman filter technique is used to estimate the state variables of the inertial navigation system in real time. ; Step 7: Acquire image information through a vision sensor, remove distortion from the acquired image, obtain the correct coordinates of the three-dimensional spatial points projected onto the original image, and read all the correct coordinates to form the image after distortion removal; Step 8: Extract feature points from the image after distortion removal in Step 7; Step 9: Track the image feature points extracted in Step 8, use optical flow for tracking, analyze the pixel changes based on the changes in pixel intensity, estimate the relationship between corresponding feature points, and obtain the relative pose changes between key frames. Step 10: Loop closure detection based on bag-of-words model. The similarity score given by the image information is used to determine whether the current scene is a previously visited location, to determine whether loops exist, and to retain keyframes with loops. Step 11: Calculate the difference between the initial pose accumulated between keyframes and the pose after feature matching to obtain the optical flow error equation and the reprojection error equation of the keyframe. Combine the optical flow error equation and the reprojection error equation of the keyframe into a minimum objective function and solve for the solution that minimizes the objective function. Step 12: Use sliding window graph optimization to perform global optimization of the camera trajectory, construct graph optimization and perform maximum a posteriori estimation, and obtain the optimal system state variables when the objective function composed of error terms is minimized, and update the covariance matrix in real time. Step 13: After comparing the state variables of the system estimated in real time by Kalman filtering with the state variables output by the sliding window graph optimization algorithm, the results are output to the integrated navigation result output module.

2. The vision-assisted multi-source information hybrid navigation method according to claim 1, characterized in that: The specific process of step 6 is as follows: Step 6.1, set the inertial navigation speed Speed ​​measured by speed measuring equipment The speed measurement is obtained after subtraction. , inertial navigation position Location provided by Global Navigation Satellite System Position measurement is obtained after subtraction. The specific formula is as follows: （1）； Step 6.2: Select velocity error as the error state of the inertial navigation system. Attitude error vector Position error gyroscope zero bias accelerometer zero point Meanwhile, the scaling factor error of the odometer is also considered. Heading installation angle error and pitch installation angle error Then the error vector of the inertial navigation system As shown in formula (2) below: （2）； In the formula, , , , The attitude error angles are, in order, eastward, upward, and northward. The state equation of the inertial navigation system is shown in equation (3): （3）； in, This is gyroscope noise; Accelerometer noise; The state transition matrix of the time update equation is shown in the following formula (4): （4）； in, This is the transition matrix corresponding to the error equation of the inertial navigation system.

3. The vision-assisted multi-source information hybrid navigation method according to claim 2, characterized in that: The specific process of step 7 is as follows: Step 7.1: Project the 3D spatial point onto the normalized image plane. Let the normalized coordinates of the 3D spatial point be... The radial and tangential distortions of points on the normalized plane are calculated as shown in the following formula (5): （5）； In the formula, Represents the normalized coordinates of the point after distortion. For radial distortion parameters, These are tangential distortion parameters; Step 7.2: Project the distorted point obtained in Step 7.1 onto the pixel plane through the intrinsic parameter matrix to obtain the correct position of the point on the image. Read all the correct coordinates to form the distorted image. The coordinates are shown in the following formula (6): （6）； In the formula, Indicates the correct position coordinates. This indicates a translation from the pixel origin. Pixels This represents the camera's intrinsic parameters.

4. The vision-assisted multi-source information hybrid navigation method according to claim 3, characterized in that: The specific process of step 8 is as follows: Step 8.1, will The feature points are evenly distributed on an image, and A radius is The non-overlapping circles fill the entire image, with feature points distributed at the dot of each circle. In this case, the pixel distance between each feature point and its nearest neighbor is 0. The specific calculation method is as follows: （7）； In the formula, Indicates the image pixel height; Indicates the width of the image in pixels; Step 8.2, calculate the uniformity coefficient of feature point distribution. C The specific calculation method is shown in the following formula (8): （8）； In the formula, The uniformity evaluation coefficient is used. Indicates the first The pixel distance between a feature point and its nearest neighbor feature point; Step 8.3: Select the corner point with the smallest uniformity evaluation coefficient within each image block as the feature point.

5. The vision-assisted multi-source information hybrid navigation method according to claim 4, characterized in that: The specific process of step 9 is as follows: Step 9.1: Calculate the pixel velocity in the camera coordinate system using the optical flow method, as shown in the following formula: （9）； in: ； In the formula, These represent the gray values ​​of pixels in the image along... direction and The partial derivative at time t, The value is … ; The edges of pixels in the image The speed of the shaft's movement; Time Discretization is used to solve for the position of the target pixel in a series of image frames. The calculation formula is shown in (10): （10）； Step 9.2, consider a point in three-dimensional space. and projection point spatial point The coordinates are The pixel coordinates of the projection point are Corresponding to the second frame image The image pixel coordinates of the point are Assuming the motion between the first and second frames is rotation Translation vector Then the two have the following homogeneous transformation relationship: （11）； By accumulating the poses of all ordinary frames between two keyframes, the poses of the keyframes under optical flow constraints are derived. and The relative pose changes between them are: （12）； In the formula, Indicates inclusion , , ,..., , The transformation matrix of j.

6. The vision-assisted multi-source information hybrid navigation method according to claim 5, characterized in that: The specific process of step 10 is as follows: Take a prior similarity This represents the similarity between a keyframe image at a certain moment and a keyframe at the previous moment. The similarity score between the current frame and other keyframes is calculated using the following formula: （13）； In the formula, Indicates prior similarity, Indicates the similarity between the current frame and a previous keyframe; if If a loop is found, the keyframe is considered to have a possible loop; otherwise, it is considered not to have a loop and is discarded.

7. The vision-assisted multi-source information hybrid navigation method according to claim 6, characterized in that: The specific process of step 11 is as follows: Step 11.1: Based on the initial pose given in step 9.2, calculate the difference between the accumulated initial pose between keyframes and the pose after feature matching to obtain the optical flow error equation: Based on camera model relationships: （14）； In the formula, Indicates camera pose. For the camera's internal parameters, These are the three-dimensional coordinates obtained by projecting the pixel. Let the depth of this feature point be denoted; at this point, the keyframe can be calculated. and The error equation between them is solved by the optical flow tracing process method: （15）； Step 11.2, set For spatial points The theoretical projection point in the right figure, For spatial points The pose estimated by feature matching is actually projected onto the projection points in the right figure. (Two projection points...) and There is a distance between them. By adjusting the camera pose, the projection point can be... and The distance between them is minimized, spatial points The coordinates are The pixel coordinates of the projection point are From the camera coordinate system relationship in the camera model, we can see that: （16）； Reprojection error equation for keyframes: （17）； Step 11.3, combine the optical flow error equation and the reprojection error equation of the keyframe into a minimized objective function: （18）； In the formula, This represents the total number of optical flow tracing points. This represents the total number of keyframes after loop closure detection. This represents the total number of feature points; Equation (19) is solved nonlinearly to find the optimal camera pose. Minimize the objective function: （19）。 8. The vision-assisted multi-source information hybrid navigation method according to claim 7, characterized in that: The specific process of step 12 is as follows: Step 12.1: Represent the relationship between constraints and camera poses by constructing a pose graph. Assume the camera poses at two time points are... and The relative motion between the two moments is The following can be written in the form of a Lie group: （20）； Step 12.2: The camera's motion process has been constructed into a graphical model. The optimal system state variables are obtained by minimizing the objective function composed of the error terms, and the covariance is updated in real time. Specifically: Let represent the set of all edges in the sliding window graph optimization model, then the minimum objective function is: （21）； In the formula, Indicates from Time's up Position error at any given time Let be the covariance matrix.

Images