Two-degree-of-freedom parallel ankle joint mechanism of humanoid robot and kinematics and mechanics resolving method

Abstract

The invention relates to the technical field of robots, and discloses a two-degree-of-freedom parallel ankle joint mechanism of a humanoid robot and a kinematics and mechanics resolving method, the two-degree-of-freedom parallel ankle joint mechanism comprises a seven-connecting-rod transmission structure, and two groups of parallel four-connecting-rods form closed chain transmission. The motors I and II are installed at the upper ends of the shanks and rotate to drive the connecting rods IV and III to move through the connecting rods I, II, V and VI, and then the pitching degree of freedom and the transverse degree of freedom of the ankle joints are controlled in a coupling mode. According to the mechanism, dual-motor load balancing is achieved through parallel transmission, the equivalent torque is improved, meanwhile, kinematics and mechanics resolving methods can be combined, accurate control can be achieved only through motor feedback, the sensor cost is reduced, and the mechanism is suitable for the high-stability movement requirement of the biped robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and specifically to a two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot and a kinematic and mechanical calculation method. Background Art

[0002] The ankle of a humanoid robot plays an important role in improving the flexibility and motion stability of the robot. In order to reduce the weight of the robot's calf, the ankles of current humanoid biped robots mostly adopt a single-degree-of-freedom design, such as Digit and Unitree H1. The driving motors are located at the knee joint, and the ankle is driven by a connecting rod to perform single-degree-of-freedom motion. This structure is not conducive to the lateral stability control of the robot. For example, the biped robot "Wukong-IV" developed by Zhejiang University makes up for the above deficiencies. The ankle adopts a two-degree-of-freedom design, and two driving motors are located above the calf. Driven by connecting rods, they respectively control the motion of the ankle in the pitch and lateral directions. Although this scheme improves the flexibility of the ankle, a single motor only controls the motion in one direction, resulting in a large load and high requirements for the performance of the motor, such as maximum torque and power density.

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

[0004] The purpose of the present invention is to provide a two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot and a kinematic and mechanical calculation method to solve the problems raised in the above background art.

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

[0006] A two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot and a kinematic and mechanical calculation method, including a seven-link transmission mechanism. The seven-link transmission mechanism includes a rotating shaft I, a rotating shaft II, a rotating shaft III, a rotating shaft IV, a link I, a link II, a link III, a link IV, a link V, a link VI, a link VII, a motor I, and a motor II. The link I, the link II, the link IV, the link III, and the link VII are connected in sequence two by two, and the link V, the link VI, the link IV, the link III, and the link VII are connected in sequence two by two to form a closed-chain structure.

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

[0008] Seven-link transmission mechanism, let O 1 and O 2 be the intersection points of the rotating shaft I, the rotating shaft II and the link VII respectively, O 3 is the intersection point of the rotating shaft III and the rotating shaft IV (also the intersection point of the link III and the link VII), A is the connection point of the link I and the rotating shaft I, C is the intersection point of the link I and the link II, B is a point on the rotating shaft I such that O 1 B⊥CB, D is the intersection point of the link II and the link IV, E is the intersection point of the rotating shaft IV and the link IV, F is the connection point of the link V and the rotating shaft II, H is the intersection point of the link V and the link VI, G is a point on the rotating shaft II such that O 2 G⊥HG, I is the intersection point of the link VI and the link IV, J 1 and J 2 are two points on the rotating shaft III, satisfying O 3 J 1 ⊥DJ 1 and O 3 J 2 ⊥IJ 2 . O 1 O 3 , CD, O 1 C, O 3 D form a parallelogram four-link, O 2 O 3 , HI, O 1 H, O 3 I form a parallelogram four-link;

[0009] Combined with the forward solution (calculating joint torque from motor angle) and inverse solution (back-calculating motor input from expected torque) in kinematics and mechanics calculation methods, precise closed-loop control is achieved;

[0010] Among them, the forward solution includes the following steps:

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

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

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

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

[0015] The inverse solution process includes the following steps:

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

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

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

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

[0020] Compared with the prior art, the beneficial effects of the present invention are as follows: The rotations of motors I and II act on the ankle pitch and yaw directions simultaneously through the asymmetric parallel mechanism, resulting in an increase in the equivalent torque and a reduction in the performance requirements for a single motor.

[0021] Utilize the seven-link closed-chain structure and the parallel four-link design to improve the lateral motion stiffness and enhance the dynamic stability of the robot;

[0022] Through the kinematic and mechanical calculation methods, based only on the motor encoder and current feedback, the position, speed, and torque at the joint can be calculated in real time. Also, based on the expected position, speed, and torque at the joint, the expected position, speed, and torque at the motor can be calculated in real time. It does not rely on passive encoders and torque sensors installed at the joint, reducing the cost of the machine system and achieving stable control of the ankle of the biped robot. Description of the Drawings

