Robot foot-ground interaction force control method and system based on joint position servo

Abstract

The present invention proposes a robot foot-ground interaction force control method based on joint position servo, including: obtaining an interaction force relationship model generated by robot motion and robot-external interaction based on the virtual force acting on the robot by the joint torque and the joint space dynamics model when the robot interacts with the outside world; solving the overall interaction force estimation value of the robot-external interaction according to the single-foot support phase or double-foot support phase state of the robot based on the interaction force relationship model; using the robot joint angular acceleration to correct the overall interaction force estimation value of the robot-external interaction so that it follows the expected overall interaction force of the robot system. No expensive interaction force sensor and joint torque sensor are required, and no joint torque servo technology with high technical difficulty is required, which reduces the implementation cost of robot foot-ground interaction force perception and estimation.

Description

Technical Field

[0001] The present invention belongs to the technical field of robots, and in particular relates to a robot foot-ground interaction force control method and system based on joint position servo. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] The biped robot realizes the movement of the main body through the interaction between its feet and the ground. Its motion base coordinates change their orientation in real time with the movement of the robot, so it is called a floating base coordinate system. Compared with the common fixed base coordinate system (such as a fixed base manipulator), the movement of the floating base coordinate system depends not only on the changes in its own joint configuration, but also on the size of the interaction force with the environment. Therefore, sensing the external interaction force on the floating base coordinate system and actively adjusting the interaction force is the basis for realizing high-performance biped robot movement.

[0004] The interaction force between each foot of a point-legged biped robot and the ground can be decomposed into three dimensions: horizontal forward force, horizontal lateral force, and vertical force. Most of the traditional methods for sensing the interaction force between the foot and the ground rely on the measurement of the three-dimensional force sensor of the foot, or on the calculation based on the torque feedback of the joint; the adjustment of the interaction force is achieved through the precise force servo of the joint. Most of the above methods require the robot driver to have a high control and response frequency (usually not less than 1kHz), a relatively small drive transmission ratio, or to have precise force sensing elements, and have high requirements for processing accuracy and hardware circuits. In comparison, the joint driver using position servo is more mature and low-cost, and the position servo technology is relatively simple. For DC motors, the angle or speed adjustment can be achieved by changing the duty cycle of its driving voltage. In addition, position servo drivers of the same volume and weight usually have stronger load capacity than force servo drivers. However, due to the high motion stiffness of the position servo and the difficulty in accurately obtaining the output force, the lack of force sensing and joint torque servo capabilities makes the existing foot-ground interaction force sensing and control methods impossible to implement on robots. Summary of the invention

[0005] In order to overcome the shortcomings of the above-mentioned prior art, the present invention provides a robot foot-ground interaction force control and system based on joint position servo, which estimates the overall interaction force between the robot and the environment based on the feedback value of the robot joint configuration and trunk posture, and then actively adjusts the overall interaction force by iteratively controlling the robot joint angular velocity. The algorithm and system do not require the feedback value of the joint angular acceleration and trunk position, and the required feedback parameters are all commonly used sensor variables for position-controlled leg-foot robots.

[0006] To achieve the above objectives, a first aspect of the present invention provides a robot foot-ground interaction force control method based on joint position servo, comprising:

[0007] Based on the virtual force of joint torque acting on the robot and the joint space dynamics model when the robot interacts with the outside world, the interaction force relationship model between the robot motion and the robot-external interaction is obtained;

[0008] Based on the interaction force relationship model, solving the estimated value of the overall interaction force of the robot-external interaction according to the single-leg support phase or the double-leg support phase state of the robot;

[0009] The robot joint angular acceleration is used to correct the estimated value of the overall interaction force between the robot and the outside world so that it follows the expected overall interaction force of the robot system.

[0010] A second aspect of the present invention provides a robot foot-ground interaction force control system based on joint position servo, comprising:

[0011] Dynamic model building module: based on the posture information of the interaction between the humanoid robot, the target object and the environment, a system interaction force model is established, and the dynamic models of the humanoid robot and the target object are respectively established according to the established system interaction force model;

[0012] Constraint establishment module: based on the speed and force constraints satisfied by the interaction between the humanoid robot, the target object and the environment, a system speed constraint model and a system force constraint model are established, and the robot joint angular acceleration constraint conditions are obtained according to the established system speed constraint model, system force constraint model and the dynamics model of the humanoid robot and the target object;

