Satellite visual-inertial odometry tightly coupled system for navigation state estimation

Abstract

The application discloses a satellite visual inertial odometer tightly coupled system for navigation state estimation, which comprises a Visual module, an inertial sensor module, an odometer module, a GNSS module, a data processing module and a nonlinear optimization module; the data processing module comprises three-dimensional RISS data processing, original GNSS data processing, Visual data detection and tracking, GNSS data initialization and VR initialization. Since the existing visual inertial navigation method has the problem of inertial device drift, the system tightly couples visual data, inertial data and odometer data with multi-constellation GNSS original measurement data, and is based on an online coarse-to-fine initialization method and is used for initializing a global navigation satellite system and visual inertial state, so that the system has real-time estimation capacity and is suitable for various complex environments.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning systems, and more particularly to a satellite visual-inertial odometry tightly coupled system for navigation state estimation. Background Technology

[0002] Navigation and positioning are fundamental functions of spatial perception technology, typically relying on different types of sensors, algorithms, and technologies, including Global Navigation Satellite Systems (GNSS), inertial navigation, visual navigation, and fusion navigation. They are mainly used in fields such as autonomous driving, robot navigation, aviation, and indoor positioning.

[0003] Sensor fusion is a technique that combines data from different types of sensors. In recent years, sensor fusion methods have received increasing attention because, due to the complementarity provided by heterogeneous sensors, sensor fusion algorithms can significantly improve the accuracy and robustness of state estimation systems. On one hand, Visual-Inertial Navigation (VINS) combines information from visual sensors and an IMU (Inertial Measurement Unit). The IMU can provide information in real time, unaffected by external environmental interference. Therefore, in certain situations, such as indoor navigation or when GPS signals are unavailable, the IMU can be used to maintain navigation continuity. Visual navigation uses visual sensors to capture images of the surrounding environment and uses this image information to estimate the position and attitude of objects. These two sensors can complementarily provide information about the motion of objects. The visual sensor perceives position by capturing images of the surrounding environment, while the IMU measures the acceleration and angular velocity of the object. Through complex sensor fusion algorithms, VINS can estimate the motion state of objects. However, because both the camera and the IMU operate within a local frame, there is an odometry drift problem, where position estimation accumulates errors over time. On the other hand, the Global Navigation Satellite System (GNSS) is an important component in the field of navigation and positioning. GNSS systems, such as GPS, GLONASS, Galileo, and BeiDou, provide high-precision position, velocity, and time information to receivers through a group of satellites distributed in Earth orbit. They offer drift-free and globally aware solutions for positioning tasks and are widely used in various scenarios. However, GNSS can be affected by signal blockage, multipath effects, or signal loss in certain situations, leading to unstable or inaccurate navigation and thus failing to provide smooth and consistent position estimates. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention discloses a tightly coupled satellite visual-inertial odometry system for navigation state estimation, specifically comprising:

[0005] The Visual module is used to receive images of the surrounding environment captured by the camera;

[0006] An inertial sensor module used to receive measurement data collected by accelerometers and gyroscopes;

[0007] Odometer module for obtaining wheel speed information for users;

[0008] GNSS module used to acquire raw GNSS data;

[0009] It also includes a data processing module, which includes a Visual data detection and tracking module, a 3D RISS data processing module, a raw GNSS data processing module, a GNSS data initialization module, and a VR initialization module;

[0010] The data processing module receives data from the Visual module, inertial sensor module, odometer module, and GNSS module. The Visual data detection and tracking module detects and tracks sparse feature points from the surrounding environment image sequence and transmits the acquired feature information to the VR initialization module. The 3D RISS data processing module receives measurement data from the odometer module, accelerometer, and gyroscope and calculates the navigation status and trajectory, then transmits the calculated navigation status to the VR initialization module. The raw GNSS data processing module preprocesses the raw GNSS data, filtering out low-altitude and unstable satellite signals, and transmits the preprocessed data to the GNSS data initialization module. The VR initialization module receives data from the Visual data detection and tracking module and the 3D RISS processing module, aligns the navigation trajectory calculated by the 3D RISS processing module with the feature information processed by the Visual data detection and tracking module. After VR initialization alignment, the GNSS data initialization module performs a coarse-to-fine GNSS initialization process and checks and processes GNSS degradation.

[0011] The nonlinear optimization module performs joint estimation of the system state.

[0012] The 3D RISS data processing module calculates the received pitch, roll, and yaw angles and transmits the acquired position and velocity information to the VR initialization module.

[0013] The VR initialization module receives data from the Visual data detection and tracking module and the 3D RISS data processing module, uses the SfM algorithm to estimate the pose of all frames within the sliding window using monocular vision, and then performs visual-inertial joint calibration using a fusion and alignment method of visual and RISS pre-integration results.

[0014] The SfM algorithm uses the feature information between images to infer the three-dimensional position information of each point in the scene from the perspective of vision to motion.

[0015] By adopting the above technical solution, the present invention provides a satellite visual-inertial odometry tightly coupled system for navigation state estimation. This system tightly couples visual data, inertial data, and odometry data with raw measurement data from multiple constellations of GNSS. Based on an online coarse-to-fine initialization method, it is used to initialize the Global Navigation Satellite System (GNSS) and visual-inertial state. Therefore, the system has real-time estimation capability and is suitable for various complex environments. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of this application 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 recorded in this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 This is a structural block diagram of the system of the present invention.

[0018] Figure 2 This is a schematic diagram of the working principle of the system of the present invention. Detailed Implementation

[0019] To make the technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention:

[0020] like Figure 1 The illustrated satellite visual-inertial odometry tightly coupled system for navigation state estimation includes a Visual module, an inertial sensor module, an odometry module, a GNSS module, a data processing module, and a nonlinear optimization module. The Visual module, inertial sensor module, odometry module, and GNSS module are connected to the data processing module, which in turn is connected to the nonlinear optimization module. The data processing module includes a 3D RISS data processing module, a raw GNSS data processing module, a Visual data detection and tracking module, a GNSS data initialization module, and a VR initialization module.

[0021] The Visual module receives image data from the camera and passes it to the Visual data detection and tracking module within the data processing module. The odometry module receives measurement data from the odometry, while the inertial sensor module receives measurement data from the accelerometer and gyroscope and passes it to the 3D RISS data processing module within the data processing module. The GNSS module receives raw GNSS data and transmits it to the raw GNSS data processing module within the data processing module.

[0022] The data processing module preprocesses the received Visual data, inertial measurement data, odometry data, and raw GNSS data. First, it sends the received data to the Visual data detection and tracking module, the 3D RISS data processing module, and the raw GNSS data processing module for necessary preparation. Next, the preprocessed Visual data, inertial measurement data, and odometry data are sent to the VR initialization module, while the processed GNSS data is transmitted to the GNSS data initialization module. Finally, the initialized data is sent to the nonlinear optimization module for further state estimation and optimization. The Visual data detection and tracking module detects and tracks sparse feature points from the image sequence, providing crucial visual information for subsequent state estimation. The 3D RISS data processing module preprocesses the inertial measurement data and odometry data, including gyroscope data, accelerometer data, and odometry data, to obtain navigation state information such as the object's position, velocity, and attitude. The raw GNSS data processing module filters out low-altitude and unstable satellite signals to ensure high-quality GNSS data is used for initialization. The VR initialization module aligns the visual-motion structure with the RISS trajectory to recover key information such as scale, velocity, gravity, and RISS bias. The GNSS initialization module performs a coarse-to-fine GNSS initialization process, including obtaining an initial position estimate using the SPP algorithm, then associating the local coordinate system with the global coordinate system during the yaw alignment phase, and finally refining and improving the position estimate.

[0023] The GNSS initialization module obtains coarse positioning results through the single-point positioning (SPP) algorithm, uses local velocity correlation local and global frames from visual-inertial initialization and GNSS Doppler measurements to align yaw angles, utilizes precise local trajectories and applies time synchronization constraints to further refine the global position of anchor points, checks and handles GNSS degradation, and if GNSS is unavailable or cannot be correctly initialized, it is converted into a visual-inertial odometry tightly coupled system.

[0024] The 3D RISS data processing module performs RISS pre-integration based on the measurements from the inertial measurement unit and the odometry measurement to calculate the navigation state and trajectory, and predicts the position, velocity and attitude between image frames.

[0025] The VR initialization module uses the SfM algorithm to estimate the pose of all frames within the sliding window using monocular vision. Then, it performs visual-inertial joint calibration through visual and RISS pre-integration fusion alignment. A frame search is performed between the last frame of the current visual image and the frames before the sliding window to find frames with more than 30 tracked feature points and a disparity greater than 20. The essential matrix is ​​initialized using a 5-point method to recover estimates of relative rotation and translation. The PnP algorithm is used to estimate the camera pose of all other frames within the sliding window, and global bundle adjustment is performed within the sliding window to minimize reprojection error by optimizing the pose of all frames.

[0026] The Visual data detection and tracking module corrects the two-dimensional feature points to be distortion-free, then projects them onto a unit sphere after outlier removal, performs F matrix testing, removes outliers using the RANSAC algorithm, and marks a frame as a key frame if the average feature point disparity between the current frame and the nearest key frame exceeds a threshold or if the number of feature points tracked in the current frame is less than a threshold.

[0027] The nonlinear optimization module receives data from the data processing module and uses a nonlinear optimization framework to establish constraints from all measurements in order to jointly estimate the system state.

