A method, device, terminal equipment and storage medium for avoiding singularity of a robotic arm

Abstract

The present application is applicable to the field of robot arm control technology, and provides a method, device, terminal device and storage medium for avoiding singularity of a robot arm, the method comprising: calculating the operability according to the joint position of the robot arm, determining that the robot arm needs to avoid singularity when the operability is less than or equal to a preset threshold, and calculating the virtual environment external force required to avoid singularity according to the potential energy change rate of the operability of the robot arm, performing admittance control calculation based on the virtual environment external force, the current end force of the robot arm and the desired end trajectory of the robot arm, and obtaining the end correction trajectory of the robot arm; determining the target position of each of the joints based on the end correction trajectory, and controlling the movement of the robot arm according to the target position. The present application adds admittance control when the robot arm avoids singularity, and will not stop the joint movement. The present application can achieve the effect of avoiding singularity through the calculated virtual environment external force, and can also improve the compliance of the robot arm movement through admittance control.

Description

Technical Field

[0001] The present application belongs to the field of robot arm control technology, and in particular, relates to a robot arm singularity avoidance method, device, terminal equipment and storage medium. Background Art

[0002] With the development of robotic arms, they are helping humans in their work and life in the form of collaborative or service robots. Limited by the structure of the robotic arm, the robotic arm may move to a singular configuration during operation, causing the robotic arm to lose some freedom of movement and affecting the motion performance of the robotic arm. Therefore, how to avoid singular configurations during the movement of the robotic arm has always been a focus of research.

[0003] At present, when a robotic arm approaches a singular position, the movement of the joints is often restricted to prevent the robotic arm from continuing to approach the singular position. This method not only affects the movement of the robotic arm, but also causes position jumps when the robotic arm resumes movement, and the movement compliance of the robotic arm is poor. Summary of the invention

[0004] The embodiments of the present application provide a method, apparatus, terminal device and storage medium for avoiding singularities of a robotic arm, which can solve the problem of poor flexibility of the current robotic arm when avoiding singularities.

[0005] In a first aspect, an embodiment of the present application provides a method for avoiding singularity of a robotic arm, comprising:

[0006] When it is determined that the robot arm needs to avoid singularity, based on the current joint positions of each joint on the robot arm, the virtual environment external force required for the robot arm to avoid singularity is calculated;

[0007] Obtaining the current end force of the robotic arm and the desired end trajectory of the robotic arm;

[0008] Performing an admittance control calculation based on the virtual environment external force, the current end force and the end desired trajectory to obtain a corrected end trajectory of the robotic arm;

[0009] The movement of the robot arm is controlled based on the end correction trajectory.

[0010] In a second aspect, an embodiment of the present application provides a device for avoiding singularity of a robot arm, comprising:

[0011] An external force calculation module, used for calculating the virtual environment external force required for the robotic arm to avoid the singularity based on the current joint positions of each joint on the robotic arm when it is determined that the robotic arm needs to avoid the singularity;

[0012] A data acquisition module, used to acquire the current end force of the robotic arm and the desired end trajectory of the robotic arm;

[0013] An admittance control calculation module, used to perform admittance control calculation based on the virtual environment external force, the current end force and the end desired trajectory, to obtain the end correction trajectory of the robotic arm;

[0014] A motion control module is used to control the movement of the robotic arm based on the end correction trajectory.

[0015] In a third aspect, an embodiment of the present application provides a terminal device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the method for avoiding singularities of a robotic arm described in any one of the first aspects above is implemented.

[0016] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method for avoiding singularities of a robotic arm described in any one of the first aspects above is implemented.

[0017] In a fifth aspect, an embodiment of the present application provides a computer program product. When the computer program product is run on a terminal device, the terminal device executes the method for avoiding singularities of a robotic arm as described in any one of the first aspects above.

