Inverse Kinematics Solving Method for Humanoid Upper Limb Robot Based on Virtual Dynamics Constraint

Abstract

Inverse kinematics solution method for humanoid upper limb robot based on virtual dynamic constraints, belonging to the technical field of humanoid upper limb robots. The method includes: establishing a link coordinate system for the humanoid upper limb robot, obtaining the forward kinematics equation through parametric modeling; taking the fourth derivative of the forward kinematics equation to obtain a fourth-order forward kinematics model; introducing virtual flexible joint dynamic constraints; substituting the virtual flexible joint dynamic constraints into the fourth-order forward kinematics model to obtain a state space equation for the inverse kinematics solution problem; designing a control law to make the state variables in the obtained state space equation converge to zero; according to the designed control law, and then using the virtual flexible joint dynamic constraints, solving to obtain the values of the joint angle trajectories of the two arms. The present invention has the advantages of fast convergence speed, smooth angle and angular velocity curves obtained, and the ability to suppress noise. It is applied to the inverse kinematics calculation of humanoid upper limb robots, and the robot has more excellent kinematic performance.

Description

Technical Field

[0001] The present invention relates to the technical field of humanoid upper limb robots, and particularly to an inverse kinematics solution method for humanoid upper limb robots based on virtual dynamics constraints. Background Technique

[0002] In the process of robot or manipulator control, for a serial joint robot / manipulator, if the angles of each joint are known to calculate the position and pose of the end of the robot / manipulator, this process is forward kinematics solution; conversely, if the position and pose of the end are known to calculate the angles of each joint, this process is called inverse kinematics solution, that is, inverse kinematics describes the mapping relationship from the position and pose of the end of the robot / manipulator to the angles of each joint of the robot / manipulator.

[0003] Early methods generally used the pseudo-inverse method to calculate, but there are disadvantages such as unavoidable joint limits and large influence of visual noise. In recent years, scholars have proposed methods such as the gradient descent method and recursive neural networks for solving the inverse kinematics of manipulators with constraints. However, the gradient descent method has problems of being sensitive to noise and unable to solve at singular configurations, and the recursive neural network method has problems of slow convergence speed and non-smooth speed curve. And these methods generally need to add an additional Kalman filtering link for noise reduction processing when applied to visual servo. Summary of the Invention

[0004] In order to solve the problems of being sensitive to noise, slow convergence speed, and non-smooth speed existing in the existing methods, the present invention proposes an inverse kinematics solution method for humanoid upper limb robots based on virtual dynamics constraints.

[0005] The inverse kinematics solution method for humanoid upper limb robots based on virtual dynamics constraints includes the following steps:

[0006] S1. Establish the link coordinate system of the humanoid upper limb robot, and obtain the forward kinematics equation through D-H parameter modeling;

[0007] S2. Take the fourth derivative of the forward kinematics equation to obtain the fourth-order forward kinematics model;

[0008] S3. Introduce virtual flexible joint dynamics constraints;

[0009] S4. Substitute the virtual flexible joint dynamics constraints in step S3 into the fourth-order forward kinematics model obtained in step S2 to obtain the state space equation for the inverse kinematics solution problem;

[0010] S5. Design a control law to make the state variables in the state space equation obtained in step S4 converge to zero;

[0011] S6. According to the designed control law, and then using the virtual flexible joint dynamic constraints in step S3, solve to obtain the values of the joint angle trajectories of the two arms.

[0012] Further, the fourth-order forward kinematic model obtained in step S2 is:

[0013] Y (4) =Δ Y +JΘ (4)

[0014] In the formula, represents the end position vector of the two arms, is the joint angle vector of the m degrees of freedom of the two arms, J represents the Jacobian matrix of the two arms, and Δ Y represents the disturbance term.

[0015] Further, the virtual flexible joint dynamic constraints introduced in step S3 are:

[0016]

[0017] In the formula, M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, Λ(·) is the output torque of the damper, is the rotation angle of the active system, is the joint angle vector of the m degrees of freedom of the two arms, J represents the Jacobian matrix of the two arms, is the control input of the active system.

[0018] Further, the state space equation obtained for the inverse kinematics solution problem in step S4 is:

[0019]

[0020] In the formula, M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, J represents the Jacobian matrix of the two arms, and E Y is the tracking error of the end trajectory, is the control input of the active system, and Δ E is the combined disturbance term.

[0021] Further, the control law Q designed in step S4 is: In the formula, A = [a 1 , a 2 , a 3 , a 4 T is the state feedback coefficient, P = JM -1 KD -1 J T .

[0022] The beneficial effects of the present invention compared with the prior art are:​

[0023] The method of the present invention can solve the inverse kinematics problem of a humanoid robot, that is, when a given end trajectory is provided, the joint angle trajectory can be solved; it has the advantages of fast convergence speed, smooth angle and angular velocity curves obtained, and the ability to suppress noise.

[0024] Compared with the traditional pseudo-inverse method, the present invention has the functions of avoiding joint angle limits and joint angular velocity limits, and this method has a certain resistance to noise.

[0025] When the present invention is applied to the inverse kinematics calculation of a humanoid upper limb robot, the robot has more excellent kinematic performance and consumes less energy.

[0026] The following further describes the present invention in conjunction with the accompanying drawings and embodiments: Description of the Drawings

[0027] Figure 1 is the implementation flowchart of the method for solving the inverse kinematics of a humanoid upper limb robot based on virtual dynamic constraints provided by the present invention;

[0028] Figure 2 is the design diagram of the method for solving the inverse kinematics of a humanoid upper limb robot based on virtual dynamic constraints provided by the present invention;

[0029] Figure 3 is the joint angle curve of the first robotic arm obtained when using the method provided by the present invention to solve the inverse kinematics problem of the robot in the embodiment;

[0030] Figure 4 is the joint angle curve of the second robotic arm obtained when using the method provided by the present invention to solve the inverse kinematics problem of the robot in the embodiment;

[0031] Figure 5 is the graph of the end tracking position error of the robotic arm obtained when using the method provided by the present invention to solve the inverse kinematics problem of the robot in the embodiment;

[0032] Figure 6 is the trajectory graph of the humanoid upper limb robot obtained when using the method provided by the present invention to solve the inverse kinematics problem of the robot in the embodiment. Specific Embodiments

[0033] The following will describe in detail the embodiments of the technical solution of the present invention in conjunction with the accompanying drawings. Unless otherwise specified, the technical terms or scientific terms used in this application have the ordinary meanings understood by those skilled in the art.

[0034] Figure 1 Disclosed is a method for solving the inverse kinematics of a humanoid upper limb robot based on virtual dynamic constraints, which includes the following steps:

[0035] S1. Establish the link coordinate system of the humanoid upper limb robot, and obtain the forward kinematic equation through D-H parameter modeling;

[0036] S2. Take the fourth derivative of the forward kinematic equation to obtain the fourth-order forward kinematic model;

[0037] S3. Introduce the virtual flexible joint dynamic constraint; it not only has the ability to filter noise but also can avoid the joint angle limit and speed limit;

[0038] S4. Substitute the virtual flexible joint dynamic constraint in step S3 into the fourth-order forward kinematic model obtained in step S2 to obtain the state space equation for the inverse kinematics solution problem;

[0039] S5. Design the control law to make the state variables in the state space equation obtained in step S4 converge to zero;

[0040] S6. According to the designed control law, and then use the virtual flexible joint dynamic constraint in step S3 to solve for the values of the joint angle trajectories of the two arms.

[0041] Furthermore, the fourth-order forward kinematic model obtained in step S2 is:

[0042] Y (4) =Δ Y +JΘ (4)

[0043] In the formula, represents the end position vector of the two arms, is the joint angle vector of the m degrees of freedom of the two arms, J represents the Jacobian matrix of the two arms, and Δ Y represents the disturbance term.

[0044] The virtual flexible joint dynamic constraint introduced in step S3 is:

[0045] The flexible joint system consists of two subsystems, the active and passive ones. Under the control of the active system, the elastic element deforms. The resulting elastic torque forces the passive system to displace. When the displacement and speed approach their preset limits, the damper will inhibit the passive system from moving forward. From the perspective of signals and systems, the flexible joint is a non-linear low-pass filter with the ability to filter noise. Therefore, a virtual flexible joint dynamic constraint is introduced into the model. The virtual flexible joint dynamic constraint is a virtual dynamic constraint with the ability to filter noise, as shown below. The first row of the formula is the model of the passive system, and the second row is the model of the active system;

[0046]

[0047] where, M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, Λ(·) is the output torque of the damper, is the rotation angle of the active system, is the joint angle vector of the m degrees of freedom of the two arms, J represents the Jacobian matrix of the two arms, is the control input of the active system.

[0048] Substitute the virtual dynamic constraint in step S3 into the fourth-order forward kinematic model obtained in step S2, and then calculate the difference between it and the desired end trajectory to obtain the tracking error. The state space equation for the inverse kinematics solution problem obtained in step S4 is:

[0049]

[0050] where, M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, J represents the Jacobian matrix of the two arms, E Y is the tracking error of the end trajectory, is the control input of the active system, Δ E is the combined disturbance term.

[0051] Then, based on the estimated value of Δ E and the full state feedback control strategy, the control law Q designed in step S4 is: where, A = [a 1 , a 2 , a 3 , a 4 T is the state feedback coefficient, P = JM -1 KD -1 J T .

[0052] Make the tracking error E Y in the state space equation obtained in step four converge to 0, that is, the actual end position of the humanoid robot's two arms coincides with the desired end position trajectory.

[0053] According to the designed control law Q, and then using the virtual dynamic constraint in step S3, solve to obtain the joint angle trajectory of the two arms, that is, the value of Θ.

[0054] The following further illustrates the technical solution of the present application in conjunction with embodiments:

[0055] In this embodiment, a humanoid upper limb robot with two 8-degree-of-freedom robotic arms and a 3-degree-of-freedom waist is selected. First, establish a link coordinate system, and obtain the forward kinematic equation according to the D-H parameter modeling:

[0056] Y = F(θ, θ B ​)

[0057] In the formula, represents the end position vector of the two arms, is the two-arm joint angle vector with a dimension of 16 to be solved,

[0058] θ B represents the 3-degree-of-freedom joint vector of the waist. Since the kinematic formula of the position layer is highly nonlinear, the fourth-order derivative of both sides of the equation with respect to time is taken, and the fourth-order kinematic model can be obtained as:

[0059] Y (4) =Δ Y +JΘ (4)

[0060] In the formula, Δ Y is other complex terms after derivation, representing the disturbance term.

[0061] From the perspective of signals and systems, the flexible joint is a nonlinear low-pass filter and inherently has the ability to filter noise. In the joint space of the arm, the following virtual flexible joint dynamic constraints are introduced:

[0062]

[0063] In the formula, M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, Λ(·) is the output torque of the damper, is the rotation angle of the active system, is the control input of the active system.

[0064] Substitute the above virtual dynamic constraints into the fourth-order kinematic formula and subtract it from the desired end trajectory to obtain the following state-space equation:

[0065]

[0066] In the formula, E Y is the tracking error of the end trajectory, P = JM -1 KD -1 J T , P represents the variable substitution used to simplify the formula after combination; Δ E is the combined disturbance term.

[0067] Based on the estimated value of Δ E and the full-state feedback control strategy, design the following control law Q:

[0068]

[0069] In the formula, A = [a 1 , a 2 , a3 , a 4 T is the state feedback coefficient.

[0070] Applying the control law Q designed above to the virtual dynamic constraints with noise filtering ability, the joint angle trajectory and angular velocity trajectory can be solved.

[0071] In this embodiment, the trajectories of the two-arm ends are set as circular motions. The obtained joint trajectories, end tracking errors, and the action diagrams of the robot are as follows Figures 3 - 6 shown.

[0072] Figure 3 Among the eight diagrams of, they represent the eight joint angle trajectories of the left arm; θ 11 …θ 18 respectively represent the curves of the eight joint angle trajectories of the left arm of the humanoid upper limb robot changing with time. It can be seen that the 8 joint angle curves of the left arm obtained by using this invention have good smoothness. At the same time, the y-axis range of each curve diagram corresponds to the upper and lower limits of the joint angles. It can be seen from the diagram that the obtained joint angle curves all satisfy the constraints of the angle limits of each joint of the left arm.

[0073] Figure 4 Among the eight diagrams of, they represent the eight joint angle trajectories of the right arm: θ 21 …θ 28 respectively represent the curves of the eight joint angle trajectories of the right arm of the humanoid upper limb robot changing with time. It can be seen that the 8 joint angle curves of the left arm obtained by using this invention have good smoothness. At the same time, the y-axis range of each curve diagram corresponds to the upper and lower limits of the joint angles. It can be seen from the diagram that the obtained joint angle curves all satisfy the constraints of the angle limits of each joint of the left arm.

[0074] Figure 5 It represents the error curve diagram of the circular motion with a tracking period of 5 s for the two-arm ends. Among them, E1 x , E1 y and E1 z respectively represent the numerical values of the tracking error of the left arm in the x, y, and z components of the link coordinate system. Among them, E2 x , E2 y and E2 z respectively represent the numerical values of the tracking error of the right arm in the x, y, and z components of the link coordinate system. It can be seen from the diagram that during the process of tracking the dynamic trajectory, that is, the circular motion, the end tracking errors of the two arms tend to be stable, and the tracking accuracy reaches within 0.1 mm.

[0075] Figure 6 ​Shows a trajectory diagram of the ends of the two arms of a humanoid upper limb robot. The figure shows the trajectory diagram of the entire movement process of the ends of the two arms of the humanoid upper limb robot. Among them, the blue line is the desired circular motion trajectory, and the green line is the actual motion trajectory calculated according to the method proposed in the present invention. It can be seen that the two basically completely overlap, demonstrating excellent tracking performance.

[0076] The present invention has been disclosed above with preferred embodiments. However, it is not intended to limit the present invention. Any person skilled in the art, without departing from the scope of the technical solution of the present invention, can make some changes or modifications to equivalent embodiments of equivalent changes by using the above-disclosed structure and technical content, and all still fall within the scope of the technical solution of the present invention.

Claims

1. A method for solving inverse kinematics of a humanoid upper limb robot based on virtual dynamic constraints, characterized in that: The following steps are involved: S1. Establish the link coordinate system of the humanoid upper limb robot and obtain the positive kinematic equation through DH parameter modeling; S2, taking the fourth-order derivative of the forward kinematics equation to obtain a fourth-order forward kinematics model; S3, introduce virtual flexible joint dynamic constraints; The virtual flexible joint dynamics constraints are: Where M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, Λ(·) is the damper output torque, is the active system rotation angle, is the joint angle vector of the m degrees of freedom of the double arms, J represents the Jacobian matrix of the double arms, It is the control input of the active system; S4, substituting the virtual flexible joint dynamics constraint of step S3 into the fourth-order forward kinematics model obtained in step S2, and obtaining the state space equation for solving the inverse kinematics problem; The state space equation is: In the formula, E Y is the tracking error of the terminal trajectory, Δ E is the combined disturbance term; S5, designing a control law so that the state variables in the state space equation obtained in step S4 converge to zero; The control law is: Where A=[a1,a2,a3,a4] T is the state feedback coefficient, P = JM - 1 KD -1 J T ; S6. According to the designed control law, the virtual flexible joint dynamics constraints in step S3 are then used to solve the values ​​of the joint angle trajectories of the two arms.

2. The inverse kinematics solution method of the humanoid upper limb robot based on virtual dynamics constraints according to claim 1 is characterized in that: The fourth-order forward kinematics model obtained in step S2 is: Y (4) =D Y +JΘ (4) In the formula, represents the end position vector of the two arms, Δ Y represents the disturbance term.

3. The inverse kinematics solution method of the humanoid upper limb robot based on virtual dynamics constraints according to claim 1 is characterized in that: The forward kinematics equation Y=F(θ,θ B ), is the arm joint angle vector with dimension 16 to be solved, θ B Represents the 3-DOF joint vector of the waist.

4. The inverse kinematics solution method of a humanoid upper limb robot based on virtual dynamics constraints according to claim 1, characterized in that: The virtual flexible joint dynamics constraint in step S3 is a virtual dynamics constraint with noise filtering capability.

5. The inverse kinematics solution method of the humanoid upper limb robot based on virtual dynamics constraints according to claim 2 is characterized in that: Jacobian matrix in step S2

Images