Visual inertial odometer divergence anomaly recovery system and processing method

Abstract

The invention discloses a visual inertial odometer divergence anomaly recovery system and a processing method. The visual inertial odometer divergence anomaly recovery system comprises a memory, a restart control module, a divergence point recovery module and a motion state detection and estimation module, the memory is used for storing historical state information of the machine body within a period of time before divergence; the restart control module controls the visual inertia odometer process to be restarted according to the motion state condition of the machine body; the divergence point recovery module recovers divergence point state information at the divergence moment; the motion state detection and estimation module monitors and estimates the motion state of the body. According to the method, the body motion state when the visual inertial odometer is diverged is efficiently recovered, a high-precision initial body state is provided for the visual inertial odometer, the problem that the current accurate state information cannot be acquired during initialization when the visual inertial odometer is restarted abnormally in divergence is solved, and the success rate of dynamic restarting of the visual inertial odometer is improved.

Description

Technical Field

[0001] This invention relates to computer vision and inertial navigation technology, and more particularly to a system and method for handling and recovering from divergence anomalies that occur during the operation of a visual inertial odometer. Background Technology

[0002] With the rapid development of robotics, autonomous driving, augmented reality, and virtual reality, visual inertial odometry (VIO) positioning technology has been widely applied. However, in practical applications, VIO systems face three core problems: First, complex ambient lighting conditions may lead to inaccurate image features acquired by the visual sensor, and the inertial measurement unit (IMU) suffers from noise and drift, resulting in a continuous increase in accumulated errors after long-term operation. VIO systems are susceptible to divergence anomalies due to these factors. Second, initialization is difficult when restarting after a divergence anomaly. After a VIO diverges, it needs to be reinitialized, but at this time, it may be in the running state, making it difficult to obtain accurate initial position, velocity, and attitude information, resulting in a low initialization success rate. Third, the robustness of anomaly handling schemes is insufficient. Some methods can only handle specific types of divergence causes and lack universality. Other methods, although able to detect divergence anomalies, have inefficient recovery mechanisms, resulting in poor accuracy and stability after system recovery.

[0003] The development of VIO technology not only relies on the iterative upgrades of sensor hardware, but also on the innovative optimization of algorithms, especially breakthroughs in anomaly handling and system recovery mechanisms. These are crucial for improving the positioning accuracy and robustness of VIO systems in complex environments. Summary of the Invention

[0004] Purpose of the invention: The purpose of this invention is to provide a VIO divergence anomaly recovery system that improves the success rate of VIO dynamic restart. Another purpose of this invention is to provide a processing method for this system.

[0005] Technical solution: The visual inertial odometry divergence anomaly recovery system of the present invention includes:

[0006] Memory storage: Stores the organism's historical state information for a period of time before divergence, including PVQ state storage unit, IMU information storage unit and zero bias information storage unit;

[0007] Restart control module: Controls the restart of the visual inertial odometry process according to the body's motion status, and provides the visual inertial odometry with the body's motion status information at the time of restart, including the body's position, velocity, attitude, acceleration zero bias and angular velocity zero bias information;

[0008] The divergence point recovery module includes a zero-bias recovery unit, a time synchronization unit, a median integration unit, a weighted fitting unit, and a filter tracking unit. Based on historical state information, it performs zero-bias recovery, time synchronization, median integration, weighted fitting, and filter tracking to restore the divergence point state information at the divergence moment.

[0009] Motion state detection and estimation module: includes a motion state detection unit and a motion state estimation unit; the motion state detection unit detects the uniform motion state of the body based on the acceleration and angular velocity of the inertial sensor; the motion state estimation unit estimates the motion state of the body based on the recovered divergence point positioning information and the diverged acceleration and angular velocity.

[0010] The PVQ state storage unit uses a queue of fixed time length to store the PVQ information released after the visual inertial odometry initialization is completed, including the body's position vector, velocity vector, attitude quaternion information and corresponding timestamp information.

[0011] After the visual inertial odometry is initialized, the IMU information storage unit uses a queue to store inertial sensor observation data within the same time period as the PVQ information record, including the body's acceleration vector, angular velocity vector, and corresponding timestamp information.

