Short-distance inertia auxiliary positioning method and system for a composite robot

Abstract

The present application relates to the technical field of mobile robot navigation and positioning, and provides a short-distance inertial auxiliary positioning method and system for a compound robot, comprising: calculating an environment degradation index based on real-time multi-source sensor data; continuously adjusting the confidence weight of the main navigation and inertial sensor to perform short-distance dead reckoning when the degradation index reaches a threshold; constructing a virtual observation constraint to suppress integral drift based on kinematic constraints or quasi-stationary state in the reckoning mode; obtaining a restored absolute observation pose and correcting the reckoning pose based on a gradual strategy when the degradation index falls back. The present application eliminates navigation switching pose jump, effectively suppresses short-distance inertial drift, and realizes positioning continuity and global pose seamless connection in the feature degradation area.

Description

Technical Field

[0001] This invention relates to the field of mobile robot navigation and positioning technology, specifically to a short-range inertial-assisted positioning method and system for a composite robot. Background Technology

[0002] In mobile robot navigation technology, LiDAR or vision sensors are typically used as the primary navigation source, supplemented by inertial measurement units (IMUs) for state estimation. However, in environments with degraded features, such as long corridors, narrow passages, or dynamically changing areas, the primary navigation sensor is prone to failure due to feature loss or interference. Existing technologies often employ threshold-triggered hard switching logic directly relying on the inertial sensor when primary navigation failure is detected. This step-like weight switching causes abrupt jumps in confidence between filter predictions and observations, leading to discontinuities or even divergence in pose estimation. Furthermore, low-cost inertial sensors suffer from cumulative drift in short-distance estimations lacking external absolute observations, and the repositioning process after leaving degraded areas is prone to control oscillations, making it difficult to meet the requirements of high-precision operations. Summary of the Invention

[0003] To address the problems of pose jumps during navigation transitions, cumulative drift in inertial estimation, and repositioning oscillations in existing technologies under feature degradation scenarios, this invention provides a short-range inertial-assisted localization method, device, and system for a composite robot, achieving lossless connection between localization continuity and global pose within feature degradation regions. This invention provides the following technical solution: A short-range inertial-assisted localization method for a composite robot, comprising: Based on real-time multi-source sensor data from the composite robot, the degradation index of the current environment is calculated. When the degradation index reaches a preset degradation threshold, the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter are continuously adjusted based on the degradation index to achieve short-range dead reckoning. In the short-distance dead reckoning mode, virtual observation constraints are constructed based on the kinematic physical constraints and / or quasi-stationary state of the composite robot, and the virtual observation constraints are used to suppress the integral drift error of the inertial sensor. When the degradation index falls below the degradation threshold, the absolute observation pose recovered by the main navigation sensor is obtained, and the pose calculated by the short-distance dead reckoning is corrected in a closed loop based on a progressive correction strategy to achieve continuous positioning of the composite robot.

[0004] Optionally, the calculation of the degradation index of the current environment based on real-time multi-source sensor data from the composite robot includes: Extract a multimodal environmental degradation feature vector, which includes a first feature characterizing the degradation of environmental geometric features and a second feature characterizing the degradation of environmental dynamic disturbances; The degradation index is obtained by weighting and fusing the multimodal environment degradation feature vectors based on preset weights. The step of continuously adjusting the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter based on the degradation index includes: The degradation index is input into a continuously differentiable smooth transition function to dynamically calculate the confidence weight of the inertial sensor and the complementary weight of the main navigation sensor. The observation noise covariance matrix of the main navigation sensor is dynamically scaled based on the confidence weights of the inertial sensor.

[0005] Optionally, the first feature includes local point cloud planarity variance and feature matching inlier rate, and the second feature includes inter-frame optical flow variance and dynamic obstacle pixel ratio. The degradation index is calculated by weighting and summing the first feature and the second feature according to a preset normalized weighting coefficient; wherein, the normalized complementary value of the flatness variance relative to its maximum threshold, the complementary value of the feature matching inlier rate, the proportion of dynamic obstacle pixels, and the normalized value of the optical flow variance relative to its maximum threshold are respectively used as terms in the weighted summation. The smaller the flatness variance, the lower the feature matching inlier rate, the higher the proportion of dynamic obstacle pixels, and the larger the optical flow variance, the larger the degradation index. The smooth transition function is the Sigmoid function. The greater the difference between the degradation index and the preset degradation threshold, the greater the confidence weight of the inertial sensor, and the change of the confidence weight is continuously differentiable. The observation noise covariance matrix of the main navigation sensor is dynamically scaled based on the basic observation noise covariance matrix and the confidence weight of the inertial sensor. The larger the confidence weight, the larger the scaling factor of the observation noise covariance matrix.

[0006] By quantifying the local point cloud planarity variance, feature matching inlier rate, optical flow variance, and the pixel ratio of dynamic obstacles, the feature degradation degree of long corridors, narrow channels, and dynamic regions is accurately captured and weighted. The Sigmoid function is used to achieve continuous differentiable change of the confidence weight of the inertial sensor, thereby dynamically amplifying the observation noise covariance matrix of the main navigation sensor. This avoids the pose step jump caused by hard threshold switching in traditional navigation, ensuring the smoothness of motion and control stability of the composite robot when entering and exiting feature degradation regions.

[0007] Optionally, the construction of virtual observation constraints based on the kinematic physical constraints and / or quasi-stationary state of the composite robot includes: The lateral and vertical velocities of the composite robot in the body coordinate system are constrained to zero, and a nonholonomic constrained virtual observation equation is constructed. Based on the longitudinal velocity and yaw rate of the composite robot, the observation noise covariance of the nonholonomic constraint virtual observation equation is dynamically adjusted, wherein the observation noise covariance is positively correlated with the longitudinal velocity and the yaw rate. In response to the detection that the composite robot meets the preset quasi-stationary conditions, a zero-velocity update mechanism is triggered to construct a zero-velocity virtual observation equation and perform a forced contraction operation on the velocity error covariance matrix.

[0008] Optionally, the observation noise covariance of the nonholonomic constrained virtual observation equation is dynamically adjusted based on the standard deviation of the base noise and the absolute values ​​of the longitudinal velocity and yaw rate in the body coordinate system. The observation noise covariance is positively correlated with the absolute values ​​of the longitudinal velocity and yaw rate. The preset quasi-stationary conditions include: the variance of the acceleration sliding window is less than a first threshold and the variance of the angular velocity sliding window is less than a second threshold; the duration of satisfying the variance conditions is greater than or equal to a preset settling time; and the absolute value of the longitudinal velocity is less than an extremely low speed threshold. The forced shrinkage operation on the velocity error covariance matrix includes: multiplying the velocity error covariance matrix by a preset shrinkage factor, wherein the shrinkage factor is a constant greater than 0 and less than 1; In the short-range dead reckoning mode, the process noise covariance matrix is ​​dynamically adjusted based on the basic process noise covariance matrix, the confidence weight of the inertial sensor, and the rate of change of acceleration between the current time and the previous time. The larger the confidence weight and the rate of change of acceleration, the larger the scaling factor of the process noise covariance matrix.

[0009] By establishing a dynamic adjustment mechanism that positively correlates the nonholonomic constraint observation noise covariance with the absolute value of longitudinal / yaw angular velocity, and combining a multi-dimensional quasi-stationary condition composed of variance threshold, duration, and extremely low speed threshold, zero-speed updates are precisely triggered and the velocity error covariance is forcibly contracted. At the same time, the process noise covariance is dynamically amplified by using confidence weight and acceleration change rate. Thus, without adding external hardware labels, strong and adaptive suppression of short-distance inertial calculation integral drift is achieved, significantly improving the robustness and accuracy of robot relative positioning under high dynamic or micro-motion conditions.

[0010] Optionally, the step of acquiring the absolute observation pose recovered by the main navigation sensor and performing closed-loop correction on the pose calculated from the short-range dead reckoning based on a progressive correction strategy includes: In response to the degradation index reaching the preset degradation threshold for the first time, the absolute pose calculated by the main navigation sensor at the current moment is locked as the pose anchor point. In response to the degradation index falling below the preset degradation threshold, the pose residual between the current estimated pose and the observed pose rematched by the main navigation sensor is calculated; Within a preset number of control cycles, the dynamic correction weight for each control cycle is calculated based on the exponential decay function. Based on the dynamic correction weights, the pose residuals are progressively injected into the state estimation to smoothly correct the current estimated pose to the observed pose.

[0011] Optionally, in the short-distance dead reckoning mode, the relative displacement of the current reckoned pose with respect to the pose anchor point is monitored in real time; In response to the relative displacement exceeding a preset safe distance threshold, a calculated failure warning is triggered, and the composite robot is controlled to slow down or stop. The calculation of the dynamic correction weight for each control cycle based on the exponential decay function includes: the dynamic correction weight decreases exponentially with the increase of the control cycle number, and the initial correction strength coefficient and the smoothing time constant jointly determine the deceleration rate; The step of progressively injecting the pose residuals into the state estimation includes: based on the dynamic correction weights, superimposing the abscissa residuals, ordinate residuals, and heading angle residuals in the pose residuals onto the corresponding uncorrected inertial estimation values ​​to obtain the corrected abscissa, ordinate, and heading angle; in response to the completion of smooth correction, resetting the state covariance matrix of the error state Kalman filter to a reference value that is less than the covariance corresponding to the pose anchor point.

[0012] By monitoring the relative displacement of the inferred pose with respect to the anchor point in real time, a safety fallback warning is provided for inference failure. The pose residual is gradually injected into the state estimate according to the horizontal and vertical coordinates and the heading angle using an exponential decay function to achieve a seamless transition. After the correction is completed, the state covariance matrix of the error state Kalman filter is actively reset to a reference value that is less than the covariance corresponding to the anchor point, thereby completely cutting off the backward propagation chain of residual error and ensuring the lossless recovery of global positioning accuracy and the absolute safety of system operation after the composite robot drives out of the degradation zone.

[0013] Optionally, the real-time multi-source sensor data based on the composite robot includes: Using the high-frequency data from the inertial sensor as a time reference, the interpolation time window is dynamically adjusted based on the acceleration and angular velocity of the composite robot; Within the interpolation time window, cubic spline interpolation is used to map the low-frequency observation data of the main navigation sensor to a unified reference timestamp to eliminate motion distortion and phase difference; The adjustment amount of the interpolation time window is negatively correlated with the combined value of the linear acceleration amplitude and the angular velocity amplitude of the composite robot. The larger the linear acceleration amplitude and the angular velocity amplitude, the smaller the adjustment amount of the interpolation time window.

[0014] This invention further discloses a short-range inertial-assisted positioning system for a composite robot, comprising: The degradation index calculation module is used to calculate the degradation index of the current environment based on real-time multi-source sensor data from the composite robot. The confidence dynamic adjustment module is used to continuously adjust the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter based on the degradation index when the degradation index reaches a preset degradation threshold, so as to perform short-distance dead reckoning. The error suppression module is used to construct virtual observation constraints based on the kinematic physical constraints and / or quasi-stationary state of the composite robot in the short-distance dead reckoning mode, and to use the virtual observation constraints to suppress the integral drift error of the inertial sensor. The closed-loop correction module is used to obtain the absolute observation pose recovered by the main navigation sensor when the degradation index falls below the degradation threshold, and to perform closed-loop correction on the pose calculated by the short-distance dead reckoning based on a progressive correction strategy, so as to realize the continuous positioning of the composite robot.

[0015] The present invention further discloses a computer program product, including a computer program that implements the above-described method when executed by a processor.

[0016] According to the technical solution of the present invention, by calculating the environmental degradation index of multi-source sensor data in real time and continuously adjusting the confidence weights of the main navigation and inertial sensors in the positioning filter when the degradation index reaches a threshold, the pose jump problem caused by hard switching in traditional navigation in feature degradation scenarios such as long corridors or narrow channels is effectively solved, and a smooth transition to short-distance dead reckoning mode is achieved. In the short-distance dead reckoning process, virtual observation constraints are constructed based on the kinematic physical constraints and quasi-stationary state of the composite robot, overcoming the technical problem of rapid accumulation of integral drift of low-cost inertial sensors when lacking external absolute reference, and significantly suppressing the reckoning error. Finally, when the environmental features are restored and the degradation index falls back, a progressive correction strategy is used to perform closed-loop correction on the reckoned pose and the restored absolute observation pose, eliminating the backward propagation of residual errors, thereby ensuring that the composite robot achieves high-precision, jump-free continuous positioning in complex industrial sites and meeting the stringent requirements of precise end-point docking. Attached Figure Description

[0017] For illustrative and not limiting purposes, the present invention will now be described in conjunction with embodiments and accompanying drawings, wherein: Figure 1 This is a flowchart of the short-distance inertial-assisted positioning method for a composite robot according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the components of the composite robot short-distance inertial-assisted positioning system in an embodiment of the present invention. Detailed Implementation

[0018] To enable those skilled in the art to better understand the present invention, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0019] It should be noted that, where there is no conflict, the embodiments and features of the present invention can be combined with each other. The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0020] refer to Figure 1 This specific embodiment provides a short-range inertial-assisted localization method for a composite robot. This method focuses on the continuous localization of the composite robot in feature degradation scenarios and integrates four core components: spatiotemporal synchronization, degradation assessment and smooth switching, kinematic constraint drift suppression, and anchor point memory and closed-loop correction. This method is applicable to various mobile robots or unmanned vehicles, as long as they possess a combination of a main navigation sensor and an inertial sensor. The method includes the following steps: S100: Calculates the degradation index of the current environment based on real-time multi-source sensor data from the composite robot.

[0021] In step S100, preprocessing is performed on the real-time multi-source sensor data of the composite robot. Specifically, to eliminate motion distortion and phase difference caused by differences in sensor sampling frequencies and network transmission delays, this specific implementation adopts an adaptive sliding window interpolation alignment mechanism using high-frequency data from the inertial sensor as the time reference. For example, the sampling rate of the inertial sensor is 200Hz, while the frequency of the main navigation sensor (such as LiDAR or 3D vision) is typically 10Hz to 30Hz. The interpolation time window is dynamically adjusted according to the current linear acceleration amplitude and angular velocity amplitude of the composite robot. The adjustment amount of the interpolation time window is negatively correlated with the combined value of the linear acceleration amplitude and angular velocity amplitude of the composite robot; that is, the larger the linear acceleration amplitude and angular velocity amplitude, the smaller the adjustment amount of the interpolation time window. In a specific implementation example, the time window adjustment formula can be expressed as: ,in, Adjustment amount for the time window Let this be the linear acceleration vector of the robot. Let ω be the robot's angular velocity vector. This is the scaling factor. This is the angular velocity weighting coefficient. To prevent the smoothing constant from being divided by zero, cubic spline interpolation is used within the dynamically adjusted interpolation time window to accurately map the low-frequency observation data of the main navigation sensor to a unified reference timestamp. Through this adaptive mechanism, when the composite robot performs high-dynamic movements such as sharp turns in narrow channels, the time window automatically shrinks to improve temporal resolution and ensure that high-frequency motion information is not distorted; while during smooth movement, the time window automatically expands to reduce computational consumption. This specific implementation effectively overcomes the timestamp misalignment problem caused by wireless network jitter in industrial environments through the above data preprocessing steps, providing a high-precision spatiotemporal aligned data foundation for subsequent positioning calculations.

[0022] S200: When the degradation index reaches a preset degradation threshold, the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter are continuously adjusted based on the degradation index to achieve short-distance dead reckoning.

[0023] Specifically, this step aims to quantify the degree of degradation of environmental features, providing a basis for subsequent sensor weight adjustments. The degradation index calculation process includes: extracting a multimodal environmental degradation feature vector, which includes a first feature characterizing the degradation of environmental geometric features and a second feature characterizing the degradation of environmental dynamic interference; then, the multimodal environmental degradation feature vector is weighted and fused based on preset weights to obtain the degradation index. In this specific embodiment, the first feature specifically includes the local point cloud planarity variance and the feature matching inlier rate, and the second feature specifically includes the image frame optical flow variance and the proportion of dynamic obstacle pixels. Among them, the planarity variance is used to characterize the degree of degradation of geometrically simple scenes such as long corridors; the smaller the planarity variance, the simpler the environmental features. The feature matching inlier rate is used to characterize the degree of degradation of narrow channels or severely occluded areas; the lower the matching inlier rate, the fewer usable features. The proportion of dynamic obstacle pixels and the optical flow variance are used to characterize the degree of degradation of dynamic interference scenes such as densely populated areas or temporary cargo storage areas. The degradation index is calculated by weighting and summing the first and second features using preset normalized weighting coefficients. The normalized complementary values ​​of the flatness variance relative to its maximum threshold, the complementary values ​​of the feature matching inlier rate, the proportion of dynamic obstacle pixels, and the normalized value of the optical flow variance relative to its maximum threshold are used as the weighted summation terms. This means that the smaller the flatness variance, the lower the feature matching inlier rate, the higher the proportion of dynamic obstacle pixels, and the larger the optical flow variance, the larger the calculated degradation index, indicating more severe environmental degradation.

[0024] The specific formula for calculating the degradation index is as follows: ,in, Let be the degradation index at time t, with a value range of [0,1]. The closer it is to 1, the more severe the environmental degradation. The variance of the local point cloud flatness. This represents the historical maximum allowable threshold for the local point cloud flatness variance. The feature matching in-point rate, with a value range of [0,1]. The percentage of pixels representing dynamic obstacles; The variance of optical flow between image frames. The historical maximum allowable threshold for the inter-frame optical flow variance of the image; , , , The normalized weight coefficients for each feature satisfy... This can be obtained through training on offline scene data. In this formula, The term can accurately reflect the positioning divergence problem caused by the lack of lateral geometric features in long corridors. In long corridors, the planarity variance is extremely small, and this term approaches 1.

[0025] When the degradation index reaches a preset degradation threshold, the confidence weights of the primary navigation sensor and the inertial sensor in the positioning filter are continuously adjusted based on the degradation index to perform short-range dead reckoning. To avoid pose jumps caused by traditional hard handover, this specific implementation inputs the degradation index into a continuously differentiable smooth transition function to dynamically calculate the confidence weights of the inertial sensor and the complementary weights of the primary navigation sensor. Specifically, the smooth transition function uses the Sigmoid function, whose expression is: ,in, Let be the confidence weight of the inertial sensor at time t. Let be the degradation exponent at time t. To preset the degradation threshold, To adjust the sensitivity coefficient, the complementary weights of the main navigation sensor are: Due to the characteristics of the Sigmoid function, the larger the difference between the degradation exponent and the preset degradation threshold, the greater the confidence weight of the inertial sensor, and the change in the confidence weight is continuously differentiable. Based on this, the observation noise covariance matrix of the main navigation sensor is dynamically scaled according to the confidence weight of the inertial sensor. Specifically, the observation noise covariance matrix of the main navigation sensor... Based on the covariance matrix of the basic observation noise The confidence weights of the inertial sensors are dynamically scaled, and the calculation formula is as follows: ,in This is the covariance scaling factor. The larger the confidence weight, the larger the scaling factor of the observation noise covariance matrix. In the update step of the Kalman filter, as... As the weight increases, the Kalman gain automatically decreases, causing the filter to reduce its confidence in the current primary navigation observations and rely more on the predictions from the inertial sensors. This specific implementation, through the aforementioned smooth switching mechanism, mathematically achieves a seamless handover of confidence between primary and inertial navigation, completely eliminating pose jumps and control oscillations caused by sudden weight changes, and ensuring the smoothness of the composite robot's motion when entering and exiting feature degradation regions.

[0026] S300: In the short-distance dead reckoning mode, based on the kinematic physical constraints and / or quasi-stationary state of the composite robot, virtual observation constraints are constructed, and the virtual observation constraints are used to suppress the integral drift error of the inertial sensor.

[0027] Step S300 includes two parallel suppression mechanisms: nonholonomic constraint (NHC) virtual observation and quasi-stationary zero-rate update (Quasi-ZUPT).

