JP3585419B2 - Master arm device - Google Patents

Description

[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a master arm having seven degrees of freedom used for a master-slave manipulator, and more particularly, to a seven-degree-of-freedom master arm capable of controlling the position of an elbow of a slave arm.
[0002]
[Prior art]
In the manipulator having six degrees of freedom, when the position and posture of the hand are determined, all degrees of freedom are defined, and it is impossible to change the state of the arm between the shoulder and the hand. For this reason, obstacle avoidance and singularity avoidance cannot be performed in a state where the position and posture of the hand are determined.
On the other hand, the human arm has seven degrees of freedom, so that the position of the elbow can be arbitrarily selected even if the position and posture of the hand is determined, so that the arm does not hit even if there is an obstacle. Can be avoided.
Therefore, by giving the slave arm of the master-slave manipulator seven degrees of freedom, it is possible to make use of redundant degrees of freedom to make it move in the same way as a human arm, and to avoid singularities and obstacles. can do.
[0003]
The master arm in the master-slave manipulator is used to operate the slave arm.When the operator moves the gripper at the tip of the master arm, the movement is transmitted as a command value and the slave arm performs the same movement. Can be.
Conventionally, the master arm teaches six degrees of freedom of the position and posture of the hand held by the operator. To operate a seven-degree-of-freedom robot having redundant degrees of freedom, the other arm is used. It is necessary to give an instruction regarding the seventh degree of freedom using another means such as a push button or a lever operated by the user.
[0004]
Note that Japanese Patent Application Laid-Open No. 5-345291 discloses that the elbow position of the slave arm can be freely selected when the position of the hand is controlled by the master arm. A seven-degree-of-freedom robot has been disclosed in which interference avoidance is performed when there is a possibility of interference.
However, in the seven-degree-of-freedom robot disclosed in the above publication, the controller of the slave arm determines the elbow position independently of the master arm, and does not reflect the intention of the operator in the redundant degree of freedom. Was.
[0005]
[Problems to be solved by the invention]
Therefore, the problem to be solved by the present invention is to provide a master arm capable of reflecting not only the position and orientation of the operator's hand but also the movement of the elbow, and all degrees of freedom of the seven-degree-of-freedom slave arm in the master-slave manipulator. Is to be controlled by the operator with only one arm.
[0006]
[Means for Solving the Problems]
In order to solve the above-mentioned problem, a master arm device of the present invention is a seven-degree-of-freedom master arm having an elbow portion where three axes at a shoulder intersect at almost one point and three axes at a wrist intersect at almost one point. The elbow is provided with an elbow sensor for detecting the position of the elbow of the pilot, and a driving device for driving the elbow is provided to control the following of the elbow.
[0007]
In the master arm device of the present invention, since the three axes intersect at one point in the shoulder and the three axes intersect at one point in the wrist, each has a joint function equivalent to a ball joint. The movement of the elbow to be carried can be represented by rotation about a straight line connecting two axis intersections of a plane triangle formed by the axis intersection of the shoulder, the axis intersection of the wrist, and the elbow. The angle of rotation of this plane triangle is called the elbow angle.
In the present invention, the elbow of the master arm is provided with a sensor for detecting the movement of the elbow of the pilot, and a device for driving the elbow is provided, and the elbow of the master arm is moved following the movement of the pilot. Can be done. Therefore, when the slave arm is likely to interfere with the external environment, if the elbow is moved so as not to collide according to the judgment of the pilot, the master arm device controls the slave manipulator using the position information of the elbow to avoid the interference. Can be.
[0008]
In the master arm device of the present invention, since all the degrees of freedom of the seven-degree-of-freedom manipulator can be controlled with only one arm of the operator, the operator is required to operate the manipulator like a dual-arm robot, which has recently been researched. When the two arms need to control the manipulator arms, the position of the elbow can be controlled in addition to the position and orientation of the hand.
Also, for example, even when the forearm of the manipulator is likely to interfere with the work table on which the work object is mounted, the elbow can be moved in the horizontal direction, so that the degree of freedom in the arrangement of the robot increases, and work can be performed even in a narrow space. There are advantages such as being able to.
[0009]
In addition, each axis of the master arm may be used as a drive axis, and the force detected by the slave arm may be presented to the operator.
Further, the direction of the rotation axis of the first axis of the master arm is made substantially coincident with the direction of gravity, the second axis is a drive axis that is orthogonal to the first axis and follows the movement of the elbow, and the rotation of the third axis and the fourth axis The axes may be parallel to each other, and the counterweight of the fourth axis may be arranged on the rotation axis of the third axis, and the counterweight may be linked to the fourth axis via a parallel link mechanism.
Preferably, the elbow sensor is provided on the link of the master arm, and detects the direction in which the driver's arm moves relative to the link.
Note that the elbow sensor may be one in which a plurality of photoelectric switches are arranged.
With such a simple configuration of the elbow sensor, the control algorithm can be simplified, and the sensor unit and the control unit can be configured at extremely low cost.
[0010]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, the present invention will be described in detail based on embodiments with reference to the drawings.
FIG. 1 is a shaft arrangement diagram for explaining the configuration of one embodiment of a master arm device of the present invention, FIG. 2 is an explanatory diagram showing one example of an elbow sensor used in this embodiment, and FIG. 3 is another example of an elbow sensor. FIG. 4 is a shaft arrangement diagram for explaining an arrangement example of a counterbalance used in the present embodiment, FIG. 5 is a configuration diagram of a fourth shaft counterbalance, and FIG. 6 is a side view of a master arm device of the present embodiment. 7, FIG. 7 is a plan view of the master arm device of the present embodiment, FIG. 8 is a drawing showing the attitude of the operator in the master arm device of the present embodiment, and FIG. 9 is a control device of the master-slave manipulator device using the present embodiment. It is a block diagram showing.
[0011]
The master arm device of the present embodiment is for manipulating a seven-degree-of-freedom slave arm of a master-slave manipulator. The master arm has a link mechanism with seven degrees of freedom.
When the operator changes the position and orientation of the wrist by gripping the wrist of the master arm, the position and orientation of the wrist can be determined by the rotation angle of the axis provided from the shoulder to the wrist, and the information is used as a manipulator. To control the slave arm to follow the movement of the master arm.
Since only six degrees of freedom can be defined only by the position and orientation of the wrist of the slave arm, the remaining one degree of freedom is determined by the inclination of the arm connected to the elbow.
[0012]
The shoulder and wrist of a person can be approximated to a spherical joint such as a ball joint where three axes of rotation intersect at one point, and the position of the elbow connected by the upper arm and forearm to the shoulder and wrist is Once the position of the wrist is determined, it can be designated by a rotation angle around a line connecting the shoulder and the wrist, that is, an elbow angle. It is convenient for handling that the elbow angle is indicated by a rotation angle measured from an appropriate reference position such as a vertical plane.
In the master arm device of this embodiment, three rotation axes are provided on the shoulder so that the axes intersect at one point, and three wrists are provided on the wrist in order to make the master arm perform the same movement as a human arm. The two rotation axes are provided so that the axes intersect at one point, and the upper arm link attached to the shoulder and the forearm link attached to the wrist are joined by a rotation axis such that both links bend in a plane to form an elbow I have.
[0013]
The seven rotation axes of the master arm in the present embodiment are arranged as shown in FIG.
The first rotation axis 1 of the shoulder has an axis in the direction of gravity and is supported by a fixed member. Further, a second rotating shaft 2 is connected to a rotating portion of the first rotating shaft 1 via a first link. The second rotation axis 2 has an axis in the horizontal direction, and the axis intersects the axis of the first rotation axis 1.
The third rotating shaft 3 is connected to the rotating portion of the second rotating shaft 2 via the second link 8. The axis of the third rotating shaft 3 is substantially orthogonal to the axis of the second rotating shaft 2, and is further arranged to pass through the intersection of the first rotating shaft 1 and the second rotating shaft 2.
[0014]
With the above configuration, the shoulder of the master arm can perform a movement equivalent to a ball joint about the intersection of the rotation axes. Needless to say, the arrangement of the first to third rotating shafts may be loose enough to achieve mechanical performance.
Further, the second link 8 has a slight length so that the operator's shoulder can be accommodated near the shoulder of the master arm.
[0015]
A third link 9 serving as an upper arm is attached to a rotating portion of the third rotating shaft 3, and a fourth rotating shaft 4 is provided at a tip of the third link 9. The axis of the fourth rotation shaft 4 is provided in parallel with the axis of the third rotation shaft 3.
A fourth link 10 serving as a forearm is attached to the rotating portion of the fourth rotating shaft 4, and a fifth rotating shaft 5, which is the first rotating shaft in the wrist, is provided at the tip thereof in parallel with the fourth rotating shaft 4. Is provided.
[0016]
A sixth rotation shaft 6 is connected to the fifth rotation shaft 5 via a fifth link, and a seventh rotation shaft 7 is connected to the sixth rotation shaft 6. The fifth rotation shaft 5, the sixth rotation shaft 6, and the seventh rotation shaft 7 are disposed so as to intersect at substantially one point, forming a three-degree-of-freedom joint of the wrist.
[0017]
In the master arm device of the present embodiment, an elbow sensor 11 for detecting the movement of the operator's elbow is attached to the fourth link 10 corresponding to the forearm.
Since the elbow sensor 11 only needs to detect the direction in which the forearm 31 of the operator 30 moves relative to the fourth link 10, the elbow sensor 11 may be, for example, a plurality of photoelectric switches as shown in FIG. In the figure, four photoelectric switches are used, and these are arranged vertically to the movement of the arm. When the arm 31 is at an appropriate position, the two inner photoelectric switches detect the arm, but when the arm 31 moves in the direction approaching the fourth link 10, the photoelectric switch on the left side in the figure outputs a detection signal. Conversely, when the arm 31 moves in a direction away from the fourth link, the photoelectric switch on the right side in the figure operates to detect the movement.
The elbow sensor 11 may be mounted anywhere on the fourth link 10, but is preferably determined in consideration of detection sensitivity and the like.
[0018]
FIG. 3 is a drawing showing another example of an elbow sensor used in the present invention.
The elbow sensor shown in FIG. 3 uses a micro linear scale. The linear scale body 12 is fixed to the fourth link 10 of the master arm, and the end of the scale is tied to the band 13 wound around the forearm 31 of the pilot 30. Thus, the distance between the fourth link 10 and the forearm 31 of the operator is measured. By adjusting the distance between them to be constant, the master arm can follow the operator's arm.
[0019]
A drive motor 14 that rotates in response to the measurement signal of the elbow sensor 11 is attached to the second rotation shaft 2.
When the operator's elbow is lifted outward, the drive motor 14 extends the upper arm link 9 outward, and when the operator's elbow is lowered, the upper arm link 9 is also contracted inward. Therefore, the follow-up control of the elbow can be performed by moving the master arm so that the operator's elbow is always located at the center of the photoelectric switch group of the elbow sensor 11.
[0020]
The master arm device is provided with a pair of left and right master arms having the above configuration, and can operate a dual-arm robot. When manipulating the slave arm by the master arm device of the present embodiment, the operator gets on the device and grasps the joysticks at the wrists of the left and right master arms to guide the position and orientation of the wrist as desired.
[0021]
Then, the master arm device calculates the position and orientation of the wrist by detecting the rotation angle with encoders provided on each rotation axis of the master arm, and transmits the calculated position and posture to the control device of the slave arm. This makes it possible to transmit information of six degrees of freedom of the movement of the operator's hand and one degree of freedom of the elbow position to the slave arm.
The position and orientation of the slave arm is instructed so that the difference between the operating force of the operator operating the master arm and the reaction force received by the slave arm from the work is eliminated. It may not work.
[0022]
The slave arm control device performs inverse kinematics calculation based on the information on the position and orientation of the wrist of the master arm and the elbow angle, calculates the amount of rotation of each axis of the slave arm, and performs servo control. Thus, the position and orientation of the wrist of the slave arm can be determined as instructed by the operator.
Furthermore, since the movement of the pilot's elbow is detected and the elbow of the master arm is made to follow, when the arm or elbow of the slave arm is likely to interfere with surrounding objects, if the pilot moves the elbow, The elbow of the slave arm follows and moves through the movement of the master arm, so that interference can be avoided.
[0023]
In order to make the movement of the elbow of the master arm correspond to the elbow of the pilot, the elbow angle may be set to the same value.
In the master arm device according to the present embodiment, only the second rotation shaft 2 of the master arm actively moves to adjust the opening angle of the upper arm link 9 in response to the movement of the elbow of the operator. Since the reference axis of the elbow angle changes according to the movement of the hand, the opening angle of the upper arm link 9 does not necessarily become the elbow angle. However, the difference between the two can be absorbed by another passive rotation axis following the operator's arm. The second rotation axis 2 and the other rotation axes change according to the movement of the driver's arm, and based on the rotation angles of the respective rotation axes obtained as a result of the predetermined state, the inverse kinematics calculation is performed and the slave arm Controlling each joint causes the elbow to move according to the operator's intention.
[0024]
Since the link mechanism of the master arm is ruggedly configured to have an appropriate strength, it becomes heavy for the operator to operate as it is, and it is difficult to freely move the link mechanism.
In order to solve this difficulty, the master arm device of the present embodiment can arrange a counter balance as shown in FIG. Note that, for the portion nearer to the end than the fourth link 10, the respective components are smaller and the role of the counterbalance is relatively smaller, so the display in FIG. 4 is omitted.
The counter balance may be attached to each rotating shaft. However, since the counter balance increases the weight of the entire mechanism and increases the inertial force, it is not preferable to use many heavy counterweights.
[0025]
In a certain shaft configuration, if the counterweights are arranged on all the rotating shafts, the counterweight to be provided on the first rotating shaft, which must receive all the weights from the base of the arm to the hand, becomes the largest. In this embodiment, the counterweight is omitted by arranging the first rotary shaft 1 in the direction of gravity. Also, a drive motor 14 for driving the upper arm according to the elbow angle is attached to the second rotating shaft 2 where the weight of the counterweight should be the second largest, and the counterweight is omitted.
The third rotating shaft 3 is provided with a first counterweight 15 for appropriately canceling the moment on the hand side of the third link 9.
[0026]
Next, the third rotating shaft 3 and the fourth rotating shaft 4 are arranged so that the rotating shafts are parallel to each other, and a parallel link 17 is formed between the third rotating shaft 3 and the fourth rotating shaft 4 to form a third link. The second counterweight 16 for the four rotation shafts 4 is disposed around the third rotation shaft 3.
By arranging the second counterweight 16 around the third rotation axis, the moment around the third axis due to the weight of the second counterweight is reduced, and the weight of the first counterweight to be balanced with it can be reduced. Therefore, the load applied to the arm of the pilot 30 is significantly reduced as compared with the case where the operation is performed symmetrically with respect to the axis of the fourth rotation shaft 4 so as to be normally performed, so that the steering is more easily performed. Become.
[0027]
FIG. 5 is a mechanism diagram illustrating an example of a parallel link installed to provide a counter weight of the fourth rotating shaft on the third rotating shaft.
A first pulley is fixed to the rotation shaft of the fourth rotation shaft 4, and a second pulley having the same diameter as the first pulley is rotatably supported on the third rotation shaft 3. A second counterweight is fixed to the second pulley. Metal bands 18 and 19 are passed between the first pulley and the second pulley, and each end is fixed to the pulley by a fixing bolt 21. A tension controller 20 is set between the first pulley and the second pulley so that the metal belts 18 and 19 do not slack.
[0028]
When the operator moves the hand holding the joystick 22 provided on the wrist and the fourth link 10 rotates around the fourth rotation axis, this rotation is transmitted to the second pulley via the metal belts 18 and 19. Then, the second counter weight 16 moves. The second counterweight 16 mainly cancels the moment of the link mechanism from the fourth link 10 to the hand. Further, since the weight of the first counterweight 15 is reduced by disposing the second counterweight 16 near the third axis, the burden on the operator's arm is small.
In addition, the illustrated parallel link mechanism has a small number of additional components and can be formed so as to fit within the width of the third link 9, so that the interference area between the operator and the mechanism is reduced. can do.
[0029]
6 and 7 are an elevation view and a plan view of the master arm device of the present embodiment.
The first to third axes intersect at the center of rotation of the shoulder, and the fifth to seventh axes intersect at the hand position. The fourth shaft rotatably connects the upper arm link 9 and the forearm link 10. The third to fifth axes are arranged in parallel with each other. The rotating portion of the third shaft projects outward from the center of rotation of the shoulder by the distance of the shoulder link 8. In addition, the distance from the rotating portion of the fifth shaft to the rotation center of the wrist portion is configured to be the same as the shoulder link 8 by a mechanism interposed therebetween.
A drive motor 14 is coupled to the second shaft to adjust the opening angle of the elbow of the link 9.
[0030]
A first counterweight 15 and a second counterweight 16 are mounted around the third axis.
The first counterweight 15 is provided at a point symmetrical position with respect to the third arm so as to substantially cancel the moment of the entire hand portion below the upper arm link 9. The second counterweight 16 has a function of interlocking with the forearm link 10 by means of a parallel link mechanism provided in the upper arm link 9 to substantially cancel the moment of the mechanical portion below the forearm link 10.
The forearm link 10 is provided with an elbow sensor 11 for detecting the movement of the operator's arm.
[0031]
FIG. 8 schematically shows a state in which a pilot operates and rides on the master arm device of the present embodiment shown in FIGS. 6 and 7. The master arm device is fixed by fixing the fixing portion of the first rotation shaft to the gantry 23.
When the pilot 30 gets on the device and grips the joystick of the master arm, the center of rotation of the pilot's shoulder and the center of rotation of the master arm are sufficiently close, and the upper arm link and the forearm link are connected to the operator's arm 31. The movement of the operator's arm 31 is detected by the elbow sensor 11.
When the pilot moves the elbow without changing the position of the hand, the straight line connecting the shoulder rotation center and the wrist rotation center almost coincides with the straight line connecting the pilot's shoulder and wrist. The correspondence is simplified.
[0032]
Further, it is also possible to arrange a motor on each of the first axis to the seventh axis so that the force can be presented. The force presentation is a function of presenting a force generated when a slave arm is equipped with a force sensor and coming into contact with a workpiece to a driver. In addition, a force sensor is provided on a grip portion at a wrist portion of the master arm to detect an operation force exerted by the operator's hand on the master arm and generate a force according to the operation force between the slave arm and the work. You can also.
[0033]
FIG. 9 is a block diagram illustrating a control mechanism of a master-slave manipulator device using the master arm device of the present embodiment. The upper half of the drawing shows the drive control unit of the master arm, and the lower half shows the drive control unit of the slave arm.
The elbow angle command value is calculated by the elbow sensor 41, the direction calculation unit 42 of the operator's elbow position, the current position calculation unit 43 of the elbow angle, and the adder 44.
The elbow sensor 41 detects a change in the relative position between the arm of the pilot and the arm link of the master arm, and inputs a detection signal to the direction calculation unit 42. The direction calculation unit 42 determines the movement direction of the elbow position of the pilot. Calculate the amount of movement required for the elbow of the master arm to follow. The elbow angle command value is calculated by integrating the movement amount by the integrator 44.
[0034]
The elbow angle command value is sent to the inverse kinematics calculation unit 45 of the master arm, where the amount of rotation of each axis is calculated, and the servo motor is driven by the servo control unit 46 based on the calculated amount, and the master arm is operated by the operator. Follow your arm movements.
The elbow angle command value is also sent to the inverse kinematics calculation unit 52 of the slave arm, and drives each axis of the slave arm via each axis servo control unit 53 based on the calculated drive amount. To follow the pilot's arm.
[0035]
On the other hand, a force sensor 47 provided on the master arm measures the operating force of the driver, and a force sensor 48 provided on the slave arm measures the reaction force of the work. The subtractor 49 calculates the difference between the operation force of the driver and the reaction force of the work, and the integrator 50 integrates the difference. The arm position initial value of the slave arm is calculated from the rotation value of each axis, and the calculated value is added to the output of the integrator 50 by the adder 51. The output of the adder 51 is a hand position command value that changes the hand position in a direction in which the operator's force matches the work reaction force, and when the two match, the hand position is maintained.
The hand position command value is sent to the inverse kinematics calculation unit 52 of the slave arm, and each axis servo control unit 53 drives each axis of the slave arm based on the calculated drive amount, and controls the slave arm to move the arm of the operator. To follow. Further, it is sent to the inverse kinematics calculation unit 45 of the master arm, and based on it, the drive motor is driven by the servo control unit 46 so that the master arm follows the slave arm and presents force to the operator.
[0036]
【The invention's effect】
As described above, the master arm device of the present invention can teach not only the position of the hand but also the position of the elbow to the slave arm having seven degrees of freedom. With respect to the master-slave manipulator device of the above, the operator can move the elbow while observing the situation at the site to avoid obstacles of the slave arm.
[Brief description of the drawings]
FIG. 1 is a shaft arrangement diagram for explaining a configuration of an embodiment of a master arm device of the present invention.
FIG. 2 is an explanatory diagram illustrating an example of an elbow sensor used in the present embodiment.
FIG. 3 is an explanatory diagram showing another example of the elbow sensor used in the present embodiment.
FIG. 4 is an axial arrangement diagram illustrating an arrangement of a counter balance used in the present embodiment.
FIG. 5 is a configuration diagram of a fourth axis counterbalance used in the present embodiment.
FIG. 6 is a side view of the master arm device of the present embodiment.
FIG. 7 is a plan view of the master arm device of the present embodiment.
FIG. 8 is a drawing showing a posture of a pilot in the master arm device of the present embodiment.
FIG. 9 is a block diagram illustrating a control device of a master-slave manipulator device using the present embodiment.
[Explanation of symbols]
1, 2, 3, 4, 5, 6, 7 Rotating shaft 8, 9, 10 Link 11 Elbow sensor 12 Linear scale body 13 Linear scale band 14 Drive motor 15, 16 Counter weight 17 Parallel link 18, 19 Metal belt 20 Tension controller 21 Fixing bolt 22 Joystick 23 Mount 30 Pilot 31 Pilot's forearm

Claims (5)

An elbow for detecting the position of the pilot's elbow with respect to the sensor at the elbow in a seven-degree-of-freedom master arm having three elbows where the three axes at the shoulder intersect at approximately one point and the three axes at the wrist intersect at approximately one point a sensor, provided with a driving device for driving the elbow on the basis of the該肘angle command value to calculate the elbow angle command value using the detection output of該肘sensor, the elbow and the operator of the elbow relative A master that drives the elbow so that the elbow is not moved or that the distance between the elbow and the elbow is constant so that the elbow follows the position of the elbow of the pilot Arm device. 2. The master arm device according to claim 1, wherein a drive device is provided on a rotation axis corresponding to each degree of freedom of the master arm to perform force presentation. Of the three axes intersecting at one point on the shoulder of the master arm, the direction of the rotation axis of the first axis fixed to the shoulder is made substantially coincident with the direction of gravity, and is connected to the first axis via the first link. The second axis is a drive axis orthogonal to the first axis and following the movement of the elbow of the operator, and a third axis of rotation connected to the second axis via a second link and the third axis. If in the parallel rotation axes of the fourth shaft which is connected via a third link, the counterweight of the fourth axis is arranged near the rotational axis of the third shaft, via a parallel link mechanism the counterweight The master arm device according to claim 1, wherein the master arm device is linked with the fourth shaft. 4. The elbow sensor according to claim 1, wherein the elbow sensor is provided on a link of the master arm, and detects a direction in which a driver's arm moves relative to the link. Master arm device. The master arm device according to claim 4, wherein the elbow sensor is configured by arranging a plurality of photoelectric switches in parallel.

Images