JP2012166315A - Robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot that always maintains a stable state regardless of a surrounding environment.SOLUTION: The robot 1 includes a carriage 11 and an arm 12. The carriage 11 makes contact with a ground surface through a plurality of contact points. The arm 12 is supported by the carriage 11, and also coupled to the carriage 11 so as to be displaceable relative to the carriage. A carriage control part controls, by rotating the carriage 11 around a vertical axis, a direction of a polygon (support polygon) formed by mutually connecting the plurality of contact points relative to the arm 12. The carriage control part rotates the carriage 11 so that the ZMP position of the robot 1 comes inside the support polygon as viewed from above.

Description

  The present invention relates to a robot, and more particularly to a robot supported on the ground at a plurality of contact points.

  In a robot having an arm, the center of gravity of the robot moves due to the rotation operation and expansion / contraction operation of the arm. The robot is affected by the moment generated by the movement of the center of gravity. When this moment increases, it becomes difficult for the robot to maintain a stable state. For this reason, many techniques for controlling the position of the center of gravity of the robot have been proposed.

  For example, Patent Document 1 discloses a self-supporting traveling device that includes a plurality of driving wheels, detects movement of the center of gravity, and controls the driving wheels so that a stable inverted state can be maintained. Specifically, when an external force is applied to the self-supporting traveling device and the center of gravity of the self-supporting traveling device is displaced, the self-supporting traveling device moves in the displacement direction of the center of gravity by driving the driving wheel, and returns the center of gravity to a stable position. .

JP 2010-9226 A

  However, the self-supporting traveling device described in Patent Document 1 needs to move in parallel in the displacement direction of the center of gravity in order to maintain the inverted state. Therefore, a sufficient moving space is required around the self-supporting traveling device. Therefore, in a situation where a moving space cannot be secured around the self-supporting traveling device due to the presence of an obstacle, the self-supporting traveling device cannot return the center of gravity to a stable position. That is, the self-supporting traveling device has a problem that it cannot maintain a stable state depending on the surrounding environment.

  The present invention has been made to solve such a problem, and an object thereof is to provide a robot capable of maintaining a stable state regardless of the surrounding environment.

  A robot according to the present invention is a robot including a carriage that is in contact with the ground at a plurality of contact points, and is supported by the carriage and connected to the carriage so as to be relatively displaceable. A carriage control means for controlling the relative orientation of the polygon formed by connecting the plurality of grounding points with respect to the movable portion by rotating the carriage about a vertical axis, and The cart control means rotates the cart so that the ZMP of the robot is positioned inside the polygon in plan view. Thereby, ZMP which fluctuate | varied by the displacement of a movable part can be stored inside a support polygon, without moving a trolley | bogie in parallel.

  The cart control means may rotate the cart so that the ZMP of the robot and the polygonal side closest to the ZMP of the robot are separated in a plan view. As a result, the ZMP can be separated from the sides of the support polygon, and the stability of the robot is improved.

  In addition, the cart control means may be arranged such that the ZMP of the robot approaches a line connecting two ground points that are the farthest among the plurality of ground points in a plan view. May be rotated. As a result, the ZMP can be separated from the sides of the support polygon, and the stability of the robot is improved.

  Moreover, it further includes a movable part control means for controlling the operation of the movable part, wherein the carriage control means rotates the carriage with respect to the ground, and the movable part control means rotates the carriage with respect to the ground. The movable portion may be rotated about the vertical axis relative to the carriage in a direction opposite to the direction at substantially the same speed as the rotation speed of the carriage. Thereby, the relative direction of the support polygon with respect to the movable part can be changed while maintaining the relative direction of the movable part with respect to the ground.

  Further, the cart control means may rotate the cart so that the rotation axis of the movable portion around the vertical axis coincides with the rotation axis of the cart. Thereby, the relative direction of the support polygon with respect to the movable part can be changed while maintaining the relative position of the movable part with respect to the ground.

  A movable part trajectory calculating means for calculating a trajectory of the movable part; a ZMP trajectory calculating part for calculating a ZMP trajectory of the robot based on the trajectory of the movable part calculated by the movable part trajectory calculating means; The cart control means may rotate the cart so that the ZMP trajectory of the robot exists inside the polygon. Thereby, a cart can be rotated according to the track of a movable part.

  Further, the cart control means may rotate the cart in advance before the movable part operates so that the track of the ZMP exists inside the polygon. This eliminates the need to rotate the carriage while the movable part is moving.

  Further, the cart control means may leave the cart stationary when the ZMP exists in a non-moving region provided inside the polygon. Thereby, useless operation | movement of a trolley | bogie can be prevented.

  The polygon is a quadrangle, and the cart control means rotates the cart so that an intersection of two diagonal lines of the quadrangle and a rotation axis of the cart coincide with each other, and the ZMP does not move. The robot according to claim 8, wherein, when the vehicle is outside the region, the cart is rotated so that the ZMP and a longer diagonal of the two diagonals are brought closer to each other. Thereby, the carriage can be rotated in a more stable direction.

  The area of the immovable region may be set in proportion to the maximum rotation speed of the carriage. Thereby, even if the maximum rotation speed of the carriage is slow, the carriage can be rotated early and the ZMP can be accommodated inside the support polygon.

  According to the present invention, it is possible to provide a robot that can maintain a stable state regardless of the surrounding environment.

