A motion control method for a humanoid robot based on ground contact detection

Abstract

This invention discloses a motion control method for humanoid robots based on ground contact detection, applied to bipedal humanoid robots with a floating base structure. The method includes: constructing a model predictive controller based on a center-of-mass dynamics model; constructing a ground contact probability model; adaptively adjusting the current gait phase based on ground contact detection results; and constructing an inverse dynamics quadratic programming problem including generalized acceleration, foot contact force, and joint driving torque, solving for the joint torque command under conditions satisfying dynamic constraints, foot non-slip constraints, and friction cone constraints. By introducing ground contact detection into a nonlinear model predictive controller, designing a reasonable center-of-mass dynamics model, and adding necessary equality and inequality constraints, this invention enables adaptive adjustment of the humanoid robot in scenarios with unknown foot contact states, improving the stability and robustness of the humanoid robot's walking in complex terrain conditions.

Description

Technical Field

[0001] This invention relates to the field of motion control technology for humanoid robots, and in particular to a motion control method for humanoid robots based on ground contact detection. Background Technology

[0002] Humanoid robots, due to their structure and movement similar to humans, are more adaptable to working environments and offer greater flexibility compared to traditional robot platforms when performing tasks in human-like work scenarios. The capabilities of humanoid robots largely lie in their movement abilities. Compared to data-driven motion control methods, humanoid robot motion control algorithms based on Model Predictive Control (MPC) offer zero-sample performance, theoretical optimality, and interpretability. For tasks requiring precise control of motion states and where sample quality is low, MPC-based motion control methods remain a more efficient choice. However, during actual movement, the contact state between the feet and the ground in humanoid robots exhibits high uncertainty, especially in complex terrain environments such as non-ideal ground surfaces or uneven surfaces. Robots often exhibit premature or delayed ground contact, frequently causing discrepancies between the actual gait of the humanoid robot and the gait planned by the upper-level gait planner. This leads to a decrease in the robustness of humanoid robots in uneven terrain.

[0003] Existing model predictive control-based motion control methods for humanoid robots typically rely on gait timing or fixed ground contact times set by an upper-level gait planner. These methods assume the robot's feet make contact with the ground at predetermined times and use this information for gait switching and control decisions. For example, patent application CN115256396A relies on a preset gait timing when planning the foot placement of a bipedal robot, failing to consider the issues of early ground contact during the swing phase and delayed ground contact during the support phase. This may cause its stability criteria to fail. In practical applications, due to factors such as terrain height errors, uncertainties in the robot model, and external disturbances, the actual ground contact state of the feet often deviates from the preset gait, easily leading to the following problems: 1. When the swing leg touches the ground prematurely, the position control is still performed according to the swing trajectory, resulting in a large contact impact and causing the fall. 2. The swing leg is misjudged as being in a supporting state before it actually touches the ground, resulting in unstable control; 3. Inaccurate judgment of the ground contact state of the supporting leg affects the rationality of the contact force distribution output by the model predictive control algorithm; 4. The upper-level gait planner cannot perceive the actual ground contact state in a timely manner, making it difficult to adaptively adjust the gait timing and contact pattern, which easily leads to the accumulation of errors.

[0004] To address the aforementioned issues, some studies have attempted to introduce a single-threshold method for ground contact detection. However, this single-signal-based approach is susceptible to noise and lacks robustness, failing to meet the requirements for stable movement of humanoid robots in complex environments. For instance, patent application CN119917909A uses only joint encoder data for ground contact detection in a legged robot, making it highly susceptible to noise. Therefore, a motion control method for humanoid robots that can integrate multi-dimensional information and accurately feed back ground contact detection results to the controller is crucial. Summary of the Invention

[0005] The purpose of this invention is to overcome the problem in existing humanoid robots that lack ground contact state judgment or whose ground contact state judgment is inaccurate, making them difficult to adapt to complex terrain. This invention proposes a motion control method for humanoid robots based on ground contact detection. The invention involves five parts: constructing a humanoid robot model predictive controller, constructing a foot ground contact probability model, adaptive gait phase adjustment, whole-body torque control, and motion performance simulation verification. The key to this invention is to construct a ground contact probability model by fusing gait phase information and foot force information, thereby achieving accurate estimation of the actual foot ground contact state. The ground contact detection results are then fed back to a model predictive controller based on center-of-mass dynamics for dynamic adjustment of the gait phase, thus improving the stability of the humanoid robot's motion control and its adaptability to complex terrain. Embodiments of this invention can be applied to common humanoid robot platforms. Verification experiments on a physical simulation platform have demonstrated that the method of this invention has good versatility and practicality.

[0006] The objective of this invention is achieved through the following technical solution: a motion control method for humanoid robots based on ground contact detection, applied to a bipedal humanoid robot with a floating base structure, the method comprising the following steps: (1) Constructing a humanoid robot model predictive controller based on the center of mass dynamics model: Construct the floating base joint space of the humanoid robot, introduce the center of mass momentum matrix under the framework of the center of mass dynamics model, establish the relationship between the center of mass momentum vector and the generalized velocity of the humanoid robot, and realize the center of mass dynamics modeling; define the state vector and control input of the model predictive control, and complete the definition of the humanoid robot model predictive control problem based on the center of mass dynamics model. (2) Foot contact state estimation based on gait phase and plantar force probability fusion: Construct a corresponding contact probability model for gait phase information and normal plantar force information, and give the final foot contact probability based on Bayesian probability fusion; (3) Adaptive adjustment of gait phase based on ground contact detection results: Construct an adaptive adjustment method for gait phase based on ground contact detection results, and reposition the current gait execution phase for the cases of early ground contact of the swing phase and delayed ground contact of the support phase respectively; (4) Whole-body torque control method based on inverse dynamics quadratic programming problem: After obtaining the output of the model predictive controller, the reference trajectory is converted into the joint driving torque command of the humanoid robot through whole-body control; considering dynamic constraints, the whole-body control is constructed as an inverse dynamics quadratic programming problem, and the joint driving torque is obtained according to the inverse dynamics equation to optimize the tracking effect of the upper model predictive controller output; (5) Simulate and verify the motion performance of the humanoid robot on the dynamic simulation platform.

[0007] Further, step (1) includes the following sub-steps: (1.1) Construct the floating base joint space of the humanoid robot, establish the floating base dynamic model of the humanoid robot based on the description file of the humanoid robot, introduce the center of mass momentum matrix, establish the relationship between the center of mass momentum vector and the generalized velocity of the humanoid robot, and realize the center of mass dynamic modeling. (1.2) Define the state vector and control input of the model predictive control, wherein the state vector consists of the generalized joint angle and the joint angular velocity, and the control input consists of the joint acceleration and the generalized force of the foot. Further obtain the system state equation; define the model predictive control objective function according to the control objective and select the required constraints.

[0008] Furthermore, step (2) includes the following sub-steps: (2.1) Construct a foot contact probability model based on gait phase, and define contact probability models for swing phase and support phase respectively. The contact probability of swing phase decreases to increase, and the contact probability of support phase increases to decrease. The function is continuous at the gait switching point. (2.2) Construct a foot contact probability model based on normal plantar force, and adopt a monotonically increasing contact probability model; use joint torque and Jacobian matrix to solve the plantar force through inverse dynamics; (2.3) Calculate the final foot contact probability based on Bayesian probability fusion.

[0009] Furthermore, step (3) includes the following sub-steps: (3.1) Define the complete gait cycle, gait switching time, and current gait phase based on the results of the upper-level gait planner; (3.2) Based on the ground contact probability of the left and right feet obtained in step (2), determine the ground contact determination result of each foot and form the corresponding support phase set; (3.3) When it is detected that the swing foot touches the ground before the preset phase ends, the current gait phase is switched to the next support phase in advance to achieve phase forward adjustment in the case of early ground contact; (3.4) When it is detected that the swing foot has not touched the ground at the end of the preset phase, the current gait phase is adjusted to the previous swing phase to realize the swing phase delay adjustment in the case of delayed ground contact; (3.5) After completing the gait phase adjustment, update the current set of feet that are touching the ground and feed it back to the model prediction controller for subsequent control calculations.

[0010] Furthermore, step (4) includes the following sub-steps: (4.1) Based on the joint acceleration reference and the foot contact force reference, a quadratic programming inverse dynamics problem is constructed with the goal of minimizing the joint acceleration tracking error and the contact force deviation. The generalized acceleration error, the foot contact force error and the joint driving torque are used as the decision variables of the inverse dynamics quadratic programming problem. (4.2) Based on the floating basis dynamic equation of the humanoid robot, construct the equality constraint, and combine it with the current support state set to construct the foot non-slip constraint. Use the dynamic constraint and the foot non-slip constraint together as the equality constraint conditions of the quadratic programming problem. (4.3) Construct inequality constraint conditions that include friction cone constraints and joint driving torque constraints to ensure that the foot contact force meets the friction stability requirements and the joint driving torque meets the physical limitations of the actuator. (4.4) Solve the inverse dynamics problem in real time within the control cycle to obtain the optimal joint driving torque, and send the joint driving torque to each drive joint of the humanoid robot for execution. The joint PD control controls the joint motor movement at a frequency of 1000Hz.

[0011] Furthermore, step (5) includes the following sub-steps: (5.1) Import the unified robot description file of the humanoid robot into the dynamic simulation platform and establish a complete dynamic simulation model including robot structural parameters, mass parameters and joint constraint parameters; (5.2) Construct various terrain environment models in the dynamic simulation platform. The terrain environment includes flat terrain, non-flat terrain with random undulations and low friction discrete terrain, and set the corresponding terrain undulation parameters and friction coefficient parameters. (5.3) Run the motion control method of steps (2) to (4) in the terrain environment, analyze the walking stability and gait adjustment response of the robot during the simulation, and verify the stability and robustness of the motion control method under different terrain conditions.

[0012] The beneficial effects of this invention are: (1) Compared with model predictive control algorithms that do not perform ground contact detection, this invention constructs a probabilistic fusion method for estimating foot ground contact state based on gait phase and foot force, and adaptively adjusts the gait phase by combining the foot ground contact probability results. This enables the humanoid robot to accurately identify real ground contact events and promptly correct the current gait execution phase under complex terrain conditions with terrain height errors. While ensuring engineering feasibility, this effectively avoids gait mismatch problems caused by deviations between ground contact planning and reality, thereby significantly improving the stability and adaptability of the humanoid robot during walking in complex terrain.

[0013] (2) Compared with motion planning methods based solely on the Single Rigid Body (SRB) model or the Linear Inverted Pendulum Model (LIPM), this invention employs a model predictive control method based on the center of mass dynamics model. This method fully considers the influence of the humanoid robot's overall configuration, joint movements, and foot contact on the change in center of mass momentum during the planning process. By introducing the center of mass momentum matrix, the relationship between joint movements, foot contact, and center of mass momentum is modeled, resulting in higher physical consistency of the motion trajectory during the planning stage.

[0014] (3) Through continuous simulation verification on flat ground, uneven road surfaces, and discrete contact terrain in a dynamic simulation platform, the method of the present invention can maintain stable walking under various contact conditions, demonstrating good adaptability to complex terrain and engineering effectiveness. The present invention has certain versatility on common humanoid robot platforms, and is also applicable to some quadruped robots and other legged robots. In addition, the present invention can be used for motion skill generation in various terrains such as snow, grass, and mud. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of a humanoid robot. Figure 2 It is a graph of the ground contact probability function based on gait phase; Figure 3 It is a graph of the ground contact probability function based on plantar force; Figure 4 This is a diagram of the motion control framework for a humanoid robot; Figure 5 This is a schematic diagram of the terrain that humanoid robots will face on a dynamic simulation platform; Figure 6 These are consecutive images of a humanoid robot testing its terrain-crossing capabilities in a dynamics simulation platform. Detailed Implementation

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

[0017] This invention provides a motion control method for humanoid robots based on ground contact detection, applied to bipedal humanoid robots with a floating base structure. The method includes the following steps: Step 1: Construct a predictive controller for a humanoid robot model based on a centroid dynamics model.

[0018] In this step, a floating base joint space is first constructed based on the joint degrees of freedom of the humanoid robot. Using the humanoid robot's torso as a floating base, a six-degree-of-freedom floating base coordinate system is introduced to obtain the humanoid robot's joint space coordinate vector. ,in: This indicates the position and orientation of the floating base in the world coordinate system; Let represent the angles of each driven joint as defined by the humanoid robot; correspondingly, define the robot's generalized velocity and generalized acceleration as . and Based on the mass and inertial parameters of each link in the humanoid robot structure, the total mass of the robot is calculated. Location of the center of mass and the angular momentum of the center of mass And define the mass momentum vector as .

[0019] Within the framework of the center-of-mass dynamics model, not only are the robot's center-of-mass dynamic constraints considered, but the robot's kinematic states are also explicitly introduced, enabling the model to reflect the influence of joint motion on the center of mass and its angular momentum. A center-of-mass momentum matrix is ​​introduced. Establish the relationship between the center-of-mass momentum vector and the generalized velocity: Taking the first derivative of the above relationship, we obtain the rate of change of the center of mass momentum: On the other hand, the change in the robot's center of mass momentum can also be represented by the generalized force of contact at the foot end. ,in This represents the current set of possible foot contact points; Indicates the first The position of each foot contact point in the world coordinate system; and Let represent the contact force and contact torque at the foot contact point, respectively. Using the above equations, the robot joint acceleration and the generalized force at the foot contact point can be unified into the center-of-mass dynamics constraint, thus achieving center-of-mass dynamics modeling.

[0020] Define the state vector of model predictive control as ,in Indicates the first The robot's floating base pose and the degrees of freedom of its body joints at a predicted moment; This represents the corresponding generalized velocity; the control input for model predictive control is defined as: ,in This represents the joint acceleration that can drive the joint; Indicates the first The generalized contact force experienced by each foot contact point; This represents the number of possible contact points for the humanoid robot. In the control time domain... Within a step, to achieve comprehensive optimization of the stability of the center of mass motion, the smoothness of angular momentum, and the smoothness of joint motion, the predictive control objective function of the model is constructed in the following form:

[0021] in , Let the desired center of mass position and desired center of mass velocity be given. Let angular momentum be the center of mass of the robot; , , , , This is a weighted matrix of all cost items.

[0022] To ensure the dynamic rationality and motion stability of the humanoid robot in the control time domain, the following constraints are imposed on the model predictive control problem: 1. Center of mass dynamic constraints: .

[0023] 2. State equation constraints: ; ;in , Corresponding to floating base acceleration.

[0024] 3. Non-slip constraint on the foot when touching the ground: ;in The Jacobian matrix is ​​for contact with the foot tip.

[0025] 4. Upper-level gait planning constraints: for the foot end Apply constraints That is, non-contact generalized force; for Contact friction cone constraint must be satisfied ,in ; This is an approximately linear constraint matrix constructed based on the geometry of the humanoid robot's foot and the coefficient of friction of the terrain; and the constraint normal force must satisfy... .

[0026] 5. Joint position, joint velocity, and joint acceleration amplitude constraints:

[0027] .

[0028] in It refers to the ability to drive joint acceleration.

[0029] In summary, by constructing cost functions with clear meanings, simplifying the robot's floating basis dynamics equations using the center-of-mass momentum model, and setting multiple constraints, the predictive control problem of the humanoid robot model based on the center-of-mass dynamics model can be expressed as:

[0030] The above class of model predictive control problems based on center-of-mass dynamics can be simplified as follows:

[0031] in Indicates the first The contribution term of the generalized force at the foot contact point to the momentum of the center of mass. The term is the gravitational term. For the friction cone constraint set, It is a vector-valued function.

[0032] The humanoid robot platform "Wukong-IV" in this embodiment has an upright height of 1.6m, a weight of 45.9kg, and 21 degrees of freedom throughout its body. Figure 1 As shown, the waist has 1 degree of freedom, each leg includes 3 degrees of freedom for the hip joint, 1 degree of freedom for the knee joint, and 2 degrees of freedom for the ankle joint, and each arm includes 3 degrees of freedom for the shoulder joint and 1 degree of freedom for the elbow joint. The complete generalized coordinate system is defined as follows: , For floating base coordinates, For driveable joint coordinates.

[0033] In constructing the "Wukong-IV" model predictive controller based on center-of-mass dynamics, state variables are defined. ,in , , representing the joint angles and joint velocities of the "Wukong-IV" full-body system. Define control variables. , This indicates the joint acceleration that "Wukong-IV" can directly drive. This represents the generalized force applied to the foot, because the "Wukong-IV" gait presupposes that only one foot touches the ground. Setting it to 1 correctly represents all motion states. For the center-of-mass momentum equation of "Wukong-IV", we have the center-of-mass momentum matrix. Under the dynamic constraints of the center of mass middle It contains only one element: single-foot contact. The "Wukong-IV" uses a rigid surface contact mode for its sole contact instead of point contact, and the foot position... Therefore, the Jacobian matrix at the foot is contacted. This leads to the construction of a non-slip constraint at the foot: For the desired center-of-mass velocity The desired centroid position is obtained from input via the control handle or keyboard. Through the Obtained through numerical integration. , , , , Five-term weighted matrix and normal force constraint Based on engineering experience, adjustments were made according to specific application scenarios. The limits of each joint of "Wukong-IV" are shown in Table 1.

[0034] Table 1 Joint Limitations of "Wukong-IV"

[0035] Step 2: Estimation of foot contact state based on gait phase and plantar force probability fusion.

[0036] Based on the completion of the centroid dynamics model predictive controller, in order to enable the described model predictive controller to accurately reflect the actual contact state of the humanoid robot during actual walking, a foot contact state estimation method based on the probabilistic fusion of gait phase information and normal foot force information is constructed to determine the contact state of the humanoid robot in real time during its movement.

[0037] First, a foot-to-ground contact probability model based on gait phase is constructed. For the swing phase, a normalized time variable for the swing phase is defined. ,in Indicates the first The current swing phase of the foot has lasted for a certain period of time. This represents the total duration of the foot's swing phase. Since the swing phase includes three stages: about to leave the ground, leaving the ground, and about to touch the ground, the probability of contacting the ground during the swing phase should first decrease to a minimum and then increase. Construct a probability model for the oscillating phase touching the ground: ,in This indicates the preset midpoint of the swing, representing the most likely moment within the swing phase when the object is not in contact with the ground. This represents the scaling factor for the oscillation phase probability, controlling the width of the probability interval.

[0038] For the support phase, define the support phase normalized time variable. ,in Indicates the first The current support phase at the foot has lasted for a period of time. This represents the total duration of the foot support phase. Since the support phase includes three stages: near-ground contact, ground contact, and near-ground lift, the probability of ground contact during the support phase duration should first increase to a maximum and then decrease. Construct a probability model for the supporting phase contacting the ground: ,in This indicates the preset support midpoint, representing the most likely moment of ground contact within the support phase. This represents the scaling factor for the probability of the supporting phase, which controls the width of the probability interval.

[0039] To ensure the continuity of the ground contact probability at gait transition moments, a continuity control constant is introduced. The probabilities of the swing phase and the support phase touching the ground are adjusted as follows: ,

[0040] in , At the gait transition point, when At that time, the following conditions are met: The final contact probability model based on gait phase is unified as follows:

[0041] Secondly, a foot contact probability model based on plantar force is constructed. Define the first... The normal force on the sole of each foot along the direction perpendicular to the ground is Theoretically, the probability of contact with the ground is related to The correlation is positive, and the following monotonic ground contact probability model is constructed:

[0042] in The threshold value for the inflection point of contact force. This represents the slope control coefficient. The probabilistic model conforms to physical reality, has smooth mathematical properties, and is easy to deploy and implement in engineering.

[0043] In practical applications, the plantar force of humanoid robots can be obtained through force sensors, but these sensors often suffer from problems such as zero drift, aging, and fragility, resulting in poor practicality and accuracy. The plantar force can be solved using inverse dynamics by utilizing the joint driving torques and kinematic parameters of the humanoid robot. The dynamic equations of the floating base of the humanoid robot are as follows: , in: For the mass matrix of humanoid robots, For Coriolis force, centrifugal force, and gravity terms, The contribution of joint torque to the generalized force of the system. The selection matrix for driving joints, The contribution of contact force to the generalized force of the system. To access the Jacobian matrix.

[0044] The dynamic equations can be obtained by dividing them into blocks based on floating bases and driveable joints:

[0045] Subscript These represent the floating base degrees of freedom and the driveable joint degrees of freedom, respectively.

[0046] From the above, it is easy to conclude that: Further, through pseudo-inverse operation, we obtain: , In practical applications, the actual torque of each joint of a humanoid robot can be expressed as: ,in For joint driving torque, For the plantar force equivalent joint torque, there is We can obtain: .Pick exist The desired result can be obtained by applying the component of force in the direction. .

[0047] To integrate ground contact probability information based on gait phase and plantar force, a ground contact probability function based on Bayesian probability fusion is constructed: , Compared to probabilistic fusion models based on Kalman filtering, this probabilistic model does not require a noise covariance matrix or a grounding probability time-domain state transition model, making it highly feasible in engineering. Mathematically, this probabilistic model automatically suppresses cell information noise, only... When both are in a high or low state, Only then will it manifest as a higher or lower state.

[0048] Finally, a probability threshold is set. ,when Time judgment One foot is in the ground-touching state; otherwise, it is not in the ground-touching state.

[0049] In constructing the "Wukong-IV" foot-to-ground state estimation model, a preset swing mid-segment constant is taken. Preset support for middle section constant Oscillating phase probability scaling factor Support phase probability scaling factor To ensure the continuity of the probability function during gait switching, the chosen... and The following linear relationship must be satisfied:

[0050] In this embodiment, the following measures are taken: , The specific ground contact probability function constructed based on gait phase is as follows: Figure 2 As shown.

