A humanoid robot two-degree-of-freedom parallel ankle joint mechanism and kinematics and mechanics calculation method

Abstract

The application relates to the technical field of robots, and discloses a kind of two-degree-of-freedom parallel ankle joint mechanism of humanoid robot and kinematics and mechanics solving method, including seven-link transmission structure, which is composed of two groups of closed-chain transmission of parallel four-bar linkages.Motor I and II are installed on the upper end of the lower leg, and the rotation drives link IV and III to move through link I, II, V and VI, and then coupled control ankle joint pitch and lateral freedom degree.The mechanism realizes load balancing of double motors through parallel transmission, improves equivalent torque, and can combine kinematics and mechanics solving method, so that precise control can be realized only by motor feedback, sensor cost is reduced, and is suitable for high stability motion demand of biped robot.

Description

Technical Field

[0001] This invention relates to the field of robotics, specifically to a two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot and a method for calculating its kinematics and mechanics. Background Technology

[0002] The ankle of a humanoid robot plays a crucial role in improving its flexibility and motion stability. To reduce the weight of the robot's lower legs, most humanoid bipedal robots currently employ a single-degree-of-freedom (DOF) ankle design, such as Digit and Unitree H1. The drive motor is located at the knee joint, and the ankle is driven by a linkage for single-DOF movement. This structure is detrimental to the robot's lateral stability control. For example, the bipedal robot "Wukong-IV" developed by Zhejiang University overcomes these shortcomings with a two-DOF ankle design. Two drive motors are located above the lower leg, driven by linkages, controlling the ankle's pitch and lateral movements respectively. While this approach improves ankle flexibility, each motor only controls movement in one direction, resulting in a larger load and placing higher demands on the motor's maximum torque, power density, and other performance characteristics.

[0003] Therefore, in view of the above-mentioned problems, this technical solution proposes a two-degree-of-freedom parallel ankle joint mechanism for humanoid robots and a method for kinematic and mechanical calculation. Summary of the Invention

[0004] The purpose of this invention is to provide a two-degree-of-freedom parallel ankle joint mechanism for humanoid robots and a method for calculating its kinematics and mechanics, so as to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A two-DOF parallel ankle joint mechanism for a humanoid robot and a method for solving its kinematics and mechanics are disclosed. The mechanism includes a seven-bar linkage, comprising shaft I, shaft II, shaft III, shaft IV, link I, link II, link III, link IV, link V, link VI, link VII, motor I, and motor II. Links I, II, IV, III, and VII are connected in pairs, and links V, VI, IV, III, and VII are connected in pairs, forming a closed-loop structure.

[0007] Rotating shafts I and II are parallel to each other and arranged vertically at the upper end of connecting rod VII. Motor I is installed on rotating shaft I, and motor II is installed on rotating shaft II. Connecting rod I is connected to rotating shaft I. Connecting rod II is connected to connecting rod I and connecting rod IV through a ball joint. Rotating shaft III is located at the lower end of connecting rod VII and intersects connecting rod III perpendicularly. Rotating shaft IV passes through the midpoint of connecting rod IV and is connected to connecting rod III. Rotating shafts III and IV intersect perpendicularly in the same plane. Connecting rod III and connecting rod IV can rotate relative to each other. Connecting rod V is connected to rotating shaft II. Connecting rod VI is connected to connecting rod V and connecting rod IV through a ball joint.

[0008] In a seven-bar linkage mechanism, let O1 and O2 be the intersection points of shaft I, shaft II, and link VII, respectively; O3 be the intersection point of shaft III and shaft IV (which is also the intersection point of link III and link VII); A be the connection point of link I and shaft I; C be the intersection point of link I and link II; B be a point on shaft I such that O1B⊥CB; D be the intersection point of link II and link IV; E be the intersection point of shaft IV and link IV; F be the connection point of link V and shaft II; H be the intersection point of link V and link VI; G be a point on shaft II such that O2G⊥HG; I be the intersection point of link VI and link IV; and J1 and J2 be two points on shaft III such that O3J1⊥DJ1 and O3J2⊥IJ2. O1O3, CD, O1C, and O3D form a parallel four-bar linkage; O2O3, HI, O1H, and O3I form a parallel four-bar linkage.

[0009] By combining forward calculation (calculating joint torque from motor angles) and reverse calculation (deriving motor input from desired torque) in kinematic and mechanical calculation methods, precise closed-loop control is achieved.

[0010] The forward solution includes the following steps:

[0011] (1) Use the iterative method to obtain the ankle pitch and lateral angles;

[0012] (2) Calculate the Jacobian matrix J that maps from the motor output to the ankle joint. ma ;

[0013] (3) Calculate the ankle joint angular velocity;

[0014] (4) Calculate the equivalent torque at the ankle joint.

[0015] The reverse calculation process includes the following steps:

[0016] (1) Use the iterative method to find the angles of the two motors;

[0017] (2) Calculate the Jacobian matrix J that maps from the motor output to the ankle joint. ma ;

[0018] (3) Calculate the desired speed on the output shaft side of the motor;

[0019] (4) Calculate the desired torque on the output shaft side of the motor.

[0020] Compared with the prior art, the beneficial effects of the present invention are: by utilizing the rotation of motors I and II to act simultaneously on the ankle joint in the pitch and lateral directions through an asymmetric parallel mechanism, the equivalent torque is increased, and the performance requirements of a single motor are reduced.

[0021] The lateral motion stiffness is improved by utilizing a seven-link closed-chain structure and a parallel four-link design, thereby enhancing the robot's dynamic stability.

[0022] By using kinematic and mechanical calculation methods, the position, velocity, and torque at the joint can be calculated in real time based solely on the motor encoder and current feedback. Alternatively, the desired position, velocity, and torque at the motor can be calculated in real time based on the desired position, velocity, and torque at the joint. This eliminates the need for passive encoders and torque sensors at the joint, reducing the cost of the machine system and enabling stable control of the ankle of a bipedal robot. Attached Figure Description

[0023] Figure 1 This is a prototype drawing of a humanoid robot.

[0024] Figure 2 This is a schematic diagram of the front and rear sides of a two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot.

[0025] Among them: pivot I1, pivot II2, pivot III3, pivot IV4, connecting rod I5, connecting rod II6, connecting rod III7, connecting rod IV8, connecting rod V9, connecting rod VI10, connecting rod VII11. Detailed Implementation

[0026] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0027] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicating orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention. Furthermore, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined with "first," "second," etc., may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more.

[0028] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art will understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0029] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0030] Please see Figures 1-2 A two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot includes a seven-bar linkage mechanism. The seven-bar linkage mechanism includes a rotating shaft I1, a rotating shaft II2, a rotating shaft III3, a rotating shaft IV4, a connecting rod I5, a connecting rod II6, a connecting rod III7, a connecting rod IV8, a connecting rod V9, a connecting rod VI10, a connecting rod VII11, a motor I, and a motor II. Connecting rods I5, II6, IV8, III7, and VII11 are connected in pairs in sequence, and connecting rods V9, VI10, IV8, III7, and VII11 are connected in pairs in sequence, forming a closed-loop structure.

[0031] Rotating shafts I1 and II2 are parallel to each other and arranged vertically at the upper end of connecting rod VII11. Motor I is installed on rotating shaft I1, and motor II is installed on rotating shaft II2. Connecting rod I5 is connected to rotating shaft I1. Connecting rod II6 is connected to connecting rod I5 and connecting rod IV8 through a ball joint. Rotating shaft III3 is located at the lower end of connecting rod VII11 and intersects connecting rod III7 perpendicularly. Rotating shaft IV4 passes through the midpoint of connecting rod IV8 and is connected to connecting rod III7. Rotating shaft III3 and rotating shaft IV4 intersect perpendicularly. Connecting rod III7 and connecting rod IV8 can rotate relative to each other. Connecting rod V9 is connected to rotating shaft II2. Connecting rod VI10 is connected to connecting rod V9 and connecting rod IV8 through a ball joint.

[0032] In the seven-bar linkage mechanism, let O1 and O2 be the intersection points of shaft I1, shaft II2, and link VII11, respectively; O3 be the intersection point of shaft III3 and shaft IV4 (which is also the intersection point of link III7 and link VII11); A be the connection point of link I5 and shaft I1; C be the intersection point of link I5 and link II6; B be a point on shaft I1 such that O1B⊥CB; and D be the connection point of link I5. The intersection of link II6 and link IV8, the intersection of pivot IV4 and link IV8, the connection point of link V9 and pivot II2, the intersection of link V9 and link VI10, the point on pivot II2 such that O2G⊥HG, the intersection of link VI10 and link IV8, and the two points on pivot III3 such that O3J1⊥DJ1 and O3J2⊥IJ2. O1O3, CD, O1C, and O3D form a parallel four-link, and O2O3, HI, O1H, and O3I form a parallel four-link.

[0033] In this embodiment of the invention, taking a bipedal robot as an example, motors I and II are mounted above the lower leg and drive a seven-bar linkage mechanism via a rotating shaft. When motor I rotates, link I5 drives link II and link IV8 to move, and then transmits torque to the ankle joint through rotating shafts IV4 and III3, realizing coupled control of pitch and lateral movements.

