Patent Application
20140163735
The embodiments discussed herein are directed to a robot system, a robot, and a robot control device.
Japanese Patent No. 3300625 describes a control system for a robot. During a moving operation of a robot each of whose axes is driven by a servo motor controlled in a control system including a position control loop and a speed control loop, this control system for a robot detects whether the robot or an object supported by the robot touches an external object, and if contact is detected, adjusts gains of the position control loop and the speed control loop downward.
A robot system according to an aspect of embodiments includes a robot and a robot control device that controls the robot. The robot includes a plurality of links connected via a plurality of joint axes, servo motors that drive the joint axes, and a contact detection sensor that detects that one of the links touches an object. The robot control device includes a contact position determination unit, a retracting direction vector calculation unit, an assist torque calculation unit, and a flexible control unit. The contact position determination unit determines a contact position on the link with the object on the basis of an output from the contact detection sensor. The retracting direction vector calculation unit calculates a retracting direction vector in a retracting direction of the link corresponding to the contact position determined by the contact position determination unit. The assist torque calculation unit calculates assist torque references that are torque references for the servo motors for moving the links in the direction of the retracting direction vector. The flexible control unit adds the assist torque references to torque references for the servo motors to flexibly control the links.
A more complete appreciation of the invention and many of the attendant advantages thereof will be readily obtained as the same becomes better understood by reference to the following detailed description when considered in connection with the accompanying drawings, wherein:
The drawings may omit illustrations of portions not involved in the description.
As illustrated in
The robot 12 includes, for example, a moving unit 22 for moving on a floor surface, a body 24 that is provided on top of the moving unit 22 with a joint axis AXW therebetween and rotates in fore and aft directions about the joint axis AXW serving as the center of rotation, and arms 26 provided on both sides of the body 24. The robot control device 14 may be embedded in the robot 12.
As illustrated in
As illustrated in
Each of the contact sensors 30 can output on/off signals according to the state of contact with the object. For example, the contact sensor 30 can output the on signal when the object is in contact with the link 28, and can output the off signal when the object is apart from the link 28.
The contact sensors 30 are pasted side by side along the outer circumferential direction of the link 28. Specifically, the outer circumference of the link 28 is divided into eight regions, and the contact sensor 30 is pasted in each of the regions. This allows a position on the link 28 touched by the object to be identified at a resolution of 45 degrees with respect to an angular position about the longitudinal axis AXL of the link 28 illustrated in
The total number of the contact sensors 30 is not limited to eight. The contact sensors 30 need not be provided side by side along the outer circumferential direction of the link 28.
It is only necessary to divide an area for detection of contact with the object into a plurality of regions in the circumferential or longitudinal direction of the link 28, and paste the contact sensor 30 in each of the regions. As a first example, the area for detection of contact with the object may be divided into 16 regions in the circumferential direction of the link 28, and the contact sensor 30 may be provided in each of the regions. As a second example, the area for detection of contact with the object may be divided into eight in the circumferential direction and two in the longitudinal direction of the link 28, thus being divided into a total of 16 regions, and the contact sensor 30 may be provided in each of the regions.
The contact sensor 30 is not limited to the contact sensor that outputs the on/off signals. Other examples of the contact sensor include a sensor that adjusts the level of an output signal thereof according to the magnitude of the contact force (contact pressure) applied thereto.
These contact sensors 30 constitute an example of a contact detection sensor that can detect the contact of the link 28 with the object. The contact detection sensor only needs to be capable of detecting the position or an area including the position on the link 28 touched by the object.
As illustrated in
The reference generating unit 42 can generate position references Pref to the servo motors SVM for rotationally moving the links 28 of the robot 12 to move a tip of the arms 26. The generated position references Pref are entered into the flexible control unit 52.
The contact sensors 30 are connected to the sensor signal input unit 44. The sensor signal input unit 44 functions as an interface for the contact sensors 30. The sensor signal input unit 44 outputs signals corresponding to the on/off signals of the contact sensors 30. These signals are entered into the reference generating unit 42.
Based on the output signals of the sensor signal input unit 44, the contact position determination unit 46 can determine the position on the link 28 touched by the object as a contact position Pc.
The on/off signals output by the contact sensors 30 are entered into the contact position determination unit 46 via the sensor signal input unit 44. The contact position determination unit 46 detects the on signal output by any one of the eight contact sensors 30, and thus identifies the contact sensor 30 touched by the object.
After identifying the contact sensor 30 touched by the object, the contact position determination unit 46 can obtain a predetermined point corresponding to the contact sensor 30 as the contact position Pc illustrated in
The contact position Pc is predetermined, for example, as a center position of a surface of each of the contact sensors 30. The contact position Pc is not limited to the center position of the surface of the contact sensor 30, but may be any position that corresponds to the contact sensor 30.
Because the contact position Pc is predetermined, the position on the link 28 actually touched by the object may differ from the contact position Pc obtained by the contact position determination unit 46, as illustrated in
By using a coordinate transformation matrix T derived from angle information of the respective joint axes AX obtained from signals of the encoders ENC of the servo motors SVM driving the links 28 of the robot 12, the contact position Pc is calculated, for example, as a position in a robot coordinate system fixed to the body 24 of the robot 12.
The contact position determination unit 46 may be configured to detect a plurality of contact positions. That case allows the contact position determination unit 46 to output a plurality of calculated contact positions.
According to the contact position Pc, the first retracting direction vector calculation unit 48 illustrated in
The retracting direction vector n is predetermined, and is, for example, a unit vector in the normal direction of a surface of the link 28 in the contact position Pc. Therefore, the direction of the retracting direction vector n may differ from the direction of the external force fc received from the object in contact with the link 28 as illustrated in
The retracting direction vector n is calculated as a vector in the robot coordinate system by using the above-described coordinate transformation matrix T.
Based on the contact position Pc and the retracting direction vector n, the assist torque calculation unit 50 illustrated in
As illustrated in
The assist force determination unit 50a can determine the magnitude of the assist force f for retracting the links 28 (arm 26), and can output the assist force f as a scalar quantity. Changing the assist force f depending on the situation can adjust flexibility of the joint axes AX of the arm 26 included in the robot 12.
For example, the assist force determination unit 50a can output the assist force f having a larger first magnitude after the link 28 touches the object until a predetermined time tim (such as 1 to 10 milliseconds) passes, and can output the assist force f having a second magnitude smaller than the first magnitude after the time tim has passed. In this manner, after the link 28 touches the object, the assist force determination unit 50a first outputs the assist force f having the larger first magnitude so as to quickly move the link 28 in the retracting direction, thus suppressing the contact force (impact force).
When the contact sensor is the above-mentioned sensor that adjusts the level of the output signal thereof according to the magnitude of the contact force (contact pressure), the assist force determination unit 50a may determine the assist force f based on the level of this signal.
Based on the assist force f and the retracting direction vector n calculated by the first retracting direction vector calculation unit 48, the assist force vector calculation unit 50b can calculate an assist force vector Fa in the contact position Pc using the following equation.
Fa=f·n Equation (1)
The torque calculation unit 50c can obtain a Jacobian transpose JT in the calculated contact position from the calculated contact position calculated by the contact position determination unit 46, and can calculate the assist torque reference τa for the servo motor SVM driving each of the joint axes AX using the following equation. The assist torque reference τa is a torque reference for each of the joint axes AX for moving the link 28 in the direction of the retracting direction vector.
τa=JTFa Equation (2a)
The assist torque calculation unit may be an assist torque calculation unit 50x that further includes a weight calculation unit 50d between the assist force vector calculation unit 50b and the torque calculation unit 50c, as illustrated in
As represented in the following equation, the weight calculation unit 50d can multiply the assist force vector Fa output from the assist force vector calculation unit 50b of the preceding block by a weighting factor matrix k, and can output the result to the torque calculation unit 50c of the subsequent block. The weighting factor matrix k is a diagonal matrix for adjusting the magnitude of assist torque (torque for moving the link 28 in the direction of the retracting direction vector) to be generated by each of the joint axes AX of the link 28.
The weighting factor matrix k is set, for example, as follows.
As a first example, the weighting factor matrix k is set according to amounts of coefficients of viscous friction of reduction gears, etc. provided at the joint axes AX. Specifically, the weighting factor matrix k is set so as to increase the assist torque reference for the servo motor SVM driving each of the joint axes AX according to the amount of the coefficient of viscous friction of the joint axis AX. In other words, the weighting factor matrix k performs weighting so as to give the joint axis AX having a larger coefficient of viscous friction a larger magnitude of assist torque.
Setting the weighting factor matrix k as exemplified in the first example absorbs differences in the viscous friction of the joint axes AX, thus allowing the links 28 (arm 26) to perform a natural operation.
As a second example, the weighting factor matrix k performs weighting so as to give the largest magnitude of assist torque to the joint axis AX located at the root (base end) of the link 28 that touches the object. More specifically, the weighting factor matrix k performs weighting so as to give a larger magnitude of assist torque to the joint axis AX nearer to the contact position Pc on the link 28 that touches the object, among all of the joint axes AX located on the side nearer to the root (base end) of the arm 26 than the contact position Pc.
Setting the weighting factor matrix k as exemplified in the second example improves performance to absorb the external force received from the touched object because the axis nearer to the contact position Pc performs a retracting operation more quickly.
The torque calculation unit 50c of the assist torque calculation unit 50x calculates the assist torque references τa given by the following equation.
τa=k·JTFa Equation (2b)
Based on the position references Pref and the assist torque references τa, the flexible control unit 52 illustrated in
As illustrated in
Position/speed control loops (servo loops) are formed in the position/speed control unit 52a. The position/speed control unit 52a can output a torque reference τb according to a position error e between an angle feedback (encoder value) Pfb of each of the servo motors SVM obtained from the encoders ENC and the position reference Pref generated by the reference generating unit 42.
The torque limit value calculation unit 52b can obtain a torque limit value Tlim.
The torque limit value calculation unit 52b can obtain the torque limit value (upper or lower limit value) Tlim, for example, so as to include therebetween torque of the servo motor SVM required for motion of the links 28 (torque for accelerating both the links 28 and a tip load of the arm 26, and torque for maintaining movement velocities of the links 28).
As another example, the torque limit value calculation unit 52b can obtain a predetermined maximum torque value of the servo motor SVM as the torque limit value Tlim.
The torque limiting unit 52c can limit the torque reference τb output by the position/speed control unit 52a with the torque limit value Tlim calculated by the torque limit value calculation unit 52b, and thus can output a limited torque referenceτlim.
The gravity compensation torque calculation unit 52d can calculate torque by gravity of each of the joint axes AX as gravity compensation torque τg by calculation of dynamics based on the encoder value Pfb of each of the servo motors SVM.
The calculated gravity compensation torque τg is added together with the assist torque reference τa to the torque reference τlim output by the torque limiting unit 52c. This keeps the links 28 from dropping and the torque required for moving the links 28 from being insufficient due to the limitation of the torque reference by the torque limiting unit 52c.
A description will be made below of the operation of the robot system 10 separately for a case in which the arm 26 is not in contact with the object, and for another case in which the arm 26 is in contact with the object.
(1) Case in Which Arm 26 is Not in Contact With Object
The position/speed control unit 52a of the flexible control unit 52 outputs the torque reference τb based on the position reference Pref generated by the reference generating unit 42. If the torque reference τb exceeds the torque limit value Tlim, the torque limiting unit 52c limits the magnitude of the torque reference and outputs it as the torque reference τlim (refer to
The assist torque calculation unit 50 does not output the assist torque reference τa because each of the contact sensors 30 does not output the on signal. In other words, the magnitude of the assist torque reference τa is zero.
The gravity compensation torque calculation unit 52d outputs the gravity compensation torque τg based on the encoder value Pfb.
Accordingly, the gravity compensation torque τg calculated by the gravity compensation torque calculation unit 52d is added to the torque reference τlim, and the torque reference τref is output to the servo amplifier SVA (refer to
As a result, each of the servo motors SVM is driven according to the torque reference τref, and the arm 26 of the robot 12 operates.
The addition of the gravity compensation torque τg to the torque reference τref keeps the arm 26 from dropping by its own weight.
(2) Case in Which Arm 26 is in Contact With Object
The operation of the robot system 10 differs depending on whether the arm 26 is retracted from the touched object or moves following the external force fc received from the touched object. The operation will be described below for separate cases.
(2a) Case in Which Arm 26 is Retracted From Touched Object
Based on the signal output by the sensor signal input unit 44, the reference generating unit 42 stops outputting the position reference Pref.
The position/speed control unit 52a of the flexible control unit 52 outputs the torque reference τb according to the position error e.
The torque limit value calculation unit 52b determines that the output of the position reference Pref has been stopped, and sets the torque limit value Tlim to zero. It follows that the torque limiting unit 52c sets the torque reference τlim to zero regardless of the torque reference τb.
The assist torque calculation unit 50 outputs the assist torque reference τa according to the contact position Pc.
The gravity compensation torque calculation unit 52d outputs the gravity compensation torque τg based on the encoder value Pfb.
Accordingly, while the torque reference τlim output by the torque limiting unit 52c is set to zero, the assist torque reference τa and the gravity compensation torque τg are added to the torque reference τlim at the block subsequent to the torque limiting unit 52c, and the result is output as the torque reference τref.
As a result, each of the servo motors SVM is driven according to the torque reference τref, and the links 28 are retracted by the assist torque from the touched object.
The addition of the gravity compensation torque τg to the torque reference τref keeps the arm 26 from dropping by its own weight.
(2b) Case in Which Arm 26 Moves Following the External Force fc Received From Touched Object
The reference generating unit 42 continues outputting the position reference Pref.
The position/speed control unit 52a of the flexible control unit 52 outputs the torque reference τb based on the position reference Pref generated by the reference generating unit 42. If the torque reference τb output by the position/speed control unit 52a exceeds the torque limit value Tlim, the torque limiting unit 52c limits the magnitude of the torque reference and outputs it as the torque reference τlim.
The assist torque calculation unit 50 outputs the assist torque reference τa according to the contact position Pc.
The gravity compensation torque calculation unit 52d outputs the gravity compensation torque τg based on the encoder value Pfb.
Accordingly, the assist torque reference τa and the gravity compensation torque τg are added to the torque reference τlim output by the torque limiting unit 52c, and the torque reference τref is output.
This results in driving of each of the servo motors SVM according to the torque reference τref. The limitation of the magnitude of the torque reference τb by the torque limiting unit 52c allows the links 28 (arm 26) to follow the external force fc received from the touched object. At the same time, the continuation of the output of the position reference Pref causes the joint axes AX of the arm 26 to try to continue the movement toward a target position as much as possible while following the external force fc.
The addition of the assist torque reference τa to the torque reference τref causes the joint axes AX of the arm 26 to flexibly operate in the direction of following the external force fc.
The further addition of the gravity compensation torque τg to the torque reference τref keeps the links 28 from dropping by their own weight.
As described above, the addition of the assist torque allows the robot system 10 according to the present embodiment to flexibly operate the joint axes AX of the arm 26. In addition, an impact applied to the object touching the link 28 can be suppressed.
Subsequently, a description will be made of a robot system according to a second embodiment. The same components as those of the robot system 10 according to the first embodiment may be given the same symbols, and detailed description thereof may be omitted.
As illustrated in
As illustrated in
According to the contact position Pc, the contact portion normal vector calculation unit 148a can calculate the direction of movement of the link 28 as the retracting direction vector n. The retracting direction vector n is predetermined, and is, for example, a unit vector in the normal direction of the surface of the link 28 in the contact position.
From the angles of the joint axes AX calculated based on the encoder values Pfb of the servo motors SVM and the contact position Pc obtained by the contact position determination unit 46, the contact point movement vector calculation unit 148b can calculate a vector representing the direction of actual movement of the contact position Pc as the movement vector m.
The retracting direction vector correction unit 148c can calculate a difference vector vd between the retracting direction vector n calculated by the contact portion normal vector calculation unit 148a and the movement vector m calculated by the contact point movement vector calculation unit 148b.
The retracting direction vector correction unit 148c can further calculate a vector by combining the difference vector vd with the retracting direction vector n as a new retracting direction vector nc.
In other words, based on the encoder values Pfb of the servo motors SVM, the second retracting direction vector calculation unit 148 can correct the retracting direction vector n calculated by the contact portion normal vector calculation unit 148a to calculate the corrected retracting direction vector nc so as to be able to move the link 28 in the direction of the predetermined retracting direction vector n, as illustrated in
The robot system according to the present embodiment can move the link 28 in the predetermined retracting direction more accurately than in the case of not having the configuration of the present embodiment.
The present invention is not limited to the above-described embodiments, and can be modified within the scope of not changing the gist of the present invention. For example, the technical scope of the present invention includes a case of constituting the invention by combining some or all of the embodiments and the modification described above.
The robot is not limited to a robot that includes the moving unit, the body provided on top of the moving unit, and the arms provided on both sides of the body. The robot may be an industrial robot that includes, for example, links connected by joint axes.
The present invention is applied not only to the arms 26 exemplified in the above-described embodiments. The present invention can also be applied with respect to, for example, the joint axis AXW between the body 24 and the moving unit 22.
Additional advantages and modifications will readily occur to those skilled in the art. Therefore, the invention in its broader aspects is not limited to the specific details and representative embodiments shown and described herein. Accordingly, various modifications may be made without departing from the spirit or scope of the general inventive concept as defined by the appended claims and their equivalents.
This application is a continuation of International Application No. PCT/JP2011/068794, filed on Aug. 19, 2011, the entire contents of which are incorporated herein by reference.
| Number | Date | Country | |
|---|---|---|---|
| Parent | PCT/JP2011/068794 | Aug 2011 | US |
| Child | 14181729 | US |
