Motion control method and device, equipment and storage medium

Abstract

The embodiment of the invention discloses a motion control method and device, equipment and a storage medium. The method provided by the embodiment of the invention comprises the following steps: establishing a stable area of the stability margin of the two feet of the robot according to contact points of the two feet of the robot and the ground; the mass center position of each connecting rod of the robot is obtained, and the overall mass center of the robot is calculated; according to the whole mass center, the position of each contact point of the robot and the force acting on each contact point, the zero moment point of the robot is calculated; according to the position relation between the zero moment point and the stable area, the control moment of each joint of the robot is calculated through an inverse dynamic model, so that the gait of the robot is controlled. According to the method, the balance state of the robot is judged by combining the position relation between the stable area and the zero moment point, and then the joint control moment is calculated by using the inverse dynamic model, so that the motion trail of each joint of the robot is optimized.

Description

Technical Field

[0001] The embodiments of the present application relate to the field of robot control, and in particular to motion control methods, devices, equipment and storage media. Background Art

[0002] With the rapid development of artificial intelligence and robotics, humanoid robots are gradually being widely used in industries, services, and medical fields due to their humanoid form and flexibility. The main methods for dynamic and stable walking control of humanoid robots include the following:

[0003] 1. Humanoid robot torque control method based on position loop pre-training. According to the structure of the humanoid robot, determine the network structure of the position control strategy network, the torque control strategy network and the value network. Set the training related parameters and reward function, input the current state of the robot into the trained torque control strategy network, and control the humanoid robot in real time according to the torque. 2. Quasi-passive seven-link biped robot gait control method. Establish a quasi-passive seven-link dynamic model, combine the Newton-Raphson iteration method and the variable perturbation method, and quickly search for the fixed point state. Use the multi-stage gait characteristics and deep reinforcement learning algorithm to train the network to obtain the gait control strategy and the torque value required for each joint under different states. 3. Humanoid robot gait control method and device based on fall judgment. Obtain the motion analysis data of the humanoid robot to determine the fall state and cause of the humanoid robot. Calculate the fall probability of the humanoid robot based on the fall state, and correct the walking gait and control amount of the humanoid robot according to the fall probability and the fall cause.

[0004] The first method above represents the solution of using models and networks, but currently such solutions require adjusting parameters in different tasks, lacking a unified method and good generalization. The second method represents the solution of using dynamic models, but such as the Newton-Raphson iteration method and the variable perturbation method, there is a problem that the original equation needs to be solved multiple times by iteration, and in some cases it may not converge or converge to a non-root extreme point. The third method represents the solution of using fall cause analysis and probability judgment, but the cause analysis is often incomplete, the probability of falling is a random event rather than an inevitable event, and there is a large control error. Summary of the invention

[0005] Based on the above problems, the embodiments of the present application provide motion control methods, devices, equipment and storage media to solve the problems of cumbersome parameter adjustment, poor calculation convergence and large control error existing in the related technology, and to improve the stability and generalization ability of robot walking control.

[0006] In a first aspect, an embodiment of the present application provides a motion control method, which is applied to a robot, and the method comprises:

[0007] Establishing a stable region of the robot's bipedal stability margin according to the contact points between the robot's bipedal feet and the ground;

[0008] Obtaining the center of mass position of each connecting rod of the robot and calculating the overall center of mass of the robot;

[0009] Calculating a zero moment point of the robot according to the overall mass center, the position of each contact point of the robot, and the force acting on each contact point;

[0010] According to the positional relationship between the zero torque point and the stable area, the control torque of each joint of the robot is calculated using an inverse dynamics model to control the gait of the robot.

[0011] In one embodiment, before establishing a stable region of the robot's bipedal stability margin according to the contact points between the robot's bipedal feet and the ground, the method further comprises:

[0012] Establish the dynamics model and inverse dynamics model of the robot's arms, torso, and feet;

[0013] Among them, the arms are established as a two-link model, the torso is established as a two-link model, and the feet are established as a three-link model.

[0014] In one embodiment, the kinetic model comprises:

[0015] Among them, the dynamic equation of the two-link is:

[0016]

[0017] Where D is the inertia matrix; represents the angular acceleration of the joint; Describes the inertial force generated during acceleration; C is the Christoffel symbol used to describe the Coriolis force and centrifugal force generated by the robot arm's motion speed; represents the angular velocity of the joint; Describes the Coriolis force and centrifugal force generated by the speed of movement; g is the acceleration due to gravity, and τ is the control torque of the joint;

[0018] The dynamic equation of the three-link is:

[0019]

[0020] Among them, M 11 、M 12 、M 13 Indicates the inertia coefficient of the first joint; M 21 、M 22 、M 23represents the inertia coefficient of the second joint; M 31 、M 32 、M 33 represents the inertia coefficient of the third joint; represents the angular velocity of the joint; represents the angular acceleration of the joint; H 1 , H 2 , H 3 G represents the Coriolis force and centrifugal force of the first joint, the Coriolis force and centrifugal force of the second joint, and the Coriolis force and centrifugal force of the third joint respectively; 1 , G 2 , G 3 represents the gravity vector of the first joint, the gravity vector of the second joint, and the gravity vector of the third joint respectively; τ 1 , τ 2 , τ 3 represents the control torque of the first joint, the control torque of the second joint, and the control torque of the third joint;

