A robot arm motion planning method and system

Abstract

The present invention discloses a method and system for manipulator motion planning, which belongs to the field of manipulator control technology. The manipulator motion planning model is converted into a corresponding trust region sub-problem for solution, and the trust region radius is continuously adjusted to obtain the adjustment direction of the manipulator motion decision variable. In this process, the manipulator motion decision variable is further adjusted according to the gradient descent direction of the motion planning model to determine whether there is a large deviation between the trust region sub-problem and the motion planning model within the trust region, thereby ensuring the continuous descent of the iterative process and accelerating the manipulator motion planning speed. At the same time, in the whole process, considering the actual physical limitations of the manipulator motion decision variable, the trust region direction and the gradient descent direction are corrected respectively to ensure that the adjusted manipulator motion decision variable is still within the actual physical limitation range, and the solution speed of the manipulator motion planning can be improved under the premise of meeting the planning control requirements.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm control, and more specifically, relates to a robot arm motion planning method and system. Background Art

[0002] Many achievements and functions of robotic arms have been realized and applied in the service and manufacturing industries. Due to the infinite solution nature of a given task, the modeling formulation and solution strategy of the optimization problem directly determine the functionality and efficiency of the robotic arm. As a practical solution, a quadratic programming strategy is often constructed to handle the motion planning control of the robotic arm, and the task requirements are formulated as optimization indicators and constraints.

[0003] The robotic arm motion planning task is essentially a dynamic time-varying system with strong nonlinearity, which will produce unknown system structure changes due to factors such as motor wear, internal noise, external environment, and human-machine interaction. For example, for a multi-degree-of-freedom robotic arm, the movement of each joint will cause changes in the mass distribution, moment of inertia, arm length and other parameters of the robotic arm (especially under load), which will significantly affect its dynamic structure. Secondly, the robotic system has time delay characteristics, such as unit delays caused by periodic sampling, and time lags caused by the inertia of the electromechanical part of the system. Furthermore, the actual parameters of the robot usually have errors with the calibrated parameters. Due to the dynamic characteristics of the robot, this error will accumulate and amplify during the robot planning process, thereby reducing the stability of the system. Therefore, the robotic system usually has the characteristics of parameter uncertainty. Furthermore, intrinsically nonlinear factors such as dead zone, saturation, and hysteresis also seriously affect the system structure and dynamic performance of the robot. When it comes to more complex robotic systems, such as multi-robot systems for industrial production, mobile robot systems for the service industry, and robotic vision systems for product maintenance and medical surgery, the impact of nonlinearity and uncertainty in the system will become more significant, which will have a certain impact on the real-time solution of robot arm motion planning.

[0004] In recent years, solving the corresponding kinematic nonlinear programming problems based on neural networks has become an effective way to solve the motion planning of robotic arms. However, in the existing technologies, most of them are about the effective control of the motion planning of robotic arms, and are not compatible with solving the problems of real-time accelerated planning solutions. For example, some methods can accelerate the solution of nonlinear programming problems by introducing the Nesterov method into the dynamic evolution process of neural networks, but are usually subject to constraints such as the strong convexity or smoothness of the objective function. There are also some methods that accelerate the solution of robotic arm motion planning by neural networks by adopting activation functions or introducing time-varying coefficients, but there are still parameter sensitivity problems, which leads to large deviations between the end effector and the actual trajectory, and the accuracy of the robotic arm motion planning cannot be guaranteed. Summary of the invention

[0005] In view of the above defects or improvement needs of the prior art, the present invention provides a robot arm motion planning method and system to solve the technical problem that the prior art cannot improve the solution speed of the robot arm motion planning while meeting the control requirements of the robot arm motion planning.

[0006] In order to achieve the above objectives, in a first aspect, the present invention provides a robot arm motion planning method, comprising:

[0007] S1. Construct the motion planning model of the robot arm and construct the corresponding trust region subproblem; the cost functions of the motion planning model and the trust region subproblem are L(x(t)) and q(d tr (t)); Initialize the current time t, and initialize the robot motion decision variable x(t) and the trust region radius Δ(t) of the trust region subproblem at time t;

[0008] S2. Solve the trust region subproblem according to the motion state variables of the motion planning model at time t, and obtain the trust region direction d at time t tr (t);

[0009] S3, x(t)+d tr (t) is mapped to Ω = [η - ,η + ] range, and obtain the predicted motion decision variable x in the trust region direction tr (t); x tr The difference between (t) and x(t) is taken as the projection trust region direction at time t η - and η + are the minimum and maximum values ​​of the robot arm motion decision variables, respectively;

[0010] S4. Calculate the predicted drop of the trust region subproblem in the projected trust region direction and the actual descent of the motion planning model 0 is the zero vector;

[0011] S5. Calculate the ratio like Then x(t) is The direction is updated and based on ρ tr (t) Update Δ(t) and go to S7; otherwise, go to S6;

[0012] S6. Calculate the gradient d of L(x(t)) with respect to x(t) g (t), x(t)-d g (t) is mapped to Ω = [η - ,η +] range, and obtain the predicted motion decision variable x in the direction of gradient descent g (t); x g The difference between (t) and x(t) is used as the projected gradient descent direction at time t Calculate the predicted descent amount for the trust region subproblem in the direction of projected gradient descent and the actual descent of the motion planning model Calculate the ratio like Then x(t) is The direction is updated and based on ρ g (t) Update Δ(t) and go to S7; otherwise, reduce Δ(t) and go to S7;

[0013] S7, let t = t + τ, update the motion state variables of the motion planning model at time t, and go to S2, until t reaches the preset time, output the robot arm motion decision variables at each time to realize the robot arm motion planning;

[0014] Wherein, τ is the preset time interval; and are preset thresholds, and

[0015] Further preferably, in S5, based on ρ tr (t) Update Δ(t) includes: when When , keep Δ(t) unchanged; when When , Δ(t) increases; where, is the preset threshold,

[0016] Further preferably, in S6, based on ρ g (t) Update Δ(t) includes: when When , reduce Δ(t); when When , keep Δ(t) unchanged; when When , Δ(t) increases; where, is the preset threshold,

[0017] Further preferably, in S5, according to the formula x(t) along The direction is updated; where σ tr is the first preset adjustment parameter, 0<σ tr <1.

[0018] Further preferably, in S6, according to the formula x(t) along The direction is updated; where σ gis the second preset adjustment parameter, 0<σ g <1.

[0019] Further preferably, the robot arm motion decision variable is: a joint angle state, a joint angular velocity state or an angular acceleration state of the robot arm.

[0020] Further preferably, the motion planning model is:

[0021]

[0022] Where Λ is a preset positive definite symmetric matrix; J(t) is the Jacobian matrix of the motion planning model; when x(t) is the joint angle state of the robot arm, y d (t) is the position state of the end effector of the robot arm; when x(t) is the joint angular velocity state of the robot arm, y d (t) is the velocity state of the end effector of the robot arm; when x(t) is the joint angular acceleration state of the robot arm, y d (t) is the acceleration state of the end effector of the robot arm.

[0023] Further preferably, the motion planning model is:

[0024]

[0025] Where Λ is a preset positive symmetric matrix; λ(t) is a Lagrange multiplier; J(t) is the Jacobian matrix of the motion planning model; when x(t) is the joint angle state of the robot arm, y d (t) is the position state of the end effector of the robot arm; when x(t) is the joint angular velocity state of the robot arm, y d (t) is the velocity state of the end effector of the robot arm; when x(t) is the joint angular acceleration state of the robot arm, y d (t) is the acceleration state of the end effector of the robot arm.

[0026] In a second aspect, the present invention provides a robot arm motion planning system, comprising: a memory and a processor, wherein the memory stores a computer program, and the processor executes the robot arm motion planning method provided in the first aspect of the present invention when executing the computer program.

[0027] In a third aspect, the present invention further provides a computer-readable storage medium, which includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute the robot arm motion planning method provided in the first aspect of the present invention.

[0028] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects:

[0029] 1. The present invention provides a method for manipulator motion planning, which converts the manipulator motion planning model into a corresponding trust region sub-problem for solution, and continuously adjusts the trust region radius to obtain the most appropriate adjustment direction of the manipulator motion decision variable at each moment; and in this process, according to the ratio of the actual descent of the motion planning model in the projected trust region direction to the predicted descent of the trust region sub-problem, it is judged whether there is a large deviation between the trust region sub-problem and the motion planning model within the trust region range, and the manipulator motion decision variable is further adjusted according to the gradient descent direction of the motion planning model in the case of large deviation; by combining the gradient descent direction as a guarantee mechanism in the process of adjusting the manipulator motion decision variable based on the trust region direction, the continuous descent of the iterative process is guaranteed, and the solution speed of the manipulator motion planning is accelerated. At the same time, in the whole process, considering the actual physical limitations of the manipulator motion decision variable, the trust region direction and the gradient descent direction are respectively corrected to the corresponding projected trust region direction and the projected gradient descent direction, so as to ensure that the adjusted manipulator motion decision variable is still within the actual physical limitation range, and the solution speed of the manipulator motion planning can be improved under the premise of meeting the control requirements of the manipulator motion planning.

