A redundant dual-manipulator mutual obstacle avoidance method, device and storage medium

Abstract

The present invention discloses a redundant dual-manipulator mutual obstacle avoidance method, device and storage medium, the method includes establishing the inverse kinematics equation of the manipulator on the velocity layer through the respective end trajectories of the redundant dual-manipulator and adding the minimum energy, joint limit and mutual obstacle avoidance index; converting the inverse kinematics equation and various indexes into a time-varying quadratic convex optimization problem constrained by equations and inequalities; converting the above problem into a time-varying quadratic convex optimization problem constrained only by equations by defining a penalty function; then converting it into a time-varying matrix equation through the Lagrange equation; solving the matrix equation through a variable parameter convergent differential neural network; finally integrating the optimal solution obtained on the velocity layer to obtain the optimal solution of the joint angle. The present invention enables the dual-manipulator to successfully complete the planned motion tasks, effectively solves the problem of mutual obstacle avoidance of the redundant dual-manipulator, and has the advantages of high computational efficiency, strong real-time performance, good robustness and high precision.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a redundant dual-manipulator mutual obstacle avoidance method, device and storage medium. Background Art

[0002] Each of the redundant dual manipulators has more degrees of freedom than the number of degrees of freedom required to complete the task. Due to the more degrees of freedom, the redundant manipulator can complete additional tasks such as avoiding obstacles and extreme joint positions on top of completing the main tasks of the end effector. In the mutual obstacle avoidance of the dual manipulators, each manipulator avoids the other manipulator as an obstacle, so that the dual manipulators do not collide at any position during the entire cycle of completing the collaborative task. If the path planning of the dual manipulators does not consider the problem of mutual obstacle avoidance, it is very likely to cause a collision between the manipulators during the movement, which will lead to the sudden termination of the task and even damage to the manipulators. Therefore, the research on mutual obstacle avoidance of dual manipulators is very meaningful.

[0003] The traditional method for solving the inverse kinematics problem of redundant dual manipulators is based on the pseudo-inverse method. This method has a long calculation time, poor real-time performance, and a single problem constraint, which is greatly restricted in the application of actual manipulator motion planning. Summary of the invention

[0004] The purpose of the present invention is to overcome the deficiencies of the above-mentioned prior art and to provide a redundant dual robotic arm mutual obstacle avoidance method, device and storage medium to effectively solve the problem of redundant dual robotic arm mutual obstacle avoidance.

[0005] To achieve the above object, the technical solution of the present invention is:

[0006] In a first aspect, the present invention provides a redundant dual-manipulator mutual obstacle avoidance method comprising:

[0007] According to the desired end trajectory of the dual manipulators and the Jacobian matrix of the dual manipulators, the inverse kinematics equation of the manipulators is established on the velocity layer;

[0008] Adding constraint condition indicators based on the established inverse kinematics equation of the manipulator, wherein the constraint condition indicators include a minimum energy indicator, a joint limit indicator, and a mutual obstacle avoidance indicator;

[0009] The inverse kinematics equations and constraint indicators of the manipulator are transformed into a time-varying quadratic convex optimization problem subject to equality and inequality constraints;

[0010] The time-varying quadratic convex optimization problem is transformed into a time-varying quadratic convex optimization problem subject only to equality constraints by defining a penalty function;

[0011] The time-varying quadratic convex optimization problem subject only to equality constraints is transformed into a time-varying matrix equation through the Lagrange equation;

[0012] The time-varying matrix equation is solved by a variable parameter convergent differential neural network based on penalty function, and the optimal solution of redundant dual manipulators in the velocity layer is obtained.

[0013] The optimal solution of the redundant dual manipulator at the velocity layer is integrated to obtain the optimal solution of the joint angle.

[0014] Furthermore, the inverse kinematics equation of the robotic arm is:

[0015] f(θ)=r;

[0016] Where r is the desired end trajectory of the dual manipulators, and f(·) is the nonlinear equation about the joint angle of the manipulators. The inverse kinematics equation on the velocity layer is obtained by differentiating both sides of the equation:

[0017] Where J(θ) is the Jacobian matrix of the robot arm, and are the time derivatives of the robot joint angle and the end trajectory respectively.

[0018] Furthermore, the minimum energy index is The joint limit index is

[0019] The mutual obstacle avoidance index is Among them J N The vector representing the minimum distance between each branch of the dual manipulator in the mutual obstacle avoidance index, and β represents the reverse velocity vector.

[0020] Furthermore, the inverse kinematics equations and constraint indicators of the manipulator are transformed into a time-varying quadratic convex optimization problem constrained by equality and inequality: minimizing Subject to

[0021] Furthermore, the step of converting the time-varying quadratic convex optimization problem into a time-varying quadratic convex optimization problem subject only to equality constraints by defining a penalty function comprises:

[0022] Define a penalty function Where p represents a non-negative penalty factor, h represents the number of inequality constraint equations, σ is a penalty function used to determine The parameters of consistency, The quadratic convex optimization problem subject to equality and inequality constraints is transformed into a quadratic convex optimization problem subject only to equality constraints, specifically: minimize Subject to

[0023] Furthermore, the method of converting the time-varying quadratic convex optimization problem subject only to equality constraints into a time-varying matrix equation through the Lagrange equation includes:

[0024] Define the Lagrangian function: Where λ is the Lagrange multiplier;

[0025] Through two partial differential equations, namely and Construct the following time-varying matrix equation: Ey = e, where

[0026]

[0027] Furthermore, the time-varying matrix equation is solved by a variable parameter convergent differential neural network based on a penalty function as follows: Where γ is a constant representing the convergence rate of the neural network, and Ψ(·) represents the activation function of the neural network. From this, the optimal solution y of the time-varying matrix equation Ey=e can be obtained. * .

[0028] Furthermore, the optimal solution of the redundant dual manipulator at the speed layer is integrated to obtain the optimal solution of the joint angle: * The first n terms of the quadratic programming problem are obtained by * , for x * The optimal solution of the joint angle of the redundant dual manipulator is obtained by integration θ * .

[0029] In a second aspect, the present invention provides a redundant dual-manipulator mutual obstacle avoidance device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any of the above methods when executing the computer program.

[0030] In a third aspect, the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of any of the above methods are implemented.

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

[0032] The present invention enables the dual robotic arms to successfully complete the planned motion tasks by adding constraint indicators based on the inverse kinematics equations of the robotic arms, effectively solving the problem of redundant dual robotic arms avoiding obstacles and avoiding damage to the robotic arms. The present invention uses a neural network solver to solve the problem, which has fast calculation speed and higher efficiency; compared with the original primitive dual neural network solver based on linear variational inequalities, the present invention uses a new variable parameter convergent differential neural network solver based on penalty functions, which has faster convergence speed and better robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 A flowchart of a redundant dual-manipulator mutual obstacle avoidance method provided in Example 1 of the present invention;

[0034] Figure 2 A schematic diagram showing a comparison between redundant dual robotic arms avoiding obstacles with and without avoiding obstacles to achieve the present invention;

[0035] Figure 3 A schematic diagram of the composition of the redundant dual-manipulator mutual obstacle avoidance device provided in Example 2 of the present invention. DETAILED DESCRIPTION

[0036] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0037] Embodiment 1:

[0038] See also Figure 1 As shown, the redundant dual-manipulator mutual obstacle avoidance method provided in this embodiment mainly includes two parts: problem proposal and indicator definition, and problem transformation and solution.

[0039] The problem setting and indicator definition part mainly includes the following steps:

[0040] According to the desired end trajectory of the dual manipulators and the Jacobian matrix of the dual manipulators, the inverse kinematics equation of the manipulators is established on the velocity layer;

[0041] Constraint condition indicators are added based on the established inverse kinematics equation of the manipulator, and the constraint condition indicators include a minimum energy indicator, a joint limit indicator and a mutual obstacle avoidance indicator.

[0042] In this way, by adding constraint indicators based on the inverse kinematics equation of the robot arm, the dual robot arms can successfully complete the planned motion tasks, effectively solve the problem of redundant dual robot arms avoiding obstacles, and avoid damage to the robot arms. The specific effects are as follows: Figure 2 shown.

[0043] The problem transformation and solution part includes the following steps:

[0044] The inverse kinematics equations and constraint indicators of the manipulator are transformed into a time-varying quadratic convex optimization problem subject to equality and inequality constraints;

[0045] The time-varying quadratic convex optimization problem is transformed into a time-varying quadratic convex optimization problem subject only to equality constraints by defining a penalty function;

[0046] The time-varying quadratic convex optimization problem subject only to equality constraints is transformed into a time-varying matrix equation through the Lagrange equation;

[0047] The time-varying matrix equation is solved by a variable parameter convergent differential neural network based on penalty function, and the optimal solution of redundant dual manipulators in the velocity layer is obtained.

[0048] The optimal solution of the redundant dual manipulator at the velocity layer is integrated to obtain the optimal solution of the joint angle.

[0049] It can be seen that in the problem transformation and solution part, the present invention adopts a neural network solver to solve the problem, which has fast calculation speed and higher efficiency; compared with the original primitive dual neural network solver based on linear variational inequalities, the present invention adopts a new variable parameter convergent differential neural network solver based on penalty function, which has faster convergence speed and better robustness.

[0050] In a specific embodiment, the inverse kinematics equation of the above-mentioned robot arm is:

[0051] f(θ)=r;

[0052] Among them, r is the expected end trajectory of the dual robot, f(·) is a nonlinear equation about the robot joint angle, which means that the position of each joint of the robot at the corresponding time is calculated from the known end position. Due to the nonlinearity of the equation, the equation has multiple solutions, so it is necessary to consider the problem at the velocity layer. The derivatives of both sides of the equation with respect to time are obtained:

[0053] Where J(θ) is the Jacobian matrix of the robot arm, and are the time derivatives of the robot joint angle and the end trajectory respectively.

[0054] In order to remove the redundant motion of the redundant dual manipulator and minimize the energy loss during the motion, the minimum energy index needs to be considered. The energy consumption of the manipulator in motion is directly related to its average joint speed. Therefore, the minimum energy index of the redundant dual manipulator is to minimize its average joint speed. Written as a minimum constraint:

[0055]

[0056] In practical applications, due to the limitations of the motor itself, the joint angle and joint speed of each joint of the redundant dual robot arm have their limit values. The robot arm joint speed limit is expressed as an inequality constraint as follows:

[0057]

[0058] During the collaborative movement of redundant dual robotic arms, the problem of mutual obstacle avoidance must be considered. Otherwise, collisions between the robotic arms during movement will cause the task to terminate suddenly or even damage the robotic arms. The general principle of mutual obstacle avoidance of redundant dual robotic arms is as follows: 1) Using a method based on three-dimensional space line segment distance measurement, use sensors to measure the minimum distance between the two robotic arms; 2) Artificially set the external safety range d1 and the internal safety range d2, and adjust the corresponding joint speeds according to the relationship between the minimum distance between the robotic arms and the two safety ranges to achieve the purpose of mutual obstacle avoidance. The inequality constraints designed according to this principle are as follows

[0059]

[0060] in, The vector representing the minimum distance between each branch in the dual robot, represents the Jacobian matrix of the minimum distance point on the robot, and β represents the reverse velocity vector.

[0061] According to the above indicators, the mutual obstacle avoidance problem of redundant dual manipulators is solved by quadratic programming, and the following unified quadratic programming problem is designed:

[0062]

[0063] In order to use the variable parameter recurrent neural network to solve the above quadratic programming problem, a penalty function is defined here:

[0064]

[0065] Where p represents a non-negative penalty factor, h represents the number of inequality constraint equations, and σ is a term used to determine the penalty function. The parameters of consistency,

[0066]

[0067] The above quadratic programming problem with equality and inequality constraints is transformed into the following quadratic programming problem with only equality constraints:

[0068]

[0069] Based on the above quadratic programming problem, the Lagrangian function is constructed as follows:

[0070]

[0071] Among them, λ is the Lagrange multiplier, and the partial derivative of the equation is:

[0072]

[0073] The system of equations can be expressed as the following matrix equation

[0074] Ey=e

[0075] To solve the matrix equation, define the error function

[0076] ξ(t)=Ey-e

[0077] in

[0078]

[0079] Through the neurodynamics method, the design error converges to zero in the following way

[0080]

[0081] Among them, the parameter γe t It is a real-time variable parameter and converges faster; Ψ(·) is the activation function. Substituting the formula into the formula, we get the variable parameter recursive neural network solver based on penalty function, namely:

[0082]

[0083] The optimal solution y of the matrix equation can be obtained by the formula * , the first n items are the optimal solution φ in the quadratic programming problem * , integrate it to get the real-time optimal solution θ of the redundant dual manipulator joint angle * .

[0084] In summary, compared with the prior art, the present invention has the following advantages:

[0085] 1. Compared with the traditional pseudo-inverse method, the present invention solves the problem of mutual obstacle avoidance of redundant dual manipulators through a quadratic programming scheme, which has strong real-time performance and can consider multiple constraints;

[0086] 2. Compared with the numerical method solver, the present invention belongs to a neural network solver, which has fast calculation speed and higher efficiency;

[0087] 3. Compared with the original primitive dual neural network solver based on linear variational inequalities, the present invention adopts a new variable parameter convergent differential neural network solver based on penalty function, which has faster convergence speed and better robustness.

[0088] Embodiment 2:

[0089] See also Figure 3 As shown, the redundant dual-manipulator mutual obstacle avoidance device provided in this embodiment includes a processor 31, a memory 32, and a computer program 33 stored in the memory 32 and executable on the processor 31, such as a redundant dual-manipulator mutual obstacle avoidance program. When the processor 31 executes the computer program 33, the steps of the above-mentioned embodiment 1 are implemented, such as Figure 1 Steps shown.

[0090] Exemplarily, the computer program 33 may be divided into one or more modules / units, which are stored in the memory 32 and executed by the processor 31 to complete the present invention. The one or more modules / units may be a series of computer program instruction segments capable of completing specific functions, which are used to describe the execution process of the computer program 33 in the redundant dual-manipulator mutual obstacle avoidance device.

[0091] The redundant dual-arm mutual obstacle avoidance device may be a computing device such as a desktop computer, a notebook, a PDA, or a cloud server. The redundant dual-arm mutual obstacle avoidance device may include, but is not limited to, a processor 31 and a memory 32. Those skilled in the art will appreciate that Figure 3 It is only an example of a redundant dual-manipulator mutual obstacle avoidance device and does not constitute a limitation of the redundant dual-manipulator mutual obstacle avoidance device. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the redundant dual-manipulator mutual obstacle avoidance device may also include input and output devices, network access equipment, buses, etc.

[0092] The processor 31 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