[0012] After the visual inertial odometry is initialized, the zero-bias information storage unit uses a queue to store the zero-bias data published by the visual inertial odometry thread within the same time period as the PVQ information record, including the zero-bias vector of the body's acceleration, the zero-bias vector of the angular velocity, and the corresponding timestamp information.

[0013] The restart control module listens to the motion state information of the machine body released by the motion state detection and estimation module. If it detects that the machine body has entered a uniform speed state, it controls the visual inertial odometry process to restart.

[0014] The zero-bias recovery unit recovers the zero-bias value during the divergence point based on the historical zero-bias information stored in the zero-bias information storage unit; the time synchronization unit aligns the PVQ status with the IMU measurement information using timestamps.

[0015] The median integration unit performs median integration calculation based on the aligned PVQ state and IMU measurement information to obtain the PVQ time series before the body diverges; the weighted fitting unit combines the median integration result with the visual inertial odometry to optimize the PVQ time series and performs weighted fitting to output a highly accurate divergent body PVQ odometry.

[0016] The tracking filtering unit filters and tracks the weighted fitted PVQ time series to recover high-precision state information of the location divergence point.

[0017] The visual inertial odometry divergence anomaly recovery system processing method of the present invention includes the following steps:

[0018] (1) Monitor the working status of the visual inertial odometry. After the visual inertial odometry is initialized, proceed to step (2). When the visual inertial odometry publishes odometry information, its initialization is complete.

[0019] (2) The memory storage uses a queue to record a fixed length of the body's historical state information, including the body's position, velocity, attitude, acceleration, acceleration bias, angular velocity and angular velocity bias information. When the queue reaches a fixed length, the latest data is stored and the oldest data is discarded.

[0020] (3) Continuously monitor the working status of the visual inertial odometer, wait for the visual inertial odometer to report a divergence abnormal signal, repeat step (2), and proceed to step (4) when the visual inertial odometer diverges.

[0021] (4) The divergence point recovery module performs zero-bias recovery, time synchronization, median integration, weighted fitting and filtering tracking on the historical state data in sequence to obtain the true motion state of the body at the divergence moment.

[0022] (5) The motion state detection and estimation module determines the motion state of the body based on inertial sensor data;

[0023] (6) Continuously monitor the body's motion state. When the body enters a state of uniform motion, repeat step (5). When the body moves at a uniform speed, proceed to step (7).

[0024] (7) The motion state detection and estimation module estimates the PVQ information of the body in the current uniform motion state based on the motion state of the body at the divergence moment;

[0025] (8) After receiving the estimated PVQ information in step (7), the restart control module controls the visual inertial odometry process to restart and sets the PVQ information as the initial value when the visual inertial odometry restarts.

[0026] In step (4), the specific steps for the divergence point recovery module to obtain the actual motion state of the body at the divergence moment include:

[0027] (41) Zero bias recovery processing: Based on the historical zero bias information of the zero bias information storage unit, combined with the characteristics of the IMU zero bias being relatively stable and not changing, the quartile method is used to detect relevant information before the change, and the zero bias value at the moment before the change is used as the recovery zero bias value during the divergence period.

[0028] (42) Time synchronization processing: Based on the timestamp and visual inertial odometry processing delay time, align the PVQ historical status with the IMU historical measurement information;

[0029] (43) Median integration processing: Using the motion posture of the most original frame of the aligned PVQ history as the initial value, the median integration is performed using the aligned IMU history measurement values ​​to obtain the time series of the PVQ motion state of the organism in the period before divergence;

[0030] (44) Weighted fitting: The PVQ time series obtained by median integration is weighted and fitted with the PVQ time series aligned in step (42). The fitting weight adopts the gradient descent strategy to gradually reduce the influence of the PVQ sequence in step (42).

[0031] (45) Filtering and tracking processing: Filtering and tracking are performed based on the weighted fitted PVQ time series. The current statistical model of CS is used as the kinematic model for Kalman filtering to estimate the true motion state of the body at the divergence moment.

