Forward / Inverse Kinematics Modeling Method for the Whole Body of a Humanoid Robot with a Closed-Loop Constraint Structure

Abstract

The present invention discloses a method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure. The method establishes the world coordinate system, floating base, and local coordinate systems of each joint of the robot. Considering that the knee joint and ankle joint of the robot are driven by a four-bar linkage mechanism, an analytical expression of the closed-loop constraint is established, and the independent generalized velocities are clarified. The method further conducts dynamics modeling of the robot based on the description of six-dimensional space vectors and using the recursive form of the articulated body algorithm. By equivalently processing physical quantities such as spatial inertia, gyroscopic force, and Coriolis spatial acceleration, the components involved in the closed-loop constraint are combined into an aggregated node, and a forward / inverse dynamics model in the form of constraint embedding is established. Different from the usual methods for dynamics modeling of serial mechanisms, the present invention can be used for forward / inverse dynamics solution of a humanoid robot with a four-bar linkage drive mechanism for the knee joint and ankle joint, providing a key basis for the whole-body force control of the humanoid robot.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotics, and particularly relates to a method for forward / inverse dynamics modeling of a whole-body humanoid robot with a closed-loop constraint structure. Background Art

[0002] A humanoid robot is a complex multi-body system with more than 30 degrees of freedom, and it is a very challenging task to perform dynamics modeling on it. On the other hand, the dynamics model is the key foundation for gait generation and stable control. To simplify the analysis, domestic and foreign scholars usually approximate the robot as a linear inverted pendulum and establish a simplified linear inverted pendulum dynamics model. As described in Hideji Kajiwara's "Humanoid Robots", such a model can achieve the basic movement of the robot. With the development of humanoid robot technology, people have begun to pursue dynamic movements at higher speeds of the robot, such as whole-body control, which requires the establishment of a complete multi-rigid-body dynamics model to accurately describe the movement of the robot, and at the same time, the solution of the dynamics model needs to meet certain real-time requirements. The recent journal paper "Efficient Analytical Derivatives of Rigid-Body Dynamics Using Spatial Vector Algebra" points out that for forward dynamics modeling and inverse dynamics modeling of multi-rigid-body systems, the most efficient methods are the articulated body algorithm and the Newton-Euler recursive algorithm respectively, and their complexities are both where N is the number of rigid bodies. Abhinandan Jain's book "Robot and Multibody Dynamics" gives a detailed introduction to these two methods. At the same time, for an underactuated system such as a humanoid robot, its inverse dynamics involves solving the floating-base acceleration and the driving torque of each joint, which is essentially a hybrid dynamics problem. This book also gives a generalized articulated body algorithm for dealing with such problems, and its complexity is also

[0003] To achieve efficient walking of robots, scholars at home and abroad have considered the leg-foot structure layout with high energy efficiency and light weight. Based on the main optimization goal of reducing the moment of inertia of the robot's leg-foot structure, they arranged the drive motors towards the torso direction to reduce the mass of the leg-foot structure, and transmitted power to the knee joint and ankle joint through a four-bar linkage mechanism, as described in the recent patent "Inverse Kinematics Solving Method for a Biped Robot with a High-Energy-Efficient and Lightweight Structure" (Patent No. CN202010722914.4). The four-bar linkage mechanism introduces closed-loop constraints to the robot system. Especially for the ankle joint transmission, which involves a spatial quadrilateral, the constraint equations are very complex, greatly increasing the difficulty of dynamic modeling, and the conventional methods for dynamic modeling of serial mechanisms are no longer applicable. To handle the closed-loop system, Abhinandan Jain's book "Robot and Multibody Dynamics" presents the articulated body algorithm in an embedded constraint form, but it does not clearly indicate how to handle the external force terms (especially the gravity term) of the aggregation nodes, and how to generalize the algorithm to a generalized articulated body algorithm for handling hybrid dynamics. Summary of the Invention

[0004] An object of the present invention is to provide a method for forward / inverse dynamics modeling of a humanoid robot with a closed-loop constraint structure, aiming at the deficiencies of the existing technology.

[0005] The object of the present invention is achieved by the following technical solutions: A method for forward / inverse dynamics modeling of a humanoid robot with a closed-loop constraint structure, comprising the following steps:

[0006] Step 1: Establish the world coordinate system, floating base, and local coordinate systems of each joint of the robot. The knee joint and ankle joint of the robot are driven by a four-bar linkage mechanism. The topological structure of the robot is a closed-loop structure. Establish the analytical expressions of geometric constraint equations and corresponding first-order differential constraint equations, and clarify the independent generalized velocities.

[0007] Step 2: Equivalently transform the topological structure of the robot into an open-loop tree structure: Equivalently process the spatial inertia, spatial acceleration, gyroscopic force vector, rigid body transformation matrix, hinge mapping matrix, and Coriolis spatial acceleration. Combine the foot, universal joint, outer ankle motor, inner ankle motor, calf, knee joint link, and knee joint motor involved in the closed-loop constraint into an aggregation node, and at the same time cut off the four-bar linkage drive mechanisms of the knee joint and ankle joint.

[0008] Step 3: Based on the recursive form of the articulated body algorithm, calculate the accelerations of the floating base and each joint according to the known driving torques of each joint, and obtain the forward dynamics model in the form of embedded constraints.

[0009] Step 4: Based on the generalized articulated body algorithm in recursive form, calculate the floating base acceleration and the driving torque of each joint according to the known joint accelerations, and obtain the inverse dynamics model in the form of constraint embedding.

[0010] Further, the Step 1 is implemented through the following sub-steps:

[0011] (1.1) Take the state where the robot's legs and arms are hanging vertically as the initial state, with the X-axis, Y-axis, and Z-axis of the world coordinate system pointing forward, left, and vertically upward of the robot respectively; at the initial state, the angles of all joints are 0, and the local coordinate systems of the floating base and each joint are parallel to the world coordinate system; regard the floating base and each joint as nodes of the robot's topological structure. For node k, the 6D spatial velocity where, is the rigid body angular velocity, is the velocity of the origin of the local coordinate system. Assume that the rotation angle of node k relative to its parent node is θ k , then:

[0012]

[0013] where the superscript T is the transpose symbol, is the 6D spatial velocity of the parent node , is the rigid body transformation matrix;

[0014] (1.2) The pitching of the robot's knee joint is transmitted through a four-bar mechanism, and the geometric constraint equation is:

[0015]

[0016] where θ 1-5 , θ 1-6 , θ 1-7 are the rotation angles of the left calf, left knee joint link, and left knee joint motor respectively, and θ 5-5 , θ 5-6 , θ 5-7 are the rotation angles of the right calf, right knee joint link, and right knee joint motor respectively;

[0017] (1.3) The rolling and pitching of the robot's ankle joint are transmitted through two four-bar mechanisms. When the lengths of the inner and outer ankle links are constant, two geometric constraint equations are established for the left and right legs respectively:

[0018]

[0019]

[0020] Among them, l1 represents the left ankle outer link, l2 represents the left ankle inner link, l3 represents the right ankle inner link, l4 represents the right ankle outer link, θ 1-1 , θ 1-2 , θ 1-3 , θ 1-4 are the rotation angles of the left foot, left universal joint, left ankle outer motor, and left ankle inner motor respectively. θ 5-1 , θ 5-2 , θ 5-3 , θ 5-4 are the rotation angles of the right foot, right universal joint, right ankle inner motor, and right ankle outer motor respectively. r1, r2, r3, r4 are the position vectors of the connection points between the left ankle outer link and the left foot, the left ankle inner link and the left foot, the left ankle outer link and the left ankle outer motor, and the left ankle inner link and the left ankle inner motor respectively. r5, r6, r7, r8 are the position vectors of the connection points between the right ankle inner link and the right foot, the right ankle outer link and the right foot, the right ankle inner link and the right ankle inner motor, and the right ankle outer link and the right ankle outer motor respectively. l0 is the length of the ankle link;

[0021] (1.4) Differentiate the geometric constraint equations obtained in steps (1.2) and (1.3) to obtain the analytical expressions of the corresponding first-order differential constraint equations:

[0022]

[0023] Regarding the angular velocities of the ankle outer motor, ankle inner motor, and knee joint motor as independent generalized velocities, the angular velocities of the relative rotations of the foot, universal joint, ankle outer motor, ankle inner motor, calf, knee joint link, and knee joint motor involved in the transmission of the knee joint and ankle joint can be expressed as linear functions of these three independent generalized velocities:

[0024] θ1 = [θ 1-1 , θ 1-2 , θ 1-3 , θ 1-4 , θ 1-5 , θ 1-6 , θ 1-7 T

[0025] θ5 = [θ 5-1 , θ 5-2 , θ 5-3 , θ 5-4 , θ 5-5 , θ 5-6 , θ 5-7 T

[0026] θ R1 = [θ 1-3 , θ 1-4 , θ 1-7 ​​​T

[0027] θ R5 = [θ 5-3 , θ 5-4 , θ 5-7 T

[0028] where, θ1 represents the rotation angle vector of node 1, θ5 represents the rotation angle vector of node 5, θ R1 represents the independent rotation angle vector of node 1, θ R5 represents the independent rotation angle vector of node 5, X1 and X5 are coefficient matrices, both with dimensions of 7×3.

[0029] Furthermore, in the second step, the components involved in the closed-loop constraint, namely the foot, universal joint, external ankle motor, internal ankle motor, calf, knee joint link, and knee joint motor, are aggregated into one node. Specifically, the left foot, left universal joint, left external ankle motor, left internal ankle motor, left calf, left knee joint link, and left knee joint motor are aggregated into one node, denoted as node 1; the right foot, right universal joint, right external ankle motor, right internal ankle motor, right calf, right knee joint link, and right knee joint motor are aggregated into one node, denoted as node 5.

[0030] Furthermore, the equivalent treatment of the spatial inertia is specifically as follows:

[0031] For node k, its spatial inertia is defined as the following 6×6 matrix:

[0032]

[0033] where, m k is the mass, is the inertia tensor relative to the local coordinate system, is the position of the center of mass in the local coordinate system, is the 3×3 skew-symmetric matrix obtained by the hat mapping of p k ; the spatial acceleration is defined as the derivative of V k with respect to time t in the local coordinate system; the is defined as the gyroscopic force vector, and its expression is as follows:

[0034]

[0035] where is the following 6×6 matrix:

[0036]

[0037] The equivalent spatial inertia is defined as follows:

[0038] ​

[0039] Among them, M1 is the equivalent spatial inertia of node 1, M 1-1 ,M 1-2 ,M 1-3 ,M 1-4 ,M 1-5 ,M 1-6 ,M 1-7 are the spatial inertias of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left calf, left knee joint link, and left knee joint motor respectively. M5 is the equivalent spatial inertia of node 5, M 5-1 ,M 5-2 ,M 5-3 ,M 5-4 ,M 5-5 ,M 5-6 ,M 5-7 are the spatial inertias of the right foot, right universal joint, right outer ankle motor, right inner ankle motor, right calf, right knee joint link, and right knee joint motor respectively. diag represents constructing a block diagonal matrix.

[0040] Furthermore, the equivalent processing of the spatial acceleration is specifically as follows:

[0041]

[0042] Among them, α1 is the equivalent spatial acceleration of node 1, α 1-1 ,α 1-2 ,α 1-3 ,α 1-4 ,α 1-5 ,α 1-6 ,α 1-7 are the spatial accelerations of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left calf, left knee joint link, and left knee joint motor respectively. α5 is the equivalent spatial acceleration of node 5, α 5-1 ,α 5-2 ,α 5-3 ,α 5-4 ,α 5-5 ,α 5-6 ,α 5-7 are the spatial accelerations of the right foot, right universal joint, right outer ankle motor, right inner ankle motor, right calf, right knee joint link, and right knee joint motor respectively.

[0043] The equivalent processing of the gyro force vector is specifically as follows:

[0044]

[0045] Among them, b1 is the equivalent gyro force vector of node 1, b 1-1 ,b 1-2 ,b 1-3 ,b1-4 , b 1-5 , b 1-6 , b 1-7 are the gyroscopic force vectors of the left foot, left universal joint, left external ankle motor, left internal ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively, and b5 is the equivalent gyroscopic force vector of node 5, b 5-1 , b 5-2 , b 5-3 , b 5-4 , b 5-5 , b 5-6 , b 5-7 are the gyroscopic force vectors of the right foot, right universal joint, right external ankle motor, right internal ankle motor, right lower leg, right knee joint link, and right knee joint motor respectively.

[0046] Furthermore, the equivalent rigid body transformation matrix is specifically:

[0047] The equivalent rigid body transformation matrix connecting node 2 and node 1 and the equivalent rigid body transformation matrix connecting node 6 and node 5 are respectively:

[0048]

[0049] where A1 is the rigid body transformation matrix for the connections between the internal nodes of node 1, E1 is the rigid body transformation matrix for the connection between node 2 and the internal nodes of node 1, A5 is the rigid body transformation matrix for the connections between the internal nodes of node 5, and E5 is the rigid body transformation matrix for the connection between node 6 and the internal nodes of node 5.

[0050] Furthermore, the equivalent hinge mapping matrix is specifically:

[0051] Denote

[0052]

[0053] where H 1 is the hinge mapping matrix of node 1 considering the internal connection relationship, H1 is the combined hinge mapping matrix of node 1, H 5 is the hinge mapping matrix of node 5 considering the internal connection relationship, H5 is the combined hinge mapping matrix of node 5, H 1-1 , H 1-2 , H 1-3 , H 1-4 , H 1-5 , H 1-6 , H 1-7 are the hinge mapping matrices of the left foot, left universal joint, left external ankle motor, left internal ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively, H 5-1 , H 5-2 , H5-3 ,H 5-4 ,H 5-5 ,H 5-6 ,H 5-7 are the hinge mapping matrices of the right foot, right universal joint, right inner ankle motor, right outer ankle motor, right lower leg, right knee joint link, and right knee joint motor, respectively;

[0054] The equivalent hinge mapping matrix is defined as follows:

[0055]

[0056] where, H R1 is the equivalent hinge mapping matrix of node 1, H R5 is the equivalent hinge mapping matrix of node 5.

[0057] Furthermore, the equivalent processing of the Coriolis space acceleration is specifically:

[0058] Denote

[0059]

[0060] where, a 1 is the Coriolis space acceleration of node 1 considering the internal connection relationship, a1 is the combined Coriolis space acceleration of node 1, a 5 is the Coriolis space acceleration of node 5 considering the internal connection relationship, a5 is the combined Coriolis space acceleration of node 5, a 1-1 ,a 1-2 ,a 1-3 ,a 1-4 ,a 1-5 ,a 1-6 ,a 1-7 are the Coriolis space accelerations of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left lower leg, left knee joint link, and left knee joint motor, respectively, a 5-1 ,a 5-2 ,a 5-3 ,a 5-4 ,a 5-5 ,a 5-6 ,a 5-7 are the Coriolis space accelerations of the right foot, right universal joint, right outer ankle motor, right inner ankle motor, right lower leg, right knee joint link, and right knee joint motor, respectively;

[0061] The equivalent Coriolis space acceleration is defined as follows:

[0062]

[0063] Among them, a′1 is the equivalent Coriolis space acceleration of node 1, and a′5 is the equivalent Coriolis space acceleration of node 5.

[0064] Furthermore, the third step is implemented through the following sub-steps:

[0065] (3.1) Perform the following recursion from the end of each branch of the robot tree structure to the floating base:

[0066]

[0067]

[0068]

[0069]

[0070]

[0071] ∈ k = T k - H k ξ k ,

[0072]

[0073]

[0074] Among them, P k represents the articulated body inertia of node k, D k represents the articulated body hinge inertia of node k, G k represents the Kalman gain of node k, represents the articulated body inertia that node k transfers to its parent node , ξ k represents the correction force term of node k, C(k) represents the set of all its child nodes, ∈ k represents the correction joint torque of node k, T k represents the joint driving torque received by node k, represents the relative hinge acceleration of node k in the articulated body model, represents the correction force term that node k transfers to its parent node ;

[0075] When k = 1 or k = 5, that is, for node 1 or node 5, θ1, θ5, H1, H5, a1, a5 in the algorithm are respectively replaced by θ R1 , θ R5 , H R1 , H R5 a′1, a′5;

[0076] (3.2) Let Perform the following recursion from the floating base of the robot tree structure to the end of each branch to solve for the accelerations of each joint and the floating base;

[0077]

[0078]

[0079]

[0080] Among them, represents the spatial acceleration directly transmitted to node k by the rigid body transformation matrix, and represents the angular acceleration of node k.

[0081] (3.3) Using the accelerations of each joint and the floating base solved in step (3.2), construct 32 second-order ordinary differential equations; then transform them into a 64-dimensional first-order ordinary differential equation system, combine with the first-order differential form in step (1.4), and at the same time take θ 1-1 , θ 1-2 and θ 5-1 , θ 5-2 as independent variables, that is, construct a 68-dimensional first-order ordinary differential equation system; and numerically solve the 68-dimensional first-order ordinary differential equation system.

[0082] Furthermore, step four is implemented through the following sub-steps:

[0083] (4.1) From the end of each branch of the robot tree structure to the floating base, where the floating base is node 23, perform the following recursion:

[0084]

[0085] If k≠23, perform

[0086]

[0087]

[0088]

[0089] If k = 23, perform

[0090]

[0091]

[0092]

[0093]

[0094] ∈ k = T k -H k ξ k ,

[0095]

[0096]

[0097] (4.2) Let From the floating base of the robot tree structure to the end of each branch, the following recursive solution is carried out to obtain the floating base acceleration and the driving torque of each joint; the floating base acceleration includes 3 translational accelerations and 3 rotational accelerations of the floating base;

[0098]

[0099] If k≠23, perform

[0100]

[0101] T k = H k f k ,

[0102] If k = 23, perform

[0103]

[0104] Finally, perform

[0105]

[0106] where f k represents the spatial force exerted on node k by its parent node ;

[0107] (4.3) Using the 3 translational accelerations and 3 rotational accelerations of the floating base obtained in step (4.2), construct 6 second-order ordinary differential equations, and then transform them into a 12-dimensional first-order ordinary differential equation system, and numerically solve the 12-dimensional first-order ordinary differential equation system.

[0108] The beneficial effects of the present invention are as follows: The present invention can be used for humanoid robots with four-bar linkage mechanisms for the knee joint and ankle joint. By aggregating the components involved in the transmission of the knee joint and ankle joint into a single node, and defining physical quantities such as equivalent spatial inertia, equivalent gyroscopic force, and equivalent Coriolis spatial acceleration, a forward dynamics model is established using the recursive form of the articulated body algorithm, and an inverse dynamics model is established using the recursive form of the generalized articulated body algorithm, providing an accurate description of the robot's motion and meeting the real-time requirements, which provides a key foundation for the development of whole-body control of robots based on dynamics; the present invention can also provide a reference for the forward / inverse dynamics modeling of other robots with closed-loop constraint structures or complex multi-rigid-body systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0109] Figure 1 is a schematic diagram of a humanoid robot.

[0110] Figure 2 is a schematic diagram of the four-bar linkage mechanism for the knee joint and ankle joint of a humanoid robot.

[0111] Figure 3 is a schematic diagram of the topological structure of a humanoid robot. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0112] Here, exemplary embodiments will be described in detail, and their examples are shown in the drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. On the contrary, they are merely examples of devices and methods consistent with some aspects of the present invention as detailed in the appended claims.

[0113] The terms used in the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The singular forms "a", "the", and "said" used in the present invention and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0114] It should be understood that although the terms first, second, third, etc. may be used in the present invention to describe various information, such information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of the present invention, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein may be interpreted as "when" or "while" or "in response to a determination".

[0115] The present invention will be described in detail below with reference to the accompanying drawings. Without conflict, the features in the following embodiments and implementation manners can be combined with each other.

[0116] A method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure according to the present invention includes the following steps:

[0117] (1) Establish the world coordinate system, floating base and local coordinate systems of each joint of the robot. As Figure 2 shown, the knee joint and ankle joint of the robot are driven by a four-bar linkage mechanism, and the topological structure of the robot system is a closed-loop structure. Establish the analytical expressions of geometric constraint equations and corresponding first-order differential constraint equations, and clarify the independent generalized velocities. Specifically, it includes the following sub-steps:

[0118] (1.1) As Figure 1 shown, take the state where the robot's legs and arms hang vertically downward as the initial state. The X-axis, Y-axis and Z-axis of the world coordinate system point to the front, left and vertically upward of the robot respectively; at the initial state, the angles of each joint are 0, and the local coordinate systems of the floating base and each joint are parallel to the world coordinate system. Considering the Figure 2 knee joint and ankle joint four-bar transmission mechanism shown, the topological structure of the robot system is a closed-loop structure. As Figure 3 shown, where the floating base and each joint correspond to different nodes, and are numbered in sequence as follows: 1-1 is the left foot, 1-2 is the left universal joint, 1-3 is the left ankle outer motor, 1-4 is the left ankle inner motor, 1-5 is the left calf, 1-6 is the left knee joint link, 1-7 is the left knee joint motor, 2 is the left thigh, 3 is the left hip pitch motor, 4 is the left hip roll motor, 5-1 is the right foot, 5-2 is the right universal joint, 5-3 is the right ankle inner motor, 5-4 is the right ankle outer motor, 5-5 is the right calf, 5-6 is the right knee joint link, 5-7 is the right knee joint motor, 6 is the right thigh, 7 is the right hip pitch motor, 8 is the right hip roll motor, 9 is the left hand, 10 is the left wrist roll motor, 11 is the left wrist yaw motor, 12 is the left elbow pitch motor, 13 is the left shoulder yaw motor, 14 is the left shoulder roll motor, 15 is the right hand, 16 is the right wrist roll motor, 17 is the right wrist yaw motor, 18 is the right elbow pitch motor, 19 is the right shoulder yaw motor, 20 is the right shoulder roll motor, 21 is the head, 22 is the neck pitch motor, 23 is the torso (floating base). For node k, define a 6D spatial velocity to describe its motion, where is the rigid body angular velocity, and is the velocity of the origin of the local coordinate system. Let the rotation angle of node k relative to its parent node be θ k , where 1 ≤ 23, the following formula holds:

[0119]

[0120] where the superscript T is the transpose symbol, is the 6D spatial velocity of the parent node and is the rigid body transformation matrix:

[0121]

[0122] where I3 is the 3×3 identity matrix and 0 3×3 is the 3×3 zero matrix, is the vector from the origin of the local coordinate system to the origin of the k-th local coordinate system, the ∧ in is the hat mapping, and H k is the hinge mapping matrix that maps the generalized velocity to the 6D relative spatial velocity between node k and ; each H k is specifically as follows: H 1-1 = [0, 1, 0, 0, 0, 0] T , H 1-2 = [1, 0, 0, 0, 0, 0] T , H 1-3 = [0, 1, 0, 0, 0, 0] T , H 1-4 = [0, 1, 0, 0, 0, 0] T , H 1-5 = [0, 1, 0, 0, 0, 0] T , H 1-6 = [0, 1, 0, 0, 0, 0] T , H 1-7 = [0, 1, 0, 0, 0, 0] T , H2 = [0, 1, 0, 0, 0, 0] T , H3 = [1, 0, 0, 0, 0, 0] T , H4 = [0, 0, 1, 0, 0, 0] T , H 5-1 = [0, 1, 0, 0, 0, 0] T , H 5-2 = [1, 0, 0, 0, 0, 0] T , H 5-3 = [0, 1, 0, 0, 0, 0] T , H 5-4 = [0, 1, 0, 0, 0, 0] T , H 5-5 = [0, 1, 0, 0, 0, 0] T , H 5-6 = [0, 1, 0, 0, 0, 0] T , H5-7 = [0, 1, 0, 0, 0, 0] T , H6 = [0, 1, 0, 0, 0, 0] T , H7 = [1, 0, 0, 0, 0, 0] T , H8 = [0, 0, 1, 0, 0, 0] T , H9 = [1, 0, 0, 0, 0, 0] T , H 10 = [0, 0, 1, 0, 0, 0] T , H 11 = [0, 1, 0, 0, 0, 0] T , H 12 = [0, 0, 1, 0, 0, 0] T , H 13 = [1, 0, 0, 0, 0, 0] T , H 14 = [0, 1, 0, 0, 0, 0] T , H 15 = [1, 0, 0, 0, 0, 0] T , H 16 = [0, 0, 1, 0, 0, 0] T , H 17 = [0, 1, 0, 0, 0, 0] T , H 18 = [0, 0, 1, 0, 0, 0] T , H 19 = [1, 0, 0, 0, 0, 0] T , H 20 = [0, 1, 0, 0, 0, 0] T , H 21 = [0, 1, 0, 0, 0, 0] T , H 22 = [0, 0, 1, 0, 0, 0] T , H 23 = I6.

[0123] (1.2) Establish the transmission constraint equation of the knee joint. The pitch of the robot knee joint is transmitted through a four-bar linkage mechanism, which is always in the same plane and maintains a parallelogram during the movement. Based on this, the geometric constraint equation can be obtained:

[0124]

[0125] Among them, θ 1-5 , θ 1-6 , θ 1-7 are the rotation angles of the left calf, the left knee joint link, and the left knee joint motor respectively, θ 5-5 , θ 5-6 , θ5-7 They are the right calf, the right knee joint connecting rod, and the right knee joint motor rotation angle respectively;

[0126] (1.3) Establish the transmission constraint equations for the ankle joint. The roll and pitch of the robot's ankle joint are transmitted through two four-bar linkages. When the ankle roll angle is not zero, the four-bar linkages are no longer in the same plane but form a spatial quadrilateral, and the corresponding constraint relationships are relatively complex. Considering that the outer ankle link and the inner ankle link have sufficiently small masses compared to other objects, they can be ignored in dynamic modeling, and only the corresponding constraint relationships are retained, that is, the lengths of the inner ankle link and the outer ankle link are constants. Based on this, two geometric constraint equations can be established for the left leg and the right leg respectively:

[0127]

[0128]

[0129] Among them, l1 represents the outer left ankle link, l2 represents the inner left ankle link, l3 represents the inner right ankle link, l4 represents the outer right ankle link, θ 1-1 , θ 1-2 , θ 1-3 , θ 1-4 are the rotation angles of the left foot, the left universal joint, the outer left ankle motor, and the inner left ankle motor respectively, θ 5-1 , θ 5-2 , θ 5-3 , θ 5-4 are the rotation angles of the right foot, the right universal joint, the inner right ankle motor, and the outer right ankle motor respectively, r1, r2, r3, r4 are the position vectors of the connection points between the outer left ankle link and the left foot, the inner left ankle link and the left foot, the outer left ankle link and the outer left ankle motor, and the inner left ankle link and the inner left ankle motor respectively, r5, r6, r7, r8 are the position vectors of the connection points between the inner right ankle link and the right foot, the outer right ankle link and the right foot, the inner right ankle link and the inner right ankle motor, and the outer right ankle link and the outer right ankle motor respectively, and l0 is the length of the ankle link.

[0130] (1.4) Establish the first-order differential form of the constraint equations. Differentiate the geometric constraint equations obtained in steps (1.2) and (1.3) to obtain the analytical expressions of the corresponding first-order differential constraint equations:

[0131]

[0132] Regarding the angular velocities of the outer ankle motor, the inner ankle motor, and the calf pitch as independent generalized velocities, the angular velocities of the relative rotations of other components involved in the transmission of the knee joint and the ankle joint can be expressed as linear functions of these three independent generalized velocities:

[0133] θ1 = [θ 1-1 , θ1-2 , θ 1-3 , θ 1-4 , θ 1-5 , θ 1-6 , θ 1-7 T ,

[0134] θ5 = [θ 5-1 , θ 5-2 , θ 5-3 , θ 5-4 , θ 5-5 , θ 5-6 , θ 5-7 T

[0135] θ R1 = [θ 1-3 , θ 1-4 , θ 1-7 T

[0136] θ R5 = [θ 5-3 , θ 5-4 , θ 5-7 T

[0137] Among them, θ1 represents the rotation angle vector of node 1, θ5 represents the rotation angle vector of node 5, θ R1 represents the independent rotation angle vector of node 1, θ R5 represents the independent rotation angle vector of node 5, X1 and X5 are coefficient matrices, both with a dimension of 7×3.

[0138] (2) Equivalent the topological structure of the robot system to an open-loop tree structure: Based on the six-dimensional space vector description (the space velocity of each node k is described by a six-dimensional space vector), by equivalently processing physical quantities such as spatial inertia, gyroscopic force, and Coriolis space acceleration, aggregate the components involved in the closed-loop constraint, namely the foot, universal joint, outer ankle motor, inner ankle motor, calf, knee joint link, and knee joint motor into one node, and at the same time cut off the four-bar linkage of the knee joint and ankle joint; specifically include the following sub-steps:

[0139] (2.1) Define physical quantities for each node. For node k, define its spatial inertia as the following 6×6 matrix:

[0140]

[0141] Among them, m k is the mass, is the inertia tensor relative to the local coordinate system, is the position of the center of mass in the local coordinate system, is p​​​​k The 3×3 skew-symmetric matrix obtained through the hat mapping; defining the spatial acceleration as V k The derivative with respect to time t in the local coordinate system, and the following formula holds:

[0142]

[0143] where is the spatial acceleration of the parent node , a k is the Coriolis spatial acceleration. For the cylindrical hinge connection, its expression is as follows:

[0144]

[0145] where and are the 3×3 skew-symmetric matrices obtained through the hat mapping of ω k and v k respectively, and Δω k = ω k+1 - ω k ; defining as the gyroscopic force vector, and its expression is as follows:

[0146]

[0147] where is the following 6×6 matrix:

[0148]

[0149] (2.2) Establish the equivalent physical quantities of the aggregated nodes. As Figure 3 shown, the components involved in the knee joint drive and ankle joint drive in the left leg and right leg, namely the foot, universal joint, external ankle motor, internal ankle motor, calf, knee joint link, and knee joint motor, are respectively aggregated into one node. Specifically: the left foot, left universal joint, left external ankle motor, left internal ankle motor, left calf, left knee joint link, and left knee joint motor are aggregated into one node, denoted as node 1; the right foot, right universal joint, right external ankle motor, right internal ankle motor, right calf, right knee joint link, and right knee joint motor are aggregated into one node, denoted as node 5, and at the same time, the knee joint and ankle joint four-bar linkage mechanism is cut off; next, define the equivalent physical quantities of node 1 and node 5: the equivalent spatial inertia is defined as follows:

[0150]

[0151] where M1 is the equivalent spatial inertia of node 1, M 1-1 , M 1-2 , M 1-3 , M1-4 , M 1-5 , M 1-6 , M 1-7 They are the spatial inertias of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively. M5 is the equivalent spatial inertia of node 5, M 5-1 , M 5-2 , M 5-3 , M 5-4 , M 5-5 , M 5-6 , M 5-7 They are the spatial inertias of the right foot, right universal joint, right outer ankle motor, right inner ankle motor, right lower leg, right knee joint link, and right knee joint motor respectively. diag represents constructing a block diagonal matrix;

[0152] The equivalent spatial acceleration is defined as follows:

[0153]

[0154] Among them, α1 is the equivalent spatial acceleration of node 1, α 1-1 , α 1-2 , α 1-3 , α 1-4 , α 1-5 , α 1-6 , α 1-7 They are the spatial accelerations of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively. α5 is the equivalent spatial acceleration of node 5, α 5-1 , α 5-2 , α 5-3 , α 5-4 , α 5-5 , α 5-6 , α 5-7 They are the spatial accelerations of the right foot, right universal joint, right outer ankle motor, right inner ankle motor, right lower leg, right knee joint link, and right knee joint motor respectively;

[0155] The equivalent gyroscopic force vector is defined as follows:

[0156]

[0157] Among them, b1 is the equivalent gyroscopic force vector of node 1, b 1-1 , b 1-2 , b 1-3 , b 1-4 , b 1-5 , b 1-6 , b 1-7The gyroscopic force vectors of the left foot, left universal joint, left external ankle motor, left internal ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively. b5 is the equivalent gyroscopic force vector of node 5, b 5-1 ,b 5-2 ,b 5-3 ,b 5-4 ,b 5-5 ,b 5-6 ,b 5-7 The gyroscopic force vectors of the right foot, right universal joint, right external ankle motor, right internal ankle motor, right lower leg, right knee joint link, and right knee joint motor respectively;

[0158] The equivalent rigid body transformation matrix connecting node 2 and node 1 and the equivalent rigid body transformation matrix connecting node 6 and node 5 are defined as follows respectively:

[0159]

[0160]

[0161]

[0162]

[0163]

[0164] Among them, A1 is the rigid body transformation matrix for the connections between the internal nodes of aggregated node 1, E1 is the rigid body transformation matrix for the connection between node 2 and the internal nodes of aggregated node 1, A5 is the rigid body transformation matrix for the connections between the internal nodes of aggregated node 5, and E5 is the rigid body transformation matrix for the connection between node 6 and the internal nodes of aggregated node 5. 1-2, , φ 1-5, , φ 1-5, , φ 1-5, , φ 1-7, are the rigid body transformation matrices for the connections between the left universal joint and the left foot, between the left lower leg and the left universal joint, between the left lower leg and the left external ankle motor, between the left lower leg and the left internal ankle motor, and between the left knee joint motor and the left knee joint link respectively. φ 5-2, , φ 5-5, , φ 5-5, , φ 5-5, , φ 5-7, are the rigid body transformation matrices for the connections between the right universal joint and the right foot, between the right lower leg and the right universal joint, between the right lower leg and the right internal ankle motor, between the right lower leg and the right external ankle motor, and between the right knee joint motor and the right knee joint link respectively. I6 is the 6th order identity matrix;

[0165] Record Among them

[0166]

[0167] Among them, H 1 is the hinge mapping matrix of Node 1 considering the internal connection relationship, and H1 is the combined hinge mapping matrix of Node 1. H 5 is the hinge mapping matrix of Node 5 considering the internal connection relationship, and H5 is the combined hinge mapping matrix of Node 5, H 1-1 , H 1-2 , H 1-3 , H 1-4 , H 1-5 , H 1-6 , H 1-7 are the hinge mapping matrices of the left foot, left universal joint, left external ankle motor, left internal ankle motor, left calf, left knee joint link, and left knee joint motor respectively, H 5-1 , H 5-2 , H 5-3 , H 5-4 , H 5-5 , H 5-6 , H 5-7 are the hinge mapping matrices of the right foot, right universal joint, right internal ankle motor, right external ankle motor, right calf, right knee joint link, and right knee joint motor respectively;

[0168] The equivalent hinge mapping matrix is defined as follows:

[0169]

[0170] Among them, H R1 is the equivalent hinge mapping matrix of Node 1, H R5 is the equivalent hinge mapping matrix of Node 5;

[0171] Record

[0172]

[0173] Among them, a 1 is the Coriolis space acceleration of Node 1 considering the internal connection relationship, and a1 is the combined Coriolis space acceleration of Node 1. a 5 is the Coriolis space acceleration of Node 5 considering the internal connection relationship, and a5 is the combined Coriolis space acceleration of Node 5, a 1-1 , a 1-2 , a 1-3 , a 1-4 , a 1-5 , a 1-6 , a 1-7The Coriolis space accelerations of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left lower leg, left knee joint link, and left knee joint motor are a 5-1 , a 5-2 , a 5-3 , a 5-4 , a 5-5 , a 5-6 , a 5-7 The Coriolis space accelerations of the right foot, right universal joint, right outer ankle motor, right inner ankle motor, right lower leg, right knee joint link, and right knee joint motor are

[0174] The equivalent Coriolis space acceleration is defined as follows:

[0175]

[0176] Among them, a′1 is the equivalent Coriolis space acceleration of node 1, and a′5 is the equivalent Coriolis space acceleration of node 5.

[0177] (3) Based on the recursive form of the articulated body algorithm, the forward dynamics modeling of the robot is carried out, that is, given the driving torques of each joint, the accelerations of the floating base and each joint are obtained, and the forward dynamics model in the form of constraint embedding is obtained. Specifically, it includes the following sub-steps:

[0178] (3.1) Recursively from the end of each branch of the robot tree structure to the root node (floating base). Specifically as follows:

[0179]

[0180]

[0181]

[0182]

[0183]

[0184] ∈ k = T k -H k ξ k ,

[0185]

[0186]

[0187] Among them, P k represents the articulated body inertia of node k, D k represents the articulated body hinge inertia of node k, G k represents the Kalman gain of node k, Denotes the articulated inertia passed from node k to its parent node , ξ k Denotes the corrective force term of node k, and C(k) represents the set of all its child nodes, ∈ k Denotes the corrective joint torque of node k, T k Denotes the joint driving torque received by node k Denotes the relative hinge acceleration of node k in the articulated body model Denotes the corrective force term passed from node k to its parent node ;

[0188] The following points need to be explained:

[0189] (a) For node k, C(k) represents the set of all its child nodes. In particular, C(2) = 1, C(6) = 5;

[0190] (b) For nodes 1 and 5, θ1, θ5, H1, H5, a1, a5 in the algorithm are replaced by θ R1 , θ R5 , H R1 , H R5 , a′1, a′5 respectively; for node 23 (i.e., the floating base), is the component of V 23 in the local coordinate system, a 23 = 0 6×1 ;

[0191] (c) T1 = [T 1-3 , T 1-4 , T 1-7 T , T5 = [T 5-3 , T 5-4 , T 5-7 T , where T 1-3 , T 1-4 , T 1-7 , T 5-3 , T 5-4 , T 5-7 are the joint driving torques received by nodes 1-3, 1-4, 1-7, 5-3, 5-4, 5-7 respectively; T 23 = 0 6×1 ;

[0192] (d) M k g represents the gravity term, where the gravitational acceleration g = [0, 0, 0, 0, 0, -9.81 m / s 2 T ​​​, for nodes 1 and 5, 7 gs need to be grouped into a 42 - dimensional column vector, i.e., [0,0,0,0,0, - 9.81m / s 2 ,0,0,0,0,0, - 9.81m / s 2 ,0,0,0,0,0, - 9.81m / s 2 ,0,0,0,0,0, - 9.81m / s 2 ,0,0,0,0,0, - 9.81m / s 2 ,0,0,0,0,0, - 9.81m / s 2 ,0,0,0,0,0, - 9.81m / s 2 T ;

[0193] (3.2) Recursively go from the root node (floating base) of the robot tree structure to the end of each branch. Let The recursion is as follows:

[0194]

[0195]

[0196]

[0197] where

[0198] Solve the accelerations of each joint and the floating base;

[0199] (3.3) Using the accelerations of each joint and the floating base solved in step (3.2), a total of 32 accelerations are solved, which are in turn: left ankle external motor angular acceleration, left ankle internal motor angular acceleration, left knee joint motor angular acceleration, left hip pitch angular acceleration, left hip roll angular acceleration, left hip precession angular acceleration, right ankle internal motor angular acceleration, right ankle external motor angular acceleration, right knee joint motor angular acceleration, right hip pitch angular acceleration, right hip roll angular acceleration, right hip precession angular acceleration, left wrist roll angular acceleration, left wrist precession angular acceleration, left elbow pitch angular acceleration, left shoulder precession angular acceleration, left shoulder roll angular acceleration, left shoulder pitch angular acceleration, right wrist roll angular acceleration, right wrist precession angular acceleration, right elbow pitch angular acceleration, right shoulder precession angular acceleration, right shoulder roll angular acceleration, right shoulder pitch angular acceleration, neck pitch angular acceleration, neck precession angular acceleration, 3 translational accelerations and 3 rotational accelerations of the floating base. 32 second - order ordinary differential equations can be constructed, and then they are transformed into a 64 - dimensional first - order ordinary differential equation system to describe the motion of the robot; however, considering that there is no analytical solution to the forward kinematics problem of the geometric constraint equation of the ankle joint transmission in step (1.3), the present invention replaces them with the first - order differential form in step (1.4), and at the same time, θ 1-1 ​, θ 1-2 and θ 4-1 , θ 5-2 also serves as an independent variable, that is, a 68-dimensional system of first-order ordinary differential equations is constructed; after the initial conditions are given, the system of ordinary differential equations can be numerically solved. The initial conditions refer to the initial conditions of this 68-dimensional system of first-order ordinary differential equations. There are 68 unknown variables in the 68-dimensional system of first-order ordinary differential equations, and the numerical values of these 68 variables at the initial time t0 need to be given.

[0200] (4) Based on the recursive form of the generalized articulated body algorithm, the inverse dynamics modeling of the robot is carried out, that is, given the joint accelerations, the floating base acceleration and the joint driving torques are obtained, and the inverse dynamics model in the form of constraint embedding is obtained; specifically, it includes the following sub-steps:

[0201] (4.1) Recursively proceed from the end of each branch of the robot tree structure to the root node (floating base). Specifically as follows:

[0202]

[0203] If k≠23, proceed

[0204]

[0205]

[0206]

[0207] If k = 23 (floating base), proceed

[0208]

[0209]

[0210]

[0211]

[0212]

[0213]

[0214]

[0215] (4.2) Recursively proceed from the root node (floating base) of the robot tree structure to the end of each branch. Let The recursion is specifically as follows:

[0216]

[0217] If k≠23, proceed

[0218]

[0219] T k = H k f k ,

[0220] If k = 23 (floating base), perform

[0221]

[0222] Finally perform

[0223]

[0224] where f k represents the spatial force exerted on node k by its parent node acting on it;

[0225] (4.3) gives the numerical solution format. A total of 3 translational accelerations and 3 rotational accelerations of the floating base are solved in step (4.3), and a 12-dimensional system of first-order ordinary differential equations is constructed to describe the motion of the floating base. After the initial conditions are given, the system of ordinary differential equations can be numerically solved. The initial conditions refer to the initial conditions of this 12-dimensional system of first-order ordinary differential equations. There are a total of 12 unknown variables in the 12-dimensional system of first-order ordinary differential equations, and the numerical values of these 12 variables need to be given at the initial time t0; at the same time, 26 joint driving torques can also be solved, which are in turn: left ankle external motor torque, left ankle internal motor torque, left knee joint motor torque, left hip pitch torque, left hip roll torque, left hip yaw torque, right ankle internal motor torque, right ankle external motor torque, right knee joint motor torque, right hip pitch torque, right hip roll torque, right hip yaw torque, left wrist roll torque, left wrist yaw torque, left elbow pitch torque, left shoulder yaw torque, left shoulder roll torque, left shoulder pitch torque, right wrist roll torque, right wrist yaw torque, right elbow pitch torque, right shoulder yaw torque, right shoulder roll torque, right shoulder pitch torque, neck pitch torque, neck yaw torque.

[0226] The above embodiments are only used to illustrate the design concept and features of the present invention, and the purpose is to enable those skilled in the art to understand the content of the present invention and implement it accordingly. The protection scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes or modifications made according to the principles and design concepts disclosed by the present invention are within the protection scope of the present invention.

[0227] Other embodiments of the present application will be readily apparent to those skilled in the art upon consideration of the specification and practice of the disclosure herein. The present application is intended to cover any variations, uses, or adaptations of the present application, which follow the general principles of the present application and include known common general knowledge or conventional technical means in the technical field not disclosed in the present application. The specification and examples are only to be considered as exemplary.

[0228] It should be understood that the present application is not limited to the exact structures described above and shown in the drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A forward / inverse dynamics modeling method for the whole body of a humanoid robot with a closed-loop constraint structure, characterized in that It includes the following steps: Step 1: Establish the world coordinate system, floating base and local coordinate systems of each joint of the robot. The knee joint and ankle joint of the robot are driven by a four-bar mechanism. The topological structure of the robot is a closed-loop structure. Establish the analytical expressions of geometric constraint equations and corresponding first-order differential constraint equations, and clarify the independent generalized velocities; Step 2: Equivalent the topological structure of the robot to an open-loop tree structure: Equivalently process the spatial inertia, spatial acceleration, gyro force vector, rigid body transformation matrix, hinge mapping matrix and Coriolis spatial acceleration. Combine the foot, universal joint, external ankle motor, internal ankle motor, calf, knee joint link and knee joint motor involved in the closed-loop constraint into an aggregate node, and at the same time cut off the four-bar drive mechanism of the knee joint and ankle joint; Step 3: Based on the recursive form of the articulated body algorithm, according to the known driving torques of each joint, calculate the accelerations of the floating base and each joint, and obtain the forward dynamics model in the form of constraint embedding; Step 4: Based on the recursive form of the generalized articulated body algorithm, according to the known accelerations of each joint, calculate the floating base acceleration and the driving torques of each joint, and obtain the inverse dynamics model in the form of constraint embedding.

2. A method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure according to claim 1, characterized in that The above Step 1 is implemented through the following sub-steps: (1.1) Take the state where the robot's legs and arms hang vertically as the initial state. The X-axis, Y-axis, and Z-axis of the world coordinate system point to the front, left, and vertically upward of the robot respectively; in the initial state, the joint angles are 0, and the local coordinate systems of the floating base and each joint are parallel to the world coordinate system; regard the floating base and each joint as nodes of the robot's topological structure. For node k, the 6-dimensional spatial velocity where, is the rigid body angular velocity, is the velocity of the origin of the local coordinate system. Let the rotation angle of node k relative to its parent node be θ k , then: where the superscript T is the transpose symbol, is the parent node of the 6D spatial velocity, is the rigid body transformation matrix; H k is the generalized velocity mapped to the 6D relative spatial velocity between node k and the hinge mapping matrix; (1.2) The pitch of the robot knee joint is driven by a four-bar mechanism, and the geometric constraint equation is: where, θ 1-5 , θ 1-6 , θ 1-7 are the rotation angles of the left calf, the left knee joint connecting rod, and the left knee joint motor respectively, θ 5-5 , θ 5-6 , θ 5-7 are the rotation angles of the right calf, the right knee joint connecting rod, and the right knee joint motor respectively; (1.3) The roll and pitch of the robot ankle joint are driven by two four-bar mechanisms. When the lengths of the internal ankle link and external ankle link are constant, two geometric constraint equations are established for the left leg and the right leg respectively: Among them, l1 represents the left ankle outer link, l2 represents the left ankle inner link, l3 represents the right ankle inner link, l4 represents the right ankle outer link, θ 1-1 , θ 1-2 , θ 1-3 , θ 1-4 are the rotation angles of the left foot, left universal joint, left ankle outer motor, and left ankle inner motor respectively, θ 5-1 , θ 5-2 , θ 5-3 , θ 5-4 are the rotation angles of the right foot, right universal joint, right ankle inner motor, and right ankle outer motor respectively, r1, r2, r3, r4 are the position vectors of the connection points between the left ankle outer link and the left foot, the left ankle inner link and the left foot, the left ankle outer link and the left ankle outer motor, and the left ankle inner link and the left ankle inner motor respectively, r5, r6, r7, r8 are the position vectors of the connection points between the right ankle inner link and the right foot, the right ankle outer link and the right foot, the right ankle inner link and the right ankle inner motor, and the right ankle outer link and the right ankle outer motor respectively, and l0 is the length of the ankle link; (1.4) Differentiate the geometric constraint equations obtained in steps (1.2) and (1.3) to obtain the analytical expressions of the corresponding first-order differential constraint equations: Regard the angular velocities of the external ankle motor, internal ankle motor and knee joint motor as independent generalized velocities. The angular velocities of relative rotation of the foot, universal joint, external ankle motor, internal ankle motor, calf, knee joint link and knee joint motor involved in the transmission of the knee joint and ankle joint can be expressed as linear functions of these three independent generalized velocities: θ1 = [θ 1-1 , θ 1-2 , θ 1-3 , θ 1-4 , θ 1-5 , θ 1-6 , θ 1-7 T ​ θ5 = [θ 5-1 , θ 5-2 , θ 5-3 , θ 5-4 , θ 5-5 , θ 5-6 , θ 5-7 T ​ θ R1 = [θ 1-3 , θ 1-4 , θ 1-7 T ​ θ R5 = [θ 5-3 , θ 5-4 , θ 5-7 T ​ Among them, θ1 represents the rotation angle vector of node 1, θ5 represents the rotation angle vector of node 5, and θ R1 represents the independent rotation angle vector of node 1, and θ R5 represents the independent rotation angle vector of node 5. X1 and X5 are coefficient matrices, both with a dimension of 7×3.

3. A method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure according to claim 1, characterized in that, In the above Step 2, the components involved in the closed-loop constraint, namely the foot, universal joint, external ankle motor, internal ankle motor, calf, knee joint link and knee joint motor, are aggregated into one node. Specifically, the left foot, left universal joint, left external ankle motor, left internal ankle motor, left calf, left knee joint link and left knee joint motor are aggregated into one node, denoted as node 1; the right foot, right universal joint, right external ankle motor, right internal ankle motor, right calf, right knee joint link and right knee joint motor are aggregated into one node, denoted as node 5.

4. A method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure according to claim 1, characterized in that, The equivalent processing of the spatial inertia is specifically as follows: For node k, define its spatial inertia as the following 6×6 matrix: where m k is the mass, is the inertia tensor with respect to the local coordinate system, is the position of the center of mass in the local coordinate system, is p k is the 3×3 skew-symmetric matrix obtained by the hat map; define the spatial acceleration as V k which is the derivative of V with respect to time t in the local coordinate system; define as the gyroscopic force vector, and its expression is as follows: wherein is a 6×6 matrix as follows: Among them, and are respectively the 3×3 skew-symmetric matrices obtained by the hat mapping of ω k and v k ; V k is the 6D spatial velocity; The equivalent spatial inertia is defined as follows: Among them, M1 is the equivalent spatial inertia of node 1, M 1-1 , M 1-2 , M 1-3 , M 1-4 , M 1-5 , M 1-6 , M 1-7 are the spatial inertias of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left calf, left knee joint link, and left knee joint motor respectively. M5 is the equivalent spatial inertia of node 5, M 5-1 , M 5-2 , M 5-3 , M 5-4 , M 5-5 , M 5-6 , M 5-7 are the spatial inertias of the right foot, right universal joint, right outer ankle motor, right inner ankle motor, right calf, right knee joint link, and right knee joint motor respectively. diag represents constructing a block diagonal matrix.

5. A method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure according to claim 1, characterized in that, The equivalent processing of the spatial acceleration is specifically as follows: Among them, α1 is the equivalent spatial acceleration of node 1, α 1-1 , α 1-2 , α 1-3 , α 1-4 , α 1-5 , α 1-6 , α 1-7 are the spatial accelerations of the left foot, left universal joint, left external ankle motor, left internal ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively. α5 is the equivalent spatial acceleration of node 5, α 5-1 , α 5-2 , α 5-3 , α 5-4 , α 5-5 , α 5-6 , α 5-7 are the spatial accelerations of the right foot, right universal joint, right external ankle motor, right internal ankle motor, right lower leg, right knee joint link, and right knee joint motor respectively. The superscript T represents the transpose symbol; The equivalent processing of the gyro force vector is specifically as follows: Among them, b1 is the equivalent gyroscopic force vector of node 1, b 1-1 , b 1-2 , b 1-3 , b 1-4 , b 1-5 , b 1-6 , b 1-7 are the gyroscopic force vectors of the left foot, left universal joint, left external ankle motor, left internal ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively. b5 is the equivalent gyroscopic force vector of node 5, b 5-1 , b 5-2 , b 5-3 , b 5-4 , b 5-5 , b 5-6 , b 5-7 are the gyroscopic force vectors of the right foot, right universal joint, right external ankle motor, right internal ankle motor, right lower leg, right knee joint link, and right knee joint motor respectively.

6. A method for forward / inverse dynamics modeling of a whole-body humanoid robot with a closed-loop constraint structure according to claim 1, characterized in that The equivalent processing of the rigid body transformation matrix is specifically as follows: Equivalent rigid body transformation matrix for the connection between Node 2 and Node 1 and the equivalent rigid body transformation matrix for the connection between Node 6 and Node 5 are respectively: Among them, A1 is the rigid body transformation matrix for the connection between the internal nodes of node 1, E1 is the rigid body transformation matrix for the connection between node 2 and the internal nodes of node 1, A5 is the rigid body transformation matrix for the connection between the internal nodes of node 5, and E5 is the rigid body transformation matrix for the connection between node 6 and the internal nodes of node 5.

7. A method for forward / inverse dynamics modeling of a whole-body anthropomorphic robot with a closed-loop constraint structure according to claim 1, characterized in that, The equivalent processing hinge mapping matrix is specifically as follows: Record Among them, H 1 is the hinge mapping matrix of Node 1 considering the internal connection relationship, H1 is the combined hinge mapping matrix of Node 1, and A1 is the rigid body transformation matrix aggregating the connections between the internal nodes of Node 1. H 5 is the hinge mapping matrix of Node 5 considering the internal connection relationship, H5 is the combined hinge mapping matrix of Node 5, and A5 is the rigid body transformation matrix aggregating the connections between the internal nodes of Node 5, H 1-1 ,H 1-2 ,H 1-3 ,H 1-4 ,H 1-5 ,H 1-6 ,H 1-7 are the hinge mapping matrices of the left foot, left universal joint, left outer ankle motor, left inner ankle motor, left calf, left knee joint link, and left knee joint motor respectively, and diag represents constructing a block diagonal matrix; 5-1 ,H 5-2 ,H 5-3 ,H 5-4 ,H 5-5 ,H 5-6 ,H 5-7 are the hinge mapping matrices of the right foot, right universal joint, right inner ankle motor, right outer ankle motor, right calf, right knee joint link, and right knee joint motor respectively. The equivalent hinge mapping matrix is defined as follows: where the superscript T represents the transpose symbol, H R1 is the equivalent hinge mapping matrix of node 1, H R5 is the equivalent hinge mapping matrix of node 5, and X1 and X5 are coefficient matrices.

8. A method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure according to claim 1, characterized in that, The equivalent processing Coriolis space acceleration is specifically as follows: Record where the superscript T represents the transpose symbol, A1 is the rigid body transformation matrix for the connections between the internal nodes of aggregation node 1, and A5 is the rigid body transformation matrix for the connections between the internal nodes of aggregation node 5. a 1 is the Coriolis space acceleration of node 1 considering the internal connection relationship, and a1 is the combined Coriolis space acceleration of node 1. a 5 is the Coriolis space acceleration of node 5 considering the internal connection relationship, and a5 is the combined Coriolis space acceleration of node 5. a 1-1 ,a 1-2 ,a 1-3 ,a 1-4 ,a 1-5 ,a 1-6 ,a 1-7 are the Coriolis space accelerations of the left foot, left universal joint, left external ankle motor, left internal ankle motor, left lower leg, left knee joint link, and left knee joint motor respectively. a 5-1 ,a 5-2 ,a 5-3 ,a 5-4 ,a 5-5 ,a 5-6 ,a 5-7 are the Coriolis space accelerations of the right foot, right universal joint, right external ankle motor, right internal ankle motor, right lower leg, right knee joint link, and right knee joint motor respectively; The equivalent Coriolis space acceleration is defined as follows: Where, H1 is the combined hinge mapping matrix of node 1, H5 is the combined hinge mapping matrix of node 51, a′1 is the equivalent Coriolis space acceleration of node 1, and a′5 is the equivalent Coriolis space acceleration of node 5.

9. A method for forward / inverse dynamics modeling of the whole body of a humanoid robot with a closed-loop constraint structure according to claim 2, characterized in that The third step is implemented through the following sub-steps: (3.1) Perform the following recursion from the end of each branch of the robot tree structure to the floating base: ∈ k = T k -H k ξ k , Among them, M k represents the spatial inertia of node k, a k is the Coriolis spatial acceleration, is the gyroscopic force vector, g is the gravitational acceleration, P k represents the articulated body inertia of node k, D k represents the articulated body hinge inertia of node k, G k represents the Kalman gain of node k, represents the articulated body inertia that node k transfers to its parent node ; ξ k represents the corrective force term of node k, C(k) represents the set of all its child nodes, ∈ k represents the corrective joint torque of node k, T k represents the joint driving torque received by node k, represents the relative hinge acceleration of node k in the articulated body model, represents the corrective force term that node k transfers to its parent node ; When k = 1 or k = 5, that is, node 1 or node 5, θ1, θ5, H1, H5, a1, a5 in the articulated body algorithm in recursive form are respectively replaced by θ R1 , θ R5 , H R1 , H R5 , a′1, a′5; where H1 is the combined hinge mapping matrix of node 1, H5 is the combined hinge mapping matrix of node 5, a1 is the combined Coriolis space acceleration of node 1, a5 is the combined Coriolis space acceleration of node 5, H R1 is the equivalent hinge mapping matrix of node 1, H R5 is the equivalent hinge mapping matrix of node 5; a′1 is the equivalent Coriolis space acceleration of node 1, a′5 is the equivalent Coriolis space acceleration of node 5; (3.2) Let From the floating base of the robot tree structure to the end of each branch, perform the following recursion to solve the accelerations of each joint and the floating base; Among them, represents the parent node is directly transferred to the spatial acceleration of node k through the rigid body transformation matrix, represents the angular acceleration of node k; is the parent node of the spatial acceleration; (3.3) Construct 32 second-order ordinary differential equations by using the accelerations of each joint and the floating base obtained in step (3.2); then transform them into a 64-dimensional first-order ordinary differential equation system, combine with the first-order differential form in step (1.4), and at the same time take θ 1-1 , θ 1-2 and θ 5-1 , θ 5-2 as independent variables, that is, construct a 68-dimensional first-order ordinary differential equation system; and numerically solve the 68-dimensional first-order ordinary differential equation system.

10. A method for building a forward / inverse dynamics model of a humanoid robot body with a closed-loop constraint structure as claimed in claim 1, wherein, The fourth step is implemented through the following sub-steps: (4.1) Perform the following recursion from the end of each branch of the robot tree structure to the floating base, where the floating base is node 23: If k≠23, perform If k = 23, perform ∈ k = T k -H k ξ k , Among them, P k represents the articulated body inertia of node k, represents the articulated body inertia that node k transfers to its parent node φ(k), where the superscript T represents the transpose symbol, and M k represents the spatial inertia of node k, and ξ k represents the correction force term of node k, represents the correction force term that node k transfers to its parent node . H k is the hinge mapping matrix that maps the generalized velocity to the 6-dimensional relative spatial velocity between node k and . represents the angular acceleration of node k, and a k is the Coriolis spatial acceleration, is the gyroscopic force vector, g is the gravitational acceleration, c(k) represents the set of all its child nodes, and D k represents the articulated body hinge inertia of node k, G k represents the Kalman gain of node k, and ∈ k represents the corrected joint torque of node k, and T k represents the joint driving torque applied to node k, represents the relative hinge acceleration of node k in the articulated body model;​ (4.2) Let From the floating base of the robot tree structure to the end of each branch, the following recursive solution is carried out to obtain the floating base acceleration and the driving torque of each joint; the floating base acceleration includes 3 translational accelerations and 3 rotational accelerations of the floating base. If k≠23, perform T k = H k f k , If k = 23, perform Finally, perform Among them, f k represents the spatial force exerted on node k by the parent node ; represents the spatial acceleration directly transmitted to node k by the parent node through the rigid body transformation matrix; is the rigid body transformation matrix, is the spatial acceleration of the parent node ; represents the relative hinge acceleration of node k in the articulated body model. (4.3) Use the 3 translational accelerations and 3 rotational accelerations of the floating base solved in step (4.2) to construct 6 second-order ordinary differential equations, and then transform them into a 12-dimensional first-order ordinary differential equation system, and numerically solve the 12-dimensional first-order ordinary differential equation system.

Images