[0034] A kinematic and mechanical calculation method based on a two-degree-of-freedom parallel ankle joint mechanism of a humanoid robot includes two parts: forward calculation and inverse calculation. By combining forward calculation (calculating joint torque from motor angles) and inverse calculation (deriving motor input from desired torque), precise closed-loop control can be achieved.

[0035] Specifically as follows:

[0036] Forward calculation is the process of calculating the equivalent angle, velocity, and torque at the ankle joint when the shaft-side feedback values ​​of the motor output (including angle, velocity, current, etc.) are known. It includes the following steps:

[0037] (1) The ankle pitch and lateral angles are obtained using an iterative method. For ease of description, a local coordinate system is established at the ankle, with the origin at O3 and the positive X-axis direction as... The positive direction of the Z-axis is... The positive Y-axis direction is determined using the right-hand rule. m1 q m2 The ankle joint's flexion / extension and lateral angles are unknowns, denoted as q. ap q ar The coordinates of points C, D, H, and I are as follows:

[0038]

[0039] Among them, R x (·) represents the rotation matrix about the X-axis, R y (·) represents the rotation matrix about the Y-axis. All the above lengths are known geometric parameters. Based on the aforementioned parallel four-bar structure, the relationship between the bar lengths can be constructed as follows: Two sets of equations containing two unknowns were established. Using the Newton-Raphson algorithm, q can be quickly calculated with a small number of iterations. ap q ar Numerical solution.

[0040] (1) Calculate the Jacobian matrix J that maps from the motor output to the ankle joint. ma After step (1), the ankle joint angle is known. Because... Since the values ​​are constant, the linear velocities of points C and D are equal, and the linear velocities of points H and I are equal. Based on this relationship, differentiating both sides of the equation in step (1) with respect to time, we obtain the following equation:

[0041]

[0042] in,

[0043]

[0044] Finally, based on the Jacobian mapping relation

[0045]

[0046] achievable

[0047] (2) Calculate the ankle joint angular velocity. The angular velocity at the motor output is known feedback. After step (2), the Jacobian mapping matrix is ​​known. According to... The ankle joint angular velocity can be obtained.

[0048] (3) Calculate the equivalent torque at the ankle joint. The torque at the motor output can be obtained by multiplying the current feedback by the torque coefficient, i.e.:

[0049]

[0050] According to the principle of virtual work, the equivalent torque at the ankle joint is:

[0051]

[0052] Reverse engineering is the process of calculating the desired angle, velocity, and torque on the motor output shaft side, given the desired values ​​at the ankle joint (including angle, velocity, torque, etc.). It includes the following steps:

[0053] (1) Calculate the two motor angles using an iterative method. Similar to step (1) in the forward calculation process, establish a system containing the two motor angles q. m1 q m2 Using the Newton-Raphson algorithm, q can be quickly calculated from the two sets of equations with the unknown variable in a relatively small number of iterations. m1 q m2 Numerical solution.

[0054] (2) Calculate the Jacobian matrix J that maps from the motor output to the ankle joint. ma The same as step (2) in the forward solution process.

[0055] (3) Calculate the desired speed on the motor output shaft side. Based on the Jacobian mapping relationship:

[0056]

[0057] The desired speed on the output shaft side of the motor can be obtained.

[0058] (4) Calculate the desired torque on the motor output shaft side. The desired torque on the motor output shaft side is:

[0059] In this embodiment, the maximum number of iterations of the Newton-Raphson algorithm is set to 500, the accuracy is 1.0e-12, the program is written in C++, and the Release compilation mode is used. The iterative solution takes about 0.1ms, which meets the 1kHz control frequency requirement.

[0060] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those skilled in the art, various changes can be made without departing from the spirit of the present invention.

Claims