[0093] The memory 32 may be an internal storage unit of the redundant dual-manipulator mutual obstacle avoidance device, such as a hard disk or memory of the redundant dual-manipulator mutual obstacle avoidance device. The memory 32 may also be an external storage device of the redundant dual-manipulator mutual obstacle avoidance device, such as a plug-in hard disk, a smart memory card (SmartMedia Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (FlashCard), etc. equipped on the redundant dual-manipulator mutual obstacle avoidance device. Further, the memory 32 may also include both an internal storage unit and an external storage device of the redundant dual-manipulator mutual obstacle avoidance device. The memory 32 is used to store the computer program and other programs and data required by the redundant dual-manipulator mutual obstacle avoidance device. The memory 32 may also be used to temporarily store data that has been output or is to be output.

[0094] Embodiment 3:

[0095] This embodiment provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the steps of the method described in Embodiment 1 are implemented.

[0096] The computer-readable medium shown can be any device that can contain, store, communicate, propagate or transmit a program for use with an instruction execution system, device or apparatus or in conjunction with these instruction execution systems, devices or apparatuses. More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection portion with one or more wirings (electronic devices), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and editable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium can even be paper or other suitable media on which the program can be printed, such as by optically scanning the paper or other medium, then editing, interpreting or processing in other suitable ways as necessary to obtain the program electronically, and then storing it in a computer memory.

[0097] The above embodiments are only for illustrating the technical concept and features of the present invention, and their purpose is to enable ordinary technicians in the field to understand the content of the present invention and implement it accordingly, and they cannot be used to limit the protection scope of the present invention. Any equivalent changes or modifications made based on the essence of the content of the present invention should be included in the protection scope of the present invention.

Claims

1. A redundant dual-manipulator mutual obstacle avoidance method, characterized in that: include: According to the desired end trajectory of the dual manipulators and the Jacobian matrix of the dual manipulators, the inverse kinematics equation of the manipulators is established on the velocity layer; Adding constraint condition indicators based on the established inverse kinematics equation of the manipulator, wherein the constraint condition indicators include a minimum energy indicator, a joint limit indicator, and a mutual obstacle avoidance indicator; The inverse kinematics equations and constraint indicators of the manipulator are transformed into a time-varying quadratic convex optimization problem subject to equality and inequality constraints; The time-varying quadratic convex optimization problem is transformed into a time-varying quadratic convex optimization problem subject only to equality constraints by defining a penalty function; The time-varying quadratic convex optimization problem subject only to equality constraints is transformed into a time-varying matrix equation through the Lagrange equation; The time-varying matrix equation is solved by a variable parameter convergent differential neural network based on penalty function, and the optimal solution of redundant dual manipulators in the velocity layer is obtained. The optimal solution of the redundant dual manipulator at the velocity layer is integrated to obtain the optimal solution of the joint angle.

2. The redundant dual-manipulator mutual obstacle avoidance method according to claim 1, characterized in that: The inverse kinematics equation of the manipulator is: f(θ)=r; Where r is the desired end trajectory of the dual manipulators, and f(·) is the nonlinear equation about the joint angle of the manipulators. The inverse kinematics equation on the velocity layer is obtained by differentiating both sides of the equation: Where J(θ) is the Jacobian matrix of the robot arm, and are the time derivatives of the robot joint angle and the end trajectory respectively.

3. The redundant dual-manipulator mutual obstacle avoidance method according to claim 2, characterized in that: The minimum energy index is T is the matrix transpose; the joint limit index is The mutual obstacle avoidance index is Among them J N The vector representing the minimum distance between each branch of the dual manipulator in the mutual obstacle avoidance index, and β represents the reverse velocity vector.

4. The redundant dual-manipulator mutual obstacle avoidance method as claimed in claim 3, characterized in that: The inverse kinematics equations and constraint indicators of the manipulator are transformed into a time-varying quadratic convex optimization problem constrained by equality and inequality: Minimize Subject to 5. The redundant dual-manipulator mutual obstacle avoidance method as claimed in claim 4, characterized in that: The method of transforming the time-varying quadratic convex optimization problem into a time-varying quadratic convex optimization problem subject only to equality constraints by defining a penalty function includes: Define a penalty function Where p represents a non-negative penalty factor, h represents the number of inequality constraint equations, σ is a penalty function used to determine The parameters of consistency, The quadratic convex optimization problem subject to equality and inequality constraints is transformed into a quadratic convex optimization problem subject only to equality constraints, specifically: minimize Subject to I is the identity matrix.

6. The redundant dual-manipulator mutual obstacle avoidance method as claimed in claim 5, characterized in that: The method of converting the time-varying quadratic convex optimization problem subject only to equality constraints into a time-varying matrix equation through the Lagrange equation includes: Define the Lagrangian function: Where λ is the Lagrange multiplier; Through two partial differential equations, namely and Construct the following time-varying matrix equation: Ey = e, where 7. The redundant dual-manipulator mutual obstacle avoidance method according to claim 6, characterized in that: The specific method of solving the time-varying matrix equation by using a variable parameter convergent differential neural network based on a penalty function is as follows: Where γ is a constant representing the convergence rate of the neural network, and Ψ(·) represents the activation function of the neural network. From this, the optimal solution y of the time-varying matrix equation Ey=e can be obtained. * .

8. The redundant dual-manipulator mutual obstacle avoidance method according to claim 7, characterized in that: The optimal solution of the redundant dual manipulator at the speed layer is integrated to obtain the optimal solution of the joint angle: * The first n terms of the quadratic programming problem are obtained by * , for x * The optimal solution of the joint angle of the redundant dual manipulator is obtained by integration θ * .

9. A redundant dual-manipulator mutual obstacle avoidance device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 8 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 8 are implemented.

Images