Research On Graph Optimization-based Visual/InertialGNSS Integrated Navigation Method

With the advent of artificial intelligence era,the intelligent unmanned system has drawn much attention recently,and relevant technologies have also been rapidly developed.Highprecision and reliable position and orientation(i.e.pose)information is the basis for some key functional modules,such as environmental perception,path planning,and motion control,etc.Therefore,the multi-source integrated navigation technology has become the core technology for intelligent unmanned system to achieve autonomy.The traditional GNSS/INS integrated navigation system can continuously provide high-precision pose information,but it will degenerate into INS only mode under GNSS-denied environment,where the positioning and attitude errors will quickly diverge for low-cost IMU.Visual-inertial navigation system is also commonly used in intelligent unmanned system,but the global position and yaw will eventually diverge as a dead reckoning system,which is further susceptible to the visual texture of the external environment.Loop closure can eliminate accumulated drift to a certain extent,but as an opportunistic correction information,it may be hard to achieve in actual scenarios.In contrast,the multi-source integrated navigation which uses visual-inertial and GNSS positioning information at the same time can make full use of the complementarity of the three information sources in navigation aspect,and overcome the limitations of using only one or two information sources for navigation.Aiming at the requirement of continuous,robust and high-precision pose information for intelligent unmanned systems outdoors,this thesis has conducted relevant research on visual/inertial/GNSS fusion navigation.The proposed scheme uses a non-linear optimizer(graph optimization)as the basis for multi-source fusion state estimation.The whole scheme consists of two parts,namely the front-end and back-end.The main functions of front-end include ORB feature extraction and stereo matching,local map tracking,motion-only estimation,keyframe selection,and GNSS outlier detection.The main functions of back-end include map maintenance,in-motion alignment,and sliding window optimization.The IMU measurements are pre-integrated as one “measurement” in this framework,which is used both in front-end and back-end.Based on the proposed multi-source integrated navigation framework,the main works and contributions of this research are as follows:1.An improved IMU pre-integration algorithm is designed for medium-and high-grade IMUs.The current IMU pre-integration algorithms are mainly designed for low-cost MEMS IMU,which is insufficiently rigorous and has ignored some important details such as the earth rotation and gravity change(direction and amplitude).However,the medium-and high-grade IMUs can effectively sense the earth rotation,and the MEMS IMUs widely used in intelligent unmanned system are reaching the tactical grade nowadays.And the gravity direction and amplitude will also change when the current position is far from the world reference frame.Aimed at these two issues,the IMU pre-integration algorithm is re-designed by carefully taking consideration of the earth rotation and gravity change.Meanwhile,the coning and sculling compensations are also considered in the mechanization equations of the IMU pre-integration.The uncertainty of the pre-integration error is directly quantified and propagated by using square-root information matrix,in order to achieve better numerical stability.The test results on ground vehicle datasets show that,for medium-and high-grade IMUs,the positioning and attitude accuracy of monocular and stereo visual-inertial odometry have been significantly improved after using the re-designed pre-integration algorithm.Specifically,the positioning and attitude accuracy of monocular and stereo VIO using high-grade IMU is improved by50?97%;The positioning and attitude accuracy of monocular and stereo VIO using mediumgrade MEMS IMU(quasi-tactical grade MEMS)is improved by 21?41%;As for the consumergrade MEMS IMU that cannot sense the earth rotation effectively,the re-designed preintegration algorithm has no much advantage.2.A hybrid sliding window optimizer is designed by combining the advantages of three mainstream sliding window estimators,namely the conditioning-based,prior-based sliding window optimizer,and MSCKF.Specifically,the keyframe nodes outsides the sliding window are directly fixed in order to keep the visual observations whose tracking length are larger than the size of sliding window.The sliding window is divided into two parts,namely mature area and growing area,the nodes within the mature area have been estimated more times and doubtless possess greater precision than the ones in the growing area.The mappoint will be marginalized out only if all its observations have slipped out of the growing area,in order to make linearization error negligible.Considering the independence of mappoints,the multi-state constraint(MSC)residual functions are constructed first by using left null space marginalization method,and then the normal equation about marginalization are constructed by using the MSC residual functions and other related linearized residual functions.Finally the prior residual function for next optimization can be obtained by using Schur complement marginalization method.By this strategy,the dimension of the matrix to be inversed in the Schur complement matrix will decrease significantly,thus the computation efficiency of marginalization is greatly improved.For example,when the number of mappoints need to be marginalized out is 150,compared with the traditional marginalization method that does not consider the independence between mappoints,the time-consumption of the marginalization strategy proposed in this thesis will be reduced by 83.3%.Theoretically,the performance of this hybrid sliding window optimizer can tend to any of these three mainstream sliding window estimators.Specifically,if the fixed area is removed and the size of the mature area is increased at the same time,its performance will tend to a prior-based sliding window optimizer;If the number of iterations of non-linear optimization algorithm is reduced at the same time,its performance will tend to MSCKF;If the fixed area is retained while reducing the size of the mature area,its performance will tend to a conditioning-based sliding window optimizer.The framework of the hybrid sliding window optimizer is very flexible,and can overcome the shortcoming of each estimator to a certain extent by setting reasonable parameters.The accuracy of the multi-source fusion state estimation is not affected with marginalization efficiency improvement.The test results show that the monocular VIO using hybrid sliding window optimizer can achieve similar or better positioning accuracy than six open-source VIO on the Eu Ro C dataset.The positioning error drift rate of monocular VIO using hybrid sliding window optimizer is 0.3%?0.6% on ground vehicle datasets,which has reached the accuracy of cutting-edge VIO schemes.3.An in-motion initialization algorithm is specifically designed for visual/inertial/GNSS integrated navigation for calculating the initial value of motion state and the external parameters between sensors.The whole algorithm consists of three main steps: First,the relative rotation between IMU and camera is calculated by hand-eye calibration method,and the gyroscope bias is calculated on the basis of the relative rotation and camera attitudes.Second,the alignment matrix is calculated by using gravity and motion displacement vectors.The specific calculation procedure is different depending on whether estimating the lever arm of GNSS antenna.The gravity vector and the relative translation between IMU and camera are calculated in the same equation group.Finally,the accelerometer bias and the alignment matrix deviate are calculated on the basis of all related known estimation values.After the in-motion initialization converges,a reliable iterative initial value can be recovered for non-linear optimizer.The test results on five straight road and five curve road data sequences show that the rotation between camera and IMU,the biases of IMU gyroscope and accelerometer,the alignment matrix between the two world frames can all be effectively estimated.And the convergence time of alignment matrix estimation is about 5?13 seconds.For a quasi-tactical grade MEMS IMU,the final average estimation errors are 1.11 and 1.44 degrees on curve and straight roads.The convergence time and final accuracy fully meet the needs of graph optimization-based visual/inertial/GNSS integrated navigation.4.A visual/inertial/GNSS multi-sensor data acquisition and algorithm research software and hardware platform is designed and built,which has realized precise time synchronization between sensors,internal and external parameter calibration,as well as a complete vision/inertial/GNSS integrated navigation algorithm,including ORB feature extraction and stereo matching,local map tracking,motion-only estimation,keyframe selection,and GNSS outlier detection,in-motion alignment,and sliding window optimization,etc.Finally,the performance of visual/inertial/GNSS integrated navigation is thoroughly tested based on the hardware and software platform,the test results show that: a)If the GNSS RTK positioning result is available,the vision/inertial/GNSS integrated navigation using MEMS IMU can achieve centimeter-level positioning accuracy,the horizontal attitude accuracy is better than0.04 degrees,and the heading accuracy is better than 0.07 degrees;b)If the GNSS positioning result is unavailable,the system will degenerate to VIO and the positioning and attitude accuracy of VIO are related to the grade of IMU and monocular or stereo camera configuration.Generally,the higher the grade of IMU,the better accuracy of positioning and orientation of VIO.And,the positioning and horizontal attitude accuracy of stereo VIO are higher than monocular VIO.Compared with traditional INS/NHC/wheel encoder integrated navigation used in ground vehicle navigation,the VIO can achieve better performance under low-speed and non-open environment.At the same time,in addition to the data sequence collected at particularly open area,the average positioning error drifts of monocular and stereo VIO using different grade of MEMS IMUs are not exceed 0.5%,and have reached the accuracy level of cutting-edge VIO schemes.In summary,this thesis has conducted a comprehensive research on the visual/inertial/GNSS integrated navigation method.The main contributions cover core algorithm design and experimental verification,including the construction of multi-sensor synchronous acquisition system and algorithm research platform.The performance of the visual/inertial/GNSS integrated navigation and its core algorithm are thoroughly tested on ground vehicle datasets.The research of this thesis provides a comprehensive and feasible solution for the navigation of outdoor intelligent unmanned systems.