JP2013116530A - Gait generating device for legged mobile robot and operational target generating device for robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a device capable of sequentially generating the desired motion of a robot while sequentially determining an appropriate Jacobian matrix which can compensate an influence of the non-linearity of relation between quantities of states associated with each other by the Jacobian matrix.SOLUTION: A gait generating device 32 for generating a target gait (desired motion) of the robot 1 sequentially determines a desired value vector Vb_cmd2 of a body motion velocity vector by multiplying a desired value vector S_cmd of a vector S to be controlled having a desired value vector P_cmd2 or the like of a translational momentum vector of the whole gravity center of the robot 1 as a component by a pseudo inverse matrix of the Jacobian matrix, and integrates the vector Vb_cmd2 to determine a desired position and a desired posture of the body 2. In determining the Jacobian matrix, a future gait is estimated, and the Jacobian matrix is determined using the operational state of the robot 1 specified by the estimated gait as origination.

Description

  The present invention relates to a gait generator for a legged mobile robot and an operation target generator for a robot.

  When generating various types of robot motion targets such as a target gait of a legged mobile robot, a Jacobian matrix may be used to generate the motion targets. For example, Patent Document 1 discloses a linear mapping from a change amount (first-order differential value) per unit time of a displacement amount of each joint of a legged mobile robot to a translational momentum of the entire center of gravity of the robot and an angular momentum around the entire center of gravity. Describes a technique for determining a target value of a displacement amount of each joint of a legged mobile robot corresponding to a target value of a translational momentum of the robot's overall center of gravity and an angular momentum around the overall center of gravity using a Jacobian matrix representing Yes.

JP 2006-315095 A

  If the above-mentioned Jacobian matrix is generalized, a first state constituted by a change amount (time change rate) per unit time of a predetermined kind of state amount of the robot, such as a displacement amount of the joint of the robot, as a component It is a matrix showing the said linear mapping regarding a quantity vector and the 2nd state quantity vector represented as a vector formed by linearly mapping this 1st state quantity vector. In other words, the Jacobian matrix is a matrix that represents the sensitivity of the change in the integrated value of the second state quantity vector to the minute change of the integration of the first state quantity vector (the vector having the predetermined type of state quantity as a component). It is.

  Such a Jacobian matrix generally changes in accordance with the overall operation state of the robot defined by a set of displacement amounts of each joint of the robot. For this reason, when a Jacobian matrix is used in the process of determining the instantaneous instantaneous motion target of the robot, the Jacobian matrix usually has a change in state quantity as an integral value of each component of the first state quantity vector. It is calculated as a starting point of the motion state defined by the latest value of the determined robot motion targets.

  When determining the robot motion target using the Jacobian matrix calculated in this way, for example, the other target value corresponding to one of the first state quantity vector and the second state quantity vector is The other target value calculated using the Jacobian matrix prescribes the amount of change in the integral value of the first state quantity vector or the second state quantity vector for each generation processing cycle of the robot motion target. Then, the operation target of the robot is determined.

  On the other hand, the relationship between the integration of the first state quantity vector (the vector having the predetermined type of state quantity as a component) and the integration value of the second state quantity vector may be a non-linear relationship. Many. For this reason, the Jacobian matrix is calculated starting from the motion state defined by the latest value of the robot motion targets that have already been determined as described above, and the robot motion target is set to a predetermined value using the calculated Jacobian matrix. When it is determined sequentially in the calculation processing cycle, the relationship between the change in the integration value of the first state quantity vector and the change in the integration value of the second state quantity vector for each calculation processing cycle is actually Deviation tends to occur with respect to the relationship.

  As a result, the operation target of the robot that is sequentially generated becomes inconsistent with the target value of one of the first state quantity vector and the second state quantity vector. There is an inconvenience that a shifted operation target is generated.

  For example, in the motion control of a legged mobile robot as shown in Patent Document 1, the translational momentum of the robot's overall center of gravity and the angular momentum around the overall center of gravity in the target gait generated by the robot are respectively Deviation from the target value is likely to occur.

  The present invention has been made in view of such a background, and in a legged mobile robot as an example of a robot, an appropriate Jacobian matrix capable of compensating for the influence of nonlinearity of the relationship between state quantities related by the Jacobian matrix. It is an object of the present invention to provide a gait generator capable of sequentially generating a target gait of the mobile robot while sequentially determining.

  Further, the present invention, as a more general aspect, sequentially determines an appropriate Jacobian matrix that can compensate for the influence of nonlinearity of the relationship between the state quantities related by the Jacobian matrix in various different robots. An object of the present invention is to provide an operation target generation device capable of sequentially generating operation targets of a robot.

In order to achieve the above object, a gait generator for a legged mobile robot according to the present invention is a legged movement that moves by the movement of a plurality of leg links that extend from a base and each have a plurality of joints. A gait generator that sequentially generates a target gait that defines a displacement amount of each joint of a robot,
Target leg position / posture determination means for sequentially determining a target leg position / posture which is a target value of the position and posture of the tip of each leg link of the mobile robot as a component of the target gait;
A target control target vector, which is a target value of a control target vector expressed as a vector obtained by linearly mapping a base body motion speed vector composed of the amount of change per unit time in the position and orientation of the base body, is sequentially determined. A target control target vector determining means;
Substrate motion speed target value determination that sequentially determines a target value of the base body motion speed vector by multiplying the target control target vector by a pseudo inverse matrix of the Jacobian matrix while sequentially determining a Jacobian matrix representing the linear mapping Means,
A target substrate position / posture that sequentially determines a target substrate position / posture that is a target value of the position / posture of the base as a component of the target gait by sequentially integrating the determined target values of the base body velocity vector. A determination means,
The base body motion speed target value determining means determines the Jacobian matrix used for determining a new target value of the base body motion speed vector, and is the latest value of the already determined target gaits of the mobile robot. The mobile robot within a period between a time corresponding to a target gait of the robot and a time corresponding to a new target gait in which a target base body position / posture is defined according to a new target value of the base body motion speed vector Is estimated based on the latest target gait and the previous one or more target gaits of the past value, and the estimated future gait instantaneous value is: The Jacobian matrix corresponding to the operating state of the starting point of the mobile robot, which is used as the operating state of the mobile robot that becomes the starting point of the change in the position and orientation of the base body by the new target value of the base body motion velocity vector Determination characterized in that it (the first invention).

  According to the first aspect of the invention, the target leg position / posture of each leg link determined by the target leg position / posture determination means (target value of the position and posture of the tip of each leg link) and the base body motion speed vector target By successively integrating the target value of the base body motion speed vector (a vector constituted by using the amount of change per unit time of the base body position and orientation as a component) sequentially determined by the value determining means by the target base body position and orientation determining means. The determined target base body position and orientation (target values of the base body position and orientation) are sequentially determined as components of the target gait.

  In this case, the base body motion speed target value determining means for determining the target value of the base body motion speed vector sequentially determines a Jacobian matrix representing a linear mapping from the base body motion speed vector to the control target vector, and the Jacobian matrix The target value of the base body motion speed vector is determined by multiplying the basic target value of the control target vector by the pseudo inverse matrix.

  For this reason, the target value of the base body motion speed vector, and hence the target gait having the target base body position / posture as components, are sequentially determined with the values of the respective components of the target control target vector as targets.

  Here, in the first invention, the base body motion speed target value determining means determines the Jacobian matrix used for determining a new target value of the base body motion speed vector when the mobile robot already determined is determined. Between the time corresponding to the latest target gait of the target gait and the time corresponding to the new target gait in which the target base body position / posture is defined according to the new target value of the base body motion speed vector The instantaneous value of the future gait of the mobile robot within the period of is estimated based on the target gait of the latest value and the target gait of one or more past values immediately before it, and the estimated future Using the instantaneous value of the gait of the mobile robot as the operating state of the mobile robot that is the starting point of the change in the position and posture of the base body based on the base body speed vector, the Jacobian corresponding to the operating state of the mobile robot at the starting point. To determine the emissions matrix.

  That is, an operation state shifted toward a predicted operation state in the future from the operation state of the mobile robot defined by the latest target gait is changed to the position and posture of the substrate by the substrate movement speed vector. The Jacobian matrix is determined as the starting point of the change (in other words, the change in the integral value of the base body motion velocity vector).

  In this way, even if the relationship between the position and orientation of the base body as the integral value of the base body motion speed vector and the state quantity corresponding to the integral value of the control target vector is a non-linear relationship, It is possible to compensate for the influence of the non-linearity and determine the target value of the base body motion speed vector having a high degree of coincidence with the optimal base body motion speed vector value for satisfying the target control target vector. As a result, the target gait that can satisfy the target control target vector can be generated with high reliability.

  Therefore, according to the first invention, the target of the mobile robot that can satisfy the target control target vector while sequentially determining an appropriate Jacobian matrix that can compensate for the influence of the nonlinearity of the relationship between the state quantities related by the Jacobian matrix Gait can be generated sequentially.

  In the first invention, the following configuration can be adopted as a more specific form. In other words, the base body speed target value determination means determines a target base body position / posture in the target gait having the latest value and a target base body position / posture in the target gait having a previous value immediately before the target gait having the latest value. By adding a correction amount obtained by multiplying the difference by a predetermined coefficient K within a range between “0” and “1” to the target base body position / posture in the latest target gait, the target base body position / posture And a target leg position / posture in the target gait of the latest value and a target leg position / posture in the target gait of the previous previous value of the latest value for each leg link. Means for estimating a future instantaneous value of the target leg position / posture of each leg link by adding a correction amount obtained by multiplying the difference by the coefficient K to the target leg position / posture of the latest target gait; Of the estimated target substrate position and posture The gait to future instantaneous value and the components of the target leg position and posture of the instantaneous value and each leg link of years, is used as the operating state of the starting point of the mobile robot (second invention).

  According to the second aspect of the invention, by using the coefficient K, the latest gait of the latest gait among the already determined target gaits and the previous target gait of the previous value are updated. The instantaneous value of the future gait of the mobile robot within a period between the time corresponding to the target gait of the value and the time corresponding to the new target gait is appropriately estimated by a simple method. Can do.

  In the first invention or the second invention, various kinds of control target vectors can be adopted. As an example, the control target vector may be, for example, a vector including at least each component of the translational momentum vector of the overall center of gravity of the mobile robot and each component of the angular momentum vector around the overall center of gravity of the mobile robot ( Third invention).

  Here, the temporal change rate of the translational momentum vector around the entire center of gravity of the mobile robot and the temporal change rate of the angular momentum vector around the overall center of gravity define the external force acting on the mobile robot. Therefore, by including each component of the translational momentum vector of the overall center of gravity of the mobile robot and each component of the angular momentum vector around the overall center of gravity in the component of the control target vector, it acts on the mobile robot as a result. The target gait of the mobile robot can be generated so that the external force becomes the required external force.

  The component of the control target vector is not limited to that of the third aspect of the present invention, and any kind of state quantity that can be controlled according to the motion of the substrate can be employed.

  In the first to third aspects of the invention, the robot is a legged mobile robot. However, the technology for determining the Jacobian matrix as in the first aspect of the invention is not limited to the legged mobile robot. The present invention can be applied to various types of robots such as work robots.

Therefore, in the present invention, a robot motion target generation apparatus of the present invention is constructed as an invention that is a more generalized version of the first invention. The motion target generation device includes a first state quantity vector configured with a change amount per unit time of a predetermined kind of state quantity of a robot configured by connecting a plurality of links by a plurality of joints, Displacement of each joint of the robot using the Jacobian matrix while sequentially determining a Jacobian matrix representing the linear mapping between the second state quantity vector expressed as a vector obtained by linearly mapping one state quantity vector In an apparatus for sequentially determining an operation target of the robot that defines an amount,
When determining a new motion target of the robot, a time period between a time corresponding to the latest motion target of the robot motion targets already determined and a time corresponding to the new motion target is determined. An instantaneous value of the future motion target of the robot is estimated based on the latest motion target and one or more previous motion targets immediately before, and the estimated future motion target instantaneous value is Jacobian matrix determining means for determining the Jacobian matrix corresponding to the operation state of the starting point of the robot, using the operation state of the robot as a starting point of the change of the predetermined type of state amount by the first state amount vector. It is provided (4th invention).

  According to the fourth invention, when the Jacobian matrix determining means determines a new motion target of the robot, it determines the Jacobian matrix used for determining the motion target in the same manner as in the first invention. . That is, the Jacobian matrix determining means is configured to determine whether or not the robot within the period between the time corresponding to the latest motion target among the motion targets already determined and the time corresponding to the new motion target. An instantaneous value of a future motion target is estimated based on the latest motion target and one or more previous motion targets immediately before, and the estimated future motion target instantaneous value is the first value. The Jacobian matrix corresponding to the operation state of the starting point of the robot is determined using the operation state of the robot as the starting point of the change of the predetermined type of state amount by the state amount vector.

  Accordingly, a change in the state quantity of the predetermined type by the first state quantity vector is performed by shifting an operation state that is shifted closer to a predicted operation state in the future than the operation state of the robot specified by the latest operation target. The Jacobian matrix is determined as a starting point (in other words, a change in the integral value of the first state quantity vector).

  By doing so, the relationship between the predetermined type of state quantity as the integral value of the first state quantity vector and the state quantity corresponding to the integral value of the second state quantity vector is a non-linear relationship. However, the influence of the nonlinearity is compensated, and the other target value can be determined with high reliability from one target value of the first state quantity vector and the second state quantity vector. As a result, a target gait that can satisfy one target value of the first state quantity vector and the second state quantity vector can be generated with high reliability.

  In the fourth invention, when the base motion velocity vector and the control target vector in the first invention are adopted as the first state quantity vector and the second state quantity vector, respectively, the Jacobian matrix in the fourth invention is The determination process is equivalent to the determination process of the Jacobian matrix in the first invention.

The figure which shows schematic structure of the leg type mobile robot in one Embodiment of this invention. The figure which shows the structure regarding control of the leg type mobile robot shown in FIG. The block diagram which shows the function of the gait generator shown in FIG. The figure which shows the example of the track | orbit of the target leg position and orientation determined by the target leg position and orientation determination unit shown in FIG. The figure which shows typically the dynamic model used by the process of the basic operation target value determination part shown in FIG. The graph for demonstrating the process of the expected exercise speed target value determination part shown in FIG. FIGS. 7A and 7B are graphs showing examples of changes over time in the posture (tilt angle) of the base of the robot in the examples and comparative examples, respectively. FIGS. 8A and 8B are graphs showing examples of changes over time in the translational momentum error of the entire center of gravity of the robot in the example and the comparative example, respectively. FIGS. 9A and 9B are graphs showing examples of changes over time in errors of angular momentum around the entire center of gravity of the robot in the example and the comparative example, respectively.

  Hereinafter, an embodiment of the present invention will be described with reference to FIGS.

  FIG. 1 schematically shows a schematic configuration of a legged mobile robot exemplified in this embodiment. The legged mobile robot 1 (hereinafter simply referred to as the robot 1) is a biped mobile robot having a base body 2 and a pair of left and right (two) leg links 3R and 3L extending from the base body 2.

  In this specification, a symbol “R” is added to the variable indicating the right member or the amount related to the member toward the front of the robot 1, and the left member or the member toward the front of the robot 1 is added. The code “L” is added to the variable indicating the related quantity. However, when there is no need to distinguish between the right side and the left side, the symbols “R” and “L” may be omitted.

  The base 2 is a link portion corresponding to the upper body of the robot 1. In this embodiment, the base body 2 is composed of two element links of a lower base body 10 and an upper base body 11 disposed above the lower base body 10. The lower base 10 is a part corresponding to the waist of the robot 1, and the upper base 11 is a part corresponding to the chest of the robot 1. The upper substrate 11 is connected to the lower substrate 10 via a substrate rotation joint 12 having a degree of freedom of rotation about the yaw axis (around the Z axis).

  The pair of left and right leg links 3R, 3L have the same structure. Specifically, each leg link 3 includes a thigh 14 connected to the lower base 10 via a hip joint 13, and a crus connected to the thigh 14 via a knee joint 15. 16 and a foot 18 connected to the crus 16 via an ankle joint 17 is provided as a plurality of element links constituting the leg link 3. In this case, the distal end portion of each leg link 3 is constituted by a foot portion 18.

  The hip joint portion 13 of each leg link 3 is composed of three joints 19, 20, and 21 having degrees of freedom of rotation around the yaw axis, the pitch axis (around the Y axis), and the roll axis (around the X axis), respectively. Has been. The knee joint portion 15 is constituted by a joint 22 having a degree of freedom of rotation around the pitch axis. The ankle joint portion 17 includes two joints 23 and 24 each having a degree of freedom of rotation about the pitch axis and the roll axis.

  Therefore, the foot 18 which is the tip of each leg link 3 has six degrees of freedom of movement with respect to the lower base body 10 in this embodiment. Note that the rotation axes (roll axis, pitch axis, yaw axis) of the joints 19 to 24 of each leg link 3 in the above description indicate the rotation axes in a state where the leg link 3 extends in the vertical direction. .

  The above is the basic structure of the robot 1 of the present embodiment. In the robot 1 having such a configuration, by driving the six joints 19 to 24 of each leg link 3, spatial movement of each leg link 3 is performed, and it is possible to move on the floor by this movement. ing. For example, it is possible to perform the walking motion or the traveling motion of the robot 1 by moving the leg links 3R, 3L in the same form (gait) as the human walking motion or the traveling motion.

  In the present embodiment, the lower base body 10 and the upper base body 11 constituting the base body 2 are connected via the joint 12, so that the base body 2 is moved to its trunk axis by driving the joint 12. It is possible to twist around the rotation axis of the joint 12 corresponding to.

  Supplementally, the robot 1 according to the present embodiment is mounted not only on the base 2 and the leg links 3R and 3L but also on arm links extending from both sides of the upper base 11 and the upper end of the upper base 11, for example. The head may be provided. Further, the joint 12 of the base 2 may be omitted, and the lower base 10 and the upper base 11 may be configured integrally.

  Although not shown in FIG. 1, the robot 1 is equipped with a joint actuator 30 that rotationally drives each of the joints and a control processing unit 31 that controls the operation of the robot 1, as shown in FIG. 2. Has been.

  The joint actuator 30 is constituted by, for example, an electric motor or a hydraulic actuator, and is provided for each joint. In this case, the drive mechanism of each joint by each joint actuator 30 may have a known structure. The joint actuator 30 is not limited to a rotary actuator, and may be a direct acting actuator.

  The control processing unit 31 is an electronic circuit unit including a CPU, a RAM, a ROM, an interface circuit, and the like. The control processing unit 31 includes a gait generator 32 that sequentially generates a target gait of the robot 1 and main joint functions of the robot 1 according to the target gait. And a joint actuator control unit 33 that controls the amount of displacement (rotation angle) through the joint actuator 30.

  The gait generator 32 provides the control processing unit 31 with the function of the apparatus of the present invention. This gait generator 32 is a target leg position / posture which is a target value of the spatial position and posture of the foot 18 constituting the tip of each leg link 3 as a component of the target gait of the robot 1. Then, the process of sequentially determining the target substrate position and orientation, which is the target value of the spatial position and orientation of the substrate 2, is executed as described later.

  In this case, in this embodiment, since the base body 2 includes the joint 12, the upper base body 11 can be twisted around the rotation axis of the joint 12 with respect to the lower base body 10. Therefore, more specifically, the target base body position / posture in the present embodiment is the target value of the spatial position and posture of the lower base 10 and the torsion angle (the joint 12 of the joint 12) with respect to the lower base 10 of the upper base 11. Displacement) and a target value.

  More specifically, the position of the foot 18 of each leg link 3 is the position of a predetermined representative point of the foot 18, and the posture of the foot 18 is the position of the foot 18. Means spatial orientation. The same applies to the position and posture of the lower base 10.

  These positions and postures are described as positions and postures viewed in a global coordinate system, which is an inertial coordinate system fixed with respect to the floor surface of the operating environment of the robot 1. This global coordinate system may be arbitrarily determined in design. As an example, for example, the supporting leg of the leg links 3R, 3L of the robot 1 (the other leg that supports the weight of the robot 1 when the foot 18 of the leg link 3 as a free leg is moved in the air). The leg link 3) has an origin on the floor surface directly below (or in the vicinity of) the foot 18 when the foot 18 is in contact with the ground, and the foot 18 has a horizontal axis in the front-rear direction and left and right A three-axis orthogonal coordinate system (so-called support leg coordinate system) in which the horizontal axis in each direction is the X axis and the Y axis, and the vertical axis is the Z axis can be used as the global coordinate system.

  In the present embodiment, for the sake of convenience, the support leg coordinate system is used as the global coordinate system. Unless otherwise specified, the X-axis, Y-axis, and Z-axis mean coordinate axes in the support leg coordinate system.

  Supplementally, if the spatial position and posture of one base 10 or 11 of the lower base 10 and the upper base 11 constituting the base 2 and the displacement amount of the joint 12 are determined, the space of the other base 11 or 10 is determined. The specific position and posture are also uniquely determined. Therefore, the target base body position / posture may be configured by a set of a target value of the spatial position and posture of the upper base body 11 and a target value of the displacement amount of the joint 12.

  The joint actuator control unit 33 makes the actual displacement amount of each joint follow the target value of the displacement amount of each joint of the robot 1 defined by the target gait determined by the gait generation unit 32. The joint actuator 30 corresponding to the joint is controlled.

  In this case, the actual displacement amount (rotation angle) of each joint is detected by a displacement amount sensor such as a rotary encoder (not shown). The joint actuator controller 33 controls the driving force of the joint actuator 30 by feedback control according to the deviation between the detected value of the actual displacement amount of each joint and the target value.

  Hereinafter, the process of the gait generator 32 of the control processing unit 31 will be described in detail.

  As shown in FIG. 3, the gait generator 32 is a main processing unit for generating a target gait, a target leg position / posture determination unit 41 that sequentially determines a target leg position / posture, and each leg link 3. A basic motion target value determining unit that sequentially determines a basic motion target related to a predetermined type of state quantity of the robot 1 other than the position and posture of the foot 18.

  In this case, the motion target determined by the basic motion target value determination unit 42 is the basic target center-of-gravity position ↑ Xg_cmd1, which is the basic target value of the position of the overall center of gravity of the robot 1, and the overall translational momentum vector (= total of the robot 1). Basic target translational momentum vector ↑ P_cmd1, which is a basic target value of the translational momentum vector of the center of gravity), and basic target angular momentum vector ↑ L_cmd1, which is a basic target value of the overall angular momentum vector around the entire center of gravity of the robot 1, It consists of a basic target base body position / posture which is a basic target value of the position and posture of the base body 2.

  Note that the basic target center-of-gravity position ↑ Xg_cmd1, the basic target translational momentum vector ↑ P_cmd1, and the basic target angular momentum vector ↑ L_cmd1 are each a three-component vector (vertical vector) having each coordinate component of the global coordinate system as a component. . In the description of this embodiment, “↑” is used as a symbol representing a vector (vertical vector).

  Further, the gait generator 32 sequentially determines a corrected basic target translational momentum vector ↑ P_cmd2 obtained by correcting the basic target translational momentum vector ↑ P_cmd1 among the basic motion targets determined by the basic motion target value determination unit 42. Basic target translational momentum correcting unit 43, and a basic target basic body that sequentially determines a basic target basic body motion speed vector ↑ Vb_cmd1 that is a basic target value of a change amount (ie, a change speed) of the position and orientation of the base body 2 per unit time The amount of change per unit time of the position and posture of the base body 2 with the movement speed determination unit 44, the corrected basic target translational momentum vector ↑ P_cmd2, the basic target angular momentum vector ↑ L_cmd1, and the basic target body movement speed vector ↑ Vb_cmd1 And a base body motion speed target value determining unit 45 for sequentially determining a target base body motion speed vector ↑ Vb_cmd2 that is a target value of.

  The basic target substrate motion velocity vector ↑ Vb_cmd1 and the target substrate motion velocity vector ↑ Vb_cmd2 are respectively the translational movement speeds of the representative points of the substrate 2 (representative points of the lower substrate 10 in this embodiment) viewed in the global coordinate system. Each coordinate axis component, an angular velocity component around each coordinate axis of the posture of the base body 2 (in this embodiment, the posture of the lower base body 10) viewed in the global coordinate system, and a temporal change rate of the displacement amount of the joint 12 of the base body 2 ( 7-component vector (vertical vector) arranged with (angular velocity).

  In other words, the target base body motion speed vector ↑ Vb_cmd2 is a temporal change rate of the target base body position / posture in the target gait sequentially determined by the gait generator 32.

  Further, the gait generator 32 sequentially determines the target base body position / posture by integrating the target base body speed vector ↑ Vb_cmd2 sequentially determined by the base body speed target value determining section 45 (integrating each component of ↑ Vb_cmd2). On the target gait, which is the position of the overall center of gravity of the robot 1 defined by the target gait having the target base body position / posture determination unit 46 and the target base body position / posture and the target leg position / posture of the both leg links 3R, 3L as constituent elements. And a gravity center position calculation unit 47 on the target gait that sequentially calculates the gravity center position ↑ Xg_cmd2.

  The center centroid position ↑ Xg_cmd2 in the target gait is a three-component vector (vertical vector) having each coordinate component in the global coordinate system as a constituent element, like the basic target centroid position ↑ Xg_cmd1.

  In the present embodiment, the gait generator 32 further uses the target base body position / posture constituting the target gait and the target leg position / posture of the both leg links 3R, 3L as a target value of the displacement amount of each joint of the robot 1. A target joint displacement amount determination unit 48 that sequentially determines a certain target joint displacement amount is provided. However, the target joint displacement amount determination unit 48 may be provided in the joint actuator control unit 33.

  The gait generator 32 sequentially executes the processing of each of the above-described processing units 41 to 48 at a predetermined calculation processing cycle, so that the target base body position / posture, which is a component of the target gait, and the both leg links 3R, 3L. The target leg position / posture are sequentially determined, and the target joint displacement amount of each joint corresponding to the target gait is sequentially determined.

  Below, the whole process of the gait generator 32 will be described, including details of the processes of the processing units 41 to 48 of the gait generator 32. In the present embodiment, the process of the gait generator 32 will be mainly described by taking as an example the case of generating a target gait for performing movement by the walking motion or running motion of the robot 1.

  The gait generator 32 first performs the processing of the target leg position / posture determination unit 41 and the basic motion target value determination unit 42 in each calculation processing cycle.

  In the present embodiment, the target leg position / posture determination unit 41 and the basic motion target value determination unit 42 include a plurality of steps including the next free leg each time the free leg of the leg links 3R and 3L is landed. A target landing position and posture that is a target landing position and landing posture of the foot 18 of the free leg and a target landing time that is a target landing time are given. Instead of the target landing time, a target time for a period of one step (gait cycle) of the robot 1 may be used.

  The target landing position / posture and the target landing time are determined based on a movement plan of the robot 1 and a movement command from the control device. The determination process may be performed by the control processing unit 31, but may be performed by a server outside the robot 1.

  Then, the target leg position / posture determination unit 41 determines the next target landing position / posture from the position and posture of the foot 18 of each leg link 3 in the ground contact state immediately before leaving the floor according to the given target landing position / posture and the target landing time. The target trajectory of the position and posture to reach the landing position / posture (time series pattern of the target value of the position and posture of the foot 18) is determined, and the target leg position / posture of each leg link 3 is sequentially determined as the instantaneous value of the target trajectory. decide.

  In this case, the target trajectory (the trajectory of the target leg position / posture) of the position and posture of the foot 18 of each leg link 3 is a period in which the leg link 3 is a free leg, for example, a pattern as illustrated in FIG. Thus, it is determined that the foot 18 moves in the air while the target position and the target posture of the foot 18 change continuously. The position and posture of the foot 18 at the start of the target track correspond to the target landing position and posture at the time of landing before leaving the floor, and the position and posture of the foot 18 at the end of the target track are the next target. This corresponds to the landing position / posture.

  During the ground contact of the foot portion 18 of each leg link 3, the target trajectory of the position and posture of the leg link 3 keeps the position and posture of the foot portion 18 of the leg link 3 constant or substantially constant. Of course, it may be determined in such a way that the ground contact portion of the foot portion 18 may be determined so as to shift from the portion closer to the heel of the foot portion 18 to the portion closer to the toe. Good.

  Further, the trajectory of the foot portion 18 of each leg link 3 in the air may be determined so that the posture of the foot portion 18 is kept constant or substantially constant.

  The basic motion target value determination unit 42 according to the given target landing position / posture and target landing time (or according to the trajectory of the target leg position / posture during contact of the foot 18 of each leg link 3). The trajectory of the target ZMP which is the target position of ZMP (Zero Moment Point) is determined.

  In this case, the trajectory of the target ZMP is located in a region where the target ZMP is not so close to the boundary in the so-called support polygon (ZMP existence allowable region), and no discontinuous change occurs. Is set as follows. As a method for setting the trajectory of the target ZMP, a known method such as the method described by the present applicant in Japanese Patent No. 3675789 can be used.

  Then, the basic motion target value determination unit 42 determines the time series of the target leg position / posture determined by the target leg position / posture determination unit 41 and the dynamics of the robot 1 (external force acting on the robot 1 (here, floor reaction force)). The position and posture of the base body 2 that can satisfy the target ZMP on the dynamic model using the predetermined dynamic model representing the relationship between the robot and the motion of the robot 1). Are sequentially determined.

  Here, the position and posture of the base body 2 that can satisfy the target ZMP are the time series of the position and posture of the base body 2 and the time series of the target leg position and posture of the both leg links 3R and 3L in the dynamic model. The component around the horizontal axis (the component around the X axis and the component around the Y axis) of the moment generated around the target ZMP by the resultant force of the inertial force generated by the entire motion of the robot 1 defined by the above and the gravity acting on the robot 1 It means the position and posture of the substrate 2 such that the component) becomes zero.

  As the dynamic model, a highly linear dynamic model is used so that the position and posture of the substrate 2 that can satisfy the target ZMP can be calculated relatively easily. For example, the dynamic model (the dynamic model shown in FIG. 10 of Japanese Patent No. 3675789) described by the applicant of the present application in Japanese Patent No. 3675789 is adopted as the dynamic model in the processing of the basic motion target value determining unit. Can do.

  As shown in FIG. 5, the dynamic model includes three mass points 3mR, 3mL, 2m corresponding to the leg links 3R, 3L and the base body 2 (the upper body of the robot 1), and the posture of the base body 2 (in detail). Is a dynamic model including a flywheel FH (flywheel having inertia without a mass) that generates an inertial force moment accompanying changes in the lower base body 10 around the X axis and the Y axis. . In this case, the basic target base body position / posture can be determined by a method similar to the target body position / posture determination method described by the applicant of the present application in Japanese Patent No. 3675789.

  By doing so, on the dynamic model, the ZMP falls within the support polygon, and the frictional force component of the floor reaction force acting on the robot 1 falls within an appropriate allowable range. Therefore, the basic target base body position / posture can be determined so as to satisfy the required dynamic constraints.

  In the present embodiment, of the basic target base body position and orientation, the target value of the displacement amount of the joint 12 of the base body 2 is generated around the target ZMP as the upper base body 11 rotates around the rotation axis of the joint 12, for example. The moment around the vertical axis is determined so as to cancel out the moment around the vertical axis generated around the target ZMP by the translational motion of each of the mass points 3mR, 3mL, and 2m.

  However, the target value of the displacement amount of the joint 12 may be a fixed value, for example, or is set to change periodically (with a period synchronized with the movement of the leg links 3R, 3L) in a fixed pattern. It may be.

  In the present embodiment, the basic motion target value determination unit 42 further includes a time series of the basic target base body position / posture determined as described above, and a time series of the target leg position / posture determined by the target leg position / posture determination unit 41. From the dynamic model, the position of the total center of gravity of the robot 1 on the dynamic model, the translational momentum vector of the total center of gravity, and the angular momentum vector around the total center of gravity, The target center of gravity position ↑ Xg_cmd1, the basic target translational momentum vector ↑ P_cmd1, and the basic target angular momentum vector ↑ L_cmd1 are calculated.

  In this case, the basic target center-of-gravity position ↑ Xg_cmd1 is the total center of gravity of the mass points 3mR, 3mL, 2m defined by the target leg position / posture of the leg links 3R, 3L and the basic target base body position / posture in each calculation processing cycle. Calculated as a position (position vector). That is, the position vector of each mass point 3mR, 3mL, 2m defined by the target leg position / posture of each leg link 3R, 3L and the basic target base body position / posture is multiplied by the mass of each mass point 3mR, 3mL, 2m. A vector obtained by dividing the vector sum vector by the total mass of the robot 1 (= mass mass of the mass points 3 mR, 3 mL, 2 m) is calculated as the basic target center-of-gravity position ↑ Xg_cmd1.

  The basic target translational momentum vector ↑ P_cmd1 is the center of gravity of the mass points 3mR, 3mL, and 2m defined by the time series of the target leg position and posture of the leg links 3R and 3L and the time series of the basic target body position and posture. Is calculated by multiplying the movement speed vector (= ↑ Xg_cmd1 with time) by the total mass of the robot 1. That is, the translational momentum vector of the entire center of gravity of the mass points 3mR, 3mL, and 2m is calculated as the basic target translational momentum vector ↑ P_cmd1.

  The basic target angular momentum vector ↑ L_cmd1 is the translational momentum vector of the mass points 3mR and 3mL defined by the time series of the target leg position and posture of the leg links 3R and 3L, and the mass points 3mR, 3mL and 2m as a whole. Angular momentum vector generated around the center of gravity of the entire mass point 3mR, 3mL, 2m, the angular momentum vector generated around the center of gravity of the material and the translational momentum vector of the mass point 2m defined by the time series of the basic target body position and orientation And the angular momentum vector generated by the rotational motion of the flywheel FH defined by the time series of the posture of the base body 2 in the basic target base body position / posture, and the rotational movement of the upper base body 11 relative to the lower base body 10 (of the joint 12). It is calculated as a sum of angular momentum vectors with the angular momentum vector generated by the rotational motion around the rotational axis.

  Supplementally, when the robot 1 includes another link that is movable with respect to the base 2 such as an arm link, the basic motion target value determination unit 42 sets a target value that defines the displacement amount of each joint of the other link. (For example, the target position and target posture of the tip of the other link or the target value of the displacement amount of each joint of the tip of the other link) is also determined.

  In this case, in the target gait generation process for performing the walking motion or the traveling motion of the robot 1, the target value that defines the displacement amount of each joint of the other link (for example, the arm link) is, for example, that of the base 2 The target value may be such that the relative position and posture of each part of the other link with respect to the upper base 11 are constantly maintained constant. In this case, the mass of the mass point 2 m and the inertia (moment of inertia) of the flywheel FH in the dynamic model may be set including the influence of the other links.

  After executing the processing of the target leg position / posture determination unit 41 and the basic motion target value determination unit 42 as described above, the gait generation unit 32 then performs the processing of the center of gravity position calculation unit 47 on the target gait, The basic target translational momentum correcting unit 43 and the basic target base body motion speed determining unit 44 are executed.

  In the target gait center-of-gravity position calculation unit 47, the constituent elements of the target gait (target leg position / posture and target base body position / posture) finally determined by the gait generation unit 32 in the previous calculation processing cycle, in other words, The component of the latest (current) desired gait among the desired gaits already determined by the gait generator 32 is given.

  Then, the center-of-gravity position calculation unit 47 on the target gait calculates the position of the entire center of gravity of the robot 1 defined by the components of the given target gait (position vector viewed in the global coordinate system) as the geometry of the robot 1. It is calculated by geometric calculation based on the model (rigid link model), and this calculated position is determined as the center of gravity position ↑ Xg_cmd2 on the target gait.

  More specifically, the geometric model of the robot 1 used for the geometric calculation is a model in which a mass point (a mass point having the mass of the element link) is set at the center of gravity of each element link of the robot 1, for example.

  Then, the center of gravity position calculation unit 47 on the target gait first calculates the displacement amount of each joint of the robot 1 defined by the target gait by the process of inverse kinematics calculation from the latest target gait given. To do.

  Next, the center of gravity position calculation unit 47 in the target gait is the geometric model defined by the calculated displacement amount of each joint and the target leg position or target body position / posture of the latest target gait given. The position of each mass point is calculated.

  Then, the center of gravity position calculation unit 47 in the target gait calculates the entire robot 1 as the positions of the mass centers of all the mass points of the geometric model from the positions of the mass points calculated in this way and the masses of the mass points. Calculate the position of the center of gravity. The position of the overall center of gravity of the robot 1 calculated in this way is determined as the center of gravity position ↑ Xg_cmd2 in the target gait.

  It should be noted that the target gait center of gravity position ↑ Xg_cmd2 corresponding to the latest target gait finally determined in the previous calculation processing cycle of the gait generator 32 is obtained by the target gait center of gravity position calculation unit 47 as described above. The determination process may be performed in the previous calculation processing cycle.

  Supplementally, when the robot 1 is provided with another link that is movable with respect to the base 2 such as an arm link, the center of gravity position ↑ Xg_cmd2 in the target gait using a geometric model considering the mass of the other link. Can be determined.

  In this case, in addition to the target leg position and target base body position / posture, the target gait center-of-gravity position calculation unit 47 sets a target value (for example, the other link) that defines the displacement amount of each joint of the other link. The target position and target posture of the tip of the link, or the target value of the displacement amount of each joint at the tip of the other link) is also given.

  Then, the center of gravity position ↑ Xg_cmd2 in the target gait as the position of the entire center of gravity of the robot 1 may be calculated by a geometric calculation based on a geometric model including a mass point corresponding to the other link.

  However, when the mass of the other link is sufficiently smaller than the mass of the base body 2, the geometric model constructed by ignoring the mass of the other link, or the mass of the other link The center of gravity position ↑ Xg_cmd2 in the target gait may be determined using a geometric model configured by including the above in the mass of the base 2.

  Further, for example, when the arm link is extended from the upper base body 11 of the base body 2, the posture of the arm link with respect to the upper base body 11 in the walking operation or the traveling operation of the mobile robot 1 is kept constant or substantially constant. In some cases, the arm link is substantially an integral part of the upper base 11. Therefore, in this case, the center of gravity position ↑ Xg_cmd2 on the target gait may be determined using a geometric model configured by including the mass of the other link in the mass of the upper base 11.

  The basic target translational momentum correcting unit 43 is sequentially given the basic target center-of-gravity position ↑ Xg_cmd1 and the basic target translational momentum vector ↑ P_cmd1 from the basic motion target value determining unit 42 and from the target gait center-of-gravity position calculating unit 47 to the target The center of gravity position ↑ Xg_cmd2 in the gait is given sequentially.

  In the following description, regarding the values of the variables such as ↑ Xg_cmd1, ↑ P_cmd1, ↑ Xg_cmd2, etc. determined by the processing of the gait generator 32, the current (current) calculation processing cycle of the gait generator 32 is used. When it is necessary to distinguish between the determined value and the value determined in the previous calculation processing cycle, the former value may be referred to as the current value and the latter value may be referred to as the previous value.

  The basic target translational momentum correcting unit 43 first calculates a deviation (= ↑ Xg_cmd1- ↑ Xg_cmd2) between the given basic target center-of-gravity position ↑ Xg_cmd1 and the target gait center-of-gravity position ↑ Xg_cmd2 by the calculation unit 43a.

  In this case, the current value of the target gait corresponding to the current value of the basic target center-of-gravity position ↑ Xg_cmd1 has not yet been determined. In this embodiment, the current value of the center of gravity position ↑ Xg_cmd2 on the target gait is the position of the overall center of gravity of the robot 1 corresponding to the previous value of the target gait. Therefore, in the processing of the calculation unit 43a, the deviation (hereinafter referred to as the gravity center position deviation) is calculated using the previous value of the basic target gravity center position ↑ Xg_cmd1 and the current value of the gravity center position ↑ Xg_cmd2 on the target gait.

  Further, the basic target translational momentum correcting unit 43 executes a process of multiplying the gravity center position deviation deviation (= ↑ Xg_cmd1- ↑ Xg_cmd2) calculated by the calculating unit 43a by a predetermined gain Kg by the calculating unit 43b. Thereby, an operation amount (feedback operation amount) for correcting the basic target translational momentum vector ↑ P_cmd1 so that the deviation approaches zero is calculated.

  Next, the basic target translational momentum correcting unit 43 executes a process of correcting the current value of the basic target translational momentum vector ↑ P_cmd1 by the operation amount calculated by the calculation unit 43b in the calculation unit 43c. This correction process is performed by adding the operation amount calculated by the calculation unit 43b to the current value of ↑ P_cmd1. As a result, a corrected basic translational momentum vector ↑ P_cmd2 (current value) is calculated as an output of the calculation unit 43c.

  Accordingly, the corrected basic translational momentum vector ↑ P_cmd2 (current value) is calculated by correcting the current value of the basic target translational momentum vector ↑ P_cmd1 by the following equation (1) in each calculation processing cycle.

↑ P_cmd2 = ↑ P_cmd1 + Kg ・ (↑ Xg_cmd1− ↑ Xg_cmd2) …… (1)

That is, ↑ P_cmd2 is determined by correcting ↑ P_cmd1 by the feedback operation amount determined so that the center-of-gravity position deviation (↑ Xg_cmd1− ↑ Xg_cmd2) approaches zero by the proportional law as the feedback control law.

  The gain Kg may be a scalar or a diagonal matrix (gain matrix). When the gain Kg is a diagonal matrix, it is possible to set a feedback gain for each component (X-axis, Y-axis, and Z-axis coordinate axis components) of the gravity center position deviation.

  The basic target base body motion speed determination unit 44 is sequentially given the basic target base body position / posture from the basic motion target value determination unit 42 and the target base body position / posture from the target base body position / posture determination unit 46 (details will be described later). Given.

  The basic target base body motion speed determination unit 44 first calculates a deviation (= basic target base body position / posture−target base body position / posture) between the given basic target base body position / posture and the target base body position / posture by the arithmetic unit 44a. To do.

  In this case, the target base position / posture corresponding to the current value of the basic target base body position / posture has not been determined yet. For this reason, in the processing of the calculation unit 44a, the deviation (hereinafter referred to as the base body position / posture deviation) is calculated using the previous value of the basic target base body position / posture and the previous value of the target base body position / posture.

  Further, the basic target base body motion speed determination unit 44 executes a process of multiplying the base body position / posture deviation calculated by the arithmetic unit 44a by a predetermined gain Kb in the arithmetic unit 44b. Thereby, the unit time of the position and orientation of the substrate 2 is used as an operation amount (feedback operation amount) for making the substrate position and orientation deviation close to zero so that the target substrate position and orientation does not deviate from the basic target substrate position and orientation. A basic target base body motion speed vector ↑ Vb_cmd1 constituted by the target value of the amount of change per hit is calculated.

  Accordingly, in each arithmetic processing cycle, the basic target base body motion speed vector ↑ Vb_cmd1 is calculated by the following equation (2).

