Humanoid robot stability enhancement method based on multi-contact dcm-mpc and whole-body motion control

Abstract

This invention discloses a method for enhancing the stability of humanoid robots based on multi-contact DCM-MPC and whole-body motion control, belonging to the field of robot motion control technology. Step 1: Multi-contact DCM dynamic modeling of the humanoid robot's foot and hand contact interactions; Step 2: Design of contact strategies based on the dynamic modeling in Step 1; Step 3: Design of a multi-contact model predictive controller based on the contact strategies in Step 2; Step 4: Design of a multi-contact whole-body motion controller based on the contact strategies in Step 2. This invention aims to solve three key problems restricting the adaptability of humanoid robots to complex environments: limited stability margin in complex dynamic tasks due to lower limb-dominated stability control methods; insufficient integration of hand contact forces into whole-body dynamic stability prediction and control; and difficulty in actively planning environmental effects in multi-contact control, coupled with the high computational complexity of whole-body dynamic models that hinders real-time performance.

Description

Technical Field

[0001] This invention belongs to the field of robot motion control technology, and specifically relates to a method for enhancing the stability of humanoid robots based on multi-contact DCM-MPC and whole-body motion control. Background Technology

[0002] Humanoid robots, with their human-like movement patterns and environmental adaptability, have broad application prospects in complex environment operations, intelligent manufacturing, and service robots. However, humanoid robots typically possess characteristics such as floating bases, multiple degrees of freedom, and underactuation, making them susceptible to external disturbances, terrain changes, and variations in support conditions during actual movement. This can cause the robot to experience center of mass shift, posture changes, or even loss of stability. These factors significantly limit the ability of humanoid robots to move from the laboratory to real-world scenarios. Therefore, improving the dynamic stability of humanoid robots in complex environments is a crucial technical challenge for realizing their practical applications.

[0003] Currently, the dynamic stability control of humanoid robots mainly relies on leg movement regulation, including ankle, hip, and stride strategies. These methods achieve stable walking by adjusting the center of mass movement, foot position, and contact force, and have achieved significant results in regular ground environments. However, their stability regulation capability is primarily limited by the foot contact area. When the robot is subjected to large external disturbances, or in environments with limited foot adjustment space, such as stairs or narrow passages, relying solely on leg regulation is insufficient to provide adequate stability margin. Furthermore, during the single-leg support phase, the robot is more prone to lateral posture deviation and dynamic instability due to the reduced support area.

[0004] To improve the dynamic stability of robots, researchers have proposed a dynamic stability control method based on capture points (CP) and divergent motion components. Compared to traditional static stability criteria, dynamic stability control (DCM) can describe the unstable divergent components in the robot's center-of-mass motion and adjust the robot's dynamic equilibrium state through virtual repulsion points (VRPs). However, existing motion planning methods based on DCM are generally still mainly based on foot contact constraints, and their stability adjustment capabilities are limited by the foot's range of motion and support area, thus restricting their further application in complex environments.

[0005] On the other hand, when faced with external disturbances or complex terrain, humans do not rely solely on their legs to maintain balance. Instead, they utilize upper limb movements and various forms of contact with the environment to assist in regulating body stability. Inspired by human movement mechanisms, some recent studies have begun to explore multi-contact motion control methods for humanoid robots. By introducing contact between the hands or other body parts and the environment, additional constraints and assistive forces are provided to the robot.

[0006] However, existing multi-contact control methods mainly focus on contact state planning, contact force distribution, and local motion coordination. They still fall short in integrating multi-contact constraints into low-dimensional dynamic model prediction and further combining them with whole-body motion control to achieve enhanced stable motion. In addition, while some existing whole-body control methods can coordinate the robot's whole-body motion under the conditions of satisfying robot dynamic constraints, contact constraints, and joint constraints, they are usually used as low-level motion execution methods and lack deep integration with high-level dynamic stability planning.

[0007] Furthermore, existing humanoid robot stability control methods mainly revolve around the motion of the center of mass, the position of the foot, and the contact force of the foot. Their control capabilities are limited by the range of leg movement and the constraints of foot contact. As robot motion tasks evolve from walking on regular ground to more complex dynamic motion and environmental interaction, it is difficult to further improve the stability margin of the system by relying solely on lower limb movement adjustment. Existing motion planning methods mainly focus on dynamic planning under foot support conditions, and do not make full use of the additional contact information between the robot and the environment. How to further integrate hand contact force into the dynamic stability prediction and control process while maintaining the feasibility of whole-body dynamics still needs to be explored in depth. Existing multi-contact control methods focus more on contact execution and whole-body coordination, lacking methods that actively integrate environmental contact effects into dynamic stability planning. In addition, methods based on whole-body dynamics models usually have high computational complexity, making it difficult to satisfy complex contact constraints while ensuring real-time performance.

