Robot asynchronous trajectory repositioning method based on degeneration evaluation

Abstract

The application discloses a robot asynchronous trajectory relocation method based on degradation evaluation, and realizes the following steps: acquiring multi-sensor observation and prior pose, identifying sensor degradation based on motion consistency residual, and demarcating the starting time, continuous interval and recovery time of the degradation section; after recovery, determining the to-be-relocated degradation subsequence according to the stability and spatial consistency of global positioning sensor and magnetic sensor measurement, constructing an independent asynchronous relocation optimization problem with key frame six-degree-of-freedom pose as a variable, jointly correcting the inter-frame prior motion constraint, global positioning sensor global position anchor and magnetic sensor global attitude anchor, and replacing or smoothly splicing the corrected trajectory and the main trajectory. The application does not interrupt real-time positioning, can reduce the degradation trajectory drift and recovery constraint conflict, and improves the trajectory accuracy, continuity and stability in the degradation environment.

Description

Technical Field

[0001] This invention belongs to the field of robot localization and navigation technology, and further relates to a robot asynchronous trajectory relocalization method based on degradation assessment in the field of multi-sensor fusion localization and navigation technology. This invention can be used for trajectory correction and continuous localization tasks in multi-sensor fusion localization systems under conditions of satellite signal blockage, magnetic field interference, indoor-outdoor transitions, and other complex degradation environments. Background Technology

[0002] Multi-sensor fusion simultaneous localization and mapping (SMART) technology improves the positioning accuracy and robustness of robots, unmanned systems, and mobile devices in complex environments by integrating information from multiple sources, including vision, inertial, satellite positioning, and magnetometry. Compared to methods relying solely on visual sensors or inertial measurement units (IMUs), incorporating global positioning and orientation information from Global Navigation Satellite System (GNSS) and magnetometers allows the system to suppress cumulative drift and obtain more stable global pose estimations across a wide range of scenarios. However, in practical engineering applications, sensors are often subject to occlusion or interference. For example, GNSS signals are prone to interruption or sudden drops in accuracy indoors, in tunnels, urban canyons, and densely built-up areas; magnetometers are susceptible to interference and unstable orientation measurements in electromagnetic environments; and visual observations may degrade in highly dynamic or low-texture areas. In such cases, multi-sensor fusion systems lacking effective degradation identification and recovery mechanisms are prone to problems such as trajectory drift, global offset, and positioning instability. Especially after the degraded sensor is restored, there is often a significant contradiction between the restored measurement and the trajectory that was offset during the degradation period. If the restored measurement is directly added back to the main optimization problem, it can easily lead to constraint conflicts, trajectory jumps, or even optimization failure.

[0003] Linghu Intelligent (Shenzhen) Co., Ltd. disclosed a map construction method for an embodied robot control system in its patent application "Map Construction Method for Embossed Robot Control System" (Application No. CN 202610133904.4, Publication No. CN 121639958 A). This method mainly extracts stable candidate features from low-resolution image sequences and sparse point cloud data, constructs a feature stability weight map, and forms a high-confidence map capable of relocation. This scheme focuses on static environment map feature selection, incremental map updates, and control reference map construction, which can improve map quality and relocation capability under weak perception conditions. However, the method still has shortcomings. Its technical focus is on map representation and map updating, mainly solving the problem of constructing high-confidence maps in static environments, without addressing online determination of sensor degradation during multi-sensor fusion positioning, degradation interval division, and post-correction of degraded trajectories. Therefore, when sensors degrade, relying solely on this type of high-confidence map scheme cannot repair the pose drift accumulated during degradation, and the restored positioning result may still deviate from the true trajectory.

[0004] The State Grid Jiangxi Electric Power Co., Ltd. Ultra-High Voltage Branch disclosed a multi-source fusion robot navigation method based on adaptive disaster recovery in its patent application, "Multi-source Fusion Robot Navigation Method and System Based on Adaptive Disaster Recovery Processing" (Application No. CN 202511959381.0, Publication No. CN121409254 A). This method mainly switches between main mode, auxiliary mode, and disaster recovery mode based on the matching degree between laser and BeiDou navigation and the BeiDou positioning error ellipse, and performs repositioning, return to home, or wait for recovery when positioning fails. This scheme focuses on maintaining navigation continuity and disaster recovery control under abnormal operating conditions, and can prevent the system from completely losing control when some sensors fail. However, the method still has shortcomings. Its technical approach mainly relies on mode switching to continue navigation or wait for recovery, primarily addressing navigation mode switching and disaster recovery operation after failure. Although it can maintain navigation continuity under abnormal operating conditions, it does not establish an independent a posteriori optimization correction mechanism at the degradation segment level for the historical pose deviations accumulated during the failure period. Therefore, once the sensor recovers, if recovery measurements are directly reactivated and connected to the main optimization process, the recovery measurements are likely to conflict with the cumulative drift formed during the degradation period. Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of the existing technology by proposing an asynchronous trajectory relocalization method for robots based on degradation assessment. This method aims to solve the technical problems of the existing technology, such as the difficulty in accurately dividing the degradation range, the conflict between global measurement and drift trajectory after recovery, and the difficulty in balancing real-time main process and a posteriori correction.

[0006] The technical approach to achieving the objective of this invention is as follows: First, based on existing multi-sensor degradation assessment results, the invention identifies degraded segments and combines the stability and spatial consistency after recovery to screen degraded subsequences to be relocated. Then, the degraded subsequences to be relocated are independently extracted from the main trajectory to construct a subsequence optimization problem. Therefore, without interrupting the main real-time positioning process, asynchronous a posteriori repair of historical trajectory offsets formed during degradation can be performed. Furthermore, the reliable global measurement corresponding to the recovery time index of the degraded subsequence is used as a global measurement anchor point to resolve the constraint conflict between the recovery measurement and the drift trajectory. This invention utilizes existing sensor degradation assessment techniques to obtain degradation state information. Based on this, it constructs a degradation segment temporal identification and extraction mechanism. By defining the start time, duration interval, and recovery time of the degradation segment, and judging recovery stability and spatial consistency based on the GNSS positioning measurement standard deviation and magnetometry orientation measurement deviation within a preset recovery time window, it identifies the degradation subsequences requiring asynchronous correction for repositioning. After the sensor recovers and meets the judgment conditions, this invention constructs an asynchronous degradation segment repositioning mechanism. Without interrupting the main real-time positioning process, it creates an asynchronous optimization task outside the main real-time positioning process. Each time, it independently constructs and solves the degradation subsequence optimization problem for only one degradation subsequence to be repositioned, thereby solving the problem that the trajectory continues to drift during degradation and the recovery measurement cannot be directly incorporated into the main optimization. This invention uses a priori skeleton and recovery anchor point joint optimization mechanism to simultaneously utilize the inter-frame prior motion constraints retained during degradation, the recovered GNSS global position anchor points, and the magnetometer global attitude anchor points for joint correction during degradation subsequence optimization, thereby solving the problem of conflict between the recovered global measurement and the drift trajectory. This invention utilizes a parallel correction mechanism that does not affect the real-time main process, decoupling the relocation process from the main localization process for asynchronous execution. Asynchronous posterior correction is performed using the prior trajectory of the degraded segment and the recovered reliable global measurements. After correction, according to the chronological order of each degraded segment in the main trajectory, the corresponding corrected trajectory replaces the degraded segment in the main trajectory, or a smooth stitching is performed at the connection point between the corrected trajectory and the main trajectory. This solves the problem of simultaneously managing the real-time main process and posterior correction, thereby achieving smooth relocation of the degraded segment trajectory.

[0007] To achieve the above objectives, the specific implementation steps of the present invention include the following:

[0008] Step 1: Obtain the output of the existing sensor degradation assessment process, process the sensor measurement sequence arranged in time frame by frame, identify the start time, duration interval and recovery time of each degradation segment, and determine the degradation subsequence to be relocated corresponding to each degradation segment based on the stability and spatial consistency after recovery.

[0009] Step 2: For each degenerate subsequence to be relocalized, construct an objective function for an independent asynchronous relocalization optimization problem with only the six-DOF pose of the keyframe of the subsequence as the optimization variable, and jointly optimize the inter-frame motion constraints of the prior trajectory of the degenerate segment and the constraints of the recovered reliable global measurement anchor points.

[0010] Step 3: Asynchronously solve the independent asynchronous relocation optimization problem to obtain the correction trajectory for each degenerate segment;

[0011] Step 4: Repeat steps 2 and 3, processing only one degenerate subsequence at a time, until all degenerate subsequences have been processed;

[0012] Step 5: Combine the correction trajectories of all degenerate segments with the main trajectory and output the final trajectory.

[0013] Furthermore, the output of the existing sensor degradation assessment process includes: a time-ordered sequence of sensor measurements. Motion consistency residuals across frames Degradation judgment threshold Threshold for the number of consecutive normal frames The post-recovery stability criterion requires that GNSS positioning measurements and magnetometer measurements be obtained simultaneously within a preset post-recovery time window, and includes a GNSS positioning measurement standard deviation threshold. And magnetic orientation measurement deviation threshold .

[0014] Furthermore, the step of processing the time-ordered sensor measurement sequence frame by frame to identify the start time, duration interval, and recovery time of each degradation segment includes:

[0015] The first step is to set the degenerate subsequences. Current degenerate subsequence All are initialized to an empty set, and consecutive normal frames are counted. Set as Spatial consistency marker Set to false, and set the current recovery event pending judgment flag. Set to false and change the state of the previous frame. Initialize to normal state;

[0016] The second step is to measure the sequence of sequential sensors. Frame-by-frame processing yields a set of degraded subsequences. and the recovery time index corresponding to the degenerate subsequence This is to identify the start time, duration, and recovery time of each degradation segment; among which, , This indicates the total number of frames in the measurement sequence;

[0017] when At that time, the current frame state will be... Set to a degraded state and count consecutive normal frames. Set to zero, where, Indicates the first Frame motion consistency residuals, degradation judgment threshold This represents the threshold for motion consistency residuals used to distinguish between normal and degenerate states. Pre-select from the positive real number candidate range based on the motion consistency residual distribution under normal and degraded working conditions, sensor type, and application scenario;

[0018] when At that time, Set to normal state and add ;

[0019] when It is in a degraded state, and the state of the previous frame is... When in normal state, the current frame join in ;

[0020] when and When all are in a degraded state, change the current frame state. Merging ;

[0021] when Normal state For a degenerate state, and When not empty, Merging ,Will Set as ,Will Set to true and clear. ;in, This represents the threshold for the number of consecutive normal frames. The number of consecutive normal frames required for recovery status confirmation, the measurement sequence frame rate, and the recovery confirmation duration are pre-selected; after processing the current frame, the status of the previous frame is updated. ;

[0022] when ,and When not empty, The subsequences to be relocated will be retained and stability determination will be performed after subsequent data arrives and the recovery status is detected.

[0023] The third step, for the currently closed recovery event, is to complete data collection within its corresponding preset recovery post-time window and... If the result is true, a stability check will be performed once. After the check is completed, If set to false, the recovery event will not be processed again in subsequent frames;

[0024] The fourth step is to output the set of degenerate subsequences after frame-by-frame processing and updating. and the recovery time index corresponding to the degenerate subsequence .

[0025] Furthermore, the step of determining the degenerate subsequence to be relocated corresponding to each degenerate segment based on the restored stability and spatial consistency is as follows:

[0026] The first step is to start from the updated set of degenerate subsequences. Select an unprocessed degenerate subsequence as the current processing object;

[0027] The second step involves obtaining the standard deviation of GNSS positioning measurements within the time window after the selected degenerate subsequence has been collected and both GNSS positioning measurements and magnetometer measurements are present simultaneously. And magnetic orientation measurement deviation ;when and At that time, spatial consistency flags Set to true, otherwise, Set as false; in When true, the currently processed degenerate subsequence is recorded as the degenerate subsequence to be relocated. And index the recovery time corresponding to the degraded subsequence. The corresponding reliable global measurements serve as global measurement anchor points for the degraded subsequence to be relocated; when the time window lacks GNSS positioning measurements or magnetometer measurements, Set to false and do not select ;in, This represents the threshold for the standard deviation of GNSS positioning measurements. It is pre-selected from the non-negative real number candidate range based on the positioning accuracy and measurement standard deviation distribution under normal GNSS operating conditions; This indicates the magnetic orientation measurement deviation threshold, which... It is pre-selected from the non-negative real number candidate range based on the magnetic force orientation measurement deviation distribution and magnetometer measurement accuracy under normal magnetic field conditions;

[0028] The third step is to determine whether the set of degenerate subsequences has been processed. If any of the degenerate subsequences in the given set are true, then output the degenerate subsequence to be relocated that satisfies the conditions for stability and spatial consistency after restoration. Spatial consistency markers And the set of global measurement anchor points corresponding to the degenerate subsequence to be relocated; otherwise, return to step one.

[0029] Furthermore, the pose variables of the six-degree-of-freedom rigid body are:

[0030] ;

[0031] in, Indicates the first degenerate subsequence to be relocated Keyframes in the world coordinate system The pose variables below, Indicates the first Keyframes in the world coordinate system The positional parameters below, Represents the first quaternion corresponding to the unit quaternion. Keyframes in the world coordinate system The following are the attitude parameters to be optimized; the unit quaternion has three independent rotational degrees of freedom. and Together constitute the first Six-DOF rigid body pose variables for each keyframe This represents the index of the last keyframe in the degraded subsequence to be relocated, which contains... There are 1 keyframe, with keyframe indices as follows: to .

[0032] Furthermore, the objective function expression for the independent asynchronous relocation optimization problem is as follows:

[0033] ;

[0034] in, Indicates the optimization variable Find the minimum value; This represents the summation of residual terms with respect to their subscripts; for any residual vector and weight Define the weighted norm ;when When it is a scalar, it is agreed that For objective functions where weights are not labeled, Take the weights of the identity matrix; This represents the index of the keyframe in the degraded subsequence to be relocated. Indicates the relationship with the first The next keyframe adjacent to each keyframe, and ; This represents the inter-frame prior residual, which is used in the summation of inter-frame prior residuals. Pick to , This represents the prior relative observations between adjacent keyframes. and They represent the first The and the first The body coordinate system of each keyframe This represents the positive definite weight matrix corresponding to the prior relative observations; Indices representing GNSS observations This represents the set of pairs of GNSS observation indices and keyframe indices of the subsequence to be relocated. ; This represents the GNSS global position anchor residual. Indicates that the index is And corresponding to the first GNSS observations for each keyframe This represents the positive weight of the GNSS observations; Indicates and The set of keyframe indices corresponding to the selected magnetometer attitude observations within the internal keyframes, and , express Keyframe index in; This represents the global attitude anchor point residual of the magnetometer. This indicates the magnetometer's attitude observation.

[0035] The inter-frame prior residual for:

[0036] ;

[0037] in, Represents prior relative observations between adjacent keyframes and optimization variables The calculated inter-frame prior residuals In the prior trajectory of the degenerate segment, the first... Keyframes in the world coordinate system The fixed reference rotation matrix below, Represents the transpose of a matrix. These represent the optimization variables. The Middle The and the first Keyframes in the world coordinate system The positional parameters below, This represents the prior position increment between adjacent keyframes. Represents the quaternion logarithmic mapping, and Represent the attitude parameters to be optimized in unit quaternion form. To represent quaternion multiplication, express The quaternion inverse, This represents the unit quaternion prior pose increment between adjacent keyframes.

[0038] The GNSS global position anchor point residual and the global attitude anchor point residual of the magnetometer They are respectively:

[0039] ;

[0040] ;

[0041] in, Indicates based on GNSS observations and optimization variables The calculated GNSS global position anchor point residuals, This indicates that the magnetometer attitude observations are accurate. and optimization variables The calculated global attitude anchor point residual of the magnetometer Indicates the attitude parameters to be optimized The determined first Keyframes in the world coordinate system The candidate rotation matrix is ​​used to transform vectors in the body coordinate system to the world coordinate system. This represents the lever arm parameters of the GNSS relative to the aircraft's coordinate system. Indicates the first GNSS position measurements corresponding to each keyframe express The Middle The unit quaternion global attitude observations corresponding to each keyframe, provided by the magnetometer and converted to the world coordinate system, are used to construct the magnetometer's global attitude anchor points.

[0042] Furthermore, the asynchronous solution of the independent asynchronous relocation optimization problem refers to creating an asynchronous optimization task outside the main real-time positioning process without interrupting the main real-time positioning process, solving the independent asynchronous relocation optimization problem of the currently processed degenerate subsequence to be relocated, and outputting the corresponding degenerate segment correction trajectory.

[0043] Furthermore, the phrase "combining the correction trajectories of all degenerate segments with the main trajectory" refers to replacing the degenerate segments in the main trajectory with the corresponding correction trajectories according to the time sequence of each degenerate segment in the main trajectory, or performing smooth splicing at the connection point between the correction trajectories and the main trajectory.

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

[0045] First, because this invention utilizes existing multi-sensor motion consistency degradation assessment results to identify degradation intervals and determines the degraded subsequence to be relocated based on the stability and spatial consistency after recovery, it can divide degradation intervals and determine the start time, duration, and recovery time of degradation segments when sensor performance degrades. This overcomes the shortcomings of unclear degradation boundaries and difficulty in confirming recovery states in the prior art, making subsequent trajectory repair have clear time boundaries and correction objects, and providing a stable basis for post-hoc correction of degradation segments.

[0046] Second, because this invention targets each degraded subsequence to be relocated, it only uses the six-DOF pose of the key frame of the subsequence as the optimization variable, and adopts the joint optimization technology of the inter-frame motion constraints of the prior trajectory of the degraded segment and the recovered GNSS global position anchor point and magnetometer global attitude anchor point. It also decouples the relocation process from the main positioning process and executes them asynchronously. This allows the degraded segment to be extracted separately from the main trajectory and an independent asynchronous relocation optimization problem to be constructed for only one degraded subsequence each time. This overcomes the shortcomings of directly incorporating the restored measurement into the main optimization, which can easily lead to constraint conflicts, trajectory jumps and optimization instability, as well as the difficulty in balancing the real-time main process and the a posteriori correction. This allows the trajectory of the degraded segment to return to a globally consistent state through replacement or smooth splicing without interrupting the main real-time output, and significantly improves the trajectory accuracy in degraded scenarios.

[0047] Third, since this invention is designed for complex scenarios such as GNSS masking, visual degradation, IMU degradation, and indoor-outdoor transition, and simulation experiments have proven that it can reduce the peak error, decrease the global offset during the recovery phase, and improve the consistency between the degraded trajectory and the recovered GNSS global position anchor point and magnetometer global attitude anchor point in GNSS degradation, visual degradation, and IMU degradation scenarios, it overcomes the shortcomings of existing technologies in terms of insufficient adaptability and easy deterioration of accuracy in complex degradation environments. This makes the invention have good positioning accuracy and robustness in multiple degradation scenarios. Attached Figure Description

[0048] Figure 1 This is an overall flowchart of the method of the present invention;

[0049] Figure 2 This is a flowchart of the degradation segment identification and recovery determination based on continuous spatiotemporal state analysis of the present invention;

[0050] Figure 3 This is a schematic diagram illustrating the construction of the degenerate subsequence relocation optimization problem in this invention;

[0051] Figure 4 This is a schematic diagram of asynchronous solution and trajectory fusion output in this invention. Detailed Implementation

[0052] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0053] Reference Figure 1 The specific implementation steps of this invention are described in detail below. Step one is the preliminary basic process 1, and step two is the preliminary basic process 2. These two preliminary basic processes describe the basic processes for sensor degradation assessment utilized in this invention, including multi-sensor data acquisition and basic pose estimation, as well as obtaining the sensor degradation state using existing degradation assessment techniques. Steps three, four, and five are the core innovative parts of this invention, describing the degradation segment identification, asynchronous degradation subsequence relocation optimization, and trajectory fusion output process based on existing degradation assessment results.

[0054] Step 1, Preliminary Basic Process 1, Multi-sensor Data Acquisition and Basic Pose Estimation.

[0055] A multi-sensor fusion positioning system is established by acquiring temporal observation data from GNSS and magnetometers, and optionally from cameras and IMUs. GNSS and magnetometers are used at least for post-recovery stability assessment, while visual sensors and IMUs are optional preliminary positioning data sources. A sliding window optimization method is employed to obtain prior pose estimation results for each keyframe of the system, providing a basic trajectory skeleton for subsequent degradation assessment and trajectory relocalization. During the optimization process, robust kernel functions and outlier removal mechanisms can be used to handle potential anomalies, improving the stability of prior pose estimation results in complex degradation scenarios.

[0056] Step 2, Preliminary Basic Process 2, Constructing Sensor Motion Descriptors and Degradation Assessment.

[0057] Utilizing existing motion-consistency-based sensor degradation assessment techniques, for each type of sensor, a six-degree-of-freedom relative motion transformation vector is constructed between the latest frame at the current moment and multiple historical key frames within a sliding window. These vectors are then arranged chronologically to form the corresponding sensor's motion descriptor, thus unifying the representation of system motion across different sensors. Specifically, the visual sensor calculates inter-frame relative pose through feature matching and motion estimation (PnP), the IMU obtains displacement and rotation increments through pre-integration, the GNSS calculates position displacement using global position difference, and the magnetometer calculates attitude changes using global attitude observations.

[0058] Based on this, cross-validation is performed on the motion descriptors of different sensors, and the motion consistency residuals among the sensors are calculated. ,in and These represent the two sensors participating in the cross-validation. Indicates the current keyframe index. The similarity measure between sensor motion descriptors is obtained by weighted summation of position difference terms (using Mahalanobis distance) and attitude difference terms (using the squared Euclidean distance between axis-angle vectors and normalized). Indicates sensor The corresponding degradation judgment threshold is pre-selected based on sensor characteristics, the distribution of motion consistency residuals under normal and degraded operating conditions, and the application scenario. When the minimum consistency residual between a sensor and other sensors exceeds the preset degradation threshold... When this happens, the sensor is determined to have degraded. Upon confirmation of degradation, the optimization constraints corresponding to the degraded sensor are downweighted or temporarily removed, and a lenient-in, strict-out state switching mechanism is used to avoid frequent jitter. The time interval weights in the motion descriptor... It is determined based on the time interval between the current frame and historical keyframes; the larger the time interval, the better. The smaller.

[0059] The aforementioned pre-process 1 and pre-process 2 are the sensor degradation assessment basic processes used in the embodiments of the present invention. The degradation state variables and motion consistency residuals of each frame output by them serve as the input basis for steps three, four and five of the present invention.

[0060] Step 3: Degenerate segment identification and status analysis.

[0061] Reference Figure 2 Based on the degradation assessment results on continuous time frames, the start time, duration, and recovery time of the degradation segment are identified. The degradation segment identification process comprehensively considers whether the current frame is determined to be degraded, whether the number of consecutive normal frames meets the recovery judgment condition, whether GNSS positioning measurements and magnetometer measurements are obtained simultaneously within the recovery time window, whether the standard deviation of GNSS positioning measurements and the deviation of magnetic orientation measurements simultaneously meet the corresponding thresholds, and whether the recovery measurements meet the spatial consistency condition, so as to accurately delineate the degraded subsequences that need to be relocated for trajectory posterior correction.

[0062] The set of degenerate subsequences is processed one by one according to the degenerate event, with only one degenerate subsequence processed at a time, so that the currently processed degenerate subsequence corresponds one-to-one with the corresponding recovery time index, spatial consistency flag and global measurement anchor point set.

[0063] The state analysis process input includes a sequence of sensor measurements arranged in time. Motion consistency residuals across frames Degradation judgment threshold Threshold for the number of consecutive normal frames The post-recovery stability criterion requires that GNSS positioning measurements and magnetometer measurements be obtained simultaneously within a preset post-recovery time window, and includes a GNSS positioning measurement standard deviation threshold. And magnetic orientation measurement deviation threshold ;in, This represents the motion consistency residual threshold used to distinguish between normal and degraded states. This threshold is pre-selected from a range of positive real number candidates based on the motion consistency residual distribution under normal and degraded conditions, sensor type, and application scenario. This represents the threshold for the number of consecutive normal frames, which is pre-selected based on the number of consecutive normal frames required for recovery status confirmation, the measurement sequence frame rate, and the recovery confirmation duration. This represents the standard deviation threshold for GNSS positioning measurements. This threshold is pre-selected from a range of non-negative real number candidates based on the positioning accuracy and standard deviation distribution under normal GNSS operating conditions. This represents the magnetic orientation measurement deviation threshold, which is pre-selected from a range of non-negative real numbers based on the magnetic orientation measurement deviation distribution under normal magnetic field conditions and the magnetometer measurement accuracy. The output includes a set of degenerate subsequences. and the recovery time index corresponding to the degenerate subsequence After restoring and determining stability and spatial consistency, the degenerate subsequence to be relocated is output. Spatial consistency markers And the global measurement anchor set. This process first includes the aforementioned degenerate subsequence set. Initialized as an empty set, current degenerate subsequence Initialized to an empty set, consecutive normal frame count Spatial consistency markers Current recovery event pending determination flag and the state of the previous frame Initialize to normal state; to The sequential sensor measurement sequence is processed frame by frame, and the motion consistency residual of the current frame is assigned a value based on the degradation assessment result. ;when At that time, the frame state will be... Set to degraded state and count consecutive normal frames. Set to zero; when At that time, the frame state will be... Set to normal state and count consecutive normal frames. Add one; when It is in a degraded state and the state of the previous frame. When in normal state, the current frame Add to the current degenerate subsequence ;when and When all frames are in a degraded state, the current frame will be... Merging ;when Normal state For a degenerate state, and When not empty, Merging ,Will Set as ,Will Set to true and clear. After processing the current frame, update the state of the previous frame. For a closed current recovery event, data collection is completed within its corresponding preset recovery post-time window and... If true, check if both GNSS positioning measurements and magnetometer measurements exist simultaneously within the window; if both exist, calculate the standard deviation of the GNSS positioning measurements. And magnetic orientation measurement deviation ,when and At that time, Set to true, from the set of degenerate subsequences Select the degenerate subsequence corresponding to the current recovery event, and denote it as the degenerate subsequence to be relocated. And index the recovery time corresponding to the degraded subsequence. The corresponding reliable global measurements are used as global measurement anchor points for the degraded subsequence to be relocated; the global measurement anchor points include GNSS global position anchor points and magnetometer global attitude anchor points in the optimization; otherwise, Set to false and do not select Alternatively, add the aforementioned anchor points; if GNSS positioning measurements or magnetometer measurements are missing from the window, then... Set to false and do not select After completing the current recovery event determination, Setting it to false means that the recovery event will not be processed again in subsequent frames; when and When not empty, The subsequences to be relocated are retained and the stability determination is performed again after subsequent data arrives and the recovery status is detected.

[0064] Step 4: Construct the asynchronous relocation optimization problem for degenerate subsequences.

[0065] Reference Figure 3This indicates that the identified degenerate subsequences to be relocated... Construct independent optimization problems, in which This represents the index of the last keyframe in the degenerate subsequence to be relocated, where the number of keyframes is [number missing]. The optimization variables consist of the six-degree-of-freedom rigid body pose variables of each keyframe in the degenerate subsequence to be relocalized, thus focusing the relocalization problem on the trajectory of the degenerate segment itself. ;

[0066] in, Indicates the first degenerate subsequence to be relocated Keyframes in the world coordinate system The pose variables of a six-DOF rigid body are as follows: Indicates the first Keyframes in the world coordinate system The positional parameters below, Represent the first quaternion in unit quaternion form Keyframes in the world coordinate system The following are the attitude parameters to be optimized; the unit quaternion has three independent rotational degrees of freedom. and Together they constitute the six-DOF rigid body pose variables of the keyframe, and .

[0067] The optimization objective is defined as follows:

[0068] ;

[0069] ;

[0070] ;

[0071] ;

[0072] in, This indicates the degenerate subsequence to be relocated. This represents the set of pairs of GNSS observation indices and keyframe indices of the subsequence to be relocated. and , Indicates and The set of keyframe indices corresponding to the selected magnetometer attitude observations within the internal keyframes, and , Indices representing GNSS observations This represents the index of the keyframe in the degraded subsequence to be relocated, and is included in the summation of the prior residuals between frames. Pick to , Indicates the relationship with the first The next keyframe adjacent to a keyframe, express Keyframe index in and They represent the first The and the first The body coordinate system of each keyframe This represents the inter-frame prior residual. Representing prior relative observables and optimization variables The inter-frame prior residuals are used as inputs for calculation. This represents the prior relative observations between adjacent keyframes. This represents the positive definite weight matrix corresponding to the prior relative observations. This represents the GNSS global position anchor residual. Indicated by GNSS observations and optimization variables The input is the calculated global position anchor point residual. Indicates GNSS observations, Indicates the positive weight of GNSS observations. This represents the global attitude anchor point residual of the magnetometer. Indicates the measurement by magnetometer attitude. and optimization variables The input is the calculated global attitude anchor point residual. Represents the magnetometer attitude observation; for any residual vector and weight Define the weighted norm ,when When it is a scalar, it is agreed that Unweighted values ​​in the objective function Take the weights of the identity matrix, i.e. .

[0073] Indicates the attitude parameters to be optimized The determined candidate rotation matrix is ​​used to transform vectors in the body coordinate system to the world coordinate system. This represents the fixed reference rotation matrix in the prior trajectory of the degenerate segment. and These represent the optimization variables. The Middle The and the first Keyframes in the world coordinate system The positional parameters below, and Represent the attitude parameters to be optimized in unit quaternion form. Indicates the inverse of a quaternion. This represents the prior position increment between adjacent keyframes. This represents the unit quaternion prior pose increment between adjacent keyframes. This represents the lever arm parameters of the GNSS relative to the aircraft's coordinate system. Indicates the first GNSS position measurements corresponding to each keyframe express The Middle The unit quaternion global attitude observations corresponding to each keyframe, provided by the magnetometer and transformed to the world coordinate system, are used to construct the magnetometer global attitude anchor points. The inter-frame prior residuals represent the inter-frame motion constraints within the subsequence; the GNSS global position anchor residuals and the magnetometer global attitude anchor residuals provide global correction references, thereby correcting the entire degraded sequence to a position and attitude consistent with the recovered reliable global observations.