[0028] First, regarding nonholonomic constraint virtual observation, the lateral and vertical velocities of the composite robot in the body coordinate system are constrained to zero, and nonholonomic constraint virtual observation equations are constructed. Specifically, let the three-dimensional velocities in the body coordinate system calculated by the inertial sensor be... Forced lateral speed and vertical velocity With constraints approximately zero, construct the NHC observation equations: ,in, For nonholonomic constraint observation vectors, For the observation matrix, Let be the error state vector. To address the observation noise, the physical basis for this constraint is that when the composite robot navigates in confined spaces such as narrow passages, significant lateral slippage or vertical hopping is virtually impossible due to physical space limitations. To avoid filter divergence caused by minute actual sideslip during extreme conditions such as high-speed turns, this specific implementation dynamically adjusts the observation noise covariance of the nonholonomic constraint virtual observation equation based on the composite robot's longitudinal velocity and yaw rate. Specifically, the observed noise covariance is based on the baseline noise standard deviation. The absolute values ​​of longitudinal velocity and yaw rate in the body coordinate system are dynamically adjusted, as shown in the formula: ,in and These are the weighting coefficients for linear velocity and angular velocity, respectively. This indicates a positive correlation between the observation noise covariance and the absolute values ​​of longitudinal velocity and yaw angular velocity; that is, the faster the speed or the sharper the turn, the more relaxed the constraint weights should be. Through this dynamic coupling mechanism, this specific implementation can effectively suppress lateral drift when traveling at a constant speed in a narrow channel, and prevent the filter from diverging due to excessive constraints when turning at high speeds, achieving a balance between robustness and accuracy.

[0029] Secondly, regarding the quasi-stationary zero-velocity update, in response to the detection that the composite robot meets the preset quasi-stationary conditions, the zero-velocity update mechanism is triggered, a zero-velocity virtual observation equation is constructed, and a forced contraction operation is performed on the velocity error covariance matrix. It is important to note that the quasi-stationary detection (Quasi-ZUPT) in this specific embodiment differs from conventional zero-velocity updates; it is specifically designed for the deceleration-micro-adjustment-stopping characteristics of the composite robot when docking with a machine or passing through a narrow gate. The preset quasi-stationary conditions include three sub-conditions that must be met simultaneously: the variance of the acceleration sliding window is less than a first threshold and the variance of the angular velocity sliding window is less than a second threshold; the duration of satisfying the variance conditions is greater than or equal to a preset settling time; and the absolute value of the longitudinal velocity is less than an extremely low-velocity threshold. When it is determined that the robot has entered a quasi-stationary state, the ZUPT mechanism is triggered, and the zero-velocity observation equation is constructed: After the Kalman filter update step, the velocity error covariance matrix is... Perform forced contraction: ,in, It is a contraction factor. It can instantly reduce the uncertainty of velocity estimation during the docking micro-motion stage, effectively cut off the cumulative error of position integration over short distances, and ensure the absolute high accuracy of the final docking posture.

[0030] Furthermore, in the short-distance dead reckoning model, the process noise covariance matrix It is not fixed, but rather based on the fundamental process noise covariance matrix. Confidence weights of inertial sensors And the rate of change of acceleration between the current moment and the previous moment is dynamically adjusted. Specifically, the formula for calculating the noise covariance of the dynamic process is: ,in, The basic process noise covariance matrix, To dynamically adjust the gain, and These are the acceleration measurements at the current and previous moments, respectively. The sampling period is defined as [value]. The greater the confidence weight and the rate of change of acceleration, the greater the scaling factor of the process noise covariance matrix. This means that when environmental degradation is severe and the robot is in a highly dynamic adjustment state, the filter will reduce its confidence in the historical inertial integral model, thus relying more strongly on the aforementioned NHC and ZUPT virtual observations to correct the current pose in the update step, preventing the filter from diverging under high dynamic conditions. This specific implementation significantly improves the relative positioning accuracy within feature degradation regions without increasing the hardware cost of any external positioning tags by integrating a dual virtual observation mechanism that combines kinematic constraints and operational characteristics.

[0031] S400: When the degradation index falls below the degradation threshold, the absolute observation pose recovered by the main navigation sensor is obtained, and the pose calculated by the short-distance dead reckoning is closed-loop corrected based on a progressive correction strategy to achieve continuous positioning of the composite robot.

[0032] Specifically, in response to the degradation index reaching a preset degradation threshold for the first time, the absolute pose calculated by the main navigation sensor at the current moment is locked as the pose anchor point. This anchor point records the high-precision absolute coordinates at the instant of entering the degradation zone. and the corresponding covariance matrix As the sole absolute reference for subsequent short-range pure inertial deductions, it severs the error propagation chain with earlier historical trajectories. In short-range dead reckoning mode, the relative displacement of the current deduced pose with respect to the pose anchor point is also monitored in real time, calculated using the following formula: ,in, Calculate the relative displacement of the pose with respect to the anchor point at the current time t. These are the two-dimensional coordinates of the current estimated pose. These are the two-dimensional coordinates of the anchor point. This is in response to the relative displacement exceeding a preset safety distance threshold. This triggers a calculation failure warning and controls the composite robot to slow down or stop. This safety boundary monitoring mechanism sets strict physical boundaries for pure inertial calculations, preventing errors from diverging infinitely or even causing collisions due to excessively long degradation regions or IMU drift exceeding suppression capabilities.