[0051] To construct a ground contact probability model based on plantar normal force, the slope control coefficient is selected. Threshold of ground contact force inflection point The specific ground contact probability function based on plantar force is constructed as follows: Figure 3 As shown.

[0052] In constructing the system for solving the plantar force using the joint driving torque and kinematic parameters of the "Wukong-IV" humanoid robot, the "Wukong-IV" floating base dynamics are constructed in the following form: , in The mass, moment of inertia, and geometric parameters of each link in the "Wukong-IV" URDF were calculated. This represents the Coriolis force, centrifugal force, and gravitational term experienced by "Wukong-IV" during its motion. This represents the joint selection matrix for "Wukong-IV," used to select directly driveable joints, along with the joint drive torque vector. Used to execute the controller's output. It is based on the contact Jacobian matrix of surface contact. Since ground contact detection needs to consider the plantar forces of both moving legs, the generalized plantar force is... It has the following forms:

[0053] For the block-based floating basis dynamics equations ,have , , , , , , , For the "Wukong-IV" single-leg support mode, the Moore–Penrose pseudo-inverse can be used to obtain:

[0054] In engineering Approximate, where This represents the total torque of the "Wukong-IV" joint. This represents the joint driving torque of "Wukong-IV," which can be read via an encoder in the physical machine and directly in the simulation platform. Let a ground contact probability threshold be set. .

[0055] Step 3: Adaptive adjustment of gait phase based on ground contact detection results.

[0056] After obtaining the probability estimation results of the foot contact state, in order to ensure that the gait execution of the humanoid robot in complex terrain is consistent with reality, and at the same time not to destroy the periodic gait structure generated by the upper gait planner, a gait phase adaptive adjustment method based on the contact detection results is constructed to reposition the current gait execution phase.

[0057] Let the time of a complete gait cycle output by the gait planner be... In a gait cycle, each leg is divided into a swing phase and a stance phase, with the gait transition time being... Define the current gait phase variable as .in This indicates the robot's current position within its gait cycle. It increases monotonically over time and resets to zero when it reaches 𝑇.

[0058] For the oscillating phase, if the ground contact detection module determines... This indicates that the swing leg touches the ground prematurely, before the planned gait, and the terrain may appear bumpy. To avoid unnecessary collisions or instability caused by continuing to execute according to the swing phase, the current gait phase is adjusted to: ; This involves advancing the current gait phase to the corresponding phase at the start of the next gait state, such as the moving leg. If the leg touches the ground prematurely at the beginning of the swing phase, adjust the movement leg. The gait phase enters the support phase in advance.

[0059] For the support phase, if the ground contact detection module determines... This indicates a delayed ground contact of the swing leg, meaning the ground contact event lags behind the planned gait, and the terrain may show signs of depression. To avoid decreased stability due to insufficient effective contact within the stance phase, the current gait phase is adjusted to: ; This means resetting the current gait phase to the corresponding phase at the start of the previous gait state, such as the moving leg. If the foot touches the ground late in the support phase, adjust the movement leg. The gait phase returns to the starting phase of the oscillation phase, thus prolonging the actual oscillation phase time.

[0060] Except for the two cases mentioned above, gait phase is not adjusted. This adjustment method does not change the gait sequence and periodic structure generated by the gait planner; it only relocates the position of the current execution moment within the gait period and feeds it back to the model prediction controller, adjusting the set of ground-contacting feet. It can respond quickly to two abnormal situations: premature ground contact and delayed ground contact, thereby improving the stability of humanoid robots' gait execution under complex terrain conditions and has engineering practicality.

[0061] For the gait phase adaptive adjustment process of "Wukong-IV" based on ground contact detection results, the complete gait cycle time is taken. .

[0062] Step 4: Whole-body torque control method based on inverse dynamics quadratic programming problem.

[0063] The gait phase is adaptively adjusted, and the output of the model predictive controller is obtained. and Subsequently, the reference trajectory needs to be converted into joint drive torque commands for the humanoid robot through Whole Body Control (WBC). To optimize the tracking performance of the upper-level model predictor controller output, and considering dynamic constraints, WBC is constructed as the following inverse dynamic quadratic programming (QP) problem:

[0064] in For the friction cone constraint set, , This represents the tracking error of the upper-level model's predicted control output, and the torque obtained by solving the above quadratic programming problem in each control cycle. This refers to the joint driving torques deployed at each joint of the humanoid robot, which can be solved using the QP solver in engineering.