[0074] Step 5: Asynchronous solution and trajectory fusion output.

[0075] Reference Figure 4 Without interrupting the main real-time localization process, an asynchronous optimization task is created outside of it. Each time, only one degenerate subsequence to be relocalized is asynchronously solved for its independent asynchronous relocalization optimization problem, and the corresponding degenerate segment correction trajectory is output. This asynchronous solution process is repeated until all degenerate subsequences to be relocalized have been processed. After obtaining all correction results, according to the chronological order of each degenerate segment in the main trajectory, the corresponding degenerate segment correction trajectory replaces the degenerate segment in the main trajectory, or a smooth concatenation is performed at the connection point between the correction trajectory and the main trajectory, outputting a continuous, stable, and globally consistent final trajectory.

[0076] Taking the handling of a degradation event as an example, the complete execution process of the method of the present invention is described as follows: First, the motion consistency residual is calculated frame by frame for the sensor measurement sequence arranged in time. When the residual of the current frame is greater than the degradation judgment threshold, the frame is added to the current degradation subsequence. When the degradation state ends and the number of consecutive normal frames exceeds the threshold, the degradation process continues. At that time, close the current degenerate segment and record the recovery time. Subsequently, measurement data within a preset recovery time window is collected, only when both GNSS positioning measurements and magnetometer measurements exist simultaneously within the window, and the following conditions are met. and Only then is the degenerate subsequence identified as a degenerate subsequence to be relocated. And index the recovery time corresponding to the degraded subsequence. The corresponding reliable global measurements are used as global measurement anchors; these global measurement anchors include GNSS global position anchors and magnetometer global attitude anchors in the optimization process. Then, an asynchronous optimization task is created outside the main real-time positioning process. Using the six-degree-of-freedom pose of each keyframe within the degraded subsequence to be relocated as optimization variables, the independent asynchronous relocation optimization problem is solved by combining inter-frame prior motion constraints, GNSS global position anchors, and magnetometer global attitude anchors to obtain the correction trajectory of the degraded segment. Finally, the degraded segment is replaced or smoothly stitched together according to its temporal position in the main trajectory. For other degraded events, the above process is repeated until all degraded subsequences to be relocated have been processed.

[0077] In visual degradation scenarios, feature points with excessive reprojection errors can be removed as visual outliers, and the weight of visual residuals can be reduced, making state estimation more dependent on IMU measurements and reliable GNSS orientation measurements.

[0078] The effectiveness of this invention can be further demonstrated through the following experiments.

[0079] 1. Simulation experimental conditions.

[0080] The simulation experiment hardware platform of this invention is: Intel Xeon Silver-4210R CPU processor and 64GB memory.

[0081] 2. Simulation content and result analysis.

[0082] The simulation experiments of this invention include two parts: ablation experiments and degradation experiments. The degradation experiments include GNSS degradation experiments, visual degradation experiments, and IMU degradation experiments. Here, "sequence" refers to a continuous trajectory acquisition sequence from the dataset used in the experiment, used to describe continuous running data under different test scenarios. ARE represents absolute rotation error, in degrees (°), used to measure the angular deviation between the trajectory attitude estimation result and the true trajectory; ATE represents absolute trajectory error, in meters (m), used to measure the distance deviation between the trajectory position estimation result and the true trajectory.

[0083] In the above sequences, urban38 and urban39 are from the KAIST public dataset, collected in a complex urban canyon environment, including cameras, IMUs, and GNSS, and exhibit intermittent GNSS signal degradation, multipath interference, and high dynamic scenes; building00, building01, building02, parking00, and playground00 are from the i2Nav public dataset, where building00 to building02 are highly obscured urban building cluster scenes, parking00 is an underground parking lot scene with completely obscured GNSS signals, and playground00 is an open low-degradation scene; gym, street, playground_low, and playground_high are from a self-made private degradation dataset, used to supplement visual degradation and low-cost IMU degradation scenes that are difficult to reproduce systematically in the public dataset.

[0084] 2.1. The ablation experiment is a simulation of the effectiveness of the asynchronous relocation mechanism.

[0085] To verify the contribution of the asynchronous relocation mechanism to trajectory correction, a system variant with the asynchronous relocation module disabled was used as a comparison scheme, while retaining sensor degradation assessment and degradation processing, and compared with the method of the present invention, as shown in Table 1.

[0086] Table 1 Summary of Ablation Experiment Results

[0087] sequence Disable asynchronous relocation (ARE / ATE) This invention (ARE / ATE) playground00 0.48 / 0.15 0.48 / 0.15 building00 0.56 / 0.51 0.55 / 0.51 building02 0.66 / 0.23 0.69 / 0.22 building01 1.07 / 0.72 0.78 / 0.30 parking00 2.51 / 0.98 0.53 / 0.48

[0088] As shown in Table 1, in long-term GNSS degradation scenarios such as building01 and parking00, the system exhibits significant global offset after disabling asynchronous relocation. However, the present invention can perform overall posterior correction on the trajectory of the degradation segment, effectively reducing the error peak and minimizing trajectory jumps and global offsets during the recovery phase. In the playground00 scenario without significant long-term degradation, the results of disabling and enabling asynchronous relocation are basically consistent, indicating that the mechanism will not have a negative impact on the fusion results under normal circumstances.

[0089] 2.2. The GNSS degradation experiment is a simulation of the positioning accuracy under GNSS signal degradation scenarios.

[0090] The GNSS degradation experiment was based on the sequences urban38, urban39, building00, building01, building02, and parking00. The output pose results of this invention were compared in two cases: using real GNSS orientation measurement and using real trajectory position to simulate GNSS orientation measurement. The results are shown in Table 2.

[0091] Table 2 GNSS Degradation Experiment Results

[0092] sequence This invention (real GNSS) This invention (simulated GNSS) urban38 1.24 / 4.37 1.07 / 3.89 urban39 1.51 / 4.58 1.47 / 4.12 building00 0.34 / 0.46 0.34 / 0.42 building01 0.33 / 0.22 0.31 / 0.21 building02 0.19 / 0.22 0.20 / 0.19 parking00 0.43 / 0.48 0.28 / 0.24

