Link mechanism control device

Abstract

To adequately control directivity of a radio wave of a control signal when the control signal is transmitted to a link of a link mechanism.SOLUTION: A link mechanism control device 1 includes: a link mechanism 10 which has a plurality of links 11 connected by joints; a link side control part 20 which has a link side communication part 21 arranged on the link 11 and a joint control part 22 which changes an angle of the joint according to the control signal and transmits angle information after changing the joint; a base side communication part 30; and a comprehensive control part 40 which transmits the control signal for instructing a change of the angle of the joint to the link side control part 20 and receives the angle information after changing the joint through the base side communication part 30. The comprehensive control part 40 controls directivity of the base side communication part 30 so that a radio wave is transmitted to a position of the link side communication part 21 specified by using the angle information, which is received from the link side control part 20, after changing the joint when transmitting the control signal.SELECTED DRAWING: Figure 1

Description

[Technical field]

[0001] The present invention relates to a link mechanism control device that controls a link mechanism. [Background technology]

[0002] Conventionally, in a link mechanism in which multiple links, such as a robot arm, are connected by joints, the joints of the link mechanism are driven by wirelessly transmitting and receiving control signals in order to reduce the amount of control wiring (see, for example, Patent Document 1). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent Publication No. 2021-151675 [Patent Document 2] JP 2022-055683 A [Patent Document 3] JP 2016-046657 A Summary of the Invention [Problem to be solved by the invention]

[0004] When using radio waves with short wavelengths such as millimeter waves to wirelessly transmit a control signal to a link of a link mechanism, it is possible to form a directional beam in order to ensure gain. However, if the angle of the joint is not changed as instructed by the control signal due to an external force or other factors, a difference will occur between the position of the communication unit on the link side recognized by the side transmitting the control signal and the actual position of the communication unit on the link side, and it will not be possible to achieve appropriate transmission and reception of the control signal thereafter. Here, the factors that cause the angle of the joint not to be changed as instructed by the control signal are not particularly limited, but the following factors can be cited, for example:

[0005] (1) When a torque exceeding the allowable torque is required. For example, when a link mechanism is used to transport an object that is heavier than expected, a joint that requires a torque exceeding the allowable torque may not be able to change to the desired angle. In this case, a difference occurs between the angle specified by the control signal and the actual angle after the joint is changed.

[0006] (2) When the power required for operation is not sufficiently supplied. For example, a motor that drives a certain joint may not be supplied with enough power, and as a result, the joint may not be able to change to the desired angle. In this case, too, a difference occurs between the angle specified by the control signal and the actual angle after the joint is changed.

[0007] (3) When the control flow of the motor driving a joint goes into an unstable mode. For example, when the motor driving a certain joint goes into an unstable mode, such as when it vibrates, the joint may not be able to change to the desired angle. In this case, too, a difference occurs between the angle specified by the control signal and the actual angle after the joint is changed.

[0008] (4) In the case of an emergency stop caused by a safety device, etc. For example, if physical interference between the link mechanism and surrounding equipment is detected and the link mechanism is stopped in an emergency by a safety device, etc., the joint cannot be changed to the desired angle. In this case, too, a difference occurs between the angle instructed by the control signal and the actual angle after the joint is changed.

[0009] As described in the above Patent Documents 2 and 3, the positions of mobile wireless devices and terminal stations are estimated using wireless technology. Therefore, it is possible to estimate the position of a communication unit on the link side in a similar manner, but when the method described in Patent Document 2 is applied to communication using radio waves with short wavelengths such as millimeter waves, it is necessary to scan the beam, which causes a problem of taking extra time. In addition, when the method described in Patent Document 3 is used to estimate the position of a communication unit provided on a link of a link mechanism, there is a problem that the fingerprint may not be used to properly identify the position because the fingerprint depends on the shape of the link mechanism and the object held by the hand unit at the tip of the link mechanism.

[0010] The present invention has been made in consideration of the above-mentioned situation, and aims to provide a link mechanism control device that can control the directivity of radio waves so that, when transmitting radio waves to a communication section on the link side of a link mechanism, the radio waves are transmitted appropriately to the communication section on the desired link side, even if the angle of the joint has not been changed as instructed by the control signal. [Means for solving the problem]

[0011] In order to achieve the above-mentioned object, a link mechanism control device according to one embodiment of the present invention comprises a link mechanism having a plurality of links connected by joints driven by a drive means, one or more link side control units having link side communication units for transmitting and receiving radio waves, each of which is provided on one or more of the plurality of links, and a joint control unit that changes the angle of the joint in response to a control signal received via the link side communication units and transmits information including the changed angle of the joint via the link side communication units (hereinafter referred to as "angle information"), one or more base side communication units for transmitting and receiving radio waves with the one or more link side communication units, and an overall control unit that transmits a control signal instructing each of the one or more link side control units to change the angle of the joint via the one or more base side communication units and receives the changed angle information of the joints of the link mechanism, and when transmitting a control signal, the overall control unit controls the directivity of the base side communication units so that radio waves are transmitted to the position of the link side communication unit identified using the changed angle information of the joint received from the link side control unit.

[0012] With this configuration, even if the angle of the joint is not changed as instructed by the control signal, the overall control unit can identify the position of the link side communication unit, and by controlling the directivity of the radio wave of the control signal according to the identification result, it is possible to realize appropriate transmission of the control signal to the link side communication unit. Also, unlike the case where the method of Patent Document 2 is applied to communication using radio waves with short wavelengths such as millimeter waves, there is no need to scan the beam, so no extra time is required accordingly. Also, unlike the method of Patent Document 3, the position of the link side communication unit can be identified without using a fingerprint.

[0013] In addition, in a link mechanism control device according to one aspect of the present invention, the joint control unit may transmit changed angle information of the joint to the overall control unit via the link side communication unit only when the angle of the joint changed in response to a control signal received from the overall control unit differs from the angle indicated by the control signal.

[0014] With this configuration, the overall control unit can identify the changed angle of each joint by receiving changed angle information of the joint, or by not receiving changed angle information of the joint, and can accordingly identify the position of each link side communication unit.

[0015] In addition, in a link mechanism control device according to one embodiment of the present invention, when the base side communication unit and the link side communication unit to which the control signal is sent cannot communicate line of sight, the overall control unit may control the directivity of the base side communication unit so that the reflected wave of the radio wave emitted from the base side communication unit is received by the link side communication unit.

[0016] With this configuration, it is possible to realize appropriate transmission of control signals from the base station side communication section to the link side communication section by utilizing the reflection of radio waves as well.

[0017] Furthermore, in a link mechanism control device according to one aspect of the present invention, when the overall control unit sends a control signal to a certain joint control unit, it sends a control signal including current angle information of joints other than the joint whose angle is to be changed by that joint control unit, and when the joint control unit of the link side control unit sends changed angle information of the joint whose angle has been changed based on the control signal sent from the overall control unit, it may control the directivity of the link side communication unit so that radio waves are transmitted from the position of the link side communication unit of the link side control unit identified using the current angle information of each joint included in the control signal to the position of the base side communication unit.

[0018] With this configuration, it is possible to realize transmission with appropriate directivity even when transmitting radio waves from the link-side control unit to the base station-side communication unit. Effect of the Invention

[0019] According to one aspect of the link mechanism control device of the present invention, even if the angle of the joint is not changed as instructed by the control signal, the control signal can be more reliably transmitted to the intended link side communication section. [Brief description of the drawings]

[0020] [Figure 1] FIG. 1 is a schematic diagram showing a configuration of a link mechanism control device according to an embodiment of the present invention; [Diagram 2] A flowchart showing the operation of the link mechanism control device according to the embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0021] A link mechanism control device according to the present invention will be described below using an embodiment. In the following embodiments, components and steps with the same reference numerals are the same or equivalent, and repeated description may be omitted. The link mechanism control device according to this embodiment transmits angle information after a change in the joint in the link mechanism from the link side control unit to the overall control unit, so that even if the angle of the joint is not changed as instructed by the control signal, the actual position of the link side communication unit can be identified, and by controlling the directivity when transmitting the control signal according to the identification result, more reliable transmission of the control signal to the link side communication unit can be achieved.

[0022] 1 is a schematic diagram showing the configuration of a link mechanism control device 1 according to this embodiment. The link mechanism control device 1 according to this embodiment includes a link mechanism 10 having a plurality of links 11a to 11d connected by joints 12a to 12c, link side control units 20b and 20c provided on the links 11b and 11c of the link mechanism 10, base side communication units 30a and 30b, and an overall control unit 40. When the links 11a to 11d, the joints 12a to 12c, the link side control units 20b and 20c, and the base side communication units 30a and 30b are not particularly distinguished from one another, they may be referred to as the link 11, the joint 12, the link side control unit 20, and the base side communication unit 30. The same applies to the other components.

[0023] The link mechanism 10 has a plurality of links 11a, 11b, 11c, and 11d connected by joints 12a, 12b, and 12c driven by a driving means. In the link mechanism 10, the four connected links 11 are rotatable at three joints 12 by a driving means. In this embodiment, the link mechanism 10 is mainly described as having motors 13a, 13b, and 13c as driving means for driving the joints 12a, 12b, and 12c. In addition, each joint 12 of the link mechanism 10 may be provided with a sensor such as an encoder for acquiring angle information of the joint 12. In addition, among the plurality of links 11, the link 11a on the base end side may be fixed to a base 15, for example.

[0024] The link mechanism 10 may be, for example, a vertical multi-joint robot, a horizontal multi-joint robot, or another robot arm in which a plurality of links 11 are connected by joints 12. The link mechanism 10 may also be, for example, something other than a robot arm. There is no particular limitation on the link mechanism 10 other than a robot arm, but examples of the link mechanism 10 include the legs of a quadruped robot, and the arms and legs of a humanoid robot.

[0025] In the present embodiment, the link mechanism 10 is mainly described as having four links 11, but the number of links 11 included in the link mechanism 10 is not limited. For example, the link mechanism 10 may have two or three links 11, or may have five or more links 11. The multiple links 11 are usually connected in series. In the present embodiment, for convenience of explanation, the hand unit (end effector) at the tip of the link mechanism 10 is also described as one link 11. The hand unit may, for example, hold a transport object or the like, perform work such as assembly or welding, or may be used for other purposes. Power may be supplied to each link 11 by using, for example, a wired wiring, or may be supplied wirelessly at a joint portion as in Patent Document 1. In addition, when the link 11a on the base end side is fixed to the base 15, the supply of power to the motor 13a included in the link 11a and the transmission of a control signal may be performed by using a wired wiring. In this embodiment, this case will be mainly described.

[0026] Links 11b and 11c are provided with link side control units 20b and 20c, respectively. The link side control unit 20 has a link side communication unit 21 for transmitting and receiving radio waves, and a joint control unit 22 for changing the angle of the joint 12 in response to a control signal received via the link side communication unit 21 and transmitting angle information of the joint 12 after the change via the link side communication unit 21. The radio waves used for transmitting and receiving the control signal and the angle information of the joint 12 after the change may be, for example, millimeter waves, submillimeter waves, terahertz waves, or other radio waves with a wavelength of 10 mm or less, taking into consideration reduction of radio wave interference with peripheral devices. In this embodiment, a case where the control signal and the like are transmitted and received by millimeter wave radio waves will be mainly described.

[0027] In this embodiment, the link mechanism 10 has a plurality of links 11a to 11d, and two links 11b and 11c are provided with the link side control units 20b and 20c, respectively. However, it is sufficient that one or more link side control units 20 are provided for each of the plurality of links 11 in the link mechanism 10, and the number of links 11 provided with the link side control units 20 does not matter. For example, the link 11b may not be provided with the link side control unit 20b, and the motor 13b of the joint 12b may also be controlled by the link side control unit 20c provided for the link 11c. In addition, for example, when the link 11d, which is a hand unit, also has a control target such as a driving means, the link 11d may also be provided with the link side control unit 20.

[0028] The link side communication unit 21 may have, for example, an antenna for transmitting and receiving radio waves, and a communication circuit for transmitting and receiving radio signals via the antenna. It is preferable that the antenna is capable of changing the directivity of the radio waves to be transmitted and received. It is preferable that the antenna is disposed outside the housing of the link 11. When transmitting and receiving radio waves with short wavelengths such as millimeter waves, the cost of the cable connecting the antenna and the communication circuit increases. Therefore, for example, the antenna and the communication circuit may be provided integrally. In this case, for example, the link side communication unit 21 may be disposed outside the housing of the link 11. Also, for example, an antenna cover (radome) for protecting the antenna may be provided. A material with high radio wave transmittance is usually used for the antenna cover.

[0029] The joint control unit 22 may, for example, control the angle of the joint 12 on the tip side of the link 11 to which the joint control unit 22 is provided, or may control the angle of the joint 12 on the base end side of the link 11, or may do both. In this embodiment, a case in which the joint control unit 22 controls the angle of the joint 12 on the tip side of the link 11, that is, a case in which the joint control unit 22b controls the motor 13b to control the angle of the joint 12b, and the joint control unit 22c controls the motor 13c to control the angle of the joint 12c, will be mainly described.

[0030] When the joint control unit 22 receives a control signal via the link side communication unit 21, the joint control unit 22 drives the motor 13 in response to the control signal to change the angle of the joint 12. The control signal may include, for example, absolute angle information of the joint 12 after the change, or may include a difference in the angle of the joint 12, i.e., relative angle information. When the control signal includes absolute angle information of the joint 12 after the change, the joint control unit 22 may control the motor 13 so that the angle of the joint 12 becomes the absolute angle. When the control signal includes relative angle information of the joint 12, the joint control unit 22 may control the motor 13 so that the angle of the joint 12 is changed by the relative angle. In this embodiment, a case where the control signal includes absolute angle information of the joint 12 after the change will be mainly described.

[0031] The joint control unit 22 transmits the angle information of the joint 12 after the change to the overall control unit 40 via the link side communication unit 21 and the base side communication unit 30. This angle information to be transmitted may be, for example, absolute angle information or relative angle information. In this embodiment, a case in which the absolute angle information of the joint 12 after the change is transmitted will be mainly described. This angle information of the joint 12 after the change may be acquired by a sensor such as an encoder. Furthermore, the joint control unit 22 may be disposed inside the housing of the link 11, for example.

[0032] The base side communication unit 30 transmits and receives radio waves to and from one or more link side communication units 21. The link side communication unit 21 changes its position according to the change in the angle of each joint 12 of the link mechanism 10, whereas the base side communication unit 30 is a fixed communication unit whose position does not change. Therefore, for example, the base side communication unit 30 may be considered to correspond to an access point whose position is fixed, and the link side communication unit 21 may be considered to correspond to a mobile terminal (STA: station) whose position may change. The base side communication unit 30 may have, for example, an antenna for transmitting and receiving radio waves, and a communication circuit for transmitting and receiving wireless signals via the antenna. It is preferable that the antenna is capable of changing the directivity of the radio waves to be transmitted and received. In the base side communication unit 30, for example, the antenna and the communication circuit may also be provided integrally. Also, for example, an antenna cover for protecting the antenna may be provided.

[0033] The antenna capable of changing the directivity may be, for example, an array antenna such as a phased array antenna. In this embodiment, the case where the antenna of the link side communication unit 21 and the antenna of the base station side communication unit 30 are each a phased array antenna will be mainly described. In a phased array antenna having a plurality of antenna elements, for example, the directivity can be changed by changing the relative phase of each antenna element by a phase shifter. Also, for example, the directivity of the phased array antenna may be changed by digital beam forming.

[0034] In this embodiment, the link mechanism control device 1 is mainly described as having two base side communication units 30a and 30b, but the number of base side communication units 30 that the link mechanism control device 1 has is not important. For example, the link mechanism control device 1 may have one base side communication unit 30, or may have three or more base side communication units 30. In this embodiment, the base side communication unit 30 is mainly described as being arranged on the ceiling 5 of the environment in which the link mechanism 10 is installed, but the location where the base side communication unit 30 is arranged is not important. For example, the base side communication unit 30 may be arranged on the floor surface 4 or a wall surface of the environment in which the link mechanism 10 is installed.

[0035] The overall control unit 40 transmits a control signal to each of one or more link side control units 20 via one or more base side communication units 30 to instruct them to change the angle of the joint 12, and receives the changed angle information of the joint 12 of the link mechanism 10 transmitted from the link side control unit 20. For example, when it is desired to change the angle of the joint 12b, the overall control unit 40 may transmit a control signal to the link side control unit 20b that controls the joint 12b to instruct it to change the angle of the joint 12b. The overall control unit 40 may also receive the changed angle information of the joint 12b transmitted from the link side control unit 20b after the angle of the joint 12b is changed. For example, the overall control unit 40 may store the received changed angle information of each joint 12 in a storage unit (not shown) as the current angle information of each joint 12.

