CN116383574B - Humanoid upper limb robot inverse kinematics solving method based on high-order differentiator - Google Patents

Abstract

A method for solving inverse kinematics of a humanoid upper limb robot based on a higher-order differentiator, the method comprising: establishing a mathematical model in the form of a time-varying linear equation set; obtaining an error signal and an integral signal of the tail end position of the mechanical arm according to the mathematical model; solving a higher derivative of the error signal according to a higher differentiator, and establishing a dynamic error model; designing a time-varying linear equation set solver based on dynamic errors, and outputting the optimal action of the mechanical arm aiming at the equation set; and establishing a state space model according to the optimal action to obtain an optimal solution of the joint variable. The invention has the advantages of faster convergence speed, higher solving precision, low energy consumption and higher operability when being applied to the inverse kinematics calculation of the mechanical arm.

Description

A method for solving inverse kinematics of humanoid upper limb robot based on high-order differentiator

Technical Field

The invention belongs to the field of computer technology and relates to an inverse kinematics solution method for a humanoid upper limb robot based on a high-order differentiator.

Background technique

Linear equations are the most common type of problem in fields such as control theory and engineering. How to solve this type of problem is the key technology to solve practical application-related problems such as robot inverse kinematics, image reconstruction, and signal processing.

Generally, there are two methods to solve this problem: direct method and iterative method. The direct method mainly includes Gaussian elimination method. Although the direct method has an absolutely accurate solution in theory, once the number of variables increases, the cost of solving this method will be very high; the iterative method is more effective for systems with more variables, and the commonly used methods are Newton iteration method and gradient neural network. These methods have better results when solving static constant problems, and can ensure that the tracking error is as small as possible and the operation speed is fast.

However, actual application systems are essentially time-varying systems. The above methods often cannot guarantee the decrease of the error function due to the lag error of the time-varying parameters. The traditional methods are only applicable to the solution of linear steady-state systems. When applied to the solution of time-varying linear equations, the lag error of the time-varying parameters will lead to large errors. Therefore, the above methods are not suitable for solving time-varying linear equations. In recent years, scholars have proposed a zeroing neural network (ZNN) to solve time-varying problems, but this type of model is still in the deep exploration stage and still has problems such as slow convergence speed and insufficient accuracy.

Summary of the invention

In order to overcome the existing technology, the present invention provides a method for solving the inverse kinematics of a humanoid upper limb robot based on a high-order differentiator.

A method for solving inverse kinematics of a humanoid upper limb robot based on a high-order differentiator includes:

S1. Establish a mathematical model in the form of a system of time-varying linear equations;

S2, obtaining the error signal and integral signal of the end position of the robot arm according to the mathematical model;

S3, solving the high-order derivative of the error signal according to the high-order differentiator, and establishing a dynamic error model;

S4. Design a solver for the time-varying linear equations based on dynamic errors and output the optimal action of the robot arm for the equations;

S5. Establish a state space model based on the optimal action to obtain the optimal solution for the joint variables.

Compared with the prior art, the present invention has the following beneficial effects:

The present invention can realize the solution of time-varying linear equations, and is not like the traditional method which can only be used for the solution of linear steady-state systems.

Compared with the emerging zeroing neural network method, the present invention has the advantages of faster convergence speed and higher solution accuracy.

When the present invention is applied to the inverse kinematics solution of a humanoid upper limb robot, it consumes lower energy, has higher operability, and the obtained solution has the characteristic of global optimization.

The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments:

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a method for solving a time-varying linear equation system based on a high-order differentiator;

FIG2 is a design diagram for solving a time-varying linear equation system based on a high-order differentiator established by the method of the present invention;

FIG3 is a graph showing a tracking position error curve of a robot end obtained when the method of the present invention is used to solve the inverse kinematics problem of a robot in an embodiment;

FIG4 is a graph showing the joint angles of the first robot arm obtained when the inverse kinematics problem of the robot is solved by the method of the present invention in an embodiment;

FIG. 5 is a second robot arm joint angle curve diagram obtained when the inverse kinematics problem of the robot is solved using the method of the present invention in an embodiment.

Detailed ways

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 should have the common meanings understood by those skilled in the art to which the present invention belongs.

1-2, a method for solving inverse kinematics of a humanoid upper limb robot based on a high-order differentiator in this embodiment includes:

S1. Establish a mathematical model in the form of a system of time-varying linear equations;

S2, obtaining the error signal and integral signal of the end position of the robot arm according to the mathematical model;

S3, solving the high-order derivative of the error signal according to the high-order differentiator, and establishing a dynamic error model;

S4. Design a solver for time-varying linear equations based on dynamic errors and output the optimal action of the robot arm for the equations;

S5. Establish a state space model based on the optimal action to obtain the optimal solution for the joint variables.

This implementation method overcomes the problem that the traditional method is only applicable to solving linear steady-state systems. When solving time-varying linear equations, the lag error of time-varying parameters will lead to large errors. This implementation method overcomes the current problems of slow convergence speed and low accuracy of the ZNN method.

Furthermore,

In step S1, a mathematical model of an actual physical system or a numerical solution system in the form of a time-varying linear equation system is established, for example: P(t)Q(t)=U(t), where: and/> is a known time-varying parameter based on a mathematical model,/> is the time-varying vector to be solved, and the time-varying parameter matrix P(t) and vector U(t) in the mathematical model are obtained through the system's own properties and system sensors.

In step S2, the error function equation E(t) = P(t)Q(t)-U(t) is designed, and the integral signal of the error is obtained.

In step S3, the time high-order derivative of the residual error E(t) is solved according to the high-order differentiator, where E(t)=P(t)Q(t)-U(t), and a new multi-objective optimization model-dynamic error is established according to the error, the integral signal of the error and the high-order derivative information of the error;

Where c ₀ , c ₁ , … , c _L are the parameters to be designed.

In this step, the multi-objective optimization model takes into account both the integral information of the error, which is beneficial to improving the accuracy of solving time-varying linear equations; it also takes into account the differential information of each order of the error, which helps to improve the stability of the solution model and the global optimal search capability. At present, the establishment of the multi-objective optimization model implemented in this method is not available in other methods.

Step S4 is to design a time-varying linear equation solver based on a high-order differentiator based on the dynamic error value obtained by the differentiator, and the optimal action output is:

Where α and β are the solver parameters to be designed, β∈[Δ _max ,∞), α∈(β,∞), σ(·) is a class that satisfies/> The sigmoid function of the property, /> is a linear differential equation, and ε(t) is a decaying function with 0 as its asymptote.

Step S5 is to design the optimal action Q _IN , and then use the state space model defined as follows;

Obtain the optimal solution of joint variables with fast convergence speed and high accuracy.

Based on the above inventive concept, the present invention is further described in the form of embodiments:

Example

The method for solving time-varying linear equations based on high-order differentiators is applied to the inverse kinematics solution of the humanoid upper limb robot to obtain the joint variables of the arms.

In this embodiment, a humanoid upper limb robot with two 8-DOF mechanical arms and a 3-DOF waist is selected, and a time-varying linear equations solver based on a high-order differentiator is applied to the inverse kinematics solution of the humanoid upper limb robot;

First, obtain the positive kinematics equation of the above humanoid upper limb robot:

Y _k =F _k ( Θ _k , Θ _B )

In the formula, is the joint angle vector of dimension 3 at the waist,/> is the joint angle vector of the arms with dimension 8. The subscript k is used to distinguish the left arm from the right arm. /> is the end position vector of the two arms with a dimension of 3. Because the kinematic formula of the position layer is highly nonlinear, the kinematic formula of the velocity layer can be obtained by taking the derivative of both sides of the equation with respect to time:

In the formula, and/> The Jacobian matrix of the humanoid upper limb robot is a time-varying variable that changes with the robot's motion state. are the joint angular velocity vectors of the two 8-DOF manipulators, /> is a 3-DOF waist joint angular velocity vector.

Solving the inverse kinematics of a robot is actually to give the desired trajectory of the end of the robotic arm, and then calculate the corresponding joint angle and angular velocity values based on the kinematic mapping relationship.

Defining symbols is the desired end position trajectory, it and θ _B are both smooth known time-varying functions, then the inverse kinematics solution of the humanoid upper limb robot is equivalent to solving the following equation, and making the following equation converge to 0;

Let P _k = J _k , It can be seen that the velocity layer inverse kinematics solution formula is consistent with the error function equation obtained in step S2 of the above implementation. And on this basis, the integral signal of the error can be calculated, that is, the position tracking error of the end of the dual manipulator, as shown in Figure 3:

Because the forward kinematics model of the humanoid robot's upper limbs and the expected end position trajectory are known, the end position tracking error of the manipulator can be obtained. Then, based on the high-order differentiator, the high-order differential information such as the velocity tracking error, acceleration tracking error, and jerk tracking error of the end of the manipulator can be obtained. In order to improve the tracking accuracy of the manipulator, improve the stability of the inverse kinematics solution model, and reduce the energy consumption of the manipulator, a new multi-objective optimization model (dynamic error) is established to constrain the position error of the end of the manipulator and the differential information of each order, and it is called the dynamic error. The expression is:

In the formula, c ₀ , c ₁ , … , c _L are the gain coefficients of the dual-arm position tracking error and each order differential information. In order to ensure the tracking accuracy, c ₀ , c ₁ are set to larger parameter values. At the same time, in order to ensure the stability of the model and reduce the energy consumed by the robot arm, that is, to constrain the acceleration tracking error and the jerk error, c ₂ , … , c _L can be set to smaller parameter values.

According to the dynamic error value, the optimal motion of the designed robot arm is:

Where α and β are the solver parameters to be designed, β∈[Δ _max ,∞), α∈(β,∞), σ(·) is a class that satisfies/> The sigmoid function of the property, ε(t) is a decay function with 0 as the asymptote,/> It is a linear combination of the joint angle, angular velocity and acceleration of the robot arm, and is solved by the solution method of the linear differential equation system, that is, the state space equation defined as follows is used to obtain the solution of the time-varying linear equation system with fast convergence speed and high accuracy:

Wherein, _X1 = Q ⁽⁰⁾ , _X2 = Q ⁽¹⁾ , ..., _XL = Q ^(L-1) respectively represent the obtained joint angle vector, joint velocity vector, joint acceleration vector and other information. By constraining acceleration, jerk, etc., the energy consumed by the robot arm is reduced.

It can be seen that the method of solving the time-varying linear equations based on the high-order differentiator is practical and feasible to solve the inverse kinematics problem of the humanoid upper limb robot. The simulation results are shown in Figures 3 and 4.

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 (7)

1. The inverse kinematics solving method of the humanoid upper limb robot based on the high-order differentiator is characterized by comprising the following steps of: comprising:

s1, establishing a mathematical model in the form of a time-varying linear equation set;

S2, obtaining an error signal and an integral signal of the tail end position of the mechanical arm according to a mathematical model;

s3, solving a higher derivative of the error signal according to the higher differentiator, and establishing a dynamic error model;

In step S3, the time higher derivative of the residual error E (t) is solved according to the higher-order differentiator, where E (t) =p (t) Q (t) -U (t), and a new dynamic error model is built according to the error, the integral signal of the error, and the higher derivative information of the error;

Wherein c ₀,c₁,…,c_L is a parameter to be designed;

s4, designing a time-varying linear equation set solver based on dynamic errors, and outputting the optimal action of the mechanical arm aiming at the equation set;

step S4, designing a time-varying linear equation set solver based on a higher-order differentiator according to the dynamic error value obtained based on the differentiator in step S3, and outputting the optimal motion of the mechanical arm as:

Wherein, alpha and beta are to be designed into solver parameters, beta epsilon [ delta _max, ], alpha epsilon (beta, +#), Sigma (·) is a class of satisfaction/>Sigmoid type function of property,/>Is a linear differential equation, ε (t) is an attenuation function with 0 as the asymptote;

S5, a state space model is established according to the optimal action, and an optimal solution of the joint variable is obtained;

step S5, according to the optimal action Q _IN, utilizing a state space model defined as follows;

And obtaining the robot joint variable.

2. The method for solving the inverse kinematics of the humanoid upper limb robot based on the higher-order differentiator according to claim 1, which is characterized by comprising the following steps of: in step S1 a mathematical model of the actual physical system in the form of a time-varying system of linear equations is built.

3. The method for solving the inverse kinematics of the humanoid upper limb robot based on the higher-order differentiator according to claim 1, which is characterized by comprising the following steps of: in step S1, a mathematical model in the form of a time-varying linear equation set is established, and a time-varying parameter matrix P (t) and a vector U (t) in the mathematical model are obtained as follows:

P(t)(Q(t)＝U(t)

In the method, in the process of the invention, And/>Is a known time-varying parameter based on a mathematical model,/>Is the time-varying vector to be solved.

4. The method for solving the inverse kinematics of the humanoid upper limb robot based on the higher-order differentiator according to claim 1, which is characterized by comprising the following steps of: the error function equation designed in the step S2 is E (t) =P (t) Q (t) -U (t), and an integral signal of the error is obtained

5. The method for solving the inverse kinematics of the humanoid upper limb robot based on the higher-order differentiator according to claim 1, which is characterized by comprising the following steps of: the humanoid upper limb robot is provided with two 8-degree-of-freedom mechanical arms and a 3-degree-of-freedom waist.

6. The method for solving the inverse kinematics of the humanoid upper limb robot based on the higher-order differentiator according to claim 5, which is characterized by comprising the following steps of: step S1, obtaining a kinematic formula of a speed layer as followsIn the/>And/>Jacobian matrix representing humanoid upper limb robot,/>Is the joint angular velocity vector of the 8-degree-of-freedom mechanical arm,/>The joint angular velocity vector of the waist with 3 degrees of freedom is used for distinguishing the left arm from the right arm by the subscript k, and the values are k=1 and k=2 respectively.

7. The method for solving the inverse kinematics of the humanoid upper limb robot based on the higher-order differentiator according to claim 1, which is characterized by comprising the following steps of: a mathematical model in the form of a time-varying system of linear equations is built based on the desired target state input by the external environment and the external sensor data.