A singular point avoidance method and device in a constant force control process of a mechanical arm

Abstract

The application provides a singular point avoidance method and device in a constant force control process of a mechanical arm, and the method comprises the following steps: a kinematic model of a target mechanical arm is established, and a calculation mode of a Jacobian matrix and an operation index is determined based on the kinematic model; a Cartesian space virtual force is calculated based on joint angles of the target mechanical arm and by using the calculation mode of the Jacobian matrix and the operation index; the Cartesian space virtual force is input into a constant force controller of the target mechanical arm to obtain a Cartesian space pose control amount; and the target mechanical arm is controlled to move by using the Cartesian space pose control amount until an operation index corresponding to the target mechanical arm is greater than a virtual force enable threshold. Therefore, the Cartesian space virtual force is introduced into the calculation of an input force signal of the constant force controller, so that the mechanical arm is subjected to repulsion from a singular region in the constant force control process, online singular point avoidance is realized, and the requirement of the constant force control on end pose keeping is met.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm singularity avoidance, in particular to a singularity avoidance method and device in a constant force control process of a mechanical arm, BACKGROUND

[0002] Currently, constant force control is mainly applied in polishing, assembly and other fields. In these scenarios, the working environment is known, and the operator can obtain a working path that avoids singularities through offline programming or on-site demonstration, so that there is no need to consider avoiding singularities in the constant force control process. However, this method cannot be applied to scenarios where the working environment is not known in advance or the working path needs to be generated online.

[0003] Currently, for the singularity avoidance problem of a mechanical arm in Cartesian space, known solutions mainly include potential field method and damping reciprocal method and their variants. For example, a Chinese invention patent with the application number 202110968731.5 and the name of a general singularity avoidance method and system for a mechanical arm discloses that a repulsive potential field is formed with singularities, and then the joint space repulsive velocity is calculated, and then the Cartesian space repulsive velocity is calculated using the Jacobian matrix, and finally the joint velocity is calculated by combining the drive velocity to complete the singularity avoidance. However, in the application process of this method, the pose of the robot end tool in the Cartesian space may change, which is not suitable for online constant force control processes where the working environment is not known in advance. A Chinese invention patent with the application number 202011439303.5 and the name of a singularity avoidance method and system for a non-spherical wrist mechanical arm discloses a method for deriving the singularity factor of a non-spherical wrist mechanical arm using a Jacobian matrix variation method. However, this method modifies the Jacobian matrix, which introduces a variation when converting Cartesian space velocity to joint space velocity, causing the end tool pose of the robot to deviate uncontrollably from the expected value.

[0004] In summary, the existing singularity avoidance solutions for mechanical arms all affect the pose of the end tool, making it difficult to meet the requirements of constant force control for maintaining the end pose. SUMMARY

[0005] Therefore, the embodiments of the present application provide a singularity avoidance method and device in a constant force control process of a mechanical arm to solve the problem that the existing singularity avoidance methods for mechanical arms cause the pose of the end tool to change and do not meet the requirements of constant force control for maintaining the end pose.

[0006] According to a first aspect, the embodiments of the present application provide a singularity avoidance method in a constant force control process of a mechanical arm, comprising:

[0007] establishing a kinematics model of a target robot arm, and determining a calculation method of a Jacobian matrix and an operation index based on the kinematics model;

[0008] calculating a virtual force in a Cartesian space based on joint angles of the target robot arm and using the calculation method of the Jacobian matrix and the operation index;

[0009] inputting the virtual force in the Cartesian space into a constant force controller of the target robot arm to obtain a pose control amount in the Cartesian space;

[0010] controlling movement of the target robot arm using the pose control amount in the Cartesian space, and returning to the step of calculating the virtual force in the Cartesian space based on the joint angles of the target robot arm and using the calculation method of the Jacobian matrix and the operation index until an operation index corresponding to the target robot arm is greater than a virtual force enabling threshold.

[0011] Optionally, the establishing of the kinematics model of the target robot arm comprises:

[0012] establishing a local coordinate system on each link of the target robot arm;

[0013] calculating a conversion relationship of local coordinate systems corresponding to two adjacent links, respectively.

[0014] Optionally, the determining of the calculation method of the Jacobian matrix and the operation index based on the kinematics model comprises:

[0015] calculating the Jacobian matrix based on joint attributes of a joint formed by two adjacent links of the target robot arm and the conversion relationship of the local coordinate systems corresponding to the two adjacent links, the joint attributes comprising: joint degrees of freedom and joint types;

[0016] calculating a distance from a current joint angle of the target robot arm to a singular point based on the Jacobian matrix;

[0017] determining the distance as the operation index.

[0018] Optionally, the calculating of the virtual force in the Cartesian space based on the joint angles of the target robot arm and using the calculation method of the Jacobian matrix and the operation index comprises:

[0019] obtaining current joint angles of the target robot arm;

[0020] calculating a virtual force scaling factor of the current joint angles based on preset virtual force parameters, the preset virtual force parameters comprising: a gain factor, a virtual force enabling threshold, and a virtual force limiting threshold;

[0021] based on the current joint angle, the Jacobian matrix and the calculation mode of the operation index, the projection of the Cartesian space virtual force direction vector of the target robot end in different directions after the target robot end moves in different directions at a first speed and a second speed in the next control cycle is calculated, and the speed direction of the first speed is opposite to that of the second speed;

[0022] based on the projection of the Cartesian space virtual force direction vector of the target robot end in different directions and the virtual force scaling factor, the Cartesian space virtual force is calculated.

[0023] Optionally, the calculation of the projection of the Cartesian space virtual force direction vector of the target robot end in different directions after the target robot end moves in different directions at a first speed and a second speed in the next control cycle based on the current joint angle, the Jacobian matrix and the calculation mode of the operation index comprises:

[0024] based on the current joint angle and the Jacobian matrix calculation mode, the second joint angle and the third joint angle corresponding to the movement of the target robot end in the current direction at a first speed and a second speed respectively in the next control cycle are calculated;

[0025] the first operation index, the second operation index and the third operation index corresponding to the current joint angle, the second joint angle and the third joint angle are calculated respectively by using the calculation mode of the Jacobian matrix and the operation index;

[0026] based on the relationship among the first operation index, the second operation index and the third operation index, the projection of the Cartesian space virtual force direction vector of the target robot end in the current direction is determined.

[0027] Optionally, the input of the Cartesian space virtual force into the constant force controller of the target robot to obtain the Cartesian space pose control quantity comprises:

[0028] the environmental force and the desired force of the target robot end are obtained;

[0029] the force difference between the desired force and the Cartesian space virtual force and the environmental force is calculated;

[0030] the force difference is input into the constant force controller to calculate the Cartesian space position offset;

[0031] the Cartesian space position offset and the target desired position are summed to obtain the Cartesian space pose control quantity.

[0032] Optionally, the control of the target robot motion by using the Cartesian space pose control quantity comprises:

[0033] The Cartesian space pose control quantity is used to control the target robot arm to move in a current control cycle.

[0034] According to a second aspect, an embodiment of the present application provides a singularity avoidance device in a constant force control process of a robot arm, comprising:

[0035] A first processing module is configured to establish a kinematics model of a target robot arm, and determine a calculation manner of a Jacobian matrix and an operation index based on the kinematics model;

[0036] A second processing module is configured to calculate a Cartesian space virtual force based on joint angles of the target robot arm and by using the calculation manner of the Jacobian matrix and the operation index;

[0037] A third processing module is configured to input the Cartesian space virtual force into a constant force controller of the target robot arm to obtain a Cartesian space pose control quantity;

[0038] A fourth processing module is configured to control the target robot arm to move by using the Cartesian space pose control quantity, and return to the step of calculating the Cartesian space virtual force based on the joint angles of the target robot arm and by using the calculation manner of the Jacobian matrix and the operation index, until an operation index corresponding to the target robot arm is greater than a virtual force enabling threshold.

[0039] According to a third aspect, an embodiment of the present application provides a computer readable storage medium, the computer readable storage medium stores computer instructions, and the computer instructions are executed by a processor to implement the method in the first aspect and any optional manner thereof.

[0040] According to a fourth aspect, an embodiment of the present application provides an electronic device, comprising a memory and a processor, which are in communication connection with each other, the memory stores computer instructions, and the processor executes the computer instructions to implement the method in the first aspect and any optional manner thereof.

[0041] The technical scheme of the present application has the following advantages:

[0042] The embodiment of the present application provides a singular point avoiding method and device in a constant force control process of a mechanical arm, the singular point avoiding method and device are as follows: a kinematic model of a target mechanical arm is established, and a calculation mode of a Jacobian matrix and an operation index is determined based on the kinematic model; a Cartesian space virtual force is calculated based on joint angles of the target mechanical arm and by using the calculation mode of the Jacobian matrix and the operation index; the Cartesian space virtual force is input into a constant force controller of the target mechanical arm to obtain a Cartesian space pose control quantity; the Cartesian space pose control quantity is used to control the movement of the target mechanical arm, and the step of calculating the Cartesian space virtual force based on the joint angles of the target mechanical arm and by using the calculation mode of the Jacobian matrix and the operation index is returned until the operation index corresponding to the target mechanical arm is greater than a virtual force enabling threshold. Therefore, the Cartesian space virtual force is calculated, the size of the virtual force is in a negative correlation with the current operation index of the mechanical arm, that is, the greater the current operation index of the mechanical arm, the smaller the virtual force, and the direction of the virtual force is the same as the direction that makes the mechanical arm away from the singular point. Then, the virtual force is introduced into the calculation of the input force signal of the constant force controller, so that the mechanical arm is subjected to repulsion force from the singular region in the constant force control process, thereby realizing online singular point avoidance, and the joint position is adjusted through the Cartesian space pose control quantity, so that the end posture of the mechanical arm can be kept unchanged in the process of avoiding the singular point, and the requirement of constant force control on end posture keeping is met. BRIEF DESCRIPTION OF DRAWINGS

[0043] In order to more clearly illustrate the technical solutions in the specific embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the specific embodiments or prior art description. Obviously, the drawings described below are some embodiments of the present application, and those skilled in the art can obtain other drawings according to these drawings without creative labor.

[0044] Figure 1 The flow chart of the singular point avoiding method in the constant force control process of the mechanical arm in the embodiment of the present application;

[0045] Figure 2 The structural schematic diagram of the singular point avoiding device in the constant force control process of the mechanical arm in the embodiment of the present application;

[0046] Figure 3 The structural schematic diagram of the electronic device in the embodiment of the present application. DETAILED DESCRIPTION

[0047] The technical solutions of the present application will be described clearly and completely below with reference to the drawings, obviously, the described embodiments are some of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the protection scope of the present application.

[0048] In addition, the technical features involved in the different embodiments of the application described below can be combined with each other as long as they do not conflict with each other.

[0049] Currently, constant force control is mainly applied in polishing, assembly and other fields. In these scenarios, the working environment is known, and the operator can obtain a working path that avoids singular points through offline programming or on-site demonstration, so that the problem of avoiding singular points does not need to be considered in the constant force control process. However, this method cannot be applied to scenarios where the working environment is not known in advance or the working path needs to be generated online. In the prior art, the singular point avoidance scheme of the mechanical arm will affect the pose of the end of the mechanical arm, and it is difficult to meet the requirements of constant force control on the end pose.

[0050] Based on the above problems, the embodiment of the application provides a singular point avoidance method in a constant force control process of a mechanical arm, as shown in Figure 1 The singular point avoidance method in the constant force control process of the mechanical arm specifically includes the following steps:

[0051] Step S101: Establish a kinematic model of a target mechanical arm, and determine the calculation method of the Jacobian matrix and the operation index based on the kinematic model.

[0052] Step S102: Calculate the Cartesian space virtual force based on the joint angle of the target mechanical arm and using the calculation method of the Jacobian matrix and the operation index.

[0053] Step S103: Input the Cartesian space virtual force into the constant force controller of the target mechanical arm to obtain a Cartesian space pose control amount.

[0054] Step S104: Control the movement of the target mechanical arm using the Cartesian space pose control amount, and return to step S102 until the operation index corresponding to the target mechanical arm is greater than the virtual force enable threshold.

[0055] Specifically, the movement of the target mechanical arm in the current control period is controlled by using the Cartesian space pose control amount.

[0056] By performing the above steps, the singularity avoidance method in the constant force control process of the mechanical arm provided by the embodiment of the application is realized. The virtual force in the Cartesian space is calculated. The size of the virtual force is in a negative correlation with the current operating index of the mechanical arm, that is, the larger the current operating index of the mechanical arm is, the smaller the virtual force is. The direction of the virtual force is the same as the direction in which the mechanical arm is away from the singularity. Then, the virtual force is introduced into the calculation of the input force signal of the constant force controller, so that the repulsive force from the singularity region is generated in the constant force control process of the mechanical arm, thereby realizing online singularity avoidance. The joint position is adjusted through the pose control amount in the Cartesian space, so that the end pose of the mechanical arm can be kept unchanged in the process of avoiding the singularity, thereby meeting the requirement of the constant force control on the end pose keeping.

[0057] Specifically, the kinematic model of the target mechanical arm is established in the step S101, and the establishment specifically includes the following steps.

[0058] Step S201: Local coordinate systems are established on each link of the target mechanical arm.

[0059] Specifically, the MDH (Modified Denavit-Hartenberg) method is used to establish the local coordinate systems on each link of the mechanical arm.

[0060] Step S202: The transformation relationship of the local coordinate systems corresponding to two adjacent links is calculated respectively.

[0061] Specifically, it is assumed that f i represents the local coordinate system attached to the link l i , where i represents the link number. The transformation relationship of the local coordinate systems f i and f j on two adjacent links l i and l j is described by the following matrix.

[0062]

[0063] wherein, represents the transformation relationship of the local coordinate systems f i and f j , respectively represents the description of the unit vector along each axis of the local coordinate system f j in the local coordinate system f i . represents the description of the origin of the local coordinate system f j in the local coordinate system f i .

[0064] Specifically, the calculation manner of the Jacobian matrix and the operating index based on the kinematic model in the step S101 specifically includes the following steps.

[0065] Step S301: Calculate the Jacobian matrix based on the joint properties of the joint formed by two adjacent links on the target robot arm and the transformation relationship of the corresponding local coordinate system.

[0066] Among them, joint properties include: joint degrees of freedom and joint types.

[0067] Specifically, the Jacobian matrix can be described by the following formula:

[0068]

[0069] Where J(q) represents the Jacobian matrix, q represents the joint angle vector, and n represents the degree of freedom of the manipulator joint; κ i Indicates the type of the i-th joint. If the joint is a rotation type, the value is 0; Represents κ i The inverted value; L i,n represents the local coordinate system f i Origin to local coordinate system f n The vector of the origin.

[0070] The Jacobian matrix can be expressed as follows:

[0071]

[0072] Where q represents the joint angle vector; J represents the Jacobian matrix; J T (q), J R (q) is a 3×n matrix, which represents the part of the Jacobi matrix that maps the joint angular velocity to the translation velocity and rotation velocity of the end of the robot arm in Cartesian space.

[0073] Step S302: Calculate the distance from the target manipulator at the current joint angle to the singular point based on the Jacobi matrix.

[0074] Step S303: Determine the distance as the operation index.

[0075] Specifically, as shown in the following formula, the distance from the robot's current posture to the singular point is usually used as the operation index to measure the singularity of the robot's current posture. (This is the existing technology)

[0076]

[0077] During constant force control, only the workspace position of the end effector output by the interpolator is adjusted, and the workspace posture of the end effector output by the interpolator needs to be kept unchanged. Therefore, the augmented translation Jacobian matrix shown below is introduced into the above formula.

[0078]

[0079] The operation index can be described by the following formula.

[0080]

[0081] wherein ω(q) represents an operation matrix, represents an augmented translation Jacobian matrix, represents a pseudo-inverse matrix of a rotation Jacobian matrix; represents a transpose matrix of a translation Jacobian matrix.

[0082] Specifically, the above step S102 specifically comprises the following steps:

[0083] Step S401: Obtain the current joint angle of the target robot arm.

[0084] Step S402: Calculate the virtual force scaling factor of the current joint angle based on the preset virtual force parameter.

[0085] The preset virtual force parameter comprises a gain factor, a virtual force enabling threshold and a virtual force limiting threshold.

[0086] Specifically, the virtual force scaling factor calculation formula is,

[0087]

[0088] wherein ξ(ω) represents the virtual force scaling factor, μ represents the gain factor; ω th represents the virtual force enabling threshold; ω cr represents the virtual force limiting threshold.

[0089] When the current operation index of the robot arm is less than the virtual force enabling threshold, the robot scaling factor is not equal to zero. The scaling factor is greater when the operation index of the robot arm is closer to the limiting threshold. In this way, the range of the virtual force can be clearly defined, and the robot will only be affected by the virtual force when it is close to the singular region, avoiding the influence of the virtual force on the constant force control throughout the process and thus affecting the effect of the constant force control.

[0090] Step S403: Calculate the projection of the Cartesian space virtual force direction vector in different directions after the end of the target robot arm moves at the first speed and the second speed in different directions in the next control cycle based on the current joint angle, the Jacobian matrix and the calculation method of the operation index.

[0091] The first speed and the second speed are opposite in direction.

[0092] Specifically, the step S403 calculates the second joint angle and the third joint angle corresponding to the target robot end moving at the first speed and the second speed respectively along the current direction in the next control period based on the current joint angle and the Jacobian matrix calculation mode; calculates the first operation index, the second operation index and the third operation index corresponding to the current joint angle, the second joint angle and the third joint angle respectively by using the Jacobian matrix and the operation index calculation mode; and determines the projection of the Cartesian space virtual force direction vector of the target robot end in the current direction based on the relationship among the first operation index, the second operation index and the third operation index.

[0093] Further, the Cartesian space virtual force direction vector can be expressed as:

[0094] A(q) = [A x A y A z A Rx A Ry A Rz ] T

[0095] wherein A x represents the projection of the direction vector in the x-axis translation direction, A Rx represents the projection of the direction vector in the x-axis rotation direction, and other symbols have the same meaning.

[0096] Hereinafter, the calculation process of A x is described as an example: the following formula can represent the joint angle value corresponding to the movement of the robot end at the speed V′ + (i.e. the first speed) in the positive direction of the x-axis for 1 control period.

[0097] q′ + = q0 + J + V′ + Δt

[0098] wherein q0 represents the joint angle value of the robot before the singularity avoidance; J + represents the pseudo-inverse matrix of the Jacobian matrix; V′ + = [ +1 0 0 0 0 0]T represents the speed vector with a positive sign; and Δt represents the control period.

[0099] The operation index change caused by the movement of the robot end in the Cartesian space can be described by the following formula.

[0100] Δω + = ω(q′ + )- ω(q0)

[0101] Similarly, q′ _ and Δω can be calculated.- .

[0102] The end of the mechanical arm moves at a speed V' along the x-axis - (i.e. the second speed) for one control period, and the corresponding joint angle value is:

[0103] q' _ = q0+ J + V' _ Δt

[0104] where V' _ = [-1 0 0 0 0 0] T represents a speed vector with a negative sign.

[0105] The change in the operation index caused by the movement of the end of the mechanical arm in the negative direction of the x-axis in the Cartesian space can be described by the following formula.

[0106] Δω - = ω(q' - )- ω(q0)

[0107] The direction vector element A x can be represented by the following formula.

[0108]

[0109] Step S404: Based on the projection of the Cartesian space virtual force direction vector of the target mechanical arm end in different directions and the virtual force scaling factor, the Cartesian space virtual force is calculated.

[0110] Specifically, referring to the calculation method of the above-mentioned direction vector element A x , all elements in A(q) can be calculated in the same way. The Cartesian space virtual force can be described by the following formula:

[0111] F v = ζ(ω)A(q)

[0112] where ξ(ω) represents the virtual force scaling factor, and A(q) represents the Cartesian space virtual force direction vector.

[0113] At this time, the size of the virtual force is negatively related to the current pose operation index of the mechanical arm, that is, the larger the current operation index, the smaller the virtual force. The direction of the virtual force is the same as the direction of moving the end of the mechanical arm away from the singularity.

[0114] Specifically, the above-mentioned step S103 specifically includes the following steps:

[0115] Step S501: Obtain the environmental force and the desired force of the target mechanical arm end.

[0116] Specifically, the environmental force can be obtained by a force sensor mounted on the end of the robot arm, or can be estimated by using a robot arm dynamics model combined with joint motion information, and the desired force is a force desired to be applied to control the robot arm.

[0117] Step S502: Calculate the force difference between the desired force and the Cartesian space virtual force and the environmental force.

[0118] Specifically, the force difference can be calculated by the following formula:

[0119] ΔF = F d - F v - F e

[0120] Where, ΔF is the force difference, F v represents the Cartesian space virtual force, F e represents the environmental force, and F d represents the desired force.

[0121] Step S503: Input the force difference into the constant force controller to calculate the Cartesian space position offset.

[0122] Step S504: Sum the Cartesian space position offset and the target desired position to obtain the Cartesian space pose control amount.

[0123] Specifically, the user sets the parameters according to the actual use scene. The desired position signal P d should be located within the workspace of the robot arm. The desired force signal F d should be less than the load weight of the robot arm. By using the mobility control law to establish a constant force controller, the force signal ΔF is input into the constant force controller to obtain the Cartesian space velocity command value V c . The discrete integral method is used to calculate the Cartesian space position offset ΔP according to the velocity command value V c (the specific calculation process is a prior art, which can be implemented by referring to the related calculation method in the prior art, and will not be described here). Finally, the desired position P d and the position offset ΔP are added to obtain the Cartesian space position control amount P c .

[0124] Since the Cartesian space virtual force is introduced into the input force signal of the constant force controller, and the direction of the Cartesian space virtual force is such that the end of the robot arm is away from the singularity point, and the size is determined by the operation index of the current pose, that is, the closer the robot arm is to the singular pose, the greater the virtual force that escapes from the singularity point. Therefore, the virtual force has the ability to pull the end of the robot arm away from the singular region.

[0125] The Cartesian space position control amount P cAs the input quantity of the motion controller, the robot drives to complete a control cycle of motion, and repeatedly calculates the control quantity P of the Cartesian space position c and drives the robot to move until the operation index corresponding to the pose of the robot is greater than the virtual force enabling threshold to complete the singular point avoidance. Thus, the robot motion caused by modifying the Jacobian matrix is avoided, and the predictability and controllability of the robot motion state are ensured. Through the calculation of the virtual force in the Cartesian space and the combination of the linearly extended Jacobian matrix, the attitude of the mechanical arm end in the constant force control process of avoiding the singular point can be kept unchanged.

[0126] The prior art calculates the repulsion velocity in the joint space of the mechanical arm through the potential field method, then calculates the synthesized velocity in the joint space, and finally drives the mechanical arm to move to avoid the singular point. The embodiment of the present application calculates the Cartesian space virtual force in the working space of the mechanical arm, then introduces the Cartesian space virtual force into the constant force controller to calculate the working space velocity instruction, converts the working space velocity instruction into the joint velocity instruction through the augmented translation Jacobian matrix, and finally calculates the joint position instruction according to the joint velocity instruction by using the discrete integral method. Compared with the prior art, the advantages of this are that the mechanical arm end attitude can be kept unchanged in the process of avoiding the singular point, meeting the requirement of constant force control on the end attitude keeping.

[0127] In the process of calculating the joint space repulsion velocity by using the potential field method, the prior art relies on the partial derivative of the condition number with respect to the joint angle value. In the process of calculating the working space repulsion velocity, the embodiment of the present application uses a numerical solution method. Compared with the prior art, there is no need to solve the partial derivative of the operation index with respect to each joint angle, reducing the algorithm implementation complexity.

[0128] By performing the above steps, the singular point avoidance method in the constant force control process of the mechanical arm provided by the embodiment of the present application calculates the Cartesian space virtual force, the size of the virtual force is negatively related to the current operation index of the mechanical arm, that is, the larger the current operation index of the mechanical arm is, the smaller the virtual force is, and the direction of the virtual force is the same as the direction of making the mechanical arm away from the singular point. Then, by introducing the virtual force into the calculation of the input force signal of the constant force controller, the repulsion force from the singular region in the constant force control process of the mechanical arm can be obtained, so as to realize online singular point avoidance, and the joint position is adjusted through the Cartesian space pose control quantity, so that the mechanical arm end attitude can be kept unchanged in the process of avoiding the singular point, meeting the requirement of constant force control on the end attitude keeping.

[0129] The embodiment of the present application also provides a singular point avoidance device in a constant force control process of a mechanical arm, as shown in Figure 2 The singular point avoidance device in the constant force control process of the mechanical arm specifically comprises:

[0130] The first processing module 101 is used to establish a kinematic model of the target manipulator and determine a calculation method of the Jacobian matrix and the operation index based on the kinematic model. For details, please refer to the description of step S101 in the above method embodiment, which will not be repeated here.

[0131] The second processing module 102 is used to calculate the Cartesian space virtual force based on the joint angle of the target manipulator and using the calculation method of the Jacobian matrix and the operation index. For details, please refer to the relevant description of step S102 in the above method embodiment, which will not be repeated here.

[0132] The third processing module 103 is used to input the Cartesian space virtual force into the constant force controller of the target manipulator to obtain the Cartesian space posture control value. For details, please refer to the relevant description of step S103 in the above method embodiment, which will not be repeated here.

[0133] The fourth processing module 104 is configured to control the motion of the target robotic arm using the Cartesian space pose control variable and to re-invoke the second processing module 102 until the operation index corresponding to the target robotic arm exceeds the virtual force enable threshold. For details, see the description of step S104 in the above method embodiment and will not be repeated here.

[0134] The further functional description of each of the above modules is the same as that of the above corresponding method embodiments and will not be repeated here.

[0135] Through the coordinated cooperation of the above-mentioned components, the singularity avoidance device in the process of constant force control of the manipulator provided by the embodiment of the present invention calculates the Cartesian space virtual force. The magnitude of the virtual force is negatively correlated with the current operation index of the manipulator, that is, the larger the current operation index of the manipulator, the smaller the virtual force. At the same time, the direction of the virtual force is the same as the direction that makes the manipulator away from the singularity. Then, by introducing the virtual force into the calculation of the input force signal of the constant force controller, the manipulator can be subjected to the repulsive force from the singular area during the constant force control process, thereby achieving online singularity avoidance, and adjusting the joint position through the Cartesian space posture control amount, so that the posture of the end of the manipulator can be kept unchanged in the process of avoiding the singularity, meeting the requirements of constant force control for maintaining the posture of the end.

[0136] The embodiment of the present invention further provides an electronic device, such as Figure 3 As shown, the electronic device may include a processor 901 and a memory 902, wherein the processor 901 and the memory 902 may be connected via a bus or other means. Figure 3 The bus connection is taken as an example.

[0137] The processor 901 can be a central processing unit (CPU). The processor 901 can also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, or combinations thereof.

[0138] The memory 902, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs, non-transitory computer-executable programs and modules, such as program instructions / modules corresponding to the method in the embodiments of the present application. The processor 901 performs various functional applications and data processing of the processor by running the non-transitory software programs, instructions and modules stored in the memory 902, that is, implements the above method.

[0139] The memory 902 can include a program storage area and a data storage area, wherein the program storage area can store application programs required by the operation of the device and at least one function; the data storage area can store data created by the processor 901 and the like. In addition, the memory 902 can include a high-speed random access memory, and can also include a non-transitory memory, such as at least one magnetic disk storage device, a flash memory device, or other non-transitory solid-state memory device. In some embodiments, the memory 902 can optionally include a memory disposed remotely with respect to the processor 901, and these remote memories can be connected to the processor 901 through a network. Examples of the above network include but are not limited to the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0140] One or more modules are stored in the memory 902, and when executed by the processor 901, the above method is performed.

[0141] The above electronic device can correspond to the specific details of the above method embodiments, and the corresponding related descriptions and effects can be understood, which will not be repeated here.

[0142] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program. The implemented program can be stored in a computer readable storage medium. When the program is executed, it can include the processes of the above-mentioned embodiment methods. The storage medium can be a magnetic disc, an optical disc, a read-only memory (ROM), a random access memory (RAM), a flash memory, a hard disk drive (HDD) or a solid-state drive (SSD), etc. The storage medium can also include a combination of the above-mentioned types of memories.

[0143] The above embodiments are only used to illustrate the technical solutions of the present application but not to limit the present application. Although the present application has been described in detail with reference to the above embodiments, those skilled in the art should understand that the specific embodiments of the present application can be modified or replaced equivalently without departing from the spirit and scope of the present application, and any modification or equivalent replacement should be covered in the scope of the claims of the present application.

Claims

1. A singularity avoidance method in a constant force control process of a robot arm, characterized by, The method comprises the following steps: establishing a kinematics model of a target robot arm, and determining a calculation method of a Jacobian matrix and an operation index based on the kinematics model; calculating a virtual force in a Cartesian space based on joint angles of the target robot arm and the calculation method of the Jacobian matrix and the operation index; inputting the virtual force in the Cartesian space into a constant force controller of the target robot arm to obtain a pose control amount in the Cartesian space; controlling movement of the target robot arm by using the pose control amount in the Cartesian space, and returning to the step of calculating the virtual force in the Cartesian space based on the joint angles of the target robot arm and the calculation method of the Jacobian matrix and the operation index until an operation index corresponding to the target robot arm is greater than a virtual force enabling threshold; the step of establishing the kinematics model of the target robot arm comprises: establishing a local coordinate system on each link of the target robot arm; calculating a conversion relationship of the local coordinate systems corresponding to two adjacent links respectively; the step of determining the calculation method of the Jacobian matrix and the operation index based on the kinematics model comprises: calculating the Jacobian matrix based on joint attributes of a joint formed by two adjacent links on the target robot arm and the conversion relationship of the local coordinate systems, the joint attributes comprising a joint degree of freedom and a joint type; calculating a distance of the target robot arm from a current joint angle to a singular point based on the Jacobian matrix; determining the distance as the operation index.

2. The method of claim 1, wherein, the step of calculating the virtual force in the Cartesian space based on the joint angles of the target robot arm and the calculation method of the Jacobian matrix and the operation index comprises: obtaining current joint angles of the target robot arm; calculating a virtual force scaling factor of the current joint angles based on preset virtual force parameters, the preset virtual force parameters comprising a gain factor, a virtual force enabling threshold and a virtual force limiting threshold; calculating projections of a virtual force direction vector of an end of the target robot arm in different directions after the end moves at a first speed and a second speed in different directions in a next control cycle based on the current joint angles, the calculation method of the Jacobian matrix and the operation index, the first speed being opposite in direction to the second speed; calculating the virtual force in the Cartesian space based on the projections of the virtual force direction vector of the end of the target robot arm in different directions and the virtual force scaling factor.

3. The method of claim 2, wherein, the step of calculating the projections of the virtual force direction vector of the end of the target robot arm in different directions after the end moves at the first speed and the second speed in different directions in the next control cycle based on the current joint angles, the calculation method of the Jacobian matrix and the operation index comprises: calculating second joint angles and third joint angles of the end of the target robot arm after the end moves at the first speed and the second speed respectively in a current direction in the next control cycle based on the current joint angles and the calculation method of the Jacobian matrix; calculating first, second and third operation indexes corresponding to the current joint angles, the second joint angles and the third joint angles respectively by using the calculation method of the Jacobian matrix and the operation index; Based on the relationship between the first operation index, the second operation index and the third operation index, a projection of the Cartesian space virtual force direction vector of the target robotic arm end in the current direction is determined.

4. The method of claim 1, wherein, The step of inputting the Cartesian space virtual force into the constant force controller of the target manipulator to obtain the Cartesian space posture control value includes: Obtaining the environmental force and the expected force at the end of the target robotic arm; Calculating a force difference between the desired force and the Cartesian space virtual force and the environmental force; Inputting the force difference into the constant force controller to calculate the Cartesian space position offset; The Cartesian space position offset and the target desired position are summed to obtain the Cartesian space posture control value.

5. The method of claim 1, wherein, The method of controlling the movement of the target robotic arm by using the Cartesian space posture control variable includes: The Cartesian space posture control quantity is used to control the target manipulator to move in the current control cycle.

6. A singularity avoidance device in a constant force control process of a robot arm, characterized by, include: A first processing module is used to establish a kinematic model of the target robotic arm and determine a calculation method of a Jacobi matrix and an operation index based on the kinematic model; The kinematic model of the target manipulator is established, including: establishing a local coordinate system on each link of the target manipulator; respectively calculating the transformation relationship between two adjacent links corresponding to the local coordinate system; the calculation method of determining the Jacobi matrix and the operation index based on the kinematic model includes: calculating the Jacobi matrix based on the joint properties of the joint formed by two adjacent links on the target manipulator and the transformation relationship of the corresponding local coordinate system, the joint properties including: joint degrees of freedom and joint types; calculating the distance from the current joint angle of the target manipulator to the singular point based on the Jacobi matrix; and determining the distance as the operation index; A second processing module is configured to calculate a Cartesian space virtual force based on the joint angle of the target robotic arm and by using a Jacobi matrix and an operation index calculation method; a third processing module, configured to input the Cartesian space virtual force into the constant force controller of the target manipulator to obtain a Cartesian space posture control variable; The fourth processing module is used to control the movement of the target robotic arm using the Cartesian space posture control quantity, and return to the step of calculating the Cartesian space virtual force based on the joint angle of the target robotic arm and using the calculation method of the Jacobi matrix and the operation index, until the operation index corresponding to the target robotic arm is greater than the virtual force enable threshold.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions, and when the computer instructions are executed by a processor, the method according to any one of claims 1 to 5 is implemented.

8. An electronic device, comprising: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the method according to any one of claims 1 to 5 by executing the computer instructions.

Images