1. A two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot, characterized in that, include: The seven-bar linkage consists of shaft I (1), shaft II (2), shaft III (3), shaft IV (4), connecting rod I (5), connecting rod II (6), connecting rod III (7), connecting rod IV (8), connecting rod V (9), connecting rod VI (10), and connecting rod VII (11). The rotating shaft I (1) and rotating shaft II (2) are arranged in parallel vertically on the upper end of the connecting rod VII (11) and are respectively connected to motor I and motor II; The connecting rod I (5) is fixed to the rotating shaft I (1), and the connecting rod II (6) is hinged to the connecting rod I (5) and the connecting rod IV (8) through a ball head; The connecting rod V (9) is fixed to the rotating shaft II (2), and the connecting rod VI (10) is hinged to the connecting rod V (9) and the connecting rod IV (8) through a ball head; The rotating shaft III (3) is located at the lower end of the connecting rod VII (11) and intersects the connecting rod III (7) perpendicularly. The rotating shaft IV (4) is hinged to the connecting rod III (7) through the midpoint of the connecting rod IV (8). The rotating shaft III (3) and the rotating shaft IV (4) intersect perpendicularly to form a closed chain transmission. Links I (5), II (6), IV (8) and III (7) form the first parallel four-link, and links V (9), VI (10), IV (8) and III (7) form the second parallel four-link; The seven-bar linkage mechanism, , These are the intersections of shaft I (1), shaft II (2), and connecting rod VII (11), respectively. It is the intersection of pivot III (3) and pivot IV (4), and also the intersection of link III (7) and link VII (11). The connection point between connecting rod I (5) and rotating shaft I (1) is... The intersection of link I (5) and link II (6) Let a point on axis I (1) be such that , The intersection of link II (6) and link IV (8) The intersection of pivot IV (4) and connecting rod IV (8) The connection point between connecting rod V (9) and rotating shaft II (2) is... Let V be the intersection of link V (9) and link VI (10). For a point on axis II (2) such that , The intersection of link VI (10) and link IV (8) , For two points on axis Ⅲ (3), satisfying , , , , , Forming a parallel four-bar linkage, , , , Forming a parallel four-bar linkage; The rotation of motor I and motor II drives the pitch and lateral movements of the ankle joint through the seven-bar linkage.

2. A method for calculating the kinematics and mechanics of the mechanism described in claim 1, characterized in that, It includes forward calculation and reverse calculation, wherein the forward calculation includes the following steps: (1) Use the iterative method to obtain the ankle pitch and lateral angles; (2) Calculate the Jacobian matrix that maps from the motor output to the ankle joint. ; (3) Calculate the ankle joint angular velocity; (4) Calculate the equivalent torque at the ankle joint; The reverse calculation process includes the following steps: (1) Use the iterative method to obtain the angles of the two motors; (2) Calculate the Jacobian matrix that maps from the motor output to the ankle joint. ; (3) Calculate the desired speed on the output shaft side of the motor; (4) Calculate the desired torque on the output shaft side of the motor; The forward calculation part is: the process of calculating the equivalent angle, velocity and torque at the ankle joint given the feedback value of the motor output shaft; The specific steps are as follows: (1) Use the iterative method to obtain the ankle pitch and lateral angles, and establish a local coordinate system at the ankle with the origin located at , The positive direction of the axis is , The positive direction of the axis is , The positive direction of the shaft is determined according to the right-hand rule. The angles of the output shafts of motor I and motor II are obtained through their internal encoders and are denoted as follows: , The ankle joint's flexion / extension and lateral angles are unknowns, denoted as follows: , 、 、 The coordinates of the points are as follows: in, Indicates circling The rotation matrix of the axis. Indicates circling Based on the aforementioned parallel four-bar structure, the rotation matrix of the axis is used to construct the link length relationships: By establishing two sets of equations containing two unknowns, and using Newton-Raphson's algorithm iteratively, the result can be calculated. , Numerical solution; (2) Calculate the Jacobian matrix that maps from the motor output to the ankle joint. After step (1), the ankle joint angle is known. For a constant value, 、 The linear velocity of the point is equal. 、 Since the linear velocities are equal, based on this relationship, differentiating both sides of the equation in step (1) with respect to time yields the following equation: in, Finally, according to the Jacobian mapping: get ; (3) Calculate the ankle joint angular velocity. The angular velocity at the motor output end is known feedback. After step (2), the Jacobian mapping matrix is ​​known. According to Calculate the ankle joint angular velocity ; (4) Calculate the equivalent torque at the ankle joint. The torque at the motor output end is obtained by multiplying the current feedback by the torque coefficient, i.e.: According to the principle of virtual work, the equivalent torque at the ankle joint is: ； The reverse calculation part is: given the expected value at the ankle joint, the process of calculating the expected angle, speed and torque on the output shaft side of the motor; The specific steps are as follows: The two motor angles are obtained using an iterative method, consistent with step (1) in the forward calculation process, thus establishing a system containing the two motor angles. , The two sets of equations with unknowns can be calculated iteratively using Newton-Raphson's algorithm. , Numerical solution; calculate the Jacobian matrix mapping from the motor output to the ankle joint. The process is the same as step (2) in the forward solution; calculate the desired speed on the motor output shaft side, based on the Jacobian mapping relationship: Obtain the desired speed on the output shaft side of the motor ; Calculate the desired torque on the motor output shaft side. The desired torque on the motor output shaft side is: .

Images