[0093] As shown in Table 2, under conditions of severe GNSS signal obstruction or multipath interference, the present invention can maintain stable estimation by relying on the visual-inertial subsystem, and smoothly reintroduce global constraints after GNSS recovery. The shifted pose is asynchronously corrected by posterior global measurement, and finally a relatively accurate and stable result is obtained.

[0094] 2.3. Visual degradation experiments are simulations of localization robustness in scenarios of visual feature degradation.

[0095] Compared to the playground_low sequence, the playground_high sequence features large and persistent dynamic objects that frequently appear in the image, leading to frequent degradation and failure of visual features. The performance of this invention under visual degradation conditions was tested by comparing these two sequences, as shown in Table 3.

[0096] Table 3 Results of Visual Degradation Experiment

[0097] sequence This invention (ARE / ATE) playground_low 0.27 / 0.32 playground_high 0.35 / 0.33

[0098] As shown in Table 3, in the playground_high sequence where visual features are more severely degraded, the present invention can automatically reduce the visual residual weight or directly remove feature points with excessive reprojection errors, making pose estimation rely more on IMU and GNSS orientation measurements, avoiding divergence in estimation results due to visual degradation, and maintaining the accuracy of positioning results.

[0099] 2.4. The IMU degradation experiment is a simulation of the localization robustness under the degradation scenario of a low-cost IMU.

[0100] Experiments were conducted on a private dataset using a tactical-grade IMU (Adis16465) and an industrial-grade IMU (Deta30), respectively. The experimental results show that although the low-cost IMU introduces greater noise and drift, the present invention can still suppress error propagation through multi-sensor consistency constraints, demonstrating good robustness to IMU degradation, as shown in Table 4.

[0101] Table 4 Results of IMU Degradation Experiment

[0102] sequence Adis16465 (ARE / ATE) Deta30 (ARE / ATE) gym 0.33 / 0.24 0.34 / 0.27 street 0.32 / 0.29 0.37 / 0.28 playground_low 0.27 / 0.32 0.30 / 0.40 playground_high 0.35 / 0.33 0.42 / 0.51

[0103] As shown in Table 4, even when the IMU noise is large or the drift is more obvious, the present invention can still maintain the stability of trajectory estimation by means of multi-sensor consistency constraints, which shows that the present invention has good adaptability to IMU degradation.

Claims

1. A robot asynchronous trajectory relocalization method based on degradation assessment, characterized in that, The steps of this relocation method include the following: Step 1: Obtain the output of the existing sensor degradation assessment process, process the sensor measurement sequence arranged in time frame by frame, identify the start time, duration interval and recovery time of each degradation segment, and determine the degradation subsequence to be relocated corresponding to each degradation segment based on the stability and spatial consistency after recovery. Step 2: For each degenerate subsequence to be relocalized, construct an objective function for an independent asynchronous relocalization optimization problem with only the six-DOF pose of the keyframe of the subsequence as the optimization variable, and jointly optimize the inter-frame motion constraints of the prior trajectory of the degenerate segment and the constraints of the recovered reliable global measurement anchor points. Step 3: Asynchronously solve the independent asynchronous relocation optimization problem to obtain the correction trajectory for each degenerate segment; Step 4: Repeat steps 2 and 3, processing only one degenerate subsequence at a time, until all degenerate subsequences have been processed; Step 5: Combine the correction trajectories of all degenerate segments with the main trajectory and output the final trajectory.

2. The asynchronous trajectory relocalization method for robots according to claim 1, characterized in that, The output of the existing sensor degradation assessment process described in step 1 includes: a sequence of sensor measurements arranged in chronological order. Motion consistency residuals across frames Degradation judgment threshold Threshold for the number of consecutive normal frames The post-recovery stability criterion requires that GNSS positioning measurements and magnetic orientation measurements be obtained simultaneously within a preset post-recovery time window, and includes a GNSS positioning measurement standard deviation threshold. And magnetic orientation measurement deviation threshold .

3. The asynchronous trajectory relocalization method for robots according to claim 2, characterized in that, Step 1, which involves processing the time-ordered sensor measurement sequence frame by frame to identify the start time, duration, and recovery time of each degradation segment, includes the following steps: The first step is to set the degenerate subsequences. Current degenerate subsequence All are initialized to an empty set, and consecutive normal frames are counted. Set as Spatial consistency marker Set to false, and set the current recovery event pending judgment flag. Set to false and change the state of the previous frame. Initialize to normal state; The second step is to measure the sequence of sequential sensors. Frame-by-frame processing yields a set of degraded subsequences. and the recovery time index corresponding to the degenerate subsequence This is to identify the start time, duration, and recovery time of each degradation segment; among which, , This indicates the total number of frames in the measurement sequence; when At that time, the current frame state will be... Set to a degraded state and count consecutive normal frames. Set to zero, where, Indicates the first Frame motion consistency residuals, degradation judgment threshold This represents the threshold for motion consistency residuals used to distinguish between normal and degenerate states. Pre-select from the positive real number candidate range based on the motion consistency residual distribution under normal and degraded working conditions, sensor type, and application scenario; when At that time, Set to normal state and add ; when It is in a degraded state, and the state of the previous frame is... When in normal state, the current frame join in ; when and When all are in a degraded state, change the current frame state. Merging ; when Normal state For a degenerate state, and When not empty, Merging ,Will Set as ,Will Set to true and clear. ;in, This represents the threshold for the number of consecutive normal frames. The number of consecutive normal frames required for recovery status confirmation, the measurement sequence frame rate, and the recovery confirmation duration are pre-selected; after processing the current frame, the status of the previous frame is updated. ; when ,and When not empty, The subsequences to be relocated will be retained and stability determination will be performed after subsequent data arrives and the recovery status is detected. The third step, for the currently closed recovery event, is to complete data collection within its corresponding preset recovery post-time window and... If the result is true, a stability check will be performed once. After the check is completed, If set to false, the recovery event will not be processed again in subsequent frames; The fourth step is to output the set of degenerate subsequences after frame-by-frame processing and updating. and the recovery time index corresponding to the degenerate subsequence .

