CN116945147A - Singular point avoiding method and device in mechanical arm constant force control process - Google Patents
Singular point avoiding method and device in mechanical arm constant force control process Download PDFInfo
- Publication number
- CN116945147A CN116945147A CN202210389123.3A CN202210389123A CN116945147A CN 116945147 A CN116945147 A CN 116945147A CN 202210389123 A CN202210389123 A CN 202210389123A CN 116945147 A CN116945147 A CN 116945147A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- cartesian space
- force
- operation index
- virtual force
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 84
- 230000008569 process Effects 0.000 title claims abstract description 35
- 239000011159 matrix material Substances 0.000 claims abstract description 63
- 238000004364 calculation method Methods 0.000 claims abstract description 45
- 230000015654 memory Effects 0.000 claims description 20
- 238000012545 processing Methods 0.000 claims description 16
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000007613 environmental effect Effects 0.000 claims description 5
- 238000012423 maintenance Methods 0.000 abstract description 7
- 238000013519 translation Methods 0.000 description 6
- 230000008859 change Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 4
- 230000003190 augmentative effect Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 239000012636 effector Substances 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000005498 polishing Methods 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 210000000707 wrist Anatomy 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1669—Programme controls characterised by programming, planning systems for manipulators characterised by special application, e.g. multi-arm co-operation, assembly, grasping
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The invention provides a singular point avoiding method and a singular point avoiding device in a constant force control process of a mechanical arm, wherein the method comprises the following steps: the calculation mode of the jacobian matrix and the operation index is determined based on the kinematic model by establishing the kinematic model of the target mechanical arm; calculating Cartesian space virtual force based on the joint angle of the target mechanical arm and by using a calculation mode of a jacobian matrix and an operation index; inputting the Cartesian space virtual force into a constant force controller of a target mechanical arm to obtain the Cartesian space pose control quantity; and controlling the target mechanical arm to move by using the Cartesian space pose control quantity until the operation index corresponding to the target mechanical arm is greater than the virtual force enabling threshold. The method comprises the steps of calculating the Cartesian space virtual force, introducing the Cartesian space virtual force into the calculation of the input force signal of the constant force controller, enabling the mechanical arm to receive repulsive force from a singular area in the constant force control process, achieving online singular point avoidance, and meeting the requirements of constant force control on terminal gesture maintenance.
Description
Technical Field
The invention relates to the technical field of the avoidance of singular points of mechanical arms, in particular to a method and a device for avoiding the singular points in the constant force control process of the mechanical arms,
background
At present, the constant force control is mainly applied to the fields of polishing, assembly and the like. The working environment is known in the scenes, and an operator can obtain a working path for avoiding the singular point through off-line programming or on-site teaching and the like, so that the problem of avoiding the singular point is not needed to be considered in the constant force control working process. But this approach cannot be applied to scenarios where the job environment is not known in advance or where the job path needs to be generated online.
At present, aiming at solving the problem of avoiding singular points of a mechanical arm in a Cartesian space, known schemes mainly comprise a potential field method, a damping reciprocal method and variants thereof. For example: the application number is: 202110968731.5, the name is: a general avoidance method of singular points of a mechanical arm and Chinese patent of the system disclose that a repulsive potential field is formed by singular points, joint space repulsive speed is calculated according to the repulsive potential field, cartesian space repulsive speed is calculated by means of a jacobian matrix, final joint speed is obtained by means of calculation by combining driving speed, and avoidance of the singular points is completed. However, the attitude of the robot end tool in the Cartesian space may change during the application of the method, which is not suitable for the online constant force control process of which the working environment is not known in advance. The application number is: 202011439303.5, the name is: chinese patent invention patent of a method and a system for avoiding singular points of an aspheric wrist mechanical arm discloses a method for deriving singular factors of the aspheric wrist mechanical arm by utilizing a jacobian matrix change method. However, the method modifies the jacobian matrix, and introduces a variable quantity when the Cartesian space velocity is converted into the joint space velocity, so that the position of the end tool of the robot is uncontrollably deviated from the expected value.
In summary, in the prior art, the singular point avoidance scheme of the mechanical arm can affect the pose of the end tool, so that the requirement of constant force control on the end pose maintenance is difficult to meet.
Disclosure of Invention
In view of the above, the embodiments of the present invention provide a method and an apparatus for avoiding singular points in a constant force control process of a mechanical arm, so as to solve the problem that in the prior art, the singular point avoiding mode of the mechanical arm may cause the pose of an end tool to change, so that the requirement of constant force control on maintaining the pose of the end is not met.
According to a first aspect, an embodiment of the present invention provides a method for avoiding singular points in a constant force control process of a mechanical arm, including:
establishing a kinematic model of a target mechanical arm, and determining a calculation mode of a jacobian matrix and an operation index based on the kinematic model;
calculating Cartesian space virtual force based on the joint angle of the target mechanical arm and by using a calculation mode of a jacobian matrix and an operation index;
inputting the Cartesian space virtual force into a constant force controller of the target mechanical arm to obtain a Cartesian space pose control quantity;
and controlling the movement of the target mechanical arm by using the Cartesian space pose control quantity, returning to the step of calculating the Cartesian space virtual force by using a calculation mode based on the joint angle of the target mechanical arm and using a jacobian matrix and an operation index until the operation index corresponding to the target mechanical arm is larger than a virtual force enabling threshold.
Optionally, the establishing a kinematic model of the target mechanical arm includes:
establishing a local coordinate system on each connecting rod of the target mechanical arm;
and respectively calculating the conversion relation of the local coordinate systems corresponding to the two adjacent connecting rods.
Optionally, the calculating manner of determining the jacobian matrix and the operation index based on the kinematic model includes:
based on the joint attribute of the joint formed by two adjacent connecting rods on the target mechanical arm and the conversion relation of the corresponding local coordinate system, calculating a jacobian matrix, wherein the joint attribute comprises: degree of freedom of joint and type of joint;
calculating the distance from the current joint angle to the singular point of the target mechanical arm based on the jacobian matrix;
the distance is determined as the operation index.
Optionally, the calculating the cartesian space virtual force based on the joint angle of the target mechanical arm and by using a jacobian matrix and an operation index calculation mode includes:
acquiring the current joint angle of the target mechanical arm;
calculating a virtual force scaling factor of the current joint angle based on preset virtual force parameters, wherein the preset virtual force parameters comprise: gain factor, virtual force enable threshold, virtual force limit threshold;
calculating the projection of the Cartesian space virtual force direction vector in different directions after the tail end of the target mechanical arm moves in different directions at a first speed and a second speed in the next control period based on the current joint angle, the jacobian matrix and the calculation mode of the operation index, wherein the first speed is opposite to the second speed in the speed direction;
and calculating the Cartesian space virtual force based on the projection of the Cartesian space virtual force direction vector at the tail end of the target mechanical arm in different directions and the virtual force scaling factors.
Optionally, the calculating, based on the current joint angle, the jacobian matrix, and the calculation manner of the operation index, the projection of the cartesian space virtual force direction vector on the next control period after the movement of the target manipulator end in the first and second directions, includes:
based on the current joint angle and the jacobian matrix calculation mode, calculating a second joint angle and a third joint angle corresponding to the tail end of the target mechanical arm after moving at a first speed and a second speed along the current direction in the next control period;
calculating a first operation index, a second operation index and a third operation index corresponding to the current joint angle, the second joint angle and the third joint angle respectively by using a calculation mode of the jacobian matrix and the operation index;
and determining the projection of the Cartesian space virtual force direction vector of the tail end of the target mechanical arm in the current direction based on the relation among the first operation index, the second operation index and the third operation index.
Optionally, the inputting the cartesian space virtual force into the constant force controller of the target mechanical arm obtains a cartesian space pose control amount, including:
acquiring the environmental force and the expected force of the tail end of the target mechanical arm;
calculating a force difference of the desired force with the cartesian space virtual force and the ambient force;
inputting the force difference into the constant force controller to calculate a Cartesian space position offset;
and summing the Cartesian space position offset and the target expected position to obtain the Cartesian space pose control quantity.
Optionally, the controlling the movement of the target mechanical arm by using the cartesian space pose control amount includes:
and controlling the target mechanical arm to move in the current control period by using the Cartesian space pose control quantity.
According to a second aspect, an embodiment of the present invention provides a singular point avoidance apparatus in a mechanical arm constant force control process, including:
the first processing module is used for establishing a kinematic model of the target mechanical arm and determining a calculation mode of the jacobian matrix and the operation index based on the kinematic model;
the second processing module is used for calculating the Cartesian space virtual force based on the joint angle of the target mechanical arm and by using a calculation mode of a jacobian matrix and an operation index;
the third processing module is used for inputting the Cartesian space virtual force into the constant force controller of the target mechanical arm to obtain the Cartesian space pose control quantity;
and the fourth processing module is used for controlling the movement of the target mechanical arm by using the Cartesian space pose control quantity, returning to the step of calculating the Cartesian space virtual force by using the calculation mode based on the joint angle of the target mechanical arm and using the jacobian matrix and the operation index until the operation index corresponding to the target mechanical arm is larger than a virtual force enabling threshold value.
According to a third aspect, embodiments of the present invention provide a computer readable storage medium storing computer instructions which, when executed by a processor, implement a method according to the first aspect of the present invention and any one of its alternatives.
According to a fourth aspect, an embodiment of the present invention provides an electronic device, including: the system comprises a memory and a processor, wherein the memory and the processor are in communication connection, the memory stores computer instructions, and the processor executes the computer instructions, so as to execute the method according to the first aspect of the invention and any optional mode thereof.
The technical scheme of the invention has the following advantages:
the embodiment of the invention provides a singular point avoidance method and a singular point avoidance device in a constant force control process of a mechanical arm, wherein 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; calculating Cartesian space virtual force based on the joint angle of the target mechanical arm and by using a calculation mode of a jacobian matrix and an operation index; inputting the Cartesian space virtual force into a constant force controller of a target mechanical arm to obtain the Cartesian space pose control quantity; and controlling the movement of the target mechanical arm by using the Cartesian space pose control quantity, returning to a calculation mode based on the joint angle of the target mechanical arm and by using the jacobian matrix and the operation index, and calculating the Cartesian space virtual force until the operation index corresponding to the target mechanical arm is larger than the virtual force enabling threshold. Therefore, the magnitude of the virtual force is in a negative correlation with the current operation index of the mechanical arm through calculation of the Cartesian space virtual force, namely, the larger the current operation index of the mechanical arm is, the smaller the virtual force is, and meanwhile, the direction of the virtual force is the same as the direction of the mechanical arm away from the singular point. And then introducing the virtual force into the calculation of the input force signal of the constant force controller, so that the repulsive force from the singular region can be received in the constant force control process of the mechanical arm, thereby realizing the on-line singular point avoidance, and the joint position is adjusted by the Cartesian space pose control quantity, so that the pose of the tail end of the mechanical arm can be kept unchanged in the process of avoiding the singular point, and the requirement of the constant force control on the maintenance of the pose of the tail end is met.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are needed in the description of the embodiments or the prior art will be briefly described, and it is obvious that the drawings in the description below are some embodiments of the present invention, and other drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flowchart of a method for avoiding singular points in a constant force control process of a mechanical arm according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a singular point avoidance device in a constant force control process of a mechanical arm according to an embodiment of the present invention;
fig. 3 is a schematic structural diagram of an electronic device according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made apparent and fully in view of the accompanying drawings, in which some, but not all embodiments of the invention are shown. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
In addition, the technical features of the different embodiments of the present invention described below may be combined with each other as long as they do not collide with each other.
At present, the constant force control is mainly applied to the fields of polishing, assembly and the like. The working environment is known in the scenes, and an operator can obtain a working path for avoiding the singular point through off-line programming or on-site teaching and the like, so that the problem of avoiding the singular point is not needed to be considered in the constant force control working process. But this approach cannot be applied to scenarios where the job environment is not known in advance or where the job path needs to be generated online. In the prior art, the singular point avoidance scheme of the mechanical arm can influence the pose of the tail end of the mechanical arm, and the requirement of constant force control on the maintenance of the pose of the tail end is difficult to meet.
Based on the above problems, the embodiment of the present invention provides a method for avoiding singular points in a constant force control process of an mechanical arm, as shown in fig. 1, where the method for avoiding singular points in the constant force control process of the mechanical arm specifically includes the following steps:
step S101: and establishing a kinematic model of the target mechanical arm, and determining a calculation mode of the jacobian matrix and the operation index based on the kinematic model.
Step S102: based on the joint angle of the target mechanical arm and by using the calculation mode of the jacobian matrix and the operation index, calculating the Cartesian space virtual force.
Step S103: and inputting the Cartesian space virtual force into a constant force controller of the target mechanical arm to obtain the Cartesian space pose control quantity.
Step S104: and controlling the target mechanical arm to move by using the Cartesian space pose control quantity, and returning to the step S102 until the operation index corresponding to the target mechanical arm is greater than the virtual force enabling threshold.
Specifically, the target mechanical arm is controlled to move in the current control period by using the Cartesian space pose control amount.
By executing the steps, the singular point avoidance method in the mechanical arm constant force control process provided by the embodiment of the invention calculates the Cartesian space virtual force, wherein the magnitude of the virtual force and the current operation index of the mechanical arm are in a negative correlation relationship, namely the larger the current operation index of the mechanical arm is, the smaller the virtual force is, and meanwhile, the direction of the virtual force is the same as the direction of the mechanical arm away from the singular point. And then introducing the virtual force into the calculation of the input force signal of the constant force controller, so that the repulsive force from the singular region can be received in the constant force control process of the mechanical arm, thereby realizing the on-line singular point avoidance, and the joint position is adjusted by the Cartesian space pose control quantity, so that the pose of the tail end of the mechanical arm can be kept unchanged in the process of avoiding the singular point, and the requirement of the constant force control on the maintenance of the pose of the tail end is met.
Specifically, the step S101 of establishing a kinematic model of the target mechanical arm specifically includes the following steps:
step S201: a local coordinate system is established on each link of the target robotic arm.
Specifically, a local coordinate system is established on each link of the robot arm by using an MDH (Modified device-Hartenberg) method.
Step S202: and respectively calculating the conversion relation of the local coordinate systems corresponding to the two adjacent connecting rods.
Specifically, assume f i Indicating attachment to the link l i And a local coordinate system on the upper part, wherein i represents a connecting rod serial number. The adjacent two links l are described by the following matrix i And l j Local coordinate system f i And f j Is a transformation relation of (a).
Wherein, the liquid crystal display device comprises a liquid crystal display device,representing a local coordinate system f i And f j Is a transformation relation of->Respectively along the local coordinate system f j The unit vector of each axis is in the local coordinate system f i Is described in (3). />Representing a local coordinate system f j Origin of (f) in local coordinate system f i Is described in (3).
Specifically, the calculation method for determining the jacobian matrix and the operation index based on the kinematic model in the step S101 specifically includes the following steps:
step S301: based on the joint attribute of the joint formed by two adjacent connecting rods on the target mechanical arm and the conversion relation of the corresponding local coordinate system, calculating the jacobian matrix.
Wherein the joint attributes include: degree of freedom of joint and type of joint.
Specifically, the jacobian matrix can be described by the following formula:
wherein J (q) represents a jacobian matrix, q represents a joint angle vector, and n represents a degree of freedom of a joint of the mechanical arm; kappa (kappa) i Indicating the type of the ith joint, and if the joint is of a rotation type, the value is 0;represent kappa i Taking the value after the inversion; l (L) i,n Representing a local coordinate system f i Origin and local coordinate System f n Vector of origin.
The jacobian matrix can be represented written in a form,
wherein q represents a joint angle vector; j represents a jacobian matrix; j (J) T (q)，J R (q) is a 3 xn matrix representing the portion of the jacobian matrix that maps joint angular velocity to manipulator end translation and rotation velocity in cartesian space, respectively.
Step S302: and calculating the distance from the current joint angle to the singular point of the target mechanical arm based on the jacobian matrix.
Step S303: the distance is determined as an operation index.
Specifically, as shown in the following formula, the singular degree of the current pose of the robot arm is generally measured using the distance from the current pose of the robot to the singular point as an operation index. (this is the prior art)
During constant force control, only the end effector workspace position of the interpolator output is adjusted and the end effector workspace pose of the interpolator output needs to be kept unchanged, so an augmented translation jacobian matrix as shown below is introduced in the above equation.
The operation index can be described by the following formula.
Wherein ω (q) represents an operation matrix,representing an augmented translation jacobian matrix, +.>A pseudo-inverse matrix representing a rotated jacobian matrix; />Representing the transpose of the translation jacobian.
Specifically, the step S102 specifically includes the following steps:
step S401: acquiring a current joint angle of a target mechanical arm;
step S402: and calculating a virtual force scaling factor of the current joint angle based on the preset virtual force parameter.
The preset virtual force parameters comprise: gain factor, virtual force enable threshold, and virtual force limit threshold.
In particular, the virtual force scaling factor is calculated by the formula,
where ζ (ω) represents the virtual force scaling factor and μ represents the gain factor; omega th Representing virtual forcesEnabling a threshold; omega cr Representing a virtual force limit threshold.
When the current operating index of the robotic arm is less than the virtual force enable threshold, the robotic scaling factor is not equal to zero. The greater the scaling factor as the manipulator operating index approaches the limit threshold. Therefore, the virtual force acting range can be defined, the virtual force can be acted only when the mechanical arm approaches to the singular region, and the influence of the virtual force on the whole constant force control process is avoided, so that the constant force control effect is influenced.
Step S403: and calculating the projection of the Cartesian space virtual force direction vector in different directions after the tail end of the target mechanical arm moves in different directions at the first speed and the second speed in the next control period based on the calculation mode of the current joint angle, the jacobian matrix and the operation index.
Wherein the first speed is opposite to the second speed.
Specifically, in the step S403, the second joint angle and the third joint angle corresponding to the target manipulator end moving at the first speed and the second speed in the current direction in the next control period are calculated by calculating based on the current joint angle and the jacobian matrix; calculating a first operation index, a second operation index and a third operation index corresponding to the current joint angle, the second joint angle and the third joint angle respectively by using a calculation mode of the jacobian matrix and the operation index; and determining the projection of the Cartesian space virtual force direction vector of the tail end of the target mechanical arm in the current direction based on the relation among the first operation index, the second operation index and the third operation index.
Further, the cartesian virtual force direction vector may be expressed as:
A(q)＝[A x A y A z A Rx A Ry A Rz ] T
wherein A is x Representing the projection of the direction vector in the x-axis translation direction, A Rx Representing the projection of the direction vector in the direction of rotation of the x-axis, other symbologies and so on.
Hereinafter referred to as A x For example, the calculation flow of a (q) is described:the following can represent the velocity V 'of the end of the mechanical arm in the positive direction of the x-axis' + The corresponding joint angle value after 1 control cycle of movement (i.e., the first speed).
q′ + ＝q 0 +J + V′ + Δt
Wherein q 0 Representing a mechanical arm joint angle value before the avoidance of the singular point; j (J) + A pseudo-inverse matrix representing a jacobian matrix; v'. + ＝[+1 0 0 0 0 0]T represents a velocity vector with positive sign; Δt represents a control period.
The change in the operation index due to the movement of the robot arm tip in the cartesian space can be described by the following formula.
Δω + ＝ω(q′ + )-ω(q 0 )
The same principle can calculate q' _ And Δω - 。
The end of the mechanical arm is at the speed V 'on the x axis' - The corresponding joint angle value after 1 control period of movement (i.e. the second speed) is:
q′ _ ＝q 0 +J + V′ _ Δt
wherein V 'is' _ ＝[-1 0 0 0 0 0] T Representing a velocity vector with a negative sign.
The change in the operating index due to the movement of the robot arm tip in the negative x-axis direction in cartesian space can be described by the following formula.
Δω - ＝ω(q′ - )-ω(q 0 )
Direction vector element A x Can be represented by the following formula.
Step S404: and calculating the Cartesian space virtual force based on the projection of the Cartesian space virtual force direction vector at the tail end of the target mechanical arm in different directions and the virtual force scaling factors.
Specifically, referring to the above-described direction vector element a x In the same way as that of (a)The processor may calculate all elements in a (q). The Cartesian space virtual force can be described by the following formula:
F v ＝ζ(ω)A(q)
where ζ (ω) represents the virtual force scaling factor and a (q) represents the cartesian space virtual force direction vector.
At this time, the magnitude of the virtual force and the current pose operation index of the mechanical arm are in a negative correlation relationship, namely, the larger the current operation index is, the smaller the virtual force is. The direction of the virtual force is the same as the direction that the end of the mechanical arm is away from the singular point.
Specifically, the step S103 specifically includes the following steps:
step S501: and acquiring the environmental force and the expected force of the tail end of the target mechanical arm.
Specifically, the environmental force may be obtained by a force sensor mounted on the distal end of the mechanical arm, or may be estimated by using a mechanical arm dynamics model in combination with joint motion information, and the desired force is the force desired to be applied to control the mechanical arm.
Step S502: the force difference of the desired force from the cartesian space virtual force and the ambient force is calculated.
Specifically, the force difference may be calculated by the following formula:
ΔF＝F d -F v -F e
wherein, deltaF force is poor, F v Representing Cartesian space virtual forces, F e Representing environmental forces, F d Indicating the desired force.
Step S503: the force difference is input into a constant force controller to calculate the Cartesian space position offset.
Step S504: and summing the Cartesian space position offset and the target expected position to obtain the Cartesian space pose control quantity.
Specifically, the user performs parameter setting according to the actual use scenario. Desired position signal P d Should be located within the robot arm working space. Desired force signal F d Should be less than the robot arm load weight. By establishing a constant force controller using an admittance control law, a force signal DeltaF is input to the constant force controller to obtain a Cartesian space velocity command value V c . According to the speed command value V using discrete integration c The cartesian space position offset Δp is calculated (the specific calculation process is the prior art, and may be implemented by referring to the related calculation method in the prior art, which is not described herein in detail). Finally, the expected position P d Adding the position offset delta P to obtain a Cartesian space position control quantity P c 。
Because 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 enables the tail end of the mechanical arm to be far away from the singular point, the magnitude is determined by the operation index of the current pose, namely, the virtual force which is separated from the singular point is larger when the mechanical arm is closer to the odd pose. Thus enabling the virtual force to pull the end of the arm away from the odd region.
By Cartesian space position control quantity P c The robot is driven to complete the motion of one control period by the input quantity as the motion controller, and the Cartesian space position control quantity P is repeatedly calculated c And driving the robot to move until the operation index corresponding to the pose of the robot is larger than the virtual force enabling threshold value to finish the avoidance of the singular point. Therefore, robot motion caused by modifying the jacobian matrix is avoided, and predictability and controllability of the motion state of the robot are ensured. According to the embodiment of the invention, the attitude of the tail end of the mechanical arm can be kept unchanged in the constant force control process of the robot for avoiding singular points by calculating the virtual force in the Cartesian space and combining with the linear expansion jacobian matrix.
In the prior art, the repulsive speed is calculated in the joint space of the mechanical arm through a potential field method, then the synthetic speed is calculated in the joint space, and finally the mechanical arm is driven to move so as to avoid singular points. According to the embodiment of the invention, the Cartesian space virtual force is calculated in the working space of the mechanical arm, then the Cartesian space virtual force is introduced into a constant force controller to calculate a working space speed instruction, the working space speed instruction is converted into a joint speed instruction through an augmented translation jacobian matrix, and finally the joint position instruction is calculated according to the joint speed instruction by using a discrete integration method. Compared with the prior art, the method has the advantages that the tail end gesture of the mechanical arm can be kept unchanged in the process of avoiding singular points, and the requirement of constant force control on tail end gesture keeping is met.
The prior art relies on the partial derivative of the condition number with respect to the joint angle value in calculating the joint space rejection rate using the potential field method. In the process of calculating the rejection rate of the working space, the embodiment of the invention uses a numerical solution. Compared with the prior art, the partial derivative of the operation index with respect to each joint angle is not needed to be solved, and the algorithm implementation complexity is reduced.
By executing the steps, the singular point avoidance method in the mechanical arm constant force control process provided by the embodiment of the invention calculates the Cartesian space virtual force, wherein the magnitude of the virtual force and the current operation index of the mechanical arm are in a negative correlation relationship, namely the larger the current operation index of the mechanical arm is, the smaller the virtual force is, and meanwhile, the direction of the virtual force is the same as the direction of the mechanical arm away from the singular point. And then introducing the virtual force into the calculation of the input force signal of the constant force controller, so that the repulsive force from the singular region can be received in the constant force control process of the mechanical arm, thereby realizing the on-line singular point avoidance, and the joint position is adjusted by the Cartesian space pose control quantity, so that the pose of the tail end of the mechanical arm can be kept unchanged in the process of avoiding the singular point, and the requirement of the constant force control on the maintenance of the pose of the tail end is met.
The embodiment of the invention also provides a singular point avoiding device in the constant force control process of the mechanical arm, as shown in fig. 2, which specifically comprises:
the first processing module 101 is configured to establish a kinematic model of the target mechanical arm, and determine a calculation mode of the jacobian matrix and the operation index based on the kinematic model. For details, refer to the related description of step S101 in the above method embodiment, and no further description is given here.
The second processing module 102 is configured to calculate a cartesian space virtual force based on the joint angle of the target manipulator and by using a jacobian matrix and a calculation method of an operation index. For details, refer to the related description of step S102 in the above method embodiment, and no further description is given here.
And the third processing module 103 is used for inputting the Cartesian space virtual force into the constant force controller of the target mechanical arm to obtain the Cartesian space pose control quantity. For details, see the description of step S103 in the above method embodiment, and the details are not repeated here.
The fourth processing module 104 is configured to control the movement of the target mechanical arm by using the cartesian space pose control amount, and recall the second processing module 102 until the operation index corresponding to the target mechanical arm is greater than the virtual force enabling threshold. For details, refer to the related description of step S104 in the above method embodiment, and no further description is given here.
Further functional descriptions of the above respective modules are the same as those of the above corresponding method embodiments, and are not repeated here.
Through the cooperative cooperation of the above components, the singular point avoidance device in the mechanical arm constant force control process provided by the embodiment of the invention calculates the Cartesian space virtual force, wherein the magnitude of the virtual force and the current operation index of the mechanical arm are in a negative correlation relationship, namely the larger the current operation index of the mechanical arm is, the smaller the virtual force is, and meanwhile, the direction of the virtual force is the same as the direction of the mechanical arm away from the singular point. And then introducing the virtual force into the calculation of the input force signal of the constant force controller, so that the repulsive force from the singular region can be received in the constant force control process of the mechanical arm, thereby realizing the on-line singular point avoidance, and the joint position is adjusted by the Cartesian space pose control quantity, so that the pose of the tail end of the mechanical arm can be kept unchanged in the process of avoiding the singular point, and the requirement of the constant force control on the maintenance of the pose of the tail end is met.
The present invention also provides an electronic device, as shown in fig. 3, which may include a processor 901 and a memory 902, where the processor 901 and the memory 902 may be connected by a bus or other means, and in fig. 3, the connection is exemplified by a bus.
The processor 901 may be a central processing unit (Central Processing Unit, CPU). The processor 901 may also be other general purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), field programmable gate arrays (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or a combination thereof.
The memory 902 is used as a non-transitory computer readable storage medium for storing non-transitory software programs, non-transitory computer executable programs, and modules, such as program instructions/modules corresponding to the methods of the embodiments of the present invention. The processor 901 performs various functional applications of the processor and data processing, i.e., implements the above-described methods, by running non-transitory software programs, instructions, and modules stored in the memory 902.
The memory 902 may include a storage program area and a storage data area, wherein the storage program area may store an operating device, at least one application program required for a function; the storage data area may store data created by the processor 901, and the like. In addition, the memory 902 may include high-speed random access memory, and may also include non-transitory memory, such as at least one magnetic disk storage device, flash memory device, or other non-transitory solid state storage device. In some embodiments, memory 902 optionally includes memory remotely located relative to processor 901, which may be connected to processor 901 via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
One or more modules are stored in the memory 902 that, when executed by the processor 901, perform the methods described above.
The specific details of the electronic device may be correspondingly understood by referring to the corresponding related descriptions and effects in the above method embodiments, which are not repeated herein.
It will be appreciated by those skilled in the art that implementing all or part of the above-described methods in the embodiments may be implemented by a computer program for instructing relevant hardware, and the implemented program may be stored in a computer readable storage medium, and the program may include the steps of the embodiments of the above-described methods when executed. The storage medium may be a magnetic Disk, an optical Disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), a Flash Memory (Flash Memory), a Hard Disk (HDD), or a Solid State Drive (SSD); the storage medium may also comprise a combination of memories of the kind described above.
The above embodiments are only for illustrating the technical aspects of the present invention and not for limiting the same, and although the present invention has been described in detail with reference to the above embodiments, it should be understood by those of ordinary skill in the art that: modifications and equivalents may be made to the specific embodiments of the invention without departing from the spirit and scope of the invention, which is intended to be covered by the scope of the claims.
Claims (10)
1. The singular point avoiding method in the constant force control process of the mechanical arm is characterized by comprising the following steps of:
establishing a kinematic model of a target mechanical arm, and determining a calculation mode of a jacobian matrix and an operation index based on the kinematic model;
calculating Cartesian space virtual force based on the joint angle of the target mechanical arm and by using a calculation mode of a jacobian matrix and an operation index;
inputting the Cartesian space virtual force into a constant force controller of the target mechanical arm to obtain a Cartesian space pose control quantity;
and controlling the movement of the target mechanical arm by using the Cartesian space pose control quantity, returning to the step of calculating the Cartesian space virtual force by using a calculation mode based on the joint angle of the target mechanical arm and using a jacobian matrix and an operation index until the operation index corresponding to the target mechanical arm is larger than a virtual force enabling threshold.
2. The method of claim 1, wherein the establishing a kinematic model of the target robotic arm comprises:
establishing a local coordinate system on each connecting rod of the target mechanical arm;
and respectively calculating the conversion relation of the local coordinate systems corresponding to the two adjacent connecting rods.
3. The method according to claim 2, wherein said determining the calculation of the jacobian matrix and the operation index based on the kinematic model comprises:
based on the joint attribute of the joint formed by two adjacent connecting rods on the target mechanical arm and the conversion relation of the corresponding local coordinate system, calculating a jacobian matrix, wherein the joint attribute comprises: degree of freedom of joint and type of joint;
calculating the distance from the current joint angle to the singular point of the target mechanical arm based on the jacobian matrix;
the distance is determined as the operation index.
4. The method of claim 1, wherein calculating the cartesian space virtual force based on the joint angle of the target robotic arm and using the jacobian matrix and the operation index calculation comprises:
acquiring the current joint angle of the target mechanical arm;
calculating a virtual force scaling factor of the current joint angle based on preset virtual force parameters, wherein the preset virtual force parameters comprise: gain factor, virtual force enable threshold, virtual force limit threshold;
calculating the projection of the Cartesian space virtual force direction vector in different directions after the tail end of the target mechanical arm moves in different directions at a first speed and a second speed in the next control period based on the current joint angle, the jacobian matrix and the calculation mode of the operation index, wherein the first speed is opposite to the second speed in the speed direction;
and calculating the Cartesian space virtual force based on the projection of the Cartesian space virtual force direction vector at the tail end of the target mechanical arm in different directions and the virtual force scaling factors.
5. The method of claim 4, wherein calculating the projection of the cartesian virtual force direction vector in different directions for the target manipulator end after the next control cycle moves in different directions at the first and second speeds based on the current joint angle, the jacobian matrix, and the calculation of the operation index, comprises:
based on the current joint angle and the jacobian matrix calculation mode, calculating a second joint angle and a third joint angle corresponding to the tail end of the target mechanical arm after moving at a first speed and a second speed along the current direction in the next control period;
calculating a first operation index, a second operation index and a third operation index corresponding to the current joint angle, the second joint angle and the third joint angle respectively by using a calculation mode of the jacobian matrix and the operation index;
and determining the projection of the Cartesian space virtual force direction vector of the tail end of the target mechanical arm in the current direction based on the relation among the first operation index, the second operation index and the third operation index.
6. The method of claim 1, wherein the inputting the cartesian space virtual force into the constant force controller of the target robotic arm results in a cartesian space pose control amount, comprising:
acquiring the environmental force and the expected force of the tail end of the target mechanical arm;
calculating a force difference of the desired force with the cartesian space virtual force and the ambient force;
inputting the force difference into the constant force controller to calculate a Cartesian space position offset;
and summing the Cartesian space position offset and the target expected position to obtain the Cartesian space pose control quantity.
7. The method of claim 1, wherein said controlling the target robotic arm motion using the cartesian space position control quantity comprises:
and controlling the target mechanical arm to move in the current control period by using the Cartesian space pose control quantity.
8. The utility model provides a singular point avoids device in arm constant force control process which characterized in that includes:
the first processing module is used for establishing a kinematic model of the target mechanical arm and determining a calculation mode of the jacobian matrix and the operation index based on the kinematic model;
the second processing module is used for calculating the Cartesian space virtual force based on the joint angle of the target mechanical arm and by using a calculation mode of a jacobian matrix and an operation index;
the third processing module is used for inputting the Cartesian space virtual force into the constant force controller of the target mechanical arm to obtain the Cartesian space pose control quantity;
and the fourth processing module is used for controlling the movement of the target mechanical arm by using the Cartesian space pose control quantity, returning to the step of calculating the Cartesian space virtual force by using the calculation mode based on the joint angle of the target mechanical arm and using the jacobian matrix and the operation index until the operation index corresponding to the target mechanical arm is larger than a virtual force enabling threshold value.
9. A computer readable storage medium storing computer instructions which, when executed by a processor, implement the method of any one of claims 1-7.
10. An electronic device, comprising:
a memory and a processor, the memory and the processor being communicatively coupled to each other, the memory having stored therein computer instructions that, when executed, cause the processor to perform the method of any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210389123.3A CN116945147A (en) | 2022-04-13 | 2022-04-13 | Singular point avoiding method and device in mechanical arm constant force control process |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210389123.3A CN116945147A (en) | 2022-04-13 | 2022-04-13 | Singular point avoiding method and device in mechanical arm constant force control process |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116945147A true CN116945147A (en) | 2023-10-27 |
Family
ID=88458905
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210389123.3A Pending CN116945147A (en) | 2022-04-13 | 2022-04-13 | Singular point avoiding method and device in mechanical arm constant force control process |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116945147A (en) |
-
2022
- 2022-04-13 CN CN202210389123.3A patent/CN116945147A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109159151B (en) | Mechanical arm space trajectory tracking dynamic compensation method and system | |
| JP6576255B2 (en) | Robot trajectory generation method, robot trajectory generation apparatus, and manufacturing method | |
| US11845186B2 (en) | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same | |
| CN106945043B (en) | Multi-arm cooperative control system of master-slave teleoperation surgical robot | |
| CN111002315B (en) | Trajectory planning method and device and robot | |
| CN112417755A (en) | Master-slave mode surgical robot track prediction control method | |
| Fan et al. | Data-driven motion-force control scheme for redundant manipulators: A kinematic perspective | |
| JP6750909B2 (en) | Robot trajectory generation method, robot trajectory generation apparatus, and manufacturing method | |
| KR102030141B1 (en) | Method and system for controlling elbow of robot | |
| CN111702767A (en) | Manipulator impedance control method based on inversion fuzzy self-adaptation | |
| CN111684380A (en) | Robot motion control method, control system and storage device | |
| CN111230860B (en) | Robot control method, robot control device, computer device, and storage medium | |
| CN114055467B (en) | Space pose online simulation system based on five-degree-of-freedom robot | |
| CN111515928A (en) | Mechanical arm motion control system | |
| Liu et al. | Frequency-division based hybrid force/position control of robotic arms manipulating in uncertain environments | |
| CN115890735B (en) | Mechanical arm system, mechanical arm, control method of mechanical arm system, controller and storage medium | |
| CN113103262A (en) | Robot control device and method for controlling robot | |
| CN111670093B (en) | Robot motion control method, control system and storage device | |
| CN116945147A (en) | Singular point avoiding method and device in mechanical arm constant force control process | |
| CN114800523B (en) | Mechanical arm track correction method, system, computer and readable storage medium | |
| Jasim et al. | Adaptive sliding mode control of switched constrained robotic manipulators | |
| Xu et al. | Extended state observer based dynamic iterative learning for trajectory tracking control of a six-degrees-of-freedom manipulator | |
| Gu et al. | Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following | |
| KR101334356B1 (en) | Apparatus for controlling robot | |
| Afzali et al. | A Modified Convergence DDPG Algorithm for Robotic Manipulation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |