A hierarchical adaptive state estimation and parameter optimization method for humanoid robots

Abstract

The application belongs to the technical field of state estimation, and discloses a layered adaptive state estimation and parameter optimization method for a humanoid robot, which comprises off-line closed-loop optimization of a noise covariance matrix based on sub-axis weighted error; multi-modal foot end contact detection with time domain blind area and dynamic baseline latching; adaptive state estimation based on layered residual monitoring, and output of a state estimation result. Through a double cooperative mechanism of online adaptation and off-line parameter optimization, the application realizes high-precision and high-robustness estimation of the state of the humanoid robot under a complex unstructured terrain, and lays a solid foundation for high-dynamic stable walking and whole-body cooperative control of the robot.

Description

Technical Field

[0001] This invention belongs to the field of state estimation technology, specifically relating to a hierarchical adaptive state estimation and parameter optimization method for humanoid robots. Background Technology

[0002] Humanoid robots, as ideal carriers of general intelligence, rely heavily on state feedback provided by underlying state estimators for their highly dynamic and stable walking and whole-body coordinated control in unstructured environments. However, the inherent characteristics of humanoid robots, such as their large inertia floating bases and high degrees of freedom, present significant challenges. At the level of adaptive algorithms for state estimation, current research largely focuses on online covariance adjustment based on pure mathematical statistics (such as adaptive Kalman filtering), computationally intensive nonlinear filtering theories (such as contact-assisted invariant extended Kalman filtering based on Lie groups), or deep learning methods that rely on massive amounts of training data. These methods often struggle to balance the limitations of underlying computing power with robustness under unstructured perturbations. Therefore, a systematic solution remains lacking for providing accurate, high-frequency, and low-latency state feedback from large inertia floating bases in complex terrain.

[0003] Specifically, in practical engineering applications, the following three major technical bottlenecks still exist:

[0004] First, the foot contact state perception mechanism is rigid and prone to misjudgment due to large inertia and dynamic coupling. Accurate contact state is a prerequisite for state estimation and observation updates. Traditional humanoid robots rely on six-dimensional force sensors installed on the soles of the feet, which not only significantly increases hardware costs and leg end-effector inertia but is also easily damaged in harsh environments. Existing sensorless (pure algorithm) contact detection schemes typically use absolute threshold comparison methods (such as setting fixed force or torque thresholds). However, humanoid robot legs have large inertia, which can generate strong dynamic coupling pseudo-forces when rapidly starting and swinging or encountering terrain undulations. Due to the inaccuracy of the dynamic model, virtual forces often drift to zero. Simple fixed threshold comparison cannot distinguish between "real foot contact collision" and "high-frequency internal forces generated by active leg swinging," leading to a high risk of false contact misjudgment. Once such misjudgment is introduced into the state estimator, it will directly undermine the foundation of kinematic observation, causing the system to collapse instantly.

[0005] Second, existing filtering algorithms are prone to divergence and consume significant computational resources under unstructured disturbances. Robots face two fundamentally different types of disturbances: one is the "single-frame discrete transient abrupt change" at the moment of slippage or landing, and the other is the "cross-frame continuous steady-state noise" caused by changes in road surface roughness. On the one hand, traditional Kalman filtering uses a fixed noise covariance matrix, which cannot cope with erroneous observations during slippage; classic online adaptive filtering algorithms are prone to non-positive definite covariance matrices due to drastic changes in residuals when facing high dynamic shocks, leading to instantaneous system divergence; while methods based on Lie group theory are theoretically rigorous, the complex mathematical matrix calculation process results in extremely high computational consumption, making it difficult to deploy in high-frequency real-time on low-cost platforms. In addition, deep learning-based methods, which have emerged in recent years, can predict noise parameters end-to-end, but they heavily rely on massive amounts of training data, have poor generalization ability across terrain and machine models, and as "black box" models, lack physical interpretability, making it difficult to ensure the absolute safety of the system in the underlying control loop. On the other hand, existing adaptive mechanisms generally rely on globally unified parameter tuning. This dimensionality reduction approach has a fatal flaw when faced with highly directional physical disturbances: for example, when the robot experiences local slippage only in the forward direction (X-axis), the overall residual scalar increases sharply, and existing systems blindly amplify the observation noise across all three-dimensional coordinate axes simultaneously. This causes the previously completely undisturbed high-precision kinematic observations in the vertical direction (Z-axis) to be shielded, leading to extremely severe and unnecessary height estimation drift and attitude instability.

[0006] Third, the core parameters of the state estimator rely on human experience and lack a data-driven calibration mechanism. The process and observation noise matrices in the Kalman filter directly determine the estimation performance of the system. Traditional parameter calibration relies heavily on trial and error based on human experience, which is time-consuming and difficult to generalize. Existing automated optimization methods mostly use general black-box optimization algorithms and generally treat the parameters in three-dimensional space uniformly, using only the overall root mean square error (RMSE) as a single evaluation metric. This conventional approach ignores the physical uniqueness of humanoid robot gait dynamics: its vertical direction (Z-axis) is subjected to extremely high-frequency ground impacts, while the horizontal direction (XY-axis) needs to maintain extremely high sensitive tracking bandwidth. Without strict axis decoupling, the severe impacts on the Z-axis can easily pollute the horizontal estimation gain. More critically, the general optimization objective lacks constraints on high-frequency jitter, causing the optimized parameters, even if the macroscopic RMSE is small, to output velocity signals carrying microscopic high-frequency oscillations to the control layer. Even small velocity jitters can immediately cause the joint driving torque to diverge. Summary of the Invention

[0007] The purpose of this invention is to provide a hierarchical adaptive state estimation and parameter optimization method for humanoid robots. This method aims to solve the technical problems in the prior art, such as the inertial coupling misjudgment caused by foot contact perception when the leg swings rapidly with a large inertia, the inability of single-layer adaptive filtering algorithms to simultaneously cope with the filtering divergence caused by transient impact and steady-state noise drift, and the high dependence of noise covariance matrix parameters on human experience and the lack of a systematic calibration mechanism for the anisotropic characteristics of humanoid robot gait. This invention aims to achieve accurate, high-frequency, and low-latency state feedback in complex terrain.

[0008] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0009] A hierarchical adaptive state estimation and parameter optimization method for a humanoid robot includes the following steps:

[0010] Offline parameter optimization: The process noise matrix and observation noise matrix in the linear Kalman filter framework are decoupled in three dimensions and used as the set of hyperparameters to be optimized. A split-axis asymmetric objective function is constructed, and the set of hyperparameters that minimize the split-axis asymmetric objective function is obtained by using the solver, which serves as the benchmark process noise matrix and benchmark observation noise matrix.

[0011] Foot contact determination: The foot contact state of the humanoid robot is determined based on a multimodal foot contact detection method with temporal blind zone and dynamic baseline latching.

[0012] Hierarchical adaptive state estimation: Under the linear Kalman filter framework, for each leg currently in the supported state, the three-dimensional consistency residual vector between the filter posterior estimated velocity and the kinematic observation velocity is independently calculated. If the absolute value of the three-dimensional consistency residual vector of the leg in any axis is greater than the three-dimensional transient slippage velocity threshold vector, the first layer of assignment is entered, and a fuse scaling factor is assigned to the corresponding axis of the leg; otherwise, the second layer of assignment is entered, the variance ratio is calculated based on the three-dimensional consistency residual vector, and a steady-state scaling factor is assigned to each axis of the leg based on the variance ratio. The independent scaling factors of each leg in the supported state are used to perform weighted updates on the sub-blocks corresponding to the legs in the reference observation noise matrix, and combined with the reference process noise matrix, the state estimation of the humanoid robot is completed, and the state estimation result is output.

[0013] Several alternative methods are provided below, but they are not intended as additional limitations on the overall solution above. They are merely further additions or optimizations. Provided there are no technical or logical contradictions, each alternative method can be combined individually with respect to the overall solution above, or multiple alternative methods can be combined with each other.

[0014] Preferably, the three-dimensional decoupling of the process noise matrix and the observation noise matrix in the linear Kalman filter framework includes:

[0015] The process noise matrix is ​​decoupled into three independent dimensions: X-axis, Y-axis, and Z-axis. The observation noise matrix is ​​decoupled into independent planar and height terms.

[0016] Preferably, the biaxially asymmetric objective function includes:

[0017] The root mean square error of the X-axis estimated velocity, the root mean square error of the Y-axis estimated velocity, the root mean square error of the Z-axis estimated velocity, and the jitter penalty term are weighted and summed as the asymmetric objective function for each axis. The jitter penalty term is the standard deviation of the first difference of the Z-axis estimated velocity.

[0018] Preferably, the set of hyperparameters obtained by using the solver to minimize the biaxially asymmetric objective function includes:

[0019] The actual recorded sensor data and the corresponding true values ​​of the base motion are used as the input data stream;

[0020] A Bayesian optimization solver is used as the driving engine to dynamically modify the hyperparameter set and perform multiple closed-loop replays. In each closed-loop replay, the state estimator is configured with the current hyperparameter set, the input data stream is replayed at double speed, and the objective function value of the whole process is calculated based on the split-axis asymmetric objective function.

[0021] After iterative convergence, the set of hyperparameters that minimizes the objective function is extracted.

[0022] Preferably, the multimodal foot contact detection method with temporal blind zone and dynamic baseline latching is implemented with the following steps;

[0023] When the gait phase crosses the blind zone phase, the estimated value of the external contact force of the swing leg in the air at the current moment is sampled as the dynamic baseline of the contact force;

[0024] The raw knee joint torque signal at the current moment is filtered to obtain a smooth torque value;

[0025] When any one of the three detection channels is satisfied, the foot contact state is determined to be a supporting state; otherwise, it is a swinging state. The three detection channels are as follows:

[0026] First detection channel: The current gait phase is greater than or equal to the blind zone phase, and the difference between the estimated external contact force of the foot and the dynamic baseline of the contact force is greater than the abrupt change increment threshold, and the estimated external contact force of the foot is greater than the lower limit of the absolute value of the contact force.

[0027] Second detection channel: The current gait phase is greater than or equal to the blind zone phase, and the smoothing torque value falls within the torque safety range;

[0028] Third detection channel: The gait phase at the current moment is greater than or equal to the phase catch-up threshold.

[0029] Preferably, if the absolute value of the three-dimensional consistency residual vector of the leg in any axis is greater than the three-dimensional transient slippage velocity threshold vector, then the first layer of assignment is entered, and a fuse scaling factor is assigned to the corresponding axis of the leg, including:

[0030] The three-dimensional consistency residual vector includes the velocity consistency deviation of the leg on the X, Y, and Z axes in the body coordinate system; the three-dimensional transient slippage velocity threshold includes the transient slippage velocity threshold on the X, Y, and Z axes in the body coordinate system.

[0031] Independently determine whether the absolute value of the speed consistency deviation of each axis is greater than the transient slippage speed threshold of the corresponding axis. If it is greater, assign a circuit breaker scaling factor to the scaling factor of the axis; otherwise, assign an initial reference value to the scaling factor of the axis, wherein the circuit breaker scaling factor is greater than the initial reference value.

[0032] Preferably, the step of calculating the variance ratio based on the three-dimensional consistency residual vector includes:

[0033] Store the squared vector of the current three-dimensional consistency residual vector into a vector of length [length missing]. The residual sliding window is used to record the square vector of the three-dimensional consistent residual vector of the support leg at historical moments within the current support cycle; when the leg switches from the support state to the swing state, the corresponding residual sliding window is cleared.

[0034] Calculate the actual residual variance of each axis within the residual sliding window based on the speed consistency deviation of each axis;

[0035] The ratio of the actual residual variance of each axis to the pre-calibrated theoretical expected variance of each axis is used as the variance ratio of each axis.

[0036] Preferably, the step of assigning steady-state scaling factors to each axis of the leg based on the variance ratio includes:

[0037] A limiting operation is performed on the variance ratio, and the variance ratio after the limiting operation is used as the steady-state scaling factor. The steady-state scaling factor is then assigned to the scaling factor corresponding to the axis.

[0038] Preferably, the step of using the independent scaling factors of each leg in the supported state to perform a weighted update of the sub-blocks corresponding to the legs in the reference observation noise matrix includes:

[0039] The three diagonal sub-blocks corresponding to the kinematic observation velocities of the left and right legs are extracted from the baseline observation noise matrix. The diagonal sub-blocks of the current supporting leg are then updated element-wise using the scaling factor corresponding to the leg, resulting in the updated observation noise matrix.