[0032] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages: (1) It provides a generalized VIO divergence anomaly recovery scheme, which does not require processing for specific types of divergence causes and has strong versatility; (2) It makes full use of the motion history information of the diverging body, integrates weighted fitting and tracking filtering algorithms, and improves the positioning accuracy and operational stability after VIO recovery; (3) The processing method adopted is simple and lightweight, highly practical and has excellent real-time performance, and is easy to implement; (4) It significantly enhances the positioning robustness of the VIO system in complex environments. Attached Figure Description

[0033] Figure 1 This is a system structure block diagram of the present invention;

[0034] Figure 2 This is a flowchart of the divergence point recovery module of the present invention;

[0035] Figure 3 This is a flowchart of the system processing method of the present invention. Detailed Implementation

[0036] The technical solution of the present invention will be further described below with reference to the accompanying drawings.

[0037] Example 1

[0038] This embodiment provides a VIO divergence anomaly recovery system, including a memory storage unit, a restart control module, a divergence point recovery module, and a motion state detection and estimation module, such as... Figure 1 As shown, it specifically includes:

[0039] Memory storage: Used to store the historical state information of the organism for a period of time before divergence, including the organism's position, velocity, attitude, acceleration, acceleration bias, angular velocity and angular velocity bias information, in preparation for the subsequent recovery of the diverging organism's motion state;

[0040] Restart control module: Used to control the restart of the VIO process according to the body's motion state, and at the same time provide the VIO with the body's motion state information at the time of restart, including the body's position, velocity, attitude, zero-bias acceleration and zero-bias angular velocity information;

[0041] Divergence point recovery module: used to implement the VIO positioning divergence state recovery algorithm;

[0042] Motion state detection and estimation module: used to implement the algorithm for detecting and estimating the body's motion state.

[0043] The memory storage unit includes a PVQ state storage unit, an IMU information storage unit, and a zero-bias information storage unit. The PVQ state storage unit uses a queue of fixed time length to store the PVQ information released after VIO initialization, including the body's position vector, velocity vector, attitude quaternion information, and corresponding timestamp information. After VIO initialization, the IMU information storage unit uses a queue to store inertial sensor observation data within the same time period as recording the PVQ information, including the body's acceleration vector, angular velocity vector, and corresponding timestamp information. After VIO initialization, the zero-bias information storage unit uses a queue to store the zero-bias data released by the VIO thread within the same time period as recording the PVQ information, including the body's acceleration zero-bias vector, angular velocity zero-bias vector, and corresponding timestamp information.

[0044] Specifically, assuming the inertial sensor is a 6-axis IMU with an observation data frequency of 100Hz, and the visual sensor is a stereo camera with an output image frame rate of 30Hz, VIO, based on the VINS-FUSION algorithm, fuses the observation data from the inertial and visual sensors to generate the PVQ odometry at a frequency of 100Hz, and optimizes the zero-bias information at a frequency of 15Hz. The specific working method is as follows:

[0045] (1) The PVQ state storage unit uses a queue of fixed time length to store the PVQ odometer and corresponding timestamps released after the VIO initialization work. The queue storage time is limited to 2 seconds, and the corresponding length is 100×2. When the queue reaches the fixed length, the latest data is stored and the oldest data is discarded to ensure that the queue length remains unchanged. When the VIO diverges abnormally, the queue is stopped from being updated.

[0046] (2) The IMU information storage unit uses a queue of fixed time length to store the inertial sensor observation data after VIO initialization. The queue storage time is limited to 2 seconds, and the corresponding length is 100×2. When the queue reaches the fixed length, the latest data is stored and the oldest data is discarded to ensure that the queue length remains unchanged. When VIO diverges abnormally, the queue is stopped from being updated.

[0047] (3) The zero-bias information storage unit uses a queue of fixed time length to store the zero-bias data and corresponding timestamps released after the VIO initialization work. The queue storage time is limited to 2 seconds, and the corresponding length is 15×2. When the queue reaches the fixed length, the latest data is stored and the oldest data is discarded to ensure that the queue length remains unchanged. When the VIO diverges abnormally, the queue is stopped from being updated.

[0048] The restart control module listens to the body motion status released by the motion state detection and estimation module. When the body enters a uniform motion state, it controls the VIO process to restart, improving the success rate of VIO initialization. Specifically, assuming that the restart function and the positioning initial value setting function are available, after the motion state detection and estimation module generates the PVQ odometry of the body in a uniform motion state, the restart control module sets the initial odometry state of VIO restart according to the PVQ odometry released by the motion state detection and estimation module, and then releases a restart signal to control the VIO process to restart.

[0049] The divergence point recovery module includes a zero-bias recovery unit, a time synchronization unit, a median integration unit, a weighted fitting unit, and a filter tracking unit, such as... Figure 2 As shown, the zero-bias recovery unit recovers the zero-bias value during the divergence point based on the historical zero-bias information stored in the zero-bias information storage unit; the time synchronization unit aligns the PVQ state and IMU measurement information using timestamps; the median integration unit performs median integration calculation based on the aligned PVQ state and IMU measurement information to obtain the PVQ time series before the body diverges; the weighted fitting unit combines the median integration result with the VIO-optimized PVQ time series for weighted fitting, outputting a high-precision diverging body PVQ odometry; and the tracking filtering unit filters and tracks the weighted fitted PVQ time series to recover high-precision state information for locating the divergence point.

[0050] Specifically, assuming that a divergence anomaly occurs after VIO has been working for a period of time, the memory stores and records the data within 2 seconds before the divergence, saves it to the corresponding queue, and the specific working method is as follows:

[0051] (1) The zero bias recovery unit uses the quartile method to detect whether there is a mutation value based on the historical zero bias information of the zero bias information storage unit and the characteristic that the IMU zero bias is relatively stable and does not change. If there is a mutation, the zero bias at the moment before the mutation is used as the zero bias value during the recovery divergence period. If there is no mutation value, the oldest value of the zero bias queue is used as the zero bias value during the recovery divergence period.

[0052] (2) The time synchronization unit counts the VIO processing delay time. If the frequency of VIO process optimizing zero bias information is 15Hz, the delay time is 66ms (1 / 15s). Based on the delay and timestamp, the IMU historical measurement and PVQ historical state are aligned. When the difference between the IMU historical measurement and PVQ historical state timestamp is within [66,76] milliseconds, it is considered to be synchronized. The asynchronous measurement and state values ​​are removed to obtain the aligned IMU and PVQ time series.

[0053] (3) The median integration unit takes the motion attitude of the oldest frame of the aligned PVQ history as the initial value, and performs median integration calculation using the aligned IMU history measurement values ​​to obtain the PVQ time series within 2 seconds before divergence;

[0054] (4) The weighted fitting unit performs weighted fitting of the PVQ time series calculated by the median integral and the PVQ time series optimized by VIO. The fitting weight of the PVQ time series optimized by VIO decreases by gradient as time increases, and its weight decreases uniformly from 1 to 0.005 according to 2s time and sequence length.

[0055] (5) The filtering and tracking unit performs filtering and tracking processing based on the weighted fitted PVQ time series. The current statistical model of CS is used as the kinematic model, the maneuver frequency is set to 0.05, and linear Kalman filtering is used as the filtering and tracking method to restore the true motion state of the body at the divergence moment.

[0056] The motion state detection and estimation module includes a motion state detection unit and a motion state estimation unit. The motion state detection unit uses the current acceleration and angular velocity from the inertial sensor to detect in real time whether the machine has entered a uniform motion state. The motion state estimation unit uses the positioning information of the recovered divergence point and the diverged acceleration and angular velocity to estimate the current motion state of the machine. Specifically, assuming that after VIO divergence, the machine enters attitude control mode, the controller relies on inertial navigation to maintain the current attitude of the machine and enters a uniform motion process. The acceleration noise when the machine is stationary is 0.1 m / s^2, and the angular velocity noise when the machine is stationary is 0.01 rad / s. When the divergence point state is successfully recovered, the motion state detection unit monitors the current acceleration and angular velocity from the inertial sensor, sets the acceleration change threshold to 1.5 times the acceleration noise when the machine is stationary, i.e., 0.15 m / s^2, and sets the angular velocity change threshold to 1. The angular velocity noise is 5 times that of the stationary body, i.e., 0.015 rad / s. When the changes in acceleration and angular velocity are both less than the corresponding threshold and last for 1 second, it is considered to have entered a uniform motion state. The motion state estimation unit saves the diverged IMU observation data in a variable array. Taking the recovered divergence point as the starting point, it uses the median integration of the diverged acceleration and angular velocity to calculate the body PVQ odometry at the current moment. Once the motion state detection unit determines that the body has entered a uniform motion state, it saves the body PVQ odometry at the current moment and sends it to the restart control module.