[0036] Next, the transmission of radio waves from the base side communication unit 30 to the link side communication unit 21 will be described. The overall control unit 40 can know the current angle information of each joint 12 of the link mechanism 10 by using the changed angle information of the joint 12 received from the link side control unit 20. In the case of the base end side link 11a fixed to the base 15, the control of the joint 12a may be controlled by a wired wiring, and the overall control unit 40 may acquire the changed angle information acquired by a sensor such as an encoder for acquiring the angle information of the joint 12a via a wiring. In addition, when information such as the arrangement position of the link mechanism 10 in a coordinate system in which the link mechanism 10 is arranged (hereinafter referred to as a "link mechanism coordinate system"), the length of each link 11 in the link mechanism 10, and the position of the link side communication unit 21 in the link 11 are stored in a storage unit (not shown), the overall control unit 40 can specify the position of the link side communication unit 21 in the link mechanism coordinate system by using the information and the current angle information of each joint 12. The position of the link side communication unit 21 may be calculated in the same manner as in forward kinematics, for example. Furthermore, when the arrangement position of the base-side communication unit 30 is stored in the storage unit, the overall control unit 40 may, for example, set the directivity of the base-side communication unit 30 using the identified position of the link-side communication unit 21 and the position of the base-side communication unit 30 read from the storage unit so as to form a directional beam from the base-side communication unit 30 to the link-side communication unit 21. Note that, although the present embodiment will mainly describe a case in which the arrangement position of the link mechanism 10 and the like are shown in the link mechanism coordinate system, it goes without saying that such information may be shown in other coordinate systems.

[0037] In this way, when transmitting a control signal via the base side communication unit 30, the overall control unit 40 may control the directivity of the base side communication unit 30 so that radio waves are transmitted to the position of the link side communication unit 21 specified using the changed angle information of the joint 12 received from the link side control unit 20. The base side communication unit 30 that is the subject of the directivity control is the base side communication unit 30 that transmits the control signal. More strictly, the position of the link side communication unit 21 may be the position of the antenna that the link side communication unit 21 has. The link side communication unit 21 that receives the control signal may receive the control signal non-directionally, for example. If the link side communication unit 21 has an array antenna, when receiving radio waves non-directionally, for example, one antenna element included in the array antenna may be used to receive the radio waves.

[0038] The overall control unit 40 preferably controls the directivity of the base side communication unit 30 so that the base side communication unit 30 and the link side communication unit 21, which is the destination of the control signal, can communicate in a line of sight. For example, when two or more base side communication units 30 are arranged, the overall control unit 40 may select a base side communication unit 30 that can perform line-of-sight communication with the link side communication unit 21 using the positions of the two or more base side communication units 30 in the link mechanism coordinate system, the position of the destination link side communication unit 21, and the shape of the link mechanism 10, and set the directivity of the selected base side communication unit 30 so that a directional beam is formed from the selected base side communication unit 30 toward the destination link side communication unit 21. Note that when there are multiple base side communication units 30 that can perform line-of-sight communication, the overall control unit 40 may select one base side communication unit 30 to be used for transmitting the control signal from the multiple base side communication units 30. As an example, the base side communication unit 30 that is the shortest distance from the link side communication unit 21, which is the destination of the control signal, may be selected. Also, for example, if a link 11 or a joint 12 of the link mechanism 10 exists between the position of the base side communication unit 30 and the position of the link side communication unit 21, it may be determined that line-of-sight communication between the two is not possible.

[0039] On the other hand, when the base side communication unit 30 and the link side communication unit 21 to which the control signal is to be transmitted cannot communicate line-of-sight, the overall control unit 40 may control the directivity of the base side communication unit 30 so that the reflected wave of the radio wave emitted from the base side communication unit 30 is received by the link side communication unit 21. Specifically, when the base side communication units 30a, 30b and the link side communication unit 21c in the link mechanism coordinate system are located at the positions shown in Fig. 1, the overall control unit 40 may determine, taking into consideration the shape of the link mechanism 10, that line-of-sight communication cannot be performed between the base side communication units 30a, 30b and the link side communication unit 21c, but non-line-of-sight communication can be performed between the base side communication unit 30a and the link side communication unit 21c using the reflection of the radio wave on the floor surface 4, and may set the directivity of the base side communication unit 30b so that the radio wave reflected on the floor surface 4 is transmitted from the base side communication unit 30a to the link side communication unit 21c. In this case, the overall control unit 40 may set the directivity of the base station communication unit 30a so that a directional beam is formed from the base station communication unit 30a toward a reflection position on the floor surface 4. When the directivity is controlled taking into account the reflection of radio waves, information indicating the positions of the floor surface 4, ceiling 5, wall surfaces, etc. in the environment in which the link mechanism 10 is placed is also stored in a storage unit (not shown), and the overall control unit 40 may use this information to control the directivity taking into account the reflection of radio waves.