[0065] For the inverse dynamics QP of "Wukong-IV", there is , To output the desired joint angle acceleration trajectory for MPC, , To output the desired plantar generalized force trajectory for MPC, , In actual deployment Typically only for the acceleration of driveable joints Weighted, floating base joint acceleration Generally, the constraint is determined directly through constraints. The specific form of the friction cone constraint is as follows: For each contact foot end, the following condition is satisfied:

[0066] In this embodiment, the whole-body control module is located after the model predictive controller, and obtains the optimal solution by calling a real-time QP solver (OSQP / qpOASES / HPIPM, etc.). ,Will The commands are then distributed to the actuators of each joint of the "Wukong-IV". Through the above steps, the WBC problem is constructed into a convex quadratic programming problem that includes generalized acceleration, foot contact force, and joint driving torque. Under the premise of satisfying floating basis dynamics, contact retention constraints, and friction cone constraints, the optimal joint torque command is obtained in real time.

[0067] Step 5: Simulate and verify the motion performance of the humanoid robot on the dynamic simulation platform.

[0068] To verify the effectiveness of the proposed humanoid robot motion control method under complex terrain conditions, the motion performance of the humanoid robot was further verified through dynamic simulation on a dynamic simulation platform. In this embodiment, the "Wukong-IV" robot and its URDF model were used to perform simulation verification of the algorithm on the RaiSim platform. The RaiSim platform adopts a numerical integration method based on rigid body dynamics and contact constraints, which can accurately simulate the motion behavior of multi-rigid-body systems under complex contact conditions and is suitable for motion simulation of humanoid robots in complex terrain environments.

[0069] The algorithm modules built in steps one through four are arranged according to... Figure 4 The logical framework shown is applied to the "Wukong-IV" motion control system, and the effectiveness of the motion control method proposed in this invention is verified in a simulation platform. A complex terrain is constructed in the RaiSim simulation platform, consisting of three different terrains: flat ground, uneven road surface, and discrete glass road surface, continuously spliced ​​together. Figure 5 As shown, flat ground was used to verify the basic stability of the algorithm, with a ground friction coefficient set to 0.8. Uneven ground was used to verify the adaptability of the humanoid robot in terrain with continuously changing heights, mainly examining the ground contact detection algorithm's ability to identify and process "early / delayed ground contact," with a maximum height difference set to [value missing]. The ground friction coefficient is 0.8. Discrete glass pavement is used to simulate terrain with abrupt height changes, to verify the humanoid robot's ability to overcome "early / delayed ground contact" and its anti-slip capability under discontinuous terrain changes. The height difference between blocks is set to... The ground friction coefficient is 0.6, and the splicing method is random splicing. During the simulation test, speed commands are sent to the MPC control module via the keyboard, and the speed magnitude remains constant. Dynamic simulations were conducted on flat ground, uneven surfaces, and discrete glass terrain to verify the performance. The results show that the humanoid robot can stably traverse these terrains and smoothly cross terrain boundaries. The test results are as follows: Figure 6 As shown.

[0070] The humanoid robot motion control method based on ground contact detection proposed in this invention can ensure that the humanoid robot maintains stable walking under complex terrain conditions, verifying the effectiveness and robustness of the control method.

[0071] It will be understood by those skilled in the art that the above descriptions are merely preferred examples of the invention and are not intended to limit the invention. Although the invention has been described in detail with reference to the foregoing examples, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the invention should be included within the scope of protection of the invention.

Claims

1. A motion control method for a humanoid robot based on ground contact detection, characterized in that, Applied to bipedal humanoid robots with a floating base structure, the method includes the following steps: (1) Constructing a humanoid robot model predictive controller based on the center of mass dynamics model: Construct the floating base joint space of the humanoid robot, introduce the center of mass momentum matrix under the framework of the center of mass dynamics model, establish the relationship between the center of mass momentum vector and the generalized velocity of the humanoid robot, and realize the center of mass dynamics modeling; define the state vector and control input of the model predictive control, and complete the definition of the humanoid robot model predictive control problem based on the center of mass dynamics model. (2) Foot contact state estimation based on gait phase and plantar force probability fusion: Construct a corresponding contact probability model for gait phase information and normal plantar force information, and give the final foot contact probability based on Bayesian probability fusion; (3) Adaptive adjustment of gait phase based on ground contact detection results: Construct an adaptive adjustment method for gait phase based on ground contact detection results, and reposition the current gait execution phase for the cases of early ground contact of the swing phase and delayed ground contact of the support phase respectively; (4) Whole-body torque control method based on inverse dynamics quadratic programming problem: After obtaining the output of the model predictive controller, the reference trajectory is converted into the joint driving torque command of the humanoid robot through whole-body control; considering dynamic constraints, the whole-body control is constructed as an inverse dynamics quadratic programming problem, and the joint driving torque is obtained according to the inverse dynamics equation to optimize the tracking effect of the upper model predictive controller output; (5) Simulate and verify the motion performance of the humanoid robot on the dynamic simulation platform.

2. The humanoid robot motion control method based on ground contact detection according to claim 1, characterized in that, Step (1) includes the following sub-steps: (1.1) Construct the floating base joint space of the humanoid robot, establish the floating base dynamic model of the humanoid robot based on the description file of the humanoid robot, introduce the center of mass momentum matrix, establish the relationship between the center of mass momentum vector and the generalized velocity of the humanoid robot, and realize the center of mass dynamic modeling. (1.2) Define the state vector and control input of the model predictive control, wherein the state vector consists of the generalized joint angle and the joint angular velocity, and the control input consists of the joint acceleration and the generalized force of the foot. Further obtain the system state equation; define the model predictive control objective function according to the control objective and select the required constraints.

3. The humanoid robot motion control method based on ground contact detection according to claim 1, characterized in that, Step (2) includes the following sub-steps: (2.1) Construct a foot contact probability model based on gait phase, and define contact probability models for swing phase and support phase respectively. The contact probability of swing phase decreases to increase, and the contact probability of support phase increases to decrease. The function is continuous at the gait switching point. (2.2) Construct a foot contact probability model based on normal plantar force, and adopt a monotonically increasing contact probability model; use joint torque and Jacobian matrix to solve the plantar force through inverse dynamics; (2.3) Calculate the final foot contact probability based on Bayesian probability fusion.

4. The humanoid robot motion control method based on ground contact detection according to claim 1, characterized in that, Step (3) includes the following sub-steps: (3.1) Define the complete gait cycle, gait switching time, and current gait phase based on the results of the upper-level gait planner; (3.2) Based on the ground contact probability of the left and right feet obtained in step (2), determine the ground contact determination result of each foot and form the corresponding support phase set; (3.3) When it is detected that the swing foot touches the ground before the preset phase ends, the current gait phase is switched to the next support phase in advance to achieve phase forward adjustment in the case of early ground contact; (3.4) When it is detected that the swing foot has not touched the ground at the end of the preset phase, the current gait phase is adjusted to the previous swing phase to realize the swing phase delay adjustment in the case of delayed ground contact; (3.5) After completing the gait phase adjustment, update the current set of feet that are touching the ground and feed it back to the model prediction controller for subsequent control calculations.

5. The humanoid robot motion control method based on ground contact detection according to claim 1, characterized in that, Step (4) includes the following sub-steps: (4.1) Based on the joint acceleration reference and the foot contact force reference, a quadratic programming inverse dynamics problem is constructed with the goal of minimizing the joint acceleration tracking error and the contact force deviation. The generalized acceleration error, the foot contact force error and the joint driving torque are used as the decision variables of the inverse dynamics quadratic programming problem. (4.2) Based on the floating basis dynamic equation of the humanoid robot, construct the equality constraint, and combine it with the current support state set to construct the foot non-slip constraint. Use the dynamic constraint and the foot non-slip constraint together as the equality constraint conditions of the quadratic programming problem. (4.3) Construct inequality constraint conditions that include friction cone constraints and joint driving torque constraints to ensure that the foot contact force meets the friction stability requirements and the joint driving torque meets the physical limitations of the actuator. (4.4) Solve the inverse dynamics problem in real time within the control cycle to obtain the optimal joint driving torque, and send the joint driving torque to each drive joint of the humanoid robot for execution. The joint PD control controls the joint motor movement at a frequency of 1000Hz.

6. The humanoid robot motion control method based on ground contact detection according to claim 1, characterized in that, Step (5) includes the following sub-steps: (5.1) Import the unified robot description file of the humanoid robot into the dynamic simulation platform and establish a complete dynamic simulation model including robot structural parameters, mass parameters and joint constraint parameters; (5.2) Construct various terrain environment models in the dynamic simulation platform. The terrain environment includes flat terrain, non-flat terrain with random undulations and low friction discrete terrain, and set the corresponding terrain undulation parameters and friction coefficient parameters. (5.3) Run the motion control method of steps (2) to (4) in the terrain environment, analyze the walking stability and gait adjustment response of the robot during the simulation, and verify the stability and robustness of the motion control method under different terrain conditions.

Images