A redundancy robot motion planning method and device and a storage medium

Abstract

The application discloses a kind of redundancy mechanical arm motion planning method, device and storage medium, the method includes according to desired mechanical arm end trajectory and mechanical arm Jacobian matrix, redundancy mechanical arm inverse kinematics equation is established on speed layer;The redundancy mechanical arm inverse kinematics equation established is converted into the time-varying convex quadratic programming problem of multiple equation and inequality constraint;Joint angle constraint is introduced, and time-varying convex quadratic programming problem is converted into the universal form of time-varying quadratic programming problem;The universal form of time-varying quadratic programming problem is defined as Lagrange function and is converted into KKT condition;Barrier function and slack variable vector are introduced to modify KKT condition, and the KKT condition after modification is obtained;The KKT condition after modification is solved by differential neural network, and the optimal solution of redundancy mechanical arm on speed layer is obtained, and the optimal solution of joint angle is obtained by integration again.The application has good real-time, high solving efficiency, and can meet the requirement of motion planning online generation.

Description

Technical Field

[0001] This invention relates to robotic arm motion planning, specifically to a redundancy robotic arm motion planning method, apparatus, and storage medium. Background Technology

[0002] With social development and technological advancements, labor costs are rising, while robot prices are falling. A redundant robotic arm refers to a robotic arm with more degrees of freedom than necessary to complete a task. Due to these additional degrees of freedom, a redundant robotic arm has countless angular space solutions while performing the primary task of its end effector, allowing it to perform additional tasks such as obstacle avoidance, extreme shutdown positions, and handling unusual robotic arm states.

[0003] Traditional methods for solving the inverse kinematics problem of redundant robotic arms are based on pseudo-inverse methods. These methods are computationally intensive, have poor real-time performance, and only consider a single constraint, thus significantly limiting their application in practical robotic arms. In recent years, quadratic programming methods have been proposed to address the repetitive motions of redundant robotic arms, which can be further divided into numerical solvers and neural network solvers. Compared to numerical solvers, neural network solvers offer advantages such as higher efficiency and better real-time performance. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a redundancy robotic arm motion planning method, device, and storage medium to meet the requirements of online motion planning generation.

[0005] To achieve the above objectives, the technical solution of the present invention is as follows:

[0006] In a first aspect, the present invention provides a motion planning method for a redundant robotic arm, comprising the following steps:

[0007] Based on the desired end-effector trajectory and the Jacobian matrix of the robotic arm, the inverse kinematics equations of the redundant robotic arm are established at the velocity level.

[0008] The established inverse kinematic equations of the redundant robotic arm are transformed into a time-varying convex quadratic programming problem constrained by multiple equality and inequality equations.

[0009] By introducing joint angle constraints, the time-varying convex quadratic programming problem is transformed into a general form of the time-varying quadratic programming problem;

[0010] The general form of the time-varying quadratic programming problem is defined as a Lagrangian function and transformed into KKT conditions.

[0011] By introducing a barrier function and a relaxation variable vector, the KKT conditions are modified to obtain the modified KKT conditions.

[0012] The modified KKT conditions are solved using a differential neural network to obtain the optimal solution for the redundancy robot arm at the velocity level.

[0013] Integrate the optimal solution of the redundancy manipulator at the velocity level to obtain the optimal solution of the joint angle.

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

[0015] f(θ(t))=r(t) (1)

[0016] Where r is the desired end-effector trajectory of the robotic arm, and f(·) is a nonlinear equation about the joint angles of the robotic arm;

[0017] Differentiating both sides of the equation yields the inverse kinematic equations for the velocity layer:

[0018]

[0019] Where J(θ(t)) is the Jacobian matrix of the robotic arm. and These are the time derivatives of the robotic arm joint angles and the end effector trajectory, respectively.

[0020] Furthermore, the specific steps of transforming the established inverse kinematics equations of the redundant robotic arm into a time-varying convex quadratic programming problem constrained by multiple equality and inequality equations are as follows:

[0021] minimize

[0022] Subject to

[0023] δ - <θ(t)<δ + (5)

[0024] Where, δ - δ represents the lower limit of the fusion of joint angles and joint angular velocities of the robotic arm. + This indicates the upper limit of the fusion of the joint angle and joint angular velocity of the robotic arm.

[0025] Furthermore, the general form of transforming the time-varying convex quadratic programming problem into a time-varying quadratic programming problem is as follows:

[0026] Minimize 1 / 2x T (t)S(t)x(t)+b T (t)x(t) (6)

[0027] Constrained by C(t)x(t)=d(t) (7)

[0028] Q(t)x(t)≤p(t) (8)

[0029] in, S(t)=I, b(t)=0, C(t)=J(θ(t)), Q(t)=[I;-I], p(t)=δ + ;δ - ]; Here, x(t), S(t), b(t), C(t), Q(t), d(t), and p(t) are the coefficient vectors in the general form of time-varying quadratic programming.

[0030] Furthermore, the generalization of the time-varying quadratic programming problem into a Lagrangian function and its transformation into KKT conditions includes:

[0031] The Lagrange function is constructed as follows:

[0032]

[0033] Where, λ E (t), λ I (t) represent the Lagrange multiplier vectors for equality and inequality constraints, respectively; the Lagrange function is transformed into KKT conditions:

[0034] {S(t)x(t)+b(t)+C T (t)λ E (t)+Q T (t)λ I (t)=0 (10)

[0035] C(t)x(t)-d(t)=0 (11)

[0036] Q(t)x(t)-p(t)≤0 (12)

[0037]

[0038] λ I (t)≥0 (14)

[0039] Among them, operators Representing the Adama product:

[0040]

[0041] Furthermore, the process of introducing a barrier function and a relaxation variable vector to modify the KKT conditions, resulting in modified KKT conditions, includes:

[0042] By introducing the slack variable vector σ(t), the inequality constraints in (12) are transformed into equality constraints.

[0043] Q(t)x(t)+σ(t)-p(t)=0 (16)

[0044] Introducing the barrier function Using the complementary relaxation condition in the smoothness formula (13), we obtain:

[0045]

[0046] in, (3-7) is an r×1 unit vector; the modified KKT conditions are:

[0047] f(y(t),t)=W(t)y(t)-u(t)=0 (18)

[0048] in,

[0049] y(t)=[x(t),λ E (t),λ I (t),σ] (19)

[0050]

[0051] Γ(t)=diag(p(t)-Q(t)x(t) (21)

[0052] u(t)=[-b(t),d(t),p(t),1 / βe r ] ((twenty two).

[0053] Furthermore, the differential neural network is as follows:

[0054]

[0055] in,

[0056]

[0057]

[0058] The optimal solution y of matrix equation (18) can be obtained from equation (23). * (t).

[0059] Furthermore, by y * The first n terms of (t) yield the optimal solution x to the quadratic programming problem. * , will x * The optimal solution θ for the joint angles of the redundant robotic arm is obtained by integration. * .

[0060] In a second aspect, the present invention provides a redundancy robotic arm motion planning device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of any of the methods described above.

[0061] Thirdly, the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of any of the methods described above.

[0062] Compared with the prior art, the advantages of this invention are as follows:

[0063] Compared with existing methods for solving redundancy in robotic arm motion planning, this invention has the following advantages:

[0064] 1. Compared with traditional pseudo-inverse-based methods, this invention solves the motion planning problem of redundant robotic arms through a quadratic programming scheme, which has good real-time performance and can meet the requirements of online generation of motion plans;

[0065] 2. Compared with numerical method solvers, this invention belongs to neural network solvers, which have faster calculation speed and higher efficiency;

[0066] 3. Compared with the classic recurrent neural network solver, the present invention adopts a novel barrier function differential neural network, which has a faster convergence speed, higher computational accuracy, and better robustness. Attached Figure Description

[0067] Figure 1 A flowchart of the redundancy robotic arm motion planning method provided in Embodiment 1 of the present invention;

[0068] Figure 2 A schematic diagram illustrating the simulated motion of a robotic arm with redundancy based on the motion planning data generated in this invention;

[0069] Figure 3 This is a schematic diagram of the composition of the redundancy robotic arm motion planning device provided in Embodiment 2 of the present invention. Detailed Implementation

[0070] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0071] Example 1:

[0072] See Figure 1 As shown, the redundancy robotic arm motion planning method provided in this embodiment mainly consists of three parts: problem statement, problem transformation, and problem solution.

[0073] The problem statement section mainly includes the following two steps:

[0074] Based on the desired end-effector trajectory and the Jacobian matrix of the robotic arm, the inverse kinematics equations of the redundant robotic arm are established at the velocity level.

[0075] The established inverse kinematic equations of the redundant robotic arm are transformed into a time-varying convex quadratic programming problem constrained by multiple equality and inequality equations.

[0076] The problem transformation section includes the following steps:

[0077] By introducing joint angle constraints, the time-varying convex quadratic programming problem is transformed into a general form of time-varying quadratic programming problem. The purpose of introducing joint angle constraints is that in reality, the joints of the robotic arm cannot rotate indefinitely. Adding joint constraints can prevent the robotic arm from being damaged due to the joint angles rotating beyond their own angle limits.

[0078] The general form of the time-varying quadratic programming problem is defined as a Lagrangian function and transformed into KKT (Karush-Kuhn-Tucker) conditions;

[0079] The KKT conditions are modified by introducing a barrier function and a slack variable vector to obtain the modified KKT conditions. By adding slack variables, the inequality constraints are transformed into equality constraints for solving. However, since the inequality constraints do not always take effect in time-varying problems, the transition between the effective and ineffective inequality constraints is not smooth and means that they are not differentiable. This makes it difficult to solve using the following neurodynamic formula (24). Therefore, a barrier function is introduced to smooth the transition between the effective and ineffective inequality constraints.

[0080] The problem-solving section includes the following steps:

[0081] The modified KKT conditions are solved using a differential neural network to obtain the optimal solution for the redundancy robot arm at the velocity level.

[0082] Integrate the optimal solution of the redundancy manipulator at the velocity level to obtain the optimal solution of the joint angle.

[0083] In one specific embodiment, the inverse kinematics equation of the aforementioned redundant robotic arm is:

[0084] f(θ) = r, (1)

[0085] Where r is the desired end-effector trajectory, and f(·) is a nonlinear equation concerning the robot arm's shutdown angle. Equation (1) represents the calculation of the positions of each joint of the robot arm at corresponding times from the known end-effector position. Due to the nonlinearity of the equation, the solution is not unique, so the problem is considered at the velocity level. Taking the derivative of both sides of the equation with respect to time, we get...

[0086]

[0087] Where J(θ) is the Jacobian matrix of the robotic arm, and These are the time derivatives of the robotic arm joint angles and the end effector trajectory, respectively.

[0088] The problem of non-repetitive motion in redundant robotic arms is solved using quadratic programming. The following unified quadratic programming problem is designed:

[0089] minimize

[0090] Subject to

[0091] δ - <θ(t)<δ + (5)

[0092] Where, δ - δ represents the lower limit of the fusion of joint angles and joint angular velocities of the robotic arm. + This indicates the upper limit of the fusion of joint angles and joint angular velocities in the robotic arm. Regarding the redundancy, the effect of robotic arm motion planning is as follows: Figure 2 As shown. Traditional methods for solving the inverse kinematics problem of redundant robotic arms are based on pseudo-inverse methods. These methods are computationally intensive, have poor real-time performance, and only consider a single constraint, which greatly limits their application in practical robotic arms. In recent years, quadratic programming methods have been proposed to solve the repetitive motions of redundant robotic arms, which can be further divided into numerical solvers and neural network solvers. Compared with numerical solvers, neural network solvers have the advantages of higher efficiency and better real-time performance. The barrier function differential neural network solver proposed in this invention has higher computational accuracy and better robustness than the classic recurrent neural network solver.

[0093] To better describe the problem, the time-varying convex quadratic programming problem constrained by equality and inequality is transformed into the general form of time-varying quadratic programming as follows:

[0094] Minimize 1 / 2x T (t)S(t)x(t)+bT( (6)

[0095] Constrained by C(t)x(t)=d(t), (7)

[0096] Q(t)x(t)≤p(t) (8)

[0097] in, S(t)=I, b(t)=0, C(t)=J(θ(t)), Q(t) = [I; -I], p(t) = δ + ;δ -Here, x(t), S(t), b(t), C(t), Q(t) are coefficient vectors in the general form of time-varying quadratic programming, used to facilitate the description of subsequent methods.

[0098] Then the Lagrange function is constructed as follows:

[0099]

[0100] Where, λ E (t), λ I (t) represent the Lagrange multiplier vectors for equality and inequality constraints, respectively. The Lagrange function can be transformed into KKT conditions:

[0101] {S(t)x(t)+b(t)+C T (t)λ E (t)+Q T (t)λ I (t)=0 (10)

[0102] C(t)x(t)-d(t)=0 (11)

[0103] Q(t)x(t)-p(t)≤0 (12)

[0104]

[0105] λ I (t)≥0 (14)

[0106] Among them, operators Represents Adama product

[0107]

[0108] By introducing the slack variable vector σ(t), the inequality constraint in (12) is transformed into an equality constraint.

[0109] Q(t)x(t)+σ(t)-p(t)=0 (16)

[0110] Introducing the barrier function Using the complementary relaxation condition in the smoothness formula (10), we obtain:

[0111]

[0112] in, (3-7) is an r×1 unit vector. The modified KKT conditions are as follows:

[0113] f(y(t),t)=W(t)y(t)-u(t)=0, (18)

[0114] in,

[0115] y(t)=[x(t),λ E (t),λI(t),σ], (19)

[0116]

[0117] Γ(t)=diag(p(t)-Q(t)x(t), (21)

[0118] u(t)=[-b(t),d(t),p(t),1 / βe r ], (twenty two)

[0119] To solve the KKT conditions (10)-(14), the objective function is to make the error function,

[0120] ε(t)=f(y(t),t), (23)

[0121] Using neurodynamics, the design error converges to zero in the following manner.

[0122]

[0123] Where γ is a parameter that adjusts the convergence rate, and Φ(·) is the activation function. Substituting equation (23) into equation (24) yields the differential neural network solver, i.e.,

[0124]

[0125] in,

[0126]

[0127]

[0128] The optimal solution y of matrix equation (18) can be obtained from equation (25). * The first n terms are the optimal solutions x of the general forms (6), (7) and (8) of the time-varying quadratic programming. * , for x * Integration yields the optimal solution θ for the joint angles of the redundant robotic arm. * .

[0129] It should be understood that, although Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1At least some of the steps in the process may include multiple steps or multiple stages. These steps or stages are not necessarily completed at the same time, but may be executed at different times. The execution order of these steps or stages is not necessarily sequential, but may be executed in turn or alternately with other steps or at least some of the steps or stages in other steps.

[0130] Example 2:

[0131] See Figure 3 As shown, the redundancy robotic arm motion planning 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 redundancy robotic arm motion planning program. When the processor 31 executes the computer program 33, it implements the steps of Embodiment 1 described above, for example... Figure 1 The steps are shown.

[0132] For example, the computer program 33 can 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 can be a series of computer program instruction segments capable of performing specific functions, which describe the execution process of the computer program 33 in the redundant robotic arm motion planning device.

[0133] The processor 31 may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), 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.

[0134] The memory 32 can be an internal storage unit of the redundant robotic arm motion planning device, such as a hard drive or memory. The memory 32 can also be an external storage device, such as a plug-in hard drive, SmartMedia Card (SMC), Secure Digital (SD) card, or Flash Card equipped on the redundant robotic arm motion planning device. Furthermore, the memory 32 can include both internal and external storage units of the redundant robotic arm motion planning device. The memory 32 is used to store the computer program and other programs and data required by the redundant robotic arm motion planning device. The memory 32 can also be used to temporarily store data that has been output or will be output.

[0135] Example 3:

[0136] This embodiment provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method described in Embodiment 1.

[0137] The computer-readable medium shown can be any means that can contain, store, communicate, propagate, or transmit a program for use in or in conjunction with an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Additionally, the computer-readable medium can even be paper or other suitable media on which the program can be printed, for example, by optically scanning the paper or other medium, then editing, interpreting, or otherwise processing it as necessary to obtain the program electronically, and then storing it in computer memory.

[0138] The above embodiments are merely illustrative of the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement it accordingly. They should not be construed as limiting the scope of protection of the present invention. All equivalent changes or modifications made based on the essence of the content of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A redundancy-based robotic arm motion planning method, characterized in that, Includes the following steps: Based on the desired end-effector trajectory and the Jacobian matrix of the robotic arm, the inverse kinematics equations of the redundant robotic arm are established at the velocity level. The established inverse kinematic equations of the redundant robotic arm are transformed into a time-varying convex quadratic programming problem constrained by multiple equality and inequality equations. By introducing joint angle constraints, the time-varying convex quadratic programming problem is transformed into a general form of the time-varying quadratic programming problem; The general form of the time-varying quadratic programming problem is defined as a Lagrangian function and transformed into KKT conditions. By introducing a barrier function and a relaxation variable vector, the KKT conditions are modified to obtain the modified KKT conditions. The modified KKT conditions are solved using a differential neural network to obtain the optimal solution for the redundancy robot arm at the velocity level. Integrate the optimal solution of the redundancy manipulator at the velocity level to obtain the optimal solution of the joint angle; The general form of the time-varying quadratic programming problem is defined as a Lagrange function and transformed into KKT conditions, including: The Lagrange function is constructed as follows: in, S(t)=I, b(t)=0, C(t)=J(θ(t)), Q(t)=[I;-I], Here, x(t), S(t), b(t), C(t), Q(t), d(t), and p(t) are the coefficient vectors in the general form of time-varying quadratic programming; I represents the identity matrix; λ E (t), λ I (t) represent the Lagrange multiplier vectors for equality and inequality constraints, respectively; the Lagrange function is transformed into KKT conditions: {S(t)x(t)+b(t)+C T (t)λ E (t)+Q T (t)λ I (t)=0 (10) C(t)x(t)-d(t)=0 (11) Q(t)x(t)-p(t)≤0 (12) l I (t)≥0 (14) Among them, operators Representing the Adama product:

2. The redundancy robotic arm motion planning method as described in claim 1, characterized in that, The inverse kinematics equation of the redundant robotic arm is: f(θ(t))=r(t) (1) Where r is the desired end-effector trajectory of the robotic arm, and f(·) is a nonlinear equation about the joint angles of the robotic arm; Differentiating both sides of the equation yields the inverse kinematic equations for the velocity layer: Where J(λ(t)) is the Jacobian matrix of the robotic arm. and These are the time derivatives of the robotic arm joint angles and the end effector trajectory, respectively.

3. The redundancy robotic arm motion planning method as described in claim 2, characterized in that, The specific steps for transforming the established inverse kinematics equations of the redundant robotic arm into a time-varying convex quadratic programming problem constrained by multiple equality and inequality equations are as follows: Minimize Subject to d - <θ(t)<δ + ) (5) Where, δ - δ represents the lower limit of the fusion of joint angles and joint angular velocities of the robotic arm. + This represents the upper limit of the fusion of the joint angles and joint angular velocities of the robotic arm; θ(t) is the joint angle matrix.

4. The redundancy robotic arm motion planning method as described in claim 3, characterized in that, The general form of transforming the time-varying convex quadratic programming problem into a time-varying quadratic programming problem is as follows: Minimize 1 / 2x T (t)S(t)x(t)+b T (t)x(t) (6) Constrained by C(t)x(t)=d(t) (7) Q(t)x(t)≤p(t) (8) in, S(t)=I, b(t)=0, C(t)=J(θ(t)), Q(t)=[I;-I], p(t)=[δ + ;δ - ]; Here, x(t), S(t), b(t), C(t), Q(t), d(t), and p(t) are the coefficient vectors in the general form of time-varying quadratic programming; I represents the identity matrix.

5. The redundancy robotic arm motion planning method as described in claim 1, characterized in that, The method of introducing a barrier function and a relaxation variable vector to modify the KKT conditions, resulting in modified KKT conditions, includes: By introducing the slack variable vector σ(t), the inequality constraint in formula (12) is transformed into an equality constraint. Q(t)x(t)+σ(t)-p(t)=0 (16) Introducing the barrier function Using the complementary relaxation condition in the smoothness formula (13), we obtain: in, It is an r×1 unit vector; the modified KKT conditions are: f(y(t),t)=W(t)y(t)-u(t)=0 (18) in, y(t)=[x(t),λ E (t),λ I (t),σ] (19) Γ(t)=diag(p(t)-Q(t)x(t) (21) u(t)=[-b(t),d(t),p(t),1 / βe r ] (22)。 6. The redundancy robotic arm motion planning method as described in claim 5, characterized in that, The differential neural network is: in, The optimal solution y of matrix equation (18) is obtained from equation (23). * (t).

7. The redundancy robotic arm motion planning method as described in claim 6, characterized in that, By y * The first n terms of (t) yield the optimal solution x to the quadratic programming problem. * , will x * The optimal solution θ for the joint angles of the redundant robotic arm is obtained by integration. * .

8. A redundancy robotic arm motion planning 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, it implements the steps of the method as described in any one of claims 1 to 7.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 7.

Images