[0008] Therefore, it is necessary to propose a multi-contact dynamic stability control method for humanoid robots that integrates environmental contact constraints. By establishing a dynamic model that includes environmental contact effects, efficient stability prediction can be achieved using a low-dimensional dynamic model. At the same time, the external forces generated by hand contact are integrated into the robot's motion planning and control process, thereby improving the robot's dynamic stability under complex environments and external disturbances. Summary of the Invention

[0009] This invention provides a method for enhancing the stability of humanoid robots based on multi-contact DCM-MPC and whole-body motion control. It aims to solve three key problems that restrict the adaptability of humanoid robots to complex environments: the limited stability margin of existing lower limb-dominated stability control methods in complex dynamic tasks, the insufficient integration of hand contact forces into whole-body dynamic stability prediction and control, and the difficulty in actively planning environmental effects in multi-contact control and the high computational complexity of whole-body dynamic models, which makes it difficult to meet real-time requirements.

[0010] This invention is achieved through the following technical solution: A method for enhancing the stability of a humanoid robot based on multi-contact DCM-MPC and whole-body motion control is proposed. The method first plans the footing point and hand contact strategy according to the environmental structure information, which, together with the robot's current state, serve as the input to MPC. MPC predicts the robot's center of mass and DCM state in a finite time domain and optimizes it using VRP as the control variable. Once the robot establishes hand contact with the environment, the hand contact force can change the center of mass dynamics and expand the feasible range of VRP. By solving the MPC optimization problem, the achievable VRP under hand contact constraints is obtained, and the desired DCM and center of mass trajectory are further calculated. Subsequently, the optimized centroid reference trajectory and the predefined end-effector motion target input trajectory generator are used to calculate the complete reference trajectory required for robot motion through polynomial interpolation. The generated Cartesian space reference trajectory is input into the inverse kinematics (IK) solver and the multi-contact motion (WBC) solver, respectively. The IK solver generates joint position and velocity references based on the desired CoM and end-effector motion. At the same time, the MC-WBC combines the desired motion task with contact constraints, friction constraints, and joint motion restrictions to uniformly optimize the robot's whole-body dynamics, achieve coordinated distribution of contact forces between the feet and hands, and finally calculate the desired joint torque.

[0011] Furthermore, the method specifically includes the following steps: Step 1: Multi-contact DCM dynamic modeling of the contact between the humanoid robot's feet and hands; Step 2: Design the contact strategy based on the dynamic modeling in Step 1; Step 3: Design a multi-contact model predictive controller based on the contact strategy in Step 2; Step 4: Design a multi-contact whole-body motion controller based on the contact strategy in Step 2.

[0012] Furthermore, step 1 specifically involves providing the center of mass dynamics equation for a robot with multiple environmental contact points; VRP is a virtual reference point representing the point where the center of mass is subjected to a virtual repulsive force, and substituting the center of mass dynamics equation into VRP yields the VRP dynamics equation. For traditional bipedal walking robots, the contact force mainly comes from the foot force, while the introduction of hand contact increases the hand contact force; substituting this into the VRP dynamic equation yields the equivalent VRP point in the case of multiple contact. DCM originates from the linear inverted pendulum model, which decomposes the second-order center-of-mass dynamics into a stable first-order convergent component and an unstable first-order divergent component, representing the divergent trend of robot motion. Based on the divergent trend of motion, the dynamic equation of CoM is obtained, and after differentiation and simplification, the dynamic equation of DCM under multi-contact conditions is obtained. When a robot walks on uneven ground or is disturbed by external forces, the natural frequency of the DCM changes, which can be extended into a time-varying DCM.

[0013] Furthermore, step 2 specifically involves: given a known elevation map of a multi-contact scenario with clear structural information; manually planning the robot's required landing points and hand contact point sequence in advance; defining the contact state variables of the soles and palms; adjusting the hand assistance based on stability margin; and setting... As a safety threshold, when When the robot can maintain stability solely through foot contact, the hand contact force is canceled or kept at a low level. Conversely, when the system has insufficient stability margin, the controller activates the hand assistance function, and the hand contact affects the center of mass movement by influencing the VRP offset.

[0014] Furthermore, step 3 specifically involves obtaining the continuous-time state-space equation based on the multi-contact DCM dynamic equation, decoupling the XYZ directions and writing them in matrix form, and then solving the QP quadratic programming problem based on the discretized state-space equation, according to the robot's current CoM position. and DCM location Calculate the motion state of the robot's center of mass that satisfies the dynamic equation after N discrete time steps.

[0015] Furthermore, the QP optimization problem should satisfy state equation constraints, VRP constraints, and extended boundary constraints.

[0016] Furthermore, step 4 specifically involves the following: the humanoid robot is a standard floating-base robot system, and its generalized position vector includes the floating base and the driven joints; when the robot comes into contact with the environment, its complete whole-body dynamics are described by the Eulerian-Lagrange form; based on the dynamic model, the MC-WBC is completely formulated into a quadratic programming problem, and a weighted task form is used to coordinate different task objectives, with various hard constraints having a higher constraint priority than the task tracking objective.

[0017] Furthermore, the various hard constraints include dynamic constraints, contact constraints, friction cone constraints, joint motion constraints, joint torque constraints, and contact force constraints. Through these constraints, the MC-WBC solver can simultaneously optimize the robot's joint torque, whole-body motion acceleration, and multi-contact forces, enabling the centroid trajectory, DCM state, and hand-assisted strategy generated by the planning layer to be stably executed under whole-body dynamic constraints.

[0018] A stability enhancement system for humanoid robots based on multi-contact DCM-MPC and whole-body motion control is disclosed. The system utilizes the aforementioned stability enhancement method for humanoid robots based on multi-contact DCM-MPC and whole-body motion control. The system comprises: Modeling module: Multi-contact dynamics modeling; Strategy Design Module: Design contact strategies; Controller design module: Design of multi-contact model predictive controller and design of multi-contact whole-body motion controller.

[0019] A method for enhancing the stability of humanoid robots based on multi-contact DCM-MPC and whole-body motion control, as described above, is applied to humanoid robots in complex terrain, external disturbances, and limited support conditions, achieving stable movement through multi-contact motion planning and whole-body coordinated control.

[0020] The beneficial effects of this invention are: Inspired by the biological behavior of humans utilizing environmental contact to assist movement during walking, this invention establishes a dynamic model of the Dynamic Center of Mass (DCM) that includes hand-environmental contact. Hand contact forces are introduced into the center of mass dynamics, and their impact on the center of mass motion is described using an equivalent VRP offset. Based on this, the feasible region of VRP is expanded according to hand contact capabilities, enabling environmental contact to participate in the regulation of the robot's dynamic stability.

[0021] This invention designs a hand contact triggering strategy based on gait and stability margin. The hand contact state is determined according to the robot's motion stage and the current stability margin. When the foot support capacity is insufficient, environmental contact is used to provide additional stability adjustment. When the palm does not establish environmental contact, the multi-contact planning framework degenerates into the traditional DCM-MPC model under foot support, avoiding unnecessary hand actions in a stable state.

[0022] This invention constructs a control method combining multi-contact DCM-MPC and MC-WBC, explicitly integrating hand contact forces into the planning and control process. MPC optimizes the VRP trajectory and hand assistance based on the robot's current motion state and multi-contact constraints; MC-WBC achieves coordinated control of whole-body motion and multi-contact forces while satisfying robot dynamics, contact, friction, joint, and torque constraints.

[0023] This invention incorporates upper limb environmental contact, integrating the influence of hand contact forces on the robot's center of mass dynamics into the dynamics of the dynamic center of mass (DCM), and expanding the feasible region of the dynamic field of motion planning (VRP), thereby providing the robot with additional stability adjustment capabilities. Based on this, a multi-contact whole-body control method is further designed, achieving coordinated control of the robot's whole-body motion and contact forces while considering whole-body dynamics and contact constraints. Simulation and experimental results based on the Yushu G1 humanoid robot show that hand contact can effectively reduce the robot's center of mass and body posture deviations, expand the stability margin boundary, and improve motion stability during stair climbing, while reducing the adjustment load on the lower limb joints. Experimental results verify the effectiveness and applicability of the proposed multi-contact motion planning and control framework. Attached Figure Description

[0024] Figure 1 This is a system block diagram of the present invention.

[0025] Figure 2 This is the contact phase diagram of the present invention.

[0026] Figure 3 This is the VRP extension diagram of the present invention.

[0027] Figure 4 This is the stability margin boundary radar diagram of the present invention.

[0028] Figure 5 These are screenshots and data curves from the static support anti-disturbance experiment of the present invention, wherein (a) is a screenshot of the experiment without hand support, (b) is a screenshot of the experiment under the condition of hand support, (c) is a comparison of the centroid trajectory curve in the Y direction during the experiment, (d) is a comparison of the torso posture roll angle curve during the experiment, (e) is a comparison of the left ankle joint roll moment curve during the experiment, (f) is the hand contact force curve in the Y direction during the experiment, and (g) is the VRP expansion curve in the Y direction during the experiment.

[0029] Figure 6 These are screenshots and data curves from the stair climbing experiment of the present invention, wherein (a) is a screenshot of the experiment without hand support, (b) is a screenshot of the experiment with hand support, (c) is a comparison of the center of mass trajectory curves during the experiment, (d) is a comparison of the torso posture curves during the experiment, (e) is the vertical foot force curve under hand support conditions, (f) is the hand contact force curve in the Y direction during the experiment, and (g) is the VRP expansion curve in the Y direction during the experiment. Detailed Implementation

[0030] 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.

[0031] Implementation Method 1 This embodiment provides a method for enhancing the stability of a humanoid robot based on multi-contact DCM-MPC and whole-body motion control. The proposed humanoid robot multi-contact motion control framework is as follows: Figure 1 As shown.

[0032] The control framework consists of a multi-contact motion planner, a reference trajectory generator, an inverse kinematics solver, a multi-contact whole-body controller, and a state estimator.

[0033] First, the landing point and hand contact strategy are planned based on the environmental structure information, and together with the robot's current state, serve as inputs to the MPC (Multi-Contact Kinematics) algorithm. The MPC predicts the robot's center of mass and direct current mechanism (DCM) states within a finite time domain and optimizes them using variable repetition propagation (VRP) as the control variable. Once the robot establishes hand contact with the environment, the hand contact force can alter the center of mass dynamics and expand the feasible range of the VRP. The achievable VRP under hand contact constraints is obtained by solving the MPC optimization problem, and the desired DCM and center of mass trajectory are further calculated. Subsequently, the optimized center of mass reference trajectory is input to a predefined end-effector motion target trajectory generator, and polynomial interpolation is used to calculate the complete reference trajectory required for robot motion. The generated Cartesian space reference trajectory is input to the inverse kinematics (IK) solver and the multi-contact motion bridge (MC-WBC), respectively. The IK solver generates joint position and velocity references based on the desired CoM and end-effector motion. Simultaneously, the MC-WBC, combining the desired motion task with contact constraints, friction constraints, and joint motion limitations, performs unified optimization of the robot's whole-body dynamics, achieving coordinated distribution of foot and hand contact forces, and finally calculating the desired joint torque.

[0034] Includes the following steps: Step 1: Multi-contact dynamics modeling.

[0035] This method establishes a multi-contact DCM dynamic model that integrates foot and hand contact interactions.

[0036] For a robot with multiple environmental contact points, its center-of-mass dynamics can be expressed as: (1) in The location of the center of mass. For robot quality, It is the acceleration due to gravity. Indicates the number of contact points. Indicates the first Contact force at each contact point.

[0037] VRP is a virtual reference point, representing the point at which the center of mass experiences a virtual repulsive force, defined as: (2) in It is the natural frequency of a linear inverted pendulum, given the nominal height of its center of mass. .

[0038] Substituting the center-of-mass dynamics into equation (2), we get: (3) For traditional bipedal walking robots, the contact force mainly comes from the force of the soles of the feet. The introduction of hand contact increased

[0039] (4) in , These are the contact forces at the feet and hands, respectively. Substituting them into formula (3) yields the equivalent VRP point for the multi-contact case. : (5) The VRP generated by foot contact is: (6) Define the offset generated by hand contact on VRP as ,but: (7) DCM originates from the linear inverted pendulum model, decomposing the second-order center-of-mass dynamics into a stable first-order convergent component and an unstable first-order divergent component, representing the divergent tendency of the robot's motion, defined as: (8) in Indicates DCM point, Let be the velocity of the center of mass. Rearranging the above equation yields the dynamic equation for CoM: (9) Differentiating equation (8) yields the DCM differential equation: (10) Substituting the multi-contact VRP equation and the CoM kinetic equation into formula (10), we obtain the DCM kinetic equation for the multi-contact case: (11) When a robot walks on uneven ground or is disturbed by external forces, the natural frequency of the DCM changes, which can be extended into a time-varying DCM: (12) in It is the time-varying DCM natural frequency.

[0040] The above formula derivation shows that during multi-contact motion, DCM dynamics are affected not only by the foot VRP but also by the VRP offset caused by hand contact. Compared to the traditional foot-support model, the multi-contact model adds additional contact points introduced by environmental interaction, enabling the robot to actively utilize environmental contact to expand its dynamic stability range.

[0041] Step 2: Contact strategy design.

[0042] In real-world environments such as stair climbing or narrow passages, the space available for robot support is often limited by the environmental structure. Therefore, this method does not optimize for the contact point search problem in general environments, but rather for multi-contact scenarios with well-defined structural information, pre-setting the robot's foothold and hand contact positions.