[0040] The hierarchical adaptive state estimation and parameter optimization method for humanoid robots provided by this invention has the following advantages compared with the prior art:

[0041] This invention constructs a lightweight state estimation architecture that couples online real-time disturbance rejection with offline closed-loop optimization through sensorless multimodal fusion technology.

[0042] For the online layer, this invention introduces a temporal blind zone shielding and dynamic baseline latching mechanism, upgrading the foot contact judgment from a fixed threshold comparison to dynamic abrupt change detection relative to the aerial background. This fundamentally solves the problem of inertial coupling misjudgment during rapid leg swings with large inertia. Combined with knee joint torque exponential moving average filtering smoothing and bilateral safety interval constraints, as well as gait phase limit fallback channels, it achieves high-confidence multimodal contact perception without the need for a six-dimensional force sensor, providing a highly confident observation premise for subsequent filtering. Subsequently, within the Kalman filter framework, a three-axis decoupled hierarchical mutually exclusive adaptive state machine with transient slippage hard-fusing and steady-state variance matching is constructed. This machine can simultaneously handle two fundamentally different types of disturbances: single-frame discrete impact events and cross-frame continuous noise drift. It can also accurately identify the direction of disturbance through the partial axis residual vector. When encountering physical disturbances (such as slippage or impact), it performs instantaneous physical fusing or variance amplification on the disturbed axis, while retaining high-precision kinematic observation information in other directions. This online cascaded architecture, featuring high-confidence perception and targeted lightweight disturbance rejection, enhances the robustness and accuracy of the state estimator when faced with unstructured disturbances.

[0043] For the offline layer, this invention addresses the unique characteristics of humanoid robot gait dynamics by constructing a decoupled three-dimensional heterogeneous parameter search space and introducing an asymmetric objective function that includes a high-frequency jitter penalty along the Z-axis. This overcomes the bottleneck of general optimization algorithms that blindly tune parameters and cannot simultaneously ensure motion stability. Ultimately, the optimal initial physical benchmark established through offline optimization is combined with online two-layer adaptive dynamic observation and adjustment to achieve a closed-loop process from parameter initialization to online disturbance rejection. This enables the humanoid robot to provide high-precision, high-frequency, low-latency, and highly smooth state feedback for whole-body coordinated control even in extremely complex unstructured terrain. Attached Figure Description

[0044] Figure 1 The overall flowchart of the hierarchical adaptive state estimation and parameter optimization method for humanoid robots provided in the embodiments of the present invention is shown below.

[0045] Figure 2 This is a framework diagram of offline closed-loop optimization of the noise covariance matrix based on the axis-weighted error in an embodiment of the present invention;

[0046] Figure 3 This is a logic block diagram of multimodal foot contact detection with temporal blind zone and dynamic baseline latching in an embodiment of the present invention;

[0047] Figure 4 This is a control block diagram of a two-layer mutually exclusive adaptive state estimation based on triaxial decoupling and hierarchical residual monitoring in an embodiment of the present invention;

[0048] Figure 5 This is a comparison chart of positional drift in ablation simulation experiments conducted under unstructured terrain, according to an embodiment of the present invention. Detailed Implementation

[0049] 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 some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0050] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to limit the invention.

[0051] like Figure 1 As shown, this invention discloses a hierarchical adaptive state estimation and parameter optimization method for humanoid robots. The method mainly includes the following three cascaded steps: offline closed-loop optimization of the noise covariance matrix based on the axis-weighted error; multimodal foot contact detection with temporal blind zone and dynamic baseline latching; and adaptive state estimation based on hierarchical residual monitoring.

[0052] Step 1: Offline closed-loop optimization based on the noise covariance matrix of the axis-weighted error.

[0053] This embodiment automatically determines the required reference observation noise matrix in the linear Kalman filter framework of step three through offline closed-loop optimization. and reference process noise matrix A closed-loop offline evaluation method based on real sensor data, using a "record-playback" approach, employs a tree-structured Parsons estimator as the search engine. Taking into account the unique characteristics of humanoid robot gait dynamics, a customized physical parameter search space and asymmetric cost function are constructed to guide the underlying optimizer in searching for the physical benchmark that best matches the robot's motion characteristics, outputting the globally optimal combination of initial parameters. Figure 2 As shown, the specific steps include:

[0054] (1) Multidimensional heterogeneous parameterization configuration of noise covariance matrix.

[0055] During the walking process, the kinematic noise characteristics of humanoid robots vary in multiple dimensions depending on the direction and part of the robot, specifically in the following three aspects:

[0056] Firstly, axial differences: The vertical direction (Z-axis) generates severe transient noise due to the impact of each step, and its amplitude and frequency characteristics are completely different from those in the horizontal plane (XY-axis), requiring independent tuning;

[0057] Secondly, the difference between forward and backward and lateral directions: humanoid robots mainly move in the forward direction (X-axis), and the control system is significantly more dependent on the speed accuracy of this axis than in the lateral direction (Y-axis). The process noise tuning targets for the two axes are different.

[0058] Third, differences between left and right feet: Due to manufacturing tolerances of the robot structure, asymmetry of the gait phase of the left and right legs, and local differences in terrain contact conditions, there may be systematic deviations in the observed noise levels of the left and right feet, and uniform configuration cannot eliminate this deviation.

[0059] Based on this physical prior, this embodiment optimizes the process noise matrix in the linear Kalman filter framework from the set of hyperparameters to be optimized. With observation noise matrix Axial decoupling and heterogeneous configuration were implemented. For example, the process noise parameter of the base speed was strictly decoupled into mutually independent X-axis parameters. Y-axis Z-axis Three independent search dimensions; simultaneously separating observation noise into planar and height terms. This specific artificially decoupled topology enables the optimization process to simultaneously eliminate systematic noise biases at three levels: axial, forward / backward, and left / right symmetry.

[0060] (2) Introduce a penalty objective function for steady-state interception and asymmetric weights.