[0021] M 11 、M 12 、M 13 、M 22 、M 23 、M 33 , H 1 , H 2 , H 3 , G 1 , G 2 , G 3 Specifically expressed as:

[0022]

[0023]

[0024] Among them, θ 1 Expressed as the angle between the first link and the vertical direction; θ 2 is the angle between the first connecting rod and the second connecting rod, θ 3 I represents the angle between the second connecting rod and the third connecting rod; 1 ,I 2 、i 3 M represents the moment of inertia of the first connecting rod, the moment of inertia of the second connecting rod, and the moment of inertia of the third connecting rod respectively; 1 、m 2 、m 3 represents the mass of the first connecting rod, the mass of the second connecting rod, and the mass of the third connecting rod respectively; l 1 , l 2 , l 3represents the length of the first connecting rod, the length of the second connecting rod, and the length of the third connecting rod respectively; d 1 , d 2 , d 3 It represents the distance from the center of mass to the first joint, the distance from the center of mass to the second joint, and the distance from the center of mass to the third joint respectively;

[0025] The inverse dynamics model comprises:

[0026] The two-link counterdynamics equation is:

[0027]

[0028] in, represents the Lagrangian, T is the kinetic energy of the two-link, and V is the gravitational potential energy of the two-link;

[0029]

[0030] in, describes the coupled kinetic energy between the two rods;

[0031] V=(m 1 +m 2 )gl 1 sin(θ 1 )+m 2 gl 2 sin(θ 2 )

[0032] Among them, (m 1 +m 2 )gl 1 sin(θ 1 ) describes the potential energy of the first rod and its load; m 2 gl 2 sin(θ 2 ) describes the potential energy of the second rod;

[0033] The inverse dynamics equation of the three-link is:

[0034]

[0035] in,

[0036] V=m 1 gl 1 sin(θ 1 )+m 2 g(l 1 sin(θ 1 )+l 2 sin(θ 2))+m 3 g(l 1 sin(θ 1 )+l 2 sin(θ 2 )+l 3 sin(θ 3 )).

[0037] In one embodiment, establishing a stable region of stability margin of the robot's feet according to the contact points between the robot's feet and the ground includes:

[0038] Establishing a stability margin projection plane using two parallel lines where the robot's two feet touch the ground;

[0039] In the stability margin projection plane, a quadrilateral stable area is formed by the front contact point of the left foot, the rear contact point of the left foot, the front contact point of the right foot and the rear contact point of the right foot.

[0040] In one embodiment, after obtaining the center of mass position of each connecting rod of the robot and calculating the center of mass of the robot as a whole, the method further includes:

[0041] Projecting the overall centroid onto the stability margin projection plane, and determining the positional relationship between the overall centroid and the stable area;

[0042] If the overall center of mass is within the stable region, it is determined that the robot is in a stable state;

[0043] If the overall center of mass is not within the stable region, it is determined that the robot is in an unstable state.

[0044] In one embodiment, the calculation expression for obtaining the center of mass position of each connecting rod of the robot and calculating the center of mass of the entire robot is:

[0045]

[0046] Among them, m i is the mass of the ith rod, is the center of mass position vector of the ith rod, and n is the total number of connecting rods.

[0047] In one embodiment, the expression for calculating the zero moment point of the robot according to the overall center of mass, the position of each contact point of the robot and the force acting on each contact point is:

[0048]

[0049] Among them, r j is the position of the jth contact point, CoM is the position of the overall center of mass, F jis the force acting on the jth contact point.

[0050] In one embodiment, the method further comprises:

[0051] Calculate the rotational freedom, angle and axial rotation angle of each joint according to the control torque of each joint;

[0052] The rotational degrees of freedom, angles, and axial rotation angles of each joint are calculated and linearly interpolated to perform robot gait control.

[0053] In a second aspect, an embodiment of the present application further provides an electronic device, including:

[0054] CPU, memory, input and output interfaces;

[0055] The memory is a short-term storage memory or a persistent storage memory;

[0056] The central processing unit is configured to communicate with the memory and execute instruction operations in the memory to perform any one of the above-mentioned motion control methods.

[0057] In a third aspect, an embodiment of the present application further provides a robot, comprising: a trunk of the robot, two arms connected to the trunk, and two feet connected to the trunk;

[0058] The torso of the robot includes the head, chest and abdomen of the robot; the head includes eyes, nose, mouth, ears, forehead and hair;

[0059] The robot's arms include shoulders, upper arms, elbows, lower arms, wrists, hands, and fingers;

[0060] The robot's bipedal feet include the robot's hip, thigh, knee, calf, ankle, and foot;

[0061] The robot moves by any one of the above-mentioned motion control methods.

[0062] It can be seen from the above technical solutions that the embodiments of the present application have the following advantages:

[0063] The technical solution of the present application combines the positional relationship between the stable area and the zero torque point to determine the current dynamic equilibrium state of the robot, and then uses the inverse dynamics model to calculate the joint control torque, thereby realizing gait adjustment and optimizing the motion trajectory of each joint of the robot. It significantly improves the robustness and generalization ability of the control method, and solves the problems of complex parameter adjustment, non-convergence of calculation and large control error in the prior art. It can be widely used in robot walking control tasks in industries such as industry, services and medical care. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0065] Figure 1 A schematic diagram of a motion control method flow chart provided in an embodiment of the present application;

[0066] Figure 2 A schematic diagram of a double-arm two-link model provided in an embodiment of the present application;

[0067] Figure 3 A schematic diagram of a two-link model of a trunk provided in an embodiment of the present application;

[0068] Figure 4 A schematic diagram of a bipedal three-link provided in an embodiment of the present application;

[0069] Figure 5 A schematic diagram of establishing a projection surface using two-foot grounding rods provided in an embodiment of the present application;

[0070] Figure 6 A schematic diagram of a stable area provided in an embodiment of the present application;

[0071] Figure 7 A schematic diagram of projecting an overall centroid onto a projection plane provided in an embodiment of the present application;

[0072] Figure 8 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0073] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0074] The various embodiments of the present application are described in further detail below in conjunction with the accompanying drawings.

[0075] The present application embodiment provides a motion control method, which is applied to a robot, such as Figure 1 As shown, the method includes steps S101-S104.

[0076] S101: Establishing a stable region of the robot's bipedal stability margin according to the contact points between the robot's bipedal feet and the ground.

[0077] In robot motion control, the key to the dynamic stability of the robot lies in evaluating the relationship between the center of gravity of the robot and the contact area. The embodiment of the present application establishes a stable area for measuring stability by analyzing the contact points between the robot's feet and the ground. Specifically, the stable area is usually based on the contact points of the robot's feet and is defined as a polygon (such as a convex hull), whose boundary is formed by the lines connecting the contact points. This stable area reflects the range in which the robot can maintain stability during walking or standing, and provides a basis for the subsequent motion control strategy of the robot.

[0078] In addition, the definition of the stable area can be dynamically updated to adapt to the different postures and gait changes of the robot. For example, when the robot is supported on one foot, the stable area will be reduced to the area around the contact point of the single foot; when it is supported on both feet, the stable area will expand to the quadrilateral range formed by the contact points of both feet. By dynamically adjusting the stable area, the system can reflect the current support conditions of the robot in real time, provide accurate input data for subsequent joint control torque calculation and gait adjustment, improve the system's adaptability to complex environments, and provide a guarantee for achieving high stability in robot walking.

[0079] S102: Obtain the center of mass position of each connecting rod of the robot, and calculate the overall center of mass of the robot.

[0080] The overall center of mass (CoM) of the robot is the basis for dynamic balance control. In the embodiment of the present application, the center of mass position of each link of the robot can be calculated by its geometric structure and the current posture of the robot. Specifically, the center of mass of each link can be located by obtaining the position information of each joint of the robot and the parameters such as the length, mass and direction of each link.

[0081] In a feasible embodiment, the overall center of mass can be understood as the mass-weighted average of the centers of mass of all connecting rods, and the overall center of mass can intuitively reflect the center of gravity position of the robot in space. Optionally, the center of mass position of each connecting rod of the robot is obtained, and the calculation expression of the overall center of mass CoM of the robot is calculated as:

[0082]

[0083] Among them, m i is the mass of the ith connecting rod, is the center of mass position vector of the ith connecting rod, and n is the total number of connecting rods.

[0084] The position of the overall center of mass directly affects the distribution of the zero moment point (ZMP) and the change of the stability margin during the robot's walking process. During the robot's movement, the posture of each link will continue to change, so the center of mass position will also shift accordingly, and the overall center of mass also needs to be calculated in real time. By accurately calculating the overall center of mass, the system can evaluate the relationship between the center of gravity projection and the stable area in real time, providing reliable input for gait control.

[0085] S103: Calculate the zero moment point of the robot according to the overall center of mass, the position of each contact point of the robot, and the force acting on each contact point.

[0086] The zero moment point indicates the point where the resultant moment of the ground reaction force of the robot in the current state is zero. Specifically, the ZMP position is determined by the position of the robot's overall center of mass, the distribution of the contact points, and the support force it receives. In the embodiment of the present application, the overall center of mass provides information on the point of action of the robot's gravity, while the position of the contact points and the support force determine the distribution of the robot's force system. The combination of the two can accurately calculate the position of the ZMP.

[0087] Optionally, based on the overall center of mass, the position of each contact point of the robot and the force acting on each contact point, the expression for calculating the zero moment point ZMP of the robot is:

[0088]

[0089] Among them, r j is the position of the jth contact point, CoM is the position of the overall center of mass, and F j is the force acting on the jth contact point.

[0090] S104: According to the positional relationship between the zero torque point and the stable area, the control torque of each joint of the robot is calculated using the inverse dynamics model to control the gait of the robot.

[0091] In robot motion control, the positional relationship between the zero torque point and the stable area directly determines the current dynamic stability of the robot. The embodiment of the present application analyzes the position of the ZMP relative to the stable area, combines the inverse dynamics model, calculates the control torque of each joint, and then adjusts the robot gait to ensure its stable walking. The robot's gait here includes walking, turning, uphill, downhill, trotting, running, and jumping.

[0092] Specifically, when the ZMP is inside the stable area, the robot is in a balanced state and the system can maintain the current gait; when the ZMP exceeds the stable area, it indicates that the robot is about to become unstable and needs to adjust the joint torque through the inverse dynamics model to pull the ZMP back to the stable area.

[0093] The inverse dynamics model is based on the inverse dynamics equation of the robot. It derives the control torque required for the joint through the known joint position, speed and target adjustment value. Specifically, the required compensation torque is calculated by combining the distance and direction of the ZMP deviation from the stable area with the dynamic characteristics of the robot. After this compensation torque is distributed to each joint of the robot, the overall gait can be adjusted. For example, when the ZMP deviates from the front boundary, the system can calculate the forward compensation torque of the leg joint to restore the ZMP to the inside of the stable area.

