Motion constraint based robot arm dragging method, electronic device and storage medium

Abstract

The application discloses a mechanical arm dragging method based on motion constraint, an electronic device and a storage medium. The mechanical arm dragging method constructs a joint external force observer of a mechanical arm to obtain joint external moments of the mechanical arm and workspace external forces. Meanwhile, an operation index and a position constraint threshold are used to constrain the motion state of the mechanical arm to obtain first workspace virtual forces and second workspace virtual forces. The desired position can be calculated and solved based on the workspace external forces, the first workspace virtual forces and the second workspace virtual forces. The motion of the mechanical arm is controlled according to the desired position, so that the situation that the joint of the mechanical arm is overspeed or collision occurs in the dragging process can be avoided, and the safety of the mechanical arm in the dragging process is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of motion control of a mechanical arm, and particularly relates to a mechanical arm dragging method based on motion constraints, an electronic device and a storage medium. BACKGROUND

[0002] The drag teaching, also known as manual teaching, is a teaching programming work of a mechanical arm completed by an operator through manual dragging, and then the mechanical arm can repeatedly execute the teaching trajectory according to an external signal trigger. The mechanical arm dragging function generally refers to that an operator can change the pose of the robot through manual dragging, which is a kind of human-computer interaction means of the mechanical arm.

[0003] At present, technical solutions for realizing the mechanical arm dragging function mainly include two categories: torque control and position control. First, in the torque control solution, the dynamic model obtained through identification is used to calculate the gravity compensation value of each joint, and the value is sent to the servo system of each joint in torque mode, so that the robot is in a “hovering” state and has the ability of manual dragging. Second, in the position control solution, the six-axis force sensor at the end of the robot is used to obtain the external force in the workspace, a control law about the external force and the position command is constructed, and the position command value in the workspace is calculated and sent to the robot position controller, so that the end of the robot follows the external force to move and realizes the dragging function. However, the current technical solutions do not consider the motion constraints in the workspace during the dragging process, so that the robot may collide with the surrounding environment or reach a singular pose at high speed during the dragging, which has safety risks. SUMMARY

[0004] The present application provides a mechanical arm dragging method based on motion constraints, an electronic device and a storage medium to avoid the situation of joint overspeed or collision of the mechanical arm during the dragging process, and improve the safety of the mechanical arm during the dragging process.

[0005] The first aspect of the present application provides a mechanical arm dragging method based on motion constraints, which comprises the following steps:

[0006] Obtaining a dynamic model of the mechanical arm, and constructing a joint external force observer of the mechanical arm based on the dynamic model;

[0007] Inputting the current state of the mechanical arm and the measured torque obtained by the joint torque sensor into the joint external force observer to obtain the joint external torque of the mechanical arm;

[0008] Obtaining the external force in the workspace of the mechanical arm based on the joint external torque;

[0009] obtaining a joint angle vector based on a current state of the robot arm, determining an operation index according to the joint angle vector, and determining a virtual force gradient vector according to a change of the operation index caused by translation and rotation of the robot arm end in each axis of the Cartesian space;

[0010] obtaining a first workspace virtual force of the robot arm with the operation index and the virtual force gradient vector;

[0011] obtaining an end effector position based on the current state of the robot arm;

[0012] constraining the end effector position by using a position constraint key threshold and a position constraint enable threshold to obtain a second workspace virtual force of the robot arm with a workspace position constraint;

[0013] determining a workspace resultant force of the robot arm by using the workspace external force, the first workspace virtual force and the second workspace virtual force;

[0014] solving a desired position of the robot arm according to the workspace resultant force, and controlling the robot arm to move according to the desired position.

[0015] Optionally, the workspace external force of the robot arm is obtained based on the joint external torque, and the obtaining includes:

[0016] translating the joint external torque into the workspace external force of the robot arm by using a Jacobian matrix transpose pseudo-inverse matrix.

[0017] Optionally, the operation index is determined according to the joint angle vector, and the determining includes:

[0018] obtaining a translation Jacobian matrix and a rotation Jacobian matrix by using the Jacobian matrix and the joint angle vector, wherein the translation Jacobian matrix and the rotation Jacobian matrix are used to describe a relationship between a joint angular velocity vector and a Cartesian space velocity vector;

[0019] determining the operation index according to the rotation Jacobian matrix and a pseudo-inverse matrix thereof, and the translation Jacobian matrix and a transpose matrix thereof.

[0020] Optionally, the virtual force gradient vector is determined according to a change of the operation index caused by translation and rotation of the robot arm end in each axis of the Cartesian space, and the determining includes:

[0021] obtaining a corresponding joint angle value of the robot arm end after moving in each axis of the Cartesian space for a control period according to the joint angle vector, a control period and a velocity vector of each axis of the Cartesian space;

[0022] determining a change of the operation index caused by movement of the robot arm end by using an expression of the operation index and the joint angle vector and the Cartesian space velocity vector;

[0023] The gradient elements of the axes are determined according to the operation index changes of the axes, and a virtual force gradient vector is composed, wherein the gradient elements are projections of the gradient vector of the virtual force in the translation direction or the rotation direction of the axes.

[0024] Optionally, the gradient elements of the axes are determined according to the operation index changes of the axes, and the gradient elements are determined by:

[0025] The operation index positive change and the operation index negative change of the axes are obtained, wherein the operation index positive change is a difference between an operation index corresponding to a joint angle value determined by a positive velocity vector and an operation index corresponding to a current joint angle vector, and the operation index negative change is a difference between an operation index corresponding to a joint angle value determined by a negative velocity vector and the operation index corresponding to the current joint angle vector.

[0026] When the operation index positive change of the axes is greater than the operation index negative change and the operation index positive change is a positive value, the absolute value of the operation index positive change is determined as the gradient element.

[0027] When the operation index positive change of the axes is less than or equal to the operation index negative change and the operation index negative change is a positive value, the negative of the absolute value of the operation index negative change is determined as the gradient element.

[0028] When the operation index corresponding to the joint angle value determined by the positive velocity vector of the axes and the operation index corresponding to the joint angle value determined by the negative velocity vector are less than or equal to the operation index corresponding to the joint angle vector, the gradient element is set to a first preset value.

[0029] Optionally, the first working space virtual force of the manipulator with the operation index constraint is obtained by using the operation index and the virtual force gradient vector, and the first working space virtual force is obtained by:

[0030] When the operation index corresponding to the joint angle vector is less than or equal to a virtual force enabling threshold, a virtual force scaling factor is determined by using a gain factor, the operation index corresponding to the joint angle vector, the virtual force enabling threshold and a virtual force limiting threshold.

[0031] When the operation index corresponding to the joint angle vector is greater than the virtual force enabling threshold, the virtual force scaling factor is set to a second preset value.

[0032] The first working space virtual force of the manipulator with the operation index constraint is obtained by using the virtual force scaling factor and the virtual force gradient vector.

[0033] Optionally, the second working space virtual force of the manipulator with the working space position constraint is obtained by using the position constraint key threshold and the position constraint enabling threshold to constrain the end effector position, and the second working space virtual force is obtained by:

[0034] when the axis component of the end effector position is greater than or equal to the upper bound of the axis position constraint enabling threshold and less than or equal to the upper bound of the axis position constraint critical threshold, the axis workspace virtual force of the end effector position is determined according to the axis component of the end effector position, the upper bound of the axis position constraint enabling threshold and the upper bound of the axis position constraint critical threshold;

[0035] when the axis component of the end effector position is greater than the lower bound of the axis position constraint enabling threshold and less than the upper bound of the axis position constraint enabling threshold, the axis workspace virtual force of the end effector position is set to a third preset value;

[0036] when the axis component of the end effector position is greater than or equal to the lower bound of the axis position constraint critical threshold and less than or equal to the lower bound of the axis position constraint enabling threshold, the axis workspace virtual force of the end effector position is determined according to the axis component of the end effector position, the lower bound of the axis position constraint critical threshold and the lower bound of the axis position constraint enabling threshold;

[0037] the second workspace virtual force of the position constraint of the manipulator is determined by using the axis workspace virtual force of the end effector position.

[0038] Optionally, the desired position of the manipulator is obtained according to the workspace resultant force, including:

[0039] the drag control rate of the manipulator is obtained according to the dynamic model of the manipulator;

[0040] the discrete expression form of the desired velocity and acceleration is obtained based on the drag control rate based on the zero-order holder principle, and the discrete expression of the desired velocity is obtained based on the discrete expression form of the desired acceleration;

[0041] the desired position of the manipulator is obtained by discretely integrating the discrete expression of the desired velocity according to the workspace resultant force.

[0042] The second aspect of the present application provides an electronic device, which includes a memory and a processor coupled with each other, and the processor is configured to execute program instructions stored in the memory to implement the manipulator dragging method as described above.

[0043] The third aspect of the present application provides a computer readable storage medium, which stores a computer program, and the computer program can be executed by a processor to implement the manipulator dragging method as described above.

[0044] The beneficial effects of the present application are: different from the prior art, the present application constructs a joint external force observer of the mechanical arm, obtains the joint external moment and the workspace external force of the mechanical arm based on the joint external force observer; the current state of the mechanical arm is constrained by using the operation index to obtain the first workspace virtual force, and the position of the end effector of the mechanical arm is constrained by using the position constraint key threshold and the position constraint enable threshold to obtain the second workspace virtual force; the desired position of the mechanical arm can be calculated and solved based on the workspace external force, the first workspace virtual force and the second workspace virtual force, the movement of the mechanical arm is controlled according to the desired position, the situation of joint overspeed or collision of the mechanical arm in the dragging process can be avoided, and the safety of the mechanical arm in the dragging process is improved.

[0045] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, but not limiting the present application. BRIEF DESCRIPTION OF DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0047] Figure 1 is a flowchart of an embodiment of the present application based on motion constraint mechanical arm dragging method;

[0048] Figure 2 is Figure 1 the first specific flowchart of step S14 in

[0049] Figure 3 is Figure 1 the second specific flowchart of step S14 in

[0050] Figure 4 is Figure 3 the specific flowchart of step S145 in

[0051] Figure 5 is Figure 1 the specific flowchart of step S15 in

[0052] Figure 6 is Figure 1 the specific flowchart of step S17 in

[0053] Figure 7 is Figure 1 the specific flowchart of step S19 in

[0054] Figure 8is a frame schematic diagram of an embodiment of an electronic device of the present application.

[0055] Figure 9 is a frame schematic diagram of an embodiment of a computer readable storage medium of the present application. DETAILED DESCRIPTION

[0056] In order for those skilled in the art to better understand the technical solutions of the present application, the motion-constraint-based robot arm dragging method, electronic device and storage medium provided by the present application will be described in detail below with reference to the accompanying drawings and specific embodiments. It can be understood that the described embodiments are only a part of the embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0057] The terms "first", "second", and the like in the present application are used to distinguish different objects, not to describe a specific order. In addition, the terms "include" and "have" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but can optionally include steps or units not listed, or can optionally include other steps or units inherent to the process, method, product or device.

[0058] The present application provides a motion-constraint-based robot arm dragging method to avoid joint overspeed or collision during the dragging process of the robot arm, and to improve the safety of the robot arm during the dragging process. Please refer to Figure 1 , Figure 1 is a flow schematic diagram of an embodiment of the motion-constraint-based robot arm dragging method of the present application.

[0059] The execution subject of the robot arm dragging method of the present application can be a robot arm dragging device, for example, the robot arm dragging method can be executed by a terminal device or a server or other processing device, wherein the robot arm dragging device can include a user equipment (User Equipment, UE), a mobile device, a user terminal, a terminal, a cellular phone, a wireless phone, a personal digital assistant (Personal Digital Assistant, PDA), a handheld device, a computing device, a vehicle-mounted device, a wearable device, etc. In some possible implementation manners, the robot arm dragging method can be realized by a processor calling computer readable instructions stored in a memory.

[0060] Specifically, the robot arm dragging method of the embodiments of the present application can include the following steps:

[0061] Step S11: Obtain the dynamics model of the robot arm, and construct a joint external force observer of the robot arm based on the dynamics model.

[0062] Generally, the inverse dynamics model of an n-degree-of-freedom robot under external force can be described by the following expression:

[0063]

[0064] wherein τ is the joint torque of the robot; M(q) is the inertia matrix; is the Coriolis force and centrifugal force term; G(q) is the gravity term; τ f is the friction force term; τ ext is the joint external torque; q, are the joint angle, angular velocity, and angular acceleration, respectively.

[0065] In this embodiment, the Lagrange method is used to obtain the character expression of the dynamics model of the robot arm, and the parameter set to be identified and the observation matrix are extracted from the character expression of the dynamics model.

[0066] Meanwhile, the optimal trajectory is calculated by constructing an optimization problem, wherein the optimal trajectory is the optimal path of the movement of the robot arm. Further, the movement of the robot arm is driven using the optimal trajectory, and the joint movement data and joint torque sensor data of the robot arm are recorded. Further, the joint movement data and joint torque sensor data obtained are processed using the least square method in matrix form, and the numerical solution of the dynamics parameter set is obtained.

[0067] The numerical solution of the dynamics parameter set calculated is brought into the character expression of the dynamics model, and the independent inertia matrix, Coriolis and centrifugal force matrix, gravity matrix, and friction torque matrix are obtained by the parameter zeroing method.

[0068] The state space equation eliminating the joint angular acceleration is obtained using the generalized momentum principle, and the joint external torque observer of the robot arm is constructed.

[0069] Specifically, the generalized momentum can be expressed as follows:

[0070]

[0071] wherein p is the generalized momentum.

[0072] The differential expression of the generalized momentum with respect to time is:

[0073]

[0074] Substituting the above formula into the inverse dynamics model, the following formula is obtained:

[0075]

[0076] If the Coriolis matrix is expressed by Christoffel symbols, then:

[0077]

[0078] Therefore, the above formula can be rewritten as:

[0079]

[0080] To use the above model in combination with the observer, the state space equation is constructed according to the above formula.

[0081]

[0082] where I n is an n-dimensional identity matrix; 0 n is an n-dimensional zero matrix; ω m is an uncertainty term related to the model; ω τ is an uncertainty term related to the external torque; and u is:

[0083]

[0084] where, is a model obtained by parameter identification.

[0085] The joint external torque observer can be constructed using the above state space equation.

[0086] Step S12: input the current state of the robot arm and the measured torque obtained by the joint torque sensor into the joint external force observer to obtain the joint external torque of the robot arm.

[0087] wherein the current state of the robot arm, i.e. the joint vector, and the measured torque obtained by the joint torque sensor are input into the joint external force observer constructed according to step S11, so as to obtain the joint external torque of the robot arm.

[0088] Step S13: obtain the workspace external force of the robot arm based on the joint external torque.

[0089] wherein the torque conversion formula is provided, and the joint external torque obtained in step S12 is input into the torque conversion formula, so as to calculate the workspace external force of the robot arm.

[0090] Specifically, the joint external torque can be converted into the workspace external force of the robot arm by using the transpose pseudo-inverse matrix of the Jacobian matrix. The torque conversion formula in the embodiment is specifically shown in formula (1):

[0091] F est = J + τ est (1)

[0092] wherein F est is the external force of the workspace of the robot arm, J + is the transpose pseudo-inverse matrix of the Jacobian matrix, τ est is the joint external force moment obtained by the joint external force observer.

[0093] Step S14: obtaining a joint angle vector based on the current state of the robot arm, determining an operation index according to the joint angle vector, and determining a virtual force gradient vector according to the change of the operation index caused by the translation and rotation of the end of the robot arm in the Cartesian space.