[0018] Compared with the prior art, the first aspect of the present application has the following beneficial effects: when the robot arm needs to avoid singularity, the present application calculates the virtual environment external force required to avoid singularity according to the current joint position of the joint on the robot arm, performs admittance control calculation based on the virtual environment external force, the current end force of the robot arm and the desired end trajectory of the robot arm, and obtains the end correction trajectory of the robot arm; the robot arm movement is controlled based on the end correction trajectory. Since admittance control can make the movement of the robot arm smoother, the present application introduces the avoidance of singularity into the admittance control through the calculated virtual environment external force, so that the robot arm will not cause the joint movement to jump and freeze during the movement during the process of avoiding singularity, thereby improving the smoothness of the robot arm movement.

[0019] It can be understood that the beneficial effects of the second to fifth aspects mentioned above can be found in the relevant description of the first aspect mentioned above, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0021] Figure 1 It is a flowchart of a method for avoiding singularity of a robot arm provided in one embodiment of the present application;

[0022] Figure 2 It is a flowchart of a method for determining whether singularity avoidance is required provided in an embodiment of the present application;

[0023] Figure 3 It is a flowchart of a method for determining a virtual environment external force required to avoid singularity provided by an embodiment of the present application;

[0024] Figure 4 It is a flowchart of a method for determining a terminal correction trajectory provided by an embodiment of the present application;

[0025] Figure 5 is a flow chart of a method for avoiding singularity of a robot arm provided by another embodiment of the present application;

[0026] Figure 6 It is a schematic diagram of the structure of a robot arm for avoiding singularity provided by an embodiment of the present application;

[0027] Figure 7 It is a structural schematic diagram of a singularity avoidance device of a robotic arm provided in one embodiment of the present application;

[0028] Figure 8 It is a structural diagram of a terminal device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0029] It should be understood that when used in the present specification and the appended claims, the term "comprising" indicates the presence of described features, wholes, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components and / or combinations thereof.

[0030] It should also be understood that the term “and / or” used in the specification and appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0031] As used in the specification and appended claims of this application, the term "if" can be interpreted as "when" or "uponce" or "in response to determining" or "in response to detecting", depending on the context. Similarly, the phrase "if it is determined" or "if [described condition or event] is detected" can be interpreted as meaning "uponce it is determined" or "in response to determining" or "uponce [described condition or event] is detected" or "in response to detecting [described condition or event]", depending on the context.

[0032] In addition, in the description of the present application specification and the appended claims, the terms "first", "second", "third", etc. are only used to distinguish the descriptions and cannot be understood as indicating or implying relative importance.

[0033] References to "one embodiment" or "some embodiments" etc. described in the specification of this application mean that one or more embodiments of the present application include specific features, structures or characteristics described in conjunction with the embodiment. Therefore, the phrases "in one embodiment", "in some embodiments", "in some other embodiments", "in some other embodiments", etc. appearing in different places in this specification do not necessarily refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in other ways.

[0034] At present, the compliance of the robot arm is mainly improved through admittance control. The input of admittance control is force, and the output is position. Admittance control does not have the ability to avoid the singular configuration of the robot arm. When the robot arm interacts with humans, it is more likely to encounter singular configurations, which poses a safety hazard. Therefore, it is very necessary to avoid singular configurations during admittance control.

[0035] At present, when a robot arm approaches a singular configuration, it often prevents the robot arm from continuing to approach a singular configuration by limiting some or all of the joint movements. After limiting the joint movement, position jumps are likely to occur when the joint movement is restored, and the flexibility of the robot arm is poor. In addition, limiting the joint movement requires a lot of computing time to determine the joint where the singularity occurs, and the real-time control performance is poor. Thirdly, if force interaction occurs (external force acts on the robot arm) during the process of avoiding singularity, the movement of the robot arm will be offset. Therefore, adding admittance control during the process of avoiding singularity can not only make the joint movement more flexible, but also reduce the amount of calculation and improve the performance of real-time control.

[0036] Based on the above reasons, the present application proposes a method for avoiding singularities of a robotic arm, which adds admittance control during the process of avoiding singularities of the robotic arm to make the movement of the robotic arm smoother and the response faster.

[0037] Specifically, the present application uses the maneuverability of the robot arm as a criterion for determining whether the robot arm is away from a singular configuration, and determines whether the robot arm needs to avoid the singular configuration based on the maneuverability.

[0038] When the robot arm needs to avoid singularity, the operability is converted to obtain the joint torque. According to the conversion relationship between the joint torque and the terminal output, the joint torque is introduced into the terminal external force of the admittance control to achieve the integration of singularity avoidance and admittance control, and then control the joint movement; this application avoids singular configurations from the source of interaction, so that the robot arm can avoid position deviation as much as possible even if it is affected by external forces during the process of avoiding singularity. This application is applicable to position-controlled, force-controlled, redundant or non-redundant robot arms, and has a wide range of uses.

[0039] Figure 1 A schematic flow chart of the method for avoiding singularity of the robot arm provided by the present application is shown, referring to Figure 1 , the method is described in detail as follows:

[0040] S101, when it is determined that the robot arm needs to avoid singularity, based on the current joint position of each joint on the robot arm, calculate the virtual environment external force required for the robot arm to avoid singularity.

[0041] In some embodiments, whether the robot arm needs to avoid singularity is mainly determined by the degree to which the robot arm is close to the singular configuration. In this embodiment, the degree to which the robot arm is close to the singular configuration is characterized by the maneuverability of the robot arm. The maneuverability is determined based on the current joint position of each joint.

[0042] In this embodiment, the virtual environment external force required by the robot arm to avoid singularity represents the additional force required by the end of the robot arm to avoid singularity.

[0043] like Figure 2 As shown, specifically, the method for determining whether the robot arm needs to avoid singularity may include:

[0044] S201, obtaining the current joint position of each joint on the robotic arm.

[0045] In this embodiment, the current joint position of the joint can be obtained from the joint sensor on the robot arm. For example, when the joint sensor measures the joint angle, the current joint position can be represented by the joint angle.

[0046] S202: Calculate the maneuverability of the robotic arm based on the current joint positions.

[0047] Specifically, a greater maneuverability indicates that the robotic arm is further away from the singular configuration, and a smaller maneuverability indicates that the robotic arm is closer to the singular configuration.

[0048] The maneuverability of the robot arm is calculated using a first model, wherein the first model is m(q sensor ) is the operability, q sensor is the position vector composed of the current joint positions of each joint, J(q sensor ) is about q sensor The Jacobian matrix, J(q sensor ) T is J(q sensor ), det[J(q sensor )J(q sensor ) T ] is the matrix J(q sensor )J(q sensor ) T The Jacobian matrix refers to the partial differential relationship between the end position of the robot arm and the current joint position of each joint. sensor ∈R n , n is the degree of freedom of the robot arm. J(q sensor )∈R 6×n .

[0049] S203: If the operability is less than or equal to a preset threshold, it is determined that the robot arm needs to avoid singularity.

[0050] In this embodiment, the preset threshold can be set as needed.

[0051] S204: If the operability is greater than the preset threshold, it is determined that the robot arm does not need to avoid singularity.

[0052] S102, obtaining the current end force of the robotic arm and the desired end trajectory of the robotic arm.

[0053] In this embodiment, the current end force of the robot arm can be measured by the end sensor of the robot arm. The desired trajectory of the end of the robot arm can be obtained from the motion planner.

[0054] The desired trajectory of the terminal can be characterized by the desired posture of the terminal, the desired velocity of the terminal, and the desired acceleration of the terminal.

[0055] S103, performing admittance control calculation based on the virtual environment external force, the current end force and the end desired trajectory to obtain a corrected end trajectory of the robotic arm.

[0056] Specifically, the virtual environment external force and the current end force are input into the admittance controller to obtain the end trajectory correction of the manipulator. The end trajectory correction may include the end posture correction, the end velocity correction, and the end acceleration correction.

[0057] The terminal desired trajectory is corrected by using the terminal trajectory correction amount to obtain the terminal corrected trajectory of the robot arm. The terminal corrected trajectory includes terminal corrected posture, terminal corrected velocity, terminal corrected acceleration, etc.

[0058] S104: Control the movement of the robot arm based on the end correction trajectory.

[0059] In this embodiment, the target position of each of the joints is determined based on the end correction trajectory, and the movement of the robot arm is controlled according to the target position.

