CN1771114A - Robot arm control method and control device - Google Patents

Abstract

By appropriately selecting an instruction value or an actually measured value as an angular velocity used for friction torque calculation, the friction compensation may be valid in both cases when the operation is active according to the angular velocity instruction and when the operation passive, i.e., when pushed by an external force. Moreover, after a collision is detected, when the motor rotation direction and the collision direction are opposite, the position control is switched to current control so that the motor generates a torque of the direction opposite to the motor rotation, thereby reducing the motor rotation speed and mitigating the collision energy. After this, when the motor rotation speed becomes a set value or below, the mode is set to flexible control, thereby eliminating the distortion of a reduction device or the like generated by the collision. On the other hand, when the motor rotation direction and the collision direction are identical, the position control is switched directly to the flexible control not passing through the current control. By performing operation in accordance with the collision force, it is possible to mitigate the collision torque.

Description

The method and apparatus of control manipulator

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 θ _Com(1) and real electrical machinery anglec of rotation θ _FbCan obtain feedback current order I when carrying out PID calculating and carrying out electric current restriction (3) _Com(4).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) _Ml(17) can followingly obtain.Will be to motor rotate command θ _Com(1) differentiates (12) and the angular speed θ of (14) twice acquisition _Com(15) multiply by motor inertia J (16).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 _t(18), can obtain the I that calculates by expression formula (3) _Ml(17).

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}{cc}-1& (\mathrm{\&omega;}<0)\\ 0& (\mathrm{\&omega;}=0)\\ 1& (\mathrm{\&omega;}>0)\end{array}\right.$

τμd＝D*ω (7)

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 actual angular speed ω that obtains when (11) differentiating _FbIn the time of the angular speed that uses when (23) being used as using computing block (21) to calculate friction torque τ μ (20) by expression formula (5), 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 _Com(4) torque that causes is suppressed to strengthen compliance and to become when being lower than actual static friction torque tau μ s, even produced anglec of rotation order θ _Com(1), robot does not move yet.

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

But, when angular speed order ω _ComWhen (13) being used for calculating the employed angular velocity omega of (21) friction torque τ μ (20), can be according to angular speed order ω _Com(13) 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 _Com(13) 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 ω _Com(13) be 0 when being promoted robot and robot and be stopped by external force when stopping robot, 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 ω _Com(13) 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 ω _Com(13) reach at 0 o'clock, 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 _Com(4) torque that is produced by motor is 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) _FbWhen (23) coming 100% ground friction compensation torque tau μ, 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 the employed angular velocity omega of (21) friction torque τ μ (20) _Com(13) under 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}{cc}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(1) operating axis UA (41) on 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.

, for axle FA (42) returned collision detection position or suddenly stop a spool FA (42), 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, 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 shows being used in the 3rd embodiment and controls 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 _Com(4): by feedback controller (2) from anglec of rotation order θ _Com(1) 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) _Ml(17).Ought be motor rotate command θ _Com(1) angular acceleration that obtains when carrying out differential calculation (12), (14) twice _Com(15) multiply by motor inertia J (16).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 _t(18).By this way, can calculate I _Ml(17).

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 angular speed order ω that obtains when (1) differentiating _Com(13), perhaps work as real electrical machinery anglec of rotation θ _FbThe actual angular speed ω that obtains when (11) differentiating (24) _Fb(23).

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

$\mathrm{\&omega;}=\left\{\begin{array}{cc}\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 ω _Com(13) absolute value and actual angular speed ω _Fb(23) 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 θ _Com(1) 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 ω _Com(13) and actual angular speed ω _FbWhen promoting robot and position deviation and increase by external force τ dis when there are differences (23), if actual angular speed ω _Fb(23) absolute value becomes greater than angular speed order ω _Com(13) absolute value then adopts actual angular speed ω _Fb(23) be 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 ω _Com(13) at actual angular speed ω _Fb(23) become when becoming 0 before 0, if actual angular speed ω _Fb(23) absolute value becomes greater than angular speed order ω _Com(13) absolute value then adopts ω _Fb(23) be 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 ω _Com(13) 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 _Com(4) 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}{cc}\mathrm{\&omega;fb}& \left(\right|\mathrm{kcl}*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">kc</figure-callout>}2|\&le;|\mathrm{\&omega;fb}\left|\right)\\ \mathrm{\&omega;com}& \left(\right|\mathrm{kcl}*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">kc</figure-callout>}2|>|\mathrm{\&omega;fb}\left|\right)\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) _Fb(23) multiply by friciton compensation rate k μ.

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