[0023] Figure 1 It is a diagram of a humanoid robot prototype.

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

[0025] Wherein: rotating shaft I 1, rotating shaft II 2, rotating shaft III 3, rotating 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, connecting rod VII 11. Detailed Embodiments

[0026] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0027] In the description of the present invention, it should be understood that the orientation or positional relationship indicated by the terms "center", "longitudinal", "transverse", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings. It is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus should not be construed as a limitation to the present invention. In addition, the terms "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first", "second", etc. may explicitly or implicitly include one or more of such features. In the description of the present invention, unless otherwise specified, the meaning of "a plurality" is two or more.

[0028] In the description of the present invention, it should be noted that unless otherwise clearly specified and limited, the terms "mounted", "connected", "coupled" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection, an electrical connection; it can be directly connected, or indirectly connected through an intermediate medium, and it can be the communication inside two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood through specific circumstances.

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

[0030] Please refer to Figure 1 - Figure 2 , a two-degree-of-freedom parallel ankle joint mechanism for a humanoid robot, including a seven-link transmission mechanism. The seven-link transmission mechanism includes a rotating shaft Ⅰ1, a rotating shaft Ⅱ2, a rotating shaft Ⅲ3, a rotating shaft Ⅳ4, a link Ⅰ5, a link Ⅱ6, a link Ⅲ7, a link Ⅳ8, a link Ⅴ9, a link Ⅵ10, a link Ⅶ11, a motor Ⅰ and a motor Ⅱ. The link Ⅰ5, the link Ⅱ6, the link Ⅳ8, the link Ⅲ7, and the link Ⅶ11 are connected in sequence two by two, and the link Ⅴ9, the link Ⅵ10, the link Ⅳ8, the link Ⅲ7, and the link Ⅶ11 are connected in sequence two by two to form a closed-chain structure.

[0031] The rotating shaft Ⅰ1 and the rotating shaft Ⅱ2 are parallel to each other and arranged vertically above and below the upper end of the link Ⅶ11. The motor Ⅰ is mounted on the rotating shaft Ⅰ1, and the motor Ⅱ is mounted on the rotating shaft Ⅱ2. The link Ⅰ5 is connected to the rotating shaft Ⅰ1. The link Ⅱ6 is connected to the link Ⅰ5 and the link Ⅳ8 through a ball joint. The rotating shaft Ⅲ3 is located at the lower end of the link Ⅶ11 and intersects the link Ⅲ7 perpendicularly. The rotating shaft Ⅳ4 passes through the midpoint of the link Ⅳ8 and is connected to the link Ⅲ7. The rotating shaft Ⅲ3 and the rotating shaft Ⅳ4 intersect perpendicularly. The link Ⅲ7 and the link Ⅳ8 can rotate relative to each other. The link Ⅴ9 is connected to the rotating shaft Ⅱ2. The link Ⅵ10 is connected to the link Ⅴ9 and the link Ⅳ8 through a ball joint.

[0032] The seven-bar linkage mechanism makes O 1 and O 2 be the intersection points of the first rotating shaft 1, the second rotating shaft 2 and the seventh connecting rod 11 respectively, O 3 be the intersection point of the third rotating shaft 3 and the fourth rotating shaft 4 (which is also the intersection point of the third connecting rod 7 and the seventh connecting rod 11), A be the connection point of the first connecting rod 5 and the first rotating shaft 1, C be the intersection point of the first connecting rod 5 and the second connecting rod 6, B be a point on the first rotating shaft 1 such that O 1 B⊥CB, D be the intersection point of the second connecting rod 6 and the fourth connecting rod 8, E be the intersection point of the fourth rotating shaft 4 and the fourth connecting rod 8, F be the connection point of the fifth connecting rod 9 and the second rotating shaft 2, H be the intersection point of the fifth connecting rod 9 and the sixth connecting rod 10, G be a point on the second rotating shaft 2 such that O 2 G⊥HG, I be the intersection point of the sixth connecting rod 10 and the fourth connecting rod 8, J 1 and J 2 be two points on the third rotating shaft 3, satisfying O 3 J 1 ⊥DJ 1 and O 3 J 2 ⊥IJ 2 . O 1 O 3 , CD, O 1 C, O 3 D form a parallelogram four-bar linkage, O 2 O 3 , HI, O 1 H, O 3 I form a parallelogram four-bar linkage.

[0033] In an embodiment of the present invention, taking a biped robot as an example, the first motor and the second motor are installed above the calf and drive the seven-bar linkage mechanism through the rotating shafts. When the first motor rotates, the first connecting rod 5 drives the second connecting rod and the fourth connecting rod 8 to move, and then transmits the torque to the ankle joint through the fourth rotating shaft 4 and the third rotating shaft 3 to realize the coupled control of the pitch and roll motions.

[0034] A kinematic and mechanical calculation method based on a two-degree-of-freedom parallel ankle joint mechanism of a humanoid robot includes forward calculation and inverse calculation. Combining forward calculation (calculating joint torque from motor angle) and inverse calculation (back-calculating motor input from expected torque), precise closed-loop control can be achieved;

[0035] Specifically as follows:

[0036] Forward calculation is a process of calculating the equivalent angle, speed and torque at the ankle joint known the feedback values (including angle, speed, current, etc.) on the output shaft side of the motor, and includes the following steps:

[0037] (1) Use the iterative method to obtain the ankle pitch and roll angles. For ease of description, a local coordinate system is established at the ankle, and the coordinate origin is located at O 3 , the positive direction of the X-axis is , the positive direction of the Z-axis is , and the positive direction of the Y-axis is determined according to the right-hand rule. The angles of the output shafts of Motor Ⅰ and Motor Ⅱ can be obtained through their internal encoders, and are denoted as q m1 and q m2 respectively. The ankle pitch and roll angles are unknowns, and are denoted as q ap and q ar respectively. The coordinates of points C, D, H, and I are as follows:

[0038]

[0039] where, R x (·) represents the rotation matrix about the X-axis, and R y (·) represents the rotation matrix about the Y-axis. The above lengths are all known geometric parameters. According to the aforementioned parallel four-bar linkage structure, the relationship of the link lengths can be constructed: Two groups of equations containing two unknowns are established. Using the Newton-Raphson algorithm, after a small number of iterations, the numerical solutions of q ap and q ar can be quickly calculated.

[0040] (1) Calculate the Jacobian matrix J ma that maps from the motor output end to the ankle joint end. After step (1), the ankle joint angle is known. Since is a fixed value, 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, both sides of the equation in step (1) are differentiated with respect to time to obtain the equation relationship:

[0041]

[0042] where,

[0043]

[0044] Finally, according to the Jacobian mapping relationship

[0045]

[0046] we can obtain

[0047] (2) Calculate the ankle joint angular velocity. The angular velocity of the motor output end is known feedback. After step (2), the Jacobian mapping matrix is known. According to we can obtain the ankle joint angular velocity

[0048] (3) Calculate the equivalent moment at the ankle joint. The moment at the motor output end 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 moment at the ankle joint is:

[0051]

[0052] Inverse calculation is a process of calculating the expected angles, speeds, and torques on the motor output shaft side given the expected values (including angles, speeds, torques, etc.) at the ankle joint, and it includes the following steps:

[0053] (1) Use the iterative method to obtain the angles of the two motors. Similar to step (1) in the forward calculation process, establish two sets of equations containing the two motor angles q m1 、q m2 unknowns. Using the Newton-Raphson algorithm, after a small number of iterations, the numerical solutions of q m1 、q m2 can be quickly calculated.

[0054] (2) Calculate the Jacobian matrix J ma mapping from the motor output end to the ankle joint end. It is the same as step (2) in the forward calculation process.

[0055] (3) Calculate the expected speed on the motor output shaft side. According to the Jacobian mapping relationship:

[0056]

[0057] the expected speed on the motor output shaft side can be obtained

[0058] (4) Calculate the expected torque on the motor output shaft side. The expected 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 precision is 1.0e-12, the program is written in C++ language, and the Release compilation method is used. The time-consuming for iterative solution is about 0.1 ms, which meets the requirement of 1 kHz control frequency.

[0060] The above has described the preferred embodiments of the present invention in detail, but the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those of ordinary skill in the art, various changes can be made without departing from the gist 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-link transmission mechanism is composed of a rotating shaft I (1), a rotating shaft II (2), a rotating shaft III (3), a rotating shaft IV (4), a connecting rod I (5), a connecting rod II (6), a connecting rod III (7), a connecting rod IV (8), a connecting rod V (9), a connecting rod VI (10), and a connecting rod VII (11); The rotating shaft I (1) and the rotating shaft II (2) are arranged in parallel at the upper end of the connecting rod VII (11) and connected to the motor I and the motor II respectively; The connecting rod I (5) is fixedly connected 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 fixedly connected 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 with the connecting rod III (7) vertically. 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 vertically to form a closed chain transmission.

2. The ankle joint mechanism according to claim 1, characterized in that: The connecting rods I (5), II (6), IV (8) and III (7) form a first parallelogram linkage, and the connecting rods V (9), VI (10), IV (8) and III (7) form a second parallelogram linkage.

3. The ankle joint mechanism according to claim 2, characterized in that: In the seven-link transmission mechanism, O1 and O2 are the intersection points of the rotating shaft I (1), the rotating shaft II (2) and the connecting rod VII (11), respectively; O3 is the intersection point of the rotating shaft III (3) and the rotating shaft IV (4), and is also the intersection point of the connecting rod III (7) and the connecting rod VII (11); A is the connection point of the connecting rod I (5) and the rotating shaft I (1); C is the intersection point of the connecting rod I (5) and the connecting rod II (6); B is a point on the rotating shaft I (1) such that O1B⊥CB; and D is the connecting rod. Ⅱ(6) and connecting rod Ⅳ(8), E is the intersection of axis Ⅳ(4) and connecting rod Ⅳ(8), F is the connection point of connecting rod Ⅴ(9) and axis Ⅱ(2), H is the intersection of connecting rod Ⅴ(9) and connecting rod Ⅵ(10), G is a point on axis Ⅱ(2) such that O2G⊥HG, I is the intersection of connecting rod Ⅵ(10) and connecting rod Ⅳ(8), J1 and J2 are two points on axis Ⅲ(3), satisfying O3J1⊥DJ1 and O3J2⊥IJ2. O1O3, CD, O1C, O3D form a parallelogram linkage, and O2O3, HI, O1H, O3I form a parallelogram linkage.

4. The ankle joint mechanism according to claim 1, characterized in that: The rotation of the motor I and the motor II is coupled through the seven-link transmission structure to drive the pitch and lateral movement of the ankle joint.

5. A kinematic and mechanical solution method based on the mechanism described in claim 1, characterized in that: The method includes forward solution and reverse solution, wherein the forward solution includes the following steps: (1) Use the iterative method to calculate the ankle pitch and lateral angles; (2) Calculate the Jacobian matrix J mapping from the motor output end to the ankle joint end ma ; (3) Calculate ankle joint angular velocity; (4) Calculate the equivalent moment at the ankle joint. The inverse solution process includes the following steps: (1) Use the iterative method to obtain the two motor angles; (2) Calculate the Jacobian matrix J mapping from the motor output end to the ankle joint end ma ; (3) Calculate the expected speed of the motor output shaft; (4) Calculate the expected torque on the motor output shaft side.

6. The kinematic and mechanical solution method of a two-degree-of-freedom parallel ankle joint mechanism of a humanoid robot according to claim 5, characterized in that: The forward solution part is: the process of calculating the equivalent angle, speed and torque at the ankle joint when the feedback value on the motor output shaft side is known; 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 at O3 and the positive direction of the X axis at The positive direction of the Z axis is The positive direction of the Y axis is determined according to the right-hand rule. The angles of the output shafts of motors I and II can be obtained through their internal encoders and are denoted as q m1 ,q m2 , the ankle joint pitch and lateral angle are unknown quantities, denoted as q ap ,q ar , the coordinates of points C, D, H, and I are: Among them, R x (·) represents the rotation matrix around the X axis, R y (·) represents the rotation matrix around the Y axis. The above lengths are all known geometric parameters. According to the aforementioned parallel four-bar linkage structure, the link length relationship is constructed: Establish two sets of equations with two unknowns and use the Newton-Raphson algorithm to quickly calculate q with a small number of iterations. ap ,q ar Numerical solution of ; (2) Calculate the Jacobian matrix J mapping from the motor output end to the ankle joint end ma , after step (1), the ankle joint angle is known, since is a 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, the time derivative of both sides of the equation in step (1) is obtained to obtain the equation relationship: in, Finally, according to the Jacobi mapping relationship: Available (3) Calculate the ankle joint angular velocity. The angular velocity of the motor output terminal is a known feedback. After step (2), the Jacobian mapping matrix is ​​known. According to The ankle joint angular velocity (4) Calculate the equivalent torque at the ankle joint. The torque at the motor output end can be obtained by multiplying the current feedback by the torque coefficient, that is: According to the principle of virtual work, the equivalent moment at the ankle joint is:

7. The kinematic and mechanical solution method of a two-degree-of-freedom parallel ankle joint mechanism of a humanoid robot according to claim 5, characterized in that: The reverse solution part is: the process of calculating the expected angle, speed and torque on the motor output shaft side when the expected value at the ankle joint is known; The specific steps are as follows: (1) Use the iterative method to obtain the two motor angles, which is similar to step (1) in the forward solution process. Establish a system containing the two motor angles q m1 ,q m2 Two sets of equations with unknown numbers can be quickly calculated using the Newton-Raphson algorithm with a small number of iterations. m1 ,q m2 Numerical solution of ; (2) Calculate the Jacobian matrix J mapping from the motor output end to the ankle joint end ma , which is the same as step (2) in the forward solution process; (3) Calculate the expected speed of the motor output shaft according to the Jacobian mapping relationship: The expected speed of the motor output shaft side can be obtained (4) Calculate the expected torque on the motor output shaft side. The expected torque on the motor output shaft side is:

Images