[0057] Example 2

[0058] This embodiment provides a working mode of a VIO divergence anomaly recovery system, and its complete operation flow is as follows:

[0059] After VIO initialization is successful, the memory uses a queue to start recording the body's state information, including the body's position, velocity, attitude, acceleration, acceleration bias, angular velocity, angular velocity bias information and timestamp. When the queue reaches a time length of 2 seconds, the latest data is stored and the oldest data is discarded to ensure that the queue length remains unchanged.

[0060] When VIO divergence occurs, the memory stops updating the queue, and the divergence point recovery module further processes the aforementioned 2-second data. The divergence point recovery module uses the zero-bias recovery unit to detect abrupt changes using the quartile method. If a change occurs, the zero bias at the moment before the change is used as the zero bias value during the divergence point recovery period; otherwise, the oldest value in the zero-bias queue is used. Then, the time synchronization unit uses VIO processing delays and timestamps to align historical IMU measurements with historical PVQ states, eliminating out-of-sync measurements and state values. The weighted fitting unit then performs a weighted fitting between the PVQ time series calculated by median integration and the VIO-optimized PVQ time series. The weight of the VIO-optimized PVQ time series decreases uniformly from 1 to 0.005 based on the 2-second time and sequence length. Finally, the filtering and tracking unit performs filtering and tracking processing on the weighted PVQ time series, using a linear Kalman filter based on the current CS statistical model as the filtering and tracking method, setting the maneuver frequency to 0.05, to estimate the actual motion state of the machine at the divergence moment.

[0061] After VIO divergence, the aircraft enters attitude control mode. The controller relies on inertial navigation to maintain the current attitude of the aircraft and enters a uniform motion process. The motion state detection and estimation module monitors the acceleration and angular velocity of the current inertial sensor through the motion state detection unit. When the changes in acceleration and angular velocity are both less than the corresponding thresholds and remain below them for 1 second, it is considered to have entered a uniform motion state. The motion state estimation unit uses the median integration of the diverged acceleration and angular velocity, and calculates the current PVQ odometry of the aircraft, using the recovered divergence point as the starting point. Once the motion state detection unit determines that the aircraft has entered a uniform motion state, it saves the current PVQ odometry and sends it to the restart control module.

[0062] The restart control module sets the initial odometer state for VIO restart based on the PVQ odometer settings issued by the motion state detection and estimation module, and then issues a restart signal to control the VIO process to restart.

[0063] Example 3

[0064] This embodiment provides a method for handling VIO divergence anomaly recovery systems, such as... Figure 3 As shown, it includes the following steps:

[0065] (1) Monitor the VIO working status. After the VIO initialization is completed, proceed to step (2). When the VIO publishes the odometer information, its initialization is completed.

[0066] (2) The memory storage uses a queue to record a fixed length of the body's historical state information, including the body's position, velocity, attitude, acceleration, acceleration bias, angular velocity and angular velocity bias information. When the queue reaches a fixed length, the latest data is stored and the oldest data is discarded.

[0067] (3) Continuously monitor the working status of VIO, wait for VIO to report a divergence abnormal signal, repeat step (2), and enter step (4) when VIO divergence is detected.

[0068] (4) The divergence point recovery module performs zero-bias recovery, time synchronization, median integration, weighted fitting and filtering tracking on the historical state data in sequence to obtain the true motion state of the body at the divergence moment.

[0069] (5) The motion state detection and estimation module determines the motion state of the body based on inertial sensor data;