[0030] 2. Furthermore, the robot arm motion planning method provided by the present invention, when When ρ tr (t) is a credible direction, then according to the ratio ρ tr (t) is compared with the set threshold. If This indicates that ρ tr The value of (t) is around 0.5, with ρ tr (t) The trust region subproblem as a decision variable is basically consistent with the approximation of the motion planning model. In this case, the trust region radius is appropriate and does not need to be adjusted. This indicates that ρ tr (t) is large, with ρ tr (t) If the trust region subproblem as a decision variable has a good approximation to the motion planning model, the trust range can be widened by increasing the trust region radius.

[0031] 3. Furthermore, the robot arm motion planning method provided by the present invention, when When tr (t) is not a credible direction, then take safeguard measures and choose ρ g (t) is used as the credible direction, according to the ratio ρ g (t) is compared with the set threshold. If This indicates that ρ g (t) is small, with ρ g(t) The approximation between the trust region subproblem as a decision variable and the motion planning model is not ideal, and the trust region radius can be reduced; if This indicates that ρ g The value of (t) is around 0.5, with ρ g (t) The trust region subproblem as a decision variable is basically consistent with the approximation of the motion planning model. In this case, the trust region radius is appropriate and does not need to be adjusted. This indicates that ρ g (t) is large, with ρ g (t) If the trust region subproblem as a decision variable has a good approximation to the motion planning model, the trust range can be widened by increasing the trust region radius.

[0032] 4. Furthermore, the robot arm motion planning method provided by the present invention, when When tr (t) is a credible direction that can fully reduce the objective function of the motion planning model. Based on the principle of neural dynamics, the formula is constructed x(t) along The direction is updated to update x(t) more accurately.

[0033] 5. Furthermore, the robot arm motion planning method provided by the present invention, when and When g (t) is a credible direction that can fully reduce the objective function of the motion planning model. Based on the principle of neural dynamics, the formula is constructed x(t) along The direction is updated to update x(t) more accurately. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 A flow chart of the robot arm motion planning method provided by the present invention;

[0035] Figure 2 Schematic diagram of the motion trajectory of the end effector of the robot arm generated by the method of the present invention provided in the embodiment of the present invention

[0036] Figure 3 A schematic diagram of the three-dimensional spatial position state of a robotic arm end effector generated by applying the method of the present invention provided in an embodiment of the present invention;

[0037] Figure 4 A schematic diagram of the error state between the three-dimensional spatial position of the end effector of the robot arm and the expected trajectory generated by the method of the present invention provided in an embodiment of the present invention;

[0038] Figure 5A schematic diagram of the angle states of each joint of a robotic arm generated by applying the method of the present invention provided in an embodiment of the present invention;

[0039] Figure 6 The embodiment of the present invention provides the angular velocity states of each joint of the robotic arm generated by applying the method of the present invention. DETAILED DESCRIPTION

[0040] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0041] In order to achieve the above objectives, firstly, Figure 1 As shown, the present invention provides a robot arm motion planning method, comprising:

[0042] S1. Construct the motion planning model of the robot arm and construct the corresponding trust region subproblem; the cost functions of the motion planning model and the trust region subproblem are L(x(t)) and q(d tr (t)); Initialize the current time t, and initialize the robot motion decision variable x(t) and the trust region radius Δ(t) of the trust region subproblem at time t;

[0043] It should be noted that the robot arm motion decision variables are: the joint angle state, joint angular velocity state or angular acceleration state of the robot arm.

[0044] In an optional implementation, the process of constructing the motion planning model is as follows:

[0045] A nonlinear transformation model of the robot arm's forward kinematics from the joint space to the Cartesian workspace is established, and the robot arm's kinematic transfer function is shown below:

[0046] y d (t) = f(x(t))

[0047] Where f(x(t)) is the kinematic transfer function of the robot arm; y d (t) is determined according to the motion planning task of the robot arm; specifically, when x(t) is the joint angle state of the robot arm, y d (t) is the position state of the end effector of the robot arm; when x(t) is the joint angular velocity state of the robot arm, y d (t) is the velocity state of the end effector of the robot arm; when x(t) is the joint angular acceleration state of the robot arm, y d (t) is the acceleration state of the end effector of the robot arm.