[0094] wherein the joint angle vector can be obtained based on the current state of the robot arm, and the operation index is further determined according to the joint angle vector, and the virtual force gradient vector is determined according to the change of the operation index caused by the translation and rotation of the end of the robot arm in the Cartesian space.

[0095] Specifically, the embodiment can be calculated based on the Jacobian matrix, wherein the Jacobian matrix is specifically shown in formula (2):

[0096]

[0097] wherein J is the Jacobian matrix, q is the joint angle vector, J T (q) is a 3×n matrix, and is specifically a translation Jacobian matrix, J R (q) is a 3×n matrix, and is specifically a rotation Jacobian matrix.

[0098] Further, the process of specifically determining the operation index according to the joint angle vector can be further referred to Figure 2 , Figure 2 is Figure 1 the first specific flowchart of step S14 in the embodiment. Specifically, the following steps are included:

[0099] Step S141: obtaining a translation Jacobian matrix and a rotation Jacobian matrix by using the Jacobian matrix and the joint angle vector.

[0100] wherein the translation Jacobian matrix obtained by the embodiment is J T (q), and the rotation Jacobian matrix obtained by the embodiment is J R (q). Wherein the translation Jacobian matrix J T (q) and the rotation Jacobian matrix J R (q) are used to describe the relationship between the joint angular velocity vector and the Cartesian space velocity vector.

[0101] Step S142: Determine the operation index according to the rotation Jacobian matrix and its pseudo-inverse matrix, the translation Jacobian matrix and its transpose matrix.

[0102] In this embodiment, the operation index can be calculated according to the rotation Jacobian matrix and its pseudo-inverse matrix, the translation Jacobian matrix and its transpose matrix, and the specific calculation formula can be shown in formula (3):

[0103]

[0104] wherein ω(q) is the operation index, denotes the pseudo-inverse matrix of the rotation Jacobian matrix, denotes the pseudo-inverse matrix of the translation Jacobian matrix, and I denotes the unit matrix.

[0105] Further, the process of determining the virtual force gradient vector according to the operation index change caused by the translation and rotation of the mechanical arm end in the Cartesian space will be described in detail with reference to Figure 3 , Figure 3 is Figure 1 the second specific flowchart of step S14 in FIG. 14. Specifically, the following steps are included:

[0106] Step S143: Obtain the joint angle value corresponding to the movement of the mechanical arm end in the Cartesian space after one control period according to the joint angle vector, the control period and the velocity vector of each axis in the Cartesian space.

[0107] In this embodiment, the joint angle calculation formula is preset, and the joint angle vector, the control period and the velocity vector of each axis in the Cartesian space are input into the joint angle calculation formula, so that the joint angle value corresponding to the movement of the mechanical arm end in the Cartesian space after one control period can be calculated, and the specific calculation formula can be shown in formula (4):

[0108] q′=q0+J + V′Δt (4)

[0109] wherein q` is the joint angle value of the mechanical arm end after moving for one control period, q0 is the joint angle value of the mechanical arm before entering the singularity avoidance algorithm, and can be considered as the joint angle vector of the current mechanical arm end; V` is the Cartesian space velocity vector, and Δt is the control period.

[0110] Step S144: Determine the operation index change caused by the movement of the mechanical arm end according to the joint angle vector and the Cartesian space velocity vector by using the expression of the operation index.

[0111] Wherein, the expression of the operation index of the embodiment is formula (3), and the corresponding operation indexes ω(q') and ω(q0) can be calculated by bringing q' and q0 into formula (3), and the difference between the two can be obtained as the operation index change Δω caused by the motion of the end of the robot arm.

[0112] Step S145: According to the operation index change of each axis, the gradient elements of each axis are determined, and a virtual force gradient vector is composed.

[0113] Wherein, the joint angle values of the end of the robot arm after the different axes in the Cartesian space are translated or rotated at a speed vector for one control period are calculated according to formula (4), and then the operation index changes of each axis are calculated according to the joint angle values, the gradient elements of each axis are determined, and a virtual force gradient vector is composed. Wherein, the gradient elements are the projections of the gradient vector of the virtual force in the translation direction or the rotation direction of each axis.

[0114] Specifically, the speed vector V' of the different axes in the translation direction or the rotation direction can be as follows:

[0115] V′ x+ =[+1 0 0 0 0 0] T

[0116] V′ x- =[-1 0 0 0 0 0] T

[0117] V′ y+ =[0 +1 0 0 0 0] T

[0118] V′ y- =[0 -1 0 0 0 0] T

[0119] V′ z+ =[0 0 +1 0 0 0] T

[0120] V′ z- =[0 0 -1 0 0 0] T

[0121] V′ Rx+ =[0 0 0 +1 0 0] T

[0122] V′ Rx- =[0 0 0 -1 0 0] T

[0123] V′ Ry+ =[0 0 0 0 +1 0] T

[0124] V′ Ry- = [0 0 0 0 -1 0] T

[0125] V′ Rz+ = [0 0 0 0 0 +1] T

[0126] V′ Rz- = [0 0 0 0 0 -1] T

[0127] Specifically, V` can be V` + or V` - , wherein V` + represents a positive velocity vector, i.e., the robot arm moves at a unit positive velocity for one control period, V` - represents a negative velocity vector, i.e., the robot arm moves at a unit negative velocity for one control period. Wherein, V` x is the velocity vector of the robot arm moving in the x-axis translation direction for one control period, V` y is the velocity vector of the robot arm moving in the y-axis translation direction for one control period, V` z is the velocity vector of the robot arm moving in the z-axis translation direction for one control period, V` Rx is the velocity vector of the robot arm moving in the x-axis rotation direction for one control period, V` Ry is the velocity vector of the robot arm moving in the y-axis rotation direction for one control period, V` Rz is the velocity vector of the robot arm moving in the z-axis rotation direction for one control period.

[0128] Based on the above-mentioned multiple different velocity vectors, the corresponding operation index changes can be calculated. Taking the movement of the robot arm end in the x-axis translation direction for one control period as an example, the corresponding gradient elements can be shown in formula (5):

[0129]

[0130] Specifically, by integrating the gradient elements A x corresponding to the x-axis translation direction, the gradient elements A y corresponding to the y-axis translation direction, the gradient elements A z corresponding to the z-axis translation direction, the gradient elements A Rx corresponding to the x-axis rotation direction, the gradient elements A Ry corresponding to the y-axis rotation direction, and the gradient elements A Rz corresponding to the z-axis rotation direction, the virtual force gradient vector can be obtained, which is specifically shown in formula (6):

[0131] A(q) = [Ax A y A z A Rx A Ry A Rz ] T (6)

[0132] Optionally, the process of specifically composing the virtual force gradient vector please continue to refer to Figure 4 , Figure 4 is Figure 3 the specific flowchart of step S145 in the method. Specifically, it includes the following steps:

[0133] Step S1451: Obtain the operation index positive change and the operation index negative change of each axis.

[0134] The operation index positive change is the difference between the operation index corresponding to the joint angle value determined by the positive velocity vector and the operation index corresponding to the current joint angle vector. The operation index negative change is the difference between the operation index corresponding to the joint angle value determined by the negative velocity vector and the operation index corresponding to the current joint angle vector.

[0135] Specifically, taking the movement of the mechanical arm end in the x-axis translation direction for one control period as an example, as shown in formula (5), the operation index positive change is the difference between the operation index corresponding to the joint angle value determined by the positive velocity vector and the operation index corresponding to the current joint angle vector, that is, Δω + The operation index negative change is the difference between the operation index corresponding to the joint angle value determined by the negative velocity vector and the operation index corresponding to the current joint angle vector, that is, Δω - .

[0136] According to the size relationship between the absolute value of the operation index positive change and the operation index negative change, one of step S1452, step S1453 or step S1454 is executed.

[0137] Step S1452: When the operation index positive change of each axis is greater than the operation index negative change, and the operation index positive change is positive, the absolute value of the operation index positive change is determined as the gradient element.

[0138] Step S1453: When the operation index positive change of each axis is less than or equal to the operation index negative change, and the operation index negative change is positive, the negative of the absolute value of the operation index negative change is determined as the gradient element.

[0139] Step S1454: When the operation index corresponding to the joint angle value determined by the positive velocity vector of each axis, and the operation index corresponding to the joint angle value determined by the negative velocity vector are all less than or equal to the operation index corresponding to the current joint angle vector, the gradient element is set to a first preset value.

[0140] In the embodiment, the first preset value is specifically zero.

[0141] Step S15: obtaining the first workspace virtual force of the manipulator with the introduction of the operation index constraint by using the operation index and the virtual force gradient vector.

[0142] In the embodiment, the first workspace virtual force of the manipulator with the introduction of the operation index constraint can be calculated based on the virtual force gradient vector A(q) determined by formula (6) and the operation index ω(q) determined by formula (3).

[0143] Optionally, for details of the process of obtaining the first workspace virtual force, please refer to Figure 5 , Figure 5 is Figure 1 the specific flowchart of step S15 in

[0144] Step S151: when the operation index corresponding to the joint angle vector is less than or equal to the virtual force enabling threshold, determining the virtual force scaling factor by using the gain factor, the operation index corresponding to the joint angle vector, the virtual force enabling threshold and the virtual force limiting threshold.

[0145] In the embodiment, the expression of the virtual force scaling factor can be as shown in formula (7):

[0146]

[0147] In the embodiment, the expression of the virtual force scaling factor can be as shown in formula (7): th In the embodiment, the expression of the virtual force scaling factor can be as shown in formula (7): cr

[0148] Step S152: when the operation index corresponding to the joint angle vector is greater than the virtual force enabling threshold, setting the virtual force scaling factor as a second preset value.

[0149] In the embodiment, the second preset value is specifically zero.

[0150] Step S153: obtaining the first workspace virtual force of the manipulator with the introduction of the operation index constraint by using the virtual force scaling factor and the virtual force gradient vector.

[0151] In the embodiment, the first workspace virtual force of the manipulator with the introduction of the operation index constraint can be calculated based on the virtual force scaling factor ξ(ω) obtained by formula (7) and the virtual force gradient vector A(q) determined by formula (6). vper vper In the embodiment, the expression of the first workspace virtual force F

[0152] ​​F vper =ξ(ω)A(q) (8)

[0153] Step S16: Obtain the position of the end effector based on the current state of the robotic arm.

[0154] In this embodiment, the position of the end effector can be obtained based on the current state of the robotic arm. Specifically, it can be calculated based on the forward kinematics model of the robotic arm, and the specific calculation formula is shown in formula (9):

[0155] p = f(q) (9).

[0156] Where p is the position of the end effector, f is the forward kinematics model of the robotic arm, and q is the joint angle vector, which represents the current state of the robotic arm.

[0157] Step S17: Constrain the position of the end effector using the position constraint key threshold and the position constraint enable threshold to obtain the second workspace virtual force of the robot arm that introduces workspace position constraints.

[0158] Taking the x-axis as an example, the expression for the virtual force in the second workspace of this embodiment can be shown in formula (10):

[0159]

[0160] Where Fvposx is the second virtual force in the workspace corresponding to the x-axis position constraint, p xcr + and p xcr - These represent the upper and lower bounds of the critical thresholds for the x-axis position constraint in the workspace, respectively. xth + and p xth - These represent the upper and lower bounds of the workspace x-axis position constraint enable threshold, respectively. p x Let λ represent the x-axis position component of the end effector in Cartesian space, and let λ represent the computation factor.

[0161] Specifically, the virtual force F of the second workspace can be obtained by combining the virtual forces corresponding to the positional constraints of the x-axis, y-axis, and z-axis of the workspace. vpos Specifically, as shown in formula (11):

[0162] F vpos =[F vposx F vposy F vposz 0 0 0] T (11)

[0163] Optionally, please refer to the following for details on the process of obtaining virtual forces in the second workspace. Figure 6 , Figure 6 yes Figure 1A specific flowchart of step S17 is shown. Specifically, the following steps are included:

[0164] Step S171: When the axis component of the end effector position is greater than or equal to the upper limit of the axis position constraint enabling threshold and less than or equal to the upper limit of the axis position constraint critical threshold, the axis workspace virtual force of the end effector position is determined according to the axis component of the end effector position, the upper limit of the axis position constraint enabling threshold, and the upper limit of the axis position constraint critical threshold.

[0165] Step S172: When the axis component of the end effector position is greater than the lower limit of the axis position constraint enabling threshold and less than the upper limit of the axis position constraint enabling threshold, the axis workspace virtual force of the end effector position is set to a third preset value.

[0166] In this embodiment, the third preset value is specifically zero.

[0167] Step S173: When the axis component of the end effector position is greater than or equal to the lower limit of the axis position constraint critical threshold and less than or equal to the lower limit of the axis position constraint enabling threshold, the axis workspace virtual force of the end effector position is determined according to the axis component of the end effector position, the lower limit of the axis position constraint critical threshold, and the lower limit of the axis position constraint enabling threshold.

[0168] Step S174: The second workspace virtual force of the mechanical arm introducing the workspace position constraint is determined by using the axis workspace virtual force of the end effector position.

[0169] In this embodiment, based on the size relationship between the axis component of the end effector position and the axis position constraint enabling threshold and the axis position constraint critical threshold, the corresponding second workspace virtual force can be obtained, that is, the second workspace virtual force is determined by one of steps S171, S172, or S173.

[0170] Step S18: The workspace resultant force of the mechanical arm is determined by using the workspace external force, the first workspace virtual force, and the second workspace virtual force.

[0171] In this embodiment, the workspace external force Fextof the mechanical arm is determined based on formula (1) est , the first workspace virtual force Fv1of the mechanical arm is determined based on formula (8) vper , and the second workspace virtual force Fv2of the mechanical arm is determined based on formula (11) vpos , so as to calculate the workspace resultant force of the mechanical arm.

[0172] Specifically, the workspace resultant force F of the mechanical arm in this embodiment s is equal to the workspace external force Fext est , the first workspace virtual force Fv1 vperand the second workspace virtual force F vpos .

[0173] Step S19: obtaining the desired position of the robot arm according to the workspace resultant force, and controlling the robot arm to move according to the desired position.

[0174] The process of obtaining the desired position of the robot arm in the embodiment will be described in detail with reference to Figure 7 , Figure 7 is Figure 1 The specific flowchart of step S19 in the embodiment is shown in FIG. 19. Specifically, the following steps are included.

[0175] Step S191: obtaining the drag control rate of the robot arm according to the dynamic model of the robot arm.

[0176] The expression of the dynamic model of the robot arm can be shown in formula (12):

[0177]

[0178] wherein m is an inertia parameter, c is a damping parameter, k is a stiffness parameter, x0 is an equilibrium position, x, are the desired position, the desired velocity, and the desired acceleration, respectively.

[0179] In the embodiment, the stiffness parameter, the desired position, the desired velocity, and the desired acceleration are set to zero, and the control rate is obtained, which is shown in formula (13):

[0180]

[0181] Step S192: obtaining the discrete expression of the desired velocity and acceleration based on the zero-order holder principle and the drag control rate, and obtaining the discrete expression of the desired velocity based on the discrete expression of the desired acceleration.

[0182] The discrete expression of the desired velocity can be calculated by substituting the discrete expression of the desired acceleration into the velocity-acceleration relationship. Specifically, the discrete expression of the desired velocity and acceleration can be shown in formula (14), and the discrete expression of the desired velocity can be shown in formula (15):

[0183]

[0184]

[0185] wherein F S (k) is the detected force at the kth moment, is the desired velocity at the kth moment, is the desired acceleration at the kth moment, and T is the sampling period.

[0186] Step S193: Discrete integration is performed on the discrete expression of the desired velocity according to the workspace resultant force to obtain the desired position of the robot arm.

[0187] wherein the discrete integration of formula (15) is solved to obtain the expression of the desired position of the robot arm, which can be specifically shown as formula (16):

[0188]

[0189] wherein x d (k) is the desired position at the kth moment.

[0190] Specifically, the desired position at the corresponding moment is solved according to formula (16), and the robot arm is controlled to move according to the corresponding desired position. The method of robot arm movement control can be any method in the prior art, which is not described herein.

[0191] The application obtains the joint external force moment and the workspace external force of the robot arm based on the joint external force observer. The current state of the robot arm is constrained by the manipulation index to obtain the first workspace virtual force, and the position of the end effector of the robot arm is constrained by the position constraint key threshold and the position constraint enable threshold to obtain the second workspace virtual force. The desired position of the robot arm can be calculated and solved based on the workspace external force, the first workspace virtual force and the second workspace virtual force, and the robot arm is controlled to move according to the desired position. The motion constraint based on the manipulation index can avoid the uncontrollable situation of joint overspeed caused by dragging the robot arm to the singular region, and the motion constraint based on the workspace position can limit the dragging range of the workspace, avoid the collision between the robot arm and the surrounding environment, and improve the safety of the robot arm during the dragging process.

[0192] The application also provides an electronic device, please refer to Figure 8 , Figure 8 is a frame schematic diagram of an embodiment of the electronic device of the application. The electronic device 80 includes a memory 81 and a processor 82 coupled to each other. The processor 82 is used to execute the program instructions stored in the memory 81 to realize the steps in any of the above robot arm dragging method embodiments. In one specific implementation scenario, the electronic device 80 can include but is not limited to a microcomputer, a server, in addition, the electronic device 80 can also include a notebook computer, a tablet computer and other mobile devices, which are not limited herein.

[0193] Specifically, the processor 82 is configured to control itself and the memory 81 to implement the steps in any of the above robotic arm dragging method embodiments. The processor 82 can also be referred to as a CPU (Central Processing Unit). The processor 82 can be an integrated circuit chip having a processing capability of signals. The processor 82 can also be a general processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array) or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component. The general processor can be a microprocessor or the processor can also be any conventional processor or the like. In addition, the processor 82 can be jointly implemented by integrated circuit chips.

[0194] The present application also provides a computer readable storage medium, please refer to Figure 9 , Figure 9 is a schematic diagram of an embodiment of the computer readable storage medium of the present application. The computer readable storage medium 90 stores a computer program 91 capable of being executed by a processor, and the computer program 91 is configured to implement the steps in any of the above robotic arm dragging method embodiments.

[0195] In some embodiments, the apparatus provided by the embodiments of the present application has functions or includes modules for performing the methods described in the above method embodiments, and the specific implementation can refer to the description of the above method embodiments. For brevity, details are not repeated here.

[0196] The above description of various embodiments tends to emphasize the differences between various embodiments, and the same or similar parts can be mutually referred to, and for brevity, details are not repeated here.

[0197] In several embodiments provided in the present application, it should be understood that the disclosed method and device can be implemented in other ways. For example, the above-described device implementation is only schematic, for example, the division of modules or units is only a logical function division, and actual implementation can have another division manner, for example, a unit or component can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the shown or discussed mutual elements can be indirect coupling or communication connection through some interface, device or unit, and can be electrical, mechanical or other forms.

[0198] In addition, each of the functional units in the various embodiments of the present application can be integrated in one processing unit, or each unit can be physically present separately, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0199] If the integrated unit is realized in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present application, essentially or in part, or all or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium, and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to perform all or part of the steps of the methods in the various embodiments of the present application. The foregoing storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disk, and various other media that can store program codes.

[0200] The above is only an embodiment of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation based on the content of the specification and drawings, or direct or indirect application in other related technical fields, is also included in the patent protection scope of the present application.

Claims

1. A robotic arm dragging method based on motion constraints, characterized in that, The robotic arm dragging method includes: Obtain the dynamic model of the robotic arm, and construct a joint external force observer of the robotic arm based on the dynamic model; The current state of the robotic arm and the measured torque obtained by the joint torque sensor are input into the joint external force observer to obtain the joint external torque of the robotic arm; The external force in the workspace of the robotic arm is obtained based on the joint external torque; Based on the current state of the robotic arm, the joint angle vector is obtained, the operation index is determined according to the joint angle vector, and the virtual force gradient vector is determined according to the change of the operation index caused by the translation and rotation of the robotic arm end in each axis of Cartesian space. Using the operation index and the virtual force gradient vector, the first workspace virtual force of the robotic arm under operation index constraint is obtained; The position of the end effector is obtained based on the current state of the robotic arm; By using position constraint key threshold and position constraint enable threshold to constrain the position of the end effector, a second workspace virtual force is obtained that introduces workspace position constraints into the robotic arm; The resultant force in the workspace of the robotic arm is determined by using the external force in the workspace, the first virtual force in the workspace, and the second virtual force in the workspace. The desired position of the robotic arm is obtained by solving the resultant force in the workspace, and the movement of the robotic arm is controlled according to the desired position.

2. The robotic arm dragging method according to claim 1, characterized in that, The method of obtaining the external force in the workspace of the robotic arm based on the joint external torque includes: By using the transpose of the Jacobi matrix as a pseudo-inverse matrix, the joint external torque is converted into the working space external force of the robotic arm.

3. The robotic arm dragging method according to claim 1, characterized in that, The step of determining the operation index according to the joint angle vector includes: The translational Jacobian matrix and the rotational Jacobian matrix are obtained using the Jacobian matrix and the joint angle vector; wherein, the translational Jacobian matrix and the rotational Jacobian matrix are used to describe the relationship between the joint angular velocity vector and the Cartesian space velocity vector; The operational index is determined by rotating the Jacobi matrix and its pseudo-inverse, and translating the Jacobi matrix and its transpose.

4. The robotic arm dragging method according to claim 3, characterized in that, The step of determining the virtual force gradient vector based on the changes in the operational index caused by the translation and rotation of the robotic arm's end effector along each axis in Cartesian space includes: Based on the joint angle vector, control cycle, and velocity vectors of each axis in Cartesian space, obtain the joint angle value corresponding to the end effector of the robotic arm after moving one control cycle along each axis in Cartesian space. Using the expression for the operation index, the change in the operation index caused by the end effector motion of the robotic arm is determined by the joint angle vector and the Cartesian space velocity vector; Based on the changes in the operating exponents of each axis, the gradient elements of each axis are determined and a virtual force gradient vector is formed. The gradient elements are the projections of the virtual force gradient vector onto the translation or rotation directions of each axis.

5. The robotic arm dragging method according to claim 4, characterized in that, The step of determining the gradient elements of each axis based on the changes in the operating exponent of each axis includes: The positive and negative changes in the operating index of each axis are obtained. The positive change in the operating index is the difference between the operating index corresponding to the joint angle value determined by the positive velocity vector and the operating index corresponding to the joint angle vector. The negative change in the operating index is the difference between the operating index corresponding to the joint angle value determined by the negative velocity vector and the operating index corresponding to the joint angle vector. When the positive change of the operating index on each axis is greater than the negative change of the operating index, and the positive change of the operating index is positive, the absolute value of the positive change of the operating index is determined as the gradient element. When the positive change of the operating index on each axis is less than or equal to the negative change of the operating index, and the negative change of the operating index is positive, the negative number of the absolute value of the negative change of the operating index is determined as the gradient element. When the operation index corresponding to the joint angle value determined by the positive velocity vector of each axis and the operation index corresponding to the joint angle value determined by the negative velocity vector are both less than or equal to the operation index corresponding to the joint angle vector, the gradient element is set to a first preset value.

6. The robotic arm dragging method according to any one of claims 3 to 5, characterized in that, The step of obtaining the first workspace virtual force of the robotic arm under the constraint of the operation index by utilizing the operation index and the virtual force gradient vector includes: When the operating index corresponding to the joint angle vector is less than or equal to the virtual force enable threshold, the virtual force scaling factor is determined using the gain factor, the operating index corresponding to the joint angle vector, the virtual force enable threshold, and the virtual force limit threshold. When the operation exponent corresponding to the joint angle vector is greater than the virtual force enable threshold, the virtual force scaling factor is set to the second preset value; Using the virtual force scaling factor and the virtual force gradient vector, the first workspace virtual force of the robotic arm under the introduced operational index constraint is obtained.

7. The robotic arm dragging method according to claim 1, characterized in that, The method of constraining the position of the end effector using position constraint key thresholds and position constraint enable thresholds to obtain the second workspace virtual force of the robotic arm that introduces workspace position constraints includes: When each axis component at the end effector position is greater than or equal to the upper bound of the axis position constraint enable threshold and less than or equal to the upper bound of the axis position constraint key threshold, the virtual force of each axis workspace at the end effector position is determined based on each axis component at the end effector position, the upper bound of the axis position constraint enable threshold, and the upper bound of the axis position constraint key threshold. When each axis component at the end effector position is greater than the lower bound of the axis position constraint enable threshold and less than the upper bound of the axis position constraint enable threshold, the virtual force of each axis workspace at the end effector position is set to a third preset value. When each axis component at the end effector position is greater than or equal to the lower bound of the critical threshold of each axis position constraint and less than or equal to the lower bound of the enable threshold of each axis position constraint, the virtual force of each axis workspace at the end effector position is determined according to each axis component at the end effector position, the lower bound of the critical threshold of each axis position constraint, and the lower bound of the enable threshold of each axis position constraint. By utilizing the virtual forces in the workspace of each axis at the position of the end effector, a second virtual force in the workspace that introduces workspace position constraints for the robotic arm is determined.

8. The robotic arm dragging method according to claim 1, characterized in that, The step of obtaining the desired position of the robotic arm by solving for the resultant force in the workspace includes: Based on the dynamic model of the robotic arm, the drag control rate of the robotic arm is obtained; Based on the zero-order hold principle, a discrete expression of the desired acceleration is obtained based on the drag control law, and a discrete expression of the desired velocity is obtained based on the discrete expression of the desired acceleration. Based on the resultant force in the workspace, the discrete integral of the discrete expression for the desired velocity is calculated to obtain the desired position of the robotic arm.

9. An electronic device, characterized in that, It includes a memory and a processor coupled to each other, the processor being used to execute program instructions stored in the memory to implement the robotic arm dragging method as described in any one of claims 1-8.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the robotic arm dragging method as described in any one of claims 1-8.

Images