[0043] Given the environmental elevation map, manually plan the required footholds and hand contact point sequence: (13) in For the predicted first The position of the foot landing point For the first The point of contact with the palm.

[0044] Define the contact state variables of the sole of the foot and the palm of the hand as follows: (14) in This indicates that the palm is in a non-contact state. This indicates that the robot's hand has established effective contact with the environment. This indicates that the robot is in a bipedal position. -1 and 1 represent support on the left foot and support on the right foot, respectively.

[0045] Taking grabbing a handrail and climbing stairs as an example, the contact phase during the robot's movement is as follows: Figure 2 As shown in the figure, the robot's hand does not need to remain in contact with the handrail throughout the entire movement. During the bipedal support phase, the robot has a larger support area and a higher stability margin. At this time, the arm completes the swinging motion, adjusting the hand to the next target contact point according to the preset contact position. During the monopedal support phase, the robot is allowed to establish hand contact, and the system stability is enhanced by introducing additional contact constraints and auxiliary forces.

[0046] To avoid the robot applying excessive hand force during multiple contact phases, this method adjusts the hand assistance based on a stability margin. The stability margin is defined as follows: (15) in This indicates the feasible region of VRP as defined by foot contact constraints. Indicates the boundary of the feasible region. This represents the minimum distance between the current DCM and the foot support stability boundary.

[0047] set up As a safety threshold, when When the robot can maintain stability solely through foot contact, the hand contact force is either eliminated or kept at a low level. Conversely, if the system's stability margin is insufficient, the controller activates the hand assistance function.

[0048] Hand contact affects the motion of the center of mass by influencing the VRP offset. The reference value can be calculated using the following formula: (16) in and Indicate the expected and actual DCM locations. and Represents the proportional and differential feedback coefficients. Specific VRP expansion amount. It is obtained through further optimization calculations in MPC.

[0049] In summary, this method employs a hand contact triggering strategy combining gait and stability margin, ensuring the robot maintains at least two effective environmental contact constraints throughout its movement. This enhances the robot's ability to resist external disturbances and maintain dynamic balance. When the hand does not establish environmental contact, the multi-contact motion planning framework degenerates into the traditional foot-supported DCM-MPC model, eliminating the need to consider hand contact-related dynamic constraints and optimization variables. Furthermore, MC-WBC eliminates the need to handle hand contact-related constraints and contact force optimization calculations, reducing control computational complexity and improving system real-time performance.

[0050] Step 3: Design of a multi-contact model predictive controller.

[0051] Based on the multi-contact DCM dynamic equation formula (12), the continuous-time state-space equation can be obtained. This method decouples the XYZ directions and writes them in matrix form. Taking the X direction as an example: (17) in For state variables, To control variables, Here is the state transition matrix. This is the control matrix. The state-space equations in the Y and Z directions have similar forms. This decoupling method is used to reduce computational complexity, while the coupling effects are handled by the underlying MC-WBC.

[0052] make Where is the discrete sampling time for MPC, and N is the prediction window. During the MPC sampling time interval The inner part is piecewise constant, written below as... : (18) The above continuous-time state-space equations are discretized based on the zero-order preservation method: (19) Based on the discretized state-space equations described above, by solving the QP quadratic programming problem, the current position of the robot's centroid can be determined. and DCM location Calculate the motion state of the robot's center of mass that satisfies the dynamic equation after N discrete time steps.

[0053] The QP optimization problem (i.e., equation (27)) satisfies the state equation constraints, VRP constraints, and extended boundary constraints: 1) State equation constraints At time k: (20) Write it in matrix form: (twenty one) in Let k be the set of state variables predicted for the next N steps. For the set of control variables for the next N steps, Let k be the state of the center of mass at time k. and The matrix is ​​formed by stacking the discrete state transition matrix and control matrix at each step.

[0054] 2) VRP constraints In traditional bipedal movement planning, the VRP (Vibration Restriction Plane) is typically constrained to the feasible range corresponding to the foot contact area, i.e. .

[0055] Once the robot establishes hand contact with the environment, the feasible region of the VRP (Virtual Resource Planning) is constrained by the hand contact force. Therefore, the feasible region of the VRP under multi-contact conditions can be represented as: (twenty two) It should be noted that, unlike foot contact, hand contact does not provide significant vertical support force, but mainly alters the robot's center of mass motion through horizontal contact force. Therefore, hand contact does not directly create a new ground support area, but rather changes the dynamic constraints of the center of mass by providing additional horizontal contact force, allowing the feasible VRP region to expand along the direction of the contact force, thus forming a multi-contact stable region, such as... Figure 3 As shown.