1 is a side view of a robot according to a first embodiment. FIG. 3 is a rear view of the robot according to the first exemplary embodiment. FIG. 2 is a bottom view of the robot according to the first exemplary embodiment. 1 is a plan view of a robot according to a first embodiment. FIG. 2 is a block diagram of a control unit of the robot according to the first embodiment. 3 is a flowchart illustrating an operation of the robot according to the first exemplary embodiment. FIG. 6 is a diagram for explaining a rotation operation of the robot according to the first embodiment; (A) is a figure before a trolley rotates. (B) is a figure which shows the case where a trolley rotates counterclockwise. (C) is a figure which shows the case where a trolley rotates clockwise. It is a figure for demonstrating the relative positional relationship of the trolley | bogie and arm concerning Embodiment 1. FIG. (A) is a top view before rotation. (B) is a plan view after rotation. FIG. 6 is a rear view showing the rotation operation of the robot according to the first exemplary embodiment. FIG. 3 is a block diagram of a robot according to a second embodiment. 6 is a flowchart showing the operation of the robot according to the second exemplary embodiment. FIG. 10 is a diagram for explaining the operation of the robot according to the second embodiment. (A) is a top view before rotation. (B) is a plan view after rotation. FIG. 6 is a block diagram of a robot according to a third embodiment. 10 is a flowchart showing the operation of the robot according to the third exemplary embodiment. It is a figure for demonstrating the immovable area | region concerning Embodiment 3. FIG. It is a figure for demonstrating calculation of the rotation amount of the trolley | bogie concerning Embodiment 3. FIG. (A) is a figure before rotation. (B) is a view after rotation. FIG. 10 is a diagram for explaining a method for setting a non-moving area according to the third embodiment. It is a top view of the robot concerning other examples. It is a top view of the robot concerning other examples.

