Obstacle-avoiding and joint-avoiding extreme motion planning method for humanoid upper limb robot

Abstract

An obstacle avoidance and joint avoidance extreme motion planning method for a humanoid upper limb robot belongs to the technical field of robots, and comprises the following steps: establishing a mapping relation between the tail end position posture of the humanoid upper limb robot and the angle of each joint to obtain a speed layer forward kinematics model; introducing an estimation error of a pseudo-inverse matrix corresponding to the Jacobian matrix and an estimation error of a null space basis matrix; establishing a hybrid error model based on the estimation error; constructing a recurrent neural network based on the hybrid error model; and according to a matrix value obtained by the recurrent neural network, a joint angle track of the humanoid upper limb robot is calculated through a control law. The joint limit and the obstacle constraint are considered at the same time, and the tracking precision and the convergence speed of the robot can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of humanoid robots, and particularly to a method for obstacle avoidance and joint limit constraint motion planning of a humanoid upper limb robot based on a hybrid error type recurrent neural network. Background Art

[0002] In the field of robot or manipulator control, for a serial joint type robot / manipulator, the process of obtaining the end position and posture from the joint angles is called forward kinematics solution; conversely, the process of obtaining the joint angles from the known end position and posture is called inverse kinematics solution, that is, inverse kinematics describes the mapping relationship from the end position and posture of the robot / manipulator to the joint angles.

[0003] In the early stage, the pseudo-inverse method was commonly used for inverse kinematics solution, but this method has deficiencies such as being unable to effectively avoid joint limits, not considering the position of obstacles, and complex calculation of the pseudo-inverse of the Jacobian matrix. In recent years, some scholars have proposed methods such as the gradient descent method and recurrent neural networks to solve the inverse kinematics problem of manipulators with constraints. However, the gradient descent method has defects such as being sensitive to noise and unable to solve singular configurations. Although the recurrent neural network method reduces the time complexity of the matrix inversion process, has a faster convergence speed and stronger robustness, existing methods mostly focus on the innovation of activation functions, do not optimize the error function itself, and there are few studies considering both joint limits and obstacle constraints at the same time, while these two constraints are necessary conditions for the safe movement of robots. Summary of the Invention

[0004] In order to overcome the deficiencies of the prior art, the present invention proposes a method for obstacle avoidance and joint limit avoidance motion planning of a humanoid upper limb robot, which can improve the tracking accuracy and convergence speed of the robot.

[0005] A method for obstacle avoidance and joint limit avoidance motion planning of a humanoid upper limb robot includes the following steps:

[0006] S1. Establish a mapping relationship between the end position and posture of the humanoid upper limb robot and the angles of each joint to obtain a velocity layer forward kinematics model;

[0007] S2. Introduce the estimation error of the pseudo-inverse matrix corresponding to the Jacobian matrix and the estimation error of the null space basis matrix;

[0008] S3. Based on the estimation error in step S2, establish a hybrid error model;

[0009] S4. Construct a recurrent neural network based on the hybrid error model;

[0010] S5. According to the matrix values obtained by the recurrent neural network, calculate the joint angle trajectory of the humanoid upper limb robot through a control law.

[0011] Furthermore, in step S1, a mapping relationship is established between the end - position and orientation of the humanoid upper - limb robot and the angles of each joint, and the following forward kinematic model of the velocity layer is obtained:

[0012]

[0013] In the formula, represents the end - position and orientation vector of the two arms of the humanoid upper - limb robot, is the joint - angle vector of the m degrees of freedom of the humanoid upper - limb robot, represents the Jacobian matrix of the humanoid upper - limb robot.

[0014] Furthermore, the estimation errors of the pseudo - inverse matrix and the null - space basis matrix corresponding to the Jacobian matrix introduced in step S2 are respectively:

[0015] E X = JX - I m×m

[0016] E Y = JY - O m×n

[0017] In the formula, is the estimated value of the pseudo - inverse matrix of the Jacobian matrix, is the estimated value of the null - space basis matrix of the Jacobian matrix, I m×m is the identity matrix, O m×n is the zero matrix.

[0018] Furthermore, based on the said estimation error, the following hybrid - error model is established in step 3:

[0019]

[0020] In the formula, is an empirical parameter.

[0021] Furthermore, in step S4, a recursive neural network is constructed based on the said hybrid - error model, and the values of matrices X and Y are obtained:

[0022]

[0023] In the formula, Φ(·) represents the activation function of the neural network, which is a monotonically increasing odd function passing through the zero point.

[0024] Furthermore, in step S5, according to the values of matrices X and Y obtained from the recursive neural network, the joint - angle trajectory of the humanoid upper - limb robot is calculated through the following control law:

[0025]