[0033] When the composite robot leaves the degradation zone, in response to the degradation index falling below a preset degradation threshold, it calculates the pose residual between the currently estimated pose and the observed pose rematched by the main navigation sensor. Specifically, it obtains the absolute observed pose rematched by the main navigation sensor. Calculate the current estimated pose The residual: ,in, The function is used to normalize the angle difference to The interval ensures the shortest path correction of the heading angle residual. To avoid chassis jerking or sudden changes in the robot arm coordinate system caused by directly cutting the pose to the observed value, this specific implementation calculates the dynamic correction weight for each control cycle based on an exponential decay function within multiple preset control cycles. Specifically, the dynamic correction weight... The number of cycles decreases exponentially as the control cycle number k increases, as shown in the formula: ,in The initial correction strength coefficient, As a smoothing time constant, both factors jointly determine the deceleration rate. Based on dynamic correction weights, the pose residual is progressively injected into the state estimate to smoothly correct the currently estimated pose to the observed pose. Specifically, in the k-th control cycle, the corrected pose calculation formula is: ,in, , , These are the x-coordinate, y-coordinate, and heading angle after correction in the k-th control cycle. , , These are the uncorrected inertia estimates for the k-th control cycle. As k increases, The correction amount decreases exponentially, gradually reducing to achieve a smooth transition. In response to the completion of the smooth correction, for example... or The state covariance matrix of the error state Kalman filter is reset to a baseline value that is less than the covariance corresponding to the pose anchor point. This reset operation explicitly declares to the filter that the current pose has been restored to absolute confidence, providing an excellent prior initial value for the next normal navigation phase. This specific implementation, through the aforementioned anchor point memory and progressive correction strategy, completely solves the control oscillation problem caused by repositioning when leaving the degradation zone, achieving lossless recovery of global positioning accuracy and a safety net.

[0034] refer to Figure 2 This embodiment further discloses a short-range inertial-assisted positioning system for a composite robot, including a degradation index calculation module 21, a confidence dynamic adjustment module 22, an error suppression module 23, and a closed-loop correction module 24. These modules work together to achieve the aforementioned positioning method, which will be described in detail below: The degradation index calculation module 21 is used to calculate the degradation index of the current environment based on real-time multi-source sensor data from the composite robot. Specifically, the degradation index calculation module 21 is responsible for evaluating the degree of feature degradation of the current environment in real time. This module receives multi-source data from the main navigation sensors, such as LiDAR, 3D vision, and inertial sensors. First, it extracts multimodal environmental degradation feature vectors, including the first feature characterizing the degradation of environmental geometric features, namely the local point cloud planarity variance and feature matching inlier rate, and the second feature characterizing the degradation of environmental dynamic disturbances, namely the image frame inter-frame optical flow variance and the pixel ratio of dynamic obstacles. Then, the module performs weighted fusion of the multimodal environmental degradation feature vectors based on preset normalized weight coefficients to calculate the degradation index. The degradation index ranges from [0,1], with values ​​closer to 1 indicating more severe environmental degradation. This module outputs the calculated degradation index in real time to the confidence dynamic adjustment module 22 and the closed-loop correction module 24, serving as the core criterion for system mode switching.

[0035] The confidence dynamic adjustment module 22 is used to continuously adjust the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter based on the degradation index when the degradation index reaches a preset degradation threshold, in order to perform short-range dead reckoning. Specifically, the confidence dynamic adjustment module 22 is responsible for dynamically adjusting the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter according to the degradation index. When the degradation index reaches the preset degradation threshold, the module inputs the degradation index into a continuously differentiable smooth transition function to dynamically calculate the confidence weight of the inertial sensor. Complementary weights of the main navigation sensor Simultaneously, this module dynamically scales the observation noise covariance matrix of the main navigation sensor based on the confidence weights of the inertial sensor. ,in This is the covariance scaling factor. In this way, the confidence dynamic adjustment module mathematically achieves a seamless and smooth handover of confidence between the main navigation and inertial navigation systems, avoiding the pose jump problem caused by traditional hard handovers. This module outputs the adjusted confidence weights and covariance matrix to the error suppression module 23 and the positioning filter to guide the subsequent pose estimation process.

[0036] Error suppression module 23 is used to construct virtual observation constraints based on the kinematic physical constraints and / or quasi-stationary state of the composite robot in the short-distance dead reckoning mode, and to suppress the integral drift error of the inertial sensor using the virtual observation constraints. Specifically, error suppression module 23 is responsible for constructing virtual observation constraints to suppress the integral drift error of the inertial sensor in the short-distance dead reckoning mode. This module includes two parallel sub-modules: a nonholonomic constraint (NHC) virtual observation sub-module and a quasi-stationary zero-velocity update (Quasi-ZUPT) sub-module. The nonholonomic constraint virtual observation sub-module constrains the lateral and vertical velocities in the composite robot's body coordinate system to zero, constructs the NHC observation equations, and dynamically adjusts the observation noise covariance based on the longitudinal velocity and yaw rate. The quasi-stationary zero-velocity update submodule monitors the motion state of the composite robot in real time. When it detects that the acceleration sliding window variance, angular velocity sliding window variance, duration, and longitudinal velocity simultaneously meet the preset quasi-stationary conditions, it triggers the zero-velocity update mechanism, constructs the zero-velocity observation equation, and performs a forced contraction operation on the velocity error covariance matrix. Furthermore, the error suppression module dynamically adjusts the process noise covariance matrix based on the confidence weights and acceleration change rate of the inertial sensor. This module achieves dual adaptation between "environment and dynamics." It outputs the constructed virtual observation constraints to the positioning filter, effectively truncating the cumulative position integration error over short distances.