[0094] The embodiment of the present application realizes the whole process from ZMP offset detection to gait adjustment through the above-mentioned control mechanism, so that the robot can maintain dynamic balance in complex environments and respond quickly to external interference and task changes, thereby improving the robustness and adaptability of motion control.

[0095] In one embodiment, before step S101, the method further includes:

[0096] Establish the dynamics model and inverse dynamics model of the robot's arms, torso, and feet;

[0097] Among them, the arms are established as a two-link model, the torso is established as a two-link model, and the feet are established as a three-link model.

[0098] Specifically, please refer to Figure 2 , Figure 3 , Figure 4 . Figure 2 The figure is a schematic diagram of the two-arm two-link model. The two-arm two-link model describes the motion characteristics of the robot's two arms and is constructed by two links d1 and d2 and their corresponding rotation angles θ1 and θ2. d1 is the length of the link from the robot's shoulder joint to the elbow joint, representing the physical length of the upper arm. In the operation task, the length of the upper arm determines the swing range of the upper arm and the initial motion radius of the entire arm. d2 is the length of the link from the robot's elbow joint to the end of the hand (or wrist joint), representing the physical length of the forearm, and determines the size of the robot's workspace, that is, the range of motion of the robot's end effector (such as a gripper or tool). θ1 represents the rotation angle of the shoulder joint relative to the vertical line. By adjusting θ1, the robot can control the swing direction and angle of the upper arm, such as lifting, lowering, or swinging back and forth. θ2 represents the bending angle of the elbow joint, which is used to control the relative movement of the forearm, such as straightening or bending the forearm, and mainly affects the flexibility of the robot arm and the precise positioning ability of the end effector.

[0099] Figure 3The figure is a schematic diagram of the two-link model of the torso. d1 is the distance from the center point of the robot head to the neck joint. d2 is the distance from the neck joint to the center of the robot's chest and abdomen. θ1 is the rotation angle of the head relative to the neck, which is used to control the tilt and rotation of the head. For example, the robot can raise its head, lower its head, or turn sideways by adjusting θ1. θ2 is the rotation angle of the neck joint relative to the chest and abdomen, which determines the relative posture of the neck and torso, and is used to control the tilt of the robot's upper body, such as leaning forward, backward, or bending sideways.

[0100] Figure 4 A schematic diagram of a bipedal three-linkage provided in an embodiment of the present application, d1 represents the distance from the contact point (ground) of the robot's two feet to the ankle joint. d2 is the distance from the ankle joint to the knee joint, representing the length of the robot's calf. d3 represents the distance from the knee joint to the hip joint, describing the length of the robot's thigh. θ1 represents the rotation angle of the ankle joint relative to the ground, which is used to adjust the inclination direction of the robot's foot, maintain dynamic balance or adapt to different terrains. θ2 represents the bending angle of the knee joint, which is mainly used to control the swing and extension movements of the leg. For example, when the robot lifts its foot, θ2 increases; when standing, θ2 decreases to near zero. θ3 represents the rotation angle of the hip joint, which determines the overall swing direction of the leg. By adjusting θ3, the robot can step forward and backward or adjust the width of the gait.

[0101] In one embodiment, the kinetic model includes:

[0102] Among them, the dynamic equation of the two-link is:

[0103]

[0104] Where D is the inertia matrix; represents the angular acceleration of the joint; Describes the inertial force generated during acceleration; C is the Christoffel symbol used to describe the Coriolis force and centrifugal force generated by the robot arm's motion speed; represents the angular velocity of the joint; Describes the Coriolis force and centrifugal force generated by the speed of movement; g is the acceleration due to gravity, and τ is the control torque of the joint;

[0105] The dynamic equation of the three-link is:

[0106]

[0107] Among them, M 11 、M 12 、M 13 represents the inertia coefficient of the first joint; M 21 、M 22 、M23 represents the inertia coefficient of the second joint; M 31 、M 32 、M 33 represents the inertia coefficient of the third joint; represents the angular velocity of the joint; represents the angular acceleration of the joint; H 1 , H 2 , H 3 G represents the Coriolis force and centrifugal force of the first joint, the Coriolis force and centrifugal force of the second joint, and the Coriolis force and centrifugal force of the third joint respectively; 1 , G 2 , G 3 represents the gravity vector of the first joint, the gravity vector of the second joint, and the gravity vector of the third joint respectively; τ 1 , τ 2 , τ 3 represents the control torque of the first joint, the control torque of the second joint, and the control torque of the third joint;

[0108] M 11 、M 12 、M 13 、M 22 、M 23 、M 33 , H 1 , H 2 , H 3 , G 1 , G 2 , G 3 Specifically expressed as:

[0109]

[0110] Among them, θ 1 Expressed as the angle between the first link and the vertical direction; θ 2 is the angle between the first connecting rod and the second connecting rod, θ 3 I represents the angle between the second connecting rod and the third connecting rod; 1 ,I 2 ,I 3 represents the moment of inertia of the first connecting rod, the moment of inertia of the second connecting rod, and the moment of inertia of the third connecting rod respectively; m 1 、m 2 、m 3 represents the mass of the first connecting rod, the mass of the second connecting rod, and the mass of the third connecting rod respectively; l 1 , l 2 , l 3 represents the length of the first connecting rod, the length of the second connecting rod, and the length of the third connecting rod respectively; d 1 , d2 , d 3 It represents the distance from the center of mass to the first joint, the distance from the center of mass to the second joint, and the distance from the center of mass to the third joint respectively;