[0026] In the formula, rd represents the desired end-effector trajectory of the robotic arms, where \(e = r\). d \(-r\) represents the end-effector tracking error of the robotic arms, \(G(\cdot)\) and \(H(\cdot)\) represent the cost functions with respect to joint limits and obstacles respectively, and \(\rho\), \(\alpha\), and \(\beta\) are gain parameters.

[0027] The beneficial effects of the present invention compared with the prior art are as follows:

[0028] When the method of the present invention is applied to the inverse kinematics solution of a humanoid upper limb robot, it does not need to rely on the traditional numerical inverse method of the Jacobian matrix. Instead, it uses a recurrent neural network to reduce the time complexity of the matrix inversion process, making the solution process have a faster convergence speed and stronger robustness.

[0029] The method of the present invention can solve the inverse kinematics problem of a humanoid robot, that is, when a given end-effector trajectory is provided, the method of the present invention can solve for the joint angle trajectory.

[0030] Compared with the traditional pseudo-inverse method, the method of the present invention has the functions of avoiding joint angle limits and avoiding obstacles, and has a certain anti-noise ability, ensuring the safety and stability of the robot operation.

[0031] The following further illustrates the solution of the application with reference to the drawings and embodiments: Description of the Drawings

[0032] Figure 1 is the implementation flowchart of the method for obstacle avoidance and joint limit avoidance motion planning of a humanoid upper limb robot based on a hybrid error type recurrent neural network according to the present invention;

[0033] Figure 2 is the logic control block diagram of the method for obstacle avoidance and joint limit avoidance motion planning of a humanoid upper limb robot based on a hybrid error type recurrent neural network according to the present invention;

[0034] Figure 3 are the angle curves of three joints of the torso part obtained when solving the inverse kinematics problem of a humanoid upper limb robot with obstacle and joint limit constraints by using the method proposed in this application in the embodiment;

[0035] Figure 4 are the angle curves of eight joints of the left arm part obtained when solving the inverse kinematics problem of a humanoid upper limb robot with obstacle and joint limit constraints by using the method proposed in this application in the embodiment;

[0036] Figure 5 are the angle curves of eight joints of the right arm part obtained when solving the inverse kinematics problem of a humanoid upper limb robot with obstacle and joint limit constraints by using the method proposed in this application in the embodiment;

[0037] Figure 6The graph of the tracking position error of the left arm end obtained when solving the inverse kinematics problem of the humanoid upper limb robot with obstacle and joint limit constraints by using the method proposed in this application in the embodiment; in the graph, the vertical coordinate "tackingerror" represents the tracking error, and the horizontal coordinate "time" represents time; the first six symbols in the upper right of the graph represent the magnitudes of the six directions of the error vector, and the last symbol represents the error magnitude.

[0038] Figure 7 The graph of the tracking position error of the right arm end obtained when solving the inverse kinematics problem of the humanoid upper limb robot with obstacle and joint limit constraints by using the method proposed in this application in the embodiment; in the graph, the vertical coordinate "tackingerror" represents the tracking error, and the horizontal coordinate "time" represents time; the first six symbols in the upper right of the graph represent the magnitudes of the six directions of the error vector, and the last symbol represents the error magnitude.

[0039] Figure 8 The graph of the distance between two obstacles and the robot body obtained when solving the inverse kinematics problem of the humanoid upper limb robot with obstacle and joint limit constraints by using the method proposed in this application in the embodiment; in the graph, "obstacle" represents the obstacle, the vertical coordinate "distance" represents the distance, and the horizontal coordinate "time" represents time.

[0040] Figure 9 The trajectory graph of the humanoid upper limb robot obtained when solving the inverse kinematics problem of the robot by using the method proposed in this application in the embodiment. Detailed implementation manners

[0041] Hereinafter, embodiments of the technical solution of the present invention will be described in detail with reference to 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.

[0042] The purpose of this embodiment is to solve the problems of the complexity of the Jacobian matrix pseudo-inverse calculation, sensitivity to noise, slow convergence speed, and failure to fully consider joint limit constraints and obstacle collision problems in the existing methods, and propose a method for obstacle avoidance and joint limit avoidance motion planning of a humanoid upper limb robot based on a hybrid error type recurrent neural network.

[0043] The following details this embodiment. The method for obstacle avoidance and joint limit avoidance motion planning of a humanoid upper limb robot based on a hybrid error type recurrent neural network in this embodiment simultaneously considers joint limits and obstacle constraints, and calculates the joint angle values of the humanoid upper limb robot. In this embodiment, a humanoid upper limb robot with two 8-degree-of-freedom manipulators and a 3-degree-of-freedom torso is selected; first, a link coordinate system is established, and according to the D-H parameter modeling, the kinematic formula of the velocity layer can be obtained as:

[0044]

[0045] In the formula, represents the end position and attitude vector of the two arms, is the two-arm and torso joint angle vector with 19 dimensions to be solved, represents the Jacobian matrix of the robot;

[0046] Then, the estimated values and estimation errors of the pseudo-inverse matrix and the null space basis matrix corresponding to the Jacobian matrix are introduced;

[0047] E X = JX - I 12×12

[0048] E Y = JY - O 12×19

[0049] In the formula, is the estimated value of the pseudo-inverse matrix of the Jacobian matrix, is the estimated value of the null space basis matrix of the Jacobian matrix, I 12×12 is the identity matrix, O 12×19 is the zero matrix;

[0050] Based on the above estimation errors, the integral term of the estimation errors is fused to establish the following hybrid error model;

[0051]

[0052] In the formula, is an empirical parameter, which is taken as 5 in this embodiment.

[0053] Based on the above hybrid error model, the values of matrices X and Y are calculated by constructing the following recurrent neural network;

[0054]

[0055] In the formula, Φ(·) represents the activation function of the neural network, which is a monotonically increasing odd function passing through the zero point.

[0056] Finally, the joint angle trajectory of the humanoid upper limb robot is calculated through the following control law;

[0057]

[0058] In the formula, r d represents the desired end trajectory of the robot's two arms, e = r d - r represents the end tracking error of the robot's two arms, G(·) and H(·) respectively represent the cost functions regarding joint limits and obstacles, and ρ, α, and β are empirical gain parameters.

[0059] In this embodiment, the trajectories of the two arm ends are set as fixed points [-0.6; 0.2; 0.45] and [-0.6; 0.2; -0.5], and the positions of the two obstacles are set as [-0.2; 0.35; 0.38] and [-0.3; 0.35; -0.4] respectively. The obtained joint trajectories, end-effector pose tracking errors, minimum distances from the obstacles to the robot, and the action diagram of the robot are as Figures 3 - 9 shown. It can be seen that the method proposed in this application can improve the tracking accuracy and convergence speed of the robot.

[0060] 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 can make some changes or modifications within the scope of the technical solution of the present invention to obtain equivalent embodiments with equivalent changes, which still fall within the scope of the technical solution of the present invention.

Claims

1. A method for obstacle avoidance and joint limit avoidance motion planning of a humanoid upper limb robot, characterized in that: It includes the following steps: S1. Establish a mapping relationship between the end position and posture of the humanoid upper limb robot and the angles of each joint to obtain the forward kinematics model of the velocity layer; S2. Introduce the estimation errors of the pseudo-inverse matrix corresponding to the Jacobian matrix and the null space basis matrix; S3. Based on the estimation errors in step S2, establish a hybrid error model; S4. Construct a recurrent neural network based on the hybrid error model; S5. According to the matrix values obtained from the recurrent neural network, calculate the joint angle trajectory of the humanoid upper limb robot through the control law.

2. The method for obstacle avoidance and avoidance of joint limit motion planning of a humanoid upper limb robot according to claim 1, characterized in that: In step S1, a mapping relationship is established between the end position and posture of the humanoid upper limb robot and the angles of each joint to obtain the following forward kinematics model of the velocity layer: wherein, represents the end position and attitude vector of the two arms of the humanoid upper limb robot, is the joint angle vector of the m degrees of freedom of the humanoid upper limb robot, represents the Jacobian matrix of the humanoid upper limb robot.

3. The method for obstacle avoidance and joint limit avoidance motion planning of the humanoid upper limb robot according to claim 1, wherein: In step S2, the estimation errors of the pseudo-inverse matrix corresponding to the Jacobian matrix and the null space basis matrix are respectively: E X = JX - I m×m E Y = JY - O m×n In the formula, is the estimated value of the pseudo-inverse matrix of the Jacobian matrix, is the estimated value of the null space basis matrix of the Jacobian matrix, and I m×m is the identity square matrix, and O m×n is the zero matrix.

4. The method for obstacle avoidance and joint limit avoidance motion planning of the humanoid upper limb robot according to claim 1 or 3, characterized in that: In step 3, based on the said estimation errors, the following hybrid error model is established: In the formula, is an empirical parameter.

5. The obstacle avoidance and joint limit avoidance motion planning method for the humanoid upper limb robot according to claim 4, characterized in that: In step S4, the following recurrent neural network is constructed based on the hybrid error model and the values of matrices X and Y are obtained: In the formula, Φ(·) represents the activation function of the neural network, which is a monotonically increasing odd function passing through the zero point.

6. The method for obstacle avoidance and avoidance of joint limit motion planning of the humanoid upper limb robot according to claim 5, wherein: In step S5, according to the values of matrices X and Y obtained from the recurrent neural network, the joint angle trajectory of the humanoid upper limb robot is calculated through the following control law: where r d represents the desired end - effector trajectory of the robotic dual - arms, e = r d - r represents the end - effector tracking error of the robotic dual - arms, G(·) and H(·) represent the cost functions with respect to joint limits and obstacles respectively, and ρ, α, and β are gain parameters.

Images