<Embodiment 1>
Embodiments of the present invention will be described below with reference to the drawings. The configuration of the robot 1 according to this embodiment is shown in FIGS. FIG. 1 is a side view of the robot 1, and FIG. 2 is a rear view of the robot 1. The robot 1 includes a carriage 11 and an arm 12.

  The cart 11 includes a cart body 111, drive wheels 112a and 112b, and casters 113a and 113b. The carriage 11 is in contact with the ground at the four contact points of the drive wheels 112a and 112b and the casters 113a and 113b.

  The cart body 111 has a cylindrical shape and supports the arm 12. The drive wheels 112a and 112b are coaxially disposed on the lower surface of the carriage main body 111. The rotation shafts of the drive wheels 112 a and b are fixed relatively to the carriage main body 111. The driving wheels 112a and 112b are rotationally driven by a motor (not shown). The casters 113a and 113b are free casters capable of traveling in all directions and do not have a driving means such as a motor. The casters 113a and 113b rotate in a direction corresponding to the moving direction of the carriage main body 111.

  That is, when the drive wheels 112a, b are directed in the same direction and rotated in the same direction, the robot 1 moves in the direction in which the drive wheels 112a, b are directed. On the other hand, when the driving wheels 112a and 112b are directed in the same direction and rotate in opposite directions, the robot 1 rotates around the vertical axis (yaw axis). In particular, when the drive wheels 112a, b rotate at the same speed, they rotate on the spot.

  The arm 12 includes arm members 121a to 121c, joints 122a and 122b, and a hand portion 123. The arm 12 is supported by the carriage 11 and connected to the carriage 11 so as to be relatively displaceable.

  One end of the arm member 121a is connected to the cart body 111, and the other end is connected to the arm member 121b via a joint 122a. The arm member 121b has one end connected to the arm member 121a and the other end connected to the joint 122b. The arm member 121 c has one end connected to the joint 122 b and the other end connected to the hand portion 123.

  The arm member 121a has a columnar shape and is erected in the vertical axis direction on the upper surface of the carriage body 111. The arm member 121a is fixed in position with respect to the carriage main body 111. Therefore, when the carriage 11 rotates, the arm member 121a also rotates around the vertical axis. The arm member 121b can rotate around the central axis of the arm member 121a when a motor (not shown) provided in the joint 122a is rotationally driven. When the arm member 121b rotates, the tip portion (from the joint 122b to the hand portion 123) connected to the arm member 121b rotates. That is, the robot 1 can change the direction of the arm 12 by rotating the carriage 11 or rotating the motor provided in the joint 122a.

  A motor (not shown) is also provided inside the joint 122b, and the arm members 121b and c can freely rotate within the movable range of the joint 122b. Note that the hand unit 123 is also provided with a motor (not shown), and is configured to be able to grip an object by driving the motor.

  Next, the arrangement positions of the drive wheels 112a and 112b and the casters 113a and 113b will be described in detail. FIG. 3 is a bottom view of the robot 1. In FIG. 3, the upper side is the rear of the robot 1, and the lower side is the front of the robot 1. The drive wheels 112a and 112b are arranged side by side in the left-right direction of the robot 1. As described above, the drive wheel 112a and the drive wheel 112b are disposed on the same axis L1. The casters 113a and 113b are arranged side by side in the front-rear direction of the robot 1. The casters 113a and 113b are disposed on an axis L2 that is substantially orthogonal to the axis L1. Note that the caster 113 a is disposed slightly apart from the outer peripheral surface of the carriage main body 111.

  The support polygon 90 is a quadrangle formed by connecting the grounding points 31 and 32 between the driving wheels 112a and 112b and the ground and the grounding points 33 and 34 between the casters 113a and 113b and the ground. The shortest distance between any of the contact points and the outer peripheral surface of the cart body 111 is shorter than the shortest distance between the other contact points and the outer peripheral surface of the cart body 111. The support polygon 90 is a stable region of ZMP (Zero Moment Point). ZMP is a pressure center point of the floor reaction force applied to the robot 1 and is a point on the floor where the moment due to the floor reaction force on the robot 1 is zero. Therefore, the center of gravity of the robot 1 and the ZMP are different points. The diagonal line of the support polygon 90 coincides with the axes L1 and L2.

  FIG. 4 is a plan view of the robot 1. In FIG. 4, the upper side is the rear of the robot 1, and the lower side is the front of the robot 1. In addition, illustration is abbreviate | omitted after the front-end | tip part of the arm member 121b. The central axis of the arm member 121a (the rotation axis of the arm 12) is located inside the support polygon 90 in plan view. Preferably, as shown in FIG. 4, the arm member 121 a is disposed at a position where the central axis of the arm member 121 a coincides with the rotating shaft 100 of the carriage 11. Thereby, when the trolley | bogie 11 rotates, the relative position with respect to the ground of the arm 12 does not change.

  Next, the control unit of the robot 1 will be described. FIG. 5 is a block diagram of the control unit of the robot 1. The control unit of the robot 1 includes a CPU (Central Processing Unit) 21, an arm control unit 22, a cart control unit 23, and a ZMP measurement unit 24.

  The CPU 21 is a central control device that executes various processes and commands in the control unit based on execution programs and input data. The arm control unit 22 outputs a control signal for controlling driving of the motors of the joints 122a and 122b of the arm 12 to operate the arm 12. The carriage control unit 23 outputs a control signal for controlling the driving of the motors of the drive wheels 112a and 112b of the carriage 11 and rotates the drive wheels 112a and 112b. The ZMP measurement unit 24 is configured by, for example, a robot center-of-gravity acceleration measurement device or a floor reaction force measurement device, and measures the ZMP position of the robot 1. Since the ZMP measurement technique is known, detailed description thereof is omitted.

  Next, an operation example of the robot 1 according to the present embodiment will be described with reference to the flowchart of FIG. First, the arm control unit 22 operates the arm 12 (step S101). Then, the ZMP measuring unit 24 measures the ZMP position of the robot 1 (step S102). The ZMP measurement unit 24 determines whether or not the measured ZMP position information (information for specifying the position of the ZMP) has changed (step S103). When the position of the ZMP is fluctuating (step S103: Yes), the ZMP measurement unit 24 outputs the position information of the ZMP to the cart control unit 23.

  The trolley | bogie control part 23 rotates the trolley | bogie 11 based on the positional information on ZMP (step S104). Specifically, the cart control unit 23 rotates the cart 11 so that the ZMP is positioned inside the support polygon 90 in plan view.

  Then, the arm control unit 22 determines whether or not the arm 12 is finished (step S105). When the operation of the arm 12 is finished (step S105: Yes), the operation of the robot 1 is finished. On the other hand, when the operation of the arm 12 is not completed (step S105: No), the arm control unit 22 operates the arm 12 again (step S101). Even if the position of the ZMP has not changed even after the operation of the arm 12 (step S103: No), the arm control unit 22 operates the arm 12 again (step S101).

  Here, the rotation direction of the carriage 11 for maintaining the position of the ZMP inside the support polygon 90 will be described. For example, it is rotated so that the distance between the side of the support polygon 90 located closest to the ZMP and the ZMP in plan view increases. In other words, in a plan view, the ZMP is rotated away from the normal line closest to the ZMP among the normal lines drawn from the rotation axis of the carriage 11 to each side of the support polygon 90.

  An example of the above rotation operation will be described with reference to FIG. FIG. 7A is a view before the carriage 11 is rotated, FIG. 7B is a view showing the case where the carriage 11 is rotated counterclockwise, and FIG. 7C is a diagram where the carriage 11 is rotated clockwise. FIG. In plan view, the distance between the side 101 of the supporting polygon located closest to the ZMP 200 and the ZMP 200 is d. The cart controller 23 rotates the cart 11 in the direction in which the distance d increases.

  In the example of FIG. 7, when the cart control unit 23 rotates the cart 11 counterclockwise, the distance d increases (see FIG. 7B). In other words, the cart control unit 23 moves the ZMP 200 away from the normal 102 that is closest to the ZMP 200 among the normals drawn from the rotating shaft 100 of the cart 11 to each side of the support polygon 90 (counterclockwise). ), The carriage 11 is rotated. By rotating in this direction, the ZMP 200 moves away from the sides of the support polygon 90. Therefore, it is possible to suppress the ZMP 200 from coming out of the inside of the support polygon 90. On the other hand, when the carriage 11 is rotated clockwise, the distance d decreases and the ZMP 200 approaches the normal line 102 (see FIG. 7C). As for the rotation amount (rotation angle), the rotation amount may be an arbitrary amount as long as the distance d after rotation becomes larger than the distance d before rotation.

  Further, the robot 1 rotates the carriage 11 and the arm 12 in order to change the relative orientation of the support polygon 90 with respect to the arm 12. Therefore, even if the carriage 11 rotates, the direction of the arm 12 does not change.

  A relative rotational operation between the carriage 11 and the arm 12 will be described with reference to FIG. 8A is a plan view of the robot 1 before the carriage 11 and the arm 12 are rotated, and FIG. 8B is a plan view of the robot 1 after the carriage 11 and the arm 12 are rotated. As shown in FIG. 8A, when the cart control unit 23 tries to rotate the cart 11 clockwise, the arm control unit 22 rotates the arm 12 counterclockwise in the direction opposite to the cart 11. The motor of the joint 122a is controlled so that Further, the arm control unit 22 sets the rotation speed of the arm 12 to substantially the same speed as the rotation speed of the carriage 11. In addition, the rotation direction of the cart 11 and the arm 12 in the rear view of the robot 1 is shown in FIG. This prevents the direction of the arm 12 from changing with the rotation of the carriage 11. That is, only the carriage 11 is rotated while maintaining the relative position of the arm 12 with respect to the ground. As a result, the relative orientation of the support polygon 90 with respect to the arm 12 can be changed (see FIG. 8B).

  The center of gravity of the carriage 11 is preferably on the rotation axis of the carriage 11 (or a position sufficiently close to the rotation axis). If the center of gravity of the carriage 11 is located on the rotation axis of the carriage 11, the position of the ZMP does not change even if the carriage 11 rotates (the movement of the ZMP due to the position and operation of the arm 12 is not considered). Therefore, when the carriage 11 is rotated in order to store the ZMP inside the support polygon 90, only the movement of the ZMP due to the position and operation of the arm 12 needs to be considered, and the movement of the ZMP only due to the rotation of the carriage 11 is considered. You don't have to.

  As described above, according to the configuration of the robot 1 according to the present embodiment, the cart controller 23 rotates the cart 11 around the vertical axis so that the relative direction of the support polygon 90 with respect to the arm 12 is rotated. Control. Specifically, the cart control unit 23 rotates the cart 11 so that the ZMP of the robot 1 is positioned inside the support polygon 90 in plan view. Therefore, even if the position of the ZMP varies due to the operation of the arm 12, the ZMP can be stored inside the support polygon 90. Furthermore, since the control of the orientation of the support polygon 90 by the carriage control unit 23 is realized by the rotation operation of the carriage 11, a sufficient space around the robot 1 is not required. As a result, the robot 1 can maintain a stable state regardless of the surrounding environment. The stable state means a state where the ZMP is contained in the support polygon of the robot.

  Furthermore, the configuration of the robot 1 includes a general arm robot such as a motor disposed in the drive wheels 112a and 112b for moving the robot 1 and a motor disposed in the joint 122a for rotating the arm 12. It is sufficient to provide only the drive mechanism. That is, it is not necessary to separately provide a drive mechanism for changing the orientation and shape of the support polygon of the robot. Therefore, the manufacturing cost of the robot can be suppressed.

<Embodiment 2>
A second embodiment according to the present invention will be described. FIG. 10 shows a block diagram of the control unit of the robot 2 according to the present embodiment. The control unit of the robot 2 includes a CPU 21, an arm control unit 22, a cart control unit 23, an arm trajectory planning unit 25, and a ZMP trajectory prediction unit 26. Since the other configuration is the same as that of the robot 1, description thereof will be omitted as appropriate.

  The arm trajectory planning unit 25 plans a trajectory that brings the arm 12 closer to the grasped object based on the position of the grasped object of the arm 12, the position of surrounding obstacles, and the like. As a trajectory planning method for the arm 12, an existing trajectory planning method such as RRT (Rapidly-exploring Random Trees) may be used.

  The ZMP trajectory prediction unit 26 predicts the trajectory of the ZMP based on the trajectory information of the arm 12 planned by the arm trajectory planning unit 25 (information for specifying the trajectory of the arm 12). Specifically, the ZMP trajectory prediction unit 26 models the robot 2 and predicts the ZMP trajectory from the position, moment, and acceleration of the center of gravity of the robot 2 predicted based on the trajectory information of the arm 12. Since the ZMP trajectory prediction method is a known technique, detailed description thereof is omitted.

  Next, an operation example of the robot 2 according to the present embodiment will be described with reference to a flowchart shown in FIG. First, the arm trajectory planning unit 25 plans the trajectory of the arm 12 according to the work of the arm 12 (for example, the work of the hand unit 123 grasping the grasped object) (step S201). The arm trajectory planning unit 25 outputs the planned trajectory information to the arm control unit 22 and the ZMP trajectory prediction unit 26.

  The ZMP trajectory prediction unit 26 predicts a ZMP trajectory based on the input trajectory information (step S202). The ZMP trajectory of the robot 2 is shown in FIG. In plan view, the ZMP of the robot 2 moves from the central axis of the arm member 121a to the left front of the robot 2, and as it approaches the outer peripheral surface of the carriage 11, the trajectory 201 returns to the central axis of the arm member 121a again. Draw. As shown in FIG. 12A, the ZMP trajectory 201 means a set of points in which the positions of the moving ZMP are plotted at a predetermined interval in plan view.

  At this time, a part of the ZMP track 201 protrudes outside the support polygon 90. Therefore, if the arm 12 is operated with the position of the carriage 11 shown in FIG. 12A, it is difficult for the robot 2 to maintain a stable state.

  Therefore, the carriage control unit 23 rotates the carriage 11 in advance before the operation of the arm 12 so that the ZMP trajectory fits inside the support polygon 90 (step S203). At this time, similarly to the first embodiment, the arm control unit 22 rotates the arm 12 in a direction opposite to the rotation direction of the carriage 11 at substantially the same speed as the rotation speed of the carriage 11. Thereby, the relative orientation of the support polygon 90 with respect to the arm 12 is changed, and the relative position of the arm 12 with respect to the ground is not changed. FIG. 12B shows a view after the carriage 11 and the arm 12 are rotated. After the rotation, since the ZMP trajectory 201 is located inside the support polygon 90, the robot 2 can maintain a stable state.

  Here, an example of a method for accommodating all of the ZMP trajectory 201 inside the support polygon 90 will be described. The angle θ at which all of the ZMP trajectory 201 falls within the support polygon 90 can be obtained by the steepest descent method. More specifically, out of a plurality of ZMPs included in the ZMP trajectory 201, the number of ZMPs located outside the support polygon 90 is defined as fzmp (θ), and the direction of fzmp (θ) decreases by the steepest descent method θ Will change. That is, by changing the orientation of the support polygon 90, the search is performed for θ where all the points of the ZMP trajectory 201 shown in FIG. And the trolley | bogie control part 23 rotates the trolley | bogie 11 only (theta) when fzmp ((theta)) becomes the minimum (it is 0 pieces in FIG.12 (b)). At the same time, the arm control unit 22 rotates the arm 12 by −θ. The method for calculating the angle θ at which all of the ZMP trajectory 201 falls within the support polygon 90 is not limited to the above steepest descent method, and various methods can be used.

  When the rotation operation of the carriage 11 and the arm 12 is completed, the arm control unit 22 drives the joints 122a, b provided on the arm 12 based on the trajectory information input from the arm trajectory planning unit 25 (step S204). .

  As described above, according to the configuration of the robot 2 according to the present embodiment, the ZMP trajectory prediction unit 26 predicts the ZMP trajectory based on the trajectory information of the arm 12. Then, the cart control unit 23 changes the relative orientation of the support polygon 90 with respect to the arm 12 in advance before operating the arm 12 so that the ZMP trajectory fits inside the support polygon 90. Therefore, after the operation of the arm 12, the cart control unit 23 may not change the orientation of the support polygon 90 as needed in accordance with the movement of the ZMP. Of course, since all of the ZMP trajectories are located inside the support polygon 90, the robot 2 can maintain a stable state during the operation of the arm 12.

<Embodiment 3>
A third embodiment according to the present invention will be described. FIG. 13 shows a block diagram of a control unit of the robot 3 according to the present embodiment. The control unit of the robot 3 includes a CPU 21, an arm control unit 22, a cart control unit 23, a ZMP measurement unit 24, a non-moving region setting unit 27, and a rotation determination unit 28. Since the other configuration is the same as that of the robot 1, description thereof will be omitted as appropriate.

  The immovable area setting unit 27 sets an immovable area inside the support polygon 90. Here, the immovable area is an area where the movement of the ZMP of the robot 3 is permitted. In other words, even if the ZMP of the robot 3 moves within the immovable region, the carriage control unit 23 does not rotate the carriage 11 and maintains a stationary state.

  The rotation determination unit 28 determines whether or not the ZMP position measured by the ZMP measurement unit 24 is outside the immovable region.

  Next, the operation of the robot 3 according to the present embodiment will be described with reference to the flowchart shown in FIG. First, the non-moving area setting unit 27 sets a non-moving area inside the support polygon 90 (step S301). The immovable area setting unit 27 outputs area information of the immovable area (information for specifying the position of the immovable area) to the rotation determination unit 28.

  Here, FIG. 15 shows an example of the immovable region. The immovable area 91 (hatched area) is a quadrangular area centered on the intersection of the diagonal lines of the support polygon 90 (the rotation axis of the carriage 11). That is, the intersection of the diagonal lines of the support polygon 90 and the intersection of the diagonal lines of the immobile area 91 coincide. Further, the shorter diagonal line L4 of the immovable region 91 is shorter than the shorter diagonal line L2 of the support polygon 90. The longer diagonal line L3 of the immovable region 91 is substantially the same length as the longer diagonal line L1 of the support polygon 90. Details of the method for setting the immovable area 91 will be described later.

  Next, the arm control unit 22 moves the arm 12 (step S302). As a result, the ZMP of the robot 3 moves. Then, the ZMP measuring unit 24 measures the ZMP position of the robot 3 (step S303). The ZMP measurement unit 24 outputs the ZMP position information to the carriage control unit 23 and the rotation determination unit 28.

  The rotation determination unit 28 determines whether the ZMP is located outside the non-moving area 91 based on the area information of the non-moving area 91 and the position information of the ZMP (Step S304). When the ZMP is located inside the immovable region 91 (including the region line) (step S304: No), the arm control unit 22 operates the arm 12 again (step S303). Note that the ZMP position measurement is periodically performed every predetermined time or every time the ZMP moves by a predetermined distance.

  When the ZMP is located outside the immovable region 91 (step S304: Yes), the carriage control unit 23 calculates the rotation amount θ of the carriage 11 based on the input ZMP position (step S305).

  The calculation of the rotation amount θ of the carriage 11 will be specifically described with reference to FIG. FIG. 16A is a diagram showing the support polygon 90 and the non-moving region 91 before rotation, and FIG. 16B is a diagram showing the support polygon 90 and the non-moving region 91 after rotation. 16, it is assumed that the movement of the ZMP 200 is linear (for example, when the robot 3 enters a slope, when the robot 3 is pushed from a certain direction, when the arm 12 is extended in a certain direction, etc.) ( Dashed arrow direction).

  In FIG. 16, a straight line 105 is a straight line connecting the rotary shaft 100 and the ZMP 200. The straight line 106 is a straight line connecting the point D where the distance from the rotation axis 100 is maximum and the rotation axis 100. At this time, an angle formed by the reference line 104 (in the case of FIG. 16A, the shorter diagonal line of the support polygon 90) and the straight line 105 passing through the rotation axis 100 of the carriage 11 in plan view is defined as θzmp. An angle formed by the reference line 104 and the straight line 106 is θd. When the target rotation amount of the carriage 11 is θ, the carriage control unit 23 calculates the rotation amount θ by the following equation (1).

  Then, the carriage control unit 23 rotates the carriage 11 by the rotation amount θ (step S306). Specifically, the cart control unit 23 rotates the cart 11 clockwise when the value of θ is positive, and rotates the cart 11 counterclockwise when the value of θ is negative.

  In the example shown in FIG. 16A, since θzmp−θd <0, the carriage control unit 23 rotates the carriage 11 counterclockwise by the rotation amount θ. In other words, the cart control unit 23 rotates the cart 11 so that the straight line 106 approaches the ZMP 200 in plan view. The straight line 106 is a straight line passing through the point D that is farthest from the rotation axis 100. Therefore, when the position of the ZMP 200 varies linearly, the ZMP 200 is farthest from the side of the support polygon 90 by positioning the ZMP 200 on the straight line 106. By rotating in this way, the ZMP 200 can be prevented from coming out of the support polygon 90.

  The arm control unit 22 determines whether or not the operation of the arm 12 has been completed (step S307). When the operation of the arm 12 is finished (step S307: Yes), the robot 3 finishes the operation. On the other hand, when the operation of the arm 12 is not completed (step S307: No), the arm control unit 22 operates the arm 12 again (step S302).

  Here, the setting method of the immovable area in step S301 will be described in detail with reference to FIG. As described above, the angle between the reference line 104 and the straight line 106 is θd, and the rotation amount from the reference line 104 is θm. The distance from the rotation axis 100 to the side of the support polygon 90 in plan view is R (= f (θm)). Further, the distance from the rotation axis 100 to the side of the immobile area 91 is r. Note that the maximum rotation speed of the carriage 11 (approximately obtained when the acceleration is not limited) is ω, and α is an arbitrary coefficient (positive value). At this time, r of the immovable area setting unit 27 is calculated by the following formula (2) using polar coordinates.

  In FIG. 17, if a straight line rotated by θm from the reference line 104 is a straight line 107, the amount of rotation for bringing the straight line 107 closer to the straight line 106 increases as the difference between θd and θm increases. Therefore, in the above equation (2), r is set to be smaller as θm is separated from θd. That is, the ZMP is more likely to come out of the immovable region 91 as the distance from the straight line 106 increases. Therefore, the cart control unit 23 rotates the cart 11 earlier as the ZMP moves away from the straight line 106. As a result, the straight line 106 can be brought closer to the ZMP before the ZMP exits the support polygon 90.

  Further, the area of the immovable region 91 varies depending on the maximum rotational speed ω of the carriage 11. Specifically, as the maximum rotational speed ω is slower, r becomes smaller and the immovable region 91 becomes narrower. That is, if the maximum rotation speed ω is slow, the carriage 11 cannot be suddenly rotated, and thus the immobility region 91 is narrowed. As a result, when the position of the ZMP fluctuates, it is possible to respond by rotating the carriage 11 early, and the ZMP position can be prevented from coming out of the support polygon 90.

  As described above, according to the configuration of the robot 3 according to the present embodiment, the ZMP measurement unit 24 measures the position of the ZMP, and the cart control unit 23 rotates the cart 11 according to the position of the ZMP. Thereby, it is possible to deal with the movement of the ZMP in real time. Therefore, the ZMP can be accommodated inside the support polygon 90 even during operation.

  Furthermore, the non-moving area setting unit 27 provides a non-moving area 91 inside the support polygon 90, and the cart control unit 23 rotates the cart 11 only when the ZMP is located outside the non-moving area 91. That is, the trolley control unit 23 does not perform rotation control for minute fluctuations in the position of the ZMP. Thus, even if the ZMP position fluctuates finely due to a ZMP measurement error or the like, the carriage 11 does not rotate each time and remains stationary. Therefore, unnecessary rotating operation of the carriage 11 can be prevented. Of course, since the carriage 11 is rotated when the ZMP goes out of the immovable region 91, the stable state of the robot 3 can be maintained.

<Other examples>
In the first to third embodiments described above, the case where the support polygon 90 is a square has been described. However, the shape of the support polygon 90 is not limited to this. The shape of the support polygon 90 may be a triangle. Moreover, as shown in FIG. 18, a pentagon may be sufficient. Furthermore, the polygon may be larger than that.

  Further, when the carriage main body 111 has a columnar shape, the carriage control unit 23 may rotate the carriage 11 so that the central axis of the cylinder and the rotation axis of the carriage 11 coincide. Thereby, the trolley | bogie 11 can implement | achieve rotation operation | movement, without shifting | deviating at all to front and rear, right and left. As a result, the robot can be maintained in a stable state as long as there is a space of the width (cylinder diameter) of the carriage main body 111.

  In addition, the shape of the cart body 111 is not limited to the cylindrical shape. For example, as shown in FIG. The drive system of the carriage 11 may be various omnidirectional carriages.

  The present invention is not limited to the above-described embodiment, and can be appropriately changed and combined without departing from the spirit of the present invention. For example, the position and configuration of the arm 12 are not limited to those of the above-described embodiments. Further, the movable part is not limited to the arm but may be a head or the like.

1-3 Robot 11 Cart 12 Arm 21 CPU
22 Arm control unit 23 Carriage control unit 24 ZMP measurement unit 25 Arm trajectory planning unit 26 ZMP trajectory prediction unit 27 Non-moving region setting unit 28 Rotation determination unit 90 Support polygon 91 Non-moving region 111 Bogie main body 112 Driving wheel 113 Caster 121 Arm member 122 Joint 123 Hand part 200 ZMP
201 ZMP orbit

