Heavy-load leg-foot robot control method and system considering leg momentum, storage medium and program product
By optimizing the control strategy of heavy-load leg foot robots and considering the changes in leg momentum, the problems of excessive joint torque and low movement efficiency in the prior art are solved, and more efficient movement and joint protection are achieved.
Patent Information
- Application Number
- CN202411048803.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-01
- Publication Date
- 2024-12-03
AI Technical Summary
The prior art failed to fully consider the change in leg momentum when developing large heavy-duty leg robots, resulting in the need of joint actuators to provide huge torque, increase the risk of wear and failure, and inefficient movement.
By constructing the optimal control problem, the joint speed cost, input force cost and trajectory follow-up cost based on leg momentum is used to calculate the centroid momentum matrix and projection matrix of the leg centroid, optimize the control strategy of the heavy-load leg foot robot, and reduce joint output Capacity requirements.
While maintaining the trajectory following ability, the joint output torque and output power are significantly reduced, the motion efficiency is improved, the joints are protected, and premature wear and failure are avoided.
Smart Images

Figure CN119065239A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of robot control, and more specifically, relates to a control method, system, storage medium and program product for a heavy-load leg-foot robot taking into account leg momentum. Background Art
[0002] Heavy-duty legged robots are widely used for cargo transportation in emergency and disaster relief due to their excellent carrying capacity, especially in unstructured terrain. However, developing large heavy-duty legged robots capable of agile movements faces multiple challenges, mainly due to the scale effect of the robot system and the heavy leg structure. In order to effectively carry the weight, the robot needs a strong leg structure, which in turn increases the demand for control strategies and actuator capabilities.
[0003] Trajectory optimization techniques have been proposed for planning the movements of the support leg and the swing leg, generating states and control trajectories by solving the optimal control problem in real time. Traditional trajectory optimization methods often optimize the support leg and the swing leg separately when designing control strategies, and fail to fully consider the coupling effect between the two in high-dynamic motion. Although the trajectory optimization method based on whole-body kinematics plus center of mass dynamics takes this coupling into account and provides a method for simultaneously planning the movement of the support leg and the swing leg, when faced with robots with thick legs, the existing trajectory optimization methods based on whole-body kinematics plus center of mass dynamics often use the joint end velocity or joint velocity as the movement cost. This construction of movement cost ignores the dynamic characteristics of the leg itself, often resulting in the leg hip joint actuator needing to provide huge torque to meet the requirements of the movement, thereby placing higher performance requirements on the actuator and may cause premature wear or failure of the actuator. Summary of the invention
[0004] In response to the above defects or improvement needs of the prior art, the present invention provides a control method, system, storage medium and program product for a heavy-loaded legged robot that takes into account leg momentum, with the aim of minimizing the demand for joint output capacity when the heavy-loaded legged robot achieves agile movement.
[0005] To achieve the above object, according to a first aspect of the present invention, a control method for a heavy-load legged robot taking into account leg momentum is proposed, comprising the following steps:
[0006] The optimal control problem is constructed by taking the minimum sum of the stage cost and the terminal cost of the legged robot during its movement as the objective function;
[0007] The stage cost includes the joint speed cost based on leg momentum, the input force cost and the trajectory following cost; the joint speed cost based on leg momentum L momentum The calculation formula for (x,u) is: Among them, x is the state vector, u is the input vector, is the momentum matrix A of the center of mass of the leg CL The block matrix corresponding to the actual joint part, is the actual joint velocity, h CL,ref (t) is the reference leg center of mass momentum, D is the weight matrix, ║║ 2 represents the two-norm;
[0008] The optimal control problem is transformed and solved to obtain the corresponding input vector, and the leg-foot robot is controlled based on the input vector.
[0009] As a further preferred embodiment, the leg center of mass momentum matrix A CL The calculation formula is:
[0010]
[0011] Among them, X CL is the projection matrix that projects the system momentum onto the instantaneous center of mass of the leg, T represents the transpose, I is the system inertia matrix, and J is the system Jacobian matrix.
[0012] As a further preferred embodiment, based on the center of mass momentum projection matrix of all legs in the leg-foot robot system, the projection matrix X that projects the system momentum to the instantaneous center of mass of the leg is determined. CL ;
[0013] The center of mass momentum projection matrix of the kth leg in the leg-foot robot system The calculation formula is:
[0014]
[0015] in, is the coordinate system from the i-th link to the center of mass coordinate system of the k-th leg L k The space momentum transformation matrix of represents the index of the first link in the k-th leg connected to the body, It represents the number of connecting rods contained in the kth leg, and N is the actual total number of connecting rods.
[0016] As a further preferred embodiment, the input force cost L input The calculation formula for (x,u) is:
[0017]
[0018] Where u(t) is the control input force, u ref (t) is the reference input force and R is the input force weight matrix.
[0019] As a further preferred embodiment, the trajectory following cost L trajectoryThe calculation formula for (x,u) is:
[0020]
[0021] Among them, x(t) is the system state, x ref (t) is the reference trajectory and P is the state weight matrix.
[0022] As a further preference, in the optimal control problem, the constraints include friction cone constraint, contact point no-slip constraint, output limit constraint and swing leg constraint.
[0023] As a further preferred embodiment, the SQP method is used to transform and solve the optimal control problem to obtain a corresponding input vector.
[0024] According to a second aspect of the present invention, there is provided a control system for a heavy-loaded legged robot taking into account leg momentum, comprising a processor for executing the above-mentioned control method for a heavy-loaded legged robot taking into account leg momentum.
[0025] According to a third aspect of the present invention, there is provided a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned control method for a heavy-loaded legged robot taking into account the momentum of the legs.
[0026] According to a fourth aspect of the present invention, there is provided a computer program product, characterized in that it comprises a computer program, which, when executed by a processor, implements the above-mentioned method for controlling a heavy-loaded legged robot taking into account the momentum of the legs.
[0027] In general, the above technical solution conceived by the present invention has the following technical advantages compared with the prior art:
[0028] When humans run fast, they touch the ground with their toes to minimize the change in the momentum of the swinging legs, thereby effectively protecting the leg muscles. In view of this feature, the present invention designs a speed cost function based on the change in the momentum of the center of mass of the legs. Compared with the existing control methods that use the joint speed or terminal speed as the movement cost, the control method of the present invention that uses the momentum of the center of mass of the legs as the movement cost can fully reduce the change in the momentum of the legs while keeping the overall momentum fluctuation of the robot as small as possible, so that the heavy-duty footed machine with thick legs can minimize the demand for joint output capacity when achieving agile movement, significantly reducing the maximum joint torque required for the ankle joint to achieve the same body movement, protecting the joint and avoiding injury. At the same time, the method of the present invention reduces the output power without losing the trajectory following ability, thereby improving the movement efficiency. BRIEF DESCRIPTION OF THE DRAWINGS
[0029] Figure 1 This is a schematic diagram of the configuration of a heavy-duty hexapod robot according to an embodiment of the present invention;
[0030] Figure 2 It is a schematic diagram of the body trajectory following effect when the traditional speed motion cost and the leg center of mass momentum motion cost are used to achieve the same body motion target in an embodiment of the present invention;
[0031] Figure 3 It is a schematic diagram of joint output torque when the traditional speed motion cost and the leg center of mass momentum motion cost are used to achieve the same body motion target in an embodiment of the present invention;
[0032] Figure 4 It is a schematic diagram of joint output speed when the traditional speed motion cost and the leg center of mass momentum motion cost are used to achieve the same body motion target in an embodiment of the present invention. DETAILED DESCRIPTION
[0033] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended 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.
[0034] A control method for a heavy-load legged robot taking into account leg momentum is provided in an embodiment of the present invention, comprising the following steps:
[0035] 1. System modeling:
[0036] 1.1 The legged robot consists of N+1 links and N interconnected joints, forming a tree structure topology. The movement of the links is referenced to a fixed base (inertial frame), with the fixed base as a virtual link and marked as 0. The links extending from the robot body to the end are marked from 1 to N in sequence. Each link is attached to a coordinate system to provide a reference for the quantities related to the link. The joints of the body (LINK1) in the inertial frame are virtual joints.
[0037] Therefore, among the N+1 links and N joints of the legged robot, there is one virtual link and one virtual joint, and the remaining N actual links and N-1 actual joints (in this embodiment, all the active joints of the robot).
[0038] The system velocity v is determined by the kinematic rotation v of the N connecting rods. i The stack is composed, referring to the coordinate system of each link:
[0039]
[0040] Joint Speed Indicates the six-dimensional virtual joint velocity of the body in the joint space and actual joint velocity
[0041]
[0042] System velocity v and joint velocity The system Jacobian matrix J can be used to convert them:
[0043]
[0044] System momentum vector It consists of the momentum stack of N links and can be represented by the product of the system velocity and the system inertia matrix:
[0045] h=Iv (4)
[0046] The system inertia matrix Contains the inertia matrix of the N links.
[0047] The robot's center of mass momentum Described in the centroid coordinate system, through the projection matrix Project the system momentum onto the instantaneous center of mass:
[0048]
[0049] Projection matrix X G It is defined as the momentum projection matrix from the link coordinate system to the center of mass coordinate system, which is in the form of:
[0050]
[0051] Each of these Represents the spatial momentum conversion from the i-th link coordinate system to the center of mass coordinate system G:
[0052]
[0053] in, G R i ∈SO(3) is the rotation matrix from the ith link to the center of mass, i p G is the vector from the center of mass to the origin of the ith link, [·] × Represents the cross product operator.
[0054] Center of mass momentum matrix (CMM) A G for:
[0055]
[0056] Through equations (3), (4) and (5), and joint speed The mapping relationship between them is:
[0057]
[0058] According to center-of-mass dynamics, the rate of change of the robot's center-of-mass momentum is equal to the net external force and net external torque acting at the center of mass:
[0059]
[0060] in, Indicates the contact point c i is the position vector relative to the center of mass, and is the environment at the contact point c i Contact force acting on the robot, g = [0, 0, -g] T is the gravity vector, g is the gravity constant, and m is the mass of the robot.
[0061] 1.2 Use state variables to represent the system's dynamic equations and define the state vector and input vector:
[0062]
[0063] The external support force input is composed of a stack of three-dimensional support force vectors at each contact point.
[0064] 1.3 Based on the mass center momentum of the connecting rod in 1.1, the present invention further derives the mapping matrix of the mass center momentum of the leg and converts the virtual joint velocity and actual joint velocity The contribution to the momentum of the center of mass of the legs is separated.
[0065] The center of mass momentum of the kth leg In the mass center coordinate system L of the kth leg k The origin of this coordinate system is at the instantaneous center of mass of the entire k-th leg system, which is parallel to the inertial coordinate system. The momentum of the leg center of mass can be obtained by projecting the momentum of the leg system to the instantaneous center of mass of the leg through the projection matrix:
[0066]
[0067] in, is defined as the center-of-mass momentum projection matrix of the kth leg. The kth leg system is a subsystem of the robot system G, consisting of all the links contained in the kth leg. Here, is the index of the first link in the k-th leg connected to the body, is the number of links contained in the kth leg.
[0068] The center-of-mass momentum of k legs is stacked up to obtain the center-of-mass momentum of the entire leg system h CL , through the projection matrix Project the system momentum to the instantaneous center of mass of the leg:
[0069]
[0070] According to equations (3), (4) and (14), it can be deduced that h CL and joint speed The mapping relationship between them is:
[0071]
[0072] That is, the leg center of mass momentum matrix (LCMM) A CL for:
[0073]
[0074] In order to distinguish the contribution of leg momentum to the virtual body joint velocity and the actual joint velocity, A CL In block matrix form, the virtual joint velocity can be expressed as and actual joint velocity The contribution to the momentum of the center of mass of the legs is separated:
[0075]
[0076]
[0077] 2. Construction of optimal control problem:
[0078] An optimal control problem (OCP) is constructed to optimize the motion performance of a heavy-load legged robot. The optimization objective is to minimize the cost function, which is the sum of the stage cost and the terminal cost.
[0079] The basic form of the optimal control problem is as follows:
[0080]
[0081] Where L(x(t),u(t),t) and Φ(x(T)) are the stage cost and terminal cost, respectively. g1(x(t),u(t),t) and g2(x(t),t) are the state-input equation constraint and pure state equation constraint, respectively. h(x(t),u(t),t) is the state-input inequality constraint. x0 represents the initial state of the system, x(t) is the system state, u(t) is the control input force, t is the time, and T is the total duration of the optimized trajectory. F(x(t),u(t),t) represents the system input-output equation composed of the system dynamics described by equation (10) and the whole body kinematics equation. The specific form is:
[0082]
[0083] 2.1 Cost Function
[0084] The stage cost in formula (20) includes joint velocity cost, input force cost and trajectory following cost, that is, the stage cost is the sum of the following three cost functions:
[0085] 1) Joint speed cost:
[0086]
[0087] in, is the joint velocity, is the reference joint velocity, and Q is the velocity weight matrix. This general joint velocity cost only includes the kinematic characteristics of the system but ignores the dynamic characteristics of the system, resulting in drastic changes in the swing leg joint torque part that is not modeled in the system input and output.
[0088] From formula (19), we can know that the leg center of mass momentum contributed by the actual joint velocity is is the actual joint velocity Therefore, the leg momentum change cost can be regarded as a special form of joint speed cost. The present invention adopts the following joint speed cost based on leg momentum change:
[0089]
[0090] Among them, h CL,ref (t) is the reference leg center of mass momentum, and D is the weight matrix.
[0091] 2) Input force cost:
[0092]
[0093] Where u(t) is the control input force, u ref (t) is the reference input force and R is the input force weight matrix.
[0094] 3) Trajectory following cost:
[0095]
[0096] Among them, x(t) is the system state, x ref (t) is the reference trajectory and P is the state weight matrix.
[0097] 2.2 Constraints of the optimization problem:
[0098] The optimal control problem of the present invention specifically adopts the following constraints:
[0099] 1) Friction cone constraint:
[0100] The contact force must be within the friction cone to ensure that the robot does not slip on the ground:
[0101]
[0102] Among them, μ s is the friction coefficient, ε is the relaxation operator, c i The ground reaction forces at the foot of the supporting leg in the x, y, and z directions are perpendicular to each other, and the z direction is perpendicular to the ground.
[0103] 2) No slip constraint at contact point:
[0104] To ensure that the contact points do not slip during contact:
[0105] v c =0 (26)
[0106] Among them, v c is the velocity of the contact point c.
[0107] 3) Output limit constraints:
[0108] Since the motor has a maximum output torque and a maximum rotation speed, the optimization problem needs to ensure that the output can be truly responded by the motor:
[0109]
[0110] Among them, v jmax is the maximum joint velocity vector, τ max is the maximum joint torque vector.
[0111] 4) Swinging leg restraint:
[0112] To ensure that some legs are lifted for a certain period of time without generating support forces:
[0113]
[0114] in, is the ground normal, v * (t) is the artificially designed reference speed along the ground normal, v ci c i No. Swing leg end speed.
[0115] 3. Use the SQP method to transform and solve the above optimal control problem, obtain the input vector u corresponding to the minimum objective function, and achieve optimal control of the leg-foot robot.
[0116] In order to verify the effectiveness of the proposed control method for heavy-load legged robots considering leg momentum, several simulation experiments were conducted. The following are the detailed steps and result analysis of the experiments:
[0117] Experimental setup
[0118] 1. Experimental platform: The experiment is only carried out in a simulation environment and does not involve actual robots. A heavy-duty hexapod robot is used as a simulation platform, such as Figure 1 shown.
[0119] 2. Simulation environment: The simulation is carried out in the Gazebo environment, using a control algorithm based on the center of mass dynamics model. Gazebo provides realistic physical simulation, making the experimental results more practical.
[0120] 3. Controller design: The controller is designed based on the optimal control problem (OCP), and three different speed cost functions are considered for comparison: joint speed cost function, end-effector speed cost function, and speed cost function based on the change of leg center of mass momentum to optimize the robot's motion performance.
[0121] Experimental procedures
[0122] 1. Initial state setting:
[0123] 1.1 Place the robot in simulation on a flat surface and set its initial position and posture.
[0124] 1.2 Initialize joint angles and velocities to ensure the robot is in a stable state.
[0125] 2. Gait planning:
[0126] 2.1 Three gait types are adopted, including three-point contact gait, diagonal gait and wave gait.
[0127] 2.2 Predefine contact sequences for each gait type. These contact sequences are used to form optimization problems and generate specific motion trajectories during the optimization process.
[0128] 3. Control strategy application:
[0129] Three different types of speed cost functions are designed:
[0130] Joint velocity cost function: Considers the deviation of joint velocity and aims to minimize the change of joint velocity.
[0131] End-effector velocity cost function: Considers the deviation of the end-effector velocity and aims to minimize the end-effector velocity variation.
[0132] Speed cost function based on the change of leg center of mass momentum (method of the present invention): taking the change of leg center of mass momentum into consideration, with the goal of minimizing the change of leg center of mass momentum.
[0133] It should be noted that the above cost function only refers to the change of the joint velocity cost. The terminal cost is consistent with the trajectory following cost and output force cost in the stage cost.
[0134] The optimal control method is used to calculate the optimal input of each joint; the joint input is transmitted to the robot actuator in the simulation to control the robot to move according to the optimized trajectory.
[0135] 4. Data collection and analysis:
[0136] During the simulation process, data such as joint angle, joint velocity, leg center of mass momentum and joint output torque are collected in real time.
[0137] The maximum torque of the ankle joint is analyzed in detail, and the torque changes under three different speed cost functions are compared.
[0138] Analyze the advantage of the velocity cost function based on the change of the leg center of mass momentum in reducing the maximum moment of the ankle joint.
[0139] Experimental Results
[0140] 1. Track following effect:
[0141] like Figure 2 As shown in the figure, compared with the traditional method of using the leg center of mass momentum as the speed cost function and the joint speed cost and the terminal speed cost as the speed cost function, the simulated robot can better follow the generated trajectory, and the trajectory error is within an acceptable range.
[0142] 2. Comparative analysis of speed cost function:
[0143] like Figure 3 As shown:
[0144] Under the joint velocity cost function, the joint torques of HFE and KFE are larger, especially during fast movements.
[0145] Under the end-effector velocity cost function, the joint torques of HFE and KFE are reduced, but still large.
[0146] Under the velocity cost function based on the change of the leg center of mass momentum, the HFE and KFE joint torques are significantly reduced, especially the maximum torque of the ankle joint. This is consistent with the characteristics of biological movement, which protects the joints by reducing the change of the leg center of mass momentum and making them less prone to injury.
[0147] like Figure 4 As shown:
[0148] Compared with the control strategy under the joint velocity cost function, the velocity fluctuations of all joints under the control strategy under the velocity cost function based on the change of the leg center of mass momentum and the end effector velocity cost function were significantly reduced.
[0149] Compared with the control strategy under the end effector velocity cost function, the control strategy under the velocity cost function based on the change of the leg mass center momentum has a larger velocity change on the AFE joint. However, since the mass of the end effector is lighter than that of the whole leg, Figure 3 It can be seen from the corresponding part that the maximum torque of the output joint is not significantly increased.
[0150] Through the above experiments, the effectiveness of the control method of the heavy-load legged robot considering the change of the leg center of mass momentum was verified. The experimental results show that the velocity cost function based on the change of the leg center of mass momentum can significantly reduce the maximum torque of the ankle joint, thereby protecting the joint and avoiding injury. At the same time, this method can also improve the robot's trajectory following ability.
[0151] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. A control method for a heavy-load legged robot considering leg momentum, It is characterized in that The steps include: The optimal control problem is constructed by taking the minimum sum of the stage cost and the terminal cost of the legged robot during its movement as the objective function; The stage cost includes the joint speed cost based on leg momentum, the input force cost and the trajectory following cost; the joint speed cost based on leg momentum L momentum The calculation formula for (x,u) is: Among them, x is the state vector, u is the input vector, is the momentum matrix A of the center of mass of the leg CL The block matrix corresponding to the actual joint part, is the actual joint velocity, h CL,ref (t) is the reference leg center of mass momentum, t is the time, D is the weight matrix, ║ ║ 2 represents the two-norm; The optimal control problem is transformed and solved to obtain the corresponding input vector, and the leg-foot robot is controlled based on the input vector.
2. The control method of a heavy-load legged robot considering leg momentum as claimed in claim 1, It is characterized in that The leg center of mass momentum matrix A CL The calculation formula is: Among them, X CL is the projection matrix that projects the system momentum onto the instantaneous center of mass of the leg, T represents the transpose, I is the system inertia matrix, and J is the system Jacobian matrix.
3. The control method of a heavy-load legged robot considering leg momentum as claimed in claim 2, It is characterized in that Based on the center of mass momentum projection matrix of all legs in the leg-footed robot system, determine the projection matrix X that projects the system momentum to the instantaneous center of mass of the leg CL ; The center of mass momentum projection matrix of the kth leg in the leg-foot robot system The calculation formula is: in, is the coordinate system from the i-th link to the center of mass coordinate system of the k-th leg L k The space momentum transformation matrix is: represents the index of the first link in the k-th leg connected to the body, n Lk It represents the number of connecting rods contained in the kth leg, and N is the actual total number of connecting rods.
4. The control method for a heavy-load legged robot taking into account leg momentum as claimed in claim 1, It is characterized in that The input force cost L input The calculation formula for (x,u) is: Where u(t) is the control input force, u ref (t) is the reference input force and R is the input force weight matrix.
5. The control method of a heavy-load legged robot considering leg momentum as claimed in claim 1, It is characterized in that The trajectory following cost L trajectory The calculation formula for (x,u) is: Among them, x(t) is the system state, x ref (t) is the reference trajectory and P is the state weight matrix.
6. The control method of a heavy-load legged robot considering leg momentum as claimed in claim 1, It is characterized in that In the optimal control problem, the constraints include friction cone constraint, contact point no-slip constraint, output limit constraint and swing leg constraint.
7. The control method for a heavy-load legged robot taking into account the momentum of the legs as described in any one of claims 1 to 6, It is characterized in that The SQP method is used to transform and solve the optimal control problem to obtain the corresponding input vector.
8. A control system for a heavy-load legged robot taking into account leg momentum. It is characterized in that It includes a processor, which is used to execute the heavy-load legged robot control method considering leg momentum as described in any one of claims 1-7.
9. A computer-readable storage medium having a computer program stored thereon, It is characterized in that When the computer program is executed by a processor, the method for controlling a heavy-loaded legged robot taking into account leg momentum as described in any one of claims 1 to 7 is implemented.
10. A computer program product, It is characterized in that It comprises a computer program, which, when executed by a processor, implements the control method of a heavy-loaded legged robot taking into account the leg momentum as described in any one of claims 1 to 7.
