CN108015774B - Sensor-free mechanical arm collision detection method - Google Patents
Sensor-free mechanical arm collision detection method Download PDFInfo
- Publication number
- CN108015774B CN108015774B CN201711347621.7A CN201711347621A CN108015774B CN 108015774 B CN108015774 B CN 108015774B CN 201711347621 A CN201711347621 A CN 201711347621A CN 108015774 B CN108015774 B CN 108015774B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- moment
- vector
- tail end
- joint
- 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.)
- Active
Links
Images
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/1674—Programme controls characterised by safety, monitoring, diagnostic
- B25J9/1676—Avoiding collision or forbidden zones
-
- 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
- B25J19/0095—Means or methods for testing manipulators
Abstract
The invention discloses a sensor-free mechanical arm collision detection method, which comprises the following steps: calculating an orthogonal projection matrix of a joint torque vector generated by the tail end load of the mechanical arm on the mechanical arm, and obtaining a torque transformation matrix of the tail end load of the mechanical arm according to the orthogonal projection matrix; establishing a mechanical arm body dynamic model, establishing a mechanical arm momentum deviation observer according to each item in the dynamic model, obtaining an external moment vector of a mechanical arm joint through the momentum deviation observer, and calculating the collision moment of the mechanical arm according to the external moment vector and a moment transformation matrix of a mechanical arm tail end load; and comparing the collision torque of the mechanical arm with a preset threshold value, and judging whether the mechanical arm collides. The collision detection method adopted by the invention can realize the collision detection of the mechanical arm with the load at the tail end, and is simple, efficient and low in cost.
Description
Technical Field
The invention relates to the field of industrial robots, in particular to a mechanical arm collision detection method without a sensor.
Background
The use of the serial multi-joint mechanical arm in modern automatic industrial production is more popular, and the mechanical arm can collide with people or objects around in the normal working process, so that the mechanical arm can be damaged and workers can be injured. In addition, in the human-machine cooperative robot which has been developed in recent years, it is not necessary to protect a fence, and the robot can cooperate with a worker in the same environment, and in the human-machine cooperative work, the worker sometimes needs to actively touch a mechanical arm in work to suspend the operation of the mechanical arm, so that the mechanical arm needs to be detected whether the mechanical arm is actively in normal collision or passively in abnormal collision. It can be known from synthesizing above, in order to ensure the normal operating of arm and operating personnel's personal safety, collision detection becomes the indispensable function of arm gradually.
Because the sensor price is higher, in order to reduce the total cost of the mechanical arm, the traditional mechanical arm collision detection method is a sensorless detection mode. A dynamic model of a mechanical arm body is built by a professional before the mechanical arm leaves a factory, mechanical arm body parameters are obtained through calculation in a complex parameter identification experiment process, the dynamic model of the mechanical arm body and the mechanical arm body parameters are combined to obtain a complete dynamic model of the mechanical arm, and theoretical moment obtained by the complete dynamic model of the mechanical arm is compared with actual moment obtained through mechanical arm joint current, so that whether the mechanical arm collides or not is judged. The method generally needs a relatively accurate mechanical arm body dynamic model, a certain load can be brought to the tail end of the mechanical arm in the actual working process after the mechanical arm leaves a factory, or the tail end of the mechanical arm can be changed in the load working process, the mechanical arm body dynamic model cannot adapt to the load situation, the original collision detection can bring large errors and even false alarm and missing report, the mechanical arm needs to be subjected to dynamic modeling and parameter identification again, the working generally needs to be carried out on site by professional personnel who are skilled in robot control and a large amount of calculation, common operators cannot carry out the working, inconvenience in practical application is caused, the factory operation cost is increased, and higher collision detection precision cannot be realized.
Disclosure of Invention
The invention aims to solve the technical defects and provides a sensor-free mechanical arm collision detection method, which comprises the steps of calculating an orthogonal projection matrix PF of a joint torque vector generated by a mechanical arm tail end load, and obtaining a torque transformation matrix P of the mechanical arm tail end load according to the orthogonal projection matrix PF; establishing a mechanical arm body dynamic model, establishing a mechanical arm momentum deviation observer according to each item in the dynamic model, obtaining an external moment vector r of a mechanical arm joint through the momentum deviation observer, and calculating the collision moment tau of the mechanical arm according to the external moment vector r and a moment transformation matrix P of a mechanical arm tail end loadnew(ii) a The collision torque tau of the mechanical armnewAnd comparing the current value with a preset threshold value, and judging whether the mechanical arm collides.
In order to achieve the above object, the present invention provides a method for collision measurement of a sensorless robot arm, comprising the steps of:
the method comprises the following steps: calculating an orthogonal projection matrix PF of a joint torque vector generated by the tail end load of the mechanical arm on the mechanical arm, and obtaining a torque transformation matrix P of the tail end load of the mechanical arm according to the orthogonal projection matrix PF;
step two: building machineryThe method comprises the steps of establishing a mechanical arm momentum deviation observer according to each item in the dynamic model, obtaining an external moment vector r of a mechanical arm joint through the mechanical arm momentum deviation observer, and calculating collision moment tau of a mechanical arm according to the external moment vector r and a moment transformation matrix P of a mechanical arm tail end loadnew；
Step three: the collision torque tau of the mechanical armnewAnd comparing the current value with a preset threshold value, and judging whether the mechanical arm collides.
Further, in the first step, the specific step of calculating the moment transformation matrix P of the end load of the mechanical arm is as follows:
step 1.1: obtaining the joint angle q and the angular speed fed back by the mechanical arm joint according to the data fed back by the mechanical arm driver
Angular velocity of joint
Difference is carried out to obtain joint angular acceleration
Step 1.2: establishing a manipulator velocity Jacobian matrix J according to the D-H parameters of the manipulator body, and feeding back the joint angular velocity
And obtaining a velocity vector upsilon of the tail end of the mechanical arm through a mechanical arm velocity Jacobian matrix J, wherein the expression is as follows:
step 1.3: differentiating the velocity vector upsilon at the tail end of the mechanical arm to obtain an acceleration vector a at the tail end of the mechanical arm, summing the acceleration vector a and the gravity vector g at the tail end of the mechanical arm to obtain a total acceleration vector A at the tail end of the mechanical arm, setting the load mass at the tail end of the mechanical arm to be 1kg, and multiplying the load mass at the tail end of the mechanical arm by the total acceleration vector A at the tail end of the mechanical arm to obtain a total force vector F at the tail end of the mechanical arm, wherein the impact detection precision is not influenced by the load mass at the tail end of;
step 1.4: establishing a Jacobian matrix J of the arm force according to the D-H parameters of the arm bodyFAccording to the force vector F at the tail end of the mechanical arm and the Jacobian matrix J of the mechanical arm forceFAnd calculating to obtain the moment vector tau of the mechanical arm jointtThe expression is:
τt＝JF·F
step 1.5: according to the moment vector tau of the mechanical arm jointtEstablishing a mechanical arm joint torque matrix BF ═ tautThen calculating the moment vector tau of the mechanical arm jointtThe orthogonal projection matrix PF projects any joint moment vector to the joint moment vector tau of the mechanical armtIn the vector space SpayloadAbove, the expression is:
PF＝BF·(BFT·BF)-1·BFT
step 1.6: from the orthogonal projection matrix PF, a moment transformation matrix P is calculated which projects an arbitrary joint moment vector into the vector space SpayloadOf (a) orthogonal vector space ⊥ SpayloadAbove, the expression is:
P＝I-PF
when the mechanical arm with a load at the tail end is subjected to external force in the working process, the moment expression of the joint of the mechanical arm caused by the actual external force is as follows:
τext＝τpayload+τc
in the formula, τpayloadIs a load moment term; tau iscA moment term generated by collision of the mechanical arm; tau isextThe moment of the mechanical arm joint caused by actual external force;
due to the vector space SpayloadSum vector space ⊥ SpayloadAre always orthogonal to each other, and the moment vector generated by the end load of the mechanical arm to the joint always exists in the vector space SpayloadSo as to transform the sameThe matrix P is multiplied by the moment vector of the external force of the mechanical arm, and the load moment term tau can be eliminatedpayloadThe moment generated to the mechanical arm joint has the expression:
P·τext＝P·τc+P·τpayload＝P·τc
by the formula, the moment tau of the mechanical arm joint caused by actual external forceextAfter the conversion of the moment transformation matrix P, the moment transformation matrix P cancels the load moment item taupayload。
The actual collision detection method needs a mechanical arm body dynamic model, and the mechanical arm already obtains a high-precision mechanical arm body dynamic model through parameter identification in the delivery or test stage, so that when a load is added to the tail end of the mechanical arm in the working process, the original precision dynamic model is changed, the precision of mechanical arm collision detection is reduced, and even the mechanical arm collision detection function is failed. The method provided by the invention avoids the influence of the tail end load of the mechanical arm on the collision detection precision of the mechanical arm, and has the same effect even on loads of any different shapes, sizes and weights.
Further, in the second step, the specific step of calculating the collision torque of the mechanical arm is as follows:
step 2.1: the expression of the mechanical arm body dynamic model is as follows:
wherein M (q) is an inertia term;
is a nonlinear coupling term; g (q) is the gravity term, τfIs a friction force item, and tau is an instruction torque obtained by calculation of a mechanical arm dynamic model;
due to actual mechanical arm joint acceleration
Is derived from the second derivative of position and usually containsThe large noise is large in difference with the actual real joint acceleration value, so that the mechanical arm momentum deviation observer is established according to the principle that the generalized momentum of the mechanical arm changes when the mechanical arm is subjected to an external force;
step 2.2: calculating the generalized momentum p of the mechanical arm according to an inertia term M (q) in the mechanical arm body dynamics model, wherein the expression is as follows:
step 2.3: according to an inertia term M (q) and a nonlinear coupling term in a mechanical arm body dynamic model
Computing transpose matrix of nonlinear coupling terms in mechanical arm body dynamic model
The expression is as follows:
step 2.4: the mechanical arm dynamics expression of the mechanical arm under the condition of external force is as follows:
in the formula, τmotThe torque is output by the motor. Transforming a mechanical arm dynamics expression under the condition of external force, and establishing a mechanical arm momentum deviation observer, wherein the expression is as follows:
in the formula, r is an external force moment vector of the mechanical arm joint, and K is an adjusting coefficient of the mechanical arm momentum deviation observer.
And (3) performing derivation on the equation of the mechanical arm momentum deviation observer to obtain:
and (3) carrying out Laplace transform on the formula to obtain a transfer function of the mechanical arm momentum deviation observer:
as shown in the formula of the transfer function of the mechanical arm momentum deviation observer, the expression describes a first-order system, and the input of the system is the mechanical arm joint torque tau caused by actual external forceextAnd the output is the external moment vector r of the mechanical arm joint, the momentum deviation observer can well track the change of the external moment, and the rise time of the system is changed by adjusting the adjustment coefficient K of the mechanical arm momentum deviation observer. The first-order system is convenient to design, cannot generate large vibration, cannot influence the accuracy of collision detection, and does not need to use the acceleration of the mechanical arm joint
The momentum deviation observer can be established.
Step 2.5: the moment transformation matrix P obtained in the step one is point-multiplied by the external moment vector r of the mechanical arm joint to obtain the collision moment tau of the mechanical armnewThe expression is:
τnew＝P·r
further, in the third step, the collision torque τ of the mechanical arm is adjustednewAnd comparing the mechanical arm with a preset threshold value, and judging whether the mechanical arm collides, wherein the expression is as follows:
in the equation, TH1 is a moment negative constant threshold; TH2 is a torque positive constant threshold; judging whether the mechanical arm collides or not through the numerical value of the parameter d, and detecting that the mechanical arm collides when d is 1; when d is 0, no collision of the robot arm occurs.
In the traditional sensorless mechanical arm collision detection method, some methods detect whether the robot collides through a dynamic threshold value, the method increases the complexity of the whole system of the mechanical arm if the mechanical arm finishes a working period after detecting that the mechanical arm finishes the working period, and the method is relatively simple, practical and efficient, has high precision, adopts a constant threshold value, does not need to dynamically update the threshold value, changes the precision of collision detection only by adjusting the size of the threshold value, and avoids the phenomena of missed judgment and erroneous judgment.
Compared with the prior art, the invention has the beneficial effects that:
1. the load elimination method adopted by the invention is suitable for loads with tail ends in any different shapes, sizes and weights, can eliminate the influence of the loads on the moment of the joint of the mechanical arm even under the condition that the tail end load is uncertain, and is simple, practical and efficient under the condition of not losing collision detection precision;
2. whether the mechanical arm is collided or not is judged through the constant threshold, the threshold does not need to be dynamically updated, the collision detection precision is changed only by adjusting the threshold, and the phenomena of missing judgment and erroneous judgment are avoided;
3. according to the mechanical arm collision detection method, the terminal acceleration is obtained without arranging an additional sensor on the mechanical arm, the cost is low, the structure is simple, the complexity of the system is reduced, and the anti-interference capability of the system is enhanced.
Drawings
FIG. 1 is an overall flow diagram of the method of the present invention;
FIG. 2 is a flow chart of the present invention for computing a robot arm tip load moment transformation matrix;
FIG. 3 is a block diagram of a mechanical arm momentum deviation observer of the present invention;
FIG. 4 is a graph of the moment about the outside of the robot joint over time using a conventional method;
FIG. 5 is a graph of the moment of the five external forces of the mechanical arm joint obtained by the method of the invention along with the change of time.
Detailed Description
The following describes embodiments of the present invention in detail. It should be understood that the scope of the above-described subject matter is not limited to the following examples, and any techniques implemented based on the disclosure of the present invention are within the scope of the present invention. It should be further noted that, for the convenience of description, only some but not all of the matters related to the present invention are shown in the drawings.
As shown in fig. 1, fig. 1 is a flowchart of a method for detecting a collision of a robot arm without a sensor according to an embodiment of the present invention, where a robot arm body adopted in the embodiment is a seven-axis cooperative robot, and the method includes the following specific steps:
the method comprises the following steps: and calculating an orthogonal projection matrix PF of the joint moment vector generated by the tail end load of the mechanical arm to the mechanical arm, and obtaining a moment transformation matrix P of the tail end load of the mechanical arm according to the orthogonal projection matrix PF.
As shown in fig. 2, the step of calculating the moment transformation matrix of the end load of the mechanical arm is as follows:
step 1.1: in the normal working process of the mechanical arm, data fed back by a mechanical arm driver are collected in real time, the fed-back data are subjected to low-pass filtering processing, and a joint angle q and an angular speed fed back by a mechanical arm joint are obtained
Angular velocity of joint
Difference is carried out to obtain joint angular acceleration
Step 1.2: establishing a manipulator velocity Jacobian matrix J according to the D-H parameters of the manipulator body, and feeding back the joint angular velocity
And obtaining a velocity vector upsilon of the tail end of the mechanical arm through a mechanical arm velocity Jacobian matrix J, wherein the expression is as follows:
step 1.3: differentiating the velocity vector upsilon at the tail end of the mechanical arm to obtain an acceleration vector a at the tail end of the mechanical arm, summing the acceleration vector a at the tail end and a gravity vector g to obtain a total acceleration vector A at the tail end of the mechanical arm, setting the load mass at the tail end of the mechanical arm to be 1kg for convenience of calculation, and multiplying the load mass at the tail end of the mechanical arm by the total acceleration vector A at the tail end of the mechanical arm to obtain a total force vector F at the tail end of the mechanical arm;
step 1.4: establishing a Jacobian matrix J of the arm force according to the D-H parameters of the arm bodyFAccording to the force vector F at the tail end of the mechanical arm and the Jacobian matrix J of the mechanical arm forceFAnd calculating to obtain the moment vector tau of the mechanical arm jointtThe expression is:
τ＝JF·F
step 1.5: according to the moment vector tau of the mechanical arm jointtEstablishing a mechanical arm joint torque matrix BF ═ tautThen calculating the moment vector tau of the mechanical arm jointtThe orthogonal projection matrix PF of (2), the expression is:
PF＝BF·(BFT·BF)-1·BFT
step 1.6: calculating a moment transformation matrix P according to the orthogonal projection matrix PF, wherein the expression is as follows:
P＝I-PF
step two: establishing a mechanical arm body dynamic model, establishing a mechanical arm momentum deviation observer according to each item in the dynamic model, obtaining an external moment vector r of a mechanical arm joint through the mechanical arm momentum deviation observer, and calculating a collision moment tau of the mechanical arm according to the external moment vector r and a moment transformation matrix P of a mechanical arm tail end loadnew。
Wherein the collision torque tau of the mechanical arm is calculatednewThe steps are as follows:
step 2.1: the expression of the mechanical arm body dynamic model is as follows:
step 2.2: calculating the generalized momentum of the mechanical arm according to an inertia term M (q) in a mechanical arm body dynamics model, wherein the expression is as follows:
step 2.3: according to an inertia term M (q) and a nonlinear coupling term in a mechanical arm body dynamic model
Calculating a transposed matrix of nonlinear coupling terms in a mechanical arm body dynamic model, wherein the expression is as follows:
step 2.4: establishing a mechanical arm momentum deviation observer, wherein the expression is as follows:
in the formula, r is an external force moment vector of the mechanical arm joint, and K is an adjusting coefficient of the mechanical arm momentum deviation observer. The structural frame diagram of the mechanical arm momentum deviation observer is shown in FIG. 3, wherein:
step 2.5: the moment transformation matrix P obtained in the step one is point-multiplied by the external moment vector r of the mechanical arm joint to obtain the collision moment tau of the mechanical armnewThe expression is:
τnew＝P·r
step three: the mechanical arm collision torque taunewAnd comparing the current value with a preset threshold value, and judging whether the mechanical arm collides.
Wherein the collision torque tau of the mechanical arm is adjustednewAnd comparing the mechanical arm with a preset threshold value, and judging whether the mechanical arm collides, wherein the expression is as follows:
in the equation, TH1 is a moment negative constant threshold; TH2 is a torque positive constant threshold; judging whether the mechanical arm collides or not through the numerical value of the parameter d, and detecting that the mechanical arm collides when d is 1; when d is 0, no collision of the robot arm occurs.
In order to verify the correctness of the mechanical arm collision detection method without a sensor, a mechanical arm collision detection system is built by using a simechanics module in matlab software under a simulink platform in the matlab software for simulation verification, and no matter which joint of the mechanical arm is detected to be collided, the mechanical arm is regarded as being collided. The load at the tail end of the mechanical arm is 2kg, the coordinate of the centroid position under a tail end coordinate system is (0,0,0.1), and the unit is meter (m). The simulation time is from 0 to 5 seconds, in the simulation process, when the simulation time is from 4 seconds, a cartesian space external force (2,0,0) is applied to the centroid of the five connecting rods of the mechanical arm, the unit is newton (N), the duration is 0.01 seconds, and after the simulation is finished, the obtained results are shown in fig. 4 and 5.
The curve of the change of the four external moments of the joints of the mechanical arm along with time, which is obtained by adopting the traditional sensorless mechanical arm collision detection method, is shown in fig. 4, the smooth part of the curve is the external moment caused by the load at the tail end of the mechanical arm, and the moment sudden change on the curve at the 4 th second is caused by the sudden application of collision force to the mechanical arm connecting rod five. If a moment constant threshold value is used, an external moment caused by a load can be detected, and the phenomenon of misjudgment can occur in collision detection.
The curve of the change of the external moment of the joint five of the mechanical arm along with the time, which is obtained by adopting the sensorless mechanical arm collision detection method provided by the invention, is shown in fig. 5, the moment mutation at the 4 th second on the curve is caused by the sudden application of the collision force on the mechanical arm connecting rod five, and the external moment applied to the mechanical arm when the mechanical arm collides is at least 10 times of the external moment applied to the mechanical arm when the mechanical arm normally runs, so that whether the mechanical arm collides can be detected by setting a constant threshold value.
Comparing the curves of the two graphs shows that when the mechanical arm with the load collides, the traditional method cannot detect the collision of the mechanical arm through the preset constant threshold value, the sensorless mechanical arm collision detection method provided by the invention can accurately detect the collision of the mechanical arm through the preset constant threshold value, and the simulation result shows that the sensorless mechanical arm collision detection method provided by the invention realizes the mechanical arm collision detection with the load at the tail end.
After the simulation verification of collision detection is carried out on the matlab platform, the practical application condition is considered, the collision detection test is carried out on the mechanical arm entity, and the mechanical arm collision detection method without the sensor provided by the invention is written into codes by C language and downloaded into a program in the mechanical arm controller.
Writing a teaching program to enable the mechanical arm to have the same running track every time, keeping the load clamped by the tail end of the mechanical arm unchanged in a first test, actively colliding different parts of the mechanical arm by an operator in the process of moving the mechanical arm every time, and recording the value of a parameter d; the quality of the load of the terminal centre gripping of arm is changed in the test of the second time, guarantees that operating personnel initiative in the arm motion process at every turn bumps the same position of arm, records the value of parameter d, and wherein, operating personnel initiative bump the arm and can not make operating personnel appear the sense of pain, and the experimental result is shown in following 1:
table 1 mechanical arm collision detection result table
According to the test data in table 1, for the mechanical arms with different mass loads at the tail ends, operators touch the same parts of the mechanical arms in the motion process of the mechanical arms, and the collision of the mechanical arms can be detected by setting positive and negative constant thresholds; for the mechanical arms with the same load at the tail ends, operators touch different parts of the mechanical arms in the motion process of the mechanical arms, the collision of the mechanical arms can be detected by setting positive and negative constant thresholds, and the operators do not feel pain in the collision process, so that the collision detection method provided by the invention can realize the collision detection of the sensorless mechanical arms with any load.
Claims (3)
1. A mechanical arm collision detection method without a sensor is characterized in that:
the method comprises the following steps: calculating an orthogonal projection matrix PF of a joint torque vector generated by the tail end load of the mechanical arm on the mechanical arm, and obtaining a torque transformation matrix P of the tail end load of the mechanical arm according to the orthogonal projection matrix PF;
step two: establishing a mechanical arm body dynamic model, establishing a mechanical arm momentum deviation observer according to each item in the dynamic model, obtaining an external moment vector r of a mechanical arm joint through the mechanical arm momentum deviation observer, and calculating the collision moment of the mechanical arm according to the external moment vector r and a moment transformation matrix P of a mechanical arm tail end load
；
In the second step, the collision torque of the mechanical arm is calculated
The method comprises the following specific steps:
step 2.1: establishing a mechanical arm body dynamic model, wherein the expression is as follows:
wherein M (q) is an inertia term;
is a nonlinear coupling term; g (q) is a gravity term,
is a friction force item, and tau is an instruction torque obtained by calculation of a mechanical arm dynamic model;
step 2.2: calculating the generalized momentum p of the mechanical arm according to an inertia term M (q) in the mechanical arm body dynamics model, wherein the expression is as follows:
；
step 2.3: according to an inertia term M (q) and a nonlinear coupling term in a mechanical arm body dynamic model
Calculating the transpose matrix of the nonlinear coupling terms in the mechanical arm body dynamic model
The expression is as follows:
；
step 2.4: establishing a mechanical arm momentum deviation observer, wherein the expression is as follows:
in the formula, the first and second organic solvents are,
outputting torque for a mechanical arm joint motor, wherein r is an external force torque vector of a mechanical arm joint, and K is an adjusting coefficient of a mechanical arm momentum deviation observer;
step 2.5: the moment transformation matrix P obtained in the step one is point-multiplied by the external moment vector r of the mechanical arm joint to obtain the collision moment of the mechanical arm
The expression is:
；
step three: the collision torque of the mechanical arm
And comparing the current value with a preset threshold value, and judging whether the mechanical arm collides.
2. The sensor-less robot arm collision detecting method according to claim 1, wherein: in the first step, the specific steps of calculating the moment transformation matrix P of the tail end load of the mechanical arm are as follows:
step 1.1: obtaining the joint angle q and the angular speed fed back by the mechanical arm joint according to the data fed back by the mechanical arm driver
Angular velocity of joint
Difference is carried out to obtain joint angular acceleration
；
Step 1.2: establishing a manipulator velocity Jacobian matrix J according to the D-H parameters of the manipulator body, and feeding back the joint angular velocity
Obtaining the velocity vector of the tail end of the mechanical arm by the Jacobian matrix J of the velocity of the mechanical arm
The expression is:
；
step 1.3: vector the velocity of the end of the mechanical arm
Differentiating to obtain an acceleration vector a at the tail end of the mechanical arm, summing the acceleration vector a and a gravity vector g at the tail end of the mechanical arm to obtain a total acceleration vector A at the tail end of the mechanical arm, and multiplying a load mass point at the tail end of the mechanical arm by the total acceleration vector A at the tail end of the mechanical arm to obtain a total force vector F at the tail end of the mechanical arm;
step 1.4: establishing a Jacobian matrix of the mechanical arm force according to the D-H parameters of the mechanical arm body
According to the force vector F at the tail end of the mechanical arm and the Jacobi matrix of the mechanical arm force
And calculating to obtain the moment vector of the mechanical arm joint
The expression is:
(ii) a Step 1.5: according to the moment vector of the mechanical arm joint
Establishing a moment matrix of the mechanical arm joint
Then calculating the moment vector of the mechanical arm joint
The orthogonal projection matrix PF of (2), the expression is:
；
step 1.6: calculating a moment transformation matrix P, an expression of the moment transformation matrix P of the tail end load of the mechanical arm according to the orthogonal projection matrix PF
Comprises the following steps:
。
3. the sensor-less robot arm collision detecting method according to claim 2, wherein: in the third step, the collision torque of the mechanical arm is used
And comparing the mechanical arm with a preset threshold value, and judging whether the mechanical arm collides, wherein the expression is as follows:
in the equation, TH1 is a moment negative constant threshold; TH2 is a torque positive constant threshold; judging whether the mechanical arm collides or not through the numerical value of the parameter d, and detecting that the mechanical arm collides when d is 1; when d is 0, no collision of the robot arm occurs.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711347621.7A CN108015774B (en) | 2017-12-15 | 2017-12-15 | Sensor-free mechanical arm collision detection method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711347621.7A CN108015774B (en) | 2017-12-15 | 2017-12-15 | Sensor-free mechanical arm collision detection method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108015774A CN108015774A (en) | 2018-05-11 |
| CN108015774B true CN108015774B (en) | 2020-10-13 |
Family
ID=62073853
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711347621.7A Active CN108015774B (en) | 2017-12-15 | 2017-12-15 | Sensor-free mechanical arm collision detection method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108015774B (en) |
Families Citing this family (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109062051B (en) * | 2018-08-28 | 2021-07-23 | 苏州艾利特机器人有限公司 | Method for improving robot dynamics parameter identification precision |
| CN109159120B (en) * | 2018-09-10 | 2022-07-12 | 南京邮电大学 | Active control method and system based on current feedback of joint motor of rehabilitation mechanical arm |
| CN109732599B (en) * | 2018-12-29 | 2020-11-03 | 深圳市越疆科技有限公司 | Robot collision detection method and device, storage medium and robot |
| CN110026981B (en) * | 2019-04-19 | 2021-09-17 | 中科新松有限公司 | Mechanical arm collision detection method based on model self-adaptation |
| CN110340885B (en) * | 2019-05-21 | 2021-12-07 | 南京航空航天大学 | Industrial robot collision detection method based on energy deviation observer |
| CN110990943B (en) * | 2019-11-13 | 2023-10-20 | 上海航天控制技术研究所 | Singular point judgment method based on singular geometric meaning of control moment gyro group |
| CN111872936B (en) * | 2020-07-17 | 2021-08-27 | 清华大学 | Robot collision detection system and method based on neural network |
| CN111890343B (en) * | 2020-07-29 | 2021-10-15 | 浙江广合智能科技有限公司 | Robot object collision detection method and device |
| CN113043283B (en) * | 2021-04-23 | 2022-07-08 | 江苏理工学院 | Robot tail end external force estimation method |
| CN113799142B (en) * | 2021-10-29 | 2023-02-21 | 遨博（北京）智能科技有限公司 | Collision protection method for mechanical arm, control cabinet and mechanical arm system |
| CN114012734B (en) * | 2021-12-03 | 2023-05-30 | 西安交通大学 | Parameter-adaptive robot collision detection method |
| CN114800620B (en) * | 2022-06-13 | 2023-12-12 | 湖南科技大学 | Robot external force detection method without force sensor |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006123012A (en) * | 2004-10-26 | 2006-05-18 | Matsushita Electric Ind Co Ltd | Robot control method |
| KR20090114890A (en) * | 2008-04-30 | 2009-11-04 | 현대중공업 주식회사 | Detection methode for collision of robot |
| CN103192413A (en) * | 2012-01-06 | 2013-07-10 | 沈阳新松机器人自动化股份有限公司 | Sensor-free robot crash detecting and preventing device and method |
| CN103878791A (en) * | 2014-04-12 | 2014-06-25 | 福州大学 | Industrial robot external-sensor-free external force detection method |
| CN104718055A (en) * | 2012-10-25 | 2015-06-17 | 松下知识产权经营株式会社 | Robot malfunction indication method |
| CN104985598A (en) * | 2015-06-24 | 2015-10-21 | 南京埃斯顿机器人工程有限公司 | Industrial robot collision detection method |
| CN107097233A (en) * | 2017-04-17 | 2017-08-29 | 珞石（山东）智能科技有限公司 | A kind of industrial robot dragging teaching method of non-moment sensor |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009099082A (en) * | 2007-10-19 | 2009-05-07 | Sony Corp | Dynamics simulation device, dynamics simulation method, and computer program |
-
2017
- 2017-12-15 CN CN201711347621.7A patent/CN108015774B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006123012A (en) * | 2004-10-26 | 2006-05-18 | Matsushita Electric Ind Co Ltd | Robot control method |
| KR20090114890A (en) * | 2008-04-30 | 2009-11-04 | 현대중공업 주식회사 | Detection methode for collision of robot |
| CN103192413A (en) * | 2012-01-06 | 2013-07-10 | 沈阳新松机器人自动化股份有限公司 | Sensor-free robot crash detecting and preventing device and method |
| CN104718055A (en) * | 2012-10-25 | 2015-06-17 | 松下知识产权经营株式会社 | Robot malfunction indication method |
| CN103878791A (en) * | 2014-04-12 | 2014-06-25 | 福州大学 | Industrial robot external-sensor-free external force detection method |
| CN104985598A (en) * | 2015-06-24 | 2015-10-21 | 南京埃斯顿机器人工程有限公司 | Industrial robot collision detection method |
| CN107097233A (en) * | 2017-04-17 | 2017-08-29 | 珞石（山东）智能科技有限公司 | A kind of industrial robot dragging teaching method of non-moment sensor |
Non-Patent Citations (1)
| Title |
|---|
| 一种轻型模块化协作机器人碰撞检测算法;肖聚亮等;《天津大学学报（自然科学与工程技术版）》;20171130;第50卷(第11期);1140-1147 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108015774A (en) | 2018-05-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108015774B (en) | Sensor-free mechanical arm collision detection method | |
| CN108772838B (en) | Mechanical arm safe collision strategy based on external force observer | |
| CN107433590B (en) | Gravity compensation method based on mechanical arm load mass and sensor null shift online identification | |
| CN110716557B (en) | Machine parameter identification and contact force monitoring method based on priori dynamics knowledge | |
| CN110662635B (en) | Collision handling for robots | |
| CN111618857B (en) | Multi-load self-adaptive gravity compensation method for mechanical arm | |
| CN106413997B (en) | Method for avoiding robot from colliding in work station | |
| Martin et al. | Dynamic load emulation in hardware-in-the-loop simulation of robot manipulators | |
| CN109746913B (en) | Method and system for teaching robot posture keeping dragging | |
| CN112809667B (en) | Force control method and device of industrial robot and application of force control device | |
| CN111347416B (en) | Detection robot collision detection method without external sensor | |
| CN108638070A (en) | Robot based on dynamic equilibrium loads weight parameter discrimination method | |
| CN110539302A (en) | industrial robot overall dynamics modeling and dynamics parameter identification method | |
| Li et al. | Neural learning and kalman filtering enhanced teaching by demonstration for a baxter robot | |
| JP2004364396A (en) | Controller and control method for motor | |
| Benabdallah et al. | Kinect-based Computed Torque Control for lynxmotion robotic arm | |
| CN113021353B (en) | Robot collision detection method | |
| He et al. | A momentum-based collision detection algorithm for industrial robots | |
| CN113246137A (en) | Robot collision detection method based on external moment estimation model | |
| CN114407022B (en) | Mechanical arm collision detection method based on model parameter error observer | |
| Jung et al. | Control of the manipulator position with the kinect sensor | |
| CN114367975A (en) | Verification system of series industrial robot control algorithm | |
| CN113442118B (en) | Collision response control method and system for wearable outer limb robot | |
| JP2021091064A (en) | Disturbance observer and contact point estimation means using the same | |
| JP2016179523A (en) | Robot control device and robot system |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |