A dynamic contact confidence enhanced state estimation method
Patent Information
- Application Number
- CN202611041990.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-07-14
- Publication Date
- 2026-10-09
AI Technical Summary
[0003]本发明的目的是为解决现有方法在非结构化地形下的控制稳定性差的问题，而提出了一种动态接触置信度增强的状态估计方法
[0069]本发明将强化学习策略网络的输出作为卡尔曼滤波状态估计器的智能调节器，通过端到端训练的策略网络实时生成目标动作的置信度评估值，并将评估的置信度深度耦合至预置卡尔曼滤波模块的噪声模型中，使状态估计过程能依据环境不确定性动态调整信息融合权重；最终基于基座位姿、速度信息解算关节指令，形成策略网络、置信度增强估计、模型驱动控制的闭环控制架构，在维持传统方法物理可解释性与实时性的同时，显著提升机器人在动态扰动与非结构化地形下的状态感知精度与运动稳定性。
Smart Images
Figure CN122876359A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of humanoid robot motion control technology, specifically relating to a state estimation method with enhanced dynamic contact confidence. Background Technology
[0002] In recent years, with the development of artificial intelligence, perceptual computing, and smart hardware, humanoid robot technology has made significant breakthroughs in gait control and environmental adaptability. However, the robustness of control algorithms remains a core challenge: while reinforcement learning (RL) methods can cope with complex environments, they struggle to meet the requirements of high-reliability motion control due to high sample requirements, dependence on reward function design, and policy security risks. In contrast, traditional control methods based on physical models (such as LIPM combined with whole-body control (WBC)) have become the preferred solution for industrial deployment due to their clear dynamic constraints and real-time advantages. The core of these methods lies in accurate real-time state feedback, which requires Kalman filtering (KF) to fuse multi-source data such as IMU and joint encoders to continuously estimate the humanoid robot's base posture, velocity, and other key states. However, in actual deployment, the estimation accuracy of KF is highly susceptible to sensor noise, ground impact disturbances, and sudden external forces, leading to deviations in the reconstruction of the base state. Once the estimated value of the base state deviates from the true value, it will cause decision distortion in model predictive control (MPC) and torque calculation failure in whole-body control (WBC), ultimately resulting in gait instability or even falls. Especially in unstructured terrain, the sensitivity of traditional methods to state estimation errors has become a key bottleneck restricting the improvement of the dynamic performance of humanoid robots. Summary of the Invention
[0003] The purpose of this invention is to address the problem of poor control stability in existing methods under unstructured terrain, and to propose a state estimation method with enhanced dynamic contact confidence.
[0004] The technical solution adopted by this invention to solve the above-mentioned technical problems is: a dynamic contact confidence enhancement state estimation method, the method specifically including the following steps:
[0005] Step 1: Establish a reinforcement learning model including a policy network and a value network, load a simulation environment for training the reinforcement learning model, and train the reinforcement learning model in the loaded simulation environment.
[0006] Step 2: Construct observations in real time based on the state information of the humanoid robot, and use the constructed observations as input to the trained policy network. Kalman filter estimates the position and velocity of each joint based on the output of the policy network.
[0007] Step 3: Based on the null space projection and the estimated joint positions and velocities in Step 2, generate the expected positions, velocities, and accelerations of each joint when performing each task.
[0008] Step 4: Based on the expected joint position, expected velocity, and expected acceleration when performing each task, obtain the expected joint torque when performing each task;
[0009] The final joint control torque is calculated using the PD controller and the desired joint torque.
[0010] Furthermore, both the policy network and the value network are MLPs.
[0011] Furthermore, the training of the reinforcement learning model in the loaded simulation environment specifically includes:
[0012] Step 1: Initialize the environment instance and configure the hyperparameters for the training process;
[0013] Step 2: Initialize the training algebra ;
[0014] Step 3: Initialize the number of training rounds ;
[0015] Step 4: Obtain the state information of the humanoid robot in the current environment instance, and construct an observation based on the obtained state information;
[0016] Step 5: Use the constructed observations as input to the policy network, and the output of the policy network is the confidence level over 10 consecutive time steps;
[0017] KF obtains the position and velocity estimation results of each joint based on the confidence estimation results, WBC obtains the control torque of each joint based on the position and velocity estimation results of each joint, and controls the humanoid robot based on the control torque;
[0018] Step 6: Determine if the target has been reached. Total number of training rounds;
[0019] If the first The total number of training steps for each generation indicates that all trajectory data collection during the current generation's training process is complete, based on the number of steps taken. Calculate the cumulative reward and advantage function value for each round of training; then... All collected trajectory data are spliced into a complete sequence, and the sequence is randomly shuffled and divided into multiple mini-batches. The reinforcement learning model is trained using the mini-batches, and the parameters of the actor network and critic network are updated before proceeding to step 7.
[0020] If the first [number] is not reached The total number of training rounds is then used to determine if any environments have been terminated based on the environment reset flag. If a terminated environment exists, it is initialized and then... Return to step 4. If no terminated environment exists, then directly set... Return to step 4;
[0021] Step 7: Determine if the total number of training generations has been reached;
[0022] If the total number of training generations is reached, the final trained reinforcement learning model is obtained.
[0023] If the total number of training generations has not been reached, then reinitialize the environment instance and set... Return to step 3.
[0024] Furthermore, the observations include left foot position, right foot position, left foot posture, right foot posture, left foot velocity, left foot angular velocity, right foot velocity, right foot angular velocity, left foot height, right foot height, gait phase, IMU data, left foot contact force information, right foot contact force information, and historical confidence information.
[0025] Furthermore, the reward function used during the training of the reinforcement learning model... for:
[0026]
[0027] in:
[0028] Indicates the acceleration residual bonus. Indicates reward The weights;
[0029] This indicates a reward for the smoothness of the foot position. Indicates reward The weights;
[0030] This indicates a base speed consistency reward. Indicates reward The weights;
[0031] This indicates the jitter penalty for state estimation. Indicates reward The weights;
[0032] This indicates a reward for the base position. Indicates reward The weights;
[0033] The acceleration residual bonus is specifically as follows:
[0034]
[0035] in, express The linear acceleration measured by the IMU at time t. This represents the base acceleration estimation result obtained from the velocity difference output by KF. It is an L2 norm;
[0036] The foot position smoothness reward is specifically as follows:
[0037]
[0038] in, Indicates the estimated first The height of a foot Indicates ground reference height;
[0039] The base speed consistency reward is:
[0040]
[0041] in, The actual speed of the base. The base velocity estimated by KF;
[0042] The state estimation jitter penalty is:
[0043]
[0044] In the formula, This indicates that KF estimated... The vector consisting of the base position and velocity at time t. Estimated by KF The vector consisting of the base position and velocity at time t. It is an indicator function. express The confidence level at time relative to The increment of confidence at time step, when satisfying When the value is 1, the value of the indicator function is 1; otherwise, the value of the exponential function is 0.
[0045] The base location reward is:
[0046]
[0047] in: For the base position estimation output by KF, This refers to the actual location of the base.
[0048] Furthermore, the specific process of step three is as follows:
[0049] Step 3: Sort the tasks in descending order of priority to obtain the task sorting result;
[0050] Step 3.2 Initialization ;
[0051] Step 3: Sort the tasks in the results... The task space instructions for each task are denoted as... Calculate the desired position increment, desired velocity, and desired acceleration for each joint;
[0052] Steps three and four: If there are still tasks that have not been planned, then... If the task is not completed, return to step 33; otherwise, obtain the expected joint position for each task based on the expected joint position increment during the execution of each task.
[0053] Furthermore, the specific process of step three is as follows:
[0054]
[0055]
[0056]
[0057] in, Indicates execution of the first For each task, the vector consisting of the expected position increments of each joint; Indicates execution of the first For each task, the vector consisting of the expected position increments of each joint; Represents the prediction Jacobian matrix The false reversal; Indicates the first The Jacobian matrix corresponding to each task; For the first Position error instructions for each task; Indicates execution of the first A vector consisting of the expected velocities of each joint during a task; Indicates execution of the first A vector consisting of the expected velocities of each joint during a task; Indicates the first The spatial velocity command for each task; Indicates execution of the first The vector consisting of the expected accelerations of each joint during a task; Indicates execution of the first The vector consisting of the expected accelerations of each joint during a task; Indicates the first Spatial acceleration commands for each task; Indicates based on the execution of the first The dynamic consistency pseudo-inverse matrix calculated for joint positions after each task. express The first derivative.
[0058] Furthermore, the specific process of step four is as follows:
[0059] Step 4.1 Solving for slack variables and establishing the dynamic equations of the floating basis
[0060] Based on the execution of the Expected joint acceleration during each task Construct a constrained optimization problem based on the floating basis dynamics equations, solve the optimization problem, and calculate the dynamic relaxation variables. ;
[0061] Step 42: Substitute the slack variables into the floating basis dynamics equation to calculate the desired joint torque.
[0062] Furthermore, the slack variable for:
[0063]
[0064] in, For the acceleration relaxation variables corresponding to all joints, The contact force relaxation variable is the force on the soles of both feet.
[0065] Furthermore, the specific process of step four two is as follows:
[0066]
[0067] in, For the quality matrix, Let be a vector consisting of the desired positions of each joint. Let be the vector composed of the desired accelerations of each joint. The Coriolis force / centrifugal force vector. Let be a vector consisting of the desired velocities of each joint. The gravity vector To select a matrix, the superscript T represents the transpose of the matrix. Indicates the desired torque of the joint. To access the Jacobian matrix, This refers to the actual force applied to the sole of the foot.
[0068] The beneficial effects of this invention are:
[0069] This invention uses the output of a reinforcement learning policy network as an intelligent regulator for a Kalman filter state estimator. The policy network, trained end-to-end, generates real-time confidence assessments of target actions and deeply couples these confidence assessments to a pre-defined Kalman filter module's noise model. This allows the state estimation process to dynamically adjust information fusion weights based on environmental uncertainties. Finally, joint commands are calculated based on the base pose and velocity information, forming a closed-loop control architecture of policy network, confidence enhancement estimation, and model-driven control. While maintaining the physical interpretability and real-time performance of traditional methods, this significantly improves the robot's state perception accuracy and motion stability under dynamic disturbances and unstructured terrain. Attached Figure Description
[0070] Figure 1 This is a flowchart of a dynamic contact confidence enhancement state estimation method according to the present invention;
[0071] Figure 2 This is a simulation image of the robot walking in Isaacgym.
[0072] Figure 3 This is a simulation image of the robot walking in Mujoco.
[0073] Figure 4 This is a simulation image of the robot performing a step test in the Mujoco simulation process;
[0074] Figure 5 This is a diagram showing the effect of state estimation. Detailed Implementation
[0075] The following describes the method of this invention in further detail using Unitree's G1 general-purpose humanoid robot as an example. The core feature of Unitree's G1 general-purpose humanoid robot lies in its highly biomimetic bipedal walking structure. The G1 robot has twelve active joints for driving the movement of its legs. The specific joint configuration is as follows: each hip joint has three degrees of freedom, corresponding to forward and backward flexion and extension (pitch), left and right adduction and extension (roll), and internal and external rotation of the hip (yaw), providing a foundation for a wide range of leg movements. Each knee joint has a single degree of freedom, mainly responsible for realizing the extension and flexion of the lower leg relative to the thigh, and is a key support and cushioning link in walking. Each ankle joint has two degrees of freedom, supporting the vertical flexion and extension (pitch) and left and right adduction / extension (roll), which is crucial for maintaining the robot's dynamic balance and posture stability during walking and standing.
[0076] Specific implementation method one: Combining Figure 1 This embodiment describes a state estimation method for enhancing dynamic contact confidence, which specifically includes the following steps:
[0077] Step 1: Establish a reinforcement learning model including a policy network and a value network, and load the simulation environment for training the reinforcement learning model (using Isaacgym as an example). To improve data sampling efficiency, the Ray framework is introduced to create and manage the parallel execution of multiple environment instances. A mirror-symmetric encapsulation technique is applied to each environment instance to force the model to learn the symmetric features of bipedal movement, thereby improving the policy's generalization ability.
[0078] Both the policy network and the value network are MLP networks, and the reinforcement learning model is trained in a loaded simulation environment based on the PPO algorithm; specifically:
[0079] Step 1: Initialize the environment instance. The optimizer selected during training is Adam, and a total of 4500 generations of training are performed, with each generation completing 24 rounds of learning. Configure the key hyperparameters required for the PPO algorithm to run, such as discount factor, learning rate, entropy reward weight, policy update ratio shearing coefficient, etc. The specific hyperparameter configurations and their meanings are shown in Table 1:
[0080] Table 1
[0081]
[0082] Step 2: Initialize the training algebra ;
[0083] Step 3: Initialize the number of training rounds ;
[0084] Step 4: Obtain the state information of the humanoid robot in the current environment instance, and construct an observation measurement based on the obtained state information. The observation measurement includes the left foot position (3D), right foot position (3D), left foot posture (3D), right foot posture (3D), left foot velocity (3D), left foot angular velocity (3D), right foot velocity (3D), right foot angular velocity (3D), left foot height (1D), right foot height (1D), gait phase (1D), IMU data (6D), left foot contact force information (3D), right foot contact force information (3D), and historical confidence information (20D).
[0085] Step 5: Use the constructed observations as input to the policy network. The output of the policy network is the confidence level over 10 consecutive time steps (in traditional methods, the confidence level is set to 1 when in contact and 0 when not in contact, ignoring transitional states such as partial contact, sliding, and ground deformation, which can lead to significant errors. Estimating the confidence level using the policy trained by the RL method of this invention can effectively avoid this error).
[0086] KF obtains the position and velocity estimates of each joint based on the confidence level estimation results; the WBC module obtains the control torque of each joint based on the position and velocity estimates of each joint.
[0087] In this invention, the confidence estimation strategy is updated at a frequency of 100 Hz, and the Kalman filter update, WBC module calculation, and torque output frequency are 1000 Hz. The humanoid robot reaches a new state under the action of the desired torque output by the controller.
[0088] Reward function used during reinforcement learning model training for:
[0089]
[0090] in:
[0091] Indicates the acceleration residual bonus. Indicates reward The weights;
[0092] This indicates a reward for the smoothness of the foot position. Indicates reward The weights;
[0093] This indicates a base speed consistency reward. Indicates reward The weights;
[0094] This indicates the jitter penalty for state estimation. Indicates reward The weights;
[0095] This indicates a reward for the base position. Indicates reward The weights;
[0096] The acceleration residual reward is used to encourage the velocity change of the filtered output to be consistent with the dynamics of the IMU observation, thereby suppressing velocity estimation drift and high-frequency oscillations caused by erroneous contact weights and improving the physical consistency of the state estimation; the acceleration residual reward specifically includes:
[0097]
[0098] in, express The linear acceleration measured by the IMU at time t. This represents the base acceleration estimation result obtained from the velocity difference output by KF. It is an L2 norm;
[0099] The purpose of the foot position smoothness reward is to encourage the filtered estimation to satisfy ground geometry consistency when contact is achieved; when in partial contact / off-ground, the strategy should reduce the impact of this constraint on the estimation, thereby preventing the base height from being skewed; the foot position smoothness reward specifically includes:
[0100]
[0101] in, Indicates the estimated first The height of a foot Indicates ground reference height;
[0102] The base velocity consistency reward is used to encourage the base velocity estimate output by the Kalman filter to maintain consistency with the true velocity, thereby indirectly prompting the policy network to learn an observation confidence adjustment strategy that can improve the accuracy of state estimation; the base velocity consistency reward is:
[0103]
[0104] in, The actual speed of the base. The base velocity estimated by KF;
[0105] The purpose of the state estimation jitter penalty is to suppress state jumps caused by sudden changes in confidence and improve the smoothness of noise matrix switching; the state estimation jitter penalty is as follows:
[0106]
[0107] In the formula, This indicates that KF estimated... The vector consisting of the base position and velocity at time t. Estimated by KF The vector consisting of the base position and velocity at time t. It is an indicator function. express The confidence level at time relative to The increment of confidence at time step, when satisfying When the value is 1, the value of the indicator function is 1; otherwise, the value of the exponential function is 0.
[0108] The base position reward is used to determine the confidence level estimation; the base position reward is:
[0109]
[0110] in: For the base position estimation output by KF, This refers to the actual location of the base.
[0111] Step 6: Determine if the target has been reached. Total number of training rounds;
[0112] If the first The total number of training steps for each generation indicates that all trajectory data collection during the current generation's training process is complete (including actions, rewards, termination flags, and the next state). Based on the number of steps... Calculate the cumulative reward and advantage function value for each round of training; then... All collected trajectory data are spliced into a complete sequence, and the sequence is randomly shuffled and divided into multiple mini-batches. The reinforcement learning model is trained using the mini-batches, and the parameters of the actor network and critic network are updated before proceeding to step 7.
[0113] The PPO algorithm was used for training, with 5 training epochs and 4 mini-batches per epoch. Each training epoch repeated the training on all mini-batches to improve the network's fit to the current data, for a total of 20 updates.
[0114] If the first [number] is not reached The total number of training rounds is then used to determine if any environments have been terminated based on the environment reset flag. If a terminated environment exists, it is initialized and then... Return to step 4. If no terminated environment exists, then directly set... Return to step 4;
[0115] Step 7: Determine if the total number of training generations has been reached;
[0116] If the total number of training generations is reached, the final trained reinforcement learning model is obtained.
[0117] If the total number of training generations has not been reached, then reinitialize the environment instance and set... Return to step 3.
[0118] In the training process of this invention, each update includes four stages: forward propagation, loss calculation, back propagation, and network update.
[0119] (1) In the forward propagation phase
[0120] The current mini-batch observation data is simultaneously input into the policy network and the value network. The policy network outputs the probability distribution of actions (e.g., the parameters of a Gaussian distribution) to evaluate the probability of taking each action in a given state. The value network outputs the current value estimate for the corresponding state.
[0121] (2) In the total loss calculation stage
[0122] The loss mechanism aims to minimize the objective function of the PPO algorithm and consists of three parts: policy loss, value function loss, and entropy loss. The policy loss is the core term; it measures the magnitude of policy updates by multiplying the ratio of action probabilities between the old and new policies by the advantage function. A pruning mechanism limits the range of this ratio to prevent excessive policy deviations, ensuring training stability and convergence. The value function loss is the mean squared error (MSE) between the state value predicted by the Critic network and the actual cumulative reward. The entropy loss promotes exploration, measuring the randomness (entropy) of the action distribution, which helps avoid getting trapped in local optima early in training.
[0123] (3) During the backpropagation and network update phase
[0124] Based on the total loss function value, the gradients of the policy network and value network parameters are calculated using the chain rule. Based on these gradients, the parameters of the Actor network and Critic network are updated synchronously using an optimizer (such as Adam).
[0125] This invention successfully trains a continuous confidence estimation strategy with cross-scenario generalization capability, avoiding the problem of large sudden deviations in base position estimation caused by step jumps during contact state switching in traditional binary confidence modes. It can be directly applied to various robot platforms and experimental scenarios. Through reinforcement learning framework, dynamic adaptive adjustment of the noise matrix is achieved, which significantly improves the estimation accuracy of Kalman filtering.
[0126] Step 2: Construct observations in real time based on the state information of the humanoid robot, and use the constructed observations as input to the trained policy network. Kalman filter estimates the position and velocity of each joint based on the output of the policy network.
[0127] Step 3: The kinematic planning layer generates the desired position, desired velocity, and desired acceleration of each joint based on the null space projection and the estimated joint positions and velocities in Step 2.
[0128] Specifically,
[0129] Step 3: Sort the tasks in descending order of priority to obtain the task sorting result. The priority can be determined manually.
[0130] Step 3.2 Initialization This invention processes each task sequentially in descending order of priority, and superimposes the joint information of the next priority task onto the output of the previous priority task to ensure that high priority tasks are not interfered with by low priority tasks.
[0131] Step 3: Sort the tasks in the results... The task space instructions for each task are denoted as... (include , and ), calculate the desired position increment, desired velocity, and desired acceleration for each joint:
[0132]
[0133]
[0134]
[0135] in, Indicates execution of the first For each task, the vector consisting of the expected position increments of each joint; Indicates execution of the first For each task, the vector consisting of the expected position increments of each joint; Represents the prediction Jacobian matrix The false reversal; Indicates the first The Jacobian matrix corresponding to each task; For the first Position error instructions for each task; Indicates execution of the first A vector consisting of the expected velocities of each joint during a task; Indicates execution of the first A vector consisting of the expected velocities of each joint during a task; Indicates the first The spatial velocity command for each task; Indicates execution of the first The vector consisting of the expected accelerations of each joint during a task; Indicates execution of the first The vector consisting of the expected accelerations of each joint during a task; Indicates the first Spatial acceleration commands for each task; Indicates based on the execution of the first The dynamic consistency pseudo-inverse matrix of the joint position calculation after each task (here, dynamic means that the weight matrix contains the robot's mass matrix information, making the acceleration command more dynamically optimal). express The first derivative;
[0136] Steps three and four: If there are still tasks that have not been planned, then... If the task is not completed, return to step 33; otherwise, obtain the expected joint position for each task based on the expected joint position increment during the execution of each task.
[0137] Step 4: The dynamics solution layer obtains the joint torques when performing each task based on the expected joint position, expected velocity, and expected acceleration.
[0138] Specifically,
[0139] Step 4.1 Solving for slack variables and establishing the dynamic equations of the floating basis
[0140] Based on the execution of the Expected joint acceleration during each task Construct a constrained optimization problem (such as a contact force feasible region) using the floating basis dynamics equations (e.g., a QP problem), solve the optimization problem, and calculate the dynamic relaxation variables. :
[0141]
[0142] in, The acceleration relaxation variables are for all joints (12 dimensions). The contact force relaxation variable at the bottom of both feet (6-dimensional);
[0143] The dynamic equations of the floating base are as follows:
[0144]
[0145] in, For the quality matrix, The Coriolis force / centrifugal force vector. The gravity vector To select a matrix (containing only elements 0 and 1). To access the Jacobian matrix, the superscript T represents the transpose of the matrix. External contact force;
[0146] Step 42, Calculation of Desired Joint Torque:
[0147] Substituting the relaxation variables into the floating base dynamics equations, the desired joint torque that satisfies the dynamic constraints and compensates for the Coriolis force, centrifugal force, and gravity effects can be directly calculated. The floating base dynamics equations after substituting the relaxation variables are as follows:
[0148]
[0149] in, For actual plantar force, Indicates the desired torque of the joint. Let be a vector consisting of the desired positions of each joint. Let be a vector consisting of the desired velocities of each joint. This is a vector composed of the expected accelerations of each joint;
[0150] Since the formula for calculating the desired torque is applicable to every task, superscripts and subscripts are omitted from the vector with respect to q. After WBC finally calculates the joint position, velocity, and acceleration, it uses the PD controller in the motor layer and the desired joint torque to calculate the final joint torque.
[0151] The working process of the PD controller is as follows:
[0152]
[0153] in: It is the desired torque output by the controller. It is the proportional gain, which represents the strength of the controller's response to position loop errors. It is the expected joint position after the output action is transformed. It is the differential gain, which represents the strength of the controller's response to speed errors. For the desired velocity of the joint, This is the current position of the joint. The current velocity of the joint. This represents the desired torque of the joint.
[0154] The PD parameters of each joint are shown in Table 2:
[0155] Table 2 PD Parameters
[0156]
[0157] like Figure 2 The image shows a schematic diagram of the robot walking in the simulation environment Isaacgym. Figure 3 The image shows a schematic diagram of a robot walking in the Mujoco simulation environment. Figure 4 This is a schematic diagram of a robot conducting a step test in the Mujoco simulation environment. Figure 5This is a diagram illustrating the state estimation results. This invention generates confidence feedback in real-time through an end-to-end trained reinforcement learning policy network. Combined with a pre-deployed Kalman filter state estimation module (KF) and whole-body control module (WBC), it enables the robot to actively adapt to complex environmental disturbances, uncertainties in its own state, and changes in target speed during walking. This method abandons the limitations of predefined gait templates, instead using the policy network to output a dynamic confidence assessment (value between 0 and 1) of the target foot placement point online. The confidence information is integrated into the Kalman filter, and combined with real-time sensor data (such as IMU, joint encoders, and plantar force sensors) to generate robust state estimates of the robot's joint positions and velocities. This drives the high-performance whole-body control module (WBC) to calculate joint torques and position commands in real-time based on the state estimate and the overall task objective, achieving closed-loop adaptive motion control. This method significantly improves the stability, disturbance resistance, and motion efficiency of humanoid robots in unknown or unstructured scenarios.
[0158] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention.
Claims
1. A state estimation method with enhanced dynamic contact confidence, characterized in that, The method specifically includes the following steps: Step 1: Establish a reinforcement learning model including a policy network and a value network, load a simulation environment for training the reinforcement learning model, and train the reinforcement learning model in the loaded simulation environment. Step 2: Construct observations in real time based on the state information of the humanoid robot, and use the constructed observations as input to the trained policy network. Kalman filter estimates the position and velocity of each joint based on the output of the policy network. Step 3: Based on the null space projection and the estimated joint positions and velocities in Step 2, generate the expected positions, velocities, and accelerations of each joint when performing each task. Step 4: Based on the expected joint position, expected velocity, and expected acceleration when performing each task, obtain the expected joint torque when performing each task; The final joint control torque is calculated using the PD controller and the desired joint torque.
2. The state estimation method for dynamic contact confidence enhancement according to claim 1, characterized in that, Both the policy network and the value network are MLPs.
3. The state estimation method for dynamic contact confidence enhancement according to claim 2, characterized in that, The training of the reinforcement learning model in the loaded simulation environment specifically includes: Step 1: Initialize the environment instance and configure the hyperparameters for the training process; Step 2: Initialize the training algebra ; Step 3: Initialize the number of training rounds ; Step 4: Obtain the state information of the humanoid robot in the current environment instance, and construct an observation based on the obtained state information; Step 5: Use the constructed observations as input to the policy network, and the output of the policy network is the confidence level over 10 consecutive time steps; KF obtains the position and velocity estimation results of each joint based on the confidence estimation results, WBC obtains the control torque of each joint based on the position and velocity estimation results of each joint, and controls the humanoid robot based on the control torque; Step 6: Determine if the target has been reached. Total number of training rounds; If the first The total number of training steps for each generation indicates that all trajectory data collection during the current generation's training process is complete, based on the number of steps taken. Calculate the cumulative reward and advantage function value for each round of training; then... All collected trajectory data are spliced into a complete sequence, and the sequence is randomly shuffled and divided into multiple mini-batches. The reinforcement learning model is trained using the mini-batches, and the parameters of the actor network and critic network are updated before proceeding to step 7. If the first [number] is not reached The total number of training rounds is then used to determine if any environments have been terminated based on the environment reset flag. If a terminated environment exists, it is initialized and then... Return to step 4. If no terminated environment exists, then directly set... Return to step 4; Step 7: Determine if the total number of training generations has been reached; If the total number of training generations is reached, the final trained reinforcement learning model is obtained. If the total number of training generations has not been reached, then reinitialize the environment instance and set... Return to step 3.
4. The state estimation method for dynamic contact confidence enhancement according to claim 3, characterized in that, The observations include left foot position, right foot position, left foot posture, right foot posture, left foot velocity, left foot angular velocity, right foot velocity, right foot angular velocity, left foot height, right foot height, gait phase, IMU data, left foot contact force information, right foot contact force information, and historical confidence information.
5. The state estimation method for dynamic contact confidence enhancement according to claim 4, characterized in that, The reward function used during the training of the reinforcement learning model for: in: Indicates the acceleration residual bonus. Indicates reward The weights; This indicates a reward for smoothness at the foot position. Indicates reward The weights; This indicates a base speed consistency reward. Indicates reward The weights; This indicates the jitter penalty for state estimation. Indicates reward The weights; This indicates a reward for the base position. Indicates reward The weights; The acceleration residual bonus is specifically as follows: in, express The linear acceleration measured by the IMU at time t. This represents the base acceleration estimation result obtained from the velocity difference output by KF. It is an L2 norm; The foot position smoothness reward is specifically as follows: in, Indicates the estimated first The height of a foot Indicates ground reference height; The base speed consistency reward is: in, The actual speed of the base. The base velocity estimated by KF; The state estimation jitter penalty is: In the formula, This indicates that KF estimated... The vector consisting of the base position and velocity at time t. Estimated by KF The vector consisting of the base position and velocity at time t. It is an indicator function. express The confidence level at time relative to The increment of confidence at time step, when satisfying When the value is 1, the value of the indicator function is 1; otherwise, the value of the exponential function is 0. The base location reward is: in: For the base position estimation output by KF, This refers to the actual location of the base.
6. The state estimation method for dynamic contact confidence enhancement according to claim 5, characterized in that, The specific process of step three is as follows: Step 3: Sort the tasks in descending order of priority to obtain the task sorting result; Step 3.2 Initialization ; Step 3: Sort the tasks in the results... The task space instructions for each task are denoted as... Calculate the desired position increment, desired velocity, and desired acceleration for each joint; Steps three and four: If there are still tasks that have not been planned, then... Return to step three; Otherwise, the expected joint position for each task is obtained based on the expected joint position increment for each task.
7. The state estimation method for dynamic contact confidence enhancement according to claim 6, characterized in that, The specific process of step 33 is as follows: in, Indicates execution of the first For each task, the vector consisting of the expected position increments of each joint; Indicates execution of the first For each task, the vector consisting of the expected position increments of each joint; Represents the prediction Jacobian matrix The false reversal; Indicates the first The Jacobian matrix corresponding to each task; For the first Position error instructions for each task; Indicates execution of the first A vector consisting of the expected velocities of each joint during a task; Indicates execution of the first A vector consisting of the expected velocities of each joint during a task; Indicates the first The spatial velocity command for each task; Indicates execution of the first The vector consisting of the expected accelerations of each joint during a task; Indicates execution of the first The vector consisting of the expected accelerations of each joint during a task; Indicates the first Spatial acceleration commands for each task; Indicates based on the execution of the first The dynamic consistency pseudo-inverse matrix calculated for joint positions after each task. express The first derivative.
8. The state estimation method for dynamic contact confidence enhancement according to claim 7, characterized in that, The specific process of step four is as follows: Step 4.1 Solving for slack variables and establishing the dynamic equations of the floating basis Based on the execution of the Expected joint acceleration during each task Construct a constrained optimization problem based on the floating basis dynamics equations, solve the optimization problem, and calculate the dynamic relaxation variables. ; Step 42: Substitute the slack variables into the floating basis dynamics equation to calculate the desired joint torque.
9. The state estimation method for dynamic contact confidence enhancement according to claim 8, characterized in that, The slack variable for: in, For the acceleration relaxation variables corresponding to all joints, The contact force relaxation variable is the force on the soles of both feet.
10. The state estimation method for dynamic contact confidence enhancement according to claim 9, characterized in that, The specific process of step four two is as follows: in, For the quality matrix, Let be a vector consisting of the desired positions of each joint. Let be the vector composed of the desired accelerations of each joint. The Coriolis force / centrifugal force vector. Let be a vector consisting of the desired velocities of each joint. The gravity vector To select a matrix, the superscript T represents the transpose of the matrix. Indicates the desired torque of the joint. To access the Jacobian matrix, This refers to the actual force applied to the sole of the foot.