A kind of singular point regional slowdown protection system and industrial robotTechnical field
The application relates to robotics, more particularly, it relates to a kind of singular point regional slowdown protection system and industrial robot.
Background technology
Industrial robot is one of parts important in industrial automation system, currently, multiple countries have set up certain leading superiority on Industrial Robot Technology, especially in the design of robot body, related basic technology comparative maturity, thus numerous producers of robot has put into great effort in robot application.Singular point processing method to industrial robot is main technological difficulties in robot application technology; it is the technical problem that industrial robot necessarily runs into; industrial robot once runs into or close to singular point; joint motions speed will be occurred to become big phenomenon, thus cause robot to shut down and even bring production safety problem.
Utility model content
In view of this, the application provides one singular point regional slowdown protection system and industrial robot, for solving industrial robot because in singular point region because speed becomes the problem causing greatly shutdown or production safety.
To achieve these goals, it is proposed that scheme as follows:
A kind of singular point regional slowdown protection system, is applied to the industrial robot with multiple joint, comprising:
Real-time detection module, calculate the target speed in described joint for the real-time status data in the joint according to described industrial robot and target state data, described real-time status data include real time kinematics angle and the real-time time in described joint, and described target state data includes the target movement angle corresponding to motor task and the target run duration preset；
Singular point area calculation module, for when the position in described joint is in singular point transitional region, calculates the permissible velocity in described joint；
Singular point processing module, protects for carrying out deceleration according to described target speed and described permissible velocity to described joint.
Optionally, described real-time detection module includes:
Ontology information unit, is used for obtaining described real time kinematics angle and described real-time time；
Model treatment unit, for calculating described target speed according to described real time kinematics angle, described real-time time, described target movement angle and described target run duration.
Optionally, described model treatment unit includes:
Forward and reverse solution subelement, for the target location according to the described joint of described motor task calculating and targeted attitude, and calculates described target movement angle and described target run duration according to described target location and targeted attitude；
Speed computation subunit, for calculating described target speed according to described real time kinematics angle, described real-time time, described target movement angle and described target run duration.
Optionally, described real-time detection module also includes:
Alarm unit, for when movement velocity and the position in described joint send default control instruction when being in abnormality.
Optionally, described alarm unit includes:
Hypervelocity alarm subelement, for sending the first control instruction when described movement velocity is beyond the threshold speed preset, described first control instruction is used for controlling described industrial robot and shuts down；
Working region alarm subelement, for sending the second control instruction when the position of the end of described industrial robot exceeds normal operation region, described second control instruction is used for controlling described industrial robot and shuts down；
Singular point zone alarm subelement, for sending motion state information according to the judgement of the movement position to described industrial robot.
A kind of industrial robot, is provided with singular point regional slowdown as above protection system.
Can be seen that from above-mentioned technical scheme; this application discloses a kind of singular point regional slowdown protection system and industrial robot; this system is applied to the industrial robot with multiple joint; it is specially the real-time status data in the joint according to industrial robot and target state data calculates the target speed in this joint; real-time status data include real time kinematics angle and the real-time time in joint, and target state data includes the target movement angle corresponding to motor task and the target run duration preset；Then, when the position in described joint is in singular point transitional region, permission position and the permissible velocity in joint is calculated；Carry out deceleration finally according to target speed and permissible velocity to joint to protect; i.e. after the movement velocity in the joint movement velocity in singular point transitional region exceeds permissible velocity, timely controller slows down; to avoid the generation of hypervelocity such that it is able to solve industrial robot because in singular point region because speed becomes the problem of the shutdown causing or production safety greatly.
Brief description
In order to be illustrated more clearly that the embodiment of the present application or technical scheme of the prior art, the accompanying drawing of required use in embodiment or description of the prior art will be briefly described below, apparently, accompanying drawing in describing below is only some embodiments of the present application, for those of ordinary skill in the art, on the premise of not paying creative work, other accompanying drawing can also be obtained according to these accompanying drawings.
The flow chart of a kind of singular point regional slowdown guard method that Fig. 1 provides for the embodiment of the present application
The D-H model schematic of a kind of industrial robot that Fig. 2 provides for the application；
The structured flowchart of a kind of singular point regional slowdown protection system that Fig. 3 provides for another embodiment of the application；
Fig. 4 provides the structured flowchart of another kind of singular point regional slowdown protection system for the application example；
Fig. 5 provides the structured flowchart of another singular point regional slowdown protection system for the application example.
Detailed description of the invention
Below in conjunction with the accompanying drawing in the embodiment of the present application, the technical scheme in the embodiment of the present application is clearly and completely described, it is clear that described embodiment is only some embodiments of the present application, rather than whole embodiments.Based on the embodiment in the application, every other embodiment that those of ordinary skill in the art are obtained under the premise of not making creative work, broadly fall into the scope of the application protection.
Embodiment one
The flow chart of a kind of singular point regional slowdown guard method that Fig. 1 provides for the embodiment of the present application.
As it is shown in figure 1, the singular point regional slowdown guard method that the present embodiment provides is applied to the industrial robot with multiple joint, describing for convenience, the industrial robot selected by the present embodiment has 6 joints.
Industrial robot is typically to carry out the structural modeling of robot with D-H model, as in figure 2 it is shown, position and posture changing between each two joint can use the D-H matrix of four parameters to be simulated calculating.The industrial robot structure in modal 6 joints can be represented by six D-H joint transformation matrixs, is respectively as follows:
Wherein, ci=cos (θi),si=sin (θi), i=1,2 ..., 5,6, θiIt is the articulation angle in the i-th joint, aiFor the length of connecting rod between joint i to joint i+1, diFor the connecting rod offset distance between joint i-1 to joint i.
Robot base can be obtained by calculating matrix multiplication to the transformation matrix of robot end:
Based on above-mentioned analysis, the singular point regional slowdown guard method that the present embodiment provides comprises the following steps:
S101: the real-time status data in the joint according to industrial robot and target state data calculate the target speed in joint.
Wherein real-time status data include real time kinematics angle and the real-time time in joint, and target state data includes the target movement angle corresponding to motor task and the target run duration preset.The calculating process of target speed includes:
Step 1: obtain the real-time articulation angle, θ of joint of robotiAnd real-time time t.
Step 2: use closing form solution to calculate target location and the targeted attitude of motor task, solve the target movement angle θ ' that joint needs to rotatei, and target run duration t' of motor task is gone out according to interpolation Time Calculation.So real time kinematics angle, θ according to posterior jointiAnd need the target movement angle θ ' rotatingi, target run duration t' of real-time time t and motor task, solve the target speed in the joint of motor task:
S102: when the position in joint is in singular point transitional region, calculates the permissible velocity in joint.
First, DLS (damping minimum variance) algorithm is used to carry out singular value control:
J'=JT(JJT+ρ2I)-1
Wherein, J is robot Jacobian matrix, and J' is the Jacobian matrix after addition damping factor, ρ2For damping factor.
Control to the control of damping factor adds singular value:
Wherein, δ is the minimum singular value of Jacobian matrix J, and ε is the max-thresholds of δ.
Work as Tw2≤|θ5|<Tw1When, set movement velocity threshold value k.
Calculating robot is in threshold value Tw1The movement velocity at place is as movement velocity maximumThen to threshold value Tw1The movement velocity located is normalized, and the ratio of the movement velocity not using deceleration algorithm and the movement velocity using deceleration algorithm is
Use joint 5 angle value as the variable of deceleration algorithm, calculated distance variable is
The permissible velocity in the joint after deceleration algorithm correctionFor:
S103: according to target speed and permissible velocity, deceleration is carried out to joint and protect.
When the target speed reaching for performing motor task when joint exceeds permissible velocity, control this joint in time and reduce speed, be allowed to without departing from permissible velocity, thus avoid the occurrence of hypervelocity.
Can be seen that from technique scheme; present embodiments provide a kind of singular point regional slowdown guard method; the method is applied to the industrial robot with multiple joint; it is specially the real-time status data in the joint according to industrial robot and target state data calculates the target speed in this joint; real-time status data include real time kinematics angle and the real-time time in joint, and target state data includes the target movement angle corresponding to motor task and the target run duration preset；Then, when the position in joint is in singular point transitional region, permission position and the permissible velocity in joint is calculated；Carry out deceleration finally according to target speed and permissible velocity to joint to protect; i.e. after the movement velocity in the joint movement velocity in singular point transitional region exceeds permissible velocity, timely controller slows down; to avoid the generation of hypervelocity such that it is able to solve industrial robot because in singular point region because speed becomes the problem of the shutdown causing or production safety greatly.
During the real-time status data in the joint according to industrial robot in the present embodiment and target state data calculate the target speed in joint, also include step 3, this step is alarm step, sends default control instruction for the movement velocity in the joint of industrial robot and position when being in abnormality.
First, movement velocity selected threshold T respectively for 6 jointsi, i=1,2 ..., 5,6, judge:
To flagiValue part detection, work as flagiWhen=0, robot motion is exceeded the speed limit, and equipment sends the first control instruction, and this first control instruction is used for controlling industrial robot and shuts down.
Meanwhile, setting the working region zone of robot, then to the position p of the end of industrial robot, (x, y z) are operated region decision.
To flagzoneValue detect, work as flagzoneWhen=0, robot exceeds normal operation region, at this moment sends the second control instruction, and this second control instruction is equally used for control industrial robot and shuts down, to avoid because causing security incident beyond normal operation region.
Finally, also carry out threshold decision to the joint signal of industrial robot, first obtain wrist singular point judgment value θ5.Choose singular point threshold value T for singular point respectivelyw, then judge:
To flagwValue detect, work as flagwWhen being 0 now, robot motion is to singular point transitional region, and equipment sends singular point transition warning information；Work as flagwWhen being 1 now, robot motion is to singular point decelerating area, and equipment sends singular point deceleration warning information.Work as flagwWhen being 2 now, robot proper motion.
Embodiment two
The structured flowchart of a kind of singular point regional slowdown protection system that Fig. 3 provides for the embodiment of the present application.
As shown in Figure 3; the singular point regional slowdown protection system that the present embodiment provides is applied to the industrial robot with multiple joint; describe for convenience equally; industrial robot selected by the present embodiment has 6 joints, and this system includes real-time detection module the 10th, singular point area calculation module 20 and singular point processing module 30.
Real-time detection module 10 is used for the real-time status data in the joint according to industrial robot and target state data calculates the target speed in joint.
Wherein real-time status data include real time kinematics angle and the real-time time in joint, and target state data includes the target movement angle corresponding to motor task and the target run duration preset.Real-time detection module 10 includes ontology information unit 11 and model treatment unit 12, as shown in Figure 4.
Ontology information unit 11 is for obtaining the real-time articulation angle, θ of joint of robotiAnd real-time time t.
Model treatment unit 12 includes forward and reverse solution subelement 121 and speed computation subunit 122.
Forward and reverse solution subelement 121, for using closing form solution to calculate the target location of motor task and targeted attitude, solves the target movement angle θ ' that joint needs to rotatei, and target run duration t of motor task is gone out according to interpolation Time Calculation '.
Speed computation subunit 122 is for the real time kinematics angle, θ according to jointiAnd need the target movement angle θ ' rotatingi, target run duration t' of real-time time t and motor task, solve the target speed in the joint of motor task:
Singular point area calculation module 20, for when the position in joint is in singular point transitional region, calculates the permissible velocity in joint.
First, DLS (damping minimum variance) algorithm is used to carry out singular value control:
J'=JT(JJT+ρ2I)-1
Wherein, J is robot Jacobian matrix, and J' is the Jacobian matrix after addition damping factor, ρ2For damping factor.
Control to the control of damping factor adds singular value:
Wherein, δ is the minimum singular value of Jacobian matrix J, and ε is the max-thresholds of δ.
Work as Tw2≤|θ5|<Tw1When, set movement velocity threshold value k.
Calculating robot is in threshold value Tw1The movement velocity at place is as movement velocity maximumThen to threshold value Tw1The movement velocity located is normalized, and the ratio of the movement velocity not using deceleration algorithm and the movement velocity using deceleration algorithm is
Use joint 5 angle value as the variable of deceleration algorithm, calculated distance variable is
The permissible velocity in the joint after deceleration algorithm correctionFor:
Singular point processing module 30 is protected for carrying out deceleration according to target speed and permissible velocity to joint.
When the target speed reaching for performing motor task when joint exceeds permissible velocity, control this joint in time and reduce speed, be allowed to without departing from permissible velocity, thus avoid the occurrence of hypervelocity.
Can be seen that from technique scheme; present embodiments provide a kind of singular point regional slowdown protection system; this system is applied to the industrial robot with multiple joint; it is specially the real-time status data in the joint according to industrial robot and target state data calculates the target speed in this joint; real-time status data include real time kinematics angle and the real-time time in joint, and target state data includes the target movement angle corresponding to motor task and the target run duration preset；Then, when the position in joint is in singular point transitional region, permission position and the permissible velocity in joint is calculated；Carry out deceleration finally according to target speed and permissible velocity to joint to protect; i.e. after the movement velocity in the joint movement velocity in singular point transitional region exceeds permissible velocity, timely controller slows down; to avoid the generation of hypervelocity such that it is able to solve industrial robot because in singular point region because speed becomes the problem of the shutdown causing or production safety greatly.
Real-time detection module 10 in the present embodiment also includes alarm unit 13, this alarm unit sends default control instruction for the movement velocity in the joint of industrial robot and position when being in abnormality, specifically include hypervelocity alarm subelement the 131st, working region alarm subelement 132 and singular point zone alarm subelement 133, as shown in Figure 5.
Hypervelocity alarm subelement 131 is for movement velocity selected threshold T respectively in 6 jointsi, i=1,2 ..., 5,6, judge:
Then to flagiValue part detection, work as flagiWhen=0, robot motion is exceeded the speed limit, and sends the first control instruction, and this first control instruction is used for controlling industrial robot and shuts down.
Working region alarm subelement 132 is for setting the working region zone of robot, and then to the position p of the end of industrial robot, (x, y z) are operated region decision.
To flagzoneValue detect, work as flagzoneWhen=0, robot exceeds normal operation region, at this moment sends the second control instruction, and this second control instruction is equally used for control industrial robot and shuts down, to avoid because causing security incident beyond normal operation region.
Singular point zone alarm subelement 133, for carrying out threshold decision to the joint signal of industrial robot, first obtains wrist singular point judgment value θ5.Choose singular point threshold value T for singular point respectivelywJudge:
To flagwValue detect, work as flagwWhen being 0 now, robot motion is to singular point transitional region, and equipment sends singular point transition warning information；Work as flagwWhen being 1 now, robot motion is to singular point decelerating area, and equipment sends singular point deceleration warning information.Work as flagwWhen being 2 now, robot proper motion.
Embodiment three
Present invention also provides a kind of industrial robot; this industrial robot is provided with the singular point regional slowdown protection system that above example is provided; for making this industrial robot not have hypervelocity phenomenon in singular point region, it can be avoided that the shutdown occurring because of hypervelocity or security incident.
In this specification, each embodiment uses the mode gone forward one by one to describe, and what each embodiment stressed is the difference with other embodiments, and between each embodiment, identical similar portion sees mutually.Described above to the disclosed embodiments, makes professional and technical personnel in the field be capable of or uses the application.Multiple modifications to these embodiments will be apparent from for those skilled in the art, and generic principles defined herein can realize in the case of without departing from spirit herein or scope in other embodiments.Therefore, the application is not intended to be limited to the embodiments shown herein, and is to fit to the wide scope consistent with principles disclosed herein and features of novelty.