JP7035309B2 - Master-slave system - Google Patents

Description

The present invention relates to a master-slave system that performs bilateral control.

It includes a master operating means for the operator to operate, a slave robot located at a position away from the master operating means, and a control means for performing bilateral control to operate the slave robot according to the operation of the master operating means. Master-slave systems are known (see, for example, Patent Document 1).

Japanese Unexamined Patent Publication No. 2016-022559

By the way, in the above-mentioned bilateral control, the operator can intuitively operate the slave robot with a high degree of freedom of operation. However, an inexperienced operator may erroneously operate the slave robot due to the high degree of freedom of operation of the bilateral control.

The present invention has been made to solve such a problem, and it is a main object of the present invention to provide a master-slave system capable of accurately operating a slave robot even by an inexperienced operator.

One aspect of the present invention for achieving the above object is
The master operation means that the operator operates and
A slave robot located at a position away from the master operating means,
A master-slave system including a control means for performing bilateral control for operating the slave robot according to the operation of the master operation means.
A storage means for storing a target trajectory that is a target when the slave robot operates is provided.
The control means controls the operation of the master operation means so that when the slave robot deviates from the target trajectory stored in the storage means, a resistance force is generated against the operation of the master operation means.
It is a master-slave system characterized by this.

According to the present invention, it is possible to provide a master-slave system capable of accurately operating a slave robot even by an inexperienced operator.

It is a block diagram which shows the schematic system configuration of the master-slave system which concerns on one Embodiment of this invention. It is a figure which shows an example of a slave robot. It is a figure which shows an example of the operation performed by the grip part of a slave robot. It is the figure which represented the weight amount of the voxel of a target trajectory by the size of a sphere simulated. It is the figure which expressed the voxel by a spherical body. It is a figure which expressed the shape of the grip part by a capsule shape or a rectangular parallelepiped with rounded corners. It is a figure which shows the distance between a capsule-shaped grip part and a spherical voxel. (A) It is a figure which shows an example of a capsule-shaped grip part and a spherical voxel. (B) It is a figure which shows an example of the weight amount of each spherical voxel calculated for the capsule-shaped grip portion and the spherical voxel shown in FIG. 8 (a). It is a flowchart which shows an example of the flow of the master-slave control processing which concerns on one Embodiment of this invention. It is a figure for demonstrating the damping process when a base moves. It is a figure which shows the damping process in the environment where the obstacle exists in the vicinity of the elbow part. (A) It is a figure which shows the method of generating the target trajectory by the damping process. (B) It is a figure which shows the method of generating the target trajectory by the damping process. (C) It is a figure which shows the method of generating the target trajectory by the damping process.

Hereinafter, embodiments of the present invention will be described with reference to the drawings. FIG. 1 is a block diagram showing a schematic system configuration of a master-slave system according to an embodiment of the present invention. The master-slave system 1 according to the present embodiment is a master operation unit 2 operated by an operator, a slave robot 3 arranged at a position away from the master operation unit 2, and a slave according to the operation of the master operation unit 2. It includes a bilateral control unit 4 that performs bilateral control for operating the robot 3 and a storage unit 5.

As shown in FIG. 2, the slave robot 3 has six or more joints (wrist joint, elbow joint, shoulder joint, etc.) and can operate the tip position posture (hand position posture, etc.) with six or more degrees of freedom. It is configured as an articulated arm robot. The slave robot 3 includes, for example, a base portion 31, a plurality of links 32, a joint portion 33 that rotatably connects each link 32, and a grip portion 34 provided at the tip of the link 32 to grip an object. have.

Each joint portion 33 is provided with an actuator for driving each joint portion 33. The actuator may be, for example, a motor drive, a hydraulic drive, a pneumatic drive, or the like. Each joint portion 33 is provided with an angle sensor that detects the rotation angle (hereinafter, joint angle) of each joint portion 33. Each joint portion 33 is provided with a torque sensor that detects the torque of each joint portion 33 (hereinafter referred to as joint torque). The slave robot 3 may be a leg robot having a plurality of joints (hip joint, knee joint, ankle joint, etc.).

The slave robot 3 is provided with an environment recognition sensor 35 such as a stereo camera and a depth camera. The environment recognition sensor 35 acquires environment information around the slave robot 3, such as the task execution state of the slave robot 3.

The master operation unit 2 is a specific example of the master operation means. The master operation unit 2 has an operation unit in which an operator inputs operation information via an arm or the like. The slave robot 3 operates according to the operation information input to the master operation unit 2. The master operation unit 2 is arranged at a position away from the slave robot 3. The master operation unit 2 feeds back force information such as a force or moment at the tip position of the slave robot 3 or each joint torque to an operator who operates the operation unit of the master operation unit 2.

The master operation unit 2 is, for example, a hand instruction device in which the operator instructs the hand position of the slave robot 3, or an articulated master robot worn by the operator. The master operation unit 2 has, for example, a plurality of links, a joint unit that rotatably connects each link, and an operation unit provided at the tip of the link and operated by an operator. Each joint is provided with an actuator that drives each joint. The actuator may be, for example, a motor drive, a hydraulic drive, a pneumatic drive, or the like. Each joint is provided with an angle sensor that detects each joint angle. Each joint is provided with a torque sensor that detects each joint torque.

The bilateral control unit 4 is a specific example of the control means. The bilateral control unit 4 executes bilateral control for operating the slave robot 3 in accordance with the operation of the master operation unit 2. In the bilateral control, the bilateral control unit 4 controls the slave robot 3 to operate in the same manner as the operation of the master operation unit 2 when the operator operates the master operation unit 2. At this time, the bilateral control unit 4 feeds back the force information applied to the slave robot 3 to the operator. That is, the bilateral control unit 4 controls the master operation unit 2 so that a force similar to the force applied to the slave robot 3 is generated in the master operation unit 2.

For example, the bilateral control unit 4 calculates the tip position / posture of the target slave robot 3 by using forward kinematics based on each joint angle detected by the angle sensor of the master operation unit 2. The bilateral control unit 4 calculates the target joint angle of each joint unit 33 of the slave robot 3 by using inverse kinematics based on the calculated tip position posture. The bilateral control unit 4 outputs a command signal corresponding to the calculated target joint angle to the actuator of each joint unit 33 of the slave robot 3 to perform PID control and the like.

At the same time, the bilateral control unit 4 applies a tip force (load, moment, etc.) to the tip (hand, etc.) of the slave robot 3 based on the joint torque detected by the torque sensor of each joint 33 of the slave robot 3. calculate. Based on the calculated tip force, the bilateral control unit 4 sets the actuator of each joint of the master operation unit 2 so that the same force as the tip force of the slave robot 3 is generated in the operation unit of the master operation unit 2. It controls and performs PID control and the like.

In this way, in the bilateral control, the bilateral control unit 4 operates the slave robot 3 and the slave robot 3 in the same manner as the operation of the master operation unit 2 when the operator operates the master operation unit 2. It is possible to feed back the force information applied to the operator to the operator via the operation unit of the master operation unit 2. The above bilateral control method is an example, and is not limited thereto.

For example, the master operation unit 2 may be configured as a master robot having a configuration similar to that of the slave robot 3 (configuration of joints, links, etc.). In this case, the bilateral control unit 4 may perform bilateral control so as to synchronize each joint portion of the master robot with each joint portion 33 of the corresponding slave robot 3. Specifically, the bilateral control unit 4 uses the joint angle detected by the angle sensor of each joint portion of the master robot and the joint torque detected by the torque sensor as the target joint angle of each joint portion 33 of the slave robot 3. And set to the target joint torque. The bilateral control unit 4 outputs a command signal corresponding to the set target joint angle and target joint torque to the actuator of each joint unit 33 of the slave robot 3.

At the same time, the bilateral control unit 4 uses the joint angle detected by the angle sensor of each joint 33 of the slave robot 3 and the joint torque detected by the torque sensor as the target joint angle and the target joint of each joint of the master robot. Set to torque. The bilateral control unit 4 outputs a command signal corresponding to the set target joint angle and target joint torque to the actuators of each joint portion of the master robot.

As described above, the operator, for example, refers to the environmental information (image information around the slave robot 3, etc.) acquired by the recognition sensor and operates the master operation unit 2, as if he / she directly operates the slave robot 3. It can be operated remotely with a sense of realism as if you were doing it. At the same time, the operator can sense the reaction force generated in the actual slave robot 3 by himself / herself.

The bilateral control unit 4 is, for example, a CPU (Central Processing Unit) 41 that performs arithmetic processing or the like, a ROM (Read Only Memory) or a RAM (Random Access Memory) in which an arithmetic program executed by the CPU 41 is stored. ), A memory 42, and an interface unit (I / F) 43 for inputting / outputting signals to / from the outside, and the like, each of which is composed of hardware. The CPU 41, the memory 42, and the interface unit 43 are connected to each other via a data bus or the like.

By the way, the above-mentioned bilateral control has an advantage that the operator can intuitively operate the slave robot with a high degree of freedom of operation. However, even if the slave robot itself can operate with a high degree of freedom, there are many cases where the object operates only in a certain direction. For example, objects such as doors, refrigerators, doors such as shelves, drawers such as tons and lockers, windows and shutters, and lids such as PET bottles and bottles move only in a certain direction.

When the slave robot operates an object with a limited range of motion as described above, a skilled operator can use the high degree of freedom of operation of bilateral control to make the slave robot more delicate within the constraint of the range of motion. Can be operated. However, an inexperienced operator may erroneously operate the slave robot out of the limitation of the operating range due to the high degree of freedom of operation of the bilateral control.

On the other hand, the master-slave system 1 according to the present embodiment includes a storage unit 5 that stores a target trajectory that is a target when the slave robot 3 operates. The bilateral control unit 4 controls the operation of the master operation unit 2 so that when the slave robot 3 deviates from the target trajectory stored in the storage unit 5, resistance is generated in the operation of the master operation unit 2.

As a result, even if an inexperienced operator operates the master operation unit 2 and the slave robot 3 deviates from the target trajectory, resistance is generated in the operation of the master operation unit 2, so that the slave robot 3 naturally returns to the target trajectory. As such, the operation of the master operation unit is modified. Therefore, even an inexperienced operator can accurately operate the slave robot 3 according to the target trajectory.

In the master-slave system 1 according to the present embodiment, first, a target trajectory, which is a target when the slave robot 3 operates, is stored in the storage unit 5. The storage unit 5 is a specific example of the storage means.

For example, a skilled operator operates the master operation unit 2 to move the object by the slave robot 3. By operating the master operation unit 2 by a skilled operator, an ideal trajectory of the slave robot 3 is generated. The trajectory of the grip portion 34 of the slave robot 3 at this time (hereinafter referred to as the hand trajectory) is set as the target trajectory.

More specifically, as shown in FIG. 3, the environment recognition sensor 35 captures the operation of the grip portion 34 until the grip portion 34 of the slave robot 3 grips the handle (object) of the door and opens the door. .. The bilateral control unit 4 calculates the feature amount of the grip unit 34 based on the captured image from the environment recognition sensor 35. The bilateral control unit 4 calculates a hand trajectory based on the calculated feature amount, and stores the calculated hand trajectory as a target trajectory in the storage unit 5.

Next, a method of controlling the operation of the master operation unit 2 so that a resistance force is generated in the master operation unit 2 when the slave robot 3 deviates from the target trajectory stored in the storage unit 5 will be described.

First, a method of setting a target trajectory stored in the storage unit 5 will be described.
As described above, the target trajectory stored in the storage unit 5 represents, for example, the position where the gripping unit 34 passes three-dimensionally in a voxel format. Each voxel has weight information (weight amount) indicating resistance to the operation of the master operation unit 2. The weight amount is normalized by a value (for example, 0 to 1) according to the degree of contact when the grip portion 34 passes through each voxel. The voxel particle size is set sufficiently close to the grip shape in order to accurately reproduce the target trajectory. Moreover, since the time information is not used in the subsequent processing, the time information for the voxel can be ignored. FIG. 4 is a diagram simulating the weight of a voxel in a target orbit with the size of a sphere. The weight amount of the voxel represents the ease of operation of the slave robot 3 or the operation limitation (the above-mentioned resistance force).

Next, a method of setting the weight amount of voxels will be described in detail.
For example, the bilateral control unit 4 moves the grip portion 34 along a target trajectory on a map covered with voxels, and calculates the weight amount of each voxel according to the degree of contact between the grip portion 34 and each voxel.

Here, in order to simplify the calculation of the degree of contact between the grip portion 34 and each voxel, for example, as shown in FIG. 5, the voxel may be represented not as a cube but as a sphere containing the cube. Similarly, the shape of the grip portion 34 may be a simple shape such as a capsule shape or a rectangular parallelepiped (a shape in which all four sides of the rectangular parallelepiped are surrounded by a capsule shape) so as to include the actual shape, for example, as shown in FIG. It may be expressed.

For example, as shown in FIG. 7, if the shape of the grip portion 34 is a capsule shape, the distance between the capsule-shaped grip portion 34 and the spherical voxel can be calculated by the distance between the line segment and the point, and this distance can be calculated. By using this, the degree of contact between the grip portion 34 and each voxel can be easily displayed. Further, if the shape of the grip portion 34 is a rectangular parallelepiped with rounded corners, a combination of a capsule shape and an OBB (Orient Bounding Box) shape that facilitates contact determination makes contact more accurately in a shape close to the shape of the actual grip portion 34. Judgment can be made.

The bilateral control unit 4 sets the weight amount w of each voxel to the following (1)-(4) according to the degree of contact between the grip portion 34 having a simple shape set as described above and the voxels of the spherical body around the grip portion 34. ). In this description, for example, voxels and simple shapes are defined as capsule shapes.

(1) The bilateral control unit 4 is a line segment connecting two end points of a capsule-shaped grip portion 34 (hereinafter, capsule-shaped grip portion 34) and a center of a target spherical voxel (hereinafter, spherical voxel). The nearest neighbor distance l with and is calculated.

(2) The bilateral control unit 4 calculates the weight amount w of each spherical voxel using the following contact determination formulas (A)-(C). However, the radius of the spherical voxel is r _b , and the radius of the spheres at both ends of the capsule-shaped grip portion 34 is r _c .
(A) l> r _b + r _c (when the capsule-shaped grip portion 34 and the spherical voxel do not contact)
: W = 0
(B) r _c > l + r _c (when a spherical voxel is contained in the capsule-shaped grip portion 34)
: W = 1
(C) Other than the above (A) and (B) (for example, when the capsule-shaped grip portion 34 and the spherical voxel intersect as shown in FIG. 7).
: W = (r _b + r _c -l) / 2r _b

FIG. 8A is a diagram showing an example of a capsule-shaped grip portion 34 and a spherical voxel. FIG. 8B is a diagram showing an example of the weight amount of each spherical voxel calculated for the capsule-shaped grip portion 34 and the spherical voxels shown in FIG. 8A. The method for calculating the weight amount w of each spherical voxel is an example, and the method is not limited to this. For example, the weight amount w of each spherical voxel may be set according to another index such as the volume ratio at which the capsule-shaped grip portion 34 and each spherical voxel are in contact with each other.

(3) The bilateral control unit 4 repeats the above (1) and (2) while moving the capsule-shaped grip portion 34 along the target trajectory, and determines the weight amount w of each spherical voxel in and around the target trajectory. Set.

(4) The bilateral control unit 4 sets the maximum weight amount w in each spherical voxel (x, y, z) by the weight amount of the spherical voxel to obtain w (x, y, z), and the storage unit 5 To memorize. As a result, the weight amount w (x, y, z) normalized to 0 to 1 is set in the global voxel. In addition, the bilateral control unit 4 may perform a process of taking an average value among neighboring spherical voxels and applying a filter in order to suppress the discontinuity in the post-stage process.

As described above, the weight amount w (x, y, z) can be set for the voxel of the target trajectory as shown in FIG.

Next, using the target trajectory in which the weight amount w (x, y, z) is set for the spherical voxel as described above, when the slave robot 3 deviates from the target trajectory, a resistance force is generated in the master operation unit 2. , The method of controlling the operation of the master operation unit 2 will be described.

For example, in a minute time of one control cycle, the operator has a position _pn-1 (x _n-1 , y _n-1 , z _n-1 ) and a posture (quaternion) with respect to the operation unit of the master operation unit 2. : Quaternion) q _n-1 (s _n-1 , un _-1 , v _n-1 , w _n-1 ), position _pr (x _r , y _r , z _r ), posture q _r (s _r ) , _Ur , _vr , _wr )) will be described below.

The bilateral control unit 4 calculates the candidate position of the grip portion 34 of the slave robot 3 corresponding to the position _pr and the posture _qr based on the joint angle detected by the angle sensor of each joint portion of the master operation unit 2. do.

The bilateral control unit 4 describes the above based on the target trajectory in which the weight amount w (x, y, z) is set in the spherical voxel of the storage unit 5 and the calculated candidate position of the grip unit 34 of the slave robot 3. The contact determination formula (A)-(C) is used to determine the contact between the grip portion 34 and the spherical voxel. The bilateral control unit 4 extracts a group of spherical voxels that come into contact with the grip portion 34 based on the contact determination.

The bilateral control unit 4 calculates the minimum weight amount as w _n from the weight amounts w (x, y, z) of the extracted spherical voxels.

When the grip portion 34 comes into contact with a spherical voxel having a small weight, the bilateral control unit 4 performs a damping process in which damping acts strongly and the operation of the master operation unit 2 is suppressed. For example, a maximum weight amount (w = 1) is set for a spherical voxel on a target orbit, and as the spherical voxel deviates from the target orbit, a gradually smaller weight amount (0 <w <1) is set on the spherical voxel. Is set, and when the spherical voxel completely deviates from the target trajectory, the minimum weight amount (w = 0) is set for the spherical voxel. As a result, when the grip portion 34 is on the target trajectory, the operator feels almost no resistance to the operation unit of the master operation unit 2, but as the grip portion 34 deviates from the target trajectory, the operator feels little resistance. A stronger resistance force is felt by the operation unit of the master operation unit 2, and when the grip portion 34 is completely off the target trajectory, the maximum resistance force is felt by the operation unit of the master operation unit 2.

Here, even if the operator is inexperienced, if he / she gains some experience, the number of operation mistakes will decrease. Therefore, the bilateral control unit 4 may perform a process of reducing the resistance of the damping process by increasing the weight amount _wr according to the degree of experience. This makes it possible to make adjustments that are more suitable for the current skill of the operator. For example, when the resistance force is halved, the bilateral control unit 4 has the equation (w _{n =} 0. The weight amount w _n may be corrected using 5 w _n + 0.5).
The bilateral control unit 4 sets the target position _pn and the target posture _{q n of} the grip portion 34 of the slave robot 3 by using the following equations 1 and 2 based on the weight amount calculated as described above. calculate.

なお、上記式１及び式２において、クォータニオンが用いられているが、これに限定されず、例えば、オイラー角などを用いてもよい。

Although quaternions are used in the above formulas 1 and 2, Euler angles and the like may be used without limitation.

The bilateral control unit 4 controls the actuators of the joints 33 of the slave robot 3 so as to have the calculated target position _pn and the target posture q _n , and corresponds to the hand position and posture of the slave robot 3. , Bilaterally control the position and posture of the operation unit of the master operation unit 2.

If the weight amount w _n suddenly changes to w _n-1 , the resistance felt by the operator also changes discontinuously, resulting in a poor operational feeling. Therefore, the bilateral control unit 4 performs a process of applying a temporal low-pass filter to the weight amount _wn , a process of applying a spatial filter to the weight amount between neighboring spherical voxels, and the like, and this rapid process is performed. The change may be suppressed.

In the damping process, the bilateral control unit 4 makes it easier for the operator to operate the operation unit of the master operation unit 2 (lowers the resistance force) when the grip unit 34 is on the target trajectory, and the grip unit 34 makes it easier for the operator to operate the operation unit. As the vehicle deviates from the target trajectory, the control is performed so that the operator becomes difficult to operate the operation unit of the master operation unit 2 (the resistance becomes high).

Further, the bilateral control unit 4 may perform control to make it easier for the grip portion 34 to return to the target trajectory when the grip portion 34 deviates from the target trajectory. As a result, even if the grip portion 34 deviates from the target trajectory, the operator can easily correct the trajectory and return it to the target trajectory.

For example, when w'n> 0 (when w _n is larger than w _{n -1} ), the bilateral control unit 4 calculates w _d using the following equation. _w'n is a derivative value of _w'n . In the following equation, β is a constant β> 0 and is set in advance in a memory or the like.
w _d = w _n (1 + _βw'n )

The bilateral control unit 4 substitutes the calculated w _d into the w _n of the above equations 1 and 2, and calculates the target position _pn and the target posture q _n of the grip portion 34 of the slave robot 3.

The bilateral control unit 4 grips the slave robot 3 so as to return to the target trajectory by multiplying the deviation amount from the target trajectory by the virtual spring constant using the above equations 1 and 2 in which the inertial term is introduced. The target position _pn and the target posture q _n of the unit 34 may be calculated.

Even if the bilateral control unit 4 is updated by increasing the weight amount w (x, y, z) of the spherical voxel of the storage unit 5 determined to be in contact in the contact determination between the grip portion 34 and the spherical voxel. good. This not only makes it difficult for a skilled person to deviate from the target trajectory, but also reduces damping (resistance) gradually when repeatedly operated in the deviation direction, intentionally making a trajectory different from the target trajectory of a skilled person or the like. Can be generated.

For example, the bilateral control unit 4 adds a predetermined coefficient to or adds a predetermined coefficient to the weight amount w (x, y, z) of the spherical voxel of the storage unit 5 determined to be in contact in the contact determination between the grip portion 34 and the spherical voxel. The weight amount w (x, y, z) may be increased by multiplying.

Further, the bilateral control unit 4 adds or multiplies a predetermined coefficient according to the degree of contact to the weight amount w (x, y, z) of the spherical voxel of the storage unit 5, so that the weight amount w (x). , Y, z) may be increased. For example, the predetermined coefficient may be decreased as the degree of contact increases. The weight amount w (x, y, z) after the increase is 1 or less. The predetermined coefficient is set in advance in a memory or the like, and the operator or the like can appropriately change the setting.

As mentioned above, by making the damping variable, it is possible to reflect the flexible operation by the current operator while following the target trajectory by a skilled person to some extent and ensuring safety, which is an advantage of the master-slave operation method. More flexible operation can be realized by taking advantage of the nature.

For example, when the size of the door at the time of generating the target trajectory by an expert and the size of the door at the time of the current operation by the operator are different, and the environment at the time of generating the target trajectory is different from the current environment. Is assumed. Even in such a case, the operator can perform the operation by varying the damping and effectively mitigating the deviation from the target trajectory.

Further, when the variable damping process is repeated, the weight amount of each spherical voxel gradually deviates greatly from the initial value set by an expert or the like. Therefore, the bilateral control unit 4 multiplies the weight amount w (x, y, z) of the spherical voxel of the storage unit 5, for example, by a time coefficient, so that the weight amount is restored with the passage of time. You may perform the process of returning to the initial value. As a result, even if the variable damping process is repeated, it is possible to prevent the weight amount of each spherical voxel from deviating significantly from the initial value set by an expert or the like.

FIG. 9 is a flowchart showing an example of the flow of the master-slave control process according to the present embodiment.

For example, the following initial settings are made for the master-slave system 1 (step S101). The storage unit 5 stores the target trajectory that is the target when the slave robot 3 operates. The bilateral control unit 4 sets the weight amount w of each spherical voxel of the target trajectory stored in the storage unit 5. A trigger for starting and ending the damping process is set in the bilateral control unit 4. More specifically, when the bilateral control unit 4 receives a predetermined physical input such as an operator's line-of-sight input, gesture, or voice command, the physical input is used as a trigger for starting the damping process (hereinafter referred to as a start trigger). Alternatively, the damping process is started or terminated as a trigger for the end of the damping process (hereinafter referred to as an end trigger). Further, when the bilateral control unit 4 detects the door and the gripping unit 34 of the slave robot 3 grips the door knob, the damping process is automatically started by using the detection as a trigger for starting the damping process. You may. When the bilateral control unit 4 detects the open state of the door and detects that the grip portion 34 of the slave robot 3 has released the door knob, the detection is used as an end trigger of the damping process, and the damping process is automatically terminated. You may.

The bilateral control unit 4 calculates the target position _pr and the target posture _qr of the grip unit 34 of the next provisional slave robot 3 in response to the operation of the operation unit of the master operation unit 2 by the operator (step S102). ).

The bilateral control unit 4 determines whether or not the damping process is in progress (step S103).

When the bilateral control unit 4 determines that the damping process is in progress, the bilateral control unit 4 determines whether or not to cancel (end) the damping process based on a predetermined physical input or the like (step S104).

When the bilateral control unit 4 determines that the damping process is not canceled (NO in step S104), the damping process is continued. The bilateral control unit 4 determines the contact between the grip unit 34 and the spherical voxel based on the target trajectory of the storage unit 5 and the calculated candidate position of the grip unit 34 of the slave robot 3, and the grip unit 34 and A group of spherical voxels in contact is extracted (step S105). On the other hand, when the bilateral control unit 4 determines that the damping process is canceled (YES in step S104), the process proceeds to the following (step S111).

The bilateral control unit 4 calculates the minimum weight amount as w _n from the weight amount w (x, y, z) of the extracted spherical voxels group (step S106).

The bilateral control unit 4 calculates the target position _pn and the target posture q _n of the grip portion 34 of the slave robot 3 using the above equations 1 and 2 based on the calculated weight amount w _n (step). S107).

The bilateral control unit 4 updates the weight amount w (x, y, z) of the spherical voxel of the storage unit 5 determined to be in contact in the contact determination between the grip portion 34 and the spherical voxel (step S108).

The bilateral control unit 4 performs a process of returning the weight amount w (x, y, z) of the spherical voxel of the storage unit 5 to the original initial value with the passage of time (step S108), and in a process described later (step S112). Transition.

When the bilateral control unit 4 determines that the damping process is not in progress (NO in step S103), the bilateral control unit 4 determines whether or not to start the damping process based on the trigger for starting the damping process (step S110). When the bilateral control unit 4 determines that the damping process is to be started based on the damping process start trigger (YES in step S110), the process proceeds to the above (step S105). On the other hand, when the bilateral control unit 4 determines that the damping process is not started based on the trigger for starting the damping process (NO in step S110), the target position _pn = _pr and the target posture q _n = q _r. (Step S111).

The bilateral control unit 4 controls the actuators of the joints 33 of the slave robot 3 so as to have the calculated target position _pn and the target posture q _n , and corresponds to the hand position and posture of the slave robot 3. , Bilaterally control the position and posture of the operation unit of the master operation unit 2 (step S112).

The bilateral control unit 4 increments n (step S113) and proceeds to the above (step S102).

As described above, the master-slave system 1 according to the present embodiment includes a storage unit 5 that stores a target trajectory that is a target when the slave robot 3 operates. The bilateral control unit 4 controls the operation of the master operation unit 2 so that when the slave robot 3 deviates from the target trajectory stored in the storage unit 5, resistance is generated in the operation of the master operation unit 2. As a result, even if the slave robot 3 deviates from the target trajectory, resistance is generated in the operation of the master operation unit 2. Therefore, the operation of the master operation unit is corrected so that the slave robot 3 naturally returns to the target trajectory. Therefore, even an inexperienced operator can accurately operate the slave robot 3 according to the target trajectory.

The present invention is not limited to the above embodiment, and can be appropriately modified without departing from the spirit.
In the above embodiment, the slave robot 3 is fixed to a base or the like, but is not limited thereto. For example, the slave robot 3 may have wheels and legs and may be configured to move. As shown in FIG. 10, even if the base moves, the damping process can be performed in the same manner as when the base is fixed due to the relative positional relationship between the base and the object, and the same effect can be obtained.

In the above embodiment, the bilateral control unit 4 determines the contact between the grip portion 34 (hand) of the slave robot 3 and the spherical voxel, calculates the weight amount w _n , and obtains the target position _pn and the target of the grip portion 34. The posture q _n is calculated, but the present invention is not limited to this. Similar to the slave lot gripping unit 34, the bilateral control unit 4 also determines the contact between the elbow portion and the spherical voxel, calculates the weight amount, and calculates the target position _pn and the target posture q _n of the elbow portion. May be good. As a result, for example, even in an environment where an obstacle exists in the vicinity of the elbow as shown in FIG. 11, an expert can use the redundancy degree of freedom of bilateral control in advance to avoid the obstacle. Along the target trajectory, even an inexperienced person can safely and accurately realize remote control of the slave robot 3 in a complicated environment.

In the above embodiment, the bilateral control unit 4 may perform the damping process to generate a target trajectory.
Here, even a skilled person may find it difficult to remotely control the slave robot 3 in an inexperienced environment. Therefore, as shown in FIG. 12A, first, the bilateral control unit 4 determines the contact between the grip portion 34 and the spherical voxel based on the target trajectory in which the spherical voxel having a small weight amount is set. , The target position _pn and the target posture q _n of the grip portion 34 are calculated, and bilateral control is performed. At this time, due to the damping processing effect, the operation of the master operation unit 2 becomes low speed, but erroneous operation is reduced and accurate operation can be expected. Then, the bilateral control unit 4 increases and updates the weight amount of each spherical voxel by repeating the damping process. As a result, as shown in FIG. 12 (b), the weight amount of each spherical voxel on and around the orbit of the grip portion 34 increases, and finally, as shown in FIG. 12 (c), a target orbit is generated. .. In this way, the bilateral control unit 4 repeats the damping process and the update of the weight amount of each spherical voxel from the target trajectory of the spherical voxel having a small weight amount to generate the target trajectory.

The present invention can also be realized, for example, by causing the CPU to execute a computer program for the process shown in FIG.

Programs can be stored and supplied to a computer using various types of non-transitory computer readable medium. Non-temporary computer-readable media include various types of tangible storage media. Examples of non-temporary computer-readable media include magnetic recording media (eg, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (eg, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs. Includes CD-R / W, semiconductor memory (eg, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)).

The program may be supplied to the computer by various types of transient computer readable medium. Examples of temporary computer readable media include electrical, optical, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

1 Master-slave system, 2 Master operation unit, 3 Slave robot, 4 Bilateral control unit, 5 Storage unit, 31 Base unit, 32 Link, 33 Joint unit, 34 Grip unit, 35 Environment recognition sensor

Claims (2)

The master operation means that the operator operates and
A slave robot that is arranged at a position away from the master operating means and has a gripping portion that grips and operates an object.
A master-slave system including a control means for performing bilateral control for operating the slave robot according to the operation of the master operation means.
A storage means for storing a target trajectory that is a target when the slave robot operates is provided.
The control means controls the operation of the master operation means so that when the slave robot deviates from the target trajectory stored in the storage means, a resistance force is generated against the operation of the master operation means.
The target trajectory stored in the storage means is associated with a passing position through which the grip portion of the slave robot passes, and a resistance force against the operation of the master operating means is set.
The control means reduces and updates the resistance force of the passing position determined to be in contact in the contact determination between the grip portion and the passing position.
The position through which the grip portion of the slave robot passes is expressed in a voxel format, and each voxel has a weight amount indicating resistance to the operation of the master operating means.
The control means increases the weight amount by adding or multiplying the weight amount of the voxels of the storage means by a predetermined coefficient according to the degree of contact .
A master-slave system characterized by that. The master-slave system according to claim 1 .
The control means performs a process of returning the weight amount to the original initial value with the passage of time by multiplying the weight amount of the voxel of the storage means by a time coefficient.
A master-slave system characterized by that.

Images