Robotic arm motion planning method based on sigmoid penalty strategy

Abstract

The present invention provides a robot motion planning method based on a sigmoid penalty strategy, comprising the following steps: establishing an inverse kinematics model of the robot according to a Jacobian matrix and a preset target trajectory of the end of the robot; establishing a physical limit double-ended inequality constraint of the robot according to the actual joint physical limit constraint parameters of the robot; formulating the inverse kinematics model of the robot and the physical limit double-ended inequality constraint as a time-varying quadratic programming model; designing a variable parameter convergent differential neural network based on a sigmoid penalty strategy to solve the time-varying quadratic programming model; transmitting the various angle information of the robot obtained by the solution to the lower computer controller of the robot, and the lower computer controller drives the robot to move to complete the target trajectory tracking task. The present invention solves redundant mechanical repetitive motions with the advantages of high computational efficiency, good real-time performance, and strong robustness.

Description

Technical Field

[0001] The invention belongs to a redundant manipulator repetitive motion planning method, and in particular relates to a manipulator motion planning method based on a sigmoid-type penalty strategy. Background Art

[0002] The repetitive motion of the robot is a very common motion in industrial production. Repetitive motion requires that each joint of the robot can return to its initial state after completing a periodic motion with the end trajectory closed. In this case, the initial state of each periodic motion of the robot can be guaranteed to be consistent. If the joints of the robot do not all return to their initial state after completing a periodic motion, it is considered that there is a joint offset phenomenon. If the joint offset phenomenon occurs during the periodic motion, the accuracy of the robot motion control will decrease, or an additional reset adjustment process will be required, resulting in a serious decrease in production efficiency. Therefore, in the actual robot arm control, in order to achieve the periodic motion of the robot arm, it must be ensured that the robot arm does not have joint drift within a task cycle. At the same time, in the actual motion of the robot arm, it may be subject to various constraints to ensure the safety of the robot arm in actual applications, such as: joint physical limit constraints. There are joint physical limits for almost all robot arms. If the algorithm does not take into account the avoidance of joint physical limits, the control amount generated by the algorithm will easily exceed the actual joint limit of the robot arm. The idea of ​​the general joint physics avoidance algorithm is to rewrite the joint physical limits of the robot arm into inequality double-ended constraints, and then take it into account in the quadratic programming scheme.

[0003] The traditional kinematic solution method for the manipulator is based on the pseudo-inverse method. However, since the pseudo-inverse method requires the calculation of the inverse of the matrix, and the tasks faced by the manipulator in practical applications are becoming more and more diverse, it is difficult to handle more complex tasks in real time in practical applications. Compared with the pseudo-inverse method, the method of solving the motion planning of the manipulator based on the optimization problem is more widely used. The methods for solving the optimization problem include numerical method solvers and neural network solvers. Due to the characteristics of numerical iteration, the numerical method solver has the advantages of being convenient for control on a digital computer and easy to generate pulse signals for driving the motor. For example, the 94LVI numerical method is a method for solving a quadratic programming problem based on equality constraints and is applied to the motion planning of the manipulator, but the numerical method requires a lot of computer resources to perform iterative calculations, which increases the calculation time and is difficult to apply to the real-time motion planning of the manipulator. Compared with the numerical method solver, the recursive neural network solver based on neurodynamics has a parallel computing mechanism and is considered to be a more powerful solver for real-time optimization problems. This neural network solver is widely used in a variety of practical applications due to its high computational efficiency and the convenience of hardware implementation. Zhang et al. proposed a recursive neural network solver, called the zeroing neural network, which is used to solve time-varying quadratic programming problems (Zhang Y., Ge S..A General Recurrent Neural Network Model for Time-Varying Matrix Inversion [A]. In: 42nd IEEE International Conference on Decision and Control [C]. Maui, HI, USA: IEEE, 2003. 6: 6169-6174.), and to solve the redundant manipulator motion planning problem. Due to the use of the derivative information of the parameters, the zeroing neural network can successfully obtain the optimal solution of the time-varying quadratic programming problem. However, this traditional zeroing neural network can only approximate the theoretical optimal solution in infinite time. In order to overcome the physical constraint problem of the robot's joints, a primal-dual neural network based on linear variational inequalities is proposed to solve the corresponding quadratic programming problem, and is applied to a redundant robot terminal trajectory tracking task.However, LVI-PDNN cannot achieve an exponential convergence speed. In order to make full use of the time-varying system characteristics of the robot's motion planning problem, Zhang et al. designed a variable-parameter convergent differential neural network with super-exponential convergence effect (Z. Zhang, Y. Lu, L. Zheng, S. Li, Z. Yu, and Y. Li, "A new varying-parameter convergent-differential neural-network for solving time-varying convex QP problem constrained by linear-equality," IEEE Transactions on Automatic Control, vol. 63, no. 12, pp. 4110–4125, 2018.), and applied the network to the repeated motion planning scheme of redundant robots. However, the variable-parameter convergent differential neural network cannot effectively solve the inequality constraint problems in the robot arm motion planning scheme, such as: joint limit physical constraints.