[0013] Motion execution module: Based on the established robot joint angular acceleration constraints, the system motion boundary conditions are obtained. The motion trajectory of the humanoid robot performs non-grasping mobile operation tasks while satisfying the system motion boundary conditions.

[0014] The third aspect of the present invention provides a computer device, comprising: a processor, a memory and a bus, wherein the memory stores machine-readable instructions executable by the processor, and when the computer device is running, the processor and the memory communicate through the bus, and when the machine-readable instructions are executed by the processor, a method for controlling a non-grasping mobile pushing task of a humanoid robot is performed.

[0015] A fourth aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, executes a method for controlling a non-grasping mobile pushing task of a humanoid robot.

[0016] One or more of the above technical solutions have the following beneficial effects:

[0017] In the present invention, the position servo-based joint driver constructs a model of the interaction force relationship between the robot motion and the robot-external interaction, obtains an estimated value of the interaction force between the robot and the environment based on the inverse dynamics algorithm and the robot's kinematic feedback information, and then uses the robot joint angular acceleration to correct the estimated value of the interaction force between the robot and the environment, so that the magnitude of the interaction force can follow the given value of the system. This method has low requirements on the robot hardware platform, does not require expensive interaction force sensors and joint torque sensors, and does not require joint torque servo technology with high technical difficulty, which reduces the implementation cost of robot foot-ground interaction force perception and estimation.

[0018] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0020] Figure 1 This is a flow chart of a robot foot-ground interaction force control method based on joint position servo in Embodiment 1 of the present invention. DETAILED DESCRIPTION

[0021] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0022] It should be noted that the terms used herein are for describing specific embodiments only and are not intended to be limiting of exemplary embodiments according to the present invention.

[0023] In the absence of conflict, the embodiments of the present invention and the features of the embodiments may be combined with each other.

[0024] Embodiment 1

[0025] like Figure 1 As shown, this embodiment discloses a robot foot-ground interaction force control method based on joint position servo, comprising:

[0026] Step 1: Based on the virtual force of the joint torque acting on the robot and the joint space dynamics model when the robot interacts with the outside world, the interaction force relationship model between the robot motion and the robot-external interaction is obtained;

[0027] Step 2: Based on the interaction force relationship model, the estimated value of the overall interaction force between the robot and the outside world is solved according to the single-leg support phase or the double-leg support phase state of the robot;

[0028] Step 3: Correct the estimated value of the overall interaction force between the robot and the outside world by controlling the robot joint angular acceleration so that it follows the expected overall interaction force of the robot system.

[0029] In this description, the symbol 0 is defined m×n Represents a zero matrix of dimension m×n, symbol I n×n Represents the identity matrix of dimension n×n.

[0030] In step 1 of this embodiment, the configuration of the bipedal robot system is written in the following vector form:

[0031]

[0032] in, Represents the n degrees of freedom of the robot body, including its position degrees of freedom and posture degrees of freedom, n≤6. Represents the a joints of the robot's swinging limb. Represents the b joints of the robot's supporting legs.

[0033] The kinematics of the biped robot can be expressed in differential form as:

[0034]

[0035] in, Represents the position and attitude angle of the end of the robot's swinging limb, and k is the dimension of the position and attitude angle vector of the end of the swinging limb. represents the position vector of the robot foot interacting with the ground, and l is the dimension of the position vector of the robot foot interacting with the ground. J is the Jacobian matrix of the system. If the elements in e are linearly independent, then J is a full row rank matrix. For Jacobian matrices with insufficient row rank, the linearly independent elements in e can be found by performing SVD decomposition on J, thereby constructing a new full row rank Jacobian matrix.

[0036] Assume that the interaction force between the robot foot and the ground satisfies the friction cone constraint, that is, there is no relative sliding between the robot foot and the ground, and the position of the interaction point remains constant, that is:

[0037]

[0038] Among them, e C It represents the position vector where the robot's foot interacts with the ground. In addition, a dot above a letter represents the first-order derivative of the variable represented by the letter, and two dots above it represent the second-order derivative.

[0039] Regardless of whether the robot is moving or not, as long as the robot interacts with the external environment, its joint space dynamics in the inertial system can be expressed as:

[0040]

[0041] in, is the robot inertia matrix, n is the number of degrees of freedom of the robot body, is the robot's Coriolis force / centripetal force matrix, is the robot gravity vector, S τ =[0 (a+b)×n I (a+b)×(a+b) ] T Select the matrix for the robot's driving joints, It is the force generated by the interaction between the robot foot and the ground in the normal state.

[0042] In this embodiment, based on the above J and M matrices, the following matrix V is defined and its upper and lower parts are represented as V1 and V2 respectively:

[0043]

[0044] The joint torque τ of the bipedal robot can be mapped to the virtual force γ at the interaction point between the robot trunk and the foot-ground by the following formula:

[0045] Sτ=J T γ+Vτ0,τ0∈Null(V1) (6)

[0046] Where τ0 is an arbitrary matrix vector in the null space of V1.

[0047] It should be noted that f in formula (4) C It refers to the force generated by the interaction between the robot foot and the ground, while γ in formula (6) represents the virtual force caused by the joint motion.

[0048] Substituting formula (6) into formula (4) yields:

[0049]

[0050] According to formula (2), the second-order kinematic equation of the system can be written as:

[0051]

[0052] Formula (7) multiplied by JM on the left -1 Then combined with formula (8), we can get:

[0053]

[0054] Since J is a full row rank matrix, JM -1 J T It should be a reversible matrix. Formula (9) is multiplied by (JM -1 J T ) -1 We can find γ:

[0055]

[0056] Substituting formula (10) into formula (6), we can obtain:

[0057]

[0058] in,

[0059]

[0060] Based on formulas (1)(2)(3)(5)(6), formula (11) can be written as follows:

[0061]

[0062] The mathematical relationship between the robot motion and its interaction force can be expressed as follows:

[0063]

[0064] Considering Formula (13) can be transformed into:

[0065]

[0066] in,

[0067]

[0068] In formula (14), the joint torque variable is eliminated, and the relationship between the robot trunk acceleration, the swing limb joint acceleration and the robot-environment interaction force is obtained, which is the foot-ground interaction spatial dynamics of the point-legged bipedal robot system.

[0069] In step 2 of this embodiment, in formula (14), represents the foot-ground interaction force f C The virtual force generated on the robot trunk. When the robot is in the single-leg support phase, J pC For full row rank, we can solve f by equation (14) C ; When the robot is in the bipedal support phase, its interaction force can be equivalent to a 5-dimensional vector, namely a 3-dimensional force and a 2-dimensional torque. This embodiment represents this dimensionality reduction process as:

[0070] f G =HfC (15)

[0071]

[0072] in, is the linearly independent force vector of each element, Represents the transformation matrix used in the dimensionality reduction process, g≤l and g≤n.

[0073] In physical terms, f G represents the interaction force between the point-legged biped robot as a whole and the external environment, that is, the “overall interaction force”. If the robot is in the single-leg support phase, f G =f C .

[0074] Substituting formula (16) into formula (14), we can obtain the overall interaction force f for the system G The kinetic expression of:

[0075]

[0076] in,

[0077] J G =(HH T ) -1 HJ pC (18)

[0078] Multiply equation (17) by We can get:

[0079]

[0080] In actual systems, it is difficult to obtain joint angular acceleration To solve this problem, this embodiment starts with the single-joint motion dynamics of a multi-degree-of-freedom robot:

[0081]

[0082] Among them, τ a is the output torque of the joint actuator, τ d is the external torque on the joint, including the torque caused by friction and interaction, I m is the total moment of inertia of the joint load, and θ is the joint rotation angle.

[0083] In a high-stiffness position-controlled biped robot system, the output torque of its joint actuator can be regarded as a large gain variable related to the joint angle error:

[0084] τ a =k a ( dθ-θ) (21)

[0085] Among them, k a is the joint position gain, d θ is the desired value of the joint angle.

[0086] Define s as the Laplace operator, and combine equations (20) and (21) to obtain:

[0087]

[0088] When the robot's limbs are in the swing phase and not interacting with the ground, the external torques and joint loads on its joints will not change significantly, so s 2 I m and 2 τ d The value of is bounded. When the joint stiffness k a When is very large, formula (22) can be simplified to:

[0089]

[0090] Formula (23) shows that for a robot system with large joint stiffness, its large motion stiffness can suppress the influence of external torque and joint load in joint motion. Therefore, for a robot system controlled by position control, the joint angular acceleration of its swing limb can be approximately considered to be the same as the corresponding given value, that is:

[0091]

[0092] Therefore, the overall interaction force f of the robot system is G It can be obtained by the following formula:

[0093]

[0094] In formula (25), the superscript ^ represents the estimated value of the data, and the superscript ~ represents the systematic observation value of the data. The expressions in the following text are similar.

[0095] According to formula (1), the system vector q is further expressed as:

[0096]

[0097] Among them, x and are the position vector and posture vector of the robot trunk respectively. By verification, it can be found that the total observation value of formula (25) can be based on and In a position-controlled point-footed biped robot system, the above value can be obtained by the following method:

[0098] and They represent the observed values ​​of the robot's swing limb joint angle and the supporting leg joint angle, which can be read by the joint angle sensor. and Through and Differentiation is obtained;

[0099] and Measured by a gyroscope mounted on the robot's torso, yes The first order differential of

[0100] is the acceleration of the robot trunk, measured by the acceleration sensor installed on the robot trunk;

[0101] It can be obtained through the robot leg odometer or state estimation algorithm.

[0102] In step 3 of this embodiment, an iterative method is used to correct the overall interaction force between the robot foot and the ground so that it follows a given value. The following variables are defined:

[0103]

[0104] If the overall interaction force between the robot foot and the ground is ( It can be obtained by the interaction force estimation formula (25), and the overall interaction force expected by the system is You need to set the joint angular acceleration of the robot's interactive limbs for:

[0105]

[0106] Among them, k I is the integral parameter vector, and Δt represents the control period of the system. Note that in equation (27), Λ is assumed to be a full-rank matrix. If Λ is not full-rank, its generalized inverse can be used. The effectiveness of equation (27) in correcting the overall interaction force of the system is proved in the following steps.

[0107] Specifically, if Λ is of full rank, multiplying equation (17) by Y on the left yields:

[0108]

[0109] Combining equations (2), (3), (18) we can obtain:

[0110]

[0111] Substituting formula (29) into formula (28), we can obtain:

[0112]

[0113] Substituting the joint angular acceleration formula in equation (27) into equation (30), the dynamics of the system at time (t+Δt) becomes:

[0114]

[0115] If the control cycle Δt of the system is very short, the joint configuration and interaction force of the system will not change much in a single cycle, so it can be considered that:

[0116]

[0117] Combining (31) and (32) we can get:

[0118]

[0119] in,

[0120]

[0121] ε0 represents the error caused by the approximation in formula (32).

[0122] The left side of the equal sign of formula (33) is a proportional-integral (PI) controller for the error between the desired interaction force and the actual interaction force. The PI controller can eliminate the error so that the actual value of the interaction force follows the given value.

[0123] In a point-legged bipedal robot system based on joint position servo, it is difficult to directly control the joint angular acceleration of the robot. Therefore, this embodiment adopts a method of setting the joint angle to achieve indirect setting of the joint angular acceleration:

[0124]

[0125] in, Indicates the joint angular acceleration required to be achieved in the next control cycle, d θ C(t+Δt) Indicates the joint angle setting value of the next control cycle; and are the feedback values ​​of the joint angle and angular velocity of the current control cycle respectively; Δt is the time interval between two control cycles; k θ Represents the gain vector, which is a diagonal matrix whose diagonal elements are gain values ​​and can be adjusted manually.

[0126] Embodiment 2

[0127] This embodiment provides a robot foot-ground interaction force control system based on joint position servo, comprising:

[0128] Interaction force relationship model building module: Based on the virtual force acting on the robot by the joint torque and the joint space dynamics model when the robot interacts with the outside world, the interaction force relationship model generated by the robot motion and the robot-external interaction is obtained;

[0129] An overall interaction force estimation value calculation module is used to calculate an overall interaction force estimation value of the robot-external interaction based on the interaction force relationship model and the single-leg support phase or the double-leg support phase state of the robot.

[0130] Overall interaction force estimation correction module: The overall interaction force estimation value of the robot-external interaction is corrected by using the robot joint angular acceleration to make it follow the expected overall interaction force of the robot system.

[0131] Embodiment 3

[0132] The purpose of this embodiment is to provide a computing device, including: a processor, a memory and a bus, the memory stores machine-readable instructions executable by the processor, when the computer device is running, the processor and the memory communicate through the bus, and when the machine-readable instructions are executed by the processor, a robot foot-ground interaction force control method based on joint position servo is executed.

[0133] Embodiment 4

[0134] The purpose of this embodiment is to provide a computer-readable storage medium.

[0135] A computer-readable storage medium is characterized in that a computer program is stored on the computer-readable storage medium, and when the computer program is run by a processor, a robot foot-ground interaction force control method based on joint position servo is executed.

[0136] The steps involved in the apparatuses of the above embodiments 2, 3 and 4 correspond to the method embodiment 1, and the specific implementation methods can refer to the relevant description part of embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood to include any medium that can store, encode or carry an instruction set for execution by a processor and enable the processor to execute any method in the present invention.

[0137] Those skilled in the art should understand that the modules or steps of the present invention described above can be implemented by a general-purpose computer device, or alternatively, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0138] Although the above describes the specific implementation mode of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A robot foot-ground interaction force control method based on joint position servo, characterized in that: include: Based on the virtual force of joint torque acting on the robot and the joint space dynamics model when the robot interacts with the outside world, the interaction force relationship model between the robot motion and the robot-external interaction is obtained; Based on the interaction force relationship model, solving the estimated value of the overall interaction force of the robot-external interaction according to the single-leg support phase or the double-leg support phase state of the robot; By controlling the robot joint angular acceleration, the estimated value of the overall interaction force between the robot and the outside world is corrected so that it follows the expected overall interaction force of the robot system; The joint angle acceleration of the robot's interactive limb is indirectly set by setting the robot's joint angle, specifically: in, Indicates the joint angular acceleration required to be achieved in the next control cycle, d θC (t+Δt) Indicates the joint angle setting value of the next control cycle; and are the feedback values ​​of the joint angle and angular velocity of the current control cycle respectively; Δt is the time interval between two control cycles; k θ represents the gain vector.

