CN108015774A - A kind of sensorless mechanical arm collision checking method - Google Patents

A kind of sensorless mechanical arm collision checking method Download PDF

Info

Publication number
CN108015774A
CN108015774A CN201711347621.7A CN201711347621A CN108015774A CN 108015774 A CN108015774 A CN 108015774A CN 201711347621 A CN201711347621 A CN 201711347621A CN 108015774 A CN108015774 A CN 108015774A
Authority
CN
China
Prior art keywords
mechanical arm
mrow
joint
torque
mover
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.)
Granted
Application number
CN201711347621.7A
Other languages
Chinese (zh)
Other versions
CN108015774B (en
Inventor
康永利
黎意枫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Ailite Technology Co Ltd
Original Assignee
Beijing Ailite Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Ailite Technology Co Ltd filed Critical Beijing Ailite Technology Co Ltd
Priority to CN201711347621.7A priority Critical patent/CN108015774B/en
Publication of CN108015774A publication Critical patent/CN108015774A/en
Application granted granted Critical
Publication of CN108015774B publication Critical patent/CN108015774B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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 kind of sensorless mechanical arm collision checking method, including：The orthogonal intersection cast shadow matrix for the joint moment vector that calculating machine arm end load produces mechanical arm, the torque transfer matrix of mechanical arm tail end load is obtained according to the orthogonal intersection cast shadow matrix；Establish mechanical arm body kinetic model, items in the kinetic model establish mechanical arm momentum deviation observer, joint of mechanical arm external force moment vector is obtained by momentum deviation observer, the torque transfer matrix computations loaded according to the external force moment vector and mechanical arm tail end go out the collision torque of mechanical arm；The collision torque of mechanical arm is compared with predetermined threshold value, judges whether mechanical arm collides.The collision checking method that the present invention uses, can realize the loaded mechanical arm collision detection of end band, and simple efficient, cost is low.

Description

A kind of sensorless mechanical arm collision checking method
Technical field
The present invention relates to industrial robot field, more particularly to a kind of sensorless mechanical arm collision checking method.
Background technology
The use of series connection multi-joint mechanical arm is more popularized in modern automation industrial production, in the normal work of mechanical arm During, it may collide with the people of surrounding or object, such case may not only cause to damage to mechanical arm, it is also possible to right Staff damages.In addition, the man-machine collaboration robot gradually to grow up in recent years need not protect fence, can be with the people that works Member cooperates in same environment, and when man-machine coordination works, staff is sometimes for actively touching just at work Mechanical arm is to suspend the operation of mechanical arm, so all must for mechanical arm either active normal impingement or passive abnormal collision It must be detected.In summary understand, in order to ensure the personal safety of the normal operation of mechanical arm and operating personnel, collision Detection is increasingly becoming the essential function of mechanical arm.
Due to sensor higher price, in order to reduce mechanical arm totality cost, traditional mechanical arm collision checking method compared with For it is common be sensorless detection mode.Common mechanical arm is established the dynamics of mechanical arm body by professional before dispatching from the factory Model, by complicated parameter identification experimentation by mechanical arm body parameter is calculated, by the power of mechanical arm body Learn model and mechanical arm body parameter is combined to obtain the Holonomic Dynamics model of mechanical arm, by by the Holonomic Dynamics of mechanical arm The ideal torque that model obtains is compared with the actual torque obtained by joint of mechanical arm electric current, so as to judge that mechanical arm is It is no to collide.This method generally requires more accurate mechanical arm body kinetic model, and the actual work of mechanical arm after dispatching from the factory End can take in a fixed load or loaded work piece and can change during work, and mechanical arm body kinetic model cannot The loaded situation of this band is adapted to, original collision detection can bring large error or even report by mistake and fail to report, it is necessary to again to machine Tool arm carries out Dynamic Modeling and parameter identification, and this work generally require be proficient in the professional scene of robot control into Row operation and a large amount of calculating, normal operations worker can not carry out, and cause the inconvenience in practical application, increase factory operation cost And it can not realize higher collision detection precision.
The content of the invention
The purpose of the present invention aims to solve the problem that the technological deficiency, proposes a kind of sensorless mechanical arm side collision detection Method, the orthogonal intersection cast shadow matrix PF for the joint moment vector that calculating machine arm end load produces mechanical arm, is just trading according to this Shadow matrix PF obtains the torque transfer matrix P of mechanical arm tail end load；Mechanical arm body kinetic model is established, according to the power The items learned in model establish mechanical arm momentum deviation observer and obtain joint of mechanical arm external force by momentum deviation observer Moment vector r, the torque transfer matrix P loaded according to the external force moment vector r and mechanical arm tail end calculate the impact force of mechanical arm Square τnew；By the collision torque τ of mechanical armnewCompared with predetermined threshold value, judge whether mechanical arm collides.
To achieve these goals, the present invention provides a kind of mechanical arm collision survey method of no sensor, including following step Suddenly：
Step 1：The orthogonal intersection cast shadow matrix PF for the joint moment vector that calculating machine arm end load produces mechanical arm, The torque transfer matrix P of mechanical arm tail end load is obtained according to orthogonal intersection cast shadow matrix PF；
Step 2：Mechanical arm body kinetic model is established, the items in the kinetic model are established mechanical arm and moved Deviation observer is measured, joint of mechanical arm external force moment vector r is obtained by the mechanical arm momentum deviation observer, according to the external force The moment vector r and torque transfer matrix P of mechanical arm tail end load calculates the collision torque τ of mechanical armnew；
Step 3：By the collision torque τ of the mechanical armnewCompared with predetermined threshold value, judge whether mechanical arm touches Hit.
Further, in the step one, the torque transfer matrix P specific steps of calculating machine arm end load are such as Under：
Step 1.1：The data fed back according to mechanical arm driver obtain the joint angles q of joint of mechanical arm feedback, angle speed DegreeBy joint angular speedCarry out difference and obtain joint angular acceleration
Step 1.2：Mechanical arm speed Jacobian matrix J is established according to the D-H parameters of mechanical arm body, according to the pass of feedback Save angular speedThe velocity υ of mechanical arm tail end is obtained with mechanical arm speed Jacobian matrix J, expression formula is：
Step 1.3：By the velocity υ difference of the mechanical arm tail end, the acceleration a of mechanical arm tail end is obtained, The acceleration a of the mechanical arm tail end and gravitational vectors g are summed to obtain the total acceleration A of mechanical arm tail end, by machine Tool arm end load quality settings are 1kg, and the total acceleration of the mechanical arm tail end load quality dot product mechanical arm tail end is sweared A is measured, obtains the total force vector F of mechanical arm tail end, wherein, the size of mechanical arm tail end load quality does not influence collision detection essence Degree；
Step 1.4：Mechanical arm strength Jacobian matrix J is established according to the D-H parameters of mechanical arm bodyF, according to mechanical arm end Hold force vector F and mechanical arm strength Jacobian matrix JF, joint of mechanical arm moment vector τ is calculatedt, expression formula is：
τt=JF·F
Step 1.5：According to joint of mechanical arm moment vector τt, establish joint of mechanical arm torque matrix B F=τt, then calculate Go out joint of mechanical arm moment vector τtOrthogonal intersection cast shadow matrix PF, orthogonal intersection cast shadow matrix PF makes any joint moment vector project To joint of mechanical arm moment vector τtThe vector space S at placepayloadOn, its expression formula is：
PF=BF (BFT·BF)-1·BFT
Step 1.6：According to orthogonal intersection cast shadow matrix PF, torque transformation matrix P is calculated, torque transfer matrix P makes any pass Section moment vector projects to vector space SpayloadOrthogonal vector space ⊥ SpayloadOn, expression formula is：
P=I-PF
When being subject to external force during the work time for the loaded mechanical arm of end band, mechanical arm caused by actual external force closes Saving torque expression formula is：
τext=τpayload+τc
In formula, τpayloadFor loading moment item；τcCollide the torque item of generation for mechanical arm；τextDraw for actual external force The joint of mechanical arm torque risen；
Due to vector space SpayloadWith vector space ⊥ SpayloadIt is always mutually orthogonal, and mechanical arm tail end loads The moment vector produced to joint always exists in vector space Spayload, so transformation matrix P is multiplied by mechanical arm external force Moment vector, you can eliminate loading moment item τpayloadThe torque produced to joint of mechanical arm, its expression formula are：
P·τext=P τc+P·τpayload=P τc
By the formula, joint of mechanical arm torque τ caused by actual external forceextBy torque transfer matrix P conversion it Afterwards, torque transfer matrix P counteracts loading moment item τpayload。
Actual collision detection method needs mechanical arm body kinetic model, and mechanical arm is dispatching from the factory or test phase passes through ginseng Number identification has obtained degree of precision mechanical arm body kinetic model, and therefore, when mechanical arm, end adds during the work time Load, can cause the precise kinetic model of script to change, and reduce the precision of mechanical arm collision detection, even result in machinery Arm collision checking function fails.Method provided by the present invention avoids mechanical arm tail end load to mechanical arm collision detection precision Influence, also have same effect even for the load of any different shapes and sizes, weight.
Further, in the step two, the collision torque of calculating machine arm comprises the following steps that：
Step 2.1：The expression formula of mechanical arm body kinetic model is：
In formula, M (q) is Inertia；For Non-linear coupling item；G (q) is gravity item, τfFor frictional force item, τ is The instruction torque being calculated by Manipulator Dynamic；
Due to actual machine shoulder joint accelerationIt is to be obtained by the second differential of position, usually contains loud noise, Differ larger with actual real joint velocity value, so when the present invention is subject to external force according to mechanical arm, mechanical arm broad sense is moved Measure changed principle and establish mechanical arm momentum deviation observer；
Step 2.2：According to the Inertia M (q) in mechanical arm body kinetic model, the generalized momentum p of calculating machine arm, Expression formula is：
Step 2.3：Inertia M (q) and Non-linear coupling item in mechanical arm body kinetic model The transposed matrix of Non-linear coupling item in calculating machine arm body kinetic modelExpression formula is：
Step 2.4：Mechanical arm is in the Manipulator Dynamics expression formula in the case of by external force：
In formula, τmotFor motor output torque.Manipulator Dynamics expression formula in the case of by external force is converted, is built Vertical mechanical arm momentum deviation observer, expression formula are：
In formula, r is joint of mechanical arm external force moment vector, and K is mechanical arm momentum deviation observer adjustment factor.
By the formula derivation of tool arm momentum deviation observer, obtain：
Laplace transform is carried out to above formula and obtains the transmission function of tool arm momentum deviation observer：
From the transmission function of tool arm momentum deviation observer, this formula finds out that the expression formula describes a level System, the input of system is joint of mechanical arm torque τ caused by actual external forceext, export as joint of mechanical arm external force moment vector r, should Momentum deviation observer can track the change of moment of face well, pass through adjusting mechanical arm momentum deviation observer adjustment factor The size of K changes the system rise time.First-order system design is convenient, will not produce larger concussion, not interfere with collision detection Accuracy, and joint of mechanical arm acceleration need not be usedWith regard to the momentum deviation observer can be established.
Step 2.5：The torque transfer matrix P dot products joint of mechanical arm external force moment vector r obtained in step 1 is obtained To the collision torque τ of mechanical armnew, expression formula is：
τnew=Pr
Further, in the step 3, by the collision torque τ of mechanical armnewCompared with predetermined threshold value, machinery is judged Whether arm collides, and expression formula is：
In formula, TH1 is torque negative sense constant threshold；TH2 is torque forward direction constant threshold；By the numerical value of parameter d come Judge whether mechanical arm collides, when d is 1, detect that mechanical arm collides；When d is 0, mechanical arm collisionless hair It is raw.
It is traditional without in sensor mechanism arm collision checking method, whether some uses detect robot by dynamic threshold Collide, this method be mechanical arm repeat work operations in, if detect mechanical arm complete a duty cycle, for In judging that the dynamic threshold whether mechanical arm collides is updated, such a process increases the complexity of mechanical arm total system Degree, the mechanical arm collision relatively easy practicality and high efficiency of survey method of no sensor proposed by the invention, precision is high, and the present invention Using constant threshold, it is not necessary to which dynamic renewal threshold value, just changes the essence of collision detection only by the size for adjusting threshold value Degree, avoids the phenomenon failed to judge and judged by accident.
Compared with prior art, beneficial effects of the present invention：
1st, the load removing method that the present invention uses is suitable for the load of end any different shapes and sizes, weight, even if In the case of end load is uncertain, influence of the load to joint of mechanical arm torque can be also eliminated, is not losing collision detection It is simple and practical efficient in the case of precision；
2nd, the present invention is judged using by constant threshold whether mechanical arm collides, it is not necessary to which dynamic is more New threshold value, just changes the precision of collision detection only by the size for adjusting threshold value, avoids the phenomenon failed to judge and judged by accident；
3rd, the mechanical arm collision checking method that the present invention uses, the acquisition to end acceleration to mechanical arm without configuring volume Outer sensor, cost is low, simple in structure, reduces the complexity of system, the antijamming capability of strengthening system.
Brief description of the drawings
Fig. 1 is the overall flow figure of the method for the present invention；
Fig. 2 is the flow chart of the calculating machine arm end load torque transfer matrix of the present invention；
Fig. 3 is the mechanical arm momentum deviation Observer Structure block diagram of the present invention；
Fig. 4 is the four moment of face time history plot of joint of mechanical arm obtained using conventional method；
Fig. 5 is the five moment of face time history plot of joint of mechanical arm that the method for the present invention obtains.
Embodiment
The embodiment of the present invention is described below in detail.But the scope that this should not be interpreted as to the above-mentioned theme of the present invention is only limitted to Following embodiment, it is all that the scope of the present invention is belonged to based on the technology that present invention is realized.Further need exist for explanation It is, for the ease of description, part related to the present invention rather than full content to be show but in attached drawing.
As shown in Figure 1, Fig. 1 is the flow of the sensorless mechanical arm collision checking method of one embodiment of the invention Figure, the mechanical arm body that this specific embodiment uses are seven axis cooperation robots, are comprised the following steps that：
Step 1：The orthogonal intersection cast shadow matrix PF for the joint moment vector that calculating machine arm end load produces mechanical arm, The torque transfer matrix P of mechanical arm tail end load is obtained according to orthogonal intersection cast shadow matrix PF.
Wherein, as shown in Fig. 2, the torque transfer matrix step of calculating machine arm end load is as follows：
Step 1.1：In mechanical arm course of normal operation, the data of real-time collection machinery arm driver feedback, by these The data of feedback carry out low-pass filtering treatment, obtain joint angles q, the angular speed of joint of mechanical arm feedbackBy joint angle speed DegreeCarry out difference and obtain joint angular acceleration
Step 1.2：Mechanical arm speed Jacobian matrix J is established according to the D-H parameters of mechanical arm body, according to the pass of feedback Save angular speedThe velocity υ of mechanical arm tail end is obtained with mechanical arm speed Jacobian matrix J, expression formula is：
Step 1.3：By the velocity υ difference of mechanical arm tail end, the acceleration a of mechanical arm tail end is obtained, by this The acceleration a and gravitational vectors g of end sum to obtain the total acceleration A of mechanical arm tail end, calculate for convenience, will Mechanical arm tail end load quality is arranged to 1kg, and by the total acceleration of the mechanical arm tail end load quality dot product mechanical arm tail end Vector A, obtains the total force vector F of mechanical arm tail end；
Step 1.4：Mechanical arm strength Jacobian matrix J is established according to the D-H parameters of mechanical arm bodyF, according to mechanical arm end Hold force vector F and mechanical arm strength Jacobian matrix JF, joint of mechanical arm moment vector τ is calculatedt, expression formula is：
τ=JF·F
Step 1.5：According to the joint of mechanical arm moment vector τt, establish joint of mechanical arm torque matrix B F=τt, then Calculate the joint of mechanical arm moment vector τtOrthogonal intersection cast shadow matrix PF, expression formula is：
PF=BF (BFT·BF)-1·BFT
Step 1.6：According to orthogonal intersection cast shadow matrix PF, torque transformation matrix P is calculated, expression formula is：
P=I-PF
Step 2：Mechanical arm body kinetic model is established, the items in the kinetic model are established mechanical arm and moved Deviation observer is measured, joint of mechanical arm external force moment vector r is obtained by the mechanical arm momentum deviation observer, according to the external force The moment vector r and torque transfer matrix P of mechanical arm tail end load calculates the collision torque τ of mechanical armnew。
Wherein, the collision torque τ of calculating machine armnewThe step of it is as follows：
Step 2.1：The expression formula of mechanical arm body kinetic model is：
Step 2.2：According to the Inertia M (q) in mechanical arm body kinetic model, the generalized momentum of calculating machine arm, Expression formula is：
Step 2.3：Inertia M (q) and Non-linear coupling item in mechanical arm body kinetic model The transposed matrix of Non-linear coupling item, expression formula are in calculating machine arm body kinetic model：
Step 2.4：Mechanical arm momentum deviation observer is established, expression formula is：
In formula, r is joint of mechanical arm external force moment vector, and K is mechanical arm momentum deviation observer adjustment factor.Mechanical arm Momentum deviation Observer Structure frame diagram is as shown in figure 3, wherein：
Step 2.5：The torque transfer matrix P dot products joint of mechanical arm external force moment vector r obtained in step 1 is obtained To the collision torque τ of mechanical armnew, expression formula is：
τnew=Pr
Step 3：By mechanical arm collision torque τnewCompared with predetermined threshold value, judge whether mechanical arm collides.
Wherein, by the collision torque τ of mechanical armnewCompared with predetermined threshold value, judge whether mechanical arm collides, table It is up to formula：
In formula, TH1 is torque negative sense constant threshold；TH2 is torque forward direction constant threshold；By the numerical value of parameter d come Judge whether mechanical arm collides, when d is 1, detect that mechanical arm collides；When d is 0, mechanical arm collisionless hair It is raw.
In order to verify the correctness of sensorless mechanical arm collision checking method proposed by the invention, in matlab Under simulink platforms in software, mechanical arm collision detection is built with the simmechanics modules carried in matlab softwares System carries out simulating, verifying, and being detected collision for whichever joint of mechanical arm is all considered as mechanical arm and collides.Wherein, Mechanical arm tail end load is 2kg, and coordinate of the centroid position under ending coordinates system is (0,0,0.1), and unit is rice (m).Emulation Time terminated since 0 by the 5th second, in simulation process, when simulation time was by the 4th second, was applied at the barycenter of robot linkage five A cartesian space external force (2,0,0) is added, unit is newton (N), and the duration is 0.01 second, after emulation, is obtained Result as shown in Figure 4, Figure 5.
Four moment of face of joint of the mechanical arm obtained using traditional no sensor mechanism arm collision checking method is with the time The curve of change is as shown in figure 4, the part of curve smoothing is the moment of face as caused by loading mechanical arm tail end, the 4th on curve Torque mutation during the second is as caused by robot linkage five is applied in impact force suddenly.If use torque constant threshold, meeting The caused moment of face of load is detected, the phenomenon of erroneous judgement occurs in collision detection.
Five external force of joint of the mechanical arm obtained using no sensor mechanism arm collision checking method provided by the present invention The curve that square changes over time is as shown in figure 5, the torque mutation on curve at the 4th second is applied suddenly by robot linkage five Add caused by impact force, 10 times of suffered moment of face, institute when suffered moment of face is at least up when mechanical arm collides To be capable of detecting when whether mechanical arm collides by setting constant threshold, which has very high sensitive Degree, is not in erroneous judgement and the phenomenon failed to judge.
The curve for comparing two width figures understands that during for colliding with loaded mechanical arm, traditional method passes through pre- If constant threshold can not detect that mechanical arm collides, no sensor mechanism arm collision checking method provided by the present invention Cross default constant threshold and accurately detect out very much mechanical arm and collide, simulation result shows, proposed by the invention need not The mechanical arm collision checking method of sensor realizes the loaded mechanical arm collision detection of end band.
After the simulating, verifying that collision detection is carried out on matlab platforms, practical situations are considered, it is real in mechanical arm Collision detection experiment is carried out on body, sensorless mechanical arm collision checking method provided by the present invention is compiled with C language Write as code, downloaded in the program in mechanical arm controller.
Writing tutorial program makes mechanical arm each run track identical, and the end clamping of mechanical arm is kept in testing for the first time Load it is constant, the different parts of operating personnel's active collision machine arm during each manipulator motion, recording parameters d's Value；Change the quality of the load of the end clamping of mechanical arm in second of experiment, ensure to operate during each manipulator motion The same area of personnel's active collision machine arm, the value of recording parameters d, wherein, operating personnel's active collision machine arm will not make Operating personnel keenly feel, and result of the test is as shown in table 1 below：
1 mechanical arm collision detection result table of table
According to the test data of table 1, the mechanical arm of different quality load is carried for end, in manipulator motion mistake Operating personnel touch the identical position of mechanical arm in journey, by setting positive, negative sense constant threshold to be capable of detecting when that mechanical arm is sent out Raw collision；The mechanical arm of identical load is carried for end, it is different to touch mechanical arm by operating personnel during manipulator motion Position, by setting positive, negative sense constant threshold to be capable of detecting when that mechanical arm collides, and operator in collision process Member is not in pain, so collision checking method provided by the present invention can be realized with arbitrary load without sensor machine Tool arm collision detection.

Claims (4)

  1. A kind of 1. sensorless mechanical arm collision checking method, it is characterised in that：
    Step 1：The orthogonal intersection cast shadow matrix PF for the joint moment vector that calculating machine arm end load produces mechanical arm, according to Orthogonal intersection cast shadow matrix PF obtains the torque transfer matrix P of mechanical arm tail end load；
    Step 2：Mechanical arm body kinetic model is established, it is inclined that the items in the kinetic model establish mechanical arm momentum Poor observer, obtains joint of mechanical arm external force moment vector r, according to the external force motor by the mechanical arm momentum deviation observer The amount r and torque transfer matrix P of mechanical arm tail end load calculates the collision torque τ of mechanical armnew；
    Step 3：By the collision torque τ of the mechanical armnewCompared with predetermined threshold value, judge whether mechanical arm collides.
  2. 2. sensorless mechanical arm collision checking method as claimed in claim 1, it is characterised in that：The step 1 In, the torque transfer matrix P of calculating machine arm end load is comprised the following steps that：
    Step 1.1：The data fed back according to mechanical arm driver obtain the joint angles q of joint of mechanical arm feedback, angular speed By joint angular speedCarry out difference and obtain joint angular acceleration
    Step 1.2：Mechanical arm speed Jacobian matrix J is established according to the D-H parameters of mechanical arm body, according to the joint angle of feedback SpeedThe velocity υ of mechanical arm tail end is obtained with mechanical arm speed Jacobian matrix J, expression formula is：
    <mrow> <mi>&amp;upsi;</mi> <mo>=</mo> <mi>J</mi> <mo>&amp;CenterDot;</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> </mrow>
    Step 1.3：By the velocity υ difference of the mechanical arm tail end, the acceleration a of mechanical arm tail end is obtained, by this The acceleration a and gravitational vectors g of mechanical arm tail end sum to obtain the total acceleration A of mechanical arm tail end, by mechanical arm End load quality point multiplies the total acceleration A of mechanical arm tail end, obtains the total force vector F of mechanical arm tail end；
    Step 1.4：Mechanical arm strength Jacobian matrix J is established according to the D-H parameters of mechanical arm bodyF, according to mechanical arm tail end force vector Measure F and mechanical arm strength Jacobian matrix JF, joint of mechanical arm moment vector τ is calculatedt, expression formula is：
    τt=JF·F
    Step 1.5：According to the joint of mechanical arm moment vector τt, establish joint of mechanical arm torque matrix B F=τt, then calculate Go out the joint of mechanical arm moment vector τtOrthogonal intersection cast shadow matrix PF, expression formula is：
    PF=BF (BFT·BF)-1·BFT
    Step 1.6：According to the orthogonal intersection cast shadow matrix PF, the torque transfer matrix P of calculating machine arm end load, expression formula For：
    P=I-PF.
  3. 3. sensorless mechanical arm collision checking method as claimed in claim 2, it is characterised in that：The step 2 In, the collision torque τ of calculating machine armnewComprise the following steps that：
    Step 2.1：Mechanical arm body kinetic model is established, expression formula is：
    <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mo>+</mo> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>+</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;tau;</mi> <mi>f</mi> </msub> <mo>=</mo> <mi>&amp;tau;</mi> </mrow>
    In formula, M (q) is Inertia；For Non-linear coupling item；G (q) is gravity item, τfFor frictional force item, τ is by machine The instruction torque that tool arm kinetic model is calculated；
    Step 2.2：According to the Inertia M (q) in mechanical arm body kinetic model, the generalized momentum p of calculating machine arm, expression Formula is：
    <mrow> <mi>p</mi> <mo>=</mo> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> </mrow>
    Step 2.3：Inertia M (q) and Non-linear coupling item in mechanical arm body kinetic modelComputer The transposed matrix of Non-linear coupling item in tool arm body kinetic modelExpression formula is：
    <mrow> <msup> <mi>C</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mo>=</mo> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> </mrow> </mrow>
    Step 2.4：Mechanical arm momentum deviation observer is established, expression formula is：
    <mrow> <mi>r</mi> <mo>=</mo> <mi>K</mi> <mo>&amp;lsqb;</mo> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>m</mi> <mi>o</mi> <mi>t</mi> </mrow> </msub> <mo>+</mo> <msup> <mi>C</mi> <mi>T</mi> </msup> <mo>(</mo> <mrow> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> </mrow> <mo>)</mo> <mo>-</mo> <mi>G</mi> <mo>(</mo> <mi>q</mi> <mo>)</mo> <mo>-</mo> <msub> <mi>&amp;tau;</mi> <mi>f</mi> </msub> <mo>-</mo> <mi>r</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> <mo>-</mo> <mi>p</mi> <mo>&amp;rsqb;</mo> </mrow>
    In formula, τmotFor mechanical arm joint motor output torque, r is joint of mechanical arm external force moment vector, and K is mechanical arm momentum Deviation observer adjustment factor；
    Step 2.5：The torque transfer matrix P dot products joint of mechanical arm external force moment vector r obtained in step 1 is obtained into machine The collision torque τ of tool armnew, expression formula is：
    τnew=Pr.
  4. 4. sensorless mechanical arm collision checking method as claimed in claim 3, it is characterised in that：The step 3 In, by the collision torque τ of mechanical armnewCompared with predetermined threshold value, judge whether mechanical arm collides, expression formula is：
    <mrow> <mi>d</mi> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;tau;</mi> <mrow> <mi>n</mi> <mi>e</mi> <mi>w</mi> </mrow> </msub> <mo>&amp;GreaterEqual;</mo> <mi>T</mi> <mi>H</mi> <mn>2</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>T</mi> <mi>H</mi> <mn>1</mn> <mo>&lt;</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>n</mi> <mi>e</mi> <mi>w</mi> </mrow> </msub> <mo>&lt;</mo> <mi>T</mi> <mi>H</mi> <mn>2</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;tau;</mi> <mrow> <mi>n</mi> <mi>e</mi> <mi>w</mi> </mrow> </msub> <mo>&amp;le;</mo> <mi>T</mi> <mi>H</mi> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
    In formula, TH1 is torque negative sense constant threshold；TH2 is torque forward direction constant threshold；Judged by the numerical value of parameter d Whether mechanical arm collides, and when d is 1, detects that mechanical arm collides；When d is 0, mechanical arm collisionless occurs.
CN201711347621.7A 2017-12-15 2017-12-15 Sensor-free mechanical arm collision detection method Active CN108015774B (en)

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 true CN108015774A (en) 2018-05-11
CN108015774B 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)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109062051A (en) * 2018-08-28 2018-12-21 苏州艾利特机器人有限公司 A method of improving Identification of Dynamic Parameters of Amanipulator precision
CN109159120A (en) * 2018-09-10 2019-01-08 南京邮电大学 Active Control Method and system based on rehabilitation mechanical arm joint motor current feedback
CN109732599A (en) * 2018-12-29 2019-05-10 深圳市越疆科技有限公司 A kind of robot collision checking method, device, storage medium and robot
CN110026981A (en) * 2019-04-19 2019-07-19 中科新松有限公司 A kind of mechanical arm collision checking method based on model adaptation
CN110340885A (en) * 2019-05-21 2019-10-18 南京航空航天大学 A kind of industrial robot collision checking method based on energy deviation observer
CN110990943A (en) * 2019-11-13 2020-04-10 上海航天控制技术研究所 Singular point judgment method based on singular geometric meaning of control moment gyro group
CN111872936A (en) * 2020-07-17 2020-11-03 清华大学 Robot collision detection system and method based on neural network
CN111890343A (en) * 2020-07-29 2020-11-06 浙江广合智能科技有限公司 Robot object collision detection method and device
CN113043283A (en) * 2021-04-23 2021-06-29 江苏理工学院 Robot tail end external force estimation method
CN113799142A (en) * 2021-10-29 2021-12-17 遨博（北京）智能科技有限公司 Collision protection method for mechanical arm, control cabinet and mechanical arm system
CN114012734A (en) * 2021-12-03 2022-02-08 西安交通大学 Parameter-adaptive robot collision detection method
CN114800620A (en) * 2022-06-13 2022-07-29 湖南科技大学 Robot external force detection method for force-free sensor

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006123012A (en) * 2004-10-26 2006-05-18 Matsushita Electric Ind Co Ltd Robot control method
US20090105997A1 (en) * 2007-10-19 2009-04-23 Sony Corporation Dynamics simulation device, dynamics simulation method, and computer program
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

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006123012A (en) * 2004-10-26 2006-05-18 Matsushita Electric Ind Co Ltd Robot control method
US20090105997A1 (en) * 2007-10-19 2009-04-23 Sony Corporation Dynamics simulation device, dynamics simulation method, and computer program
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)

