Inverse kinematics solving method of humanoid upper limb robot based on virtual dynamics constraint
Through the method based on virtual dynamic constraints, the problem of noise sensitivity and slow convergence speed in the inverse kinematics solution of human-like upper limb robots is solved, and fast and smooth joint angle trajectory solution and noise suppression are achieved, which improves the robot's kinematics performance and Energy efficiency.
Patent Information
- Application Number
- CN202411021424.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-29
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-07-29
AI Technical Summary
The existing inverse kinematics solution method for human-imitating upper limb robots has problems such as noise sensitivity, slow convergence speed, and unsmooth speed curve. Especially in visual servo, additional Kalman filtering is required for noise reduction processing.
Using a method based on virtual dynamics constraints, by establishing the link coordinate system of human-like upper limb robots, finding the fourth-order positive kinematic model, introducing virtual flexible joint dynamics constraints, designing control laws, and making the state variables in the state space equation go to Zero convergence, thus solving the joint angle trajectory.
The rapid solution of inverse kinematic problems is achieved, the obtained angle and angular velocity curves are smooth, and the noise suppression ability is avoided, the limits of joint angle and angular velocity are improved, and the kinematic performance is reduced and energy consumption is reduced.
Smart Images

Figure CN118963122A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of humanoid upper limb robots, and in particular to an inverse kinematics solution method for a humanoid upper limb robot based on virtual dynamics constraints. Background Art
[0002] In the process of robot or robotic arm control, for a serially articulated robot / robotic arm, if the angles of each joint are known to calculate the position and posture of the end of the robot / robotic arm, this process is called forward kinematics solution; conversely, if the position and posture 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 posture of the end of the robot / robotic arm to the angles of each joint of the robot / robotic arm.
[0003] Early methods generally used pseudo-inverse methods for calculation, but they had disadvantages such as being unable to avoid joint limits and being greatly affected by visual noise. In recent years, scholars have proposed methods such as gradient descent and recursive neural networks for solving the inverse kinematics of constrained manipulators, but the gradient descent method is sensitive to noise and cannot solve singular configurations, and the recursive neural network method has problems such as slow convergence and uneven velocity curves. In addition, these methods generally require additional Kalman filtering for noise reduction when applied to visual servoing. Summary of the invention
[0004] In order to solve the problems of noise sensitivity, slow convergence and uneven speed in existing methods, the present invention proposes an inverse kinematics solution method for a humanoid upper limb robot based on virtual dynamic constraints.
[0005] The inverse kinematics solution method of the humanoid upper limb robot based on virtual dynamic constraints includes the following steps:
[0006] S1. Establish the link coordinate system of the humanoid upper limb robot and obtain the positive kinematic equation through DH parameter modeling;
[0007] S2, taking the fourth-order derivative of the forward kinematics equation to obtain a fourth-order forward kinematics model;
[0008] S3, introduce virtual flexible joint dynamic constraints;
[0009] 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;
[0010] S5, designing a control law so that the state variables in the state space equation obtained in step S4 converge to zero;
[0011] 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.
[0012] Furthermore, the fourth-order forward kinematics 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 double arms, J represents the Jacobian matrix of the double arms, Δ Y represents the disturbance term.
[0015] Furthermore, the virtual flexible joint dynamics constraint introduced in step S3 is:
[0016]
[0017] 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, is the control input for the active system.
[0018] Furthermore, the state space equation for solving the inverse kinematics problem obtained in step S4 is:
[0019]
[0020] Where M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, J is the Jacobian matrix of the two arms, and E is Y is the tracking error of the terminal trajectory, is the control input of the active system, Δ E is the combined disturbance term.
[0021] Furthermore, the control law Q designed in step S4 is: Where A=[a1,a2,a3,a4] T is the state feedback coefficient, P = JM -1 KD -1 J T .
[0022] Compared with the prior art, the present invention has the following beneficial effects:
[0023] The method of the invention can solve the inverse kinematics problem of a humanoid robot, that is, when the terminal trajectory is given, the joint angle trajectory can be solved; it has the advantages of fast convergence speed, smooth angle and angular velocity curves, and noise suppression capability.
[0024] Compared with the traditional pseudo-inverse method, the present invention has the function of avoiding joint angle limits and joint angular velocity limits, and the method has a certain ability to resist noise.
[0025] When the present invention is applied to the inverse kinematics solution of a humanoid limb robot, the robot has more superior kinematic performance and consumes less energy.
[0026] The present invention will be further described below in conjunction with the accompanying drawings and embodiments: BRIEF DESCRIPTION OF THE DRAWINGS
[0027] Figure 1 A flowchart of the implementation of the inverse kinematics solution method of the humanoid upper limb robot based on virtual dynamics constraints provided by the present invention;
[0028] Figure 2 A design diagram of the inverse kinematics solution method for a humanoid upper limb robot based on virtual dynamics constraints provided by the present invention;
[0029] Figure 3 is a joint angle curve of the first robot arm obtained when solving the inverse kinematics problem of the robot using the method proposed in the present invention in the embodiment;
[0030] Figure 4 is a joint angle curve of the second robot arm obtained when solving the inverse kinematics problem of the robot using the method proposed in the present invention in the embodiment;
[0031] Figure 5 The figure is a graph of the tracking position error of the end of the manipulator obtained when the inverse kinematics problem of the robot is solved by the method proposed in the present invention in the embodiment;
[0032] Figure 6 It is a trajectory diagram of the humanoid upper limb robot obtained when the inverse kinematics problem of the robot is solved by the method proposed in the present invention in the embodiment. DETAILED DESCRIPTION
[0033] The embodiments of the technical solution of the present invention will be described in detail below in conjunction with the accompanying drawings. Unless otherwise specified, the technical terms or scientific terms used in this application are generally understood by those skilled in the art.
[0034] Figure 1 A method for solving inverse kinematics of a humanoid upper limb robot based on virtual dynamic constraints is presented, which includes the following steps:
[0035] S1. Establish the link coordinate system of the humanoid upper limb robot and obtain the positive kinematic equation through DH parameter modeling;
[0036] S2, taking the fourth-order derivative of the forward kinematics equation to obtain a fourth-order forward kinematics model;
[0037] S3, introduce virtual flexible joint dynamic constraints; it has the ability to filter noise and avoid joint angle limits and speed limits;
[0038] 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;
[0039] S5, designing a control law so that the state variables in the state space equation obtained in step S4 converge to zero;
[0040] 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.
[0041] Furthermore, the fourth-order forward kinematics 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 double arms, J represents the Jacobian matrix of the double arms, Δ Y represents the disturbance term.
[0044] The virtual flexible joint dynamics constraints introduced in step S3 are:
[0045] The flexible joint system consists of two subsystems, active and passive. Under the control of the active system, the elastic element deforms. The elastic torque induced by this forces the passive system to displace. When the displacement and speed are close to their preset limits, the damper will inhibit the passive system from continuing to move forward. From the perspective of signals and systems, the flexible joint is a nonlinear 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 noise filtering capability, as shown below. The first row of formulas 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 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, is the control input for the active system.
[0048] Substitute the virtual dynamic constraints of step S3 into the fourth-order forward kinematics 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 is the Jacobian matrix of the two arms, and E is Y is the tracking error of the terminal trajectory, is the control input of the active system, Δ E is the combined disturbance term.
[0051] Then, according to Δ E The estimated value of and the control strategy of full state feedback, the control law Q designed in step S4 is: Where A=[a1,a2,a3,a4] T is the state feedback coefficient, P = JM -1 KD -1 J T .
[0052] Let the tracking error E in the state space equation obtained in step 4 be Y Converges to 0, that is, the actual end position of the humanoid robot's arms coincides with the expected end position trajectory.
[0053] According to the designed control law Q, the virtual dynamic constraints in step S3 are then used to solve the joint angle trajectory of the two arms, that is, the value of Θ.
[0054] The technical solution of the present application is further described below in conjunction with embodiments:
[0055] In this embodiment, a humanoid upper limb robot with two 8-DOF mechanical arms and a 3-DOF waist is selected. First, a connecting rod coordinate system is established, and the positive kinematic equation is obtained according to the DH parameter modeling:
[0056] Y=F(θ,θ B )
[0057] In the formula, represents the end position vector of the two arms, is the double-arm joint angle vector with a dimension of 16 to be solved,
[0058] θ B Represents the 3-DOF 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 calculated, and the fourth-order kinematic model can be obtained as follows:
[0059] Y (4) =Δ Y +JΘ (4)
[0060] In the formula, Δ Y are other complex terms after differentiation, representing disturbance terms.
[0061] From the perspective of signals and systems, the flexible joint is a nonlinear low-pass filter that inherently has the ability to filter out noise. In the joint space of the arm, the following virtual flexible joint dynamics constraints are introduced:
[0062]
[0063] 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, Input for active system control.
[0064] Substituting the above virtual dynamic constraints into the fourth-order kinematics formula and subtracting it from the desired terminal trajectory, we get the following state space equation:
[0065]
[0066] In the formula, E Y is the tracking error of the terminal trajectory, P = JM -1 KD -1 J T , P represents the variable replacement used to simplify the formula after merging; Δ E is the combined disturbance term.
[0067] According to Δ E The estimated value of and the control strategy of full state feedback are used to design the following control law Q:
[0068]
[0069] Where A=[a1,a2,a3,a4] T is the state feedback coefficient.
[0070] By applying the control law Q designed above to the virtual dynamics constraint with noise filtering capability, the joint angle trajectory and angular velocity trajectory can be solved.
[0071] In this embodiment, the end trajectory of the double arms is set to circular motion. The joint trajectory, end tracking error and robot motion diagram obtained are as follows: Figure 3-Figure 6 shown.
[0072] Figure 3 The eight figures show the eight joint angle trajectories of the left arm; θ 11 …θ 18 The graphs respectively represent the eight joint angle trajectories of the left arm of the humanoid upper limb robot changing with time. It can be seen that the eight joint angle curves of the left arm solved by the invention have good smoothness. At the same time, the y-axis range of each graph corresponds to the upper and lower limits of the joint angle. It can be seen from the figure that the solved joint angle curves all meet the constraints of the angle limits of each joint of the left arm.
[0073] Figure 4 The eight figures show the eight joint angle trajectories of the right arm: θ 21 …θ 28 The graphs respectively represent the eight joint angle trajectories of the right arm of the humanoid upper limb robot changing with time. It can be seen that the eight joint angle curves of the left arm solved by the invention have good smoothness. At the same time, the y-axis range of each curve corresponds to the upper and lower limits of the joint angle. It can be seen from the figure that the solved joint angle curves all meet the constraints of the angle limits of each joint of the left arm.
[0074] Figure 5 The error curve of the double-arm end tracking circular motion with a period of 5s, where E1 x 、E1 y and E1 z They represent the values of the tracking error of the left arm in the x, y, and z components of the link coordinate system. x 、E2 y and E2 z They represent the values of the tracking error of the right arm in the x, y, and z components of the connecting rod coordinate system. It can be seen from the figure that in the process of tracking the dynamic trajectory, that is, circular motion, the end tracking error of the double arms tends to be stable, and the tracking accuracy reaches within 0.1mm.
[0075] Figure 6 A trajectory diagram of the end of the two arms of the humanoid upper limb robot is shown. The figure shows the trajectory diagram of the entire motion process of the end of the two arms of the humanoid upper limb robot, where the blue line is the expected circular motion trajectory, and the green line is the actual motion trajectory calculated by the method proposed in the present invention. It can be seen that the two are basically completely overlapped, showing excellent tracking performance.
[0076] The present invention has been disclosed as above with preferred implementation cases, but it is not used to limit the present invention. Any technician familiar with the profession can make slight changes or modifications to equivalent implementation cases with equivalent changes by using the above-disclosed structures and technical contents without departing from the scope of the technical solution of the present invention, which 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; 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; S5, designing a control law so that the state variables in the state space equation obtained in step S4 converge to zero; 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, is the joint angle vector of the m degrees of freedom of the double arms, J represents the Jacobian matrix of the double 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 virtual flexible joint dynamics constraints introduced in step S3 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, is the control input for the active system.
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 state space equation for solving the inverse kinematics problem obtained in step S4 is: Where M is the inertia of the passive system, D is the inertia of the active system, K is the system stiffness, J is the Jacobian matrix of the two arms, and E is Y is the tracking error of the terminal trajectory, is the control input of the active system, Δ E is the combined disturbance term.
5. The inverse kinematics solution method for a humanoid upper limb robot based on virtual dynamics constraints according to claim 4 is characterized in that: The control law Q designed in step S4 is: Where A = [a1, a2, a3, a4], T is the state feedback coefficient, P = JM -1 KD -1 J T .
6. The inverse kinematics solution method of a humanoid upper limb robot based on virtual dynamics constraints according to claim 1, 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.
7. 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.
8. The inverse kinematics solution method of a humanoid upper limb robot based on virtual dynamics constraints according to claim 2, characterized in that: Jacobian matrix in step S2
Citation Information
Patent Citations
Collaborative path planning method for kinematic redundant two-arm space robot
CN110104216A
Inverse kinematics solving method and system for four-degree-of-freedom series robot
CN113510690A
Redundant mechanical arm humanoid inverse kinematics solution method based on human body muscle-bone model
CN114329986A
Inverse kinematics solving method for humanoid upper limb robot based on high-order differentiator
CN116383574A
Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same
US20220009096A1
Cited By
Inverse kinematics solving method for three-arm series space robot integrating trunk height constraint
CN120307289A
Obstacle-avoiding and joint-avoiding extreme motion planning method for humanoid upper limb robot
CN120347732A
