CN100584547C - Method for controlling mechanical hand - Google Patents

Abstract

When either a command value or an actually measured value is appropriately selected as an angular velocity used for the frictional torque calculation, the frictional compensation can be made valid at all times in both the case in which a robot is actively operated according to an angular velocity command and the case in which the robot is passively operated being pushed by an external force. In the case where a motor rotating direction and a collision direction are reverse to each other after a collision has been detected, the control mode is switched from the positional control to the electric current control and a torque, the direction of which is reverse to the direction of the motor rotation is generated by the motor, so that the motor rotating speed can be reduced and the collision energy can be alleviated. After that, when the motor rotating speed is reduced to a value not more than the setting value, the control mode is switched to the compliance control and the distortion caused in a reduction gear is dissolved. On the other hand, in the case where the motor rotating direction and the collision direction are the same, the control mode is directly switched from the positional control to the compliance control without passing through the electric current control. When the robot is operated whole following a collision force, the collision force can be alleviated.

Description

The method of control manipulator

The application is to be July 2, application number in 2004 the dividing an application for the patent application of " method and apparatus of control manipulator " that be 200480009285.X, denomination of invention the applying date.

Technical field

The present invention relates to the method and apparatus of a kind of control by motor-driven manipulator.More specifically, the present invention relates to a kind of compliance (compliance) servo control technique that is used to control manipulator, promptly the present invention relates to a kind of method and apparatus that the manipulator that carries out after the collision that has detected manipulator and object stops that being used to be controlled at.

Background technology

Recently, robot not only has been used for industrial circle, and the person field that is used for the public consumption.Therefore, guarantee that safety has become important.But according to sensor detects the stop component that the external force that is applied to robot when colliding stops robot by making firmly, manufacturing cost and weight undesirably increase.So expectation is not used sensor and strengthened the servo-controlled performance of compliance and strengthen the performance of the control of stop motion, described compliance SERVO CONTROL comprises collision detection.

Do not use sensor to realize the servo-controlled method of compliance about being used for, often adopt a kind of method, even wherein when in position feedback control, increasing position deviation, also by suppressing in motor, not produce too high torque with respect to the increase of the current order (electric current command) of the increase of position deviation.

According to the amount of suppression of the current order in FEEDBACK CONTROL, also suppress intensity, so that can strengthen compliance by the torque of motor generation.

In JP-A-09-179632 (No. the 5994864th, United States Patent (USP)), disclose a kind of mode that is used for being suppressed at the current order of FEEDBACK CONTROL, wherein limited current order.In JP-A-08-155868, also disclose a kind of mode that is used for being suppressed at the current order of FEEDBACK CONTROL, wherein reduced feedback oscillator.

As mentioned above, in order to strengthen the compliance of manipulator, the current order that is suppressed in the FEEDBACK CONTROL is important.If do not suppress electric current in FEEDBACK CONTROL, then the compliance of manipulator becomes near common servo rigidity.Therefore, reduced servo-controlled compliance.

But, for the manipulator, must produce driving torque by motor, wherein to consider inertia torque, friction torque and gravitational torque.Therefore, when only coming the manipulator, be difficult to suppress the current order of motor by FEEDBACK CONTROL.

Fig. 3 is illustrated in the block diagram that wherein uses actual speed to control the conventional method of friciton compensation.In described accompanying drawing, drawing reference numeral 1 is motor anglec of rotation order θ _Com, drawing reference numeral 2 is feedback controllers, and drawing reference numeral 3 is electric current limiting parts, and drawing reference numeral 4 is FEEDBACK CONTROL current order I _Com, drawing reference numeral 5 is current of electric I _m, drawing reference numeral 6 is scopes of expression (motor+actual load), drawing reference numeral 7 is motor torque constant K _t, drawing reference numeral 8 is that motor produces torque tau mm, and drawing reference numeral 9 is the external force τ μ+τ dyn+ τ dis that provide to motor, and drawing reference numeral 10 is transfer functions of motor inertia, and drawing reference numeral 11 is motor anglec of rotation θ _Fb, drawing reference numeral 12 is differential operators, drawing reference numeral 13 is motor angular velocity order ω _Com, drawing reference numeral 14 is differential operators, drawing reference numeral 15 is motor angular acceleration order α _Com, drawing reference numeral 16 is motor inertia (rotor+reduction gearing primary side) J, drawing reference numeral 17 is the needed current of electric I of manipulation robot _Ml, drawing reference numeral 18 is 1/K reciprocal of motor torque constant _t, drawing reference numeral 19 is calculated values of dynamic torque τ dyn, and drawing reference numeral 20 is calculated values of frictional force τ μ, and drawing reference numeral 21 is friction computing blocks, and drawing reference numeral 22 is dynamic calculation pieces, drawing reference numeral 23 is motor angular velocity ω _Fb, drawing reference numeral 24 is differential operators, drawing reference numeral 25 is other anglecs of rotation.

When the motor-driven side is seen, be expressed in the motor that the man-hour of operating machines produces by expression formula (1) and produce torque tau m.When load side is seen, express described motor by expression formula (2) and produce torque tau m.

τmm＝K _t*I _m (1)

τml＝J*α+τμ+τdyn+τdis (2)

In this case, the drawing reference numeral of using in expression formula (1) and (2) is defined as follows.

K _t: the motor torque constant

I _m: current of electric

α: motor angular acceleration

ω: motor angular velocity

J: motor inertia (primary side of rotor+reduction gearing)

τ μ: friction torque (being converted into motor shaft end)

τ dyn: (dynamic torque is the summation of gravitational torque, inertia force, earth deflecting force (Coriolis) and elastic force to dynamic torque, and it is converted into motor shaft end.)

τ dis: (disturbing torque is contact torque or the parameter error that provides from the outside to disturb torque.Disturb torque to be converted into motor shaft end.)

When in expression formula (2), disturbing torque tau dis=0, also may calculate current of electric I by expression formula (1) and (2) _Ml, it is that the manipulator is needed.

I _ml＝(J*α+τμ+τdyn)/K _t (3)

As shown in Figure 3, the I that calculates when expression formula (3) _MlBe added to feedback current order I _ComThe time, if disturb torque tau dis=0, even then when feedback current is 0, manipulator also becomes and also may arrive destination locations.

In Fig. 3, when feedback controller 2 from anglec of rotation order θ _Com1 and real electrical machinery anglec of rotation θ _FbCarrying out PID calculates and carries out electric current and limit and can obtain feedback current order I at 3 o'clock _Com4.About being used to limit the mode of electric current 3, provide a kind of system and a kind of system that wherein reduces feedback oscillator that wherein is provided with described restriction.

On the other hand, the I that calculates by expression formula (3) _Ml17 can followingly obtain.Will be to motor rotate command θ _ComThe 1 angular acceleration θ that differentiates 12 and 14 twice acquisition _Com15 multiply by motor inertia J16.Friction torque τ μ 20 and dynamic torque τ dyn 19 are added to the value of acquisition like this.With the 1/K reciprocal with the motor torque constant on duty that is obtained _t18, can obtain the I that calculates by expression formula (3) _Ml17.

That is, when can be by the needed current of electric I of operation of expression formula (3) accurate Calculation robot _MlThe time, become and may in FEEDBACK CONTROL, suppress current order.Therefore, can strengthen the compliance of robot.

But, in fact, owing to the parameter error of expression formula (3) causes the error of calculation.Therefore, when suppressing electric current consumingly in FEEDBACK CONTROL, becoming can not compensating error, and manipulator becomes out of hand, and can not reduce position deviation, promptly has manipulator possibility out of control.

When making that in FEEDBACK CONTROL current order is 0, if, then do not produce the power that makes manipulator return initial position because the contact torque that provides from the outside has enlarged position deviation.

As mentioned above, can suppress what depend on expression formula (3) by the current order of FEEDBACK CONTROL computational accuracy.

Friction torque τ μ is the current of electric I that is calculated by expression formula (3) _MlOne of dominant term, and be that the manipulation robot is needed, it comprises: static friction torque tau μ s and dynamic friction torque tau μ m, determine them by the action direction of power; And viscous friction τ μ d (the coefficient D of viscosity), it and speed are proportional.

τμ＝τμs+τμm+τμd (4)

But in expression formula (4), each is calculated as follows.

τμs＝τμs0*sgn1(ω) (5)

τμm＝τμm0*sgn1(ω) (6)