[0060] In this embodiment, the terminal correction trajectory is input into the motion controller to obtain the target position of each joint.

[0061] When the motion controller is a position control controller, the target position is the joint position. When the motion controller is a force control controller, the target position is the joint torque.

[0062] In an embodiment of the present application, when the robot arm needs to avoid singularity, the virtual environment external force required to avoid singularity is calculated based on the current joint position of the joint on the robot arm, and the admittance control calculation is performed based on the virtual environment external force, the current end force of the robot arm, and the desired end trajectory of the robot arm to obtain the end correction trajectory of the robot arm; the movement of the robot arm is controlled based on the end correction trajectory. Since admittance control can make the movement of the robot arm more flexible, the present application introduces the avoidance of singularity into the admittance control through the calculated virtual environment external force, so that the robot arm will not cause the joint movement to jump and freeze during the movement during the process of avoiding singularity, thereby improving the flexibility of the robot arm movement.

[0063] like Figure 3 As shown, in a possible implementation manner, the implementation process of step S101 may include:

[0064] S1011, obtaining the operability of the robotic arm, wherein the operability is determined based on the current joint position.

[0065] In this embodiment, the operability is determined by the above step S202. Please refer to the above step S202 and will not be described in detail here.

[0066] S1012: Based on the operability, obtain the joint torque required by the robotic arm to avoid singularity.

[0067] In this embodiment, the maneuverability potential energy is calculated based on the maneuverability. The derivative of the maneuverability potential energy is calculated to obtain the joint torque required by the robot arm to avoid singularity. The maneuverability potential energy is used to characterize the energy required by the robot arm to avoid singularity.

[0068] Specifically, the operability potential is: V m (q sensor ) is the operability potential energy; k SA is the preset adjustment gain; m0 is the preset operability threshold; m(q sensor ) is operability.

[0069] Specifically, the joint torque is: τ SA is the joint torque, J m ( sensor ) T For J m ( sensor ), J m ( sensor ) is the maneuverability Jacobian matrix, which represents the partial differential relationship between the maneuverability and the current joint position. m ( sensor )∈R n .

[0070] Specifically, J m ( sensor ) include:

[0071] i=1……n.J m ( sensor )=[ m,i (q sensor )……J m,n (q sensor )]. i is the current joint position of the i-th joint.

[0072] The process of deriving the determinant is:

[0073]

[0074] Where, trace(A) represents the trace of matrix A; trace(kA) = k × trace(A); adj(A) represents the adjoint matrix of matrix A; adj(A) = det(A) × A -1 .

[0075] From the derivation process of the above determinant, we can get:

[0076]

[0077] In the formula, J m,i For J m,i (q sensor ); JJ T is J(q sensor )J(qsensor ) T ]; J is J(q sensor );J T is J(q sensor ) T ; The operator vec is used to represent the expansion of a matrix into a vector. trace(A+B)=trace(A)+trace(B); trace(AB)=trace(BA); Represents the Hessian matrix The i-th item of . H i The transpose of .

[0078] S1013, equating the joint torque to the end force of the robotic arm, and obtaining the virtual environment external force required by the robotic arm to avoid singularity.

[0079] Specifically, the second model is used to calculate the external force of the virtual environment. The second model is: SA = J(q sensor ) +T τ SA , F SA is the virtual environment external force, J(q sensor ) +T is J(q sensor ) T In order to reduce the computation time, J(q sensor ) +T You can also use J(q sensor )replace.

[0080] Specifically, F SA =2×k SA [m0-m(q sensor )]×J(q sensor ) +T J m (q sensor ) T .

[0081] like Figure 4 As shown, in a possible implementation manner, the implementation process of step S103 may include:

[0082] S1031, using the virtual environment external force to correct the current end force to obtain the target end force of the robotic arm.

[0083] Specifically, the virtual environment external force and the current terminal force are added to obtain the target terminal force.

[0084] S1032, calculating the end trajectory correction amount of the robotic arm based on the target end force and preset admittance parameters.

[0085] In this embodiment, the admittance parameters may include an inertia matrix, a damping matrix, and a stiffness matrix.

[0086] Specifically, the admittance control principle includes:

[0087]

[0088]

[0089] Among them, x ref is the desired position of the end, is the desired terminal speed, is the desired acceleration at the end. com Correct the pose of the end. is the terminal correction speed, is the terminal correction acceleration. M is the preset inertia matrix, B is the preset damping matrix, and K is the stiffness matrix. F sensor is the current end force, F SA is the external force of the virtual environment. The terminal trajectory correction includes and e(t). is the terminal posture correction; is the terminal velocity correction. e(t) is the terminal acceleration correction. e(t)=x com -x ref .

[0090] According to the above formula, we can get Afterwards, you can get and e(t).

[0091] S1033, correcting the desired terminal trajectory based on the terminal trajectory correction amount to obtain a terminal correction trajectory of the robot arm.

[0092] Specifically, the terminal trajectory correction amount is added to the terminal desired trajectory to obtain the terminal corrected trajectory.

[0093] Specifically, the terminal posture correction amount is added to the terminal desired posture to obtain the terminal correction posture.

[0094] Add the terminal speed correction amount to the terminal desired speed to obtain the terminal correction speed.

[0095] Add the terminal acceleration correction to the terminal desired acceleration to obtain the terminal correction acceleration.

[0096] like Figure 5As shown, in a possible implementation, the above method may further include:

[0097] S11, obtaining the current joint position of each joint on the robotic arm.

[0098] S12, calculating the manipulability of the robot arm based on the current joint position. Specifically, the method for calculating the manipulability is the same as the above step S202, which will not be repeated here.

[0099] S13, if the operability is less than or equal to the preset threshold, the operability potential is calculated using the operability. Specifically, the method for calculating the operability potential is the same as the above step S1012, which will not be repeated here.

[0100] S14, the manipulability potential energy is derived to obtain the joint torque required by the robot arm to avoid singularity. Specifically, the method for calculating the joint torque is the same as the above step S1012, which will not be repeated here.

[0101] S15, converting the joint torque into the end force of the robot arm, and obtaining the virtual environment external force required by the robot arm to avoid singularity. Specifically, the method of calculating the virtual environment external force is the same as the above step S1013, which will not be repeated here.

[0102] S16, using the virtual environment external force to correct the current end force to obtain the target end force of the robotic arm.

[0103] S17, based on the target end force and the preset admittance parameter, the end trajectory correction amount of the robot arm is calculated. Specifically, the method for calculating the end trajectory correction amount is the same as the above step S1032, which will not be repeated here.

[0104] S18, obtaining the desired trajectory of the end of the robotic arm.

[0105] S19, based on the terminal trajectory correction amount, the terminal desired trajectory is corrected to obtain the terminal correction trajectory of the robot arm, and the terminal correction trajectory is used to control the movement of the robot arm.

[0106] like Figure 6 As shown, in a possible implementation, J in the figure is the above J(q sensor ), m is the above m(q sensor ), J m For the above J m ( sensor ), V m For the above V m (q sensor ), e is the above e(t), For the above For the above

[0107] The above method may further include:

[0108] Step 1: Get the current joint position q of each joint of the robot arm sensor .

[0109] Step 2: Calculate the current joint position q sensor The Jacobian matrix J.

[0110] Step 3, based on the current joint position q sensor Calculate the operability m and the Hessian matrix H.

[0111] Step 4: When the operability is less than or equal to a preset threshold, calculate the operability potential energy V based on the operability m. m The calculation method of operability is the same as that of step S202 above, which will not be described in detail here. The calculation method of operability potential is the same as that of step S1012 above, which will not be described in detail here.

[0112] Step 5: Use the Hessian matrix H to calculate the operability Jacobian matrix J m The calculation method of the operability Jacobian matrix is ​​the same as that of the above step S1012, and will not be repeated here.

[0113] Step 6: Use the operable Jacobian matrix J m and operability potential V m , calculate the joint torque τ required by the robot arm to avoid singularity SA . Joint torque τ SA The calculation method of is the same as that of the above step S1012 and will not be repeated here.