↑ Vb_cmd1 = Kb (Basic target base body position / posture-Target base body position / posture) (2)

That is, ↑ Vb_cmd1 is determined as a feedback operation amount that is determined so that the base body position / posture deviation (= basic target base body position / posture−target base body position / posture) approaches zero by a proportional law as a feedback control law.

  The gain Kb may be either a scalar or a diagonal matrix (gain matrix). When the gain Kb is a diagonal matrix, each component of the base body position / posture deviation (deviation of the position of the base body 2 in each coordinate axis direction, deviation of the posture of the base body 2 around each coordinate axis (angle deviation)), and It is possible to set a feedback gain for each displacement amount deviation (angle deviation) of the joint 12.

  As described above, after executing the processes of the center of gravity position calculating unit 47 on the target gait, the basic target translational momentum correcting unit 43, and the basic target base body motion speed determining unit 44, the gait generating unit 32 then selects the base body motion speed target. The process of the value determining unit 45 is executed.

  The base body motion speed target value determination unit 45 calculates a translational motion amount vector ↑ P of the entire center of gravity of the robot 1 from a base body motion speed vector ↑ Vb composed of a change amount per unit time of the position and orientation of the base body 2 as a component. This represents a linear mapping to a control target vector ↑ S, which is a vector composed of each component, each component of the angular momentum vector ↑ L around the entire center of gravity of the robot 1, and each component of the base body motion speed vector ↑ Vb. A target substrate motion speed vector ↑ Vb_cmd2 which is a target value of the substrate motion speed vector ↑ Vb is determined using the Jacobian matrix.

The control target vector ↑ S is a vertical vector in which the components ↑ P, ↑ L, and ↑ Vb are arranged. More specifically, ↑ P, ↑ L, and ↑ Vb are expressed using the respective components as ↑ P≡ [Px, Py, Pz] ^T , ↑ L≡ [Lx, Ly, Lz] ^T , ↑ Vb≡ [Vbx, Vby, Vbz, ωbx, ωby, ωbz, ωbt] ^T (superscript T represents transposition) ↑ S is a vertical vector expressed by the following equation (3), for example It is.

↑ S≡ [↑ P, ↑ L, ↑ Vb] ^T
≡ [Px, Py, Pz, Lx, Ly, Lz, Vbx, Vby, Vbz, ωbx, ωby, ωbz, ωbt] ^T
...... (3)

Px, Py, and Pz are the X-axis direction component, Y-axis direction component, and Z-axis direction component of ↑ P, respectively. Lx, Ly, and Lz are the X-axis component and the Y-axis component of ↑ L, respectively. , The component around the Z axis. Vbx, Vby, and Vbz are the X-axis direction component, the Y-axis direction component, the Z-axis direction component, and ωbx, ωby, and ωbz, respectively, of the moving speed of the representative point of the base body 2, respectively. Of the time rate of change, the angular velocity component around the X axis, the angular velocity component around the Y axis, the angular velocity component around the Z axis, and ωbt are the temporal change rate (angular velocity) of the displacement amount of the joint 12 of the base 2. .

  If a Jacobian matrix representing a linear mapping from ↑ Vb to ↑ S is A, the linear mapping is represented by the following equation (4).

↑ S = A ・ ↑ Vb (4)

In this case, the Jacobian matrix A has inertia matrices M and H representing linear mapping from ↑ Vb to ↑ P and ↑ L, and linear mapping from ↑ Vb to ↑ Vb, as shown in the following equation (5). Is a matrix in which a unit matrix E (↑, unit matrix of the same order as the number of components of Vb) is vertically arranged. In this embodiment, since ↑ Vb is a 7-component vector, M and H are 3 × 7 inertia matrices, and E is a 7 × 7 unit matrix. Therefore, in the present embodiment, the Jacobian matrix A is a 13 × 7 matrix.

  The entire processing of the base body motion speed target value determination unit 45 using the Jacobian matrix A is performed as follows.

That is, the base body motion speed target value determination unit 45 first determines the Jacobian matrix A representing the linear mapping of Expression (4) in each calculation processing cycle. Further, the base body motion speed target value determination unit 45 determines a pseudo inverse matrix A ⁺ of the calculated Jacobian matrix A.

Then, the base body motion speed target value determination unit 45 includes the corrected basic translational momentum vector ↑ P_cmd2 (current value) determined by the basic target translational momentum correction unit 43 and the basic target determined by the basic motion target value determination unit 42. The angular momentum vector ↑ L_cmd1 (current value) and the basic target base body motion speed vector ↑ Vb_cmd1 (current value) determined by the basic target base body speed determination unit 44 are respectively represented by ↑ P and ↑ L in the above equation (3). , ↑ Vb target value ↑ S_cmd of the control target vector ↑ S (hereinafter referred to as target control target vector ↑ S_cmd) and the pseudo inverse matrix A ⁺ By performing the calculation on the right side (the calculation process of multiplying ↑ S_cmd by A ⁺ ), the target base body motion speed vector ↑ Vb_cmd2 as the target value of ↑ Vb corresponding to ↑ S_cmd is determined.

↑ Vb_cmd2 = A ⁺・ ↑ S_cmd (6)

In the process of determining the target base body motion speed vector ↑ Vb_cmd2 in this way, the base body speed target value determination unit 45 determines the Jacobian matrix A in the present embodiment as follows.

  First, the premise regarding the determination method of the Jacobian matrix A in the present embodiment will be described. The Jacobian matrix A representing the linear mapping of the equation (4) is a Jacobian matrix having M and H of its inertia matrixes M and H and unit matrix E as variables.

  The Jacobian matrix A is basically based on the operation state of the robot 1 defined by the current target gait of the robot 1 (the target gait determined in the previous calculation processing cycle). Represents the generation sensitivity of the translational momentum vector ↑ P of the overall center of gravity of the robot 1 and the angular momentum vector ↑ L around the overall center of gravity of the robot 1 that is generated when the position and orientation components of the base 2 are slightly changed. What is necessary is just to calculate as a matrix.

  In this case, the translational momentum vector ↑ P of the total center of gravity of the robot 1 and the angular momentum around the total center of gravity of the robot 1 generated by minutely changing each component of the position and posture of the base 2 from the operating state of the starting point. The vector ↑ L is, for example, a dynamic model in which the mass point of the center of gravity and the inertia (moment of inertia) around the center of gravity are set for each link of the robot 1 (hereinafter, such a dynamic model is referred to as a full model). Can be used to calculate. A known method can be adopted as the calculation method.

  However, in this embodiment, in order to further improve the reliability of the target base body motion speed vector ↑ Vb_cmd2 determined according to the target control target vector ↑ S_cmd, the current time (the time of the current processing cycle) is changed to the next time. Estimate the instantaneous value of the future target gait (target leg position / posture and target base body position / posture) within the period (within the time of one computation process cycle) from the time of the computation cycle, and the estimated future The Jacobian matrix A is determined based on the operation state of the robot 1 defined by the instantaneous value of the target gait.

  In other words, in each calculation processing cycle, the operation state of the robot 1 that is the starting point for calculating the Jacobian matrix A is shifted from the operation state defined by the current target gait to a predicted operation state in the future. After shifting, the Jacobian matrix A is calculated.

  This is due to the following reason. That is, since the relationship between the minute change in the position and posture of the base 2 and the ↑ P and ↑ L generated due to the change is generally nonlinear, the current target gait state of the robot 1 is the starting point. When the Jacobian matrix A determined as is used, ↑ Vb_cmd2 determined by the above equation (6) is likely to deviate from the optimum value for realizing ↑ S_cmd.

  For example, the position Xb of the base 2 and the position Xg of the total center of gravity of the robot 1 when the positions and postures of the foot portions 18R and 18L of the leg links 3R and 3L are kept constant are multiplied by the total mass Mall of the robot 1. Focusing on the relationship between the objects (= Mall · Xg), this relationship is generally a non-linear relationship as illustrated by a solid curve a1 in FIG.

  Here, for convenience of understanding, Xb and Xg are assumed to be positions in one of the X-axis direction, Y-axis direction, and Z-axis direction of the global coordinate system, for example, positions in the X-axis direction. In this case, Mall · Xg is a state quantity corresponding to the integral value of the translational momentum of the entire center of gravity of the robot 1 in the X-axis direction.

  If the current position of the base 2 in the current desired gait is, for example, Xb (k) shown in FIG. 6, the Jacobian matrix A (hereinafter referred to as the starting point of the current desired gait state) In the basic Jacobian matrix A1, the relationship between the minute change of Xb and the corresponding change of Mall · Xg is a linearity indicated by the tangent line a2 of the curve a1 at the position of Xb (k). It will be expressed as a relationship.

  Therefore, when the target base body motion speed vector ↑ Vb_cmd2 corresponding to the target control target vector ↑ S_cmd is determined by the above equation (6) using the pseudo inverse matrix of the basic Jacobian matrix A1, the gait generator 32 The target amount of change in the position Xb of the base 2 in time during one calculation process cycle (one calculation process period) Δt is ΔXb_a shown in FIG.

  This ΔXb_a is a value obtained by multiplying Δt by the value of the component Vbx corresponding to the temporal change rate of the position Xb in ↑ Vb_cmd2 determined using the pseudo inverse matrix of the basic Jacobian matrix A1 as described above. In other words, ΔXb_a is the amount of change in Mall · Xg corresponding to ΔXb_a, assuming that the relationship between the position Xb of the substrate 2 and Mall · Xg is the relationship represented by the tangent line a2. Is the amount of change in Xb such that the target value Px_cmd2 of the translational momentum in the X-axis direction of the ↑ P_cmd2 component of the target control target vector ↑ S_cmd is multiplied by Δt (= Px_cmd2 · Δt) is there.

  However, when the actual relationship between the position Xb of the substrate 2 and Mall · Xg is the relationship represented by the curve a1, the actual change amount of Mall · Xg corresponding to ΔXb_a is shown in FIG. The middle Mall · ΔXg_a. This value Mall · ΔXg_a generally causes an error with respect to the value Px_cmd2 · Δt as the target value of the change amount of Mall · Xg defined by the target control target vector ↑ S_cmd.

  If the actual relationship between the position Xb of the substrate 2 and Mall · Xg is the relationship represented by the curve a1, the change amount of Mall · Xg is defined by the target control target vector ↑ S_cmd. An appropriate amount of change in the position Xb of the base body 2 to achieve the target value Px_cmd2 · Δt of the amount of change of Mall · Xg is ΔXb_b (≠ ΔXb_a) shown in FIG.

  On the other hand, an intermediate position Xb (k) + ΔXb_c between the position Xb (k) of the base 2 in the current target gait and the position Xb (k) + ΔXb_b changed from the position Xb (k) by ΔXb_b. However, when 0 <ΔXb_c <ΔXb_b) is assumed, the slope of the tangent line a3 of the curve a1 at the intermediate position Xb (k) + ΔXb_c is calculated based on the mean value theorem in calculus and the position Xb (k). Can be matched or approximated to the slope of the straight line a4 connecting the point on the curve a1 with the point on the curve a1 at the position Xb (k) + ΔXb_b.

Accordingly, the Jacobian matrix A is determined based on a future target gait such that the position Xb of the base 2 becomes the intermediate position Xb (k) + ΔXb_c, and the pseudo inverse matrix A ⁺ of the Jacobian matrix A is used. If Vb_cmd2 is determined, the amount of change in the position Xb of the substrate 2 defined by ↑ Vb_cmd2 is made to coincide with the above-mentioned appropriate amount of change ΔXb_b, or approximate accuracy with respect to the amount of change ΔXb_b. Can be high. That is, the reliability of ↑ Vb_cmd2 can be improved.

  This matter is not limited to the component of ↑ Vb_cmd2 that affects ↑ P_cmd2 of ↑ S_cmd, but also applies to the component of ↑ Vb_cmd2 that affects ↑ L_cmd1.

  Therefore, in the present embodiment, as described above, the future in the period (within the time of one arithmetic processing cycle) between the current time (the time of the current arithmetic processing cycle) and the time of the next arithmetic processing cycle. The instantaneous value of the target gait (target leg position / posture and target base body position / posture) is estimated, and the Jacobian matrix A is determined from the motion state of the robot 1 defined by the estimated instantaneous value of the future target gait. It was decided to.

  In this case, in the present embodiment, a target leg that is a component of a future target gait that defines an operating state for determining the Jacobian matrix A (hereinafter referred to as a Jacobian matrix determination target gait). The instantaneous values of the position / posture and the target base body position / posture (hereinafter referred to as the Jacobian matrix determination target leg position / posture and the Jacobian matrix determination target base body position / posture) are determined as follows.

  That is, as shown in the following equation (7a), the base movement speed target value determination unit 45 determines the current target base body position / posture (the target base body position / posture determined in the previous calculation processing cycle. (denoted as (k)) and the target substrate position / posture one calculation processing cycle before (the target substrate position / posture determined in the previous calculation processing cycle, hereinafter referred to as target substrate position / posture (k-1)) ), That is, a target base body position / posture change amount determined in the previous calculation processing cycle (= target base body position / posture (k) −target base body position / posture (k−1)) is set to a predetermined coefficient Kp. By adding a correction term obtained by multiplication to the target base body position / posture (k), the target base body position / posture for determining the Jacobin An matrix is determined.

  Similarly, the base body motion speed target value determination unit 45, as shown by the following equation (7b), present target leg position / posture for each leg link 3 (target leg position / posture determined in the previous calculation processing cycle. , This is expressed as a target leg position / posture (k)) and a target leg position / posture one calculation processing cycle before (the target leg position / posture determined in the previous calculation processing cycle. k-1)), that is, the amount of change in the target leg position / posture determined in the previous calculation processing cycle (= target leg position / posture (k) −target leg position / posture (k-1)) Is added to the target leg position / posture (k) to determine the target base body position / posture for determining the Jacobin An matrix.

  The coefficient Kp is a value determined in advance in the range of 0 ≦ Kp ≦ 1. In this embodiment, for example, Kp = 0.5.

Target base body position / posture for determining the Jacobinan matrix = target base body position / posture (k) + Kp (target base body position / posture (k) -target base body position / posture (k-1))
...... (7a)

Target leg position / posture for determining the Jacobinan matrix = target leg position / posture (k) + Kp (target leg position / posture (k) -target leg position / posture (k-1))
...... (7b)

Therefore, in the present embodiment, the target gait for determining the Jacobian matrix is the current target gait according to the latest value in the time series of the change amount of the target gait for each arithmetic processing cycle of the gait generator 32. Is determined as a gait obtained by correcting.

  When the robot 1 is provided with other links movable relative to the base 2 such as arm links, the future target position and target posture of the tip of the other links, or each joint of the other links The future target displacement amount may be obtained by the same method as the target body position / posture for determining the Jacobin matrix and the target leg position / posture for determining the Jacobian matrix.

  The target gait for determining the Jacobian matrix is determined from the target gaits for three or more calculation processing cycles including the current target gait and the target gait one calculation processing cycle before. Also good.

  Then, the base body motion speed target value determination unit 45 starts from the state of the target gait for determining the Jacobian matrix determined as described above, and minutely changes the position and posture components of the base body 2 from the state of the start point. The translational momentum vector ↑ P of the entire center of gravity of the robot 1 and the Jacobian matrix A as a matrix representing the generation sensitivity of the angular momentum vector ↑ L around the entire center of gravity are calculated using the full model. This calculation method may be the same as a known method.

  The dynamic model used to calculate the Jacobian matrix A may be a dynamic model different from the full model (for example, a dynamic model that ignores inertia of some links), but the dynamic model is as much as possible. It is desirable to use a dynamic model having a high dynamic accuracy (a dynamic model having a higher dynamic accuracy than the dynamic model used in the processing of the basic operation target value determination unit 42). Here, the high dynamic accuracy means that the approximate accuracy of the total inertia force generated on the dynamic model by the movement of the robot 1 to the total inertia force generated in the actual robot 1 is high. To do.

Next, the pseudo inverse matrix A ⁺ of the Jacobian matrix A determined as described above is determined as follows in this embodiment.

In the present embodiment, a so-called singular point low sensitivity motion decomposition matrix (SR-Inverse) is determined as a pseudo inverse matrix A ⁺ . In this case, the pseudo inverse matrix A ⁺ of the Jacobian matrix A can be calculated by the following equation (8a) or (8b), where W and V are arbitrary weight matrices (diagonal matrices).

A ⁺ = W ^-1 · ^AT · (A · W ^-1 · ^AT + V) ^-1 (8a)
A ⁺ = (A ^T · V ^-1 · A + W) ^-1 · A ^T · V ^-1 (8b)

The pseudo inverse matrix A ⁺ calculated by these equations (8a) or (8b) is a target substrate motion velocity vector ↑ Vb_cmd2 that minimizes the value of the evaluation function f given by the following equation (9): It is a pseudo inverse matrix that can be calculated by the equation (6).

f = (↑ Vb_cmd2) ^T・ W ・ ↑ Vb_cmd2
+ (↑ S_cmd-A ・ ↑ Vb_cmd2) ^T・ V ・ (↑ S_cmd-A ・ ↑ Vb_cmd2)
...... (9)

Note that the value of the evaluation function f is minimum means that the square value of each component of ↑ Vb_cmd2 is linearly combined with each diagonal component of W as a weighting factor, and ↑ S_cmd and A · ↑ Vb_cmd2 This means that the sum of the square value of each component of the difference vector and the value obtained by linearly combining each diagonal component of V as a weighting coefficient is minimized.

  The weight matrix W is a diagonal matrix having the same order (seventh order in the present embodiment) as the number of components of the target base body motion speed vector ↑ Vb_cmd2. The value (weighting coefficient) of each diagonal component of the weight matrix W may be set in consideration of the allowable range of the value of each component of ↑ Vb_cmd2, for example.

  The weight matrix V is a diagonal matrix having the same order (13th order in this embodiment) as the target control target vector ↑ S_cmd. The value (weighting coefficient) of each diagonal component of the weight matrix V is, for example, the target value defined by the target control target vector ↑ S_cmd of the value of each component of the control target vector ↑ S calculated by A · ↑ Vb_cmd2. It may be set according to the tolerance of the error with respect to.

  Each of the weight matrices W and V may be a unit matrix. Further, V in Expression (8a) and W in Expression (8b) may each be a zero matrix.

Although the pseudo inverse matrix A ⁺ of the Jacobian matrix A may be calculated by any of the equations (8a) and (8b), in the present embodiment, the base body motion speed target value determination unit 45, for example, The pseudo inverse matrix A ⁺ is calculated using equation (8b). In this case, in this embodiment, since the basic target base body motion speed vector ↑ Vb_cmd1 is set as a reference target for the target base body motion speed vector ↑ Vb_cmd2, the weight matrix W is set to, for example, a zero matrix.

  The weight matrix V corresponds to, for example, the target value (= ↑ P_cmd2) of the translational momentum vector ↑ P and the target value (= ↑ L_cmd1) of the angular momentum vector ↑ L in the target control target vector ↑ S_cmd. Among the diagonal components (six diagonal components) and the target value (= ↑ Vb_cmd) of the base body motion velocity vector ↑ Vb in ↑ S_cmd, the component excluding the target value of the displacement amount of the joint 12 (that is, the lower side) The diagonal component (six diagonal components) corresponding to the target value of the position and orientation of the base body 10 and the diagonal component corresponding to the target value of the displacement amount of the joint 12 (one diagonal component) By classifying into three types of diagonal components, the value of each type of diagonal component was set.

  As an example, among the diagonal components of the weight matrix V, the values of the six diagonal components corresponding to the target value of the translational momentum vector ↑ P and the target value of the angular momentum vector ↑ L are 1.0, The value of six diagonal components corresponding to the target values of 10 positions and postures is 0.5, and the value of one diagonal component corresponding to the target value of the displacement amount of the joint 12 of the base 2 is 0.1. Thus, a weight matrix V was set.

In the present embodiment, the base body motion speed target value determination unit 45 uses the weight matrixes W and V set in advance as described above in each calculation processing cycle, and the pseudo inverse matrix A of the Jacobian matrix A according to the above equation (8b). Determine ⁺ .

Then, from the pseudo inverse matrix A ⁺ and the target control target vector ↑ S_cmd (current value), the target base body motion speed vector ↑ Vb_cmd2 (current value) is determined by the above equation (6).

  In this case, since the weight matrix W is a zero matrix in this embodiment, the second term on the right side of Equation (9) is minimized, that is, each component of the difference vector of ↑ S_cmd and A · ↑ Vb_cmd2 ↑ Vb_cmd2 is determined so as to minimize a value obtained by linearly combining the squared values of each of the diagonal components of the weight matrix V as weighting coefficients.

  Further, in this case, ↑ S_cmd is a basic target base body motion speed vector ↑ Vb_cmd1 as an operation amount for bringing the base body position deviation close to zero in addition to the corrected basic translational momentum vector ↑ P_cmd2 and the basic target angular momentum vector ↑ L_cmd1. Therefore, ↑ Vb_cmd2 does not deviate from ↑ Vb_cmd1, and the translational momentum vector ↑ P of the control target vector (= A · ↑ Vb_cmd2) calculated by the right side of the equation (6), The angular momentum vector ↑ L is determined so as to coincide with the corrected target translational momentum vector ↑ P_cmd2 and the target angular momentum vector ↑ L_cmd1 as much as possible.

  The above is the details of the processing of the base body motion speed target value determination unit 45 in the present embodiment.

  The gait generator 32 executes the processing of the target body position / posture determination unit 46 after executing the processing of the target body speed target value determination unit 45 as described above.

  The target base body position / posture determination unit 46 determines the target base body position / posture by sequentially integrating the target base body speed vector ↑ Vb_cmd2 determined by the base body speed target value determination unit 45.

  Specifically, the target base body position / posture determination unit 46 multiplies the current value of the target base body motion velocity vector ↑ Vb_cmd2 by the time Δt of the arithmetic processing cycle of the gait generator 32 in each arithmetic processing cycle (= ↑ Vb_cmd2 · Δt) is added to the current target base body position / posture value (the value determined in the previous calculation processing cycle) to determine the current value of the target base body position / posture.

  In this embodiment, in each calculation processing cycle of the gait generator 32, the target base body position / posture (current value) determined by the target base body position / posture determination unit 46 as described above and the target leg position / posture determination unit 41 are as described above. The determined target leg position / posture (current value) of each leg link 3 is a component of the target gait (current value).

  In the present embodiment, the gait generator 32 further executes the processing of the target joint displacement amount determiner 48. The target joint displacement amount determination unit 48 is provided with components of the determined target gait (target base body position / posture and target leg position / posture) in each calculation processing cycle.

  Then, the target joint displacement amount determination unit 48 calculates the target joint displacement amount of each joint of the robot 1 defined by the given target gait by inverse kinematics based on the geometric model (rigid link model) of the robot 1. Calculated by arithmetic processing. In this case, the geometric model of the robot 1 is the same as that used by the gravity center position calculation unit 47 in the target gait.

  In the target gait center of gravity position calculation unit 47, instead of the target gait (target base body position / posture and target leg position / posture), the target joint displacement amount of each joint determined by the target joint displacement amount determination unit 48 is used. (Previous value) may be used to determine the center of gravity position ↑ Xg_cmd2 in the target gait. In such a case, the target gait center-of-gravity position calculation unit 47 does not need to calculate the displacement amount of each joint defined by the target gait.

  Supplementally, when the robot 1 is provided with another link that is movable with respect to the base body 2 such as an arm link, the component related to the other link in the target gait of the robot 1 is, for example, the basic motion target value determination What is necessary is just to make it determine suitably in the part 42. As an example, when the robot 1 includes an arm link extending from the upper base 11, the target gait generation process for performing the walking motion or the running motion of the robot 1 is performed on the upper base 11 in the target gait. The entire posture of the arm link (and hence the amount of displacement of each joint of the arm link) is set to a constant posture, or the entire arm link is periodically moved around the rotation axis of the joint 12 of the base 2. The target gait of the arm link may be adopted.

  The above is the details of the processing of the gait generator 32 in the present embodiment.

  Here, the correspondence between the embodiment described above and the present invention will be supplemented. In the present embodiment, the control processing unit 31 includes the gait generator 32, thereby having a function as a gait generator or an operation target generator of the present invention. In this case, the target gait sequentially generated by the gait generator 32 corresponds to the operation target in the operation target generation device of the present invention.

  Then, the target leg position / posture determination in the gait generator of the present invention is performed by the processing of the target leg position / posture determination unit 41, the base body speed target value determination unit 45, and the target base body position / posture determination unit 46 of the gait generation unit 32, respectively. Means, base body movement speed target value determination means, and target base body position / posture determination means are realized. Furthermore, the overall processing of the basic motion target value determination unit 42, the basic target translational momentum correction unit 43, and the basic target base body motion speed determination unit 44 realizes a target control target vector determination unit in the gait generator of the present invention. The

  Further, the processing for determining the Jacobian matrix A in the base body motion speed target value determination unit 45 determines the Jacobian matrix determination means in the motion target generation device of the present invention. In this case, the base body motion speed vector ↑ Vb and the control target vector ↑ S correspond to the first state quantity vector and the second state quantity vector in the operation target generation device of the present invention, respectively.

According to the present embodiment described above, the base body motion speed target value determination unit 45 includes the corrected basic translational momentum vector ↑ P_cmd2, the basic target angular momentum vector ↑ L_cmd1, and the basic target base body motion speed vector ↑ Vb_cmd1. A target base motion velocity vector ↑ Vb_cmd2 is sequentially determined by multiplying the ↑ S_cmd by the pseudo inverse matrix A ⁺ of the Jacobian matrix A with the target control target vector ↑ S_cmd as a target. Then, by integrating this ↑ Vb_cmd2, the target base body position / posture in the target gait is determined.

  In this case, the corrected basic translational momentum vector ↑ P_cmd2 set as the target value of the translational momentum vector ↑ P among the components of the target control target vector ↑ S_cmd is the basic target translation determined by the basic motion target value determination unit 42. With the momentum vector ↑ P_cmd1 as a reference target, the center of gravity position ↑ Xg_cmd2 on the target gait is controlled by the feedback operation amount for suppressing the deviation from the basic target centroid position ↑ Xg_cmd1 determined by the basic motion target value determination unit 42. It is determined by correcting the basic target translational momentum vector ↑ P_cmd1.

  Further, the basic angular translational momentum vector ↑ P_cmd2 determined by the basic motion target value determining unit 42 is set as the target value of the angular momentum vector ↑ L among the components of the target control target vector ↑ S_cmd.

  The basic target base body motion speed vector ↑ Vb_cmd1 set as the target value of the base body motion speed vector ↑ Vb among the constituent elements of the target control target vector ↑ S_cmd is the basic base motion in the target gait. It is determined as a feedback operation amount for suppressing deviation from the basic target base body position / posture determined by the target value determination unit 42.

  Therefore, the target base body motion speed vector ↑ Vb_cmd2 sequentially determined by the base body motion speed target value determination unit 45 does not destroy the dynamics satisfied on the dynamic model in the processing of the basic motion target value determination unit 42 as much as possible. However, the target base body position / posture determined by integrating the target base body velocity vector ↑ Vb_cmd2 is determined so as not to deviate from the basic target base body position / posture determined by the basic motion target value determination unit 42. The Rukoto.

  Therefore, according to this embodiment, it is possible to generate a target gait that satisfies the dynamic requirements and does not cause the robot 1 to lose its posture.

Further, in the processing of the base body motion speed target value determination unit 45, the position and orientation of the base body 2 of the robot 1 and the state quantity corresponding to the integral value of the translational momentum vector of the entire center of gravity of the robot 1 and the angular momentum vector around the entire center of gravity The Jacobian matrix A is determined with the target gait for determining the Jacobian matrix as an estimated value of the future target gait as a starting point in consideration of the nonlinearity with the state quantity corresponding to the integral value of Therefore, the target base body motion determined by multiplying the target control target vector ↑ S_cmd by the pseudo inverse matrix A ⁺ of the Jacobian matrix A as the value of the base body motion speed vector ↑ Vb for realizing the target control target vector ↑ S_cmd. The reliability of the velocity vector ↑ Vb_cmd2 can be improved.

  In other words, the degree of coincidence of the value of the control target vector ↑ S obtained by multiplying the determined Jacobian matrix A by ↑ Vb_cmd2 with respect to the target control target vector ↑ S_cmd can be increased.

In addition, in the present embodiment, since the pseudo inverse matrix A ⁺ of the Jacobian matrix A is a singular point low sensitivity motion decomposition matrix (SR-Inverse), the substrate motion speed is used by using the pseudo inverse matrix of the Jacobian matrix. Determine the target value of the vector ↑ Minimize the value obtained by linearly combining the square value of each component of the vector of the difference between S_cmd and A · ↑ Vb_cmd2 by the weighting factor that is each diagonal component of the diagonal matrix V can do.

  Therefore, an appropriate target gait can be sequentially determined for realizing the target control target vector ↑ S_cmd.

  Here, verification results regarding the above-described effects of the present embodiment will be described below with reference to FIGS.

  A graph a <b> 1 in FIG. 7A shows the change with time of the inclination angle around the Y axis (around the pitch axis) of the lower substrate 10 in the target substrate position and orientation in the example of the present embodiment. In this case, the target gait of the robot 1 is a gait for walking with a maximum stride of 400 mm, for example. Of the basic target substrate position and posture in this embodiment, the posture of the lower substrate 10 around the Y axis (around the pitch axis) is 0 deg.

  A graph a2 in FIG. 7B shows a change with time of the inclination angle around the Y axis (around the pitch axis) of the lower substrate 10 in the target substrate position and orientation in the comparative example.

  In this case, the target gait of the robot 1 in the comparative example is a gait that walks with a maximum stride of 400 mm, as in the embodiment of FIG. Of the basic target substrate position and posture in this comparative example, the posture around the Y axis (around the pitch axis) of the lower substrate 10 is 0 deg as in the embodiment of FIG.

  However, in this comparative example, the basic body motion velocity vector ↑ Vb_cmd1 is excluded from the target control target vector, and the target control target vector is constituted by the corrected basic target translational momentum vector ↑ P_cmd2 and the basic target angular momentum vector ↑ L_cmd1. To do.

  As can be seen with reference to FIG. 7B, in the comparative example not including the base body motion speed vector ↑ Vb_cmd1 as a constituent element of the target control target vector ↑ S_cmd, the target base body position / posture is finally the basic target base body position / posture ( = 0 deg), a steady deviation of about 13 deg was generated.

  On the other hand, in the embodiment including the base body motion speed vector ↑ Vb_cmd1 as a component of the target control target vector ↑ S_cmd, the target base body position / posture is stationary with respect to the basic target base body position / posture as shown in FIG. The gap was resolved.

  Therefore, according to the present embodiment including the base body motion speed vector ↑ Vb_cmd1 as a constituent element of the target control target vector ↑ S_cmd, the target motion speed vector ↑ Vb_cmd1 is compared with the case where the base control speed vector ↑ Vb_cmd1 is not included in the target control target vector ↑ S_cmd. It can be seen that the base position / posture can be made to follow the basic target base position / posture without greatly deviating from the basic target base position / posture.

  Next, the graphs b1 and c1 in FIG. 8A and FIG. 9A respectively indicate the X-axis direction of the entire center of gravity of the robot 1 defined by the target gait created in the example of this embodiment. Error of translational momentum of the robot (error with respect to the X-axis direction component of the basic target translational momentum vector ↑ P_cmd1), error of angular momentum around the Y axis (around the pitch axis) at the entire center of gravity of the robot 1 defined by the target gait It is a graph which shows a time-dependent change (error with respect to the component of the basic target angular momentum vector ↑ L_cmd1 around the Y axis). In this case, the target gait of the robot 1 is a gait for walking with a maximum stride of, for example, 355 mm.

  Also, the graphs b2 and c3 in FIG. 8B and FIG. 9B respectively show the translational momentum errors in the X-axis direction of the entire center of gravity of the robot 1 defined by the target gait created in the comparative example. (Error with respect to X-axis direction component of basic target translational momentum vector ↑ P_cmd1), error of angular momentum around Y axis (around pitch axis) at the total center of gravity of robot 1 defined by the target gait (basic target angular momentum It is a graph which shows a time-dependent change of the error about the Y axis component of the vector ↑ L_cmd1.

  However, in the comparative example of FIG. 8B and FIG. 9B, the operation state of the robot 1 as a starting point when calculating the Jacobian matrix A in each arithmetic processing cycle is as shown in FIG. 8A and FIG. Unlike the embodiment of (a), the operation state is defined by the target gait determined in the previous calculation processing cycle (the latest value of the target gait already determined).

  8A and 9A and the comparative example of FIGS. 8B and 9B, in order to clarify the effect due to the difference in the determination process of the Jacobian matrix A. Further, among the components of the target control target vector ↑ S_cmd, the corrected basic translational momentum vector ↑ P_cmd2 is made to coincide with the basic target translational momentum vector ↑ P_cmd1.

  8B, in the comparative example, the absolute value of the error of the translational momentum (translational momentum in the X-axis direction) of the entire center of gravity of the robot 1 is 2.3 kg · m / s at the maximum. It was about. On the other hand, as can be seen from the graph b1 in FIG. 8A, in the embodiment, the absolute value of the error of the translational momentum (translational momentum in the X-axis direction) of the entire center of gravity of the robot 1 is 0. It is below 008kg · m / s.

  9B, in the comparative example, the absolute value of the error of the angular momentum around the entire center of gravity of the robot 1 (angular momentum around the Y axis) is 0.8 kg · It was about m / s. On the other hand, as can be seen from the graph c1 in FIG. 9A, in the embodiment, the absolute value of the error of the angular momentum around the entire center of gravity of the robot 1 (angular momentum around the Y axis) is 0 at the maximum. It is within 0.003kg ・ m / s.

  Therefore, this embodiment uses an operation state shifted from the operation state defined by the current target gait closer to the predicted operation state in the future as the operation state of the robot 1 for calculating the Jacobian matrix A. According to the above, the target control target vector ↑ S_cmd is followed more accurately than when the operation state defined by the current target gait is used as the operation state of the starting point of the robot 1 for calculating the Jacobian matrix A. It can be seen that the desired gait can be generated.

  In the above embodiment, the biped mobile robot 1 having two leg links has been described as an example. However, the gait generator of the present invention can also be applied to a legged mobile robot having three or more leg links. .

In the embodiment, the pseudo inverse matrix A ⁺ is calculated by the equation (8b), but may be calculated by the equation (8a).

  The robot 1 in the above embodiment is a robot having the joint 12 on the base body 2, but the lower base body 10 and the upper base body 11 may be integrally formed. In this case, the basic target base body motion velocity vector ↑ Vb_cmd1 and the target base body motion speed vector ↑ Vb_cmd2 may be configured by six-component vectors excluding the temporal change rate component of the displacement amount of the joint 12.

  Alternatively, a joint (a rotatable joint around two or more axes) having two or more degrees of freedom may be provided between the lower base 10 and the upper base 11. In this case, the basic target base body motion speed vector ↑ Vb_cmd1 and the target base body motion speed vector ↑ Vb_cmd2 are set to the components of the temporal change rate of the position and orientation of the lower base body 10 and the lower base body 10 and the upper base body 11. You may comprise by the component of the temporal change rate of the displacement amount of the direction around each axis | shaft of a joint between.

  In addition, each leg link 3 of the robot 1 in the embodiment is a link mechanism having 6 degrees of freedom, but may be a link mechanism having 7 degrees of freedom or more.

  In the embodiment, the base body motion speed vector ↑ Vb is included as a component of the control target vector ↑ S. However, from the translational momentum vector ↑ P and the angular momentum vector ↑ L, excluding ↑ Vb. The control target vector ↑ S may be configured.

  Further, regarding the motion target generation device of the present invention, the target robot is not limited to the legged mobile robot such as the robot 1, but may be, for example, an installation work robot or means for movement. A mobile work robot having wheels or a crawler may be used.

  In this case, as the first state quantity vector in the motion target generation device of the present invention, for example, a vector configured with the amount of change per unit time of the displacement amount of each joint included in the work robot as described above as a component, etc. Can be adopted. Further, the second state quantity vector includes a time change rate (translation speed or rotational angular velocity) of the position or posture of the tip of the work robot, a force acting on the tip, or the total center of gravity of the work robot. A vector or the like configured with a translational momentum or an angular momentum around the entire center of gravity as a component can be employed. The types of the first state quantity vector and the second state quantity vector can be appropriately set in design in consideration of the robot specifications, the required operation target or performance, and the like.

  DESCRIPTION OF SYMBOLS 1 ... Mobile robot, 2 ... Base | substrate, 3R, 3L ... Leg link, 41 ... Target leg position / posture determination part (target leg position / posture determination means), 42 ... Basic motion target value determination part (target control object vector determination means), 43: Basic target translational momentum correction unit (target control target vector determination means) 44: Basic target base body motion speed determination section (target control target vector determination means) 45: Base body motion speed target value determination section (base motion speed target value) Determination means, Jacobian matrix determination means), 46... Target substrate position / posture determination unit (target substrate position / posture determination means).

Claims (4)

A gait generator that sequentially generates a target gait that defines a displacement amount of each joint of a legged mobile robot that extends from a base and moves by movement of a plurality of leg links each provided with a plurality of joints. And
Target leg position / posture determination means for sequentially determining a target leg position / posture which is a target value of the position and posture of the tip of each leg link of the mobile robot as a component of the target gait;
A target control target vector, which is a target value of a control target vector expressed as a vector obtained by linearly mapping a base body motion speed vector composed of the amount of change per unit time in the position and orientation of the base body, is sequentially determined. A target control target vector determining means;
Substrate motion speed target value determining means for sequentially determining the Jacobian matrix representing the linear mapping and multiplying the target control target vector by the pseudo inverse matrix of the Jacobian matrix to determine the target value of the base body motion speed vector When,
A target substrate position / posture that sequentially determines a target substrate position / posture that is a target value of the position / posture of the base as a component of the target gait by sequentially integrating the determined target values of the base body velocity vector. A determination means,
The base body motion speed target value determining means determines the Jacobian matrix used for determining a new target value of the base body motion speed vector, and is the latest value of the already determined target gaits of the mobile robot. The mobile robot within a period between a time corresponding to a target gait of the robot and a time corresponding to a new target gait in which a target base body position / posture is defined according to a new target value of the base body motion speed vector Is estimated based on the latest target gait and the previous one or more target gaits of the past value, and the estimated future gait instantaneous value is: The Jacobian matrix corresponding to the operating state of the starting point of the mobile robot, which is used as the operating state of the mobile robot that becomes the starting point of the change in the position and orientation of the base body by the new target value of the base body motion velocity vector Determining gait generating device of a legged mobile robot which is characterized in that. The gait generator for a legged mobile robot according to claim 1,
The base body movement speed target value determining means is configured to calculate a difference between a target base body position / posture in the latest target gait and a target base body position / posture in a previous target gait immediately before the latest target gait. By adding a correction amount obtained by multiplying a predetermined coefficient K within the range between “0” and “1” to the target base body position / posture in the latest target gait, the future of the target base body position / posture And a difference between the target leg position / posture in the target gait with the latest value and the target leg position / posture in the target gait with the previous previous value of the latest value for each leg link. Means for estimating a future instantaneous value of the target leg position / posture of each leg link by adding a correction amount obtained by multiplying the coefficient K to the target leg position / posture of the latest target gait. Future moment of the desired target body position and orientation The future of gait instantaneous value and the components, the gait generation system of a legged mobile robot which comprises using as the operating state of the starting point of the mobile robot of the target leg position and posture of each leg link. The gait generator for a legged mobile robot according to claim 1 or 2,
The control object vector is a legged movement characterized in that the vector includes at least each component of the translational momentum vector of the overall center of gravity of the mobile robot and each component of the angular momentum vector around the overall center of gravity of the mobile robot. Robot gait generator. A first state quantity vector composed of a change amount per unit time of a predetermined kind of state quantity of a robot configured by connecting a plurality of links by a plurality of joints, and a linear mapping of the first state quantity vector The robot of the robot defining a displacement amount of each joint of the robot using the Jacobian matrix while sequentially determining a Jacobian matrix representing the linear mapping between the second state quantity vector expressed as a vector In an apparatus for sequentially determining an operation target,
When determining a new motion target of the robot, a time period between a time corresponding to the latest motion target of the robot motion targets already determined and a time corresponding to the new motion target is determined. An instantaneous value of the future motion target of the robot is estimated based on the latest motion target and one or more previous motion targets immediately before, and the estimated future motion target instantaneous value is Jacobian matrix determining means for determining the Jacobian matrix corresponding to the operation state of the starting point of the robot, using the operation state of the robot as a starting point of the change of the predetermined type of state amount by the first state amount vector. A robot motion target generation apparatus comprising:

Images