[0061] First, a steady-state truncation mechanism is implemented: non-steady-state transient data is removed during the initial start-up phase to ensure that the objective function only evaluates the algorithm's true disturbance rejection capability during steady-state walking. During the initial start-up phase (e.g., the first 1.5 seconds), the covariance matrix of the state estimator is in a transient transition period from its initial value to a steady state; simultaneously, the robot's physical state is also transitioning from a stationary standing position to dynamic walking. Data during this period exhibits strong non-stationary characteristics, and including it in the optimization calculation would bias the cost function. Therefore, initial non-steady-state transient data is forcibly removed to ensure that the objective function only evaluates the algorithm's true disturbance rejection capability during steady-state walking.

[0062] Subsequently, a biaxially asymmetric objective function is constructed. The formula is as follows:

[0063]

[0064] in, The set of hyperparameters to be optimized (including the process noise matrix) Independent diagonal elements along the X, Y, and Z axes and the observation noise matrix (Shared terms on the horizontal plane XY axes and independent terms on the vertical height Z axis). These are the weighting coefficients for the X, Y, and Z axes, respectively, and the weighting coefficients for each axis... This reflects the physical fact that the humanoid robot control system has the highest requirement for speed accuracy in the forward direction (X-axis), followed by the lateral direction (Y-axis). These are the root mean square errors of the posterior estimated velocity (or simply estimated velocity) for each axis filter. Weighting for jitter penalties The standard deviation of the first-order difference of velocity along the Z-axis is estimated to characterize vertical jitter.

[0065]

[0066] In the formula, This represents the total number of frames after steady-state truncation of the offline optimization data packets. This is the frame index for the offline data sequence; For offline playback Estimated velocity along the Z-axis of the frame. For offline playback Estimated velocity along the Z-axis of the frame.

[0067] Introduction The physical basis for the penalty term is that the high-frequency jitter generated by the ground impact in the Z-axis velocity estimation, even if it does not affect the mean RMSE, will still cause high-frequency oscillations of joint torque through the control loop, impairing motion stability; simply targeting RMSE cannot suppress this type of jitter, and Directly quantize the inter-frame estimated rate of change of velocity, and specifically punish high-frequency noise caused by impact.

[0068] (3) Closed-loop iteration and benchmark establishment.

[0069] After establishing the aforementioned parameter topology and evaluation criteria, the system uses the actually recorded sensor data (including pre-calibrated Inertial Measurement Unit (IMU) data and joint states) and the corresponding true values ​​of base motion as the input data stream. A Bayesian optimization solver (in this embodiment, the Tree-Structured Parzen Estimator (TPE) algorithm) is used as the driving engine to perform multiple closed-loop replays and cost evaluations within the aforementioned heterogeneous parameter space. After iteration until the split-axis asymmetric objective function converges, the globally optimal set of hyperparameters that minimizes this customized objective function is extracted and used as the physical initial benchmark for the online hierarchical adaptive algorithm in step three.

[0070] Typically, a humanoid robot only needs to be optimized once. If the structure of the humanoid robot changes or parts are replaced, the optimization operation can be repeated. New sample data is recorded in each optimization, and the robot simulator automatically performs dozens to hundreds of speed playbacks and parameter iterations (100 times in this embodiment) until the split-axis asymmetric objective function converges and the optimal hyperparameter set is found.

[0071] Step 2: Multimodal foot contact detection with temporal blind zone and dynamic baseline latching.

[0072] This embodiment addresses the high-frequency motion characteristics of large-inertia floating bases in complex terrain by employing a sensorless ontological perception strategy. It generates high-confidence contact state flags by constructing multimodal fusion logic with state memory. For example... Figure 3 As shown, the specific steps include:

[0073] (1) Virtual plantar force estimation based on inverse dynamic solution.

[0074] Establish a rigid body dynamics model of the humanoid robot and obtain the positions of each joint. ,speed and driving torque Based on the dynamic equations of the floating base, the estimated value of the external contact force at the foot is calculated by inverse kinematics. The calculation formula is as follows:

[0075]

[0076] In the formula, For the foot Jacobian matrix, It is the time derivative of the Jacobian matrix. For the whole body inertial matrix, Jacobian matrix at the foot transpose, These are nonlinear force terms (Coriolis force, centrifugal force, and gravity). The matrix represents the Coriolis force and the centrifugal force. For velocity-dependent nonlinear forces, This is the gravity term.

[0077] (2) Signal fusion decision with time-domain blind zone and dynamic baseline latching.

[0078] The humanoid robot's high-inertia legs during the initial leg swing phase (phase) When the value is relatively small, rapid acceleration will generate significant inertial coupling force, leading to changes in the inverse dynamic solution value. False peaks appeared. If we directly address this stage... Applying a fixed threshold for comparison is prone to false ground contact misjudgments, and the torque signal contains high-frequency glitches during rapid oscillations, making direct use for threshold judgment susceptible to false triggering. Therefore, this embodiment constructs a three-channel independent decision mechanism based on time-domain blind zone shielding, dynamic baseline latching, and torque exponential moving average filtering. The specific logic is as follows:

[0079] First, set the phase threshold for the temporal physical blind zone. If the current gait phase When the system determines it is in a highly dynamic, unobservable blind zone, it forcibly blocks the force and torque detection channels, thus physically immunizing against inertial coupling misjudgments during the initial stage. Within the blind zone, the system only blocks the multimodal ground contact detection signal, meaning the states of the supporting leg and swing leg do not switch. Subsequent gait state updates are not performed; the ultimate result of multimodal contact detection is updating the gait state, i.e., whether the current supporting leg and swing leg have changed. However, the state estimator itself continues to use the current supporting leg information and IMU data for observation and updates in each control cycle. In other words, in this embodiment, blind zone blocking only applies to the gait switching logic of the state machine; the underlying Kalman filter state estimation module maintains high-frequency continuous operation throughout its entire lifecycle.

[0080] Secondly, at the instant the gait phase crosses the blind zone phase, the system triggers a zero-delay latching mechanism, automatically sampling and maintaining the actual virtual force of the swing leg in the air at the current moment (estimated external contact force at the foot tip), using it as the dynamic baseline of the contact force. :

[0081]

[0082] The baseline The virtual force background value of the swing leg in the air was recorded, reflecting the systematic bias of the dynamic model within the current gait cycle. The ground contact determination of subsequent channel A (i.e., the first detection channel) uses this baseline as a reference to detect the relative abrupt change, rather than using a fixed absolute threshold.

[0083] In this embodiment, the specific numerical determination logic for detecting whether the gait phase crosses the blind zone phase is as follows:

[0084] Parameter settings: Set the blind zone phase threshold to 0.35 and initialize the Boolean blind zone lock.

[0085] Hold within blind zone: Lock holds true when the real-time gait phase is less than 0.35 (correct).

[0086] Instantaneous triggering and sampling: When the phase increases with the control cycle and first reaches a real-time gait phase greater than or equal to 0.35 (i.e., the first control frame after crossing the boundary), since the lock is still true, the system immediately performs sampling (assigning the current plantar force to the dynamic baseline) and immediately locks within the same frame, i.e., sets it to false (error).

[0087] Subsequent masking: Locks are false until the end of the current gait cycle.

[0088] Furthermore, regarding the original signal of knee joint torque... In the case of high-frequency spikes during rapid leg swing, this embodiment applies an exponential moving average filter to the original torque to obtain a smoothed torque value. :

[0089]

[0090] In the formula, For the first The smoothed torque value obtained from each control cycle For the first The smoothed torque value obtained from each control cycle For smoothing coefficients, For the first The original signal of knee joint torque for each control cycle This is the index for the current control cycle.

[0091] Subsequently, the multimodal fusion logic decision maker simultaneously evaluates the following three detection channels for each swing leg to determine whether to switch to the supporting leg: Channel A (dynamic plantar force mutation channel), Channel B (i.e., the second detection channel, which is the knee joint torque safety range channel), and Channel C (i.e., the third detection channel, the gait phase bottoming channel). When any channel condition is met, the foot contact state is determined to be the supporting state, and the leg is switched to the supporting leg; otherwise, the swing state is maintained as the swing leg.

[0092] Channel A: Outside the blind spot ( Real-time monitoring of estimated external contact force at the foot Relative to the dynamic baseline of contact force The mutation increment. The foot force triggering condition for channel A is that both of the following conditions must be met simultaneously:

[0093]

[0094] In the formula, The threshold for the abrupt change in relative to the baseline is used to capture the surge in contact force at the moment of impact. This is the lower limit of the absolute value of the contact force to prevent mathematical false triggering when the baseline is negative. For logical AND operation.

[0095] Channel B: Outside the blind spot ( The smoothing torque value is monitored in real time. The knee joint torque trigger condition for channel B is that the smoothing torque value falls within the preset safe range.

[0096]

[0097] In the formula, This is the lower limit of the load-bearing moment, ensuring that the knee joint has withstood sufficient support load; To ensure a safe upper limit for load-bearing torque, abnormally large torques generated near the limits of leg swing inertia or joint movement are excluded, preventing the misjudgment of torque peaks in non-contact states as ground contact.

[0098] Channel C: Channel C is not subject to blind zone shielding and operates as an independent fallback mechanism throughout the entire process. It is triggered when the gait phase reaches a preset fallback threshold.

[0099]

[0100] In the formula, This is a phase fallback threshold. The purpose of this channel is to ensure that when neither channel A nor channel B is triggered due to sensor malfunction, dynamic model error, or extreme terrain, the leg state switch can still be forcibly completed just before the swing phase ends, preventing the system from getting stuck.

[0101] Final output contact flag bit The Boolean logic expression is:

[0102]

[0103] in, For logical OR operation, , , The expressions are as follows:

[0104]

[0105]

[0106]

[0107] Step 3: Two-layer mutually exclusive adaptive state estimation based on triaxial decoupling and hierarchical residual monitoring.

[0108] Based on accurate contact status acquisition, this step designs a three-axis fully decoupled and strictly mutually exclusive two-layer adaptive architecture within a linear Kalman filter framework to cope with disturbances in unstructured environments. This adaptive architecture operates independently only for the leg currently determined to be in a supported state. Figure 4 As shown, the three-axis decoupled mutual exclusion two-layer adaptive architecture in this step specifically includes the following cascading and branching logic:

[0109] (1) Layered triggering criteria: Calculation of three-dimensional consistency residual vector based on the support leg.

[0110] Within each filtering cycle, for each leg currently in a supported state (leg identifier denoted as...) , Indicates the left leg. (Representing the right leg), independently calculate the posterior estimation velocity of the filter in real time. With the kinematic velocity provided by the leg The three-dimensional consistency residual vector between :

[0111]

[0112] In the formula, the three-dimensional consistency residual vector Include These represent the velocity consistency deviations of the support leg along the three physically orthogonal axes X, Y, and Z in the body coordinate system, respectively. Angular velocity, Let be the position vector of the foot.

[0113] This residual comprehensively reflects the degree of inconsistency between the current inertial integral and the specific leg kinematic observations. The left leg residual is solved independently in space. With right leg disability This forms the physical basis for the dual-leg isolation adaptive mechanism and serves as the sole criterion for subsequent bi-layer adaptive mechanisms.

[0114] When the feet slip or the ground becomes soft. Significant jumps will occur within a single frame; this will happen when the road surface remains rough or the terrain type changes. This manifests as an increase in statistical variance across multiple frames. These two characteristics are distinctly different in the time domain, forming the physical basis for layered processing. .

[0115] (2) First level: Split axis transient slippage hard fuse mechanism.

[0116] Considering the different physical sensitivities of each axis to slippage and impact, a three-dimensional transient slippage velocity threshold vector is set. For any axis When the absolute value of the residual on any axis exceeds its specific threshold, i.e. When the system determines that the robot has encountered a single-frame slippage or abrupt impact in that specific direction, which is a single-frame discrete abrupt event, it immediately generates a very large penalty scaling factor only for that axis to trigger a hard circuit breaker.

[0117]

[0118] In the formula, For the axis of the leg Scaling factor, This is the hard circuit breaker penalty coefficient. The value typically ranges from [50.0, 100.0] (preferred value is 60.0). This value is on the order of 1, capable of instantly suppressing the Kalman gain of the corresponding axis to near zero. For axes that do not trigger this condition, their... By assigning an initial reference of 1 and independently assigning values ​​to each axis, directional isolation of interference was achieved.

[0119] The observation noise matrix of the supporting leg at the current moment. The corresponding term in the code is multiplied by a huge penalty coefficient. This approach cuts off unreliable kinematic observations, forcing the filter to rely solely on IMU integration for short-term extrapolation, thus preventing state divergence. It does not modify any historical statistics, does not affect the sliding window state of the second layer, and automatically closes in the next frame. This ensures that transient shocks do not contaminate the steady-state tracking layer, creating complete state isolation between the two layers. If so, the first layer does not intervene and proceeds to the second layer.

[0120] (3) Second level: Variance matching mechanism for the steady-state environment of the split axis.

[0121] This embodiment employs a strict mutual exclusion lock to protect the purity of steady-state statistics: if the current support leg triggers the aforementioned transient hard fuse in any axial direction, i.e. The system will skip the second-level calculation and stop updating the sliding window of that leg to prevent discrete slippage anomalies from contaminating the steady-state noise statistics of the terrain. The second level will only be entered if none of the three axes of that leg trigger transient circuit breakers (i.e., no severe slippage occurs).

[0122] The second layer, based on sliding window statistics, continuously tracks the steady-state level of kinematic observation noise, enabling automatic adaptation to gradually changing environmental characteristics such as road surface roughness. Residual windows are updated only for legs currently in a supported state; when a leg switches from a supported state to a swinging state, the system immediately clears the residual sliding window dedicated to that leg. This state isolation and window reset mechanism isolates invalid kinematic noise during the swinging phase from contaminating the background noise statistics and ensures that the leg can quickly re-adapt to the new local terrain roughness with zero historical baggage upon its next landing.

[0123] The current residual squared vector Store a string of length A residual sliding window is used to store historical residual squared values. The actual residual variance for each axis within the window is calculated separately. :

[0124]

[0125] Furthermore, the variance ratio of the actual variance to the expected variance is calculated independently for each axis. :

[0126]

[0127] In the formula, Indicates legs corresponding axis The actual residual variance Indicates legs The corresponding residual sliding window within the first The central axis of the three-dimensional consistent residual vector Speed ​​consistency deviation, To pre-calibrate the robot's axis on a flat, hard surface The theoretical expected variance vector, Indicates legs corresponding axis The variance ratio. This variance ratio reflects the noise amplification factor of the current operating environment relative to the calibration environment: This indicates that the current terrain is noisier than the calibration environment. This indicates that the current terrain is smoother. After constraining the variance ratio using a limiting function to prevent extreme degradation, this constraint is assigned to the scaling factor of the corresponding axis. To achieve smooth updates:

[0128]

[0129] In the formula, and are the lower and upper limits of the scaling factor, respectively, which are used to prevent the observation matrix from degrading under extreme conditions; the observation noise matrix is adjusted by the scaling factor to enable the filter to automatically adapt to the current road surface roughness. In this embodiment, take , that is, the lower limit of the scaling factor is 1, indicating that the minimum scaling multiple of the observation noise is 1. That is, in the most ideal case, the observation noise is not amplified, and the filter has the highest confidence in the observation. Take , that is, the upper limit of the scaling factor is 15, indicating that the observation noise is amplified at most 15 times. It is a relatively conservative upper limit empirically, which can effectively suppress the interference of abnormal observations on state estimation.

[0130] The observation noise matrix of the Kalman filter in this embodiment simultaneously includes independent parameter blocks for the left and right legs (each occupying 3 diagonal dimensions), while the process noise matrix remains unchanged at the benchmark value obtained through offline optimization during online adaptation. Regarding the object of action of the adaptation mechanism: the system only performs residual calculation and noise adjustment on the leg currently determined to be in the support state. If it is a single support phase, only one leg is adjusted; if it is a double support phase, slip detection and matrix update are performed simultaneously, in parallel, and completely independently on the two supporting legs. The dual-leg physical isolation mechanism ensures that unilateral pothole skidding will never contaminate the good leg on the other side.

[0131] (4) Observation noise matrix Dual-leg isolation, axis-by-axis dynamic update, and Kalman gain solution of

[0132] Through the above double-layer exclusive mechanism, for the leg in the support state, the system independently outputs a dedicated three-dimensional scaling factor vector .

[0133] The observation noise matrix of the Kalman filter includes independent diagonal sub-blocks corresponding to the kinematic observation velocities of the left and right legs respectively. The system extracts the sub-block corresponding to the current supporting leg in and performs element-by-element multiplication update using the obtained corresponding scaling factor vector

[0134]

[0135] In the formula, is the observation noise sub-block after single-leg update, is the X-axis diagonal sub-block in the observation noise matrix of this supporting leg, is the Y-axis diagonal sub-block in the observation noise matrix of this supporting leg, is the Z-axis diagonal sub-block in the observation noise matrix of this supporting leg. Subsequently, the system reorganizes the updated left and right leg sub-blocks into a complete global observation noise matrix

[0136] Finally, the system will globally update the overall matrix. Substituting the Kalman gain into the dynamically updated Kalman gain calculation equation, the posterior update of the system state and covariance is completed through low-level intervention on the observation noise matrix. Through this low-level axis-splitting matrix operation, a stable and smooth high-precision state estimation result of the base is finally output in various unstructured terrains, including pose information, mainly the position and velocity of the base (which cannot be obtained by simply using the system and can only be obtained through state estimation), as well as the filtered and updated linear acceleration, angular velocity, quaternions, Euler angles, and the position and velocity of both feet.

[0137] This completes the hierarchical adaptive state estimation and offline parameter optimization method for humanoid robots in unstructured environments. Through the organic combination of the above steps, this invention achieves a closed-loop process from offline optimal initialization to online hierarchical adaptation, significantly improving the accuracy and robustness of state estimation for humanoid robots in unstructured environments.

[0138] In one embodiment, to demonstrate the effectiveness of the method of the present invention, the following simulation experiment is conducted. The simulation conditions are set as follows:

[0139] The Fourier Intelligence Nexus-01 robot was used as the test and verification platform. The underlying control algorithm and state estimation nodes were implemented using the Robot Operating System 2 (ROS2) distributed framework, and the Multi-Joint Dynamics with Contact (MuJoCo) high-fidelity physics engine was used as the robot's dynamics simulator.

[0140] The goal of the simulation experiment is to overcome the limitations of manual parameter tuning through offline closed-loop optimization, establishing an optimal initial noise baseline for the robot. Based on this optimal baseline, when the Fourier N1 robot faces complex unstructured environments (such as dynamic landslides, abrupt material changes on flat ground, etc.) causing random foot slippage and drastic variations in observation noise, the method of this invention can effectively suppress filter divergence and significantly improve the absolute accuracy of base velocity estimation and system robustness. The specific operation steps are as follows:

[0141] Step 1: Offline parameter optimization.

[0142] First, sensor data packets of the Fourier N1 robot performing standard walking gait were recorded in the MuJoCo simulation environment. These data packets synchronously recorded high-frequency IMU data, joint states, and the absolute truth (Ground Truth) output by the simulator.

[0143] Subsequently, the offline optimization script is launched. The TPE offline optimization closed loop can be achieved by calling the Optuna automatic hyperparameter optimization engine in the Python environment, replaying data packets at double speed, and dynamically modifying the process noise parameters and observation noise parameters in the state estimation node configuration file.

[0144] In each TPE iteration, with the current TPE... Hyperparameter combination for next iteration sampling Configure the state estimator, replay data packets at double speed, and calculate the objective function value for the entire process. This information is then fed back to the TPE model; after multiple iterations, the globally optimal combination of hyperparameters that minimizes the objective function value is output. Extract the baseline process noise matrix. With reference observation noise matrix , serving as the initial baseline value for the online two-layer adaptive mechanism.

[0145] Step 2: Online simulation.

[0146] First, the optimal parameter combination obtained through optimization is loaded into the ROS2 state estimation node.

[0147] Subsequently, the Fourier N1 robot was controlled to enter a complex test scenario, and the base velocity estimation trajectory was recorded in real time for evaluation.

[0148] Finally, four sets of comparative simulation ablation experiments were designed using the controlled variable method:

[0149] Control group 1 (baseline): The basic state estimation algorithm based on the single-modal contact detection method of dynamic foot force is adopted, and a fixed noise covariance matrix is ​​set by human experience.

[0150] Control Group 2 (Method A): Based on Control Group 1, the multimodal contact detection of the present invention (including a three-in-one mechanism of phase, torque, and foot force) is introduced as the input of the Kalman filter algorithm, and a fixed noise covariance matrix set by human experience is used.

[0151] Control Group 3 (Method B): Based on Control Group 2, a hierarchical adaptive filtering mechanism is introduced, but its initial noise parameters are still manually set;

[0152] Experimental group (method C): The complete scheme of the present invention, that is, based on control group 3, further loading the optimal combination of split axis parameters obtained through offline optimization.

[0153] The results of ablation simulation experiments conducted on unstructured terrain are recorded in Table 1.

[0154] Table 1. Results of ablation simulation experiments in unstructured terrain.

[0155]

[0156] Combining Table 1 and Figure 5 It can be seen that under the simulation test of repeatedly walking about 20 meters on complex unstructured terrain, control group 1, relying solely on conventional contact detection and a fixed noise covariance matrix, exhibited significant velocity estimation bias and position divergence. Its X-axis velocity RMSE was 0.0216 m / s, its Z-axis velocity RMSE was 0.0245 m / s, and its three-dimensional combined velocity RMSE reached 0.0447 m / s. Regarding cumulative position drift, control group 1 drifted by 0.394 m, with a drift rate of 2.12%.

[0157] With the gradual introduction of various modules of this invention (such as multimodal contact detection, hierarchical mutual exclusion adaptive filtering, etc.), the state estimation accuracy of control group 2 and control group 3 has been steadily improved. By control group 3, the RMSE of the X-axis and Z-axis velocities of the system has decreased to 0.0200 m / s and 0.0192 m / s, respectively, and the position drift has also been initially reduced to 0.304 m.

[0158] After further introducing the complete scheme of this invention (experimental group), the offline sub-axis parameter optimization framework and the online two-layer mutually exclusive adaptive mechanism produced excellent synergy, achieving the best overall performance in an unstructured environment. Regarding velocity estimation accuracy, the experimental group achieved the lowest RMSE for X-axis, Y-axis, and Z-axis velocities, at 0.0193 m / s, 0.0265 m / s, and 0.0186 m / s respectively, with the overall 3D velocity RMSE reduced to 0.0377 m / s. Compared to the basic control group 1, the experimental group improved the velocity estimation accuracy for the X-axis and Z-axis by approximately 10.6% and 24.1%, respectively.

[0159] More importantly, thanks to the three-axis decoupling-based directional disturbance rejection and high-frequency jitter penalty strategy, the system's position divergence during long-distance macroscopic walking is fundamentally suppressed. Experimental data shows that the cumulative position drift of the experimental group was exceptionally well suppressed to 0.103m, with a drift rate of only 0.56%. Compared to control group 1, its position drift error was significantly reduced by approximately 73.9%, verifying that the present invention can still provide the robot with extremely accurate and robust state feedback when facing strong disturbances such as foot slippage and sudden terrain changes, achieving an optimal balance between high precision, high stability, and high safety.

[0160] In summary, this invention establishes a robust Kalman filter algorithm by integrating multimodal contact detection with an adaptive filtering mechanism based on hierarchical residual monitoring. Simultaneously, based on the TPE closed-loop optimization algorithm, it achieves data-driven automated calibration of initial noise parameters, overcoming the limitations of traditional manual parameter tuning. Through a dual collaborative mechanism of online adaptation and offline parameter optimization, high-precision and robust estimation of the humanoid robot's state is achieved in complex unstructured terrain, laying a solid foundation for the robot's highly dynamic and stable walking and whole-body coordinated control.

[0161] In another embodiment, the present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of a hierarchical adaptive state estimation and parameter optimization method for a humanoid robot.

[0162] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the methods described above. Any references to memory, storage, databases, or other media used in the embodiments provided by this invention can include non-volatile and / or volatile memory.

[0163] In another embodiment, the present invention also provides a computer device, including a processor and a memory storing a plurality of computer instructions, wherein the computer instructions, when executed by the processor, implement the steps of a hierarchical adaptive state estimation and parameter optimization method for a humanoid robot.

[0164] The memory and processor are electrically connected directly or indirectly to enable data transmission or interaction. For example, these components can be electrically connected to each other via one or more communication buses or signal lines. The memory stores a computer program that can run on the processor, and the processor implements the method of the present invention by running the computer program stored in the memory.

[0165] The memory can be, but is not limited to, Random Access Memory (RAM), Read Only Memory (ROM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (EPROM), and Electrically Erasable Programmable Read-Only Memory (EEPROM). The memory stores the program, which the processor executes upon receiving execution instructions.

[0166] A processor can be an integrated circuit chip with data processing capabilities. The processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this invention.

[0167] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this invention.

[0168] The above embodiments merely illustrate several implementation methods of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of the present invention should be determined by the appended claims.

Claims

1. A hierarchical adaptive state estimation and parameter optimization method for a humanoid robot, characterized in that, Includes the following steps: Offline parameter optimization: The process noise matrix and observation noise matrix in the linear Kalman filter framework are decoupled in three dimensions and used as the set of hyperparameters to be optimized. A split-axis asymmetric objective function is constructed, and the set of hyperparameters that minimize the split-axis asymmetric objective function is obtained by using the solver, which serves as the benchmark process noise matrix and benchmark observation noise matrix. Foot contact determination: The foot contact state of the humanoid robot is determined based on a multimodal foot contact detection method with temporal blind zone and dynamic baseline latching. Hierarchical adaptive state estimation: Under the linear Kalman filter framework, for each leg currently in a supported state, the three-dimensional consistency residual vector between the filter's posterior estimated velocity and the kinematic observation velocity is independently calculated. If the absolute value of the leg's three-dimensional consistency residual vector in any axis is greater than the three-dimensional transient slippage velocity threshold vector, then the first layer of assignment is entered, and a circuit breaker scaling factor is assigned to the corresponding axis of the leg; otherwise, the second layer of assignment is entered, the variance ratio is calculated based on the three-dimensional consistency residual vector, and a steady-state scaling factor is assigned to each axis of the leg based on the variance ratio. The sub-blocks corresponding to the legs in the reference observation noise matrix are weighted and updated using the independent scaling factors of each leg in the supported state. Combined with the reference process noise matrix, the state estimation of the humanoid robot is completed, and the state estimation result is output.

2. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, The three-dimensional decoupling of the process noise matrix and the observation noise matrix in the linear Kalman filter framework includes: The process noise matrix is ​​decoupled into three independent dimensions: X-axis, Y-axis, and Z-axis. The observation noise matrix is ​​decoupled into independent planar and height terms.

3. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, The biaxially asymmetric objective function includes: The root mean square error of the X-axis estimated velocity, the root mean square error of the Y-axis estimated velocity, the root mean square error of the Z-axis estimated velocity, and the jitter penalty term are weighted and summed as the asymmetric objective function for each axis. The jitter penalty term is the standard deviation of the first difference of the Z-axis estimated velocity.

4. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, The set of hyperparameters obtained by using the solver to minimize the partial-axis asymmetric objective function includes: The actual recorded sensor data and the corresponding true values ​​of the base motion are used as the input data stream; A Bayesian optimization solver is used as the driving engine to dynamically modify the hyperparameter set and perform multiple closed-loop replays. In each closed-loop replay, the state estimator is configured with the current hyperparameter set, the input data stream is replayed at double speed, and the objective function value of the whole process is calculated based on the split-axis asymmetric objective function. After iterative convergence, the set of hyperparameters that minimizes the objective function is extracted.

5. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, The multimodal foot contact detection method with temporal blind zone and dynamic baseline latching is executed in the following steps; When the gait phase crosses the blind zone phase, the estimated value of the external contact force of the swing leg in the air at the current moment is sampled as the dynamic baseline of the contact force; The raw knee joint torque signal at the current moment is filtered to obtain a smooth torque value; When any one of the three detection channels is satisfied, the foot contact state is determined to be a supporting state; otherwise, it is a swinging state. The three detection channels are as follows: First detection channel: The current gait phase is greater than or equal to the blind zone phase, and the difference between the estimated external contact force of the foot and the dynamic baseline of the contact force is greater than the abrupt change increment threshold, and the estimated external contact force of the foot is greater than the lower limit of the absolute value of the contact force. Second detection channel: The current gait phase is greater than or equal to the blind zone phase, and the smoothing torque value falls within the torque safety range; Third detection channel: The gait phase at the current moment is greater than or equal to the phase catch-up threshold.

6. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, If the absolute value of the three-dimensional consistency residual vector of the leg in any axis is greater than the three-dimensional transient slippage velocity threshold vector, then the first layer of assignment is entered, and a fuse scaling factor is assigned to the corresponding axis of the leg, including: The three-dimensional consistency residual vector includes the velocity consistency deviation of the leg on the X, Y, and Z axes in the body coordinate system; the three-dimensional transient slippage velocity threshold includes the transient slippage velocity threshold on the X, Y, and Z axes in the body coordinate system. Independently determine whether the absolute value of the speed consistency deviation of each axis is greater than the transient slippage speed threshold of the corresponding axis. If it is greater, assign a circuit breaker scaling factor to the scaling factor of the axis; otherwise, assign an initial reference value to the scaling factor of the axis, wherein the circuit breaker scaling factor is greater than the initial reference value.

7. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, The calculation of the variance ratio based on the three-dimensional consistency residual vector includes: Store the squared vector of the current three-dimensional consistency residual vector into a vector of length [length missing]. The residual sliding window is used to record the square vector of the three-dimensional consistent residual vector of the support leg at historical moments within the current support cycle; when the leg switches from the support state to the swing state, the corresponding residual sliding window is cleared. Calculate the actual residual variance of each axis within the residual sliding window based on the speed consistency deviation of each axis; The ratio of the actual residual variance of each axis to the pre-calibrated theoretical expected variance of each axis is used as the variance ratio of each axis.

8. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, The step of assigning steady-state scaling factors to each axis of the leg based on the variance ratio includes: A limiting operation is performed on the variance ratio, and the variance ratio after the limiting operation is used as the steady-state scaling factor. The steady-state scaling factor is then assigned to the scaling factor corresponding to the axis.

9. The hierarchical adaptive state estimation and parameter optimization method for humanoid robots according to claim 1, characterized in that, The step of using independent scaling factors for each leg in a supported state to perform a weighted update of the sub-blocks corresponding to the legs in the benchmark observation noise matrix includes: The three diagonal sub-blocks corresponding to the kinematic observation velocities of the left and right legs are extracted from the baseline observation noise matrix. The diagonal sub-blocks of the current supporting leg are then updated element-wise using the scaling factor corresponding to the leg, resulting in the updated observation noise matrix.

Images