[0048] Due to the nonlinearity and redundancy of the robot arm kinematic transfer function f(x(t)), it is usually difficult to directly obtain the required robot arm motion decision variable x(t), so that y d (t) is consistent with the ideal plan of the given robot arm motion plan, so the Jacobian matrix of the robot arm kinematic transfer function f(x(t)) can be calculated And establish the motion planning model of the robot arm as shown below:

[0049]

[0050] Wherein, Λ is a preset positive definite symmetric matrix; J(t) is the Jacobian matrix of the motion planning model; for y d (t) is the derivative variable of

[0051] It should be noted that the above motion planning model expression is not unique. In addition to the above constraints, it can also include the energy constraints of the robot arm motion planning. etc. constraints, where e(t) is the preset energy function of the robot arm motion planning.

[0052] In order to facilitate the solution, in an optional implementation, based on the Lagrange multiplier method, the above-mentioned constrained motion planning model is converted into an unconstrained form, specifically:

[0053]

[0054] Where λ(t) is the Lagrange multiplier.

[0055] The trust region subproblem corresponding to building the motion planning model of the robotic arm is as follows:

[0056]

[0057] Here, the symbol ||·|| represents any vector norm, usually the 2 norm; d tr (t) is the trust region direction variable to be determined; Δ(t) is the trust region radius; d g (t) is the gradient of L(x(t)) with respect to x(t).

[0058] S2. Solve the trust region subproblem according to the motion state variables of the motion planning model at time t, and obtain the trust region direction d at time t tr (t);

[0059] Among them, the motion state variables of the motion planning model at time t include: the Jacobian matrix J(t) of the motion planning model, the motion planning task given y of the robot arm d (t), and the corresponding derivative variables

[0060] S3, x(t)+d tr (t) is mapped to Ω = [η - ,η + ] range, and obtain the predicted motion decision variable x in the trust region direction tr (t); x tr The difference between (t) and x(t) is taken as the projection trust region direction at time t η - and η + are the minimum and maximum values ​​of the robot arm motion decision variables, respectively;

[0061] Specifically,

[0062] Among them, P Ω [x(t)+d tr (t)] = max[min[x(t)+d tr (t),η + ],η - ] represents the projection operator, which limits the predicted motion decision variables updated by the trust region direction to a certain bounded range [x(t)+d tr (t)]∈Ω=[η - ,η + ].

[0063] It should be noted that the above projection operator is not the only one. For example, it can also be P Ω [x(t)+d tr (t)] = max[min[x(t)+αd tr (t),η + ],η - ], where α is the preset coefficient.

[0064] S4. Calculate the predicted drop of the trust region subproblem in the projected trust region direction and the actual descent of the motion planning model Among them, 0 is the zero vector.

[0065] S5. Calculate the ratio like Then x(t) is The direction is updated and based on ρ tr (t) Update Δ(t) and go to S7; otherwise, go to S6;

[0066] In an optional implementation, in S5, based on ρ tr (t) Update Δ(t) includes: when When , keep Δ(t) unchanged; when When , Δ(t) increases; where, is the preset threshold,

[0067] It should be noted that when It shows that the projection trust region direction can fully reduce the redundant robot arm motion planning model, ρ tr (t) is a credible direction. At this time, based on ρ tr (t) updates Δ(t) and according to the ratio ρ tr (t) is compared with the set threshold. If This indicates that ρ tr The value of (t) is around 0.5, with ρ tr (t) The trust region subproblem as a decision variable is basically consistent with the approximation of the motion planning model. In this case, the trust region radius is appropriate and does not need to be adjusted. This indicates that ρ tr (t) is large, with ρ tr (t) If the trust region subproblem as a decision variable has a good approximation to the motion planning model, the trust range can be widened by increasing the trust region radius.

[0068] Preferably, the formula Update Δ(t); where, is the preset threshold, γ1 is a first preset trust region radius parameter, γ1>1.

[0069] It should be noted that the above formula is not the only form. When , keep Δ(t) unchanged; when When , just increase Δ(t).

[0070] In an alternative embodiment, according to the neurodynamic formula x(t) along The direction is updated; where σ tr is the first preset adjustment parameter, specifically the adjustment parameter of the convergence speed of the first neural dynamics network, 0<σ tr <1. Through this design, the equilibrium point of the first neural dynamics network can be made the same as the optimal solution of the robot arm motion planning model.

[0071] It should be noted that this is not the only update method. You can also update x(t) directly to I won’t go into details here.

[0072] S6. Calculate the gradient d of L(x(t)) with respect to x(t) g (t), x(t)-d g(t) is mapped to Ω = [η - ,η + ] range, and obtain the predicted motion decision variable x in the direction of gradient descent g (t); x g The difference between (t) and x(t) is used as the projected gradient descent direction at time t Calculate the predicted descent amount for the trust region subproblem in the direction of projected gradient descent and the actual descent of the motion planning model Calculate the ratio like Then x(t) is The direction is updated and based on ρ g (t) update Δ(t), and go to S7; otherwise, reduce Δ(t), and go to S7; where τ is a preset time interval; and are preset thresholds, and

[0073] It should be noted that when When it indicates that the dependent domain direction cannot make the redundant robot arm motion planning model fully descend, the projected gradient descent direction is used as the new update direction to achieve acceleration.

[0074] Specifically,

[0075] Among them, P Ω [x(t)-d g (t)] = max[min[x(t)-d g (t),η + ],η - ] represents the projected gradient operator, which limits the predicted motion decision variable after the projected gradient descent direction is updated to a certain bounded range [x(t)-d g (t)]∈Ω=[η - ,η + ];

[0076] It should be noted that the above is not the only form. For example, P Ω [x(t)-d g (t)] = max[min[x(t)-βd g (t),η + ],η - ], where β is the preset coefficient.

[0077] In an optional implementation, in S6, based on ρ g (t) Update Δ(t) includes: when When , reduce Δ(t); when When , keep Δ(t) unchanged; when When , Δ(t) increases; where, is the preset threshold,

[0078] It should be noted that when When tr (t) is not a credible direction, then take safeguard measures and choose ρ g (t) as the credible direction, and x(t) along The direction is updated and the ratio ρ is used g (t) is compared with the set threshold. If This indicates that ρ g (t) is small, with ρ g (t) The approximation between the trust region subproblem as a decision variable and the motion planning model is not ideal, and the trust region radius can be reduced; if This indicates that ρ g The value of (t) is around 0.5, with ρ g (t) The trust region subproblem as a decision variable is basically consistent with the approximation of the motion planning model. In this case, the trust region radius is appropriate and does not need to be adjusted. This indicates that ρ g (t) is large, with ρ g (t) If the trust region subproblem as a decision variable has a good approximation to the motion planning model, the trust range can be widened by increasing the trust region radius.

[0079] Preferably, when When using the formula Update Δ(t); where, is the preset threshold, is the second preset trust region radius parameter,

[0080] It should be noted that the above formula is not the only form. When , reduce Δ(t); when When , keep Δ(t) unchanged; when When , just increase Δ(t).

[0081] It should be noted that when This indicates that ρ g (t) is smaller, ρ g (t) is not a credible direction, so x(t) is not updated and the trust region radius Δ(t) is reduced. The way to reduce the trust region radius Δ(t) is the same as when The method of reducing Δ(t) is the same as that of reducing Δ(t), which will not be described here.

[0082] In an alternative embodiment, according to the neurodynamic formula x(t) along The direction is updated; where σ g is the second preset adjustment parameter, specifically the adjustment parameter of the convergence speed of the second neural dynamics network, 0<σ g <1. Through this design, the equilibrium point of the second neural dynamics network can be made the same as the optimal solution of the robot arm motion planning model.

[0083] It should be noted that this is not the only update method. You can also update x(t) directly to I won’t go into details here.

[0084] S7, let t = t + τ, update the motion state variables of the motion planning model at time t, and go to S2, until t reaches the preset time, output the robot arm motion decision variables at each time to realize the robot arm motion planning;

[0085] Specifically, the robot arm motion decision variables at each moment can be substituted into the robot arm motion planning model to obtain the corresponding y d (t), and then based on y d (t) Obtain the control signal of the robot arm motion planning and transmit it to the lower computer controller to perform the robot arm motion planning task.

[0086] In one implementation, taking the angular velocity information of each joint of the robot arm as an example of a robot arm motion decision variable, the angular velocity information of each joint of the robot arm at each moment is used to perform a robot arm motion planning task, including: obtaining the angular velocity information of the end effector of the robot arm at each moment according to the relationship between the angular velocity of each joint of the robot arm and the angular velocity of the end effector of the robot arm in the robot arm motion planning model; integrating the angular velocity information of the end effector of the robot arm at each moment to obtain the position sequence information of the end effector of the robot arm; converting the position sequence information of the end effector of the robot arm into a control signal for the robot arm motion planning and transmitting it to the lower computer controller to perform the robot arm motion planning task.

[0087] In order to illustrate the robot arm motion planning method provided by the present invention, a specific embodiment is described in detail below:

[0088] In this embodiment, x(t) is the robot arm joint angle state y d (t) is the velocity state of the end effector of the robot arm; the motion planning model adopts an unconstrained form after conversion based on the Lagrange multiplier method.

[0089] The entire robot arm motion planning process includes the following steps:

[0090] A1. Given the ideal trajectory r of the robot arm motion planning d (t), ideal trajectory speed of robot arm motion planning Get the position state r(t) of the robot arm end effector and the speed state of the robot arm end effector Robot arm joint angle state θ(t), joint angular velocity state The robot arm kinematic transfer function f(θ(t)) can be used to establish the nonlinear transformation model of the robot arm forward kinematics from the joint space to the Cartesian workspace, as shown below

[0091] r(t)=f(θ(t))

[0092] Due to the nonlinearity and redundancy of the robot arm kinematic transfer function f(θ(t)), it is usually difficult to directly obtain the required robot arm joint angle state θ(t), so that the robot arm end effector position state r(t) is consistent with the given robot arm motion planning ideal trajectory r d (t) is consistent, that is, r d (t) = r(t). In contrast, the mapping from joint angle space to joint angular velocity space is an affine, so the Jacobian matrix of the robot arm kinematic transfer function f(θ(t)) can be calculated: And establish the robot arm motion planning model in the joint angular velocity space, as shown below

[0093]

[0094] Without loss of generality, the joint angular velocity space energy is used as the objective function Wherein, the superscript T represents the vector transpose operation; Λ is a preset positive definite symmetric matrix, which is used to adjust the weight of each joint angular velocity contribution to the joint angular velocity space energy; considering the actual physical limitations, the angular velocity of each joint is limited to a certain bounded range where η - and η + Represents the minimum and maximum angular velocity of each individual joint. In summary, the multi-constraint robot arm motion planning model can be expressed as follows:

[0095]

[0096] Specifically, in this embodiment, the experimental simulation is performed by using MATLAB software and a seven-degree-of-freedom robotic arm to specify the same path and repeat motion generation as an example;

[0097] Initialize the relevant parameters of the trust region projection neural network, including the initial time t=0, the end time t f =10s (seconds), trust region range threshold The first neural network adjusts the parameter σ tr =0.001, the second neural network adjustment parameter σ g =0.05; Get the initial position r(0) and initial speed of the end effector of the robot arm The initial angles of each joint of the robot arm are θ(0), and the initial angular velocity is The ideal trajectory position r of the robot arm end effector at the initial moment d (0), the ideal trajectory speed at the initial moment The Jacobian matrix J(θ(0)) of the robot arm at the initial moment is expressed as follows:

[0098]

[0099] 2. By introducing the Lagrange multiplier λ(t), the multi-constrained robot arm motion planning model is transformed into an unconstrained robot arm motion planning model As shown below:

[0100]

[0101] Using existing technology, an unconstrained robot arm motion planning model can be obtained The first-order gradient vector Unconstrained Robot Arm Motion Planning Model The second-order gradient matrix Λ;

[0102] Furthermore, the corresponding trust region subproblem of the unconstrained robotic arm motion planning model can be constructed, which is specifically stated as follows:

[0103]

[0104] Here, the symbol ||·|| represents any vector norm, usually the 2 norm, d(t) is the trust region direction variable to be determined, and Δ(t) is the trust region radius.

[0105] A3. Existing technologies and methods can be used to solve the trust region subproblem of the unconstrained robot arm motion planning model, and the trust region direction is d tr (t); then according to the trust region direction d tr (t), and the projection trust region direction is obtained, which is specifically expressed as follows:

[0106]

[0107] in, Represents the projection operator, which limits the angular velocity of each joint after the trust region direction update to a certain bounded range

[0108] Calculate the projected trust region direction The actual descent amount of the generated unconstrained robot arm motion planning model is specifically expressed as follows:

[0109]

[0110] Furthermore, the predicted decrease of the trust region subproblem of the robot arm motion planning model generated by the projection trust region direction is calculated, which is specifically expressed as follows:

[0111]

[0112] Furthermore, some judgment thresholds are set 0<γ2<1<γ1;

[0113] Further, the calculation is based on the projection trust region direction The ratio between the actual drop generated and the predicted drop

[0114] Furthermore, the ratio between the actual drop and the predicted drop is compared with the judgment threshold; there are two cases:

[0115] 1) If The first neural network is constructed to solve the robot arm motion planning model, which is specifically described as follows:

[0116]

[0117] Among them, 0<σ tr <1 is the adjustment parameter of the convergence speed of the first neural network. Through this design idea, the equilibrium point of the first neural network is the same as the optimal solution of the robot arm motion planning model. Then, the trust region radius is updated in the following way:

[0118]

[0119] Then, using existing technology, the first neural network is solved within the sampling interval τ to obtain the joint angular velocity at the next moment Obtain the Jacobian matrix J(t+τ) of the robot arm kinematic transfer function at the next moment; obtain the corresponding robot arm end effector position state through the robot arm motion planning model

[0120] 2) If It shows that the trust region direction cannot make the robot arm motion planning model fully descend, so the projected gradient descent direction is selected as the new update direction. The specific expression is as follows:

[0121]

[0122] in, Represents the projected gradient operator, which limits the angular velocity of each joint after the projected gradient descent direction is updated to a certain bounded range

[0123] Calculate the actual descent amount of the robot arm motion planning model generated by the projected gradient descent direction. The specific expression is as follows:

[0124]

[0125] Furthermore, the predicted descent amount of the trust region subproblem of the robot arm motion planning model generated by the projected gradient descent direction is calculated, which is specifically expressed as follows:

[0126]

[0127] Further, the calculation is carried out by projecting the gradient descent direction The ratio between the actual drop generated and the predicted drop

[0128] Furthermore, if The second neural network is constructed to solve the robot arm motion planning model, which is specifically described as follows:

[0129]

[0130] Among them, 0<σ g <1 is the adjustment parameter of the convergence speed of the projected gradient neural network. Then, the trust region radius is updated as follows:

[0131]

[0132] in, is the preset threshold, is the second preset trust region radius parameter,

[0133] Then, using existing technology, the second neural network is solved to obtain the joint angular velocity at the next moment within the sampling interval τ. Obtain the Jacobian matrix J(t+τ) of the robot arm kinematic transfer function at the next moment; obtain the corresponding robot arm end effector position state through the robot arm motion planning model

[0134] like In the sampling interval τ, no calculation is performed, and the joint angular velocity at the current moment is output as the joint angular velocity at the next moment And reduce the trust region radius Δ = γ1Δ;

[0135] A4. Determine the current time t and the end time t fThe size of f , jump to A3; otherwise, stop the calculation and obtain the position state sequence of the end effector of the robot arm {r(t), r(t+τ), r(t+2τ), …}, which is transmitted to the lower computer controller to realize the robot arm motion planning task.

[0136] The experimental results of the present invention are as follows Figure 2-6 As shown:

[0137] Figure 2 The motion trajectory of the end effector of the robot arm generated by the method of the present invention, wherein the solid line represents the motion state of the 7 joints, and the dotted line represents the generated motion trajectory. It can be seen from the figure that the end effector of the robot arm successfully executes the motion generation of the specified trajectory clover, and the motion of each joint angle is reasonable, which reflects the feasibility and efficiency of the present invention;

[0138] Figure 3 The three-dimensional spatial position state of the end effector of the robot arm generated by the method of the present invention, wherein the solid line represents the position state in the x-axis direction, the dotted line represents the position state in the y-axis direction, and the dashed line represents the position state in the z-axis direction. The position state trajectories in the three directions are continuous and smooth, which reflects the feasibility and rationality of the present invention.

[0139] Figure 4 The error state between the three-dimensional spatial position of the end effector of the robot arm and the expected trajectory generated by the method of the present invention, where the solid line represents the error state in the x-axis direction, the dashed line represents the error state in the y-axis direction, and the dotted line represents the error state in the z-axis direction. The error states in the x-axis direction and the z-axis direction do not exceed 2×10 -4 The error in the y-axis direction does not exceed 5×10 -4 Meters, which reflects the superiority and accuracy of the robot arm motion planning of the present invention;

[0140] Figure 5 and Figure 6 The angle states and angular velocity states of each joint of the robotic arm generated by the method of the present invention are continuous and smooth, and the maximum joint angle does not exceed 4 rad, the minimum joint angle does not exceed -3 rad, the maximum joint angular velocity does not exceed 0.4 rad / s, and the minimum joint angular velocity does not exceed -0.6 rad / s, which reflects the effectiveness of the robotic arm motion planning of the present invention on the constraints on each joint angle and angular velocity.

[0141] In a second aspect, the present invention provides a robot arm motion planning system, comprising: a memory and a processor, wherein the memory stores a computer program, and the processor executes the robot arm motion planning method provided in the first aspect of the present invention when executing the computer program.

[0142] The related technical solution is the same as the robot arm motion planning method provided in the first aspect of the present invention, and will not be described in detail here.

[0143] In a third aspect, the present invention further provides a computer-readable storage medium, which includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute the robot arm motion planning method provided in the first aspect of the present invention.

[0144] The related technical solution is the same as the robot arm motion planning method provided in the first aspect of the present invention, and will not be described in detail here.

[0145] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot arm motion planning method, characterized in that: include: S1. Build a motion planning model for the robot and construct the corresponding trust region sub-problem; The cost functions of the motion planning model and the trust region subproblem are L(x(t)) and q(d tr (t)); Initialize the robot motion decision variables x(t) at the current time t and the trust region radius Δ(t) of the trust region subproblem; S2. Solve the trust region subproblem according to the motion state variables of the motion planning model at time t, and obtain the trust region direction d at time t tr (t); S3, x(t)+d tr (t) is mapped to Ω = [η - ,η + ] range, and obtain the predicted motion decision variable x in the trust region direction tr (t); x tr The difference between (t) and x(t) is taken as the projection trust region direction at time t η - and η + are the minimum and maximum values ​​of the robot arm motion decision variables, respectively; S4. Calculate the predicted drop of the trust region subproblem in the projected trust region direction and the actual descent of the motion planning model S5. Calculate the ratio like Then x(t) is The direction is updated and based on ρ tr (t) Update Δ(t) and go to S7; otherwise, go to S6; S6. Calculate the gradient d of L(x(t)) with respect to x(t) g (t), x(t)-d g (t) is mapped to Ω = [η - ,η + ] range, and obtain the predicted motion decision variable x in the direction of gradient descent g (t); x g The difference between (t) and x(t) is used as the projected gradient descent direction at time t Calculate the predicted descent amount for the trust region subproblem in the direction of projected gradient descent and the actual descent of the motion planning model Calculate the ratio like Then x(t) is The direction is updated and based on ρ g (t) Update Δ(t) and go to S7; otherwise, reduce Δ(t) and go to S7; S7, let t = t + τ, update the motion state variables of the motion planning model at time t, and go to S2, until t reaches the preset time, output the robot arm motion decision variables at each time to realize the robot arm motion planning; Wherein, τ is the preset time interval; and are preset thresholds, and 2. The robot arm motion planning method according to claim 1, characterized in that: In the S5, the tr (t) Update Δ(t) includes: when When , keep Δ(t) unchanged; when When , increase Δ(t); where, is the preset threshold, 3. The robot arm motion planning method according to claim 1, characterized in that: In the S6, the g (t) Update Δ(t) includes: when When , reduce Δ(t); when When , keep Δ(t) unchanged; when When , increase Δ(t); where, and These are preset thresholds.

4. The robot arm motion planning method according to claim 1, characterized in that: In S5, according to the formula x(t) along The direction is updated; among them, σ tr is the first preset adjustment parameter, 0<σ tr <1.

5. The robot arm motion planning method according to claim 1, characterized in that: In S6, according to the formula x(t) along The direction is updated; among them, σ g is the second preset adjustment parameter, 0<σ g <1.

6. The robot arm motion planning method according to claim 1, characterized in that: The robot arm motion decision variables are: the joint angle state, joint angular velocity state or angular acceleration state of the robot arm.

7. The robot arm motion planning method according to any one of claims 1 to 6, characterized in that: The motion planning model is: min x(t) L(x(t))=x T (t)Λx(t) x(t)∈Ω Wherein, Λ is a preset positive definite symmetric matrix; J(t) is the Jacobian matrix of the motion planning model; when x(t) is the joint angle state of the robot arm, y d (t) is the position state of the end effector of the robot arm; when x(t) is the joint angular velocity state of the robot arm, y d (t) is the velocity state of the end effector of the robot arm; when x(t) is the joint angular acceleration state of the robot arm, y d (t) is the acceleration state of the end effector of the robot arm.

8. The robot arm motion planning method according to any one of claims 1 to 6, characterized in that: The motion planning model is: Wherein, Λ is a preset positive definite symmetric matrix; λ(t) is a Lagrange multiplier; J(t) is the Jacobian matrix of the motion planning model; when x(t) is the joint angle state of the robot arm, y d (t) is the position state of the end effector of the robot arm; when x(t) is the joint angular velocity state of the robot arm, y d (t) is the velocity state of the end effector of the robot arm; when x(t) is the joint angular acceleration state of the robot arm, y d (t) is the acceleration state of the end effector of the robot arm.

9. A robot arm motion planning system, characterized in that: include: A memory and a processor, wherein the memory stores a computer program, and the processor executes the robot arm motion planning method according to any one of claims 1 to 8 when executing the computer program.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute the robot arm motion planning method according to any one of claims 1 to 8.

Images