[0111] Inverse dynamics models, including:

[0112] The anti-dynamic equation of the two-link is:

[0113]

[0114] in, represents the Lagrangian, T is the kinetic energy of the two-link, and V is the gravitational potential energy of the two-link;

[0115]

[0116] in, describes the coupled kinetic energy between the two rods;

[0117] V=(m 1 +m 2 )gl 1 sin(θ 1 )+m 2 gl 2 sin(θ 2 )

[0118] Among them, (m 1 +m 2 )gl 1 sin(θ 1 ) describes the potential energy of the first rod and its load; m 2 gl 2 sin(θ 2 ) describes the potential energy of the second rod;

[0119] The inverse dynamics equation of the three-link is:

[0120]

[0121] in,

[0122] V=m 1 gl 1 sin(θ 1 )+m 2 g(l 1 sin(θ 1 )+l 2 sin(θ 2 ))+m 3 g(l 1 sim(θ 1 )+l2 sin(θ 2 )+l 3 sin(θ 3 )).

[0123] In one embodiment, step S102 specifically includes: establishing a stability margin projection plane using two parallel lines where the robot's two feet contact the ground; within the stability margin projection plane, forming a quadrilateral stability area using the front contact point of the left foot, the rear contact point of the left foot, the front contact point of the right foot, and the rear contact point of the right foot.

[0124] Specifically, please refer to Figure 5 , using the two parallel lines where the robot's feet touch the ground as the reference, a stability margin projection plane is established. As a simplified projection of three-dimensional space, this plane can reflect the geometric relationship between the robot's center of gravity and the support area, and provide a two-dimensional reference frame for evaluating stability. In this way, the complex three-dimensional dynamic balance problem can be transformed into a geometric problem in a two-dimensional plane, which simplifies the complexity of the calculation and facilitates real-time analysis and control.

[0125] Please refer to Figure 6 In the projection plane, the front contact point of the left foot, the rear contact point of the left foot, the front contact point of the right foot, and the rear contact point of the right foot are used as corner points to form a quadrilateral stable area, which identifies the area where the robot can maintain dynamic balance during walking or standing. When the distance between the two foot contact points is large, the stable area increases and the robot has a higher stability margin; on the contrary, when the distance between the contact points decreases, the stable area becomes smaller and the stability margin decreases accordingly.

[0126] In one embodiment, after step 102, the method of the embodiment of the present application also includes: projecting the overall center of mass to the stability margin projection plane, and determining the positional relationship between the overall center of mass and the stable area; if the overall center of mass is within the stable area, determining that the robot is in a stable state; if the overall center of mass is not within the stable area, determining that the robot is in an unstable state.

[0127] In this embodiment, by projecting the overall mass center onto the stability margin projection plane and determining its positional relationship with the stable area, the current dynamic balance state of the robot can be further evaluated. Figure 7Specifically, when the projection point of the overall center of mass is within the stable area, it indicates that the current gravity point of the robot falls within the support area, and sufficient support torque can be provided by the two feet to resist external interference, and the robot is in a stable state. At this time, the system does not need to adjust the gait or torque additionally, and the robot can continue to perform tasks according to the current gait. However, when the projection point of the overall center of mass exceeds the stable area, it indicates that the robot is in an unstable state, which means that external disturbances or gait changes cause the center of gravity to shift, and the system needs to generate control instructions for adjusting the robot's gait by combining the calculation results of ZMP with the inverse dynamics model. For example, by adjusting the joint torque, the center of mass projection point is pulled back into the stable area to restore the robot to a stable state. The embodiment of the present application significantly simplifies the complexity of dynamic balance analysis by using the positional relationship between the projection of the overall center of mass and the stable area as a judgment criterion, and at the same time can more accurately adjust the gait to ensure the stability and robustness of the robot in complex tasks and environments.

[0128] In one embodiment, the method of the embodiment of the present application also includes: calculating the rotational degrees of freedom, angles and axial rotation angles of each joint based on the control torque of each joint; linearly interpolating the calculated rotational degrees of freedom, angles and axial rotation angles of each joint and performing robot gait control.

[0129] In this embodiment, based on the control torque of each joint, the rotational degrees of freedom, angle and axial rotation angle of each joint are further calculated to provide more accurate motion parameters for gait control. Specifically, the control torque is the driving force of the robot joint movement, and its magnitude and direction determine the rotation characteristics of the joint. Through dynamic calculation, the rotational degrees of freedom (DOF, Degree of Freedom) of each joint can be derived, that is, the independent motion dimension that the joint can achieve in space, and the angle change and axial rotation angle between the joints are determined. These parameters together describe the complete motion state of the robot joint and provide key input for gait planning.

[0130] After calculating the rotational degrees of freedom, angles, and axial rotation angles of each joint, the system needs to further perform linear interpolation on these parameters to generate a smooth, continuous angle change trajectory during the robot's joint motion to avoid gait instability or oscillation due to discontinuous joint movements. The interpolation process can smooth the angle changes based on time or gait cycle, ensuring the robot's stability and adaptability in dynamic environments.

[0131] In order to implement the motion control method of the embodiment of the present application, the embodiment of the present application further provides a motion control device, which includes:

[0132] A stable region establishing unit, used for establishing a stable region of the robot's bipedal stability margin according to the contact points between the robot's bipedal feet and the ground;

[0133] An overall center of mass calculation unit, used to obtain the center of mass position of each connecting rod of the robot and calculate the overall center of mass of the robot;

[0134] a zero moment point calculation unit, used for calculating the zero moment point of the robot according to the overall center of mass, the position of each contact point of the robot and the force acting on each contact point;

[0135] The joint torque control unit is used to calculate the control torque of each joint of the robot according to the positional relationship between the zero torque point and the stable area by using an inverse dynamics model to control the gait of the robot.

[0136] In one embodiment, the device further comprises: a model building unit; the model building unit is used to:

[0137] Establish the dynamics model and inverse dynamics model of the robot's arms, trunk, and feet;

[0138] Among them, the arms are established as a two-link model, the torso is established as a two-link model, and the feet are established as a three-link model.

[0139] In one embodiment, the kinetic model comprises:

[0140] Among them, the dynamic equation of the two-link is:

[0141]

[0142] Where D is the inertia matrix; represents the angular acceleration of the joint; Describes the inertial force generated during acceleration; C is the Christoffel symbol used to describe the Coriolis force and centrifugal force generated by the robot arm's motion speed; represents the angular velocity of the joint; Describes the Coriolis force and centrifugal force generated by the speed of movement; g is the acceleration due to gravity, and τ is the control torque of the joint;

[0143] The dynamic equation of the three-link is:

[0144]

[0145] Among them, M 11 、M 12 、M 13 Indicates the inertia coefficient of the first joint; M 21 、M 22 、M 23represents the inertia coefficient of the second joint; M 31 、M 32 、M 33 represents the inertia coefficient of the third joint; represents the angular velocity of the joint; represents the angular acceleration of the joint; H 1 , H 2 , H 3 G represents the Coriolis force and centrifugal force of the first joint, the Coriolis force and centrifugal force of the second joint, and the Coriolis force and centrifugal force of the third joint respectively; 1 , G 2 , G 3 represents the gravity vector of the first joint, the gravity vector of the second joint, and the gravity vector of the third joint respectively; τ 1 , τ 2 , τ 3 represents the control torque of the first joint, the control torque of the second joint, and the control torque of the third joint;

[0146] M 11 、M 12 、M 13 、M 22 、M 23 、M 33 , H 1 , H 2 , H 3 , G 1 , G 2 , G 3 Specifically expressed as:

[0147]

[0148] Among them, θ 1 Expressed as the angle between the first link and the vertical direction; θ 2 is the angle between the first connecting rod and the second connecting rod, θ 3 I represents the angle between the second connecting rod and the third connecting rod; 1 ,I 2 、i 3 represents the moment of inertia of the first connecting rod, the moment of inertia of the second connecting rod, and the moment of inertia of the third connecting rod respectively; m 1 、m 2 、m 3 represents the mass of the first connecting rod, the mass of the second connecting rod, and the mass of the third connecting rod respectively; l 1 , l 2 , l 3 represents the length of the first connecting rod, the length of the second connecting rod, and the length of the third connecting rod respectively; d 1 , d2 , d 3 successively represent the distance from the overall centroid to the first joint, the distance from the overall centroid to the second joint, and the distance from the overall centroid to the third joint;

[0149] The inverse dynamics model includes:

[0150] The inverse dynamics equation of the two-link is:

[0151]

[0152] where represents the Lagrangian, T is the kinetic energy of the two-link, and V is the gravitational potential energy of the two-link;

[0153]

[0154] where describes the coupled kinetic energy between the two rods;

[0155] V = (m 1 + m 2 )gl 1 sin(θ 1 ) + m 2 gl 2 sin(θ 2 )

[0156] where, (m 1 + m 2 )gl 1 sin(θ 1 ) describes the potential energy of the first rod and its load; m 2 gl 2 sin(θ 2 ) describes the potential energy of the second rod;

[0157] The inverse dynamics equation of the three-link is:

[0158]

[0159] where

[0160] V = m 1 gl 1 sin(θ 1 ) + m 2 g(l 1 sin(θ 1 ) + l 2 sin(θ 2 )) + m 3 g(l 1 sin(θ 1)+l 2 sin(θ 2 )+l 3 sin(θ 3 )).

[0161] In one embodiment, the stable region establishing unit is used to:

[0162] Establishing a stability margin projection plane using two parallel lines where the robot's two feet touch the ground;

[0163] In the stability margin projection plane, a quadrilateral stable area is formed by the front contact point of the left foot, the rear contact point of the left foot, the front contact point of the right foot and the rear contact point of the right foot.

[0164] In one embodiment, the device further includes: a processing unit; the processing unit is configured to:

[0165] Projecting the overall centroid onto the stability margin projection plane, and determining the positional relationship between the overall centroid and the stable area;

[0166] If the overall center of mass is within the stable region, it is determined that the robot is in a stable state;

[0167] If the overall center of mass is not within the stable region, it is determined that the robot is in an unstable state.

[0168] In one embodiment, the calculation expression for obtaining the center of mass position of each connecting rod of the robot and calculating the center of mass of the entire robot is:

[0169]

[0170] Among them, m i is the mass of the ith rod, is the center of mass position vector of the ith rod, and n is the total number of connecting rods.

[0171] In one embodiment, the expression for calculating the zero moment point of the robot according to the overall center of mass, the position of each contact point of the robot and the force acting on each contact point is:

[0172]

[0173] Among them, r j is the position of the jth contact point, CoM is the position of the overall center of mass, F j is the force acting on the jth contact point.

[0174] In one embodiment, the processing unit is further configured to:

[0175] Calculate the rotational freedom, angle and axial rotation angle of each joint according to the control torque of each joint;

[0176] The rotational degrees of freedom, angles, and axial rotation angles of each joint are calculated and linearly interpolated to perform robot gait control.

[0177] It should be noted that: the above embodiment provides a motion control device, and only uses the division of the above program modules as an example to illustrate when performing motion control. In actual applications, the above processing can be assigned to different program modules as needed, that is, the internal structure of the device is divided into different program modules to complete all or part of the processing described above. In addition, the motion control device and the motion control method embodiment provided in the above embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, which will not be repeated here.

[0178] The embodiment of the present application further provides a robot, specifically comprising: a trunk of the robot, two arms connected to the trunk, and two feet connected to the trunk;

[0179] The torso of the robot includes the head, chest and abdomen of the robot; the head includes eyes, nose, mouth, ears, forehead and hair;

[0180] The robot's arms include shoulders, upper arms, elbows, lower arms, wrists, hands, and fingers;

[0181] The robot's bipedal feet include the robot's hip, thigh, knee, calf, ankle, and foot;

[0182] The robot moves by any one of the above-mentioned motion control methods.

[0183] Based on the hardware implementation of the above program modules, and in order to implement a motion control method provided in an embodiment of the present application, an embodiment of the present application further provides an electronic device, such as Figure 3 As shown, the electronic device 300 includes:

[0184] CPU 301, memory 302 and input / output interface 303;

[0185] The memory 302 is a temporary storage memory or a permanent storage memory;

[0186] The central processor 301 is configured to communicate with the memory 302 and execute instructions in the memory 302 to perform any one of the above motion control methods.

[0187] Of course, in actual application, the various components in the electronic device 300 are coupled together through the bus system 304. It can be understood that the bus system 304 is used to realize the connection and communication between these components. In addition to the data bus, the bus system 304 also includes a power bus, a control bus and a status signal bus. However, for the sake of clarity, Figure 3 Various buses are labeled as bus system 304 .

[0188] The memory 302 in the embodiment of the present application is used to store various types of data to support the operation of the electronic device 300. Examples of such data include: any computer program used to operate on the electronic device 300.

[0189] It is understandable that when the processor in the electronic device described above executes the computer program, it can also implement the functions of the various units in the above-mentioned corresponding device embodiments, which will not be repeated here. Exemplarily, the computer program can be divided into one or more modules / units, and one or more modules / units are stored in the memory and executed by the processor to complete the various embodiments of the present application. One or more modules / units can be a series of computer program instruction segments that can perform specific functions, and the instruction segments are used to describe the execution process of the computer program in the electronic device. For example, the computer program can be divided into the various units in the above-mentioned electronic device, and each unit can implement the specific functions described in the above-mentioned corresponding electronic device.

[0190] The electronic device may be a computing device such as a desktop computer, a notebook, a PDA, and a cloud server. The electronic device may include, but is not limited to, a processor and a memory. Those skilled in the art will appreciate that the processor and the memory are merely examples of electronic devices and do not constitute a limitation on the electronic device. The electronic device may include more or fewer components, or a combination of certain components, or different components. For example, the electronic device may also include input and output devices, network access devices, buses, etc.

[0191] The processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc. The processor is the control center of an electronic device, and uses various interfaces and lines to connect various parts of the entire electronic device.

[0192] The memory can be used to store computer programs and / or modules. The processor realizes various functions of the electronic device by running or executing the computer programs and / or modules stored in the memory, and calling the data stored in the memory. The memory can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, at least one application required for a function, etc.; the data storage area can store data created according to the use of the terminal, etc. In addition, the memory can include a high-speed random access memory, and can also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (SMC, Smart Media Card), a secure digital (SD, Secure Digital) card, a flash card (FlashCard), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0193] An embodiment of the present application further provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, any one of the above-mentioned motion control methods is executed.

[0194] An embodiment of the present application also provides a computer program product having a computer program / instruction stored thereon. When the computer program / instruction is executed by a processor, it is used to implement the motion control method described in the first aspect of the embodiment of the present application or any specific implementation method of the first aspect.

[0195] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0196] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0197] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0198] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0199] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable an electronic device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code.

Claims

1. A motion control method, characterized in that: Applied to a robot, the method comprises: Establishing a stable region of the robot's bipedal stability margin according to the contact points between the robot's bipedal feet and the ground; Obtaining the center of mass position of each connecting rod of the robot and calculating the overall center of mass of the robot; Calculating a zero moment point of the robot according to the overall mass center, the position of each contact point of the robot, and the force acting on each contact point; According to the positional relationship between the zero torque point and the stable area, the control torque of each joint of the robot is calculated using an inverse dynamics model to control the gait of the robot.

2. The motion control method according to claim 1, characterized in that: Before establishing a stable region of the robot's bipedal stability margin according to the contact points between the robot's bipedal feet and the ground, the method further includes: Establish the dynamics model and inverse dynamics model of the robot's arms, torso, and feet; Among them, the arms are established as a two-link model, the torso is established as a two-link model, and the feet are established as a three-link model.

3. The motion control method according to claim 2, characterized in that: The kinetic model, include: Among them, the dynamic equation of the two-link is: Where D is the inertia matrix; represents the angular acceleration of the joint; Describes the inertial force generated during acceleration; C is the Christoffel symbol used to describe the Coriolis force and centrifugal force generated by the robot arm's motion speed; represents the angular velocity of the joint; Describes the Coriolis force and centrifugal force generated by the speed of movement; g is the acceleration due to gravity, and τ is the control torque of the joint; The dynamic equation of the three-link is: Among them, M 11 、M 12 、M 13 Indicates the inertia coefficient of the first joint; M 21 、M 22 、M 23 represents the inertia coefficient of the second joint; M 31 、M 32 、M 33 represents the inertia coefficient of the third joint; represents the angular velocity of the joint; represents the angular acceleration of the joint; H1, H2, H3 represent the Coriolis force and centrifugal force of the first joint, the Coriolis force and centrifugal force of the second joint, and the Coriolis force and centrifugal force of the third joint respectively; G1, G2, G3 represent the gravity vector of the first joint, the gravity vector of the second joint, and the gravity vector of the third joint respectively; τ1, τ2, τ3 represent the control torque of the first joint, the control torque of the second joint, and the control torque of the third joint; M 11 、M 12 、M 13 、M 22 、M 23 、M 33 , H1, H2, H3, G1, G2, G3 are specifically expressed as: Among them, θ1 represents the angle between the first link and the vertical direction; θ2 represents the angle between the first link and the second link, and θ3 represents the angle between the second link and the third link; I1, I2, and I3 represent the moment of inertia of the first link, the moment of inertia of the second link, and the moment of inertia of the third link respectively; m1, m2, and m3 represent the mass of the first link, the mass of the second link, and the mass of the third link respectively; l1, l2, and l3 represent the length of the first link, the length of the second link, and the length of the third link respectively; d1, d2, and d3 represent the distance from the overall center of mass to the first joint, the distance from the overall center of mass to the second joint, and the distance from the overall center of mass to the third joint respectively; The inverse dynamics model comprises: The two-link counterdynamics equation is: in, represents the Lagrangian, T is the kinetic energy of the two-link, and V is the gravitational potential energy of the two-link; in, describes the coupled kinetic energy between the two rods; V=(m1+m2)gl1sin(θ1)+m2gl2sin(θ2) Here, (m1+m2)gl1sin(θ1) describes the potential energy of the first rod and its load; m2gl2sin(θ2) describes the potential energy of the second rod; The inverse dynamics equation of the three-link is: in, V=m1gl1sin(θ1)+m2g(l1sin(θ1)+l2sin(θ2))+m3g(l1sin(θ1)+l2sin(θ2)+l3sin(θ3)).

4. The motion control method according to claim 1, characterized in that: The step of establishing a stable region of stability margin of the robot's two feet according to the contact points between the robot's two feet and the ground comprises: Establishing a stability margin projection plane using two parallel lines where the robot's two feet touch the ground; In the stability margin projection plane, a quadrilateral stable area is formed by the front contact point of the left foot, the rear contact point of the left foot, the front contact point of the right foot and the rear contact point of the right foot.

5. The motion control method according to claim 4, characterized in that: After obtaining the center of mass position of each connecting rod of the robot and calculating the center of mass of the robot as a whole, the method further includes: Projecting the overall centroid onto the stability margin projection plane, and determining the positional relationship between the overall centroid and the stable area; If the overall center of mass is within the stable region, it is determined that the robot is in a stable state; If the overall center of mass is not within the stable region, it is determined that the robot is in an unstable state.

6. The motion control method according to claim 1, characterized in that: The calculation expression for obtaining the center of mass position of each connecting rod of the robot and calculating the center of mass of the entire robot is: Among them, m i is the mass of the ith rod, is the center of mass position vector of the ith rod, and n is the total number of connecting rods.

7. The motion control method according to claim 1, characterized in that: The expression for calculating the zero moment point of the robot according to the overall mass center, the position of each contact point of the robot and the force acting on each contact point is: Among them, r j is the position of the jth contact point, CoM is the position of the overall center of mass, F j is the force acting on the jth contact point.

8. The motion control method according to claim 1, characterized in that: The method further comprises: Calculate the rotational freedom, angle and axial rotation angle of each joint according to the control torque of each joint; The rotational degrees of freedom, angles, and axial rotation angles of each joint are calculated and linearly interpolated to perform robot gait control.

9. An electronic device, characterized in that: include: CPU, memory and input / output interface; The memory is a short-term storage memory or a persistent storage memory; The central processing unit is configured to communicate with the memory and execute instruction operations in the memory to perform the motion control method according to any one of claims 1 to 8.

10. A robot, characterized in that: include: A torso of the robot, two arms connected to the torso, and two feet connected to the torso; The torso of the robot includes the head, chest and abdomen of the robot; the head includes eyes, nose, mouth, ears, forehead and hair; The robot's arms include shoulders, upper arms, elbows, lower arms, wrists, hands, and fingers; The robot's bipedal feet include the robot's hip, thigh, knee, calf, ankle, and foot; The robot moves by the motion control method as described in any one of claims 1-8.

Images