[0028] like Figure 2 The system, as shown, takes raw GNSS data measured by the GNSS receiver, forward velocity and acceleration measured by the odometry, vertical angular velocity measured by the gyroscope, forward and lateral acceleration measured by the forward and lateral accelerometers, and image data acquired by the camera as input, and transmits them to the data preprocessing section. The data preprocessing section first processes the inertial measurement data and odometry data, calculates the pitch and roll angles using the forward and lateral accelerometer measurements, and further combines this with the gyroscope measurement data to obtain the heading angle. Then, the calculation results are combined with the odometry measurements to obtain navigation information such as position, velocity, and attitude. It also detects and tracks sparse feature points from the image sequence. These feature points, detected from the images, are used to track camera motion. For the raw GNSS data, low-altitude and unstable satellite signals are first filtered out because they easily introduce errors, thus ensuring that only satellites continuously locked within a certain time period are included in the system.

[0029] Following the preprocessing stage, the initialization stage takes place. First, Structure from Motion (SfM) relying solely on the vision sensor is used to estimate camera motion and environmental structure. Then, the RISS trajectory is aligned with the SfM results to reconstruct information such as scale, velocity, gravity, and RISS bias. After visual and RISS initialization, coarse-to-fine GNSS initialization is performed. First, a preliminary anchor point positioning result is obtained using the SPP algorithm. Then, the local and global coordinate systems are associated using visual and GNSS data, and heading alignment is performed. The initialization stage concludes with anchor point refinement, using precise local trajectory information and imposing constraints on clock bias to further improve the accuracy of global position estimation for anchor points. Simultaneously, the system checks and handles GNSS degradation to ensure the robustness of the navigation system. If GNSS is unavailable or cannot be correctly initialized, the system naturally degrades to Visual-Inertial Odometry (VIO), i.e., navigation relying solely on visual and RISS data.

[0030] The RISS algorithm is a method for preprocessing inertial measurement data, enabling the determination of navigation parameters, including position, velocity, and attitude, on land vehicles. This algorithm is based on a pair of accelerometers and a gyroscope, along with an odometry sensor, configured on the vehicle. In this configuration, the forward and lateral accelerometers measure the vehicle's acceleration in the forward and backward and left-right directions, respectively, while the gyroscope, aligned with the vehicle's vertical axis, measures the vehicle's angular velocity. The odometry sensor measures the vehicle's wheel speed. By solving for the navigation parameters, calculating the three-dimensional position, velocity, and attitude, and further merging the preprocessed inertial and visual data through time synchronization and spatial alignment, inertial and visual navigation can be achieved, providing more accurate three-dimensional position, velocity, and attitude estimates.

[0031] In the nonlinear optimization stage, the nonlinear optimization module adopts a nonlinear optimization framework. The system integrates constraints from all measurements (including visual, inertial and GNSS data) into a single model and jointly estimates the state of the navigation system by minimizing the error. To ensure real-time performance and handle degenerate motion in the visual-inertial system, a two-way marginalization strategy is adopted.

[0032] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A tightly coupled satellite visual-inertial odometry system for navigation state estimation, characterized in that... include: The Visual module is used to receive images of the surrounding environment captured by the camera; An inertial sensor module used to receive measurement data collected by accelerometers and gyroscopes; Odometer module for obtaining wheel speed information for users; GNSS module used to acquire raw GNSS data; It also includes a data processing module, which includes a Visual data detection and tracking module, a 3D RISS data processing module, a raw GNSS data processing module, a GNSS data initialization module, and a VR initialization module; The data processing module receives data from the Visual module, inertial sensor module, odometer module, and GNSS module. The Visual data detection and tracking module detects and tracks sparse feature points from the surrounding environment image sequence and transmits the acquired feature information to the VR initialization module. The 3D RISS data processing module receives measurement data from the odometer module, accelerometer, and gyroscope and calculates the navigation status and trajectory, then transmits the calculated navigation status to the VR initialization module. The raw GNSS data processing module preprocesses the raw GNSS data, filtering out low-altitude and unstable satellite signals, and transmits the preprocessed data to the GNSS data initialization module. The VR initialization module receives data from the Visual data detection and tracking module and the 3D RISS processing module, aligns the navigation trajectory calculated by the 3D RISS processing module with the feature information processed by the Visual data detection and tracking module. After VR initialization alignment, the GNSS data initialization module performs a coarse-to-fine GNSS initialization process and checks and processes GNSS degradation. The nonlinear optimization module performs joint estimation of the system state; The VR initialization module receives data information transmitted by the Visual data detection and tracking module and the 3D RISS data processing module, uses the SfM algorithm to estimate the pose of all frames within the sliding window using monocular vision, and then performs visual-inertial joint calibration through the fusion and alignment method of visual and RISS pre-integration results. The SfM algorithm uses the feature information between images to infer the three-dimensional position information of each point in the scene from the perspective of vision to motion.

2. The satellite visual-inertial odometry tightly coupled system for navigation state estimation according to claim 1, characterized in that: The 3D RISS data processing module calculates the received pitch, roll, and yaw angles and transmits the acquired position and velocity information to the VR initialization module.

Images