[0004] In summary, the industry urgently needs to develop a neural network solver or solution method with an exponential convergence speed and that can effectively solve the robotic arm motion planning problem constrained by physical limits. Summary of the invention

[0005] The purpose of the present invention is to overcome the deficiencies of the above-mentioned prior art and to provide a robotic arm motion planning method with an exponential convergence speed and capable of effectively solving a variable parameter convergent differential neural network based on a sigmoid penalty strategy constrained by physical limits.

[0006] The present invention is achieved by at least one of the following technical solutions.

[0007] Compared with the existing redundant robot arm repetitive motion planning scheme, the present invention has the following advantages:

[0008] 1. Compared with the classic recursive neural network solver, the present invention adopts a new variable parameter convergent differential neural network with a sigmoid penalty function to solve the time-varying quadratic programming model, which has faster convergence speed and higher calculation accuracy, and can effectively solve the inequality constraints in the quadratic programming model.

[0009] 2. Compared with the traditional pseudo-inverse method, the present invention solves the motion planning model of the robot arm through a quadratic programming scheme, which has the characteristics of good real-time performance and can consider multiple constraints;

[0010] 3. Compared with the numerical method solver, the present invention belongs to a neural network solver with fast calculation speed and higher efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] The drawings in the specification, which constitute a part of this application, are used to provide a further understanding of the present invention. The illustrative embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0012] Figure 1 A schematic diagram of the structure flow of a robot arm motion planning method based on a sigmoid penalty strategy in an embodiment;

[0013] Figure 2 A connection diagram of a robot arm motion planning method based on a sigmoid-type penalty strategy according to an embodiment;

[0014] Figure 3a The figure is a simulated motion trajectory diagram of the six-link mechanical arm of the embodiment;

[0015] Figure 3b is a trajectory tracking error diagram of the robot arm of the embodiment;

[0016] Figure 4a The simulated motion trajectory of the linkage robot arm of Example 6;

[0017] Figure 4b A real-time state diagram of the embodiment joints subject to physical limit constraints;

[0018] Figure 4c is a trajectory tracking error diagram of the robot arm of the embodiment;

[0019] Figure 5 The present invention is a structural flow chart of the robot arm motion planning method based on the sigmoid penalty strategy. DETAILED DESCRIPTION

[0020] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0021] Example 1

[0022] like Figure 1 , Figure 5As shown, this embodiment is a robot arm motion planning method based on a sigmoid penalty strategy, which mainly consists of three parts: problem raising, problem transformation and problem solving. First, an inverse kinematics model on the velocity layer is established according to the preset robot arm end trajectory and Jacobian matrix, and the robot arm motion planning model is designed as a time-varying convex quadratic programming model, in which the robot arm repetitive motion is designed as an optimization index, the inverse kinematics model is designed as an equality constraint, and the physical limit constraint is designed as a double-end inequality constraint. The quadratic programming model is converted into a time-varying matrix model using the Lagrange equation, and finally the time-varying matrix model is solved by the designed variable parameter convergent differential neural network based on the sigmoid penalty strategy.

[0023] The specific steps include:

[0024] First, consider the inverse kinematics model of the robot arm:

[0025] f(θ)=r E (1)

[0026] where θ∈R n Represents the angular velocity of each joint of the robotic arm, r E ∈R m is the preset end trajectory of the robot, f(·) is a nonlinear mapping model. m represents the dimension of the robot's workspace. If it is a three-dimensional space, m = 3. n is the number of joints of the robot.

[0027] Equation (1) indicates that the angle information of each joint of the robot arm at the corresponding time is obtained from the known end position. Due to the nonlinearity of the f(·) function in equation (1), it is difficult to obtain an analytical solution to equation (1). Therefore, in order to linearize equation (1), the problem is considered at the velocity layer. The derivatives of both sides of equation (1) with respect to time are obtained:

[0028]

[0029] Among them J E ∈R m×n is the Jacobian matrix of the robot, and are the time derivatives of the robot joint angle and the end trajectory respectively.

[0030] Almost all manipulators have joint physical limits. If the algorithm does not take joint physical limit avoidance into account, the control amount generated by the algorithm will easily exceed the actual joint limit of the manipulator. The idea of ​​the general joint physical avoidance algorithm is to rewrite the joint physical limit of the manipulator into a double-ended inequality constraint:

[0031]

[0032] where θ∈R n Represents the angles of each joint of the robotic arm, θ - and θ + They represent the upper and lower physical limits of the joint angles of the robotic arm respectively. and They represent the upper and lower physical limits of the angular velocity of the robot joints. Considering that the inverse kinematics problem of the robot is solved at the velocity layer, the above formula is converted into a double-end constraint condition at the velocity layer:

[0033]

[0034] Among them, - , + Defined as:

[0035]

[0036]

[0037] in:

[0038]

[0039]

[0040]

[0041] β is a positive number in the interval [0, 1], ζ - ∈R n Indicates angular velocity The double-terminal limit, ζ + ∈R n Indicates angular velocity The double-ended upper limit of and Respectively represent θ - and θ + The first and second limiting coefficients of 1 and η 2 ζ - and + The first and second limit coefficients of . To simplify the operation, the two-sided inequality constraint Transformed into a one-sided inequality constraint, namely: Where K = [E, -E] T ∈R 2n×n , e = [ζ + ,-ζ - ] T ∈R 2n , the quadratic programming model of the robot arm constrained by physical limits is: Where b = ω(θ(t) - θ(0)), where the parameter ω is the response coefficient of the joint offset.

[0042] In order to solve the problems of equations (3) to (5), an exponential penalty model P(t) is proposed, which transforms the time-varying quadratic programming model containing inequality constraints and equality constraints into a time-varying quadratic programming model subject only to equality constraints, and the inequality constraints are equivalent to the penalty terms in the target optimization terms (performance indicators, optimization terms after min.). The structure of the exponential penalty model P(t) is:

[0043]

[0044] Where σ>0, p is a positive number close to 0, e i and K i are the vector e and the i-th row vector of the matrix K, respectively. θ represents the angles of each joint of the robot arm. n represents the constant related to the number of constraint terms in the expression. t represents the time variable of the time-varying system. After using the penalty model P(t), equations (3) to (5) can be transformed into:

[0045]

[0046] The above quadratic programming model can be transformed into a matrix equation model through the Lagrangian algorithm, and the Lagrangian model is constructed as follows:

[0047]

[0048] Where λ is the Lagrange multiplier. Using the Lagrange algorithm to solve equation (8) we get the following model:

[0049]

[0050] Formula (9) can be expressed as a time-varying matrix model:

[0051] B(t)y(t)=G(t) (10)

[0052] in

[0053]

[0054]

[0055] In order to solve the time-varying matrix model, the error function is defined as:

[0056] ε(t)=B(t)y(t)-G(t) (11)

[0057] By using the neurodynamic approach, the error converges to zero as follows:

[0058]

[0059] Where γexp(t) is the parameter for adjusting the convergence rate, and Φ(·) is the activation function structure. Combining (11) with (12), we can obtain a variable parameter convergent differential neural network solver based on the Sigmoid penalty strategy, namely:

[0060]

[0061] in

[0062]

[0063]

[0064]

[0065]

[0066] in represents the derivative of the Jacobian matrix, The derivative of the response representing the joint displacement, represents the acceleration of the joint end effector, The derivative of the transpose of the first eigenterm representing the one-sided inequality constraint, The derivative of the second characteristic term representing the one-sided inequality constraint, the circuit connection diagram of the variable parameter convergent differential neural network based on the Sigmoid penalty strategy is as follows Figure 2 shown. Figure 2 The y in 1 ,y 2 …y n+k Represents the output of the circuit diagram, b 1,1 , b 1,2 …b n+k,n+k The sub-term of the first characteristic term of the time-varying matrix of the time-varying quadratic programming model of the manipulator, g 1 , g 2 …g n+k The sub-term of the second characteristic term of the time-varying matrix of the time-varying quadratic programming model of the manipulator, r 1,1 、r 1,2 …r n+k,n+k Represents the sub-term of the third characteristic term of the time-varying matrix of the time-varying quadratic programming model of the manipulator. 1,1 ,t 1,2 …t n+k,n+k The fourth characteristic term of the time-varying matrix of the time-varying quadratic programming model of the manipulator, v 1,1 、v 1,2 …v n+k,n+k Represents the sub-term of the fifth eigenterm of the time-varying matrix of the time-varying quadratic model of the robot arm.

[0067] The optimal solution y of the time-varying matrix model can be obtained from formula (13): * , the first n items are the optimal solutions of the quadratic programming model (3)-(5) right The optimal solution of the redundant manipulator joint angle θ can be obtained by integration * The overall flow chart of the method for solving the redundant manipulator repetitive motion planning using a variable parameter convergent differential neural network based on a sigmoid penalty strategy is as follows: Figure 1 shown.

[0068] Embodiment 2

[0069] In the implementation of the scheme of the six-link robot arm tracking the trajectory of the five-pointed star, the initial position joint angle of the robot arm is set to [1.685; 2.873; -3.266; 4.107; -1.51; -2.35] radians, and the upper limit of the angle is set to θ + = [5.1; 5.1; 5.1; 5.1: 5.1; 5.1] radians, the lower limit of the angle is set to θ - =[-5.1;-5.1;-5.1;-5.1;-5.1;-5.1] radians, the upper speed limit is set to rad / sec, the lower speed limit is set to Radians / second. The variable parameter differential neural network solver based on the sigmoid penalty strategy used is also:

[0070]

[0071] The calculated joint velocity is integrated to obtain the optimal solution of the joint angle, which is then transmitted to the robot controller to control the movement of the robot arm and thus track the trajectory of the four-leaf clover.

[0072] Figure 3a is the simulated motion trajectory of the six-link robot, the black curve is the robot motion trajectory, and the black thick curve is the expected five-pointed star trajectory. It can be found that by using a variable parameter differential neural network solver method based on a sigmoid penalty strategy, the six-link robot can track the expected five-pointed star trajectory well. Figure 3b This is a trajectory tracking error diagram of the robot arm, and its error accuracy reaches 10 to the -5th power, indicating that the tracking accuracy of the present invention is relatively high.

[0073] Embodiment 3

[0074] Considering the physical double-end constraints and joint limit constraints, the six-link robot tracks the butterfly trajectory. First, the joint angles of the robot at the initial position are set to [1.685; 2.873; -3.266; 4.107; -1.51; -2.35] radians, and the upper limit of the angle is set to θ + = [5.1; 5.1; 5.1; 5.1: 5.1; 5.1] radians, the lower limit of the angle is set to θ - =[-5.1;-5.1;-5.1;-5.1;-5.1;-5.1] radians, the upper speed limit is set to rad / sec, the lower speed limit is set to Radians / second. The variable parameter differential neural network solver based on the sigmoid penalty strategy used is also:

[0075]

[0076] At the same time, considering the physical double-end constraints and joint limit constraints, the calculated joint velocities are integrated to obtain the optimal solution of the joint angles, which are then transmitted to the robot controller to control the movement of the robot arm and thus track the butterfly trajectory.

