JPH0445304B2 - - Google Patents

Description

Detailed Description of the Invention [Industrial Application Field] The present invention relates to a manipulator used for remote control in various industrial fields, and in particular to a manipulator that has a force sensation transmission function between a master arm and a slave arm. The present invention relates to a master-slave manipulator with bilateral control system.

[Prior Art] As conventional bilateral control type master-slave manipulators, three types are known as shown in FIG. It is being (“Introduction to Robot Industry” by Eiji Nakano (Publisher: Ohmsha, Publication date:
(September 10, 1981, pages 74-76). In FIG. 13, M indicates the joint axis of the master arm, and S indicates the joint axis of the slave arm.

_{First ,} in the _symmetrical form shown in FIG _. 2, and torque is applied to the slave side in the direction of making θ ₂ match θ ₁ , and on the master side, θ ₁ becomes θ ₂
When it is ahead of θ 1, it suppresses θ ₁ and generates a torque on the master side proportional to the deviation so that θ ₂ matches θ _1. Since no special torque detection device is required, it is the most This is a simple method.

However, this symmetrical design has the problem that it is not suitable for heavy-load manipulators because deviations cannot be accurately extracted unless the manipulator is relatively small and has low friction.

Next _, in the _force reverse feed type shown in FIG. 3 and transmits it to the master side via the torque transmitter 4, and can be applied as is to large manipulators.

However, with this type, the inertia force of the slave is directly detected and applied to the master side, for example at the start of an operation or a quick operation, so if the slave side is heavy, the operation on the master side becomes particularly difficult and difficult. The drawback is that it is difficult.

The force feedback type shown in Fig. 13C eliminates this drawback. In this, the torque on the master side and the slave side are detected by the detector 3, and the deviation T ₁ - T ₂ of both torques is calculated as the torque It is added to the master side via the adjuster 5 in the direction of decreasing the size. Therefore, for example, at the start of operation, an inertia force corresponding to the weight of the master acts on the slave side as well as on the master side, and this cancels the inertia force on the slave side, making operation easier. There is.

As described above, according to the conventional technology, the control of a master-slave manipulator having a force sensation transmission function is selected from the above methods depending on the size of the manipulator, the length of the transmission system, etc. In terms of transmitting the force sensation, the force feedback type shown in FIG. 13C is considered to be the most preferable.

[Problems to be Solved by the Invention] However, in the conventional master-slave manipulator as described above, even in the force feedback type shown in FIG. 13C, the joint axes of the master and slave do not correspond. They are positioned and move synchronously, and a force (signal) detected on a certain axis on the slave side is transmitted to the corresponding axis on the master side at a constant ratio. Therefore, all external forces and disturbances applied to the slave side act on the master side as they are. For this reason, for example, if the slave arm is subjected to an unexpected impact force while the manipulator is operating, there is a problem in that the impact force is transmitted to the master side and is transmitted to the operator as an unpleasant operating feeling.
Furthermore, if you want the slave arm to perform advanced operations such as quickly avoiding obstacles or inserting precision parts, it is extremely difficult to perform these operations with instructions from the master that involve minute movements. It was difficult.

[Means for Solving the Problems] According to the first invention, in the master-slave manipulator having a force sensation transmission function between the master arm and the slave arm, each of the master arm and the slave arm has a a force detector that detects applied force; and a force detector configured to connect the master arm and slave arm so that the force detected by the force detector is input and the displacement of the master arm and slave arm in response to the input is output. a virtual mechanical model calculating section that performs calculations according to a virtual mechanical model that defines force transfer characteristics between the two; and a servo control system that controls the master arm and the slave arm using an output from the virtual mechanical model calculating section as a command value. A master-slave manipulator is provided.

According to a second aspect of the invention, in the master-slave manipulator having a force sensation transmission function between the master arm and the slave arm, there is a force detection method for detecting the force applied to each of the master arm and the slave arm. between the master arm and the slave arm so that the force applied to the master arm and the force applied to the slave arm are input, and the displacement of the master arm according to the input is output. a master virtual mechanics model calculation section that performs calculations according to a master virtual mechanics model that defines force transmission characteristics; outputs the force applied to the slave arm and displacement from the master virtual mechanics model calculation section; a slave virtual mechanics model computing section that computes according to a predetermined slave virtual mechanics model such that the displacement of the slave arm according to the response is output; A master-slave manipulator is provided that includes a servo control system that controls the master arm and slave arm using an output as a command value.

Furthermore, according to a third aspect of the invention, in the master-slave manipulator having a force sensation transmission function between the master arm and the slave arm, a force detection unit detects the force applied to each of the master arm and the slave arm. and a force between the master arm and the slave arm such that the force detected by the force detector is input, and the position, velocity, and acceleration of the master arm and slave arm according to the input are output. a virtual mechanical model calculating section that calculates according to a virtual mechanical model that defines the transfer characteristics of the master arm and the slave arm; a position detector that detects the positions of each of the master arm and the slave arm; and a position detector that detects the speed of each of the master arm and the slave arm. A master-slave manipulator comprising a speed detector for detecting speed, and a servo control system for controlling the master arm and slave arm based on outputs from the virtual dynamic model calculation unit, the position detector, and the speed detector. is provided.

[Operation] In the first, second, and third inventions described above, the force applied to the master arm is detected by the force detector and input to the virtual dynamic model calculation section. This virtual dynamic model calculation unit stores the position and force transmission characteristics from the master arm to the slave arm in advance, calculates the position output according to the input according to the characteristics, and calculates the position output according to the input as a displacement. Give it to the control system. Further, the force applied to the slave arm is also detected by the force detector, and is transmitted to the master arm side by being input to the virtual dynamic model calculation section.

[Example] The description will be given below based on the drawings.

FIG. 1 is a diagram showing the basic concept of the present invention. In the figure, 11 is a position detector that detects the axial positions θ _M and θ _S of master arm M and slave arm S, respectively, and 12 is a position detector that detects the axial positions θ M and θ S of master arm _M and slave arm _S , respectively. 13 is a virtual dynamic model calculation unit that performs calculations based on a virtual dynamic model, which will be described later. 14 is a servo control system that controls the master arm and slave arm. Note that as the position detector, for example, an encoder is used, and as the force detector, for example, a strain gauge is used.

The virtual mechanical model calculation unit 13 stores T _M and T _S detected by the master and slave force detectors 12.
are input, and displacements x _M and x _S of the master and slave arms according to these inputs are output.
The displacement signals x _M and x _S are input to the servo control system 14 along with the corresponding outputs θ _M and θ _S from the position detector 11, respectively, and in each servo control system 14, these inputs x _M and θ Based on _M , x _S and θ _S, signals are output to drive the master arm and slave arm, respectively.

The servo control system 14 is a means for controlling the movements of the master and slave tips to follow a virtual dynamic model, and is configured to perform, for example, the calculation expressed by the following equation. In addition, commands from the virtual mechanical model calculation unit 13 to the servo control system 14, the position or displacement of each arm
In addition to x _M and x _S , it also includes velocity and acceleration as necessary, and each arm has a virtual dynamic model calculation unit 13
The servo control system 14 is configured to follow the commands from the controller without delay.

Here, a configuration example of the virtual dynamic model and the servo control system 14 in the present invention will be explained.

First, consider M _V x ¨ _V + D _V x ¨ _V + K _V x _V = f _M + f _S (1) as a virtual mechanical model that allows the master arm to follow. However, M _V : Virtual mass D _V : Virtual viscosity constant K _V : Virtual elastic constant x _V : Position of the virtual mechanical model expressed in the working coordinate system f _M : Force applied by the operator to the master arm f _S : Force applied to slave arm Also, consider MiΔx¨＋DiΔx¨＋KiΔx＝f _S (2) as an internal model that causes deviation in the slave arm. However, Mi: virtual mass Di: virtual viscous constant Ki: virtual elastic constant xi: position of the internal model expressed in the working coordinate system Δx=xi−xv By rearranging these two equations, we can create the master and slave virtual mechanical models. is expressed as d/dtxvi xvi=0 K ₁ I _2o K ₂ xvi xvi0 G (3). However, I _2o is a 2n×2n unit matrix, xvi=xv xi, uvi=f _M f _S G=M ^-1 _v M ^-1 _v M ^-1 _v M ^-1 _v +M ^-1 _i K ₁ = −M ^-1 _v Kv M ^-1 _i Ki−M ^-1 _v Kv M ^-1 _i Ki 0 K ₂ = −M ^-1 _v Dv M ^-1 _i Di−M ^-1 _v Dv −M ^-1 _i Di 0 On the other hand, the equation of motion of the manipulator is generally expressed as τ=M(θ)θ¨+f(θ, θ¨)+τ _F (4). However, τ is the joint driving torque, θ is the angle expressed in joint coordinates, M(θ) is the inertia matrix, f(θ, θ〓θ) is the Coriolis force and inertial force, and τ _F represents the gravity etc. that the manipulator receives.

On the other hand, the relationship between x in the work coordinate system and θ in the joint coordinate system is generally expressed by the nonlinear function x=T(θ), and when this is differentiated with respect to time t, x〓=Jθ〓 (5) x ¨=J〓θ〓+Jθ¨ (6) Here, J is an n×n matrix defined by J=∂T/∂θ, and is called a Jacobian matrix. Usually, the inverse matrix J ^-1 of the Jacobian matrix exists in the robot's workspace, so equations (5) and (6) are expressed as θ〓＝J ^-1 x〓 (7) θ¨＝J ^-1 (x ¨−J〓θ〓) (8) It can be transformed as follows. Therefore, if the acceleration x of the trajectory of the manipulator in the work coordinate system is given, the desired acceleration θ in the joint coordinates can be calculated using the joint coordinates θ and its velocity θ. Then, the input torque τ that causes this desired acceleration θ¨ to be generated at the joint can be calculated using equation (4) expressing the dynamic characteristics of the manipulator.

Thus, controlling the manipulator in the work coordinate system means finding the acceleration x in the work coordinate system, which is expressed as d/dtx x = 0 0 In 0x x + 0 Inu (9) This can be thought of as a problem to find the input u. If we extend this to master and slave and express them with subscripts m and s, we get d/dt xms x〓ms=0 0 0 I _2o xms x〓ms + 0 I _2o ums (10). However, xms = x _M x _S , ums = x¨ _M x¨ _S Next, the error between the trajectory (position) xvi of the manipulator in the virtual mechanical model expressed by equation (3) and the actual trajectory (position) x _ns If e=x _ns −xvi (11), then from equations (3) and (10), d/dte e〓=0 0 I _2o 0e e〓+I _2o 0u _e (12) However, u _e = u _ns An error system is obtained, expressed as −x¨xvi = u _ns − (K ₁ xvi + K ₂ x〓vi + Guvi).

The aforementioned servo control system 14 is configured to provide a ue that stabilizes this error system.
Such ue is given by the following equation, for example.

u _e =F ₁ e+F ₂ e〓+F ₃ ∫ ^t e(t')dt' (13) Figure 2 shows the calculation unit 13 of the virtual mechanical model expressed by the above equation (3) and the FIG. 2 is a block diagram of a system comprising a servo control system 14 that generates u _e , and linearized master-slave arms M and S that input the u _ns of equation (10).

Next, FIG. 3 is a diagram showing a configuration example of the first invention. This means that, in the basic configuration shown in FIG.
x _M = x _S ).

In the master-slave manipulator shown in FIG. 3, the master arm M and slave arm S are constituted by, for example, a vertically articulated robot as shown in FIG. 8, and each joint is driven by a servo arm. Furthermore, the angular positions θ _M , θ _S and angular velocities θ〓 _M , θ〓 _S of each joint are detected by position detectors M11, S11 and speed detectors M21, S21, respectively, while the master arm M and slave arm Force detectors M12 and S12 attached to each tip of arm S detect the force applied to each arm M and S.

The output from each force detector M12, S12 is the coordinate system of the arm tip (for example, x _W , y _W , z _W in Fig. 8).
Since this is _{a value expressed} as The information is input to section 13.

The virtual mechanical model calculation unit 13 inputs the values of these converted external forces f _M and f _S and expresses the trajectory x in which the tips of the master arm M and the slave arm S should move in the reference orthogonal coordinate system of the manipulator. Output with the specified value.

The output x is converted into an angle θ of each joint by the position coordinate conversion calculation unit 16 and is provided to the master and slave servo control systems M14 and S14.

The servo control systems M14 and S14 control the deviations θ-θ _M and θ-θ S between the angle signal θ from the position coordinate conversion calculation unit 16 and the angle feedback signals θ _M and θ _S from the position detectors M11 and S11, _respectively . subtracters M17 and S17 that obtain the angular deviation, position control compensators M18 and S18 that input the angular deviation, and position control compensator M
Servo amplifier M19, S that amplifies the output from 18, S18 and supplies it to the actuator of each arm.
Here, the angular velocity feedback signals _θ〓M , _θ〓S from the speed detectors M21, S21 are further input to the position control compensators M18, S18. Note that, for example, a tachometer generator is used as the speed detector.

FIG. 4 shows an example of a virtual mechanical model in the virtual mechanical model calculating section 13. This virtual mechanical model consists of virtual mass M, virtual viscosity D, and virtual elastic constant K.
If x is the displacement when external force Δf is applied, then it is expressed by the following equation.

Mx+Dx〓+Kx=Δf (14) This means that the displacement x is generated as the output of the mechanical system consisting of the mass M, the damper constant D, and the spring constant K for the input Δf.

Here, if Δf=f _M +f _S (15), it becomes the same as the equation (1) above, and depending on the inputs f _M and f _S to the virtual dynamic model calculation unit 13 in FIG. 3,
The trajectory (position) x in which the tips of the master arm M and slave arm S should move is output.

The virtual mechanical model calculating section 13 based on the virtual mechanical model shown in FIG. 4 is constructed as shown in the block diagram of FIG. 5 by transforming the above equation (14) as follows.

x¨=1/MΔf-D/Mx〓-K/Mx (16) In other words, the input Δf is the sum of the force f _M applied to the master arm and the force f _S applied to the slave arm, and The displacement x is calculated by combining the feedback D/M due to viscous resistance and the feedback K/M due to the spring. Note that the output from the virtual mechanical model calculation unit 13 is the output x of the virtual mechanical model described above.
plus the arm's initial position x ₀ .

Next, FIG. 6 shows another example of a virtual mechanical model. This virtual mechanical model is composed of virtual mass M and virtual viscosity D, and is expressed by the following equation, where x is the displacement when external force Δf is applied.

Mx¨+Dx〓=Δf=f _M +f _S (17) That is, x¨=1/MΔf−D/Mx〓 (18) Therefore, the virtual dynamic model calculation unit 13 in this case
is constructed as shown in the block diagram of FIG.

According to the master-slave manipulator according to the first aspect of the invention, a desired displacement of each arm is given from a virtual mechanical model calculating section in which force transmission characteristics from the master arm to the slave arms, etc. are stored in advance. Therefore, flexibility is provided between the master and slave, so that, for example, when an unexpected impact force is received, the force can be absorbed by both arms. In addition, by using the force detection signal on the slave side as input to the virtual dynamic model, the force received on the slave side can be rationally transmitted to the master side, eliminating unreasonable movements of each arm and improving operability. can be improved.

Next, FIG. 9 shows a configuration example of the second invention. In the basic configuration shown in FIG. 1, the virtual mechanics model calculation section 13 is configured with a virtual mechanics model calculation section M13 for the master arm and a virtual mechanics model calculation section S13 for the slave arm. The model calculation unit M13 includes a master arm M
The force applied to the slave arm S and the force applied to the slave arm S are inputted to the slave arm virtual mechanical model calculating section S13.The force applied to the slave arm S and the force applied to the master arm virtual mechanical model calculating section M13 The master and slave virtual mechanical model calculation units M13 and S13 input the displacement calculated by the master and slave according to the converted and input force values f _M and _f Trajectories x M and x _S along which the tips of arm _M and slave arm S should move are output as values expressed in the reference orthogonal coordinate system of the manipulator. And each output x _M ,
x _S is the corresponding position coordinate transformation calculation unit M1
6. S16 converts the angles of each joint into angles θ _M and θ _S , and the master and slave servo control systems M14,
It is given to S14.

Note that the other configurations including the servo control systems M14 and S14 are similar to the master-slave manipulator shown in FIG. 3.

FIG. 10 is a block diagram showing the configuration of the virtual dynamic model calculating sections M13 and S13 shown in FIG. 9. In this case, the virtual mechanical model for the master arm is composed of virtual mass M _M , virtual viscosity D _M , and virtual elastic constant K _M , and as in Fig. 5, M _M x¨ _n + D _M x〓 _n + K _M x _n = Δf = f _M + f _S (19) That is, x¨ _n = 1/M _M Δf−D _M /M _M x〓 _n −K _M /M _M x _n (20) Dynamic model calculation section M13
In response to the input f _M +f _S , a trajectory x _M =x _n +x _M0 along which the tip of the master arm M should move is output.

On the other hand, the virtual mechanical model for the slave arm is
From equation (2) above, which shows the internal model that causes deviation in the slave arm S, M _S Δx¨＋D _S Δx〓＋K _S Δx＝f _S (21) where, M _S : Virtual mass D _S : Virtual viscosity constant K _S : Virtual elastic constant x _o : Position of internal model expressed in working coordinate system Expressed as Δx = x _o - x _n . Then, in the slave virtual dynamic model calculating section S13, the output Δx of the internal model is calculated according to the input f _S , and the trajectory x _S =X _o +x _S0 along which the tip of the master arm M should move is output.

According to the master-slave manipulator according to the second aspect of the invention, by giving operating characteristics to each of the master and slave arms separately, for example, when the slave arm touches some obstacle,
Even if the master arm continues to be manually operated, the slave arm can modify its operation to follow the master arm while avoiding obstacles. In addition, by adding an internal model that selectively creates a deviation between the master and slave according to the force detected by the slave, it has a compliance function for the slave, and when the slave receives an impact force, etc. This prevents an unpleasant operational feeling from being conveyed to the operator, and also allows advanced work that cannot be fully instructed by the master side to be easily performed by utilizing the reaction force received from the work object.

Next, a configuration example of the third invention shown in FIG. 11 will be shown.
This is so that not only displacement x but also velocity x〓 and acceleration , the trajectory x in which the tips of the master arm M and slave arm S should move, as well as the velocity x〓 and acceleration x〓, according to the converted input forces f _M and f _S , respectively, are expressed in the reference orthogonal coordinate system of the manipulator. Output the value expressed as . These outputs are the position, velocity, and acceleration data x _M , x _M , x _M , x _S , x _S , x _S for moving each of the master and slave arms M and S.
Position control compensator M of servo control system M14, S14
18, given to S18. Furthermore, position control compensators M18 and S18 include position detectors M11 and S1.
1 and the outputs θ _M , θ _S and θ _M , θ _S of the speed detectors M21 and S21 are also input.

The master and slave position control compensators M18 and S18 shown in FIG.
1. The outputs of S11 and speed detectors M21 and S21 are respectively data xf _M in the coordinate system of the tip of each arm,
xf _S and xf _M , position coordinate conversion calculation unit M that converts to xf _S
31, S31 and speed coordinate conversion calculation unit M32, S3
2.

The position data xf _M and xf _S from the position coordinate conversion calculation units M31 and S31 are converted into position data from the virtual dynamic model calculation unit 13 by subtractors M33 and S33.
It is subtracted from x _M and X _S , and the deviation between them is input to transfer function elements M34 and S34. The transfer function elements M34 and S34 have an integral compensation function.

Similarly, the speed data xf _M and xf _S from the speed coordinate conversion calculation units M32 and S32 are also input to the subtracters M35 and S35.
The speed input from the virtual dynamic model calculation unit 13 is
It is subtracted from x _M and x _S , and the deviation between them is input to transfer function elements M36 and S36.

The acceleration inputs x _M , x _S from the virtual dynamic model calculation unit 13 and the deviation data from the transfer function elements M34, S34 and M36, S36 are sent to the adder M3.
7 and S37, and their sum becomes the acceleration command x for each arm, but since this is expressed as a value in the reference orthogonal coordinate system, the angle of each joint is calculated by the acceleration coordinate conversion calculation unit M38, S38. It is converted into acceleration θ and then sent to nonlinear compensation calculation unit M39, S3.
9 is input.

In the nonlinear compensation calculation units M39 and S39, the above-mentioned
Compensation calculations are made for nonlinear elements such as gravity, inertia force, and Coriolis force included in equation (4), and torques τ _M and τ _S of each joint axis are output.

FIG. 12 is a block diagram showing the configuration of the virtual mechanical model calculating section 13 shown in FIG. 11.
This is similar to Figure 5, but the output is velocity x
and acceleration x are also taken out.

According to the master-slave manipulator according to the third aspect of the invention, the speed and acceleration command values are extracted from the calculation section of the virtual dynamic model and compensated separately from the position command, so that, for example, the master arm can be quickly activated. The slave arm can be made to faithfully follow the motion and acceleration/deceleration motion.

[Effects of the Invention] As described above, the present invention assumes a virtual mechanical model that defines the force transmission characteristics between the master arm and the slave arm of a master-slave manipulator, and develops a model-following servo control system. Each arm is controlled to follow its virtual dynamic model, and the inputs to the virtual dynamic model are the force applied to the master by the operator and the force detected when the slave touches an object. By providing this, a master-slave manipulator with a bilateral servo system is constructed as a whole, and the slave arm has shock absorption, compliance function, fail-safe function, etc., while the master side has the effect of improving the operation feeling. play.

[Brief explanation of drawings]

Fig. 1 is a diagram showing the basic concept of the present invention, Fig. 2 is a block diagram showing an example of the configuration of the system shown in Fig. 1,
3 is a diagram showing a configuration example of the first invention, FIG. 4 is a diagram showing an example of a virtual dynamic model, FIG. 5 is a block diagram showing the virtual dynamic model calculation of FIG. 4, and FIG.
The figure shows another example of the virtual mechanical model, FIG. 7 is a block diagram showing the virtual mechanical model calculation of FIG. 6, and FIG. 8 is an example of a vertically articulated robot constituting a master-slave manipulator. Figure 9 showing
The figure shows an example of the configuration of the second invention, FIG. 10 is a block diagram of the virtual dynamic model calculation section of FIG. 9, FIG. 11 shows an example of the configuration of the third invention, and FIG. FIG. 11 is a block diagram of a calculation section of a virtual dynamic model, and FIG. 13 is a diagram showing a conventional master-slave manipulator control system. M...Master arm, S...Slave arm,
11...Position detector, 12...Force detector, 13...
...Virtual mechanical model calculation unit, 14... Servo control system, 15... Force coordinate conversion calculation unit, 16... Position coordinate conversion calculation unit, M18, S18... Position control compensator, M19, S19... Servo amplifier, M21,
S21...Speed detector.

Claims (1)

[Scope of Claims] 1. A master-slave manipulator having a function of transmitting force sensation between a master arm and a slave arm, comprising: a force detector that detects a force applied to each of the master arm and the slave arm; A force transmission characteristic between the master arm and the slave arm is defined so that the force detected by the force detector is input, and the displacement of the master arm and slave arm in response to the input is output. A master comprising: a virtual mechanical model calculating section that performs calculations according to a virtual mechanical model; and a servo control system that controls the master arm and slave arm using an output from the virtual mechanical model calculating section as a command value. Slave manipulator. 2. A master-slave manipulator having a force sensation transmission function between a master arm and a slave arm, comprising: a force detector that detects a force applied to each of the master arm and the slave arm; and a force applied to the slave arm are input, and a force transmission characteristic between the master arm and the slave arm is defined so that the displacement of the master arm corresponding to the input is output. a master virtual mechanics model calculation section that calculates according to a virtual mechanics model; and inputs the force applied to the slave arm and the displacement from the master virtual mechanics model calculation section, and calculates the displacement of the slave arm according to the input. a slave virtual mechanics model computing section that computes according to a predetermined slave virtual mechanics model to be used as an output; and a servo control system for controlling a slave arm. 3. A master-slave manipulator having a force sensation transmission function between a master arm and a slave arm, comprising: a force detector that detects a force applied to each of the master arm and the slave arm; and a force detector that detects force applied to each of the master arm and slave arm. virtual mechanics that defines force transmission characteristics between the master arm and the slave arm so that the force applied to the master arm and the slave arm is input, and the position, velocity, and acceleration of the master arm and slave arm according to the input are output. a virtual mechanical model calculation unit that calculates according to the model; a position detector that detects the position of each of the master arm and the slave arm; a speed detector that detects the speed of each of the master arm and the slave arm; A master-slave manipulator comprising: a virtual dynamic model calculation section; and a servo control system that controls the master arm and slave arm based on outputs from the position detector and the speed detector.