Motion online generation and control method and system for whole-body coordinated walking of humanoid robot, computer readable storage medium and computer program product

Abstract

The invention belongs to the field of robot motion control, and discloses an online generation and control method and system for whole-body coordinated walking motion of a humanoid robot, a computer readable storage medium and a computer program product. According to the method, in walking motion generation, a whole-body walking motion track including upper body joint motion information is generated in real time by adopting a whole-body kinematics-centroid dynamics model; a model predictive control model is adopted to realize organic combination of motion trail generation and real-time tracking control; and solving the model prediction control model, and taking the optimal state x * and the corresponding control input u * as the input of whole body control. According to the method, in the planning stage, namely the whole-body walking motion trail including upper body joint motion information, the whole-body dynamic characteristics of the arms, the waist and the like are comprehensively considered, and a new method of long-time-domain trajectory optimization and online control is combined, so that real whole-body coordinated walking is fundamentally realized.

Description

Technical Field

[0001] The present invention belongs to the field of robot motion control, and more specifically, relates to a method, system, computer-readable storage medium, and computer program product for online generation and control of whole-body coordinated walking of a humanoid robot. Background Art

[0002] Due to its humanoid structure, a humanoid robot can adapt to complex and changing human production and living environments, showing broad application prospects in fields such as industrial production, medical rehabilitation, disaster relief, and the service industry. When a human walks upright, while lifting the lower limbs and taking steps, the upper limbs will swing synchronously and be accompanied by a slight torsion of the torso. This whole-body coordinated movement not only helps to maintain dynamic balance but also significantly reduces energy consumption and improves the stability and efficiency of walking. Imitating this coordination mechanism to generate human-like whole-body movements has always been a key and hot research direction for humanoid robots.

[0003] Currently, the methods for realizing whole-body coordinated walking of humanoid robots are mainly divided into two categories: model-based and learning-based. The learning-based methods collect human body data through means such as motion capture, teleoperation, and video extraction, and deploy the model to the actual robot after training in a simulation environment. Although commercial companies such as Unitree Technology, Zhongqing, and Boston Dynamics have achieved relatively natural whole-body coordinated walking effects, this method generally has problems such as large data requirements, complex and time-consuming data annotation and processing, expensive data acquisition, black-box characteristics in the training process, insufficient interpretability, difficult-to-guarantee safety, and limited generalization ability in complex or new environments, and the core algorithms of the above companies have not been made public.

[0004] In contrast, after years of development, the model-based methods have formed a relatively complete theoretical system. The mainstream solution is a predictive and reactive control strategy that combines model predictive control (MPC) with whole-body control (WBC). However, since a humanoid robot has numerous redundant degrees of freedom, its complete dynamic model is extremely complex, and directly using it for long-time-domain calculations in MPC will bring a huge computational burden. Therefore, most control methods adopt simplified models in online trajectory optimization and control, such as the inverted pendulum model (see Patent CN111377004B, Patent Application CN118331283A), the single rigid body model (Patent Application CN118682750A), and the multi-particle model (Patent Application CN119635625A). However, these simplified models often cannot fully reflect the motion characteristics of the upper body joints, resulting in difficulties for the robot to achieve coordinated unity of actions such as swinging the arms, twisting the waist, and lifting the legs during walking.

[0005] Existing research mainly focuses on bipedal gait and balance control. In kinetic modeling, the leg movement is often considered alone, while the dynamic characteristics of the waist and arms are ignored. For example, in Patent CN111377004B and Patent Application CN118331283A, the ZMP gait control method is derived based on the inverted pendulum model, but the upper body movement is not covered; while Patent Application CN118682750A using a single rigid body model is still insufficient to describe the upper body joint state. To generate and control the upper body movement, the common method currently is to manually design the upper body (including the waist and arms) trajectory based on the predefined leg movement and use an independent controller for position tracking (as shown in Patent Application CN118528270A), but this method has cumbersome steps and decoupled control of the upper and lower bodies, making it difficult to meet the overall kinetic constraints and thus unable to fundamentally achieve whole-body coordinated control.

[0006] Physiological research shows that angular momentum plays a crucial role in coordinating human walking. "Angular momentum in human walking" (H. Herr, M. Popovic, 2008) points out that the yaw angular momentum generated by leg swing may cause slipping of the soles of the feet, while the reverse movement of the arms and waist can effectively compensate for this yaw angular momentum, thereby enhancing the overall stability. Therefore, the upper body movement generation method based on the angular momentum compensation mechanism is considered an important way to achieve whole-body coordinated control. The literature "Walking With Arm Swinging and Pelvis Rotation Generated With the Relative Angular Acceleration" (Akinori Miyata et al., 2020) proposes a method based on the floating base momentum balance principle, introducing a low-priority momentum balance task in the whole-body control layer, so as to automatically generate the upper limb swing to compensate for the yaw angular momentum and avoid manually designing the upper body trajectory. However, this method still has the problems that the planning layer depends on the traditional ZMP gait planning and only focuses on the leg movement without fully considering the upper body dynamics, resulting in insufficient overall coordination; at the same time, although the control layer can achieve instantaneous upper limb compensation, they mainly rely on feedback control and have insufficient prediction ability for future states, so there are significant limitations in aspects such as joint limit and self-collision prediction; in addition, the task priority hierarchical control is complex to adjust in case of multi-task conflicts, which may lead to the non-existence of control solutions and affect the system stability.

[0007] In summary, the deficiencies of the existing model-based methods lie in that the gait generation and motion planning stages do not comprehensively consider the whole-body dynamic characteristics including the arms and waist, resulting in the generated motion trajectory being difficult to ensure overall coordination; while in the whole-body control stage, although a complete dynamic model is introduced for instantaneous inverse dynamics optimization, this subsequent correction is difficult to fundamentally make up for the defects in the planning stage.

[0008] Therefore, it is urgent to comprehensively consider the whole-body dynamic characteristics such as the arms and waist during the planning stage, and combine new methods of long-time domain trajectory optimization and online control to fundamentally achieve true whole-body coordinated walking. Summary of the Invention

[0009] In view of the above-mentioned defects or improvement requirements of the prior art, the present invention provides an online whole-body coordinated motion generation and control method that fully considers whole-body dynamic constraints, aiming to solve the technical problem of motion planning and control that fully considers the dynamics of all degrees of freedom.

[0010] To achieve the above object, according to one aspect of the present invention, there is provided an online generation and control method for the whole-body coordinated walking motion of a humanoid robot, which uses a whole-body kinematics-centroid dynamics model to real-time generate a whole-body walking motion trajectory including upper body joint motion information; and uses a model predictive control model to achieve an organic combination of motion trajectory generation and real-time tracking control; wherein:

[0011] The whole-body kinematics-centroid dynamics model is as follows:

[0012]

[0013] Wherein, represents the centroid momentum, including the linear momentum l com and the angular momentum α com , T represents the transpose; r com,ci represents the coordinate of the contact point ci relative to the centroid, m represents the total mass of the robot, g is the acceleration due to gravity, f c is the three-dimensional contact force, n c represents the number of contact points, and i represents the contact point serial number;

[0014] The model predictive control model is as follows:

[0015]

[0016] Wherein, represents the state variable, is the transpose of the centroid momentum , q b is the position of the floating base in the world coordinate system, q j is the joint position, represents the control input, including the contact force and the joint velocity The superscript T represents the transpose, and T m represents the total time of one foot lift; represents the system state equation, and t represents time; L(x,u,t) and φ(x(T m)) represent the stage cost and the terminal cost respectively; g(x, u, t) and h(x, u, t) represent the equality constraint and the inequality constraint respectively; x(0) = x0 is the initial condition, where x0 represents the initial value of x at t = 0;

[0017] The optimal solution of x obtained by solving the above model predictive control model is the optimal state x * , and the corresponding u is the optimal control input u * , taking the optimal state x * and the corresponding control input u * as the input for whole-body control.

[0018] Furthermore, the objective function of the stage cost L(x, u, t) is:

[0019]

[0020] where the first term is used to minimize the tracking error, and the second term is used to minimize the input; the subscripts Q and R are the positive semi-definite and positive definite weight matrices respectively; x ref is the reference trajectory; the reference value of the center-of-mass momentum is set to 0; the joint angle reference adopts the default value recorded in the standing posture of the robot and performs local search during walking.

[0021] Furthermore, the equality constraint is used to generate the robot motion trajectory, including:

[0022]

[0023] where represents the velocity of the c i -th contact point of the supporting leg; represents the contact force of the c i -th contact point of the swinging leg; represents the velocity of the c i -th contact point of the swinging leg in the z direction, v * (t) represents the reference value of the component of v(t) in the z direction in the foot trajectory of the swinging leg; A CAM,j represents the matrix related to the angular momentum and the linear momentum in the center-of-mass momentum matrix of the joint; represents the generalized velocity of the joint;

[0024] The above (20), (21), (22) and (23) constitute the equality constraint of the model predictive controller and are used to generate whole-body coordinated motion.

[0025] Furthermore, the inequality constraint is used to ensure that the planned motion satisfies the robot dynamics and physical constraints, including:

[0026]

[0027] d k (A,B) ≥ ε k (28)

[0028] wherein, and are the lower and upper limits of the joint angle respectively; represents the maximum rotational speed of the joint; represents the supporting force the z - component of are respectively the x - and y - components of c is the friction coefficient; d k (A,B) represents the minimum distance between link A and link B of the k - th potential collision pair, and ε k is the allowable safety margin.

[0029] To achieve the above object, according to another aspect of the present invention, there is provided an online generation and control system for the whole - body coordinated walking motion of a humanoid robot, including a memory, a processor, and a computer program stored on the memory, and the processor executes the computer program to implement the online generation and control method for the whole - body coordinated walking motion of a humanoid robot as described in any one of the previous items.

[0030] To achieve the above object, according to another aspect of the present invention, there is provided a computer - readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, it implements the online generation and control method for the whole - body coordinated walking motion of a humanoid robot as described in any one of the previous items.

[0031] To achieve the above object, according to another aspect of the present invention, there is provided a computer program product, including a computer program, and when the computer program is executed by a processor, it implements the online generation and control method for the whole - body coordinated walking motion of a humanoid robot as described in any one of the previous items.

[0032] To achieve the above object, according to another aspect of the present invention, there is provided an online generation and control system for the whole - body coordinated walking motion of a humanoid robot, including:

[0033] a user input module, a foot - end trajectory generation module, a body trajectory generation module, a gait scheduling module, a model predictive controller, a whole - body controller, a state estimator, and a mapping module between the upper - layer control and the lower - layer hardware;

[0034] wherein,

[0035] the user input module inputs a gait switching instruction and a body motion instruction;

[0036] The gait scheduling module generates a contact timing table according to the gait switching instruction and outputs it to the foot-end trajectory generation module and the model predictive controller;

[0037] The foot-end trajectory generation module generates the foot-end trajectory of the swing leg according to the contact timing table and outputs it to the model predictive controller;

[0038] The fuselage trajectory generation module generates the expected fuselage trajectory according to the fuselage motion instruction and outputs it to the model predictive controller;

[0039] The state estimator receives the joint feedback information and IMU information returned by the robot, generates the joint state and the fuselage state, and outputs them to the model predictive controller and the whole-body controller;

[0040] The model predictive controller generates an optimized whole-body motion trajectory according to the contact timing table, the foot-end trajectory of the swing leg, the expected fuselage trajectory, and the joint feedback information and IMU information, and outputs it to the whole-body controller;

[0041] The whole-body controller generates the joint control instruction of the robot according to the optimized whole-body motion trajectory and outputs it to the robot;

[0042] The mapping module between the upper-layer control and the lower-layer hardware is used to map the control instruction of the whole-body controller to the robot, and map the joint feedback information and IMU information returned by the robot to the state estimator.

[0043] Furthermore, the model predictive controller is used to implement the online generation and control method for the whole-body coordinated walking motion of the humanoid robot as described in any one of the preceding items.

[0044] Generally speaking, compared with the prior art, the above technical solution conceived by the present invention can achieve the following beneficial effects:

[0045] 1. The present invention adopts a whole-body kinematics-centroid dynamics model, which not only meets the real-time calculation requirements but also fully considers the motion information of the whole-body joints, so as to generate the upper-body joint motion trajectory and the whole-body centroid motion trajectory at the planning layer. On this basis, the coordinated control of the whole-body motion is realized by solving the model predictive control model, so as to realize the coordinated planning of various parts of the robot during walking, such as arm swinging, waist twisting, and stepping and lifting the leg.

[0046] 2. The present invention adopts equality constraints and / or inequality constraints to ensure the physical limitations of the robot and the stability of contact with the ground respectively.

[0047] 3. The lifting movement of the legs in the present invention is generated by the equality constraint of the foot tip trajectory; the step length of the step is optimized according to the desired body movement speed; at the same time, the arm swinging and waist twisting movements are generated automatically by applying an equality constraint, that is, the yaw component in the coupled angular momentum generated by the whole body joints at the center of mass is zero, so as to generate upper body movements to compensate for the yaw angular momentum generated by the leg swinging. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 It is a schematic diagram of the system architecture of the preferred embodiment of the present invention;

[0049] Figure 2 It is a schematic diagram of the control algorithm framework of the preferred embodiment of the present invention;

[0050] Figure 3 It is a schematic diagram of the definition of the humanoid robot coordinate system of the preferred embodiment of the present invention;

[0051] Figure 4 It is a schematic diagram of the simplification of the foot tip contact model of the preferred embodiment of the present invention;

[0052] Figure 5 It is a schematic diagram of the definition of the biped gait of the preferred embodiment of the present invention;

[0053] Figure 6 It is a schematic diagram of the foot tip trajectory of the swinging leg of the preferred embodiment of the present invention;

[0054] Figure 7 It is a schematic diagram of the processing method of the serial-parallel mechanism of the robot of the preferred embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0055] In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0056] The present invention proposes a method for generating and controlling the movement of a humanoid robot's whole body coordinated walking, and its control algorithm framework is as Figure 2 shown, and the module definition and implementation steps are as Figure 1 shown. The controller in this article mainly includes a model predictive controller and a whole body controller. Among them, the model predictive controller uses a whole body kinematics-centroid dynamics model to predict the movement trajectory, while the whole body controller realizes movement tracking and real-time control based on the floating base multi-rigid body dynamics model.

[0057] The control system of the present invention includes a user input module, a foot-end trajectory generation module, a body trajectory generation module, a gait scheduling module, a model predictive controller, a whole-body controller, a state estimator, and a mapping module between the upper-layer control and the lower-layer hardware. Among them, the user input module inputs gait switching instructions and body movement instructions; the gait scheduling module generates a contact timing table according to the gait switching instructions and outputs it to the foot-end trajectory generation module and the model predictive controller; the foot-end trajectory generation module generates the foot-end trajectory of the swinging leg according to the contact timing table and outputs it to the model predictive controller; the body trajectory generation module generates the desired body trajectory according to the body movement instructions and outputs it to the model predictive controller; the state estimator receives the joint feedback information and IMU information returned by the robot, generates the joint state and the body state, and outputs them to the model predictive controller and the whole-body controller; the model predictive controller generates an optimized whole-body movement trajectory according to the contact timing table, the foot-end trajectory of the swinging leg, the desired body trajectory, and the joint feedback information and IMU information, and outputs it to the whole-body controller; the whole-body controller generates the joint control instructions of the robot according to the optimized whole-body movement trajectory and outputs them to the robot; the mapping module between the upper-layer control and the lower-layer hardware is used to map the control instructions of the whole-body controller to the robot, and map the joint feedback information and IMU information returned by the robot to the state estimator.

[0058] Under the planning-tracking control framework combining MPC (Model Predictive Control) and WBC (Whole-Body Control), MPC adopts a whole-body kinematics-centroid dynamics model, which not only meets the real-time calculation requirements but also fully considers the movement information of the whole-body joints, thereby generating the upper-body joint movement trajectory at the planning layer; while WBC is based on the complete dynamics model and performs real-time correction on the trajectory generated by MPC by solving the instantaneous inverse dynamics problem, so as to realize the coordinated control of the whole-body movement.

[0059] MPC constructs a nonlinear optimization problem. Among them, the leg lifting movement is generated by the equality constraint of the foot-end trajectory; the step length is optimized according to the desired body movement speed; at the same time, the arm swinging and waist twisting movements are generated automatically by applying an equality constraint, that is, the yaw component in the coupled angular momentum generated by the whole-body joints at the centroid is zero, so as to generate the upper-body movement to compensate for the yaw angular momentum generated by the leg swinging. By solving this optimization problem, the coordinated planning of the movements of the arm swinging, waist twisting, stepping and leg lifting and other parts of the robot during walking can be realized.

[0060] The WBC adopts a weight-based whole-body control method instead of a hierarchical strict priority control method. Among them, equality constraints and inequality constraints are used to ensure the physical limitations of the robot and the stability of contact with the ground; while the trajectory tracking task obtained by MPC planning is used as a soft constraint, and its relative priority is adjusted by setting corresponding weight coefficients. The equality constraints include: the multi-rigid-body dynamics equation of the floating base, the fixed end of the supporting leg foot, and the zero contact force at the end of the swinging leg foot; the inequality constraints include: joint torque limit and friction cone constraint; the soft constraints include: swinging leg foot end tracking, waist joint tracking, arm joint tracking, base tracking, and supporting leg foot end contact force tracking tasks. In a preferred embodiment, their corresponding weights are set to 10, 10, 10, 1, and 0.1 respectively according to experience.

[0061] The adopted robot model converts the transmission components such as parallel mechanisms and linear push rods existing in the actual robot into a simplified model of series joints, so as to realize the efficient calculation of robot dynamics; at the same time, a conversion module for the position, speed, and force of the joint end and the motor end is added between the actual machine and the upper control program to ensure that the control algorithm is adapted to robots with a variety of different configurations and degrees of freedom, and to achieve an effective decoupling between the control algorithm and the robot configuration.

[0062] The corresponding introduction to the algorithm modules involved in the present invention is as follows:

[0063] S1 Dynamics modeling: First, a dynamics model is constructed for the controller.

[0064] Figure 3 The coordinate systems defined in it include: the world coordinate system W, which is an inertial system; the base coordinate system B fixed on the fuselage; and the center-of-mass coordinate system G.

[0065] S11: Modeling of the foot contact point. As Figure 4 shown, to simplify the surface contact modeling between the foot and the ground, each foot uses three point contacts (three-dimensional force) to simulate the surface contact (six-dimensional force) effect between the sole of the foot and the ground. There are a total of 6 contact points for both feet, so n c = 6. Through this simplified method, when the same vertical foot end trajectory is given to the three contact points on one foot, the sole of the foot can be constrained to be parallel to the ground in real time, making the control of the sole attitude more convenient;

[0066] S12: The multi-rigid-body dynamics model of the floating base of the whole-body controller. Construct a multi-rigid-body dynamics model of the floating base, and its expression is:

[0067]

[0068] Among them, q represents the generalized coordinate, represents the generalized velocity, denotes the generalized acceleration; M is the inertia matrix; C represents non-linear terms such as centrifugal force and Coriolis force; G represents the gravity term; τ j is the joint output torque; S is the selection matrix used to project the joint torque onto the generalized coordinates. J c represents the contact Jacobian, f c is the three-dimensional contact force, n c represents the number of contact points, and i represents the contact point sequence number.

[0069] S13: Whole-body kinematics - center-of-mass dynamics model of the model predictive controller. Considering that the above model in Equation (1) has a high computational complexity and is difficult to solve in real time for long-time prediction in model predictive control, the present invention simplifies Equation (1). The specific method is as follows:

[0070] Extract the floating-base part of Equation (1) to obtain:

[0071]

[0072] where the subscript u represents that the floating base is underactuated.

[0073] Transform Equation (2) into the form of Newton-Euler equation to obtain:

[0074]

[0075] where, denotes the center-of-mass momentum, including the linear momentum l com and the angular momentum α com , and the center-of-mass momentum is defined as the sum of the momenta of each link of the whole body projected onto the center of mass. r com,ci represents the coordinate of the contact point ci relative to the center of mass, m represents the total mass of the robot, and g is the gravitational acceleration.

[0076] Equation (3) is the center-of-mass dynamics model, and its physical meaning is that the resultant external force is equal to the change rate of the center-of-mass momentum of the robot. This model ignores the joint-space dynamics and only considers the effect of the contact force on the center of mass, thus greatly reducing the model complexity.

[0077] Meanwhile, the center-of-mass momentum and the generalized velocity satisfy the following relationship:

[0078]

[0079] where the subscripts b and j represent the floating base and the joint degrees of freedom respectively. A G (q) represents the center-of-mass momentum matrix, which is related to the system Jacobian matrix and the inertia.

[0080] By combining Formula (3) and (4), a full-body kinematics-centroid dynamics model that ignores joint space dynamics but fully considers full-body kinematic information is obtained, which is applicable to the online optimization calculation of model predictive control.

[0081] S2: This module is used to provide a reference trajectory for the model predictive controller, and its core functional modules include:

[0082] S21: Gait scheduler. As Figure 5 shown, the present invention designs a bipedal gait, and the gait parameters can be selected according to experience. In this embodiment, preferably:

[0083] ρ L = 0.5, ρ R = 0.5,

[0084] where L represents the left leg and R represents the right leg. ρ and respectively represent the duty cycle and phase shift. The gait scheduler generates the contact state time series for each foot end according to these parameters, and defines that the states of the contact points on the same foot are consistent and correspond to the gait time series.

[0085] S22: Foot end trajectory generator. As Figure 6 shown, the reference trajectory of the swing leg foot end in the vertical direction is described by a piecewise spline curve. From lifting the foot to the highest height is the first segment, and from the highest height to touching the ground is the second segment.

[0086] The general expression of the spline curve is:

[0087]

[0088] where s(t), v(t) and a(t) represent the position, velocity and acceleration at time t respectively.

[0089] For each segment of the spline curve, the boundary conditions include the position and velocity boundary conditions at the initial and end points.

[0090] The boundary conditions for the first segment (left segment) curve are:

[0091] s(0) = 0, v(0) = v0, s(T m / 2) = h, v(T m / 2) = 0 (6)

[0092] where v0 represents the foot-lifting speed, h represents the foot-lifting height, and T m represents the total time for one foot lift.

[0093] Substituting (6) into (5) gives the parameters of the first segment of the spline curve as:

[0094]

[0095] The boundary conditions of the second (right) spline curve are as follows:

[0096] s(T m / 2) = h, v(T m / 2) = 0, s(T m ) = 0, v(T m ) = v1(8)

[0097] where v1 represents the touchdown velocity.

[0098] Substituting (8) into (5), the parameters of the second spline curve are obtained as follows:

[0099]

[0100] Thus, for 0 ≤ t ≤ T m the vertical motion trajectory of a certain contact point at the foot end of the swinging leg within the time range is:

[0101]

[0102] In the above formulas, l represents the first (left) spline curve, and r represents the second (right) one.

[0103] When the parameters v0 = 0.03 m / s, v1 = -0.03 m / s, h = 0.05 m, and T m = 0.4 s are taken, the vertical leg-lifting trajectory curve at the foot end is as shown in Figure 6 the figure.

[0104] S23: Body trajectory generator. The user sends a body speed command through the remote controller

[0105]

[0106] This control command is expressed in the body base coordinate system B, controlling the forward, lateral movement, and turning speed components of the robot in the horizontal plane B v x , B v y and B ω z . The superscript cmd represents the control command.

[0107] The current body pose obtained through state estimation is:

[0108]