$\mathrm{sgn}2\left(\mathrm{\&omega;}\right)\left\{\begin{array}{c}-1(\mathrm{\&omega;}<0)\\ 0(\mathrm{\&omega;}=0)\\ 1(\mathrm{\&omega;}>0)\end{array}\right.$

τμd＝D*ω (7)

Wherein, τ μ s0 is the absolute value of τ μ s, with the motor irrelevant to rotation direction; τ μ m0 is the absolute value of τ μ m, with the motor irrelevant to rotation direction.

Can find out from expression formula (5)-(7), calculate all friction torques according to angular velocity omega.

About being used to calculate the angular velocity omega of friction torque, be subjected to the angular velocity omega of feedback impedance _FBBe used for above-mentioned conventional example (JP-A-9-179632).In another conventional example (JP-A-10-180663), use as position command θ _ComThe angular speed order ω that obtains during by differential _Com

But, in the compliance control of robot, provide two kinds of situations.One is that robot is according to position command θ _ComInitiatively do the situation of action.Another is robot is promoted to do passively action by external force a situation.

As shown in Figure 3, when using to real electrical machinery anglec of rotation θ _FbThe 11 actual angular speed ω that obtain when differentiating _Fb23 use when being used as using computing block 21 to calculate friction torque τ μ 20 by expression formula (5) angular speed the time, reflect the velocity perturbation that causes by the external force τ dis of a part, so that can strengthen the computational accuracy of friction torque τ μ 20 as the interference torque 9 that is provided to motor.

But when the state that stops fully from the robot man-hour of operating machines on one's own initiative, direction of operating is unknown, begins operation up to robot.Therefore, can not calculate static friction torque τ μ s.

Begin operation, actual angular speed ω up to robot _FBBe 0, and the dynamic friction torque tau μ m that calculates by expression formula (6) and (7) and viscous friction τ μ d yes 0.Therefore, the friction torque of being calculated by expression formula (4) is τ μ 0, and does not produce the motor torque that is used for the manipulation robot.

In this state, as current order I by FEEDBACK CONTROL _Com4 torques that cause are suppressed to strengthen compliance and to become when being lower than actual static friction torque tau μ s, even produced anglec of rotation order θ _Com1, robot does not move yet.

On the other hand, as shown in Figure 4, when to motor anglec of rotation θ _Com1 angular velocity omega that obtains when differentiating _Com13 when being used to calculate the employed angular velocity omega of 21 friction torque τ μ 20, and these problems are solved.That is, even when the inoperation robot, when using angular speed order ω _Com13 come to calculate 21 friction torque τ μ 20 and it is added to feedback current order I by expression formula (4) _Com, can compensate actual friction torque at 4 o'clock.Even when the electric current I that has suppressed by FEEDBACK CONTROL _Com4 o'clock, also can the manipulation robot.

But, when angular speed order ω _Com13 when being used to calculate the employed angular velocity omega of 21 friction torque τ μ 20, can be according to angular speed order ω _Com13 come manipulation robot on one's own initiative.But the way is owing to disturbing torque tau dis to cause under the situation of angular velocity fluctuation, at angular speed order ω in operation _Com13 and actual angular speed ω _FbCause big error between (drawing reference numeral 23 in Fig. 3).So, increased the error of calculation by the viscous friction torque tau μ d of expression formula (7) calculating.

When at angular speed order ω _Com13 is 0 when being promoted robot and robot and be stopped by external force when stopping robot, and the friction torque τ μ that is calculated by expression formula (4) always 0.Therefore, can not compensate actual friction torque at all.

And, even when robot is not promoted by external force, in practical operation, in the FEEDBACK CONTROL of being undertaken, cause the delay of following by feedback controller.Therefore, when stopping robot, at actual angular speed ω _Fb(drawing reference numeral 23 in Fig. 3) became before 0, angular speed order ω _Com13 become 0.Therefore, the friction torque τ that calculated by expression formula (4) this moment also becomes 0, and does not carry out friciton compensation.That is, at angular speed order ω _Com13 reach at 0 o'clock, and robot is stopped suddenly, and can not reach the target location, also may cause vibration.

In this case, at current order I according to FEEDBACK CONTROL _Com4 torques that produced by motor are suppressed under the situation that is lower than actual friction torque, even when external force increase position deviation, and also inoperation robot, and can not reduce position deviation.

In other words, though carry out friciton compensation, can not the feedback current order be set to be lower than actual friction torque.Therefore, can not strengthen the compliance of robot.

As mentioned above, calculating under the situation of friction torque τ μ, by expression formula (4) according to wherein using actual angular speed ω _FbWith angular speed order ω _ComOne of be used as the method for angular speed, even when the electric current I of using the friction torque τ μ that calculated to calculate by expression formula (3) _MlWhen being added to the FEEDBACK CONTROL electric current, can not compensate actual friction torque.

As shown in Figure 3, when using actual angular speed ω by angular speed for use when utilizing (5) to calculate 21 friction torque τ μ 20 _Fb23 when coming 100% ground friction compensation torque tau μ, and the feedback characteristics of control system can experience friciton compensation.Therefore, it is the same that operation control system just looks like not friction.Thereby, though can strengthen compliance, the reponse system vibration that becomes.

On the other hand, as shown in Figure 4, using angular speed order ω for calculating 21 friction torque τ μ, 20 employed angular velocity omegas _ComUnder 13 the situation, the feedback characteristics of control system is unaffected.Therefore, in order to improve the target following feature, 100% compensation is carried out in expectation.

Then, below second conventional example will be described.

About not using sensor to obtain to collide the method for torque, make in the following method usually.When the torque that produces from the drive current by motor deducts the torque loss that produces motor and reduction gearing, obtain motor and produce torque.When produce from the motor that finds previously torque deduct by dynamic calculation obtain and be called as dynamic torque, during for the necessary torque of output of reduction gearing, obtain colliding torque.

For example, corresponding to the friction torque of the loss of the torque that produces by motor be defined as with speed proportional (viscous friction torque) and static (coulomb friction torque) with and calculated.This for example is disclosed among the JP-A-2002-283276.

According to JP-A-6-083403 (No. the 6298283rd, United States Patent (USP)), the technology below having proposed.When being added to torque (electric current) order, eliminated fluctuating factor when the fluctuation of the parameter of coming the calculating robot by the algorithm of estimating and with it.In this traditional example, corresponding to the friction torque of the loss that causes in the torque that produces by motor be defined as with proportional of speed and static and, and estimate by algorithm for estimating.

So, when producing the dynamic torque that torque deducts robot from motor, do not use sensor and obtain colliding under the situation of torque or the servo following feature that improving the FEEDFORWARD CONTROL by dynamic torque so that demonstrate under the situation of maximum motor-driven power, need accurately calculate the reduction gearing that produces torque and robot by motor and export needed torque.

When the motor-driven side is seen, can produce torque tau m by the motor that expression formula (8) be expressed in the man-hour of operating machines.And, when when load side is seen, can produce torque tau m by the motor that expression formula (9) be expressed in the man-hour of operating machines.

τm＝K _t*I _m-(J*α+D*ω+τμsgn(ω)) (8)

τml＝τdyn++τdis (9)

In this connection, be defined as follows in the reference numeral shown in expression formula (1) and (2).

K _t: the motor torque constant

I _m: current of electric

α: motor angular acceleration

ω: motor angular velocity

J: motor inertia (primary side of rotor+reduction gearing)

D: viscous friction coefficient (being converted into motor shaft end)

τ μ: friction torque (being converted into motor shaft end)

τ g: gravitational torque (being converted into motor shaft end)

τ dyn: dynamic torque (dynamic torque be gravitational torque, inertia force, earth deflecting force and elastic force and, it is converted into motor shaft end.)

τ dis: (disturbing torque is collision torque or parameter error to disturb torque.Disturb torque to be converted into motor shaft end.)

$\mathrm{sgn}\left(\mathrm{\&omega;}\right)\left\{\begin{array}{c}1(\mathrm{\&omega;}>0)\\ 0(\mathrm{\&omega;}\&NotEqual;0)\\ -1(\mathrm{\&omega;}<0)\end{array}\right.$

Because motor and manipulator are connected to each other by reduction gearing, therefore must use the deceleration ratio that the item except motor inertia item J in the expression formula (9) is converted to motor shaft end.

When supposition during τ mm=ml, can obtain colliding torque tau dis by the expression formula (10) of time facial disfigurement in (8) and (9).

τdis＝K _t*I _m-(J*α+D*ω+τμ*sgn(ω)+τdyn)(10)

In conventional example, the dynamic friction item τ μ in expression formula (10) is calculated as fixed value.But when dynamic friction torque item was calculated as fixed value, motor produced under the torque condition with higher when quickening and slow down, cause motor produce torque about 10% than the computation error.

On the other hand, can following realization purpose be to improve the FEEDBACK CONTROL of SERVO CONTROL characteristic.Under the condition of disturbing torque tau dis=0, promptly do not contact and do not cause under the condition of parameter error, obtain current of electric I by expression formula (10) with the outside in robot _mThe electric current that so obtains is represented as I _FfWhen adding I to current order _FfThe time, can realize FEEDBACK CONTROL.

I _ff＝(J*α+D*ω+τμ*sgn(ω)+τdyn)/K _t (11)

In traditional example, do not use the calculating of expression formula (11), but estimate dynamic friction item τ μ by algorithm for estimating.Yet it does not change along with the time, but friction torque has changed in the short time of quickening and slowing down widely.So,, produce the delay of phase place, and can not intactly compensate by the estimation of algorithm for estimating.

When calculating in advance, do not cause the delay of phase place not by algorithm for estimating but by dynamic torque.But when using expression formula (11) when calculating when the dynamic friction torque is arranged on fixed value, motor produces under the torque condition with higher when quickening and slow down, cause motor produce torque about 10% than the computation error.

This error will be illustrated as follows.

Figure 11 shows the view that the motor that calculates by expression formula (8) and (9) produces torque tau mm 1, τ ml 2 and speed 3 when making that robot carries out reciprocating motion shown in Figure 12.

In this case, the manipulator that use is many abutments robot of six vertical type, and its portable quality is 6kg, and its total brachium is about 1.3 meters.In Figure 12, omitted three wrist axles, and shown three basic axles.When measuring, operate FA axle as the 3rd.

In this case, under the condition of disturbing torque tau dis=0, promptly do not contact and do not cause under the condition of parameter error in robot and measure with the outside.

As shown in figure 11, in τ mm 1 and τ ml 2, produce 4% error 4 of about peak torque, can understand that promptly expression formula (8) has error factor.

When comparing when quickening and slow down, the result is as follows.

τmm＞τml

In the acceleration and deceleration of operation part, though angular acceleration and angular deceleration direction are opposite each other, equal and opposite in direction.With respect to gravity, manipulator is operated with symmetric pattern.

Therefore, in order to reduce error, must increase dynamic friction torque tau μ by reducing τ dyn.But,, when dynamic friction torque tau μ is increased,, increased the error 5 when constant speed when being retained as constant value though reduced error 4 when peak torque when as shown in figure 13.

That is, when dynamic friction torque tau μ is taken as constant, can not eliminate the error factor that causes by τ μ.Therefore, in the expression formula that comprises τ μ (10) and (11), produce identical error.

Therefore, in the collision torque of no sensor detects, when dynamic friction torque tau μ is taken as constant, though manipulator when quickening or slow down not with object collision, expression formula (11) output is used as colliding torque corresponding to the electric current of described error.For above-mentioned reasons, in order to prevent wrong detection, must reduce collision detection sensitivity.

On the other hand, export in reduction gearing under the situation of FEEDFORWARD CONTROL of needed torque, when dynamic friction torque tau μ is used as constant, increased the error of calculation, and might the feedforward compensation torque become not enough.When using algorithm for estimating so that preventing to produce the error of calculation, be difficult to estimate the friction torque of sudden change when quickening or slow down and the delay that do not cause phase place.So, can not prevent the variation of control performance fully.

Then the method that stops manipulator after collision detection will be described.Proposed following method: manipulator returns the method (JP-A-2002-117618 (No. the 6429617th, United States Patent (USP))) of the position that detects collision; Speed command is arranged on 0 forcibly so that stop the method (JP-A-2000-52286) of manipulator; Stop the method No. the 2871993rd, Japan Patent No. 3212571 (No. the 6298283rd, United States Patent (USP)) and Japan Patent ((No. the 5418440th, United States Patent (USP))) of manipulator by the maximum reverse motor torque opposite with the direction of rotation of motor.

Return the method for collision detection position according to manipulator, manipulator returns initial position by Position Control.Therefore, dwell time depends on the response characteristic of Position Control.Generally, the response characteristic maximum of Position Control is tens hertz.Therefore, response characteristic is not very high when stopping to collide, and has expanded dwell time, and can not prevent the generation of the loss that caused by collision.

According to by speed command being arranged on forcibly 0 method that stops manipulator, dwell time depends on the response characteristic of speed control.In this case, response characteristic is the hundreds of hertz, and this is greater than the response characteristic under the Position Control situation.But it is inferior to the response characteristic (several thousand Hz) of Current Control.

According to this method,, therefore when stopping manipulator, kept the distortion that causes by collision because higher by the servo rigidity of integrated Position Control of speed control.Therefore,, when the speed integration gain of making is 0, softened the Position Control rigidity, therefore can solve the problem on deformation that causes by collision according to JP-A-2000-052286.But, in order to strengthen compliance, must reduce gain pro rata with speed, this makes speed responsive characteristic variation and expanded dwell time.Be difficult to make dwell time and compliance compatible each other.

According to the method that stops manipulator by the maximum reverse motor torque with respect to the direction of rotation of motor, the response characteristic of Current Control that is used to produce opposite torque is very high, makes that response characteristic can be several thousand Hz, and promptly response characteristic is good.But,, must set in advance the time that applies opposing torque according to disclosed method in No. the 3212571st, Japan Patent.When this application time more in short-term, can not underspeed fully, and the infringement that is caused by collision increases.When this application time is longer, is being reversed redundant motion, and might causing collision once more by manipulator.According to disclosed method in No. the 2871993rd, Japan Patent, a kind of method has been proposed, wherein apply the maximum reverse torque, up to stopping motor.In this method, needn't pre-determine the application time that applies opposing torque.Therefore, solved above-mentioned problem.But, only when motor stops, can not solving the problem of the distortion that causes by collision.Because the generation of maximum reverse torque itself is wherein to control the state that just produces maximum output in open loop, therefore even to such an extent as to can not damage under the situation of robot during when the robot collision object in that speed is very low, existence applies the high risk of opposing torque.

In any one system, in the axle that its collision course overlaps with the direction of rotation of motor, when manipulator returns the collision detection position or stops suddenly, increased the collision intensity of force.

Figure 15 shows the view of this state, has used the diaxon robot to be used for explanation therein.Generally, common many abutments of vertical-type robot is made up of 6 axles.But, for the purpose of simplifying the description, 2 model will be described below.

In Figure 15 (a), at angular velocity omega _Fb Operating axis UA 41 on 1 the direction, and at angular velocity omega _Fb Operating axis FA 42 on the direction of ' 6.When the time in the past and each manipulator operate on the direction shown in Figure 15 (b) and collide with barrier 43 in, produce impact force 44, and provide the power opposite to axle UA 41, promptly on the direction that can underspeed, provide collision torque tau dis to axle UA 41 with the direction of rotation of motor.On the other hand, on the direction identical, provide power, promptly on the direction that can push the speed, provide collision torque tau dis ' 10 to axle FA 42 to axle FA 42 with the direction of rotation of motor.

, in order axle FA42 to be returned collision detection position or suddenly to stop a spool FA42, need by motor produce torque, so that can reduce the motor rotation thereafter.But the direction of this torque is opposite with the direction of collision torque tau dis ' 10.Therefore, increased the intensity of collision torque on the contrary.

According to the method for manipulator being returned the collision detection position, though the axle that its motor direction of rotation is opposite with collision course (the axle UA among Figure 15) returns the collision detection position, but the axle that its motor direction of rotation is identical with collision course (the axle FA among Figure 15) is not inverted, and the operation that continues to have carried out is up to collision.By this way, solved above-mentioned problem.

But, do not using sensor to come under the situation of collision detection, estimate to collide torque from the information of mechanical parameter, position, speed, acceleration and the electric current of robot.Therefore, compare, increased the detection error with the situation that collision detection sensor is provided.For above-mentioned reasons, under the situation of the lower axle of its collision detection torque, might detect described direction mistakenly, and can not select the suitable mode that stops.

Under the situation of the lower axle of its detected collision torque value, collision detection direction and reduction motor rotary speed be not so that reduce kinetic energy than safety.Therefore but because collision course is unknown, it is better not reduce the motor rotary speed in some cases, promptly with different in the method described in No. the 2871993rd, the Japan Patent, should do not apply opposite torque and stop up to motor.Even very low so when the collision of robot and barrier, can not damage under the situation of robot, should not apply opposing torque in the motor rotary speed.

And, under the situation of many abutments of vertical-type robot, can not ignore the perturbed force between axle.So, might provide speed to reduce power to an axle that should not reduce its speed by the perturbed force that provides from the axle that has been applied in opposing torque.In either case, should apply opposing torque to needed axle in the minimum time cycle.

Summary of the invention

The present invention has been implemented and has solved the problems referred to above.The purpose of this invention is to provide a kind of control method, can be by in compliance control, carrying out FEEDBACK CONTROL so that the electric current restriction is suppressed to strengthen its compliance for being lower than friction torque by motor-driven robot.

Another object of the present invention provides a kind of control method that is used to control robot, and it can be by making the dynamic friction torque of reduction gearing not overlap with fixed value and overlaps the collision detection torque of pin-point accuracy ground with value corresponding to actual characteristic.

Another object of the present invention provides a kind of control method that is used to control robot, it can be by making the dynamic friction torque of reduction gearing not overlap with fixed value overlaps with value corresponding to actual characteristic and strengthen the motor torque computational accuracy before operation, and obtains the less optimal feedback compensation of its phase retardation.

According to the present invention, provide a kind of and controlled method by motor-driven manipulator via reduction gearing, may further comprise the steps: calculate the generation torque that produces by motor; Calculate the dynamic friction torque from calculating the dynamic torque that obtains by inverse kinematics; And by deduct from described generation torque with motor inertia multiply by the motor angular acceleration the result, with viscous friction coefficient multiply by motor angular velocity the result, with described dynamic friction torque times with the result of parameter of expression direction of rotation and described dynamic torque and, calculate the collision torque.

According to the present invention, a kind of method of controlling robot is provided, it is characterized in that: the anglec of rotation that detects the motor that is used for the driven machine people; Calculate the actual measured value of angular speed from the described anglec of rotation; One of angular speed by using the bid value that calculates from the bid value that is provided to motor and angular speed of actual measured value calculate friction torque, wherein, use the angular speed with higher absolute value in this calculates; And when coming drive motors, to the value of the bid value increase that is provided to motor corresponding to described friction torque according to above-mentioned bid value.Because this method, the electric current restriction by FEEDBACK CONTROL can be suppressed to and be lower than friction torque.Therefore, might realize the control method that its compliance is high.

When one of value of described bid value and actual measurement suitably is chosen as when being used for the angular speed that friction torque calculates and changing the friciton compensation rate simultaneously, can prevent the feedback characteristics vibration, and can improve the target following feature.

The invention provides and a kind ofly control method by motor-driven robot via reduction gearing, it is characterized in that: when the reduction gearing that obtains by the dynamic calculation that deducts from the torque that is produced by motor by robot is exported needed torque when calculating external force, the dynamic friction torque of reduction gearing is calculated as corresponding to reduction gearing and exports needed torque and increase.

The invention provides and a kind ofly control method by motor-driven robot via reduction gearing, it is characterized in that: according to being used to obtain the inverse kinematics calculating that reduction gearing is exported the robot of needed torque, and also, carry out the motor output torque compensation by FEEDBACK CONTROL according to the dynamic friction torque calculation of reduction gearing; And when carrying out FEEDBACK CONTROL, the dynamic friction torque of reduction gearing is calculated as exports needed torque with reduction gearing and increases pro rata.

According to the present invention, under the situation that detects its collision torque direction of collision back axle opposite with the direction of rotation of motor, when the robot control model is used for making the Position Control of current order of actual location following position command when being used for Current Control that command current produces the torque opposite with the motor direction of rotation of its direction by motor and changing from being used to produce, the motor rotary speed is lowered, and has reduced collision energy.Thereafter, when motor speed was reduced to the value that is lower than the value of setting, described control model was switched to compliance control, so that manipulator can be followed the impact force direction, and solved the problem on deformation that is caused by collision in reduction gearing.Might stop and reducing described speed by the highest Current Control of its response characteristic, and when the monitoring motor speed, can determine the application time of the motor torque that its direction is opposite with the motor direction of rotation.Therefore, needn't set in advance the motor torque application time.

On the other hand, under the situation of the direction of its collision torque axle identical with the motor direction of rotation, do not carry out Current Control, control model is directly changed to compliance control from Position Control.When the described axle of operation when following impact force, can weaken the collision torque.

Be lower than in its motor rotary speed under the situation of axle of the value of setting in when collision, no matter motor direction of rotation and collision torque direction are how, control model directly from Position Control to compliance control conversion, and do not carry out Current Control.Therefore, not producing when unnecessary only is the open loop situations of Current Control.

Description of drawings

Fig. 1 shows being used in first and second embodiment and controls the block diagram of the control method of friciton compensation.

Fig. 2 is illustrated in being used among the 3rd embodiment to control the block diagram of the control method of friciton compensation.

Fig. 3 shows the block diagram of the control method that is used to control friciton compensation in traditional example, has used actual speed in described method.

Fig. 4 shows the block diagram of the control method that is used to control friciton compensation in traditional example, has used speed command in described method.

Fig. 5 shows the block diagram of the collision torque detection method of embodiments of the invention 1.

The reduction gearing that Fig. 6 shows mediation (harmonic) reduction gearing is exported the view of an example of needed dynamic friction torque characteristics.

The reduction gearing that Fig. 7 shows the RV reduction gearing is exported the view of an example of needed dynamic friction torque characteristics.

Fig. 8 shows the view of the parameter in dynamic friction torque approximate expression.

Fig. 9 shows the view of exporting needed torque error according to the reduction gearing of dynamic friction torque calculation method of the present invention.

Figure 10 is the block diagram that the reduction gearing in embodiments of the invention 2 is exported needed torque FEEDFORWARD CONTROL.

Figure 11 shows the view of exporting needed torque error according to the reduction gearing of the dynamic friction torque calculation method of conventional example.

Figure 12 shows the view of the operation when the measurement reduction gearing is exported needed torque.

Figure 13 shows the view that under the situation that increases dynamic friction torque reduction gearing is exported needed torque error.

Figure 14 shows the sequential chart of the collision method for controlling stopping in first embodiment.

Figure 15 shows the velocity attitude when collision and collides the robot manipulation figure of torque direction.

Figure 16 shows the block diagram of the collision stop control spare (position control mode) in first embodiment.

Figure 17 shows the block diagram of the collision stop control spare (current control mode) in first embodiment.

Figure 18 shows the block diagram of the collision stop control spare (compliance control model) in first embodiment.

Figure 19 shows the block diagram of collision stop control spare (position control mode) in a second embodiment.

Figure 20 shows the block diagram of collision stop control spare (current control mode) in a second embodiment.

Figure 21 shows the block diagram of the collision stop control spare (current control mode) in the 4th embodiment.

Figure 22 shows the block diagram of the collision stop control spare (position control mode) in the 3rd embodiment.

Figure 23 shows the block diagram of the collision stop control spare (current control mode) in the 3rd embodiment.

Figure 24 shows the block diagram of the collision stop control spare (compliance control model) in the 3rd embodiment.

Figure 25 shows the sequential chart of the collision method for controlling stopping in the 3rd embodiment.

The specific embodiment

Referring to accompanying drawing, with the preferred embodiment of following explanation robot control method of the present invention.

First embodiment

Fig. 1 shows the block diagram of control method of the present invention.In Fig. 1, drawing reference numeral 26 is rate conversion devices, and drawing reference numeral 27 is angular velocity omegas of being selected by the rate conversion device.Can obtain feedback current order I in the following manner _Com4: by feedback controller 2 from anglec of rotation order θ _Com1 and real electrical machinery anglec of rotation θ _FbCarry out PID and calculate, and carry out electric current restriction 3.About being used for electric current restriction 3 Mode, a kind of system and a kind of system that wherein reduces feedback oscillator that wherein is provided with restriction is provided.

On the other hand, can followingly calculate I by expression formula (3) _Ml17.Ought be motor rotate command θ _Com1 carries out the angular acceleration that differential calculation obtained at 12,14 o'clock twice _Com15 multiply by motor inertia J16.Friction torque τ μ 20 and dynamic torque τ dyn 19 are added to the value of acquisition like this.With the 1/K reciprocal with the motor torque constant on duty that so obtains _t18.By this way, can calculate I _Ml17.

Calculate the angular velocity omega 27 that 21 friction torque τ μ 20 use for working as by expression formula (5)-(9), rate conversion device 26 is selected to work as motor anglec of rotation order θ _ComThe 1 angular speed order ω that obtains when differentiating _Com13, perhaps work as real electrical machinery anglec of rotation θ _FbThe 11 actual angular speed ω that differentiate and obtained in 24 o'clock _Fb23.

Come conversion speed by rate conversion device 26 according to following expression formula.

$\mathrm{\&omega;}=\left\{\begin{array}{c}\mathrm{\&omega;fb}\left(\right|\mathrm{\&omega;com}|\&le;|\mathrm{\&omega;fb}\left|\right)\\ \mathrm{\&omega;com}\left(\right|\mathrm{\&omega;com}|>|\mathrm{\&omega;fb}\left|\right)\end{array}\right.---\left(12\right)$

In expression formula (12), with angular speed order ω _Com13 absolute value and actual angular speed ω _Fb23 absolute value compares mutually, and selects higher value to be used as ω 27.

Use this ω, calculate friction torque τ μ 20 by expression formula (5)-(7).

When selecting as mentioned above, even when compliance control becomes effective, at input position order θ _Com1 and on one's own initiative under manipulator's the situation or by external force τ dis promotion robot and passively under manipulator's the situation, also can suitably calculate friction torque τ μ 20 and do not eased down to 0.

Even work as in the active operation robot and at angular speed order ω _Com13 and actual angular speed ω _FbWhen promoting robot and position deviation and increase by external force τ dis when there are differences between 23, if actual angular speed ω _Fb23 absolute value becomes greater than angular speed order ω _Com13 absolute value then adopts actual angular speed ω _Fb23 are used as ω 27.Therefore, can in the calculating of friction torque τ μ 20, reduce the factor that causes error.

And, when shut-down operation, even as angular speed order ω _Com13 at actual angular speed ω _Fb23 become when becoming 0 before 0, if actual angular speed ω _Fb23 absolute value becomes greater than angular speed order ω _Com13 absolute value then adopts ω _Fb23 are used as ω 27.Therefore, the friciton compensation by the friction torque τ that calculated by expression formula (4) can continue from this time point.That is, might work as angular speed order ω _Com13 have reached stopping suddenly that the elimination that prevented by friciton compensation causes at 0 o'clock.Therefore, might prevent such problem: manipulator can not reach the target location or produce vibration in manipulator.

Because even above-mentioned formation is when the electric current I by FEEDBACK CONTROL _Com4 restriction is suppressed to and is lower than actual friction torque so that when strengthening the compliance of control, and also might prevent the appearance of following point: manipulator can not reach the target location or especially vibrate when manipulator is stopped.

Second embodiment

In the expression formula (12) of the conversion that shows the speed in first embodiment, at least one of speed command value and actual measured value is multiplied by weight coefficient.

$\mathrm{\&omega;}=\left\{\begin{array}{c}\mathrm{\&omega;fb}\left(\right|\mathrm{<figure-callout\; id="1"\; label="kc"\; filenames="C2007101497660029H1.png,C2007101497660030H1.png"\; state="\{\{state\}\}">kc</figure-callout>}1*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="C2007101497660031H1.png,C2007101497660035H1.png"\; state="\{\{state\}\}">kc</figure-callout>}2|\&le;|\mathrm{\&omega;fb}\left|\right)\\ \mathrm{\&omega;com}\left(\right|\mathrm{<figure-callout\; id="1"\; label="kc"\; filenames="C2007101497660029H1.png,C2007101497660030H1.png"\; state="\{\{state\}\}">kc</figure-callout>}1*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="C2007101497660031H1.png,C2007101497660035H1.png"\; state="\{\{state\}\}">kc</figure-callout>}2|>|\mathrm{\&omega;fb})\end{array}\right.---\left(13\right)$

When setting up described formation shown in the expression formula (13), provide priority to one of speed command value and actual measured value, so that it can be adopted to speed.

Because the value ω of actual measurement _FbComprise measure error, for example, when the weight coefficient in expression formula (13) is set at following value, can preferentially select speed command ω _Com

Kc1＞1 and kc2＞0 (14)

The 3rd embodiment

Fig. 2 shows the block diagram of the control method of the 3rd embodiment.

Be imported into the actual angular speed ω of rate conversion device 26 _Fb23 multiply by friciton compensation rate k μ.

It is expressed as follows by expression formula (15).

$\mathrm{\&omega;}=\left\{\begin{array}{c}\mathrm{k\&mu;}*\mathrm{\&omega;fb}\left(\right|\mathrm{<figure-callout\; id="1"\; label="kc"\; filenames="C2007101497660029H1.png,C2007101497660030H1.png"\; state="\{\{state\}\}">kc</figure-callout>}1*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="C2007101497660031H1.png,C2007101497660035H1.png"\; state="\{\{state\}\}">kc</figure-callout>}2|\&le;|\mathrm{\&omega;fb}\left|\right)\\ \mathrm{\&omega;com}\left(\right|\mathrm{<figure-callout\; id="1"\; label="kc"\; filenames="C2007101497660029H1.png,C2007101497660030H1.png"\; state="\{\{state\}\}">kc</figure-callout>}1*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="C2007101497660031H1.png,C2007101497660035H1.png"\; state="\{\{state\}\}">kc</figure-callout>}2|>|\mathrm{\&omega;fb}\left|\right)\end{array}\right.---\left(15\right)$

K μ: friciton compensation rate

When using the angular velocity omega 27 that obtains by above-mentioned expression formula (15), selecting actual angular speed ω for the angular speed that when calculating 21 friction torque τ μ 20, uses by expression formula (5) _FbUnder 23 the situation, when friciton compensation rate k μ is set at when being not more than 1 value, can not 100% ground friction compensation torque tau μ 20.Therefore, might adjust and make feedback characteristics not vibrate.

On the other hand, use angular speed order ω at angular velocity omega for the calculating 21 that is used for friction torque τ μ 20 _ComUnder 13 the situation, can 100% ground friction compensation torque tau μ 20, and do not influence the feedback characteristics of control system.Therefore, can improve the target following feature.

The 4th embodiment

Fig. 6 and 7 shows the view of measurement result, wherein, is used for the dynamic friction torque of the typical reduction gearing of robot with respect to the fluctuation measurement of dynamic torque τ dyn under the condition of disturbing torque tau dis=0.Fig. 6 shows the view of the characteristic under mediation reduction gearing situation, and Fig. 7 shows at the view as the characteristic under the RV reduction gearing situation of centering error dynamic formula reduction gearing type.

Can find out that according to the increase of dynamic torque τ dyn, the dynamic friction torque is increased from Fig. 6 and 7.Can be similar to the dynamic friction torque by expression formula (16).

$\mathrm{\&tau;\&mu;a}=\left\{\begin{array}{c}A*\mathrm{\&tau;dyn}+B(\mathrm{\&tau;dyn}\&GreaterEqual;\mathrm{\&tau;th})\\ C*{\mathrm{\&tau;<figure-callout\; id="2"\; label="dyn"\; filenames="C2007101497660031H1.png,C2007101497660035H1.png"\; state="\{\{state\}\}">dyn</figure-callout>}}^2+D\left(\right|\mathrm{\&tau;dyn}|<\mathrm{\&tau;th})\\ E*\mathrm{\&tau;dyn}+F(\mathrm{\&tau;dyn}\&le;-\mathrm{\&tau;th})\end{array}\right.---\left(16\right)$

In expression formula (16), drawing reference numeral A, B, C, D, E and F are the constants that is similar to, and τ th is provided with threshold value.

In Fig. 8, described at the above-mentioned parameter shown in Fig. 6 and 7.

Fig. 9 shows the view of result of calculation, wherein, calculates dynamic friction torque approximation τ μ a according to expression formula (16), and calculates and relatively more necessary torque in the reduction gearing output identical with reduction gearing output shown in Figure 11.Can in Fig. 9, find out, compare, not increase in the error 5 of constant speed, and be lowered in the error 3 of peak torque with Figure 11.

Then, when using the τ μ a that calculates by expression formula (16) to be out of shape expression formula (10), can obtain following expression.

τdisa＝K _t*I _m-(J*α+D*ω+τμa*sgn(ω)+τdyn) (17)

When calculating collision torque tau disa by expression formula (17), compare with expression formula (10), becoming especially to reduce error before collision.Therefore, collision detection sensitivity needn't be reduced, and the collision detection precision can be strengthened.

Fig. 5 shows the block diagram of this method.

In Fig. 5, with motor anglec of rotation bid value θ _Ref11 and motor anglec of rotation θ from obtaining by the dotted line institute area surrounded 12 that shows (motor)+(actual load) _M13 compare, and controller 14 provides electric current I to motor _m15.In motor, produce torque, it is to work as electric current I _m15 multiply by torque constant K _tObtained in 16 o'clock.From this torque deduct dynamic torque τ yn, collision torque tau dis and friction torque τ μ * sgn (ω) with 17.Be used to drive the single motor body of expressing from described torque of subtracting each other acquisition by square frame 18.Collision torque calculation part 19 is passed through expression formula 17 from motor anglec of rotation θ _M13 and electric current I _m15 calculate collision torque detection value τ disa 20.

About this point,, in expression formula (17), can use current of electric torque tau mO=K though increased error _t* I _mReplace dynamic torque τ dyn.

The 5th embodiment

Then, the following describes the fifth embodiment of the present invention.

At first, about the dynamic friction torque, use expression formula (16).When expression formula (11) is out of shape, can obtain following expression (18).

I _ff＝(J*α+D*ω+τμa*sgn(ω)+τdyn)/K _t (18)

In this expression formula (18), when passing through for motor anglec of rotation order θ _RefDifferentiate and when calculating angular velocity omega and angular acceleration, expression formula (18) can be deformed into expression formula (19).Therefore, can not use feedback signal to calculate and be used to produce the electric current I of the needed torque of motor _Ff

I _ff＝(J*s ²(θ _ref)+D*s(θ _ref)+τμa*sgn(ω)+τdyn)/K _t (19)

Figure 10 shows in the electric current I that feedovers by this _FfCarry out the block diagram of the embodiment under the situation of feedforward compensation.

In Figure 10, with motor anglec of rotation bid value θ _Ref11 and from by the motor anglec of rotation θ that obtains the dotted line institute area surrounded 12 that shows (motor)+(actual load) _M13 compare, and FEEDBACK CONTROL 21 is exported the current order I that flows in motor _Com22.As the feedforward current order I that obtains by the square frame 23 that shows expression formula (9) _Ff24 are added to the current order I that is obtained by this FEEDBACK CONTROL _Com, might realize that the sum of errors of wherein estimating postpones less FEEDBACK CONTROL at 22 o'clock.

The 7th embodiment

Figure 16 shows the view of the seventh embodiment of the present invention.

In Figure 16, item 26 is the collision torque detection parts that are used for collision detection torque tau disd 27, apply described collision torque tau disd 27 so that drive arm to manipulator by the impact force that is provided to the driving device hand, item 25 is collision judgment parts, be used for judging collision, and be used to export collision sensing signal D by the collision torque threshold of relatively colliding torque detection value τ disd 27 and be provided with _Col30, drawing reference numeral 24 is motor rotation detection parts 23, is used for from motor anglec of rotation θ _Fb22 detect the angular velocity omega of motor _Fb, drawing reference numeral 23 is collision course decision means, is used for by relatively colliding the torque detection side to exporting collision course sign D with the motor direction of rotation _Ir31, drawing reference numeral 32 is motor deceleration decision means, is used for by comparing motor angular velocity ω _Fb1 with the described threshold value that has been provided with and come output motor to slow down by the deceleration of confirming motor and judge signal D _Th33, drawing reference numeral 15 is control model converting members, and the back will illustrate it.Therein under the situation of motor direction of rotation and the reciprocal axle of collision torque direction, when being used to produce current order so that motor anglec of rotation θ _FbFollow anglec of rotation order θ _ComWhen 11 Position Control parts 12 were switched to Current Control parts 13, the motor rotary speed was lowered, and wherein said Current Control parts 13 are used to provide the order that produces electric current so that can produce the torque opposite with the motor direction of rotation of its direction by motor.When the motor rotary speed was lowered to the value that is not more than the value of setting, described control model converting member was transformed into pattern the compliance control assembly 14 of the direction of following impact force.Under the situation of the axle that the motor direction of rotation is identical with the collision torque direction, the control model converting member is transformed into compliance control assembly 14 with pattern from Position Control parts 12 therein.

Then, referring to Figure 16, with the method for controlling stopping that after collision detection, carries out following detailed description.By as the feedback controller 12 of Position Control parts from motor anglec of rotation order θ _Com11 and real electrical machinery anglec of rotation θ _Fb22 obtain being used to carry out the current order I of Position Control _Coml2.Feedback controller 12 is made of PID control usually.

In the common Position Control of before collision detection, carrying out, by control model conversion block 15 with current order I _Coml2 select as current of electric I _m16, and it is applied on (motor)+(actual load) 17.

As current of electric I _m16 are multiplied by torque constant K _tThe motor torque τ that obtained in 18 o'clock _Mm19 and disturb torque 20 to be applied in and use on the described transfer function 21 of motor inertia J.

Disturb torque 20 be friction torque τ μ, gravitational torque τ g, dynamic torque τ dyn (inertia force, centrifugal force and earth deflecting force and) and collision torque tau dis with.

Motor anglec of rotation θ _FbThe 22nd, from motor transfer function 21 output, and detected by the encoder of the encoder of light type or magnetic type usually.

Detect in the piece 26 in the collision torque, followingly obtain colliding torque detection value τ disd 27.By under the situation that does not produce collision torque tau dis, using this motor anglec of rotation 22, another spindle motor anglec of rotation 29, can calculating by inverse kinematics and obtain the needed torque of motor by carry out angular speed, angular acceleration and the robot machine parameter that time diffusion is drawn for them.Deduct actual motor current I from the value of acquisition like this _Mm16 multiply by torque constant K _t18 and the value that obtains.By this way, can obtain colliding torque detection value τ disd 27.

When surpassing predetermined collision detection threshold tau at one of another collision torque detection value that obtains 28 and collision torque detection value τ disd 27 in the same manner _CthThe time, 25 judgements of collision judgment piece have caused collision.Then, send collision sensing signal D to control model conversion block 15 _Col30.

Detect in the piece 24 motor anglec of rotation θ at motor angular velocity _Fb22 by differential, so that obtain motor angular velocity ω _Fb1.Collision course decision block 23 comes from motor angular velocity ω by following expression formula _Fb1 and collision torque detection value τ disd 27 calculate collision course sign D _Ir31.

$\mathrm{Dir}=\left\{\begin{array}{c}1(\mathrm{\&omega;fb}*\mathrm{\&tau;disd}<0)\\ 0(\mathrm{\&omega;fb}*\mathrm{\&tau;disd}\&GreaterEqual;0)\end{array}\right.---\left(20\right)$

In expression formula (20), as motor angular velocity ω _FbWhen the direction of 1 direction and collision torque detection value τ disd 27 is opposite each other, collision course sign D _Ir31 become 1.Under other situations in addition, collision course sign D _Ir31 become 0.

In the operation shown in fig. 15, in axle UA 41, D _Ir=1, and in axle FA 42, D _Ir,=0.

As input collision sensing signal D _Col30 o'clock, control model conversion block 15 was according to collision course sign D _Ir31 information is come transform mode control.

Because D in axle UA _Ir=1, therefore by Current Control piece 13 from motor angular velocity ω _Fb1 generation is used to produce the electric current I of torque _Com23, the direction of described torque is opposite with the motor direction of rotation.Then, as shown in figure 17, control model conversion block 15 is selected I _Com23 are used as current of electric I _m16, promptly pattern is transformed into current control mode.

According to above-mentioned formation,, therefore collide torque tau dis 9 and after collision detection, can reduce because axle UA 41 slows down suddenly.

When axle UA 41 slows down and angular velocity omega _FbAbsolute value become less than predetermined deceleration judgment threshold ω _Th5 o'clock, motor deceleration decision block 32 output motors slowed down and judge signal D _Th33.

D _th＝1(|ω _fb|＜ω _th) (22)

When slowing down, this motor of output judges signal D _Th33 o'clock, control model conversion block 15 was selected I _Com34 are used as current of electric I _m6, and pattern is transformed into compliance control model shown in Figure 180.

About this point, at angular velocity omega _Fb1 absolute value is less than predetermined threshold ω _Th5 and when when collision detection, satisfying the condition of expression formula (22), pattern is not transformed into current control mode (shown in Figure 17) from common control model (shown in Figure 16), but pattern is transformed into compliance control model (shown in Figure 180), but does not slow down by applying opposing torque to motor.

In this case, can realize compliance control in such a way: compliance controll block 14 is for by the current order I from feedback controller 12 outputs _ComlThe electric current of 2 controls limits, and increases the gravity compensation electric current then, so that prevent that robot is owing to the weight of itself falls.

Because even above-mentioned situation is when increasing at motor anglec of rotation order θ _Com11 and motor anglec of rotation θ _FbDuring deviation between 22,, therefore reduced the servo rigidity of Position Control, so that can strengthen the compliance of control because limited current of electric.

About the restriction of electric current, might be by being reduced in the restriction that gain in the feedback controller 12 realizes electric current.

When slowing down, motor in expression formula (22) judges signal D _Th=1 o'clock, motor angular velocity ω _Fb1 is lower than threshold value ω _Thr, promptly motor almost is stopped, and inertia energy is low.Therefore, when pattern is transformed into compliant mode, can solve the problem of the distortion of the reduction gearing that when collision, produces.

On the other hand, under the situation of axle FA 42, the collision course sign D in expression formula (2) _Ir'=0.Therefore, when to quicken or during deboost axle FA 42 the collision torque tau suddenly with the identical mode of axle UA 41 _Dis' 10 increase on the contrary.

Therefore, collision course sign D when collision takes place _IrUnder the situation of '=0, control model conversion block 15 is converted to compliance control model (shown in Figure 180) with control model from common control model (shown in Figure 16), and without current control mode (shown in Figure 17).

Because above-mentioned formation controls operating axis FA42 by compliance when following impact force.Therefore, can weaken the collision torque.

Figure 14 is the sequential chart that shows above-mentioned control method with time series.

The 8th embodiment

Figure 19 shows the view of the eighth embodiment of the present invention.

The characteristic of the 8th embodiment shown in Figure 19 is described as follows.Figure 16 with reference to showing the 7th embodiment in the embodiment shown in Figure 19, provides collision torque threshold decision means 34, and it will collide torque detection value τ _Disd27 judge that with collision course torque threshold compares.Collide torque detection value τ therein _Disd27 are lower than collision course judges under the situation of axle of torque threshold, no matter how are motor direction of rotation and collision torque direction, Position Control parts 12 are switched to Current Control parts 13, so that motor can produce the torque opposite with the motor direction of rotation of its direction, and can reduce motor rotary speed ω _Fb1.When the motor rotary speed was lowered to the value that is not more than the value of setting, control model converting member 15 translative mode were to compliance control assembly 14.

Referring to Figure 19, with operation and the function function, collision torque threshold decision block 34 of explanation as increase.

In the 7th embodiment, only pass through the collision course sign D that determines by expression formula (20) _Ir31 determine whether control model is switched to current control mode (shown in Figure 17).

But, about the collision torque detection value τ that in the condition judgment of expression formula (20), uses _Disd27, do not using torque sensor to estimate to collide torque detection value τ shown in the 7th embodiment _DisdUnder 27 the situation, because estimate to collide torque tau from the information of mechanical parameter, motor position, angular speed, angular acceleration and the electric current of robot _Dis, therefore comparing with the situation that collision detection sensor wherein is provided has increased the detection error.

Therefore, collide torque detection value τ therein _DisdUnder 27 lower and the situations near 0 axle, might be by detecting error at collision torque detection value τ _DisdCause mistake in 27 the symbol.

In other words, motor speed ω therein _FbUnder the situation of 1 higher axle, promptly therein under the situation of the higher axle of inertia energy, as collision torque detection value τ _Disd27 when low, might export collision course sign D mistakenly _Ir31.

As collision torque detection value τ in an axle _Disd27 during greater than predetermined collision judgment threshold tau dth, and collision judgment piece 25 is judged and caused collision.Therefore, its detected value be higher than the collision judgment threshold value the axle situation under, do not carry out collision course sign D _Ir31 mistake output.

Under the situation of the lower axle of its collision torque tau dis, except the axle that has wherein detected collision, because do not provide strong external force to described axle, therefore reducing motor is safer so that reduce inertia energy fast.But as long as can not judge collision course, unactual in some cases reducing motor is better.Therefore, the absolute value of the speed when collision detection is low so that does not damage under the situation of robot, the reducing motor not by providing opposing torque to motor.Under fast situation, motor is not decelerated, and stop fully up to rotation, but motor should be decelerated to the speed that does not cause infringement in robot.

Therefore, as shown in figure 19, the collision torque threshold decision block 34 that has increased newly is judged signal D to control model conversion block 15 output collision torque threshold _Tht35.

$\mathrm{Dtnt}=\left\{\begin{array}{c}1\left(\right|\mathrm{\&tau;disd}|<\mathrm{\&tau;thr})\\ 0\left(\right|\mathrm{\&tau;disd}|\&GreaterEqual;\mathrm{\&tau;thr})\end{array}\right.---\left(23\right)$

τ _Thr: collision course is judged torque threshold

0＜τ _Thr≤ τ _Cth(collision detection judgment threshold)

Shown in expression formula (23), as collision torque detection value τ _Disd27 absolute value becomes less than predetermined collision course judgment threshold τ _ThrThe time, the collision torque threshold is judged signal D _Tht35 become 1.

Judge torque threshold τ at collision course _ThrBe not more than collision detection judgment threshold τ _CthCondition under, collision course in expression formula (23) is judged torque threshold τ _ThrCan be set to larger than collision torque detection value τ _Disd27 detection error.

When the collision torque threshold is judged signal D _Tht=1 o'clock, no matter as the collision course sign D of the output signal of impact velocity torque direction decision block 23 _Ir31 how, and when detecting collision, control model is transformed into current control mode (shown in Figure 20) from common control model (shown in Figure 19), and motor slows down.Carry out with seven embodiment identical processing thereafter.

At this moment, the threshold value ω in the expression formula in the 7th embodiment (22) _Th5 are set in the time of can not damaging the velocity amplitude of robot the angular velocity omega when collision detection _Fb1 absolute value is less than threshold value ω _Th5, so that do not damage robot.In this case, control model is not changed to current control mode (shown in Figure 17) from common control model (shown in Figure 16), but control model is to compliance control model (shown in Figure 180) conversion, but do not slow down by applying opposing torque to motor.

Even the angular velocity omega when in collision detection _Fb1 when higher, if motor is decelerated to as the threshold value ω that does not damage the speed of robot _Th5, then control model is transformed into compliance control model (shown in Figure 180).

When the collision torque threshold is judged signal D _Tht=0 o'clock, carry out with the 7th embodiment in identical processing.

The 9th embodiment

In the 7th embodiment, after collision detection, collide therein under the situation of the torque direction axle opposite with the motor direction of rotation, control is transformed into Current Control from Position Control, wherein in Position Control, generation is used to make physical location to follow the current order of position command, and described Current Control command current is so that can produce the torque opposite with the motor direction of rotation of its direction by motor.Because this conversion operations, the motor rotary speed is lowered, and has weakened collision energy.Thereafter, when the motor rotary speed is reduced to the value that is not more than the value of setting, control transitions to compliance control, in this compliance control, the direction of impact force is followed in the motor rotation, so that can solve the problem on deformation in reduction gearing that is caused by collision.

But, under the speed condition with higher when collision detection, only, can not solve the problem of the distortion of the reduction gearing that causes by collision fully when described when controlling transitions to compliance control when motor rotation is slowed down.

Therefore, motor direction of rotation and the opposite each other and motor rotary speed of collision torque direction and collision torque detection value are higher than respectively under the situation of axle of the value of setting therein, when carrying out Current Control after reducing the motor rotary speed (wherein the torque that its direction is opposite with the motor direction of rotation is produced by motor), the continuous rightabout torque that applies is reversed up to velocity attitude.After the part of the distortion that has solved the reduction gearing that is produced by collision, the speed of counter-rotating is increased to the value that is not less than the value of setting.Then, control transitions to the compliance control that the impact force direction is followed in wherein motor rotation.

When carrying out above-mentioned control method, control system is complicated, and has increased the opposing torque application time, and might manipulator bounce-back widely on the rightabout of collision course.On the other hand, can promptly solve problem on deformation in reduction gearing.

Figure 22 shows the view of the ninth embodiment of the present invention.

Characteristic at the 9th embodiment shown in Figure 22 is described as follows.With reference to the Figure 16 that shows the 7th embodiment, motor deceleration decision means 32 is changed to motor deceleration and counter-rotating decision means 39.Except the collision torque threshold decision means 34 that in the 8th embodiment, provides, impact velocity decision means 37 also newly is provided, be used to judge the motor speed when collision detection.Because above-mentioned situation, motor direction of rotation and collision torque direction are opposite each other and judge that motor rotary speed and collision torque detection value surpass under the situation of axle of the value of setting therein, pattern be transformed into wherein by motor be created in motor direction of rotation rightabout in the current control mode of torque.Even after having reduced the motor rotary speed, the torque in in the opposite direction also keeps being applied in, and is inverted up to velocity attitude.After the part of the distortion that has solved the reduction gearing that is caused by collision, when the speed of counter-rotating increases to when being not less than the value of setting, control model is switched to the compliance control that the impact force direction is followed in wherein motor rotation.The 9th embodiment comprises the control model converting member 15 of operation as mentioned above.

Referring to Figure 22, will illustrate as the impact velocity decision means 37 that increases parts with as the motor deceleration of change parts and the operation and the function of counter-rotating decision means 39.

In the 7th embodiment, only pass through the collision course sign D that determines by expression formula (20) _Ir31 determine the conversion to current control mode (shown in Figure 17).

In the 9th embodiment, provide the collision torque threshold decision means 34 that is added to the 8th embodiment.The 9th embodiment only is applied to wherein D in expression formula (23) _Tht=0 situation promptly only is applied to the absolute value that wherein collides torque detection value τ disd 27 and is not less than predetermined collision course judgement torque threshold τ _ThrAnd collision course sign D _IrThe situation of=1 (motor direction of rotation and collision torque direction are opposite each other).

In the situation except above-mentioned situation, control according to the control method of embodiment 7 or 8.

At motor angular velocity ω _Fb1 absolute value is greater than predetermined impact velocity judgment threshold ω _ThsIn 39 the situation, impact velocity decision means 37 output impact velocities are judged signal D _Ths38.

D _ths＝1(|ω _fb|＜ω _ths) (24)

ω _Ths〉=ω _Th(deceleration judgment threshold)

When the collision torque threshold is judged signal D _Tht=0 and collision course sign D _Ir=1 o'clock, export this motor deceleration and judge signal D _Ths=1.Then, control model conversion block 15 is selected I _Com23 are used as current of electric I _m6, and pattern is transformed into the current control mode (shown in Figure 23) that wherein by applying opposing torque to motor the motor rotation is slowed down from common control model (shown in Figure 22).

Motor deceleration component 32 judges that described opposing torque reduces and the motor speed ω that reversed by applying _Fb1.On concrete, the following operation.In expression formula (22),, motor judges signal D when slowing down _Th33 pass through " 1 (collision) " → " 0 (deceleration) " → " 1 (counter-rotating) " when changing, and control model conversion block 15 is selected I _Com34 are used as current of electric I _m6, and pattern is transformed into compliance control model shown in Figure 24.Figure 25 is the sequential chart that shows above-mentioned control method with time series.

The tenth embodiment

Figure 21 shows the view of the tenth embodiment of the present invention.

Figure 21 shows a kind of layout, and wherein, with respect to the Figure 19 that shows the 8th embodiment, the Current Control parts become 36 from 13.The tenth embodiment comprises Current Control parts 36, is used to control an electric current, and described electric current produces the torque capacity of motor on the direction opposite with the motor direction of rotation when selecting the Current Control parts in control model converting member 15.

Because above-mentioned formation might be carried out maximum brake operating for motor.Therefore, can weaken collision energy, so that it can be reduced to low as far as possible.

Certainly, can in the Figure 16 that shows the 7th embodiment and the Figure 22 that shows the 9th embodiment, carry out identical change.

Application on the industry

As mentioned above, the invention provides a kind of method of control, it is characterized in that: detect and use The anglec of rotation in driven machine people's motor; Calculate the actual measured value of angular speed from the anglec of rotation; When inciting somebody to action The angle of the absolute value of the angular speed of the bid value that calculates from the order that is provided to motor and the value of actual measurement When the absolute value of speed is compared, select its absolute value greater than an angle of the absolute value of another angular speed Speed, and calculate friction torque with that angular speed; When coming drive motors according to bid value, To the value of the bid value increase that is provided to motor corresponding to friction torque; And in following two kinds of situations Always effectively carry out friciton compensation: come on one's own initiative manipulator's situation according to the angle order, by external force Promote and manipulator's situation passively. Because above-mentioned characteristic, can be with by FEEDBACK CONTROL Current limit is suppressed to less than friction torque. Therefore, can realize control method than highly conforming properties.

When the absolute value in speed relatively, at least one of bid value and actual measured value be multiply by or add During weight coefficient, speed command value or actual measured value can preferentially be adopted to speed. Therefore, example As, can preferentially select the little speed command value of its measure error.

And, use value to rub as being multiplied by when at least one of speed command value and actual measured value The angular speed that obtains when wiping cancellation ratio. Because above-mentioned situation, might prevent the feedback characteristics vibration that becomes, Simultaneously can improve the target following feature.

And, according to robot control method of the present invention, corresponding to the loss of the torque that is produced by motor The dynamic friction torque of reduction gearing be calculated as with dynamic torque and increase pro rata. Because above-mentioned feelings Condition can strengthen the accuracy of detection of colliding torque.

When the dynamic torque of reduction gearing is calculated as when increasing pro rata with dynamic torque, can strengthen The computational accuracy of dynamic friction torque, and can realize optimal feedforward compensation.

As mentioned above, according to robot control method of the present invention, after detecting collision, when motor revolves When veer and collision course were opposite each other, control model was turned to current control mode from position control mode Change, and motor produces the torque opposite with the motor direction of rotation of its direction. For above-mentioned reasons, motor Slowed down, and weakened collision energy. When motor rotary speed be lowered to be not more than setting thereafter, During the value of value, control model is switched to compliance control, and in the reduction gearing that produces in collision Distortion be solved. On the other hand, in the motor direction of rotation situation identical with collision course, control Pattern directly is transformed into compliance control and without Current Control from Position Control. When following collision During the manipulator, can weaken the collision torque in the time of power. The structure when colliding shut-down operation as mentioned above During one-tenth, the infringement of the robot that caused by collision can be suppressed to minimum.

And, collide therein in the situation of absolute value less than the axle of settings of torque detection value, no matter How are motor direction of rotation and collision torque direction, and control model is changed to Current Control from Position Control, And motor produces the torque opposite with the motor direction of rotation of its direction in order to reduce the motor rotary speed. When When the motor rotary speed was reduced to the value that is not more than settings, control model was transformed into the pattern of complying with. Because Above-mentioned reason in the situation of the axle that has been provided high-intensity torque, is revolved according to collision course and motor Veer is carried out suitable shut-down operation. Be not provided another axle of high-intensity collision torque by fast Stop, in order to can reduce inertia energy fastly. Therefore, even do not use therein sensor and cause In the situation that the collision torque of bigger detection error detects, might select the suitable mode that stops.

And, when carrying out following Current Control: namely therein the motor direction of rotation and the collision torque direction Opposite each other and motor rotary speed is produced by motor above in the axle of settings with the collision torque detection value During the torque opposite with the motor direction of rotation of its direction, after reducing the motor rotary speed, keep being applied to Torque oppositely is until the reverse motors rotary speed. Solved the reducing gear that in collision, causes After the part of the distortion in the wheel, when speed reversal surpasses settings, when control model is transformed into wherein When manipulator is followed the compliance control of impact force direction, can promptly solve the change in reduction gearing Shape.

And motor produces the Current Control of its direction torque opposite with the motor direction of rotation therein In the situation, provide an order in order to can produce maximum motor torque. Because this formation can Weaken collision energy the biglyyest.

Claims (2)

1. control method via reduction gearing for one kind, may further comprise the steps by motor-driven manipulator:

The generation torque that calculating is produced by motor;

Calculate the dynamic friction torque from calculating the dynamic torque that obtains by inverse kinematics; And

By deduct from described generation torque with motor inertia multiply by the motor angular acceleration the result, with viscous friction coefficient multiply by motor angular velocity the result, with described dynamic friction torque times with the result of function of expression direction of rotation and described dynamic torque and, calculate the collision torque.

2. control method via reduction gearing for one kind, comprising by motor-driven manipulator:

Export the inverse kinematics of the robot of needed torque and calculate according to being used to obtain reduction gearing, and also according to the dynamic friction torque calculation of reduction gearing, the step of carrying out the motor output torque compensation by FEEDBACK CONTROL,

Wherein, under the situation of carrying out FEEDBACK CONTROL, the dynamic friction torque of reduction gearing and reduction gearing are exported needed torque and are increased pro rata.

Images