[0077] Figure 4a is the simulated motion trajectory of the six-link robot, the black curve is the robot motion trajectory, and the thick black curve is the expected butterfly trajectory. It can be found that by using a variable parameter differential neural network solver method based on a sigmoid penalty strategy, the six-link robot can still track the expected butterfly trajectory well even under environmental constraints. Figure 4b This is a real-time state diagram of the joints subject to physical limit constraints, and the joint angles are effectively restricted to the feasible region. Figure 4c This is the trajectory tracking error diagram of the robotic arm. The error accuracy reaches 10 to the -5th power, which means that under the constraints of the double-ended inequality, the control accuracy of the robotic arm is still very high.

[0078] The above specific implementation modes are preferred embodiments of the present invention and cannot be used to limit the present invention. Any other changes or other equivalent replacement methods that do not deviate from the technical solution of the present invention are included in the protection scope of the present invention.

Claims

1. A robot arm motion planning method based on a sigmoid penalty strategy, characterized in that: The steps include: S1. According to the structural parameters of the robot arm, a model of the robot arm is established, the Jacobian matrix of the end effector is obtained, and the inverse kinematics model of the robot arm is established according to the Jacobian matrix and the preset target trajectory of the end of the robot arm; S2. According to the actual joint physical limit constraint parameters of the robot arm, a physical limit double-ended inequality constraint of the robot arm is established on the velocity layer; S3, the inverse kinematics model of the manipulator and the physical limit double-end inequality constraint are formulated as a time-varying quadratic programming model, and the motion planning model of the manipulator constrained by the physical limit is solved using a quadratic programming scheme; wherein the repeated motion index is used as a performance index of the time-varying quadratic programming model; S4. Establish a variable parameter convergent differential neural network based on the sigmoid penalty strategy model to solve the time-varying quadratic programming model; then the first n elements of the solution of the variable parameter convergent differential neural network based on the sigmoid penalty strategy are the optimal solution Integrate it to get θ * ,θ * That is the optimal joint angle of the robot arm; S5, transmitting the solved angle information of the manipulator to the lower computer controller of the manipulator, and the lower computer controller drives the manipulator to move and complete the target trajectory tracking task; The sigmoid penalty strategy model is the sigmoid penalty model P(t): Where σ>0, p represents the adjustment factor of the penalty function, which is a positive number close to 0, i represents the i-th constraint, and e i (t) and K i (t) are the characteristic term e of the inequality constraint and the i-th row vector of the matrix K, θ represents the angle of each joint of the robot, and n represents the constant related to the number of constraint terms in the expression; Indicates the angular velocity of each joint of the robotic arm; represents the first adjustment parameter of the curve characteristic of the sigmoid penalty model P(t); σ represents the second adjustment parameter of the curve characteristic of the sigmoid penalty model P(t); t represents the time variable of the time-varying system; The sigmoid penalty model P(t) is used to transform the inequality constraints in the time-varying quadratic programming model in the robotic arm system into penalty terms in the performance index: Where b represents the angle increment of each joint of the robot arm, J E represents the Jacobian matrix of the robot, represents the velocity vector of the end effector of the robot arm; The physical limit double-ended inequality constraints include: The physical limit double-ended inequality constraint Transformed into physical limit one-sided inequality constraint; The inverse kinematics model of the robot and the physical limit one-sided inequality constraints are formulated as a time-varying quadratic programming model; The physical limit one-sided inequality constraint is: Where K = [E, -E] T ∈R 2n×n , e = [ζ + ,-ζ - ] T ∈R 2n , where E represents an n×n identity matrix; ζ - ∈R n Indicates angular velocity The double-terminal limit, ζ + ∈R n Indicates angular velocity The double-ended upper limit of the constraint; K represents the first characteristic matrix of the one-sided inequality constraint, R 2n The number of rows representing K is 2n.

2. The robot arm motion planning method based on the sigmoid penalty strategy according to claim 1, characterized in that: The time-varying quadratic programming model with penalty terms is transformed into a matrix equation model using the Lagrange multiplier method, and the following time-varying equation model is obtained: B(t)y(t)=G(t), Where λ is the Lagrangian operator, B(t) represents the first eigenvalue of the time-varying matrix of the time-varying quadratic programming model of the manipulator, E represents the unit matrix, m and n represent the number of rows and columns of the matrix respectively, and y(t) represents the joint angular velocity and the Lagrangian operator λ, G(t) represents the second eigenvalue of the time-varying matrix of the time-varying quadratic programming model of the manipulator, J E represents the Jacobian matrix of the robot arm, b represents the angle increment of each joint of the robot arm, p represents the adjustment factor of the penalty function, e i and K i are the inequality constraint characteristic term e and the i-th dimension row vector of matrix K, respectively. represents the velocity vector of the end effector of the robot arm, represents the first adjustment parameter of the curve characteristic of the sigmoid penalty model P(t); σ represents the second adjustment parameter of the curve characteristic of the sigmoid penalty model P(t); t represents the time variable of the time-varying system.

3. The robot arm motion planning method based on sigmoid penalty strategy according to claim 1, characterized in that: The time-varying equation model is solved using a variable parameter recurrent neural network: The error function is defined as: ε(t)=B(t)y(t)-G(t)∈R n+m In order to make the error converge to zero, the following neural dynamics criterion is adopted: dε(t) / dt=-γexp(t)ψ(ε(t)) Where ψ(`)∶P n+m →R n+m is the activation function, ψ(·) is a monotonically increasing odd function, γexp(t) represents the adjustment parameter of the convergence speed of the neural network. According to the neural dynamics criterion and the same type of merging, a variable parameter convergent differential neural network based on the sigmoid penalty strategy is obtained: in, Among them, R(t) represents the third characteristic term of the time-varying matrix of the time-varying quadratic programming model of the manipulator, T(t) represents the fourth characteristic term of the time-varying matrix of the time-varying quadratic programming model of the manipulator, and V(t) represents the fifth characteristic term of the time-varying matrix of the time-varying quadratic model of the manipulator. The sixth eigenvalue of the time-varying matrix of the time-varying quadratic programming model of the manipulator; represents the derivative of the Jacobian matrix of the robot, Represents the derivative of the angle increment of each joint of the robot arm, represents the acceleration vector of the end effector of the robot arm, represents the derivative of the transpose of the i-th dimension row vector of matrix K, represents the derivative of the o-th row vector of the inequality constraint feature term e; y(t) represents the angular velocity of the joint and the Lagrangian operator λ; t represents the time variable of the time-varying system; Represents the derivative vector of the vector y(t) with respect to the time variable t of the time-varying system; Then the first n elements of the solution of the variable parameter convergent differential neural network based on the sigmoid penalty strategy are the optimal solution Integrate it to get θ * ,θ * This is the optimal joint angle of the robotic arm.

4. The robot arm motion planning method based on sigmoid penalty strategy according to claim 1, characterized in that: The joint physical limit constraint parameters in step S2 include: joint angle constraint and angular velocity constraint: The physical limit double-ended inequality constraint of the manipulator established on the velocity layer is: Among them, - ∈R n Indicates angular velocity The double-terminal limit, ζ + ∈R n Indicates angular velocity The double-ended upper limit of is defined as: where θ∈R n ,θ - and θ + Respectively represent the upper and lower physical limits of the joint angles of the robot arm; Represents the angular velocity of each joint of the robotic arm, and They represent the upper and lower physical limits of the angular velocity of the robot joints respectively; and Respectively represent θ - and θ + The first and second limit coefficients of ; η1 and η2 represent ζ - and + The first and second boundary coefficients of are: Where β is a positive number in the interval [0,1], α1 and α2 are constant coefficients, which are used to represent With θ - , With θ + multiple relationship.

5. The robot arm motion planning method based on sigmoid penalty strategy according to claim 1, characterized in that: Structural parameters include connecting rod length and number of degrees of freedom.

6. The robot arm motion planning method based on sigmoid penalty strategy according to claim 1, characterized in that: The inverse kinematics model of the robotic arm is: f(θ)=r E where θ∈R n Represents the angular velocity of each joint of the robotic arm, r E ∈R m is the preset end trajectory of the robot, f(·) is the nonlinear mapping model, m represents the dimension of the robot's workspace, and n is the number of joints of the robot.

Images