Control device and method of articulated mechanism, articulated device, robot device and its control method

Description: TECHNICAL FIELD [Detailed description of the invention]

【0001】

[Technical field to which the invention belongs]

The present invention relates to a control device and method of a joint mechanism, a joint device, and a robot device and a control method thereof, and is suitable for being applied to, for example, a bipedal robot.

【0002】

[Conventional technology]

Conventionally, in a bipedal robot, a pair of leg units are connected to a body unit via a hip joint mechanism. Further, each leg unit is formed by connecting the thigh and the lower leg via the knee joint mechanism and connecting the foot to the lower leg via the ankle joint mechanism.

【0003】

In such a bipedal walking robot, actuators for the number of degrees of freedom required for each joint mechanism (usually an AC (Alternating Current) servomotor, hereinafter referred to as a motor) are incorporated, and each of these is incorporated. By individually driving and controlling each motor of the joint mechanism and driving each leg unit in a predetermined pattern, the robot as a whole can perform a walking motion.

【0004】

[Problems to be Solved by the Invention]

By the way, in such a bipedal walking robot, various sensors such as a pressure sensor, an inclination angle sensor, an acceleration sensor and a microswitch are arranged on the ankle joint mechanism and the foot. In such a bipedal walking robot, the walking road surface has inclination and unevenness by controlling the inclination of the foot so that the back surface of the foot always follows the walking road surface based on the output of these sensors during walking motion. It is constructed so that it can perform stable walking even on rough ground.

【0005】

However, when such a sensor is actually attached to the ankle joint mechanism or the foot, the weight of the entire robot increases by the weight of the sensor, and these sensors are electrically connected to the control system inside the robot. There is a problem that wiring is also required and the configuration of the robot as a whole becomes complicated. Further, when the sensor is attached, it is necessary to process the sensor information by software in the control system inside the robot, which causes a problem that the walking control becomes complicated.

【0006】

Further, in the conventional bipedal walking robot, for example, as shown in FIG. 23, the main control unit 1 that controls the operation of the entire robot is connected to each motor 3 (3-1 to 3-n) via the multi-axis controller 2. The multi-axis controller 2 is configured to be connected and control each motor 3 to a individually specified state based on a control command output from the main control unit 1.

【0007】

However, in such a bipedal robot, three cables (U-phase, V-phase and W-phase) for rotational drive and four cables (A-phase, A-phase) for rotational position sensors are used to connect the multi-axis controller 2 and motor 3. B-phase and Z-phase), one cable 4 for ABS position serial signal is required in total, which causes a problem that the configuration becomes complicated because the amount of wiring of the robot as a whole is large.

【0008】

The present invention has been made in consideration of the above points, and an object of the present invention is to propose a control device and method of a joint mechanism, a joint device, a robot device, and a control method thereof that can simplify the configuration.

【0009】

[Means for solving problems]

In order to solve such a problem, in the present invention, in the control device of the joint mechanism, based on the current detecting means for detecting the current value of the driving current of the actuator for driving the joint mechanism and the current value detected by the current detecting means. , An external force torque detecting means for detecting the magnitude of the torque due to the external force applied to the output shaft of the actuator is provided. As a result, in this control device, the external force applied to the first or second link can be obtained without the need for a sensor or the like.

【0010】

Further, in the present invention, in the method of controlling the joint mechanism, the first step of detecting the current value of the drive current of the actuator that drives the joint mechanism and the external force applied to the output shaft of the actuator based on the detected current value. A second step of detecting the magnitude of the torque is provided. As a result, according to this control method, the external force applied to the first or second link can be obtained without the need for a sensor or the like.

【0011】

Further, in the present invention, in the robot device, an external force applied to the output shaft of the actuator is applied based on the current detecting means for detecting the current value of the drive current of the actuator of the joint mechanism and the current value detected by the current detecting means. An external force torque detecting means for detecting the magnitude of the torque is provided. As a result, in this robot device, the external force applied to the first or second component can be obtained without the need for a sensor or the like.

【0012】

Further, in the present invention, in the control method of the robot device, the first step of detecting the current value of the drive current of the actuator and the magnitude of the torque due to the external force applied to the output shaft of the actuator based on the detected current value. A second step to detect the current is provided. As a result, according to the control method of this robot device, it is possible to obtain the external force applied to the first or second component without the need for a sensor or the like.

【0013】

Further, in the present invention, in the two-legged walking type robot device, an actuator provided in the ankle joint mechanism to generate a rotational torque having a magnitude corresponding to the driving current, and a current detection for detecting the current value of the driving current of the actuator. Based on the means and the current value detected by the current detecting means, the external force torque detecting means for detecting the magnitude of the torque due to the external force applied to the output shaft of the actuator, and the actuator based on the detection result of the external force torque detecting means. A control means for controlling the actuator is provided so that the torque due to the external force applied to the output shaft of the above is eliminated. As a result, this robot device can walk on uneven ground without disturbing the balance without the need for sensors or the like.

【0014】

Further, in the present invention, in the control method of the two-legged walking type robot device, the first method of detecting the current value of the drive current of the actuator provided in the ankle joint mechanism and generating the rotational torque having a magnitude corresponding to the drive current. Step, and the second step to detect the magnitude of torque due to the external force applied to the output shaft of the actuator based on the detected current value, and the output shaft of the actuator based on the detection result of the second step. A third step for controlling the actuator is provided so that the torque due to the external force applied to the actuator is eliminated. As a result, according to this control method, it is possible to make the robot device walk on the rough ground without losing the balance without requiring a sensor or the like.

【0015】

Further, in the present invention, in the joint device, a motor unit for generating rotational torque and a motor control means for driving and controlling the motor unit are provided in the actuator, and the motor control means is arranged in the motor unit. As a result, in this joint device, the amount of wiring between the actuator and the outside can be significantly reduced.

【0016】

Further, in the present invention, in the robot device, a motor unit that generates rotational torque and a motor control means that drives and controls the motor unit are provided in the actuator that drives the joint mechanism, and the motor control means is arranged in the motor unit. I made it. As a result, in this robot device, the amount of wiring for each actuator of the joint mechanism can be significantly reduced.

【0017】

BEST MODE FOR CARRYING OUT THE INVENTION

Hereinafter, one embodiment of the present invention will be described in detail with reference to the drawings.

【0018】

(1) Overall configuration of the robot according to this embodiment In FIGS. 1 and 2, 10 indicates a bipedal walking robot according to the present embodiment as a whole, and the head unit 12 is arranged on the upper part of the body unit 11 and the upper part of the body unit 11. Arm units 13A and 13B having the same configuration are arranged on the left and right, respectively, and leg units 14A and 14B having the same configuration are attached to the lower left and right of the body unit 11 at predetermined positions, respectively.

【0019】

In the body unit 11, the frame 20 forming the upper part of the trunk and the waist base 21 forming the lower part of the trunk are connected to each other via the waist joint mechanism 22, and the waist base 21 of the lower part of the trunk is connected to the waist base 21. By driving the motors M-1 and M-2 of the fixed hip joint mechanism 22, the upper part of the trunk is independently rotated around the orthogonal roll axis 23 and pitch axis 24 shown in FIG. Is made to be able to.

【0020】

Further, the head unit 12 is attached to the center of the upper surface of the shoulder base 25 fixed to the upper end of the frame 20 via the neck joint mechanism 26, and the motors M-3 and M-4 of the neck joint mechanism 26 are attached. By driving each of them, they can be independently rotated around the orthogonal pitch axis 27 and the yaw axis 28 shown in FIG.

