JP2012051043A - Robot system or robot control device - Google Patents

Abstract

Provided are a robot system and a robot control apparatus capable of more optimally controlling a manipulator having redundant degrees of freedom.
A manipulator (2) having one or more actuators and a controller (3) for controlling the manipulator (2), the controller (3) sets a part of the actuator as a redundant axis, sets a target position / posture of the manipulator, Generates a motion trajectory from when the manipulator reaches the target position / posture, and associates the sub-regions within the reachable range of the manipulator with the redundant axis angle parameters corresponding to each sub-region. The redundant angle definition table 7 is attached, the corresponding small area in the redundant angle definition table is selected, the redundant axis angle is set based on the selection result, and the operation command is issued based on the operation locus and the redundant axis angle. Generate.
[Selection] Figure 1

Description

  The present invention relates to a robot system and a robot control apparatus using a manipulator.

An articulated robot may be required to control a robot having more degrees of freedom (redundancy degrees of freedom) than the required position and orientation degrees of freedom.
In other words, since the robot has redundant degrees of freedom, the posture of the manipulator can be changed in various ways even when the position and orientation obtained with respect to the object is taken, so that the redundant degrees of freedom are appropriately controlled. Thus, there is an advantage that even in a relatively narrow place, the work can be performed while avoiding interference with the manipulator itself and surrounding objects during the work.
Patent Document 1 discloses a technique for controlling a manipulator having redundant degrees of freedom.

JP 2007-203380 A

By the way, many manipulators operate along an operation path taught in advance. However, depending on the application, there are cases where work is performed on a work object whose position is uncertain, such as the work object moving. In such a case, the target position and orientation of the manipulator are corrected based on information such as a sensor that detects the position of the work object. Thereby, the work can be performed while the manipulator follows the work object.

However, when the manipulator has redundant degrees of freedom, as in Patent Document 1, in the method in which the operation amounts of all the drive shafts including the redundant degrees of freedom are determined by teaching in advance, the position of the work object as described above is used. When the robot moves, the position of the redundancy degree of freedom is not necessarily an appropriate position, and interference (contact) or the like may occur with the work object or the manipulator itself.
Moreover, it is conceivable that the manipulator is unintentionally changed to a singular point posture and the manipulator is in a troubled state by correcting the motion path following the work target with respect to the taught motion path.
Such inconvenience is not limited to correcting the target position and orientation based on a sensor or the like, but when an obstacle existing in the operating range of the manipulator moves or when an unintended contact between the manipulator and surrounding objects occurs. The same inconvenience may occur when correcting the motion trajectory of the above.

  The present invention has been made in view of such problems, and an object of the present invention is to provide a robot system and a robot control apparatus that can more optimally control a manipulator having a redundancy degree of freedom.

  In order to solve the above problems, a robot system of the present invention includes a manipulator having one or more actuators and a controller for controlling the operation of each actuator, and the controller uses a part of the actuator as a redundant axis. A redundant axis setting unit to be set, a target position / posture generation unit for setting a target position / posture of the manipulator, and a simulation unit for generating an operation trajectory until the manipulator reaches the target position / posture from the current posture based on the target position / posture; , Based on the output result of the target position and orientation generation unit, and the redundant angle definition table that associates the small region set by dividing the region within the reachable range of the manipulator with the redundant axis angle parameter corresponding to each small region Region determining means for selecting a corresponding small region in the redundant angle definition table, and region determining means The redundant axis angle setting unit for setting the redundant axis angle based on the selection result and the redundant angle definition table, and the interpolation calculation unit for generating an operation command for each of the actuators based on the operation locus and the redundant axis angle It is characterized by having.

  Moreover, it is preferable to have an object detection sensor that detects the position of the work target, and the target position and orientation generation unit corrects the target position and orientation based on the detection result of the object detection sensor.

An external force sensor that detects an external force applied to the manipulator, and the controller controls the operation of the actuator and performs an avoidance operation in a direction in which the actuator avoids the external force based on the detection result of the sensor unit. It is preferable to have an avoidance operation execution unit.
The controller preferably includes a table display / input unit that displays a redundant angle definition table and receives correction of the contents of the redundant angle definition table.
The manipulator includes a base, a first joint that operates the first structural member relative to the base, a second joint that operates the second structural member relative to the first structural member, and a second structural member. On the other hand, a third joint that operates the third structural member, a fourth joint that operates the fourth structural member relative to the third structural member, and a fifth joint that operates the fifth structural member relative to the fourth structural member. And a sixth joint that operates the sixth structural member relative to the fifth structural member, and a seventh joint that rotates the flange relative to the sixth structural member.

Moreover, it is preferable that each of the small areas defined in the redundant angle definition table has a rectangular parallelepiped shape.
Moreover, it is preferable that the small area defined in the redundant angle definition table is set in advance in consideration of the variation in the position of the work object.

Preferably, the region determination means outputs an abnormal signal to the object detection sensor when it is determined from the output result of the target position and orientation generation unit that the corresponding small region does not exist in the redundant angle definition table.
The robot control apparatus of the present invention is a robot control apparatus that controls the operation of a manipulator having one or more actuators, and includes a redundant axis setting unit that sets a part of the actuators as redundant axes, and a target of the manipulator A target position and orientation generation unit that sets the position and orientation, a simulation unit that generates an operation trajectory until the manipulator reaches the target position and orientation based on the target position and orientation, and an area within the reachable range of the manipulator And a redundant angle definition table that associates the small area set with the parameters for the redundant axis angle corresponding to each small area, and the corresponding small area in the redundant angle definition table based on the output result of the target position and orientation generation unit The redundant axis angle based on the area determination means for selecting, the selection result of the area determination means and the redundant angle definition table Based on the redundant axis angle setting unit for setting the operation path and a redundant axis angle, it is characterized by having a interpolation calculation unit for generating an operation command for each actuator.

  According to the present invention, even when the target position and orientation is changed, the redundant axis of the manipulator can be appropriately set with a smaller amount of calculation, and the manipulator having redundant degrees of freedom can be more optimally controlled. it can.

Schematic configuration diagram for explaining the overall configuration of a robot system according to an embodiment of the present invention It is a side view showing the composition of the robot concerning the embodiment of the present invention. Schematic structure diagram showing the configuration of the sensor unit Schematic diagram for explaining the region of the three-dimensional space defined in the redundant angle definition table The figure which shows typically an end effector and a target part (work object) coordinate relationship The figure which shows an example of a redundant angle definition table

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
In the present embodiment, an embodiment of the present invention will be described using a robot system that performs operations such as inspection, disinfection, and milking of livestock using the manipulator 2 as an example.

As shown in FIG. 1, the robot system 100 according to the present embodiment includes a floor F in which livestock 1 is stored, a manipulator 2, a controller 3, and a sensor unit 4.
As shown in FIG. 2, the manipulator 2 includes a base 40 fixed to an installation surface (wall surface, floor, etc.) 101, and a first structural member 41 and a second structure in order from the base 40 to the tip of the manipulator 2. The material 42, the third structure material 43, the fourth structure material 44, the fifth structure material 45, the sixth structure material 46, and the flange 47 are connected via an actuator (rotary joint) that is driven to rotate.

  The base 40 and the first structural member 41 are connected via a first actuator (first joint) 41A, and the first structural member 41 is rotated by driving the first actuator 41A. . The first structural member 41 and the fourth structural member 42 are connected via a second actuator (second joint) 42A, and the second structural member 42 turns by driving the second actuator 42A. ing.

The second structural member 42 and the third structural member 43 are connected via a third actuator (third joint) 43A, and the third structural member 43 is rotated by driving the third actuator 43A. ing.
The third structural member 43 and the fourth structural member 44 are connected via a fourth actuator (fourth joint) 44A, and the fourth structural member 44 turns by driving the fourth actuator 44A. ing.

The fourth structural member 44 and the fifth structural member 45 are connected via a fifth actuator (fifth joint) 45A, and the fifth structural member 45 is rotated by driving the fifth actuator 45A. ing. The fifth structural member 45 and the sixth structural member 46 are connected via a sixth actuator (sixth joint) 46A, and the sixth structural member 46 turns by driving the sixth actuator 46A. ing.
The sixth structural member 46 and the flange 47 are connected via a seventh actuator (seventh joint) 47A, and the end effector 2A such as a hand attached to the flange 47 and the flange 47 by driving the seventh actuator 47A. Is designed to rotate.

  In the present embodiment, the third actuator (third joint) 43A is preset as a redundant axis (redundant axis setting unit), and the rotational position of the third actuator 43A is optimally controlled by the function of the controller 3 described later. Will be. In addition, the actuator 43A is referred to as a redundant axis, the actuators 41A and 42A are referred to as a first axis and a second axis, respectively, and the actuators 44A to 47A are referred to as a third axis to a sixth axis, respectively. When the first to sixth axes are not specified, they are simply referred to as axes.

Various tools (not shown) such as an inspection device, a milking device, and a disinfection device are attached to the end effector 2A, and operations such as inspection, disinfection, and milking are performed on the target portion 1A of the livestock 1. It has become. In the case of the manipulator 2 of the present embodiment, the control point that is the position for controlling the operating position of the manipulator 2 is a point that can be linearly moved by driving the actuators 44A to 47A. It is located near the axis of 46A.
The positional relationship among the control point, the flange 47A, the end effector 2A, and each tool is input to the controller 3 in advance, and the position of each tool can be controlled by controlling the position and orientation of the control point. .
In addition, an object detection sensor (in this embodiment, a camera but other various sensors can be applied) 2B is attached to the end effector (in other words, the tip portion of the manipulator 2) 2A.

The object detection sensor 2B is oriented with respect to a sufficiently wide detection region including the target portion 1A of the livestock 1 that moves irregularly within the floor F partitioned on all sides.
The image acquired by the object detection sensor 2B is input to the target recognition unit 2C of the controller 3, and the image recognition unit 2C detects the position of the target unit 1A of the livestock 1 and uses the detection result as a target position for the operation command unit 5 described later. It is designed to input in real time.
In the present embodiment, for ease of explanation, the controller 3 is shown as an integrated control device. However, the controller 3 is actually composed of a plurality of computers and the like, and a target recognition unit 2C and an external force detection unit (described later). ) May be configured by a control device that is separate from the robot controller that controls the operation of the manipulator 2.

  Each of the first to seventh actuators 41A to 47A is constituted by a speed reducer-integrated servo motor having a hollow portion through which a cable (not shown) can be inserted, and the rotational position of each actuator depends on the actuator. A signal from a built-in encoder is input to the controller 3 via a cable.

  As shown in FIG. 3, the sensor unit 4 includes a sensor fixing jig 25 and four sensors S <b> 1 to S <b> 4, and the disk-shaped sensor fixing jig 25 is a stator of the first actuator 41 </ b> A of the manipulator 2. Attached to the base.

Each sensor S1-S4 is embedded in the disk-shaped sensor fixing jig 25, and each sensor S1-S4 is arrange | positioned at equal intervals along the same circular arc (virtual circle). Each of the sensors S1 to S4 uses a crystal as a piezoelectric body, and each of the sensors S1 to S4 detects the amount of strain in the radial direction of the sensor fixing jig 15 as a voltage, and the obtained voltage is an amplifier unit. It is amplified via (not shown) and input to the controller 3.
By using a crystal resonator (quartz piezoelectric element) for each of the sensors S1 to S4, the response time is smaller than when a strain gauge or other piezoelectric element is used, and a satisfactory response compared to the calculation cycle of the servo unit 14 described later. Therefore, it is possible to sufficiently mitigate an impact caused by contact with an object around the manipulator.

In the present embodiment, as described above, a crystal resonator is used for reasons such as fast response time. However, any type of sensor S1 to S4 can be used as long as good response can be obtained. Can be applied. In addition, the quartz piezoelectric sensor is superior in durability compared to a strain gauge and a normal collision sensor, and can detect sufficiently sufficiently even when the sensor unit 4 is provided at the base end where the load due to the weight of the manipulator 2 is the largest. There is also an advantage of being able to.
The operation of each drive part including each actuator 41 </ b> A to 47 </ b> A of the manipulator 2 is controlled by the controller 3.

The controller 3 is composed of a computer having a storage device, an electronic calculator, an input device, and a display device (all of which are not shown in detail), and is connected to each drive part of the manipulator 2 so as to be able to communicate with each other.
As shown in FIG. 1, the controller 3 includes a simulation unit 5, a redundant angle definition table generation unit 6, a redundant angle definition table 7, a table display / input unit 8, a target position and orientation generation unit 10, an area determination unit 11, The redundant axis angle setting unit 12, the interpolation calculation unit 13, the servo control unit 14, the amplifier unit 15, and the external force detection unit (avoidance operation execution unit) 16 are included.

The simulation unit 5 receives information such as the position of an obstacle in the operable region of the manipulator 2 input in advance, the target position / posture of the control point from the target position / posture generation unit 10, and detection signals of the encoders of the actuators 41A to 47A. Based on the current position and orientation of the manipulator 2 obtained from the above, an operation locus from the current position and orientation to the target position and orientation is generated. Note that various algorithms can be applied to the algorithm for generating the motion trajectory.
The generation of the motion trajectory by the simulation unit 5 may be performed during the teaching operation of the manipulator 2. In the automatic operation, the redundant angle can be determined by referring to the redundant axis angle definition table 7, and the solution (target position command for each actuator 41A to 47A) is determined in a practical time while reducing the load on the CPU. Is possible.

  As shown in FIG. 4, the redundant angle definition table generating unit 6 generates a manipulator for a region 101 in a three-dimensional space set in advance in the operable region of the manipulator 2 based on the input information from the table display / input unit 8. 2 and the surrounding objects or the movement of the livestock 1 are taken into consideration, and the region 101 is divided into discrete regions according to experiments performed in advance.

The region 101 and each small region (m, n, l) are related to the reference coordinate system Σc by the reference coordinate system Σw of the manipulator 2 and the simultaneous conversion matrix. The region 101 is divided into L pieces in the Z direction, N pieces in the Y direction, and M pieces in the X direction based on the experimental results as described above.
More specifically, the redundant angle definition table generating unit 6 matches the region 101 in accordance with the solid variation of the livestock 1 (for example, the variation in the position of the target unit 1A or the position of an obstacle such as the leg of the livestock 1). Determine the size of. That is, when the variation is large, the size of the area 101 is set to be large, and when the variation is small, the size is set to be small.
Here, each divided small region Sc (m, n, l) is set to be a rectangular parallelepiped. The shape of the small region Sc (m, n, l) is preferably a square in terms of suppressing the amount of calculation, but can be set to various shapes as necessary.

The redundant angle definition table 7 is associated with parameters for setting the position of the redundant axis for each of the divided areas 101.
Information on the region 101, that is, information on the position, size, shape, etc. of the small region Sc (m, n, l) and information on the parameters corresponding thereto are displayed on the display device by processing of the table display / input unit 8. Such information can be input and corrected by an operator via an input device.

  The target position / posture generation unit 10 generates the target position / posture of various tools held by the end effector 2A of the manipulator 2 based on input information from the object detection sensor 4 and input from an external force detection unit 16 described later. It is configured as follows.

  On the other hand, when the external force detection unit 16 detects an external force, the target position / posture generation unit 10 temporarily stops calculation based on the detection result of the object detection sensor 4 and avoids contact with the target position / posture input from the external force detection unit 16. The target position and orientation is obtained by reflecting the correction value for use in the latest target position and orientation.

The area determination unit 11 selects the corresponding small area S of the redundant angle definition table from the small areas Sc (m, n, l) from the output of the target position / orientation generation unit 10 and the output of the redundant angle definition table 7. It is like that.
In addition, when the region determination unit 11 determines that there is no small region corresponding to the output of the target position and orientation generation unit in the small region Sc (m, n, l), an abnormal signal is sent to the object detection sensor 4. For example, avoidance work such as focus adjustment is performed on the object detection sensor 4 side.
Alternatively, if it is determined that there is no small area corresponding to the output of the target position / orientation generation unit 10 in the small area Sc (m, n, l), an error warning is displayed via the display device of the controller 3. It may be configured to.
The redundant axis angle setting unit 12 sets the angle (target position) of the redundant axis based on the parameters of the redundant angle definition table 7 corresponding to the region selected by the region determining unit 11.

  The interpolation calculation unit 13 performs reverse kinematics calculation based on the angular position of the redundant axis 43A set by the redundant axis angle setting unit 12 and the motion trajectory obtained by the simulation unit 5, and performs the target position / posture from the current position. The operation modes (or the operation speeds) of the respective actuators 41A to 47A are interpolated and the target position commands of the respective actuators 41A to 47A are output every predetermined calculation cycle.

  The servo control unit 14 performs position / velocity feedback based on the target position commands of the actuators 41A to 47A output from the interpolation calculation unit 13 at predetermined intervals and the input signals from the encoders of the actuators 41A to 47A. It comes to perform control.

The amplifier unit 15 performs operation control of the actuators 41A to 47A based on the operation command output from the servo control unit 14.
The external force detector 16 detects whether or not an external force is generated on the manipulator 2 based on the input signals from the sensors S1 to S4, and avoids contact of the target position for the manipulator 2 to avoid the external force. Correction values are obtained and input to the target position and orientation generation unit 10 side.

Since the robot system 100 according to an embodiment of the present invention is configured as described above, at the start of work, the object detection sensor 4 measures the position of the target unit 1A of the livestock 1, and the measurement result is input to the controller 3. The target position generation unit 10 calculates the target position and orientation of the control point.
Note that the measurement information obtained from the object detection sensor 4 is three pieces of positional information with three degrees of freedom of translation of X, Y, and Z, and has four degrees of freedom with respect to seven degrees of freedom of movement of the manipulator 2. become.

  Therefore, in order to generate a target position / posture of 6 degrees of freedom of rotation 3 degrees of freedom in addition to 3 degrees of freedom of translation based on the detection information of the object detection sensor 4 of the present embodiment, the target unit 1A is a nipple of a domestic animal. As shown in FIG. 5, the nipple is assumed to be a uniform shape around the Z ″ axis, and the target portion 1 </ b> A faces downward, so that Y ″ and X “By assuming that the rotation angle around is constant, the target position generation unit 10, the region determination unit 11, and the redundant axis angle setting unit 12 change Rz (rotation position about the Z” axis) and elbow angle Ex to the redundant axis angle. It is determined by Table 7.

  That is, the position and orientation (Xs, Ys, Zs) of the target unit 1A based on the object detection sensor coordinate system obtained by the object detection sensor 4 are input to the region determination unit 11 and are exemplified in FIG. With reference to the redundant axis angle definition table 7, the number of the corresponding small area is determined from the small areas Sc (m, n, l).

For example, if region No. 2 is selected in FIG. 6, 47 and 75 are set by the redundant axis angle setting unit 12 as the values of Rz and Ex, respectively. The set position and orientation angle are input to the interpolation calculation unit 13, target position commands for each of the actuators 41 </ b> A to 47 </ b> A are generated based on inverse kinematics, and input to the servo control unit 14 and the amplifier unit 15. The manipulator 2 positions the target portion 1A on the target portion 1A (see FIG. 5). The above operation is repeatedly executed at every sampling period of the measurement frequency (for example, 100 Hz or more) of the object detection sensor 4.
Note that the calculation cycle of the position / velocity feedback control in the servo control unit 14 is shorter than the sampling cycle.

As described above, according to the robot system according to the present embodiment, the target position and orientation are calculated for each calculation cycle based on the detection result input from the object sensor 4 for each measurement frequency, and the redundant angle is defined based on the target position and orientation. Since the angular position of the redundant axis is set from the collation result with the table 7, the redundant axis of the manipulator can be appropriately set with a smaller amount of calculation, and the manipulator having redundant degrees of freedom can be controlled more optimally. it can.
Thus, even when the manipulator 2 is operated in a narrow work space where an obstacle such as a leg portion of the livestock 1 moves, the posture of the redundant shaft is changed according to the position of the target portion 1A. The contact probability between the manipulator 2 and the obstacle can be reduced, and work efficiency can be maintained high.

  Further, even when the manipulator 2 and the obstacle are in contact with each other, the target position / posture to be avoided with respect to the external force is corrected by the external force detection unit (avoidance operation executing unit) 16. Obstacles can be avoided without touching. Further, since the angular position of the redundant axis is set from the collation result with the redundant angle definition table 7 based on the corrected target position and orientation during the contact avoiding operation, another obstacle is newly detected during the obstacle avoiding operation. It is possible to appropriately control the posture of the manipulator 2 so as to reduce the contact probability.

The embodiments of the present invention have been described above. However, the present invention is not limited to the above-described embodiments, and can be appropriately changed from the above embodiments without departing from the spirit of the present invention. Moreover, it is also possible to use combining the method of said each embodiment suitably. That is, it is needless to say that even a technique in which such changes are made is included in the technical scope of the present invention. Note that the present invention is limited to the use of working on livestock as in the embodiment. It is not a thing and can be applied to various uses. For example, the present invention can be applied to a case where the position of an obstacle within the operation range of the manipulator varies.
In the above-described embodiment, a seven-degree-of-freedom manipulator having seven actuators has been described as an example. However, the degree of freedom of the manipulator is not limited to this, and the data of the redundant angle definition table can be set appropriately. The present invention can also be applied to a device having 8 degrees of freedom or more.
Further, even if the manipulator has 6 degrees of freedom or less, if one or more degrees of freedom are allowed in the approach to the work object, the manipulator has a relative degree of freedom. Even in such a case, the present invention can be applied.

DESCRIPTION OF SYMBOLS 1 Livestock 2 Manipulator 2A End effector 2B Object detection sensor 3 Controller 4 Visual sensor 5 Simulation part 6 Redundant angle definition table generation part 7 Redundant angle definition table 8 Redundant angle table display part 10 Target position and posture generation part 11 Area determination part 12 Redundant axis Angle setting unit 13 Interpolation calculation unit 14 Servo control unit 15 Amplifier unit 40 Bases 41 to 46 Arm structure materials 41A to 47A Actuator 47 Flange

Claims (9)

A manipulator having one or more actuators;
A controller for controlling the operation of each of the actuators,
The controller is
A redundant axis setting unit for setting a part of the actuator as a redundant axis;
A target position and orientation generation unit for setting a target position and orientation of the manipulator;
A simulation unit that generates an operation trajectory until the manipulator reaches the target position and posture from the current posture based on the target position and posture;
A redundant angle definition table that associates a small area set by dividing an area within the reachable range of the manipulator with a parameter for a redundant axis angle corresponding to each small area;
An area determination means for selecting the corresponding small area in the redundant angle definition table based on the output result of the target position and orientation generation unit;
Based on the selection result of the region determination means and the redundant angle definition table, a redundant axis angle setting unit for setting the redundant axis angle, and for each of the actuators based on the motion locus and the redundant axis angle And an interpolation calculation unit that generates an operation command. An object detection sensor for detecting the position of the work object;
The robot system according to claim 1, wherein the target position and orientation generation unit corrects the target position and orientation based on a detection result of the object detection sensor. An external force sensor for detecting an external force applied to the manipulator;
The controller is
In addition to controlling the operation of the actuator, the actuator has an avoidance operation execution unit that performs an avoidance operation in a direction to avoid the external force based on a detection result of the sensor unit. The robot system according to claim 1 or 2. The controller is
4. The display device according to claim 1, further comprising a table display / input unit that displays the redundant angle definition table and receives correction of the contents of the redundant angle definition table. 5. Robot system. The manipulator
The base,
A first joint for operating the first structural member relative to the base;
A second joint for operating a second structural material relative to the first structural material;
A third joint for operating the third structural member relative to the second structural member;
A fourth joint for operating the fourth structural member relative to the third structural member;
A fifth joint for operating the fifth structural member relative to the fourth structural member;
A sixth joint for operating the sixth structural member relative to the fifth structural member;
The robot system according to claim 1, further comprising: a seventh joint that rotates a flange with respect to the sixth structural member.   The robot system according to any one of claims 1 to 5, wherein each of the small areas defined in the redundant angle definition table has a rectangular parallelepiped shape. The robot system according to any one of claims 2 to 6, wherein the small area defined in the redundant angle definition table is set in advance in consideration of variations in the position of the work object. . The region determination means outputs an abnormal signal to the object detection sensor when it is determined from the output result of the target position and orientation generation unit that the corresponding small region in the redundant angle definition table does not exist. The robot system according to any one of claims 2 to 7, characterized in that: A robot controller for controlling the operation of a manipulator having one or more actuators,
A redundant axis setting unit for setting a part of the actuator as a redundant axis;
A target position and orientation generation unit for setting a target position and orientation of the manipulator;
A simulation unit that generates an operation trajectory until the manipulator reaches the target position and posture from the current posture based on the target position and posture;
A redundant angle definition table that associates a small area set by dividing an area within the reachable range of the manipulator with a parameter for a redundant axis angle corresponding to each small area;
An area determination means for selecting the corresponding small area in the redundant angle definition table based on the output result of the target position and orientation generation unit;
Based on the selection result of the region determination means and the redundant angle definition table, a redundant axis angle setting unit for setting the redundant axis angle, and for each of the actuators based on the motion locus and the redundant axis angle And an interpolation calculation unit for generating an operation command.

Images