Humanoid robot inverse kinematics trajectory planning method and device, computer equipment and storage medium

Abstract

The invention relates to a humanoid robot inverse kinematics trajectory planning method and device, computer equipment and a storage medium. The method comprises the following steps: planning an initial motion trajectory of a robot; according to the initial motion trail, the theoretical joint angle and the theoretical joint speed of the robot are obtained in an inverse kinematics mode; a kinematics mapping relation between the joint space speed and the working space speed of the robot is obtained; according to the kinematics mapping relation, constraint conditions are obtained, and the constraint conditions comprise a joint angle constraint condition, a joint speed constraint condition and a speed-level joint torque constraint condition; according to the constraint conditions, the theoretical joint angle and the theoretical joint speed are optimized, and an optimized joint angle and an optimized joint speed are obtained; and according to the optimized joint angle and the optimized joint speed, an optimized joint movement track is obtained. According to the invention, the joint motion trail is smooth.

Description

Technical Field

[0001] The present invention relates to the field of robots, and in particular to a method, device, computer equipment and storage medium for inverse kinematics trajectory planning of a humanoid robot. Background Art

[0002] With the continuous improvement of information technology, automation and intelligence, humanoid robots are increasingly used in complex working environments because of their higher flexibility and adaptability.

[0003] Kinematics and inverse kinematics are the basis of humanoid robot control. Kinematics refers to determining the position and posture of the end effector according to the angles of each joint, while inverse kinematics refers to determining the angles of each joint of the robot according to the position and posture of the end effector.

[0004] The humanoid robot has multiple joints throughout its body, including some redundant joints, especially the leg joints. Each joint corresponds to a degree of freedom. The additional degrees of freedom expand the workspace of the humanoid robot and make it more adaptable to walk in irregular environments. However, since the redundant structure has multiple solutions to the inverse kinematics problem, the additional degrees of freedom will also lead to the problem of redundant solution.

[0005] The traditional methods to solve inverse kinematics problems are analytical methods or numerical iteration methods. However, this method has high computational cost, high complexity, low efficiency in considering singular points, and its application scope is greatly limited. Summary of the invention

[0006] In order to solve the above technical problem or at least partially solve the above technical problem, the present invention provides a method, device, computer equipment and storage medium for inverse kinematics trajectory planning of a humanoid robot.

[0007] In a first aspect, the present invention provides a method for inverse kinematics trajectory planning of a humanoid robot, the method comprising:

[0008] Plan the robot's initial motion trajectory;

[0009] According to the initial motion trajectory, obtaining the theoretical joint angles and theoretical joint velocities of the robot by means of inverse kinematics;

[0010] Obtain the kinematic mapping relationship between the robot joint space velocity and the workspace velocity;

[0011] According to the kinematic mapping relationship, constraint conditions are obtained, wherein the constraint conditions include: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions;

[0012] According to the constraint conditions, the theoretical joint angle and the theoretical joint speed are optimized to obtain an optimized joint angle and an optimized joint speed;

[0013] According to the optimized joint angle and the optimized joint speed, an optimized joint motion trajectory is obtained.

[0014] Optionally, obtaining the kinematic mapping relationship between the robot joint space velocity and the workspace velocity includes:

[0015] According to the theoretical joint angles, the left foot end posture and the right foot end posture are obtained;

[0016] Acquire a left Jacobian matrix according to the left foot end posture, and acquire a right Jacobian matrix according to the right foot end posture;

[0017] The kinematic mapping relationship is acquired according to the left Jacobian matrix and the right Jacobian matrix.

[0018] Optionally, the left foot end posture and the right foot end posture are obtained according to the theoretical joint angle in the following manner:

[0019] X1(q)=[p x1 p y1 p z1 r x1 r y1 r z1 ]

[0020] X2(q)=[p x2 p y2 p z2 r x2 r y2 r z2 ]

[0021] Among them, X1(q) is the left foot end posture, X2(q) is the right foot end posture, and q is the theoretical joint angle;

[0022] The left Jacobian matrix is ​​obtained according to the left foot end posture in the following manner:

[0023]

[0024] The right Jacobian matrix is ​​obtained according to the right foot end posture in the following manner:

[0025]

[0026] Among them, J1(q) is the left Jacobian matrix, and J2(q) is the right Jacobian matrix;

[0027] The kinematic mapping relationship is obtained according to the left Jacobian matrix and the right Jacobian matrix in the following manner:

[0028]

[0029] Among them, V i is the workspace speed, is the space velocity of the robot joint, i=1,2.

[0030] Optionally, according to the kinematic mapping relationship, the joint angle constraint condition and the joint velocity constraint condition are obtained in the following manner:

[0031]

[0032] sq i,lb ≤q i,d ≤q i,ib

[0033]

[0034] Where i = 1, 2, q i,d is the joint angle, is the joint velocity, is the robot joint space velocity, is the speed of the workspace, J(q) is the Jacobian matrix, γ is the parameter, W i is the weight of the left and right foot, st is used to represent the constraint condition, q i,lb is the lower bound of the joint angle constraint, q i,ib is the upper bound of the joint angle constraint, is the lower bound of the joint velocity constraint, is the upper bound of the joint velocity constraint.

[0035] Optionally, according to the kinematic mapping relationship, the speed level joint torque constraint condition is as follows:

[0036]

[0037] Among them, τ min is the minimum joint torque, M(q) is the joint space inertia matrix, is the speedometer joint torque, is the joint velocity, Δt is, τ max is the maximum joint torque, is the Coriolis and centripetal coupling matrix, G(q) is the gravity load, J T f c is the contact torque, and Δt is the sampling interval.

[0038] Optionally, the optimized joint motion trajectory is obtained according to the optimized joint angle and the optimized joint speed in the following manner:

[0039]

[0040] Among them, q d ′ is the optimized joint motion trajectory, q′ is the joint angle, is the joint velocity after optimization, and Δt is the sampling interval.

[0041] In a second aspect, a humanoid robot inverse kinematics trajectory planning device is provided, the device comprising:

[0042] Trajectory planning unit, used to plan the initial motion trajectory of the robot;

[0043] A processing unit, used for obtaining a theoretical joint angle and a theoretical joint speed of the robot in an inverse kinematics manner according to the initial motion trajectory;

[0044] A mapping unit, used to obtain a kinematic mapping relationship between the robot joint space velocity and the workspace velocity;

[0045] A constraint unit, used for obtaining constraint conditions according to the kinematic mapping relationship, wherein the constraint conditions include: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions;

[0046] An optimization unit, used for optimizing the theoretical joint angle and the theoretical joint speed according to the constraint conditions to obtain an optimized joint angle and an optimized joint speed;

[0047] The trajectory planning unit is also used to obtain an optimized joint motion trajectory according to the optimized joint angle and the optimized joint speed.

[0048] Optionally, the constraint unit is further used to set the speed level joint torque constraint condition according to the kinematic mapping relationship in the following manner:

[0049]

[0050] Among them, τ min is the minimum joint torque, M(q) is the joint space inertia matrix, is the speedometer joint torque, is the joint velocity, Δt is, τ max is the maximum joint torque, is the Coriolis and centripetal coupling matrix, G(q) is the gravity load, J T f c is the contact torque, and Δt is the sampling interval.

[0051] In a third aspect, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements any of the methods described above when executing the computer program.

[0052] In a fourth aspect, a computer-readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the method as described in any one of the above items is implemented.

[0053] The present invention provides a method, device, computer equipment and storage medium for inverse kinematic trajectory planning of a humanoid robot, the method comprising: planning the initial motion trajectory of the robot; obtaining the theoretical joint angle and theoretical joint speed of the robot in an inverse kinematic manner according to the initial motion trajectory; obtaining the kinematic mapping relationship between the robot joint space speed and the workspace speed; obtaining the constraint conditions according to the kinematic mapping relationship, the constraint conditions comprising: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions; optimizing the theoretical joint angle and the theoretical joint speed according to the constraint conditions to obtain the optimized joint angle and the optimized joint speed; obtaining the optimized joint motion trajectory according to the optimized joint angle and the optimized joint speed. The method of the embodiment of the present invention obtains the constraint conditions after the inverse kinematics is solved, the constraint conditions comprising: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions. Optimizing the theoretical joint angle and the theoretical joint speed according to the constraint conditions can simplify and accelerate the process of inverse kinematics solution, and make the result obtained by inverse kinematics solution more accurate. In addition, considering the robot torque constraint in the constraint conditions can make the obtained solution more consistent with the robot structure, making the generated trajectory smooth enough, so that the humanoid robot can accurately achieve the expected trajectory of the plan. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0055] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0056] Figure 1 The figure shows an application environment diagram of the inverse kinematics trajectory planning method for a humanoid robot according to an embodiment of the present invention;

[0057] Figure 2FIG. 1 is a flow chart of a method for inverse kinematics trajectory planning of a humanoid robot according to an embodiment of the present invention;

[0058] Figure 3 FIG. 1 is a structural block diagram of an inverse kinematics trajectory planning device for a humanoid robot according to an embodiment of the present invention;

[0059] Figure 4 FIG. 2 is a diagram showing the internal structure of a computer device in an embodiment of the present invention. DETAILED DESCRIPTION

[0060] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0061] Figure 1 FIG. 1 is an application environment diagram of a humanoid robot inverse kinematics trajectory planning method in an embodiment. Figure 1 , the humanoid robot inverse kinematics trajectory planning method is applied to the humanoid robot inverse kinematics trajectory planning system. The humanoid robot inverse kinematics trajectory planning method includes a terminal 110 and / or a server 120. The terminal 110 and the server 120 are connected via a network. The terminal 110 can be a desktop terminal or a mobile terminal, and the mobile terminal can be at least one of a mobile phone, a tablet computer, a laptop computer, etc. The server 120 can be implemented by an independent server or a server cluster composed of multiple servers.

[0062] The humanoid robot inverse kinematics trajectory planning method of the present invention is applied to the terminal 110 and / or the server 120 .

[0063] like Figure 2 As shown, in one embodiment, a method for inverse kinematics trajectory planning of a humanoid robot is provided. This embodiment mainly applies the method to the above Figure 1 The server 120 in FIG. Figure 2 , the humanoid robot inverse kinematics trajectory planning method includes:

[0064] Step 210, planning the initial motion trajectory of the robot;

[0065] Step 220, obtaining the theoretical joint angles and theoretical joint velocities of the robot in an inverse kinematics manner according to the initial motion trajectory;

[0066] Step 230, obtaining a kinematic mapping relationship between the robot joint space velocity and the workspace velocity;

[0067] Step 240, obtaining constraint conditions according to the kinematic mapping relationship, wherein the constraint conditions include: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions;

[0068] Step 250, optimizing the theoretical joint angle and the theoretical joint speed according to the constraint conditions to obtain an optimized joint angle and an optimized joint speed;

[0069] Step 260: Acquire an optimized joint motion trajectory according to the optimized joint angle and the optimized joint speed.

[0070] The method of the embodiment of the present invention obtains constraints after solving the inverse kinematics, and the constraints include: joint angle constraints, joint speed constraints, and speed-level joint torque constraints. Optimizing the theoretical joint angles and theoretical joint speeds according to the constraints can simplify and accelerate the process of solving the inverse kinematics, and make the results obtained by the inverse kinematics solution more accurate. In addition, considering the robot torque constraint in the constraints can make the obtained solution more consistent with the structure of the robot, so that the generated trajectory is smooth enough, so that the humanoid robot can accurately realize the expected trajectory of the plan.

[0071] In the embodiment of the present invention, the humanoid and robot lower limbs are established as a kinematic model of joints and connecting rods. In the kinematic model, the joint configuration space is as follows:

[0072] q=[q1,q2,q3,…,q m ] T

[0073] Among them, q1, q2, q3, ..., q m They are the corresponding joint angles of the joints numbered 1, 2, 3, …, m.

[0074] The humanoid robot of the embodiment of the present invention has 20 degrees of freedom of the whole body, of which the floating base of the fuselage has 6 uncontrollable degrees of freedom, the waist has 2 controllable degrees of freedom, and the two legs have 6 controllable degrees of freedom respectively.

[0075] The embodiments of the present invention can also realize inverse kinematics solution and optimization with more degrees of freedom.

[0076] Step 230, obtaining the kinematic mapping relationship between the robot joint space velocity and the workspace velocity, includes:

[0077] According to the theoretical joint angles, the left foot end posture and the right foot end posture are obtained;

[0078] Acquire a left Jacobian matrix according to the left foot end posture, and acquire a right Jacobian matrix according to the right foot end posture;

[0079] The kinematic mapping relationship is acquired according to the left Jacobian matrix and the right Jacobian matrix.

[0080] In the embodiment of the present invention, the left foot end posture and the right foot end posture are obtained according to the theoretical joint angle in the following manner:

[0081] X1(q)=[p x1 p y1 p z1 r x1 r y1 r z1 ]

[0082] X2(q)=[p x2 p y2 p z2 r x2 r y2 r z2 ]

[0083] Among them, X1(q) is the left foot end posture, X2(q) is the right foot end posture, and q is the theoretical joint angle;

[0084] The left Jacobian matrix is ​​obtained according to the left foot end posture in the following manner:

[0085]

[0086] The right Jacobian matrix is ​​obtained according to the right foot end posture in the following manner:

[0087]

[0088] Among them, J1(q) is the left Jacobian matrix, and J2(q) is the right Jacobian matrix;

[0089] The kinematic mapping relationship is obtained according to the left Jacobian matrix and the right Jacobian matrix in the following manner:

[0090]

[0091] Among them, V i is the workspace speed, is the robot joint space velocity, i=1,2.

[0092] In the embodiment of the present invention, in step 240, the joint angle constraint condition and the joint speed constraint condition are obtained according to the kinematic mapping relationship in the following manner:

[0093]

[0094] sq i,lb ≤q i,d ≤q i,UB (Formula 2)

[0095]

[0096] Where i = 1, 2, q i,d is the joint angle, is the joint velocity, is the robot joint space velocity, is the speed of the workspace, J(q) is the Jacobian matrix, γ is the parameter, W i is the weight of the left and right foot, st is used to represent the constraint condition, q i,lb is the lower bound of the joint angle constraint, q i,ub is the upper bound of the joint angle constraint, is the lower bound of the joint velocity constraint, is the upper bound of the joint velocity constraint.

[0097] In the embodiment of the present invention, in step 240, according to the kinematic mapping relationship, the speed level joint torque constraint condition is as follows:

[0098]

[0099] Among them, τ min is the minimum joint torque, M(q) is the joint space inertia matrix, is the speedometer joint torque, is the joint velocity, Δt is, τ max is the maximum joint torque, is the Coriolis and centripetal coupling matrix, G(q) is the gravity load, J T f c is the contact torque, and Δt is the sampling interval.

[0100] In the embodiment of the present invention, the robot dynamics are as follows:

[0101]

[0102] Where M(q) is the joint space inertia matrix, are the Coriolis and centripetal coupling matrices, G(q) is the gravity load, τ is the joint torque, J T f c is the contact torque.

[0103] The joint torque constraint is written as follows:

[0104]

[0105]

[0106] The above dynamic constraints are described at the acceleration level. In the embodiment of the present invention, the speed level can be controlled according to the mechanical structure and requirements of the humanoid robot, so the joint acceleration can be Approximately:

[0107]

[0108] Based on the acceleration approximation, Equation 6 can be reconstructed into Equation 4

[0109] In the embodiment of the present invention, step 260, obtaining the optimized joint motion trajectory according to the optimized joint angle and the optimized joint speed, is performed in the following manner:

[0110]

[0111] Among them, q d ′ is the optimized joint motion trajectory, q′ is the joint angle, is the joint velocity after optimization, and Δt is the sampling interval.

[0112] The method of the embodiment of the present invention obtains constraints after solving the inverse kinematics, and the constraints include: joint angle constraints, joint speed constraints, and speed-level joint torque constraints. Optimizing the theoretical joint angles and theoretical joint speeds according to the constraints can simplify and accelerate the process of solving the inverse kinematics, and make the results obtained by the inverse kinematics solution more accurate. In addition, considering the robot torque constraint in the constraints can make the obtained solution more consistent with the structure of the robot, so that the generated trajectory is smooth enough, so that the humanoid robot can accurately realize the expected trajectory of the plan.

[0113] like Figure 3 As shown, the present invention also provides a humanoid robot inverse kinematics trajectory planning device, the device comprising:

[0114] A trajectory planning unit 310 is used to plan the initial motion trajectory of the robot;

[0115] The processing unit 320 is used to obtain the theoretical joint angles and theoretical joint velocities of the robot in an inverse kinematics manner according to the initial motion trajectory;

[0116] A mapping unit 330 is used to obtain a kinematic mapping relationship between the robot joint space velocity and the workspace velocity;

[0117] A constraint unit 340 is used to obtain constraint conditions according to the kinematic mapping relationship, wherein the constraint conditions include: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions;

[0118] An optimization unit 350, configured to optimize the theoretical joint angle and the theoretical joint speed according to the constraint conditions to obtain an optimized joint angle and an optimized joint speed;

[0119] The trajectory planning unit 310 is further configured to obtain an optimized joint motion trajectory according to the optimized joint angle and the optimized joint speed.

[0120] In the embodiment of the present invention, the mapping unit 330 is further used for:

[0121] According to the theoretical joint angles, the left foot end posture and the right foot end posture are obtained;

[0122] Acquire a left Jacobian matrix according to the left foot end posture, and acquire a right Jacobian matrix according to the right foot end posture;

[0123] The kinematic mapping relationship is acquired according to the left Jacobian matrix and the right Jacobian matrix.

[0124] In the embodiment of the present invention, the mapping unit 330 is further used to obtain the left foot end posture and the right foot end posture according to the theoretical joint angle in the following manner:

[0125] X1(q)=[p x1 p y1 p z1 r x1 r y1 r z1 ]

[0126] X2(q)=[p x2 p y2 p z2 r x2 r y2 r z2 ]

[0127] Among them, X1(q) is the left foot end posture, X2(q) is the right foot end posture, and q is the theoretical joint angle;

[0128] The mapping unit 330 is further configured to obtain a left Jacobian matrix according to the left foot end posture in the following manner:

[0129]

[0130] The mapping unit 330 is further configured to obtain a right Jacobian matrix according to the right foot end posture in the following manner:

[0131]

[0132] Among them, J1(q) is the left Jacobian matrix, and J2(q) is the right Jacobian matrix;

[0133] The kinematic mapping relationship is obtained according to the left Jacobian matrix and the right Jacobian matrix in the following manner:

[0134]

[0135] Among them, V i is the workspace speed, is the robot joint space velocity, i=1,2.

[0136] In the embodiment of the present invention, the constraint unit 340 is further used to obtain the joint angle constraint condition and the joint speed constraint condition according to the kinematic mapping relationship in the following manner:

[0137]

[0138] sq i,lb ≤q i,d ≤q i,ub

[0139]

[0140] Where i = 1, 2, q i,d is the joint angle, is the joint velocity, is the robot joint space velocity, is the speed of the workspace, J(q) is the Jacobian matrix, γ is the parameter, W i is the weight of the left and right foot, st is used to represent the constraint condition, q i,lb is the lower bound of the joint angle constraint, q i,ub is the upper bound of the joint angle constraint, is the lower bound of the joint velocity constraint, is the upper bound of the joint velocity constraint.

[0141] In the embodiment of the present invention, the constraint unit 340 is further used to constrain the speed level joint torque according to the kinematic mapping relationship in the following manner:

[0142]

[0143] Among them, τ min is the minimum joint torque, M(q) is the joint space inertia matrix, is the speedometer joint torque, is the joint velocity, Δt is, τmax is the maximum joint torque, is the Coriolis and centripetal coupling matrix, G(q) is the gravity load, J T f c is the contact torque, and Δt is the sampling interval.

[0144] In the embodiment of the present invention, the constraint unit 340 is further configured to obtain the optimized joint motion trajectory according to the optimized joint angle and the optimized joint speed in the following manner:

[0145]

[0146] Among them, q d ′ is the optimized joint motion trajectory, q′ is the joint angle, is the joint velocity after optimization, and Δt is the sampling interval.

[0147] The embodiments of the present invention can simplify and accelerate the process of inverse kinematics solution, and make the result obtained by inverse kinematics solution more accurate. In addition, considering the robot torque constraint in the constraint condition can make the obtained solution more consistent with the structure of the robot, make the generated trajectory smooth enough, and make the humanoid robot accurately realize the expected trajectory of the plan.

[0148] An embodiment of the present invention also provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the following method when executing the computer program: planning the initial motion trajectory of the robot; obtaining the theoretical joint angles and theoretical joint velocities of the robot in an inverse kinematic manner based on the initial motion trajectory; obtaining the kinematic mapping relationship between the joint space velocity and the workspace velocity of the robot; obtaining constraints based on the kinematic mapping relationship, the constraints comprising: joint angle constraints, joint velocity constraints, and speed-level joint torque constraints; optimizing the theoretical joint angles and the theoretical joint velocities based on the constraints to obtain optimized joint angles and optimized joint velocities; obtaining an optimized joint motion trajectory based on the optimized joint angles and the optimized joint velocities.

[0149] An embodiment of the present invention also provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the following method is implemented: planning the initial motion trajectory of the robot; obtaining the theoretical joint angles and theoretical joint velocities of the robot in an inverse kinematic manner based on the initial motion trajectory; obtaining the kinematic mapping relationship between the joint space velocity of the robot and the workspace velocity; obtaining constraints based on the kinematic mapping relationship, and the constraints include: joint angle constraints, joint velocity constraints, and speed-level joint torque constraints; optimizing the theoretical joint angles and the theoretical joint velocities based on the constraints to obtain optimized joint angles and optimized joint velocities; obtaining an optimized joint motion trajectory based on the optimized joint angles and the optimized joint velocities.

[0150] The above-mentioned humanoid robot inverse kinematics trajectory planning method achieves the beneficial effect of being able to solve the technical problems raised in the background technology.

[0151] Figure 2 FIG. 1 is a flow chart of a method for inverse kinematics trajectory planning of a humanoid robot in one embodiment. Figure 2 The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 2 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0152] Figure 4 The internal structure diagram of a computer device in one embodiment is shown. The computer device may specifically be Figure 1 The server 120 in FIG. Figure 4As shown, the computer device includes a processor, a memory, a network interface, an input device and a display screen connected through a system bus. Among them, the memory includes a non-volatile storage medium and an internal memory. The non-volatile storage medium of the computer device stores an operating system and may also store a computer program. When the computer program is executed by the processor, the processor can implement the inverse kinematics trajectory planning method for a humanoid robot. The internal memory may also store a computer program. When the computer program is executed by the processor, the processor can implement the inverse kinematics trajectory planning method for a humanoid robot. The display screen of the computer device may be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device may be a touch layer covering the display screen, or a button, trackball or touchpad provided on the housing of the computer device, or an external keyboard, touchpad or mouse, etc.

[0153] Those skilled in the art will understand that Figure 4 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present invention, and does not constitute a limitation on the computer device to which the solution of the present invention is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0154] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by instructing the relevant hardware through a computer program, and the program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided by the present invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0155] It should be noted that, in this article, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0156] The foregoing is merely a specific embodiment of the present invention, which enables those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features claimed herein.

Claims

1. A humanoid robot inverse kinematics trajectory planning method, characterized in that: The method comprises: Plan the robot's initial motion trajectory; According to the initial motion trajectory, obtaining the theoretical joint angles and theoretical joint velocities of the robot by means of inverse kinematics; Obtain the kinematic mapping relationship between the robot joint space velocity and the workspace velocity; According to the kinematic mapping relationship, constraint conditions are obtained, wherein the constraint conditions include: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions; According to the constraint conditions, the theoretical joint angle and the theoretical joint speed are optimized to obtain an optimized joint angle and an optimized joint speed; According to the optimized joint angle and the optimized joint speed, an optimized joint motion trajectory is obtained.

2. The method according to claim 1, characterized in that: The obtaining of the kinematic mapping relationship between the robot joint space velocity and the workspace velocity comprises: According to the theoretical joint angles, the left foot end posture and the right foot end posture are obtained; Acquire a left Jacobian matrix according to the left foot end posture, and acquire a right Jacobian matrix according to the right foot end posture; The kinematic mapping relationship is acquired according to the left Jacobian matrix and the right Jacobian matrix.

3. The method according to claim 2, characterized in that The left foot end posture and the right foot end posture are obtained according to the theoretical joint angle in the following manner: X1(q)=[p x1 p y1 p z1 r x1 r y1 r z1 ] X2(q)=[p x 2 p y 2 p z 2 r x2 r y2 r z2 ] Among them, X1(q) is the left foot end posture, X2(q) is the right foot end posture, and q is the theoretical joint angle; The left Jacobian matrix is ​​obtained according to the left foot end posture in the following manner: The right Jacobian matrix is ​​obtained according to the right foot end posture in the following manner: Among them, J1(q) is the left Jacobian matrix, and J2(q) is the right Jacobian matrix; The kinematic mapping relationship is obtained according to the left Jacobian matrix and the right Jacobian matrix in the following manner: Among them, V i is the workspace speed, is the space velocity of the robot joint, i=1,2.

4. The method according to claim 3, characterized in that According to the kinematic mapping relationship, the joint angle constraint conditions and the joint velocity constraint conditions are obtained in the following manner: s.t.q i,lb ≤q i,d ≤q i,ub Where i = 1, 2, q i,d is the joint angle, is the joint velocity, is the robot joint space velocity, is the speed of the workspace, J(q) is the Jacobian matrix, γ is the parameter, W i is the weight of the left and right foot, st is used to represent the constraint condition, q i,lb is the lower bound of the joint angle constraint, q i,ub is the upper bound of the joint angle constraint, is the lower bound of the joint velocity constraint, The upper bound of the joint velocity constraint.

5. The method according to claim 4, characterized in that According to the kinematic mapping relationship, the speed level joint torque constraint condition is as follows: Among them, τ min is the minimum joint torque, M(q) is the joint space inertia matrix, is the speedometer joint torque, is the joint velocity, Δt is, τ max is the maximum joint torque, is the Coriolis and centripetal coupling matrix, G(q) is the gravity load, J T f c is the contact torque, and Δt is the sampling interval.

6. The method according to claim 5, characterized in that The optimized joint motion trajectory is obtained according to the optimized joint angle and the optimized joint speed in the following manner: Among them, q d ′ To optimize the joint motion trajectory, q ′ is the joint angle, is the joint velocity after optimization, and Δt is the sampling interval.

7. A humanoid robot inverse kinematics trajectory planning device, characterized in that: The device comprises: Trajectory planning unit, used to plan the initial motion trajectory of the robot; A processing unit, used for obtaining a theoretical joint angle and a theoretical joint speed of the robot in an inverse kinematics manner according to the initial motion trajectory; A mapping unit, used to obtain a kinematic mapping relationship between the robot joint space velocity and the workspace velocity; A constraint unit, used for obtaining constraint conditions according to the kinematic mapping relationship, wherein the constraint conditions include: joint angle constraint conditions, joint speed constraint conditions and speed level joint torque constraint conditions; An optimization unit, used for optimizing the theoretical joint angle and the theoretical joint speed according to the constraint conditions to obtain an optimized joint angle and an optimized joint speed; The trajectory planning unit is also used to obtain an optimized joint motion trajectory according to the optimized joint angle and the optimized joint speed.

8. The device according to claim 7, characterized in that The constraint unit is also used to constrain the speed level joint torque according to the kinematic mapping relationship in the following manner: Among them, τ min is the minimum joint torque, M(q) is the joint space inertia matrix, is the speedometer joint torque, is the joint velocity, Δt is, τ max is the maximum joint torque, is the Coriolis and centripetal coupling matrix, G(q) is the gravity load, J T f c is the contact torque, and Δt is the sampling interval.

9. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the method according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images