[0040] Next, the transmission of radio waves from the link side communication unit 21 to the base side communication unit 30 will be described. In this case, the joint control unit 22 needs to know the angle information of the joint 12 other than the control target. For example, in order for the joint control unit 22b to specify the position of the link side communication unit 21b in the link mechanism coordinate system, it is necessary to acquire the angle information of the joint 12a. Therefore, when transmitting a control signal to a certain joint control unit 22, the overall control unit 40 may transmit a control signal including the current angle information of the joint 12 other than the joint 12 whose angle is changed by the joint control unit 22. Here, the current angle information of the joint 12 included in the control signal may be, for example, the current angle information of all the joints 12 in the link side communication unit 21 other than the control target by the joint control unit 22, or may be the current angle information of some of the joints 12. It is preferable that the angle information of the joint 12 other than the control target included in the control signal is absolute angle information. In this way, in the case where the control signal includes the current angle information of another joint 12 and there are two or more link side control units 20, the overall control unit 40 may repeat the process of transmitting a control signal to the link side control unit 20 and receiving the changed angle information of the joint 12 from that link side control unit 20 for each of the two or more link side control units 20. In other words, it is preferable that after the process of transmitting a control signal and receiving changed angle information for one link side control unit 20 is completed, the process of transmitting a control signal and receiving changed angle information for another link side control unit 20 is started.

[0041] Furthermore, when information such as the arrangement position of the link mechanism 10 in the link mechanism coordinate system, the length of each link 11 in the link mechanism 10, and the position of the link side communication unit 21 in the link 11 are stored in a storage unit (not shown) accessible to the joint control unit 22, the joint control unit 22 can specify the position of the link side communication unit 21 in the link mechanism coordinate system that the joint control unit 22 uses in wireless communication with the overall control unit 40 by using such information, current angle information of the joint 12 included in the control signal, and, as necessary, current angle information of the joint 12 to be controlled. Furthermore, when the arrangement position of the base side communication unit 30 is stored in a storage unit, the joint control unit 22 may set the directivity of the link side communication unit 21 so that a directional beam is formed from the link side communication unit 21 to the base side communication unit 30, for example, by using the specified position of the link side communication unit 21 and the position of the base side communication unit 30 read from the storage unit.

[0042] In this way, when the joint control unit 22 of a certain link side control unit 20 transmits angle information after the angle of the joint 12 has been changed based on a control signal transmitted from the overall control unit 40, the joint control unit 22 may control the directivity of the link side communication unit 21 so that radio waves are transmitted from the position of the link side communication unit 21 of the link side control unit 20 specified using the current angle information of the joint 12 included in the control signal to the position of the base side communication unit 30. Note that the positions of the link side communication unit 21 and the base side communication unit 30 may, more strictly speaking, be the positions of the antennas possessed by the link side communication unit 21 and the base side communication unit 30.

[0043] In addition, when there are two or more base-side communication units 30, the joint control unit 22 may, for example, set the base-side communication unit 30 that is the source of the control signal as the transmission destination, or may set the base-side communication unit 30 that is the shortest distance from the link-side communication unit 21 that transmits the changed angle information of the joint 12 and that can perform line-of-sight communication as the transmission destination. In the former case, as an example, information for identifying the base-side communication unit 30 that transmitted the control signal is included in the control signal, and the joint control unit 22 may use the information to identify the base-side communication unit 30 that transmitted the control signal. It is preferable that the transmission of this changed angle information is also performed by line-of-sight communication, as with the transmission of the control signal. However, if line-of-sight communication is not possible, the directivity of the link-side communication unit 21 may be controlled so that the reflected wave of the radio wave emitted from the link-side communication unit 21 is received by the base-side communication unit 30. The base-side communication unit 30 that receives the changed angle information may receive the changed angle information by, for example, an antenna with directivity set to receive radio waves from the transmitting link-side communication unit 21. In this case, it is preferable that the joint control unit 22 and the base communication unit 30 or the overall control unit 40 use the same algorithm to determine the base communication unit 30 to which the changed angle information is to be transmitted.

[0044] Furthermore, the angle of the joint 12 required when a certain joint control unit 22 specifies the position of the link side communication unit 21 when transmitting the changed angle information of the joint 12 is each angle from the joint 12 on the base end side of the link 11 on which the link side communication unit 21 is provided to the joint 12a on the base end side in the entire link mechanism 10. As an example, the angle of the joint 12 required when the joint control unit 22c specifies the position of the link side communication unit 21c in the link mechanism coordinate system is the angle of the joint 12b on the base end side of the link 11c and the angle of the joint 12a on the base end side in the entire link mechanism 10. Therefore, when transmitting a control signal to a certain joint control unit 22, the overall control unit 40 may transmit a control signal including only the current angle information of the joint 12 required for the joint control unit 22 to specify the position of the link side communication unit 21 used when transmitting the changed angle information.

[0045] Next, the operation of the link mechanism control device 1 will be described with reference to the flowchart of FIG. (Step S101) The overall control unit 40 determines whether or not to control the joints 12 of the link mechanism 10. If the joints 12 are to be controlled, the process proceeds to step S102, and if not, the process of step S101 is repeated until it is determined that the joints 12 are to be controlled. For example, when an instruction to control the link mechanism 10 according to certain teaching data is input, the overall control unit 40 may determine to control the joints 12 until a series of controls of the angles of the joints 12 according to the teaching data is completed, and when that series of controls is completed, may determine not to control the joints 12.

[0046] (Step S102) Overall control unit 40 sets counter i to 1.

[0047] (Step S103) The overall control unit 40 refers to the arrangement position of the link mechanism 10 in the link mechanism coordinate system stored in a storage unit (not shown), the length of each link 11, the position of the link side communication unit 21 in the link 11, the current angle information of each joint 12, etc., to identify the position of the i-th link side communication unit 21 in the link mechanism coordinate system. Note that the i-th link side communication unit 21 may be the link side communication unit 21 included in the i-th link side control unit 20.

[0048] (Step S104) The overall control unit 40 sets the directivity of the base-side communication unit 30 so that radio waves are transmitted to the position of the link-side communication unit 21 identified in step S103, and transmits a control signal with the set directivity. This control signal may include, for example, target angle information of the joint 12 to be controlled by the link-side control unit 20 that received the control signal, and current angle information of the joint 12 other than the control target. The control signal transmitted from the overall control unit 40 via the base-side communication unit 30 is received by the i-th link-side communication unit 21. The control signal may be received, for example, by an omnidirectional antenna.

[0049] (Step S105) The i-th joint control unit 22 changes the angle of the joint 12 to be controlled in response to the control signal received in step S104. For example, the angle of the joint 12 to be controlled may be changed so that the angle of the joint 12 matches the target angle information included in the control signal. Note that the i-th joint control unit 22 may be the joint control unit 22 included in the i-th link side control unit 20.

[0050] (Step S106) The i-th joint control unit 22 identifies the position of the i-th link side communication unit 21 in the link mechanism coordinate system using the current angle information of the joint 12 other than the controlled joint contained in the control signal received in step S104, and, if necessary, the changed angle information of the controlled joint 12, and information such as the position of the link mechanism 10 in the link mechanism coordinate system, the length of each link 11, and the position of the link side communication unit 21 in the link 11, all of which are stored in a memory unit not shown.

[0051] (Step S107) The i-th joint control unit 22 sets the directivity of the i-th link side communication unit 21 so that radio waves are transmitted from the position of the i-th link side communication unit 21 specified in step S106 to the position of the base side communication unit 30, and transmits the changed angle information of the joint 12 to be controlled to the base side communication unit 30 with the set directivity. The changed angle information transmitted from the i-th joint control unit 22 via the i-th link side communication unit 21 is received by the base side communication unit 30 and transmitted to the overall control unit 40. The changed angle information may be received, for example, by an antenna whose directivity is set to receive radio waves from the i-th link side communication unit 21. This received changed angle information may be stored in a storage unit (not shown) by the overall control unit 40, for example.

[0052] (Step S108) Overall control unit 40 increments counter i by one.

[0053] (Step S109) The overall control unit 40 judges whether or not there is an i-th link side control unit 20. If there is an i-th link side control unit 20, the process returns to step S103, and if not, the process proceeds to step S110.

[0054] (Step S110) The overall control unit 40 controls the angle of the base-side joint 12a controlled by a wire. In this case, the overall control unit 40 may also obtain the angle information after the change of the base-side joint 12a and store the angle information after the change in a storage unit (not shown). Then, the process returns to step S101.

[0055] 2 is an example, and the order of the steps may be changed as long as the same results can be obtained. For example, the process of controlling the angle of the joint 12a by wire (step S110) may be performed before the process of controlling the angle of the joint 12 by wireless (steps S102 to S109). Furthermore, if there is no joint 12 in the link mechanism 10 that is controlled by wire, the process of step S110 may not be performed. Furthermore, in the flowchart of FIG. 2, the process ends when the power is turned off or an interrupt is issued to end the process.

[0056] Next, the operation of the link mechanism control device 1 according to this embodiment will be described using a specific example. In this specific example, the overall control unit 40 is assumed to hold the current absolute angle information of each joint 12 in a storage unit (not shown). The overall control unit 40 is assumed to sequentially change the angle of each joint 12 of the link mechanism 10 according to the teaching data stored in the storage unit (not shown).

[0057] When the overall control unit 40 receives an instruction to control each joint 12 according to the teaching data stored in a storage unit (not shown), it determines to control each joint 12 and reads out the first target angle information of each joint 12 included in the teaching data (step S101). It is assumed that the first target angles of each joint 12, 12a to 12c, are θa, θb, and θc, respectively.

[0058] In this specific example, the first link side control unit 20 and the second link side control unit 20 are the link side control units 20c and 20b, respectively. Then, the overall control unit 40 first identifies the position of the first link side communication unit 21c in the link mechanism coordinate system using the arrangement position of the link mechanism 10 in the link mechanism coordinate system stored in a storage unit (not shown), the length of each link 11, the position of the link side communication unit 21 in the link 11, the current angle information of each joint 12, and the like (steps S102 and S103). At this point in time, as shown in FIG. 1, it is assumed that line-of-sight communication cannot be performed between the base side communication units 30a and 30b and the link side communication unit 21c, but non-line-of-sight communication using the reflection of radio waves on the floor surface 4 can be performed between the base side communication unit 30a and the link side communication unit 21c. Then, the overall control unit 40 decides to transmit a control signal from the base side communication unit 30a to the link side communication unit 21c, and sets the directivity of the base side communication unit 30a so that a directional beam is formed from the base side communication unit 30a toward the reflection position of the radio wave on the floor surface 4. Then, the overall control unit 40 causes the base side communication unit 30a to transmit a control signal including the current angle information of the joints 12a and 12b and the target angle θc information of the joint 12c with the set directivity. The control signal is received by the omnidirectional antenna of the link side communication unit 21c and passed to the joint control unit 22c (step S104).

[0059] The joint control unit 22c that has received the control signal controls the motor 13c so that the angle of the joint 12c becomes the target angle θc included in the control signal (step S105). The joint control unit 22c also specifies the position of the link side communication unit 21c in the link mechanism coordinate system using the current angle information of the joints 12a and 12b included in the control signal, and information such as the arrangement position of the link mechanism 10 in the link mechanism coordinate system stored in a storage unit (not shown), the length of each link 11, and the position of the link side communication unit 21 in the link 11 (step S106). Then, the joint control unit 22c determines to transmit a control signal from the link side communication unit 21c to the base side communication unit 30a using the specified position of the link side communication unit 21c and the arrangement positions of the base side communication units 30a and 30b stored in the storage unit, and sets the directivity of the link side communication unit 21c so that a directional beam directed from the link side communication unit 21c toward the reflection position of the radio wave on the floor surface 4 is formed. The joint control unit 22c also acquires the changed angle information of the joint 12c, and transmits the acquired changed angle information from the link side communication unit 21c with the set directivity. It is assumed that the base side communication unit 30a maintains the directivity of the antenna set when the control signal was transmitted. Therefore, the transmitted changed angle information is received by the base side communication unit 30a via the antenna, and is passed to the overall control unit 40 (step S107). The overall control unit 40 then stores the received changed angle information in a storage unit (not shown) as the current angle information of the joint 12c. It is assumed in this specific example that the changed angle of the joint 12c is equal to θc.

[0060] Thereafter, the overall control unit 40 similarly identifies the position of the second link side communication unit 21b (steps S108, S109, S103). At this point, as shown in FIG. 1, it is assumed that line-of-sight communication cannot be performed between the base side communication unit 30a and the link side communication unit 21b, but line-of-sight communication can be performed between the base side communication unit 30b and the link side communication unit 21b. Then, the overall control unit 40 decides to transmit a control signal from the base side communication unit 30b to the link side communication unit 21b, and sets the directivity of the base side communication unit 30b so that a directional beam is formed from the base side communication unit 30b toward the link side communication unit 21b. Then, the overall control unit 40 causes the base side communication unit 30b to transmit a control signal including the current angle information of the joint 12a and the information of the target angle θb of the joint 12b with the set directivity. The control signal is received by the non-directional antenna of the link side communication unit 21b and passed to the joint control unit 22b (step S104).

[0061] The joint control unit 22b that has received the control signal controls the motor 13b so that the angle of the joint 12b becomes the target angle θb included in the control signal (step S105). The joint control unit 22b also specifies the position of the link side communication unit 21b in the link mechanism coordinate system using the current angle information of the joint 12a included in the control signal and information such as the arrangement position of the link mechanism 10 in the link mechanism coordinate system, the length of each link 11, and the position of the link side communication unit 21 in the link 11 stored in a storage unit (not shown) (step S106). Then, the joint control unit 22b determines to transmit the changed angle information from the link side communication unit 21b to the base side communication unit 30b using the specified position of the link side communication unit 21b, and sets the directivity of the link side communication unit 21b so that a directional beam directed from the link side communication unit 21b to the base side communication unit 30b is formed. In addition, the joint control unit 22b acquires the changed angle information of the joint 12b, and transmits the acquired changed angle information from the link side communication unit 21b with the set directivity. It is assumed that the base side communication unit 30b maintains the directivity of the antenna set when the control signal was transmitted. Therefore, the transmitted changed angle information is received by the base side communication unit 30b via the antenna and passed to the overall control unit 40 (step S107). Then, the overall control unit 40 stores the received changed angle information in a storage unit (not shown) as the current angle information of the joint 12b. It is assumed in this specific example that the changed angle of the joint 12b is different from θb due to some factor. In this case, in future processing, the position of the link side communication unit 21 is identified using the newly stored current angle information of the joint 12b instead of θb, thereby making it possible to perform more accurate control of the directivity.

[0062] Thereafter, the overall control unit 40 controls the motor 13a of the joint 12a via a wire to change the angle of the joint 12a to θa, and obtains angle information of the joint 12a after the change and stores it in a storage unit (not shown) (steps S108 to S110). By repeating such processing, the link mechanism 10 is operated according to the teaching data.

[0063] As described above, according to the link mechanism control device 1 of the present embodiment, when at least some of the joints 12 of the links 11 in the link mechanism 10 are controlled by wireless communication, the changed angle information is returned to the overall control unit 40, so that even if the angle of the joint 12 has not been changed as instructed by the control signal, when transmitting radio waves from the overall control unit 40 to the link side communication unit 21 of the link mechanism 10, the directivity of the base side communication unit 30 can be controlled so that the radio waves are appropriately transmitted to the target link side communication unit 21. Therefore, more reliable transmission of the control signal to the link side communication unit 21 can be realized. In addition, when the control signal contains current angle information of the joints 12 other than the control target, the link side control unit 20 can control the directivity of the link side communication unit 21 so that the radio waves are appropriately transmitted to the base side communication unit 30 when returning the changed angle information of the joints 12 of the control target to the overall control unit 40 by using the current angle information of the joints 12 other than the control target contained in the control signal. In this way, high-gain wireless communication between the link mechanism 10 side and the base side communication unit 30 can be realized. Furthermore, when transmitting radio waves from the base side communication unit 30 to the link side communication unit 21, by receiving the radio waves with a non-directional antenna, the link side can receive the radio waves even if the position of the link side communication unit 21 is unknown. In this case, it is also possible to increase the transmission power of the radio waves in the base side communication unit 30 in response to receiving the radio waves with a non-directional antenna. On the other hand, when transmitting radio waves from the link side communication unit 21 to the base side communication unit 30, the link side communication unit 21 transmits the radio waves with an antenna in which a directional beam directed toward the base side communication unit 30 of the transmission destination is formed, and the base side communication unit 30 receives the radio waves with an antenna in which a directional beam directed toward the link side communication unit 21 of the transmission source is formed, so that the radio waves can be transmitted with smaller transmission power.

[0064] In the present embodiment, the case where the joint control unit 22 changes the angle of the joint 12 in response to a control signal received from the overall control unit 40 and transmits the changed angle information of the joint 12 to the overall control unit 40 via the link side communication unit 21 has been mainly described. However, this is not limited to this embodiment, and the following form is also conceivable. If the changed angle information of the joint 12 is transmitted to the overall control unit 40 every time the angle of the joint 12 is changed in response to a control signal, the amount of communication of wireless communication increases accordingly. Therefore, for example, the joint control unit 22 may transmit the changed angle information of the joint 12 to the overall control unit 40 via the link side communication unit 21 only when the angle of the joint 12 changed in response to a control signal received from the overall control unit 40 is different from the angle specified by the control signal. In this way, when the angle of the joint 12 can be changed according to the control signal, the changed angle information of the joint 12 is not transmitted from the joint control unit 22 to the overall control unit 40, and the amount of communication of wireless communication can be reduced. In this case, for example, the transmission of the changed angle information of the joint 12 may be specified to be performed within a predetermined time after the control signal is transmitted. Then, for example, if changed angle information is not received within a predetermined time after a control signal is transmitted, the overall control unit 40 may overwrite the current angle information of the joint 12, which was the target of the angle change by the control signal and which is stored in a memory unit (not shown), with the target angle information included in the control signal.

[0065] In addition, in the present embodiment, the case where the current angle information of the joint 12 other than the control target is included in the control signal has been mainly described, but this is not necessarily the case. For example, the angle information of the joint 12 may be transmitted to the link side control unit 20 separately from the control signal. As an example, in the case where the angle of the joint 12 on the tip side of the link 11 is controlled by the link side control unit 20 included in each link 11 as in the link mechanism 10 shown in FIG. 1, when the angle of a certain joint 12 is changed, the angle information after the change may be transmitted to each link side control unit 20 on the tip side of the joint 12. Also, for example, in the case where the angle of the joint 12 on the tip side of the link 11 is controlled by the link side control unit 20 included in each link 11 as in the link mechanism 10 shown in FIG. 1, the positional relationship between the link side communication unit 21 and the base side communication unit 30 does not change between the time of transmitting the control signal and the time of transmitting the angle information after the change of the joint 12. Therefore, in this case, for example, the control signal may include information indicating the positions of the link side communication unit 21 and the base side communication unit 30 in the link mechanism coordinate system. The base side communication unit 30 may be the base side communication unit 30 used in transmitting the control signal. In this case, the joint control unit 22 may set the directivity of the link side communication unit 21 so that a directional beam from the link side communication unit 21 to the base side communication unit 30 is formed, for example, using the positions of the link side communication unit 21 and the base side communication unit 30 included in the control signal. The control signal may also include information for forming a directional beam from the link side communication unit 21 to the base side communication unit 30, for example, information such as the relative phase between antenna elements in the link side communication unit 21. In this case, the joint control unit 22 may set the directivity of the link side communication unit 21, for example, using information for forming a directional beam from the link side communication unit 21 to the base side communication unit 30, which is included in the control signal.

[0066] In the present embodiment, each component may be configured by dedicated hardware, or a component that can be realized by software may be realized by executing a program. For example, each component may be realized by a program execution unit such as a CPU reading and executing a software program recorded on a recording medium such as a hard disk or semiconductor memory.

[0067] The above-mentioned embodiments are merely examples for specifically implementing the present invention, and are not intended to limit the technical scope of the present invention. The technical scope of the present invention is indicated by the claims, not by the description of the embodiments, and is intended to include modifications within the literal scope of the claims and the scope of equivalent meanings. [Explanation of symbols]

[0068] 1 Link mechanism control device 10 Link mechanism 11, 11a, 11b, 11c, 11d Links 12, 12a, 12b, 12c joints 13, 13a, 13b, 13c Motor 20, 20b, 20c Link side control section 21, 21b, 21c Link side communication unit 22, 22b, 22c Joint control section 30, 30a, 30b Base side communication department 40 Overall control unit

Claims

1. a link mechanism having a plurality of links connected by joints driven by a driving means; one or more link side control units each having a link side communication unit for transmitting and receiving radio waves and a joint control unit for changing an angle of the joint in response to a control signal received via the link side communication unit and transmitting information of the changed angle of the joint via the link side communication unit, each of the link side control units being provided in one or more of the multiple links; one or more base station side communication units for transmitting and receiving radio waves to and from the one or more link side communication units; an overall control unit that transmits a control signal to each of the one or more link side control units via the one or more base side communication units to instruct the change of an angle of the joint, and receives angle information of the joint of the link mechanism after the change, The link mechanism control device, wherein the overall control unit controls the directivity of the base-side communication unit so that, when transmitting a control signal, radio waves are transmitted to the position of the link-side communication unit identified using the post-change angle information of the joint received from the link-side control unit.

2. 2. The link mechanism control device according to claim 1, wherein the joint control unit transmits changed angle information of the joint to the overall control unit via the link side communication unit only when the angle of the joint changed in response to the control signal received from the overall control unit differs from the angle instructed by the control signal.

3. 2. The link mechanism control device according to claim 1, wherein the overall control unit controls the directivity of the base side communication unit so that a reflected wave of a radio wave emitted from the base side communication unit is received by the link side communication unit when the base side communication unit and the link side communication unit to which the control signal is sent cannot communicate line of sight.

4. When transmitting a control signal to a certain joint control unit, the overall control unit transmits a control signal including current angle information of a joint other than a joint whose angle is to be changed by the certain joint control unit; 4. A link mechanism control device according to claim 1, wherein, when transmitting angle information after a change in angle of a joint whose angle has been changed based on a control signal transmitted from the overall control unit, a joint control unit of the link side control unit controls a directivity of the link side communication unit so that radio waves are transmitted from a position of the link side communication unit of the link side control unit identified using current angle information of each joint included in the control signal to a position of the base side communication unit.

Images