[0109] The above body attitude It represents the xyz position coordinates and Euler angles of the fuselage in the world coordinate system W, and the superscript est represents the information obtained from state estimation. Since the desired fuselage motion speed and the current pose are not in the same coordinate system, coordinate transformation is required.

[0110] For angular velocity control, only the yaw angle is concerned (the roll angle and pitch angle are both set to 0), so the desired angular velocities in the world coordinate system and the fuselage coordinate system and are approximately the same:

[0111]

[0112] Define the desired velocity in the base coordinate system as:

[0113]

[0114] where B v x , B v y are respectively the desired velocities of the fuselage in the x and y directions in the base coordinate system;

[0115] Then the desired linear velocity of the fuselage in the world coordinate system is:

[0116]

[0117] where W R B is the rotation matrix from the base coordinate system to the world coordinate system, W v x , W v y are respectively the desired velocities of the fuselage in the x and y directions in the world coordinate system.

[0118] Assume that the sampling period of the model predictive control is ΔT. Then, after time ΔT, the desired fuselage pose of the robot is:

[0119]

[0120] where represents the desired fuselage pose of the robot after time ΔT. During horizontal movement, the desired height of the fuselage always remains at a default height h default , and the desired pitch angle and roll angle of the fuselage are both set to 0, which are respectively the desired x and y coordinates of the fuselage in the world coordinate system and the Euler angle about the z-axis.

[0121] For By performing linear interpolation within the sampling period ΔT, a discrete-time reference trajectory can be obtained as the desired trajectory of the fuselage, guiding the robot to walk and turn in any direction in the horizontal plane.

[0122] S3: Controller module. This module is the core of the system, including a model predictive controller, a whole-body controller, and a state estimator, which realizes the organic combination of motion trajectory generation and real-time tracking control.

[0123] S31: The general form of model predictive control can be expressed as:

[0124]

[0125] where represents the state variable, is transpose of, q b is the position of the floating base in the world coordinate system, q j is the joint position, represents the control input (including the contact force and the joint velocity ); represents the system state equation, t represents time, and the present invention adopts a whole-body kinematics-centroid dynamics model; L(x, u, t) and φ(x(T m )) represent the stage cost and the terminal cost respectively; g(x, u, t) and h(x, u, t) represent the equality constraint and the inequality constraint respectively; x(0) = x0 is the initial condition, and x0 represents the initial value of x at t = 0.

[0126] The system state equation is:

[0127]

[0128] where n a is the number of driving joints;

[0129] The objective function of the stage cost L(x, u, t) is:

[0130]

[0131] where the first term is used to minimize the tracking error, and the second term is used to minimize the input; Q and R are semi-positive definite and positive definite weight matrices respectively. x ref is the reference trajectory, where is provided by the fuselage trajectory generator; the reference value of the center-of-mass momentum is set to 0 (the corresponding term weight in Q is set to 0 and not tracked); the joint angle reference adopts the default value recorded in the standing posture of the robot and performs local search during walking.

[0132] The designed equality constraints are mainly used to generate the robot's motion trajectory:

[0133] For the contact points of the supporting legs, a non-moving constraint is set:

[0134]

[0135] Among them, represents the velocity of the c i -th contact point;

[0136] For the contact points of the swinging legs, since they are not in contact with the ground, the contact force of the c i -th contact point should be 0:

[0137]

[0138] For the vertical trajectory tracking of the end of the swinging leg (refer to Figure 5 ), the constraint form is:

[0139]

[0140] Among them, represents the velocity of the c i -th contact point in the z direction, and v * (t) represents the reference value of the component of v(t) in the z direction in the end trajectory of the swinging leg;

[0141] The above equality constraints are used to generate leg movements; for the waist and arm movements, the reference trajectories are not directly given, but by constraining the yaw-direction coupling angular momentum generated by the whole body joints at the center of mass to be 0, the coordinated upper body movements are automatically generated.

[0142] Thus, the body angular velocity and the center-of-mass angular velocity can be independently controlled through the above relationship. Further, by adding the following equality constraints:

[0143]

[0144] Formula (23) can constrain the yaw-direction coupling angular momentum generated by the whole body joints at the center of mass to be 0, so as to automatically generate upper body movements to compensate for the yaw angular momentum generated by the leg swing and avoid the supporting foot from slipping due to the yaw torque.

[0145] The above (20), (21), (22) and (23) constitute the equality constraints of the model predictive controller for generating the whole body coordinated movements.

[0146] Inequality constraints are used to ensure that the planned movements meet the robot dynamics and physical constraints, mainly including:

[0147] Joint angle limit:

[0148]

[0149] Among them, and are the lower and upper limits of the joint angle respectively.

[0150] Joint rotational speed limit:

[0151]

[0152] Among them, represents the maximum rotational speed of the joint.

[0153] Support force ( z - component) limit:

[0154]

[0155] Friction cone constraint:

[0156]

[0157] Among them, are respectively x and y components of c and μ

[0158] Self - collision constraint of the limb:

[0159] d k (A,B)≥ε k (28)

[0160] Among them, d k (A,B) represents the minimum distance between link A and link B of the k - th potential collision pair, and ε k is the allowable safety margin.

[0161] Formulas (24) to (28) constitute the inequality constraints of the model predictive controller.

[0162] The present invention uses the multiple - shooting method to transform the optimal control problem into a nonlinear optimization problem, and uses the sequential quadratic programming (SQP) method for online solution. Each QP sub - problem is solved using the open - source QP solver library qpOASES. Other methods such as DDP, ILQR, etc. can also be used to solve the optimal control problem.

[0163] After the solution is completed, the optimal state x * and the corresponding control input u * are obtained as the inputs for whole - body control.

[0164] S32: Whole-body controller. The x of the whole-body control problem * and u * is solved by using the method based on weighted quadratic programming, and its basic form is:

[0165]

[0166] This problem includes three parts: equality constraints, inequality constraints and soft constraints. The specific tasks and parameters are shown in the following table:

[0167] Design of whole-body control tasks for humanoid robots

[0168]

[0169] Among them, the equality and inequality constraints are used to ensure that the solution results meet the actual dynamic requirements of the robot, the stability of the floating base and the joint torque limit, and are all strictly enforced as hard constraints.

[0170] For the motion trajectory generated by model predictive control, it is introduced into the objective function (33) as a soft constraint, and corresponding weights are assigned according to the importance of each task.

[0171] Since the simplified model may cause deviations in the support force, the support foot end force tracking task is given the lowest weight; while the swing leg, waist and arm joint trajectory tracking tasks are crucial for the stability of the robot, so they are set as high priorities; the base trajectory tracking task is given medium priority.

[0172] Preferably, the quadratic programming problem of whole-body control can also be solved by qpOASES.

[0173] S33: State estimator. The state estimator acquires the feedback data of the robot joint encoder and IMU in real time, and uses the leg motion to calculate the body pose based on the position of the support foot. In each control cycle, the body pose is obtained by Kalman filter iteration and transmitted to the controller module to provide reliable state feedback for motion control.

[0174] S4: Low-level control strategy. To effectively decouple the control algorithm from the robot hardware configuration, the present invention adopts Figure 7 the processing method shown.

[0175] The dynamic model and the controller are both derived based on the simplified series joint configuration, and the URDF model used in the simulation system is also based on the series configuration, so as to verify the correctness of the algorithm.

[0176] For an actual robot, due to the possible existence of transmission structures such as parallel mechanisms and linear drive joints, the drive motor may not coincide with the robot joint. Therefore, it is necessary to convert the position, speed, and torque between the joint end and the motor drive end.

[0177] S41: The conversion module based on C++. Taking the Figure 3 humanoid robot shown as an example, its ankle joint adopts a parallel mechanism, and a series-parallel structure is formed between the ankle joint and the ankle motor through a two-stage four-bar linkage mechanism (passing through the knee). The present invention derives the angle, rotational speed, and torque relationships between the knee and ankle joint ends and the motor drive end in this series-parallel mechanism and implements them as a C++ program module. This module is embedded between the controller and the actual robot motor drive program, thereby achieving an effective decoupling between the control algorithm and the hardware configuration. It can also be implemented using other programming languages, not limited to C++.

[0178] Generally speaking, through the above preferred embodiments, the present invention converts the transmission components such as parallel mechanisms and linear push rods in the actual robot into a simplified model of series joints and applies it to the control algorithm. Then, a conversion module for position, speed, and force between the joint and the motor end is added between the actual machine and the upper control software, achieving an effective decoupling between the control algorithm and the robot configuration. The present invention realizes the coordinated movements such as automatic arm swinging, waist twisting, leg lifting, and stepping during the walking of the humanoid robot. In the generation of the walking motion of the present invention, a full-body kinematics-centroid dynamics model is adopted to generate the full-body walking motion trajectory including the upper body joint motion information in real time; the leg-lifting motion is controlled by a given foot-end reference trajectory, the step length of the step is optimized according to the desired body speed, and the arm-swinging and waist-twisting actions are automatically generated by a strategy for compensating the yaw angular momentum generated by the swinging leg.

[0179] It is easy for those skilled in the art to understand that the above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An online generation and control method for the whole-body coordinated walking motion of a humanoid robot, characterized in that, A full-body kinematics - centroid dynamics model is adopted to generate in real time a full-body walking motion trajectory including upper body joint motion information; a model predictive control model is adopted to organically combine motion trajectory generation and real-time tracking control; wherein: The full-body kinematics - centroid dynamics model is as follows: Among them, represents the center-of-mass momentum, including the linear momentum l com and the angular momentum α com , T represents the transpose; r com,ci represents the coordinate of the contact point ci relative to the center of mass, m represents the total mass of the robot, g is the acceleration due to gravity, f c is the three-dimensional contact force, n c represents the number of contact points, and i represents the contact point serial number; The model predictive control model is as follows: Among them, represents the state variable, is the transpose of the center-of-mass momentum q, b is the position of the floating base in the world coordinate system, q j is the joint position, represents the control input, including the contact force and the joint velocity The superscript T represents the transpose, T m represents the total time of one foot lift; represents the system state equation, where t represents time; L(x, u, t) and φ(x(T m )) represent the stage cost and the terminal cost respectively; g(x, u, t) and h(x, u, t) represent the equality constraint and the inequality constraint respectively; x(0) = x0 is the initial condition, and x0 represents the initial value of x at t = 0; The optimal solution of x obtained by solving the model predictive control model is the optimal state x * , and the corresponding u is the optimal control input u * , take the optimal state x * and the corresponding control input u * as the input for whole-body control.

2. The online generation and control method for the whole-body coordinated walking motion of a humanoid robot according to claim 1, wherein The objective function of the stage cost L(x, u, t) is: Among them, the first item is used to minimize the tracking error, and the second item is used to minimize the input; the subscripts Q and R are semi - positive definite and positive definite weight matrices respectively; x ref is the reference trajectory; the reference value of the center - of - mass momentum is set to 0; the joint - angle reference adopts the default value recorded in the standing posture of the robot and performs local search during walking.

3. The online generation and control method for the whole-body coordinated walking motion of a humanoid robot according to claim 1, characterized in that, Equality constraints are used to generate the robot motion trajectory, including: Among them, represents the velocity of the c-th i contact point of the support leg; represents the contact force of the c-th i contact point of the swing leg; represents the velocity of the ci-th contact point of the swing leg in the z direction, v * (t) represents the reference value of the component of v(t) in the z direction in the foot trajectory of the swing leg; A CAM,j represents the matrix related to angular momentum and linear momentum in the centroid momentum matrix of the joint; represents the generalized velocity of the joint; The above (20), (21), (22) and (23) constitute the equality constraints of the model predictive controller and are used to generate full-body coordinated motion.

4. The online generation and control system for the whole-body coordinated walking motion of a humanoid robot according to claim 1, characterized in that, Inequality constraints are used to ensure that the planned motion satisfies the robot dynamics and physical constraints, including: wherein, and are the lower and upper limits of the joint angle respectively; represents the maximum rotational speed of the joint; represents the z - component of the support force , and are respectively the x - and y - components of, μ c is the friction coefficient; d k (A,B) represents the minimum distance between the k - th potential collision pair of link A and link B, and ε k is the allowable safety margin.

5. A whole-body coordinated walking motion online generation and control system for a humanoid robot, comprising a memory, a processor, and a computer program stored on the memory, characterized in that, The processor executes the computer program to implement the online generation and control method for the full-body coordinated walking motion of the humanoid robot according to any one of claims 1 to 4.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the online generation and control method for the full-body coordinated walking motion of the humanoid robot according to any one of claims 1 to 4.

7. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the online generation and control method for the full-body coordinated walking motion of the humanoid robot according to any one of claims 1 to 4.

8. An online generation and control system for the whole-body coordinated walking motion of a humanoid robot, characterized in that, Including: A user input module, a foot-end trajectory generation module, a fuselage trajectory generation module, a gait scheduling module, a model predictive controller, a full-body controller, a state estimator, and a mapping module between the upper layer control and the lower layer hardware; Wherein, The user input module inputs a gait switching instruction and a fuselage motion instruction; The gait scheduling module generates a contact timing table according to the gait switching instruction and outputs it to the foot-end trajectory generation module and the model predictive controller; The foot-end trajectory generation module generates the foot-end trajectory of the swinging leg according to the contact timing table and outputs it to the model predictive controller; The fuselage trajectory generation module generates the expected fuselage trajectory according to the fuselage motion instruction and outputs it to the model predictive controller; The state estimator receives the joint feedback information and IMU information returned by the robot, generates the joint state and the fuselage state, and outputs them to the model predictive controller and the full-body controller; The model predictive controller generates an optimized full-body motion trajectory according to the contact timing table, the foot-end trajectory of the swinging leg, the expected fuselage trajectory, and the joint feedback information and IMU information, and outputs it to the full-body controller; The full-body controller generates the joint control instruction of the robot according to the optimized full-body motion trajectory and outputs it to the robot; The mapping module between the upper layer control and the lower layer hardware is used to map the control instruction of the full-body controller to the robot, and map the joint feedback information and IMU information returned by the robot to the state estimator.

9. The online generation and control system for the whole-body coordinated walking motion of a humanoid robot according to claim 8, characterized in that, The said model predictive controller is used to implement the online generation and control method for the full-body coordinated walking motion of the humanoid robot according to any one of claims 1 to 4.

Images