$\mathrm{\&omega;}=\left\{\begin{array}{cc}k\mathrm{\&mu;}*\mathrm{\&omega;fb}& \left(\right|\mathrm{kcl}*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">kc</figure-callout>}2|\&le;|\mathrm{\&omega;fb}\left|\right)\\ \mathrm{\&omega;com}& \left(\right|\mathrm{kcl}*\mathrm{\&omega;com}+\mathrm{<figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.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 under the situation of actual angular speed ω FB (23) for the angular speed that when calculating (21) friction torque τ μ (20), uses by expression formula (5), 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) _Com(13) under 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}{cc}A*\mathrm{\&tau;dyn}+B& (\mathrm{\&tau;dyn}\&GreaterEqual;\mathrm{\&tau;th})\\ C*\mathrm{\&tau;}{\mathrm{<figure-callout\; id="2"\; label="dyn"\; filenames="A200480009285.X00311.png,A200480009285.X00321.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, will be from the motor anglec of rotation bid value θ that obtains by the dotted line institute area surrounded (12) that shows (motor)+(actual load) _Ref(11) and motor anglec of rotation θ _M(13) compare, and controller (14) provides electric current I to motor _m(15).In motor, produce torque, it is to work as electric current I _m(15) multiply by torque constant K _t(16) obtain the time.That deduct dynamic torque τ yn, collision torque tau dis and friction torque τ μ * sgn (ω) from this torque and (17).Be used for driving 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 θ _M(13) and electric current I _m(15) 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 Fig. 5, will be from motor anglec of rotation bid value θ by acquisition the dotted line institute area surrounded (12) that shows (motor)+(actual load) _Ref(11) and motor anglec of rotation θ _M(13) compare, and FEEDBACK CONTROL (21) is exported the current order I that flows in motor _Com(22).As the feedforward current order I that obtains by the square frame that shows expression formula (9) (23) _Ff(24) be added to the current order I that obtains by this FEEDBACK CONTROL _Com(22) time, might realize that the sum of errors of wherein estimating postpones less FEEDBACK CONTROL.

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 part that is 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 the 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 _Col(30), drawing reference numeral (24) is a motor rotation detection part (23), is used for from motor anglec of rotation θ _Fb(22) angular velocity omega of detection motor _Fb, drawing reference numeral (23) is the 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 _Ir(31), drawing reference numeral (32) is a motor deceleration decision means, is used for by comparing motor angular velocity ω _Fb(1) come the output motor deceleration to judge signal D with the described threshold value that has been provided with and by the deceleration of confirming motor _Th(33), drawing reference numeral (15) is the control model converting member, 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 Position Control parts (12) (11) are switched to Current Control parts (13), the motor rotary speed is 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 θ _Com(11) and real electrical machinery anglec of rotation θ _Fb(22) obtain being used to carry out the current order I of Position Control _Com1(2).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 _Com1(2) select as current of electric I _m(16), and with it be applied on (motor)+(actual load) (17).

As current of electric I _m(16) be multiplied by torque constant K _t(18) the motor torque τ that obtains the time _Mm(19) and disturb torque (20) to be applied in and use on the motor described transfer function of inertia J (21).

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 θ _Fb(22), and detected by the encoder of the encoder of light type or magnetic type usually from motor transfer function (21) output.

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 _Mm(16) multiply by torque constant K _t(18) 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 (28) that obtains and collision torque detection value τ disd (27) in the same manner _CthThe time, collision judgment piece (25) judgement having caused collision.Then, send collision sensing signal D to control model conversion block (15) _Col(30).

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

$\mathrm{Dir}=\left\{\begin{array}{cc}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 direction (1) and collision torque detection value τ disd (27) is opposite each other, collision course sign D _Ir(31) become 1.Under other situations in addition, collision course sign D _Ir(31) 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 _Col(30) time, control model conversion block (15) is according to collision course sign D _Ir(31) information is come transform mode control.

Because D in axle UA _Ir=1, therefore by Current Control piece (13) from motor angular velocity ω _Fb(1) generation is used to produce the electric current I of torque _Com2(3), 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 _Com2(3) be used as current of electric I _m(16), 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 ω _Th(5) time, motor deceleration decision block (32) output motor slows down and judges signal D _Th(33).

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

When slowing down, this motor of output judges signal D _Th(33) time, control model conversion block (15) is selected I _Com3(4) be used as current of electric I _m(6), and pattern be transformed into compliance control model shown in Figure 180.

About this point, at angular velocity omega _Fb(1) absolute value is less than predetermined threshold ω _Th(5) 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) output _Com1(2) Kong Zhi electric current 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 θ _Com(11) and motor anglec of rotation θ _FbDuring deviation (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 ω _Fb(1) 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 _{Ir '}Under=0 the situation, 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 FA (42) 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 τ _Disd(27) compare with collision course judgement torque threshold.Collide torque detection value τ therein _Disd(27) being 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 ω _Fb(1).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 was 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) _Ir(31) 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 _Disd(27), do not using torque sensor to estimate to collide torque detection value τ shown in the 7th embodiment _Disd(27) under 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 _Disd(27) under lower and the situation near 0 axle, might be by detecting error at collision torque detection value τ _Disd(27) cause mistake in the symbol.

In other words, motor speed ω therein _Fb(1) under the situation of higher axle, promptly therein under the situation of the higher axle of inertia energy, as collision torque detection value τ _DisdWhen (27) hanging down, might export collision course sign D mistakenly _Ir(31).

As collision torque detection value τ in an axle _DisdDuring (27) greater than predetermined collision judgment threshold tau dth, collision judgment piece (25) is judged and has been 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 _Ir(31) 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 _Tht(35).

$\mathrm{Dtnt}=\left\{\begin{array}{cc}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 τ _Disd(27) absolute value becomes less than predetermined collision course judgment threshold τ _ThrThe time, the collision torque threshold is judged signal D _Tht(35) 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 τ _Disd(27) 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) _Ir(31) how, 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) _Th(5) be set in the time of not damage the velocity amplitude of robot the angular velocity omega when collision detection _Fb(1) absolute value is less than threshold value ω _Th(5), 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 _FbWhen (1) higher, if motor is decelerated to as the threshold value ω that does not damage the speed of robot _Th(5), 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 that changes 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) _Ir(31) definite 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 and wherein collides torque detection value τ _Disd(27) absolute value is not less than predetermined collision course and judges 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 ω _Fb(1) absolute value is greater than predetermined impact velocity judgment threshold ω _Ths(39) in the situation, impact velocity decision means (37) output impact velocity is judged signal D _Ths(38).

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 _Com2(3) be used as current of electric I _m(6), and pattern be transformed into wherein the current control mode (shown in Figure 23) that the motor rotation is slowed down by applying opposing torque to motor 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 _Fb(1).On concrete, the following operation.In expression formula (22),, motor judges signal D when slowing down _Th(33) pass through " 1 (collision) " → " 0 (deceleration) " → " 1 (counter-rotating) " when changing, control model conversion block (15) is selected I _Com3(4) be used as current of electric I _m(6), and pattern be 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 (13)

1. method of controlling manipulator comprises:

Detection is used for the step of the anglec of rotation of the motor of driving device hand;

Calculate the step of the magnitude of angular velocity of actual measurement from the described anglec of rotation;

Come the step of calculation command angular speed from the bid value that is provided to motor;

Calculate the step of friction torque according to one of angular speed of ordering angular speed and actual measurement, wherein, the absolute value of the angular speed of order absolute value of angular speed and actual measurement is compared, and use angular speed to calculate friction torque with higher absolute value; And

When coming drive motors, to the step of the bid value increase that is provided to motor corresponding to the value of described friction torque according to described bid value.

2. according to the method that is used to control manipulator of claim 1, also comprise step: at least one of the angular speed of described order angular speed and actual measurement is multiplied by weight coefficient, perhaps described weight coefficient is added at least one of angular speed of described order angular speed and actual measurement.

3. according to the method that is used to control manipulator of claim 1, also comprise step: at least one of the angular speed of described order angular speed and actual measurement be multiply by the friciton compensation rate.

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

Calculating is by the step of the generation torque of motor generation;

From the step of the torque calculation friction torque calculated by inverse kinematics, described friction torque comprises the dynamic friction torque of the needed reduction gearing of output of reduction gearing at least;

Increase the step of the dynamic torque of reduction gearing according to the needed friction torque of the output of reduction gearing; And

By deducting the step that the friction torque that is increased is calculated external force from the torque that is produced.

5. 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.

6. the method for control robot in the shut-down operation of after detecting, carrying out by the collision of motor-driven robot, therein in the reciprocal axle of motor direction of rotation and collision torque direction, when control model from Position Control when Current Control is changed, the motor rotary speed is lowered, and when the motor rotary speed is lowered to the value that is not more than the value of setting, control model is switched to the compliance control that robot follows the impact force direction, wherein in Position Control, generation is used to make physical location to follow the current order of position command, in Current Control, control an electric current that produces the torque opposite of its direction by motor with the motor direction of rotation; And

In the axle that the motor direction of rotation is identical with the collision torque direction, control model is changed to compliance control from Position Control therein.

7. according to the method for the control robot of claim 6, wherein, in the axle of collision detection torque less than the value of setting, no matter how are motor direction of rotation and collision torque direction, control model is changed to Current Control from Position Control, and when motor produced the torque opposite with the motor direction of rotation of its direction, the motor rotary speed was lowered, and when the motor rotary speed was lowered to the value that is not more than the value of setting, control model was transformed into compliance control.

8. according to the method for the control robot of claim 6, wherein, when carrying out following Current Control: promptly motor direction of rotation and the opposite each other and motor rotary speed of collision torque direction and collision torque detection value surpass in the axle of the value of setting when producing the torque opposite with the motor direction of rotation of its direction by motor therein, after reducing the motor rotary speed, keep applying reverse torque up to the speed reversal direction, and when the electrode rotary speed increased to the value that is not less than the value of setting, control model was transformed into the compliance control that robot wherein follows the impact force direction.

9. according to the method for the control robot of claim 6, wherein, when carrying out producing the Current Control of its direction torque opposite with the motor direction of rotation by motor after collision detection, control is used to produce the electric current of motor torque capacity.

10. equipment of controlling robot comprises:

Collision torque detection part is used to detect the intensity and the direction of the torque that is provided to the motor of driving device hand by the impact force that is provided to manipulator;

The collision judgment parts are used for judging collision by colliding torque detection value with the threshold of the collision torque that has been provided with;

Motor rotates detection part, is used to detect the rotary speed and the direction of rotation of motor;

The collision course identification component, be used for relatively colliding the torque detection side to the motor direction of rotation; And

Motor deceleration decision means, be used for by electrode rotary speed and the threshold that has been provided with being confirmed the deceleration of motor, wherein, therein in the reciprocal axle of motor direction of rotation and collision torque direction, when control model from Position Control when Current Control is changed, the motor rotary speed is lowered, and when the motor rotary speed is lowered to the value that is not more than the value of setting, control model is switched to the compliance control that robot follows the impact force direction, wherein in Position Control, generation is used to make physical location to follow the current order of position command, in Current Control, control one and produce the electric current of its direction torque opposite with the motor direction of rotation by motor;

The equipment of control robot also comprises the control model converting member, is used for the axle that the motor direction of rotation is identical with the collision torque direction therein and changes to the compliance control assembly from the Position Control parts.

11. the equipment according to the control robot of claim 10 also comprises:

Collision torque threshold decision means is used for the collision torque detection value is compared with collision course identification torque threshold; And

The control model converting member, being used for colliding therein torque detection value changes from the Position Control parts control model less than the axle of collision course identification torque threshold to the Current Control parts, no matter and motor direction of rotation and collision torque direction are how, and be used for when control model being transformed into the compliance control assembly when reducing the motor rotary speed by produce the torque opposite of its direction by motor with the motor direction of rotation, so that reduce the motor rotary speed, and the motor rotary speed is reduced to the value that is not more than the value of setting.

12. equipment according to the control robot of claim 10, wherein, when carrying out following Current Control: promptly motor direction of rotation and the opposite each other and motor rotary speed of collision torque direction and collision torque detection value surpass in the axle of the value of setting when producing the torque opposite with the motor direction of rotation of its direction by motor therein, after reducing the motor rotary speed, keep applying reverse torque up to the speed reversal direction, and when described electrode rotary speed increased to the value that is not less than the value of setting, control model was transformed into the compliance control that robot follows the impact force direction.

13. equipment according to the control robot of claim 10, also comprise: the Current Control parts, be used for when when detecting the collision back by control model converting member selection Current Control parts, command current produces its direction torque capacity opposite with the motor direction of rotation by motor.

Images