CN109397283A - A kind of robot collision checking method and device based on velocity deviation - Google Patents
A kind of robot collision checking method and device based on velocity deviation Download PDFInfo
- Publication number
- CN109397283A CN109397283A CN201810046422.0A CN201810046422A CN109397283A CN 109397283 A CN109397283 A CN 109397283A CN 201810046422 A CN201810046422 A CN 201810046422A CN 109397283 A CN109397283 A CN 109397283A
- Authority
- CN
- China
- Prior art keywords
- robot
- speed
- ideal
- filter
- collision
- 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
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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
Abstract
A kind of robot collision checking method and device based on velocity deviation, it is specific using optimization is filtered to ideal calculating speed, ideal calculating speed is handled using the hysteresis quality of filter；Itself and actual speed is set to subtract each other to obtain optimal speed deviation；Robot collision detection bound threshold value is set；Robot stop motion is controlled when detecting that optimal speed deviation is more than threshold value or other is taken to evade algorithm avoids collision bring personal injury and robotic damage.By above-mentioned setting, detection sensitivity is improved, so that robot acceleration and deceleration interference hits be avoided to detect.The method is simple and effective, does not need to increase additional force snesor, does not also need the other parameters such as detection acceleration, electric current, torque, does not also include complicated robot dynamics' operation in algorithm.The experimental results showed that this method can it is effective, sensitive, be accurately detected collision, based on this can control robot stop motion immediately and avoid damaging people and robot itself.
Description
Technical field
The present invention relates to technical field of robot control, and in particular to a kind of method and device of robot collision detection.
Background technique
Robot can replace people and execute efficient, the repeated manual labor of high-precision.Machine man-hour, and if surrounding
Environment collides, and may cause ambient enviroment or robot body damage.In the task of some complex precises, generally require
Robot and the work of worker's coordinated.At this moment, the safety of robot is particularly important.Robot needs to take necessary safety
Anticollision measure carrys out the safety of guarantor and robot body.When colliding generation, robot wants to detect and collides and take
Certain control strategy, which avoids collision, causes serious consequence.
Currently, some scholars propose different collision detection schemes from multiple angles.Such as in robotic surface packet
Wrap up in skin sensor.This method can be accurately detected collision and colliding part, but considerably increase the complexity of robot with
Cost reduces flexibility.Some schemes increase force snesor at joint of robot.The program increases robot cost simultaneously
And it is only applicable to be assembled with the manipulator shaft of force snesor.There is scholar to propose to utilize visual sensor observation collision.In the program
Image processing data amount is huge, real-time is poor, and visual sensor generally all has blind area.
Scholar proposes to judge to be by the error of comparison joint of robot position command value and practical joint position value
The no scheme to collide.Some scholars further propose by joint of robot position command by after algorithm process with
The scheme of practical joint position value comparison.In fact, the position ring of joint of robot motor servo control is in robot servo
Most outer ring.The error of joint of robot position command value and practical joint position value is not " sensitive ".The position as caused by collision
The error for setting instruction value and practical joint position value is very small, very easy to be covered by other noises and judge that robot is
It is no to be collided.The feasibility of the program is low.
In addition, there are also scholars to propose that the contrasting detection for calculating torque and actual samples torque according to robot dynamics is collided
And the detection scheme based on energy, momentum.These schemes or calculating complexity or real-time, precision be not high.
And it has not yet to see scholar and proposes based on the correlation handled robot speed to judge collision detection
Scheme.
Summary of the invention
It is an object of the invention to have to increase additional sensors or calculating again for existing robot collision detection scheme
The disadvantages of miscellaneous or real-time is not high provides and a kind of is not required to additionally to increase sensor, simply and effectively based on the machine of velocity deviation
People's collision checking method and device.
The present invention is achieved through the following technical solutions:
A kind of robot collision checking method based on velocity deviation, which comprises the steps of:
1) setting robot collides upper limit threshold Δ ωupperWith collision lower threshold Δ ωlower, Δ ω is velocity deviation；
2) motion profile, the speed, acceleration of robot are set；
3) the ideal movements speed omega of robot is solved according to selected Motion trajectory algorithmideal；
4) by ideal movements speed omegaidealOptimal speed ω ' is obtained by the processing of filterideal；
5) real-time measurement values of robot motion track are obtained；
6) it differentiates to the motion profile of robot and brings real-time measurement values into, obtain robot actual speed ωreal；
7) by optimal speed ω 'idealWith actual speed ωrealSubtract each other to obtain the velocity deviation Δ ω:
Δ ω=ω 'ideal-ωreal； (1)
8) judge whether to collide, if Δ ω > Δ ωupperOr Δ ω < Δ ωlower, then it is assumed that collision occurs；
9) after detecting that collision occurs, robot takes corresponding measure to avoid collision to robot body or its ambient enviroment
It causes serious harm.
Further, the motion profile of robot, speed, acceleration are respectively set to each joint of robot in step 2
Joint angle, angular speed, angular acceleration.
Further, the Motion trajectory algorithms selection quintic algebra curve interpolating function in step 3:
θ (t)=a0+a1t+a2t2+a3t3+a4t4+a5t5 (2)
Its single order and second dervative are as follows:
In above formula, θ (t),The respectively joint angle of robot, angular speed, angular acceleration；a0、a1、a2、a3、
a4、a5For polynomial coefficient；T is time variable；
For wherein one section of motion profile, according to each joint angle Track Initiation shape of robot and terminal position, angular speed,
Angular acceleration quantity of state, counter 6 coefficient a for solving quintic algebra curve interpolating function0、a1、a2、a3、a4、a5It is as follows:
Wherein tfIt solves joint of robot ideal in this section of motion profile according to coefficient obtained by (5) formula for the terminal time and transports
Dynamic speed are as follows:
A in above formula (6)1、a2、a3、a4、a55 coefficients are replaced by formula (5) respectively.
Further, the Motion trajectory algorithm in step 3 may be selected to be cubic algebraic curves algorithm, high order polynomial
Formula interpolation algorithm, cubic spline interpolation algorithm, the linear interpolation algorithm with parabolic transition.
Further, this method is suitable for the online trajectory planning of Off-Line Trajectory Planning of Manipulators or robot；In offline rail
In mark planning, the optimal speed is obtained by filter process, optimal speed data are then stored in host controller
In, it is compared in real time with actual speed, for judging whether collision occurs；In online trajectory planning, host controller according to
Trajectory planning instruction obtains the optimal speed by filter process and is compared with actual speed, touches for judging
It hits and whether occurs.
Further, the filter in step 4 has k, and k is the integer more than or equal to 1,
ω′ideal=η1ω′ideal1+η2ω′ideal2+…+ηkω′ideal(k) (7)
Wherein, ω 'ideal1、ω′ideal2、……ω′ideal(k)Respectively the 1st, the 2nd ... after k-th of filter process
Optimal speed, η1, η2... ηkRespectively the 1st, there are η in the 2nd, weight shared by k-th of filter1+η2+…ηk=1.
Another aspect of the present invention provides a kind of robot controller for realizing the above method, the control device
Including host controller and servo-system；
The host controller for robot trajectory planning, systemic hierarchial signal processing, upper layer algorithm realize and it is man-machine
Interaction；
Servo-system includes position ring controller, speed ring controller and current loop controller, the position ring controller,
Speed ring controller and current loop controller are sequentially connected, for adjusting servo motor；
The control device further includes filter, is connected to host controller, ideal movements speed omegaidealPass through filter
Processing obtain optimal speed ω 'ideal。
Further, the filter has k, and k is the integer more than or equal to 1,
ω′ideal=η1ω′ideal1+η2ω′ideal2+…+ηkω′ideal(k)
Wherein, ω 'ideal1、ω′ideal2、……ω′ideal(k)Respectively the 1st, the 2nd ... after k-th of filter process
Optimal speed, η1, η2... ηkRespectively the 1st, the 2nd ... weight shared by k-th of filter has η1+η2+…ηk=1.
Above-mentioned technical proposal of the invention has following beneficial technical effect:
1) ideal calculating speed is handled using the hysteresis quality of filter, it is enable to suit well with actual speed.In this way
It can reduce velocity deviation when robot works normally, improve detection sensitivity, so that robot acceleration and deceleration interference be avoided to touch
Hit detection；
2) for access speed as test object, calculation method is simple and effective, does not need to increase additional sensor, not need
Acceleration, electric current are detected, torque etc. does not need to carry out complicated robot dynamics' operation yet；
3) robot collision detection bound threshold value is set；Machine is controlled when detecting that optimal speed deviation is more than threshold value
People's stop motion or take other evade algorithm avoid collision bring personal injury and robotic damage；
4) simplify robot collision detecting system, reduce robot cost, promote the development of robot technology.
Detailed description of the invention
Fig. 1 is robot control system schematic diagram in the prior art；
Fig. 2 is the robot control system schematic diagram based on velocity deviation；
Fig. 3 is the collision experiment result figure under ideal velocity deviation；
Fig. 4 is optimal speed deviation algorithm block diagram；
Fig. 5 is the collision experiment result figure under optimal speed deviation；
Fig. 6 is the robot control system schematic diagram based on optimal speed deviation；
Fig. 7 is the collision experiment comparative result figure under actual speed deviation and optimal speed deviation；
Fig. 8 is the robot control system schematic diagram of multiple filter optimization speed.
Appended drawing reference:
1: host controller；2: position ring controller；3: speed ring controller；4: current loop controller；5: servo-system；
6: filter.
Specific embodiment
In order to make the objectives, technical solutions and advantages of the present invention clearer, With reference to embodiment and join
According to attached drawing, the present invention is described in more detail.It should be understood that these descriptions are merely illustrative, and it is not intended to limit this hair
Bright range.In addition, in the following description, descriptions of well-known structures and technologies are omitted, to avoid this is unnecessarily obscured
The concept of invention.
It is an object of the invention to have to increase additional sensors or calculating again for existing robot collision detection scheme
The disadvantages of miscellaneous or real-time is not high provides a kind of side for being not required to additionally increase sensor, simple and effective robot collision detection
Case.In general, a robot has multiple freedom degrees, and the control system of each freedom degree is as shown in Figure 1.Upper control
Device is mainly responsible for the functions such as robot trajectory planning, systemic hierarchial signal processing, the realization of upper layer algorithm, human-computer interaction.Servo system
System mainly includes position ring, speed ring, electric current loop, adjusts servo motor and runs on optimal performance state.Host controller is being done
There can be various trajectory planning algorithms when robot trajectory planning, the present invention is applicable in.By taking one section of track as an example, to machine
The whole story pose of device people asks Inverse Kinematics Solution to obtain the initial value θ (0) and final value θ (t of joint of robotf).It can be adopted in algorithm
With cubic algebraic curves function, cubic spline interpolation, highe order polynomial interpolation function, with the linear interpolation letter of parabolic transition
The joint trajectories interpolating functions such as number, by taking interpolating function is quintic algebra curve function as an example:
θ (t)=a0+a1t+a2t2+a3t3+a4t4+a5t5 (1)
Its single order and second dervative are also all smooth multinomials:
In above formula, θ (t),Respectively joint of robot angle, angular speed, angular acceleration；a0、a1、a2、a3、a4、
a5For multinomial coefficient；T is time variable.
The position of robot running track, speed, acceleration constitute 6 constraint conditions to polynomial function (1).By machine
When device person joint Track Initiation quantity of state t=0 and terminal quantity of state t=tfWhen bring into 6 constraint conditions such as following formula:
According to formula (4), 6 coefficient a of quintic algebra curve interpolating function counter can be solved0、a1、a2、a3、a4、a5It is respectively as follows:
Therefore in this section of track, joint of robot ideal movements speed are as follows:
According to above-mentioned algorithm, either joint of robot can be obtained and exist by off-line calculation, or online calculating in real time
The ideal calculating speed ω at each moment under a certain motion profileideal。
When colliding in view of robot, actual path can shift compared to the track of planning, and its actual speed
Also it can deviate ideal calculating speed, the invention proposes judge whether robot occurs based on practical and ideal calculating speed deviation
The method of collision.
As shown in Fig. 2, being increased in host controller in real time on the basis of robot control system shown in Fig. 1
Read machine person joint's values for actual speed ωreal, and to actual value ωrealIt is solved with the algorithm by above-mentioned introduction
ωidealIt makes the difference and compares to obtain velocity deviation Δ ω are as follows:
Δ ω=ωideal-ωreal (7)
Fig. 3 shows the collision experiment under ideal velocity deviation as a result, in robot collision experiment, a certain pass of robot
Ideal calculating speed, actual speed and the ideal velocity of section and the deviation of actual speed.It can be seen that in robot uniform motion
When, ωrealWith ωidealDeviation very little, almost 0.But during acceleration and deceleration, ωrealWith ωidealDeviation will become
It obtains very big or even also bigger than velocity deviation caused by collision.Which results in control algolithm cannot distinguish between velocity deviation be by
Caused by robot acceleration and deceleration, or as caused by collision.The reason of causing this velocity deviation is that host controller exists
After providing position command, the computing relay by servo-system, control delay are needed, the ability such as execute delay, sampling delay
Robot is set to reach designated position.This makes values for actual speed lag behind ideal calculating speed value.
To solve the above-mentioned problems, the invention proposes a kind of, and the velocity deviation based on optimization carries out robot collision detection
To avoid interference of the velocity deviation caused by robot acceleration and deceleration to detection.Specific practice is as shown in figure 4, host controller calculates
It is not compared out with the actual speed at the moment directly after the robot ideal speed of service, but makes ωidealPass through filtering
Device obtains optimal speed ω 'idealAfterwards with actual speed ωrealCompare to obtain optimal speed deviation delta ω.
Δ ω=ω 'ideal-ωreal (8)
By the filter optimization that is mentioned of the present invention treated optimal speed ω 'idealWith actual speed ωrealAgree with
It is very good.Even if velocity deviation is also able to maintain small value during robot acceleration and deceleration.In this way, smaller touch can be set
Detection threshold value is hit, to keep the collision detection of robot more sensitive.As shown in figure 5, setting collision detection bound threshold value Δ
ωupper、Δωlower, when velocity deviation Δ ω is more than bound threshold value, i.e. Δ ω > Δ ωupperOr Δ ω < Δ ωlower
When, it controls robot stop motion or progress is evaded algorithm accordingly and caused seriously to avoid collision to people or robot body
Damage.
Therefore, the whole control block diagram of system on the basis of existing robot control system as shown in fig. 6, increase filter
Wave device, to ideal velocity ωidealBy obtaining optimal speed ω ' after filtering optimizationideal, with actual speed ωrealSubtract each other
To velocity deviation Δ ω, by being compared with collision detection bound threshold value to determine whether colliding.
Fig. 7 shows the comparison diagram of actual speed deviation and optimal speed deviation, it can be seen that excellent not through wave filter
The actual speed deviation fluctuation of change is larger, has fallen into oblivion the velocity deviation at the moment that really collides, therefore is difficult to judge machine
People is under normal acceleration deceleration state or to be collided.On the contrary, optimal speed and reality after filter optimization
Speed relatively after speed deviation, all in collision detection upper and lower limits, work as hair under robot normal operating condition
After raw collision, speed deviation is significantly increased, and has been more than the upper and lower limits of collision detection, therefore can accurately detect whether
There is collision, evades movement for further control robot stop motion or progress accordingly and provide the foundation, to avoid
It collides and causes serious damage to people or robot body.
Based on above content, the present invention provides a kind of robot collision checking method based on velocity deviation, the side
Method includes the following steps:
1) setting joint of robot collides upper limit threshold Δ ωupper, collide lower threshold Δ ωlower；
2) according to need of work, by staff or robot autonomous design robot motion profile, speed, acceleration；
3) each joint ideal movements speed of robot is solved according to trajectory planning algorithm used.It is inserted with quintic algebra curve
For value function, for wherein one section of track, according to each joint trajectories initial shape of robot and terminal position, speed, acceleration
Spend quantity of state, counter 6 coefficient a for solving quintic algebra curve interpolating function0、a1、a2、a3、a4、a5:
4) according to coefficient obtained by above formula, joint of robot ideal movements speed in this section of track is solved:
5) by ideal movements speed omegaidealOptimal speed ω ' is obtained by the processing of filterideal；
6) it is measured by motor encoder, obtains the real-time joint angles θ of robot after algorithm process；
7) robot joint angles are differentiated to obtain joint of robot values for actual speed ωreal；
8) to optimal speed ω 'idealWith actual speed ωrealIt makes the difference to obtain velocity deviation Δ ω
Δ ω=ω 'ideal-ωreal；
9) judgement collision, if Δ ω > Δ ωupperOr Δ ω < Δ ωlower, then it is assumed that collision occurs；
10) after detecting collision, robot takes corresponding measure to avoid collision and makes to robot body or its ambient enviroment
At serious damage.
Another aspect of the present invention provides a kind of robot controller for realizing the above method, the control device
Including host controller and servo-system；The host controller for robot trajectory planning, systemic hierarchial signal processing,
Layer algorithm is realized and human-computer interaction；Servo-system includes position ring controller, speed ring controller and current loop controller, described
Position ring controller, speed ring controller and current loop controller are sequentially connected, for adjusting servo motor；The control device
Further include filter, is connected to host controller, ideal movements speed omegaidealOptimal speed is obtained by the processing of filter
ω′ideal。
The present invention be suitable for various trajectory planning algorithms, it is above-mentioned be using quintic algebra curve as interpolating function for, may be used also
With select cubic algebraic curves algorithm, highe order polynomial interpolation algorithm, cubic spline interpolation algorithm, with the line of parabolic transition
Property interpolation algorithm etc..The mentioned algorithm of the present invention is not only suitable for offline trajectory planning case, can also be applied to online trajectory planning
Case.In offline trajectory planning, proposed optimization speed can be calculated according to the above method by the third party software of such as MATLAB
Degree.Then optimal speed data are stored in host controller, are compared in real time with actual speed, for judging collision.
Offline trajectory planning case can save upper control occupancy.In online trajectory planning case, need host controller according to rail
Mark planning instruction, proposed optimal speed is calculated using the above method, and be compared with actual speed in real time, is touched for judging
It hits.Different cases is that the platform of calculating is different, and specific steps needed for method are consistent.
The number and type of filter can be selected according to actual condition and demand in Fig. 6.Generally require filter
With hysteresis quality, low pass or with general character etc..Low-pass first order filter, Butterworth filter, Chebyshev filtering such as can be used
Device.There is different filters different performance can design each filter different weights for different operating condition and demand
Coefficient contributes the processing of final optimization pass speed as shown in figure 8, designing each filter.
By taking confirmatory experiment of the invention as an example, which has used single order wave digital lowpass filter and Butterworth filter
To ideal velocity ωidealIt is handled.
The Processing Algorithm of single order wave digital lowpass filter is as follows:
ω′ideal1(i)=(1- α) ω 'ideal1(i-1)+αωideal(i) (9)
Wherein, filtering coefficientTsMinimum period, T are run for robot systemωFor the period
Design parameter can be adjusted according to the actual situation；ωidealIt (i) is i-th of ideal calculating speed, i=1 ... m, robot is complete
At shared m ideal calculating speed target set point of a certain movement；ω′ideal1It (i-1) is (i-1) a optimal speed,
ω′ideal1It (i) is i-th of optimal speed.
Square of Butterworth filter amplitude-frequency characteristic mould are as follows:
Wherein, Ω is frequency symbol, and unit rad/s, j are imaginary unit, and n is the order of filter, ΩcFor filter
By frequency.
This experiment uses second order Butterworth filter.Its digital differential equation is as follows:
ω′ideal2(i)=ε1ωideal(i)+ε2ωideal(i-1)+ε3ωideal(i-2)-β1ω′ideal2(i-1)-β2
ω′ideal2(i-2)
Wherein, ωideal(i)、ωideal(i-1)、ωidealIt (i-2) is i-th, (i-1), (i-2) a ideal calculating speed,
ω′real2(i-1)、ω′real2It (i-2) is respectively (i-1) (i-2) a optimal speed, ε1、ε2、ε3、β1、β2Respectively design
Parameter.
According to robot collision algorithm entire block diagram is judged, designs weight shared by each filter of needs and obtain
Final optimal speed are as follows:
ω′ideal(i)=η1ω′ideal1(i)+η2ω′ideal2(i) (11)
Wherein, ω 'ideal1(i)、ω′ideal2It (i) is respectively the optimal speed of the 1st, the 2nd filter, η1, η2Respectively
1st, weight shared by the 2nd filter, there is η1+η2=1.
Certainly, the number of the filter can have k, and k is the integer more than or equal to 1, then optimal speed are as follows:
ω′ideal=η1ω′ideal1+η2ω′ideal2+…+ηkω′ideal(k) (12)
Wherein, ω 'ideal1、ω′ideal2、……ω′ideal(k) be respectively the 1st, the 2nd ... after k-th of filter process
Optimal speed, η1, η2... ηkRespectively the 1st, there are η in the 2nd, weight shared by k-th of filter1+η2+…ηk=1；Each filter
The weight of wave device is configured according to actual needs.
In conclusion the present invention provides a kind of robot collision checking method and device based on velocity deviation, described
Method handles ideal calculating speed using optimization is filtered to ideal calculating speed, using the hysteresis quality of filter；Make its with
Actual speed subtracts each other to obtain optimal speed deviation；Robot collision detection bound threshold value is set；When detecting that optimal speed is inclined
Robot stop motion is controlled when difference is more than threshold value or other is taken to evade algorithm avoids collision bring personal injury and machine
People's mechanical damage.The present invention is suitable for various robots, is suitable for offline trajectory planning and online trajectory planning case, is suitable for
Various trajectory planning algorithms, robot control strategy.The present invention does not need to increase additional sensor, does not need detection and accelerates
Degree, electric current, torque etc. do not need to carry out complicated robot dynamics' operation yet；Will not have control strategy to robot to make
At influence, robot control delay etc. not will increase.The experimental results showed that this method can it is effective, sensitive, be accurately detected
Collision can control robot stop motion immediately based on this and avoid damaging people and robot itself.
It should be understood that above-mentioned specific embodiment of the invention is used only for exemplary illustration or explains of the invention
Principle, but not to limit the present invention.Therefore, that is done without departing from the spirit and scope of the present invention is any
Modification, equivalent replacement, improvement etc., should all be included in the protection scope of the present invention.In addition, appended claims purport of the present invention
Covering the whole variations fallen into attached claim scope and boundary or this range and the equivalent form on boundary and is repairing
Change example.
Claims (8)
1. a kind of robot collision checking method based on velocity deviation, which comprises the steps of:
1) setting robot collides upper limit threshold Δ ωupperWith collision lower threshold Δ ωlower, Δ ω is velocity deviation；
2) motion profile, the speed, acceleration of robot are set；
3) the ideal movements speed omega of robot is solved according to selected Motion trajectory algorithmideal；
4) by ideal movements speed omegaidealOptimal speed ω ' is obtained by the processing of filterideal；
5) real-time measurement values of robot motion track are obtained；
6) it differentiates to the motion profile of robot and brings real-time measurement values into, obtain robot actual speed ωreal；
7) by optimal speed ω 'idealWith actual speed ωrealSubtract each other to obtain the velocity deviation Δ ω:
Δ ω=ω 'ideal-ωreal； (1)
8) judge whether to collide, if Δ ω > Δ ωupperOr Δ ω < Δ ωlower, then think collision；
9) after detecting that collision occurs, robot takes corresponding measure to avoid collision and causes to robot body or its ambient enviroment
Serious damage.
2. according to the method described in claim 1, it is characterized by: the motion profile of robot, speed, acceleration in step 2
It is respectively set to the joint angle, angular speed, angular acceleration in each joint of robot.
3. according to the method described in claim 2, it is characterized in that, the Motion trajectory algorithms selection in step 3 is more than five times
Item formula interpolating function:
θ (t)=a0+a1t+a2t2+a3t3+a4t4+a5t5 (2)
Its single order and second dervative are as follows:
In above formula, θ (t),The respectively joint angle of robot, angular speed, angular acceleration；a0、a1、a2、a3、a4、a5
For polynomial coefficient；T is time variable；
For the motion profile of wherein one section of joint angle, according to each joint angle Track Initiation shape of robot and terminal position, angle
Speed, angular acceleration quantity of state, counter 6 coefficient a for solving quintic algebra curve interpolating function0、a1、a2、a3、a4、a5It is as follows:
Wherein tfJoint of robot ideal movements speed in this section of motion profile is solved according to coefficient obtained by (5) formula for the terminal time
Degree are as follows:
A in above formula (6)1、a2、a3、a4、a55 coefficients are replaced by formula (5) respectively.
4. according to the method described in claim 2, it is characterized in that, the Motion trajectory algorithm in step 3 may be selected to be three
Preserving Interpolation Using algorithm, highe order polynomial interpolation algorithm, cubic spline interpolation algorithm, the linear interpolation calculation with parabolic transition
Method.
5. method according to claim 1-4, which is characterized in that this method is advised suitable for robot off-line track
Draw or the online trajectory planning of robot；In offline trajectory planning, then the optimal speed is obtained by filter process
Optimal speed data are stored in host controller, are compared in real time with actual speed, for judging whether collision occurs；
In online trajectory planning, host controller is instructed according to trajectory planning, obtains the optimal speed simultaneously by filter process
It is compared with actual speed, for judging whether collision occurs.
6. method according to claim 1-5, which is characterized in that the filter in step 4 has k, and k is
Integer more than or equal to 1,
ω'ideal=η1ω'ideal1+η2ω'ideal2++ηkω'ideal(k) (7)
Wherein, ω 'ideal1、ω'ideal2、……ω'ideal(k)Respectively the 1st, the 2nd ... it is excellent after k-th of filter process
Change speed, η1,η2,…ηkRespectively the 1st, there are η in the 2nd, weight shared by k-th of filter1+η2+…ηk=1.
7. a kind of robot controller for realizing method described in any one of claims 1-6, which is characterized in that the control
Device includes host controller and servo-system；
The host controller is for robot trajectory planning, systemic hierarchial signal processing, the realization of upper layer algorithm and human-computer interaction；
Servo-system includes position ring controller, speed ring controller and current loop controller, the position ring controller, speed
Ring controller and current loop controller are sequentially connected, for adjusting servo motor；
The control device further includes filter, is connected to host controller, ideal movements speed omegaidealPass through the place of filter
Reason obtains optimal speed ω 'ideal。
8. device according to claim 7, which is characterized in that the filter has k, and k is the integer more than or equal to 1,
ω'ideal=η1ω'ideal1+η2ω'ideal2++ηkω'ideal(k) (8)
Wherein, ω 'ideal1、ω'ideal2、……ω'ideal(k)Respectively the 1st, the 2nd ... it is excellent after k-th of filter process
Change speed, η1,η2,…ηkRespectively the 1st, the 2nd ... weight shared by k-th of filter has η1+η2+…ηk=1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810046422.0A CN109397283B (en) | 2018-01-17 | 2018-01-17 | Robot collision detection method and device based on speed deviation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810046422.0A CN109397283B (en) | 2018-01-17 | 2018-01-17 | Robot collision detection method and device based on speed deviation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109397283A true CN109397283A (en) | 2019-03-01 |
| CN109397283B CN109397283B (en) | 2019-12-24 |
Family
ID=65463415
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810046422.0A Active CN109397283B (en) | 2018-01-17 | 2018-01-17 | Robot collision detection method and device based on speed deviation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109397283B (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110328664A (en) * | 2019-06-28 | 2019-10-15 | 希格斯动力科技（珠海）有限公司 | Collision checking method and device |
| CN110365271A (en) * | 2019-07-20 | 2019-10-22 | 中国船舶重工集团公司第七二四研究所 | A kind of motor speed accurate detection fusion filtering method in real time |
| CN111580512A (en) * | 2020-04-28 | 2020-08-25 | 平安科技（深圳）有限公司 | Movement control method and device, storage medium and computer equipment |
| CN111890343A (en) * | 2020-07-29 | 2020-11-06 | 浙江广合智能科技有限公司 | Robot object collision detection method and device |
| CN113997282A (en) * | 2021-10-13 | 2022-02-01 | 杭州景业智能科技股份有限公司 | Mechanical arm control method, mechanical arm control device, electronic device and storage medium |
| CN114055521A (en) * | 2020-08-04 | 2022-02-18 | 北京福田康明斯发动机有限公司 | Mobile robot collision buffering method and system |
| CN114619439A (en) * | 2020-12-11 | 2022-06-14 | 郑州思昆生物工程有限公司 | Multi-shaft mechanical arm anti-collision protection control system |
| WO2022199250A1 (en) * | 2021-03-25 | 2022-09-29 | 南京泉峰科技有限公司 | Self-propelled device and collision detection method thereof |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08229864A (en) * | 1995-03-01 | 1996-09-10 | Daikin Ind Ltd | Industrial robot collision detecting method and device thereof |
| KR20130121592A (en) * | 2012-04-27 | 2013-11-06 | 성균관대학교산학협력단 | Device for collision detection using band designed disterbanc observer, and the method |
| CN105846793A (en) * | 2016-04-14 | 2016-08-10 | 北京智联安科技有限公司 | Dynamic and variable time constant filter and filtering method |
| JP2017019057A (en) * | 2015-07-13 | 2017-01-26 | セイコーエプソン株式会社 | Robot control device, robot, and robot system |
| DE102015009892A1 (en) * | 2015-07-30 | 2017-02-02 | Kuka Roboter Gmbh | Method and system for controlling a robot |
| CN107351085A (en) * | 2017-08-21 | 2017-11-17 | 西北工业大学 | A kind of robot for space collision avoidance method based on multiple control points |
-
2018
- 2018-01-17 CN CN201810046422.0A patent/CN109397283B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08229864A (en) * | 1995-03-01 | 1996-09-10 | Daikin Ind Ltd | Industrial robot collision detecting method and device thereof |
| KR20130121592A (en) * | 2012-04-27 | 2013-11-06 | 성균관대학교산학협력단 | Device for collision detection using band designed disterbanc observer, and the method |
| JP2017019057A (en) * | 2015-07-13 | 2017-01-26 | セイコーエプソン株式会社 | Robot control device, robot, and robot system |
| DE102015009892A1 (en) * | 2015-07-30 | 2017-02-02 | Kuka Roboter Gmbh | Method and system for controlling a robot |
| CN105846793A (en) * | 2016-04-14 | 2016-08-10 | 北京智联安科技有限公司 | Dynamic and variable time constant filter and filtering method |
| CN107351085A (en) * | 2017-08-21 | 2017-11-17 | 西北工业大学 | A kind of robot for space collision avoidance method based on multiple control points |
Non-Patent Citations (1)
| Title |
|---|
| XU WENZHONG等: "Sensorless Robot Collision Detection Based on Optimized Velocity Deviation", 《2017 CHINESE AUTOMATION CONGRESS (CAC)》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110328664A (en) * | 2019-06-28 | 2019-10-15 | 希格斯动力科技（珠海）有限公司 | Collision checking method and device |
| CN110365271A (en) * | 2019-07-20 | 2019-10-22 | 中国船舶重工集团公司第七二四研究所 | A kind of motor speed accurate detection fusion filtering method in real time |
| CN110365271B (en) * | 2019-07-20 | 2021-03-26 | 中国船舶重工集团公司第七二四研究所 | Fusion filtering method for real-time accurate detection of motor rotating speed |
| CN111580512A (en) * | 2020-04-28 | 2020-08-25 | 平安科技（深圳）有限公司 | Movement control method and device, storage medium and computer equipment |
| CN111890343A (en) * | 2020-07-29 | 2020-11-06 | 浙江广合智能科技有限公司 | Robot object collision detection method and device |
| CN111890343B (en) * | 2020-07-29 | 2021-10-15 | 浙江广合智能科技有限公司 | Robot object collision detection method and device |
| CN114055521A (en) * | 2020-08-04 | 2022-02-18 | 北京福田康明斯发动机有限公司 | Mobile robot collision buffering method and system |
| CN114619439A (en) * | 2020-12-11 | 2022-06-14 | 郑州思昆生物工程有限公司 | Multi-shaft mechanical arm anti-collision protection control system |
| CN114619439B (en) * | 2020-12-11 | 2023-06-13 | 郑州思昆生物工程有限公司 | Multi-axis mechanical arm anti-collision protection control system |
| WO2022199250A1 (en) * | 2021-03-25 | 2022-09-29 | 南京泉峰科技有限公司 | Self-propelled device and collision detection method thereof |
| CN113997282A (en) * | 2021-10-13 | 2022-02-01 | 杭州景业智能科技股份有限公司 | Mechanical arm control method, mechanical arm control device, electronic device and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109397283B (en) | 2019-12-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109397283A (en) | A kind of robot collision checking method and device based on velocity deviation | |
| CN108772838B (en) | Mechanical arm safe collision strategy based on external force observer | |
| US10766140B2 (en) | Teach mode collision avoidance system and method for industrial robotic manipulators | |
| KR101495949B1 (en) | Reducer abnormality determination method, abnormality determination device, and robot system | |
| CN109940622B (en) | Non-sensing collision detection method for robot mechanical arm based on motor current | |
| Garrido et al. | Anti-swinging input shaping control of an automatic construction crane | |
| Duchaine et al. | General model of human-robot cooperation using a novel velocity based variable impedance control | |
| US9346162B2 (en) | Robot system, control device of robot, and robot control device | |
| CN108015774B (en) | Sensor-free mechanical arm collision detection method | |
| CN103909522A (en) | Method of six-DOF industrial robot passing singular region | |
| US20170357239A1 (en) | Method for compensating for a deviation in an operating point | |
| WO2000045229A1 (en) | Uncalibrated dynamic mechanical system controller | |
| CN112947293B (en) | Sliding mode-based mechanical arm safety track tracking control method | |
| CN111267105A (en) | Kinetic parameter identification and collision detection method for six-joint robot | |
| CN108367437A (en) | Identify the method that robots arm is bumped against with object and the robot with robots arm | |
| JP6705851B2 (en) | Vibration analysis device and vibration analysis method | |
| CN112936260A (en) | Sensor-free collision detection method and system for six-axis industrial robot | |
| CN114012734A (en) | Parameter-adaptive robot collision detection method | |
| Zhang et al. | Robot collision detection without external sensors based on time-series analysis | |
| JP2004364396A (en) | Controller and control method for motor | |
| CN114589694A (en) | Robot collision detection threshold updating method and device and storage medium | |
| CN113021353B (en) | Robot collision detection method | |
| CN112123333A (en) | Multi-joint robot collision detection method and device | |
| CN111356558A (en) | Method and system for simulating a braking process of a robot | |
| CN110914019A (en) | Robot stopping distance simulation method |
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 |