[0056] To maintain the linearity of the constraint, the constraint region can be simplified to a rectangle similar in shape to the sole of the foot. The original size range of the rectangular constraint region is used... and It is indicated that the constraint center is composed of This indicates that hand contact extends the VRP constraint area. (twenty three) in and The VRP constraint region expansion is represented by the following formula: (twenty four) The extended X-direction VRP constraint is as follows: (25) The VRP region expansion caused by hand contact is not infinite; the size of the expanded region is related to the contact force that the end of the arm can provide. Therefore, an expansion boundary constraint needs to be added to the QP. (26) In this method, the ZMP vertical coordinate is always consistent with the landing point height. This is determined by the landing point sequence based on the terrain and cannot be modified by MPC. Therefore, the VRP constrained area is planar.

[0057] Based on the above derivation, the MPC optimization problem is constructed as follows: (27) The cost function consists of three parts: state tracking, VRP offset, and control input change rate. The state weight matrix is... To control the weight matrix, This is the VRP offset weight matrix. The state tracking term is used to reduce the error between the predicted state and the reference trajectory, ensuring that the robot's center of mass and DCM move along the desired trajectory. The VRP offset term is used to limit the VRP expansion caused by hand contact, minimizing hand assistance requirements while satisfying state tracking and stability constraints, thereby avoiding unnecessary environmental contact forces. The control input rate of change term is used to suppress drastic changes in VRP between adjacent time steps, improving the continuity and smoothness of the optimization results.

[0058] Solving this optimization problem allows us to optimize the VRP position in the X direction for the next N steps, obtaining a set of VRP positions that minimize the cost function and satisfy the constraints. Based on the dynamic equations, the desired DCM trajectory can be calculated to guide the center of mass motion. Similarly, similar optimization problems can be constructed in the Y and Z directions.

[0059] VRP offset obtained from MPC optimization Corresponding to the hand assistance required to satisfy the current VRP constraints, its physical meaning is further mapped to the desired hand contact force through multi-contact dynamics, and realized in MC-WBC.

[0060] Step 4: Design of a multi-touch full-body motion controller.

[0061] This method combines multi-contact dynamics to design a task space-based multi-contact whole-body motion controller.

[0062] Humanoid robots are standard floating-base robotic systems, and their generalized position vector consists of two parts: the floating base and the driven joints. (28) in For generalized coordinate vectors, Let the floating base be the position and orientation in the world coordinate system. For robots The generalized position of each driven joint. When the robot comes into contact with the environment, its complete whole-body dynamics can be described in Eulerian-Lagrange form, and its dynamic equations are: (29) in The inertia matrix, To account for the Coriolis effect and the centrifugal effect in the matrix, For gravity, Choose the matrix for driving the robot's degrees of freedom. This is the joint torque vector. The stacked matrix is ​​the Jacobian matrix of the foot contact. This is a stacked matrix of Jacobian matrices representing hand contact. For plantar contact force, For hand contact force, and The number of contact points between the feet and hands and the ground.

[0063] Based on the dynamic model, MC-WBC is fully formulated as a quadratic programming problem. A weighted task approach is used to coordinate different task objectives, with various hard constraints having higher priority than the task tracking objective. The task priorities in MC-WBC are shown in Table 1.

[0064] Table 1 MC-WBC Task Priorities Based on the above description, the detailed structure of the MC-WBC optimization problem is as follows: (30) Decision variables include generalized acceleration Drive joint torque and end contact wrenches and .

[0065] In the cost function and constraints, the terms related to hand contact are preceded by a definition of hand contact state, which specifies the state when the palm is not in contact with the environment. At this point, the item is removed.

[0066] The objective function includes the Cartesian trajectory tracking cost, the generalized acceleration cost, and the contact wrench cost.

[0067] 1) Trajectory tracking cost: The total number of tasks is set, including center of mass trajectory tasks, torso posture tasks, foot trajectory tasks, and arm trajectory tasks. For the task Jacobian matrix, For trajectory tracking tasks The weight matrix, The desired acceleration for the target task is calculated using the following PD controller: (31) in , and The first The expected position, velocity, and acceleration for each task. and The first The actual location and speed of each task and The first The proportion of each task and the differential feedback coefficient.

[0068] 2) Generalized acceleration cost: This is the weight matrix for the acceleration optimization task. In multi-contact states, due to increased contact constraints and contact force distribution, the system exhibits higher dynamic redundancy. This term effectively suppresses high-frequency acceleration changes, resulting in smoother optimization results.

[0069] 3) Cost of contacting the wrench: and These are the weight matrices for optimizing the foot and hand contact wrench tasks, respectively. To ensure the whole-body controller can achieve the desired contact assistance effect, the desired contact wrench is introduced into the contact wrench optimization term. and For reference only.

[0070] For foot contact, the desired contact force is adjusted based on feedback from the center-of-mass position error: (32) Meanwhile, to improve fuselage attitude stability, a desired restoring torque is generated based on the fuselage attitude error: (33) For hand contact, the desired contact force is mainly determined by the VRP offset obtained from MPC optimization. According to the multi-contact dynamics model, the hand auxiliary force and the VRP offset satisfy formula (7), therefore the desired hand contact force is: (34) in The VRP offset is obtained for MPC optimization. For the palm contact torque, feedback adjustment is made based on the palm posture error. (35) The constraints set in the optimization problem are as follows: 1) Dynamic constraints: Formula (29) is used as the dynamic equation constraint of the optimization problem. This constraint ensures that the joint torque, robot acceleration and contact force obtained by optimization satisfy the complete whole-body dynamic relationship, so that the robot can maintain dynamic balance in the multi-contact state.

[0071] 2) Contact constraints: During robot walking, the acceleration of the end cap that maintains contact with the environment is 0 relative to the environment. Contact constraints ensure stable contact between the foot and the ground and between the hand and the environment, preventing slippage or disengagement due to movement of the contact point.

[0072] 3) Friction Cone Constraint: To ensure that the contact force between the robot and the environment meets frictional limits, the contact forces at both the foot and hand ends must satisfy the friction cone constraint. Specifically, and Let represent the linearized friction cone matrices corresponding to foot and hand contacts, respectively. For foot contact, the friction constraint mainly restricts the relationship between tangential and normal forces to prevent slippage of the robot's foot. For hand contact, since the handrail mainly provides horizontal constraint forces, this constraint limits the range of tangential forces on the palm, ensuring contact stability during movement.

[0073] 4) Joint motion constraints: To prevent robot joint motion from exceeding the limitations of the mechanical structure, the position, speed, and acceleration of the driven joints are constrained to ensure that the robot meets the physical limits of the joints during movement and to improve the safety of the control process.

[0074] 5) Joint torque constraint: To prevent the robot's joint torque from exceeding the limit, the driving joint torque is constrained. and These represent the minimum and maximum allowable output torque for the robot's driven joints, respectively.

[0075] 6) Contact Force Constraints: To achieve stable control under multi-contact conditions, further constraints are imposed on the foot and hand contact forces. The foot contact force range is determined by the robot's support capacity and friction conditions, while the hand contact force range is determined by the mechanical structure's load-bearing capacity and environmental interaction requirements. Simultaneously, the desired hand contact force is calculated from the VRP offset output by the MPC. Therefore, the MC-WBC actively generates auxiliary contact forces while satisfying physical constraints, thereby enhancing the robot's stability through the upper limbs.

[0076] With the above constraints, the MC-WBC solver can simultaneously optimize robot joint torques, whole-body motion acceleration, and multi-contact forces, enabling the centroid trajectory, DCM state, and hand-assisted strategy generated by the planning layer to be stably executed under whole-body dynamic constraints.

[0077] Implementation Method 2 This embodiment provides a humanoid robot stability enhancement system based on multi-contact DCM-MPC and whole-body motion control. The system uses the humanoid robot stability enhancement method based on multi-contact DCM-MPC and whole-body motion control as described in Embodiment 1. The system includes: Modeling module: Multi-contact dynamics modeling; Strategy Design Module: Design contact strategies; Controller design module: Design of multi-contact model predictive controller and design of multi-contact whole-body motion controller.

[0078] Implementation Method 3 This embodiment provides a method for enhancing the stability of a humanoid robot based on multi-contact DCM-MPC and whole-body motion control, as described in Embodiment 1. The method is applied to humanoid robots in complex terrain, external disturbances, and limited support conditions, and achieves stable movement through multi-contact motion planning and whole-body coordinated control.

Claims

1. A method for enhancing the stability of a humanoid robot based on multi-contact DCM-MPC and whole-body motion control, characterized in that, The method first plans the landing point and hand contact strategy based on the environmental structure information, which, together with the robot's current state, serve as the input to MPC; MPC predicts the robot's center of mass and DCM state within a finite time domain, and optimizes it using VRP as the control variable; Once the robot establishes hand contact with the environment, the hand contact force can change the center of mass dynamics and expand the feasible range of VRP. By solving the MPC optimization problem, the achievable VRP under hand contact constraints is obtained, and the desired DCM and center of mass trajectory are further calculated. Subsequently, the optimized centroid reference trajectory and the predefined end-effector motion target input trajectory generator are used to calculate the complete reference trajectory required for robot motion through polynomial interpolation. The generated Cartesian space reference trajectory is input into the inverse kinematics (IK) solver and the multi-contact motion (WBC) solver, respectively. The IK solver generates joint position and velocity references based on the desired CoM and end-effector motion. At the same time, the MC-WBC combines the desired motion task with contact constraints, friction constraints, and joint motion restrictions to uniformly optimize the robot's whole-body dynamics, achieve coordinated distribution of contact forces between the feet and hands, and finally calculate the desired joint torque.

2. The method according to claim 1, characterized in that, The method specifically includes the following steps: Step 1: Multi-contact DCM dynamic modeling of the contact between the humanoid robot's feet and hands; Step 2: Design the contact strategy based on the dynamic modeling in Step 1; Step 3: Design a multi-contact model predictive controller based on the contact strategy in Step 2; Step 4: Design a multi-contact whole-body motion controller based on the contact strategy in Step 2.

3. The method according to claim 2, characterized in that, Specifically, step 1 involves providing the center of mass dynamics equation for a robot with multiple environmental contact points; VRP is a virtual reference point representing the point where the center of mass is subjected to a virtual repulsive force, and substituting the center of mass dynamics equation into VRP yields the VRP dynamics equation. For traditional bipedal walking robots, the contact force mainly comes from the force of the soles of the feet, while the introduction of hand contact increases the hand contact force; Substituting this into the VRP dynamics equation yields the equivalent VRP point in the multi-contact case; DCM originates from the linear inverted pendulum model, which decomposes the second-order center-of-mass dynamics into a stable first-order convergent component and an unstable first-order divergent component, representing the divergent trend of robot motion. Based on the divergent trend of motion, the dynamic equation of CoM is obtained, and after differentiation and simplification, the dynamic equation of DCM under multi-contact conditions is obtained. When a robot walks on uneven ground or is disturbed by external forces, the natural frequency of the DCM changes, which can be extended into a time-varying DCM.

4. The method according to claim 2, characterized in that, Step 2 specifically involves: given a well-defined elevation map of a multi-contact scenario with clear structural information, manually planning the robot's required landing points and hand contact point sequence in advance, defining the contact state variables for the soles of the feet and palms, adjusting the hand assistance based on stability margin, and setting... As a safety threshold, when When the robot can maintain stability solely through foot contact, the hand contact force is canceled or kept at a low level. Conversely, when the system has insufficient stability margin, the controller activates the hand assistance function, and the hand contact affects the center of mass movement by influencing the VRP offset.

5. The method according to claim 3, characterized in that, Step 3 specifically involves obtaining the continuous-time state-space equation based on the multi-contact DCM dynamics equation, decoupling the XYZ directions and writing them in matrix form, and then solving the QP quadratic programming problem based on the discretized state-space equation, according to the robot's current CoM position. and DCM location Calculate the motion state of the robot's center of mass that satisfies the dynamic equation after N discrete time steps.

6. The method according to claim 5, characterized in that, QP optimization problems should satisfy state equation constraints, VRP constraints, and extended boundary constraints.

7. The method according to claim 5, characterized in that, Step 4 specifically involves the following steps: the humanoid robot is a standard floating-base robot system, and its generalized position vector includes the floating base and the driven joints; when the robot comes into contact with the environment, its complete whole-body dynamics are described in Eulerian-Lagrange form; based on the dynamics model, the MC-WBC is completely formulated into a quadratic programming problem, and a weighted task form is used to coordinate different task objectives, with various hard constraints having a higher constraint priority than the task tracking objective.

8. The method according to claim 7, characterized in that, The various hard constraints include dynamic constraints, contact constraints, friction cone constraints, joint motion constraints, joint torque constraints, and contact force constraints. Through these constraints, the MC-WBC solver can simultaneously optimize the robot's joint torque, whole-body motion acceleration, and multi-contact forces, enabling the centroid trajectory, DCM state, and hand-assisted strategy generated by the planning layer to be stably executed under whole-body dynamic constraints.

9. A stability enhancement system for humanoid robots based on multi-contact DCM-MPC and whole-body motion control, characterized in that, The system uses the humanoid robot stability enhancement method based on multi-contact DCM-MPC and whole-body motion control as described in any one of claims 1-8, and the system includes: Modeling module: Multi-contact dynamics modeling; Strategy Design Module: Design contact strategies; Controller design module: Design of multi-contact model predictive controller and design of multi-contact whole-body motion controller.

10. A method for enhancing the stability of a humanoid robot based on multi-contact DCM-MPC and whole-body motion control as described in any one of claims 1-8, characterized in that, The method is applied to humanoid robots to achieve stable movement through multi-contact motion planning and whole-body coordinated control in complex terrain, external disturbances, and limited support conditions.

Images