* Cited by examiner, † Cited by third party
Title
肖聚亮等: "一种轻型模块化协作机器人碰撞检测算法", 《天津大学学报（自然科学与工程技术版）》 *

Cited By (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109062051A (en) * 2018-08-28 2018-12-21 苏州艾利特机器人有限公司 A method of improving Identification of Dynamic Parameters of Amanipulator precision
CN109159120A (en) * 2018-09-10 2019-01-08 南京邮电大学 Active Control Method and system based on rehabilitation mechanical arm joint motor current feedback
CN109159120B (en) * 2018-09-10 2022-07-12 南京邮电大学 Active control method and system based on current feedback of joint motor of rehabilitation mechanical arm
CN109732599A (en) * 2018-12-29 2019-05-10 深圳市越疆科技有限公司 A kind of robot collision checking method, device, storage medium and robot
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
CN110026981A (en) * 2019-04-19 2019-07-19 中科新松有限公司 A kind of mechanical arm collision checking method based on model adaptation
CN110340885A (en) * 2019-05-21 2019-10-18 南京航空航天大学 A kind of industrial robot collision checking method based on energy deviation observer
CN110340885B (en) * 2019-05-21 2021-12-07 南京航空航天大学 Industrial robot collision detection method based on energy deviation observer
CN110990943A (en) * 2019-11-13 2020-04-10 上海航天控制技术研究所 Singular point judgment method based on singular geometric meaning of control moment gyro group
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
CN111872936A (en) * 2020-07-17 2020-11-03 清华大学 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
CN111890343A (en) * 2020-07-29 2020-11-06 浙江广合智能科技有限公司 Robot object collision detection method and device
CN113043283A (en) * 2021-04-23 2021-06-29 江苏理工学院 Robot tail end external force estimation method
CN113043283B (en) * 2021-04-23 2022-07-08 江苏理工学院 Robot tail end external force estimation method
CN113799142A (en) * 2021-10-29 2021-12-17 遨博（北京）智能科技有限公司 Collision protection method for mechanical arm, control cabinet and mechanical arm system
CN113799142B (en) * 2021-10-29 2023-02-21 遨博（北京）智能科技有限公司 Collision protection method for mechanical arm, control cabinet and mechanical arm system
CN114012734A (en) * 2021-12-03 2022-02-08 西安交通大学 Parameter-adaptive robot collision detection method
CN114800620A (en) * 2022-06-13 2022-07-29 湖南科技大学 Robot external force detection method for force-free sensor
CN114800620B (en) * 2022-06-13 2023-12-12 湖南科技大学 Robot external force detection method without force sensor

Also Published As

Publication number Publication date
CN108015774B (en) 2020-10-13

Similar Documents

Publication Publication Date Title
CN108015774A (en) A kind of sensorless mechanical arm collision checking method
Zhang et al. A sensorless hand guiding scheme based on model identification and control for industrial robot
US8583285B2 (en) Method and device for stopping a manipulator
CN101332604B (en) Control method of man machine interaction mechanical arm
Albahkali et al. Swing-up control of the pendubot: an impulse–momentum approach
CN108772838A (en) A kind of mechanical arm safety collision strategy based on outer force observer
CN105479459A (en) Zero-force control method and system for robot
CN109676607A (en) A kind of zero-g control method of non-moment sensing
CN108372505A (en) A kind of collision detection algorithm and system based on dynamic (dynamical) robot
CN105945979B (en) The method that Shared control is carried out to the paw mechanism of drive lacking two
CN105404744B (en) A kind of space manipulator total state dynamics semi-physical system
CN102189550A (en) Robot having learning control function
Martin et al. Dynamic load emulation in hardware-in-the-loop simulation of robot manipulators
Shang et al. Dynamic model based nonlinear tracking control of a planar parallel manipulator
CN111267105A (en) Kinetic parameter identification and collision detection method for six-joint robot
CN110666794B (en) Method for compensating load force and moment of end tool of robot
CN108638070A (en) Robot based on dynamic equilibrium loads weight parameter discrimination method
CN110682290B (en) Closed-loop mechanical arm system collision detection method based on momentum observer
Benabdallah et al. Kinect-based Computed Torque Control for lynxmotion robotic arm
CN114367975A (en) Verification system of series industrial robot control algorithm
Wang et al. Traching control of a redundant manipulator with the assistance of tactile sensing
Song et al. A PD computed torque control method with online self-gain tuning for a 3UPS-PS parallel robot
Wang et al. Manipulation of a mobile modular manipulator interacting with the environment with the assistance of tactile sensing feedback
Jing et al. Real-time collision detection for manipulators based on fuzzy synthetic evaluation
Pascault Finite-time tracking of trajectories in task space for stewart platform

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