[0037] The closed-loop correction module 24 is used to acquire the absolute observation pose recovered by the main navigation sensor when the degradation index falls below the degradation threshold, and to perform closed-loop correction on the pose calculated by the short-distance dead reckoning based on a progressive correction strategy, so as to achieve continuous localization of the composite robot. The closed-loop correction module 24 is responsible for achieving lossless recovery of the global pose and providing a safety fallback after the composite robot leaves the degradation zone. This module includes three sub-functions: anchor point memory, safety boundary monitoring, and smooth closed-loop correction. The anchor point memory function is triggered when the degradation index first reaches the degradation threshold, locking the absolute pose calculated by the main navigation sensor at the current moment. and the corresponding covariance matrix As a pose anchor point, the safety boundary monitoring function calculates the relative displacement of the current estimated pose with respect to the pose anchor point in real time during short-range dead reckoning mode. When the relative displacement exceeds the preset safety distance threshold When the degradation index falls below the degradation threshold, a calculation failure warning is triggered, and the composite robot is controlled to slow down or stop. The smooth closed-loop correction function is triggered when the degradation index falls below the degradation threshold. It calculates the pose residual between the current calculated pose and the observed pose rematched by the main navigation sensor, and applies it based on an exponential decay function over multiple preset control cycles. The module calculates dynamic correction weights and progressively injects the pose residuals into the state estimate to achieve smooth correction. After correction, the module resets the state covariance matrix of the error state Kalman filter to a baseline value that is less than the covariance corresponding to the pose anchor point. This provides excellent prior initial values ​​for the next normal navigation phase.

[0038] In a specific hardware implementation example, the above system can be deployed in the edge computing controller of a composite robot. For example, an ARM Cortex-A series processor is used as the main control chip, equipped with a MEMS-IMU (three-axis accelerometer + three-axis gyroscope) with a sampling rate of 200Hz, a 10Hz LiDAR, and a 10-30Hz 3D vision module. The degradation index calculation module, confidence dynamic adjustment module, error suppression module, and closed-loop correction module are stored in the controller's memory as software programs and executed by the processor. The modules exchange data through shared memory or message queues. The positioning filter is implemented using Error State Kalman Filter (ESKF), and its operating frequency is synchronized with the IMU sampling rate (200Hz). The positioning calculation latency of the entire system is controlled within 20ms, meeting the real-time navigation requirements of the composite robot. Through this modular and systematic implementation, this invention not only protects the specific algorithm flow but also covers the hardware device and the entire system carrying the algorithm, constructing a complete intellectual property protection system.

[0039] This embodiment provides a non-transitory computer-readable storage medium that stores computer instructions that cause a computer to execute the methods provided in the above-described embodiments.

[0040] This embodiment provides a computer program product, including a computer program that, when executed by a processor, implements the methods provided in the above-described method embodiments.

[0041] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can occur depending on design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A short-range inertial-assisted positioning method for a composite robot, characterized in that, include: Based on real-time multi-source sensor data from the composite robot, the degradation index of the current environment is calculated. When the degradation index reaches a preset degradation threshold, the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter are continuously adjusted based on the degradation index to perform short-range dead reckoning. In the short-distance dead reckoning mode, virtual observation constraints are constructed based on the kinematic physical constraints and / or quasi-stationary state of the composite robot, and the virtual observation constraints are used to suppress the integral drift error of the inertial sensor. When the degradation index falls below the degradation threshold, the absolute observation pose recovered by the main navigation sensor is obtained, and the pose calculated by the short-distance dead reckoning is corrected in a closed loop based on a progressive correction strategy to achieve continuous positioning of the composite robot.

2. The positioning method according to claim 1, characterized in that, The calculation of the current environmental degradation index based on real-time multi-source sensor data from the composite robot includes: Extract a multimodal environmental degradation feature vector, which includes a first feature characterizing the degradation of environmental geometric features and a second feature characterizing the degradation of environmental dynamic disturbances; The degradation index is obtained by weighting and fusing the multimodal environment degradation feature vectors based on preset weights. The step of continuously adjusting the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter based on the degradation index includes: The degradation index is input into a continuously differentiable transition function to dynamically calculate the confidence weight of the inertial sensor and the complementary weight of the main navigation sensor. The observation noise covariance matrix of the main navigation sensor is dynamically scaled based on the confidence weights of the inertial sensor.

3. The positioning method according to claim 2, characterized in that, The first feature includes local point cloud planarity variance and feature matching inlier rate; the second feature includes inter-frame optical flow variance and dynamic obstacle pixel ratio. The degradation index is calculated by weighting and summing the first feature and the second feature according to a preset normalized weighting coefficient; wherein, the normalized complementary value of the flatness variance phase, the complementary value of the feature matching inlier rate, the proportion of dynamic obstacle pixels, and the normalized value of the optical flow variance are respectively used as the terms of the weighted summation. The smaller the flatness variance, the lower the feature matching inlier rate, the higher the proportion of dynamic obstacle pixels, and the larger the optical flow variance, the larger the degradation index. The continuously differentiable transition function is the Sigmoid function. The greater the difference between the degradation index and the preset degradation threshold, the greater the confidence weight of the inertial sensor, and the change of the confidence weight is continuously differentiable. The observation noise covariance matrix of the main navigation sensor is dynamically scaled based on the basic observation noise covariance matrix and the confidence weight of the inertial sensor. The larger the confidence weight, the larger the scaling factor of the observation noise covariance matrix.

4. The positioning method according to claim 1, characterized in that, The virtual observation constraints, constructed based on the kinematic physical constraints and / or quasi-stationary state of the composite robot, include: The lateral and vertical velocities of the composite robot in the body coordinate system are constrained to zero, and a nonholonomic constrained virtual observation equation is constructed. Based on the longitudinal velocity and yaw rate of the composite robot, the observation noise covariance of the nonholonomic constraint virtual observation equation is dynamically adjusted, wherein the observation noise covariance is positively correlated with the longitudinal velocity and the yaw rate. In response to the detection that the composite robot meets the preset quasi-stationary conditions, a zero-velocity update mechanism is triggered to construct a zero-velocity virtual observation equation and perform a forced contraction operation on the velocity error covariance matrix.

5. The positioning method according to claim 4, characterized in that, The observation noise covariance of the nonholonomic constrained virtual observation equation is dynamically adjusted based on the standard deviation of the base noise and the absolute values ​​of the longitudinal velocity and yaw rate in the body coordinate system. The observation noise covariance is positively correlated with the absolute values ​​of the longitudinal velocity and yaw rate. The preset quasi-stationary conditions include: the variance of the acceleration sliding window is less than a first threshold and the variance of the angular velocity sliding window is less than a second threshold; the duration of satisfying the variance conditions is greater than or equal to a preset stabilization time; and the absolute value of the longitudinal velocity is less than an extremely low speed threshold. The forced shrinkage operation on the velocity error covariance matrix includes: multiplying the velocity error covariance matrix by a preset shrinkage factor, wherein the shrinkage factor is a constant greater than 0 and less than 1; In the short-range dead reckoning mode, the process noise covariance matrix is ​​dynamically adjusted based on the basic process noise covariance matrix, the confidence weight of the inertial sensor, and the rate of change of acceleration between the current time and the previous time. The larger the confidence weight and the rate of change of acceleration, the larger the scaling factor of the process noise covariance matrix.

6. The positioning method according to claim 1, characterized in that, The step of acquiring the absolute observation pose recovered by the main navigation sensor and performing closed-loop correction on the pose calculated from the short-range dead reckoning based on a progressive correction strategy includes: In response to the degradation index reaching the preset degradation threshold for the first time, the absolute pose calculated by the main navigation sensor at the current moment is locked as the pose anchor point. In response to the degradation index falling below the preset degradation threshold, the pose residual between the current estimated pose and the observed pose rematched by the main navigation sensor is calculated; Within a preset number of control cycles, the dynamic correction weight for each control cycle is calculated based on the exponential decay function. Based on the dynamic correction weights, the pose residuals are progressively injected into the state estimation to smoothly correct the current estimated pose to the observed pose.

7. The positioning method according to claim 6, characterized in that, In the short-distance dead reckoning mode, the relative displacement of the current reckoned pose with respect to the pose anchor point is monitored in real time; In response to the relative displacement exceeding a preset safe distance threshold, a calculated failure warning is triggered, and the composite robot is controlled to slow down or stop. The calculation of the dynamic correction weight for each control cycle based on the exponential decay function includes: the dynamic correction weight decreases exponentially with the increase of the control cycle number, and the initial correction strength coefficient and the smoothing time constant jointly determine the deceleration rate; The step of progressively injecting the pose residuals into the state estimation includes: based on the dynamic correction weights, superimposing the abscissa residuals, ordinate residuals, and heading angle residuals in the pose residuals onto the corresponding uncorrected inertial estimation values ​​according to their respective weights to obtain the corrected abscissa, ordinate, and heading angle; in response to the completion of progressive correction, resetting the state covariance matrix of the error state Kalman filter to a reference value that is less than the covariance corresponding to the pose anchor point.

8. The positioning method according to claim 1, characterized in that, The real-time multi-source sensor data based on the composite robot includes: Using the high-frequency data from the inertial sensor as a time reference, the interpolation time window is dynamically adjusted based on the acceleration and angular velocity of the composite robot; Within the interpolation time window, cubic spline interpolation is used to map the low-frequency observation data of the main navigation sensor to a unified reference timestamp to eliminate motion distortion and phase difference; The adjustment amount of the interpolation time window is negatively correlated with the combined value of the linear acceleration amplitude and the angular velocity amplitude of the composite robot. The larger the linear acceleration amplitude and the angular velocity amplitude, the smaller the adjustment amount of the interpolation time window.

9. A short-range inertial-assisted positioning system for a composite robot, characterized in that, include: The degradation index calculation module is used to calculate the degradation index of the current environment based on real-time multi-source sensor data from the composite robot. The confidence dynamic adjustment module is used to continuously adjust the confidence weights of the main navigation sensor and the inertial sensor in the positioning filter based on the degradation index when the degradation index reaches a preset degradation threshold, so as to perform short-distance dead reckoning. The error suppression module is used to construct virtual observation constraints based on the kinematic physical constraints and / or quasi-stationary state of the composite robot in the short-distance dead reckoning mode, and to use the virtual observation constraints to suppress the integral drift error of the inertial sensor. The closed-loop correction module is used to obtain the absolute observation pose recovered by the main navigation sensor when the degradation index falls below the degradation threshold, and to perform closed-loop correction on the pose calculated by the short-distance dead reckoning based on a progressive correction strategy, so as to realize the continuous positioning of the composite robot.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the method of any one of claims 1-8.

Images