[0114] Step 7: Use the Jacobian matrix J to transform the joint torque τ SA Equivalently converted into the end force of the robotic arm, the virtual environment external force F required by the robotic arm to avoid singularity is obtained SA Virtual environment external force F SA The calculation method of is the same as that of the above step S1013 and will not be repeated here.

[0115] Step 8: Get the current end force of the robot arm. SA And the current end force is input to the admittance controller to obtain the end trajectory correction e of the robot arm. and

[0116] Step 9: Get the desired trajectory x of the end of the robot arm from the motion planner ref , and where x ref is the desired position of the end, is the desired terminal speed, is the desired acceleration at the end.

[0117] Step 10: Correct the end trajectory by e. and The expected trajectory x ref , and Input into the motion controller together to get the joint position q com or τ com , and use the joint positions to control the movement of the robotic arm. com is the joint position, τ com is the joint torque.

[0118] It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0119] Corresponding to the method for avoiding singularity of the robot arm described in the above embodiment, Figure 7 A structural block diagram of a singularity avoidance device of a robotic arm provided in an embodiment of the present application is shown. For ease of explanation, only the parts related to the embodiment of the present application are shown.

[0120] Reference Figure 7 The device 300 may include: an external force calculation module 310, a data acquisition module 320, an admittance control calculation module 330 and a motion control module 340.

[0121] The external force calculation module 310 is used to calculate the virtual environment external force required for the robotic arm to avoid the singularity based on the current joint positions of each joint on the robotic arm when it is determined that the robotic arm needs to avoid the singularity;

[0122] A data acquisition module 320 is used to acquire the current end force of the robotic arm and the desired end trajectory of the robotic arm;

[0123] An admittance control calculation module 330 is used to perform an admittance control calculation based on the virtual environment external force, the current end force and the end desired trajectory to obtain a corrected end trajectory of the robotic arm;

[0124] The motion control module 340 is used to control the motion of the robotic arm based on the end correction trajectory.

[0125] In a possible implementation, the external force calculation module 310 is also connected to:

[0126] A joint position acquisition module, used to acquire the current joint position of each joint on the robotic arm;

[0127] an operability calculation module, used to calculate the operability of the robotic arm based on the current joint position;

[0128] The singularity avoidance judgment module is used to determine that the robot arm needs to avoid singularity if the operability is less than or equal to a preset threshold.

[0129] In a possible implementation, the singularity avoidance judgment module may also be used to:

[0130] If the operability is greater than the preset threshold, it is determined that the robot arm does not need to avoid singularity.

[0131] In a possible implementation, the operability calculation module may also be used to:

[0132] The maneuverability of the robot arm is calculated using a first model, wherein the first model is m(q sensor ) is the operability, q sensor is the position vector composed of the current joint positions of each joint, J(q sensor ) is about q sensor The Jacobian matrix, J(q sensor ) T is J(q sensor ), det[J(q sensor )J(q densor ) T ] is the matrix J(q sensor )J(q sensor ) T The determinant of ;

[0133] The virtual environment external force is: F SA = J(q sensor ) +T τ SA , F SA is the virtual environment external force, J(q sensor ) +T is J(q sensor ) T The pseudo-inverse matrix of .

[0134] In a possible implementation, the external force calculation module 310 may also be used to:

[0135] Obtaining the manipulability of the robotic arm, wherein the manipulability is determined based on the current joint position;

[0136] Based on the operability, obtaining the joint torque required by the robotic arm to avoid singularity;

[0137] The joint torque is equivalently converted into the end force of the robotic arm to obtain the virtual environment external force required by the robotic arm to avoid singularity.

[0138] In a possible implementation, the joint torque is: τ SA =2×k SA [m0-m(q sensor )×J m ( sensor ) T , τ SA is the joint torque, k SA is the preset adjustment gain, m0 is the preset operability threshold, m(q sensor ) is the operability, J m ( sensor ) T For J m ( sensor ), J m ( sensor ) is the maneuverability Jacobian matrix, which represents the partial differential relationship between the maneuverability and the current joint position.

[0139] In a possible implementation, the motion control module 340 may also be used to:

[0140] Using the virtual environment external force to correct the current end force to obtain the target end force of the robotic arm;

[0141] Calculating a terminal trajectory correction amount of the robotic arm based on the target terminal force and a preset admittance parameter;

[0142] The desired end trajectory is corrected based on the end trajectory correction amount to obtain the end correction trajectory of the robot arm.

[0143] It should be noted that the information interaction, execution process, etc. between the above-mentioned devices / units are based on the same concept as the method embodiment of the present application. Their specific functions and technical effects can be found in the method embodiment part and will not be repeated here.

[0144] The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0145] The present application also provides a terminal device, see Figure 8 The terminal device 400 may include: at least one processor 410, a memory 420, and a computer program stored in the memory 420 and executable on the at least one processor 410. When the processor 410 executes the computer program, the steps in any of the above-mentioned method embodiments are implemented, for example Figure 1 Steps S101 to S104 in the illustrated embodiment. Alternatively, when the processor 410 executes the computer program, the functions of each module / unit in the above-mentioned device embodiments are implemented, for example Figure 7 The functions of the external force calculation module 310 to the motion control module 340 are shown.

[0146] Exemplarily, the computer program may be divided into one or more modules / units, one or more modules / units are stored in the memory 420, and executed by the processor 410 to complete the present application. The one or more modules / units may be a series of computer program segments capable of completing specific functions, and the program segments are used to describe the execution process of the computer program in the terminal device 400.

[0147] Those skilled in the art will understand that Figure 8 It is only an example of a terminal device and does not constitute a limitation on the terminal device. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components, such as input and output devices, network access devices, buses, etc.

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

[0149] The memory 420 may be an internal storage unit of the terminal device, or an external storage device of the terminal device, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. The memory 420 is used to store the computer program and other programs and data required by the terminal device. The memory 420 may also be used to temporarily store data that has been output or is to be output.

[0150] The bus may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, the bus in the drawings of the present application is not limited to only one bus or one type of bus.

[0151] The method for avoiding singularity of a robotic arm provided in the embodiment of the present application can be applied to terminal devices such as computers, tablet computers, laptop computers, netbooks, personal digital assistants (PDAs), etc. The embodiment of the present application does not impose any restrictions on the specific type of the terminal device.

[0152] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0153] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0154] In the embodiments provided in the present application, it should be understood that the disclosed terminal equipment, devices and methods can be implemented in other ways. For example, the terminal equipment embodiments described above are only schematic. For example, the division of the modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0155] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0156] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0157] If the integrated unit is implemented 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 this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and when the computer program is executed by one or more processors, the steps of each of the above-mentioned method embodiments can be implemented.

[0158] If the integrated unit is implemented 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 this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and when the computer program is executed by one or more processors, the steps of each of the above-mentioned method embodiments can be implemented.

[0159] Similarly, as a computer program product, when the computer program product runs on a terminal device, the terminal device can implement the steps in the above-mentioned method embodiments when executing the computer program product.

[0160] The computer program includes computer program code, which may be in source code form, object code form, executable file or some intermediate form, etc. The computer readable medium may include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium, etc. It should be noted that the content contained in the computer readable medium may be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer readable media do not include electric carrier signals and telecommunication signals.

[0161] The embodiments described above are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, a person skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A method for avoiding singularity of a robot arm, characterized in that: include: When it is determined that the robot arm needs to avoid singularity, based on the current joint positions of each joint on the robot arm, the virtual environment external force required for the robot arm to avoid singularity is calculated; Obtaining the current end force of the robotic arm and the desired end trajectory of the robotic arm; Performing an admittance control calculation based on the virtual environment external force, the current end force and the end desired trajectory to obtain a corrected end trajectory of the robotic arm; Controlling the movement of the robotic arm based on the end correction trajectory; The step of calculating the virtual environment external force required by the robotic arm to avoid singularity based on the current joint positions of the joints on the robotic arm comprises: Obtaining the manipulability of the robotic arm, wherein the manipulability is determined based on the current joint position; Based on the operability, obtaining the joint torque required by the robotic arm to avoid singularity; The joint torque is equivalently converted into the end force of the robotic arm to obtain the virtual environment external force required by the robotic arm to avoid singularity; The joint torque is: τ SA =2×k SA [m0-m(q sensor )]×J m (q sensor ) T , τ SA is the joint torque, k SA is the preset adjustment gain, m0 is the preset operability threshold, m(q sensor ) is the operability, J m (q sensor ) T For J m (q sensor ), J m (q sensor ) is a maneuverability Jacobian matrix, wherein the maneuverability Jacobian matrix represents a partial differential relationship between the maneuverability and the current joint position; The virtual environment external force is: F SA = J(q sensor ) +T τ SA , F SA is the virtual environment external force, J(q sensor ) +T is J(q sensor ) T The pseudo-inverse matrix of .