[0070] (6) Continuously monitor the body's motion state. When the body enters a state of uniform motion, repeat step (5). When the body moves at a uniform speed, proceed to step (7).

[0071] (7) The motion state detection and estimation module estimates the PVQ information of the body in the current uniform motion state based on the motion state of the body at the divergence moment;

[0072] (8) After receiving the estimated PVQ information in step (7), the restart control module controls the VIO process to restart and sets the PVQ information as the initial value when the VIO restarts.

[0073] In step (4), the divergence point recovery module performs zero-bias recovery, time synchronization, median integration, weighted fitting, and filtering tracking on the historical state data in sequence to obtain the true motion state of the organism at the divergence moment. The specific process includes:

[0074] (41) Zero bias recovery processing: Based on the historical zero bias information of the zero bias information storage unit, combined with the characteristics of the IMU zero bias being relatively stable and not changing, the quartile method is used to detect relevant information before the change, and the zero bias value at the moment before the change is used as the recovery zero bias value during the divergence period.

[0075] (42) Time synchronization processing: Based on the timestamp and VIO processing delay time, align the historical status of PVQ with the historical measurement information of IMU;

[0076] (43) Median integration processing: Using the motion posture of the most original frame of the aligned PVQ history as the initial value, the median integration is performed using the aligned IMU history measurement values ​​to obtain the time series of the PVQ motion state of the organism in the period before divergence;

[0077] (44) Weighted fitting: The PVQ time series obtained by median integration is weighted and fitted with the PVQ time series aligned in step (42). The fitting weight adopts the gradient descent strategy to gradually reduce the influence of the PVQ sequence in step (42).

[0078] (45) Filtering and tracking processing: Filtering and tracking are performed based on the weighted fitted PVQ time series. The current statistical model of CS is used as the kinematic model for Kalman filtering to estimate the true motion state of the body at the divergence moment.

[0079] In summary, the VIO divergence anomaly recovery system processing method provided in this embodiment can achieve highly versatile, highly accurate, and highly real-time VIO divergence anomaly recovery; it can improve the accuracy and stability of VIO recovery by making full use of the historical information of the body's motion before divergence, and is highly practical, intelligent, and adaptable.

Claims

1. A visual inertial odometry divergence anomaly recovery system, characterized in that, include: Memory storage: Stores the organism's historical state information for a period of time before divergence, including PVQ state storage unit, IMU information storage unit and zero bias information storage unit; Restart control module: Controls the restart of the visual inertial odometry process according to the body's motion status, and provides the visual inertial odometry with the body's motion status information at the time of restart, including the body's position, velocity, attitude, acceleration zero bias and angular velocity zero bias information; The divergence point recovery module includes a zero-bias recovery unit, a time synchronization unit, a median integration unit, a weighted fitting unit, and a filter tracking unit. Based on historical state information, it performs zero-bias recovery, time synchronization, median integration, weighted fitting, and filter tracking to restore the divergence point state information at the divergence moment. Motion state detection and estimation module: includes a motion state detection unit and a motion state estimation unit; The motion state detection unit detects the uniform motion state of the body based on the acceleration and angular velocity of the inertial sensor; the motion state estimation unit estimates the motion state of the body based on the recovered divergence point positioning information and the diverged acceleration and angular velocity.

2. The visual inertial odometry divergence anomaly recovery system according to claim 1, characterized in that, The PVQ state storage unit uses a queue of fixed time length to store the PVQ information released after the visual inertial odometry is initialized, including the body's position vector, velocity vector, attitude quaternion information and corresponding timestamp information.

3. The visual inertial odometry divergence anomaly recovery system according to claim 1, characterized in that, After the visual inertial odometry is initialized, the IMU information storage unit uses a queue to store inertial sensor observation data within the same time period as the PVQ information record, including the body's acceleration vector, angular velocity vector, and corresponding timestamp information.

4. The visual inertial odometry divergence anomaly recovery system according to claim 1, characterized in that, After the visual inertial odometry is initialized, the zero-bias information storage unit uses a queue to store the zero-bias data published by the visual inertial odometry thread within the same time period as the PVQ information record, including the zero-bias vector of the body's acceleration, the zero-bias vector of the angular velocity, and the corresponding timestamp information.

5. The visual inertial odometry divergence anomaly recovery system according to claim 1, characterized in that, The restart control module listens to the motion state information of the machine body released by the motion state detection and estimation module. If it detects that the machine body has entered a uniform speed state, it controls the visual inertial odometry process to restart.

6. The visual inertial odometry divergence anomaly recovery system according to claim 1, characterized in that, The zero-bias recovery unit recovers the zero-bias value during the divergence point based on the historical zero-bias information stored in the zero-bias information storage unit; the time synchronization unit aligns the PVQ status with the IMU measurement information using timestamps.

7. The visual inertial odometry divergence anomaly recovery system according to claim 1, characterized in that, The median integration unit performs median integration calculation based on the aligned PVQ state and IMU measurement information to obtain the PVQ time series before the body diverges; the weighted fitting unit combines the median integration result with the visual inertial odometry to optimize the PVQ time series and performs weighted fitting to output a highly accurate divergent body PVQ odometry.

8. The visual inertial odometry divergence anomaly recovery system according to claim 1, characterized in that, The tracking filtering unit filters and tracks the weighted fitted PVQ time series to recover high-precision state information of the location divergence point.

9. A method for handling divergence anomaly recovery systems in visual inertial odometry, characterized in that, Includes the following steps: (1) Monitor the working status of the visual inertial odometry. After the visual inertial odometry is initialized, proceed to step (2). When the visual inertial odometry publishes odometry information, its initialization is complete. (2) The memory storage uses a queue to record a fixed length of the body's historical state information, including the body's position, velocity, attitude, acceleration, acceleration bias, angular velocity and angular velocity bias information. When the queue reaches a fixed length, the latest data is stored and the oldest data is discarded. (3) Continuously monitor the working status of the visual inertial odometer, wait for the visual inertial odometer to report a divergence abnormal signal, repeat step (2), and proceed to step (4) when the visual inertial odometer diverges. (4) The divergence point recovery module performs zero-bias recovery, time synchronization, median integration, weighted fitting and filtering tracking on the historical state data in sequence to obtain the true motion state of the body at the divergence moment. (5) The motion state detection and estimation module determines the motion state of the body based on inertial sensor data; (6) Continuously monitor the body's motion state. When the body enters a state of uniform motion, repeat step (5). When the body moves at a uniform speed, proceed to step (7). (7) The motion state detection and estimation module estimates the PVQ information of the body in the current uniform motion state based on the motion state of the body at the divergence moment; (8) After receiving the estimated PVQ information in step (7), the restart control module controls the visual inertial odometry process to restart and sets the PVQ information as the initial value when the visual inertial odometry restarts.

10. The method for processing visual inertial odometry divergence anomaly recovery system according to claim 9, characterized in that, In step (4), the specific steps for the divergence point recovery module to obtain the actual motion state of the body at the divergence moment include: (41) Zero bias recovery processing: Based on the historical zero bias information of the zero bias information storage unit, combined with the characteristics of the IMU zero bias being relatively stable and not changing, the quartile method is used to detect relevant information before the change, and the zero bias value at the moment before the change is used as the recovery zero bias value during the divergence period. (42) Time synchronization processing: Based on the timestamp and visual inertial odometry processing delay time, align the PVQ historical status with the IMU historical measurement information; (43) Median integration processing: Using the motion posture of the most original frame of the aligned PVQ history as the initial value, the median integration is performed using the aligned IMU history measurement values ​​to obtain the time series of the PVQ motion state of the organism in the period before divergence; (44) Weighted fitting: The PVQ time series obtained by median integration is weighted and fitted with the PVQ time series aligned in step (42). The fitting weight adopts the gradient descent strategy to gradually reduce the influence of the PVQ sequence in step (42). (45) Filtering and tracking processing: Filtering and tracking are performed based on the weighted fitted PVQ time series. The current statistical model of CS is used as the kinematic model for Kalman filtering to estimate the true motion state of the body at the divergence moment.

Images