4. The asynchronous trajectory relocalization method for a robot according to claim 3, characterized in that, The steps described in step 1 for determining the degenerate subsequence to be relocated corresponding to each degenerate segment based on the restored stability and spatial consistency are as follows: The first step is to start from the updated set of degenerate subsequences. Select an unprocessed degenerate subsequence as the current processing object; The second step involves obtaining the standard deviation of GNSS positioning measurements within the time window after the selected degenerate subsequence has been collected and both GNSS positioning measurements and magnetic orientation measurements are simultaneously present. And magnetic orientation measurement deviation ;when and At that time, spatial consistency flags Set to true, otherwise, Set as false; in When true, the currently processed degenerate subsequence is recorded as the degenerate subsequence to be relocated. And index the recovery time corresponding to the degraded subsequence. The corresponding reliable global measurements serve as global measurement anchor points for the degenerate subsequence to be relocated; When the time window lacks GNSS positioning measurements or magnetometer measurements, Set to false and do not select ;in, This represents the threshold for the standard deviation of GNSS positioning measurements. It is pre-selected from the non-negative real number candidate range based on the positioning accuracy and measurement standard deviation distribution under normal GNSS operating conditions; This indicates the threshold for magnetic orientation measurement deviation. It is pre-selected from the non-negative real number candidate range based on the magnetic force orientation measurement deviation distribution and magnetometer measurement accuracy under normal magnetic field conditions; The third step is to determine whether the set of degenerate subsequences has been completely processed. If any of the degenerate subsequences in the given set are true, then output the degenerate subsequence to be relocated that satisfies the conditions for stability and spatial consistency after restoration. Spatial consistency markers And the set of global measurement anchor points corresponding to the degenerate subsequence to be relocated; otherwise, return to step one.

5. The asynchronous trajectory relocalization method for a robot according to claim 4, characterized in that, The six-degree-of-freedom rigid body pose variables mentioned in step 2 are: ； in, Indicates the first degenerate subsequence to be relocated Keyframes in the world coordinate system The pose variables below, Indicates the first Keyframes in the world coordinate system The positional parameters below, Represents the first quaternion corresponding to the unit quaternion. Keyframes in the world coordinate system The following are the attitude parameters to be optimized; the unit quaternion has three independent rotational degrees of freedom. and Together constitute the first Six-DOF rigid body pose variables for each keyframe This represents the index of the last keyframe in the degraded subsequence to be relocated, which contains... There are 1 keyframe, with keyframe indices as follows: to .

6. The asynchronous trajectory relocalization method for a robot according to claim 5, characterized in that, The objective function expression for the independent asynchronous relocation optimization problem described in step 2 is as follows: ； in, Indicates the optimization variable Find the minimum value; This represents the summation of residual terms with respect to their subscripts; for any residual vector and weight Define the weighted norm ;when When it is a scalar, it is agreed that For objective functions where weights are not labeled, Take the weights of the identity matrix; This indicates the index of the keyframe in the degraded subsequence to be relocated. Indicates the relationship with the first The next keyframe adjacent to each keyframe, and ; This represents the inter-frame prior residual, which is used in the summation of inter-frame prior residuals. Pick to , This represents the prior relative observations between adjacent keyframes. and They represent The first and the second The body coordinate system of each keyframe This represents the positive definite weight matrix corresponding to the prior relative observations; Indices representing GNSS observations This represents the set of pairs of GNSS observation indices and keyframe indices of the subsequence to be relocated. ; This represents the GNSS global position anchor point residual. Indicates that the index is And corresponding to the first GNSS observations for each keyframe This represents the positive weight of the GNSS observations; Indicates and The set of keyframe indices corresponding to the selected magnetometer attitude observations within the internal keyframes, and , express Keyframe index in; This represents the global attitude anchor point residual of the magnetometer. This indicates the magnetometer's attitude observation.

7. The asynchronous trajectory relocalization method for a robot according to claim 6, characterized in that, The inter-frame prior residual for: ； in, Represents prior relative observations between adjacent keyframes and optimization variables The calculated inter-frame prior residuals In the prior trajectory of the degenerate segment, the first... Keyframes in the world coordinate system The fixed reference rotation matrix is ​​below. To represent the transpose of a matrix, These represent the optimization variables. The Middle The and the first Keyframes in the world coordinate system The positional parameters below, This represents the prior position increment between adjacent keyframes. Represents the quaternion logarithmic mapping, and Represent the attitude parameters to be optimized in unit quaternion form. To represent quaternion multiplication, express The quaternion inverse, This represents the unit quaternion prior pose increment between adjacent keyframes.

8. The asynchronous trajectory relocalization method for a robot according to claim 6, characterized in that, The GNSS global position anchor point residual and the global attitude anchor point residual of the magnetometer They are respectively: ； ； in, Indicates based on GNSS observations and optimization variables The calculated GNSS global position anchor point residuals, This indicates that the magnetometer attitude observations are accurate. and optimization variables The calculated global attitude anchor point residual of the magnetometer Indicates the attitude parameters to be optimized The determined first Keyframes in the world coordinate system The candidate rotation matrix is ​​used to transform vectors in the body coordinate system to the world coordinate system. This represents the lever arm parameters of the GNSS relative to the aircraft's coordinate system. Indicates the first GNSS position measurements corresponding to each keyframe express The Middle The unit quaternion global attitude observations corresponding to each keyframe, provided by the magnetometer and converted to the world coordinate system, are used to construct the magnetometer's global attitude anchor points.

9. The asynchronous trajectory relocalization method for a robot according to claim 1, characterized in that, The asynchronous solution of the independent asynchronous relocation optimization problem mentioned in step 3 refers to creating an asynchronous optimization task outside the main real-time localization process without interrupting the main real-time localization process, solving the independent asynchronous relocation optimization problem of the currently processed degenerate subsequence to be relocated, and outputting the corresponding degenerate segment correction trajectory.

10. The asynchronous trajectory relocalization method for a robot according to claim 1, characterized in that, The step 5, which involves combining the correction trajectories of all degenerate segments with the main trajectory, means replacing the degenerate segments in the main trajectory with the corresponding correction trajectories according to the time sequence of each degenerate segment in the main trajectory, or performing smooth splicing at the connection between the correction trajectories and the main trajectory.

Images