2. The method for avoiding singularity of a robot arm as claimed in claim 1, characterized in that: The method further comprises: Obtaining the current joint position of each joint on the robotic arm; Calculating the maneuverability of the robotic arm based on the current joint position; If the operability is less than or equal to a preset threshold, it is determined that the robot arm needs to avoid singularity.

3. The method for avoiding singularity of a robot arm as claimed in claim 2, characterized in that: After calculating the maneuverability of the robot arm based on the current joint position, the method further includes: If the operability is greater than the preset threshold, it is determined that the robot arm does not need to avoid singularity.

4. The method for avoiding singularity of a robot arm as claimed in claim 2, characterized in that: The calculating the manipulability of the robot arm based on the current joint position includes: The maneuverability of the robot arm is calculated using a first model, wherein the first model is m(q sensor ) is the operability, q sensor is the position vector composed of the current joint positions of each joint, J(q sensor ) is about q sensor The Jacobian matrix, J(q sensor ) T is J(q sensor ), det[J(q sensor )J(q sensor ) T ] is the matrix J(q sensor )J(q sensor ) T The determinant of .

5. The method for avoiding singularity of a robot arm as claimed in claim 1, characterized in that: The method further comprises: performing an admittance control calculation based on the virtual environment external force, the current end force and the desired end trajectory to obtain the end correction trajectory of the robotic arm, including: Using the virtual environment external force to correct the current end force to obtain the target end force of the robotic arm; Calculating a terminal trajectory correction amount of the robotic arm based on the target terminal force and a preset admittance parameter; The desired end trajectory is corrected based on the end trajectory correction amount to obtain the end correction trajectory of the robot arm.

6. A device for avoiding strangeness of a robot arm, characterized in that: include: An external force calculation module, used for calculating the virtual environment external force required for the robotic arm to avoid the singularity based on the current joint positions of each joint on the robotic arm when it is determined that the robotic arm needs to avoid the singularity; A data acquisition module, used to acquire the current end force of the robotic arm and the desired end trajectory of the robotic arm; An admittance control calculation module, used to perform admittance control calculation based on the virtual environment external force, the current end force and the end desired trajectory, to obtain the end correction trajectory of the robotic arm; A motion control module, used for controlling the motion of the robotic arm based on the end correction trajectory; The external force calculation module is further specifically used to: obtain the maneuverability of the robotic arm, wherein the maneuverability is determined based on the current joint position; based on the maneuverability, obtain the joint torque required by the robotic arm to avoid singularity; The joint torque is equivalently converted into the end force of the robotic arm to obtain the virtual environment external force required by the robotic arm to avoid singularity; The joint torque is: τ SA =2×k SA [m0-m(q sensor )]×J m (q sensor ) T , τ SA is the joint torque, k SA is the preset adjustment gain, m0 is the preset operability threshold, m(q sensor ) is the operability, J m (q sensor ) T For J m (q sensor ), J m (q sensor ) is a maneuverability Jacobian matrix, wherein the maneuverability Jacobian matrix represents a partial differential relationship between the maneuverability and the current joint position; The virtual environment external force is: F SA = J(q sensor ) +T τ SA , F SA is the virtual environment external force, J(q sensor ) +T is J(q sensor ) T The pseudo-inverse matrix of .

7. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the method for avoiding singularities of the robotic arm as described in any one of claims 1 to 5 is implemented.

8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method for avoiding singularities of a robotic arm as described in any one of claims 1 to 5 is implemented.

Images