Claims (10)

A robot comprising a carriage in contact with the ground at a plurality of ground points,
A movable part supported by the carriage and connected to the carriage so as to be relatively displaceable;
A carriage control means for controlling the relative orientation of the polygon formed by connecting the plurality of grounding points with respect to the movable portion by rotating the carriage about a vertical axis;
The cart control means is a robot that rotates the cart so that the ZMP of the robot is positioned inside the polygon in plan view.   The robot according to claim 1, wherein the cart control means rotates the cart so that the ZMP of the robot and the polygonal side closest to the ZMP of the robot are separated in a plan view.   The cart control means rotates the cart so that the ZMP of the robot approaches a line connecting two ground points having the longest distance between the ground points in a plan view. The robot according to claim 1 or 2 to be caused. It further comprises a movable part control means for controlling the operation of the movable part,
The cart control means rotates the cart with respect to the ground, and the movable part control means has a rotational speed substantially the same as the rotational speed of the cart in a direction opposite to the rotation direction of the cart with respect to the ground. The robot according to claim 1, wherein the movable unit is rotated about the vertical axis relative to the carriage.   The robot according to claim 4, wherein the cart control unit rotates the cart so that a rotation axis of the movable portion around the vertical axis coincides with a rotation axis of the cart. A movable part trajectory calculating means for calculating a trajectory of the movable part;
A ZMP trajectory calculating unit that calculates a trajectory of the ZMP of the robot based on the trajectory of the movable part calculated by the movable part trajectory calculating means;
The robot according to any one of claims 1 to 5, wherein the carriage control unit rotates the carriage so that a ZMP trajectory of the robot exists inside the polygon.   The robot according to claim 6, wherein the carriage control unit rotates the carriage in advance before the movable portion operates so that the trajectory of the ZMP exists inside the polygon.   The robot according to any one of claims 5 to 7, wherein the carriage control means keeps the carriage stationary when the ZMP is present in an immovable region provided inside the polygon. The polygon is a rectangle,
The cart control means rotates the cart so that an intersection of two diagonal lines of the quadrangle and a rotation axis of the cart coincide with each other,
The robot according to claim 8, wherein when the ZMP exists outside the immovable region, the cart is rotated so that the ZMP and a longer diagonal of the two diagonals are brought closer to each other.   The robot according to claim 8 or 9, wherein an area of the immovable region is set in proportion to a maximum rotation speed of the carriage.

Images