【0021】

Furthermore, the arm units 13A and 13B are attached to the left and right sides of the shoulder base 25 via the shoulder joint mechanism 29, respectively, and drive the motors M-5 and M-6 of the corresponding shoulder joint mechanism 29, respectively. Can be rotated independently around the orthogonal pitch axis 30 and roll axis 31 shown in FIG.

【0022】

In this case, in each of the arm units 13A and 13B, the motor M-8 forming the forearm is connected to the output shaft of the motor M-7 forming the upper arm via the elbow joint mechanism 32, and the tip of the forearm is connected. It is configured by attaching the hand part 33 to the.

【0023】

In each arm unit 13A and 13B, the forearm is rotated around the yaw axis 34 shown in FIG. 3 by driving the motor M-7, and the forearm is moved to FIG. 3 by driving the motor M-8. It is designed so that it can be rotated around the pitch axis 35 shown.

【0024】

On the other hand, the leg units 14A and 14B are attached to the lower back base 21 of the lower part of the trunk via the hip joint mechanism 36, respectively, and the corresponding motors M-9 to M-11 of the hip joint mechanism 36 are respectively attached to each leg unit 14A and 14B. By driving, they can be independently rotated around the yaw axis 37, the roll axis 38, and the pitch axis 39, which are orthogonal to each other as shown in FIG.

【0025】

In this case, in each of the leg units 14A and 14B, the frame 42 forming the lower leg is connected to the lower end of the frame 40 forming the thigh via the knee joint mechanism 41, and the frame 42 forming the lower leg is connected to the lower end of the frame 42. It is configured by connecting the foot 44 via the ankle joint mechanism 43.

【0026】

As a result, in each of the leg units 14A and 14B, the lower leg can be rotated around the pitch axis 45 shown in FIG. 3 by driving the motor M-12 forming the knee joint mechanism 41. By driving the motors M-13 and M-14 of the ankle joint mechanism 43, respectively, the foot 44 can be independently rotated around the orthogonal pitch axis 45 and roll axis 46 shown in FIG. ing.

【0027】

The configuration of the hip joint mechanism 36 of the robot 10 is shown in FIGS. 4 and 5, and the configuration of the ankle joint mechanism 43 is shown in FIG.

【0028】

As is clear from FIGS. 4 and 5, in each hip joint mechanism 36, the motor M-9 is fixed to the waist base 21 at the lower part of the trunk. The output shaft of the motor M-10 is connected to the output shaft of the motor M-9 via a U-shaped connecting member 50, and the U-shaped member 51 is fixed to the side surface of the motor M-10. There is.

【0029】

Further, the joint mechanism pulley 52 is rotatably attached to the U-shaped member 51, one end side of the upper end thereof is fixed to the joint mechanism pulley 52, and the other end side of the upper end is rotatably attached to the member 51. A frame 40 forming the thighs of the leg units 14A and 14B is arranged.

【0030】

The motor M-11 of the hip joint mechanism 36 is fixed to the frame 40, and the pulley 53 attached to the output shaft is connected to the joint mechanism pulley 52 via the timing belt 54.

【0031】

As a result, in the hip joint mechanism 36, the corresponding leg units 14A and 14B are independently rotated around the yaw axis 37, the roll axis 38, and the pitch axis 39 by driving the respective motors M-9 to M-11, respectively. It is designed to be able to be driven.

【0032】

In this embodiment, the yaw shaft 37 passing through the center of the output shaft of the motor M-9 of the hip joint mechanism 36, the roll shaft 38 passing through the center of the output shaft of the motor M-10, and the rotation center of the frame 40 are used. The position of each component is selected so that the pitch axis 39 through which it passes intersects at a point in the space.

【0033】

Further, in the ankle joint mechanism 43, as is clear from FIG. 6, the output shaft of the motor M-14 is fixed to the U-shaped connecting member 60 fixed to the foot portion 44. A joint mechanism pulley 62 is fixed to the side surface of the motor M-14 via a U-shaped member 61.

【0034】

The frame 42 forming the lower legs of the leg units 14A and 14B described above is rotatably attached to the center position of the joint mechanism pulley 62 on one end side of the lower end thereof, and the other end side of the lower end is the side surface of the connecting member 61. It is arranged so that it can be rotatably attached to.

【0035】

Further, the motor M-13 is fixed to the frame 42, a pulley 63 is fixed to the output shaft of the motor M-13, and the pulley 63 and the joint mechanism pulley 62 are connected via a timing belt 64. There is.

【0036】

As a result, in the ankle joint mechanism 43, the foot portion 44 can be independently rotated around the roll shaft 45 and the pitch shaft 46 by driving the motors M-13 and M-14, respectively. ..

【0037】

On the other hand, in the case of this robot 10, on the back side of the waist base 21 forming the lower part of the trunk of the body unit 11, as shown in FIG. 7, a main control unit 70 that controls the operation of the entire robot 10 and a main control unit 70 A control unit 72 is provided in which peripheral circuits 71 such as a power supply circuit and a communication circuit and a battery (not shown) are housed in a box.

【0038】

The control unit 72 is a sub-control unit 73A ~ arranged in each constituent unit (body unit 11, head unit 12, arm units 13A, 13B, and leg units 14A, 14B), respectively. It is connected to the 73D so that it can supply the necessary power supply voltage to these sub-control units 73A to 73D and communicate with these sub-control units 73A to 73D.

【0039】

In addition, as shown in FIG. 8, each sub-control unit 73A to 73D has each motor M-1 to M-14 in the corresponding constituent unit, two drive voltage supply cables 80, and two control voltages. It is connected in parallel via the supply cable 81 and one synchronous clock supply cable 82, and is connected to each of these motors M-1 to M-14 via two serial communication cables 83 as described later. It is connected to each housed control board by a daisy chain method. Note that FIG. 8 shows the connection relationship between the sub-control unit 74D and the motors M-9 to M-14 in the leg units 14A and 14B.

【0040】

At this time, the main control unit 70 is used to form various states such as "standing state" and "sitting state" of the robot 10, when the robot 10 transitions from one state to another, walking motion, and the like. The output shaft of each motor M-1 to M-14 forms a series of time-series forms for each predetermined time interval (for example, 0.5 seconds, hereinafter referred to as the first time interval) when performing the predetermined operation of. It is memorized as the rotation angle of.

【0041】

Then, the main control unit 70 outputs the motors M-1 to M-14 in each of the series of forms stored as described above when the robot 10 changes the state or performs an operation. The rotation angle of the shaft is switched in chronological order for each of the first time intervals described above, and is sent to the corresponding sub-control units 74A to 74D.

【0042】

On the other hand, each of the sub-control units 74A to 74D provides a drive voltage supply line 80 and a control voltage supply line 81 to the corresponding motors M-1 to M-14 based on the power supply voltage supplied from the control unit 72, respectively. The power supply voltage for driving the motor and the control voltage are supplied via the device.

【0043】

Further, each of the sub-control units 74A to 74D sets the first time interval to n (from the rotation angles of the motors M-1 to M-14 given by the main control unit 70 at the first time interval as described above. n is an integer of 2 or more) The rotation angle, rotation speed, or rotation torque of each of the corresponding motors M-1 to M-14 for each timing (hereinafter referred to as 1 [ms] interval) when divided equally. Calculate and control each of these motors M-1 to M-14 based on the calculation result.

【0044】

In this way, in the robot 10, each motor M-1 to M-14 sequentially matches the form of the robot 10 with each of the series of time-series forms stored in the main control unit 70 during operation. The rotation of the robot is controlled so that various movements can be performed or transitions to various states can be performed with predetermined movements.

【0045】

(3) Configuration of motors M-1 to M-14 (3-1) Overall configuration of motors M-1 to M-14 Here, the configurations of the motors M-1 to M-14 used in the robot 10 will be described. As shown in FIG. 9, each of the motors M-1 to M-14 of the robot 10 has a motor unit 90 that generates rotational torque and torque amplification that amplifies and outputs the rotational torque generated in the motor unit 90. It is composed of part 91.

【0046】

In this case, the motor unit 90 is provided with a rotor shaft 94 rotatably supported by rotary bearings 93A and 93B inside the motor case 92, and the rotor shaft 94 is provided with a rotor base 95 and FIGS. 10 (B) and (C). ), The rotor 97 is formed by coaxially integrating the ring-shaped rotor magnets 96 magnetized in four poles.

【0047】

As shown in FIGS. 8 and 9 (A), six stator cores 98A to 98F are fixed to the inside of the motor case 92 at equal intervals (45 [°] intervals) so as to surround the rotor 97. , Coil 99A to 99F is formed by winding each of these stator cores 98A to 98F.

【0048】

As a result, in the motor unit 90, two sets of two coils 99A and 99D, 99B and 99E, 99C and 99F (three sets in total) facing 180 [°] are regarded as U phase, V phase and W phase, respectively. The rotor 97 can be rotationally driven by applying a drive current that is out of phase by 120 [°] to each of the phase, V-phase, and W-phase coils 99A to 99F, and thus rotational torque can be generated. It is done like this.

【0049】

On the other hand, the torque amplification unit 91 has a gear case 100 detachably fixed to the tip of the motor case 92 as shown in FIGS. 9 and 13 (A) to 13 (C). Inside, the annular internal gear 101, the sun gear 102 fixed to the tip of the rotor shaft 94, and the first to third planets arranged at 120 [°] intervals between the internal gear 101 and the sun gear 102. A planetary gear mechanism 104 including gears 103A to 103C is provided.

【0050】

At this time, in the torque amplification unit 91, the shafts 105A to 105C of the first to third planetary gears 103A to 103C of the planetary gear mechanism 104 are fixed to the output shaft 106 rotatably arranged at the tip of the gear case 100, respectively. Thus, the rotational torque given from the motor unit 90 via the rotor shaft 94 can be transmitted to the output shaft 106 via the planetary gear mechanism 104 and output to the outside via the output shaft 106. There is.

【0051】

Further, in the torque amplification unit 91, the annular resin magnet 107 fixed to the output shaft 106 and the first and second first and second resin magnets 107 fixed to the outer peripheral surface of the gear case 100 so as to face the outer peripheral surface of the resin magnet 107. A one-turn absolute angle sensor 109 including Hall elements 108A and 108B is provided.

【0052】

In this case, the resin magnet 107 is magnetized so that the magnetic flux density φ (θg) changes in two poles and over one circumference as shown in FIG. 14, and is fixed to the output shaft 106 as shown in FIG. 13 (A). Has been done. Further, the first and second Hall elements 108A and 108B are fixed to the outer peripheral surface of the gear case 100 with a phase difference of 90 [°] as shown in FIG. 13 (B).

【0053】

As a result, in the one-rotation absolute angle sensor 109, the rotation angle of the output shaft 106 is set to the magnetic flux density φ (θg) at the arrangement positions of the first and second Hall elements 108A and 108B accompanying the rotation of the output shaft 106. It is detected as a change, and the detection results are obtained from the first and second Hall elements 108A and 108B in the first and second rotations of the waveform given by sin (θg) and cos (θg) as shown in FIG. 15, respectively. It is designed so that it can be output as absolute angle sensor signals S1A and S1B.

【0054】

In addition to this configuration, in the case of a motor, inside the motor case 92 of the motor unit 90, a rotor shaft magnetic pole angle sensor 100 that detects the magnetic pole angle of the rotor shaft 94 and a control command from the corresponding sub-control units 74A to 74D are sent. A control board 111 that controls the rotation angle, rotation speed, rotation torque, etc. of the output shaft 94 based on the control board 111, and a power board 112 that supplies a drive current to the coils 99A to 99F of the motor unit 90 under the control of the control board 111. And are stored.

【0055】

The rotor shaft magnetic pole angle sensor 110 is formed of a resin magnet 113 fixed to the front end surface of the rotor base 95 of the rotor 97 and first to fourth Hall elements 114A to 114D mounted on the control board 111. .. Then, as shown in FIGS. 10B and 10C, the resin magnet 113 is magnetized to the same four poles as the rotor magnet 96 of the rotor 97, and is fixed to the rotor substrate 95 in the same phase as the rotor magnet 96. There is.

【0056】

As shown in FIG. 16B, the first to fourth Hall elements 114A to 114D have the first and second Hall elements 114A and 114B facing each other 180 [°] on a concentric circle with the rotor shaft 94. The third and fourth Hall elements 114C and 114D are mounted on the control board 111 so as to be positioned 45 [°] out of phase in the same direction as the first and second Hall elements 114A and 114B. ing.

【0057】

As a result, in the rotor shaft rotation angle sensor 110, the magnetic pole angles of the rotor shaft 94 are arranged with the first to fourth Hall elements 114A to 114D accompanying the rotation of the resin magnet 113 that rotates integrally with the roller shaft 94. It is designed so that it can be detected as a change in magnetic flux density at a position.

【0058】

The magnetic pole angle of the rotor shaft 94 means an angle obtained by multiplying the mechanical rotation angle of the rotor shaft 94 by half the number of magnetic poles of the rotor magnet 96. In this embodiment, since the rotor magnet 96 is magnetized to four poles, the magnetic angle is in the range of 0 to 2π.

【0059】

On the other hand, as shown in FIGS. 9, 10 (A), 16 and 17, the control board 111 has a one-chip microcomputer 115 and a crystal oscillator for generating a CPU clock on one side of a circularly formed printed wiring board. The 116 is mounted, and the first to fourth Hall elements 114A to 114D of the rotor shaft rotation angle sensor 110 described above and the temperature sensor 117 are mounted on the other surface side.

【0060】

Then, as shown in FIG. 17, the control board 111 has the outputs of the first and second Hall elements 114A and 114B in the rotor shaft magnetic pole angle sensor 110 and the outputs of the third and fourth Hall elements 114C and 114D, respectively. The first and second rotor axis magnetic pole angle sensor signals S2A and S2B are added to the first and second subtraction circuits 118A and 118B and captured in the one-chip microcomputer 115, and the one-rotation absolute angle sensor 109 (Fig. 9. The first and second one-turn absolute angle sensor signals S1A and S1B supplied from FIG. 13 (C) via cable 119 (FIG. 9) can be captured in the one-chip microcomputer 115. ..

【0061】

Further, the control board 111 is connected to the corresponding sub-control units 74A to 74D (FIG. 8) through the two control power supply lines and the two drive power supply lines included in the second cable 120, thus 1 The chip microcomputer 115 can take in various power supply voltages and communicate with the corresponding sub-control units 74A to 74D via the second cable 120.

【0062】

Then, the 1-chip microcomputer 115 specifies the rotation angle, rotation speed, or rotation torque of the output shaft 106 (FIG. 9) given every 1 [ms] from the sub-control units 74A to 74D via the second cable 120. The values (hereinafter referred to as the specified rotation angle, the specified rotation speed, and the specified rotation torque, respectively), the first and second absolute angle of one rotation sensor signals S1A and S1B, and the first and second rotor shaft magnetic pole angles. U-phase, V-phase, and W-phase coils 99A to 99F based on the sensor signals S2A and S2B and the first to third drive current detection signals S3A to S3C supplied from the power board 112 as described later. The values of the drive currents to be applied to each of the above (hereinafter, these are referred to as the first to third current command values) are calculated, and the calculated first to third current command values are used for the third cable 121. It is sent to the power board 112 via.

【0063】

As shown in FIGS. 9, 12 (B) and 12 (C), the power substrate 112 has a plurality of power transistor chips forming the coil drive block 122 shown in FIG. 18 on one side of a printed wiring board formed in an annular shape. It is configured by installing 123.

【0064】

The coil drive block 122 has U-phase, V-phase, and W-phase coils 99A to 99F of the motor unit 90 based on the first to third current command values given by the one-chip microcomputer 115 of the control board 111. The rotor 97 of the motor unit 90 is rotationally driven by applying a drive current of a magnitude corresponding to the corresponding first to third current command values.

【0065】

At this time, the coil drive block 122 detects the magnitude of the drive current applied to each of the U-phase, V-phase, and W-phase coils 99A to 99F, and obtains the detection results of the first to third. The drive current detection signals S3A to S3C are sent to the control board 111 via the third cable 121 (FIG. 9).

【0066】

In this way, in the motors M-1 to M14, the designated rotation speeds given by the sub-control units 74A to 74D by the control circuit including the one-chip microcomputer 115 of the control board 111 and the coil drive block 122 of the power board 112. , The motor unit 90 is driven according to the specified rotation speed or the specified rotation torque.

【0067】

(2-2) Configuration of 1-chip microprocessor 115 and coil drive block 122 Here, as shown in FIG. 19, the one-chip microcomputer 115 includes an arithmetic processing block 128, a register 129, a rotor shaft rotation angle detection processing block 130, a torque-3 phase current signal conversion processing block 131, a current control processing block 132, and a current control processing block 132. It is composed of first to fourth analog / digital conversion circuits 133 to 136.

【0068】

Then, in this one-chip computer 45, the first to third drive current detection signals S3A to S3C given from the power board 112 are digitally converted by the first analog / digital conversion circuit 133, respectively, and the first to obtained first to third are obtained. The third drive current detection data D3A and D3B are given to the current control processing block 132, and the first to third drive current detection data D3A and D3B are stored in the register 129.

【0069】

Further, in the one-chip microcomputer 115, the first and second one-rotation absolute angle sensor signals S1A and S1B supplied from the one-rotation absolute angle sensor 109 (FIGS. 9 and 13 (C)) are transmitted to the third analog / digital. The first and second one-rotation absolute angle sensor data D1A and D1B obtained by digital conversion in the conversion circuit 135 are stored in the register 129.

【0070】

Further, in the one-chip microcomputer 115, the first and second rotor shaft pole angle sensor signals S2A, based on the output of the rotor shaft pole angle sensor 110 given from the first and second subtraction circuits 118A and 118B (FIG. 17), S2B is digitally converted in the second analog / digital conversion circuit 134, and the obtained first and second rotor shaft magnetic pole angle sensor data D2A and D2B are input to the rotor shaft rotation angle detection processing block 130.

【0071】

The rotor shaft rotation angle detection processing block 130 is based on the supplied first and second rotor shaft magnetic pole angle sensor data D2A and D2B, and the magnetic pole rotation angle of the rotor shaft 24 (hereinafter, this is referred to as a rotor shaft magnetic pole rotation angle). ) Pml and the magnetic pole angle θp are detected, the rotor shaft rotation angle Pml is stored in the register 129, and the magnetic pole angle θp is sent to the torque-3 phase current signal conversion processing block 131.

【0072】

The magnetic pole rotation angle of the rotor shaft 94 (rotor shaft magnetic pole rotation angle Pml) is a pair of N adjacent resin magnets 113 detected by the first to fourth Hall elements 114A to 114D as the rotor shaft 94 rotates. The angle at which the change in magnetic poles due to the poles and S poles is one cycle (0 to 2π). In this embodiment, since the resin magnet 113 is magnetized to four poles, the rotor shaft magnetic pole rotation angle Pml has a value in the range of 0 to 4π.

【0073】

Then, the arithmetic processing block 128 includes the first and second absolute angle sensor data D1A and D1B stored in the register 129, the rotor shaft magnetic pole rotation angle Pml, and the designated rotation angle given by the sub-control unit. , The target rotation torque (hereinafter referred to as the target rotation torque) T0 is calculated based on the specified rotation speed or the specified rotation torque, and the calculation result is stored in the register 129. The target rotation torque T0 is calculated every 1 [ms] when the specified rotation angle, the specified rotation speed, or the specified rotation torque is given by the sub-control unit.

【0074】

Then, the target torque T0 is sequentially read from the register 129 by the torque-3 phase current signal conversion processing block 131. The torque-3 phase current signal conversion processing block 131 is based on the target torque T0 and the magnetic pole angle θp of the rotor shaft 94 given by the rotor shaft rotation angle detection processing block 130, and the U phase and V in the motor unit 90. Calculate the above-mentioned third to third current command values Ur, Vr, and Wr, which represent the values of the drive current to be applied to each of the phase and W phase coils 99A to 99F, and send them to the current control processing block 132. To do.

【0075】

The current control processing block 132 has the first to third current command values Ur, Vr, and Wr given by the torque-3 phase current signal conversion processing block 131, and the first analog / digital conversion circuit 133. ~ Based on the 3rd drive current detection data D3A ~ D3C, the 1st to 3rd current command values Ur, Vr, and Wr are subjected to predetermined signal processing including compensation processing for voltage fluctuation, and then this is applied. PWM (Pulse Width Modulation) modulation is performed, and the obtained first to third PWM signals S4A to S4C are transmitted to the coil drive block 121 of the power board 112 via the third cable 121.

【0076】

The third cable 121 is provided with two lines for each of the first to third PWM signals S4A to S4C. When the output shaft 106 (FIG. 9) is driven in the forward rotation, the current control processing block 132 transmits the first to third PWM signals S4A to S4C via the first line of each of the coil drive blocks of the power board 112. While sending to 122, the 1st to 3rd PWM signals S4A to S4C are logical "0" level signals (hereinafter, these are referred to as 1st to 3rd reference signals) S5A to S5C are the other 2nd. It is sent to the coil drive block 122 of the power board 112 via each of the lines.

【0077】

Further, when the output shaft 106 is reversely driven, the current control processing block 132 sends the first to third PWM signals S4A to S4C to the coil drive block 122 of the power board 112 via the second line, respectively, and also sends the first to third PWM signals S4A to S4C to the coil drive block 122 of the power board 112. The first to third reference signals S5A to S5C are transmitted to the coil drive block 122 of the power board 112 via the first line, respectively.

【0078】

On the other hand, in the coil drive block 122, as shown in FIG. 18, the first unit having the same configuration consisting of four amplifiers 138A to 138C corresponding to the U-phase, V-phase, and W-phase coils 99A to 99F, respectively. ~ 3rd gate drive circuit 139A ~ 139C and 1st ~ 3rd inverter circuits 140A ~ 140C with the same configuration consisting of 2 PNP type transistors TR1 and TR2 and 2 NPN type transistors TR3 and TR4 respectively Has been done.

【0079】

Then, in this coil drive block 122, the first and third amplifiers 138A and 138C of the first to third gate drive circuits 139A to 139C corresponding to the first lines of the U phase, the V phase and the W phase, respectively, are used. It is connected to the base of the second PNP type transistor TR2 and the base of the first NPN type transistor TR3 of the corresponding first to third inverter circuits 140A to 140C via each of the U phase, V phase and W phase. The first to third inverter circuits 140A to 140C corresponding to the first to third gate drive circuits 140A to 140C corresponding to the second line via the second and fourth amplifiers 138B and 138D, respectively. It is connected to the base of the 2nd PNP type transistor TR2 and the base of the 1st NPN type transistor TR4.

【0080】

In the coil drive block 122, the collector of the first PNP transistor TR1 in the first to third inverter circuits 140A to 140C corresponding to the U-phase, V-phase, and W-phase coils 99A to 99F of the motor unit 90, respectively. It is connected between the connection midpoint of the collector of the first NPN transistor TR3 and the collector of the second PNP transistor TR2 and the collector of the second NPN transistor TR4.

【0081】

As a result, in this coil drive block 122, the first to third PWM signals S4A to S4C given via the first or second line correspond to each of the U phase, V phase, and W phase, respectively. Convert the analog waveform drive currents Iu, Iv, and Iw in the first to third inverter circuits 140A to 140C, and apply these to the corresponding U-phase, V-phase, and W-phase coils 99A to 99F, respectively. Is made to be able to.

【0082】

Further, in the coil drive block 122, the magnitudes of the drive currents Iu, Iv, and Iw supplied to the U-phase, V-phase, and W-phase coils 99A to 99F are provided in the first to third inverter circuits 140A to 140C, respectively. It is detected by the current sensor 141 composed of the coil, and the detection result is used as the first to third first to third drive current detection signals S3A to S3C as described above. It is designed to be sent to the analog / digital conversion circuit 133 (Fig. 19) of 1.

【0083】

The detailed configuration of the arithmetic processing block 128 will be described with reference to FIG.

【0084】

The arithmetic processing block 128 includes a CPU (Central Processing Unit) 148, a ROM (Read Only Memory) 149 in which various programs are stored, a RAM (Random Access Memory) 150 as a work memory of the CPU 148, and a corresponding sub-control unit. The input / output circuit 152 for serial communication, which is the input / output interface circuit between 73A to 73D, the servo interrupt signal S10 with 1 [ms] cycle for servo interrupt, and the PWM with 50 [μm] cycle, which is the PWM cycle. A predetermined period of 1 [ms] or more for the CPU 148 to correctly detect whether the servo interrupt signal S10 is correctly generated from the servo interrupt signal generation circuit 153 that generates the pulse signal S11 and the servo interrupt signal generation circuit 153. The watchdog signal generation circuit 154 that generates the watchdog signal S12 having the above is connected to each other via the CPU bus 155.

【0085】

In this case, when the control voltage (5 [V]) is given from the corresponding sub-control units 73A to 73D, the CPU 148 first has a serial communication input / output circuit 152 and a counter timer based on the initial program stored in the ROM 149. -Executes start-up processing such as setting processing of various initial values and parameters for the control circuit 153, rotor shaft rotation angle detection processing block 130, torque-3 phase current signal conversion processing block 131, current control processing block 132, and the like.

【0086】

The CPU 148 also has a target rotational torque T as described above, based on the corresponding programs stored in the servo interrupt signals S10 and ROM 149 given by the counter timer control circuit 153 as a result.₀The motor rotation control calculation process for generating the above, the serial communication control process with the corresponding sub-control units 73A to 73D, and the like are executed in a time-division manner in a cycle of 1 [ms].

【0087】

(2-3) Software processing Here, in the arithmetic processing block 128, as described above, the CPU 148 divides the time in 1 [ms] cycles based on the corresponding programs stored in the servo interrupt signals S10 and ROM149 given from the counter timer control circuit 153. Motor rotation control calculation processing, serial communication control processing, etc. are executed. The processing of the CPU 148 in these processing modes will be described below.

【0088】

(2-3-1) Processing of CPU 148 in motor rotation control calculation processing mode The processing of the CPU 148 in the motor rotation control calculation processing mode is to specify the value of the specified rotation position, the specified rotation speed, or the specified rotation torque given every 1 [ms] from the corresponding sub-control units 73A to 73D as described above. It is to calculate the target rotation torque T0 according to it.

【0089】

Then, the CPU 148 rotates the rotor shaft magnetic pole stored in the register 129 by the rotor shaft rotation angle detection processing block 130 when the designated rotation position Pref is given by the corresponding sub-control units 73A to 73D for the target rotation torque T0. The rotation position Pm of the output shaft 94 (Fig. 9) is calculated based on the angle Pm1, and the following equation is used using this rotation position Pm. [0090]

[Number 1]

<img file="JP2000296484A_D0001.tif" />【0091】

[Number 2]

<img file="JP2000296484A_D0002.tif" />【0092】

Is calculated to calculate the target rotation speed Vmref for the specified rotation position Pref and the current rotation speed Vm of the output shaft. Then, from the equations (1) and (2) obtained in this way, the following equation [0093]

[Number 3]

<img file="JP2000296484A_D0003.tif" />【0094】

The target rotation torque T0 is calculated by executing the calculation of.

【0095】

If the specified rotation speed Vref is given by the sub-control units 73A to 73D, the current rotation speed Vm of the output shaft 94 is calculated using Eq. (2), and the following equation is used based on this rotation speed Vm. [0096]

[Number 4]

<img file="JP2000296484A_D0004.tif" />【0097】

The target rotation torque T0 is calculated by calculating. If the specified rotational torque Tref is given by the sub-control units 73A to 73D, this is set as the target rotational torque T0 as it is.

【0098】

In these equations (1) to (4), S represents the Laplace operator, and Kpp, Kvi, and Kvp each represent the control gain parameters set by the sub-control unit. By changing the values of the control gain parameters Kpp, Kvi and Kvp, the response of the motors M-1 to M-14 to the specified rotation angle Pref and the specified rotation speed Vref can be changed.

【0099】

Incidentally, FIG. 20 shows a specific processing procedure of the CPU 148 in such a motor rotation control calculation processing mode.

【0100】

When the specified rotation angle Pref is given by the sub-control units 73A to 73D, the CPU 148 first has the magnetic pole rotation speed of the rotor shaft 94 based on the first and second absolute angle sensor data D1A and D1B stored in the register 129. Calculate Nm (hereinafter referred to as rotor shaft pole rotation speed) (step SP1).

【0101】

The rotor shaft magnetic flux rotation speed Nm is a pair of adjacent N poles of the resin magnet 113 detected by the first to fourth Hall elements 114A to 114D of the rotor shaft magnetic flux angle sensor 110 as the rotor shaft 94 rotates. The change in magnetic flux due to the S pole is defined as the number of rotations per rotation. In this embodiment, since the resin magnet 113 is magnetized to four poles, the rotor shaft magnetic pole rotation speed Nm becomes 2 when the rotor shaft 94 makes one mechanical rotation.

【0102】

The rotor shaft pole rotation speed Nm is the phase θg of the first and second one-turn absolute angle sensor signals S1A and S1B represented by sinθg and cosθg, respectively, according to the rotor shaft pole rotation speed detection processing procedure shown in FIG. Is calculated by software processing based on the first and second absolute angle sensor data D1A and D1B stored in the register 41 (step SP1A), and the gear of the planetary gear mechanism unit 16 of the torque amplification unit 3 is set to this phase θg. Multiply the ratio N (step SP1B), divide this multiplication result by 2π, and add the number of magnetic poles of the resin magnet 113 of the rotor shaft magnetic pole angle sensor 110 (FIG. 9) to the integer part of the division result (in this embodiment). Can be obtained by multiplying the value Np, which is half the value of 4) (step SP1C).

【0103】

Further, as shown in FIG. 20, the CPU 148 has the following equation based on the rotor shaft magnetic pole rotation speed Nm calculated in this way and the rotor shaft magnetic pole rotation angle data Pm1 stored in the register 129. [0104]

[Number 5]

<img file="JP2000296484A_D0005.tif" />【0105】

With Pm0 given in, as the initial value, [0106]

[Number 6]

<img file="JP2000296484A_D0006.tif" />【0107】

The rotation angle Pm of the output shaft 106 at that time is calculated by executing the calculation of (step SP2).

【0108】

Then, the CPU 148 detects an error with respect to the specified rotation angle Pref (hereinafter, this is referred to as a rotation angle error) Pe by subtracting this rotation angle Pm from the specified rotation angle Pref (step SP3).

【0109】

Subsequently, the CPU 148 calculates the target rotation angle Vmref with respect to the specified rotation angle Pref by multiplying this rotation angle error Pe by the proportional gain Kpp (step SP4).

【0110】

Next, the CPU 148 calculates the rotation speed Vm of the output shaft 106 at that time by differentiating the rotor shaft magnetic pole rotation angle Pm1 stored in the register 129 (step SP5), and then calculates the target rotation speed in step SP4. The velocity error Ve is calculated by subtracting the rotational speed Vm calculated in step SP5 from Vmref (step SP6).

【0111】

Next, CPU148 is calculated by the following equation for this speed error Ve. [0112]

[Number 7]

<img file="JP2000296484A_D0007.tif" />【0113】

Sequentially multiply the velocity integral gain and proportional gain Kvp given in (step SP7 and step SP8). As a result, the target rotational torque T0 can be obtained.

【0114】

In the motor rotation control calculation processing mode, the CPU 148 starts this processing from step SP6 when the specified rotation speed Vref is given by the sub-control units 73A to 73D, and starts this processing when the rotation torque Tref is given. It is stored in the register 129 as the target rotation torque T0 as it is.

【0115】

(2-3-2) CPU148 processing in serial communication processing mode In the serial communication processing mode, the CPU 148 communicates with the sub-control units 73A to 73D, inputs control commands and change parameters from the sub-control units 73A to 73D, or sends an internal signal for monitoring to the sub-control units 73A to 73D. Send to 73D.

【0116】

(2-4) Relationship between coil drive current and output torque Here, the drive currents Iu, Iv, Iw and the output shaft 106 applied to the U-phase, V-phase, and W-phase coils 99A to 99F of the motor unit 90 in the motors M-1 to M-14 thus formed. The relationship with the rotational torque (hereinafter referred to as output torque) output to the outside via the above will be described.

【0117】

First, the magnetic flux density at which the U-phase, V-phase, and W-phase coils 99A to 99F intersect when the drive currents Iu, Iv, and Iw are applied to the U-phase, V-phase, and W-phase coils 99A to 99F is φu. , Φv, φw, the output torque T (θp) is calculated by the following equation using the magnetic flux angle θp of the rotor shaft 94 of the motor unit 90. [0118]

[Number 8]

<img file="JP2000296484A_D0008.tif" />【0119】

Given like. In Eq. (8), K0 represents a constant coefficient value when drive currents Iu, Iv, and Iw are applied to each coil 99A to 99F.

【0120】

Here, the drive currents Iu, Iv, and Iw applied to the U-phase, V-phase, and W-phase coils 99A to 99F are as follows. [0121]

[Number 9]

<img file="JP2000296484A_D0009.tif" />【0122】

[Number 10]

<img file="JP2000296484A_D0010.tif" />【0123】

[Number 11]

<img file="JP2000296484A_D0011.tif" />【0124】

Therefore, each magnetic flux density is φu, φv, and φw are the following equations, respectively. [0125]

[Number 12]

<img file="JP2000296484A_D0012.tif" />【0126】

[Number 13]

<img file="JP2000296484A_D0013.tif" />【0127】

[Number 14]

<img file="JP2000296484A_D0014.tif" />【0128】

Will be.

【0129】

Therefore, the output torque T (θp) is calculated by substituting these equations (9) to (14) into equation (8) and the following equation. [0130]

[Number 15]

<img file="JP2000296484A_D0015.tif" />【0131】

It can be expressed as.

【0132】

Therefore, it can be seen that the motors M-1 to M-14 can obtain an output torque proportional to the magnitudes of the drive currents Iu, Iv, and Iw applied to the coils 99A to 99F.

【0133】

(3) Rough terrain walking control processing In addition to this configuration, in the case of this robot 10, the sub-control units 73D of each leg unit 14A and 14B have ankles so that the robot 10 can walk correctly without losing balance even when the walking road surface is uneven ground. Rough terrain walking control processing is performed to control each of the motors M-13 and M-14 of the joint mechanism 43.

【0134】

In this case, in such a rough terrain walking control process, the back surface of the foot 44 that is in contact with the pedestrian road surface is made to imitate the inclination or unevenness of the pedestrian road surface (inclined according to the inclination or unevenness of the pedestrian road surface). This can be done by controlling the rotation of each of the motors M-13 and M-14 of the ankle joint mechanism 43. In such control, the external forces applied to the output shafts 106 (Fig. 9) of the motors M-13 and M-14 of the ankle joint mechanism 43 are always "0", and the motors M-13 and M- This can be done by controlling 14 rotations.

【0135】

Therefore, in the sub-control unit 73D of each leg unit 14A and 14B, the rotation that should be the target of each motor M-9 to M-14 at the first time interval from the main control unit 70 (Fig. 7) during walking operation. Each time an angle is given, the above-mentioned rough terrain walking control is performed according to the rough terrain walking processing procedure RT1 shown in FIG. 22, especially for the motors M-13 and M-14 of the ankle joint mechanism 43.

【0136】

That is, when the sub-control units 73D of the leg units 14A and 14B are given the target rotation angles of the motors M-13 and M-14 of the ankle joint mechanism 43 from the main control unit 70, this rough terrain walking process is performed. Step RT1 is started in step SP10, and in the following step SP11, the rotation angles to be the targets of the motors M-13 and M-14 of the ankle joint mechanism 43 given by the main control unit 70 in advance, and the main control this time. From the target rotation angles of these motors M-13 and M-14 given by unit 70, each motor M-13, which is the control cycle of each motor M-13 and M-14, for each 1 [ms]. Calculate the specified rotation angle, specified rotation speed, or specified rotation torque of M-14, respectively.

【0137】

Therefore, for example, if the main control unit 70 gives the sub control unit 73D the rotation angle that each motor M-13 and M-14 should target every 0.5 seconds, each motor M-13 every 1 [ms] , M-14 specified rotation angle, specified rotation speed or specified rotation torque are calculated in total of 50 in chronological order.

【0138】

Further, in this step SP11, the sub-control unit 73D sets the order of the designated rotation angle, the designated rotation speed, or the designated rotation torque (that is, the designated rotation angle, the designated rotation speed, or the designated rotation torque) counted by the internal counter in chronological order. Resets the count value that represents (the rank of some of the 50) to "0".

【0139】

Next, the sub-control unit 73D increments the count value of the internal counter by 1 in the subsequent step SP12, and then proceeds to step SP13 to set the designated rotation angle, the designated rotation speed, or the designated rotation torque corresponding to the count value to the ankle joint mechanism 43. It is sent to each of the motors M-13 and M-14.

【0140】

At this time, the sub-control unit 73D is connected to the power board 112 (FIG. 18) from the motors M-13 and M-14 by communicating with the motors M-13 and M-14 of the ankle joint mechanism 43 as described above. The 1st to 3rd drive current detection signals S3A to S3C (Fig. 18) output from the current sensors 71 (Fig. 18) of each coil drive block 140A to 140C (Fig. 18) are connected to the 1-chip microcomputer 115 (Fig. 19). ), The first to third drive current detection data D3A to D3C (FIG. 19) obtained by digital conversion in the first analog / digital conversion circuit 133 (FIG. 19) are given.

【0141】

Thus, the sub-control unit 73D applies the first to third drive current detection data D3A to D3C to the first to third drive current detection data D3A to supplied from the motors M-13 and M-14 in step SP14. D3C is taken in, and in the following step SP15, the output torque Tm of each of the motors M-13 and M-14 of the ankle joint mechanism 43 at that time is calculated based on the first to third drive current detection data D3A to D3C.

【0142】

Further, the sub-control unit 73D proceeds to step SP16 and subtracts the torque Tg generated on the output shaft 106 of each motor M-13 and M-14 due to its own weight at the time of the form stored in advance from this output torque Tm. , The torque generated in the output shaft 106 of each of the motors M-13 and M-14 due to the external force (hereinafter, this is referred to as the external force torque Tf) is calculated.

【0143】

Further, the sub-control unit 73D then proceeds to step SP17 and determines whether or not the calculated value of the external force torque Tf is 0 for each of the motors M-13 and M-14.

【0144】

Wherein the step to obtain a positive result in the flop SP17 means that the example is the back surface of or the toe portion 44 foot portion 44 is not grounded is grounded following the inclined or uneven walking road, this When the sub-control unit 74D returns to step SP12, the sub-control unit 74D processes the steps SP2 and subsequent steps in the same manner every 1 [ms] thereafter.

【0145】

On the other hand, obtaining a negative result in step SP17 means that, for example, the back surface of the foot portion 44 is in contact with the ground without following the inclination or unevenness of the pedestrian road surface. Proceed to step SP18 and give the motors M-13 and M-14 the next specified rotation so that the value of the external torque Tf applied to the output shafts 108 of the motors M-13 and M-14 approaches "0". After correcting the angle, the specified rotation speed, or the specified rotation torque, the process returns to step SP12, and then step SP2 and subsequent steps are processed in the same manner every 1 [ms].

【0146】

In this way, in this robot 10, the designated rotation angle, the designated rotation speed, or the designation given to the motors M-13 and M-14 of the ankle joint mechanism 43 by the sub-control unit 73D of each leg unit 14A and 14B as needed. By correcting the rotational torque, it is possible to walk correctly without losing balance even on rough terrain.

【0147】

(4) Operation and effect of this embodiment In the above configuration, in this robot 10, the external force torque Tf applied to the output shafts 106 of the motors M-13 and M-14 of the ankle joint mechanism 43 of the leg units 14A and 14B is always "0" during the walking motion. The rotation of these motors M-13 and M-14 is controlled so as to be.

【0148】

Therefore, in this robot 10, even when the walking road surface is uneven ground with slopes and irregularities, the back surface of the foot 44 can always be made to imitate the walking road surface, and the walking can be performed stably by that amount. .. In this way, in this robot 10, various sensors for controlling walking on rough terrain, which have been conventionally used, can be omitted.

【0149】

Further, in this robot 10, since the control board 111 and the power board 112 for driving and controlling the motor unit 90 are housed inside the motors M-1 to M-14, each sub control unit is as shown in FIG. All motors M-1 to M-14 can be connected to 73A to 73D via a total of 7 cables, and the wiring amount of the robot as a whole can be reduced accordingly.

【0150】

Further, in this robot 10, since the motor unit 90 and the torque amplification unit 91 are compactly integrated as the motors M-1 to M-14 as shown in FIG. 9, the motor unit 90 and the torque amplification unit are used. Each joint mechanism (lumbar joint mechanism 22, neck joint mechanism 26, shoulder joint mechanism 29, elbow joint mechanism 32, hip joint mechanism 36, knee joint mechanism 41 and ankle joint mechanism) is compared with the one in which the part 91 is configured separately. The configuration of 43) can be simplified, and each joint mechanism (and robot 10) can be miniaturized accordingly.

【0151】

According to the above configuration, in the bipedal walking type robot 10, the external force applied to the output shafts 106 of the motors M-13 and M-14 of the ankle joint mechanism 43 of the leg units 14A and 14B during the walking motion. By controlling the rotation of these motors M-13 and M-14 so that the torque Tg is always "0", it is always stable even when the walking road surface is uneven ground with slopes and irregularities. Can walk. In this way, various sensors for controlling walking on rough terrain, which have been conventionally used, can be omitted, and thus a robot capable of simplifying the configuration can be realized.

【0152】

Further, in the robot 10, the control board 111 and the power board 112 for driving and controlling the motor unit 90 are housed inside each of the motors M-1 to M-14, thereby reducing the wiring amount of the robot as a whole. Therefore, it is possible to realize a robot that can further simplify the configuration.

【0153】

(5) Other embodiments In the above-described embodiment, the case where the present invention is applied to the bipedal robot 10 has been described, but the present invention is not limited to this, and can be widely applied to various other robots. it can. In this case, the invention relating to the control of the joint mechanism in the present invention can be widely applied to a quadruped walking robot and other leg walking robots. Further, the invention in which the motor control means is provided in the motor unit as a driving source of each joint mechanism in the present invention can be widely applied to robots other than walking robots.

【0154】

Further, in the above-described embodiment, the invention relating to the control of the joint mechanism in the present invention is the lower leg portion as the first link (first component) of the leg units 14A and 14B, and the first. The case where the application is applied to the ankle joint 43 connecting the foot portion 44 as the link (second component) of 2 has been described, but the present invention is not limited to this, and the present invention is not limited to this, for example, the wrist joint other than the ankle joint 43. It can be widely applied to such as.

【0155】

Further, in the above-described embodiment, a coil provided with a current sensor 71 as a current detecting means for detecting the drive currents Iv, Iu, and Iw (FIG. 18) of the motors M-1 to M-14 as shown in FIG. However, the present invention is not limited to this, and various other configurations can be widely applied depending on the configuration of the actuator to be applied.

【0156】

Further, in the above-described embodiment, the motors M-13 and M are based on the drive currents Iv, Iu and Iw (FIG. 18) of the motors M-13 and M-14 detected by the current sensor 71 (FIG. 18). The case where the torque due to the external force applied to the output shaft 106 of -14 (the function as the external force torque detecting means for detecting the external force torque Tf is provided in the sub-control unit 73D of each leg unit 14A and 14B has been described. The present invention is not limited to this, and such a function may be provided to the arithmetic processing block 128 (FIG. 19) in the motors M-13 and M-14.

【0157】

Further, in the above-described embodiment, the control board 111 and the power board 122 as the motor control means for driving and controlling the motor unit 90 of each of the motors M-1 to M-14 configured as shown in FIG. 9 are shown in FIGS. 17 to 17 to 4. Although the case where the configuration is as shown in FIG. 21 has been described, the present invention is not limited to this, and various other configurations can be widely applied.

【0158】

[Effect of the invention]

As described above, according to the present invention, in the control device of the joint mechanism, the actuator is equipped with a current detecting means for detecting the current value of the driving current of the actuator of the joint mechanism and a current value detected by the current detecting means. By providing an external force torque detecting means for detecting the magnitude of the torque due to the external force applied to the output shaft, the external force applied to the first or second link can be obtained. In this way, it is possible to realize a control device for the joint mechanism that can simplify the overall configuration by omitting the installation of a sensor or the like for directly measuring the external force.

【0159】

Further, according to the present invention, in the method of controlling the joint mechanism, the first step of detecting the current value of the drive current of the actuator that drives the joint mechanism and the detected current value are applied to the output shaft of the actuator. By providing the second step of detecting the magnitude of the torque due to the external force, the external force applied to the first or second link can be obtained. In this way, it is possible to realize a control device for the joint mechanism that can simplify the configuration of the joint mechanism as a whole by omitting the installation of a sensor or the like for directly measuring the external force.

【0160】

Further, according to the present invention, in the robot device, an external force applied to the output shaft of the actuator based on the current detecting means for detecting the current value of the drive current of the actuator of the joint mechanism and the current value detected by the current detecting means. By providing the external force torque detecting means for detecting the magnitude of the torque due to the above, the external force applied to the first or second component can be obtained. In this way, it is possible to realize a robot device that can simplify the overall configuration by omitting the installation of a sensor or the like for directly measuring the external force.

【0161】

Further, according to the present invention, in the control method of the robot device, the first step of detecting the current value of the drive current of the actuator and the magnitude of the torque due to the external force applied to the output shaft of the actuator based on the detected current value. By providing a second step for detecting the torque, it is possible to obtain an external force applied to the first or second component. In this way, it is possible to realize a control method of the robot device that can simplify the configuration of the robot device as a whole by omitting the installation of a sensor or the like for directly measuring the external force.

【0162】

Further, according to the present invention, in a two-legged walking type robot device, an actuator provided in the ankle joint mechanism to generate rotational torque for rotationally driving the foot around a predetermined axis, and a current value of the drive current of the actuator. The detection result of the external force torque detecting means and the external force torque detecting means for detecting the magnitude of the torque due to the external force applied to the output shaft of the actuator based on the current value detected by the current detecting means and the current detecting means. By providing a control means to control the actuator so that the torque due to the external force applied to the output shaft of the actuator is eliminated, the balance of the rough ground is not lost without the need for a sensor or the like. It is possible to realize a robot device capable of walking and thus simplifying the overall configuration.

【0163】

Further, in the present invention, in the control method of the two-legged walking type robot device, the current value of the drive current of the actuator provided in the ankle joint mechanism and generating the rotational torque for rotationally driving the foot around a predetermined axis is calculated. Based on the detection results of the first step to be detected, the second step to detect the magnitude of the torque due to the external force applied to the output shaft of the actuator based on the detected current value, and the detection result of the second step. By providing a third step to control the actuator so that the torque due to the external force applied to the output shaft of the actuator is eliminated, the robot device does not lose the balance of the rough ground without the need for a sensor or the like. It is possible to realize a control method of the robot device which can simplify the configuration as a whole.

【0164】

Further, in the present invention, in the joint device, a motor unit for generating rotational torque and a motor control means for driving and controlling the motor unit are provided in the actuator, and the motor control means is arranged in the motor unit. The amount of wiring between the actuator and the outside can be significantly reduced, and thus a joint device capable of simplifying the configuration can be realized.

【0165】

Further, in the present invention, in the robot device, a motor unit that generates rotational torque and a motor control means that drives and controls the motor unit are provided in the actuator that drives the joint mechanism, and the motor control means is arranged in the motor unit. As a result, the amount of wiring for each actuator of the joint mechanism can be remarkably reduced, and thus a robot device capable of simplifying the configuration can be realized.

[Simple explanation of drawings]

[Figure 1]

It is a perspective view which shows the structure of the bipedal walking type robot by this embodiment.

[Figure 2]

It is a perspective view which shows the structure of the bipedal walking type robot by this embodiment.

[Fig. 3]

It is a conceptual diagram which shows the structure of the bipedal walking type robot by this embodiment.

[Fig. 4]

The front view and the side view which show the structure of the hip joint mechanism respectively.

[Fig. 5]

It is the top view and the perspective view which show the structure of the hip joint mechanism, respectively.

[Fig. 6]

It is a side view, the front view and the side view which show the structure of the ankle joint mechanism, respectively.

[Fig. 7]

It is a block diagram which shows the internal structure of the bipedal walking type robot shown in FIG.

[Fig. 8]

It is a block diagram which shows the state of the electric connection between a sub-control unit and each motor.

[Fig. 9]

It is sectional drawing which shows the structure of each motor.

[Fig. 10]

It is a schematic diagram which shows the structure of a rotor and a rotor shaft magnetic pole angle sensor.

[Fig. 11]

It is a partial cross-sectional view which provides the explanation of the positional relationship of a rotor and a stator core.

[Fig. 12]

It is a schematic diagram which shows the structure of a stator and a power board.

[Fig. 13]

It is a schematic diagram which shows the structure of the torque amplification part.

[Fig. 14]

It is a waveform diagram which provides the explanation of the magnetizing pattern of a resin magnet in a 1-turn absolute angle sensor.

[Fig. 15]

It is a waveform diagram which provides the explanation of the 1st and 2nd 1 rotation absolute angle sensor signals.

[Fig. 16]

It is a schematic plan view which shows the structure of a control board.

[Fig. 17]

It is a block diagram which shows the structure of a control board.

[Fig. 18]

It is a block diagram which shows the structure of a power board.

[Fig. 19]

It is a block diagram which shows the structure of a 1-chip microcomputer.

[Fig. 20]

It is a block diagram which provides the explanation of the arithmetic processing of a CPU in the motor rotation control processing mode.

[Fig. 21]

It is a block diagram which shows the rotor shaft magnetic pole rotation speed detection processing procedure.

[Fig. 22]

It is a flowchart which shows the walking control procedure on rough terrain.

[Fig. 23]

It is a block diagram which shows the connection relationship between each motor and a main control part in a conventional robot.

[Explanation of symbols]

10 ...... Robot, 11 ...... Body unit, 12 ...... Head unit, 13A, 13B ... Arm unit, 14A, 14B ... ... leg unit, 43 ... ankle joint mechanism, 44 ... foot, 70 ... main control, 73A ~ 73D ... sub control Department, 106 ...... Output shaft, 111 ...... Control board, 112 ...... Power board, 148 ...... CPU, M-1 ~ M14 .... .. Motor, RT1 ...... Rough terrain walking control processing procedure.