2. A robot foot-ground interaction force control method based on joint position servo as claimed in claim 1, characterized in that: The virtual force equation of the joint torque acting on the robot is derived based on the robot dynamics equation.

3. A robot foot-ground interaction force control method based on joint position servo as claimed in claim 2, characterized in that: The virtual force acting on the robot due to the joint torque is solved jointly by the virtual force equation and the second-order kinematic equation of the robot system. The solved virtual force is substituted into the virtual force equation to eliminate the joint torque variable in the virtual force equation, and the interaction force relationship model generated by the robot motion and the robot-external interaction is obtained in combination with the joint space dynamics model.

4. The robot foot-ground interaction force control method based on joint position servo as claimed in claim 1, characterized in that: The overall interaction force between the robot and the ground is estimated based on the feedback values ​​of the robot's joint angles, trunk posture angles and angular velocity, trunk linear velocity and linear acceleration.

5. The robot foot-ground interaction force control method based on joint position servo as claimed in claim 1, characterized in that: An iterative method is adopted to make the estimated value of the interaction force of the robot-external interaction follow the expected overall interaction force of the robot system, wherein the expected overall interaction force of the robot system is determined by setting the joint angular acceleration of the robot's interaction limbs.

6. A robot foot-ground interaction force control system based on joint position servo, characterized in that: include: Interaction force relationship model building module: Based on the virtual force acting on the robot by the joint torque and the joint space dynamics model when the robot interacts with the outside world, the interaction force relationship model generated by the robot motion and the robot-external interaction is obtained; An overall interaction force estimation value calculation module is used to calculate an overall interaction force estimation value of the robot-external interaction based on the interaction force relationship model and the single-leg support phase or the double-leg support phase state of the robot. Overall interaction force estimation correction module: This module corrects the overall interaction force estimation value of the robot-external interaction by controlling the robot joint angular acceleration, so that it follows the expected overall interaction force of the robot system. The joint angle acceleration of the robot's interactive limb is indirectly set by setting the robot's joint angle, specifically: in, Indicates the joint angular acceleration required to be achieved in the next control cycle, d θ C(t+Δt) Indicates the joint angle setting value of the next control cycle; and are the feedback values ​​of the joint angle and angular velocity of the current control cycle respectively; Δt is the time interval between two control cycles; k θ represents the gain vector.

7. A robot foot-ground interaction force control system based on joint position servo as claimed in claim 6, characterized in that: In the overall interaction force estimation value calculation module, the overall interaction force between the robot and the ground is estimated based on the feedback values ​​of the robot's joint angles, trunk posture angles and angular velocities, trunk linear velocities and linear accelerations.

8. A computer device, characterized in that: include: A processor, a memory and a bus, wherein the memory stores machine-readable instructions executable by the processor, and when the computer device is running, the processor and the memory communicate via the bus, and when the machine-readable instructions are executed by the processor, a robot foot-ground interaction force control method based on joint position servo is performed.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the robot foot-ground interaction force control method based on joint position servo as claimed in any one of claims 1 to 5 is executed.

Images